This repo contains the details of our investigation for Kinetostatic Analysis for 6RUS Parallel Continuum Robot presented in the paper Kinetostatic Analysis for 6RUS Parallel Continuum Robot using Cosserat Rod Theory. The paper is published in the Springer Proceedings in Advanced Robotics.
Parallel Continuum Robots (PCR) are closed-loop mechanisms but use elastic kinematic links connected in parallel between the end-effector (EE) and the base platform. PCRs are actuated primarily through large deflections of the interconnected elastic links unlike by rigid joints in rigid parallel mechanisms.
In this paper, Cosserat rod theory-based forward and inverse kinetostatic models of a
Maintainers:
- Vinay Rodrigues [email protected]
- Bingbin Yu [email protected]
The libraries used were tested successfully in Python3.8.16 and Ubuntu 18.04.6 LTS.
In this work, boundary conditions for both IK and FK are formulated for a
initial states of the rod:
pi0: base position of the flexible rod,
Ri0: orientation at the base of the flexible rod,
ni(0): internal force acting at the base of the rod,
mi(0): internal moment acting at the base of the rod.
For the given pose of the end-effector p_ee and R_ee, external force F and moment M acting at the end-effector, initial states of the rod, and unknown variable vector init_guess, then Inverse_Kinetostatic() function computes the motor angles that minimizes the residual vector residual. q1i, q2i, and q3i are the motor angles, and universal joints angles respectively whereas ni_x(0), ni_y(0), ni_z(0), mi_z(0) are the internal forces and moments at the base of the flexible link which are unknown. Due to universal joints at the base of the rod, mi_x(0)=mi_y(0)=0.
File path: ./Inverse_forward kinetostatic/IK_PCR_ROD.py
p_ee = np.array([0,0,0.5])
R_ee = np.array([np.deg2rad(10),np.deg2rad(0),np.deg2rad(0)])
#initializing the actuator variables + universal joint values for each rod--> q=[q1, q2, q3]
qi = np.array([0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0,
0,0,0])
##initializing the guess vector for the IK model
#init_guess = [n1_x(0), n1_y(0), n1_z(0), m1_z(0),...,n6_x(0), n6_y(0), n6_z(0), m6_z(0), q1i, q2i, q3i,...,q16, q26, q36] #42 variables
init_guess = np.concatenate([np.zeros(24),qi])
q1 = Inverse_Kinetostatic(p_ee, R_ee, init_guess)
6-element Vector{Real}:
0.48785652
0.48785652
0.42037453
0.28121425
0.28121425
0.42037453For a given motor angle q1i, external force F and moment M acting at the end-effector, initial states of the rod, and unknown variable vector init_guess, then Forward_Kinetostatic() function computes the pose of the end-effector, p_ee and R_ee that minimizes the residual.
File path: Inverse_forward kinetostatic/FK_PCR_ROD.py
#intial guess for the pose of the end-effector
p_ee = np.array([0,0,0.4]) #m
R_ee = np.array([np.deg2rad(0), np.deg2rad(0), np.deg2rad(0)]) #radians
#intial guess for the Motor angle in radians (values taken from IK model for p_ee=[0,0,0.5], R_ee=[0, 0, 0])
qm = np.array([0.48785652,
0.48785652,
0.42037453,
0.28121425,
0.28121425,
0.42037453])
#universal joint angle initialization in radians
qi = np.array([0,0,
0,0,
0,0,
0,0,
0,0,
0,0])
#initializing the guess vector for the FK model
#init_guess = [n1_x(0), n1_y(0), n1_z(0), m1_z(0),...,n6_x(0), n6_y(0), n6_z(0), m6_z(0), q2i, q3i,...,q26, q36, p_ee, R_ee] #42 variables
init_guess = np.concatenate([np.zeros(24),qi,p_ee,R_ee])
p_ee, R_ee = Forward_Kinetostatic(init_guess, qm)
Optimized pose of the EE:
p_ee=[9.21906358e-09 7.30108121e-04 4.97398602e-01] 'and' R_ee=[ 1.77036209e-01 -3.45502705e-08 2.06590012e-08]In this simulation, the FK model is validated by comparing the obtained solution of the EE position with samples from a reference helical trajectory under a constant load of 5 N at the EE, as depicted in Figure (left). Euclidean distance is calculated for each sample to measure the error between the FK model and the reference trajectory samples. As shown in Figure (right), the error is estimated to be on the order of
File path for the motor generation using IK model: Trajectory_comaprison/IK_PCR_Trajectory.py
ee_mass = 0.5 #mass of the end-effector platform (Kg)
#compute motor joint angles for samples representing the position of the EE from a
#helical trajectory using IK model
[optimised_states, total_time] = Inverse_Kinetostatic_traj(p_ee[i], angles, init_guess)where optimised_states includes optimized values of the guessed unknown vector init_guess, total_time is the computational time for a solution. IK_vec stores the
total_time, optimized motor angles q1i, and p_ee (p_ee info is redundant here) into an excel_file.xlsx.
Now, the computed q1i is extracted from the excel_file.xlsx then Forward_Kinetostatic_traj() function is used to compute the pose of the end-effector p_ee and R_ee which is compared with the reference helical trajectory. The error is calculated by the Euclidean distance between the two.
File path for the end-effector pose generation using FK model: Trajectory_comaprison/FK_PCR_Trajectory.py
#Provide these motor angles as input to FK model:
ee_mass = 0.5 #mass of the end-effector platform (Kg)
#initializing the guess vector for the FK model
init_guess = np.concatenate([np.zeros(24),qi,p_ee,R_ee]) #42 variables
FK_vec = Forward_Kinetostatic_traj(motor_angle[i], init_guess)FK_vec stores the total_time, optimised pose of the end-effector p_ee, and R_ee. Forward_Kinetostatic_traj() plots both the trajectory comparison and Euclidean error plots using the FK_plots().
By changing F variables in Inverse_forward kinetostatic/IK_PCR_ROD.py, the weight at the end-effector can be adjusted. For the rotation of the end-effector, variable R_ee can be adjusted by providing the orientation about z-axis in Inverse_forward kinetostatic/IK_PCR_ROD.py.
In this section, we estimate the reachable workspace for the
ee_mass = 1e-12 #mass of the end-effector platform (Kg)
#find the IK solution for each of the generated samples using
Workspace(p_ee[i], R_ee, init_guess)Function Workspace() stores the residual values in an excel_file.xlsx. This excel_file.xlsx contains columns for computational time total_time, motor angles q1_vec, universal joint angles q2_vec and q3_vec, sample position of end-effector p_ee[i], and the residual vector for each sample restrack. The excel_file.xlsx is then passed to the workspace_analysis() function where the reachable workspace is filtered and visualized based on the tolerance values restrack.
The work presented in this paper is supported by the PACOMA project (Grant No. ESA-TECMSM-SOW-022836) subcontracted to us by Airbus Defence & Space GmbH (Grant No. D.4283.01.02.01) with funds from the European Space Agency. The authors also want to acknowledge John Till's GitHub tutorial on PCR and his guidance on deriving the boundary condition equations for the proposed PCR.
@InProceedings{10.1007/978-3-031-64057-5_49,
author="Rodrigues, Vinayvivian
and Yu, Bingbin
and Stoeffler, Christoph
and Kumar, Shivesh",
editor="Lenar{\v{c}}i{\v{c}}, Jadran
and Husty, Manfred",
title="Kinetostatic Analysis for 6RUS Parallel Continuum Robot Using Cosserat Rod Theory",
booktitle="Advances in Robot Kinematics 2024",
year="2024",
publisher="Springer Nature Switzerland",
address="Cham",
pages="426--434",
abstract="Parallel Continuum Robots (PCR) are closed-loop mechanisms but use elastic kinematic links connected in parallel between the end-effector (EE) and the base platform. PCRs are actuated primarily through large deflections of the interconnected elastic links unlike by rigid joints in rigid parallel mechanisms. In this paper, Cosserat rod theory-based forward and inverse kinetostatic models of a 6-RUS PCR are proposed. A set of simulations is performed to analyze the proposed PCR structure, including workspace analysis, maneuverability in three-dimensional space through trajectory following, assessment of inverse kinetostatic solutions for both the xy planar rotation, and evaluation of the force response at the EE platform along the z-axis.",
isbn="978-3-031-64057-5"
}