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 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)
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 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 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) 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 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 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()
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 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 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 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 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 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
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
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 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 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 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 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 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 _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 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
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
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 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)
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 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)
def __init__(self): u"""Конструктор класса.""" self.base_velocity = Twist() self.gripper_position = JointPositions() self.arm_velocities = JointVelocities() self.base_speed_multiplier = 0.2 self.arm_speed_multiplier = 0.5 self.base_velocity_publisher = rospy.Publisher('/cmd_vel', Twist) self.gripper_position_publisher = rospy.Publisher( '/arm_1/gripper_controller/position_command', JointPositions) self.arm_velocities_publisher = rospy.Publisher( '/arm_1/arm_controller/velocity_command', JointVelocities) rospy.Subscriber("/joy", Joy, self.update_data_from_joy)
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 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)
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)
# msg.direction.z = hand_direction_[2] # msg.normal.x = hand_normal_[0] # msg.normal.y = hand_normal_[1] # msg.normal.z = hand_normal_[2] # msg.palmpos.x = hand_palm_pos_[0] # msg.palmpos.y = hand_palm_pos_[1] # msg.palmpos.z = hand_palm_pos_[2] # msg.ypr.x = hand_yaw_ # msg.ypr.y = hand_pitch_ # msg.ypr.z = hand_roll_ # We don't publish native data types, see ROS best practices # pub.publish(hand_direction=hand_direction_,hand_normal = hand_normal_, hand_palm_pos = hand_palm_pos_, hand_pitch = hand_pitch_, hand_roll = hand_roll_, hand_yaw = hand_yaw_) #pub_ros.publish(msg) # Save some CPU time, circa 100Hz publishing. joint_pos = JointPositions() joint_number = 0 key = getKey() if key in moveBindings.keys(): joint_number = moveBindings[key] print joint_number default_joint_one = 0.0100692 default_joint_two = 0.0100692 default_joint_three = -5.02655 default_joint_four = 0.0221239 default_joint_five = 0.110619 joint_val_1 = JointValue()
import roslib; roslib.load_manifest('youbot_examples') import rospy import brics_actuator.msg from brics_actuator.msg import JointPositions, JointValue, Poison 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"
import brics_actuator.msg 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"
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)