def _get_ik_robot(self, eef_poses, seeds=(), params=None): ik_req = SolvePositionIKRequest() for eef_pose in eef_poses: ik_req.pose_stamp.append(eef_pose) ik_req.seed_mode = ik_req.SEED_USER if len(seeds) > 0 else ik_req.SEED_CURRENT for seed in seeds: ik_req.seed_angles.append(seed.joint_state) resp = self._kinematics_services['ik']['robot']['service'].call(ik_req) solutions = [] for j, v in zip(resp.joints, resp.isValid): solutions.append(RobotState(is_diff=False, joint_state=j) if v else None) return solutions
def move_pos(des_pose): limb = "right" ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') # quat = quaternion_from_euler(des_pose[3],des_pose[4],des_pose[5]) pose = Pose() quat = Quaternion() quat.x = des_pose[3] quat.y = des_pose[4] quat.z = des_pose[5] quat.w = des_pose[6] pose.orientation = quat pose.position.x = des_pose[0] pose.position.y = des_pose[1] pose.position.z = des_pose[2] ikreq.pose_stamp = [PoseStamped(header=hdr, pose=pose)] # print ikreq try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.loginfo("Service exception")
def ik_request(self, pose): hdr = Header(stamp=rospy.Time.now(), frame_id='base') ikreq = SolvePositionIKRequest() ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose)) try: resp = self._iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) return False
def baxter_ik_move(self, rpy_pose): #quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base") quaternion_pose = convert_pose_to_msg(rpy_pose, "base") node = "ExternalTools/" + self.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, )) print "ERROR - baxter_ik_move - Failed to append pose" return 1
def baxter_sdk_ik_request(self, pose): """ Try to find a solution to the inverse kinematics of a given pose. paramters --------- pose: requested pose to find joint angles to reach it. """ header = Header(stamp=rospy.Time.now(), frame_id='base') inverse_kinematics_request = SolvePositionIKRequest() inverse_kinematics_request.pose_stamp.append( PoseStamped(header=header, pose=pose)) try: result = self.__iksvc(inverse_kinematics_request) except (ropsy.ServiceException, rospy.ROSException), e: rospy.logerr('service call failed: {}'.format(e)) return False
def ik_move(limb, pose): rospy.init_node("move_" + limb) ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') ps = PoseStamped( header=hdr, pose=pose, ) ikreq.pose_stamp.append(ps) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) return 1
def move_to(self, pose_list, arm): new_pose = self.make_pose_stamp(pose_list, \ Header(stamp=rospy.Time.now(), frame_id='base')) ns = "ExternalTools/" + arm + "/PositionKinematicsNode/IKService" print ns iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() ikreq.pose_stamp.append(new_pose) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return 0
def __init__(self): rospy.init_node("baxter_test") self._rs = baxter_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled self._rs.enable() self._rate = 5.0 #Hz #IK self.ns = "ExternalTools/left/PositionKinematicsNode/IKService" self.hdr = Header(stamp = rospy.Time(0), frame_id = "base") self.iksvc = rospy.ServiceProxy(self.ns, SolvePositionIK) self.ikreq = SolvePositionIKRequest() self._left_arm = baxter_interface.limb.Limb("left") self._right_arm = baxter_interface.limb.Limb("right") self._left_joint_names = self._left_arm.joint_names() self._right_joint_names = self._right_arm.joint_names() self._left_angles = [self._left_arm.joint_angle(joint) for joint in self._left_joint_names] self._right_angles = [self._right_arm.joint_angle(joint) for joint in self._right_joint_names]
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 solve(self, pose): posestamped = geometry_msgs.msg.PoseStamped() posestamped.pose = pose posestamped.header.stamp = rospy.Time.now() posestamped.header.frame_id = 'base' req = SolvePositionIKRequest() req.pose_stamp.append(posestamped) rospy.wait_for_service( '/ExternalTools/left/PositionKinematicsNode/IKService') try: resp = self.ik_service(req) if resp.isValid[0] == True: return resp else: rospy.logerr("Resolver without solution..........\n") return None except rospy.ServiceException as exc: rospy.logerr("Error requesting service:" + str(exc))
def calculate_joints(self): # Inverse Kinematics, could not get the task to work with this, left for later projects header = Header(stamp=rospy.Time.now(), frame_id='base') ik_request = SolvePositionIKRequest() ik_request.pose_stamp.append(PoseStamped(header=header, pose=self.pose)) ik_namespace = 'ExternalTools/' + self.limb_name + '/PositionKinematicsNode/IKService' try: ik_service = rospy.ServiceProxy(ik_namespace, SolvePositionIK) ik_response = ik_service(ik_request) ik_validation = struct.unpack('<%dB' % len(ik_response.result_type), ik_response.result_type)[0] if (ik_validation != ik_response.RESULT_INVALID): return dict(zip(ik_response.joints[0].name, ik_response.joints[0].position)) else: rospy.logerr('No Valid Pose Found!') return False except(rospy.ServiceException, rospy.ROSException), e: rospy.logerr('Service call failed: %s' % (e,)) return False
def solve(self, limb, pose): # pose = h.list_to_pose(pose_arr) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') pose = PoseStamped(header=hdr, pose=pose) ikreq.pose_stamp.append(pose) if limb is "left": ns = self.l_ns iksvc = self.l_iksvc else: ns = self.r_ns iksvc = self.r_iksvc try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return None
def ik_test(limb): rospy.init_node("rsdk_ik_service_client") ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') poses = { 'left': PoseStamped( header=hdr, pose=Pose( position=Point( x=0.657579481614, y=0.851981417433, z=0.0388352386502, ), orientation=Quaternion( x=-0.366894936773, y=0.885980397775, z=0.108155782462, w=0.262162481772, ), ), ), 'right': PoseStamped( header=hdr, pose=Pose( position=Point( x=-0.0100696086884, y=0.0856908559799, z=0.621000051498, ), ), ), } ikreq.pose_stamp.append(poses[limb]) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) return 1
def __init__(self, limb, side): # TODO: Replace limb motion with moveit motions self.side = side # TODO: JB Added # self._limb = baxter_interface.Limb(limb) self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self._limb = moveit_commander.MoveGroupCommander(limb) self._limb.set_max_velocity_scaling_factor(0.4) circle_io = baxter_interface.DigitalIO(side + '_lower_button') dash_io = baxter_interface.DigitalIO(side + '_upper_button') self.calibrating = False self.object_calib = -1 self.objects = [ 'neutral', 'Cup', 'Tea', 'Sugar', 'Left_Bread', 'Right_Bread', 'Lettuce', 'Meat' ] # self.objects = ['neutral', 'placemat', 'cup', 'plate', 'fork', 'spoon', 'knife', 'bowl', 'soda', 'wineglass'] # self.objects = ['neutral', 'cup', 'plate', 'bowl'] # self.objects = ['neutral', 'bowl'] # TODO: JB Added # self.object_pick_joint_angles = dict() self.object_pick_poses = dict() # TODO: JB Added # self.object_place_joint_angles = dict() self.object_place_poses = dict() self._gripper = baxter_interface.Gripper(side) self._gripper.calibrate() self._gripper.set_holding_force(100.0) ik_srv = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" self._iksvc = rospy.ServiceProxy(ik_srv, SolvePositionIK) self._ikreq = SolvePositionIKRequest() circle_io.state_changed.connect(self.CirclePressed_) # Variables to manage threads self.work_thread = None self.stop = False # state variables self.state = STATE.IDLE
def get_ik_joints_linear(initial_x, initial_y, initial_z, initial_w2, target_x, target_y, target_z, target_w2, n_steps): ns = "ExternalTools/left/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest( seed_mode=SolvePositionIKRequest.SEED_CURRENT) hdr = Header(stamp=rospy.Time.now(), frame_id='base') # current_pose = arm.endpoint_pose() x0 = initial_x y0 = initial_y z0 = initial_z # linear interpolate between current pose and target pose for i in xrange(n_steps): t = (i + 1) * 1. / n_steps x = (1. - t) * x0 + t * target_x y = (1. - t) * y0 + t * target_y z = (1. - t) * z0 + t * target_z pose = PoseStamped( header=hdr, pose=Pose( position=Point( x=x, y=y, z=z, ), # endeffector pointing down orientation=Quaternion( x=1., y=0., z=0., w=0., ), ), ) ikreq.pose_stamp.append(pose) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return []
def ik(): ns = "ExternalTools/right/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') poses = { 'right': PoseStamped( header=hdr, pose=Pose( position=Point( x=0.8, y=-0.32, z=0.25, ), orientation=Quaternion( x=-0.007, y=0.697, z=-0.029, w=0.716, ), ), ), } limb_joints = { 'right_s0': 0.787, 'right_s1': -0.635, 'right_w0': -0.068, 'right_w1': -1.445, 'right_w2': 0.0318, 'right_e0': -0.082, 'right_e1': 2.027, } ikreq.pose_stamp.append(poses['right']) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return 1
def _find_joint_position(self, pose, x_off=0.0, y_off=0.0, z_off=0.0): ''' Finds the joint position of the arm given some pose and the offsets from it (to avoid opening the structure all the time outside of the function). ''' ik_request = SolvePositionIKRequest() the_pose = deepcopy(pose) the_pose['position'] = Point(x=pose['position'].x + x_off, y=pose['position'].y + y_off, z=pose['position'].z + z_off) approach_pose = Pose() approach_pose.position = the_pose['position'] approach_pose.orientation = the_pose['orientation'] hdr = Header(stamp=rospy.Time.now(), frame_id='base') pose_req = PoseStamped(header=hdr, pose=approach_pose) ik_request.pose_stamp.append(pose_req) resp = self._iksvc(ik_request) return dict(zip(resp.joints[0].name, resp.joints[0].position))
def callBuiltInIK(pos, rot): # Try calling the built-in IK req = SolvePositionIKRequest() req_head = Header(stamp=rospy.Time.now(), frame_id='base') req_pose = PoseStamped(header=req_head, pose=Pose(position=pos, orientation=rot)) req.pose_stamp.append(req_pose) # Call the services rospy.wait_for_service( 'ExternalTools/left/PositionKinematicsNode/IKService') try: ik_solver = rospy.ServiceProxy( 'ExternalTools/left/PositionKinematicsNode/IKService', SolvePositionIK) response = ik_solver(req) return response except rospy.ServiceException, e: print "Call failed %s" % e
def ik_test(pose, printing=False): """ From baxter example code to return a joint_angle dictionary from an input workspace pose """ ns = "ExternalTools/right/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') ikreq.pose_stamp.append(pose) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return (1, limb.joint_angles() ) #returning current joint angles if not valid
def svc_move_to_AR_tag(self, data): ''' This service function takes the cached pose information for the lid's AR marker, calls for an IK solution that drings the designated end effector into alignment with the lid, and executes a move to the calculated joint positions. ''' # Establish connection to specific limb's IKSolver service ns = '/ExternalTools/' + self.limb + '/PositionKinematicsNode/IKService' iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() # Update header information based on current time and with reference to base frame hdr = Header(stamp=rospy.Time.now(), frame_id='base') # Update orientation of gripper based on rotation of AR tag frame to a frame compatible with Baxter's EE state tag_q_old = [ self.tag.orientation.x, self.tag.orientation.y, self.tag.orientation.z, self.tag.orientation.w ] tag_q_rot = tf.transformations.quaternion_from_euler(0, math.pi, 0) tag_q_new = tf.transformations.quaternion_multiply( tag_q_rot, tag_q_old) # Locally relevant AR tag derived poses (typ. for marker, bottle, or table) AR_pose = PoseStamped(header=hdr, pose=Pose(position=self.tag.position, orientation=Quaternion( x=tag_q_new[0], y=tag_q_new[1], z=tag_q_new[2], w=tag_q_new[3]))) # Set the desired pose in the service request message to pose information pulled from the object pose topic ikreq.pose_stamp.append(AR_pose) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call fault: %s" % (e, )) return (False, "MOTION CTRL - Service call to Baxter's IK solver failed.")
def ik_solve(limb, pos, orient): #~ rospy.init_node("rsdk_ik_service_client") ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() print "iksvc: ", iksvc print "ikreq: ", ikreq hdr = Header(stamp=rospy.Time.now(), frame_id='base') poses = { str(limb): PoseStamped(header=hdr, pose=Pose(position=pos, orientation=orient))} ikreq.pose_stamp.append(poses[limb]) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) return 1
def get_ik(self, iksvc, x, y, z, er, ep, ey, jointAngles): #print er, ep, ey q = quaternion_from_euler(er, ep, ey) hdr = Header(stamp=rospy.Time.now(), frame_id='base') pose = PoseStamped(header=hdr, pose=Pose(position=Point(x=x,y=y,z=z), orientation=Quaternion(q[0], q[1], q[2], q[3]))) ikreq = SolvePositionIKRequest() ikreq.pose_stamp.append(pose) try: if(iksvc is self.left_iksvc): rospy.wait_for_service(BaxterAction.IKSVC_LEFT_URI, 5.0) else: rospy.wait_for_service(BaxterAction.IKSVC_RIGHT_URI, 5.0) resp = iksvc(ikreq) except(rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,))
def baxter_ik_solve(limb, pose, frame): ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() #hdr = Header(stamp=rospy.Time.now(), frame_id='reference/right_hand_camera') hdr = Header(stamp=rospy.Time.now(), frame_id=frame) ps = PoseStamped( header=hdr, pose=pose, ) global posedebug target_topic.publish(ps) ikreq.pose_stamp.append(ps) try: #print 'ik_solve: waiting...' rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return 1
def _get_ik(self, iksvc, pos, rot): q = quaternion_from_euler(rot[0], rot[1], rot[2]) pose = PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id='base'), pose=Pose(position=Point( x=pos[0], y=pos[1], z=pos[2], ), orientation=Quaternion(q[0], q[1], q[2], q[3])), ) ikreq = SolvePositionIKRequest() ikreq.pose_stamp.append(pose) iksvc.wait_for_service(5.0) resp = iksvc(ikreq) positions = dict(zip(resp.joints[0].name, resp.joints[0].position)) return (positions, resp.isValid[0])
def getIKGripper(self, setPose): # Prepare the request for Baxter's IK hdr = Header(stamp=rospy.Time.now(), frame_id='base') ps = PoseStamped( header=hdr, pose=setPose, ) ikreq = SolvePositionIKRequest([ps], [], 0) # Make the service call srvName = 'ExternalTools/' + self.side + '/PositionKinematicsNode/IKService' srvAlias = rospy.ServiceProxy(srvName, SolvePositionIK) rospy.wait_for_service(srvName) resp = srvAlias(ikreq) # Get IK response and convert to joint position dict if (resp.isValid[0]): return dict(zip(resp.joints[0].name, resp.joints[0].position)) else: return None
def ObtainCartesianPos(limb, pos): try: srv = '/ExternalTools/' + limb.name + '/PositionKinematicsNode/IKService' #print "Test: Service availability: %s" % srv rospy.wait_for_service(srv, 5.0) iksvc = rospy.ServiceProxy(srv, SolvePositionIK) ikreq = SolvePositionIKRequest() pose = PoseStamped( header=std_msgs.msg.Header( stamp=rospy.Time.now(), frame_id='base'), pose=Pose( position = pos[0], orientation = pos[1] ) ) ikreq.pose_stamp.append(pose) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,))
def _find_approach(self, pose, offset): ikreq = SolvePositionIKRequest() # Add 5 cm offset in Z directio try: pose['position'] = Point(x=pose['position'][0], y=pose['position'][1], z=pose['position'][2] + offset) except Exception: pose['position'] = Point(x=pose['position'].x, y=pose['position'].y, z=pose['position'].z + offset) approach_pose = Pose() approach_pose.position = pose['position'] approach_pose.orientation = pose['orientation'] hdr = Header(stamp=rospy.Time.now(), frame_id='base') pose_req = PoseStamped(header=hdr, pose=approach_pose) ikreq.pose_stamp.append(pose_req) resp = self._iksvc(ikreq) return dict(zip(resp.joints[0].name, resp.joints[0].position))
def ik_request(self, gripper, pose): hdr = Header(stamp=rospy.Time.now(), frame_id='base') ikreq = SolvePositionIKRequest() print("IK REQUEST ***************************") ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose)) try: print("GRIPPER NAME: " + gripper.name) if (gripper.name == "left_gripper" or gripper.name == "left"): # print("IK request is going to be for LEFT GRIPPER") resp = self._iksvc_left(ikreq) elif (gripper.name == "right_gripper" or gripper.name == "right"): # print("IK request is going to be for RIGHT GRIPPER") resp = self._iksvc_right(ikreq) else: print("Unable to resolve gripper for IK_REQUEST") exit(1) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return False
def ik_request(self, pose, seed_list=None, _joint_names=None): hdr = Header(stamp=rospy.Time.now(), frame_id='base') ikreq = SolvePositionIKRequest() ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose)) if seed_list is not None: for seed_angles in seed_list: seed = JointState() seed.name = _joint_names seed.position = seed_angles seed.header = Header(stamp=rospy.Time.now(), frame_id='base') ikreq.seed_angles.append(seed) try: resp = self._iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return False
def Compute_IKin(limb, seed_name, seed_pos, pose): # ROS Params initalization ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() seed = JointState() hdr = Header(stamp=rospy.Time.now(), frame_id='base') limb_joints = {} ikreq.pose_stamp.append(pose[0]) seed.name = seed_name seed.position = seed_pos ikreq.seed_angles.append(seed) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) # except (rospy.ServiceException, rospy.ROSException), e: except rospy.ServiceException as e: rospy.logerr("Service call failed: %s" % (e,)) return 1 #Check if result is valid + type of seed resp_seeds = struct.unpack('<%dB' % len(resp.result_type),resp.result_type) if (resp_seeds[0] != resp.RESULT_INVALID): seed_str = { ikreq.SEED_USER: '******', ikreq.SEED_CURRENT: 'Current Joint Angles', ikreq.SEED_NS_MAP: 'Nullspace Setpoints', }.get(resp_seeds[0], 'None') print("SUCCESS - Valid Joint Solution Found from Seed Type: %s" %(seed_str,)) #Format solution limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position)) #print "\nIK Joint Solution:\n", limb_joints prev_name = resp.joints[0].name prev_pos = resp.joints[0].position else: print ("INVALID POSE") return limb_joints,prev_name,prev_pos
def _get_ik(self, iksvc, x, y, z, er, ep, ey, njs): q = quaternion_from_euler(er, ep, ey) pose = PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id='base'), pose=Pose(position=Point( x=x, y=y, z=z, ), orientation=Quaternion(q[0], q[1], q[2], q[3])), ) ikreq = SolvePositionIKRequest() ikreq.pose_stamp.append(pose) ikreq.seed_angles.append(njs) iksvc.wait_for_service(1.0) resp = iksvc(ikreq) positions = dict(zip(resp.joints[0].name, resp.joints[0].position)) return (positions, resp.isValid[0])
def right_ik_callback(self, poseIn): ns = "ExternalTools/right/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') command = PoseStamped(header=hdr, pose=poseIn) currentAngles = self.rightLimb.joint_angles() ikreq.pose_stamp.append(command) seed = JointState(name=currentAngles.keys(), position=currentAngles.values()) ikreq.seed_angles.append(seed) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return 1
from tf.transformations import quaternion_from_euler if __name__=='__main__': rospy.loginfo("Starting test_moveit node") rospy.init_node('test_moveit', log_level=rospy.INFO) right_arm_group = moveit_commander.MoveGroupCommander("right_arm") des_pose = [0.28, -0.62, -0.32, 0, 3.14/2, 0] # des_pose = [0.815, -1.01, 0.321, 0.271, 0.653, -0.271, 0.653] limb = "right" ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') # quat = quaternion_from_euler(des_pose[3],des_pose[4],des_pose[5]) pose = Pose() quat = Quaternion() quat.x = des_pose[3] quat.y = des_pose[4] quat.z = des_pose[5] quat.w = des_pose[6] pose.orientation = quat pose.position.x = des_pose[0] pose.position.y = des_pose[1] pose.position.z = des_pose[2] ikreq.pose_stamp = [PoseStamped(header=hdr, pose=pose)] # print ikreq try: