CoppeliaSim(Vrep)获取移动机器人底盘的位姿信息
一. 前言
本文主要采用的方法就是调用Vrep提供的两个API，getObjectPosition,getObjectOrientation,来获取全场坐标及相关角度的信息。
二. 关于位姿信息
底盘运动需要一定的信息参考，当前全场坐标，当前角度，才能往下进行路径等任务。所以，在Vrep中仿真底盘运动，也需要这样的信息，而Vrep中并没有实际现实中的陀螺仪和码盘模块，那么如何获取就成了一个必须要解决的问题。
三. 具体过程
1. 查找相关资料
如果直接百度如何从Vrep获取位姿信息类似问题，是搜索不到有用的信息的。结合之前我在官方论坛提问获得的回答，以及初学Vrep时一个视频给了我启发。想要获取位姿信息，也就是当前全场坐标和陀螺仪提供的方向，Vrep中并没有这两个东西，那肯定需要找方法等效替代。官方当时的答复是获取相对坐标有[sim.getObjectPosition(number objectHandle,number relativeToObjectHandle)](https://www.coppeliarobotics.com/api/sim.getObjectPosition(number objectHandle,number relativeToObjectHandle))API函数可以调用。而视频中也提及了小车中轨迹规划涉及了相对夹角的判断，那也有很大可能存在获取相对角度的API。
抱着这样的看法，就去官方文档API列表（按功能排列）中查找。
果然，就在getObjectPosition下面，有着getObjectOrientation。
2. API官方介绍
官方文档都是英文版的，下面做了简单的翻译。
2.1 getObjectPosition
| 描述 | 检索对象的位置。另请参见sim.setObjectPosition，sim.getObjectOrientation，sim.getObjectMatrix和其他矩阵/转换函数。 |
|---|---|
| 概要 | simInt simGetObjectPosition（simInt objectHandle，simInt relativeToObjectHandle，simFloat *位置） |
| C参数 | objectHandle：对象的句柄。可以与sim.handleflag_reljointbaseframe组合（请参阅下一个参数）relativeToObjectHandle：指示相对于我们想要位置的参考系。指定-1可以获取绝对位置，指定sim.handle_parent可以获取相对于对象父对象的位置，也可以指定对象句柄相对于我们想要位置的参考框架。如果此手柄是关节的手柄，则将返回相对于关节的移动框架的位置（除非objectHandle与sim.handleflag_reljointbaseframe组合，在这种情况下，将返回相对于关节的基础框架的位置）。 position：指向3个值（x，y和z）的指针 |
| C返回值 | 如果操作不成功则为-1 |
| Lua简介 | table_3 position = sim.getObjectPosition（number objectHandle，number relativeToObjectHandle） |
| Lua参数 | 类似于C函数对应项 |
| Lua返回值 | 位置：3个值（x，y和z）的表 |
| 等效的远程API | 基于B0的远程API：simxGetObjectPosition，simxGetObjectPose旧版远程API：simxGetObjectPosition |
之前做的获取相对关节坐标系位置点，第二个参数放了关节的句柄，而现在希望获得的是全场坐标，即绝对坐标系，因此填入-1即可。在实际操作中则会发现，官方文档的介绍并不是十分严谨，例如返回参数的单位制都没有表述。此处先提及一下，后面会说到这个。
2.2 getObjectOrientation
| 描述 | 检索对象的方向（欧拉角）。另请参见sim.getObjectQuaternion，sim.setObjectOrientation，sim.getObjectPosition，sim.getObjectMatrix和其他矩阵/转换函数。 |
|---|---|
| 概要 | simInt simGetObjectOrientation（simInt objectHandle，simInt relativeToObjectHandle，simFloat * eulerAngles） |
| C参数 | objectHandle：对象的句柄。可以与sim.handleflag_reljointbaseframe组合（请参阅下一个参数）relativeToObjectHandle：表示相对于我们要定向的参考系。指定-1可以获取绝对方向，指定sim.handle_parent可以获取相对于对象父对象的相对方向，也可以指定相对于要获取其参考框架的对象手柄的方向。如果此手柄是关节的手柄，则将返回相对于关节的移动框架的方向（除非objectHandle与sim.handleflag_reljointbaseframe组合，在这种情况下，将返回相对于关节的基础框架的方向）。 eulerAngles：欧拉角（α，β和gamma） |
| C返回值 | 如果操作不成功则为-1 |
| Lua简介 | table_3 eulerAngles = sim.getObjectOrientation（number objectHandle，number relativeToObjectHandle） |
| Lua参数 | 类似于C函数对应项 |
| Lua返回值 | eulerAngles：3个值的表格（欧拉角） |
| 等效的远程API | 基于B0的远程API：simxGetObjectOrientation旧版远程API：simxGetObjectOrientation |
这里参数填入及隐含的问题同上一API，还要简单了解一下，需要的是哪一个欧拉角，陀螺仪安置时强调在一个水平面，以及实际串口返回数据处理中，可以明确，需要的是xOy平面上的角度，则对应偏航角Y，对应返回参数中的γ。
3. 搭建模型
舵轮模型之前就搭好了，这次添加了一个Dummy，放置在车体中心位置。现在写文档时感觉如果坐标系和车体坐标系一样的话，像这个模型就是，可以省去添加Dummy这一步骤。
可以看到目前车体被我偏置了一定角度，在水平面上和右下角绝对坐标系形成了一定夹角，方便测试API的调用调试。
4. API调用代码调试
4.1 API代码编写
这里只放置了简单框架，后面会有完整的。
simxFloat PointFloats[3] = { 0 };
simxFloat AngleFloats[3] = { 0 };
while (clientID != -1)
{
simxGetObjectPosition(clientID, WholePosition, -1, &PointFloats[0], simx_opmode_blocking);
cout << "x " << PointFloats[0] << " y " << PointFloats[1] << " z " << PointFloats[2] << endl;
simxGetObjectOrientation(clientID, WholePosition, -1, &AngleFloats[0],simx_opmode_blocking);
cout << "alpha " << AngleFloats[0] << " beta " << AngleFloats[1] << " gamma " << AngleFloats[2] << endl;
Sleep(10);
}
4.2 实际效果
从实际效果可以发现，获得的数值都是精度很高的，不方便后续操作的。因此，需要想办法使数据格式统一，方便后续调用。这里采用的sprintf_s字符串格式化，再调用atof函数，让字符串转为浮点数的操作。
4.3 API代码调试
while (clientID != -1)
{
simxGetObjectPosition(clientID, WholePosition, -1, &PointFloats[0], simx_opmode_blocking);
sprintf_s(StrTemp, 20, "%.2f", PointFloats[0]);
PointFloats[0] = (float)(atof(StrTemp));
sprintf_s(StrTemp, 20, "%.2f", PointFloats[1]);
PointFloats[1] = (float)(atof(StrTemp));
sprintf_s(StrTemp, 20, "%.2f", PointFloats[2]);
PointFloats[2] = (float)(atof(StrTemp));
cout << "x " << PointFloats[0] << " y " << PointFloats[1] << " z " << PointFloats[2] << endl;
simxGetObjectOrientation(clientID, WholePosition, -1, &AngleFloats[0], simx_opmode_blocking);
sprintf_s(StrTemp, 20, "%.2f", AngleFloats[0]);
AngleFloats[0] = (float)(atof(StrTemp));
sprintf_s(StrTemp, 20, "%.2f", AngleFloats[1]);
AngleFloats[1] = (float)(atof(StrTemp));
sprintf_s(StrTemp, 20, "%.2f", AngleFloats[2]);
AngleFloats[2] = (float)(atof(StrTemp));
cout << "alpha " << AngleFloats[0] << " beta " << AngleFloats[1] << " gamma " << AngleFloats[2] << endl;
Sleep(10);
}
现在就满足需求了，开始后续完整代码编写。同时需要注意的是，位置点单位都是m,而反馈的欧拉角则是弧度制，这些官方文档并未有相应的说明，算是官方做的不好的地方。
5. 底盘位姿代码编写
加入了线程操作，使得定位和驱动任务分开运行。
#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include <iostream>
#include <process.h>
#include <Windows.h>
#include <time.h>
#include <math.h>
using namespace std;
# define M_PI 3.14159265358979323846 /* pi */
extern "C" {
#include "extApi.h"
}
char StrTemp[20];
int clientID;
simxFloat PointFloats[3] = { 0 };
simxFloat AngleFloats[3] = { 0 };
DWORD WINAPI Position(LPVOID lpparm);
DWORD WINAPI MoveMent(LPVOID lpparm);
//线程终止https://www.cnblogs.com/keepsimple/p/3415994.html
simxInt WholePosition;
simxInt LFD;
simxInt LBD;
simxInt RFD;
simxInt RBD;
simxInt LFL;
simxInt LBL;
simxInt RFL;
simxInt RBL;
int main()
{
clientID = simxStart("127.0.0.1", 19997, true, true, 5000, 5);
if (clientID != -1)
{
printf("success\n");
}
else
{
printf("error\n");
}
/*simx_opmode_blocking和simx_opmode_oneshot_wait是一个意思,
可以理解为这个操作执行一次，但是没有执行完就要等，所以速度慢，
但是会保证通信完成（和通信协议里的握手很像）。simx_opmode_oneshot
表示我把数据发出去之后就开始执行后面的语句，不管你发完了没。*/
simxStartSimulation(clientID, simx_opmode_oneshot_wait);
simxGetObjectHandle(clientID, "WholePosition", &WholePosition, simx_opmode_blocking);
simxGetObjectHandle(clientID, "LFMotorD", &LFD, simx_opmode_blocking);
simxGetObjectHandle(clientID, "LBMotorD", &LBD, simx_opmode_blocking);
simxGetObjectHandle(clientID, "RFMotorD", &RFD, simx_opmode_blocking);
simxGetObjectHandle(clientID, "RBMotorD", &RBD, simx_opmode_blocking);
simxGetObjectHandle(clientID, "LFMotorL", &LFL, simx_opmode_blocking);
simxGetObjectHandle(clientID, "LBMotorL", &LBL, simx_opmode_blocking);
simxGetObjectHandle(clientID, "RFMotorL", &RFL, simx_opmode_blocking);
simxGetObjectHandle(clientID, "RBMotorL", &RBL, simx_opmode_blocking);
HANDLE h1, h2;
h1 = CreateThread(NULL, 0, Position, NULL, 0, NULL);
cout << "定位开始运行\n" << endl;
h2 = CreateThread(NULL, 0, MoveMent, NULL, 0, NULL);
cout << "驱动开始运行" << endl;
CloseHandle(h1);
CloseHandle(h2);
while (clientID != -1)
{
if (getchar() == 'q')
{
simxSetJointTargetVelocity(clientID, LFL, 0, simx_opmode_blocking);
simxSetJointTargetVelocity(clientID, LBL, 0, simx_opmode_blocking);
simxSetJointTargetVelocity(clientID, RFL, 0, simx_opmode_blocking);
simxSetJointTargetVelocity(clientID, RBL, 0, simx_opmode_blocking);
simxStopSimulation(clientID, simx_opmode_oneshot);
simxFinish(clientID);
return 0;
}
else
{
Sleep(100);
}
}
}
DWORD WINAPI Position(LPVOID lpparm)
{
while (clientID != -1)
{
simxGetObjectPosition(clientID, WholePosition, -1, &PointFloats[0], simx_opmode_blocking);
sprintf_s(StrTemp, 20, "%.2f", PointFloats[0]);
PointFloats[0] = (float)(atof(StrTemp));
sprintf_s(StrTemp, 20, "%.2f", PointFloats[1]);
PointFloats[1] = (float)(atof(StrTemp));
sprintf_s(StrTemp, 20, "%.2f", PointFloats[2]);
PointFloats[2] = (float)(atof(StrTemp));
cout << "x " << PointFloats[0] << " y " << PointFloats[1] << " z " << PointFloats[2] << endl;
simxGetObjectOrientation(clientID, WholePosition, -1, &AngleFloats[0], simx_opmode_blocking);
sprintf_s(StrTemp, 20, "%.2f", AngleFloats[0]);
AngleFloats[0] = (float)(atof(StrTemp));
sprintf_s(StrTemp, 20, "%.2f", AngleFloats[1]);
AngleFloats[1] = (float)(atof(StrTemp));
sprintf_s(StrTemp, 20, "%.2f", AngleFloats[2]);
AngleFloats[2] = (float)(atof(StrTemp));
cout << "alpha " << AngleFloats[0] << " beta " << AngleFloats[1] << " gamma " << AngleFloats[2] << endl;
Sleep(10);
}
return 0;
}
DWORD WINAPI MoveMent(LPVOID lpparm)
{
while (clientID != -1)
{
for (int time = 0; time < 3; time++) {
Sleep(1000);
simxSetJointTargetPosition(clientID, LFD, 45 * M_PI / 180, simx_opmode_oneshot);
simxSetJointTargetPosition(clientID, LBD, 45 * M_PI / 180, simx_opmode_oneshot);
simxSetJointTargetPosition(clientID, RFD, 45 * M_PI / 180, simx_opmode_oneshot);
simxSetJointTargetPosition(clientID, RBD, 45 * M_PI / 180, simx_opmode_oneshot);
simxSetJointTargetVelocity(clientID, LFL, -5, simx_opmode_oneshot);
simxSetJointTargetVelocity(clientID, LBL, -5, simx_opmode_oneshot);
simxSetJointTargetVelocity(clientID, RFL, -5, simx_opmode_oneshot);
simxSetJointTargetVelocity(clientID, RBL, -5, simx_opmode_oneshot);
}
for (int time = 0; time < 3; time++) {
Sleep(1000);
simxSetJointTargetPosition(clientID, LFD, 135 * M_PI / 180, simx_opmode_oneshot);
simxSetJointTargetPosition(clientID, LBD, 135 * M_PI / 180, simx_opmode_oneshot);
simxSetJointTargetPosition(clientID, RFD, 135 * M_PI / 180, simx_opmode_oneshot);
simxSetJointTargetPosition(clientID, RBD, 135 * M_PI / 180, simx_opmode_oneshot);
simxSetJointTargetVelocity(clientID, LFL, -5, simx_opmode_oneshot);
simxSetJointTargetVelocity(clientID, LBL, -5, simx_opmode_oneshot);
simxSetJointTargetVelocity(clientID, RFL, -5, simx_opmode_oneshot);
simxSetJointTargetVelocity(clientID, RBL, -5, simx_opmode_oneshot);
}
for (int time = 0; time < 3; time++) {
Sleep(1000);
simxSetJointTargetPosition(clientID, LFD, 45 * M_PI / 180, simx_opmode_oneshot);
simxSetJointTargetPosition(clientID, LBD, 45 * M_PI / 180, simx_opmode_oneshot);
simxSetJointTargetPosition(clientID, RFD, 45 * M_PI / 180, simx_opmode_oneshot);
simxSetJointTargetPosition(clientID, RBD, 45 * M_PI / 180, simx_opmode_oneshot);
simxSetJointTargetVelocity(clientID, LFL, 5, simx_opmode_oneshot);
simxSetJointTargetVelocity(clientID, LBL, 5, simx_opmode_oneshot);
simxSetJointTargetVelocity(clientID, RFL, 5, simx_opmode_oneshot);
simxSetJointTargetVelocity(clientID, RBL, 5, simx_opmode_oneshot);
}
for (int time = 0; time < 3; time++) {
Sleep(1000);
simxSetJointTargetPosition(clientID, LFD, 135 * M_PI / 180, simx_opmode_oneshot);
simxSetJointTargetPosition(clientID, LBD, 135 * M_PI / 180, simx_opmode_oneshot);
simxSetJointTargetPosition(clientID, RFD, 135 * M_PI / 180, simx_opmode_oneshot);
simxSetJointTargetPosition(clientID, RBD, 135 * M_PI / 180, simx_opmode_oneshot);
simxSetJointTargetVelocity(clientID, LFL, 5, simx_opmode_oneshot);
simxSetJointTargetVelocity(clientID, LBL, 5, simx_opmode_oneshot);
simxSetJointTargetVelocity(clientID, RFL, 5, simx_opmode_oneshot);
simxSetJointTargetVelocity(clientID, RBL, 5, simx_opmode_oneshot);
}
}
return 0;
}
6. 实际效果
下图中可以看到，坐标和角度信息随着底盘的运动而实时变化。
四. 总结
没事的时候，可以翻翻官方的API函数列表，学学有哪些功能，说不定哪一天就能用上，2333。
扫一扫
555
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
