diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp index 1f28aa8c..688b8d8a 100644 --- 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 index bebd1346..11fa347e 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp @@ -188,7 +188,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();