Create your Gitee Account
Explore and code with more than 6 million developers,Free private repositories !:)
Sign up
Clone or download
parser.cpp 7.06 KB
Copy Edit Web IDE Raw Blame History
wangcongrobot authored 2018-01-10 21:16 . move .h to include.
#include "parser.h"
using namespace std;
Parser::Parser()
{
// DH Parameters of 7-F manipulator
double a1 = 0.0925;
double a2 = 0.950;
double a3 = 0.127;
double d4 = 0.522;
double d6 = 0.450;
// DH is DH model of the manipulator. Rows are DOF, cols are alpha/a/theta/d.
// The angle uses radian, the distance uses mm.
Eigen::MatrixXd dh(6, 4);
// alpha a theta d
dh << Pi / 2, a1, 0, 0,
0, a2, 0, 0,
Pi / 2, a3, 0, 0,
-Pi / 2, 0, 0, d4,
Pi / 2, 0, 0, 0,
0, 0, 0, d6;
this->dh = dh;
}
/*
DH
q1 Pi / 2, 0, 0, d1,
q2 Pi, d2, 0, 0,
q3 Pi / 2, 0, 0, -e2,
q4 2 * aa, 0, 0, -d4b,
q5 2 * aa, 0, 0, -d5b,
q6 Pi, 0, 0, -d6b;
*/
Eigen::MatrixXd Parser::Foward(Eigen::VectorXd q)
{
Eigen::VectorXd dhQ = JointAngle2DhAngle(q);
return DhFoward(dh, dhQ);
}
Eigen::MatrixXd Parser::Jacobn(Eigen::VectorXd q)
{
Eigen::VectorXd dhQ = JointAngle2DhAngle(q);
return DhJacobn(dh, dhQ);
}
Eigen::MatrixXd Parser::Jacob0(Eigen::VectorXd q)
{
Eigen::VectorXd dhQ = JointAngle2DhAngle(q);
return DhJacob0(dh, dhQ);
}
Eigen::VectorXd Parser::Inverse(Eigen::MatrixXd t, Eigen::VectorXd qR)
{
Eigen::VectorXd dhQR = JointAngle2DhAngle(qR);
return DhInverse(dh, t, dhQR);
}
// Transformation from DH algorithm to Jaco physical angles, p4
Eigen::VectorXd Parser::JointAngle2DhAngle(Eigen::VectorXd q)
{
Eigen::VectorXd dhQ(6);
dhQ << q(0), q(1), q(2), q(3), q(4), q(5);
return dhQ;
}
// Transformation from Jaco physical angles to DH algorithm, p4
Eigen::VectorXd Parser::DhAngle2JointAngle(Eigen::VectorXd dhQ)
{
Eigen::VectorXd q(6);
q << dhQ(0), dhQ(1), dhQ(2), dhQ(3), dhQ(4), dhQ(5);
return q;
}
// ÑØx¡¢y¡¢zÈýžö·œÏòµÄÆœÒÆÁ¿£¬¹¹³ÉÆœÒÆÏòÁ¿
// Ïò4X4ÆëŽÎŸØÕóÌî³äÆœÒÆÏòÁ¿
// ÆëŽÎ±ä»»ŸØÕóµÚËÄÁÐŒŽÆœÒÆÏòÁ¿
Eigen::Matrix4d Parser::Translation(Eigen::Vector3d p)
{
Eigen::Matrix4d t = Eigen::Matrix4d::Identity(4, 4); //4x4µ¥Î»ŸØÕó
t(0, 3) = p(0);
t(1, 3) = p(1);
t(2, 3) = p(2);
return t;
}
// ÑØx¡¢y¡¢zÈýžöÖáÐýתµÄÐýתŸØÕó
// Ïò4x4ÆëŽÎŸØÕóÌî³äÐýתŸØÕó
// ÆëŽÎ±ä»»ŸØÕó×óÉÏ·œ3x3ŸØÕóŒŽÐýתŸØÕó
Eigen::Matrix4d Parser::Rotation(char axis, double r) // ÐýתÖáaxis£¬ÐýתœÇr
{
Eigen::Matrix4d t = Eigen::Matrix4d::Identity(4, 4); //4x4µ¥Î»ŸØÕó
double ct = cos(r);
double st = sin(r);
switch (axis)
{
case 'x':
t << 1, 0, 0, 0,
0, ct, -st, 0,
0, st, ct, 0,
0, 0, 0, 1;
break;
case 'y':
t << ct, 0, st, 0,
0, 1, 0, 0,
-st, 0, ct, 0,
0, 0, 0, 1;
break;
case 'z':
t << ct, -st, 0, 0,
st, ct, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1;
break;
}
return t;
}
// ±ä»»ŸØÕóT΢·ÖÇóµŒ
Eigen::VectorXd Parser::DeltaT(Eigen::MatrixXd t1, Eigen::MatrixXd t2)
{
Eigen::VectorXd diff(6);
Eigen::VectorXd cross = Eigen::VectorXd::Zero(3);
for (int i = 0; i < 3; i++)
{
Eigen::Vector3d x1, x2;
x1 << t1(0, i), t1(1, i), t1(2, i);
x2 << t2(0, i), t2(1, i), t2(2, i);
cross += x1.cross(x2); // x1Óëx2œøÐвæ³ËŒÆËã
}
for (int i = 0; i < 3; i++)
{
diff(i) = t2(i, 3) - t1(i, 3);
diff(i + 3) = 0.5*cross(i);
}
return diff;
}
// žùŸÝDH²ÎÊýÒÔŒ°¹ØœÚ±äÁ¿ÇóÔ˶¯Ñ§Õýœâ
/* alpha a theta d
dh << Pi / 2, a1, 0, 0,
0, a1, 0, 0,
Pi / 2, a1, 0, 0,
-Pi / 2, 0, 0, d4,
Pi / 2, 0, 0, 0,
0, 0, 0, d6;
*/
Eigen::MatrixXd Parser::DhFoward(Eigen::MatrixXd dh, Eigen::VectorXd dhQ)
{
Eigen::Matrix4d t60 = Eigen::Matrix4d::Identity(4, 4); // 4x4µ¥Î»ŸØÕó
for (int i = 0; i < dh.rows(); i++)
{
Eigen::Vector3d px,pz;
px << dh(i, 1), 0, 0;
pz << 0, 0, dh(i, 3);
dh(i, 2) += dhQ(i);
// cout << dh << endl;
t60 *= Rotation('z', dh(i, 2)) * Translation(pz) * Translation(px) * Rotation('x', dh(i, 0));
// cout << "t" << i+1 << t60 << endl;
}
return t60; // Ä©¶ËT6Ïà¶ÔÓÚ»ù×ø±êϵT0µÄ±ä»»ŸØÕó
}
// Çó6x6Ñſ˱È
Eigen::MatrixXd Parser::DhJacobn(Eigen::MatrixXd dh, Eigen::VectorXd dhQ)
{
Eigen::MatrixXd jn = Eigen::MatrixXd::Identity(6, 6); //6x6µ¥Î»ŸØÕó
Eigen::Matrix4d t = Eigen::Matrix4d::Identity(4, 4);
for (int i = dh.rows() - 1; i >= 0; i--)
{
Eigen::Vector3d pp;
pp << dh(i, 1), 0, dh(i, 3);
dh(i, 2) += dhQ(i);
t = Rotation('z', dh(i, 2)) * Translation(pp) * Rotation('x', dh(i, 0)) * t;
Eigen::Vector3d a, o, n, p;
a << t(0, 0), t(1, 0), t(2, 0);
o << t(0, 1), t(1, 1), t(2, 1);
n << t(0, 2), t(1, 2), t(2, 2);
p << t(0, 3), t(1, 3), t(2, 3);
jn(0, i) = -a(0) * p(1) + a(1) * p(0);// ÆœÒÆ
jn(1, i) = -o(0) * p(1) + o(1) * p(0);
jn(2, i) = -n(0) * p(1) + n(1) * p(0);
jn(3, i) = a(2);// Ðýת
jn(4, i) = o(2);
jn(5, i) = n(2);
}
return jn;
}
Eigen::MatrixXd Parser::DhJacob0(Eigen::MatrixXd dh, Eigen::VectorXd dhQ)
{
Eigen::MatrixXd j0 = DhJacobn(dh, dhQ);
Eigen::MatrixXd t0 = DhFoward(dh, dhQ);
Eigen::MatrixXd t(6, 6);
t << t0(0, 0), t0(0, 1), t0(0, 2), 0, 0, 0,
t0(1, 0), t0(1, 1), t0(1, 2), 0, 0, 0,
t0(2, 0), t0(2, 1), t0(2, 2), 0, 0, 0,
0, 0, 0, t0(0, 0), t0(0, 1), t0(0, 2),
0, 0, 0, t0(1, 0), t0(1, 1), t0(1, 2),
0, 0, 0, t0(2, 0), t0(2, 1), t0(2, 2);
j0 = t * j0;
return j0;
}
Eigen::VectorXd Parser::DhInverse(Eigen::MatrixXd dh, Eigen::MatrixXd t, Eigen::VectorXd dhQR)
{
Eigen::VectorXd qR = DhAngle2JointAngle(dhQR);
Eigen::VectorXd q(6);
Eigen::VectorXd dhQ = dhQR;
Eigen::VectorXd deltaQ;
double norm = 1.0;
double min = 1e-12; // Stop condition
int count = 0;
int maxCount = 1000; // Max number of iterations
while (norm > min)
{
Eigen::VectorXd e = DeltaT(DhFoward(dh, dhQ), t);
Eigen::MatrixXd j0 = DhJacob0(dh, dhQ);
// Only for latest version of Eigen
// Eigen::VectorXd dDhQ = (j0.pinv()) *e;
Eigen::VectorXd dDhQ = j0.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(e);
dhQ += dDhQ;
for (int i = 0; i < dhQ.rows(); i++)
dhQ(i) -= ((int)(dhQ(i) / (2 * Pi))) * 2 * Pi;
norm = dDhQ.norm();
count++;
if (count > maxCount)
{
q << 10 * Pi, 0, 0, 0, 0, 0;
return q;
}
}
q = DhAngle2JointAngle(dhQ);
deltaQ = q - qR;
for (int i = 0; i < q.rows(); i++)
{
deltaQ(i) -= ((int)(deltaQ(i) / (2 * Pi))) * 2 * Pi;
if (deltaQ(i) < -Pi)
deltaQ(i) += 2 * Pi;
else if
(deltaQ(i) > Pi) deltaQ(i) -= 2 * Pi;
}
return qR + deltaQ;
}

Comment ( 0 )

Sign in for post a comment