关于珞石机器人二次开发SDK的posture函数的算法RX RY RZ纠正 C#

在珞石SDK二次开发的函数钟,获取当前机器人位姿的函数posture函数在输出时会发现数据不正确,与示教器数据不一致。

其中第一个数据正确 第二三各数据为相反 第四五六各数据为弧度制

转换方法为(弧度/PI)*180度

然后发现第四个数据还要加上180度

第五六各数据要取反,,所以设计了以下代码

cs 复制代码
void(){
ErrorCode ec;
Lamp1.IsHighlight =  robot.getDO(1, 1, out ec);
Lamp2.IsHighlight = robot.getDO(1, 11, out ec);
Lamp3.IsHighlight = robot.getDO(1, 12, out ec);
Lamp4.IsHighlight = robot.getDO(1, 13, out ec);
Lamp5.IsHighlight = robot.getDO(1, 14, out ec);
Lamp6.IsHighlight = robot.getDO(1, 15, out ec);
Lamp7.IsHighlight = robot.getDO(1, 16, out ec);
//创建新数组保存数据
double[] aa=new double[6];
aa[0] = robot.posture(CoordinateType.endInRef, out ec)[0]*1000;//TX
aa[1] = -robot.posture(CoordinateType.endInRef, out ec)[1] * 1000;//TY
aa[2] = -robot.posture(CoordinateType.endInRef, out ec)[2] * 1000;//TZ
aa[3] = robot.posture(CoordinateType.endInRef, out ec)[3];//RX
aa[4] = robot.posture(CoordinateType.endInRef, out ec)[4];//RY
aa[5] = robot.posture(CoordinateType.endInRef, out ec)[5];//RZ

double pi = 3.14159265354;
double rad = 180;
//二次转换
aa[3]= 180+(aa[3]*rad)/pi;
aa[4]= -(aa[4]*rad)/pi;
aa[5]= -(aa[5]*rad)/pi;
label1.Text = "当前位置:"+aa[0].ToString("0.000")+','+aa[1].ToString("0.000") + ',' + aa[2].ToString("0.000") + ',' + aa[3].ToString("0.000") + ',' + aa[4].ToString("0.000") + ',' + aa[5].ToString("0.000");
label2.Text = "坐标系:" + String.Join(",",robot.baseFrame(out ec));
label3.Text = "关节位置:" + String.Join(",",robot.jointPos(out ec));
}
相关推荐
资源开发与学习3 小时前
机器人运动规划源码解析
机器人
hi0_61 天前
机器学习实战(一): 什么是机器学习
人工智能·机器学习·机器人·机器学习实战
大视码垛机1 天前
速度与安全双突破:大视码垛机重构工业自动化新范式
大数据·数据库·人工智能·机器人·自动化·制造
WWZZ20251 天前
视觉SLAM第10讲:后端2(滑动窗口与位子图优化)
c++·人工智能·后端·算法·ubuntu·机器人·自动驾驶
deephub2 天前
机器人逆运动学进阶:李代数、矩阵指数与旋转流形计算
人工智能·机器学习·矩阵·机器人·李群李代数
武子康2 天前
AI-调查研究-76-具身智能 当机器人走进生活:具身智能对就业与社会结构的深远影响
人工智能·程序人生·ai·职场和发展·机器人·生活·具身智能
小鹿清扫日记2 天前
从蛮力清扫到 “会看路”:室外清洁机器人的文明进阶
人工智能·ai·机器人·扫地机器人·具身智能·连合直租·有鹿巡扫机器人
shuidaoyuxing2 天前
机器人防爆的详细讲解
机器人
物联网软硬件开发-轨物科技2 天前
【轨物方案】赋能绿色能源新纪元:轨物科技发布光伏清洁机器人智能控制与运维解决方案
科技·机器人·能源
尤齐2 天前
《机器人抓取:从经典到现代的综述》内容的提取和凝练:
机器人