[mc_rbdyn::Robot] the convexes location is updated at each forward ki…

…nematics
jrl-umi3218 · Aug 28, 2023 · c01eda4 · c01eda4
1 parent 8afa04d
commit c01eda4
Showing 1 changed file with 8 additions and 1 deletion.
diff --git a/src/mc_rbdyn/Robot.cpp b/src/mc_rbdyn/Robot.cpp
@@ -1169,11 +1169,18 @@ unsigned int mc_rbdyn::Robot::robotIndex() const
 
 void Robot::forwardKinematics()
 {
-  rbd::forwardKinematics(mb(), mbc());
+  forwardKinematics(mbc());
 }
 void Robot::forwardKinematics(rbd::MultiBodyConfig & mbc) const
 {
   rbd::forwardKinematics(mb(), mbc);
+
+  for(auto & cvx : convexes_)
+  {
+    unsigned int index = static_cast<unsigned int>(mb().bodyIndexByName(cvx.second.first));
+    sch::mc_rbdyn::transform(*(cvx.second.second.get()), mbc.bodyPosW[index]);
+  }
+
 }
 
 void Robot::forwardVelocity()