I am currently doing a personal project and get stuck in something as below. In this project, I am going to use the RRT algorithm on the ur5 robot to make it reach the target frame without any collision. So after the RRT algorithm, a path (i.e. a series of poses) would be generated. And my question is how I can use the Jacobian pseudo inverse method to make the end-effector follow the path smoothly. I am only allowed to perform differential method instead of IK solutions.

Of course the easiest way for this is to do interpolation, such as defining 10 intermediate points in the path. But such operation would make the movement not very smooth, i.e. it would first accelerate and then decelerate between each pair of points.

Can anyone provide any idea? Thank you very much!


1 Answer 1


You can use some Trajectory Generators to create intermediate points between the points calculated by RRT Algorithm. You can use Moden Robotics Library by Neuroscience and Robotics Lab at Northwestern University on Github for example to find some functions which can help you at this time step. You can take a look at "CartesianTrajectory" and "ScrewTrajectory" functions of the library for example. They allow you to create trajectories between two configurations by a specified time input. I mentioned this Trajectory Generation, because as far as I know, the reason Jacobian Inverse method is used is, the fact that it allows you to calculate the Inverse Kinematics between very small time steps.

You just calculate the Jacobian Matrix of the robot and take its inverse. Then you can use the following equations to (1) calculate the joint displacements according to the translational movement, (2) to calculate the Pseudo Inverse of the Jacobian matrix, according to the article proposed by Robert Krasnansky, Peter Valach, Dávid Soós, Javad Zarbakhsh (Reference trajectory tracking for a multi-DOF robot arm; you can find some useful information from this article as well).

enter image description here

The reason Pseudo-Inverse Jacobian is used is, most of the time the inverse Jacobian leads to a matrix which is not full-rank, especially with robots which have many joints, like UR5. When that is the case, Pseudo-Inverse is used instead of the direct inverse method. However, I worked with youBot once in a project, which uses Pseudo-Inverse method for trajectory planning and I dealt with so many singularity cases. So singularities are not guaranteed to be prevented with Pseudo-Inverse Method. To avoid singularities, you can use some tolerance levels (you are able to select a tolerance value with built-in Pseudo-Inverse functions), or make sure that your trajectory is physically feasible and not too fast for your robot.

Also you might want to use some type of Control System for the trajectory following task. Here is a video that may help you on that; it is actually part of the videos which belong to my project which I mentioned earlier. Even though it is for a Mobile Manipulator; the idea is not very different for a Robot Arm; instead of acquiring joint and wheel velocities, you receive only joint velocities:



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.