关于珞石机器人二次开发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));
}
相关推荐
ARM+FPGA+AI工业主板定制专家19 小时前
【JETSON+FPGA+GMSL】实测分享 | 如何实现激光雷达与摄像头高精度时间同步?
人工智能·数码相机·机器学习·fpga开发·机器人·自动驾驶
武子康19 小时前
AI-调查研究-105-具身智能 机器人学习数据采集:从示范视频到状态-动作对的流程解析
人工智能·深度学习·机器学习·ai·系统架构·机器人·具身智能
ARM+FPGA+AI工业主板定制专家20 小时前
Jetson AGX Orin+GMSL+AI视觉开发套件,支持自动驾驶,机器人,工业视觉等应用
人工智能·机器学习·fpga开发·机器人·自动驾驶
天天讯通21 小时前
任务型与聊天型语音机器人有什么区别
人工智能·机器人
LeeZhao@21 小时前
【具身智能】具身机器人VLA算法入门及实战(四):具身智能VLA技术行业进展
人工智能·算法·机器人
Tipriest_21 小时前
机器人逆动力学及其应用
机器人·逆动力学
一RTOS一1 天前
东土科技连投三家核心企业 发力具身机器人领域
人工智能·科技·机器人·具身智能·鸿道实时操作系统·国产嵌入式操作系统选型
互联科技报1 天前
机器人能否实现远程视频通话方便家属探视老人
机器人·音视频
坚持编程的菜鸟1 天前
LeetCode每日一题——困于环中的机器人
c语言·算法·leetcode·机器人
LeeZhao@2 天前
【具身智能】具身机器人VLA算法入门及实战(一):具身智能系统及VLA
人工智能·docker·语言模型·机器人