423 lines
15 KiB
C++
423 lines
15 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.
|
||
******************************************************************************
|
||
*/
|
||
|
||
#ifndef ROBOTICS_H
|
||
#define ROBOTICS_H
|
||
|
||
#include "utils.h"
|
||
#include "matrix.h"
|
||
|
||
namespace robotics {
|
||
// rotation matrix(R) -> RPY([yaw;pitch;roll])
|
||
Matrixf<3, 1> r2rpy(Matrixf<3, 3> R);
|
||
// RPY([yaw;pitch;roll]) -> rotation matrix(R)
|
||
Matrixf<3, 3> rpy2r(Matrixf<3, 1> rpy);
|
||
// rotation matrix(R) -> angle vector([r;θ])
|
||
Matrixf<4, 1> r2angvec(Matrixf<3, 3> R);
|
||
// angle vector([r;θ]) -> rotation matrix(R)
|
||
Matrixf<3, 3> angvec2r(Matrixf<4, 1> angvec);
|
||
// rotation matrix(R) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)]
|
||
Matrixf<4, 1> r2quat(Matrixf<3, 3> R);
|
||
// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> rotation matrix(R)
|
||
Matrixf<3, 3> quat2r(Matrixf<4, 1> quat);
|
||
// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> RPY([yaw;pitch;roll])
|
||
Matrixf<3, 1> quat2rpy(Matrixf<4, 1> q);
|
||
// RPY([yaw;pitch;roll]) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)]
|
||
Matrixf<4, 1> rpy2quat(Matrixf<3, 1> rpy);
|
||
// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> angle vector([r;θ])
|
||
Matrixf<4, 1> quat2angvec(Matrixf<4, 1> q);
|
||
// angle vector([r;θ]) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)]
|
||
Matrixf<4, 1> angvec2quat(Matrixf<4, 1> angvec);
|
||
// homogeneous transformation matrix(T) -> rotation matrix(R)
|
||
Matrixf<3, 3> t2r(Matrixf<4, 4> T);
|
||
// rotation matrix(R) -> homogeneous transformation matrix(T)
|
||
Matrixf<4, 4> r2t(Matrixf<3, 3> R);
|
||
// homogeneous transformation matrix(T) -> translation vector(p)
|
||
Matrixf<3, 1> t2p(Matrixf<4, 4> T);
|
||
// translation vector(p) -> homogeneous transformation matrix(T)
|
||
Matrixf<4, 4> p2t(Matrixf<3, 1> p);
|
||
// rotation matrix(R) & translation vector(p) -> homogeneous transformation
|
||
// matrix(T)
|
||
Matrixf<4, 4> rp2t(Matrixf<3, 3> R, Matrixf<3, 1> p);
|
||
// homogeneous transformation matrix(T) -> RPY([yaw;pitch;roll])
|
||
Matrixf<3, 1> t2rpy(Matrixf<4, 4> T);
|
||
// inverse of homogeneous transformation matrix(T^-1=[R',-R'P;0,1])
|
||
Matrixf<4, 4> invT(Matrixf<4, 4> T);
|
||
// RPY([yaw;pitch;roll]) -> homogeneous transformation matrix(T)
|
||
Matrixf<4, 4> rpy2t(Matrixf<3, 1> rpy);
|
||
// homogeneous transformation matrix(T) -> angle vector([r;θ])
|
||
Matrixf<4, 1> t2angvec(Matrixf<4, 4> T);
|
||
// angle vector([r;θ]) -> homogeneous transformation matrix(T)
|
||
Matrixf<4, 4> angvec2t(Matrixf<4, 1> angvec);
|
||
// homogeneous transformation matrix(T) -> quaternion,
|
||
// [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)]
|
||
Matrixf<4, 1> t2quat(Matrixf<4, 4> T);
|
||
// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> homogeneous transformation
|
||
// matrix(T)
|
||
Matrixf<4, 4> quat2t(Matrixf<4, 1> quat);
|
||
// homogeneous transformation matrix(T) -> twist coordinates vector ([p;rθ])
|
||
Matrixf<6, 1> t2twist(Matrixf<4, 4> T);
|
||
// twist coordinates vector ([p;rθ]) -> homogeneous transformation matrix(T)
|
||
Matrixf<4, 4> twist2t(Matrixf<6, 1> twist);
|
||
|
||
// joint type: R-revolute joint, P-prismatic joint
|
||
typedef enum joint_type {
|
||
R = 0,
|
||
P = 1,
|
||
} Joint_Type_e;
|
||
|
||
// Denavit–Hartenberg(DH) method
|
||
struct DH_t {
|
||
// forward kinematic
|
||
Matrixf<4, 4> fkine();
|
||
// DH parameter
|
||
float theta;
|
||
float d;
|
||
float a;
|
||
float alpha;
|
||
Matrixf<4, 4> T;
|
||
};
|
||
|
||
class Link {
|
||
public:
|
||
Link(){};
|
||
Link(float theta, float d, float a, float alpha, Joint_Type_e type = R,
|
||
float offset = 0, float qmin = 0, float qmax = 0, float m = 1,
|
||
Matrixf<3, 1> rc = matrixf::zeros<3, 1>(),
|
||
Matrixf<3, 3> I = matrixf::zeros<3, 3>());
|
||
Link(const Link& link);
|
||
|
||
Link& operator=(Link link);
|
||
|
||
float qmin() { return qmin_; }
|
||
float qmax() { return qmax_; }
|
||
Joint_Type_e type() { return type_; }
|
||
float m() { return m_; }
|
||
Matrixf<3, 1> rc() { return rc_; }
|
||
Matrixf<3, 3> I() { return I_; }
|
||
|
||
Matrixf<4, 4> T(float q); // forward kinematic
|
||
|
||
public:
|
||
// kinematic parameter
|
||
DH_t dh_;
|
||
float offset_;
|
||
// limit(qmin,qmax), no limit if qmin<=qmax
|
||
float qmin_;
|
||
float qmax_;
|
||
// joint type
|
||
Joint_Type_e type_;
|
||
// dynamic parameter
|
||
float m_; // mass
|
||
Matrixf<3, 1> rc_; // centroid(link coordinate)
|
||
Matrixf<3, 3> I_; // inertia tensor(3*3)
|
||
};
|
||
|
||
template <uint16_t _n = 1>
|
||
class Serial_Link {
|
||
public:
|
||
Serial_Link(Link links[_n]) {
|
||
for (int i = 0; i < _n; i++)
|
||
links_[i] = links[i];
|
||
gravity_ = matrixf::zeros<3, 1>();
|
||
gravity_[2][0] = -9.81f;
|
||
}
|
||
|
||
Serial_Link(Link links[_n], Matrixf<3, 1> gravity) {
|
||
for (int i = 0; i < _n; i++)
|
||
links_[i] = links[i];
|
||
gravity_ = gravity;
|
||
}
|
||
|
||
// forward kinematic: T_n^0
|
||
// param[in] q: joint variable vector
|
||
// param[out] T_n^0
|
||
Matrixf<4, 4> fkine(Matrixf<_n, 1> q) {
|
||
T_ = matrixf::eye<4, 4>();
|
||
for (int iminus1 = 0; iminus1 < _n; iminus1++)
|
||
T_ = T_ * links_[iminus1].T(q[iminus1][0]);
|
||
return T_;
|
||
}
|
||
|
||
// forward kinematic: T_k^0
|
||
// param[in] q: joint variable vector
|
||
// param[in] k: joint number
|
||
// param[out] T_k^0
|
||
Matrixf<4, 4> fkine(Matrixf<_n, 1> q, uint16_t k) {
|
||
if (k > _n)
|
||
k = _n;
|
||
Matrixf<4, 4> T = matrixf::eye<4, 4>();
|
||
for (int iminus1 = 0; iminus1 < k; iminus1++)
|
||
T = T * links_[iminus1].T(q[iminus1][0]);
|
||
return T;
|
||
}
|
||
|
||
// T_k^k-1: homogeneous transformation matrix of link k
|
||
// param[in] q: joint variable vector
|
||
// param[in] kminus: joint number k, input k-1
|
||
// param[out] T_k^k-1
|
||
Matrixf<4, 4> T(Matrixf<_n, 1> q, uint16_t kminus1) {
|
||
if (kminus1 >= _n)
|
||
kminus1 = _n - 1;
|
||
return links_[kminus1].T(q[kminus1][0]);
|
||
}
|
||
|
||
// jacobian matrix, J_i = [J_pi;j_oi]
|
||
// param[in] q: joint variable vector
|
||
// param[out] jacobian matix J_6*n
|
||
Matrixf<6, _n> jacob(Matrixf<_n, 1> q) {
|
||
Matrixf<3, 1> p_e = t2p(fkine(q)); // p_e
|
||
Matrixf<4, 4> T_iminus1 = matrixf::eye<4, 4>(); // T_i-1^0
|
||
Matrixf<3, 1> z_iminus1; // z_i-1^0
|
||
Matrixf<3, 1> p_iminus1; // p_i-1^0
|
||
Matrixf<3, 1> J_pi;
|
||
Matrixf<3, 1> J_oi;
|
||
for (int iminus1 = 0; iminus1 < _n; iminus1++) {
|
||
// revolute joint: J_pi = z_i-1x(p_e-p_i-1), J_oi = z_i-1
|
||
if (links_[iminus1].type() == R) {
|
||
z_iminus1 = T_iminus1.block<3, 1>(0, 2);
|
||
p_iminus1 = t2p(T_iminus1);
|
||
T_iminus1 = T_iminus1 * links_[iminus1].T(q[iminus1][0]);
|
||
J_pi = vector3f::cross(z_iminus1, p_e - p_iminus1);
|
||
J_oi = z_iminus1;
|
||
}
|
||
// prismatic joint: J_pi = z_i-1, J_oi = 0
|
||
else {
|
||
z_iminus1 = T_iminus1.block<3, 1>(0, 2);
|
||
T_iminus1 = T_iminus1 * links_[iminus1].T(q[iminus1][0]);
|
||
J_pi = z_iminus1;
|
||
J_oi = matrixf::zeros<3, 1>();
|
||
}
|
||
J_[0][iminus1] = J_pi[0][0];
|
||
J_[1][iminus1] = J_pi[1][0];
|
||
J_[2][iminus1] = J_pi[2][0];
|
||
J_[3][iminus1] = J_oi[0][0];
|
||
J_[4][iminus1] = J_oi[1][0];
|
||
J_[5][iminus1] = J_oi[2][0];
|
||
}
|
||
return J_;
|
||
}
|
||
|
||
// inverse kinematic, Levenberg-Marquardt method
|
||
// param[in] Td: target homogeneous transformation matrix of end effector
|
||
// param[in] q: initial joint variable vector(q0) for iteration
|
||
// param[in] tol: tolerance of error (norm of twist error vector)
|
||
// param[in] max_iter: maximum iterations, default 50
|
||
// param[out] q: joint variable vector
|
||
Matrixf<_n, 1> ikine(Matrixf<4, 4> Td,
|
||
Matrixf<_n, 1> q = matrixf::zeros<_n, 1>(),
|
||
float tol = 1e-4f, uint16_t max_iter = 50) {
|
||
Matrixf<4, 4> T;
|
||
Matrixf<3, 1> pe, we;
|
||
Matrixf<6, 1> err, new_err;
|
||
Matrixf<_n, 1> dq;
|
||
float lambda = 1.0f; // LM 阻尼因子
|
||
constexpr float lambda_up = 10.0f; // 发散时增大
|
||
constexpr float lambda_down = 0.1f; // 收敛时减小
|
||
constexpr float lambda_max = 1e6f;
|
||
constexpr float lambda_min = 1e-6f;
|
||
|
||
for (int iter = 0; iter < max_iter; iter++) {
|
||
T = fkine(q);
|
||
pe = t2p(Td) - t2p(T);
|
||
we = t2twist(Td * invT(T)).block<3, 1>(3, 0);
|
||
for (int k = 0; k < 3; k++) {
|
||
err[k][0] = pe[k][0];
|
||
err[k + 3][0] = we[k][0];
|
||
}
|
||
float err_norm = err.norm();
|
||
if (err_norm < tol)
|
||
return q;
|
||
|
||
Matrixf<6, _n> J = jacob(q);
|
||
Matrixf<_n, _n> JtJ = J.trans() * J;
|
||
Matrixf<_n, 1> Jte = J.trans() * err;
|
||
|
||
// LM 步：dq = (J'J + λI)^{-1} J'e
|
||
dq = matrixf::inv(JtJ + lambda * matrixf::eye<_n, _n>()) * Jte;
|
||
|
||
// 检查求逆是否成功
|
||
if (dq[0][0] == INFINITY || dq[0][0] != dq[0][0]) {
|
||
lambda *= lambda_up;
|
||
if (lambda > lambda_max) return q;
|
||
continue;
|
||
}
|
||
|
||
// 评估新误差
|
||
Matrixf<_n, 1> q_new = q + dq;
|
||
// 仅对真实循环关节（qmin≈0, qmax≈2π）做 loopLimit 折叠，消除周期歧义。
|
||
// 其余非循环关节（J2/J3/J5等小范围）不折叠：一旦超界 loopLimit 会产生大跳变
|
||
// （例如 J3 在 3.0 边界被折到 -1.0），破坏 LM 迭代连续性导致无法收敛。
|
||
// 注：Link::T() 不再做任何截断，迭代过程允许 q 暂时超界以保持连续的 Jacobian；
|
||
// 最终关节超限的检查在 StepTrajectory() 的限位验证中完成。
|
||
for (int i = 0; i < _n; i++) {
|
||
if (links_[i].type() == R) {
|
||
constexpr float TWO_PI = 2.0f * (float)M_PI;
|
||
bool is_cyclic = (links_[i].qmin_ < 0.1f) && (links_[i].qmax_ > TWO_PI - 0.1f);
|
||
if (is_cyclic)
|
||
q_new[i][0] = math::loopLimit(q_new[i][0], links_[i].qmin_, links_[i].qmax_);
|
||
}
|
||
}
|
||
T = fkine(q_new);
|
||
pe = t2p(Td) - t2p(T);
|
||
we = t2twist(Td * invT(T)).block<3, 1>(3, 0);
|
||
for (int k = 0; k < 3; k++) {
|
||
new_err[k][0] = pe[k][0];
|
||
new_err[k + 3][0] = we[k][0];
|
||
}
|
||
float new_err_norm = new_err.norm();
|
||
|
||
if (new_err_norm < err_norm) {
|
||
// 误差减小：接受步长，减小阻尼（趋近 Gauss-Newton，加速收敛）
|
||
q = q_new;
|
||
lambda *= lambda_down;
|
||
if (lambda < lambda_min) lambda = lambda_min;
|
||
} else {
|
||
// 误差增大：拒绝步长，增大阻尼（趋近梯度下降，缩小步幅）
|
||
lambda *= lambda_up;
|
||
if (lambda > lambda_max) return q; // 阻尼过大，无法继续
|
||
}
|
||
}
|
||
return q;
|
||
}
|
||
|
||
// (Reserved function) inverse kinematic, analytic solution(geometric method)
|
||
Matrixf<_n, 1> (*ikine_analytic)(Matrixf<4, 4> T);
|
||
|
||
// inverse dynamic, Newton-Euler method
|
||
// param[in] q: joint variable vector
|
||
// param[in] qv: dq/dt
|
||
// param[in] qa: d^2q/dt^2
|
||
// param[in] he: load on end effector [f;μ], default 0
|
||
Matrixf<_n, 1> rne(Matrixf<_n, 1> q,
|
||
Matrixf<_n, 1> qv = matrixf::zeros<_n, 1>(),
|
||
Matrixf<_n, 1> qa = matrixf::zeros<_n, 1>(),
|
||
Matrixf<6, 1> he = matrixf::zeros<6, 1>()) {
|
||
// forward propagation
|
||
// record each links' motion state in matrices
|
||
// [ωi] angular velocity
|
||
Matrixf<3, _n + 1> w = matrixf::zeros<3, _n + 1>();
|
||
// [βi] angular acceleration
|
||
Matrixf<3, _n + 1> b = matrixf::zeros<3, _n + 1>();
|
||
// [pi] position of joint
|
||
Matrixf<3, _n + 1> p = matrixf::zeros<3, _n + 1>();
|
||
// [vi] velocity of joint
|
||
Matrixf<3, _n + 1> v = matrixf::zeros<3, _n + 1>();
|
||
// [ai] acceleration of joint
|
||
Matrixf<3, _n + 1> a = matrixf::zeros<3, _n + 1>();
|
||
// [aci] acceleration of mass center
|
||
Matrixf<3, _n + 1> ac = matrixf::zeros<3, _n + 1>();
|
||
// temperary vectors
|
||
Matrixf<3, 1> w_i, b_i, p_i, v_i, ai, ac_i;
|
||
// i & i-1 coordinate convert to 0 coordinate
|
||
Matrixf<4, 4> T_0i = matrixf::eye<4, 4>();
|
||
Matrixf<4, 4> T_0iminus1 = matrixf::eye<4, 4>();
|
||
Matrixf<3, 3> R_0i = matrixf::eye<3, 3>();
|
||
Matrixf<3, 3> R_0iminus1 = matrixf::eye<3, 3>();
|
||
// unit vector of z-axis
|
||
Matrixf<3, 1> ez = matrixf::zeros<3, 1>();
|
||
ez[2][0] = 1;
|
||
|
||
for (int i = 1; i <= _n; i++) {
|
||
T_0i = T_0i * T(q, i - 1); // T_i^0
|
||
R_0i = t2r(T_0i); // R_i^0
|
||
R_0iminus1 = t2r(T_0iminus1); // R_i-1^0
|
||
// ω_i = ω_i-1+qv_i*R_i-1^0*ez
|
||
w_i = w.col(i - 1) + qv[i - 1][0] * R_0iminus1 * ez;
|
||
// β_i = β_i-1+ω_i-1x(qv_i*R_i-1^0*ez)+qa_i*R_i-1^0*ez
|
||
b_i = b.col(i - 1) +
|
||
vector3f::cross(w.col(i - 1), qv[i - 1][0] * R_0iminus1 * ez) +
|
||
qa[i - 1][0] * R_0iminus1 * ez;
|
||
p_i = t2p(T_0i); // p_i = T_i^0(1:3,4)
|
||
// v_i = v_i-1+ω_ix(p_i-p_i-1)
|
||
v_i = v.col(i - 1) + vector3f::cross(w_i, p_i - p.col(i - 1));
|
||
// a_i = a_i-1+β_ix(p_i-p_i-1)+ω_ix(ω_ix(p_i-p_i-1))
|
||
ai = a.col(i - 1) + vector3f::cross(b_i, p_i - p.col(i - 1)) +
|
||
vector3f::cross(w_i, vector3f::cross(w_i, p_i - p.col(i - 1)));
|
||
// ac_i = a_i+β_ix(R_0^i*rc_i^i)+ω_ix(ω_ix(R_0^i*rc_i^i))
|
||
ac_i =
|
||
ai + vector3f::cross(b_i, R_0i * links_[i - 1].rc()) +
|
||
vector3f::cross(w_i, vector3f::cross(w_i, R_0i * links_[i - 1].rc()));
|
||
for (int row = 0; row < 3; row++) {
|
||
w[row][i] = w_i[row][0];
|
||
b[row][i] = b_i[row][0];
|
||
p[row][i] = p_i[row][0];
|
||
v[row][i] = v_i[row][0];
|
||
a[row][i] = ai[row][0];
|
||
ac[row][i] = ac_i[row][0];
|
||
}
|
||
T_0iminus1 = T_0i; // T_i-1^0
|
||
}
|
||
|
||
// backward propagation
|
||
// record each links' force
|
||
Matrixf<3, _n + 1> f = matrixf::zeros<3, _n + 1>(); // joint force
|
||
Matrixf<3, _n + 1> mu = matrixf::zeros<3, _n + 1>(); // joint moment
|
||
// temperary vector
|
||
Matrixf<3, 1> f_iminus1, mu_iminus1;
|
||
// {T,R',P}_i^i-1
|
||
Matrixf<4, 4> T_iminus1i;
|
||
Matrixf<3, 3> RT_iminus1i;
|
||
Matrixf<3, 1> P_iminus1i;
|
||
// I_i-1(in 0 coordinate)
|
||
Matrixf<3, 3> I_i;
|
||
// joint torque
|
||
Matrixf<_n, 1> torq;
|
||
|
||
// load on end effector
|
||
for (int row = 0; row < 3; row++) {
|
||
f[row][_n] = he.block<3, 1>(0, 0)[row][0];
|
||
mu[row][_n] = he.block<3, 1>(3, 0)[row][0];
|
||
}
|
||
for (int i = _n; i > 0; i--) {
|
||
T_iminus1i = T(q, i - 1); // T_i^i-1
|
||
P_iminus1i = t2p(T_iminus1i); // P_i^i-1
|
||
RT_iminus1i = t2r(T_iminus1i).trans(); // R_i^i-1'
|
||
R_0iminus1 = R_0i * RT_iminus1i; // R_i-1^0
|
||
// I_i^0 = R_i^0*I_i^i*(R_i^0)'
|
||
I_i = R_0i * links_[i - 1].I() * R_0i.trans();
|
||
// f_i-1 = f_i+m_i*ac_i-m_i*g
|
||
f_iminus1 = f.col(i) + links_[i - 1].m() * ac.col(i) -
|
||
links_[i - 1].m() * gravity_;
|
||
// μ_i-1 = μ_i+f_ixrc_i-f_i-1xrc_i-1->ci+I_i*b_i+ω_ix(I_i*ω_i)
|
||
mu_iminus1 = mu.col(i) +
|
||
vector3f::cross(f.col(i), R_0i * links_[i - 1].rc()) -
|
||
vector3f::cross(f_iminus1, R_0i * (RT_iminus1i * P_iminus1i +
|
||
links_[i - 1].rc())) +
|
||
I_i * b.col(i) + vector3f::cross(w.col(i), I_i * w.col(i));
|
||
// τ_i = μ_i-1'*(R_i-1^0*ez)
|
||
torq[i - 1][0] = (mu_iminus1.trans() * R_0iminus1 * ez)[0][0];
|
||
for (int row = 0; row < 3; row++) {
|
||
f[row][i - 1] = f_iminus1[row][0];
|
||
mu[row][i - 1] = mu_iminus1[row][0];
|
||
}
|
||
R_0i = R_0iminus1;
|
||
}
|
||
|
||
return torq;
|
||
}
|
||
|
||
private:
|
||
Link links_[_n];
|
||
Matrixf<3, 1> gravity_;
|
||
|
||
Matrixf<4, 4> T_;
|
||
Matrixf<6, _n> J_;
|
||
};
|
||
}; // namespace robotics
|
||
|
||
#endif // ROBOTICS_H
|