Merge pull request #3 from Masanori-Konishi/auto-stabilizer-wheel-fix…

…-fullbody-controller-go-wheel-no-wait Auto stabilizer wheel fix fullbody controller go wheel no wait
kindsenior · Jul 13, 2022 · 3420abe · 3420abe
2 parents d2c321b + b5f5a1d
commit 3420abe
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 2 deletions.
diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l
@@ -1303,10 +1303,14 @@
    "Call goPosWheel without wait."
    (send self :autobalancerservice_goPosWheel :x xx :y yy :th th :w_x w-xx :rv_max rv-max :ra_max ra-max :tm_off tm-off)
    (send self :wait-foot-steps))
-  (:go-wheel
+  (:go-wheel-no-wait
    (xx rv-max ra-max)
    "Call goWheel without wait."
-   (send self :autobalancerservice_goWheel :x xx :rv_max rv-max :ra_max ra-max)
+   (send self :autobalancerservice_goWheel :x xx :rv_max rv-max :ra_max ra-max))
+  (:go-wheel
+   (xx rv-max ra-max)
+   "Call goWheel with wait."
+   (send self :go-wheel-no-wait xx rv-max ra-max)
    (send self :wait-foot-steps))
   (:raw-get-foot-step-param
    ()

diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
@@ -191,6 +191,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());