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)
示例#2
0
	def get_joint_velocities(self, dx, dy):
		if (not self.armInitialized):
			dx = 0
			dy = 0

			mvx = 0
			mvy = 0

		else:

			mvx = self.maxVelX;
			mvy = self.maxVelY;

			if (dx>0):
				if self.jointConstraints[0][1]-self.jointPositions[0]<self.threshold:
					mvx = (self.jointConstraints[0][1]-self.jointPositions[0])*self.maxVelX;
					#print self.jointPositions[0], "mvx\t", mvx

			else:
				if -self.jointConstraints[0][0]+self.jointPositions[0]<self.threshold:
					mvx = (-self.jointConstraints[0][0]+self.jointPositions[0])*self.maxVelX;
					#print self.jointPositions[0], "mvx\t", mvx

			#print self.jointConstraints[1], "\t", self.jointPositions[1]#, "\t",  (self.jointConstraints[1][1]-self.jointPositions[1]) 
			if (dy>0):
				if self.jointConstraints[1][1]-self.jointPositions[1]<self.threshold:
					mvy = (self.jointConstraints[1][1]-self.jointPositions[1])*self.maxVelY;
					#print self.jointPositions[1], "mvy\t", mvy
			else:
				if -self.jointConstraints[1][0]+self.jointPositions[1]<self.threshold:
					mvy = (-self.jointConstraints[1][0]+self.jointPositions[1])*self.maxVelY;
					#print self.jointPositions[1], "mvy\t", mvy

			

		jv = JointVelocities()
		jv.velocities.append(JointValue())
		jv.velocities[-1].unit = "s^-1 rad"
		jv.velocities[-1].joint_uri = 'arm_joint_1'
		jv.velocities[-1].value = dx*mvx;
		jv.velocities.append(JointValue())
		jv.velocities[-1].unit = "s^-1 rad"
		jv.velocities[-1].joint_uri = 'arm_joint_2'
		jv.velocities[-1].value = dy*mvy;
		jv.velocities.append(JointValue())
		jv.velocities[-1].unit = "s^-1 rad"
		jv.velocities[-1].joint_uri = 'arm_joint_3'
		jv.velocities[-1].value = 0;
		jv.velocities.append(JointValue())
		jv.velocities[-1].unit = "s^-1 rad"
		jv.velocities[-1].joint_uri = 'arm_joint_4'
		jv.velocities[-1].value = 0;
		jv.velocities.append(JointValue())
		jv.velocities[-1].unit = "s^-1 rad"
		jv.velocities[-1].joint_uri = 'arm_joint_5'
		jv.velocities[-1].value = 0;



		return jv
    def jsPoscontrol(self, qd):

        #Initialize Position Command Msgs
        qposMsg = JointPositions()
        qposMsg.positions = [
            JointValue(),
            JointValue(),
            JointValue(),
            JointValue(),
            JointValue()
        ]

        #Fill Position Msg
        qposMsg.positions[0].joint_uri = 'arm_joint_1'
        qposMsg.positions[0].unit = 'rad'
        qposMsg.positions[0].value = qd[0] + self.q_offset[0]

        qposMsg.positions[1].joint_uri = 'arm_joint_2'
        qposMsg.positions[1].unit = 'rad'
        qposMsg.positions[1].value = qd[1] + self.q_offset[1]
        qposMsg.positions[2].joint_uri = 'arm_joint_3'
        qposMsg.positions[2].unit = 'rad'
        qposMsg.positions[2].value = qd[2] + self.q_offset[2]

        qposMsg.positions[3].joint_uri = 'arm_joint_4'
        qposMsg.positions[3].unit = 'rad'
        qposMsg.positions[3].value = qd[3] + self.q_offset[3]

        qposMsg.positions[4].joint_uri = 'arm_joint_5'
        qposMsg.positions[4].unit = 'rad'
        qposMsg.positions[4].value = qd[4] + self.q_offset[4]

        #Publish Position Msg
        self.pub_qpos.publish(qposMsg)
示例#4
0
    def make_brics_msg_gripper(cls, arm_num, opening_m):
        
        # Turn a desired gripper opening into a brics-friendly message    
        left = opening_m/2.0
        right = opening_m/2.0
        # 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)
        if arm_num == 1:
            jvl.joint_uri = 'gripper_finger_joint_l'
            jvr.joint_uri = 'gripper_finger_joint_r'
        else:
            jvl.joint_uri = 'gripper_' + str(arm_num) + '_finger_joint_l'
            jvr.joint_uri = 'gripper_' + str(arm_num) + '_finger_joint_r'
        jvl.unit = 'm'
        jvl.value = left
        jvr.unit = 'm'
        jvr.value = right

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

        return jp    
示例#5
0
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
示例#6
0
def gripperCommand(data):
    '''moves the gripper to a predefined configuration.'''

    rospy.loginfo(rospy.get_caller_id() + "gripper data: %s", data.data)

    #creating a JointValue() List/Array
    jvaluegrippers = [0 for i in range(2)]

    #creating a JointValue() Object for gripper left
    jvalue6 = JointValue()
    jvalue6.joint_uri = 'gripper_finger_joint_l'
    jvalue6.unit = 'm'
    jvalue6.value = data.data[0]
    jvaluegrippers[0] = jvalue6

    #creating a JointValue() Object for gripper right
    jvalue7 = JointValue()
    jvalue7.joint_uri = 'gripper_finger_joint_r'
    jvalue7.unit = 'm'
    jvalue7.value = data.data[1]
    jvaluegrippers[1] = jvalue7

    #setting JointPosition msg properties
    rate = rospy.Rate(20)
    msg2 = JointPositions()
    msg2.positions = jvaluegrippers
    pubGripper.publish(msg2)
示例#7
0
    def get_zero_velocities(self):
        jv = JointVelocities()
        jv.velocities.append(JointValue())

        jv.velocities[-1].unit = "s^-1 rad"
        jv.velocities[-1].joint_uri = 'arm_joint_1'
        jv.velocities[-1].value = 0

        jv.velocities.append(JointValue())
        jv.velocities[-1].unit = "s^-1 rad"
        jv.velocities[-1].joint_uri = 'arm_joint_2'
        jv.velocities[-1].value = 0

        jv.velocities.append(JointValue())
        jv.velocities[-1].unit = "s^-1 rad"
        jv.velocities[-1].joint_uri = 'arm_joint_3'
        jv.velocities[-1].value = 0

        jv.velocities.append(JointValue())
        jv.velocities[-1].unit = "s^-1 rad"
        jv.velocities[-1].joint_uri = 'arm_joint_4'
        jv.velocities[-1].value = 0

        jv.velocities.append(JointValue())
        jv.velocities[-1].unit = "s^-1 rad"
        jv.velocities[-1].joint_uri = 'arm_joint_5'
        jv.velocities[-1].value = 0

        return jv
示例#8
0
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
示例#9
0
def main():
    pubPos = rospy.Publisher('/arm_1/arm_controller/position_command',
                             JointPositions,
                             queue_size=10)
    pubVel = rospy.Publisher('/arm_1/arm_controller/velocity_command',
                             JointVelocities,
                             queue_size=10)

    rospy.init_node('armcommand', anonymous=True)
    rate = rospy.Rate(1)  # 10hz

    jp = JointPositions()
    joints = []
    for i in range(0, 5):
        joints.append(JointValue())
        joints[i].joint_uri = "arm_joint_" + str(i + 1)
        joints[i].unit = "rad"

    jv = JointVelocities()
    vels = []
    for i in range(0, 5):
        vels.append(JointValue())
        vels[i].joint_uri = "arm_joint_" + str(i + 1)
        vels[i].unit = "s^-1 rad"

    joints[0].value = 3
    joints[1].value = 1
    joints[2].value = -1
    joints[3].value = 1.85
    joints[4].value = 3

    j = 1
    c = 1
    inverse = False
    while not rospy.is_shutdown():

        if not inverse:
            j = j + 1

        if inverse:
            j = j - 1

        if j > 50:
            inverse = True
            c = -c

        joints[0].value += 0
        joints[1].value += c * 0.008 * j
        joints[2].value += -c * 0.008 * j
        joints[3].value += 0
        joints[4].value += 0

        jp.positions = joints

        pubPos.publish(jp)

        rate.sleep()
示例#10
0
 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)
 def create_joint_value_msg_list(self, joint_values, unit):
     joint_value_msg_list = []
     for i, joint_value in enumerate(joint_values):
         joint_value_msg = JointValue()
         joint_value_msg.joint_uri = 'arm_joint_' + str(i + 1)
         joint_value_msg.unit = unit
         joint_value_msg.value = joint_value
         joint_value_msg_list.append(joint_value_msg)
     return joint_value_msg_list
示例#12
0
def make_brics_msg_arm(positions):
    # create joint positions message
    jp = JointPositions()
    for ii in range(5):
        jv = JointValue()
        jv.joint_uri = 'arm_joint_' + str(ii+1)
        jv.unit='rad' 
        jv.value = positions[ii]
        jp.positions.append(copy.deepcopy(jv))
    return jp
示例#13
0
def create_null_velocity(unit):
    msg = JointVelocities()
    time = rospy.Time.now()
    for joint_name in joint_names:
        j = JointValue()
        j.timeStamp = time
        j.joint_uri = joint_name
        j.unit = unit
        msg.velocities.append(j)
    return msg
示例#14
0
def main():

    rospy.init_node('armcommand', anonymous=True)
    pubPos = rospy.Publisher('/arm_1/arm_controller/position_command', JointPositions, queue_size=10)
    pubVel = rospy.Publisher('/arm_1/arm_controller/velocity_command', JointVelocities, queue_size=10)
    rospy.sleep(2) #wait for connection
    
    rate = rospy.Rate(70) # 10hz
    
    #initialise positions
    jp = JointPositions()
    joints = []
    for i in range(0, 5):
         joints.append(JointValue())
	 joints[i].joint_uri = "arm_joint_" + str(i+1)
         joints[i].unit = "rad"
    
    #initialise velocities
    jv = JointVelocities()
    vels = []
    for i in range(0, 5):
         vels.append(JointValue())
	 vels[i].joint_uri = "arm_joint_" + str(i+1)
         vels[i].unit = "s^-1 rad"
    
    #go to home position
    for i in range(0,5):
         joints[i].value = 0.05

    joints[2].value = -joints[2].value
    jp.positions = joints;
    pubPos.publish(jp)
    rospy.sleep(5)


    #start main movement
    j = 1
    while not rospy.is_shutdown():
	 for i in range(0,5):
             joints[i].value = 0.008*j 

	 for i in range(0,5):
             vels[i].value = 0.0001*j + abs(random.random() * 0.05)

	 joints[2].value = -joints[2].value
         vels[2].value = -vels[2].value

         jp.positions = joints;
         jv.velocities = vels;

         #pubPos.publish(jp)
	 pubVel.publish(jv)

         j = j+1
         rate.sleep()
示例#15
0
def createGripperPositionCommand(newPos, t):
    msg = JointPositions()
    joint = JointValue(timeStamp=t,
                       value=newPos,
                       unit="m",
                       joint_uri="gripper_finger_joint_l")
    msg.positions.append(joint)
    joint.joint_uri = "gripper_finger_joint_r"
    msg.positions.append(joint)

    return msg
 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
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)
示例#18
0
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
示例#19
0
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
	def execute(self, goal):
		rospy.logdebug("Executing command goal: " + str(goal))
		jnt_pos = JointPositions()
		
		for k in range(len(self.joint_names)):
			value = JointValue()
			value.joint_uri = self.joint_names[k]
			value.unit = "m"
			value.value = goal.command.position
			jnt_pos.positions.append(value)
		
		self.pub_joint_positions.publish(jnt_pos)
		
		rospy.sleep(2.0)
		
		self.server.set_succeeded()
    def arm_up_recover(self):
        arm_up = create_arm_up()
        arm_up[0] = self.configuration[0]
        arm_up[4] = self.configuration[4]

        joint_positions = JointPositions()

        # transform from ROS to BRICS message
        for j in range(len(joint_names)):
            joint_value = JointValue()
            joint_value.joint_uri = joint_names[j]
            joint_value.value = limit_joint(joint_names[j], arm_up[j])
            joint_value.unit = self.unit
            joint_positions.positions.append(joint_value)
        self.position_pub.publish(joint_positions)
        rospy.sleep(1)
示例#22
0
    def arm_up_recover(self):
        arm_up = create_arm_up()
        arm_up[0] = self.configuration[0]
        arm_up[4] = self.configuration[4]

        joint_positions = JointPositions()

        # transform from ROS to BRICS message
        for j in range(len(joint_names)):
            joint_value = JointValue()
            joint_value.joint_uri = joint_names[j]
            joint_value.value = limit_joint(joint_names[j], arm_up[j])
            joint_value.unit = self.unit
            joint_positions.positions.append(joint_value)
        self.position_pub.publish(joint_positions)
        rospy.sleep(1)
def main():
    pubPos = rospy.Publisher('/arm_1/arm_controller/position_command',
                             JointPositions,
                             queue_size=10)

    rospy.init_node('armcommand', anonymous=True)
    rate = rospy.Rate(1)  # 10hz

    jp = JointPositions()
    joints = []
    for i in range(0, 5):
        joints.append(JointValue())
        joints[i].joint_uri = "arm_joint_" + str(i + 1)
        joints[i].unit = "rad"

    while not rospy.is_shutdown():
        joints[0].value = 3
        joints[1].value = 1
        joints[2].value = -1
        joints[3].value = 1.85
        joints[4].value = 3

        jp.positions = joints

        pubPos.publish(jp)

        rate.sleep()
    def get_joint_pos_msg_from_joint_angles(joint_angles):
        """
        Converts a list of joint angles to a JointPositions message

        :joint_angles: list(float)
        :returns: brics_actuator.JointPositions

        """
        joint_position_msg = JointPositions()
        for i, joint_angle in enumerate(joint_angles):
            joint_value_msg = JointValue()
            joint_value_msg.joint_uri = 'arm_joint_' + str(i + 1)
            joint_value_msg.unit = 'rad'
            joint_value_msg.value = joint_angle
            joint_position_msg.positions.append(joint_value_msg)
        return joint_position_msg
示例#25
0
def velPub():
    rospy.init_node("velPub", anonymous=True)

    component_name = "arm"
    component_dof = 7

    pub = rospy.Publisher("/" + component_name + "_controller/command_vel",
                          JointVelocities,
                          queue_size=1)
    vel = JointVelocities()
    for x in range(0, component_dof):
        vel.velocities.append(JointValue())

    r = rospy.Rate(100)
    t0 = rospy.get_time()
    A = 0.1  #amplitude
    w = 1.0  #frequency
    phi = 0.0  #phase shift

    while not rospy.is_shutdown():
        t = rospy.get_time()
        vel.velocities[1].value = A * math.sin(w * (t - t0) + phi)

        #print vel
        pub.publish(vel)

        r.sleep()
示例#26
0
def poseCallback(simple_msg):
    hard_msg = JointPositions()
    for i in range(5):
        hard_msg.positions.append(JointValue())
        hard_msg.positions[i].joint_uri = 'arm_joint_' + str(i + 1)
        hard_msg.positions[i].unit = 'rad'
        hard_msg.positions[i].value = simple_msg.data[i]
    pose_pub.publish(hard_msg)
    def publish_arm_joint_positions(self, joint_positions):

        desiredPositions = JointPositions()

        jointCommands = []

        for i in range(5):
            joint = JointValue()
            joint.joint_uri = "arm_joint_" + str(i+1)
            joint.unit = "rad"
            joint.value = joint_positions[i]

            jointCommands.append(joint)
            
        desiredPositions.positions = jointCommands

        self.arm_joint_pub.publish(desiredPositions)
 def go_with2(self, torques):
     jt_msg = JointTorques()
     for i in range(N):
         jt_msg.torques.append(JointValue())
         jt_msg.torques[i].joint_uri = 'arm_joint_' + str(i + 1)
         jt_msg.torques[i].unit = 'm^2 kg s^-2 rad^-1'
         jt_msg.torques[i].value = torques[i]
     self.jt_pub.publish(jt_msg)
示例#29
0
    def publish_arm_joint_velocities(self, Joint_Velocities):

        desiredVelocities = JointVelocities()

        jointCommands = []

        for i in range(5):
            joint = JointValue()
            joint.joint_uri = "arm_joint_" + str(i + 1)
            joint.unit = "s^-1 rad"
            joint.value = Joint_Velocities[i]

            jointCommands.append(joint)

        desiredVelocities.velocities = jointCommands

        self.arm_joint_vel_pub.publish(desiredVelocities)
 def go_to(self, position):
     jp_msg = JointPositions()
     for i in range(N):
         jp_msg.positions.append(JointValue())
         jp_msg.positions[i].joint_uri = 'arm_joint_' + str(i + 1)
         jp_msg.positions[i].unit = 'rad'
         jp_msg.positions[i].value = position[i]
     self.jp_pub.publish(jp_msg)
 def go_with(self, velocities):
     jv_msg = JointVelocities()
     for i in range(N):
         jv_msg.velocities.append(JointValue())
         jv_msg.velocities[i].joint_uri = 'arm_joint_' + str(i + 1)
         jv_msg.velocities[i].unit = 's^-1 rad'
         jv_msg.velocities[i].value = velocities[i]
     self.jv_pub.publish(jv_msg)
示例#32
0
    def publish_arm_joint_positions(self, joint_positions):

        desiredPositions = JointPositions()

        jointCommands = []

        for i in range(5):
            joint = JointValue()
            joint.joint_uri = "arm_joint_" + str(i + 1)
            joint.unit = "rad"
            joint.value = joint_positions[i]

            jointCommands.append(joint)

        desiredPositions.positions = jointCommands

        self.arm_joint_pub.publish(desiredPositions)
示例#33
0
def createArmPositionCommand(newArmPosition, t):
    msg = JointPositions()

    for i, val in enumerate(newArmPosition, 1):
        msg.positions.append(
            JointValue(timeStamp=t,
                       value=val * pi / 180,
                       unit="rad",
                       joint_uri="arm_joint_%s" % (i, )))

    return msg
示例#34
0
    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)
示例#35
0
    def set_joints_angles(self, *args):  # TODO: remove *args
        u"""Set arm joints to defined angles in radians.

        Arguments:
            *args -- joints angles (j1, j2, j3, j4, j5)

        Устанавливает углы поворота степеней подвижности в радианах.

        Аргументы:
            *args -- уголы соотвествующих степеней (j1, j2, j3, j4, j5)
        """
        assert len(args) <= 5
        self.joints_positions.positions = []
        for i in range(5):
            tmp = JointValue()
            tmp.timeStamp = rospy.Time.now()
            tmp.joint_uri = 'arm_joint_{}'.format(i + 1)
            tmp.unit = 'rad'
            tmp.value = args[i]
            self.joints_positions.positions.append(tmp)
        self.joints_positions_publisher.publish(self.joints_positions)
 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)
示例#37
0
    def set_joints_velocities(self, *args):  # TODO: remove *args
        u"""Set velocity for each joint.

        Arguments:
            *args -- velocity for each joint (j1, j2, j3, j4, j5)

        Устанавливает скорость каждой степени подвижности в радианах/с.

        Аргументы:
            *args -- скорости соотвествующих степеней (j1, j2, j3, j4, j5)
        """
        assert len(args) == 5
        self.joints_velocities.velocities = []
        for index, value in enumerate(args):
            tmp = JointValue()
            tmp.timeStamp = rospy.Time.now()
            tmp.joint_uri = 'arm_joint_{}'.format(index + 1)
            tmp.unit = 's^-1 rad'
            tmp.value = value
            self.joints_velocities.velocities.append(tmp)
        self.joints_velocities_publisher.publish(self.joints_velocities)
示例#38
0
    def goToPose(self, q):
        jp_msg = JointPositions()
        # for i in range(1):
        # 	jp_msg.positions.append(JointValue())
        # 	jp_msg.positions[i].joint_uri = 'arm_joint_' + str(i+1)
        # 	jp_msg.positions[i].unit = 'rad'

        jp_msg.positions.append(JointValue())
        jp_msg.positions[0].joint_uri = 'arm_joint_5'
        jp_msg.positions[0].unit = 'rad'

        jp_msg.positions[0].value = q
        self.jp_pub.publish(jp_msg)
示例#39
0
    def main(self):
        # rospy.init_node('putitback')
        arm_pub = rospy.Publisher('/arm_1/arm_controller/position_command',
                                  JointPositions,
                                  queue_size=10)
        # arm_ik_control.go_to_xyz(0.1, -0.2, 0.3, arm_pub)

        initial_position.main()

        gripper_open.main()

        msg = JointPositions()
        rospy.sleep(5)
        print "moving joint 1"  #move joint 3 and joint 2
        msg.positions = [JointValue()]
        msg.positions[0].timeStamp = rospy.Time.now()
        msg.positions[0].unit = "rad"
        msg.positions[0].joint_uri = "arm_joint_1"
        msg.positions[0].value = 0.04
        arm_pub.publish(msg)

        rospy.sleep(2)
        msg.positions[0].joint_uri = "arm_joint_5"
        msg.positions[0].value = 2.8
        arm_pub.publish(msg)
        rospy.sleep(2)

        msg.positions[0].joint_uri = "arm_joint_1"
        msg.positions[0].value = 0.01005
        arm_pub.publish(msg)

        rospy.sleep(3)
        print "moving joint 2"
        msg.positions[0].joint_uri = "arm_joint_4"
        msg.positions[0].value = 1.7
        arm_pub.publish(msg)

        rospy.sleep(2)
        msg.positions[0].joint_uri = "arm_joint_3"
        msg.positions[0].value = -0.6
        arm_pub.publish(msg)

        rospy.sleep(1)
        msg.positions[0].joint_uri = "arm_joint_2"
        msg.positions[0].value = 1.4
        arm_pub.publish(msg)

        gripper_close.main()
        rospy.sleep(2)
        initial_position.main()
        arm_ik_control.go_to_xyz_rev(-0.069, -0.447, 0.12, arm_pub)
示例#40
0
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
def make_movements_callback(req):
    global file_with_data

    jp_pub = rospy.Publisher("arm_1/arm_controller/position_command",
                             JointPositions,
                             queue_size=10,
                             latch=True)
    jp_msg = JointPositions()
    for i in range(5):
        jp_msg.positions.append(JointValue())
        jp_msg.positions[i].joint_uri = 'arm_joint_' + str(i + 1)
        jp_msg.positions[i].unit = 'rad'

    file_with_poses = open(fwp_name, "r")
    string_of_poses = file_with_poses.readlines()
    file_with_poses.close()

    js_sub = rospy.Subscriber("joint_states", JointState, js_callback)

    r = rospy.Rate(1.0)
    for pose in string_of_poses:
        angles = [float(angle) for angle in pose.split()]
        for i in range(5):
            jp_msg.positions[i].value = angles[i]
        jp_pub.publish(jp_msg)

        achieved = False
        while not achieved:
            achieved = True
            r.sleep()
            lock.acquire()
            for i in range(5):
                if abs(angles[i] - cur_angles[i]) > 0.2:
                    achieved = False
                    break
            lock.release()

    js_sub.unregister()

    # for returning to startup (embryo) position
    for i in range(5):
        jp_msg.positions[i].value = 0.025
    jp_msg.positions[2].value = -0.02
    jp_msg.positions[4].value = 0.12
    jp_pub.publish(jp_msg)

    lock.acquire()
    file_with_data.close()
    lock.release()
    return EmptyResponse()
    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
    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)
    joint_position_string_pub = rospy.Publisher("/robust_position_controller/event_in", String, queue_size=100)

    joint_position_msg = JointPositions()

    string_msg= String()
    string_msg.data = "e_start"
   

    joint_names = ["arm_joint_1" , "arm_joint_5"]

    joint_value_=[4.0, 4.0]

    
    for i in range (0,len(joint_names)):
        joint_value_msg = JointValue()

        joint_value_msg.joint_uri = joint_names[i]
        
        joint_value_msg.value = joint_value_[i]

        joint_value_msg.unit="rad"

        joint_position_msg.positions.append(joint_value_msg)
    loop_rate = rospy.Rate(10)
        
    while not rospy.is_shutdown():
        joint_position_pub.publish(joint_position_msg)
        rospy.sleep(0.5)
        joint_position_string_pub.publish(string_msg)
        loop_rate.sleep()
import sys, select, termios, tty, signal

if __name__=="__main__":
    
    pub = rospy.Publisher('/arm_1/gripper_controller/position_command', JointPositions)
    rospy.init_node('simple_gripper_joint_position')
    rospy.sleep(0.5)
    
    try:
        jp = JointPositions()
        
        # if value == 0: gripper is closed
        # if value == 0.0115: griper is open
        
        jv1 = JointValue()
        jv1.joint_uri = "gripper_finger_joint_l"
        jv1.value = 0.01
        jv1.unit = "m"
        
        jv2 = JointValue()
        jv2.joint_uri = "gripper_finger_joint_r"
        jv2.value = 0.01
        jv2.unit = "m"
        
        
        p = Poison()
        jp.poisonStamp = p
        
        jp.positions = [jv1, jv2] #list
        
示例#46
0
def main(stdscr, persist):
    pub = rospy.Publisher('out',JointPositions, queue_size=1)
    rospy.init_node('bras_teleop', anonymous=True)
    theta1, theta2, theta3, theta4, theta5 = 0.111,0.11,-0.11,0.11,0.111
    jointvalues = [theta1,theta2,theta3,theta4,theta5]
    keycode = -1
    rate = rospy.Rate(100) 
    stdscr.addstr("Command\n")
    stdscr.addstr("- a/q    : theta1 ++/--\n")
    stdscr.addstr("- z/s    : theta2 ++/--\n")
    stdscr.addstr("- e/d    : theta3 ++/--\n")
    stdscr.addstr("- r/f    : theta4 ++/--\n")
    stdscr.addstr("- t/g    : theta5 ++/--\n")
    stdscr.addstr("- ESC    : exit\n")
    # We will wait for 100 ms for a key to be pressed
    if(persist): stdscr.timeout(100)
    while (not rospy.is_shutdown()) and (keycode != 27): # 27 is escape
        keycode = stdscr.getch() # read pressed key
        if keycode == -1  : pass # No key has been pressed
        elif keycode == ord('a')   : 
            if(jointvalues[0]+0.05> 0.0100692 and jointvalues[0]+0.05<5.84014 ):jointvalues[0]+=0.05
        elif keycode == ord('q')   : 
            if(jointvalues[0]-0.05>0.0100692 and jointvalues[0]-0.05< 5.84014 ):jointvalues[0]-=0.05  
        elif keycode == ord('z')   : 
            if(jointvalues[1]+0.05> 0.0100692 and jointvalues[1]+0.05< 2.61799 ):jointvalues[1]+=0.05
        elif keycode == ord('s')   :
            if(jointvalues[1]-0.05>0.0100692 and jointvalues[1]-0.05<  2.61799  ):jointvalues[1]-=0.05
        elif keycode == ord('e')   :
            if(jointvalues[2]+0.05>-5.02655 and jointvalues[2]+0.05<-0.015708  ):jointvalues[2]+=0.05
        elif keycode == ord('d')   :
            if(jointvalues[2]-0.05>-5.02655 and jointvalues[2]-0.05<-0.015708  ):jointvalues[2]-=0.05
        elif keycode == ord('r')   :
            if(jointvalues[3]+0.05>0.0221239 and jointvalues[3]+0.05<3.4292 ):jointvalues[3]+=0.05
        elif keycode == ord('f')   :
            if(jointvalues[3]-0.05>0.0221239 and jointvalues[3]-0.05<3.4292 ):jointvalues[3]-=0.05
        elif keycode == ord('t')   : jointvalues[4]+=0.05
        elif keycode == ord('g')   : jointvalues[4]-=0.05
        jp = JointPositions()
        jv1 = JointValue()
        jv2 = JointValue()
        jv3 = JointValue()
        jv4 = JointValue()
        jv5 = JointValue()
        jv1.joint_uri ="arm_joint_1"
        jv1.unit = "rad"
        jv1.value =jointvalues[0]
        jv2.joint_uri ="arm_joint_2"
        jv2.unit = "rad"
        jv2.value = jointvalues[1]
        jv3.joint_uri = "arm_joint_3"
        jv3.unit = "rad"
        jv3.value = jointvalues[2]
        jv4.joint_uri = "arm_joint_4"
        jv4.unit = "rad"
        jv4.value = jointvalues[3]
        jv5.joint_uri = "arm_joint_5"
        jv5.unit = "rad"
        jv5.value = jointvalues[4]
        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))
        pub.publish(jp)
        rate.sleep()
示例#47
0
from brics_actuator.msg import JointPositions, JointValue, Poison

import sys, select, termios, tty, signal

if __name__=="__main__":
    
    pub = rospy.Publisher('arm_1/arm_controller/position_command', JointPositions)
    
    rospy.init_node('simple_arm_gripper_position')

    rospy.sleep(0.5)
    
    try:
        jp = JointPositions()
        
        jv1 = JointValue()
        jv1.joint_uri = "arm_joint_1"
        jv1.value = 0.011
        jv1.unit = "rad"
        
        jv2 = JointValue()
        jv2.joint_uri = "arm_joint_2"
        jv2.value = 0.011
        jv2.unit = "rad"

        jv3 = JointValue()
        jv3.joint_uri = "arm_joint_3"
        jv3.value = -0.016
        jv3.unit = "rad"
        
        jv4 = JointValue()
示例#48
0
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


if __name__=="__main__":
    
    pub = rospy.Publisher('arm_1/arm_controller/position_command', JointPositions)
    
    rospy.init_node('simple_arm_gripper_position')

    rospy.sleep(0.5)
    
    try:
        jp = JointPositions()
        
        
        jv1 = JointValue()
        jv1.joint_uri = "arm_joint_1"
        jv1.unit = "rad"
        
        jv2 = JointValue()
        jv2.joint_uri = "arm_joint_2"
        jv2.unit = "rad"

        jv3 = JointValue()
        jv3.joint_uri = "arm_joint_3"
        jv3.unit = "rad"
        
        jv4 = JointValue()
        jv4.joint_uri = "arm_joint_4"
        jv4.unit = "rad"
        
 pose.pose.position.z = z
 quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
 pose.pose.orientation.x = quat[0]
 pose.pose.orientation.y = quat[1]
 pose.pose.orientation.z = quat[2]
 pose.pose.orientation.w = quat[3]
 pose.header.frame_id = "/arm_link_0"
 pose.header.stamp = rospy.Time.now()
 
 # (conf, success) = iks.call_ik_solver(pose)
 (conf, success) = iks.call_constraint_aware_ik_solver(pose)
 if (success):
     # publish solution directly as joint positions
     print(conf)
     jp = JointPositions()
     
     for i in range(5):
         jv = JointValue()
         jv.joint_uri = iks.joint_names[i]
         jv.value = conf[i]
         jv.unit = "rad"
         jp.positions.append(jv)
     
     rospy.sleep(0.5)
     print "publishing cmd"
     armpub.publish(jp)
     
     
     
 else:
     print("IK solver didn't find a solution")
示例#51
0
    def execute(self, component_name, target="", blocking=True):
        ah = ActionHandle("move_arm_joint_direct", component_name, target, blocking)

        rospy.loginfo("Move <<%s>> DIRECT to <<%s>>", component_name, target)


        # check pose
        if not type(target) is list: # check outer list
            rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name)
            print "parameter is:", target
            ah.set_failed(3)
            return ah
        else:
            #print i,"type1 = ", type(i)
            DOF = 5
            if not len(target) == DOF: # check dimension
                rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...", component_name, DOF, len(target))
                print "parameter is:", target
                ah.set_failed(3)
                return ah
            else:
                for i in target:
                    #print i,"type2 = ", type(i)
                    if not ((type(i) is float) or (type(i) is int)): # check type
                        #print type(i)
                        rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...", component_name)
                        print "parameter is:", target
                        ah.set_failed(3)
                        return ah
                    else:
                        rospy.logdebug("accepted parameter %f for %s", i, component_name)


        pose_goal = MoveToJointConfigurationGoal()

        for i in range(DOF):
            jv = JointValue()
            jv.joint_uri = self.arm1_joint_names[i]
            jv.value = target[i]
            jv.unit = "rad"
            pose_goal.goal.positions.append(jv)

        

        rospy.logdebug("calling %s action server", self.action_server_name)
        
        # trying to connect to server
        rospy.logdebug("waiting for %s action server to start", self.action_server_name)
        if not self.client.wait_for_server(rospy.Duration(5)):
            # error: server did not respond
            rospy.logerr("%s action server not ready within timeout, aborting...", self.action_server_name)
            ah.set_failed(4)
            return ah
        else:
            rospy.logdebug("%s action server ready", self.action_server_name)


        #print client_goal
        self.client.send_goal(pose_goal)
        ah.set_client(self.client)

        ah.wait_inside()

        return ah    
import copy
from cob_srvs.srv import Trigger
from brics_actuator.msg import JointVelocities
from brics_actuator.msg import JointValue

rospy.init_node("ipa_canopen_test")

rospy.wait_for_service('/tray_controller/init')
print "found init"
initService = rospy.ServiceProxy('/tray_controller/init', Trigger)
resp = initService()

velPublisher = rospy.Publisher('/tray_controller/command_vel', JointVelocities)
rospy.sleep(2.0)
v = JointVelocities()
vv1 = JointValue()
vv1.timeStamp = rospy.Time.now()
vv1.joint_uri = "tray_1_joint"
vv2 = copy.deepcopy(vv1)
vv2.joint_uri = "tray_2_joint"
vv3 = copy.deepcopy(vv1)
vv3.joint_uri = "tray_3_joint"
v.velocities = [vv1,vv2,vv3]

v.velocities[0].value = 0.0

while not rospy.is_shutdown():
    v.velocities[0].value = 0.05
    v.velocities[1].value = 0.05
    velPublisher.publish(v)
示例#53
0
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'
    def processJoints(self, joint_states):
        armStatesReceived = joint_states.name[0] == 'arm_joint_1'

        if armStatesReceived and self.joyMsg is not None:
            self.crntJointStates = copy.deepcopy(joint_states)
            for index in range(0, len(joint_states.position)-2):
                pos = JointValue()
                pos.joint_uri = "arm_joint_" + str(index + 1)
                pos.unit = "rad"
                pos.value = joint_states.position[index]
                self.crntJointPositions.positions.append(pos)
            pos = JointValue()
            pos.joint_uri = "gripper_finger_joint_l"
            pos.unit = "m"
            pos.value = joint_states.position[5]
            self.gripperPositions.positions.append(pos)

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

            # desiredPos = copy.deepcopy(self.crntJointPositions)
            desiredGripPos = copy.deepcopy(self.gripperPositions)

            self.joyMsg.axes = numpy.array(self.joyMsg.axes)
            left_axis = self.joyMsg.axes[[0, 1]]
            right_axis = self.joyMsg.axes[[2, 5]]
            l2_axis = self.joyMsg.axes[[3]] * -1
            r2_axis = self.joyMsg.axes[[4]] * -1
            square, x, circle, triangle, l1, r1, l2, r2, share, options, l3, r3, ps_button, touch_button = [button == 1 for button in self.joyMsg.buttons]
            left_dpad = self.joyMsg.axes[6] == 1
            right_dpad = self.joyMsg.axes[6] == -1
            up_dpad = self.joyMsg.axes[7] == 1
            down_dpad = self.joyMsg.axes[7] == -1

            if not self.previous_vel_change:
                if square:
                    self.maxVel += 0.25
                if circle:
                    self.maxVel -= 0.25
                if self.maxVel < 0.25:
                    self.maxVel = 0.25

            self.previous_vel_change = square or circle

            # object that will carry all desired velocities to each joint
            desiredVel = JointVelocities()

            for index in range(0, len(joint_states.position)-2):
                vel = JointValue()
                vel.joint_uri = "arm_joint_" + str(index + 1)
                vel.unit = "s^-1 rad"  # radians per second
                desiredVel.velocities.append(vel)

            desiredVel.velocities[0].value -= left_axis[0] * self.maxVel
            desiredVel.velocities[1].value -= left_axis[1] * self.maxVel
            if not r1:
                desiredVel.velocities[2].value -= right_axis[1] * self.maxVel
            else:
                desiredVel.velocities[3].value -= right_axis[1] * self.maxVel

            desiredVel.velocities[4].value -= right_axis[0] * self.maxVel * 2

            desiredGripPos.positions[0].value -= 0.0115 * r2_axis[0]
            desiredGripPos.positions[1].value = desiredGripPos.positions[0].value

            joint_message = ''
            vel_message = ''
            grip_message = ''
            for jointNum in range(5):
                joint_message += ('joint ' + str(jointNum) + ': ' + str(self.crntJointPositions.positions[jointNum].value) + '    ')
                vel_message += ('veloc ' + str(jointNum) + ': ' + str(desiredVel.velocities[jointNum].value) + '    ')

            for gripNum in [0, 1]:
                desiredGripPos.positions[gripNum].value = self.checkValues(desiredGripPos.positions[gripNum].value, gripNum + 5)
                grip_message += ('grip ' + str(gripNum) + ': ' + str(desiredGripPos.positions[gripNum].value) + '    ')

            self.gripperPositions = desiredGripPos

            print(joint_message)
            print(vel_message)
            print(grip_message)

            self.gripPub.publish(desiredGripPos)
            self.velPub.publish(desiredVel)
from brics_actuator.msg import JointPositions, JointValue, Poison

import sys, select, termios, tty, signal

if __name__=="__main__":
    
    pub = rospy.Publisher('arm_1/arm_controller/position_command', JointPositions)
    
    rospy.init_node('simple_arm_gripper_position')

    rospy.sleep(0.5)
    
    try:
        jp = JointPositions()
        
        jv1 = JointValue()
        jv1.joint_uri = "arm_joint_1"
        jv1.value = 0.0
        jv1.unit = "rad"
        
        jv2 = JointValue()
        jv2.joint_uri = "arm_joint_2"
        jv2.value = 0.0
        jv2.unit = "rad"

        jv3 = JointValue()
        jv3.joint_uri = "arm_joint_3"
        jv3.value = 0.0
        jv3.unit = "rad"
        
        jv4 = JointValue()
示例#56
0
    def execute(self, target="", blocking=True):
        component_name = "gripper"
        ah = ActionHandle("move_gripper", component_name, target, blocking)
		
        rospy.loginfo("Move <<%s>> to <<%s>>", component_name, target)

        # get pose from parameter server
        if type(target) is str:
            if not rospy.has_param(self.action_server_name_prefix + "/" + component_name + "/" + target):
                rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...", self.action_server_name_prefix + "/" + component_name + "/" + target)
                ah.set_failed(2)
                return ah
            param = rospy.get_param(self.action_server_name_prefix + "/" + component_name + "/" + target)
        else:
            param = target

        # check pose
        """
	if not type(param) is list:  # check outer list
            rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name)
            print "parameter is:", param
            ah.set_failed(3)
            return ah
        else:
            print i,"type1 = ", type(i)
            DOF = 2
            if not len(param) == DOF:  # check dimension
                rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...", component_name, DOF, len(param))
                print "parameter is:", param
                ah.set_failed(3)
                return ah
            else:
                for i in param:
                    print i,"type2 = ", type(i)
                    if not ((type(i) is float) or (type(i) is int)): # check type
                        print type(i)
                        rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...", component_name)
                        print "parameter is:", param
                        ah.set_failed(3)
                        return ah
                    else:
                        rospy.logdebug("accepted parameter %f for %s", i, component_name)
	"""

        pose_goal = MoveToJointConfigurationGoal()

        DOF = 2
        for i in range(DOF):
            jv = JointValue()
            jv.joint_uri = self.gripper1_joint_names[i]
            jv.value = param[i]
            jv.unit = "m"
            pose_goal.goal.positions.append(jv)

        action_server_name = "/arm_1/gripper_controller/MoveToJointConfigurationDirect"

        rospy.logdebug("calling %s action server", action_server_name)
        client = actionlib.SimpleActionClient(action_server_name, MoveToJointConfigurationAction)
        # trying to connect to server
        rospy.logdebug("waiting for %s action server to start", action_server_name)
        if not client.wait_for_server(rospy.Duration(5)):
            # error: server did not respond
            rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
            ah.set_failed(4)
            return ah
        else:
            rospy.logdebug("%s action server ready", action_server_name)


        print pose_goal 
        client.send_goal(pose_goal)
        ah.set_client(client)

        ah.wait_inside()
	# it should block but it does not, so we wait!
	rospy.sleep(3.0)

        return ah
示例#57
0
#!/usr/bin/env python
import roslib
roslib.load_manifest('youbot_pick_and_place')
import rospy
import sys
import select, termios, tty, signal
import moveit_commander, moveit_msgs.msg
from brics_actuator.msg import JointPositions, JointValue
import geometry_msgs.msg

gripper_l = JointValue()
gripper_l.joint_uri = 'gripper_finger_joint_l'
gripper_l.unit = 'm'
gripper_r = JointValue()
gripper_r.joint_uri = 'gripper_finger_joint_r'
gripper_r.unit = 'm'
joint_1 = JointValue()
joint_1.joint_uri = 'arm_joint_1'
joint_1.unit = 'rad'
joint_2 = JointValue()
joint_2.joint_uri = 'arm_joint_2'
joint_2.unit = 'rad'
joint_3 = JointValue()
joint_3.joint_uri = 'arm_joint_3'
joint_3.unit = 'rad'
joint_4 = JointValue()
joint_4.joint_uri = 'arm_joint_4'
joint_4.unit = 'rad'
joint_5 = JointValue()
joint_5.joint_uri = 'arm_joint_5'
joint_5.unit = 'rad'
示例#58
0
    def set_gripper_state(self, open_gripper=True):
        """Open/close gripper.

        Arguments:
            open_gripper (bool): if True - opens gripper, if False - otherwise
        """
        self.gripper_position.positions = []
        if open_gripper:
            # Open gripper
            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)
        else:
            # Close gripper
            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)
        self.gripper_position_publisher.publish(self.gripper_position)