## Contents

## 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

## 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.

```
$ 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

- args:

**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