利用罗德里格斯正逆公式，自己实现李群SO3(R),SE3(T)和李代数so3(Phi)(theta_n),se3(p,Phi)之间的转换

已于 2023-04-05 13:11:55 修改
阅读量475 收藏
点赞数
分类专栏： slam 文章标签： r语言 c++ 算法
于 2023-04-04 17:39:52 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_41253960/article/details/129940463
版权

  1. 特殊正交群R和Phi=theta_n之间的转换
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

  2. 特殊欧式群T和epsilon=(p, theta_n)之间的转换

在这里插入图片描述
3. 代码实现以上李群和李代数之间的转换计算

#include <iostream>
#include <eigen3/Eigen/Eigen>
#include <sophus/se3.hpp>
#include <cmath>
using namespace std;
class Lie_group_algebra
{
public:
    Lie_group_algebra(){}
    //特殊正交群 R 到 Phi=theta_n 的转换
    void R_2_Phi(const Eigen::Matrix3d& R, Eigen::Vector3d& Phi);
    //李代数Phi=theta_n 到特殊正交群R 的转换
    void Phi_2_R(const Eigen::Vector3d& Phi, Eigen::Matrix3d& R);
    //特殊欧式群T 到 epsilon=(p, Phi) 的转换
    void T_2_epsilon(const Eigen::Isometry3d& T, Eigen::Matrix<double, 6, 1>& epsilon);
    //李代数epsilon=(p, Phi) 到 特殊欧式群T 的转换
    void epsilon_2_T(const Eigen::Matrix<double,6,1>& epsilon, Eigen::Isometry3d& T);
};


int main(int argc, char** argv)
{
    Lie_group_algebra Lie;
    Eigen::Matrix3d R = Eigen::AngleAxisd( - 12 * M_PI, Eigen::Vector3d(4, 5, 7).normalized()).toRotationMatrix();
    Eigen::Vector3d t(11, 33, 99);
    //验证R 和 theta_n(Phi)之间的转换是否正确
    cout << "验证R 和 theta_n(Phi)之间的转换是否正确\n";
    cout << "原始R = \n" << R << endl;
    Eigen::Vector3d Phi;
    Lie.R_2_Phi(R, Phi);
    cout << "计算Phi = \n" << Phi << endl;
    Sophus::SO3d SO3_R(R);
    Eigen::Vector3d so3_Phi = SO3_R.log();
    cout << "验证so3_Phi = 利用Sophus的对数映射计算的, 理应等于Phi\n" << so3_Phi << endl;
    cout << "------------" << endl;
    Eigen::Matrix3d my_R;
    Lie.Phi_2_R(Phi, my_R);
    cout << "验证 my_R = 理应等于R\n" << my_R << endl;
    Sophus::SO3d Sophus_R = Sophus::SO3d::exp(Phi);
    cout << "Sophus_R = \n" << Sophus_R.matrix() << endl;
    //确实相等，R - my_R = 0
    cout << "R - my_R = 理应等于0矩阵\n" << R - my_R << endl;

    //验证T 和 (p,theta_n(Phi))之间的转换是否正确
    cout << "\n----------------------------------------------------";
    cout << "\n验证T 和 epsilon = (p, theta_n(Phi))之间的转换是否正确\n\n";
    Eigen::Isometry3d T(R);
    T.pretranslate(t);
    cout << "原始T = \n" << T.matrix() << endl;
    Eigen::Matrix<double,6,1> epsilon;
    Lie.T_2_epsilon(T, epsilon);
    cout << "计算epsilon = \n" << epsilon << endl;
    Sophus::SE3d SE3_T(R, t);
    Eigen::Matrix<double,6,1> se3_epsilon = SE3_T.log();
    cout << "验证se3_epsilon = 利用Sophus的对数映射计算的, 理应等于epsilon \n" << se3_epsilon << endl;
    Eigen::Isometry3d my_T = Eigen::Isometry3d::Identity();
    Lie.epsilon_2_T(epsilon, my_T);
    cout << "验证 my_T = 理应等于T \n" << my_T.matrix() << endl;
    Sophus::SE3d Sophus_T = Sophus::SE3d::exp(epsilon);
    cout << "Sophus_T = \n" << Sophus_T.matrix() << endl;
    //验证 my_T 是否等于 T
    cout << "T - my_T = 理应等于0矩阵 \n" << T.matrix() - my_T.matrix() << endl;
    return 0;
}
/// @brief 罗德里格斯公式的逆公式，对数映射，用 R 计算 Phi = theta*n  
/// 注意，当theta = 0 或 PI 时， 当=0时，R=I,任意n都满足，旋转轴n不唯一。当=PI时，
/// @param R 
/// @param Phi 
void Lie_group_algebra::R_2_Phi(const Eigen::Matrix3d& R, Eigen::Vector3d& Phi)
{
    //R = cos(theta)I + (1-cos(theta))nnT + sin(theta)n^
    //trace(R) = 3cos(theta) + 1 - cos(theta)
    //cos(theta) = (trace(R) - 1) / 2
    double cos_theta = (R.trace() - 1.0) / 2;
    // 这样的话，theta一定在[0,PI]内了
    double theta = acos(cos_theta); 
    //注意|cos_theta| = 1的情况，此时sin_theta = 0
    double sin_theta = sqrt(1.0 - cos_theta * cos_theta);
    //根据 n^ = (R - RT) / (2*sin(theta)) 求n; sin_theta = 0时，不能这么求
    if(fabs(cos_theta - 1.0) < 0.000001) //用==1判断浮点数相等不太好
    {
        cout << "Situation: theta = 0\n";
        //theta = 0,n任意
        Eigen::Vector3d n(1,0,0);
        cout << "theta = " << theta << endl;
        cout << "n不唯一, 任意皆满足, 这里给n = \n" << n << endl;
        Phi = theta * n;
        return;
    }
    else if(fabs(cos_theta + 1.0) < 0.000001) //==-1
    {
        cout << "Situation: theta = PI\n";
        //theta = PI, R = -I + 2 * n*nT, 这时候n和-n都满足，因为是旋转半圈，正负方向都对
        //根据Rn = n, 求得的不是n就是-n
        //根据 Rn = n 求n;
        //n为R特征值1对应的特征向量
        Eigen::EigenSolver<Eigen::Matrix3d> es(R);
        //特征值
        Eigen::Vector3d eigen_values = es.pseudoEigenvalueMatrix().diagonal();
        // cout << "eigen_values = \n" << eigen_values << endl;
        //特征向量
        Eigen::Matrix3d eigen_vectors = es.pseudoEigenvectors();
        // cout << "eigen_vectors = \n" << eigen_vectors << endl;
        //特征值里等于1的位置设置为0,其他元素都大于0
        Eigen::Vector3d is_one = (eigen_values - Eigen::Vector3d::Ones()).cwiseAbs();
        // cout << "is_one = \n" << is_one << endl;
        //第k个特征值为1
        Eigen::Vector3d::Index k;
        is_one.minCoeff(&k);
        // cout << "k = " << k << endl;
        //n 就是第k列特征向量
        Eigen::Vector3d n = eigen_vectors.block<3,1>(0,k);
        n = -n;  //todo 这里要不要都可以，因为当theta=PI时，n和-n都是对的旋转轴
        cout << "theta = " << theta << endl;
        cout << "此时正负n皆可 n = \n" << n << endl;
        n.normalize();
        Phi = theta * n;
        return;
    }
    else
    {
        //一般情况, 根据 n^ = (R - RT) / (2*sin(theta)) 求n
        Eigen::Matrix3d n_hat = (R - R.transpose()) / (2 * sin_theta);
        // cout << "n_hat = \n" << n_hat << endl;
        Eigen::Vector3d n(-n_hat(1,2), n_hat(0,2), -n_hat(0,1));
        cout << "theta = " << theta << endl;
        cout << "n = \n" << n << endl;
        Phi = theta * n;
    }
}
/// @brief 罗德里格斯公式，指数映射，R = cos(theta)I + (1-cos(theta))nnT + sin(theta)n^
/// @param Phi 
/// @param R 
void Lie_group_algebra::Phi_2_R(const Eigen::Vector3d& Phi, Eigen::Matrix3d& R)
{
    double theta = Phi.norm();
    Eigen::Vector3d n = Phi.normalized();
    //R = cos(theta)I + (1-cos(theta))nnT + sin(theta)n^
    R.setZero();
    R(1,2) = -n(0); R(0,2) = n(1); R(0,1) = -n(2);
    R(2,1) = n(0); R(2,0) = -n(1); R(1,0) = n(2);
    //sin(theta)n^
    R = R * sin(theta);
    R = R + cos(theta) * Eigen::Matrix3d::Identity() + (1.0 - cos(theta)) * n * n.transpose();
}

/// @brief SE3 到 se3 的对数映射， T 到 epsilon
/// @param T 
/// @param epsilon 
void Lie_group_algebra::T_2_epsilon(const Eigen::Isometry3d& T, Eigen::Matrix<double, 6, 1>& epsilon)
{
    //epsilon = (p, Phi)
    //t = Jp  所以 p = J.inverse() * t
    //J = sin(theta)/theta * I + (1 - sin(theta)/theta) nnT + (1-cos(theta))/theta * n^
    Eigen::Matrix3d R = T.rotation();
    Eigen::Vector3d t = T.translation();
    Eigen::Vector3d Phi;
    R_2_Phi(R, Phi);
    double theta = Phi.norm();
    //这里要注意 theta=0 的情况
    if(fabs(theta) < 0.000001) //theta == 0
    {
        //!!!怎么计算p
        //根据Sophus库来开，theta=0时候,t=p
        Eigen::Vector3d p = t;
        epsilon.block<3,1>(0,0) = p;
        epsilon.block<3,1>(3,0) = Phi;
        return;
    }
    Eigen::Vector3d n = Phi.normalized();
    Eigen::Matrix3d J = Eigen::Matrix3d::Zero();
    J(1,2) = -n(0); J(0,2) = n(1); J(0,1) = -n(2);
    J(2,1) = n(0); J(2,0) = -n(1); J(1,0) = n(2);
    J = J *(1.0 - cos(theta)) / theta;
    J = J + Eigen::Matrix3d::Identity() * sin(theta) / theta + (1.0 - sin(theta)/theta) * n * n.transpose();
    Eigen::Vector3d p = J.inverse() * t;
    epsilon.block<3,1>(0,0) = p;
    epsilon.block<3,1>(3,0) = Phi;
}

/// @brief 指数映射se3到SE3
/// @param epsilon 
/// @param T 
void Lie_group_algebra::epsilon_2_T(const Eigen::Matrix<double,6,1>& epsilon, Eigen::Isometry3d& T)
{
    Eigen::Vector3d p = epsilon.block<3,1>(0,0);
    Eigen::Vector3d Phi = epsilon.block<3,1>(3,0);
    double theta = Phi.norm();
    Eigen::Vector3d n = Phi.normalized();
    Eigen::Matrix3d R;
    //罗德里格斯公式
    Phi_2_R(Phi, R);
    //t = Jp
    Eigen::Matrix3d J = Eigen::Matrix3d::Zero();
    //!同样，当theta = 0时， 怎么求J, 进而求t = Jp
    if(fabs(theta) < 0.000001) //theta == 0
    {
        //theta = 0时, 怎么求J, t=Jp
        //根据Sophus库来开，theta=0时候,t=p
        Eigen::Vector3d t = p;
        T = Eigen::Isometry3d::Identity();
        T.rotate(R);
        T.pretranslate(t);
        return;
    }
    J(1,2) = -n(0); J(0,2) = n(1); J(0,1) = -n(2);
    J(2,1) = n(0); J(2,0) = -n(1); J(1,0) = n(2);
    J = J *(1.0 - cos(theta)) / theta;
    J = J + Eigen::Matrix3d::Identity() * sin(theta) / theta + (1.0 - sin(theta)/theta) * n * n.transpose();
    Eigen::Vector3d t = J * p;
    T = Eigen::Isometry3d::Identity();
    T.rotate(R);
    T.pretranslate(t);
}
awhuter
关注
  • 0
    点赞
  • 踩
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
李群与李代间的运算代码.zip
05-13
基于C++语言，建立工程文件，编写李群与李代间的运算代码
ADCS_MASTER:四元数、欧拉角和罗德里格斯参数变换库-matlab开发
05-31
四元数、欧拉角和罗德里格斯参数变换库 以下列表说明了问题变量theta：旋转角度 [rad] e: 轴向量 [e1;e2;e3] R：关联旋转矩阵 [3,3] x: 第一个欧拉角 (x, roll,theta) y：第二欧拉角（y，螺距，phi） z: 第三欧拉角 (z, yaw,psi) q：关联四元数 [q0;q1;q2;q3] gp: 罗德里格斯参数 [gp1;gp2;gp3]; pp：修改后的罗德里格斯参数 [pp1;pp2;pp3]; v：要旋转的向量 [v1;v2;v3] theta_v：关联向量角度 [x,y,z]
SLAM中的李群和李代数
qq_44711190的博客
09-30 792
群 定义 群(Group)是一种集合加上一种运算的代数结构，记作(A,⋅)。其中A代表集合，⋅是定义在该集合上的二元运算。那么，如果这个运算满足以下几个条件，则称G=(A,⋅)为群 封闭性 结合律 幺元 逆 李群的定义 李群是指具有光滑连续性质的群 特殊正交群 特殊正交群SO(n) 也就是所谓的旋转矩阵群，其中SO(2)和SO(3)最为常见。正式的记法是： \[SO(n)=\{R∈\R_{n...
视觉SLAM十四讲学习笔记——ch4李群与李代数代码实践
路拾遗的博客
03-31 982
文章目录4 李群和李代数4.1 基础知识---群4.2 李群与李代数4.2代码实践4.2.1 Sophus的基本使用方法4.2.2 例子:评估轨迹的误差4.2.3 章节小结 4 李群和李代数 4.1 基础知识—群 4.2 李群与李代数 李代数:与李群对应的一种的结构,位于向量空间,通常记作so(3),se(3). 具体李群与李代数基础可参考博客 SLAM学习——李群与李代数 理论推导部分可参考博客:视觉SLAM十四讲CH4代码解析及课后习题详解 4.2代码实践 4.2.1 Sophus的基本使用方
机器人学——李群和李代数
weixin_46025237的博客
10-25 1994
1.什么是群 群（Group），是一种集合加上一种运算组成的结构。假如我们把集合记作G，运算记作∙\begin{array}{l}\bullet\\\end{array}∙​，它要求集合和运算满足下面几个条件： 闭合律：任意a,b∈G,则a·b =c∈G 结合律：所有a,b,c∈G，(a·b)·c=a·(b·c) 单位律：存在单位元I∈G,使得I·g=g·I=g 逆：任意元素g,存在一个元素h∈G,使得h·g=g·h=I 特殊正交群和特殊欧式群就是一种群 旋转矩阵构成特殊正交群SO 位姿变换矩阵构成了
SE(3) 上的李代数求导（推导过程）
暮落晨风的博客
03-17 2139
SE(3) 上的李代数求导（推导过程） 变量基本形式 ppp 是一个三维向量 ，其齐次形式为 p=[p1](0) p = \begin{bmatrix} p \\ 1 \end{bmatrix} \tag{0} p=[p1​](0) TTT 是一个变换矩阵，其形式 T=[Rt0T1](1) T = \begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix} \tag{1} T=[R0T​t1​](1) ξ\xiξ 是一六维向量 ，其
李群的求导,附带matlab代码
chenguowen21的专栏
10-03 2766
李群资料： https://en.wikipedia.org/wiki/3D_rotation_group      http://www.ethaneade.com/latex2html/lie_groups/lie_groups.html http://www.ethaneade.com/latex2html/lie_groups/lie_groups.html https://git...
Eigen so3 and se3
博客
08-02 2970
该代码源于《视觉SLAM十四讲》 Eigen 中Geometry （几何模块） 主要操作有 so3 se3 旋转向量和旋转矩阵转换 以及 增量扰动模型更新 该页代码用于日后查找方便 （添加了部分笔记） #include &lt;iostream&gt; #include &lt;cmath&gt; using namespace std; #include &lt;Eigen/Core&...
SLAM十四讲——跑通李群与李代数代码（解决Sophus库的问题）
Williamcsj的博客
11-06 1259
一、安装准备 1、Sophus库分为模板Sophus库和非模板Sophus库。 2、视觉slam十四讲李群与李代数这部分代码采用的是非模板的Sophus库。 3、如果你在运行这部分代码时遇到了问题，那就先将已经安装或存在的Sophus文件删除干净。具体步骤： 第一步 打开计算机： 第二步 搜索Sophus： 第三步 将Sophus文件夹全部删除： 说明：有些文件夹是只读文件夹，需要采用命令行删除，具体删除命令可以参考： Ubuntu ——移动、复制、删除文件夹（文件）的简易方法 4、打开终端，将终端执
手动实现李群SE(3)到李代数se(3)之间的对指数运算关系
weixin_41855010的博客
07-22 853
我们可以推导出以下公式来进行李群和李代数之间的对应转换计算。 已知ω⃗\vec\omegaω和v⃗\vec vv，求RRR和ppp，从而获得TTT，对应T=exp(E)T= exp(E)T=exp(E)，其中E=upgrade([ω⃗,v⃗])E=upgrade([\vec \omega, \vec v])E=upgrade([ω,v])： 已知TTT，可以从其中提取出RRR和ppp，进而可以计算出ω⃗\vec \omegaω和v⃗\vec vv，即E=log(T)E=log(T)E=log(T)，[ω⃗
本文详细介绍了罗德里格斯公式的推导过程
09-20
罗德里格斯公式的推导过程，网上的推导过程乱七八糟，经常会误导大多数的读者，这是在进行项目的过程之中进行重新推导了一遍。
欧拉公式求圆周率的matlab代码-rotationconverter:简单HTML+JS页面可在不同的3D旋转格式，四元数，罗德里格斯角等之
05-23
欧拉公式求长期率的matlab代码3D旋转转换器 一个简单HTML + JavaScript页面，可在不同的3D旋转格式，四元数，罗德里格斯角，欧拉角等之间进行转换。 直接运行以在线转换旋转 依存关系 大部分转换是使用（也是MIT许可...
罗德里格斯公式推导.pdf
07-25
罗德里格斯公式推导.pdf
两种形式的罗德里格斯公式推导.docx
05-31
两种形式的罗德里格斯公式推导.docx
用R软件寻找最优子集，应用回归分析，逐步回归实例，前进法，后退法
2201_75305489的博客
04-27 1061
x1 x2 x3 x4 x5 x6 x7 x8 x9 调整R平方。用R软件寻找最优子集，
R语言高级数据管理
袁震的博客
04-27 601
绝对值函数abs(x)sqrt(x) 开平方根不小于某个数的最小整数ceiling(x)不大于某个数的最大整数floor(x)四舍五入round(x)sin(x)cos(x)log(x)
R语言贝叶斯方法在生态环境领域中的应用
zkzhzy的博客
04-27 1152
回归及结构方程模型概述及数据探索；R和Rstudio简介及入门和作图基础；R语言数据清洗-tidyverse包；贝叶斯回归与混合效应模型；贝叶斯空间自相关、时间自相关及系统发育相关数据分析；贝叶斯非线性数据分析;贝叶斯结构方程模型及统计结果作图等。
应用回归分析，R语言，多元回归模型总结(上)
最新发布
2201_75305489的博客
05-03 867
【代码】应用回归分析，R语言，多元回归模型总结(上)
OLTP和OLAP，StarRocks数据库，TiDB数据库
zhengzaifeidelushang的博客
04-30 1111
OLTP和OLAP，StarRocks数据库，TiDB数据库
你可以帮我写一个使用罗德里格斯公式求解七自由度机械臂逆解的MATLAB程序吗
05-27
当然可以，以下是使用罗德里格斯公式求解七自由度机械臂逆解的MATLAB程序。请注意，这是一个简单的示例程序，需要根据您的具体情况进行修改和调整。 ```matlab % 七自由度机械臂逆解程序（使用罗德里格斯公式） % 机械臂参数 L1 = 1; % 第一段臂长 L2 = 2; % 第二段臂长 L3 = 3; % 第三段臂长 L4 = 4; % 第四段臂长 L5 = 5; % 第五段臂长 L6 = 6; % 第六段臂长 L7 = 7; % 第七段臂长 % 目标末端执行器位姿 Rd = [1, 0, 0; 0, 1, 0; 0, 0, 1]; % 目标末端执行器旋转矩阵 pd = [1; 2; 3]; % 目标末端执行器位置向量 % 初始关节角度 q0 = [0; 0; 0; 0; 0; 0; 0]; % 迭代求解逆解 q = q0; for i = 1:100 % 迭代次数 % 计算当前末端执行器位姿 T = forward_kinematics(q, L1, L2, L3, L4, L5, L6, L7); R = T(1:3, 1:3); p = T(1:3, 4); % 计算误差 eR = 0.5 * (Rd' * R - R' * Rd); % 旋转矩阵误差 ep = pd - p; % 位置向量误差 % 计算雅可比矩阵 J = jacobian(q, L1, L2, L3, L4, L5, L6, L7); % 计算关节角度增量 dq = pinv(J) * [ep; eR(1, 3); eR(2, 1); eR(3, 2)]; % 更新关节角度 q = q + dq; end % 输出逆解 disp(q); % 正向运动学函数 function T = forward_kinematics(q, L1, L2, L3, L4, L5, L6, L7) T01 = dh_transform(0, pi/2, 0, q(1)); T12 = dh_transform(L1, 0, 0, q(2)); T23 = dh_transform(L2, 0, 0, q(3)); T34 = dh_transform(L3, 0, 0, q(4)); T45 = dh_transform(L4, pi/2, 0, q(5)); T56 = dh_transform(L5, -pi/2, 0, q(6)); T67 = dh_transform(L6, 0, 0, q(7)); T7E = eye(4); T7E(1:3, 1:3) = rotx(-pi/2); T = T01 * T12 * T23 * T34 * T45 * T56 * T67 * T7E; end % DH参数转换函数 function T = dh_transform(a, alpha, d, theta) T = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta); sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta); 0, sin(alpha), cos(alpha), d; 0, 0, 0, 1]; end % 旋转矩阵绕x轴旋转函数 function R = rotx(theta) R = [1, 0, 0; 0, cos(theta), -sin(theta); 0, sin(theta), cos(theta)]; end % 旋转矩阵绕y轴旋转函数 function R = roty(theta) R = [cos(theta), 0, sin(theta); 0, 1, 0; -sin(theta), 0, cos(theta)]; end % 旋转矩阵绕z轴旋转函数 function R = rotz(theta) R = [cos(theta), -sin(theta), 0; sin(theta), cos(theta), 0; 0, 0, 1]; end % 雅可比矩阵计算函数 function J = jacobian(q, L1, L2, L3, L4, L5, L6, L7) T01 = dh_transform(0, pi/2, 0, q(1)); T12 = dh_transform(L1, 0, 0, q(2)); T23 = dh_transform(L2, 0, 0, q(3)); T34 = dh_transform(L3, 0, 0, q(4)); T45 = dh_transform(L4, pi/2, 0, q(5)); T56 = dh_transform(L5, -pi/2, 0, q(6)); T67 = dh_transform(L6, 0, 0, q(7)); T7E = eye(4); T7E(1:3, 1:3) = rotx(-pi/2); T02 = T01 * T12; T03 = T02 * T23; T04 = T03 * T34; T05 = T04 * T45; T06 = T05 * T56; T07 = T06 * T67; z0 = [0; 0; 1]; z1 = T01(1:3, 3); z2 = T02(1:3, 3); z3 = T03(1:3, 3); z4 = T04(1:3, 3); z5 = T05(1:3, 3); z6 = T06(1:3, 3); p0 = [0; 0; 0]; p1 = T01(1:3, 4); p2 = T02(1:3, 4); p3 = T03(1:3, 4); p4 = T04(1:3, 4); p5 = T05(1:3, 4); p6 = T06(1:3, 4); p7 = T07(1:3, 4); J = [cross(z0, p7-p0), cross(z1, p7-p1), cross(z2, p7-p2), cross(z3, p7-p3), cross(z4, p7-p4), cross(z5, p7-p5), cross(z6, p7-p6); z0, z1, z2, z3, z4, z5, z6]; end ``` 该程序使用罗德里格斯公式迭代求解七自由度机械臂的逆解，其中包括正向运动学函数、DH参数转换函数、旋转矩阵绕x轴旋转函数、旋转矩阵绕y轴旋转函数、旋转矩阵绕z轴旋转函数、雅可比矩阵计算函数。您可以根据自己的具体机械臂参数和需求进行修改和调整。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值