How to get IK response for user provided pose in MoveIt(Baxter)

Question

Below is the code snippet of ik_service_client.py provided for Baxter SDK

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,
            ),
        ),
    ),
   
}

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
resp_seeds = struct.unpack('<%dB' % len(resp.result_type),
                           resp.result_type)


if (resp_seeds[0] != resp.RESULT_INVALID):
    seed_str = {
                ikreq.SEED_USER: 'User Provided Seed',
                ikreq.SEED_CURRENT: 'Current Joint Angles',
                ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
               }.get(resp_seeds[0], 'None')

Whenever I get results, it is always form the ikreq.SEED_CURRENT"'Current JointAngles'. How can I retreieve the ikreq result form the user provided input?

---

Originally posted by Joy16 on ROS Answers with karma: 112 on 2017-10-30

Post score: 0

Answer 1

The SDK has a pretty thorough walkthrough of retrieving exactly what you are looking for. The "result" you are referring to is specifically because of this line

resp_seeds = struct.unpack('<%dB' % len(resp.result_type),
                       resp.result_type)

Where you are only looking at what seeding methods were used in the original IK request. What you want to be looking at is the resp.joints field. Here is the complete service definition including comments on what all of the fields are, and here is the relevant snippet from the previously-linked walkthrough:

if (resp.isValid[0]):
    print("SUCCESS - Valid Joint Solution Found:")
    # Format solution into Limb API-compatible dictionary
    limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
    print limb_joints
else:
    print("INVALID POSE - No Valid Joint Solution Found.")

return 0

---

Originally posted by jarvisschultz with karma: 9031 on 2017-10-31

This answer was ACCEPTED on the original site

Post score: 1