Baxter kinematics now standalone class. General cleanup.

scripts/baxter_kinematics.py

@@ -27,37 +27,37 @@
 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
-import PyKDL
-
 import rospy
 
-import baxter_interface
+from baxter_pykdl import baxter_kinematics
 
-from baxter_kdl.kdl_parser import kdl_tree_from_urdf_model
-from urdf_parser_py.urdf import URDF
 
 def main():
-    print 'Getting URDF off of param server'
-    baxter = URDF.from_parameter_server(key='robot_description')
-    print 'Creating PyKDL tree'
-    tree = kdl_tree_from_urdf_model(baxter)
-
-    nf_joints = 0
-    for j in baxter.joints:
-        if j.type != 'fixed':
-            nf_joints += 1
-    print "URDF non-fixed joints: %d;" % nf_joints
-    print "URDF total joints: %d" % len(baxter.joints)
-    print "KDL joints: %d" % tree.getNrOfJoints()
-    print "URDF links: %d" % len(baxter.links)
-    print "KDL segments: %d" % tree.getNrOfSegments()
-
-    base_link = baxter.get_root()
-#    end_link = robot.links.keys()[random.randint(0, len(robot.links)-1)]
-#    chain = tree.getChain(base_link, end_link)
-#    print "Root link: %s; Random end link: %s" % (base_link, end_link)
-#    for i in range(chain.getNrOfSegments()):
-#        print chain.getSegment(i).getName()
+    rospy.init_node('baxter_kinematics')
+    print '*** Baxter PyKDL Kinematics ***\n'
+    kin = baxter_kinematics('right')
+
+    kin.print_robot_description()
+    kin.print_kdl_chain()
+    # FK Position
+    #kin.forward_position_kinematics()
+    # FK Velocity
+    kin.forward_velocity_kinematics()
+    # IK
+    pos = [0.582583, -0.180819, 0.216003]
+    rot = [0.03085, 0.9945, 0.0561, 0.0829]
+    kin.inverse_kinematics(pos)  # position, don't care orientation
+    kin.inverse_kinematics(pos, rot)  # position & orientation
+    # Jacobian
+    kin.jacobian()
+    # Jacobian Transpose
+    kin.jacobian_transpose()
+    # Jacobian Pseudo-Inverse (Moore-Penrose)
+    kin.jacobian_pseudo_inverse()
+    # Joint space mass matrix
+    kin.inertia()
+    # Cartesian space mass matrix
+    kin.cart_inertia()
 
 if __name__ == "__main__":
     main()

scripts/display_urdf.py

@@ -1,23 +1,57 @@
-#!/usr/bin/env python
+#!/usr/bin/python
+
+# Copyright (c) 2013-2014, Rethink Robotics
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+#    this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in the
+#    documentation and/or other materials provided with the distribution.
+# 3. Neither the name of the Rethink Robotics nor the names of its
+#    contributors may be used to endorse or promote products derived from
+#    this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
 
 import sys
 import argparse
 
 from urdf_parser_py.urdf import URDF
 
-parser = argparse.ArgumentParser(usage='Load an URDF file')
-parser.add_argument('file', type=argparse.FileType('r'), nargs='?', default=None, help='File to load. Use - for stdin')
-parser.add_argument('-o', '--output', type=argparse.FileType('w'), default=None, help='Dump file to XML')
-args = parser.parse_args()
 
-if args.file is None:
-    print 'FROM PARAM SERVER'
-    robot = URDF.from_parameter_server()
-else:
-    print 'FROM STRING'
-    robot = URDF.from_xml_string(args.file.read())
+def main():
+    parser = argparse.ArgumentParser(usage='Load an URDF file')
+    parser.add_argument('file', type=argparse.FileType('r'), nargs='?',
+                        default=None, help='File to load. Use - for stdin')
+    parser.add_argument('-o', '--output', type=argparse.FileType('w'),
+                        default=None, help='Dump file to XML')
+    args = parser.parse_args()
+
+    if args.file is None:
+        print 'FROM PARAM SERVER'
+        robot = URDF.from_parameter_server()
+    else:
+        print 'FROM STRING'
+        robot = URDF.from_xml_string(args.file.read())
+
+    print(robot)
 
-print(robot)
+    if args.output is not None:
+        args.output.write(robot.to_xml_string())
 
-if args.output is not None:
-    args.output.write(robot.to_xml_string())
+if __name__ == "__main__":
+    main()

setup.py

@@ -3,7 +3,7 @@
 from catkin_pkg.python_setup import generate_distutils_setup
 
 d = generate_distutils_setup()
-d['packages'] = ['baxter_pykdl', 'baxter_kdl', 'urdf_parser_py',]
+d['packages'] = ['baxter_pykdl', 'baxter_kdl', 'urdf_parser_py']
 d['package_dir'] = {'': 'src'}
 
 setup(**d)