VREP中直接設置物體姿態的函數有3個：
- simSetObjectOrientation：通過歐拉角設置姿態
- simSetObjectQuaternion：通過四元數設置姿態
- 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自由度手臂：
參考：