Generating Symbolic Expression for Forward Kinematics

The method that is going to be discussed here applies to simple manipulators. A more general form could be derived but the aim here is to understand the basics. The way ROS understands the kind of robot that is being loaded into the parameter server is through the robot_description tag which is basically an xml file more commonly known as URDF. The URDF file could be parsed to extract all the relevant parameters to compute the forward kinematics (Refer orocos kdl for deeper understanding).

Let’s take the case of a simple three degree of freedom planar manipulator. Refer to this python script for the following discussion. Some of the things are hard coded as I am not doing any URDF parsing. Launch the simulation environment so that URDF is loaded into the parameter server. Send the robot to zero position (where all the joint values are zero).Then we need to know the link names as described in the URDF (self.links). The arm_base to arm_link_0 and arm_link_3 to arm_end_effector are transformations where the rotations are constant (usually identity) and some fixed translation. Since we are considering a 3-DOF planar manipulator, we know that there are three joint space variable viz.,q1, q2 and q3. The following table can be obtained by carefully observing the output produced by the get_trans_rot() function. It takes in a source and target frame and returns the translation, quaternion and the 4 x 4 transformation matrix between them.

The symbolic forward kinematics is computed using sympy and is referenced under the name self.tfMat_abEff. There is a helper function forward_kinematics() which simplifies this expression and returns the x, y and z co-ordinates and the orientation of the end-effector with respect to the arm base (\(\phi\)). For the particular case of my robot, I got the equations as \begin{align} x &= 0.2\cos(q1) + 0.2\cos(q1 - q2) + 0.25\cos(q1 - q2 + q3) + 0.1415 \nonumber \newline y &= 0.2\sin(q1) + 0.2\sin(q1 - q2) + 0.25\sin(q1 - q2 + q3) \nonumber \end{align}