使用Kinect2.0控制VREP中的虚拟模型

时间:2021-09-11 15:38:18

VREP中直接设置物体姿态的函数有3个:

  1. simSetObjectOrientation:通过欧拉角设置姿态
  2. simSetObjectQuaternion:通过四元数设置姿态
  3. simSetObjectMatrix:通过旋转矩阵设置姿态(同时也可以设置位置)
  通过设置物体矩阵可以同时改变其位置与姿态,参数matrix是一个包含12个元素的列表: 12 simFloat values (the last row of the 4x4 matrix (0,0,0,1) is not needed).
  The x-axis of the orientation component is (matrix[0],matrix[4],matrix[8])
  The y-axis of the orientation component is (matrix[1],matrix[5],matrix[9])
  The z-axis of the orientation component is (matrix[2],matrix[6],matrix[10])
  The translation component is (matrix[3],matrix[7],matrix[11])

  下面将一个坐标系绕X轴旋转90°,则可以分别使用欧拉角、四元数或变换矩阵实现:

handle=simGetObjectHandle('Marker')

local eulerAngles = {math.pi/, , }   -- X-Y-Z Euler angles
local quaternion = {0.707, , , 0.707} -- {x,y,z,w}
local matrix = {,,,, ,,-,, ,,,0.5}
simSetObjectOrientation(handle, -, eulerAngles)
--simSetObjectQuaternion(handle, -1, quaternion)
--simSetObjectMatrix(handle, -1, matrix)

使用Kinect2.0控制VREP中的虚拟模型

  • 球关节

  Spherical joints have three DoF and are used to describe rotational movements (with 3 DoF) between objects. Their configuration is defined by three values that represent the amount of rotation around their first reference frame's x-, y- and z-axis. The three values that define a spherical joint's configuration are specified as Euler angles. In some situations, a spherical joint can be thought of as 3 concurrent and orthogonal to each other joints, that are parented in a hierarchy-chain. Spherical joints are always passive joints, and cannot act as motors.

使用Kinect2.0控制VREP中的虚拟模型

[Two equivalent mechanisms (in this configuration): spherical joint (left) and 3 revolute joints (right)]

  在场景中创建一个球关节和一个连杆(处于竖直状态),将连杆拖到球关节下面作为其子节点,球关节设置为Passive模式。下面的代码控制了球关节的姿态,使用simSetSphericalJointMatrix函数设置关节旋转矩阵,使得连杆绕X轴旋转90°

handle=simGetObjectHandle('Spherical_joint')
local matrix = {,,,, ,,-,, ,,,} -- the translational components will be ignored
-- Sets the intrinsic orientation matrix of a spherical joint object. This function cannot be used with non-spherical joints
simSetSphericalJointMatrix(handle, matrix)

使用Kinect2.0控制VREP中的虚拟模型


  • C++客户端程序与VREP通信

  进行C++客户端程序与VREP服务端通信,需要对工程进行如下配置:

  1. 在项目中包含下列文件(可以在V-REP安装路径的programming/remoteApi文件夹下找到这些文件)

  • extApi.h
  • extApi.c
  • extApiPlatform.h (contains platform specific code)
  • extApiPlatform.c (contains platform specific code)

  2. 在项目属性-->C/C++-->预处理器-->预处理器定义中定义:NON_MATLAB_PARSING 和 MAX_EXT_API_CONNECTIONS=255

使用Kinect2.0控制VREP中的虚拟模型

  3. 在项目属性-->C/C++-->常规-->附加包含目录中包含:

  C:\Program Files\V-REP3\V-REP_PRO_EDU\programming\include

  C:\Program Files\V-REP3\V-REP_PRO_EDU\programming\remoteApi


  下面创建一个简单的场景使用Kinect来控制NAO机器人头部的运动。具体步骤是获得Neck关节的姿态四元数后将其转换为欧拉角,经过适当转换后通过simxSetJointPosition函数直接设置转动关节的角度(关节要设置为Passive模式)。如果不通过关节来控制头部的运动也可以直接采用上面提到的SetObjectOrientation、SetObjectQuaternion或SetObjectMatrix方式来设置头部姿态。需要注意的是Kinect的Head关节为末端节点,不包含姿态信息,因此这里采用了Neck关节的姿态来控制头部,但效果不是很好。如果想直接获取头部姿态,可以参考Kinect for Windows SDK 2.0中的HD Face Basics例子,其中FaceFrameResult Class可以获取代表面部姿态的四元数:

hr = pFaceFrameResult -> get_FaceRotationQuaternion(&faceRotation);

  下面是一些与之相关的代码。最容易出错的部分是在于Kinect坐标系和VREP坐标系的姿态不一样,因此获得的角度要经过适当转换:Kinect中头部左右摇摆是绕Y轴,而VREP中是绕Z轴;Kinect中头向上仰为绕X轴正方向,而VREP中头向上仰是绕Y轴负方向...

#define PI 3.1415926

int   Sign(double x) { if (x < ) return -; else return ; }

float R2D(float angle){ return angle * 180.0 / PI; }

enum  RotSeq{ zyx, xyz }; // 欧拉角旋转次序

CameraSpacePoint CBodyBasics::QuaternionToEuler(Vector4 q, RotSeq rotSeq)
{
CameraSpacePoint euler = { };
const double Epsilon = 0.0009765625f;
const double Threshold = 0.5f - Epsilon; switch (rotSeq)
{
case zyx: // Z-Y-X Euler angles(RPY angles)
{
double TEST = q.w*q.y - q.x*q.z;
if (TEST < -Threshold || TEST > Threshold) // 奇异姿态,俯仰角为±90°
{
int sign = Sign(TEST);
euler.Z = - * sign * (double)atan2(q.x, q.w); // yaw
euler.Y = sign * (PI / 2.0); // pitch
euler.X = ; // roll }
else
{
euler.X = atan2( * (q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z); // roll
euler.Y = asin(- * (q.x*q.z - q.w*q.y)); // pitch
euler.Z = atan2( * (q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z); // yaw
}
}
break; case xyz: // X-Y-Z Euler angles
{
double TEST = q.x*q.z + q.w*q.y;
if (TEST < -Threshold || TEST > Threshold) // 奇异姿态,俯仰角为±90°
{
int sign = Sign(TEST);
euler.X = * sign * (double)atan2(q.x, q.w);
euler.Y = sign * (PI / 2.0);
euler.Z = ;
}
else
{
euler.X = atan2(- * (q.y*q.z - q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
euler.Y = asin( * (q.x*q.z + q.w*q.y));
euler.Z = atan2(- * (q.x*q.y - q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);
}
} }
return euler;
} /// Handle new body data
void CBodyBasics::ProcessBody(IBody* pBody)
{
HRESULT hr;
BOOLEAN bTracked = false;
hr = pBody->get_IsTracked(&bTracked); // Retrieves a boolean value that indicates if the body is tracked if (SUCCEEDED(hr) && bTracked) // 判断是否追踪到骨骼
{
Joint joints[JointType_Count];
JointOrientation jointOrientations[JointType_Count]; HandState leftHandState = HandState_Unknown;
HandState rightHandState = HandState_Unknown; DepthSpacePoint *depthSpacePosition = new DepthSpacePoint[_countof(joints)]; // 存储深度坐标系中的关节点位置 pBody->get_HandLeftState(&leftHandState); // 获取左右手状态
pBody->get_HandRightState(&rightHandState); hr = pBody->GetJointOrientations(_countof(joints), jointOrientations); // 获取joint orientation
if (SUCCEEDED(hr))
{
CameraSpacePoint euler = QuaternionToEuler(jointOrientations[JointType_Neck].Orientation, xyz); // 四元数转换为X-Y-Z欧拉角 simxSetJointPosition(clientID, Handle_Yaw, euler.Y, simx_opmode_oneshot); // 控制头部左右摆动
simxSetJointPosition(clientID, Handle_Pitch, PI-euler.X, simx_opmode_oneshot); // 控制头部俯仰 extApi_sleepMs();
} hr = pBody->GetJoints(_countof(joints), joints); // 获得25个关节点
if (SUCCEEDED(hr))
{
// Filtered Joint
filter.Update(joints);
const DirectX::XMVECTOR* vec = filter.GetFilteredJoints(); // Retrive Filtered Joints for (int type = ; type < JointType_Count; type++)
{
if (joints[type].TrackingState != TrackingState::TrackingState_NotTracked)
{
float x = 0.0f, y = 0.0f, z = 0.0f;
// Retrieve the x/y/z component of an XMVECTOR Data and storing that component's value in an instance of float referred to by a pointer
DirectX::XMVectorGetXPtr(&x, vec[type]);
DirectX::XMVectorGetYPtr(&y, vec[type]);
DirectX::XMVectorGetZPtr(&z, vec[type]);
CameraSpacePoint cameraSpacePoint = { x, y, z };
m_pCoordinateMapper->MapCameraPointToDepthSpace(cameraSpacePoint, &depthSpacePosition[type]);
}
}
DrawBody(joints, depthSpacePosition);
DrawHandState(depthSpacePosition[JointType_HandLeft], leftHandState);
DrawHandState(depthSpacePosition[JointType_HandRight], rightHandState);
}
delete[] depthSpacePosition;
}
cv::imshow("skeletonImg", skeletonImg);
cv::waitKey(); // 延时5ms /// Constructor
CBodyBasics::CBodyBasics() :
m_pKinectSensor(NULL),
m_pCoordinateMapper(NULL),
m_pBodyFrameReader(NULL)
{
clientID = simxStart((simxChar*)"127.0.0.1", , true, true, , ); // 连接VREP服务端
if (clientID != -)
{
std::cout << "Connected to remote API server" << std::endl; // Send some data to V-REP in a non-blocking fashion:
simxAddStatusbarMessage(clientID, "Connected to V-REP!", simx_opmode_oneshot); Handle_Yaw = ;
Handle_Pitch = ;
simxGetObjectHandle(clientID, "HeadYaw", &Handle_Yaw, simx_opmode_oneshot_wait); // 获取VREP中Yaw关节的句柄
simxGetObjectHandle(clientID, "HeadPitch", &Handle_Pitch, simx_opmode_oneshot_wait); // 获取VREP中Pitch关节句柄
} } /// Destructor
CBodyBasics::~CBodyBasics()
{
SafeRelease(m_pBodyFrameReader);
SafeRelease(m_pCoordinateMapper); if (m_pKinectSensor)
{
m_pKinectSensor->Close();
}
SafeRelease(m_pKinectSensor); // Close the connection to V-REP:
simxFinish(clientID);
}

  另外还有一个问题就是原始数据的抖动比较大,头部旋转的时候不够平滑,有很多种滤波方式可以解决这一问题。最简单的移动平均滤波参考代码如下:

#include <iostream>
#include <stddef.h>
#include <assert.h> using std::cout;
using std::endl; // Simple_moving_average
class SMA
{
public:
SMA(unsigned int period) :period(period), window(new double[period]), head(NULL), tail(NULL), total()
{
assert(period >= );
}
~SMA()
{
delete[] window;
} // Adds a value to the average, pushing one out if nescessary
void add(double val)
{
// Special case: Initialization
if (head == NULL)
{
head = window;
*head = val;
tail = head;
inc(tail);
total = val;
return;
} // Were we already full?
if (head == tail)
{
// Fix total-cache
total -= *head;
// Make room
inc(head);
} // Write the value in the next spot.
*tail = val;
inc(tail); // Update our total-cache
total += val;
} // Returns the average of the last P elements added to this SMA.
// If no elements have been added yet, returns 0.0
double avg() const
{
ptrdiff_t size = this->size();
if (size == )
return ; // No entries => 0 average return total / (double)size; // Cast to double for floating point arithmetic
} private:
unsigned int period;
double * window; // Holds the values to calculate the average of. // Logically, head is before tail
double * head; // Points at the oldest element we've stored.
double * tail; // Points at the newest element we've stored. double total; // Cache the total so we don't sum everything each time. // Bumps the given pointer up by one.
// Wraps to the start of the array if needed.
void inc(double * & p)
{
if (++p >= window + period)
{
p = window;
}
} // Returns how many numbers we have stored.
ptrdiff_t size() const
{
if (head == NULL)
return ;
if (head == tail)
return period;
return (period + tail - head) % period;
}
}; int main(int argc, char * * argv)
{
SMA foo();
SMA bar(); int data[] = { , , , , , , , , , };
for (int * itr = data; itr < data + ; itr++)
{
foo.add(*itr);
cout << "Added " << *itr << " avg: " << foo.avg() << endl;
} cout << endl; for (int * itr = data; itr < data + ; itr++)
{
bar.add(*itr);
cout << "Added " << *itr << " avg: " << bar.avg() << endl;
} return ;
}

  下面是NAO随着我的头部先进行俯仰,然后左右摇摆的动态图:使用Kinect2.0控制VREP中的虚拟模型

  另外也可以使用获取到的三维坐标点计算出关节夹角,以此来控制虚拟模型。下面计算出肘关节和肩关节角度,来控制VREP场景中的一个2*度手臂:

使用Kinect2.0控制VREP中的虚拟模型

参考:

Kinect2.0骨骼层次与Joint Orientation

quaternions.online

Averages/Simple moving average