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 get_joint_velocities(self, dx, dy): if (not self.armInitialized): dx = 0 dy = 0 mvx = 0 mvy = 0 else: mvx = self.maxVelX; mvy = self.maxVelY; if (dx>0): if self.jointConstraints[0][1]-self.jointPositions[0]<self.threshold: mvx = (self.jointConstraints[0][1]-self.jointPositions[0])*self.maxVelX; #print self.jointPositions[0], "mvx\t", mvx else: if -self.jointConstraints[0][0]+self.jointPositions[0]<self.threshold: mvx = (-self.jointConstraints[0][0]+self.jointPositions[0])*self.maxVelX; #print self.jointPositions[0], "mvx\t", mvx #print self.jointConstraints[1], "\t", self.jointPositions[1]#, "\t", (self.jointConstraints[1][1]-self.jointPositions[1]) if (dy>0): if self.jointConstraints[1][1]-self.jointPositions[1]<self.threshold: mvy = (self.jointConstraints[1][1]-self.jointPositions[1])*self.maxVelY; #print self.jointPositions[1], "mvy\t", mvy else: if -self.jointConstraints[1][0]+self.jointPositions[1]<self.threshold: mvy = (-self.jointConstraints[1][0]+self.jointPositions[1])*self.maxVelY; #print self.jointPositions[1], "mvy\t", mvy jv = JointVelocities() jv.velocities.append(JointValue()) jv.velocities[-1].unit = "s^-1 rad" jv.velocities[-1].joint_uri = 'arm_joint_1' jv.velocities[-1].value = dx*mvx; jv.velocities.append(JointValue()) jv.velocities[-1].unit = "s^-1 rad" jv.velocities[-1].joint_uri = 'arm_joint_2' jv.velocities[-1].value = dy*mvy; jv.velocities.append(JointValue()) jv.velocities[-1].unit = "s^-1 rad" jv.velocities[-1].joint_uri = 'arm_joint_3' jv.velocities[-1].value = 0; jv.velocities.append(JointValue()) jv.velocities[-1].unit = "s^-1 rad" jv.velocities[-1].joint_uri = 'arm_joint_4' jv.velocities[-1].value = 0; jv.velocities.append(JointValue()) jv.velocities[-1].unit = "s^-1 rad" jv.velocities[-1].joint_uri = 'arm_joint_5' jv.velocities[-1].value = 0; return jv
def jsPoscontrol(self, qd): #Initialize Position Command Msgs qposMsg = JointPositions() qposMsg.positions = [ JointValue(), JointValue(), JointValue(), JointValue(), JointValue() ] #Fill Position Msg qposMsg.positions[0].joint_uri = 'arm_joint_1' qposMsg.positions[0].unit = 'rad' qposMsg.positions[0].value = qd[0] + self.q_offset[0] qposMsg.positions[1].joint_uri = 'arm_joint_2' qposMsg.positions[1].unit = 'rad' qposMsg.positions[1].value = qd[1] + self.q_offset[1] qposMsg.positions[2].joint_uri = 'arm_joint_3' qposMsg.positions[2].unit = 'rad' qposMsg.positions[2].value = qd[2] + self.q_offset[2] qposMsg.positions[3].joint_uri = 'arm_joint_4' qposMsg.positions[3].unit = 'rad' qposMsg.positions[3].value = qd[3] + self.q_offset[3] qposMsg.positions[4].joint_uri = 'arm_joint_5' qposMsg.positions[4].unit = 'rad' qposMsg.positions[4].value = qd[4] + self.q_offset[4] #Publish Position Msg self.pub_qpos.publish(qposMsg)
def make_brics_msg_gripper(cls, arm_num, opening_m): # Turn a desired gripper opening into a brics-friendly message left = opening_m/2.0 right = opening_m/2.0 # create joint positions message jp = JointPositions() # create joint values message for both left and right fingers jvl = JointValue() jvr = JointValue() # Fill in the gripper positions desired # This is open position (max opening 0.0115 m) if arm_num == 1: jvl.joint_uri = 'gripper_finger_joint_l' jvr.joint_uri = 'gripper_finger_joint_r' else: jvl.joint_uri = 'gripper_' + str(arm_num) + '_finger_joint_l' jvr.joint_uri = 'gripper_' + str(arm_num) + '_finger_joint_r' jvl.unit = 'm' jvl.value = left jvr.unit = 'm' jvr.value = right # Append those onto JointPositions jp.positions.append(copy.deepcopy(jvl)) jp.positions.append(copy.deepcopy(jvr)) return jp
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 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 get_zero_velocities(self): jv = JointVelocities() jv.velocities.append(JointValue()) jv.velocities[-1].unit = "s^-1 rad" jv.velocities[-1].joint_uri = 'arm_joint_1' jv.velocities[-1].value = 0 jv.velocities.append(JointValue()) jv.velocities[-1].unit = "s^-1 rad" jv.velocities[-1].joint_uri = 'arm_joint_2' jv.velocities[-1].value = 0 jv.velocities.append(JointValue()) jv.velocities[-1].unit = "s^-1 rad" jv.velocities[-1].joint_uri = 'arm_joint_3' jv.velocities[-1].value = 0 jv.velocities.append(JointValue()) jv.velocities[-1].unit = "s^-1 rad" jv.velocities[-1].joint_uri = 'arm_joint_4' jv.velocities[-1].value = 0 jv.velocities.append(JointValue()) jv.velocities[-1].unit = "s^-1 rad" jv.velocities[-1].joint_uri = 'arm_joint_5' jv.velocities[-1].value = 0 return jv
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 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 Set(self,value): grp_cmd = JointPositions() j_cmd = JointValue() j_cmd.joint_uri = self.joint_name j_cmd.unit = 'm' j_cmd.value = value grp_cmd.positions.append(j_cmd) print "Sending gripper command: " + str(j_cmd.value) self._GripperCmdPublisher.publish(grp_cmd)
def create_joint_value_msg_list(self, joint_values, unit): joint_value_msg_list = [] for i, joint_value in enumerate(joint_values): joint_value_msg = JointValue() joint_value_msg.joint_uri = 'arm_joint_' + str(i + 1) joint_value_msg.unit = unit joint_value_msg.value = joint_value joint_value_msg_list.append(joint_value_msg) return joint_value_msg_list
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 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 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 createGripperPositionCommand(newPos, t): msg = JointPositions() joint = JointValue(timeStamp=t, value=newPos, unit="m", joint_uri="gripper_finger_joint_l") msg.positions.append(joint) joint.joint_uri = "gripper_finger_joint_r" msg.positions.append(joint) return msg
def _createJointPositions(self, joint_config): jp = JointPositions() for i in range(5): jv = JointValue() jv.joint_uri = self.joint_names[i] jv.value = joint_config[i] jv.unit = "rad" jp.positions.append(jv) return jp
def to_joint_positions(gripper_positions): robot = os.getenv("ROBOT") if robot == "youbot-edufill": left_gripper_val = gripper_positions[0] right_gripper_val = gripper_positions[1] pub = rospy.Publisher("/arm_1/gripper_controller/position_command", JointPositions) rospy.sleep(0.5) try: jp = JointPositions() jv1 = JointValue() jv1.joint_uri = "gripper_finger_joint_l" jv1.value = left_gripper_val jv1.unit = "m" jv2 = JointValue() jv2.joint_uri = "gripper_finger_joint_r" jv2.value = right_gripper_val jv2.unit = "m" p = Poison() jp.poisonStamp = p jp.positions = [jv1, jv2] # list pub.publish(jp) rospy.sleep(1.0) rospy.loginfo("Gripper control command published successfully") except Exception, e: rospy.loginfo("%s", e)
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 create_gripper_msg(left_gripper, right_gripper): jp = JointPositions() jv1 = JointValue() jv1.joint_uri = "gripper_finger_joint_l" jv1.value = left_gripper jv1.unit = "m" jv2 = JointValue() jv2.joint_uri = "gripper_finger_joint_r" jv2.value = right_gripper jv2.unit = "m" p = Poison() jp.poisonStamp = p jp.positions = [jv1, jv2] return jp
def execute(self, goal): rospy.logdebug("Executing command goal: " + str(goal)) jnt_pos = JointPositions() for k in range(len(self.joint_names)): value = JointValue() value.joint_uri = self.joint_names[k] value.unit = "m" value.value = goal.command.position jnt_pos.positions.append(value) self.pub_joint_positions.publish(jnt_pos) rospy.sleep(2.0) self.server.set_succeeded()
def arm_up_recover(self): arm_up = create_arm_up() arm_up[0] = self.configuration[0] arm_up[4] = self.configuration[4] joint_positions = JointPositions() # transform from ROS to BRICS message for j in range(len(joint_names)): joint_value = JointValue() joint_value.joint_uri = joint_names[j] joint_value.value = limit_joint(joint_names[j], arm_up[j]) joint_value.unit = self.unit joint_positions.positions.append(joint_value) self.position_pub.publish(joint_positions) rospy.sleep(1)
def main(): pubPos = rospy.Publisher('/arm_1/arm_controller/position_command', JointPositions, queue_size=10) rospy.init_node('armcommand', anonymous=True) rate = rospy.Rate(1) # 10hz jp = JointPositions() joints = [] for i in range(0, 5): joints.append(JointValue()) joints[i].joint_uri = "arm_joint_" + str(i + 1) joints[i].unit = "rad" while not rospy.is_shutdown(): joints[0].value = 3 joints[1].value = 1 joints[2].value = -1 joints[3].value = 1.85 joints[4].value = 3 jp.positions = joints pubPos.publish(jp) rate.sleep()
def get_joint_pos_msg_from_joint_angles(joint_angles): """ Converts a list of joint angles to a JointPositions message :joint_angles: list(float) :returns: brics_actuator.JointPositions """ joint_position_msg = JointPositions() for i, joint_angle in enumerate(joint_angles): joint_value_msg = JointValue() joint_value_msg.joint_uri = 'arm_joint_' + str(i + 1) joint_value_msg.unit = 'rad' joint_value_msg.value = joint_angle joint_position_msg.positions.append(joint_value_msg) return joint_position_msg
def velPub(): rospy.init_node("velPub", anonymous=True) component_name = "arm" component_dof = 7 pub = rospy.Publisher("/" + component_name + "_controller/command_vel", JointVelocities, queue_size=1) vel = JointVelocities() for x in range(0, component_dof): vel.velocities.append(JointValue()) r = rospy.Rate(100) t0 = rospy.get_time() A = 0.1 #amplitude w = 1.0 #frequency phi = 0.0 #phase shift while not rospy.is_shutdown(): t = rospy.get_time() vel.velocities[1].value = A * math.sin(w * (t - t0) + phi) #print vel pub.publish(vel) r.sleep()
def poseCallback(simple_msg): hard_msg = JointPositions() for i in range(5): hard_msg.positions.append(JointValue()) hard_msg.positions[i].joint_uri = 'arm_joint_' + str(i + 1) hard_msg.positions[i].unit = 'rad' hard_msg.positions[i].value = simple_msg.data[i] pose_pub.publish(hard_msg)
def publish_arm_joint_positions(self, joint_positions): desiredPositions = JointPositions() jointCommands = [] for i in range(5): joint = JointValue() joint.joint_uri = "arm_joint_" + str(i+1) joint.unit = "rad" joint.value = joint_positions[i] jointCommands.append(joint) desiredPositions.positions = jointCommands self.arm_joint_pub.publish(desiredPositions)
def go_with2(self, torques): jt_msg = JointTorques() for i in range(N): jt_msg.torques.append(JointValue()) jt_msg.torques[i].joint_uri = 'arm_joint_' + str(i + 1) jt_msg.torques[i].unit = 'm^2 kg s^-2 rad^-1' jt_msg.torques[i].value = torques[i] self.jt_pub.publish(jt_msg)
def publish_arm_joint_velocities(self, Joint_Velocities): desiredVelocities = JointVelocities() jointCommands = [] for i in range(5): joint = JointValue() joint.joint_uri = "arm_joint_" + str(i + 1) joint.unit = "s^-1 rad" joint.value = Joint_Velocities[i] jointCommands.append(joint) desiredVelocities.velocities = jointCommands self.arm_joint_vel_pub.publish(desiredVelocities)
def go_to(self, position): jp_msg = JointPositions() for i in range(N): jp_msg.positions.append(JointValue()) jp_msg.positions[i].joint_uri = 'arm_joint_' + str(i + 1) jp_msg.positions[i].unit = 'rad' jp_msg.positions[i].value = position[i] self.jp_pub.publish(jp_msg)
def go_with(self, velocities): jv_msg = JointVelocities() for i in range(N): jv_msg.velocities.append(JointValue()) jv_msg.velocities[i].joint_uri = 'arm_joint_' + str(i + 1) jv_msg.velocities[i].unit = 's^-1 rad' jv_msg.velocities[i].value = velocities[i] self.jv_pub.publish(jv_msg)
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 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 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 set_joints_angles(self, *args): # TODO: remove *args u"""Set arm joints to defined angles in radians. Arguments: *args -- joints angles (j1, j2, j3, j4, j5) Устанавливает углы поворота степеней подвижности в радианах. Аргументы: *args -- уголы соотвествующих степеней (j1, j2, j3, j4, j5) """ assert len(args) <= 5 self.joints_positions.positions = [] for i in range(5): tmp = JointValue() tmp.timeStamp = rospy.Time.now() tmp.joint_uri = 'arm_joint_{}'.format(i + 1) tmp.unit = 'rad' tmp.value = args[i] self.joints_positions.positions.append(tmp) self.joints_positions_publisher.publish(self.joints_positions)
def moveGripper(self, gPublisher, floatVal): jp = JointPositions() myPoison = Poison() myPoison.originator = 'yb_grip' myPoison.description = 'whoknows' myPoison.qos = 0.0 jp.poisonStamp = myPoison nowTime = rospy.Time.now() jvl = JointValue() jvl.timeStamp = nowTime jvl.joint_uri = 'gripper_finger_joint_l' jvl.unit = 'm' jvl.value = floatVal jp.positions.append(jvl) jvr = JointValue() jvr.timeStamp = nowTime jvr.joint_uri = 'gripper_finger_joint_r' jvr.unit = 'm' jvr.value = floatVal jp.positions.append(jvr) gPublisher.publish(jp)
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)
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 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 create_gripper_msg (left_gripper, right_gripper): jp = JointPositions() jv1 = JointValue() jv1.joint_uri = "gripper_finger_joint_l" jv1.value = left_gripper jv1.unit = "m" jv2 = JointValue() jv2.joint_uri = "gripper_finger_joint_r" jv2.value = right_gripper jv2.unit = "m" p = Poison() jp.poisonStamp = p jp.positions = [jv1, jv2] return jp
def make_movements_callback(req): global file_with_data jp_pub = rospy.Publisher("arm_1/arm_controller/position_command", JointPositions, queue_size=10, latch=True) jp_msg = JointPositions() for i in range(5): jp_msg.positions.append(JointValue()) jp_msg.positions[i].joint_uri = 'arm_joint_' + str(i + 1) jp_msg.positions[i].unit = 'rad' file_with_poses = open(fwp_name, "r") string_of_poses = file_with_poses.readlines() file_with_poses.close() js_sub = rospy.Subscriber("joint_states", JointState, js_callback) r = rospy.Rate(1.0) for pose in string_of_poses: angles = [float(angle) for angle in pose.split()] for i in range(5): jp_msg.positions[i].value = angles[i] jp_pub.publish(jp_msg) achieved = False while not achieved: achieved = True r.sleep() lock.acquire() for i in range(5): if abs(angles[i] - cur_angles[i]) > 0.2: achieved = False break lock.release() js_sub.unregister() # for returning to startup (embryo) position for i in range(5): jp_msg.positions[i].value = 0.025 jp_msg.positions[2].value = -0.02 jp_msg.positions[4].value = 0.12 jp_pub.publish(jp_msg) lock.acquire() file_with_data.close() lock.release() return EmptyResponse()
def _createGripperJointPositions(self, left, right): jp = JointPositions() jv1 = JointValue() jv1.joint_uri = "gripper_finger_joint_l" jv1.value = left jv1.unit = "m" jv2 = JointValue() jv2.joint_uri = "gripper_finger_joint_r" jv2.value = right jv2.unit = "m" p = Poison() jp.poisonStamp = p jp.positions = [jv1, jv2] #list return jp
joint_position_string_pub = rospy.Publisher("/robust_position_controller/event_in", String, queue_size=100) joint_position_msg = JointPositions() string_msg= String() string_msg.data = "e_start" joint_names = ["arm_joint_1" , "arm_joint_5"] joint_value_=[4.0, 4.0] for i in range (0,len(joint_names)): joint_value_msg = JointValue() joint_value_msg.joint_uri = joint_names[i] joint_value_msg.value = joint_value_[i] joint_value_msg.unit="rad" joint_position_msg.positions.append(joint_value_msg) loop_rate = rospy.Rate(10) while not rospy.is_shutdown(): joint_position_pub.publish(joint_position_msg) rospy.sleep(0.5) joint_position_string_pub.publish(string_msg) loop_rate.sleep()
import sys, select, termios, tty, signal if __name__=="__main__": pub = rospy.Publisher('/arm_1/gripper_controller/position_command', JointPositions) rospy.init_node('simple_gripper_joint_position') rospy.sleep(0.5) try: jp = JointPositions() # if value == 0: gripper is closed # if value == 0.0115: griper is open jv1 = JointValue() jv1.joint_uri = "gripper_finger_joint_l" jv1.value = 0.01 jv1.unit = "m" jv2 = JointValue() jv2.joint_uri = "gripper_finger_joint_r" jv2.value = 0.01 jv2.unit = "m" p = Poison() jp.poisonStamp = p jp.positions = [jv1, jv2] #list
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()
from brics_actuator.msg import JointPositions, JointValue, Poison import sys, select, termios, tty, signal if __name__=="__main__": pub = rospy.Publisher('arm_1/arm_controller/position_command', JointPositions) rospy.init_node('simple_arm_gripper_position') rospy.sleep(0.5) try: jp = JointPositions() jv1 = JointValue() jv1.joint_uri = "arm_joint_1" jv1.value = 0.011 jv1.unit = "rad" jv2 = JointValue() jv2.joint_uri = "arm_joint_2" jv2.value = 0.011 jv2.unit = "rad" jv3 = JointValue() jv3.joint_uri = "arm_joint_3" jv3.value = -0.016 jv3.unit = "rad" jv4 = JointValue()
def make_arm_msg(arm_js, joint_uri): A1 = arm_js[0] A2 = arm_js[1] A3 = arm_js[2] A4 = arm_js[3] A5 = arm_js[4] # create joint positions message jp = JointPositions() # create joint values message for all the joints jv1 = JointValue() jv2 = JointValue() jv3 = JointValue() jv4 = JointValue() jv5 = JointValue() # Fill in the arm positions. jv1.joint_uri = joint_uri[0] jv1.unit = 'rad' jv1.value = A1 jv2.joint_uri = joint_uri[1] jv2.unit = 'rad' jv2.value = A2 jv3.joint_uri = joint_uri[2] jv3.unit = 'rad' jv3.value = A3 jv4.joint_uri = joint_uri[3] jv4.unit = 'rad' jv4.value = A4 jv5.joint_uri = joint_uri[4] jv5.unit = 'rad' jv5.value = A5 # Append those onto JointPositions jp.positions.append(copy.deepcopy(jv1)) jp.positions.append(copy.deepcopy(jv2)) jp.positions.append(copy.deepcopy(jv3)) jp.positions.append(copy.deepcopy(jv4)) jp.positions.append(copy.deepcopy(jv5)) return jp
if __name__=="__main__": pub = rospy.Publisher('arm_1/arm_controller/position_command', JointPositions) rospy.init_node('simple_arm_gripper_position') rospy.sleep(0.5) try: jp = JointPositions() jv1 = JointValue() jv1.joint_uri = "arm_joint_1" jv1.unit = "rad" jv2 = JointValue() jv2.joint_uri = "arm_joint_2" jv2.unit = "rad" jv3 = JointValue() jv3.joint_uri = "arm_joint_3" jv3.unit = "rad" jv4 = JointValue() jv4.joint_uri = "arm_joint_4" jv4.unit = "rad"
pose.pose.position.z = z quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw) pose.pose.orientation.x = quat[0] pose.pose.orientation.y = quat[1] pose.pose.orientation.z = quat[2] pose.pose.orientation.w = quat[3] pose.header.frame_id = "/arm_link_0" pose.header.stamp = rospy.Time.now() # (conf, success) = iks.call_ik_solver(pose) (conf, success) = iks.call_constraint_aware_ik_solver(pose) if (success): # publish solution directly as joint positions print(conf) jp = JointPositions() for i in range(5): jv = JointValue() jv.joint_uri = iks.joint_names[i] jv.value = conf[i] jv.unit = "rad" jp.positions.append(jv) rospy.sleep(0.5) print "publishing cmd" armpub.publish(jp) else: print("IK solver didn't find a solution")
def execute(self, component_name, target="", blocking=True): ah = ActionHandle("move_arm_joint_direct", component_name, target, blocking) rospy.loginfo("Move <<%s>> DIRECT to <<%s>>", component_name, target) # check pose if not type(target) is list: # check outer list rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name) print "parameter is:", target ah.set_failed(3) return ah else: #print i,"type1 = ", type(i) DOF = 5 if not len(target) == DOF: # check dimension rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...", component_name, DOF, len(target)) print "parameter is:", target ah.set_failed(3) return ah else: for i in target: #print i,"type2 = ", type(i) if not ((type(i) is float) or (type(i) is int)): # check type #print type(i) rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...", component_name) print "parameter is:", target ah.set_failed(3) return ah else: rospy.logdebug("accepted parameter %f for %s", i, component_name) pose_goal = MoveToJointConfigurationGoal() for i in range(DOF): jv = JointValue() jv.joint_uri = self.arm1_joint_names[i] jv.value = target[i] jv.unit = "rad" pose_goal.goal.positions.append(jv) rospy.logdebug("calling %s action server", self.action_server_name) # trying to connect to server rospy.logdebug("waiting for %s action server to start", self.action_server_name) if not self.client.wait_for_server(rospy.Duration(5)): # error: server did not respond rospy.logerr("%s action server not ready within timeout, aborting...", self.action_server_name) ah.set_failed(4) return ah else: rospy.logdebug("%s action server ready", self.action_server_name) #print client_goal self.client.send_goal(pose_goal) ah.set_client(self.client) ah.wait_inside() return ah
import copy from cob_srvs.srv import Trigger from brics_actuator.msg import JointVelocities from brics_actuator.msg import JointValue rospy.init_node("ipa_canopen_test") rospy.wait_for_service('/tray_controller/init') print "found init" initService = rospy.ServiceProxy('/tray_controller/init', Trigger) resp = initService() velPublisher = rospy.Publisher('/tray_controller/command_vel', JointVelocities) rospy.sleep(2.0) v = JointVelocities() vv1 = JointValue() vv1.timeStamp = rospy.Time.now() vv1.joint_uri = "tray_1_joint" vv2 = copy.deepcopy(vv1) vv2.joint_uri = "tray_2_joint" vv3 = copy.deepcopy(vv1) vv3.joint_uri = "tray_3_joint" v.velocities = [vv1,vv2,vv3] v.velocities[0].value = 0.0 while not rospy.is_shutdown(): v.velocities[0].value = 0.05 v.velocities[1].value = 0.05 velPublisher.publish(v)
def joint_velocities(joint_velocities): robot = os.getenv('ROBOT') if robot == 'youbot-edufill': joint_velocity_1 = joint_velocities[0] joint_velocity_2 = joint_velocities[1] joint_velocity_3 = joint_velocities[2] joint_velocity_4 = joint_velocities[3] joint_velocity_5 = joint_velocities[4] pub = rospy.Publisher('arm_1/arm_controller/velocity_command', JointVelocities) rospy.sleep(0.5) try: jv = JointVelocities() jv1 = JointValue() jv1.joint_uri = "arm_joint_1" jv1.value = joint_velocity_1 jv1.unit = "s^-1 rad" jv2 = JointValue() jv2.joint_uri = "arm_joint_2" jv2.value = joint_velocity_2 jv2.unit = "s^-1 rad" jv3 = JointValue() jv3.joint_uri = "arm_joint_3" jv3.value = joint_velocity_3 jv3.unit = "s^-1 rad" jv4 = JointValue() jv4.joint_uri = "arm_joint_4" jv4.value = joint_velocity_4 jv4.unit = "s^-1 rad" jv5 = JointValue() jv5.joint_uri = "arm_joint_5" jv5.value = joint_velocity_5 jv5.unit = "s^-1 rad" p = Poison() #print p jv.poisonStamp = p jv.velocities = [jv1, jv2, jv3, jv4, jv5] pub.publish(jv) return 'arm moved successfully' except Exception, e: print e return 'arm move failure'
def processJoints(self, joint_states): armStatesReceived = joint_states.name[0] == 'arm_joint_1' if armStatesReceived and self.joyMsg is not None: self.crntJointStates = copy.deepcopy(joint_states) for index in range(0, len(joint_states.position)-2): pos = JointValue() pos.joint_uri = "arm_joint_" + str(index + 1) pos.unit = "rad" pos.value = joint_states.position[index] self.crntJointPositions.positions.append(pos) pos = JointValue() pos.joint_uri = "gripper_finger_joint_l" pos.unit = "m" pos.value = joint_states.position[5] self.gripperPositions.positions.append(pos) pos = JointValue() pos.joint_uri = "gripper_finger_joint_r" pos.unit = "m" pos.value = joint_states.position[6] self.gripperPositions.positions.append(pos) # desiredPos = copy.deepcopy(self.crntJointPositions) desiredGripPos = copy.deepcopy(self.gripperPositions) self.joyMsg.axes = numpy.array(self.joyMsg.axes) left_axis = self.joyMsg.axes[[0, 1]] right_axis = self.joyMsg.axes[[2, 5]] l2_axis = self.joyMsg.axes[[3]] * -1 r2_axis = self.joyMsg.axes[[4]] * -1 square, x, circle, triangle, l1, r1, l2, r2, share, options, l3, r3, ps_button, touch_button = [button == 1 for button in self.joyMsg.buttons] left_dpad = self.joyMsg.axes[6] == 1 right_dpad = self.joyMsg.axes[6] == -1 up_dpad = self.joyMsg.axes[7] == 1 down_dpad = self.joyMsg.axes[7] == -1 if not self.previous_vel_change: if square: self.maxVel += 0.25 if circle: self.maxVel -= 0.25 if self.maxVel < 0.25: self.maxVel = 0.25 self.previous_vel_change = square or circle # object that will carry all desired velocities to each joint desiredVel = JointVelocities() for index in range(0, len(joint_states.position)-2): vel = JointValue() vel.joint_uri = "arm_joint_" + str(index + 1) vel.unit = "s^-1 rad" # radians per second desiredVel.velocities.append(vel) desiredVel.velocities[0].value -= left_axis[0] * self.maxVel desiredVel.velocities[1].value -= left_axis[1] * self.maxVel if not r1: desiredVel.velocities[2].value -= right_axis[1] * self.maxVel else: desiredVel.velocities[3].value -= right_axis[1] * self.maxVel desiredVel.velocities[4].value -= right_axis[0] * self.maxVel * 2 desiredGripPos.positions[0].value -= 0.0115 * r2_axis[0] desiredGripPos.positions[1].value = desiredGripPos.positions[0].value joint_message = '' vel_message = '' grip_message = '' for jointNum in range(5): joint_message += ('joint ' + str(jointNum) + ': ' + str(self.crntJointPositions.positions[jointNum].value) + ' ') vel_message += ('veloc ' + str(jointNum) + ': ' + str(desiredVel.velocities[jointNum].value) + ' ') for gripNum in [0, 1]: desiredGripPos.positions[gripNum].value = self.checkValues(desiredGripPos.positions[gripNum].value, gripNum + 5) grip_message += ('grip ' + str(gripNum) + ': ' + str(desiredGripPos.positions[gripNum].value) + ' ') self.gripperPositions = desiredGripPos print(joint_message) print(vel_message) print(grip_message) self.gripPub.publish(desiredGripPos) self.velPub.publish(desiredVel)
from brics_actuator.msg import JointPositions, JointValue, Poison import sys, select, termios, tty, signal if __name__=="__main__": pub = rospy.Publisher('arm_1/arm_controller/position_command', JointPositions) rospy.init_node('simple_arm_gripper_position') rospy.sleep(0.5) try: jp = JointPositions() jv1 = JointValue() jv1.joint_uri = "arm_joint_1" jv1.value = 0.0 jv1.unit = "rad" jv2 = JointValue() jv2.joint_uri = "arm_joint_2" jv2.value = 0.0 jv2.unit = "rad" jv3 = JointValue() jv3.joint_uri = "arm_joint_3" jv3.value = 0.0 jv3.unit = "rad" jv4 = JointValue()
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
#!/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'
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)