Python JointValue.joint_uri方法代码示例

# 需要导入模块: from brics_actuator.msg import JointValue [as 别名]
# 或者: from brics_actuator.msg.JointValue import joint_uri [as 别名]
def publish_to_youbot(jointState):
    pub = rospy.Publisher('arm_1/arm_controller/position_command', JointPositions)
    rospy.sleep(0.5) 
    try:
        jp = JointPositions()
        
        jv1 = JointValue()
        jv1.joint_uri = jointState.name[0]
        jv1.value = jointState.position[0]
        jv1.unit = "rad"
        
        jv2 = JointValue()
        jv2.joint_uri = jointState.name[1]
        jv2.value = jointState.position[1]
        jv2.unit = "rad"

        jv3 = JointValue()
        jv3.joint_uri = jointState.name[2]
        jv3.value = jointState.position[2]
        jv3.unit = "rad"
        
        jv4 = JointValue()
        jv4.joint_uri = jointState.name[3]
        jv4.value = jointState.position[3]
        jv4.unit = "rad"
        
        jv5 = JointValue()
        jv5.joint_uri = jointState.name[4]
        jv5.value = jointState.position[4]
        jv5.unit = "rad"
        
        p = Poison()
        #print p
       
        jp.poisonStamp = p
        
        jp.positions = [jv1, jv2, jv3, jv4, jv5]
        
        pub.publish(jp)

        return 'arm moved successfully'

    except Exception, e:
        print e
        return 'arm move failure'

# 需要导入模块: from brics_actuator.msg import JointValue [as 别名]
# 或者: from brics_actuator.msg.JointValue import joint_uri [as 别名]
def make_grip_msg(opening_mm, joint_uri):
    
    left = opening_mm/2000.
    right = opening_mm/2000.
    # create joint positions message
    jp = JointPositions()
    
    # create joint values message for both left and right fingers
    jvl = JointValue()
    jvr = JointValue()
    
    # Fill in the gripper positions desired
    # This is open position (max opening 0.0115 m)
    jvl.joint_uri = joint_uri[5]
    jvl.unit = 'm'
    jvl.value = left
    jvr.joint_uri = joint_uri[6]
    jvr.unit = 'm'
    jvr.value = right
     
    # Append those onto JointPositions
    jp.positions.append(copy.deepcopy(jvl))
    jp.positions.append(copy.deepcopy(jvr))
    
    return jp

# 需要导入模块: from brics_actuator.msg import JointValue [as 别名]
# 或者: from brics_actuator.msg.JointValue import joint_uri [as 别名]
def make_brics_msg_gripper(opening_m):
    # Turn a desired gripper opening into a brics-friendly message    
    left = opening_m/2.
    right = opening_m/2.
    # create joint positions message
    jp = JointPositions()

    # create joint values message for both left and right fingers
    jvl = JointValue()
    jvr = JointValue()

    # Fill in the gripper positions desired
    # This is open position (max opening 0.0115 m)
    jvl.joint_uri = 'gripper_finger_joint_l'
    jvl.unit = 'm'
    jvl.value = left
    jvr.joint_uri = 'gripper_finger_joint_r'
    jvr.unit = 'm'
    jvr.value = right

    # Append those onto JointPositions
    jp.positions.append(copy.deepcopy(jvl))
    jp.positions.append(copy.deepcopy(jvr))

    return jp

# 需要导入模块: from brics_actuator.msg import JointValue [as 别名]
# 或者: from brics_actuator.msg.JointValue import joint_uri [as 别名]
def to_joint_positions(gripper_positions):
    robot = os.getenv("ROBOT")
    if robot == "youbot-edufill":
        left_gripper_val = gripper_positions[0]
        right_gripper_val = gripper_positions[1]
        pub = rospy.Publisher("/arm_1/gripper_controller/position_command", JointPositions)
        rospy.sleep(0.5)
        try:
            jp = JointPositions()
            jv1 = JointValue()
            jv1.joint_uri = "gripper_finger_joint_l"
            jv1.value = left_gripper_val
            jv1.unit = "m"
            jv2 = JointValue()
            jv2.joint_uri = "gripper_finger_joint_r"
            jv2.value = right_gripper_val
            jv2.unit = "m"
            p = Poison()
            jp.poisonStamp = p
            jp.positions = [jv1, jv2]  # list
            pub.publish(jp)
            rospy.sleep(1.0)
            rospy.loginfo("Gripper control command published successfully")
        except Exception, e:
            rospy.loginfo("%s", e)

# 需要导入模块: from brics_actuator.msg import JointValue [as 别名]
# 或者: from brics_actuator.msg.JointValue import joint_uri [as 别名]
 def update_gripper_position(self, data):
     u"""Открывает/закрывает гриппер."""
     self.gripper_position.positions = []
     # Open gripper
     if data.buttons[6]:
         tmp_gripper_position_r = JointValue()
         tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r'
         tmp_gripper_position_r.value = 0.011499
         tmp_gripper_position_r.unit = 'm'
         tmp_gripper_position_r.timeStamp = rospy.Time.now()
         self.gripper_position.positions.append(tmp_gripper_position_r)
         tmp_gripper_position_l = JointValue()
         tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l'
         tmp_gripper_position_l.value = 0.011499
         tmp_gripper_position_l.unit = 'm'
         tmp_gripper_position_l.timeStamp = rospy.Time.now()
         self.gripper_position.positions.append(tmp_gripper_position_l)
     # Close gripper
     if data.buttons[7]:
         tmp_gripper_position_r = JointValue()
         tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r'
         tmp_gripper_position_r.value = 0
         tmp_gripper_position_r.unit = 'm'
         tmp_gripper_position_r.timeStamp = rospy.Time.now()
         self.gripper_position.positions.append(tmp_gripper_position_r)
         tmp_gripper_position_l = JointValue()
         tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l'
         tmp_gripper_position_l.value = 0
         tmp_gripper_position_l.unit = 'm'
         tmp_gripper_position_l.timeStamp = rospy.Time.now()
         self.gripper_position.positions.append(tmp_gripper_position_l)

# 需要导入模块: from brics_actuator.msg import JointValue [as 别名]
# 或者: from brics_actuator.msg.JointValue import joint_uri [as 别名]
def jointStatesReceivedHandler(jointstates):
	global crntJointPositions
	global crntJointStates
	global gripperPositions
	crntJointPositions = JointPositions()
	gripperPositions = JointPositions()
	armStatesReceived = False
	if jointstates.name[0] == 'arm_joint_1':
		armStatesReceived = True
	if armStatesReceived == True:
		crntJointStates = copy.deepcopy(jointstates)
		for index in range(0, len(jointstates.position)-2):
			pos = JointValue()
			pos.joint_uri = "arm_joint_" + str(index + 1)
			pos.unit = "rad"
			pos.value = jointstates.position[index]
			crntJointPositions.positions.append(pos)
		pos = JointValue()
		pos.joint_uri = "gripper_finger_joint_l"
		pos.unit = "m"
		pos.value = jointstates.position[5]
		gripperPositions.positions.append(pos)

		pos = JointValue()
		pos.joint_uri = "gripper_finger_joint_r"
		pos.unit = "m"
		pos.value = jointstates.position[6]
		gripperPositions.positions.append(pos)

# 需要导入模块: from brics_actuator.msg import JointValue [as 别名]
# 或者: from brics_actuator.msg.JointValue import joint_uri [as 别名]
def joint_velocities(joint_velocities):
		robot = os.getenv('ROBOT')
		if robot == 'youbot-edufill':
				joint_velocity_1 = joint_velocities[0]
				joint_velocity_2 = joint_velocities[1]
				joint_velocity_3 = joint_velocities[2]
				joint_velocity_4 = joint_velocities[3]
				joint_velocity_5 = joint_velocities[4]
				pub = rospy.Publisher('arm_1/arm_controller/velocity_command', JointVelocities)
				rospy.sleep(0.5) 
				try:
						jv = JointVelocities()

						jv1 = JointValue()
						jv1.joint_uri = "arm_joint_1"
						jv1.value = joint_velocity_1
						jv1.unit = "s^-1 rad"

						jv2 = JointValue()
						jv2.joint_uri = "arm_joint_2"
						jv2.value = joint_velocity_2
						jv2.unit = "s^-1 rad"

						jv3 = JointValue()
						jv3.joint_uri = "arm_joint_3"
						jv3.value = joint_velocity_3
						jv3.unit = "s^-1 rad"

						jv4 = JointValue()
						jv4.joint_uri = "arm_joint_4"
						jv4.value = joint_velocity_4
						jv4.unit = "s^-1 rad"

						jv5 = JointValue()
						jv5.joint_uri = "arm_joint_5"
						jv5.value = joint_velocity_5
						jv5.unit = "s^-1 rad"

						p = Poison()
						#print p

						jv.poisonStamp = p

						jv.velocities = [jv1, jv2, jv3, jv4, jv5]

						pub.publish(jv)

						return 'arm moved successfully'

				except Exception, e:
						print e
						return 'arm move failure'

# 需要导入模块: from brics_actuator.msg import JointValue [as 别名]
# 或者: from brics_actuator.msg.JointValue import joint_uri [as 别名]
 def make_brics_msg_arm(cls, arm_num, positions):
     # create joint positions message
     jp = JointPositions()
     for ii in range(5):
         jv = JointValue()
         if arm_num == 1:
             jv.joint_uri = 'arm_joint_' + str(ii+1)
         else:
             jv.joint_uri = 'arm_' + str(arm_num) + '_joint_' + str(ii+1)
         jv.unit='rad' 
         jv.value = positions[ii]
         jp.positions.append(copy.deepcopy(jv))
     return jp

# 需要导入模块: from brics_actuator.msg import JointValue [as 别名]
# 或者: from brics_actuator.msg.JointValue import joint_uri [as 别名]
def create_gripper_msg (left_gripper, right_gripper):
  jp = JointPositions()
  jv1 = JointValue()
  jv1.joint_uri = "gripper_finger_joint_l"
  jv1.value = left_gripper
  jv1.unit = "m"
  jv2 = JointValue()
  jv2.joint_uri = "gripper_finger_joint_r"
  jv2.value = right_gripper
  jv2.unit = "m"
  p = Poison()
  jp.poisonStamp = p

  jp.positions = [jv1, jv2]

  return jp

# 需要导入模块: from brics_actuator.msg import JointValue [as 别名]
# 或者: from brics_actuator.msg.JointValue import joint_uri [as 别名]
 def Set(self,value):
     grp_cmd = JointPositions()
     j_cmd = JointValue()
     j_cmd.joint_uri = self.joint_name
     j_cmd.unit = 'm'
     j_cmd.value = value
     grp_cmd.positions.append(j_cmd)
     print "Sending gripper command: " + str(j_cmd.value)
     self._GripperCmdPublisher.publish(grp_cmd)

# 需要导入模块: from brics_actuator.msg import JointValue [as 别名]
# 或者: from brics_actuator.msg.JointValue import joint_uri [as 别名]
    def _createGripperJointPositions(self, left, right):
        jp = JointPositions()

        jv1 = JointValue()
        jv1.joint_uri = "gripper_finger_joint_l"
        jv1.value = left
        jv1.unit = "m"

        jv2 = JointValue()
        jv2.joint_uri = "gripper_finger_joint_r"
        jv2.value = right
        jv2.unit = "m"

        p = Poison()
        jp.poisonStamp = p

        jp.positions = [jv1, jv2] #list

        return jp

# 需要导入模块: from brics_actuator.msg import JointValue [as 别名]
# 或者: from brics_actuator.msg.JointValue import joint_uri [as 别名]
 def _createJointPositions(self, joint_config):
     jp = JointPositions()
     
     for i in range(5):
         jv = JointValue()
         jv.joint_uri = self.joint_names[i]
         jv.value = joint_config[i]
         jv.unit = "rad"
         jp.positions.append(jv)
     
     return jp

# 需要导入模块: from brics_actuator.msg import JointValue [as 别名]
# 或者: from brics_actuator.msg.JointValue import joint_uri [as 别名]
    def publish_gripper_width(self, width):
                  
        desiredPositions = JointPositions()

        jointCommands = []

        joint = JointValue()
        joint.joint_uri = "gripper_finger_joint_l"
        joint.unit = "m"
        joint.value = width
        jointCommands.append(joint)

        joint = JointValue()
        joint.joint_uri = "gripper_finger_joint_r"
        joint.unit = "m"
        joint.value = width
        jointCommands.append(joint)

        desiredPositions.positions = jointCommands

        self.gripper_pub.publish(desiredPositions)

# 需要导入模块: from brics_actuator.msg import JointValue [as 别名]
# 或者: from brics_actuator.msg.JointValue import joint_uri [as 别名]
 def moveGripper(self, gPublisher, floatVal):
     jp = JointPositions()
     myPoison = Poison()
     myPoison.originator = 'yb_grip'
     myPoison.description = 'whoknows'
     myPoison.qos = 0.0
     jp.poisonStamp = myPoison
     nowTime = rospy.Time.now()
     jvl = JointValue()
     jvl.timeStamp = nowTime
     jvl.joint_uri = 'gripper_finger_joint_l'
     jvl.unit = 'm'
     jvl.value = floatVal
     jp.positions.append(jvl)
     jvr = JointValue()
     jvr.timeStamp = nowTime
     jvr.joint_uri = 'gripper_finger_joint_r'
     jvr.unit = 'm'
     jvr.value = floatVal
     jp.positions.append(jvr)
     gPublisher.publish(jp)

# 需要导入模块: from brics_actuator.msg import JointValue [as 别名]
# 或者: from brics_actuator.msg.JointValue import joint_uri [as 别名]
def make_arm_msg(arm_js, joint_uri):
    
    A1 = arm_js[0]
    A2 = arm_js[1]
    A3 = arm_js[2]
    A4 = arm_js[3]
    A5 = arm_js[4] 
    # create joint positions message
    jp = JointPositions()
    # create joint values message for all the joints
    jv1 = JointValue()
    jv2 = JointValue()
    jv3 = JointValue()
    jv4 = JointValue()
    jv5 = JointValue()
    # Fill in the arm positions. 
    jv1.joint_uri = joint_uri[0]
    jv1.unit = 'rad'
    jv1.value = A1
    jv2.joint_uri = joint_uri[1]
    jv2.unit = 'rad'
    jv2.value = A2
    jv3.joint_uri = joint_uri[2]
    jv3.unit = 'rad'
    jv3.value = A3
    jv4.joint_uri = joint_uri[3]
    jv4.unit = 'rad'
    jv4.value = A4
    jv5.joint_uri = joint_uri[4]
    jv5.unit = 'rad'
    jv5.value = A5
    # Append those onto JointPositions
    jp.positions.append(copy.deepcopy(jv1))
    jp.positions.append(copy.deepcopy(jv2))
    jp.positions.append(copy.deepcopy(jv3))
    jp.positions.append(copy.deepcopy(jv4))
    jp.positions.append(copy.deepcopy(jv5))
    
    return jp