CoppeliaSim(Vrep)获取移动机器人底盘的位姿信息

Posted Norach

tags:

篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了CoppeliaSim(Vrep)获取移动机器人底盘的位姿信息相关的知识，希望对你有一定的参考价值。

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。

以上是关于CoppeliaSim(Vrep)获取移动机器人底盘的位姿信息的主要内容，如果未能解决你的问题，请参考以下文章

CoppeliaSim(vrep)打开闪退的解决办法

CoppeliaSim(vrep)打开闪退的解决办法

CoppeliaSim(vrep)打开闪退的解决办法

CoppeliaSim(vrep)打开闪退的解决办法

CoppeliaSim(Vrep)与VS使用多线程交互

CoppeliaSim(Vrep)与VS使用多线程交互