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 endeffector 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 pickandplace 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 endeffector.

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, higherorder 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 pickandplace 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 pickandplace 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 2link planar robot manipulator with link lengths L1 = 4 units and L2 = 3 units, plan a trajectory for the endeffector 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: 
The above code first calculates the initial and final joint angles for the given endeffector 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 endeffector position. This information is essential for trajectory planning as it allows us to convert the desired position and orientation of the endeffector into joint angles that can be controlled by the actuators. Forward kinematics, on the other hand, is the process of calculating the endeffector 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 
Solution:
The The implementation of the 
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:
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. 
Add Comment
This policy contains information about your privacy. By posting, you are declaring that you understand this policy:
 Your name, rating, website address, town, country, state and comment will be publicly displayed if entered.
 Aside from the data entered into these form fields, other stored data about your comment will include:
 Your IP address (not displayed)
 The time/date of your submission (displayed)
 Your email address will not be shared. It is collected for only two reasons:
 Administrative purposes, should a need to contact you arise.
 To inform you of new comments, should you subscribe to receive notifications.
 A cookie may be set on your computer. This is used to remember your inputs. It will expire by itself.
This policy is subject to change at any time and without notice.
These terms and conditions contain rules about posting comments. By submitting a comment, you are declaring that you agree with these rules:
 Although the administrator will attempt to moderate comments, it is impossible for every comment to have been moderated at any given time.
 You acknowledge that all comments express the views and opinions of the original author and not those of the administrator.
 You agree not to post any material which is knowingly false, obscene, hateful, threatening, harassing or invasive of a person's privacy.
 The administrator has the right to edit, move or remove any comment for any reason and without notice.
Failure to comply with these rules may result in being banned from submitting further comments.
These terms and conditions are subject to change at any time and without notice.
Comments (2)
Hey there. The sample values used in Example one bring about the following error in the function when executed:
" imeWarning: invalid value encountered in arccos "
I suggest using the following values instead L1 = 2 , L2 = 1 , x=2 , y = 1
I'd really like if you would explain to me Inverse Kinematics for a sixDoF robot, because All the tutorials and resources are not for such robots.