计算机器人自由度的公式,六自由度机器人Jacobian(雅克比)矩阵计算类!
作者:想飞的猪
说明:MLGetIdentityMat为获得单位矩阵函数
MLMatMulti为矩阵相乘函数
和OpenCV求逆矩阵函数cvInvert没有给出请大家自己写一下!很简单的!
typedef struct
RobotJacobian6
{
//变量!
//各关节传递矩阵!
union
{
struct
{
double
AMat[6][4][4];
}; double A0to1[4][4];
double A1to2[4][4];
double A2to3[4][4];
double A3to4[4][4];
double A4to5[4][4];
double A5to6[4][4];
};
union
{
struct
{
double
TMat[6][4][4];
};
struct {
double
T0to6[4][4];
double
T1to6[4][4];
double
T2to6[4][4];
double
T3to6[4][4];
double
T4to6[4][4];
double
T5to6[4][4];
};
};
//末端位姿!
double EndPose[4][4];
//D-H参数表!
double DHParam[6][4];//顺序为:Angle d_L a_L
a_A!
//雅克比矩阵!
double EndJacobian[6][6];
//逆雅克比矩阵!
double EndInvJacobian[6][6];
//基坐标的笛卡尔微分运动到末端坐标的传递矩阵!
double JBasetoEnd[6][6];
double
T_1to6[4][4];//该矩阵的姿态与基坐标一致,位置与末端坐标一致!
//以便可以按照基坐标进行平动和绕基坐标轴方向转动!
double mInput[6]; //输入!
double mOutput[6];//输出!
int mMode;
void GetAMat()
{
for (int
i=0;i<6;i++)
{
MLGetDHTransMat(AMat[i],DHParam[i][0],DHParam[i][1],DHParam[i][2],DHParam[i][3]);
}
}
void
GetTMat()
{
MLGetIdentityMat(T5to6);
MLMatMulti(AMat[5],T5to6);
MLMatMulti(AMat[4],T5to6,T4to6);MLMatMulti(AMat[3],T4to6,T3to6);
MLMatMulti(AMat[2],T3to6,T2to6);
MLMatMulti(AMat[1],T2to6,T1to6);
MLMatMulti(AMat[0],T1to6,T0to6);
}
void
UpdateAngle(double
Angles[6])//Angles为弧度!
{for (int
i=0;i<6;i++)
{
DHParam[i][0]=Angles[i];
}
GetAMat();
GetEndPose();
GetTMat(); GetEndJacobian();
GetEndInvJacobian();
GetJBasetoEnd();
}
void Inti(double DHparameter[6][4])
{
for (int
i=0;i<6;i++)
{
for (int
j=0;j<4;j++)
{
DHParam[i][j]=DHparameter[i][j];
}
}
GetAMat();
GetEndPose();
GetTMat();
GetEndJacobian();
GetEndInvJacobian();
GetJBasetoEnd();
for
(i=0;i<6;i++)
{
mInput[i]=0;
mOutput[i]=0;
}
mMode=BASE;
}
void
GetEndPose()
{
MLGetIdentityMat(EndPose);
for (int
i=0;i<6;i++)
{
MLMatMulti(AMat[5-i],EndPose);
}
}
void
GetEndJacobian()
{
for (int
i=0;i<6;i++)
{
EndJacobian[0][i]=-1*TMat[i][Xx][Nn]*TMat[i][Yy][Pp]+TMat[i][Yy][Nn]*TMat[i][Xx][Pp];
EndJacobian[1][i]=-1*TMat[i][Xx][Oo]*TMat[i][Yy][Pp]+TMat[i][Yy][Oo]*TMat[i][Xx][Pp];
EndJacobian[2][i]=-1*TMat[i][Xx][Aa]*TMat[i][Yy][Pp]+TMat[i][Yy][Aa]*TMat[i][Xx][Pp];
EndJacobian[3][i]=TMat[i][Zz][Nn];
EndJacobian[4][i]=TMat[i][Zz][Oo];
EndJacobian[5][i]=TMat[i][Zz][Aa];
}
}
void
GetEndInvJacobian()
{
double Data1[36];
CvMat Mat1 = cvMat(
6,6,CV_64FC1,Data1);
double
Data2[36];
CvMat Mat2 = cvMat(
6,6,CV_64FC1,Data2);
for (int
i=0;i<6;i++)
{
for (int
j=0;j<6;j++)
{
cvmSet(&Mat1,i,j,EndJacobian[i][j]);
}
}
cvInvert(&Mat1,&Mat2,CV_SVD);
for
(i=0;i<6;i++)
{
for (int
j=0;j<6;j++)
{
EndInvJacobian[i][j]=cvmGet(&Mat2,i,j);
}
}
}
void
EndOutput(double Input[6], double Output[6])//Output为角速度!
{
MLMatMulti_3(EndInvJacobian,Input,Output);
}
void
GetJBasetoEnd()
{
double TransMat[4][4];
MLGetIdentityMat(TransMat);
TransMat[0][3]=-1*EndPose[0][3];
TransMat[1][3]=-1*EndPose[1][3];
TransMat[2][3]=-1*EndPose[2][3];
MLMatMulti(TransMat,EndPose,T_1to6);
JBasetoEnd[0][0]=T_1to6[Xx][Nn];
JBasetoEnd[0][1]=T_1to6[Yy][Nn];
JBasetoEnd[0][2]=T_1to6[Zz][Nn];
JBasetoEnd[1][0]=T_1to6[Xx][Oo];
JBasetoEnd[1][1]=T_1to6[Yy][Oo];
JBasetoEnd[1][2]=T_1to6[Zz][Oo];
JBasetoEnd[2][0]=T_1to6[Xx][Aa];
JBasetoEnd[2][1]=T_1to6[Yy][Aa];
JBasetoEnd[2][2]=T_1to6[Zz][Aa];
for (int
i=3;i<6;i++)
{
for (int
j=0;j<3;j++)
{
JBasetoEnd[i][j]=0;
}
}
JBasetoEnd[3][3]=T_1to6[Xx][Nn];
JBasetoEnd[3][4]=T_1to6[Yy][Nn];
JBasetoEnd[3][5]=T_1to6[Zz][Nn];
JBasetoEnd[4][3]=T_1to6[Xx][Oo];
JBasetoEnd[4][4]=T_1to6[Yy][Oo];
JBasetoEnd[4][5]=T_1to6[Zz][Oo];
JBasetoEnd[5][3]=T_1to6[Xx][Aa];
JBasetoEnd[5][4]=T_1to6[Yy][Aa];
JBasetoEnd[5][5]=T_1to6[Zz][Aa];
JBasetoEnd[0][3]=T_1to6[Yy][Pp]*T_1to6[Zz][Nn]-T_1to6[Zz][Pp]*T_1to6[Yy][Nn];//(P×N)x
JBasetoEnd[0][4]=T_1to6[Zz][Pp]*T_1to6[Xx][Nn]-T_1to6[Xx][Pp]*T_1to6[Zz][Nn];//(P×N)y
JBasetoEnd[0][5]=T_1to6[Xx][Pp]*T_1to6[Yy][Nn]-T_1to6[Yy][Pp]*T_1to6[Xx][Nn];//(P×N)z
JBasetoEnd[1][3]=T_1to6[Yy][Pp]*T_1to6[Zz][Oo]-T_1to6[Zz][Pp]*T_1to6[Yy][Oo];//(P×O)x
JBasetoEnd[1][4]=T_1to6[Zz][Pp]*T_1to6[Xx][Oo]-T_1to6[Xx][Pp]*T_1to6[Zz][Oo];//(P×O)y
JBasetoEnd[1][5]=T_1to6[Xx][Pp]*T_1to6[Yy][Oo]-T_1to6[Yy][Pp]*T_1to6[Xx][Oo];//(P×O)z
JBasetoEnd[2][3]=T_1to6[Yy][Pp]*T_1to6[Zz][Aa]-T_1to6[Zz][Pp]*T_1to6[Yy][Aa];//(P×A)x
JBasetoEnd[2][4]=T_1to6[Zz][Pp]*T_1to6[Xx][Aa]-T_1to6[Xx][Pp]*T_1to6[Zz][Aa];//(P×A)y
JBasetoEnd[2][5]=T_1to6[Xx][Pp]*T_1to6[Yy][Aa]-T_1to6[Yy][Pp]*T_1to6[Xx][Aa];//(P×A)z
}
void
BaseOutput(double BaseInput[6], double
Output[6])//Output为角速度!
{
double EndInput[6];
MLMatMulti_3(JBasetoEnd,BaseInput,EndInput);
EndOutput(EndInput,Output);
}
void
SetInput(double Input[6])
{
for (int
i=0;i<6;i++)
{
mInput[i]=Input[i];
}
}
void SetMode(int
mode)
{
mMode=mode;
}
void
GetOutput(int mode=BASE)
{
int i=0;
int j=0;
switch(mode)
{
case eND:
EndOutput(mInput,mOutput);
break;
case BASE:
BaseOutput(mInput,mOutput);
break;
}
}
RobotJacobian6()
{
}
RobotJacobian6(double
DHparameter[6][4])
{
Inti(DHparameter);
}
}MLJacobian;
计算机器人自由度的公式,六自由度机器人Jacobian(雅克比)矩阵计算类!相关推荐
- 班尼机器人维修方法_六轴机器人常见故障和修理方法
康道昊威六轴机器人常见故障和修理方法,为何越来越多的企业使用六轴机器人,六轴关节机器人. 一.六轴机器人常见故障和修理方法 1.先清洁后维修 对污染较重的电气设备,先对其按钮.接线点.接触点进行清洁, ...
- 六轴机器人光机_六轴机器人主要用到哪些传感器?
康道智能解说六轴机器人主要用到哪些传感器? 一.六轴机器人主要用到哪些传感器? 1.视觉传感器 常用的计算机视觉方案也有很多种,比如双目视觉,基于TOF的深度相机,基于结构光的深度相机等. 基于结构光 ...
- 【机器人学习】UR六自由度机器人运动学分析与轨迹规划(三次多项式、五次多项式、过渡)
https://download.csdn.net/download/yjw0911/85451711下载链接 六轴机械臂本体由6个可重组的关节模组.连接部件.底座.末端部件组成,如下图所示,定义底部 ...
- 【Matlab 六自由度机器人】定义标准型及改进型D-H参数建立机器人模型(附MATLAB建模代码)
Matlab建立六自由度机器人模型 近期更新 前言 1.Matlab机器人工具箱 2.研究对象-六自由度机器人 正文 一.D-H参数(Denavit–Hartenberg parameters) 1. ...
- 六足机器人的实现原理
缘由: 在自然界和人类社会中存在一些人类无法到达的地方和可能危及人类生命的特殊场合.如行星表面.灾难发生矿井.防灾救援和反恐斗争等,对这些危险环境进行不断地探索和研究,寻求一条解决问题的可行途径成为科 ...
- STM32毕业设计——基于STM32+JAVA+Android的六足机器人控制系统设计与实现(毕业论文+程序源码)——六足机器人控制系统
基于STM32+JAVA+Android的六足机器人控制系统设计与实现(毕业论文+程序源码) 大家好,今天给大家介绍基于STM32+JAVA+Android的六足机器人控制系统设计与实现,文章末尾附有 ...
- 建立飞机的六自由度运动方程,并对飞机定常直线平飞状态进行配平
是当年大三的专业课,但是当时其实完全不懂要干什么,做的还蛮糟糕的.现在上了研究生至少明白了 一些些,所以这两天把从前的作业又做了一下,当然还是有很多不懂的地方,期待进步吧. 一.飞机配平(定常直线平飞 ...
- V-REP 六足机器人教程
在本教程中,我们将构建一个六足行走机器人.要确保在您开始看本教程之前已经阅读了 BubbleRob 教程以及导入和准备刚性体教程.与本教程( "hexapod.dxf" ) 有关, ...
- 纤巧精干:爱普生六轴机器人
精工爱普生(以下简称爱普生)是世界领先的6轴机器人厂商之一,其六轴机器人兼具高速度.高精度(低振动)以及出色的耐用性等多重优点,大量应用在电子元器件.汽车和食品等多个制造行业中.继2012年底爱普生推 ...
最新文章
- CCSprite setTextureRect 的坐标的坑
- entity、model和domain三者区别
- cocos2dx-Lua与Object的通讯机制
- 配置 --- vscode中react格式化解决方案
- c语言判断这天是星期几,【求指导!!】输入年,月,日,然后判断这天是星期几!!!!!...
- pycharm断点调试django
- AOS编排语言系列教程(六):创建共享云硬盘EVS
- (转)SQLServer_十步优化SQL Server中的数据访问 三
- [VB]用记录集填充表格函数
- Android App自动更新解决方案(DownloadManager)
- macOS 升级12.0.1后,virtualBox 报错 Kernel driver not installed
- php上js实现ajax请求,原生JS实现ajax与ajax的跨域请求实例
- ethercat转profinet网关_Profinet与EtherCAT网关使用方法
- QQ等级:QQ在线等级说明
- 隐私计算概念和技术体系
- uni-app实现本地打包安卓APK
- 解决插入word文档中的图片变得不清晰问题
- 中国的黑客究竟有多张狂?
- 算法的时间复杂度表示
- 【Python 实战基础】Flask 蓝图 Blueprint 怎么用以及怎么集成 Bootstrap
热门文章
- Linux运行webdriver,linux - 在chrome中运行Selenium WebDriver python绑定
- java中二叉树中第k大的数,寻找第k大的数
- 状态驱动的游戏智能体设计(下)
- java计算机毕业设计高校运动会MyBatis+系统+LW文档+源码+调试部署
- 神经网络计算机硬件,人工神经网络的硬件设计研究
- 科学数字_沉默的数字生命:融合艺术、哲学与科学的3D数字雕塑
- html布局结构瀑布流,三种方式实现瀑布流布局
- 删除request_images对应的n_sample中的sample 增加register_images对应的与register_images对应的n_sample中的request_images
- 257页12万字城市园林绿化养护方案
- java 通知_基于Java API实现通知机制