ik_service_client.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2013-2014, Rethink Robotics
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #
00009 # 1. Redistributions of source code must retain the above copyright notice,
00010 #    this list of conditions and the following disclaimer.
00011 # 2. Redistributions in binary form must reproduce the above copyright
00012 #    notice, this list of conditions and the following disclaimer in the
00013 #    documentation and/or other materials provided with the distribution.
00014 # 3. Neither the name of the Rethink Robotics nor the names of its
00015 #    contributors may be used to endorse or promote products derived from
00016 #    this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028 # POSSIBILITY OF SUCH DAMAGE.
00029 
00030 """
00031 Baxter RSDK Inverse Kinematics Example
00032 """
00033 import argparse
00034 import struct
00035 import sys
00036 
00037 import rospy
00038 
00039 from geometry_msgs.msg import (
00040     PoseStamped,
00041     Pose,
00042     Point,
00043     Quaternion,
00044 )
00045 from std_msgs.msg import Header
00046 
00047 from baxter_core_msgs.srv import (
00048     SolvePositionIK,
00049     SolvePositionIKRequest,
00050 )
00051 
00052 
00053 def ik_test(limb):
00054     rospy.init_node("rsdk_ik_service_client")
00055     ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
00056     iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
00057     ikreq = SolvePositionIKRequest()
00058     hdr = Header(stamp=rospy.Time.now(), frame_id='base')
00059     poses = {
00060         'left': PoseStamped(
00061             header=hdr,
00062             pose=Pose(
00063                 position=Point(
00064                     x=0.657579481614,
00065                     y=0.851981417433,
00066                     z=0.0388352386502,
00067                 ),
00068                 orientation=Quaternion(
00069                     x=-0.366894936773,
00070                     y=0.885980397775,
00071                     z=0.108155782462,
00072                     w=0.262162481772,
00073                 ),
00074             ),
00075         ),
00076         'right': PoseStamped(
00077             header=hdr,
00078             pose=Pose(
00079                 position=Point(
00080                     x=0.656982770038,
00081                     y=-0.852598021641,
00082                     z=0.0388609422173,
00083                 ),
00084                 orientation=Quaternion(
00085                     x=0.367048116303,
00086                     y=0.885911751787,
00087                     z=-0.108908281936,
00088                     w=0.261868353356,
00089                 ),
00090             ),
00091         ),
00092     }
00093 
00094     ikreq.pose_stamp.append(poses[limb])
00095     try:
00096         rospy.wait_for_service(ns, 5.0)
00097         resp = iksvc(ikreq)
00098     except (rospy.ServiceException, rospy.ROSException), e:
00099         rospy.logerr("Service call failed: %s" % (e,))
00100         return 1
00101 
00102     # Check if result valid, and type of seed ultimately used to get solution
00103     # convert rospy's string representation of uint8[]'s to int's
00104     resp_seeds = struct.unpack('<%dB' % len(resp.result_type),
00105                                resp.result_type)
00106     if (resp_seeds[0] != resp.RESULT_INVALID):
00107         seed_str = {
00108                     ikreq.SEED_USER: 'User Provided Seed',
00109                     ikreq.SEED_CURRENT: 'Current Joint Angles',
00110                     ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
00111                    }.get(resp_seeds[0], 'None')
00112         print("SUCCESS - Valid Joint Solution Found from Seed Type: %s" %
00113               (seed_str,))
00114         # Format solution into Limb API-compatible dictionary
00115         limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
00116         print "\nIK Joint Solution:\n", limb_joints
00117         print "------------------"
00118         print "Response Message:\n", resp
00119     else:
00120         print("INVALID POSE - No Valid Joint Solution Found.")
00121 
00122     return 0
00123 
00124 
00125 def main():
00126     """RSDK Inverse Kinematics Example
00127 
00128     A simple example of using the Rethink Inverse Kinematics
00129     Service which returns the joint angles and validity for
00130     a requested Cartesian Pose.
00131 
00132     Run this example, passing the *limb* to test, and the
00133     example will call the Service with a sample Cartesian
00134     Pose, pre-defined in the example code, printing the
00135     response of whether a valid joint solution was found,
00136     and if so, the corresponding joint angles.
00137     """
00138     arg_fmt = argparse.RawDescriptionHelpFormatter
00139     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
00140                                      description=main.__doc__)
00141     parser.add_argument(
00142         '-l', '--limb', choices=['left', 'right'], required=True,
00143         help="the limb to test"
00144     )
00145     args = parser.parse_args(rospy.myargv()[1:])
00146 
00147     return ik_test(args.limb)
00148 
00149 if __name__ == '__main__':
00150     sys.exit(main())


baxter_examples
Author(s): Rethink Robotics Inc.
autogenerated on Fri Oct 3 2014 16:37:39