zyx
/
engineering
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

移植上交cpp机器人学库

This commit is contained in:
宋家齐 2024-04-28 19:22:24 +08:00
parent 47689e75dc
commit df4892df01
11 changed files with 1277 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -53,7 +53,7 @@ include_directories(Inc Drivers/STM32F4xx_HAL_Driver/Inc Drivers/STM32F4xx_HAL_D
bsp bsp/adc bsp/can bsp/dwt bsp/flash bsp/gpio bsp/iic bsp/log bsp/pwm bsp/spi bsp/usart bsp/usb
modules modules/alarm modules/algorithm modules/auto_aim modules/BMI088 modules/can_comm modules/daemon modules/encoder modules/imu modules/ist8310
modules/led modules/master_machine modules/message_center modules/motor modules/oled modules/referee modules/remote modules/standard_cmd
modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa
modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa modules/robotics
modules/motor/DJImotor modules/motor/Dmmotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor
application application/chassis application/cmd application/gimbal application/to_stretch
Middlewares/Third_Party/SEGGER/RTT Middlewares/Third_Party/SEGGER/Config)

90
application/gimbal/gimbal.c → application/gimbal/gimbal.cpp
View File

@ -1,11 +1,18 @@
#ifdef __cplusplus
extern "C" {
#endif
#include "gimbal.h"
#include "robot_def.h"
#include "dji_motor.h"
#include "ins_task.h"
#include "message_center.h"
#include "general_def.h"
#include "bmi088.h"
#ifdef __cplusplus
}
#endif
#include "matrix.h"
#include "robotics.h"
static attitude_t *gimba_IMU_data; // 云台IMU数据
static DJIMotorInstance *yaw_motor, *pitch_motor;
@ -19,81 +26,7 @@ void GimbalInit()
{
gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
// YAW
Motor_Init_Config_s yaw_config = {
.can_init_config = {
.can_handle = &hcan1,
.tx_id = 1,
},
.controller_param_init_config = {
.angle_PID = {
.Kp = 8, // 8
.Ki = 0,
.Kd = 0,
.DeadBand = 0.1,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100,
.MaxOut = 500,
},
.speed_PID = {
.Kp = 50, // 50
.Ki = 200, // 200
.Kd = 0,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 3000,
.MaxOut = 20000,
},
.other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
.other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2],
},
.controller_setting_init_config = {
.angle_feedback_source = OTHER_FEED,
.speed_feedback_source = OTHER_FEED,
.outer_loop_type = ANGLE_LOOP,
.close_loop_type = ANGLE_LOOP | SPEED_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
},
.motor_type = GM6020};
// PITCH
Motor_Init_Config_s pitch_config = {
.can_init_config = {
.can_handle = &hcan2,
.tx_id = 2,
},
.controller_param_init_config = {
.angle_PID = {
.Kp = 10, // 10
.Ki = 0,
.Kd = 0,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100,
.MaxOut = 500,
},
.speed_PID = {
.Kp = 50, // 50
.Ki = 350, // 350
.Kd = 0, // 0
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 2500,
.MaxOut = 20000,
},
.other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
.other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]),
},
.controller_setting_init_config = {
.angle_feedback_source = OTHER_FEED,
.speed_feedback_source = OTHER_FEED,
.outer_loop_type = ANGLE_LOOP,
.close_loop_type = SPEED_LOOP | ANGLE_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
},
.motor_type = GM6020,
};
// 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
yaw_motor = DJIMotorInit(&yaw_config);
pitch_motor = DJIMotorInit(&pitch_config);
gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
@ -102,6 +35,13 @@ void GimbalInit()
/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
void GimbalTask()
{
/* Example 1: PUMA560 ------------------------------------------------------------------------*/
float m[6] = {0.2645, 0.17, 0.1705, 0, 0, 0};
float data[18]={0, -8.5e-2, 0, 0, 0, 0,
13.225e-2, 0, 0, 0, 0, 0,
0, 3.7e-2, 8.525e-2, 0, 0, 0};
Matrixf<3, 6> rc(data);
Matrixf<3, 3> I[6];
// 获取云台控制数据
// 后续增加未收到数据的处理
SubGetMessage(gimbal_sub, &gimbal_cmd_recv);

7
application/gimbal/gimbal.h
View File

@ -1,6 +1,9 @@
#ifndef GIMBAL_H
#define GIMBAL_H
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief 初始化云台,会被RobotInit()调用
*
@ -13,4 +16,8 @@ void GimbalInit();
*/
void GimbalTask();
#ifdef __cplusplus
}
#endif
#endif // GIMBAL_H

120
modules/algorithm/myrobotics.c Normal file
View File

@ -0,0 +1,120 @@
//
// Created by SJQ on 2024/4/26.
//
#include "myrobotics.h"
#define R_COL 3
#define R_ROW 3
#define T_COL 4
#define T_ROW 4
void r2rpy(const mat* R,float rpy[3])
{
rpy[2] = atan2f(R->pData[2 * R_COL + 1], R->pData[2 * R_COL + 2]); //roll
float temp = sqrtf(R->pData[2 * R_COL + 1] * R->pData[2 * R_COL + 1] + R->pData[2 * R_COL + 2] * R->pData[2 * R_COL + 2]);
rpy[1] = atan2f(-R->pData[2 * R_COL + 0], temp); //pitch
rpy[0] = atan2f(R->pData[1 * R_COL + 0], R->pData[0 * R_COL + 0]); //yaw
}
void rpy2r(float rpy[3],mat* R)
{
float c[3] = {arm_cos_f32(rpy[0]), arm_cos_f32(rpy[1]), arm_cos_f32(rpy[2])};
float s[3] = {arm_sin_f32(rpy[0]), arm_sin_f32(rpy[1]), arm_sin_f32(rpy[2])};
float R_data[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
};
MatInit(R,R_ROW,R_COL);
memcpy(R->pData,R_data,sizeof(float) * R_COL * R_ROW);
}
void r2quat(const mat*R,float q[4])
{
float R_trace = R->pData[0 * R_COL + 0] + R->pData[1 * R_COL + 1] + R->pData[2 * R_COL + 2];
float q_data[4] = {
0.5f * sqrtf(float_constrain(R_trace, -1, 1) + 1), // q0=cos(θ/2)
sign(R->pData[2 * R_COL + 1] - R->pData[1 * R_COL + 2]) * 0.5f *
sqrtf(float_constrain(R->pData[0 * R_COL + 0] - R->pData[1 * R_COL + 1] - R->pData[2 * R_COL + 2], -1, 1) +
1), // q1=rx*sin(θ/2)
sign(R->pData[0 * R_COL + 2] - R->pData[2 * R_COL + 0]) * 0.5f *
sqrtf(float_constrain(-R->pData[0 * R_COL + 0] + R->pData[1 * R_COL + 1] - R->pData[2 * R_COL + 2], -1, 1) +
1), // q2=ry*sin(θ/2)
sign(R->pData[1 * R_COL + 0] - R->pData[0 * R_COL + 1]) * 0.5f *
sqrtf(float_constrain(-R->pData[0 * R_COL + 0] - R->pData[1 * R_COL + 1] + R->pData[2 * R_COL + 2], -1, 1) +
1), // q3=rz*sin(θ/2)
};
memcpy(q,q_data,sizeof(float) * 4);
}
void quat2r(float q[4],mat* R)
{
float R_data[9] = {
1 - 2.0f * (q[2] * q[2] + q[3] * q[3]), // R11
2.0f * (q[1] * q[2] - q[0] * q[3]), // R12
2.0f * (q[1] * q[3] + q[0] * q[2]), // R13
2.0f * (q[1] * q[2] + q[0] * q[3]), // R21
1 - 2.0f * (q[1] * q[1] + q[3] * q[3]), // R22
2.0f * (q[2] * q[3] - q[0] * q[1]), // R23
2.0f * (q[1] * q[3] - q[0] * q[2]), // R31
2.0f * (q[2] * q[3] + q[0] * q[1]), // R32
1 - 2.0f * (q[1] * q[1] + q[2] * q[2]) // R33
};
memcpy(R->pData,R_data,sizeof(float) * R_COL * R_ROW);
}
void quat2rpy(const float q[4],float rpy[3])
{
float rpy_data[3] = {
atan2f(2.0f * (q[1] * q[2] + q[0] * q[3]),
1 - 2.0f * (q[2] * q[2] + q[3] * q[3])), // yaw
asinf(2.0f * (q[0] * q[2] - q[1] * q[3])), // pitch
atan2f(2.0f * (q[2] * q[3] + q[0] * q[1]),
1 - 2.0f * (q[1] * q[1] + q[2] * q[2])) // rol
};
memcpy(rpy,rpy_data,sizeof(float) * 3);
}
void rpy2quat(const float rpy[3],float q[4])
{
float c[3] = {cosf(0.5f * rpy[0]), cosf(0.5f * rpy[1]),
cosf(0.5f * rpy[2])}; // cos(*/2)
float s[3] = {sinf(0.5f * rpy[0]), sinf(0.5f * rpy[1]),
sinf(0.5f * rpy[2])}; // sin(*/2)
float q_data[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)
};
memcpy(q,q_data,sizeof(float) * 4);
}
void r2t(const mat* R,mat* T)
{
float T_data[16] = {R->pData[0], R->pData[1], R->pData[2], 0,
R->pData[3], R->pData[4], R->pData[5], 0,
R->pData[6], R->pData[7], R->pData[8],0,
0, 0, 0, 1};
MatInit(T,T_ROW,T_COL);
memcpy(T->pData,T_data,sizeof(float) * T_COL * T_ROW);
}
void rp2t(const mat* R,const float p[3],mat* T)
{
float T_data[16] = {R->pData[0], R->pData[1], R->pData[2], p[0],
R->pData[3], R->pData[4], R->pData[5], p[1],
R->pData[6], R->pData[7], R->pData[8],p[2],
0, 0, 0, 1};
MatInit(T,T_ROW,T_COL);
memcpy(T->pData,T_data,sizeof(float) * T_COL * T_ROW);
}

21
modules/algorithm/myrobotics.h Normal file
View File

@ -0,0 +1,21 @@
//
// Created by SJQ on 2024/4/26.
//
#ifndef BASIC_FRAMEWORK_ROBOTICS_H
#define BASIC_FRAMEWORK_ROBOTICS_H
#include "user_lib.h"
// rotation matrix(R) RPY([yaw,pitch,roll]) quaternion([q0,q1,q2,q3])
void r2rpy(const mat* R,float rpy[3]);
void rpy2r(float rpy[3],mat* R);
void r2quat(const mat*R,float q[4]);
void quat2r(float q[4],mat* R);
void quat2rpy(const float q[4],float rpy[3]);
void rpy2quat(const float rpy[3],float q[4]);
#endif //BASIC_FRAMEWORK_ROBOTICS_H

24
modules/robotics/matrix.cpp Normal file
View File

@ -0,0 +1,24 @@
/**
******************************************************************************
* @file matrix.cpp/h
* @brief Matrix/vector calculation. 矩阵/向量运算
* @author Spoon Guan
******************************************************************************
* Copyright (c) 2023 Team JiaoLong-SJTU
* All rights reserved.
******************************************************************************
*/
#include "matrix.h"
// hat of vector
Matrixf<3, 3> vector3f::hat(Matrixf<3, 1> vec) {
float hat[9] = {0, -vec[2][0], vec[1][0], vec[2][0], 0,
-vec[0][0], -vec[1][0], vec[0][0], 0};
return Matrixf<3, 3>(hat);
}
// cross product
Matrixf<3, 1> vector3f::cross(Matrixf<3, 1> vec1, Matrixf<3, 1> vec2) {
return vector3f::hat(vec1) * vec2;
}

259
modules/robotics/matrix.h Normal file
View File

@ -0,0 +1,259 @@
/**
******************************************************************************
* @file matrix.cpp/h
* @brief Matrix/vector calculation. 矩阵/向量运算
* @author Spoon Guan
******************************************************************************
* Copyright (c) 2023 Team JiaoLong-SJTU
* All rights reserved.
******************************************************************************
*/
#ifndef MATRIX_H
#define MATRIX_H
//#include "arm_math.h"
#include "user_lib.h"
// Matrix class
template <int _rows, int _cols>
class Matrixf {
public:
// Constructor without input data
Matrixf(void) : rows_(_rows), cols_(_cols) {
arm_mat_init_f32(&arm_mat_, _rows, _cols, this->data_);
}
// Constructor with input data
Matrixf(float data[_rows * _cols]) : Matrixf() {
memcpy(this->data_, data, _rows * _cols * sizeof(float));
}
// Copy constructor
Matrixf(const Matrixf<_rows, _cols>& mat) : Matrixf() {
memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float));
}
// Destructor
~Matrixf(void) {}
// Row size
int rows(void) { return _rows; }
// Column size
int cols(void) { return _cols; }
// Element
float* operator[](const int& row) { return &this->data_[row * _cols]; }
// Operators
Matrixf<_rows, _cols>& operator=(const Matrixf<_rows, _cols> mat) {
memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float));
return *this;
}
Matrixf<_rows, _cols>& operator+=(const Matrixf<_rows, _cols> mat) {
arm_status s;
s = arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_);
return *this;
}
Matrixf<_rows, _cols>& operator-=(const Matrixf<_rows, _cols> mat) {
arm_status s;
s = arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_);
return *this;
}
Matrixf<_rows, _cols>& operator*=(const float& val) {
arm_status s;
s = arm_mat_scale_f32(&this->arm_mat_, val, &this->arm_mat_);
return *this;
}
Matrixf<_rows, _cols>& operator/=(const float& val) {
arm_status s;
s = arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &this->arm_mat_);
return *this;
}
Matrixf<_rows, _cols> operator+(const Matrixf<_rows, _cols>& mat) {
arm_status s;
Matrixf<_rows, _cols> res;
s = arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_);
return res;
}
Matrixf<_rows, _cols> operator-(const Matrixf<_rows, _cols>& mat) {
arm_status s;
Matrixf<_rows, _cols> res;
s = arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_);
return res;
}
Matrixf<_rows, _cols> operator*(const float& val) {
arm_status s;
Matrixf<_rows, _cols> res;
s = arm_mat_scale_f32(&this->arm_mat_, val, &res.arm_mat_);
return res;
}
friend Matrixf<_rows, _cols> operator*(const float& val,
const Matrixf<_rows, _cols>& mat) {
arm_status s;
Matrixf<_rows, _cols> res;
s = arm_mat_scale_f32(&mat.arm_mat_, val, &res.arm_mat_);
return res;
}
Matrixf<_rows, _cols> operator/(const float& val) {
arm_status s;
Matrixf<_rows, _cols> res;
s = arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &res.arm_mat_);
return res;
}
// Matrix multiplication
template <int cols>
friend Matrixf<_rows, cols> operator*(const Matrixf<_rows, _cols>& mat1,
const Matrixf<_cols, cols>& mat2) {
arm_status s;
Matrixf<_rows, cols> res;
s = arm_mat_mult_f32(&mat1.arm_mat_, &mat2.arm_mat_, &res.arm_mat_);
return res;
}
// Submatrix
template <int rows, int cols>
Matrixf<rows, cols> block(const int& start_row, const int& start_col) {
Matrixf<rows, cols> res;
for (int row = start_row; row < start_row + rows; row++) {
memcpy((float*)res[0] + (row - start_row) * cols,
(float*)this->data_ + row * _cols + start_col,
cols * sizeof(float));
}
return res;
}
// Specific row
Matrixf<1, _cols> row(const int& row) { return block<1, _cols>(row, 0); }
// Specific column
Matrixf<_rows, 1> col(const int& col) { return block<_rows, 1>(0, col); }
// Transpose
Matrixf<_cols, _rows> trans(void) {
Matrixf<_cols, _rows> res;
arm_mat_trans_f32(&arm_mat_, &res.arm_mat_);
return res;
}
// Trace
float trace(void) {
float res = 0;
for (int i = 0; i < fmin(_rows, _cols); i++) {
res += (*this)[i][i];
}
return res;
}
// Norm
float norm(void) { return sqrtf((this->trans() * *this)[0][0]); }
public:
// arm matrix instance
arm_matrix_instance_f32 arm_mat_;
protected:
// size
int rows_, cols_;
// data
float data_[_rows * _cols];
};
// Matrix funtions
namespace matrixf {
// Special Matrices
// Zero matrix
template <int _rows, int _cols>
Matrixf<_rows, _cols> zeros(void) {
float data[_rows * _cols] = {0};
return Matrixf<_rows, _cols>(data);
}
// Ones matrix
template <int _rows, int _cols>
Matrixf<_rows, _cols> ones(void) {
float data[_rows * _cols] = {0};
for (int i = 0; i < _rows * _cols; i++) {
data[i] = 1;
}
return Matrixf<_rows, _cols>(data);
}
// Identity matrix
template <int _rows, int _cols>
Matrixf<_rows, _cols> eye(void) {
float data[_rows * _cols] = {0};
for (int i = 0; i < fmin(_rows, _cols); i++) {
data[i * _cols + i] = 1;
}
return Matrixf<_rows, _cols>(data);
}
// Diagonal matrix
template <int _rows, int _cols>
Matrixf<_rows, _cols> diag(Matrixf<_rows, 1> vec) {
Matrixf<_rows, _cols> res = matrixf::zeros<_rows, _cols>();
for (int i = 0; i < fmin(_rows, _cols); i++) {
res[i][i] = vec[i][0];
}
return res;
}
// Inverse
template <int _dim>
Matrixf<_dim, _dim> inv(Matrixf<_dim, _dim> mat) {
arm_status s;
// extended matrix [A|I]
Matrixf<_dim, 2 * _dim> ext_mat = matrixf::zeros<_dim, 2 * _dim>();
for (int i = 0; i < _dim; i++) {
memcpy(ext_mat[i], mat[i], _dim * sizeof(float));
ext_mat[i][_dim + i] = 1;
}
// elimination
for (int i = 0; i < _dim; i++) {
// find maximum absolute value in the first column in lower right block
float abs_max = fabs(ext_mat[i][i]);
int abs_max_row = i;
for (int row = i; row < _dim; row++) {
if (abs_max < fabs(ext_mat[row][i])) {
abs_max = fabs(ext_mat[row][i]);
abs_max_row = row;
}
}
if (abs_max < 1e-12f) { // singular
return matrixf::zeros<_dim, _dim>();
s = ARM_MATH_SINGULAR;
}
if (abs_max_row != i) { // row exchange
float tmp;
Matrixf<1, 2 * _dim> row_i = ext_mat.row(i);
Matrixf<1, 2 * _dim> row_abs_max = ext_mat.row(abs_max_row);
memcpy(ext_mat[i], row_abs_max[0], 2 * _dim * sizeof(float));
memcpy(ext_mat[abs_max_row], row_i[0], 2 * _dim * sizeof(float));
}
float k = 1.f / ext_mat[i][i];
for (int col = i; col < 2 * _dim; col++) {
ext_mat[i][col] *= k;
}
for (int row = 0; row < _dim; row++) {
if (row == i) {
continue;
}
k = ext_mat[row][i];
for (int j = i; j < 2 * _dim; j++) {
ext_mat[row][j] -= k * ext_mat[i][j];
}
}
}
// inv = ext_mat(:,n+1:2n)
s = ARM_MATH_SUCCESS;
Matrixf<_dim, _dim> res;
for (int i = 0; i < _dim; i++) {
memcpy(res[i], &ext_mat[i][_dim], _dim * sizeof(float));
}
return res;
}
} // namespace matrixf
namespace vector3f {
// hat of vector
Matrixf<3, 3> hat(Matrixf<3, 1> vec);
// cross product
Matrixf<3, 1> cross(Matrixf<3, 1> vec1, Matrixf<3, 1> vec2);
} // namespace vector3f
#endif // MATRIX_H

340
modules/robotics/robotics.cpp Normal file
View File

@ -0,0 +1,340 @@
/**
******************************************************************************
* @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();
}

407
modules/robotics/robotics.h Normal file
View File

@ -0,0 +1,407 @@
/**
******************************************************************************
* @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, numerical solution(Newton method)
// param[in] T: homogeneous transformation matrix of end effector
// param[in] q: initial joint variable vector(q0) for Newton method's
// iteration
// param[in] tol: tolerance of error (norm(error of twist vector))
// param[in] max_iter: maximum iterations, default 30
// 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 step = 1;
for (int i = 0; i < max_iter; i++) {
T = fkine(q);
pe = t2p(Td) - t2p(T);
// angvec(Td*T^-1), transform angular vector(T->Td) in world coordinate
we = t2twist(Td * invT(T)).block<3, 1>(3, 0);
for (int i = 0; i < 3; i++) {
err[i][0] = pe[i][0];
err[i + 3][0] = we[i][0];
}
if (err.norm() < tol)
return q;
// adjust iteration step
Matrixf<6, _n> J = jacob(q);
for (int j = 0; j < 5; j++) {
dq = matrixf::inv(J.trans() * J) * (J.trans() * err) * step;
if (dq[0][0] == INFINITY) // J'*J singular
{
dq = matrixf::inv(J.trans() * J + 0.1f * matrixf::eye<_n, _n>()) *
J.trans() * err * step;
// SVD<6, _n> JTJ_svd(J.trans() * J);
// dq = JTJ_svd.solve(err) * step * 5e-2f;
q += dq;
for (int i = 0; i < _n; i++) {
if (links_[i].type() == R)
q[i][0] = math::loopLimit(q[i][0], -PI, PI);
}
break;
}
T = fkine(q + dq);
pe = t2p(Td) - t2p(T);
we = t2twist(Td * invT(T)).block<3, 1>(3, 0);
for (int i = 0; i < 3; i++) {
new_err[i][0] = pe[i][0];
new_err[i + 3][0] = we[i][0];
}
if (new_err.norm() < err.norm()) {
q += dq;
for (int i = 0; i < _n; i++) {
if (links_[i].type() == robotics::Joint_Type_e::R) {
q[i][0] = math::loopLimit(q[i][0], -PI, PI);
}
}
break;
} else {
step /= 2.0f;
}
}
if (step < 1e-3f)
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

56
modules/robotics/utils.cpp Normal file
View File

@ -0,0 +1,56 @@
/**
******************************************************************************
* @file utils.cpp/h
* @brief General math utils. 常用数学工具函数
* @author Spoon Guan
******************************************************************************
* Copyright (c) 2023 Team JiaoLong-SJTU
* All rights reserved.
******************************************************************************
*/
#include "utils.h"
float math::limit(float val, const float& min, const float& max) {
if (min > max)
return val;
else if (val < min)
val = min;
else if (val > max)
val = max;
return val;
}
float math::limitMin(float val, const float& min) {
if (val < min)
val = min;
return val;
}
float math::limitMax(float val, const float& max) {
if (val > max)
val = max;
return val;
}
float math::loopLimit(float val, const float& min, const float& max) {
if (min >= max)
return val;
if (val > max) {
while (val > max)
val -= (max - min);
} else if (val < min) {
while (val < min)
val += (max - min);
}
return val;
}
float math::sign(const float& val) {
if (val > 0)
return 1;
else if (val < 0)
return -1;
return 0;
}

27
modules/robotics/utils.h Normal file
View File

@ -0,0 +1,27 @@
/**
******************************************************************************
* @file utils.cpp/h
* @brief General math utils. 常用数学工具函数
* @author Spoon Guan
******************************************************************************
* Copyright (c) 2023 Team JiaoLong-SJTU
* All rights reserved.
******************************************************************************
*/
#ifndef UTILS_H
#define UTILS_H
#include <stdint.h>
namespace math {
float limit(float val, const float& min, const float& max);
float limitMin(float val, const float& min);
float limitMax(float val, const float& max);
float loopLimit(float val, const float& min, const float& max);
float sign(const float& val);
} // namespace math
#endif // UTILS_H