Merge pull request start-jsk#1103 from Naoki-Hiraoka/fix-fullbody-con…

…troller [HrpsysSeqStateROSBridge.cpp] set body to reference angle when Goal comes
kazuki0824 · Jun 29, 2022 · 535106b · 535106b
2 parents 07f7e19 + 9c909ab
commit 535106b
Showing 1 changed file with 7 additions and 0 deletions.
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
@@ -183,6 +183,13 @@ void HrpsysSeqStateROSBridge::onJointTrajectory(trajectory_msgs::JointTrajectory
 
   std::vector<std::string> joint_names = trajectory.joint_names;
 
+  // set body to current reference angle
+  if ( m_mcangle.data.length() == body->joints().size() ) {
+    for (unsigned int i = 0; i < body->joints().size(); i++){
+      body->joint(i)->q = m_mcangle.data[i];
+    }
+  }
+
   for (unsigned int i=0; i < trajectory.points.size(); i++) {
     angles[i].length(body->joints().size());