I want a robotic arm to do trajectory tracking in the joint space. Suppose the waypoints are $$ [t(0),q_1(0),q_2(0),q_3(0)],\\ \cdots,\\ [t(n-1),q_1(n-1),q_2(n-1),q_3(n-1)] $$ I already know that I can use these waypoints to get a smooth trajectory $[q_1(t),q_2(t),q_3(t)]$ for each joint through interpolation, but how to control the motor (current control or torque control is supported) to track the smooth trajectory properly? More specifically, how to make sure that these motors could keep in sync?


1 Answer 1


For simplicity let's consider a 2R planar manipulator. This method can be expanded to any robot as long as you can define error-proof dynamics. Equations of motions for this robot can be defined by the following equations.

EoM 1 :

EoM 1

EoM 2:

EoM 2

Here, u1 and u2 are torques required for joints 1 and 2 respectively.

The rest of the parameters are shown in the Image. RR Bot

Note that, these equations can be derived using the lagrangian method.

These EoMs can be written in manipulator form.

Manipulator Form

The corresponding matrices are :




The desired torques to track required trajectory can be computed by :

Feedback Linearization

This equation is obtained by feedback linearization . You can read more about it here. In this equation, K matrix contains PD gains. Desired theta and desired angular velocity can be computed using either cubic or quentic polynomial.

If you have a starting angle and ending angle and theie velocities at two instants , the cubic polynomial to track the trajectory would be :



You can compute the coefficients of these cubic polynomials based on your desired trajectory. If you want jerk free motion , you can add a fifth order term to your polynomial.

You can try different PD gains or you can compute them using eigen value placement. For this robot K matrix is 2X4. You need to use following state and input matrices for eigen value placement.



Results of this implementation looks as shown below.


  • $\begingroup$ Wow, thanks so much for your wonderful and detailed answer. I see how it works now. Because it is a fully actuated system, we can cancel all those terms we do not want, for example, the term $C\dot\theta+G(\theta)$. Then we use a PD controller and a feedback term $M\ddot\theta_d$ to track the desired trajectory. That is very clear. Thanks again! $$ \tau=M(-K \begin{bmatrix} \theta-\theta_d\\ \dot\theta-\dot\theta_d \end{bmatrix}+\ddot \theta_d)+C\dot\theta+G(\theta) $$ $\endgroup$
    – DingDong
    Oct 13, 2022 at 13:02
  • $\begingroup$ You are welcome.! If you are satisfied with the answer, you can mark it as accepted ! $\endgroup$
    – McLovin
    Oct 13, 2022 at 16:12
  • $\begingroup$ Sure. The answer indeed helps me a lot. Thanks! $\endgroup$
    – DingDong
    Oct 14, 2022 at 1:41

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.