Esempio n. 1
0
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;
}
Esempio n. 3
0
Vector3d FromODERotationToEulerAngle(const dReal* rotMat)
{
	Matrix3d mat = FromODERotationToEigenMatrix(rotMat);
	Vector3d eulerAngle = mat.eulerAngles(2, 0, 1);
	return eulerAngle;
}