From df4892df01ceefb0c6875ca3d616d82a37f68ae7 Mon Sep 17 00:00:00 2001 From: HshineO <1491134420@qq.com> Date: Sun, 28 Apr 2024 19:22:24 +0800 Subject: [PATCH] =?UTF-8?q?=E7=A7=BB=E6=A4=8D=E4=B8=8A=E4=BA=A4cpp?= =?UTF-8?q?=E6=9C=BA=E5=99=A8=E4=BA=BA=E5=AD=A6=E5=BA=93?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 2 +- application/gimbal/{gimbal.c => gimbal.cpp} | 90 +---- application/gimbal/gimbal.h | 7 + modules/algorithm/myrobotics.c | 120 ++++++ modules/algorithm/myrobotics.h | 21 + modules/robotics/matrix.cpp | 24 ++ modules/robotics/matrix.h | 259 +++++++++++++ modules/robotics/robotics.cpp | 340 ++++++++++++++++ modules/robotics/robotics.h | 407 ++++++++++++++++++++ modules/robotics/utils.cpp | 56 +++ modules/robotics/utils.h | 27 ++ 11 files changed, 1277 insertions(+), 76 deletions(-) rename application/gimbal/{gimbal.c => gimbal.cpp} (53%) create mode 100644 modules/algorithm/myrobotics.c create mode 100644 modules/algorithm/myrobotics.h create mode 100644 modules/robotics/matrix.cpp create mode 100644 modules/robotics/matrix.h create mode 100644 modules/robotics/robotics.cpp create mode 100644 modules/robotics/robotics.h create mode 100644 modules/robotics/utils.cpp create mode 100644 modules/robotics/utils.h diff --git a/CMakeLists.txt b/CMakeLists.txt index e24458e..d553140 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.cpp similarity index 53% rename from application/gimbal/gimbal.c rename to application/gimbal/gimbal.cpp index e1a070f..9ad3535 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.cpp @@ -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); diff --git a/application/gimbal/gimbal.h b/application/gimbal/gimbal.h index 0bcc37e..499d5c8 100644 --- a/application/gimbal/gimbal.h +++ b/application/gimbal/gimbal.h @@ -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 \ No newline at end of file diff --git a/modules/algorithm/myrobotics.c b/modules/algorithm/myrobotics.c new file mode 100644 index 0000000..e525a5a --- /dev/null +++ b/modules/algorithm/myrobotics.c @@ -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); +} \ No newline at end of file diff --git a/modules/algorithm/myrobotics.h b/modules/algorithm/myrobotics.h new file mode 100644 index 0000000..16bba01 --- /dev/null +++ b/modules/algorithm/myrobotics.h @@ -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 diff --git a/modules/robotics/matrix.cpp b/modules/robotics/matrix.cpp new file mode 100644 index 0000000..51fc95a --- /dev/null +++ b/modules/robotics/matrix.cpp @@ -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; +} diff --git a/modules/robotics/matrix.h b/modules/robotics/matrix.h new file mode 100644 index 0000000..f6d07f7 --- /dev/null +++ b/modules/robotics/matrix.h @@ -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 +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 + 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 + Matrixf block(const int& start_row, const int& start_col) { + Matrixf 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 +Matrixf<_rows, _cols> zeros(void) { + float data[_rows * _cols] = {0}; + return Matrixf<_rows, _cols>(data); +} +// Ones matrix +template +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 +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 +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 +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 diff --git a/modules/robotics/robotics.cpp b/modules/robotics/robotics.cpp new file mode 100644 index 0000000..3a73e3b --- /dev/null +++ b/modules/robotics/robotics.cpp @@ -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(); +} diff --git a/modules/robotics/robotics.h b/modules/robotics/robotics.h new file mode 100644 index 0000000..d5f8839 --- /dev/null +++ b/modules/robotics/robotics.h @@ -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 +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 diff --git a/modules/robotics/utils.cpp b/modules/robotics/utils.cpp new file mode 100644 index 0000000..1c4b42b --- /dev/null +++ b/modules/robotics/utils.cpp @@ -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; +} + diff --git a/modules/robotics/utils.h b/modules/robotics/utils.h new file mode 100644 index 0000000..1515eb6 --- /dev/null +++ b/modules/robotics/utils.h @@ -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 + +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