ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

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

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

Joy16 gravatar image

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

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 -0600

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

Question Tools

2 followers

Stats

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

Seen: 781 times

Last updated: Oct 31 '17