# Forward and inverse kinematic model

**URL:** <https://forum.hello-robot.com/t/forward-and-inverse-kinematic-model/154>\
**Category:** Knowledge Base\
**Created:** [April 21, 2021, 2:15am UTC](https://forum.hello-robot.com/t/forward-and-inverse-kinematic-model/154 "2021-04-21T02:15:54Z")\
**Posts on this page:** 4\
**Page:** 1

<div class="post-metadata">

**Author:** ![jiawei\_li](https://avatars.discourse-cdn.com/v4/letter/j/3da27b/32.png) [@jiawei\_li](https://forum.hello-robot.com/u/jiawei_li)\
**Post date:** [April 21, 2021, 2:15am UTC](https://forum.hello-robot.com/t/forward-and-inverse-kinematic-model/154/1 "2021-04-21T02:15:54Z")

</div>

Hello! Does stretch provide the official forward kinematic formula, like Jacobian matrix?

---

<div class="post-metadata">

**Author:** ![hello-ck](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/hello-ck/32/23_2.png) [@hello-ck](https://forum.hello-robot.com/u/hello-ck)\
**Post date:** [April 21, 2021, 8:53pm UTC](https://forum.hello-robot.com/t/forward-and-inverse-kinematic-model/154/2 "2021-04-21T20:53:09Z")

</div>

Hi @jiawei_li!

You can use [tf2](http://wiki.ros.org/tf2) to obtain transforms between frames. To obtain Jacobians, you could try [KDL](http://wiki.ros.org/orocos_kinematics_dynamics).

Best wishes,  
Charlie

---

<div class="post-metadata">

**Author:** ![bshah](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/bshah/32/55_2.png) [@bshah](https://forum.hello-robot.com/u/bshah)\
**Post date:** [April 22, 2021, 10:46pm UTC](https://forum.hello-robot.com/t/forward-and-inverse-kinematic-model/154/3 "2021-04-22T22:46:08Z")

</div>

Building on Charlie’s answer here, the best place to find the information you need to build up a FK model is from your calibrated URDF. Working with ROS, you can use [tf2](http://wiki.ros.org/tf2) to lookup these transforms. Working with pure Python, the [urdfpy](https://urdfpy.readthedocs.io/en/latest/index.html) library can [return the transforms](https://urdfpy.readthedocs.io/en/latest/examples/index.html#doing-forward-kinematics) as Numpy arrays.

Kinematics and Dynamics Library (KDL), as bundled with ROS has a number of good solvers, including [KDL::ChainIkSolverVel\_wdls](http://docs.ros.org/en/indigo/api/orocos_kdl/html/classKDL_1_1ChainIkSolverVel__wdls.html) (based on the [Weighted Pseudo Inverse](https://www.springer.com/gp/book/9780387002934) method) and [KDL::ChainIdSolver\_RNE](http://docs.ros.org/en/jade/api/orocos_kdl/html/classKDL_1_1ChainIdSolver__RNE.html) (based on the [Recursive Newton Euler](https://link.springer.com/book/10.1007/978-1-4899-7560-7) method), that can assist with kinematic and dynamic control. Unfortunately, the KDL Python bindings are in need of a little TLC, so it will require some configuring to get working.

Solving FK and the Jacobian analytically is also accessible thanks to Stretch’s simple kinematic chain. For example, if we projected Stretch onto SE2 and took a bird’s eye view, we’d see a differential drive mobile base, a prismatic joint (telescoping arm), and a revolute joint (yaw gripper joint).

 ![frame0_annotated](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/1X/d8be6ea46a5d1ce23815b611b368e6d714f54585.png)  
In this simplified version of Stretch, our joint state vector, ![CodeCogsEqn](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/1X/7bfcf6d0b0877dcdf00750cc3e25bb40c8be58f4.png), is composed of the SE2 pose (x, y, and heading) of the base, and the arm joints (s1 and theta1). Forward kinematics to the end effector pose is given as:

![kinematics](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/1X/8f1edba2b9f7f3dab42a6a0b5677cb42ac1eb51b.png)

At this point, a symbolic library such as [sympy](https://docs.sympy.org/latest/modules/matrices/matrices.html#sympy.matrices.matrices.MatrixCalculus.jacobian) could be introduced to solve for the Jacobian. With this toy example, it comes out to:

![jacobian](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/1X/cc08672bda4a4b3c616cb33ef9110dcc50e24ab4.png)

To round off the example, we can do some simple control using the pseudo inverse: ![pinv](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/1X/ccaadb6cd41b0efa63e3665508307b091386602b.png). The animated gif below shows the robot following a trajectory while obeying non-holonomic constraints to three different goals, resulting in whole body coordinated motion.

 ![whole_animation_compressed](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/1X/d5e6f7d1e13ceab55b7b2de55b71b0fd7df59c4c.gif)  
Extending this example to SE3 requires us to account for the vertical prismatic joint (Lift joint) and add realistic offsets between links. Let me know if you would like to see this in example code running on the robot.

---

<div class="post-metadata">

**Author:** ![jiawei\_li](https://avatars.discourse-cdn.com/v4/letter/j/3da27b/32.png) [@jiawei\_li](https://forum.hello-robot.com/u/jiawei_li)\
**Post date:** [May 4, 2021, 1:42am UTC](https://forum.hello-robot.com/t/forward-and-inverse-kinematic-model/154/4 "2021-05-04T01:42:52Z")

</div>

Hi @bshah @hello-ck! Thank you for the help. The FK and Jacobian for SE3 have been derived and are used for implementing the admittance control for Stretch successfully in our lab.
