Difference between pages "Head Movement Example" and "IK Service Example"

(Difference between pages)
Jump to: navigation , search
(Code Walkthrough2)
 
(Edit into one page)
 
Line 2: Line 2:
 
<div class="title-block">
 
<div class="title-block">
  
<span style="font-size:120%;">'''The Head "Wobbler" Example randomly moves the head to demonstrate using the head pan .'''</span>
+
<span style="font-size:120%;">'''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.'''</span>
  
 
</div>
 
</div>
Line 8: Line 8:
 
<div class="content-block">
 
<div class="content-block">
  
== Introduction ==  
+
== Introduction ==
  
Sawyer's head can rotate left-to-right in pan mode. The pan motion swings Sawyer's 'face' (a.k.a. screen) to a settable angle.
+
This example demonstrates the usage of the IK service that is available in Sawyer. The <code>main()</code> function records the joint on which the IK service is to be called and then invokes the <code>ik_test()</code> function. This is where the rospy node is initialized, default cartesian coordinates are initialized, and the IK service is called.
 
+
Service - <code>ExternalTools/right/PositionKinematicsNode/IKService</code>
The head wobbler example is intented to be a simple demo whose code can be examined to learn how to use the head and demonstrate the use of the intera_interface Head class.
+
<br />
  
 
</div>
 
</div>
Line 20: Line 20:
 
== Usage ==
 
== Usage ==
  
From an RSDK Shell, run the head_wobbler.py demo from the intera_examples package:
+
From an RSDK Shell, run the ik_service_client.py demo from the inverse_kinematics package, ex:
  
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
$ rosrun intera_examples head_wobbler.py
+
$ rosrun intera_examples ik_service_client.py  
 
</syntaxhighlight>
 
</syntaxhighlight>
  
Sawyer's head will begin panning left and right to random angles.
+
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 and a True/False if the solution is a valid configuration for Sawyer's arm:
 +
 
 +
<syntaxhighlight lang="bash" enclose="div">
 +
True/False..TODO! add the example results in command line in here
 +
</syntaxhighlight>
 +
 
 +
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 each 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.
 +
For more information on Arm Control, see Using the Arms.TODO add Using the arms page.
  
 
</div>
 
</div>
Line 32: Line 41:
 
<div class="content-block">
 
<div class="content-block">
  
== Code Walkthrough ==  
+
== ROS APIs ==
  
Now, let's break down the code.
+
See the API Reference page for details. TODO! add API reference page
 +
 
 +
IK Solver Service: <code>/ExternalTools/<limb>/PositionKinematicsNode/IKService</code>
 +
 
 +
</div>
  
<syntaxhighlight lang="python" line start="30" enclose="div">
+
<div class="content-block">
import argparse
 
import random
 
  
import rospy
+
== Using the IK Solution Response ==
  
import intera_interface
+
The format of the SolvePositionIK Service Response is an array of <code>sensor_msgs/JointState</code> 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 <code>set_positions()</code> function. By extracting the arrays of joint names and positions into a dictionary, you can pass in the found angles to <code>set_positions()</code> to move the arm joints to the desired solution, with something along the lines of:
  
from intera_interface import CHECK_VERSION
+
<syntaxhighlight lang="bash" enclose="div">
 +
    ...
 +
    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)
 
</syntaxhighlight>
 
</syntaxhighlight>
  
This imports the intera interface for accessing the head class.
+
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 <code>joint_positions_file_playback.py</code> example for reference.
  
<syntaxhighlight lang="python" line start="40" enclose="div">
+
</div>
class Wobbler(object):
 
  
    def __init__(self):
+
<div class="content-block">
        """
 
        'Wobbles' the head
 
        """
 
        self._done = False
 
        self._head = intera_interface.Head()
 
</syntaxhighlight>
 
  
The <code>_head</code> is an object of the Head class within the interface_interface. This creates a subscriber to the topic <code>robot/head/head_state</code> and publishers to the topics <code>robot/head/command_head_pan</code>. The object <code>_head</code>, would be used to control the head panning.
+
== Code Walkthrough ==
  
<syntaxhighlight lang="python" line start="49" enclose="div">
+
Now, let's break down the code.
        # verify robot is enabled
 
        print("Getting robot state... ")
 
        self._rs = intera_interface.RobotEnable(CHECK_VERSION)
 
        self._init_state = self._rs.state().enabled
 
        print("Enabling robot... ")
 
        self._rs.enable()
 
        print("Running. Ctrl-c to quit")
 
</syntaxhighlight>
 
  
<code>intera_interface.RobotEnable(CHECK_VERSION)</code> checks if the sdk version updated in the settings is compatible with the sdk version loaded on the param server of the Robot and creates an object <code>_rs</code> of the <code>RobotEnable</code> class. The next line sets the <code>_init_state</code> of the robot to its current state. The <code>enable()</code> performs the actual enabling of the robot. It is important to note that the robot should be in enabled state before attempting to move its joints.
+
<syntaxhighlight lang="python" line start="33" enclose="div">
 +
import rospy
 +
from geometry_msgs.msg import (
 +
    PoseStamped,
 +
    Pose,
 +
    Point,
 +
    Quaternion,
 +
)
 +
from std_msgs.msg import Header
 +
from sensor_msgs.msg import JointState
  
<syntaxhighlight lang="python" line start="57" enclose="div">
+
from intera_core_msgs.srv import (
    def clean_shutdown(self):
+
    SolvePositionIK,
        """
+
    SolvePositionIKRequest,
        Exits example cleanly by moving head to neutral position and
+
)
        maintaining start state
 
        """
 
        print("\nExiting example...")
 
        if self._done:
 
            self.set_neutral()
 
        if not self._init_state and self._rs.state().enabled:
 
            print("Disabling robot...")
 
            self._rs.disable()
 
 
</syntaxhighlight>
 
</syntaxhighlight>
  
This function performs a clean shutdown of the program. The <code>_done</code> variable gets updated once the head finishes its wobbling. Then, the head is moved to its neutral position and checks if the <code>_init_state</code> was in disabled state and the current state is enabled. If so, it resets the robot to the disable state.
+
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.
  
<syntaxhighlight lang="python" line start="69" enclose="div">
+
<syntaxhighlight lang="python" line start="48" enclose="div">
    def set_neutral(self):
+
def ik_service_client(limb = "right", use_advanced_options = False):
        """
+
    ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
        Sets the head back into a neutral pose
+
    iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
        """
+
    ikreq = SolvePositionIKRequest()
        self._head.set_pan(0.0)
+
    hdr = Header(stamp=rospy.Time.now(), frame_id='base')
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The <code>set_pan</code> function moves the head to the desired joint position. It employs a PID position controller to move the head joint to that position. In order to move the head to the neutral position, the pan is set to 0.
+
The ros node is initialized. An object for the service call is initialized by passing the service name <code>ns</code> and the Request-Response message type, <code>SolvePositionIK. SolvePositionIKRequest</code> is the IK Request message type that is generated at compile time.
  
<syntaxhighlight lang="python" line start="75" enclose="div">
+
<syntaxhighlight lang="python" line start="53" enclose="div">
     def wobble(self):
+
     poses = {
        self.set_neutral()
+
        'right': PoseStamped(
        """
+
            header=hdr,
        Performs the wobbling
+
            pose=Pose(
        """
+
                position=Point(
        command_rate = rospy.Rate(1)
+
                    x=0.450628752997,
         control_rate = rospy.Rate(100)
+
                    y=0.161615832271,
        start = rospy.get_time()
+
                    z=0.217447307078,
</syntaxhighlight>
+
                ),
 +
                orientation=Quaternion(
 +
                    x=0.704020578925,
 +
                    y=0.710172716916,
 +
                    z=0.00244101361829,
 +
                    w=0.00194372088834,
 +
                ),
 +
            ),
 +
         ),
 +
    }
 +
    # Add desired pose for inverse kinematics
 +
    ikreq.pose_stamp.append(poses[limb])
 +
    # Request inverse kinematics from base to "right_hand" link
 +
    ikreq.tip_names.append('right_hand')
  
This is where the actual wobbling happens. Before the robot starts to wobble its head, it moves its head to the neutral position.
+
    if (use_advanced_options):
 +
        # Optional Advanced IK parameters
 +
        rospy.loginfo("Running Advanced IK Service Client example.")
 +
        # The joint seed is where the IK position solver starts its optimization
 +
        ikreq.seed_mode = ikreq.SEED_USER
 +
        seed = JointState()
 +
        seed.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
 +
                    'right_j4', 'right_j5', 'right_j6']
 +
        seed.position = [0.7, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4]
 +
        ikreq.seed_angles.append(seed)
  
<syntaxhighlight lang="python" line start="83" enclose="div">
+
        # Once the primary IK task is solved, the solver will then try to bias the
         while not rospy.is_shutdown() and (rospy.get_time() - start < 10.0):
+
        # the joint angles toward the goal joint configuration. The null space is
            angle = random.uniform(-2.0, 0.95)
+
        # the extra degrees of freedom the joints can move without affecting the
 +
        # primary IK task.
 +
         ikreq.use_nullspace_goal.append(True)
 +
        # The nullspace goal can either be the full set or subset of joint angles
 +
        goal = JointState()
 +
        goal.name = ['right_j1', 'right_j2', 'right_j3']
 +
        goal.position = [0.1, -0.3, 0.5]
 +
        ikreq.nullspace_goal.append(goal)
 +
        # The gain used to bias toward the nullspace goal. Must be [0.0, 1.0]
 +
        # If empty, the default gain of 0.4 will be used
 +
        ikreq.nullspace_gain.append(0.4)
 +
    else:
 +
        rospy.loginfo("Running Simple IK Service Client example.")
 
</syntaxhighlight>
 
</syntaxhighlight>
  
This retrieves a random floating point number between -2.0 and 0.95. These random angles are generated for 10 seconds.
+
The <code>poses</code> dictionary is populated with hard-coded cartesian coordinates for robot arm, also append the gain used to bias toward the nullspace goal.
  
<syntaxhighlight lang="python" line start="85" enclose="div">
+
<syntaxhighlight lang="python" line start="103" enclose="div">
            while (not rospy.is_shutdown() and
+
    try:
                  not (abs(self._head.pan() - angle) <=
+
        rospy.wait_for_service(ns, 5.0)
                      intera_interface.HEAD_PAN_ANGLE_TOLERANCE)):
+
        resp = iksvc(ikreq)
 +
    except (rospy.ServiceException, rospy.ROSException), e:
 +
        rospy.logerr("Service call failed: %s" % (e,))
 +
        return False
 
</syntaxhighlight>
 
</syntaxhighlight>
  
This ensures that it loops till the current head pan position and the random angle generated above is within the <code>HEAD_PAN_ANGLE_TOLERANCE</code>.
+
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.
 +
 
 +
<syntaxhighlight lang="python" line start="87" enclose="div">
 +
    # Check if result valid, and type of seed ultimately used to get solution
 +
    if (resp.result_type[0] > 0):
 +
        seed_str = {
 +
                    ikreq.SEED_USER: 'User Provided Seed',
 +
                    ikreq.SEED_CURRENT: 'Current Joint Angles',
 +
                    ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
 +
                  }.get(resp.result_type[0], 'None')
 +
        rospy.loginfo("SUCCESS - Valid Joint Solution Found from Seed Type: %s" %
 +
              (seed_str,))
 +
        # Format solution into Limb API-compatible dictionary
 +
        limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
 +
        rospy.loginfo("\nIK Joint Solution:\n%s", limb_joints)
 +
        rospy.loginfo("------------------")
 +
        rospy.loginfo("Response Message:\n%s", resp)
 +
    else:
 +
        rospy.loginfo("INVALID POSE - No Valid Joint Solution Found.")
  
<syntaxhighlight lang="python" line start="88" enclose="div">
+
    return True
                self._head.set_pan(angle, speed=0.3, timeout=0)
 
                control_rate.sleep()
 
            command_rate.sleep()
 
 
</syntaxhighlight>
 
</syntaxhighlight>
  
This publishes the corresponding position and the speed to the <code>robot/head/command_head_pan</code>.
+
Check the result is valid or not. The <code>limb_joints</code> 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.
  
<syntaxhighlight lang="python" line start="92" enclose="div">
+
<syntaxhighlight lang="python" line start="130" enclose="div">
        self._done = True
+
def main():
        rospy.signal_shutdown("Example finished.")
+
    """RSDK Inverse Kinematics Example
</syntaxhighlight>
 
  
Once the wobbling is completed, the <code>_done</code> variable is updated as True and a shutdown signal is sent.
+
    A simple example of using the Rethink Inverse Kinematics
 +
    Service which returns the joint angles and validity for
 +
    a requested Cartesian Pose.
  
<syntaxhighlight lang="python" line start="96" enclose="div">
+
    Run this example, the example will use the default limb
def main():
+
     and call the Service with a sample Cartesian
     """RSDK Head Example: Wobbler
+
     Pose, pre-defined in the example code, printing the
     Nods the head and pans side-to-side towards random angles.
+
     response of whether a valid joint solution was found,
     Demonstrates the use of the intera_interface.Head class.
+
    and if so, the corresponding joint angles.
 
     """
 
     """
     arg_fmt = argparse.RawDescriptionHelpFormatter
+
     rospy.init_node("rsdk_ik_service_client")
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
 
                                    description=main.__doc__)
 
    parser.parse_args(rospy.myargv()[1:])
 
  
     print("Initializing node... ")
+
     if ik_service_client():
    rospy.init_node("rsdk_head_wobbler")
+
        rospy.loginfo("Simple IK call passed!")
 
+
    else:
    wobbler = Wobbler()
+
        rospy.logerror("Simple IK call FAILED")
</syntaxhighlight>
 
  
An object of the <code>Wobbler</code> class is generated and this initializes the head interface, checks for the appropriate version and enables the robot.
+
    if ik_service_client(use_advanced_options=True):
 +
        rospy.loginfo("Advanced IK call passed!")
 +
    else:
 +
        rospy.logerror("Advanced IK call FAILED")
  
<syntaxhighlight lang="python" line start="111" enclose="div">
 
    rospy.on_shutdown(wobbler.clean_shutdown)
 
    print("Wobbling... ")
 
    wobbler.wobble()
 
    print("Done.")
 
  
 
if __name__ == '__main__':
 
if __name__ == '__main__':
Line 172: Line 229:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
Panning happens when the <code>wobble()</code> function is called.
+
The <code>ik_service_client()</code> function is called with use advanced option as an argument to calculate the IK service as explained above.
  
 
</div>
 
</div>

Revision as of 13:53, 24 November 2016

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

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 and a True/False if the solution is a valid configuration for Sawyer's arm:

True/False..TODO! add the example results in command line in here

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 each 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. For more information on Arm Control, see Using the Arms.TODO add Using the arms page.

ROS APIs

See the API Reference page for details. TODO! add API reference page

IK Solver Service: /ExternalTools/<limb>/PositionKinematicsNode/IKService

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.