Baxter PyKDL

Orocos Kinematics and Dynamics with Baxter! Please visit the Orocos KDL (Kinematics Dynamics Library) homepage for more information

Summary

This is a project for live Python KDL (Orocos Kinematics and Dynamics Library) usage with Baxter. Forward Kinematics, Inverse Kinematics, Jacobian, Jacobian Tranpose, Jacobian Pseudo-Inverse, Joint and Cartesian Inertias..

Author

Kyle Maroney

Project Site

https://github.com/RethinkRobotics/baxter_pykdl

Installation

Prerequisites

Note: These prerequisite packages have already been installed on your robot, so you should skip this step when using SSH with baxter_pykdl.

ROS IndigoROS HydroROS Groovy

    $ sudo apt-get install ros-indigo-python-orocos-kdl  ros-indigo-orocos-kinematics-dynamics ros-indigo-kdl-conversions ros-indigo-orocos-kdl

    $ sudo apt-get install ros-hydro-python-orocos-kdl ros-hydro-kdl ros-groovy-kdl-conversions ros-hydro-orocos-kdl

    $ sudo apt-get install ros-groovy-python-orocos-kdl ros-groovy-kdl ros-groovy-kdl-conversions ros-groovy-orocos-kdl

Checkout and build the baxter_pykdl package

    $ cd ~/ros_ws/src
    $ git clone https://github.com/RethinkRobotics/baxter_pykdl.git
    # All work is in master
    $ cd ~/ros_ws
    $ source ./baxter.sh
    $ catkin_make

Description

This package was based on the pykdl_utils package from Georgia Tech (Kelsey Hawkins).

The idea is to use the Orocos Project's Kinematics and Dynamics library for the kinematic and dynamic analysis of Baxter in Python using the Rethink Python SDK.

A class is provided in this package, baxter_kinematics, which reads from the parameter server of the robot you are connected to pulling the URDF. It then parses that URDF into the expected Orocos kinematic description, a kdl tree. It then, based on the limb specified creates a kinematic chain (base -> gripper frames) on which we will conduct our analysis.

As of the SDK 1.0 release, the URDF loaded to the parameter server is the same model represented in robot memory. This means that our calculations performed here in the pykdl package should match the output of the robot.

The baxter_kinematics class provides the following methods:

Class

baxter_kinematics
- args: limb (required) - limb we will work on ['left', 'right']
- description: Baxter Kinematics using Python Kinematics and Dynamics Library with the baxter_interface

Methods

forward_position_kinematics
- args: None
- description: Solves the forward position kinematics read from the limb's current joint angles
- returns numpy array [x, y, z, rot_i, rot_j, rot_k, rot_w]

forward_velocity_kinematics
- args: None
- description: Solves the forward velocity kinematics read from the limb's current joint velocities
- returns KDL FrameVel Twist (Not yet working..)

inverse_kinematics
- args:
  - position (list) - Cartesian Position, Orientation (optional) [x, y, z]
  - orientation (list) [Optional] - Quaternion Orientation [i, j, k, w]
  - seed (list) [Optional] - Seeded joint angles for solver. Searchs out from seeded joint angles for IK solution [s0, s1, e0, e1, w0, w1, w2]
- description: Solves the inverse kinematics, providing joint angles given a Cartesian pose

forward_velocity_kinematics
- args: None
- description: Solves the inverse kinematics, providing joint angles given a Cartesian pose
- returns: List of joint angles [s0, s1, e0, e1, w0, w1, w2]

jacobian
- args: None
- description: Solves the Jacobian reading the robot's current joint angles
- returns: numpy mat Jacobian

jacobian_transpose
- args: None
- description: Tranposes the solved Jacobian
- returns: numpy mat Jacobian (Transposed)

jacobian_pseudo_inverse
- args: None
- description: Solves for the Jacobian Pseudo-Inverse using numpy's linear algebra library (pinv)
- return: numpy mat Jacobian (Pseudo-Inverse)

inertia
- args: None
- description: Solves for joint space inertias
- return: numpy matrix Joint Inertias

cart_inertia
- args: None
- description: Solves for Cartesian space inertias
- return: numpy matrix Cartesian Inertias

print_kdl_chain
- args: None
- description: Crawls the kinematic chain, printing the link names

joints_to_kdl
- args: type (string) - supported types ['positions', 'velocities', 'torques']
- description: Gets the current arm joint values specified by the type argument, returns in the expected KDL form.

kdl_to_mat
- args: data (matrix) - kdl matrix in
- description: Converts output kdl matrices into useful numpy mat (matrix) form

kdl_to_mat
- args: data (matrix) - kdl matrix in
- description: Converts output kdl matrices into useful numpy mat (matrix) form

Run Examples

I've added two example scripts. One to call each of the methods, printing their result, and another simple script for verifying and validating the URDF loaded to the parameter server.

These are found in the scripts folder.

display_urdf.py

This example retrieves the current URDF loaded to the parameter server and parses it, printing the model.

Run with a RSDK_Shell properly initialized shell:

    $ cd ~/ros_ws
    $ rosrun baxter_pykdl display_urdf.py

baxter_kinematics.py

This example calls all of the baxter_kinematics classes' kinematics methods. FK, IK, Jacobian, Jacobian Transpose, Jacobian Pseudo-Inverse, Joint Inertia, Cartesian Inertias, and model analysis prints.

Run with a properly initialized shell:

    $ cd ~/ros_ws
    $ rosrun baxter_pykdl baxter_kinematics.py

That's it! Enjoy!

Let me know if you have any feedback (kmaroney at rethinkrobotics dot com)!

Troubleshooting

Drop a question on the Baxter Research Robot Users (brr-users) forum/mailing list