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

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/2, 0, 0}   -- X-Y-Z Euler angles
local quaternion = {0.707, 0, 0, 0.707} -- {x,y,z,w}
local matrix = {1,0,0,0, 0,0,-1,0, 0,1,0,0.5}
simSetObjectOrientation(handle, -1, eulerAngles)
--simSetObjectQuaternion(handle, -1, quaternion)
--simSetObjectMatrix(handle, -1, matrix)

  • 球关节

　　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.

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

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

handle=simGetObjectHandle(‘Spherical_joint‘)
local matrix = {1,0,0,0,  0,0,-1,0,  0,1,0,0}  -- 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)

  • 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

　　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 < 0) return -1; else return 1; }

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

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

CameraSpacePoint CBodyBasics::QuaternionToEuler(Vector4 q, RotSeq rotSeq)
{
    CameraSpacePoint euler = { 0 };
    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 = -2 * sign * (double)atan2(q.x, q.w); // yaw
                euler.Y = sign * (PI / 2.0); // pitch
                euler.X = 0; // roll

            }
            else
            {
                euler.X = atan2(2 * (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(-2 * (q.x*q.z - q.w*q.y));                                         // pitch
                euler.Z = atan2(2 * (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 = 2 * sign * (double)atan2(q.x, q.w);
                euler.Y = sign * (PI / 2.0);
                euler.Z = 0;
            }
            else
            {
                euler.X = atan2(-2 * (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(2 * (q.x*q.z + q.w*q.y));
                euler.Z = atan2(-2 * (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(5);
        }

        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 = 0; 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(5); // 延时5ms

/// Constructor
CBodyBasics::CBodyBasics() :
m_pKinectSensor(NULL),
m_pCoordinateMapper(NULL),
m_pBodyFrameReader(NULL)
{
    clientID = simxStart((simxChar*)"127.0.0.1", 19999, true, true, 2000, 5); // 连接VREP服务端
    if (clientID != -1)
    {
        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 = 0;
        Handle_Pitch = 0;
        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(0)
    {
        assert(period >= 1);
    }
    ~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 == 0)
            return 0; // 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 0;
        if (head == tail)
            return period;
        return (period + tail - head) % period;
    }
};

int main(int argc, char * * argv)
{
    SMA foo(3);
    SMA bar(5);

    int data[] = { 1, 2, 3, 4, 5, 5, 4, 3, 2, 1 };
    for (int * itr = data; itr < data + 10; itr++)
    {
        foo.add(*itr);
        cout << "Added " << *itr << " avg: " << foo.avg() << endl;
    }

    cout << endl;

    for (int * itr = data; itr < data + 10; itr++)
    {
        bar.add(*itr);
        cout << "Added " << *itr << " avg: " << bar.avg() << endl;
    }

    return 0;
}

　　下面是NAO随着我的头部先进行俯仰，然后左右摇摆的动态图：

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

参考：

Kinect2.0骨骼层次与Joint Orientation

quaternions.online

Averages/Simple moving average

时间： 2024-08-04 21:16:10

使用Kinect2.0控制VREP中的虚拟模型的相关文章

如何使用sourceCRT连接vmware中的虚拟主机？

如何使用sourceCRT连接vmware中的虚拟主机? 在进入主题之前,我们要先了解一些概念. 一些你应该知道的简单的概念 内网地址 我们首先要了解一下所谓的内网地址,以下面三种形式开头的被称为内网地址: 10.x.x.x 172.16.x.x到172.31.x.x 192.168.x.x 所以的内网地址就是说这些地址只在局域网内使用,在公网上,你是不能够使用这些地址的.当然还有一个127.0.0.1是本机的回路地址,这个地址只能你本机使用,使用这个地址发送数据,数据只会在你的本机转而不会跑到

schema中的虚拟属性方法

schema中的虚拟属性方法相当于vue中的计算属性,它是通过已定义的schema属性的计算\组合\拼接得到的新的值 var personSchema = new Schema({ name: { first: String, last: String } }); var Person = mongoose.model('Person', personSchema); // create a document var bad = new Person({ name: { first: 'Walt

在Apache中开启虚拟主机

最近在自学LAMP,在Apache中尝试着开启虚拟主机的时候,遇到了挺多麻烦的,这里也顺便总结一下,在Apache中开启虚拟主机的时候,主要有下面几个步骤: 1.新建一个文件夹作为虚拟主机,用来存储网站资源例如我是在Apache目录下新建了一个 htdocs_v 文件夹,里头新建了一个php文件,内容如下: <?php echo 'this is the first virtual host'; ?> 2.在配置文件中开启虚拟主机:打开Apache/conf/httpd.conf文件,修改如下

深入理解react中的虚拟DOM、diff算法

文章结构: React中的虚拟DOM是什么? 虚拟DOM的简单实现(diff算法) 虚拟DOM的内部工作原理 React中的虚拟DOM与Vue中的虚拟DOM比较 React中的虚拟DOM是什么?   虽然React中的虚拟DOM很好用,但是这是一个无心插柳的结果.   React的核心思想:一个Component拯救世界,忘掉烦恼,从此不再操心界面. 1. Virtual Dom快,有两个前提 1.1 Javascript很快  Chrome刚出来的时候,在Chrome里跑Javascript非

控制WinForm中Tab键的跳转

一,需求 在Winform中,默认情况下,按下Tab键,光标会按照我们设定的TabIndex值从小到大进行跳转. 但如果用户要求按下Tab键跳转到特定的控件,这种要求还是很合理的,比如用户只想输入几个必须填的项目. 我们可以在配置文件中配置这些必须填写的项目,并设定他们的跳转顺序.这样程序也更加灵活,利于扩展. 二,探索实现方法 1,在每个输入控件的keyDown事件里判断是Tab键,做相应的跳转处理. 最后调查发现按下Tab键,并不会触发控件keyDown事件,Tab键默认被系统处理了,悲剧了

VREP中的力触觉设备接口（CHAI3D）

力反馈技术是一种新型的人机交互技术,它允许用户借助力反馈设备触碰.操纵计算机生成的虚拟环境中的物体,并感知物体的运动和相应的力反馈信息,实现人机力觉交互.虽然传统的鼠标.键盘.触摸屏等交互手段可以满足用户与环境中物体交互的需求,但是缺乏力觉交互信息的反馈.力反馈技术结合其他的虚拟现实技术,使用户在交互过程中不仅能够通过视.听觉通道获取信息,还能够通过触觉通道感受模拟现实世界力觉交互的"触感".因此,力反馈技术的引入,使交互体验更加自然.真实. 力反馈系统的实现过程中涉及到以下关键问题:

AE控制图层中要素可见状态的几种方法

转自原文 AE控制图层中要素可见状态的几种方法 工作中常有这样的需求,一个作业图层由几个作业员来操作,我们要 控制每一个作业员只能看到他负责的区域.作业员的可见区域控制有时候是按空间区域划分,有时候是按照作业属性划分,有时候是属性和区域结合来划分,在程序中应该如何控制呢?本人总结了如下几种可用的方法,不知大家是否有更好的解决方案. ?   唯一值符号法 该方法比较简单,就是通过给图层设置一个唯一值符号渲染,把不想显示的要素符号设置为空.虽然简单,这种方法有这很大的局限性,如果我们要控制某一个区域

逻辑回归中的虚拟变量设置

系列文章收集在比特币与互联网金融风控专栏中 虚拟变量定义 ??在实际建模过程中,被解释变量不但受定量变量影响,同时还受定性变量影响.例如需要考虑性别.民族.不同历史时期.季节差异.企业所有制性质不同等因素的影响.这些因素也应该包括在模型中. ??由于定性变量通常表示的是某种特征的有和无,所以量化方法可采用取值为1或0.这种变量称作虚拟变量,用D表示.虚拟变量应用于模型中,对其回归系数的估计与检验方法与定量变量相同. 虚拟变量对模型的意义 ??通常,我们假设的因变量与自变量之间的关系既是线性的,又

Kinect2.0 Gesture Builder使用方法

=====Gesture Builder概述===== Kinect2.0 新版本SDK发布后,附带了一个Gesture Builder工具,这是微软发布的进行姿势识别的软件,就是不需要写姿势识别代码,直接调用Gesture Builder生成的.gba数据文件. =====Gesture Builder使用方法===== 以训练一个挥手的动作为例,说明Gesture Builder的使用方法.首先,要使用Kinect Studio录制一个KinectClip,作为训练样本. 现在开始正式使用G