From facea76d799ecd8512b19a04a11aa90469906f08 Mon Sep 17 00:00:00 2001
From: Naoki-Hiraoka <hiraoka@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 29 Dec 2020 12:07:04 +0900
Subject: [PATCH] [HrpsysSeqStateROSBridge.cpp] set body to reference angle
 when Goal comes

---
 hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
index bebd13460..08bb44f45 100644
--- 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());