void rpyFromQuat(const Quaterniond &q, double &roll, double &pitch, double &yaw)
{
Matrix3d r = q.toRotationMatrix() ;
Vector3d euler = r.eulerAngles(2, 1, 0) ;
yaw = euler.x() ;
pitch = euler.y() ;
roll = euler.z() ;
// pitch = 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()) ;
// yaw = asin(-2*(q.x()*q.z() - q.w()*q.y()) ) ;
// roll = 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()) ;
}
// Method to compute location of given a vector of joint configurations; note the
// returned vector could be 3D (X,Y,Z) or 6D(X,Y,Z,R,P,Y)
VectorXd JointMover::GetXYZRPY( VectorXd _q, bool both ) {
// Get current XYZ position
mRobot->setConfig(mLinks, _q);
MatrixXd qTransform = mEENode->getWorldTransform();
VectorXd qXYZRPY;
//return a 6D vector if both x,y,z and r,p,y must be computed
if(both){
Matrix3d rotM = qTransform.topLeftCorner(3,3);
VectorXd rot = rotM.eulerAngles(0,1,2);
qXYZRPY.resize(6);
qXYZRPY << qTransform(0,3), qTransform(1,3), qTransform(2,3), rot(0), rot(1), rot(2);
}
else{
qXYZRPY.resize(3);
qXYZRPY << qTransform(0,3), qTransform(1,3), qTransform(2,3);
}
return qXYZRPY;
}
Vector3d FromODERotationToEulerAngle(const dReal* rotMat)
{
Matrix3d mat = FromODERotationToEigenMatrix(rotMat);
Vector3d eulerAngle = mat.eulerAngles(2, 0, 1);
return eulerAngle;
}