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 update_gripper_position(self, data): u"""Открывает/закрывает гриппер.""" self.gripper_position.positions = [] # Open gripper if data.buttons[6]: tmp_gripper_position_r = JointValue() tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r' tmp_gripper_position_r.value = 0.011499 tmp_gripper_position_r.unit = 'm' tmp_gripper_position_r.timeStamp = rospy.Time.now() self.gripper_position.positions.append(tmp_gripper_position_r) tmp_gripper_position_l = JointValue() tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l' tmp_gripper_position_l.value = 0.011499 tmp_gripper_position_l.unit = 'm' tmp_gripper_position_l.timeStamp = rospy.Time.now() self.gripper_position.positions.append(tmp_gripper_position_l) # Close gripper if data.buttons[7]: tmp_gripper_position_r = JointValue() tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r' tmp_gripper_position_r.value = 0 tmp_gripper_position_r.unit = 'm' tmp_gripper_position_r.timeStamp = rospy.Time.now() self.gripper_position.positions.append(tmp_gripper_position_r) tmp_gripper_position_l = JointValue() tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l' tmp_gripper_position_l.value = 0 tmp_gripper_position_l.unit = 'm' tmp_gripper_position_l.timeStamp = rospy.Time.now() self.gripper_position.positions.append(tmp_gripper_position_l)
def jointStatesReceivedHandler(jointstates): global crntJointPositions global crntJointStates global gripperPositions crntJointPositions = JointPositions() gripperPositions = JointPositions() armStatesReceived = False if jointstates.name[0] == 'arm_joint_1': armStatesReceived = True if armStatesReceived == True: crntJointStates = copy.deepcopy(jointstates) for index in range(0, len(jointstates.position)-2): pos = JointValue() pos.joint_uri = "arm_joint_" + str(index + 1) pos.unit = "rad" pos.value = jointstates.position[index] crntJointPositions.positions.append(pos) pos = JointValue() pos.joint_uri = "gripper_finger_joint_l" pos.unit = "m" pos.value = jointstates.position[5] gripperPositions.positions.append(pos) pos = JointValue() pos.joint_uri = "gripper_finger_joint_r" pos.unit = "m" pos.value = jointstates.position[6] gripperPositions.positions.append(pos)
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 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 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 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 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 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 update_gripper_position(self, data): u"""Открывает/закрывает гриппер.""" self.gripper_position.positions = [] # Open gripper if data.buttons[6]: tmp_gripper_position_r = JointValue() tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r' tmp_gripper_position_r.value = 0.011499 tmp_gripper_position_r.unit = 'm' tmp_gripper_position_r.timeStamp = rospy.Time.now() self.gripper_position.positions.append(tmp_gripper_position_r) tmp_gripper_position_l = JointValue() tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l' tmp_gripper_position_l.value = 0.011499 tmp_gripper_position_l.unit = 'm' tmp_gripper_position_l.timeStamp = rospy.Time.now() self.gripper_position.positions.append(tmp_gripper_position_l) # Close gripper if data.buttons[7]: tmp_gripper_position_r = JointValue() tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r' tmp_gripper_position_r.value = 0 tmp_gripper_position_r.unit = 'm' tmp_gripper_position_r.timeStamp = rospy.Time.now() self.gripper_position.positions.append(tmp_gripper_position_r) tmp_gripper_position_l = JointValue() tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l' tmp_gripper_position_l.value = 0 tmp_gripper_position_l.unit = 'm' tmp_gripper_position_l.timeStamp = rospy.Time.now() self.gripper_position.positions.append(tmp_gripper_position_l)
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 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 make_brics_msg_arm(cls, arm_num, positions): # create joint positions message jp = JointPositions() for ii in range(5): jv = JointValue() if arm_num == 1: jv.joint_uri = 'arm_joint_' + str(ii+1) else: jv.joint_uri = 'arm_' + str(arm_num) + '_joint_' + str(ii+1) jv.unit='rad' jv.value = positions[ii] jp.positions.append(copy.deepcopy(jv)) return jp
def make_brics_msg_arm(cls, arm_num, positions): # create joint positions message jp = JointPositions() for ii in range(5): jv = JointValue() if arm_num == 1: jv.joint_uri = 'arm_joint_' + str(ii + 1) else: jv.joint_uri = 'arm_' + str(arm_num) + '_joint_' + str(ii + 1) jv.unit = 'rad' jv.value = positions[ii] jp.positions.append(copy.deepcopy(jv)) return jp
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 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 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
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 _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 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
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 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 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 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
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
def followPointsInFile(filename, pub=None, grippub=None, startpoint=0, endpoint=None, sleeptime=3): ptsTool = wptool.WpTool(filename,'r') readWp = {'wpnum':None, 'wp':None} while not(readWp['wpnum'] == startpoint): readWp = ptsTool.ReadWaypoint() print ("Following waypoints...") time.sleep(1) while not(readWp['wpnum'] == endpoint): jointPos = JointPositions() gripperPos = JointPositions() for index in range(0, 5): pos = JointValue() pos.joint_uri = "arm_joint_" + str(index + 1) pos.unit = "rad" pos.value = readWp['wp'][index] jointPos.positions.append(pos) pos = JointValue() pos.joint_uri = "gripper_finger_joint_l" pos.unit = 'm' pos.value = readWp['wp'][5] gripperPos.positions.append(pos) pos = JointValue() pos.joint_uri = "gripper_finger_joint_r" pos.unit = 'm' pos.value = readWp['wp'][6] gripperPos.positions.append(pos) if (not(pub == None)): pub.publish(jointPos) if (not(grippub == None)): grippub.publish(gripperPos) time.sleep(sleeptime) readWp = ptsTool.ReadWaypoint() ptsTool.Close()
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)
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 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 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
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 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 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 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 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 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)
from brics_actuator.msg import JointPositions, JointValue from cob_srvs.srv import Trigger if __name__ == "__main__": rospy.init_node('gripper_open_node') gripper_pos_pub = rospy.Publisher('/PG70_controller/command_pos', JointPositions) gripper_recover_srv = rospy.ServiceProxy('/PG70_controller/recover', Trigger) # recover gripper_recover_srv() rospy.sleep(4.0) # open the gripper joint_pos_msg = JointPositions() joint_val = JointValue() joint_val.timeStamp = rospy.Time.now() joint_val.joint_uri = "left_arm_top_finger_joint" joint_val.unit = "mm" joint_val.value = 16 joint_pos_msg.positions.append(joint_val) gripper_pos_pub.publish(joint_pos_msg) rospy.sleep(4.0) joint_pos_msg.positions[0].value = 4 gripper_pos_pub.publish(joint_pos_msg)
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
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
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()
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 pub.publish(jp)
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()
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()
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.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)
def update_arm_velocities(self, data): u"""Задаёт скорости осей манипулятора.""" self.arm_velocities.velocities = [] # 1 joint tmp_joint_value = JointValue() tmp_joint_value.joint_uri = 'arm_joint_1' tmp_joint_value.unit = 's^-1 rad' tmp_joint_value.value = data.axes[2] * self.arm_speed_multiplier self.arm_velocities.velocities.append(tmp_joint_value) # 2 joint tmp_joint_value = JointValue() tmp_joint_value.joint_uri = 'arm_joint_2' tmp_joint_value.unit = 's^-1 rad' tmp_joint_value.value = data.axes[3] * self.arm_speed_multiplier self.arm_velocities.velocities.append(tmp_joint_value) # 3 joint if notXOR(data.buttons[2], data.buttons[3]): tmp_joint_value = JointValue() tmp_joint_value.joint_uri = 'arm_joint_3' tmp_joint_value.unit = 's^-1 rad' tmp_joint_value.value = 0 self.arm_velocities.velocities.append(tmp_joint_value) elif data.buttons[2]: tmp_joint_value = JointValue() tmp_joint_value.joint_uri = 'arm_joint_3' tmp_joint_value.unit = 's^-1 rad' tmp_joint_value.value = 1 * self.arm_speed_multiplier self.arm_velocities.velocities.append(tmp_joint_value) elif data.buttons[3]: tmp_joint_value = JointValue() tmp_joint_value.joint_uri = 'arm_joint_3' tmp_joint_value.unit = 's^-1 rad' tmp_joint_value.value = -1 * self.arm_speed_multiplier self.arm_velocities.velocities.append(tmp_joint_value) # 4 joint if notXOR(data.buttons[0], data.buttons[1]): tmp_joint_value = JointValue() tmp_joint_value.joint_uri = 'arm_joint_4' tmp_joint_value.unit = 's^-1 rad' tmp_joint_value.value = 0 self.arm_velocities.velocities.append(tmp_joint_value) elif data.buttons[0]: tmp_joint_value = JointValue() tmp_joint_value.joint_uri = 'arm_joint_4' tmp_joint_value.unit = 's^-1 rad' tmp_joint_value.value = 1 * self.arm_speed_multiplier self.arm_velocities.velocities.append(tmp_joint_value) elif data.buttons[1]: tmp_joint_value = JointValue() tmp_joint_value.joint_uri = 'arm_joint_4' tmp_joint_value.unit = 's^-1 rad' tmp_joint_value.value = -1 * self.arm_speed_multiplier self.arm_velocities.velocities.append(tmp_joint_value) # 5 joint if notXOR(data.buttons[4], data.buttons[5]): tmp_joint_value = JointValue() tmp_joint_value.joint_uri = 'arm_joint_5' tmp_joint_value.unit = 's^-1 rad' tmp_joint_value.value = 0 self.arm_velocities.velocities.append(tmp_joint_value) elif data.buttons[4]: tmp_joint_value = JointValue() tmp_joint_value.joint_uri = 'arm_joint_5' tmp_joint_value.unit = 's^-1 rad' tmp_joint_value.value = 1 * self.arm_speed_multiplier self.arm_velocities.velocities.append(tmp_joint_value) elif data.buttons[5]: tmp_joint_value = JointValue() tmp_joint_value.joint_uri = 'arm_joint_5' tmp_joint_value.unit = 's^-1 rad' tmp_joint_value.value = -1 * self.arm_speed_multiplier self.arm_velocities.velocities.append(tmp_joint_value)
#!/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'
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() jv4.joint_uri = "arm_joint_4"
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)
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()
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")
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() jv4.joint_uri = "arm_joint_4"
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) rospy.sleep(1.0)