def pose(self): """ @rtype: geometry_msgs.Pose """ return conversions.list_to_pose_stamped( self._robot._r.get_link_pose(self._name), self._robot.get_planning_frame())
def ik_solver_request(input_limb, input_pose): print "IK solver request:" #input error checking if len(input_pose) == 6: quaternion_pose = conversions.list_to_pose_stamped(input_pose, "base") elif len(input_pose) == 7: quaternion_pose = input_pose else: print """Invalid Pose List: Input Pose to function: ik_solver_request must be a list of: 6 elements for an RPY pose, or 7 elements for a Quaternion pose""" return if input_limb == "right" or input_limb == "left": limb = input_limb else: print """Invalid Limb: Input Limb to function: ik_solver_request must be a string: 'right' or 'left'""" return #request/response handling #print quaternion_pose node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" ik_service = rospy.ServiceProxy(node, SolvePositionIK) ik_request = SolvePositionIKRequest() ik_request.pose_stamp.append(quaternion_pose) try: rospy.wait_for_service(node, 5.0) ik_response = ik_service(ik_request) except (rospy.ServiceException, rospy.ROSException), error_message: raise rospy.logerr("Service request failed: %r" % (error_message,))
def ikSolver(self,x,y,z,a,b,c,d): rpy_pose = (x,y,z,a,b,c,d) quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base") node = "ExternalTools/left/PositionKinematicsNode/IKService" ik_service = rospy.ServiceProxy(node, SolvePositionIK) ik_request = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id="base") ik_request.pose_stamp.append(quaternion_pose) try: rospy.wait_for_service(node, 10.0) ik_response = ik_service(ik_request) if ik_response.isValid[0]: print("Valid joint configuration found") # convert response to joint position control dictionary limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position)) # move limb self.limb.move_to_joint_positions(limb_joints) rospy.sleep(1) self.setTheta() rospy.sleep(1) return 0 else: print "Failed" return 1 except (rospy.ServiceException, rospy.ROSException), error_message: rospy.logerr("Service request failed: %r" % (error_message,)) sys.exit("ERROR - baxter_ik_move - Failed to append pose")
def to_stamped_pose(pose): """Take a xyzrpy or xyz qxqyqzqw pose and convert it to a stamped pose (quaternion as orientation). """ stamped_pose = conversions.list_to_pose_stamped(pose, "base") return stamped_pose
def callback(data): x = data.data[0] y = data.data[1] print x,y limb = baxter_interface.Limb('left') pose = limb.endpoint_pose() pose = limb.endpoint_pose() xw,yw,zw = pose['position'] a,b,c,d = pose['orientation'] print xw,yw,zw xw = xw + 0.01*x yw = yw + 0.01*y rpy_pose = (xw, yw, zw, 0, math.pi,0) quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base") node = "ExternalTools/" + 'left'+ "/PositionKinematicsNode/IKService" ik_service = rospy.ServiceProxy(node, SolvePositionIK) ik_request = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id="base") ik_request.pose_stamp.append(quaternion_pose) try: rospy.wait_for_service(node, 5.0) ik_response = ik_service(ik_request) except (rospy.ServiceException, rospy.ROSException), error_message: rospy.logerr("Service request failed: %r" % (error_message,)) sys.exit("ERROR - baxter_ik_move - Failed to append pose")
def solve_ik(input_limb, input_pose): # print("IK requested for", input_limb) if len(input_pose) != 6: print("6 inputs required!") return if input_limb == "right" or input_limb == "left": limb = input_limb else: print """Invalid Limb: Input Limb to function: ik_solver_request must be a string: 'right' or 'left'""" #convert the Eulers RPY into quaternion xyzw quaternion_pose = conversions.list_to_pose_stamped(input_pose, "base") #starting the IK request node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" ik_service = rospy.ServiceProxy(node, SolvePositionIK) ik_requestmessage = SolvePositionIKRequest() ik_requestmessage.pose_stamp.append(quaternion_pose) # QUICK & DIRTY: MOLT THIS INTO AN TRY and EXCEPT handler!! rospy.wait_for_service(node, 5.0) ik_response = ik_service(ik_requestmessage) if (ik_response.isValid[0]): print("Jay! Valid joint solution found") #convert response to JP control dict limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position)) return limb_joints else: print("FAILED: No valid joint configuration for this pose found")
def pose_limb(self, limb, pose): #Convert the pose to a quaternion, then request an IK solution quat_pose = conversions.list_to_pose_stamped(pose, "base") node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" ik_service = rospy.ServiceProxy(node, SolvePositionIK) ik_request = SolvePositionIKRequest() header = Header(stamp=rospy.Time.now(), frame_id="base") ik_request.pose_stamp.append(quat_pose) #Wait for solution response rospy.wait_for_service(node, 5.0) ik_response = ik_service(ik_request) if ik_response.isValid[0]: #Convert the solution to a joint position control dictionary and move limb_joints = dict( zip(ik_response.joints[0].name, ik_response.joints[0].position)) baxter_interface.Limb(limb).move_to_joint_positions(limb_joints) else: sys.exit( "pose_limb: A valid joint configuration could not be found") #The result pose only matters for the right arm if limb == "right": quat_pose = baxter_interface.Limb("right").endpoint_pose() #Update the manipulator's current pose #Orientation shouldn't change, also endpoint_pose() returns orientation #as a quaternion and that's annoying to use to update pose self.curr_pose = pose self.curr_pose[0] = quat_pose["position"][0] self.curr_pose[1] = quat_pose["position"][1] self.curr_pose[2] = quat_pose["position"][2]
def ik_solver_request(input_limb, input_pose): print "IK solver request:" # input error checking if len(input_pose) == 6: quaternion_pose = conversions.list_to_pose_stamped(input_pose, "base") elif len(input_pose) == 7: quaternion_pose = input_pose else: print """Invalid Pose List: Input Pose to function: ik_solver_request must be a list of: 6 elements for an RPY pose, or 7 elements for a Quaternion pose""" return if input_limb == "right" or input_limb == "left": limb = input_limb else: print """Invalid Limb: Input Limb to function: ik_solver_request must be a string: 'right' or 'left'""" return # request/response handling node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" ik_service = rospy.ServiceProxy(node, SolvePositionIK) ik_request = SolvePositionIKRequest() ik_request.pose_stamp.append(quaternion_pose) try: rospy.wait_for_service(node, 5.0) ik_response = ik_service(ik_request) except (rospy.ServiceException, rospy.ROSException), error_message: rospy.logerr("Service request failed: %r" % (error_message,))
def get_random_pose(self, end_effector_link=""): if len(end_effector_link) > 0 or self.has_end_effector_link(): return conversions.list_to_pose_stamped( self._g.get_random_pose(end_effector_link), self.get_planning_frame()) else: raise MoveItCommanderException( "There is no end effector to get the pose of")
def get_current_pose(self, end_effector_link=""): """ Get the current pose of the end-effector of the group. Throws an exception if there is not end-effector. """ if len(end_effector_link) > 0 or self.has_end_effector_link(): return conversions.list_to_pose_stamped( self._g.get_current_pose(end_effector_link), self.get_planning_frame()) else: raise MoveItCommanderException( "There is no end effector to get the pose of")
def transform(rpy_pose): limb = 'left' node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" ik_service = rospy.ServiceProxy(node, SolvePositionIK) quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base") ik_request = SolvePositionIKRequest() ik_request.pose_stamp.append(quaternion_pose) ik_response = ik_service(ik_request) # limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position)) # ik_response.joints[0].position = [s0, s1, e0, e1, w0, w1, w2], this is also the target point we want. return(ik_response.joints[0].position)
def get_ik_solution(self,rpy_pose): quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base") node = "ExternalTools/" + "right" + "/PositionKinematicsNode/IKService" ik_service = rospy.ServiceProxy(node, SolvePositionIK) ik_request = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id="base") ik_request.pose_stamp.append(quaternion_pose) try: rospy.wait_for_service(node, 15.0) # 5改成了15 ik_response = ik_service(ik_request) except (rospy.ServiceException, rospy.ROSException), error_message: rospy.logerr("Service request failed: %r" % (error_message,)) sys.exit("ERROR - baxter_ik_move - Failed to append pose")
def ik_move(self, limb, rpy_pose, print_result=False): quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base") node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" ik_service = rospy.ServiceProxy(node, SolvePositionIK) ik_request = SolvePositionIKRequest() ik_request.pose_stamp.append(quaternion_pose) try: rospy.wait_for_service(node, 5.0) ik_response = ik_service(ik_request) except (rospy.ServiceException, rospy.ROSException), error_message: rospy.logerr("Service request failed: %r" % (error_message, )) print "ERROR - baxter_ik_move - Failed to append pose"
def baxter_ik_move(self, limb, rpy_pose): quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base") node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" ik_service = rospy.ServiceProxy(node, SolvePositionIK) ik_request = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id="base") ik_request.pose_stamp.append(quaternion_pose) try: rospy.wait_for_service(node, 5.0) ik_response = ik_service(ik_request) except (rospy.ServiceException, rospy.ROSException), error_message: rospy.logerr("Service request failed: %r" % (error_message,)) sys.exit("ERROR - baxter_ik_move - Failed to append pose")
def _move(self, rpy_pose, move=False): print 'move:', rpy_pose quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base") node = "ExternalTools/" + self.limb + "/PositionKinematicsNode/IKService" ik_service = rospy.ServiceProxy(node, SolvePositionIK) ik_request = SolvePositionIKRequest() ik_request.pose_stamp.append(quaternion_pose) try: rospy.wait_for_service(node, 5.0) ik_response = ik_service(ik_request) except (rospy.ServiceException, rospy.ROSException), error_message: rospy.logerr("Service request failed: %r" % (error_message, )) return False
def inverse_kinematics(self, pose): """Determine the joint angles that provide a desired pose for the robot"s end-effector.""" quaternion_pose = conversions.list_to_pose_stamped(pose, "base") node = "ExternalTools/" + self.name + "/PositionKinematicsNode/IKService" ik_service = rospy.ServiceProxy(node, SolvePositionIK) ik_request = SolvePositionIKRequest() ik_request.pose_stamp.append(quaternion_pose) try: rospy.wait_for_service(node, 5.0) ik_response = ik_service(ik_request) except (rospy.ServiceException, rospy.ROSException), error_message: rospy.logerr("Service request failed: {}".format(error_message, )) sys.exit("ERROR: Failed to append pose.")
def move_by_list(self, group, position_list): pose = conversions.list_to_pose_stamped(position_list, "base") group.set_pose_target(pose) group.go() rospy.sleep(0.5)
def to_stamped_pose(pose): stamped_pose = conversions.list_to_pose_stamped(pose, "base") return stamped_pose
def pose(self): """ @rtype: geometry_msgs.Pose """ return conversions.list_to_pose_stamped(self._robot._r.get_link_pose(self._name), self._robot.get_planning_frame())
limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position)) # move limb left.move_to_joint_positions(limb_joints) else: # display invalid move message on head display #self.splash_screen("Invalid", "move") # little point in continuing so exit with error message print 'failed' print "requested move =", rpy_pose sys.exit("ERROR - baxter_ik_move - No valid joint configuration found") preDropAngles = left.joint_angles() #go = raw_input('grasp?') rpy_pose = (graspPointTorso[0]-0.03, graspPointTorso[1]-0.09, np.max((graspPointTorso[2]+0.1,0.06)), a,b,c,d) #0.06 print rpy_pose quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base") node = "ExternalTools/" + 'left'+ "/PositionKinematicsNode/IKService" ik_service = rospy.ServiceProxy(node, SolvePositionIK) ik_request = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id="base") ik_request.pose_stamp.append(quaternion_pose) try: rospy.wait_for_service(node, 5.0) ik_response = ik_service(ik_request) except (rospy.ServiceException, rospy.ROSException), error_message: rospy.logerr("Service request failed: %r" % (error_message,)) sys.exit("ERROR - baxter_ik_move - Failed to append pose") if ik_response.isValid[0]:
def streamDisp(data): print 'size of data: ', sys.getsizeof(data) global cloudy,ax,fig,graspPoint,save,kinectRotation,kinectTranslation,Trans #data = do_transform_cloud(data,Trans) left = baxter_interface.Limb('left') grip = baxter_interface.Gripper('left') if(not grip.calibrated()): go = raw_input('grip cal?') grip.calibrate() grip.set_holding_force(40) #print dir(data) #print data.fields #a = [] #qq = data.deserialize_numpy(data.data,cloudy) #print type(cloudy) #print type(qq) #print qq.size go = raw_input('find?') print 'grasp parameters', graspPoint x = graspPoint[0,0] y = graspPoint[0,1] theta = graspPoint[0,2] x = int(x) y = int(y) gen2 = pc2.read_points(data, field_names = None, skip_nans=False, uvs=[[x,y]]) #print type(gen2) in_data2 = next(gen2) print'in data', in_data2 graspWorld = np.matrix([[in_data2[0]],[in_data2[1]],[in_data2[2]]]) #reflect = np.matrix([[0,1,0],[1,0,0],[0,0,1]]) #kinectRotation = np.dot(reflect,kinectRotation) graspPointTorso = np.dot(kinectRotation,graspWorld) graspPointTorso = np.add(graspPointTorso,kinectTranslation) #graspPointTorso = np.add(graspWorld,kinectTranslation) graspPointTorso = graspWorld #print 'pointTorso',graspPointTorso[0],graspPointTorso[1],graspPointTorso[2] #print 'rotated', np.dot(kinectRotation,graspWorld) angles = left.joint_angles() angles['left_w2'] = theta left.move_to_joint_positions(angles) pose = left.endpoint_pose() xw,yw,zw = pose['position'] a,b,c,d = pose['orientation'] #print 'position',xw,yw,zw #print 'orientation',a,b,c,d #rpy_pose = (0.7*in_data[1]+0.6, 0.7*in_data[0]+0.06, in_data[2]-1.0, a,b,c,d) rpy_pose = (graspPointTorso[0]-0.02, graspPointTorso[1]-0.07, graspPointTorso[2]+0.25, a,b,c,d) #prePick = left.joint_angles() print 'pose',rpy_pose go = raw_input('move?') quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base") node = "ExternalTools/" + 'left'+ "/PositionKinematicsNode/IKService" ik_service = rospy.ServiceProxy(node, SolvePositionIK) ik_request = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id="base") ik_request.pose_stamp.append(quaternion_pose) try: rospy.wait_for_service(node, 5.0) ik_response = ik_service(ik_request) except (rospy.ServiceException, rospy.ROSException), error_message: rospy.logerr("Service request failed: %r" % (error_message,)) sys.exit("ERROR - baxter_ik_move - Failed to append pose") print 'trying new angle' angles['left_w1'] = 1.2; leftLimb.move_to_joint_positions(angles) xw,yw,zw = pose['position'] a,b,c,d = pose['orientation'] rpy_pose = (graspPointTorso[0]-0.00, graspPointTorso[1]-0.07, graspPointTorso[2]+0.25, a,b,c,d) quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base") ik_request.pose_stamp.append(quaternion_pose) try: rospy.wait_for_service(node, 5.0) ik_response = ik_service(ik_request) except (rospy.ServiceException, rospy.ROSException), error_message: rospy.logerr("Service request failed: %r" % (error_message,)) sys.exit("ERROR - baxter_ik_move - Failed to append pose")
def streamDisp(data): print 'size of data: ', sys.getsizeof(data) global cloudy, ax, fig, graspPoint, save, kinectRotation, kinectTranslation, Trans #data = do_transform_cloud(data,Trans) left = baxter_interface.Limb('left') grip = baxter_interface.Gripper('left') if (not grip.calibrated()): go = raw_input('grip cal?') grip.calibrate() grip.set_holding_force(40) #print dir(data) #print data.fields #a = [] #qq = data.deserialize_numpy(data.data,cloudy) #print type(cloudy) #print type(qq) #print qq.size go = raw_input('find?') print 'grasp parameters', graspPoint x = graspPoint[0, 0] y = graspPoint[0, 1] theta = graspPoint[0, 2] x = int(x) y = int(y) gen2 = pc2.read_points(data, field_names=None, skip_nans=False, uvs=[[x, y]]) #print type(gen2) in_data2 = next(gen2) print 'in data', in_data2 graspWorld = np.matrix([[in_data2[0]], [in_data2[1]], [in_data2[2]]]) #reflect = np.matrix([[0,1,0],[1,0,0],[0,0,1]]) #kinectRotation = np.dot(reflect,kinectRotation) graspPointTorso = np.dot(kinectRotation, graspWorld) graspPointTorso = np.add(graspPointTorso, kinectTranslation) #graspPointTorso = np.add(graspWorld,kinectTranslation) graspPointTorso = graspWorld #print 'pointTorso',graspPointTorso[0],graspPointTorso[1],graspPointTorso[2] #print 'rotated', np.dot(kinectRotation,graspWorld) angles = left.joint_angles() angles['left_w2'] = theta left.move_to_joint_positions(angles) pose = left.endpoint_pose() xw, yw, zw = pose['position'] a, b, c, d = pose['orientation'] #print 'position',xw,yw,zw #print 'orientation',a,b,c,d #rpy_pose = (0.7*in_data[1]+0.6, 0.7*in_data[0]+0.06, in_data[2]-1.0, a,b,c,d) rpy_pose = (graspPointTorso[0] - 0.02, graspPointTorso[1] - 0.07, graspPointTorso[2] + 0.25, a, b, c, d) #prePick = left.joint_angles() print 'pose', rpy_pose go = raw_input('move?') quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base") node = "ExternalTools/" + 'left' + "/PositionKinematicsNode/IKService" ik_service = rospy.ServiceProxy(node, SolvePositionIK) ik_request = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id="base") ik_request.pose_stamp.append(quaternion_pose) try: rospy.wait_for_service(node, 5.0) ik_response = ik_service(ik_request) except (rospy.ServiceException, rospy.ROSException), error_message: rospy.logerr("Service request failed: %r" % (error_message, )) sys.exit("ERROR - baxter_ik_move - Failed to append pose") print 'trying new angle' angles['left_w1'] = 1.2 leftLimb.move_to_joint_positions(angles) xw, yw, zw = pose['position'] a, b, c, d = pose['orientation'] rpy_pose = (graspPointTorso[0] - 0.00, graspPointTorso[1] - 0.07, graspPointTorso[2] + 0.25, a, b, c, d) quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base") ik_request.pose_stamp.append(quaternion_pose) try: rospy.wait_for_service(node, 5.0) ik_response = ik_service(ik_request) except (rospy.ServiceException, rospy.ROSException), error_message: rospy.logerr("Service request failed: %r" % (error_message, )) sys.exit("ERROR - baxter_ik_move - Failed to append pose")
# move limb left.move_to_joint_positions(limb_joints) else: # display invalid move message on head display #self.splash_screen("Invalid", "move") # little point in continuing so exit with error message print 'failed' print "requested move =", rpy_pose sys.exit("ERROR - baxter_ik_move - No valid joint configuration found") preDropAngles = left.joint_angles() #go = raw_input('grasp?') rpy_pose = (graspPointTorso[0] - 0.03, graspPointTorso[1] - 0.09, np.max((graspPointTorso[2] + 0.1, 0.06)), a, b, c, d) #0.06 print rpy_pose quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base") node = "ExternalTools/" + 'left' + "/PositionKinematicsNode/IKService" ik_service = rospy.ServiceProxy(node, SolvePositionIK) ik_request = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id="base") ik_request.pose_stamp.append(quaternion_pose) try: rospy.wait_for_service(node, 5.0) ik_response = ik_service(ik_request) except (rospy.ServiceException, rospy.ROSException), error_message: rospy.logerr("Service request failed: %r" % (error_message, )) sys.exit("ERROR - baxter_ik_move - Failed to append pose") if ik_response.isValid[0]:
def get_random_pose(self, end_effector_link=""): if len(end_effector_link) > 0 or self.has_end_effector_link(): return conversions.list_to_pose_stamped(self._g.get_random_pose(end_effector_link), self.get_planning_frame()) else: raise MoveItCommanderException("There is no end effector to get the pose of")
def get_current_pose(self, end_effector_link=""): """ Get the current pose of the end-effector of the group. Throws an exception if there is not end-effector. """ if len(end_effector_link) > 0 or self.has_end_effector_link(): return conversions.list_to_pose_stamped(self._g.get_current_pose(end_effector_link), self.get_planning_frame()) else: raise MoveItCommanderException("There is no end effector to get the pose of")
def move_by_list(self, group, position_list): pose = conversions.list_to_pose_stamped(position_list,"base") group.set_pose_target(pose) group.go() rospy.sleep(0.5)