joint_trajectory_action.py 文件源码

def _command_joints(self, joint_names, point, start_time, dimensions_dict):
        if (self._limb.has_collided() or not self.robot_is_enabled()
             or not self._alive or self._cuff.cuff_button()):
           rospy.logerr("{0}: Robot arm in Error state. Stopping execution.".format(
                            self._action_name))
           self._limb.exit_control_mode()
           self._result.error_code = self._result.PATH_TOLERANCE_VIOLATED
           self._server.set_aborted(self._result)
           return False
        elif self._server.is_preempt_requested():
           rospy.logwarn("{0}: Trajectory execution Preempted. Stopping execution.".format(
                            self._action_name))
           self._limb.exit_control_mode()
           self._server.set_preempted()
           return False
        velocities = []
        deltas = self._get_current_error(joint_names, point.positions)
        for delta in deltas:
            if ((math.fabs(delta[1]) >= self._path_thresh[delta[0]]
                  and self._path_thresh[delta[0]] >= 0.0)):
                rospy.logerr("%s: Exceeded Error Threshold on %s: %s" %
                             (self._action_name, delta[0], str(delta[1]),))
                self._result.error_code = self._result.PATH_TOLERANCE_VIOLATED
                self._server.set_aborted(self._result)
                self._limb.exit_control_mode()
                return False
            if self._mode == 'velocity':
                velocities.append(self._pid[delta[0]].compute_output(delta[1]))
        if self._mode == 'velocity':
            self._limb.set_joint_velocities(dict(zip(joint_names, velocities)))
        if self._mode == 'position':
            self._limb.set_joint_positions(dict(zip(joint_names, point.positions)))
        if self._mode == 'position_w_id':
            self._limb.set_joint_trajectory(joint_names,
                                            point.positions,
                                            point.velocities,
                                            point.accelerations)
        return True