The IK service example shows the very basics of calling the on-robot Inverse-Kinematics (IK) Service to obtain a joint angles solution for a given endpoint Cartesian point & orientation.
Introduction
This example demonstrates the usage of the IK service that is available in Sawyer. The main() function records 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/right/PositionKinematicsNode/IKService
If you would like to follow along with the actual source code for the example on GitHub, it can be found through this link for ik service client example.
Usage
From an RSDK Shell, run the ik_service_client.py demo from the inverse_kinematics package, ex:
$ rosrun intera_examples ik_service_client.py
The test will call the IK Service for a hard-coded Cartesian Pose for the given arm. The response will then be printed to the screen with the found joint solution if the solution is a valid configuration for Sawyer's arm.
The 'inverse_kinematics' test code is an example of how to communicate with a robot-side ROS node that provides an Inverse-Kinematics (IK) service. Normal arm control is done using joint angles or states; for instance, position joint control allows the user to set the position in radians for shoulder, elbow, and wrist joint. Often in robotics manipulation, it can be useful to work in the Cartesian space of the arm's endpoint. Inverse Kinematics is used to convert between the Cartesian (x,y,z, roll, pitch, yaw) space representation, to actual controllable 7-DOF joint states.
The example script simply calls the service for a canonical Cartesian point + orientation for either the left or right arm and as such merely provides the most basic example of how to use this service. Note that we provide this service just to have some out of the box method for IK. The complicated nature of 7-DOF Inverse Kinematics control, usually means the best IK implementations are customized for a specific use case. The node is not a complete Cartesian control solution, just a rudimentary solver exposed for convenience in elementary situations.
For anything needing high-performance IK, it makes a lot more sense to implement it on the development machine.
After run the example, the result shows on terminal:
[INFO] [WallTime: 1482347669.474299] Running Simple IK Service Client example.
[INFO] [WallTime: 1482347669.483886] SUCCESS - Valid Joint Solution Found from Seed Type: Current Joint Angles
[INFO] [WallTime: 1482347669.483992]
IK Joint Solution:
{'right_j6': -1.8083939242780869, 'right_j5': 2.7143783429757264, 'right_j4': -1.1756920541664397, 'right_j3': -2.215699739532649, 'right_j2': 0.5385031263389193, 'right_j1': 0.7321306707818217, 'right_j0': 0.5964531288816591}
[INFO] [WallTime: 1482347669.484124] ------------------
[INFO] [WallTime: 1482347669.484202] Response Message:
joints:
-
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
name: ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6']
position: [0.5964531288816591, 0.7321306707818217, 0.5385031263389193, -2.215699739532649, -1.1756920541664397, 2.7143783429757264, -1.8083939242780869]
velocity: []
effort: []
result_type: [2]
[INFO] [WallTime: 1482347669.484463] Simple IK call passed!
[INFO] [WallTime: 1482347669.484575] Running Advanced IK Service Client example.
[INFO] [WallTime: 1482347669.491490] SUCCESS - Valid Joint Solution Found from Seed Type: User Provided Seed
[INFO] [WallTime: 1482347669.491611]
IK Joint Solution:
{'right_j6': 1.8756467201212745, 'right_j5': -1.0697047509927082, 'right_j4': -1.855751853200498, 'right_j3': 1.47406455200751, 'right_j2': -1.0393484209059132, 'right_j1': -0.20372936772965106, 'right_j0': 0.9594468009963417}
[INFO] [WallTime: 1482347669.491745] ------------------
[INFO] [WallTime: 1482347669.491820] Response Message:
joints:
-
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
name: ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6']
position: [0.9594468009963417, -0.20372936772965106, -1.0393484209059132, 1.47406455200751, -1.855751853200498, -1.0697047509927082, 1.8756467201212745]
velocity: []
effort: []
result_type: [1]
[INFO] [WallTime: 1482347669.492051] Advanced IK call passed!
ROS APIs
IK Solver Service: /ExternalTools/<limb>/PositionKinematicsNode/IKService
For more ROS APIs: ROS APIs
Using the IK Solution Response
The format of the SolvePositionIK Service Response is an array of sensor_msgs/JointState solutions and a boolean array that return if the solution is valid. To use the SolvePositionIK response to control the arm joints, you can use the values returned in the JointPositions ROS message to command the arm via the Limb interface.
As a rudimentary learning example, try building off the inverse_kinematics python example by adding a Limb interface and using the set_positions() function. By extracting the arrays of joint names and positions into a dictionary, you can pass in the found angles to set_positions() to move the arm joints to the desired solution, with something along the lines of:
...
ikreq.pose_stamp.append(poses[limb])
try:
resp = iksvc(ikreq)
...
# reformat the solution arrays into a dictionary
joint_solution = dict(zip(resp.joints[0].names, resp.joints[0].angles))
# set arm joint positions to solution
arm = intera_interface.Limb(limb)
while not rospy.is_shutdown():
arm.set_positions(joint_solution)
rospy.sleep(0.01)
This is not a complete solution, as you will also need to add code to enable the robot, as is done in other arm control examples. Check out the main function of the joint_positions_file_playback.py example for reference.
Code Walkthrough
Now, let's break down the code.
33 import rospy
34 from geometry_msgs.msg import (
35 PoseStamped,
36 Pose,
37 Point,
38 Quaternion,
39 )
40 from std_msgs.msg import Header
41 from sensor_msgs.msg import JointState
42
43 from intera_core_msgs.srv import (
44 SolvePositionIK,
45 SolvePositionIKRequest,
46 )
The geometry message types are imported to build the request message for the IK service. The custom request and response message types (intera_core_msgs-SolvePositionIK) are imported from the intera_core_msgs package.
48 def ik_service_client(limb = "right", use_advanced_options = False):
49 ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
50 iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
51 ikreq = SolvePositionIKRequest()
52 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.
53 poses = {
54 'right': PoseStamped(
55 header=hdr,
56 pose=Pose(
57 position=Point(
58 x=0.450628752997,
59 y=0.161615832271,
60 z=0.217447307078,
61 ),
62 orientation=Quaternion(
63 x=0.704020578925,
64 y=0.710172716916,
65 z=0.00244101361829,
66 w=0.00194372088834,
67 ),
68 ),
69 ),
70 }
71 # Add desired pose for inverse kinematics
72 ikreq.pose_stamp.append(poses[limb])
73 # Request inverse kinematics from base to "right_hand" link
74 ikreq.tip_names.append('right_hand')
75
76 if (use_advanced_options):
77 # Optional Advanced IK parameters
78 rospy.loginfo("Running Advanced IK Service Client example.")
79 # The joint seed is where the IK position solver starts its optimization
80 ikreq.seed_mode = ikreq.SEED_USER
81 seed = JointState()
82 seed.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
83 'right_j4', 'right_j5', 'right_j6']
84 seed.position = [0.7, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4]
85 ikreq.seed_angles.append(seed)
86
87 # Once the primary IK task is solved, the solver will then try to bias the
88 # the joint angles toward the goal joint configuration. The null space is
89 # the extra degrees of freedom the joints can move without affecting the
90 # primary IK task.
91 ikreq.use_nullspace_goal.append(True)
92 # The nullspace goal can either be the full set or subset of joint angles
93 goal = JointState()
94 goal.name = ['right_j1', 'right_j2', 'right_j3']
95 goal.position = [0.1, -0.3, 0.5]
96 ikreq.nullspace_goal.append(goal)
97 # The gain used to bias toward the nullspace goal. Must be [0.0, 1.0]
98 # If empty, the default gain of 0.4 will be used
99 ikreq.nullspace_gain.append(0.4)
100 else:
101 rospy.loginfo("Running Simple IK Service Client example.")
The poses dictionary is populated with hard-coded cartesian coordinates for robot arm, also append the gain used to bias toward the nullspace goal.
103 try:
104 rospy.wait_for_service(ns, 5.0)
105 resp = iksvc(ikreq)
106 except (rospy.ServiceException, rospy.ROSException), e:
107 rospy.logerr("Service call failed: %s" % (e,))
108 return False
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.
87 # Check if result valid, and type of seed ultimately used to get solution
88 if (resp.result_type[0] > 0):
89 seed_str = {
90 ikreq.SEED_USER: 'User Provided Seed',
91 ikreq.SEED_CURRENT: 'Current Joint Angles',
92 ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
93 }.get(resp.result_type[0], 'None')
94 rospy.loginfo("SUCCESS - Valid Joint Solution Found from Seed Type: %s" %
95 (seed_str,))
96 # Format solution into Limb API-compatible dictionary
97 limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
98 rospy.loginfo("\nIK Joint Solution:\n%s", limb_joints)
99 rospy.loginfo("------------------")
100 rospy.loginfo("Response Message:\n%s", resp)
101 else:
102 rospy.loginfo("INVALID POSE - No Valid Joint Solution Found.")
103
104 return True
Check the result is valid 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.
130 def main():
131 """RSDK Inverse Kinematics Example
132
133 A simple example of using the Rethink Inverse Kinematics
134 Service which returns the joint angles and validity for
135 a requested Cartesian Pose.
136
137 Run this example, the example will use the default limb
138 and call the Service with a sample Cartesian
139 Pose, pre-defined in the example code, printing the
140 response of whether a valid joint solution was found,
141 and if so, the corresponding joint angles.
142 """
143 rospy.init_node("rsdk_ik_service_client")
144
145 if ik_service_client():
146 rospy.loginfo("Simple IK call passed!")
147 else:
148 rospy.logerror("Simple IK call FAILED")
149
150 if ik_service_client(use_advanced_options=True):
151 rospy.loginfo("Advanced IK call passed!")
152 else:
153 rospy.logerror("Advanced IK call FAILED")
154
155
156 if __name__ == '__main__':
157 main()
The ik_service_client() function is called with use advanced option as an argument to calculate the IK service as explained above.
