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'
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)
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)
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()
def poseStart(self): if self.state == 'home': joint_1.value = 0.01006921 joint_2.value = 0.01006921 joint_3.value = -0.0157081 joint_4.value = 0.0221391 joint_5.value = 0.11062 pos = JointPositions() pos.positions = (joint_1, joint_2, joint_3, joint_4, joint_5) self.armPub.publish(pos) self.state = 'start'
def poseHome(self): if self.state == 'start' or self.state == 'grap1' or self.state == 'grap2': joint_1.value = 2.9 joint_2.value = 1.1 joint_3.value = -2.5 joint_4.value = 0.023 joint_5.value = 3 pos = JointPositions() pos.positions = (joint_1, joint_2, joint_3, joint_4, joint_5) self.armPub.publish(pos) self.state = 'home'
def poseGrap1(self): if self.state == 'home': joint_1.value = 2.9544 joint_2.value = 2.617 joint_3.value = -1.0732 joint_4.value = 0.32213 joint_5.value = 3 pos = JointPositions() pos.positions = (joint_1, joint_2, joint_3, joint_4, joint_5) self.armPub.publish(pos) self.state = 'grap1'
def poseGrap2(self): if self.state == 'home': joint_1.value = 2.9 joint_2.value = 1.5 joint_3.value = -2.0 joint_4.value = 2.5 joint_5.value = 3 pos = JointPositions() pos.positions = (joint_1, joint_2, joint_3, joint_4, joint_5) self.armPub.publish(pos) self.state = 'grap2'
def talkerGripper(G): pub = rospy.Publisher('/arm_1/gripper_controller/position_command', JointPositions, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(5) # 10hz count = 0 while not rospy.is_shutdown(): joint_pos = JointPositions() joint_number = 1 joint_val_1 = JointValue() joint_val_2 = JointValue() joint_val_1.joint_uri = "gripper_finger_joint_l" joint_val_1.unit = "rad" joint_val_2.joint_uri = "agripper_finger_joint_r" joint_val_2.unit = "rad" joint_val_1 = JointValue() joint_val_1.joint_uri = "gripper_finger_joint_l" joint_val_1.unit = "m" joint_val_1.value = G[0] joint_val_2 = JointValue() joint_val_2.joint_uri = "gripper_finger_joint_r" joint_val_2.unit = "m" joint_val_2.value = G[1] poison = Poison() joint_pos.poisonStamp = poison joint_pos.positions = [joint_val_1, joint_val_2] #pub_youbotleaparm.publish(joint_pos) # rospy.loginfo(joint_pos) pub.publish(joint_pos) count+=1 rate.sleep()
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)
def main(): pub = rospy.Publisher('/arm_1/arm_controller/position_command', JointPositions, queue_size=10) # rospy.init_node('arm_test') '''initial positions''' msg = JointPositions() print "going to initial position" print "moving joint 1" msg.positions = [JointValue()] msg.positions[0].timeStamp = rospy.Time.now() msg.positions[0].joint_uri = "arm_joint_1" msg.positions[0].unit = "rad" msg.positions[0].value = 0.011 pub.publish(msg) time.sleep(1) print "moving joint 2" msg.positions[0].joint_uri = "arm_joint_2" msg.positions[0].unit = "rad" msg.positions[0].value = 0.011 pub.publish(msg) time.sleep(1) print "moving joint 3" msg.positions[0].joint_uri = "arm_joint_3" msg.positions[0].unit = "rad" msg.positions[0].value = -0.016 pub.publish(msg) time.sleep(1) print "moving joint 4" msg.positions[0].joint_uri = "arm_joint_4" msg.positions[0].unit = "rad" msg.positions[0].value = 0.0222 # msg.positions[0].value = 1 pub.publish(msg) time.sleep(1) print "moving joint 5" msg.positions[0].joint_uri = "arm_joint_5" msg.positions[0].unit = "rad" msg.positions[0].value = 0.111 pub.publish(msg)
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 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 gen_joint_msg(joint_angles): msg_arr = [0 for i in range(5)] for i in range(5): msg_arr[i] = JointPositions() msg_arr[i].positions = [JointValue()] msg_arr[i].positions[0].timeStamp = rospy.Time.now() msg_arr[i].positions[0].joint_uri = "arm_joint_"+str(i+1) msg_arr[i].positions[0].unit = "rad" # not sure about this, truncate the angles in the ik solution when they are out of range. Usually the difference is small if (i == 3 and joint_angles[i] > 3.429): msg_arr[i].positions[0].value = 3.429 elif (i == 2 and joint_angles[i] > -0.0157): msg_arr[i].positions[0].value = -0.01571 else: msg_arr[i].positions[0].value = joint_angles[i] return msg_arr
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 move_gripper_to(self, pose_name): if pose_name not in self.gripper_positions: return False joint_values = self.gripper_positions[pose_name] print(pose_name, joint_values) joint_uris = ['gripper_finger_joint_l', 'gripper_finger_joint_r'] joint_pos_msg = JointPositions() joint_value_msg_list = [] for i in range(2): joint_value = JointValue(joint_uri=joint_uris[i], unit='m', value=joint_values[i]) joint_value_msg_list.append(joint_value) joint_pos_msg.positions = joint_value_msg_list self.gripper_pos_pub.publish(joint_pos_msg) rospy.sleep(3) return True
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 safeHomeCommand(data): rospy.loginfo(rospy.get_caller_id() + "arm data: %s", data.data) #creating a JointValue() List/Array jvaluelist = [0 for i in range(4)] # #creating a JointValue() Object for joint 1 (base) # jvalue1 = JointValue() # jvalue1.joint_uri = 'arm_joint_1' # jvalue1.unit = 'rad' # jvalue1.value = data.data[0] # jvaluelist[0] = jvalue1 #creating a JointValue() Object for joint 2 jvalue2 = JointValue() jvalue2.joint_uri = 'arm_joint_2' jvalue2.unit = 'rad' jvalue2.value = data.data[1] jvaluelist[0] = jvalue2 #creating a JointValue() Object for joint 3 jvalue3 = JointValue() jvalue3.joint_uri = 'arm_joint_3' jvalue3.unit = 'rad' jvalue3.value = data.data[2] jvaluelist[1] = jvalue3 #creating a JointValue() Object for joint 4 jvalue4 = JointValue() jvalue4.joint_uri = 'arm_joint_4' jvalue4.unit = 'rad' jvalue4.value = data.data[3] jvaluelist[2] = jvalue4 #creating a JointValue() Object for joint 5 jvalue5 = JointValue() jvalue5.joint_uri = 'arm_joint_5' jvalue5.unit = 'rad' jvalue5.value = data.data[4] jvaluelist[3] = jvalue5 #setting JointPosition msg properties rate = rospy.Rate(20) msg = JointPositions() msg.positions = jvaluelist pubArm.publish(msg)
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)
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)
def move_arm_to(self, pose_name): if pose_name not in self.joint_angles: return False joint_values = self.joint_angles[pose_name] print(pose_name, joint_values) try: self.arm.set_joint_value_target(joint_values) except Exception as e: rospy.logerr('unable to set target position: %s' % (str(e))) return False status = self.arm.go(wait=True) if not status: rospy.logwarn( "Failed moving arm with moveit, trying position command") joint_pos_msg = JointPositions() joint_value_msg_list = self.create_joint_value_msg_list( joint_values, 'rad') joint_pos_msg.positions = joint_value_msg_list self.joint_pos_pub.publish(joint_pos_msg) rospy.sleep(3) return True
def main(): pub = rospy.Publisher('/arm_1/gripper_controller/position_command', JointPositions, queue_size=10) #rospy.init_node('gripper_test') msg = JointPositions() msg.positions = [JointValue()] msg.positions[0].timeStamp = rospy.Time.now() msg.positions[0].joint_uri = "gripper_finger_joint_l" msg.positions[0].unit = "m" msg.positions[0].value = 0.0 pub.publish(msg) msg.positions[0].joint_uri = "gripper_finger_joint_r" msg.positions[0].unit = "m" msg.positions[0].value = 0.0 pub.publish(msg) time.sleep(1) msg.positions[0].joint_uri = "gripper_finger_joint_l" msg.positions[0].unit = "m" msg.positions[0].value = 0.0114 pub.publish(msg) time.sleep(1) pub.publish(msg) msg.positions[0].joint_uri = "gripper_finger_joint_r" msg.positions[0].unit = "m" msg.positions[0].value = 0.0114 pub.publish(msg) def file_exit(): print "shutting down" rospy.on_shutdown(file_exit)
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.0115 jv1.unit = "m" jv2 = JointValue() jv2.joint_uri = "gripper_finger_joint_r" jv2.value = 0.0115 jv2.unit = "m" p = Poison() jp.poisonStamp = p jp.positions = [jv1, jv2] #list pub.publish(jp) rospy.sleep(1.0) except Exception, e: print e
def talker(Q): pub = rospy.Publisher('/arm_1/arm_controller/position_command', JointPositions, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(5) # 10hz count = 0 while not rospy.is_shutdown(): joint_pos = JointPositions() joint_number = 1 joint_val_1 = JointValue() joint_val_2 = JointValue() joint_val_3 = JointValue() joint_val_4 = JointValue() joint_val_5 = JointValue() joint_val_1.joint_uri = "arm_joint_1" joint_val_1.unit = "rad" joint_val_2.joint_uri = "arm_joint_2" joint_val_2.unit = "rad" joint_val_3.joint_uri = "arm_joint_3" joint_val_3.unit = "rad" joint_val_4.joint_uri = "arm_joint_4" joint_val_4.unit = "rad" joint_val_5.joint_uri = "arm_joint_5" joint_val_5.unit = "rad" joint_val_1 = JointValue() joint_val_1.joint_uri = "arm_joint_1" joint_val_1.unit = "rad" joint_val_1.value = Q[0] + 2.95 #adding offsets to all angles, we assume 0's #in candle possition, youbot diver has 0's #close to folded possition joint_val_2 = JointValue() joint_val_2.joint_uri = "arm_joint_2" joint_val_2.unit = "rad" joint_val_2.value = Q[1] + 1.05 joint_val_3 = JointValue() joint_val_3.joint_uri = "arm_joint_3" joint_val_3.unit = "rad" joint_val_3.value = Q[2] - 2.44 joint_val_4 = JointValue() joint_val_4.joint_uri = "arm_joint_4" joint_val_4.unit = "rad" joint_val_4.value = Q[3] + 1.73 joint_val_5 = JointValue() joint_val_5.joint_uri = "arm_joint_5" joint_val_5.unit = "rad" joint_val_5.value = Q[4] + 2.95 poison = Poison() joint_pos.poisonStamp = poison joint_pos.positions = [joint_val_1, joint_val_2, joint_val_3, joint_val_4, joint_val_5] #pub_youbotleaparm.publish(joint_pos) # rospy.loginfo(joint_pos) pub.publish(joint_pos) count+=1 rate.sleep()
joint2.value = check_positions[1] joint3.joint_uri = "arm_joint_3" joint3.unit = "rad" joint3.value = check_positions[2] joint4.joint_uri = "arm_joint_4" joint4.unit = "rad" joint4.value = check_positions[3] joint5.joint_uri = "arm_joint_5" joint5.unit = "rad" joint5.value = check_positions[4] joint_pos.poisonStamp = poison joint_pos.positions = [joint1, joint2, joint3, joint4, joint5] print("Check Arm-joint") for i in range(0, 5): arm_joint_publisher.publish(joint_pos) delay.sleep() for i in range(0, 20): joint1.value = joint_positions[0] joint2.value = joint_positions[1] joint3.value = joint_positions[2] joint4.value = joint_positions[3] joint5.value = joint_positions[4] print("Waiting process") delay.sleep()
def main(): pub = rospy.Publisher('/arm_1/arm_controller/position_command', JointPositions, queue_size=10) rospy.init_node('arm_test') '''initial positions''' msg = JointPositions() msg.positions = [JointValue()] msg.positions[0].timeStamp = rospy.Time.now() msg.positions[0].joint_uri = "arm_joint_1" msg.positions[0].unit = "rad" msg.positions[0].value = 0.011 msg.positions[0].joint_uri = "arm_joint_2" msg.positions[0].unit = "rad" msg.positions[0].value = 0.011 msg.positions[0].joint_uri = "arm_joint_3" msg.positions[0].unit = "rad" msg.positions[0].value = -0.016 msg.positions[0].joint_uri = "arm_joint_4" msg.positions[0].unit = "rad" msg.positions[0].value = 0.0222 msg.positions[0].joint_uri = "arm_joint_5" msg.positions[0].unit = "rad" msg.positions[0].value = 0.0111 pub.publish(msg) # time.sleep(1) # msg.positions[0].joint_uri = "arm_joint_1" # msg.positions[0].value = 1.3 # pub.publish(msg) # # msg.positions[0].joint_uri = "arm_joint_2" # msg.positions[0].unit = "rad" # msg.positions[0].value = 1.3 # pub.publish(msg) # # msg.positions[0].joint_uri = "arm_joint_3" # msg.positions[0].unit = "rad" # msg.positions[0].value = -1.2 # pub.publish(msg) # # time.sleep(2) # # msg.positions[0].joint_uri = "arm_joint_4" # msg.positions[0].unit = "rad" # msg.positions[0].value = 2.8 # pub.publish(msg) # gripper_open.main() # # time.sleep(2) # # # msg.positions[0].joint_uri = "arm_joint_2" # msg.positions[0].unit = "rad" # msg.positions[0].value = 2 # pub.publish(msg) # # gripper_close.main() # time.sleep(2) # # msg.positions[0].joint_uri = "arm_joint_2" # msg.positions[0].unit = "rad" # msg.positions[0].value = 0.5 # pub.publish(msg) # # time.sleep(2) msg.positions[0].joint_uri = "arm_joint_1" msg.positions[0].unit = "rad" msg.positions[0].value = 0.075 pub.publish(msg) time.sleep(1) msg.positions[0].joint_uri = "arm_joint_2" msg.positions[0].unit = "rad" msg.positions[0].value = 1.3 pub.publish(msg) time.sleep(1) msg.positions[0].joint_uri = "arm_joint_4" msg.positions[0].unit = "rad" msg.positions[0].value = 2.85 pub.publish(msg) gripper_open.main() time.sleep(1) msg.positions[0].joint_uri = "arm_joint_2" msg.positions[0].unit = "rad" msg.positions[0].value = 0.5 pub.publish(msg) rospy.spin()
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 pub.publish(jp) rospy.sleep(1.0) except Exception, e: print e
def closeGripper(self): gripper_l.value = 0.0025 gripper_r.value = 0.0025 pos = JointPositions() pos.positions = (gripper_l, gripper_r) self.gripperPub.publish(pos)
def openGripper(self): gripper_l.value = 0.0115 gripper_r.value = 0.0115 pos = JointPositions() pos.positions = (gripper_l, gripper_r) self.gripperPub.publish(pos)
gripper_pos_pub = rospy.Publisher('/PG70_controller/command_pos', JointPositions) gripper_vel_pub = rospy.Publisher('/PG70_controller/command_vel', JointVelocities) rospy.sleep(1.0) # set joint position gripper_pos_msg = JointPositions() gripper_pos = JointValue() gripper_pos.timeStamp = rospy.Time.now() gripper_pos.joint_uri = 'left_arm_top_finger_joint' gripper_pos.unit = 'm' gripper_pos.value = 0.025 gripper_pos_msg.positions = [gripper_pos] gripper_pos_pub.publish(gripper_pos_msg) rospy.sleep(5.0) # send joint velocities gripper_vel_msg = JointVelocities() gripper_vel = JointValue() gripper_vel.timeStamp = rospy.Time.now() gripper_vel.joint_uri = 'left_arm_top_finger_joint' gripper_vel.unit = 'm/s' gripper_vel_msg.velocities = [gripper_vel]
def callback(data): rospy.loginfo(data.data) #creating a JointValue() List/Array jvaluelist = [0 for i in range(2)] #creating a JointValue() Object for joint 2 jvalue2 = JointValue() jvalue2.joint_uri = 'arm_joint_2' jvalue2.unit = 'rad' jvalue2.value = 0.011 jvaluelist[0] = jvalue2 #creating a JointValue() Object for joint 3 jvalue3 = JointValue() jvalue3.joint_uri = 'arm_joint_3' jvalue3.unit = 'rad' jvalue3.value = -0.016 jvaluelist[1] = jvalue3 #setting JointPosition msg properties rate = rospy.Rate(20) msg = JointPositions() msg.positions = jvaluelist pub_home.publish(msg) rospy.sleep(5) #creating a JointValue() List/Array jvaluelist = [0 for i in range(3)] #creating a JointValue() Object for joint 1 (base) jvalue1 = JointValue() jvalue1.joint_uri = 'arm_joint_1' jvalue1.unit = 'rad' jvalue1.value = 0.011 jvaluelist[0] = jvalue1 #creating a JointValue() Object for joint 4 jvalue4 = JointValue() jvalue4.joint_uri = 'arm_joint_4' jvalue4.unit = 'rad' jvalue4.value = 0.0222 jvaluelist[1] = jvalue4 #creating a JointValue() Object for joint 5 jvalue5 = JointValue() jvalue5.joint_uri = 'arm_joint_5' jvalue5.unit = 'rad' jvalue5.value = 0.111 jvaluelist[2] = jvalue5 rospy.sleep(5) #setting JointPosition msg properties rate = rospy.Rate(20) msg = JointPositions() msg.positions = jvaluelist pub_home.publish(msg) #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 = 0.001 jvaluegrippers[0] = jvalue6 #creating a JointValue() Object for gripper right jvalue7 = JointValue() jvalue7.joint_uri = 'gripper_finger_joint_r' jvalue7.unit = 'm' jvalue7.value = 0.001 jvaluegrippers[1] = jvalue7 #setting JointPosition msg properties rate = rospy.Rate(20) msg2 = JointPositions() msg2.positions = jvaluegrippers pub_gripper.publish(msg2)
jv4 = JointValue() jv4.joint_uri = "arm_joint_4" jv4.value = 0.023 jv4.unit = "rad" jv5 = JointValue() jv5.joint_uri = "arm_joint_5" jv5.value = 0.12 jv5.unit = "rad" p = Poison() jp.poisonStamp = p jp.positions = [jv1, jv2, jv3, jv4, jv5] pub.publish(jp) pub.publish(jp) except Exception, e: print e ''' JOINT LIMITS: [ WARN] [1340337373.846618553]: Cannot set arm joint 1: The setpoint angle is out of range. The valid range is between 0.0100692 rad and 5.84014 rad and it is: 0.01 rad [ WARN] [1340337373.847057559]: Cannot set arm joint 2: The setpoint angle is out of range. The valid range is between 0.0100692 rad and 2.61799 rad and it is: 0.01 rad [ WARN] [1340337373.847447567]: Cannot set arm joint 3: The setpoint angle is out of range. The valid range is between -5.02655 rad and -0.015708 rad and it is: -0.01 rad
def main(self): pub = rospy.Publisher('/arm_1/arm_controller/position_command', JointPositions, queue_size = 10) # rospy.init_node('arm_test') '''initial positions''' msg = JointPositions() print "going to initial position" msg.positions = [JointValue()] msg.positions[0].timeStamp = rospy.Time.now() msg.positions[0].joint_uri = "arm_joint_1" msg.positions[0].unit = "rad" msg.positions[0].value = 0.011 pub.publish(msg) rospy.sleep(1) msg.positions[0].joint_uri = "arm_joint_2" msg.positions[0].unit = "rad" msg.positions[0].value = 0.011 pub.publish(msg) rospy.sleep(1) msg.positions[0].joint_uri = "arm_joint_3" msg.positions[0].unit = "rad" msg.positions[0].value = -0.016 pub.publish(msg) rospy.sleep(1) msg.positions[0].joint_uri = "arm_joint_4" msg.positions[0].unit = "rad" msg.positions[0].value = 0.0222 pub.publish(msg) rospy.sleep(1) msg.positions[0].joint_uri = "arm_joint_5" msg.positions[0].unit = "rad" msg.positions[0].value = 0.111 pub.publish(msg) rospy.sleep(1) print "time to move" msg.positions[0].joint_uri = "arm_joint_1" msg.positions[0].value = 0.011 pub.publish(msg) rospy.sleep(1) msg.positions[0].joint_uri = "arm_joint_2" msg.positions[0].unit = "rad" msg.positions[0].value = 1.3 pub.publish(msg) rospy.sleep(1) msg.positions[0].joint_uri = "arm_joint_3" msg.positions[0].unit = "rad" msg.positions[0].value = -1.2 pub.publish(msg) rospy.sleep(1) msg.positions[0].joint_uri = "arm_joint_5" msg.positions[0].unit = "rad" msg.positions[0].value = 2.8 pub.publish(msg) rospy.sleep(1) msg.positions[0].joint_uri = "arm_joint_4" msg.positions[0].unit = "rad" msg.positions[0].value = 2.5 pub.publish(msg) rospy.sleep(1) msg.positions[0].joint_uri = "arm_joint_3" msg.positions[0].unit = "rad" msg.positions[0].value = -1 pub.publish(msg) gripper_open.main()
jv3.unit = "rad" jv4 = JointValue() jv4.joint_uri = "arm_joint_4" jv4.value = 0.023 jv4.unit = "rad" jv5 = JointValue() jv5.joint_uri = "arm_joint_5" jv5.value = 0.12 jv5.unit = "rad" p = Poison() jp.poisonStamp = p jp.positions = [jv1, jv2, jv3, jv4, jv5] pub.publish(jp) pub.publish(jp) except Exception, e: print e ''' JOINT LIMITS: [ WARN] [1340337373.846618553]: Cannot set arm joint 1: The setpoint angle is out of range. The valid range is between 0.0100692 rad and 5.84014 rad and it is: 0.01 rad [ WARN] [1340337373.847057559]: Cannot set arm joint 2: The setpoint angle is out of range. The valid range is between 0.0100692 rad and 2.61799 rad and it is: 0.01 rad [ WARN] [1340337373.847447567]: Cannot set arm joint 3: The setpoint angle is out of range. The valid range is between -5.02655 rad and -0.015708 rad and it is: -0.01 rad [ WARN] [1340337373.853084080]: Cannot set arm joint 4:
# joint arm_joint_4 valid range is between 0.0221239 and 3.4292 # joint arm_joint_5 valid range is between 0.110619 and 5.64159 # joint_val_1.value = (((hand_normal_[0] /2) + 0.5) * (5.84014 - 0.0100692) ) + 0.0100692 # joint_val_2.value = (((hand_normal_[0] /2) + 0.5) * (2.61799 - 0.0100692) ) + 0.0100692 # joint_val_3.value = (((hand_normal_[0] /2) + 0.5) * (5.02655 - 0.015708) ) - 5.02655 # joint_val_4.value = (((hand_normal_[0] /2) + 0.5) * (3.4292 - 0.0221239) ) + 0.0221239 # joint_val_5.value = (((hand_normal_[0] /2) + 0.5) * (5.64159 - 0.110619) ) + 0.110619 poison = Poison() joint_pos.poisonStamp = poison joint_pos.positions = [joint_val_1, joint_val_2, joint_val_3, joint_val_4, joint_val_5] pub_youbotleaparm.publish(joint_pos) # print "x_vel: %f m/s, y_vel: %f m/s, ang_vel: %f m/s" % (twist.linear.x , twist.linear.y , twist.angular.z ) # print "pitch: %f m/s, yaw: %f m/s, roll: %f m/s" % (hand_pitch_ , hand_yaw_ , hand_roll_) # r.sleep() # try: # sender() # except rospy.ROSInterruptException: # pass # termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)