Robot Arm Trajectory with multiple degrees of freedom

9 views (last 30 days)
Dear Matlab users;
Think that I have a 2 DOF planar arm. I am using Peter Corke's robotic toolbox, which it has "tpoly" comment.
Note: tpoly Generate scalar polynomial trajectory
[S,SD,SDD] = tpoly(S0, SF, M) is a scalar trajectory (Mx1) that varies
smoothly from S0 to SF in M steps using a quintic (5th order) polynomial.
Velocity and acceleration can be optionally returned as SD (Mx1) and SDD
(Mx1) respectively.
tpoly(S0, SF, M) as above but plots S, SD and SDD versus time in a single
figure.
[S,SD,SDD] = tpoly(S0, SF, T) as above but the trajectory is computed at
each point in the time vector T (Mx1).
[S,SD,SDD] = tpoly(S0, SF, T, QD0, QD1) as above but also specifies the
initial and final velocity of the trajectory.
Notes::
- If M is given
- Velocity is in units of distance per trajectory step, not per second.
- Acceleration is in units of distance per trajectory step squared, not
per second squared.
- If T is given then results are scaled to units of time.
- The time vector T is assumed to be monotonically increasing, and time
scaling is based on the first and last element.
Reference:
Robotics, Vision & Control
Chap 3
Springer 2011
-----
So I use it for creating theta1, with these exact MATLAB codes:
clear all; clc; close all;
f1=0:90:180;
f2=0:90:180;
[F1,F2]=meshgrid(f1,f2);
f1rs=reshape(F1,[],1);
f2rs=reshape(F2,[],1);
f1f2=[f1rs f2rs];
sizef1f2=length(f1f2);
ss=100;
for i=1:sizef1f2
[q1(:,i),q1d(:,i),q1dd(:,i)]=tpoly(f1rs(i,1),f2rs(i,1),ss);
end
q1=reshape(q1,[],1);q1d=reshape(q1d,[],1);q1dd=reshape(q1dd,[],1);
figure;plot(q1(1:100));hold on
for i=1:(sizef1f2-1)
plot(q1((i*100+1):(i+1)*100))
end
hold off
I wrote these MATLAB codes, so it creates different trajectories for theta1/q1. Size of q1 is 900x1. (Since I used the comment of "reshape") the first 100 rows are for the trajectory of q1 from 0° to final value of 0°, the rows between 101-200 is for the trajectory from 0° to 90°, the rows between 201-300 is for the trajectory from 0° to 180°, the rows between 301-400 is for the trajectory from 90° to 0°, and so on... Shortly:
Starting theta1 angle - Final theta1 angle
q1=[ from 0° - to 0°;
from 0° - to 90°;
from 0° - to 180°;
from 90° - to 0°;
from 90° - to 90°;
from 90° - to 180°;
from 180° - to 0°;
from 180° - to 90°;
from 180° - to 180°]
Until this part, it is considered only one arm. However I am trying to implement two degrees of freedom robotic manipulator. So I will match each individual trajectories of theta1, with different theta2 trajectories. In summary, the matrix should be look like this:
Theta1 starting value - Theta1 final value Theta2 starting value-Theta2 final value
q1= [ from 0° - to 0° from 0° - to 0°;
0° - to 0° from 0° - to 90°;
0° - to 0° from 0° - to 180°;
0° - to 0° from 90° - to 0°;
0° - to 0° from 90° - to 90°;
0° - to 0° from 90° - to 180°;
0° - to 0° from 180° - to 0°;
0° - to 0° from 180° - to 90°;
0° - to 0° 180° - to 180°;
from 0° - to 90° from 0° - to 0°;
from 0° - to 90° from 0° - to 90°;
from 0° - to 90° from 0° - to 180°;
from 0° - to 90° from 90° - to 0°;
from 0° - to 90° from 90° - to 90°;
from 0° - to 90° from 90° - to 180°;
from 0° - to 90° from 180° - to 0°;
from 0° - to 90° from 180° - to 90°;
from 0° - to 90° from 180° - to 180°;
from 0° - to 180° from 0° - to 0°;
from 0° - to 180° from 0° - to 90°;
from 0° - to 180° from 0° - to 180°;
from 0° - to 180° from 90° - to 0°;
from 0° - to 180° from 90° - to 90°;
from 0° - to 180° from 90° - to 180°;
from 0° - to 180° from 180° - to 0°;
from 0° - to 180° from 180° - to 90°;
from 0° - to 180° from 180° - to 180°;
from 90° - to 0° from 0° - to 0°;
from 90° - to 0° from 0° - to 90°;
from 90° - to 0° from 0° - to 180°;
from 90° - to 0° from 90° - to 0°;
from 90° - to 0° from 90° - to 90°;
from 90° - to 0° from 90° - to 180°;
from 90° - to 0° from 180° - to 0°;
from 90° - to 0° from 180° - to 90°;
from 90° - to 0° from 180° - to 180°;
from 90° - to 90° from 0° - to 0°;
from 90° - to 90° from 0° - to 90°;
from 90° - to 90° from 0° - to 180°;
from 90° - to 90° from 90° - to 0°;
from 90° - to 90° from 90° - to 90°;
from 90° - to 90° from 90° - to 180°;
from 90° - to 90° from 180° - to 0°;
from 90° - to 90° from 180° - to 90°;
from 90° - to 90° from 180° - to 180°;
from 90° - to 180° from 0° - to 0°;
from 90° - to 180° from 0° - to 90°;
from 90° - to 180° from 0° - to 180°;
from 90° - to 180° from 90° - to 0°;
from 90° - to 180° from 90° - to 90°;
from 90° - to 180° from 90° - to 180°;
from 90° - to 180° from 180° - to 0°;
from 90° - to 180° from 180° - to 90°;
from 90° - to 180° from 180° - to 180°;
from 180° - to 0° from 0° - to 0°;
from 180° - to 0° from 0° - to 90°;
from 180° - to 0° from 0° - to 180°;
from 180° - to 0° from 90° - to 0°;
from 180° - to 0° from 90° - to 90°;
from 180° - to 0° from 90° - to 180°;
from 180° - to 0° from 180° - to 0°;
from 180° - to 0° from 180° - to 90°;
from 180° - to 0° from 180° - to 180°;
from 180° - to 90° from 0° - to 0°;
from 180° - to 90° from 0° - to 90°;
from 180° - to 90° from 0° - to 180°;
from 180° - to 90° from 90° - to 0°;
from 180° - to 90° from 90° - to 90°;
from 180° - to 90° from 90° - to 180°;
from 180° - to 90° from 180° - to 0°;
from 180° - to 90° from 180° - to 90°;
from 180° - to 90° from 180° - to 180°;
from 180° - to 180° from 0° - to 0°;
from 180° - to 180° from 0° - to 90°;
from 180° - to 180° from 0° - to 180°;
from 180° - to 180° from 90° - to 0°;
from 180° - to 180° from 90° - to 90°;
from 180° - to 180° from 90° - to 180°;
from 180° - to 180° from 180° - to 0°;
from 180° - to 180° from 180° - to 90°;
from 180° - to 180° from 180° - to 180°]
And I will have q1 and q2 trajectories for different combinations, as well as q1d, q2d, (for derivatives of q's: angular velocity); q1dd, q2dd (second derivative of q's: angular accelerations).
What I am trying to is computing Tau1 and Tau2 torques for different trajectories, like a mapping between joint angles, velocity, acceleration inputs and torque outputs. I will use these mapping for teaching an artificial neural network.
The whole MATLAB codes, including computing the torques is like this: (Since I couln't make it done yet, for simplicity, I made equal angles for theta1 and theta2: theta2=theta1; q2=q1; q2d=q1d; qdd=1dd) :
clear all; clc; close all;
f1=0:90:180;
f2=0:90:180;
[F1,F2]=meshgrid(f1,f2);
f1rs=reshape(F1,[],1);
f2rs=reshape(F2,[],1);
f1f2=[f1rs f2rs];
sizef1f2=length(f1f2);
ss=100;
for i=1:sizef1f2
[q1(:,i),q1d(:,i),q1dd(:,i)]=tpoly(f1rs(i,1),f2rs(i,1),ss);
end
q1=reshape(q1,[],1);q1d=reshape(q1d,[],1);q1dd=reshape(q1dd,[],1);
figure;plot(q1(1:100));hold on
for i=1:(sizef1f2-1)
plot(q1((i*100+1):(i+1)*100))
end
hold off
%------------------
q2=q1; q2d=q1d; q2dd=q1dd;
a1=1;a2=1;ac1=0.5;ac2=0.5;m1=50;m2=50;I1=10;I2=10;g=9.81;
for i=1:ss:length(q1)
i;
tau1(i:(i+ss-1),1)=(m1.*ac1.^2+I1+m2.*(a1.^2+ac2.^2+2.*a1.*ac2.*cos(q2(i:(i+ss-1),1)))+I2).*q1dd(i:(i+ss-1),1)+...
(m2.*(ac2.^2+a1.*ac2.*cos(q2(i:(i+ss-1),1)))+I2).*q2dd(i:(i+ss-1),1)+...
-(m2.*a1.*ac2.*sin(q2(i:(i+ss-1),1)).*(2.*q1d(i:(i+ss-1),1).*q2d(i:(i+ss-1),1)+q2d(i:(i+ss-1),1).^2))+...
m1.*g.*ac1.*cos(q1(i:(i+ss-1),1))+m2.*g.*(a1.*cos(q1(i:(i+ss-1),1))+ac2.*cos(q1(i:(i+ss-1),1)+q2(i:(i+ss-1),1)));
tau2(i:(i+ss-1),1)=(m2.*(ac2.^2+a1.*ac2.*cos(q2(i:(i+ss-1),1)))+I2).*q1dd(i:(i+ss-1),1)+...
(m2.*ac2.^2+I2).*q2dd(i:(i+ss-1),1)+...
(m2.*a1.*ac2.*sin(q2(i:(i+ss-1),1)).*q1d(i:(i+ss-1),1).^2)+...
(m2.*g.*ac2.*cos(q1(i:(i+ss-1),1)+q2(i:(i+ss-1),1)));
end
I am stuck at the point at creating the second arm link angles. Could you help me with that?
Thanks in advance

Answers (0)

Products


Release

R2017a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!