hands.py 文件源码

def set_arm_configuration(self, sidename, joints, movetime = None):
        if sidename == 'left':
            side = ArmTrajectoryRosMessage.LEFT
        elif sidename == 'right':
            side = ArmTrajectoryRosMessage.RIGHT
        else:
            rospy.logerr("Unknown side {}".format(sidename))
            return

        if movetime is None:
            movetime = self.ARM_MOVE_TIME

        for i in range(len(joints)):
            if math.isnan(joints[i]):
                joints[i] = self.last_arms[side][i]

        msg = self.make_arm_trajectory(side, ArmTrajectoryRosMessage.OVERRIDE, movetime, joints)
        self.last_arms[side] = deepcopy(joints)
        log('Setting {} arm to {}'.format(sidename, joints))
        self.arm_trajectory_publisher.publish(msg)
        rospy.sleep(movetime)