Cartesian Space Trapezoidal Planning for 6-DOF Robots: Smooth Motion Control with Position & Orientation Synchronization

1 Introduction

When planning robot motion, we have two main approaches: joint space planning and Cartesian space planning. If you've ever worked with robotic arms, you know that joint space trajectories move each joint individually, often leading to unpredictable end-effector paths. But what if we need the end-effector to follow a precise path in space, like drawing a straight line or aligning with a workpiece? That's where Cartesian space planning comes in!

What is Cartesian Space Planning?

Cartesian space planning allows us to control the motion of a 6-DOF robotic arm directly in terms of the end-effector’s position (x, y, z) and orientation (roll, pitch, yaw). Instead of worrying about each joint's motion, we simply specify the desired start and end positions, and the planner ensures a smooth path between them.

This approach is super useful in tasks like:

a) Pick-and-place operations – where the gripper must follow a straight-line approach.

b) Welding and cutting – where precise tool orientation is crucial.

c) Surgical robotics – where even tiny orientation errors can have major consequences.

By planning in Cartesian space, we ensure that the end-effector moves naturally and predictably, rather than taking shortcuts caused by joint configurations.

Fig 1: Path and Orientation Interpolation

Why Use a Trapezoidal Velocity Profile?

As I’ve covered in my joint space trapezoidal planning blog, using a trapezoidal velocity profile helps maintain smooth motion by controlling acceleration and deceleration. This avoids jerks, reduces mechanical stress, and ensures efficient and predictable motion execution. Why Do We Handle Position and Orientation Separately?

Here’s where things get interesting! In a 6-DOF robot, we have two things to control:

• Position: The x, y, z coordinates of the end-effector.

• Orientation: The roll, pitch, yaw angles that define its rotation.

If we treat them the same way, things can go wrong:

🚫 The end-effector might reach the right position but face the wrong way.

🚫 It might rotate too fast or too slow compared to its movement.

To fix this, we:

1.Calculate the total path length (distance between the start and goal positions).

2. Calculate the total orientation change (using quaternions for smooth rotation).

3. Apply the trapezoidal velocity profile to whichever is greater (position or orientation).

This way, both position and orientation changes happen in sync, and the motion feels smooth and natural.

2 Lets Do Some Mathematics........

So for example we have a current position and orientation, and desired position and orientation:

3 Code

Below is a type of Pseudo Code to understand the above concepts. I have put all the graphs and full code implementation in a Jupyter Notebook. You can access it through GitHub Code

4 Frequently Asked Questions(FAQs)

Let’s address some common questions that might come to your mind. If you have any other doubts, feel free to ask in the comments section!

Q1: We are considering only the total path length, but not treating x, y, and z separately. Will this affect the end-effector position in the actual robot?

Ans: No, it will not affect the actual end-effector position. I have tested this method on both 6-DOF and 7-DOF robotic arms, and it works correctly. The reason is that the planned trajectory forms a straight-line path in 3D space, ensuring that the resultant velocity components in x, y, and z directions follow a trapezoidal profile. Since we sample the waypoints directly from this trajectory, the robot follows the correct motion without deviations.

Q2: Can we generate other types of paths in Cartesian space, or are we limited to straight lines? How does rotation affect this?

Ans: Yes! We can generate any type of trajectory in Cartesian space, including quadratic, cubic, and quintic curves—not just straight lines. However, the key challenge is that the path parameter must be computed dynamically along the trajectory. If we don’t do this, we won’t be able to maintain a trapezoidal velocity profile in Cartesian space.

I’ll be covering other types of Cartesian space trajectories in future blogs, where I’ll explain this concept in detail. So, stay tuned! 

Q3: Won’t taking the maximum of path length and orientation length create a bottleneck in motion? For example, if the orientation length is much larger than the position length, won’t the robot move too slowly in x, y, z?

Ans: Yes, you are absolutely right! If the orientation length is significantly larger than the position length, the robot will move very slowly in Cartesian space (x, y, z) because the larger value dictates the motion speed.

However, this approach is necessary to ensure that both position and orientation remain synchronized. If we treat them separately, we risk the end-effector reaching the final position before achieving the correct orientation (or vice versa).

That said, there is always a trade-off between synchronization and independent motion control. If strict synchronization isn’t required for your application, you could control position and orientation separately to optimize performance.

5 References

1. ChatGpt

2. Robotics and Control by RK Mittal and IJ Nagrath.