SLAM--Geometric jacobian of UR series.
松灿
The geometric jacobian shows relationship between velocities of joints and velocities in TCP coordinate frame.
v = J q ˙ \bm v=\bm J \bm {\dot q} v=Jq˙
Obtain it by using Matlab
%% For UR robot arm
clc;clear all;
syms d1 a2 a3 d4 d5 d6
syms q1 q2 q3 q4 q5 q6
c1 = cos(q1);
s1 = sin(q1);
c2 = cos(q2);
s2 = sin(q2);
c3 = cos(q3);
s3 = sin(q3);
c4 = cos(q4);
s4 = sin(q4);
c5 = cos(q5);
s5 = sin(q5);
c6 = cos(q6);
s6= sin(q6);
H01 = [c1 0 s1 0;
s1 0 -c1 0;
0 1 0 d1;
0 0 0 1;];
H12 = [c2 -s2 0 a2*c2;
s2 c2 0 a2*s2;
0 0 1 0;
0 0 0 1;];
H23 = [c3 -s3 0 a3*c3;
s3 c3 0 a3*s3;
0 0 1 0;
0 0 0 1;];
H34 = [c4 0 s4 0;
s4 0 -c4 0;
0 1 0 d4;
0 0 0 1;];
H45 = [c5 0 -s5 0;
s5 0 c5 0;
0 -1 0 d5;
0 0 0 1;];
H56 = [c6 -s6 0 0;
s6 c6 0 0;
0 0 1 d6;
0 0 0 1;];
Hstack = {H01,H12,H23,H34,H45,H56};
H = H01*H12*H23*H34*H45*H56;
%H = simplify(H);
% Jpi = z_{i-1}×(pe-p_{i-1})
% z indicates the direction of each joint's Z axis.
% p indicates the position of each joint.
% the jacobian is [Jpi,Joi]' = [z_{i-1}×(pe-p_{i-1}), z_{i-1}]'
Z0 = [0; 0; 1;];
P0e = H(1:3,4);
Ht = eye(4,4);
J = [];
Z = Z0;
Ji = [cross(Z,P0e);Z];
J = [J, Ji];
for i=1:5
Ht = Ht*Hstack{i};
Z = Ht(1:3,3);
Pie = P0e - Ht(1:3,4);
Ji = [cross(Z,Pie);Z];
J = [J, Ji];
end
J = simplify(J);
J = collect(J,d6);
The result is
>> J
J =
[ (cos(q1)*cos(q5) + cos(q2 + q3 + q4)*sin(q1)*sin(q5))*d6 + d4*cos(q1) - a2*cos(q2)*sin(q1) - d5*sin(q2 + q3 + q4)*sin(q1) - a3*cos(q2)*cos(q3)*sin(q1) + a3*sin(q1)*sin(q2)*sin(q3), cos(q1)*sin(q5)*(cos(q2 + q3)*sin(q4) + sin(q2 + q3)*cos(q4))*d6 - cos(q1)*(a3*sin(q2 + q3) + a2*sin(q2) - d5*(cos(q2 + q3)*cos(q4) - sin(q2 + q3)*sin(q4))), sin(q2 + q3 + q4)*cos(q1)*sin(q5)*d6 - cos(q1)*(a3*sin(q2 + q3) - d5*cos(q2 + q3 + q4)), sin(q2 + q3 + q4)*cos(q1)*sin(q5)*d6 + d5*cos(q2 + q3 + q4)*cos(q1), (cos(q1)*cos(q2)*cos(q5)*sin(q3)*sin(q4) - cos(q1)*cos(q2)*cos(q3)*cos(q4)*cos(q5) - sin(q1)*sin(q5) + cos(q1)*cos(q3)*cos(q5)*sin(q2)*sin(q4) + cos(q1)*cos(q4)*cos(q5)*sin(q2)*sin(q3))*d6, 0]
[ (cos(q5)*sin(q1) - cos(q2 + q3 + q4)*cos(q1)*sin(q5))*d6 + d4*sin(q1) + a2*cos(q1)*cos(q2) + d5*sin(q2 + q3 + q4)*cos(q1) + a3*cos(q1)*cos(q2)*cos(q3) - a3*cos(q1)*sin(q2)*sin(q3), sin(q1)*sin(q5)*(cos(q2 + q3)*sin(q4) + sin(q2 + q3)*cos(q4))*d6 - sin(q1)*(a3*sin(q2 + q3) + a2*sin(q2) - d5*(cos(q2 + q3)*cos(q4) - sin(q2 + q3)*sin(q4))), sin(q2 + q3 + q4)*sin(q1)*sin(q5)*d6 - sin(q1)*(a3*sin(q2 + q3) - d5*cos(q2 + q3 + q4)), sin(q2 + q3 + q4)*sin(q1)*sin(q5)*d6 + d5*cos(q2 + q3 + q4)*sin(q1), (cos(q1)*sin(q5) - cos(q2)*cos(q3)*cos(q4)*cos(q5)*sin(q1) + cos(q2)*cos(q5)*sin(q1)*sin(q3)*sin(q4) + cos(q3)*cos(q5)*sin(q1)*sin(q2)*sin(q4) + cos(q4)*cos(q5)*sin(q1)*sin(q2)*sin(q3))*d6, 0]
[ 0, (sin(q2 + q3 + q4 - q5)/2 - sin(q2 + q3 + q4 + q5)/2)*d6 + a3*cos(q2 + q3) + a2*cos(q2) + d5*sin(q2 + q3 + q4), (sin(q2 + q3 + q4 - q5)/2 - sin(q2 + q3 + q4 + q5)/2)*d6 + a3*cos(q2 + q3) + d5*sin(q2 + q3 + q4), (sin(q2 + q3 + q4 - q5)/2 - sin(q2 + q3 + q4 + q5)/2)*d6 + d5*sin(q2 + q3 + q4), (- sin(q2 + q3 + q4 + q5)/2 - sin(q2 + q3 + q4 - q5)/2)*d6, 0]
[ 0, sin(q1), sin(q1), sin(q1), sin(q2 + q3 + q4)*cos(q1), cos(q5)*sin(q1) - cos(q2 + q3 + q4)*cos(q1)*sin(q5)]
[ 0, -cos(q1), -cos(q1), -cos(q1), sin(q2 + q3 + q4)*sin(q1), - cos(q1)*cos(q5) - cos(q2 + q3 + q4)*sin(q1)*sin(q5)]
[ 1, 0, 0, 0, -cos(q2 + q3 + q4), -sin(q2 + q3 + q4)*sin(q5)]
Calculate the Jacobian directly by using C++
Now we have got the expressions of jacobian. Then, use c++ to design a function that can calculate the jacobian directly so that the computation could be reduced greatly making the speed of processing faster.
void jacobian(const double* q, double* J){
double s1 = sin(*q), c1 = cos(*q); q++;
double q23 = *q, q234 = *q, s2 = sin(*q), c2 = cos(*q); q++;
double s3 = sin(*q), c3 = cos(*q); q23 += *q; q234 += *q; q++;
double s4 = sin(*q), c4 = cos(*q); q234 += *q; q++;
double s5 = sin(*q), c5 = cos(*q), q2345 = q234 + *q,q234_5 = q234 - *q; q++;
double s6 = sin(*q), c6 = cos(*q);
double s23 = sin(q23), c23 = cos(q23);
double s234 = sin(q234), c234 = cos(q234);
double s2345 = sin(q2345), s234_5 = sin(q234_5);
*J = (c1*c5 + c234*s1*s5)*d6 + d4*c1 - a2*c2*s1 - d5*s234*s1 - a3*c2*c3*s1 + a3*s1*s2*s3;J++;
*J = c1*s5*(c23*s4 + s23*c4)*d6 - c1*(a3*s23 + a2*s2 - d5*(c23*c4 - s23*s4)); J++;
*J = s234*c1*s5*d6 - c1*(a3*s23 - d5*c234); J++;
*J = s234*c1*s5*d6 + d5*c234*c1; J++;
*J = (c1*c2*c5*s3*s4 - c1*c2*c3*c4*c5 - s1*s5 + c1*c3*c5*s2*s4 + c1*c4*c5*s2*s3)*d6; J++;
*J = 0; J++;
*J = (c5*s1 - c234*c1*s5)*d6 + d4*s1 + a2*c1*c2 + d5*s234*c1 + a3*c1*c2*c3 - a3*c1*s2*s3;J++;
*J = s1*s5*(c23*s4 + s23*c4)*d6 - s1*(a3*s23 + a2*s2 - d5*(c23*c4 - s23*s4)); J++;
*J = s234*s1*s5*d6 - s1*(a3*s23 - d5*c234); J++;
*J = s234*s1*s5*d6 + d5*c234*s1; J++;
*J = (c1*s5 - c2*c3*c4*c5*s1 + c2*c5*s1*s3*s4 + c3*c5*s1*s2*s4 + c4*c5*s1*s2*s3)*d6; J++;
*J = 0; J++;
*J = 0;J++;
*J = (s234_5/2 - s2345/2)*d6 + a3*c23 + a2*c2 + d5*s234; J++;
*J = (s234_5/2 - s2345/2)*d6 + a3*c23 + d5*s234; J++;
*J = (s234_5/2 - s2345/2)*d6 + d5*s234; J++;
*J = (- s2345/2 - s234_5/2)*d6; J++;
*J = 0; J++;
*J = 0; J++;
*J = s1; J++;
*J = s1; J++;
*J = s1; J++;
*J = s234 * c1; J++;
*J = c5*s1 - c234*c1*s5; J++;
*J = 0; J++;
*J = -c1; J++;
*J = -c1; J++;
*J = -c1; J++;
*J = s234 * s1; J++;
*J = - c1*c5 - c234*s1*s5; J++;
*J = 1; J++;
*J = 0; J++;
*J = 0; J++;
*J = 0; J++;
*J = -c234; J++;
*J = -s234 * s5; J++;
}
q q q represents the robot joints’ states, and J J J is the geometric jacobian matrix.
Example of utilizing:
#define ROBOT_DOF 6
...
//get joint states from ros topic
...
double Jr[ROBOT_DOF*ROBOT_DOF] = {0};
//calculate the jacobian in real-time.
jacobian(q,Jr);
//print the elements of jacobian.
for(int i=0;i<ROBOT_DOF;i++)
printf("%1.6f %1.6f %1.6f %1.6f %1.6f %1.6f \n",Jr[i*6+0],Jr[i*6+1],Jr[i*6+2],Jr[i*6+3],Jr[i*6+4],Jr[i*6+5]);
类似资料：