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 callback(comm): msg = PoseStamped() msg.header.seq = 0 msg.header.stamp = rospy.get_rostime() msg.header.frame_id = 'base' msg.pose = comm.pose arr = SolvePositionIKRequest() arr.pose_stamp.append(msg) arr.seed_mode = 0 print arr left_ik_addr = '/ExternalTools/left/PositionKinematicsNode/IKService' right_ik_addr = '/ExternalTools/right/PositionKinematicsNode/IKService' if rospy.get_param('/robot/unityros/movelock') == True: return try: left_ik = rospy.ServiceProxy(left_ik_addr, SolvePositionIK) right_ik = rospy.ServiceProxy(right_ik_addr, SolvePositionIK) if comm.limb == 'left': ik = left_ik(arr) else: ik = right_ik(arr) right_limb = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=1) left_limb = rospy.Publisher('/robot/limb/left/joint_command', JointCommand, queue_size=1) message = JointCommand() message.mode = 1 message.command = ik.joints[0].position message.names = ik.joints[0].name rospy.loginfo(message) right_limb.publish(message) left_limb.publish(message) except rospy.ServiceException, e: print 'Service call failed: %s' % e
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
ikl = rospy.ServiceProxy( '/ExternalTools/left/PositionKinematicsNode/IKService', SolvePositionIK) pubr = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=1) publ = rospy.Publisher('/robot/limb/left/joint_command', JointCommand, queue_size=1) cmd = JointCommand() cmd.mode = 1 req = SolvePositionIKRequest() req.seed_mode = 0 M0 = transformations.quaternion_matrix((0.031, 0.808, .587, 0.028)) M0[0, 3] = 0.7 M0[1, 3] = -0.137 M0[2, 3] = 0.357 Md = np.eye(4) Mrl = np.zeros((4, 4)) Mrl[2, 3] = 0.2 Mrl[2, 0] = Mrl[0, 2] = -1 Mrl[1, 1] = 1 Mrl[3, 3] = 1 print(Mrl, matrix_msg(Mrl))
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 = JointState() # seed.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3', # 'right_j4', 'right_j5', 'right_j6'] # seed.position = [0.42, -0.38, -1.24, 1.78, 1.16, 1.11, 2.05] ############################### seed = joint_state_from_cmd(seed_cmd) ################################ ikreq.seed_angles.append(seed) # Null space goals are not supported on the baxter ## 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', 'right_j2', 'right_j3'] #goal.position = [0.409, -0.43, -1.2, 1.79] #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.5) ## else: ## rospy.loginfo("Running Simple IK Service Client example.") done, i = False, 0 while not done and i < 100: try: rospy.wait_for_service(name_of_service, 5.0) resp = iksvc(ikreq) done = True except (rospy.ServiceException, rospy.ROSException) as e: rospy.logerr("IK service call failed: %s" % (e, )) i += 1 if not done: raise IOError("IK SERVICE CALL FAILED") # 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') # rospy.loginfo("SUCCESS - Valid Joint Solution Found from Seed Type: %s" % # (seed_str,)) # Format solution into Limb API-compatible dictionary limb_joints = dict( list(zip(resp.joints[0].name, resp.joints[0].position))) # rospy.loginfo("\nIK Joint Solution:\n%s", limb_joints) # rospy.loginfo("------------------") # rospy.loginfo("Response Message:\n%s", resp) return limb_joints else: rospy.loginfo("INVALID POSE - No Valid Joint Solution Found.") raise ValueError
), orientation=Quaternion( x=0.0, y=0.71, z=0.0, w=-0.71, ), ), ), } ikreq_r = SolvePositionIKRequest() ikreq_r.pose_stamp = [poses['right']] seed = random_seed() ikreq_r.seed_angles = seed ikreq_r.seed_mode = 1 print "IKREQ_FIXED = ", ikreq_r #print "IKREQ_R : ",ikreq_r try: rospy.wait_for_service(ns_r, 5.0) resp_r = iksvc_r(ikreq_r) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return if (resp_r.isValid[0]): print("RIGHT_SUCCESS - Valid Joint Solution Found:") # Format solution into Limb API-compatible dictionary limb_joints_r = dict( zip(resp_r.joints[0].name, resp_r.joints[0].position))