Merge pull request #1124 from start-jsk/master

【auto-stabilizer】 merge origin/master into auto-stabilizer
start-jsk · Jun 10, 2022 · b5a8d86 · b5a8d86
2 parents 849e619 + 07f7e19
commit b5a8d86
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 2 deletions.
diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
@@ -457,7 +457,9 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
     trajectory_msgs::JointTrajectoryPoint point = trajectory.points[i];
     for (unsigned int j = 0; j < joint_names.size(); j++)
     {
-      parent->body->link(joint_names[j])->q = point.positions[j];
+      hrp::Link* l = parent->body->link(joint_names[j]);
+      if(l) l->q = point.positions[j];
+      else ROS_WARN_STREAM_ONCE("[" << parent->getInstanceName() << "] @onJointTrajectoryAction (" << this->groupname << ") : joint named " << joint_names[j] << " is not found. Skipping ...");
     }
 
     parent->body->calcForwardKinematics();

diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
@@ -196,7 +196,9 @@ void HrpsysSeqStateROSBridge::onJointTrajectory(trajectory_msgs::JointTrajectory
 
     trajectory_msgs::JointTrajectoryPoint point = trajectory.points[i];
     for (unsigned int j=0; j < trajectory.joint_names.size(); j++ ) {
-      body->link(joint_names[j])->q = point.positions[j];
+      hrp::Link* l = body->link(joint_names[j]);
+      if(l) l->q = point.positions[j];
+      else ROS_WARN_STREAM_ONCE("[" << getInstanceName() << "] @onJointTrajectoryAction : joint named " << joint_names[j] << " is not found. Skipping ...");
     }
 
     body->calcForwardKinematics();