Ask Your Question
0

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

asked 2017-10-30 21:57:35 -0500

Joy16 gravatar image

updated 2017-10-31 09:06:31 -0500

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?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2017-10-31 09:13:26 -0500

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
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-10-30 21:57:35 -0500

Seen: 314 times

Last updated: Oct 31 '17