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.setObjectPositionsim.getObjectOrientationsim.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:simxGetObjectPositionsimxGetObjectPose旧版远程API:simxGetObjectPosition

​ 之前做的获取相对关节坐标系位置点,第二个参数放了关节的句柄,而现在希望获得的是全场坐标,即绝对坐标系,因此填入-1即可。在实际操作中则会发现,官方文档的介绍并不是十分严谨,例如返回参数的单位制都没有表述。此处先提及一下,后面会说到这个。

2.2 getObjectOrientation

描述检索对象的方向(欧拉角)。另请参见sim.getObjectQuaternionsim.setObjectOrientationsim.getObjectPositionsim.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使用多线程交互