# Trajectory generation equation from jacobian and inverse kinematics

I just built a 4 DOF Articulated manipulator with open-loop servo motors and the servos signals are generated using a micro-controller. I followed the Angela Sodemann's videos on YouTube, I calculated properly the forward kinematics using Denavit-Hartenberg, Inverse Kinematics and the Jacobian matrix which is in this case the dimensions of the Jacobian matrix are 6x4. All joints are revolute. Now i want to move the end-effector in a particular direction. The manipulator goes in the desired direction but i also want to control its speed using the Jacobian but I cannot take the inverse of the Jacobian since it is not a square matrix. Books on robotics mention 3-rd order/5-th order polynomial for trajectory generation but I could not relate the polynomial equation with the inverse kinematics and the Jacobian matrix. Can anyone suggest on any resources on how to go from here? I will be writing all the code my self in C language without any libraries, or more precisely, how do I learn from here to go to the next nasic step of trajectory generation?

Any response will be greatly appreciated.

Thank you.

EDIT: Someone else had a same problem and was told that if only linear speed control was required, then this could be a solution,

$$\begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{z} \\ 0 \\ 0 \\ 0 \end{bmatrix}$$

I am assuming we are talking about the linear speed of the end-effector here? Have I understood this correctly?

As suggested by ben, i will put my comment as answer.

1st How to control robot

I recommend you to look about resolved motion rate control. It's a complete algorithm to control a robot from path planning, control system, inverse jacobian and simulating plant. (And you can add several other algorithm to measure the state of system if you want too, very good algorithm)

2nd How to inverse non square jacobian

You can inverse non square jacobian by using pseudo-inverse look here Because you jacobian dimension 6x4. Then the pseudo inverse of your jacobian is

A+ = (AT A)^-1 * AT

AT = A transpose

By using pseudoinverse, you also make your robot more robust to singularity

Hope that help

• Okay thank you very much, I will post my results here in a couple of days, this was very helpful! – Zzz Mar 6 at 23:57
• @Zzz dont forget to accept the answer if you satisfied :) – Albert H M Mar 7 at 7:54