Robotics Joint Space Trajectory (Cubic & Quintic Polynomial)

[00:00:00]So previously, we talked about the inverse kinematics solution using the numerical approach. In this video, we're going to talk about the joint space trajectory. We'll see an example with the UR robot how to generate smooth trajectories for each joint. You can follow along with my code and documentation, link in the video description. So let's say you have a robot in this starting configuration and you want to move it to a new configuration. This is where the joint space trajectory comes in. So we call these two points the initial joint angle and we have a final joint angle. So you have the smooth trajectory that we're going to obtain for each of the joints.

[00:00:36]And this is what we call a cubic polynomial. So this is a third order polynomial and we have these coefficients A0, A1, A2, and A3. So if we place special initial and final conditions, we could guarantee this trajectory to be smooth. So smooth means that the initial velocity is zero and the final velocity is zero. So specifically, what does that mean to set up our equations? So you can see we have a equation that has four unknowns and we could generate four equations from our initial conditions. So you see right here, we have our initial position. This is where we start and we have a final position. This is where we finish. And we could set the initial and final velocities to be Q.0 and this is the Q.not and we have the Q.final. So these are our initial and final velocities and typically, we set these to be zero. So that also simplifies our equations.

[00:01:36]And you can see right here, if we solve for the constants, we get these equations. So the constants for the first two is going to be Q0 and Q0.

[00:01:47]And then A2 and A3, you could solve it and you'll get these equations. So we have the equations for the four constants and then you could solve for it. And if you want to do it by hand, you can see these are the different expressions for the polynomial. We have Q, the first derivative Q dot, and then Q double dot. So, once you have a system of equations, you could solve for the A0, A1, A2, and A3. Now, you can see the top graph is our position graph. So, if you were to take the derivative, then you would have the second graph here.

[00:02:22]What the second graph tells us is that the initial and final velocities are both zero, which agrees with our initial and final conditions. And if we take the derivative once more, which is our third graph, this is going to give us our acceleration. So, you can see that at the very peak of our velocity, we have an acceleration of zero. So, you initially start with a excel phase, and then you have a diesel phase. Now, in some cases, you might want to place additional constraints to the acceleration. So, we didn't do that previously, but if you did want to, then you would need to have more equations.

[00:03:00]And to get more equations, you need to have a higher order polynomial, such as the quintic polynomial. This one goes up to a fifth order, so that way you have more equations. If you were to solve for it, you would get these as your constants. So, you'll have A0, A1, and A2 here, and then the bottom we have A3, A4, and A5. If I scoot over, you can see, yep, this is your A5.

[00:03:26]So, these would be your final solution if you were to use the quintic polynomial and solve your system of equations. Now, in my ROS code here, I have a section on the joint space trajectory. So, you have to first start up your hardware, which I already did, and then what you want to do is you could launch this file here, which I call the joint space trajectory.launch.py.

[00:03:47]So, you want to set the initial position you want to have, and then or I mean the target position, it's going to read the initial position based off of the joint states, and then you specify a duration.

[00:03:58]And then, if we go ahead and run this, then we can see it move to our desired position. So, you can see our graph running here in PlotJuggler, and you can see all the joints is nice and smooth.

[00:04:08]And if you're new here, my name is Kevin. I've been doing robotics and AI for 10 plus years, and have lots of resources on my channel. If you want to see more deep dive videos for my behind-the-scenes videos, make sure to come here to my membership. Join my robotics builders to get to see those behind-the-scenes videos. And if you're completely new to robotics and AI, make sure to check out my Masters Robotics AI bundle, as well as my robotics projects bundle, link in the video description.

[00:04:35]>> [music]