SLAM--Geometric jacobian of UR series.
2024-05-20 14:59:43
Catalog
- Obtain it by using Matlab
- Calculate the Jacobian directly by using C++
The geometric jacobian shows relationship between velocities of joints and velocities in TCP coordinate frame.
v = J q ˙ \bm v=\bm J \bm {\dot q} v=Jq˙
Obtain it by using Matlab
%% For UR robot arm
clc;clear all;
syms d1 a2 a3 d4 d5 d6
syms q1 q2 q3 q4 q5 q6
c1 = cos(q1);
s1 = sin(q1);
c2 = cos(q2);
s2 = sin(q2);
c3 = cos(q3);
s3 = sin(q3);
c4 = cos(q4);
s4 = sin(q4);
c5 = cos(q5);
s5 = sin(q5);
c6 = cos(q6);
s6= sin(q6);H01 = [c1 0 s1 0;s1 0 -c1 0;0 1 0 d1;0 0 0 1;];
H12 = [c2 -s2 0 a2*c2;s2 c2 0 a2*s2;0 0 1 0;0 0 0 1;];
H23 = [c3 -s3 0 a3*c3;s3 c3 0 a3*s3;0 0 1 0;0 0 0 1;];
H34 = [c4 0 s4 0;s4 0 -c4 0;0 1 0 d4;0 0 0 1;];
H45 = [c5 0 -s5 0;s5 0 c5 0;0 -1 0 d5;0 0 0 1;];
H56 = [c6 -s6 0 0;s6 c6 0 0;0 0 1 d6;0 0 0 1;];
Hstack = {H01,H12,H23,H34,H45,H56};
H = H01*H12*H23*H34*H45*H56;%H = simplify(H);
% Jpi = z_{i-1}×(pe-p_{i-1})
% z indicates the direction of each joint's Z axis.
% p indicates the position of each joint.
% the jacobian is [Jpi,Joi]' = [z_{i-1}×(pe-p_{i-1}), z_{i-1}]'
Z0 = [0; 0; 1;];
P0e = H(1:3,4);Ht = eye(4,4);
J = [];Z = Z0;
Ji = [cross(Z,P0e);Z];
J = [J, Ji];
for i=1:5Ht = Ht*Hstack{i};Z = Ht(1:3,3);Pie = P0e - Ht(1:3,4);Ji = [cross(Z,Pie);Z];J = [J, Ji];
endJ = simplify(J);
J = collect(J,d6);
The result is
>> JJ =[ (cos(q1)*cos(q5) + cos(q2 + q3 + q4)*sin(q1)*sin(q5))*d6 + d4*cos(q1) - a2*cos(q2)*sin(q1) - d5*sin(q2 + q3 + q4)*sin(q1) - a3*cos(q2)*cos(q3)*sin(q1) + a3*sin(q1)*sin(q2)*sin(q3), cos(q1)*sin(q5)*(cos(q2 + q3)*sin(q4) + sin(q2 + q3)*cos(q4))*d6 - cos(q1)*(a3*sin(q2 + q3) + a2*sin(q2) - d5*(cos(q2 + q3)*cos(q4) - sin(q2 + q3)*sin(q4))), sin(q2 + q3 + q4)*cos(q1)*sin(q5)*d6 - cos(q1)*(a3*sin(q2 + q3) - d5*cos(q2 + q3 + q4)), sin(q2 + q3 + q4)*cos(q1)*sin(q5)*d6 + d5*cos(q2 + q3 + q4)*cos(q1), (cos(q1)*cos(q2)*cos(q5)*sin(q3)*sin(q4) - cos(q1)*cos(q2)*cos(q3)*cos(q4)*cos(q5) - sin(q1)*sin(q5) + cos(q1)*cos(q3)*cos(q5)*sin(q2)*sin(q4) + cos(q1)*cos(q4)*cos(q5)*sin(q2)*sin(q3))*d6, 0]
[ (cos(q5)*sin(q1) - cos(q2 + q3 + q4)*cos(q1)*sin(q5))*d6 + d4*sin(q1) + a2*cos(q1)*cos(q2) + d5*sin(q2 + q3 + q4)*cos(q1) + a3*cos(q1)*cos(q2)*cos(q3) - a3*cos(q1)*sin(q2)*sin(q3), sin(q1)*sin(q5)*(cos(q2 + q3)*sin(q4) + sin(q2 + q3)*cos(q4))*d6 - sin(q1)*(a3*sin(q2 + q3) + a2*sin(q2) - d5*(cos(q2 + q3)*cos(q4) - sin(q2 + q3)*sin(q4))), sin(q2 + q3 + q4)*sin(q1)*sin(q5)*d6 - sin(q1)*(a3*sin(q2 + q3) - d5*cos(q2 + q3 + q4)), sin(q2 + q3 + q4)*sin(q1)*sin(q5)*d6 + d5*cos(q2 + q3 + q4)*sin(q1), (cos(q1)*sin(q5) - cos(q2)*cos(q3)*cos(q4)*cos(q5)*sin(q1) + cos(q2)*cos(q5)*sin(q1)*sin(q3)*sin(q4) + cos(q3)*cos(q5)*sin(q1)*sin(q2)*sin(q4) + cos(q4)*cos(q5)*sin(q1)*sin(q2)*sin(q3))*d6, 0]
[ 0, (sin(q2 + q3 + q4 - q5)/2 - sin(q2 + q3 + q4 + q5)/2)*d6 + a3*cos(q2 + q3) + a2*cos(q2) + d5*sin(q2 + q3 + q4), (sin(q2 + q3 + q4 - q5)/2 - sin(q2 + q3 + q4 + q5)/2)*d6 + a3*cos(q2 + q3) + d5*sin(q2 + q3 + q4), (sin(q2 + q3 + q4 - q5)/2 - sin(q2 + q3 + q4 + q5)/2)*d6 + d5*sin(q2 + q3 + q4), (- sin(q2 + q3 + q4 + q5)/2 - sin(q2 + q3 + q4 - q5)/2)*d6, 0]
[ 0, sin(q1), sin(q1), sin(q1), sin(q2 + q3 + q4)*cos(q1), cos(q5)*sin(q1) - cos(q2 + q3 + q4)*cos(q1)*sin(q5)]
[ 0, -cos(q1), -cos(q1), -cos(q1), sin(q2 + q3 + q4)*sin(q1), - cos(q1)*cos(q5) - cos(q2 + q3 + q4)*sin(q1)*sin(q5)]
[ 1, 0, 0, 0, -cos(q2 + q3 + q4), -sin(q2 + q3 + q4)*sin(q5)]Calculate the Jacobian directly by using C++
Now we have got the expressions of jacobian. Then, use c++ to design a function that can calculate the jacobian directly so that the computation could be reduced greatly making the speed of processing faster.
void jacobian(const double* q, double* J){double s1 = sin(*q), c1 = cos(*q); q++;double q23 = *q, q234 = *q, s2 = sin(*q), c2 = cos(*q); q++;double s3 = sin(*q), c3 = cos(*q); q23 += *q; q234 += *q; q++;double s4 = sin(*q), c4 = cos(*q); q234 += *q; q++;double s5 = sin(*q), c5 = cos(*q), q2345 = q234 + *q,q234_5 = q234 - *q; q++;double s6 = sin(*q), c6 = cos(*q);double s23 = sin(q23), c23 = cos(q23);double s234 = sin(q234), c234 = cos(q234);double s2345 = sin(q2345), s234_5 = sin(q234_5);*J = (c1*c5 + c234*s1*s5)*d6 + d4*c1 - a2*c2*s1 - d5*s234*s1 - a3*c2*c3*s1 + a3*s1*s2*s3;J++;*J = c1*s5*(c23*s4 + s23*c4)*d6 - c1*(a3*s23 + a2*s2 - d5*(c23*c4 - s23*s4)); J++;*J = s234*c1*s5*d6 - c1*(a3*s23 - d5*c234); J++;*J = s234*c1*s5*d6 + d5*c234*c1; J++;*J = (c1*c2*c5*s3*s4 - c1*c2*c3*c4*c5 - s1*s5 + c1*c3*c5*s2*s4 + c1*c4*c5*s2*s3)*d6; J++;*J = 0; J++;*J = (c5*s1 - c234*c1*s5)*d6 + d4*s1 + a2*c1*c2 + d5*s234*c1 + a3*c1*c2*c3 - a3*c1*s2*s3;J++;*J = s1*s5*(c23*s4 + s23*c4)*d6 - s1*(a3*s23 + a2*s2 - d5*(c23*c4 - s23*s4)); J++;*J = s234*s1*s5*d6 - s1*(a3*s23 - d5*c234); J++;*J = s234*s1*s5*d6 + d5*c234*s1; J++;*J = (c1*s5 - c2*c3*c4*c5*s1 + c2*c5*s1*s3*s4 + c3*c5*s1*s2*s4 + c4*c5*s1*s2*s3)*d6; J++;*J = 0; J++;*J = 0;J++;*J = (s234_5/2 - s2345/2)*d6 + a3*c23 + a2*c2 + d5*s234; J++;*J = (s234_5/2 - s2345/2)*d6 + a3*c23 + d5*s234; J++;*J = (s234_5/2 - s2345/2)*d6 + d5*s234; J++;*J = (- s2345/2 - s234_5/2)*d6; J++;*J = 0; J++;*J = 0; J++;*J = s1; J++;*J = s1; J++;*J = s1; J++;*J = s234 * c1; J++;*J = c5*s1 - c234*c1*s5; J++;*J = 0; J++;*J = -c1; J++;*J = -c1; J++;*J = -c1; J++;*J = s234 * s1; J++;*J = - c1*c5 - c234*s1*s5; J++;*J = 1; J++;*J = 0; J++;*J = 0; J++;*J = 0; J++;*J = -c234; J++;*J = -s234 * s5; J++;}
q q q represents the robot joints’ states, and J J J is the geometric jacobian matrix.
Example of utilizing:
#define ROBOT_DOF 6
...
//get joint states from ros topic
...
double Jr[ROBOT_DOF*ROBOT_DOF] = {0};
//calculate the jacobian in real-time.
jacobian(q,Jr);
//print the elements of jacobian.
for(int i=0;i<ROBOT_DOF;i++)printf("%1.6f %1.6f %1.6f %1.6f %1.6f %1.6f \n",Jr[i*6+0],Jr[i*6+1],Jr[i*6+2],Jr[i*6+3],Jr[i*6+4],Jr[i*6+5]);
SLAM--Geometric jacobian of UR series.相关推荐
- 傅里叶级数（Fourier Series）
1.傅里叶级数 笔记来源: 1.Intro to FOURIER SERIES: The Big Idea 2.How to Compute a FOURIER SERIES 3.The beauti ...
- MATLAB Robotics System Toolbox学习笔记（一）：一步一步建造一个机械臂
本文参考 MathWorks 中 Help Center 的 Build a Robot Step by Step ,并加以自己的理解 原网址:https://ww2.mathworks.cn/hel ...
- 机械臂中的四元素转为旋转矩阵_机器臂的运动学
机器人在这两年莫名其妙就成了热门话题了.在人们的想象之中,机器人智慧聪颖,无所不能,仿佛今天就要抢我饭碗,明天就要灭绝人类了.胡思乱想固然容易,不过想让机器人真的做到这些事,可就真是为难了我们这帮工程 ...
- Rigid Manipulators--Modelling建模--Kinematics运动学
一.Direct Kinematics正运动学 实质:关节空间->任务空间 方法: D-H方法: 四个参数中,lilil_{i}与αiαi\alpha_{i}的值是固定的,而didid_{i}与 ...
- 趣味三角——第9章——Zeno错失无穷小
第9章 如果Zeno仅需再认识到这一点 (Had Zeno Only Known This!) --无限多的数字之和可能具有有限的值 One, Two, Three-Infinity (1,2,3- ...
- First Estimate Jacobian (FEJ) 如何理解SLAM中的First Estimate Jacobian
First Estimate Jacobian (FEJ) 如何理解SLAM中的First Estimate Jacobian First Estimate Jacobian是Visual Inert ...
- 史上最简SLAM零基础解读(7) - Jacobian matrix(雅可比矩阵) → 理论分析与应用详解(Bundle Adjustment)
本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始 文末正下方中心提供了本人联系方式,点击本人照片即可显示WX→官方认证{\color{blue}{文末正下方中心}提供了本人 \co ...
- ICRA2021 SLAM方向论文汇总
ICRA 2021会议也已经开完了, 所以对原来总结SLAM方向相关的文章进行了重新整理和补全,由于总结比较广泛,大约有200多篇,本文对论文进行了分类,并给出了下载地址和开源代码地址,希望对一起做S ...
- ICRA2022 SLAM相关论文整理
目录 视觉SLAM相关 线.面.3Dfeature 多相机系统 VIO+GPS 其他 数据集 三维重建 语义SLAM Special sensor Lidar相关 视觉SLAM相关 线.面.3Dfea ...
最新文章
- javascript与浏览器学习（一）
- 德鲁克管理思想：管理的7大理论、43条原则，每一条都是精华
- Linux系统调用的实现机制分析
- android状态栏半透明灰色,Android7.0沉浸式状态栏蒙灰问题完美解决
- 为什么 RestTemplate 那么棒，看这篇就够了！
- 最简单易懂的对拍讲解
- nullnulle-人事管理系统-人事档案-变更管理-人员合同变更
- Cell.reuseIdentifier 指什么
- CCF-CSP 稀疏向量问题（2020-6）
- 11年瑞纳手动挡值多少钱_1个本科学历，值多少钱？
- excel 常用快捷键及小技巧
- 从薛定谔方程到K-S方程
- c语言写流水灯程序,用汇编和C语言 写流水灯程序
- 见信如晤::‘卷福’读信：我全心全意去拥抱您
- Nacos @RefreshScope 配置不生效问题
- 超级好用easyexcel插件
- 银行理财子公司蓄力布局A股；现金管理类理财产品整改加速
- 【AP】Robust multi-period portfolio selection(3)
- macbook air 单独安装纯净win7OS
- Spring4：AOP