29 Eigen::Vector3d
getTransitionXYZ(
double position_x,
double position_y,
double position_z)
31 Eigen::Vector3d position;
41 Eigen::Matrix4d
getTransformationXYZRPY(
double position_x,
double position_y,
double position_z ,
double roll ,
double pitch ,
double yaw)
43 Eigen::Matrix4d transformation =
getRotation4d(roll, pitch, yaw);
44 transformation.coeffRef(0,3) = position_x;
45 transformation.coeffRef(1,3) = position_y;
46 transformation.coeffRef(2,3) = position_z;
48 return transformation;
55 Eigen::Vector3d vec_boa;
56 Eigen::Vector3d vec_x, vec_y, vec_z;
57 Eigen::Matrix4d inv_t;
59 vec_boa(0) = -transform(0,3);
60 vec_boa(1) = -transform(1,3);
61 vec_boa(2) = -transform(2,3);
63 vec_x(0) = transform(0,0); vec_x(1) = transform(1,0); vec_x(2) = transform(2,0);
64 vec_y(0) = transform(0,1); vec_y(1) = transform(1,1); vec_y(2) = transform(2,1);
65 vec_z(0) = transform(0,2); vec_z(1) = transform(1,2); vec_z(2) = transform(2,2);
68 vec_x(0), vec_x(1), vec_x(2), vec_boa.dot(vec_x),
69 vec_y(0), vec_y(1), vec_y(2), vec_boa.dot(vec_y),
70 vec_z(0), vec_z(1), vec_z(2), vec_boa.dot(vec_z),
76 Eigen::Matrix3d
getInertiaXYZ(
double ixx,
double ixy,
double ixz ,
double iyy ,
double iyz,
double izz)
78 Eigen::Matrix3d inertia;
90 Eigen::Matrix3d rotation(3,3);
94 0.0, cos(angle), -sin(angle),
95 0.0, sin(angle), cos(angle);
102 Eigen::Matrix3d rotation(3,3);
105 cos(angle), 0.0, sin(angle),
107 -sin(angle), 0.0, cos(angle);
114 Eigen::Matrix3d rotation(3,3);
117 cos(angle), -sin(angle), 0.0,
118 sin(angle), cos(angle), 0.0,
126 double sr = sin(roll), cr = cos(roll);
127 double sp = sin(pitch), cp = cos(pitch);
128 double sy = sin(yaw), cy = cos(yaw);
130 Eigen::Matrix4d mat_roll;
131 Eigen::Matrix4d mat_pitch;
132 Eigen::Matrix4d mat_yaw;
152 Eigen::Matrix4d mat_rpy = (mat_yaw*mat_pitch)*mat_roll;
159 Eigen::Matrix4d mat_translation;
167 return mat_translation;
174 rpy.coeffRef(0,0) = atan2(rotation.coeff(2,1), rotation.coeff(2,2));
175 rpy.coeffRef(1,0) = atan2(-rotation.coeff(2,0), sqrt(pow(rotation.coeff(2,1), 2) + pow(rotation.coeff(2,2),2)));
176 rpy.coeffRef(2,0) = atan2 (rotation.coeff(1,0), rotation.coeff(0,0));
190 Eigen::Quaterniond quaternion;
198 Eigen::Quaterniond quaternion;
199 quaternion = rotation;
213 Eigen::Matrix3d rotation = quaternion.toRotationMatrix();
218 Eigen::Matrix3d
calcHatto(
const Eigen::Vector3d& matrix3d)
220 Eigen::MatrixXd hatto(3,3);
223 0.0, -matrix3d.coeff(2,0), matrix3d.coeff(1,0),
224 matrix3d.coeff(2,0), 0.0, -matrix3d.coeff(0,0),
225 -matrix3d.coeff(1,0), matrix3d.coeff(0,0), 0.0;
230 Eigen::Matrix3d
calcRodrigues(
const Eigen::Matrix3d& hat_matrix,
double angle)
234 Eigen::Matrix3d rodrigues = hat_matrix*sin(angle)+hat_matrix*hat_matrix*(1-cos(angle));
235 rodrigues.coeffRef(0,0) += 1;
236 rodrigues.coeffRef(1,1) += 1;
237 rodrigues.coeffRef(2,2) += 1;
246 double alpha = (rotation.coeff(0,0)+rotation.coeff(1,1)+rotation.coeff(2,2)-1.0)/2.0;
247 double alpha_dash = fabs( alpha - 1.0 );
249 Eigen::Vector3d rot_to_omega;
251 if( alpha_dash < eps )
260 double theta = acos(alpha);
263 rotation.coeff(2,1)-rotation.coeff(1,2),
264 rotation.coeff(0,2)-rotation.coeff(2,0),
265 rotation.coeff(1,0)-rotation.coeff(0,1);
267 rot_to_omega = 0.5*(theta/sin(theta))*rot_to_omega;
273 Eigen::Vector3d
calcCross(
const Eigen::Vector3d& vector3d_a,
const Eigen::Vector3d& vector3d_b)
275 Eigen::Vector3d cross;
278 vector3d_a.coeff(1,0)*vector3d_b.coeff(2,0)-vector3d_a.coeff(2,0)*vector3d_b.coeff(1,0),
279 vector3d_a.coeff(2,0)*vector3d_b.coeff(0,0)-vector3d_a.coeff(0,0)*vector3d_b.coeff(2,0),
280 vector3d_a.coeff(0,0)*vector3d_b.coeff(1,0)-vector3d_a.coeff(1,0)*vector3d_b.coeff(0,0);
285 double calcInner(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b)
287 return vector3d_a.dot(vector3d_b);
294 pose_3d.
x = transform.coeff(0, 3);
295 pose_3d.
y = transform.coeff(1, 3);
296 pose_3d.
z = transform.coeff(2, 3);
297 pose_3d.
roll = atan2( transform.coeff(2,1), transform.coeff(2,2));
298 pose_3d.
pitch = atan2(-transform.coeff(2,0), sqrt(transform.coeff(2,1)*transform.coeff(2,1) + transform.coeff(2,2)*transform.coeff(2,2)) );
299 pose_3d.
yaw = atan2( transform.coeff(1,0), transform.coeff(0,0));
Eigen::Matrix3d getRotationZ(double angle)
Eigen::Matrix4d getTranslation4D(double position_x, double position_y, double position_z)
Eigen::Matrix3d getRotationX(double angle)
Eigen::Matrix3d getRotationY(double angle)
Eigen::Quaterniond convertRPYToQuaternion(double roll, double pitch, double yaw)
Eigen::Matrix3d calcHatto(const Eigen::Vector3d &matrix3d)
Eigen::Matrix3d calcRodrigues(const Eigen::Matrix3d &hat_matrix, double angle)
Eigen::Vector3d convertRotToOmega(const Eigen::Matrix3d &rotation)
Eigen::Matrix4d getInverseTransformation(const Eigen::MatrixXd &transform)
Eigen::Quaterniond convertRotationToQuaternion(const Eigen::Matrix3d &rotation)
double calcInner(const Eigen::MatrixXd &vector3d_a, const Eigen::MatrixXd &vector3d_b)
Eigen::Vector3d convertQuaternionToRPY(const Eigen::Quaterniond &quaternion)
Eigen::Matrix4d getTransformationXYZRPY(double position_x, double position_y, double position_z, double roll, double pitch, double yaw)
Eigen::Vector3d getTransitionXYZ(double position_x, double position_y, double position_z)
Eigen::Vector3d calcCross(const Eigen::Vector3d &vector3d_a, const Eigen::Vector3d &vector3d_b)
Eigen::Matrix3d convertQuaternionToRotation(const Eigen::Quaterniond &quaternion)
Eigen::Matrix3d convertRPYToRotation(double roll, double pitch, double yaw)
Eigen::Vector3d convertRotationToRPY(const Eigen::Matrix3d &rotation)
Pose3D getPose3DfromTransformMatrix(const Eigen::Matrix4d &transform)
Eigen::Matrix3d getInertiaXYZ(double ixx, double ixy, double ixz, double iyy, double iyz, double izz)
Eigen::Matrix4d getRotation4d(double roll, double pitch, double yaw)