Stweart Platform Simulation with Trapezoidal Velocity Trajectory Planner and Local Singularity Avoidance
This Colab notebook simulates a Stewart Platform with a Trapezoidal Velocity Trajectory Planner and Local Singularity Avoidance, using four key classes:
- The Robot class models the Stewart Platform, handling forward/inverse kinematics, actuator forces, and singularity detection.
- The Trapezoidal Velocity class manages trajectory planning, generating desired joint trajectories.
- The Controller class handles control logic, issuing joint commands and tracking desired trajectories.
- The Simulation class ties it all together, running dynamic simulations, visualizing real-time behavior, and analyzing performance.
GitHubSingularityAvoidance.mov
Elements in the video:
- In blue the Stewart Platform.
- In black the closest singular configuration.
- The static series of frames is the trajectory coming from the trapezoidal class.
- The dynamic series of frames is the real trajectory of the robot.
- In the top right corner is the table of the forces felt by the actuators under gravity (enumaration at the base of the platform).
Needed libraries:
import numpy as np
from scipy.spatial.transform import Rotation as R
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from IPython.display import HTMLA short description of the key features of the four classes used in this repository is made here below.
This Stewart Platform Class has multiple uses that we will not fully cover here. The main function of the class that will be used here are the Inverse Kinematics, Forward Kinematics , Load Singularities and Find Closest singularity. Inverse and Forward kinematics are used to move from joint space to task space and vice versa when needed. The main focus of this class will be around Load Singularities and Find Closest singularity functions. The first one is used to upload the singularity positions in task space found offline, the second one is used to find the closest singularity given a pose.
Key functions description:
In the _find_closest_singularity function:
- Using the workspace boundaries, the pose of the robot and the singularities get normalized.
- The euclidean distance between the pose and all the singularities is calculated
- The minimum distance is found and the closest singularity is selected.
The Trapezoidal class is designed to generate a trapezoidal velocity profile for controlling the movement of a robotic system. This class functions as a path and trajectory planner which is generated in joint space and then converted to task space for the controller. Note : for each second of simulation 5 frames are needed, the correction factor is used to convert seconds into ticks and vice versa.
Key functions description:
In the _ init _ function:
- A copy of the robot is initialized to use IK and FK
- Initial and final pose are saved
- The time vector is defined
- The function calculate_q_profiles is called
- The function calculate_q_vel_profiles is called
- The function calculate_x_profiles is called
In the calculate_q_profiles function:
- For every every joint q :
- Limits of the trapezoidal trajectory are checked in the check_limits function. In case the minimum or maximum velocity constraints are not met, new values of velocities are proposed.
- For every t in the time vector: the time profile relative to the joint q is built.
In the calculate_q_vel_profiles function:
- For every every joint q :
- the velocity profile relative to the joint q is built.
In the calculate_x_profiles function:
- Using FK of the robot and the time profiles from calculate_q_profiles, time profiles in task space are calculated.
This controller stands in the middle between the trapezoidal velocity trajectory generation and the simulation environment. The objective is to maintain the robot onto the trapezoidal path while locally avoiding singularities if needed. Note : The controls are generated in task space and then converted into joint space for the motors.
Key functions description:
In the _ init _ function:
- The robot class is passed.
- The trapezoidal trajectory is initialized and generated.
- The time vector, initial and final position are found.
- Max joint velocity is defined.
- Standard deviation for gaussian obstacle profile generation is defined.(to be tuned)
In the calculate_cmd function:
- For every t (instant) in the time vector :
- The trajectory part of the control action is found (u=K*error). The error is calculated as the difference of the reference coming from the trapezoidal trajectory and the position of the robot in time.
- If the LocalConditionIndex is too low, the singularity part of the control action is found (u=K*closeness to singularity * direction). The closest singularity is found with the robot function getClosestSingularity. The closeness to the singularity (gaussian distance) to each component is found with the function findGaussianDistFromSingularity. K needs to be tuned.
- Velocity command is calculated and is checked for saturation.
In the findGaussianDistFromSingularity function:
- The pose of the robot and the closest singularity get normalized.
- For each pose component (x,y,z,roll,pitch,yaw), the gaussian distance from the singularity is found.
- The whole gaussian vector is then normalized.
The simulation class is used to simulate the robot side of the system. Commands arrive from the controller at every timestep, the position of the robot is updated taking into consideration random noise, feedback of the new configuration is provided to the controller. Using the library FuncAnimation it is possible to generate a video at 5 frames per second showing : the robot, the robot closest singularity, the desired trajectory, the real trajectory and the table of the forces experienced by the legs under the effect of gravity.
Key functions description:
In the _ init _ function:
- The controller and the robot are passed to the simulation class.
- The time vector, initial and final position are retrieved.
- The force to win gravity is defined (x direction).
In the start function
- For every t (instant) in the time vector :
- A new velocity command for the linear actuators is give by the controller.
- The simulation gets updated through the updateSimulation function.
- Feedback is given to the controller through the controller function updatePose.
- Multiple functions for plotting are then called.
In the updateSimulation function
- The theoretical displacement of the actuator is found.
- Random noise is added to the new displacement of the actuator to simulate real case scenario.
- New lenghts of actuators are found.
- Robot pose is updated.
In this section, the steps necessary to use the various elements is shown
- Mount the google drive.
- Define the Platform parameters.
- Upload the singularities (calculated offline with the __find_singularity_workspace stewart platform function).
# access drive to load singularities
drive.mount('/content/drive')
# Platform definition
r_b = 0.5 # Radius of base
phi_b = 50 # Angle between base joints
r_p = 0.3 # Radius of platform
phi_p = 80 # Angle between platform joints
# Workspace Boundaries definition
workspace_limits = [-0.5, 0.5, -0.5, 0.5, 0.1, 0.6]
orientation_limits = [-10, 10, -10, 10, -10, 10]
# load singularities into google colab
with open('/content/drive/My Drive/Github/filtered_singularities_task_space_2.txt', 'r') as f:
singularities_task_space = np.loadtxt(f)
# singularities_task_space = np.array([[0,0,0,0,0,0],[0,0,0,0,0,0]]) # uncomment if no singularities are available- Initialize robot class.
- Load the singularities into the robot object.
- Define the controller's parameters and initialize the controller. By initializing the controller, the trapezoidal velocity profile class is automatically constructed. Warning will pop up if the chosen set of speed and time does not meet the trapezoidal profile constraints.
- Initialize the simulation.
# Platform initialization
platform = StewartPlatform(r_b , phi_b , r_p, phi_p)
# Loading Singularities into robot.
platform.loadSingularitiesTaskSpace(singularities_task_space,workspace_limits,orientation_limits) # be coherent with the limits used for getSingularityWorkspace() function
# Define initial and final position
pose_i=np.array([0.3,0.1,0.5,0,-10,0]) # Note: FK algorithm does not like high values for angles.
pose_f=singularities_task_space[230]
# Define final time
tf_seconds=20 # defines the time in which you would like to reach the final pose
# Define controller parameters
max_joint_vel=0.03 # [m/s] # define max joint velocities
K=np.array([1,1,1,0.03,0.03,0.03])*0.5 # define proportional gain for the controller
# Initialize controller
controller = Controller(platform,pose_i,pose_f,tf_seconds,max_joint_vel,K)
# Initialize Simulation
sim = Simulation(controller)Now that the robot, the controller with trapezoidal trajectory and the simulation are setted up.
Start the simulation
If, during the trajectory, the robot passes too close to a singularity, warnings will pop up.
A series of plots is then generated to show the simulation's result.
# Start Simulation
sim.start()
Generate the video from the simulation.
Note: this may take some time as the animation is generated with matplotlib. In each second, five frames are generated.
Elements:
- In blue the Stewart Platform.
- In black the closest singular configuration.
- The static series of frames is the trajectory coming from the trapezoidal class.
- The dynamic series of frames is the real trajectory of the robot.
- In the top right corner is the table of the forces felt by the actuators under gravity (enumaration at the base of the platform).
# Generate video
HTML(sim.anim.to_html5_video())