Robotics: Theory and Examples

7. Trajectory Planning: Smoothly Controlling Robotic Manipulators

In the field of robotics, trajectory planning is a crucial concept that involves determining the optimal path for a robotic manipulator to move between an initial and a final position while satisfying certain constraints such as joint velocities, accelerations, and end-effector forces. In this article, we will explore the concept of trajectory planning and how it is used to control the movement of robotic manipulators. We will also demonstrate how the joint angles of a 2R robot arm are interpolated between the home, initial, and final positions to create smooth trajectories. Please refer to the 2R pick-and-place robot simulation on GitHub.

7.1. The Importance of Trajectory Planning

Trajectory planning plays a significant role in the control of robotic manipulators, as it ensures that the manipulator’s motion is smooth and efficient, while avoiding obstacles and satisfying any imposed constraints. Some of the essential aspects of trajectory planning include:

Defining the initial and final positions and orientations of the end-effector.
Specifying the desired motion profile, such as the velocity, acceleration, and jerk profiles.
Ensuring that the manipulator’s movement respects the joint limits and other constraints.
Accounting for the presence of obstacles in the workspace and planning paths that avoid collisions.
Optimizing the trajectory to minimize energy consumption, travel time, or other performance metrics.

7.2. Interpolation Techniques for Smooth Trajectories

In trajectory planning, interpolation techniques are often used to compute intermediate joint angles between the initial and final positions. These techniques help create smooth trajectories that respect the desired motion profiles and satisfy the imposed constraints. Some common interpolation techniques include:

Linear interpolation: This is the simplest interpolation method, where the joint angles are linearly interpolated between the initial and final positions.
Polynomial interpolation: In this method, higher-order polynomials are used to interpolate the joint angles, allowing for smoother trajectories with continuous velocity and acceleration profiles.
Spline interpolation: This method involves using piecewise polynomial functions to create smooth trajectories that pass through a set of specified waypoints.

7.3. Demonstrating Trajectory Planning with a 2R Robot Arm Simulation

In our 2R robot arm simulation, we can demonstrate trajectory planning by interpolating the joint angles between the home, initial, and final positions. This can be achieved using the following steps:

Define the home, initial, and final positions in joint space.
Specify the desired motion profile, such as the duration and the time intervals for each phase of the pick and place task.
Use an interpolation method, such as linear interpolation or polynomial interpolation, to compute the intermediate joint angles between the home, initial, and final positions.
Update the robot arm’s joint angles for each time interval, and visualize the resulting motion using the FuncAnimation class from the matplotlib library.

By following these steps, we can create smooth trajectories for the 2R robot arm’s movement, allowing it to perform a pick-and-place task efficiently and accurately.

In the 2R robot simulation code provided, the key part of trajectory planning lies in the update function, which is responsible for smoothly updating the joint angles and positions during the animation. Here’s an excerpt from the update function that demonstrates how joint angles are interpolated between home, initial, and final positions to create smooth trajectories:

# 1. Move from the home position to the initial position
if frame < num_frames / 6:
    # Calculate the current joint angles based on the frame number
    # (interpolate between the home position and the initial position)
    t = frame / (num_frames / 6)
    theta1_diff = shortest_angle_diff(theta1_home, theta1_initial)
    theta2_diff = shortest_angle_diff(theta2_home, theta2_initial)
    theta1 = theta1_home + t * theta1_diff
    theta2 = theta2_home + t * theta2_diff
# 3. Pick up the box and move it to the final position
elif frame < 3 * num_frames / 6:
    # Calculate the current joint angles based on the frame number
    # (interpolate between the initial position and the final position)
    t = (frame - 2 * num_frames / 6) / (num_frames / 6)
    theta1_diff = shortest_angle_diff(theta1_initial, theta1_final)
    theta2_diff = shortest_angle_diff(theta2_initial, theta2_final)
    theta1 = theta1_initial + t * theta1_diff
    theta2 = theta2_initial + t * theta2_diff
# 5. Return to the home position from the final position
elif frame < 5 * num_frames / 6:
    # Calculate the current joint angles based on the frame number
    # (interpolate between the final position and the home position)
    t = (frame - 4 * num_frames / 6) / (num_frames / 6)
    theta1_diff = shortest_angle_diff(theta1_final, theta1_home)
    theta2_diff = shortest_angle_diff(theta2_final, theta2_home)
    theta1 = theta1_final + t * theta1_diff
    theta2 = theta2_final + t * theta2_diff

The update function divides the total number of frames into different sections, each corresponding to a different part of the pick-and-place operation. During the movement between home, initial, and final positions, joint angles are interpolated using a linear interpolation based on the current frame number. The shortest_angle_diff function calculates the difference between two angles, which is then used to determine the interpolation step.

This approach ensures that the robot’s joint angles smoothly change between different positions, creating smooth trajectories for the robotic manipulator.

In conclusion, trajectory planning is a fundamental concept in robotics that enables the smooth and precise control of robotic manipulators. By using interpolation techniques to compute intermediate joint angles, we can ensure that the manipulator’s motion respects the desired motion profiles and satisfies any imposed constraints.

7.4. Exercise

Example 1

Explain the Cubic Polynomial Trajectory Planning method and derive the equations to obtain the coefficients for the polynomial.

Solution:

Cubic Polynomial Trajectory Planning is a method used to generate smooth trajectories for robotic manipulators by connecting initial and final positions and velocities with a continuous curve while ensuring that the velocity and acceleration constraints are satisfied.

Consider a cubic polynomial function of the form:

\[x(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3\]

The coefficients can be found by solving a system of linear equations that satisfy the boundary conditions: initial position, final position, initial velocity, and final velocity. We have:

\[\begin{align*} x(0) &= a_0 \\ \dot{x}(0) &= a_1 \\ x(T) &= a_0 + a_1 T + a_2 T^2 + a_3 T^3 \\ \dot{x}(T) &= a_1 + 2 a_2 T + 3 a_3 T^2 \end{align*}\]

where \(T\) is the total time duration for the trajectory.

By solving this system of linear equations, we can obtain the coefficients for the cubic polynomial trajectory.

Example 2

Given a 2-link planar robot manipulator with link lengths L1 = 4 units and L2 = 3 units, plan a trajectory for the end-effector to move from the initial position (x1, y1) = (5, 2) to the final position (x2, y2) = (2, 4) in 5 seconds. Use cubic polynomial trajectory planning and implement your solution using Python programming.

Solution:

To plan a trajectory using cubic polynomial trajectory planning, we can use the following equations for each joint angle:

\[q(t) = a0 + a1 \times t + a2 \times t^2 + a3 \times t^3\]

Here’s the Python code to plan the trajectory:

import numpy as np

def inverse_kinematics(x, y, L1, L2):
    d = np.sqrt(x**2 + y**2)
    a = np.arccos((L1**2 + L2**2 - d**2) / (2 * L1 * L2))
    b = np.arctan2(y, x) - np.arctan2(L2 * np.sin(a), L1 + L2 * np.cos(a))
    return b, a + b

def cubic_coefficients(q0, qf, t0, tf):
    a0 = q0
    a1 = 0
    a2 = (3 * (qf - q0) / ((tf - t0) ** 2))
    a3 = (-2 * (qf - q0) / ((tf - t0) ** 3))
    return a0, a1, a2, a3

def cubic_trajectory(t, a0, a1, a2, a3):
    return a0 + a1 * t + a2 * t**2 + a3 * t**3

L1 = 4
L2 = 3
x1, y1 = 5, 2
x2, y2 = 2, 4
t0, tf = 0, 5

theta1_0, theta2_0 = inverse_kinematics(x1, y1, L1, L2)
theta1_f, theta2_f = inverse_kinematics(x2, y2, L1, L2)

a0_1, a1_1, a2_1, a3_1 = cubic_coefficients(theta1_0, theta1_f, t0, tf)
a0_2, a1_2, a2_2, a3_2 = cubic_coefficients(theta2_0, theta2_f, t0, tf)

time_points = np.linspace(t0, tf, num=100)
joint1_trajectory = [cubic_trajectory(t, a0_1, a1_1, a2_1, a3_1) for t in time_points]
joint2_trajectory = [cubic_trajectory(t, a0_2, a1_2, a2_2, a3_2) for t in time_points]

print(f"Joint 1 trajectory: {joint1_trajectory}")
print(f"Joint 2 trajectory: {joint2_trajectory}")

The above code first calculates the initial and final joint angles for the given end-effector positions. Then, it computes the cubic polynomial coefficients for each joint angle and generates a trajectory for each joint by sampling 100 points between t0 and tf. The output will display the joint trajectories for joint 1 and joint 2.

Example 3

What is the significance of inverse and forward kinematics in trajectory planning for robotic manipulators, and how are they used?

Solution:

Inverse kinematics and forward kinematics are fundamental concepts in trajectory planning for robotic manipulators.

Inverse kinematics is the process of calculating the joint angles required for a manipulator to achieve a desired end-effector position. This information is essential for trajectory planning as it allows us to convert the desired position and orientation of the end-effector into joint angles that can be controlled by the actuators.

Forward kinematics, on the other hand, is the process of calculating the end-effector position and orientation given a set of joint angles. This is useful for verifying if the manipulator has reached the desired position and for simulating the movement of the manipulator throughout the trajectory.

Both inverse and forward kinematics are used together in trajectory planning to ensure smooth and accurate control of the robotic manipulator.

Example 4

Explain the role of the shortest_angle_diff function in the 2R robot pick and place simulation code. Provide a brief description of its implementation.

Solution:

The shortest_angle_diff function is used to compute the shortest angle difference between two angles (a1 and a2) in the given 2R robot pick and place simulation code. This function is essential for generating smooth transitions between different joint angles during the trajectory planning process.

The implementation of the shortest_angle_diff function is as follows:

def shortest_angle_diff(a1, a2):
    diff = a2 - a1
    return (diff + np.pi) % (2 * np.pi) - np.pi

In this implementation, the difference between the two angles is first calculated. Then, the modulo operation is used to find the remainder of the division by \(2\pi\). Finally, the difference is adjusted by subtracting \(\pi\), which results in the shortest angle difference between the two input angles.

Example 5

Describe the role of the time scaling function in trajectory planning for robotic manipulators and provide an example of a commonly used time scaling function.

Solution:

The time scaling function plays a crucial role in trajectory planning for robotic manipulators as it maps the normalized time parameter onto the actual time duration of the trajectory. This mapping allows for the generation of smooth trajectories while respecting the constraints on velocity and acceleration of the manipulator.

A commonly used time scaling function is the trapezoidal velocity profile. In this profile, the manipulator accelerates from the initial position at a constant rate until it reaches the maximum allowed velocity. It then maintains this velocity for some time before decelerating at a constant rate to reach the final position.

The trapezoidal velocity profile can be divided into three segments:

Acceleration phase: \(t \in [0, t_a\) ]
Constant velocity phase: \(t \in [t_a, t_d\) ]
Deceleration phase: \(t \in [t_d, T\) ]

where \(t_a\) is the time at which the acceleration phase ends, \(t_d\) is the time at which the deceleration phase starts, and \(T\) is the total time duration of the trajectory.

The time scaling function for the trapezoidal velocity profile can be defined as:

\[s(t) = \begin{cases} \frac{1}{2}at^2, & \mbox{if } t \in [0, t_a]\\ at_a(t - \frac{1}{2}t_a), & \mbox{if } t \in [t_a, t_d]\\ at_a(t_d - t_a) + \frac{1}{2}a(T^2 - 2Tt + t^2), & \mbox{if } t \in [t_d, T] \end{cases}\]

where \(s(t)\) is the scaled time parameter, \(a\) is the constant acceleration/deceleration rate, and \(t\) is the actual time.

Robotics: Theory and Examples

7. Trajectory Planning: Smoothly Controlling Robotic Manipulators

7.1. The Importance of Trajectory Planning

7.2. Interpolation Techniques for Smooth Trajectories

7.3. Demonstrating Trajectory Planning with a 2R Robot Arm Simulation

7.4. Exercise

Add Comment

Comments (2)