I am trying to implement a joint space control with the UR5 in MATLAB/Simulink and have programmed the forward kinematics of the same. Now, I want to find out the joint angles of the manipulator using the Inverse Jacobian scheme with the following formula:


I do understand the math around it and have successfully implemented the Jacobian and obtained the matrix. The problem I am currently facing is how do I use the above stated formula and program in MATLAB to find out the joint angles.

Do I need to use a for loop or something to iterate through each time step?


1 Answer 1


If u wanna find the joint angle here what can i explain, using task space. If u really wanna using joint space i will explain later.

What u asked for is Resolved Motion Rate Control, here is the step :

Note that im using Euler Angle as representation of end-effector orientation, thus matrix x will contain (in my case for 6 DOF robot):

matrix x

while matrix q

matrix x

xinit = Current end-effector pose

xfinal = end-effector pose goal

in order to obtain end-effector pose, just read the book. If i explain here i will rewrite a book. And because im using euler angle, you need to use analytical jacobian.

  1. Trajectory Planning

Make the manipulator move smoothly

xref = xinit + (xfinal-xinit) * CurrentStep/TotalStep

  1. Find derivative


  1. PD Controller

Control the speed

PD Controller

  1. Inverse Jacobian

Find q acceleration

Inverse Jacobian

  1. Double Integral

Now we need q for current state

Double Integral

  1. Memory


And repeat this step until Current step reach total step, hope this help

For translational this step should work(just use 3 upper line of jacobian). And if u wanna control the rotational read my post here

The difference between task space and joint space is in task space we use controller to control end effector position (x,y,z,phi,theta,psi). While in joint space we use controller to control every single joint to reach the goal obtained from inverse jacobian.

Note : if you use euler angle as representation, you could get error because gimbal lock. i didnt how to deal with it until this time.

Reference :

  1. Craig, John. J. (2005). Introduction to Robotic : Mechanics and Control. Pearson Education Inc.
  2. Corke, Peter P. (2017). Robotics, Vision and Control. Springer.
  3. Siciliano, B. (2009). Robotic Modelling, Planning and Contol. Springer.
  4. Paul, P.R. (1981). Robot Manipulator : Mathematics, Programming, and Control. MIT.
  • $\begingroup$ Thank you so much for explaining this. Made it a lot easier for me to understand the topic much better. $\endgroup$
    – Bumblebee
    Commented Jul 4, 2019 at 15:56
  • $\begingroup$ There are a few problems with this. Step 2, x is a pose not a position, how is orientation represented? Step 4, the Jacobian maps velocity not acceleration, for the latter you need to compute the derivative of the Jacobian. $\endgroup$ Commented Jul 4, 2019 at 20:23
  • $\begingroup$ @Gol, your welcome.. accept my answer if it help then :) $\endgroup$
    – Albert H M
    Commented Jul 5, 2019 at 10:52
  • $\begingroup$ @PeterCorke, for step two, my algorithm work if using analytical jacobian, x will contain (x,y,z,phi,theta,psi) for case im using ZYZ euler angle as representation of end-effecor orientation. for step four, dq = J^-1 * dx, if i divide both by dt, then we will get ddq = J^-1 ddx, thats my understanding, plz correct me if im wrong. This step always work for me $\endgroup$
    – Albert H M
    Commented Jul 5, 2019 at 10:54
  • $\begingroup$ i just realize, at step 6, it should be xrefold and dxrefold.. $\endgroup$
    – Albert H M
    Commented Jul 5, 2019 at 12:06

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.