def ik_service_client(self): ns = "ExternalTools/right/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() ikreq.pose_stamp.append(self.pose) ikreq.tip_names.append('right_hand') ikreq.seed_mode = ikreq.SEED_USER seed = JointState() seed.name = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] seed.position = [ self.curr_position["right_j0"], self.curr_position["right_j1"], self.curr_position["right_j2"], self.curr_position["right_j3"], self.curr_position["right_j4"], self.curr_position["right_j5"], self.curr_position["right_j6"] ] 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 False
def get_joint_angles(pose, seed_cmd=None, use_advanced_options=False, current=True): limb = "right" name_of_service = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(name_of_service, SolvePositionIK) ikreq = SolvePositionIKRequest() ikreq.pose_stamp.append(pose) ikreq.tip_names.append('right_hand') seed_joints = None if use_advanced_options: if current: ikreq.seed_mode = ikreq.SEED_CURRENT else: ikreq.seed_mode = ikreq.SEED_AUTO seed = joint_state_from_cmd(seed_cmd) ikreq.seed_angles.append(seed) try: rospy.wait_for_service(name_of_service, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return False
def get_joint_angles(pose, limb="right", seed_cmd=None, use_advanced_options=False, verbosity=0): ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() # Add desired pose for inverse kinematics stamped_pose = stamp_pose(pose) ikreq.pose_stamp.append(stamped_pose) # Request inverse kinematics from base to "right_hand" link ikreq.tip_names.append('right_hand') # rospy.loginfo("Running IK Service Client.") # seed_joints = None if use_advanced_options: # Optional Advanced IK parameters # The joint seed is where the IK position solver starts its optimization ikreq.seed_mode = ikreq.SEED_USER # if not seed_joints: # seed = JointState() # seed.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3', # 'right_j4', 'right_j5', 'right_j6'] # seed.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # else: seed = joint_state_from_cmd(seed_cmd) 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 False
def GetEndJoints(self): if not self.currentJointPositions: rospy.loginfo("Cant get current joint poses") return {} limb = self.endPoseName #limb = "right" ns = "ExternalTools/" + limb + "/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=self.endPosePosition.x, y=self.endPosePosition.y, z=self.endPosePosition.z, ), orientation=Quaternion( x=self.endOrientPosition.x, y=self.endOrientPosition.y, z=self.endOrientPosition.z, w=self.endOrientPosition.w, ), ), ), } ikreq.pose_stamp.append(poses[limb]) ikreq.tip_names.append('right_hand') rospy.loginfo("Running Advanced IK Service Client example.") ikreq.seed_mode = ikreq.SEED_CURRENT seed = JointState() seed.name = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] seed.position = self.currentJointPositions[0:6] ikreq.seed_angles.append(seed) ikreq.use_nullspace_goal.append(True) goal = JointState() goal.name = ['right_j1', 'right_j2', 'right_j3'] goal.position = [0.1, 0.1, 0.1] ikreq.nullspace_goal.append(goal) ikreq.nullspace_gain.append(0.1) 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(limb, coordinates, orientation): angles = limb.joint_angles() 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=coordinates.x, y=coordinates.y, z=coordinates.z, ), orientation=Quaternion( x=orientation.x, y=orientation.y, z=orientation.z, w=orientation.w, ), ), ), } ikreq.pose_stamp.append(poses['right']) ikreq.tip_names.append('right_hand') ikreq.seed_mode = ikreq.SEED_USER seed = JointState() seed.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6'] seed.position = [angles[a] for a in seed.name] ikreq.seed_angles.append(seed) # Optimize the null space in terms of joint configuration ikreq.use_nullspace_goal.append(True) goal = JointState() goal.name = ['right_j2'] goal.position = [0] ikreq.nullspace_goal.append(goal) ikreq.nullspace_gain.append(0.4) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException) as e: rospy.logerr("Service call failed: %s" % (e,)) limb_joints = angles # Check if result valid, and type of seed ultimately used to get solution if (resp.result_type[0] > 0): seed_str = {ikreq.SEED_USER: '******', ikreq.SEED_CURRENT: 'Current Joint Angles', ikreq.SEED_NS_MAP: 'Nullspace Setpoints', }.get(resp.result_type[0], 'None') limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position)) return limb_joints
def get_ik(pose, q_initial, verbose=True): """ Get the inverse kinematics of the robot for a given pose Arguments: pose - desired pose in the format (x,y,z,ew,ex,ey,ez), containing the position and orientation (quaternion) q_initial - list of initial joint configuration used as initial point when computing the inverse kinematics Returns: result - structure containing the joint angles from inverse kinematics and an indication of whether the values are valid """ pose_msg = pose_to_dict_message(pose) limb = "right" # Structure that the function returns result = JointResult() # Service name serv_name = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" # Client for the inverse kinematics server ik_client = rospy.ServiceProxy(serv_name, SolvePositionIK) # Message for the request ik_request = SolvePositionIKRequest() # Add desired pose for inverse kinematics ik_request.pose_stamp.append(pose_msg[limb]) # Request inverse kinematics from base to "right_hand" link ik_request.tip_names.append('right_hand') # Start the IK optimization from this joint configuration (seed) ik_request.seed_mode = ik_request.SEED_USER seed = JointState() seed.name = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] seed.position = [ q_initial['right_j0'], q_initial['right_j1'], q_initial['right_j2'], q_initial['right_j3'], q_initial['right_j4'], q_initial['right_j5'], q_initial['right_j6'] ] ik_request.seed_angles.append(seed) try: # Block until the service is available rospy.wait_for_service(serv_name, 3.0) # Service request ik_response = ik_client(ik_request) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return result
def ik_service_client(self, pose1, rospy): ns = "ExternalTools/right/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') pose1.header = hdr poses = {'right': pose1} # Add desired pose for inverse kinematics ikreq.pose_stamp.append(poses["right"]) # Request inverse kinematics from base to "right_hand" link ikreq.tip_names.append('right_hand') if (True): # Optional Advanced IK parameters #rospy.loginfo("Running Advanced IK Service Client example.") # The joint seed is where the IK position solver starts its optimization ikreq.seed_mode = ikreq.SEED_USER seed = JointState() seed.name = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] seed.position = [0.7, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4] ikreq.seed_angles.append(seed) # Once the primary IK task is solved, the solver will then try to bias the # the joint angles toward the goal joint configuration. The null space is # the extra degrees of freedom the joints can move without affecting the # primary IK task. ikreq.use_nullspace_goal.append(True) # The nullspace goal can either be the full set or subset of joint angles goal = JointState() goal.name = ['right_j1', 'right_j2', 'right_j3'] goal.position = [0.1, -0.3, 0.5] ikreq.nullspace_goal.append(goal) # The gain used to bias toward the nullspace goal. Must be [0.0, 1.0] # If empty, the default gain of 0.4 will be used ikreq.nullspace_gain.append(0.4) else: rospy.loginfo("Running Simple IK Service Client example.") 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 False
def get_joint_angles(pose, seed_cmd = None, use_advanced_options = False): limb = "right" name_of_service = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(name_of_service, SolvePositionIK) ikreq = SolvePositionIKRequest() # Add desired pose for inverse kinematics ikreq.pose_stamp.append(pose) # Request inverse kinematics from base to "right_hand" link ikreq.tip_names.append('right_hand') seed_joints = None if use_advanced_options: # Optional Advanced IK parameters # The joint seed is where the IK position solver starts its optimization ikreq.seed_mode = ikreq.SEED_USER # if not seed_joints: # seed = JointState() # seed.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3', # 'right_j4', 'right_j5', 'right_j6'] # seed.position = [0.7, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4] # else: seed = joint_state_from_cmd(seed_cmd) ikreq.seed_angles.append(seed) # Once the primary IK task is solved, the solver will then try to bias the # the joint angles toward the goal joint configuration. The null space is # the extra degrees of freedom the joints can move without affecting the # primary IK task. # ikreq.use_nullspace_goal.append(True) # The nullspace goal can either be the full set or subset of joint angles # goal = JointState() # goal.name = ['right_j1', 'right_j2', 'right_j3'] # goal.position = [0.1, -0.3, 0.5] # ikreq.nullspace_goal.append(goal) # # The gain used to bias toward the nullspace goal. Must be [0.0, 1.0] # # If empty, the default gain of 0.4 will be used # ikreq.nullspace_gain.append(0.4) # else: # rospy.loginfo("Running Simple IK Service Client example.") try: rospy.wait_for_service(name_of_service, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) return False
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)) ikreq.tip_names.append('right_hand') # NOT WORKING ''' ikreq.seed_mode = ikreq.SEED_USER seed = JointState() seed.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6'] seed.position = [-2.7, 0.12, -1.4, -1, 1.3, 1.6, 4.6] ikreq.seed_angles.append(seed) # Once the primary IK task is solved, the solver will then try to bias the # the joint angles toward the goal joint configuration. The null space is # the extra degrees of freedom the joints can move without affecting the # primary IK task. ikreq.use_nullspace_goal.append(True) # The nullspace goal can either be the full set or subset of joint angles goal = JointState() goal.name = ['right_j0', 'right_j1'] goal.position = [-2.7, 0.12] ikreq.nullspace_goal.append(goal) ikreq.nullspace_gain.append(0.4) ''' try: rospy.wait_for_service(self._iksvc_name, 5) resp = self._iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return False
def ik_service_client(limb = "right", tip_name = "right_gripper_tip"): limb_mv = intera_interface.Limb(limb) #gripper = intera_interface.Gripper() ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') # Add desired pose for inverse kinematics current_pose = limb_mv.endpoint_pose() #print current_pose movement = [0.460, -0.563,0.35] orientation = [0.0,1,0.0,0.0] #gripper.close() rospy.sleep(1.0) [dx,dy,dz] = movement [ox,oy,oz,ow] = orientation dy = constrain(dy,-0.7596394482267009,0.7596394482267009) dz = constrain(dz, 0.02, 1) # up position [0.45,-0.453,0.6] 0.11 for pick up location #poses = {'right': PoseStamped(header=hdr,pose=Pose(position=Point(x=0.450628752997,y=0.161615832271,z=0.217447307078,), #orientation=Quaternion(x=0.704020578925,y=0.710172716916,z=0.00244101361829,w=0.00194372088834,),),),} neutral pose #x= 0.5,y = 0.5, z= 0.5,w= 0.5 for enpoint facing forward (orientation) #table side parametres x=0.6529605227057904, y= +-0.7596394482267009, z=0.10958623747123714) #table straight parametres x=0.99, y=0.0, z=0.1) poses = { 'right': PoseStamped( header=hdr, pose=Pose( position=Point( x= dx, y= dy, z= dz, ), orientation=Quaternion( x= ox, y= oy, z= oz, w= ow, ), ), ), } ikreq.pose_stamp.append(poses[limb]) # Request inverse kinematics from base to "right_hand" link ikreq.tip_names.append('right_gripper_tip') # right_hand, right_wrist, right_hand_camera 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 False
def service_request_velocity(iksvc, vel_vec, side): """Move in the requested direction at a constant velocity vector""" #INCOMPLETE ns = "ExternalTools/" + side + "/PositionKinematicsNode/IKService" ikreq = SolvePositionIKRequest() limb = baxter_interface.Limb(side) hdr = Header(stamp=rospy.Time.now(), frame_id='base')
def move_gripper_to(self, x, y, z, orientation = [0,-1,0,0], timeout = 15, confirmation = True): ns = "ExternalTools/right/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() 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( x=orientation[0], y=orientation[1], z=orientation[2], w=orientation[3], ), ), ) ikreq.pose_stamp.append(pose) ikreq.tip_names.append('right_hand') try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: print(repr(e)) #rospy.logerr("Service call failed: %s" % (e,)) return 1
def call(self, pose, tip_name="/right_gripper_tip"): """ Call 'IKService' service Parameters ---------- pose : geometry_msgs.PosedStamped The pose on which to perform Inverse Kinematics. tip_name : str The ending frame name in the transform tree from which to compute Inverse Kinematics. Returns ------- response : InverseKinematicsResponse The InverseKinematicsResponse response built from the IKService service response. """ ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') poses = pose poses.hdr = hdr # Add desired pose for inverse kinematics ikreq.pose_stamp.append(poses[self.limb]) # Request inverse kinematics from base to "right_hand" link ikreq.tip_names.append(tip_name) try: resp = self.service(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,))
def ik_service_client(position_in,orientation_in): 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=position_in[0], y=position_in[1], z=position_in[2], ), orientation=Quaternion( x=orientation_in[0], y=orientation_in[1], z=orientation_in[2], w=orientation_in[3], ), ), ), } # Add desired pose for inverse kinematics ikreq.pose_stamp.append(poses["right"]) # Request inverse kinematics from base to "right_hand" link ikreq.tip_names.append('right_hand') 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 False
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 ik_service_client(limb = "right", use_advanced_options = False): limb_mv = intera_interface.Limb('right') ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') # Add desired pose for inverse kinematics camera_pose = limb_mv.tip_state("right_hand_camera") #current right_hand_camera pose print camera_pose # first position [0.45,-0.45,0.30] # second position [0.45,-0.55,0.30] # third position [0.362,-0.45,0.30] # fourth position [0.362,-0.55,0.30] movement = [0.4456, -0.563, 0.335,] [dx,dy,dz] = movement dy = constrain(dy,-0.7596394482267009,0.7596394482267009) dz = constrain(dz, 0.1, 1) #x= 0.7,y = 0.7, z= 0.0,w= 0.0 for camera facing down (orientation, facing forward) #x= 0.0,y = 1, z= 0.0,w= 0.0 for camera facing down (orientation, facing side) poses = { 'right': PoseStamped( header=hdr, pose=Pose( position=Point( x= dx, y= dy, z= dz, ), orientation=Quaternion( x= 0, y= 1, z= 0.0, w= 0.0, ), ), ), } ikreq.pose_stamp.append(poses[limb]) # Request inverse kinematics from base to "right_hand" link ikreq.tip_names.append('right_hand_camera') # right_hand, right_wrist, right_hand_camera 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 False
def get_joint_angles(self, pose, seed_cmd): """ Convert the Pose to joint angles. """ ns = "ExternalTools/" + self.limb_name +\ "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() stamped_pose = self.stamp_pose(pose) ikreq.pose_stamp.append(stamped_pose) ikreq.tip_names.append('right_hand') ikreq.seed_mode = ikreq.SEED_USER seed = JointState() seed.name = seed_cmd.keys() seed.position = seed_cmd.values() ikreq.seed_angles.append(seed) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: return False
def ik_service_client(self, limb = "right"): # def ik_service_client(limb = "right"): # ns = "Externaltools/right/PositionKinematicsNode/IKService" ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() print("Performing Inverse Kinematics") hdr = Header(stamp=rospy.Time.now(), frame_id='base') poses = {'right': PoseStamped( header = hdr, pose = Pose( position = Point( x = 0.650628752997, y = 0, z = 0.417447307078, ), orientation = Quaternion( x = 0.704020578925, y = 0.710172716916, z = 0.00244101361829, w = 0.00194372088834, ), ), ), } print("poses checkpoint") print(poses) ikreq.pose_stamp.append(poses[limb]) ikreq.tip_names.append('right_hand') print("ikreq checkpoint") print("using simple IK Service Client from ik_service_client example") try: print("trying resp") rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) print("resp successful") except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) print("resp didnt work") return False print("resp checkpoint")
def ik_service_client(limb, use_advanced_options, p_x, p_y, p_z, q_x, q_y, q_z, q_w, workspace=True): ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') # camera_center_human_right = [0.72934375, -0.0131015625, -2.2375146484375, 0.507642578125, 2.1652548828125, 1.8803798828125, -1.083087890625] # camera_center_human_left = [0.7083759765625, -0.0957626953125, -1.681681640625, 0.980623046875, 1.55379296875, 1.706056640625, 1.4645380859375] # inter_position_for_demo = [0.2603701171875, -0.628283203125, -1.1051025390625, 1.3576220703125, 0.83878515625, 1.326048828125, 0.892400390625] # if workspace: # return_joint = camera_center_human_right # else: # return_joint = camera_center_human_left # if demo: # return_joint = inter_position_for_demo # print type(limb) return_joint = intera_interface.Limb(limb).joint_ordered_angles() poses = { 'right': PoseStamped( header=hdr, pose=Pose( position=Point( x=p_x, y=p_y, z=p_z, ), orientation=Quaternion( x=q_x, y=q_y, z=q_z, w=q_w, ), ), ), } # Add desired pose for inverse kinematics ikreq.pose_stamp.append(poses[limb]) # Request inverse kinematics from base to "right_hand" link ikreq.tip_names.append('right_hand') 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 return_joint
def service_request_pose(iksvc, raw_pose, side, blocking=True): ns = "ExternalTools/" + side + "/PositionKinematicsNode/IKService" ikreq = SolvePositionIKRequest() limb = baxter_interface.Limb(side) hdr = Header(stamp=rospy.Time.now(), frame_id='base') pose_stamped = PoseStamped(header=hdr, pose=raw_pose) ikreq.pose_stamp.append(pose_stamped) 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_service_client(poses, limb = "right", use_advanced_options = False): ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() # Add desired pose for inverse kinematics ikreq.pose_stamp.append(poses[limb]) # Request inverse kinematics from base to "right_hand" link ikreq.tip_names.append('right_hand') 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_solver(self, poses, limb="right"): ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() print("Performing Inverse Kinematics") ikreq.pose_stamp.append(poses[limb]) ikreq.tip_names.append('right_hand') try: print("Calling IK Service") rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return False, resp
def waypointToJoint(self, pose): iksvc = rospy.ServiceProxy( 'ExternalTools/right/PositionKinematicsNode/IKService', SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose)) ikreq.tip_names.append('right_hand') resp = iksvc(ikreq) if (resp.result_type[0] > 0): limb_joints = dict( zip(resp.joints[0].name, resp.joints[0].position)) return limb_joints else: rospy.logerr("INVALID POSE - No Valid Joint Solution Found.") return False
def ik_service_client(limb, use_advanced_options, p_x, p_y, p_z, q_x, q_y, q_z, q_w): ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') # return_joint sends robot to pre-grasp arm configuration return_joint = [ -1.500208984375, -1.0860322265625, -0.177197265625, 1.3819580078125, 0.0950634765625, 1.3055205078125, 1.6654560546875 ] poses = { 'right': PoseStamped( header=hdr, pose=Pose( position=Point( x=p_x, # pre-grasp: 0.5609462822565073 y=p_y, # -0.3446309287328617 z=p_z, # 0.45777571205785983[-1.500208984375, -1.0860322265625, -0.177197265625, # 1.3819580078125, 0.0950634765625, 1.3055205078125, 1.6654560546875] ), orientation=Quaternion( x=q_x, # 0.20778492941475438 y=q_y, # 0.9778261053583365 z=q_z, # -0.010705090881921715 w=q_w, # -0.023810330049445317 ), ), ), } # Add desired pose for inverse kinematics ikreq.pose_stamp.append(poses[limb]) # Request inverse kinematics from base to "right_hand" link ikreq.tip_names.append('right_hand') 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 return_joint
def IK(object_location): ik_service_name = "ExternalTools/right/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ik_service_name, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') # create desired position in x,y,z (in meters) desired_pose = PoseStamped( header=hdr, pose=Pose( position=Point( x=object_location[0], # forward/backward direction (default 0.7) y=object_location[1], # left/right direction z=sawyer_z_plane, # up/down direction ), orientation=Quaternion( x=0.704020578925, y=0.710172716916, z=0.00244101361829, w=0.00194372088834, ) ) ) # request desired position to ik ikreq.pose_stamp.append(desired_pose) # assign gripper hand ikreq.tip_names.append('right_gripper_tip') # apply IK request as response resp = iksvc(ikreq) # print resp # test target position target_pose = dict(zip(resp.joints[0].name, resp.joints[0].position)) # move arm to target position # dir(assignment) to see possible joint commands limb.move_to_joint_positions(target_pose) if limb.move_to_joint_positions(target_pose) == limb.joint_angles(): gripper.close()
def ik(self, pose, speed): iksvc = rospy.ServiceProxy( 'ExternalTools/right/PositionKinematicsNode/IKService', SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose)) ikreq.tip_names.append('right_hand') resp = iksvc(ikreq) # execute the joint command if the inverse kinematics is successful if (resp.result_type[0] > 0): limb_joints = dict( zip(resp.joints[0].name, resp.joints[0].position)) rospy.loginfo(">>>>>>>>>> moving the arm >>>>>>>>>>") self.basicPositionMove(limb_joints, speed) else: rospy.logerr("INVALID POSE - No Valid Joint Solution Found.") return False
def move_to(limb, position, orientation, speed): rospy.loginfo("Starting move...") ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() poses = new_pose(position, orientation) # Add desired pose for inverse kinematics ikreq.pose_stamp.append(poses[limb]) # Request inverse kinematics from base to "right_hand" link ikreq.tip_names.append('right_hand') resp = iksvc(ikreq) # reformat the solution arrays into a dictionary joint_solution = dict(zip(resp.joints[0].name, resp.joints[0].position)) rospy.loginfo(joint_solution) # set arm joint positions to solution arm = intera_interface.Limb(limb) # set speed of move arm.set_joint_position_speed(speed) arm.move_to_joint_positions(joint_solution, timeout=5, threshold=0.01) rospy.loginfo('Move complete')
def flex_ik_service_client(self, i_Limb="right", i_Pose=None, i_UseAdvanced=False): if self.is_adding_new_item == True: # Can't move if user is in close proximity manually moving the arm return False if i_Pose == None: rospy.logerr("Error: flex_ik_service_client received 'None' in arg 'i_Pose' for target position") return False # Initialize IK service service_name = "ExternalTools/" + i_Limb + "/PositionKinematicsNode/IKService" ik_ServiceClient = rospy.ServiceProxy(service_name, SolvePositionIK) # Creates a handle to some service that we want to invoke methods on, in this case the SolvePositionIK ik_ServiceReq = SolvePositionIKRequest() # Create a request message that will give us the inverse kinematics for some set of joints (Note that this is done at compile time) msg_header = Header(stamp=rospy.Time.now(), frame_id='base') # Generates a Header for the message (Think of HTTP) # Hover over the object hover_pose = copy.deepcopy(i_Pose) hover_pose.position.z = hover_pose.position.z + self.hover_dist # hover over the object # Set the goal positions for the limb that we specified (I'm pretty sure that it's for the last joint right_j6) # Set the header that we time stamped # Add Pose object that contains a Point and Quaternion goal_positions = { 'right': PoseStamped( header=msg_header, pose=hover_pose ), } ############################################################################################## # # SOLVER IS SETUP TO FIND PATH USING SIMPLE INVERSE KINEMATICS # ############################################################################################## # Add the goal positions for the inverse kinematics ik_ServiceReq.pose_stamp.append(goal_positions[i_Limb]) # Request the inverse kinematics from the base to the "right_hand" link ik_ServiceReq.tip_names.append('right_hand') ############################################################################################## # # SETUP THE SOLVER TO FIND A PATH USING ADVANCED INVERSE KINEMATICS # ############################################################################################## if (i_UseAdvanced): rospy.loginfo("Using Advanced IK Service") # Define the joint seed. If the solver encounters the specified value for a joint, the it will attempt to do some optimisation ik_ServiceReq.seed_mode = ik_ServiceReq.SEED_USER seed = JointState() # JointState describes the state of each joint (possition, velocity, effort) seed.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6'] # The name of the various joints seed.position = [0.7, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4] # The joint angle at which the solver tries to do optimisation for the respective joints # Pass the seeded angles to the Solver via the SolvePositionIKRequest object ik_ServiceReq.seed_angles.append(seed) # NOTE Once the Primary IK Task has been solved, the IK Solver will try to bias the joint angles towards the goal joint configuration # NOTE The null space represents the extra degrees of freedom that the joints can move through without affecting the results of the Primary IK Task # Here we try to solve the Primary IK Task ik_ServiceReq.use_nullspace_goal.append(True) # NOTE The null space can either be a subset or the full set of joint angles goal = JointState() # This is the subset of joints that make up the null space "Joints A, B, C ... can try to move, but can't affect the result" goal.name = ['right_j1', 'right_j2', 'right_j3'] goal.position = [0.1, -0.3, 0.5] # If these possitions are encountered, try to do some optimisation ik_ServiceReq.nullspace_goal.append(goal) ik_ServiceReq.nullspace_gain.append(0.4) ############################################################################################## # # PASS THE INVERSE KINEMATICS REQUEST TO THE SOLVER TO GET AN ACTUAL PATH FOR THE ROBOT # ############################################################################################## # rospy.loginfo("Simple IKService Solver Running...") try: rospy.wait_for_service(service_name, timeout=self.arm_timeout) # Waits for the service (5 seconds), creates the service if it doesn't already exist response = ik_ServiceClient(ik_ServiceReq) # Get the response from the client, contains the joint positions except (rospy.ServiceException, rospy.ROSException), ex: rospy.logerr("Service Call Failed: %s" % (ex,)) return False
def IK(object_loc): alignment = 0 for x in range(0,2): object_x, object_y = pix_xy_trans(object_loc) ik_service_name = "ExternalTools/right/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ik_service_name, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') if (alignment == 0): # create desired position in x,y,z (in meters) desired_pose = PoseStamped( header=hdr, pose=Pose( position=Point( x=object_x, # forward/backward direction (default 0.7) y=object_y, # left/right direction z=sawyer_z_hover_plane, # up/down direction ), orientation=Quaternion( x=0.704020578925, y=0.710172716916, z=0.00244101361829, w=0.00194372088834, ) ) ) else: # create desired position in x,y,z (in meters) time.sleep(1) desired_pose = PoseStamped( header=hdr, pose=Pose( position=Point( x=object_x, # forward/backward direction (default 0.7) y=object_y, # left/right direction z=sawyer_z_grab_plane, # up/down direction ), orientation=Quaternion( x=0.704020578925, y=0.710172716916, z=0.00244101361829, w=0.00194372088834, ) ) ) # request desired position to ik ikreq.pose_stamp.append(desired_pose) # assign gripper hand ikreq.tip_names.append('right_gripper_tip') # apply IK request as response resp = iksvc(ikreq) # print resp # test target position target_pose = dict(zip(resp.joints[0].name, resp.joints[0].position)) # move arm to target position # dir(assignment) to see possible joint commands limb.move_to_joint_positions(target_pose) alignment+=1 # include 'from collections import Counter' in header # location_for_close = dict(set(limb.joint_angles().items()) - set(target_pose.items())) # if (location_for_close[0] <= .1) and (location_for_close[1] <= .1) and (location_for_close[2] <= .1) and (location_for_close[3] <= .1): # no need for above, gripper will close after moving to target position time.sleep(1) gripper.close()
def ik_request(self, pose, end_point='right_hand', joint_seed=None, nullspace_goal=None, nullspace_gain=0.4, allow_collision=False): """ Inverse Kinematics request sent to IK Service @type pose: geometry_msgs.Pose @param pose: Cartesian pose of the end point @type end_point: string @param end_point: name of the end point (should be in URDF) @type joint_seed: dict({str:float}) @param joint_seed: the joint angles for the initial IK guess (optional) @type nullspace_goal: dict({str:float}) @param nullspace_goal: desired joints, or subset of joints, to bias the solver (optional) @type nullspace_gain: double @param nullspace_gain: gain used to bias toward the nullspace goal [0.0, 1.0] (optional) @type allow_collision: bool @param allow_collision: does not check if Ik solution is in collision @rtype: dict({str:float}) @return: valid joint positions if exists. False if no solution is found. """ if not isinstance(pose, Pose): rospy.logerr('pose is not of type geometry_msgs.msgs.Pose') return False # Add desired pose for inverse kinematics ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose)) ikreq.tip_names.append(end_point) # The joint seed is where the IK position solver starts its optimization if joint_seed is not None: ikreq.seed_mode = ikreq.SEED_USER seed = JointState() seed.name = list(joint_seed.keys()) seed.position = list(joint_seed.values()) ikreq.seed_angles.append(seed) # Once the primary IK task is solved, the solver will then try to bias the # the joint angles toward the goal joint configuration. The null space is # the extra degrees of freedom the joints can move without affecting the # primary IK task. if nullspace_goal is not None: ikreq.use_nullspace_goal.append(True) # The nullspace goal can either be the full set or subset of joint angles goal = JointState() goal.names = list(nullspace_goal.keys()) goal.position = list(nullspace_goal.values()) ikreq.nullspace_goal.append(goal) # The gain used to bias toward the nullspace goal. Must be [0.0, 1.0] # If empty, the default gain of 0.4 will be used ikreq.nullspace_gain.append(nullspace_gain) try: resp = self._iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException) as e: rospy.logerr("IK Service call failed: %s" % (e,)) return False limb_joints = {} # Check if result valid, and type of seed ultimately used to get solution if (resp.result_type[0] > 0 or (allow_collision and resp.result_type[0] == resp.IK_IN_COLLISION)): # Format solution into Limb API-compatible dictionary limb_joints = dict(list(zip(resp.joints[0].name, resp.joints[0].position))) else: rospy.logerr("INVALID POSE - No Valid Joint Solution Found.") return False return limb_joints