Right now, items such as nh, ndh, nq, etc are inconvenient for use in indexing the state. They are appropriate for obtaining the size of certain state elements (nh=6 is the size of the momentum vector, for instance), but there should be a separate mechanism for getting the first index of a certain state element.
For example: Right now, to get the first three elements of qj (the joint position variables), we must do state_vector(si->nh + si->ndh + si->nqb, si->nh + si->ndh + si->nqb + 3), where state_vector is some structure holding the full state, and si is an instance of the LeggedRobotStates class.
More intuitively, we would like to do something like state_vector(qj_index, qj_index + 3).
Right now, items such as nh, ndh, nq, etc are inconvenient for use in indexing the state. They are appropriate for obtaining the size of certain state elements (nh=6 is the size of the momentum vector, for instance), but there should be a separate mechanism for getting the first index of a certain state element.
For example: Right now, to get the first three elements of qj (the joint position variables), we must do state_vector(si->nh + si->ndh + si->nqb, si->nh + si->ndh + si->nqb + 3), where state_vector is some structure holding the full state, and si is an instance of the LeggedRobotStates class.
More intuitively, we would like to do something like state_vector(qj_index, qj_index + 3).