This code simulates the movement of a robotic arm with 6 degrees of freedom (DOF) using a trapezoidal motion profile.
The DOF will be RPPRRR
R-rotation P-prismatic
The robot will include 6 DOF.
Rotation Prismatic Prismatic Rotation Rotation Rotation (theta1 , l1 ,l2 , theta2, theta3, theta4)
Video for stick model in ROS2-Rviz:
_.3.robotic.arm.-.dynamic.and.kinematicks.2023.-.YouTube.-.Google.Chrome_.2023-06-09.11-46-52.mp4
If you need the URDF, send me a message :)
Calculate the Forward Kinematicks for robotic arm DOF (RPPRRR)
point1=[0, 0, pi/2, 0, 250,0];
point2=[0, pi/2, pi/2, 0, 250, 250];
point3=[pi, 0, 0, 0, 500, 500];
point4=[pi, pi/2, pi/2, pi, 500, 500];
point5=[pi, 1.5*pi, 0, pi, 0, 0];
You can change the point(1,2,3,4,5) and get diffrent result
All the points the robot can reach.
(takes long time to run)
Get input for X,Y,Z and orientation matrix. the result will be the value of each DOF.
Calculate the Jacobian (parametric)
syms theta1 theta2 theta3 theta4 ;
Result:
J= [ 150*cos(theta2)*sin(theta1)*sin(theta3) - 150*cos(theta1)*cos(theta3) - l2*cos(theta1) - 151*cos(theta1), 0, -sin(theta1), 150*cos(theta1)*sin(theta2)*sin(theta3), 150*sin(theta1)*sin(theta3) - 150*cos(theta1)*cos(theta2)*cos(theta3), 0]
[- 151*sin(theta1) - 150*cos(theta3)*sin(theta1) - l2*sin(theta1) - 150*cos(theta1)*cos(theta2)*sin(theta3), 0, cos(theta1), 150*sin(theta1)*sin(theta2)*sin(theta3), - 150*cos(theta1)*sin(theta3) - 150*cos(theta2)*cos(theta3)*sin(theta1), 0]
[ 0, 1, 0, 150*cos(theta2)*sin(theta3), 150*cos(theta3)*sin(theta2), 0]
[ 0, 0, 0, 0, -cos(theta1)*sin(theta2), -cos(theta1)*sin(theta2)]
[ 0, 0, 0, 0, -sin(theta1)*sin(theta2), -sin(theta1)*sin(theta2)]
[ 1, 0, 0, 1, cos(theta2), cos(theta2)]
An analytical calculation of the moments and forces required in the robot's joints to lift a mass of M at the end effector.
Result:
tau= transpose([0 Mg 0 150*Mg*cos(theta2)*sin(theta3) 150*Mg*cos(theta3)*sin(theta2) 0 ])
For calculate the Motion planning, the inputs: start point and end point (X,Y,Z) and oreination (Roll,Pitch,Yaw)
Motion planning will base on acceleration. const speed and deceleration , trapzoid motion (end effector):
The position for the end effector (each dt) will look like:
(point [0 400 1200] -> [0 300 800])
The simulation will include a stick model for the robotic arm. Will look like this:
Figure.1.2023-05-21.12-37-13.mp4
This code simulates the movement of a robotic arm with multiple degrees of freedom (DOF) using a trapezoidal motion profile. The diffrence here is that every DOF will using trapezoidal motion profile and not the end effector.
The 3D plot for the X,Y,Z (End effector):
Simulation: