Python JointState.position[:]方法代码示例

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[:] [as 别名]
 def joint_goal(self):
     rospy.loginfo(rospy.get_caller_id() + " -> starting joint goal")
     # set in position joint mode
     self.set_state_block("DVRK_POSITION_GOAL_JOINT")
     # get current position
     initial_joint_position = []
     initial_joint_position[:] = self._position_joint_desired
     rospy.loginfo(
         rospy.get_caller_id() + " -> testing goal joint position for 2 joints of %i", len(initial_joint_position)
     )
     amplitude = math.radians(10.0)
     # create a new goal starting with current position
     goal = JointState()
     goal.position[:] = initial_joint_position
     # first motion
     goal.position[0] = initial_joint_position[0] + amplitude
     goal.position[1] = initial_joint_position[1] - amplitude
     self.set_position_goal_joint_publish_and_wait(goal)
     # second motion
     goal.position[0] = initial_joint_position[0] - amplitude
     goal.position[1] = initial_joint_position[1] + amplitude
     self.set_position_goal_joint_publish_and_wait(goal)
     # back to initial position
     goal.position[:] = initial_joint_position
     self.set_position_goal_joint_publish_and_wait(goal)
     rospy.loginfo(rospy.get_caller_id() + " <- joint goal complete")

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[:] [as 别名]
    def move_joint(self, end_joint):
        """Move the arm to the end vector, this only works with the SUJs in simulated mode.

        :param end_joint: the list of joints in which you should conclude movement
        # go to that position directly"""
        joint_state = JointState()
        joint_state.position[:] = end_joint.flat
        self.__set_position_joint_pub.publish(joint_state)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[:] [as 别名]
 def set_position_goal_joint(self, goal):
     self._goal_reached_event.clear()
     self._goal_reached = False
     joint_state = JointState()
     joint_state.position[:] = goal
     self._set_position_goal_joint_publisher.publish(joint_state)
     self._goal_reached_event.wait(180) # 3 minutes at most
     if not self._goal_reached:
         rospy.signal_shutdown('failed to reach goal')
         sys.exit(-1)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[:] [as 别名]
    def __move_joint_goal(self, end_joint):
        """Move the arm to the end vector by bypassing the trajectory generator.

        :param end_joint: the list of joints in which you should conclude movement
        :returns: true if you had succesfully move
        :rtype: Bool"""
        if (not self.__dvrk_set_state('DVRK_POSITION_GOAL_JOINT')):
            return False
        joint_state = JointState()
        joint_state.position[:] = end_joint.flat
        return self.__set_position_goal_joint_publish_and_wait(joint_state)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[:] [as 别名]
    def __move_joint_direct(self, end_joint):
        """Move the arm to the end vector by passing the trajectory generator.

        :param end_joint: the list of joints in which you should conclude movement
        :returns: true if you had succesfully move
        :rtype: Bool"""
        # go to that position directly
        joint_state = JointState()
        joint_state.position[:] = end_joint.flat
        self.__set_position_joint_pub.publish(joint_state)
        return True

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[:] [as 别名]
 def __move_joint_goal(self, end_joint):
     """Move the robot to the end vector by bypassing the trajectory generator.
     :param end_joint: the list of joints in which you should conclude movement
     :returns: true if you had succesfully move
     :rtype: Bool"""
     rospy.loginfo(rospy.get_caller_id() + ' -> starting move joint goal')
     if (self.__check_input_type(end_joint, [list,float])):
         if (not self.__dvrk_set_state('DVRK_POSITION_GOAL_JOINT')):
             return False
         joint_state = JointState()
         joint_state.position[:] = end_joint
         self.__set_position_goal_joint_publish_and_wait(joint_state)
         return True

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[:] [as 别名]
    def __move_joint_goal(self, end_joint, blocking):
        """Move the arm to the end vector by bypassing the trajectory generator.

        :param end_joint: the list of joints in which you should conclude movement
        :returns: true if you had succesfully move
        :rtype: Bool"""
        joint_state = JointState()
        joint_state.position[:] = end_joint.flat
        if blocking:
            return self.__set_position_goal_joint_publish_and_wait(joint_state)
        else:
            self.__set_position_goal_joint_pub.publish(joint_state)
        return True

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[:] [as 别名]
 def __move_joint_direct(self, end_joint):
     """Move the robot to the end vector by passing the trajectory generator.
     :param end_joint: the list of joints in which you should conclude movement
     :returns: true if you had succesfully move
     :rtype: Bool"""
     rospy.loginfo(rospy.get_caller_id() + ' -> starting move joint direct')
     if (self.__check_input_type(end_joint, [list,float])):
         if not self.__dvrk_set_state('DVRK_POSITION_JOINT'):
             return False
         # go to that position directly
         joint_state = JointState()
         joint_state.position[:] = end_joint
         self.set_position_joint.publish(joint_state)
         rospy.loginfo(rospy.get_caller_id() + ' <- completing move joint direct')
         return True

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[:] [as 别名]
 def prepare_cartesian(self):
     # make sure the camera is past the cannula and tool vertical
     initial_joint_position = self._position_joint_desired
     if ((self._robot_name == 'PSM1') or (self._robot_name == 'PSM2') or (self._robot_name == 'PSM3') or (self._robot_name == 'ECM')):
         # set in position joint mode
         self.set_state_block(state = 'DVRK_POSITION_GOAL_JOINT')
             # create a new goal starting with current position
         goal = JointState()
         goal.position[:] = initial_joint_position
         goal.position[0] = 0.0
         goal.position[1] = 0.0
         goal.position[2] = 0.12
         self._goal_reached_event.clear()
         self.set_position_goal_joint.publish(goal)
         self._goal_reached_event.wait(60) # 1 minute at most
         if not self._goal_reached:
             rospy.signal_shutdown('failed to reach goal')
             sys.exit(-1)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[:] [as 别名]
 def joint_direct(self):
     rospy.loginfo(rospy.get_caller_id() + ' -> starting joint direct')
     # set in position joint mode
     self.set_state_block('DVRK_POSITION_JOINT')
     # get current position
     initial_joint_position = []
     initial_joint_position[:] = self._position_joint_desired
     rospy.loginfo(rospy.get_caller_id() + " -> testing direct joint position for 2 joints of %i", len(initial_joint_position))
     amplitude = math.radians(10.0) # +/- 10 degrees
     duration = 5  # seconds
     rate = 200 # aiming for 200 Hz
     samples = duration * rate
     # create a new goal starting with current position
     goal = JointState()
     goal.position[:] = initial_joint_position
     for i in xrange(samples):
         goal.position[0] = initial_joint_position[0] + amplitude *  math.sin(i * math.radians(360.0) / samples)
         goal.position[1] = initial_joint_position[1] + amplitude *  math.sin(i * math.radians(360.0) / samples)
         self.set_position_joint.publish(goal)
         rospy.sleep(1.0 / rate)
     rospy.loginfo(rospy.get_caller_id() + ' <- joint direct complete')