torque_controller.py 文件源码

def attach_springs(self):
        self._start_angles = self._get_cmd_positions()
        control_rate = rospy.Rate(self._rate)

        # for safety purposes, set the control rate command timeout.
        # if the specified number of command cycles are missed, the robot
        # will timeout and disable
        self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds)

        error = [self._limb.joint_angles()[key] - self._start_angles[key] for key in self._limb.joint_angles().keys()]
        self.sum_sqr_error = sum([error[i]**2 for i in range(len(error))])
        print ('-------new set of joint position---------')
        print (self.sum_sqr_error)
        tolerance = 0.020
        # loop at specified rate commanding new joint torques
        while self.sum_sqr_error>tolerance and not rospy.is_shutdown():

            if not self._rs.state().enabled:
                    rospy.logerr("Joint torque example failed to meet "
                            "specified control rate timeout.")
                    break
            self._update_forces()
            control_rate.sleep()