Hello! Does stretch provide the official forward kinematic formula, like Jacobian matrix?
Hi @jiawei_li!
You can use tf2 to obtain transforms between frames. To obtain Jacobians, you could try KDL.
Best wishes,
Charlie
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 to lookup these transforms. Working with pure Python, the urdfpy library can return the transforms as Numpy arrays.
Kinematics and Dynamics Library (KDL), as bundled with ROS has a number of good solvers, including KDL::ChainIkSolverVel_wdls (based on the Weighted Pseudo Inverse method) and KDL::ChainIdSolver_RNE (based on the Recursive Newton Euler 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).
In this simplified version of Stretch, our joint state vector,
, 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:
At this point, a symbolic library such as sympy could be introduced to solve for the Jacobian. With this toy example, it comes out to:
To round off the example, we can do some simple control using the pseudo inverse: 
. 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.
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.
3 Likes