341 lines
11 KiB
C++
341 lines
11 KiB
C++
|
/**
|
|||
|
|
******************************************************************************
|
|||
|
|
* @file robotics.cpp/h
|
|||
|
|
* @brief Robotic toolbox on STM32. STM32机器人学库
|
|||
|
|
* @author Spoon Guan
|
|||
|
|
* @ref [1] SJTU ME385-2, Robotics, Y.Ding
|
|||
|
|
* [2] Bruno Siciliano, et al., Robotics: Modelling, Planning and
|
|||
|
|
* Control, Springer, 2010.
|
|||
|
|
* [3] R.Murry, Z.X.Li, and S.Sastry, A Mathematical Introduction
|
|||
|
|
* to Robotic Manipulation, CRC Press, 1994.
|
|||
|
|
******************************************************************************
|
|||
|
|
* Copyright (c) 2023 Team JiaoLong-SJTU
|
|||
|
|
* All rights reserved.
|
|||
|
|
******************************************************************************
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
#include "robotics.h"
|
|||
|
|
|
|||
|
|
Matrixf<3, 1> robotics::r2rpy(Matrixf<3, 3> R) {
|
|||
|
|
float rpy[3] = {
|
|||
|
|
atan2f(R[1][0], R[0][0]), // yaw
|
|||
|
|
atan2f(-R[2][0], sqrtf(R[2][1] * R[2][1] + R[2][2] * R[2][2])), // pitch
|
|||
|
|
atan2f(R[2][1], R[2][2]) // roll
|
|||
|
|
};
|
|||
|
|
return Matrixf<3, 1>(rpy);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<3, 3> robotics::rpy2r(Matrixf<3, 1> rpy) {
|
|||
|
|
float c[3] = {cosf(rpy[0][0]), cosf(rpy[1][0]), cosf(rpy[2][0])};
|
|||
|
|
float s[3] = {sinf(rpy[0][0]), sinf(rpy[1][0]), sinf(rpy[2][0])};
|
|||
|
|
float R[9] = {
|
|||
|
|
c[0] * c[1], // R11
|
|||
|
|
c[0] * s[1] * s[2] - s[0] * c[2], // R12
|
|||
|
|
c[0] * s[1] * c[2] + s[0] * s[2], // R13
|
|||
|
|
s[0] * c[1], // R21
|
|||
|
|
s[0] * s[1] * s[2] + c[0] * c[2], // R22
|
|||
|
|
s[0] * s[1] * c[2] - c[0] * s[2], // R23
|
|||
|
|
-s[1], // R31
|
|||
|
|
c[1] * s[2], // R32
|
|||
|
|
c[1] * c[2] // R33
|
|||
|
|
};
|
|||
|
|
return Matrixf<3, 3>(R);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<4, 1> robotics::r2angvec(Matrixf<3, 3> R) {
|
|||
|
|
float theta = acosf(math::limit(0.5f * (R.trace() - 1), -1, 1));
|
|||
|
|
if (theta == 0 || theta == PI) {
|
|||
|
|
float angvec[4] = {
|
|||
|
|
(1 + R[0][0] - R[1][1] - R[2][2]) / 4.0f, // rx=(1+R11-R22-R33)/4
|
|||
|
|
(1 - R[0][0] + R[1][1] - R[2][2]) / 4.0f, // ry=(1-R11+R22-R33)/4
|
|||
|
|
(1 - R[0][0] - R[1][1] + R[2][2]) / 4.0f, // rz=(1-R11-R22+R33)/4
|
|||
|
|
theta // theta
|
|||
|
|
};
|
|||
|
|
return Matrixf<4, 1>(angvec);
|
|||
|
|
}
|
|||
|
|
float angvec[4] = {
|
|||
|
|
(R[2][1] - R[1][2]) / (2.0f * sinf(theta)), // rx=(R32-R23)/2sinθ
|
|||
|
|
(R[0][2] - R[2][0]) / (2.0f * sinf(theta)), // ry=(R13-R31)/2sinθ
|
|||
|
|
(R[1][0] - R[0][1]) / (2.0f * sinf(theta)), // rz=(R21-R12)/2sinθ
|
|||
|
|
theta // theta
|
|||
|
|
};
|
|||
|
|
return Matrixf<4, 1>(angvec);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<3, 3> robotics::angvec2r(Matrixf<4, 1> angvec) {
|
|||
|
|
float theta = angvec[3][0];
|
|||
|
|
Matrixf<3, 1> r = angvec.block<3, 1>(0, 0);
|
|||
|
|
Matrixf<3, 3> R;
|
|||
|
|
// Rodrigues formula: R=I+ω^sinθ+ω^^2(1-cosθ)
|
|||
|
|
R = matrixf::eye<3, 3>() + vector3f::hat(r) * sinf(theta) +
|
|||
|
|
vector3f::hat(r) * vector3f::hat(r) * (1 - cosf(theta));
|
|||
|
|
return R;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<4, 1> robotics::r2quat(Matrixf<3, 3> R) {
|
|||
|
|
float q[4] = {
|
|||
|
|
0.5f * sqrtf(math::limit(R.trace(), -1, 1) + 1), // q0=cos(θ/2)
|
|||
|
|
math::sign(R[2][1] - R[1][2]) * 0.5f *
|
|||
|
|
sqrtf(math::limit(R[0][0] - R[1][1] - R[2][2], -1, 1) +
|
|||
|
|
1), // q1=rx*sin(θ/2)
|
|||
|
|
math::sign(R[0][2] - R[2][0]) * 0.5f *
|
|||
|
|
sqrtf(math::limit(-R[0][0] + R[1][1] - R[2][2], -1, 1) +
|
|||
|
|
1), // q2=ry*sin(θ/2)
|
|||
|
|
math::sign(R[1][0] - R[0][1]) * 0.5f *
|
|||
|
|
sqrtf(math::limit(-R[0][0] - R[1][1] + R[2][2], -1, 1) +
|
|||
|
|
1), // q3=rz*sin(θ/2)
|
|||
|
|
};
|
|||
|
|
return (Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm());
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<3, 3> robotics::quat2r(Matrixf<4, 1> q) {
|
|||
|
|
float R[9] = {
|
|||
|
|
1 - 2.0f * (q[2][0] * q[2][0] + q[3][0] * q[3][0]), // R11
|
|||
|
|
2.0f * (q[1][0] * q[2][0] - q[0][0] * q[3][0]), // R12
|
|||
|
|
2.0f * (q[1][0] * q[3][0] + q[0][0] * q[2][0]), // R13
|
|||
|
|
2.0f * (q[1][0] * q[2][0] + q[0][0] * q[3][0]), // R21
|
|||
|
|
1 - 2.0f * (q[1][0] * q[1][0] + q[3][0] * q[3][0]), // R22
|
|||
|
|
2.0f * (q[2][0] * q[3][0] - q[0][0] * q[1][0]), // R23
|
|||
|
|
2.0f * (q[1][0] * q[3][0] - q[0][0] * q[2][0]), // R31
|
|||
|
|
2.0f * (q[2][0] * q[3][0] + q[0][0] * q[1][0]), // R32
|
|||
|
|
1 - 2.0f * (q[1][0] * q[1][0] + q[2][0] * q[2][0]) // R33
|
|||
|
|
};
|
|||
|
|
return Matrixf<3, 3>(R);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<3, 1> robotics::quat2rpy(Matrixf<4, 1> q) {
|
|||
|
|
float rpy[3] = {
|
|||
|
|
atan2f(2.0f * (q[1][0] * q[2][0] + q[0][0] * q[3][0]),
|
|||
|
|
1 - 2.0f * (q[2][0] * q[2][0] + q[3][0] * q[3][0])), // yaw
|
|||
|
|
asinf(2.0f * (q[0][0] * q[2][0] - q[1][0] * q[3][0])), // pitch
|
|||
|
|
atan2f(2.0f * (q[2][0] * q[3][0] + q[0][0] * q[1][0]),
|
|||
|
|
1 - 2.0f * (q[1][0] * q[1][0] + q[2][0] * q[2][0])) // rol
|
|||
|
|
};
|
|||
|
|
return Matrixf<3, 1>(rpy);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<4, 1> robotics::rpy2quat(Matrixf<3, 1> rpy) {
|
|||
|
|
float c[3] = {cosf(0.5f * rpy[0][0]), cosf(0.5f * rpy[1][0]),
|
|||
|
|
cosf(0.5f * rpy[2][0])}; // cos(*/2)
|
|||
|
|
float s[3] = {sinf(0.5f * rpy[0][0]), sinf(0.5f * rpy[1][0]),
|
|||
|
|
sinf(0.5f * rpy[2][0])}; // sin(*/2)
|
|||
|
|
float q[4] = {
|
|||
|
|
c[0] * c[1] * c[2] + s[0] * s[1] * s[2], // q0=cos(θ/2)
|
|||
|
|
c[0] * c[1] * s[2] - s[0] * s[1] * c[2], // q1=rx*sin(θ/2)
|
|||
|
|
c[0] * s[1] * c[2] + s[0] * c[1] * s[2], // q2=ry*sin(θ/2)
|
|||
|
|
s[0] * c[1] * c[2] - c[0] * s[1] * s[2] // q3=rz*sin(θ/2)
|
|||
|
|
};
|
|||
|
|
return (Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm());
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<4, 1> robotics::quat2angvec(Matrixf<4, 1> q) {
|
|||
|
|
float cosq0;
|
|||
|
|
float theta = 2.0f * acosf(math::limit(q[0][0], -1, 1));
|
|||
|
|
if (theta == 0 || theta == PI) {
|
|||
|
|
float angvec[4] = {0, 0, 0, theta}; // θ=0||PI, return[0;θ]
|
|||
|
|
return Matrixf<4, 1>(angvec);
|
|||
|
|
}
|
|||
|
|
Matrixf<3, 1> vec = q.block<3, 1>(1, 0);
|
|||
|
|
float angvec[4] = {
|
|||
|
|
vec[0][0] / vec.norm(), // rx
|
|||
|
|
vec[1][0] / vec.norm(), // ry
|
|||
|
|
vec[2][0] / vec.norm(), // rz
|
|||
|
|
theta // theta
|
|||
|
|
};
|
|||
|
|
return Matrixf<4, 1>(angvec);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<4, 1> robotics::angvec2quat(Matrixf<4, 1> angvec) {
|
|||
|
|
float c = cosf(0.5f * angvec[3][0]), s = sinf(0.5f * angvec[3][0]);
|
|||
|
|
float q[4] = {
|
|||
|
|
c, // q0=cos(θ/2)
|
|||
|
|
s * angvec[0][0], // q1=rx*sin(θ/2)
|
|||
|
|
s * angvec[1][0], // q2=ry*sin(θ/2)
|
|||
|
|
s * angvec[2][0] // q3=rz*sin(θ/2)
|
|||
|
|
};
|
|||
|
|
return Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<3, 3> robotics::t2r(Matrixf<4, 4> T) {
|
|||
|
|
return T.block<3, 3>(0, 0); // R=T(1:3,1:3)
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<4, 4> robotics::r2t(Matrixf<3, 3> R) {
|
|||
|
|
// T=[R,0;0,1]
|
|||
|
|
float T[16] = {R[0][0], R[0][1], R[0][2], 0, R[1][0], R[1][1], R[1][2], 0,
|
|||
|
|
R[2][0], R[2][1], R[2][2], 0, 0, 0, 0, 1};
|
|||
|
|
return Matrixf<4, 4>(T);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<3, 1> robotics::t2p(Matrixf<4, 4> T) {
|
|||
|
|
return T.block<3, 1>(0, 3); // p=T(1:3,4)
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<4, 4> robotics::p2t(Matrixf<3, 1> p) {
|
|||
|
|
// T=[I,P;0,1]
|
|||
|
|
float T[16] = {1, 0, 0, p[0][0], 0, 1, 0, p[1][0],
|
|||
|
|
0, 0, 1, p[2][0], 0, 0, 0, 1};
|
|||
|
|
return Matrixf<4, 4>(T);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<4, 4> robotics::rp2t(Matrixf<3, 3> R, Matrixf<3, 1> p) {
|
|||
|
|
// T=[R,P;0,1]
|
|||
|
|
float T[16] = {R[0][0], R[0][1], R[0][2], p[0][0], R[1][0], R[1][1],
|
|||
|
|
R[1][2], p[1][0], R[2][0], R[2][1], R[2][2], p[2][0],
|
|||
|
|
0, 0, 0, 1};
|
|||
|
|
return Matrixf<4, 4>(T);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<4, 4> robotics::invT(Matrixf<4, 4> T) {
|
|||
|
|
Matrixf<3, 3> RT = t2r(T).trans();
|
|||
|
|
Matrixf<3, 1> p_ = -1.0f * RT * t2p(T);
|
|||
|
|
float invT[16] = {RT[0][0], RT[0][1], RT[0][2], p_[0][0], RT[1][0], RT[1][1],
|
|||
|
|
RT[1][2], p_[1][0], RT[2][0], RT[2][1], RT[2][2], p_[2][0],
|
|||
|
|
0, 0, 0, 1};
|
|||
|
|
return Matrixf<4, 4>(invT);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<3, 1> robotics::t2rpy(Matrixf<4, 4> T) {
|
|||
|
|
return r2rpy(t2r(T));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<4, 4> robotics::rpy2t(Matrixf<3, 1> rpy) {
|
|||
|
|
return r2t(rpy2r(rpy));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<4, 1> robotics::t2angvec(Matrixf<4, 4> T) {
|
|||
|
|
return r2angvec(t2r(T));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<4, 4> robotics::angvec2t(Matrixf<4, 1> angvec) {
|
|||
|
|
return r2t(angvec2r(angvec));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<4, 1> robotics::t2quat(Matrixf<4, 4> T) {
|
|||
|
|
return r2quat(t2r(T));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<4, 4> robotics::quat2t(Matrixf<4, 1> quat) {
|
|||
|
|
return r2t(quat2r(quat));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<6, 1> robotics::t2twist(Matrixf<4, 4> T) {
|
|||
|
|
Matrixf<3, 1> p = t2p(T);
|
|||
|
|
Matrixf<4, 1> angvec = t2angvec(T);
|
|||
|
|
Matrixf<3, 1> phi = angvec.block<3, 1>(0, 0) * angvec[3][0];
|
|||
|
|
float twist[6] = {p[0][0], p[1][0], p[2][0], phi[0][0], phi[1][0], phi[2][0]};
|
|||
|
|
return Matrixf<6, 1>(twist);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<4, 4> robotics::twist2t(Matrixf<6, 1> twist) {
|
|||
|
|
Matrixf<3, 1> p = twist.block<3, 1>(0, 0);
|
|||
|
|
float theta = twist.block<3, 1>(3, 0).norm();
|
|||
|
|
float angvec[4] = {0, 0, 0, theta};
|
|||
|
|
if (theta != 0) {
|
|||
|
|
angvec[0] = twist[3][0] / theta;
|
|||
|
|
angvec[1] = twist[4][0] / theta;
|
|||
|
|
angvec[2] = twist[5][0] / theta;
|
|||
|
|
}
|
|||
|
|
return rp2t(angvec2r(angvec), p);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<4, 4> robotics::DH_t::fkine() {
|
|||
|
|
float ct = cosf(theta), st = sinf(theta); // cosθ, sinθ
|
|||
|
|
float ca = cosf(alpha), sa = sinf(alpha); // cosα, sinα
|
|||
|
|
|
|||
|
|
// T =
|
|||
|
|
// | cθ -sθcα sθsα acθ |
|
|||
|
|
// | sθ cθcα -cθsα asθ |
|
|||
|
|
// | 0 sα cα d |
|
|||
|
|
// | 0 0 0 1 |
|
|||
|
|
T[0][0] = ct;
|
|||
|
|
T[0][1] = -st * ca;
|
|||
|
|
T[0][2] = st * sa;
|
|||
|
|
T[0][3] = a * ct;
|
|||
|
|
|
|||
|
|
T[1][0] = st;
|
|||
|
|
T[1][1] = ct * ca;
|
|||
|
|
T[1][2] = -ct * sa;
|
|||
|
|
T[1][3] = a * st;
|
|||
|
|
|
|||
|
|
T[2][0] = 0;
|
|||
|
|
T[2][1] = sa;
|
|||
|
|
T[2][2] = ca;
|
|||
|
|
T[2][3] = d;
|
|||
|
|
|
|||
|
|
T[3][0] = 0;
|
|||
|
|
T[3][1] = 0;
|
|||
|
|
T[3][2] = 0;
|
|||
|
|
T[3][3] = 1;
|
|||
|
|
|
|||
|
|
return T;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
robotics::Link::Link(float theta,
|
|||
|
|
float d,
|
|||
|
|
float a,
|
|||
|
|
float alpha,
|
|||
|
|
robotics::Joint_Type_e type,
|
|||
|
|
float offset,
|
|||
|
|
float qmin,
|
|||
|
|
float qmax,
|
|||
|
|
float m,
|
|||
|
|
Matrixf<3, 1> rc,
|
|||
|
|
Matrixf<3, 3> I) {
|
|||
|
|
dh_.theta = theta;
|
|||
|
|
dh_.d = d;
|
|||
|
|
dh_.alpha = alpha;
|
|||
|
|
dh_.a = a;
|
|||
|
|
type_ = type;
|
|||
|
|
offset_ = offset;
|
|||
|
|
qmin_ = qmin;
|
|||
|
|
qmax_ = qmax;
|
|||
|
|
m_ = m;
|
|||
|
|
rc_ = rc;
|
|||
|
|
I_ = I;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
robotics::Link::Link(const Link& link) {
|
|||
|
|
dh_.theta = link.dh_.theta;
|
|||
|
|
dh_.d = link.dh_.d;
|
|||
|
|
dh_.alpha = link.dh_.alpha;
|
|||
|
|
dh_.a = link.dh_.a;
|
|||
|
|
type_ = link.type_;
|
|||
|
|
offset_ = link.offset_;
|
|||
|
|
qmin_ = link.qmin_;
|
|||
|
|
qmax_ = link.qmax_;
|
|||
|
|
m_ = link.m_;
|
|||
|
|
rc_ = link.rc_;
|
|||
|
|
I_ = link.I_;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
robotics::Link& robotics::Link::operator=(Link link) {
|
|||
|
|
dh_.theta = link.dh_.theta;
|
|||
|
|
dh_.d = link.dh_.d;
|
|||
|
|
dh_.alpha = link.dh_.alpha;
|
|||
|
|
dh_.a = link.dh_.a;
|
|||
|
|
type_ = link.type_;
|
|||
|
|
offset_ = link.offset_;
|
|||
|
|
qmin_ = link.qmin_;
|
|||
|
|
qmax_ = link.qmax_;
|
|||
|
|
m_ = link.m_;
|
|||
|
|
rc_ = link.rc_;
|
|||
|
|
I_ = link.I_;
|
|||
|
|
return *this;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Matrixf<4, 4> robotics::Link::T(float q) {
|
|||
|
|
if (type_ == R) {
|
|||
|
|
if (qmin_ >= qmax_)
|
|||
|
|
dh_.theta = q + offset_;
|
|||
|
|
else
|
|||
|
|
dh_.theta = math::limit(q + offset_, qmin_, qmax_);
|
|||
|
|
} else {
|
|||
|
|
if (qmin_ >= qmax_)
|
|||
|
|
dh_.d = q + offset_;
|
|||
|
|
else
|
|||
|
|
dh_.d = math::limit(q + offset_, qmin_, qmax_);
|
|||
|
|
}
|
|||
|
|
return dh_.fkine();
|
|||
|
|
}
|