Introduction
This example demonstrates the usage of the IK service that is available in Baxter. The main() function records the side of the joint on which the IK service is to be called and then invokes the ik_test() function. This is where the rospy node is initialized, default cartesian coordinates are initialized, and the IK service is called.
Service - "ExternalTools/<side>/PositionKinematicsNode/IKService"
Code Walkthrough
Now, let's break down the code
33 import argparse
34 import sys
35
36 import rospy
37
38 from geometry_msgs.msg import (
39 PoseStamped,
40 Pose,
41 Point,
42 Quaternion,
43 )
44 from std_msgs.msg import Header
45
46 from baxter_core_msgs.srv import (
47 SolvePositionIK,
48 SolvePositionIKRequest,
49 )
The geometry message types are imported to build the request message for the IK service. The custom request and response message types (baxter_core_msgs-SolvePositionIK) are imported from the baxter_core_msgs package.
52 def ik_test(limb):
53 rospy.init_node("rsdk_ik_service_client")
54 ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
55 iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
56 ikreq = SolvePositionIKRequest()
57 hdr = Header(stamp=rospy.Time.now(), frame_id='base')
The ros node is initialized. An object for the service call is initialized by passing the service name ns and the Request-Response message type, SolvePositionIK.
SolvePositionIKRequest is the IK Request message type that is generated at compile time.
58 poses = {
59 'left': PoseStamped(
60 header=hdr,
61 pose=Pose(
62 position=Point(
63 x=0.657579481614,
64 y=0.851981417433,
65 z=0.0388352386502,
66 ),
67 orientation=Quaternion(
68 x=-0.366894936773,
69 y=0.885980397775,
70 z=0.108155782462,
71 w=0.262162481772,
72 ),
73 ),
74 ),
75 'right': PoseStamped(
76 header=hdr,
77 pose=Pose(
78 position=Point(
79 x=0.656982770038,
80 y=-0.852598021641,
81 z=0.0388609422173,
82 ),
83 orientation=Quaternion(
84 x=0.367048116303,
85 y=0.885911751787,
86 z=-0.108908281936,
87 w=0.261868353356,
88 ),
89 ),
90 ),
91 }
92
93 ikreq.pose_stamp.append(poses[limb])
The poses dictionary is populated with hard-coded cartesian coordinates for both the limbs. The last line indicates that the pose of the side of the limb under interest, is copied to the IK request message.
94 try:
95 rospy.wait_for_service(ns, 5.0)
96 resp = iksvc(ikreq)
97 except (rospy.ServiceException, rospy.ROSException), e:
98 rospy.logerr("Service call failed: %s" % (e,))
99 return 1
With a timeout of 5 seconds, the IK service is called along with the IK request message. The resp objects captures the response message which contains the joint positions. It throws an error on timeout.
94 if (resp.isValid[0]):
95 print("SUCCESS - Valid Joint Solution Found:")
96 # Format solution into Limb API-compatible dictionary
97 limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
98 print limb_joints
99 else:
100 print("INVALID POSE - No Valid Joint Solution Found.")
101
102 return 0
The isValid[0] field is a boolean variable that indicates whether a successfull solution for the IK was found or not. The limb_joints is dictionary that holds the joint names and their corresponding position from the response message. This can be passed as a joint command to the position controller to move the arm to the corresponding position.
111 def main():
112 """RSDK Inverse Kinematics Example
113
114 A simple example of using the Rethink Inverse Kinematics
115 Service which returns the joint angles and validity for
116 a requested Cartesian Pose.
117
118 Run this example, passing the *limb* to test, and the
119 example will call the Service with a sample Cartesian
120 Pose, pre-defined in the example code, printing the
121 response of whether a valid joint solution was found,
122 and if so, the corresponding joint angles.
123 """
124 arg_fmt = argparse.RawDescriptionHelpFormatter
125 parser = argparse.ArgumentParser(formatter_class=arg_fmt,
126 description=main.__doc__)
127 parser.add_argument(
128 '-l', '--limb', choices=['left', 'right'], required=True,
129 help="the limb to test"
130 )
131 args = parser.parse_args(rospy.myargv()[1:])
The main function parses the arguments provided by the user to capture the side of the arm on which the example is to be run.
133 return ik_test(args.limb)
134
135 if __name__ == '__main__':
136 sys.exit(main())
The ik_test() function is called with the side of the limb as an argument to calculate the IK service as explained above.
