Commit c16b7c1b authored by Valenza Florian's avatar Valenza Florian
Browse files

[C++] Specialized setIndexes for JointComposite

parent add1b837
......@@ -399,7 +399,9 @@ namespace se3
int idx_v() const { return i_v; }
JointIndex id() const { return i_id; }
void setIndexes(JointIndex id,int q,int v) { i_id = id, i_q = q; i_v = v; }
void setIndexes(JointIndex id, int q, int v) { derived().setIndexes_impl(id, q, v); }
void setIndexes_impl(JointIndex id,int q,int v) { i_id = id, i_q = q; i_v = v; }
void disp(std::ostream & os) const
{
......
......@@ -365,7 +365,9 @@ namespace se3
int nq_impl() const { return nq_composite; }
// Be sure to call it AFTER setIndexes
/**
* @brief Update the indexes of subjoints in the stack
*/
void updateComponentsIndexes()
{
int current_idx_q, current_idx_v;
......@@ -382,6 +384,12 @@ namespace se3
}
}
void setIndexes_impl(JointIndex id, int q, int v)
{
JointModelBase<JointModelComposite>::setIndexes_impl(id, q, v);
updateComponentsIndexes();
}
static std::string classname() { return std::string("JointModelComposite"); }
std::string shortname() const { return classname(); }
......
......@@ -240,8 +240,6 @@ BOOST_AUTO_TEST_CASE ( test_R3xSO3)
model_composite.addJoint(model_composite.getJointId("universe"),jmodel_composite, placement, "composite_R3xSO3_joint");
model_composite.appendBodyToJoint(model_composite.getJointId("composite_R3xSO3_joint"), body_inertia, SE3::Identity());
// When Model will be cleaned in coming pull request, this will be done in addBody(addJoint)
boost::get<JointModelComposite>(model_composite.joints[model_composite.getJointId("composite_R3xSO3_joint")]).updateComponentsIndexes();
model_ff.addJoint(model_ff.getJointId("universe"),JointModelFreeFlyer(), placement, "ff_joint");
model_ff.appendBodyToJoint(model_ff.getJointId("ff_joint"), body_inertia, SE3::Identity());
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment