26 #include "../../include/robotis_manipulator/robotis_manipulator_math.h" 39 double m21,
double m22,
double m23,
40 double m31,
double m32,
double m33)
43 temp << m11, m12, m13, m21, m22, m23, m31, m32, m33;
49 Eigen::Matrix3d inertia;
65 Eigen::Vector3d position;
74 Eigen::Matrix3d rotation(3,3);
77 0.0, cos(angle), -sin(angle),
78 0.0, sin(angle), cos(angle);
85 Eigen::Matrix3d rotation(3,3);
87 cos(angle), 0.0, sin(angle),
89 -sin(angle), 0.0, cos(angle);
96 Eigen::Matrix3d rotation(3,3);
98 cos(angle), -sin(angle), 0.0,
99 sin(angle), cos(angle), 0.0,
108 rpy.coeffRef(0,0) = atan2(rotation.coeff(2,1), rotation.coeff(2,2));
109 rpy.coeffRef(1,0) = atan2(-rotation.coeff(2,0), sqrt(pow(rotation.coeff(2,1), 2) + pow(rotation.coeff(2,2),2)));
110 rpy.coeffRef(2,0) = atan2 (rotation.coeff(1,0), rotation.coeff(0,0));
124 Eigen::Quaterniond quaternion;
132 Eigen::Quaterniond quaternion;
133 quaternion = rotation;
147 Eigen::Matrix3d rotation = quaternion.toRotationMatrix();
161 transformation.coeffRef(0,3) = position_x;
162 transformation.coeffRef(1,3) = position_y;
163 transformation.coeffRef(2,3) = position_z;
165 return transformation;
170 Eigen::Matrix4d mat_translation;
178 return mat_translation;
183 double sr = sin(roll), cr = cos(roll);
184 double sp = sin(pitch), cp = cos(pitch);
185 double sy = sin(yaw), cy = cos(yaw);
187 Eigen::Matrix4d mat_roll;
188 Eigen::Matrix4d mat_pitch;
189 Eigen::Matrix4d mat_yaw;
209 Eigen::Matrix4d mat_rpy = (mat_yaw*mat_pitch)*mat_roll;
218 Eigen::Vector3d vec_boa;
219 Eigen::Vector3d vec_x, vec_y, vec_z;
220 Eigen::Matrix4d inv_t;
222 vec_boa(0) = -transform(0,3);
223 vec_boa(1) = -transform(1,3);
224 vec_boa(2) = -transform(2,3);
226 vec_x(0) = transform(0,0); vec_x(1) = transform(1,0); vec_x(2) = transform(2,0);
227 vec_y(0) = transform(0,1); vec_y(1) = transform(1,1); vec_y(2) = transform(2,1);
228 vec_z(0) = transform(0,2); vec_z(1) = transform(1,2); vec_z(2) = transform(2,2);
231 vec_x(0), vec_x(1), vec_x(2), vec_boa.dot(vec_x),
232 vec_y(0), vec_y(1), vec_y(2), vec_boa.dot(vec_y),
233 vec_z(0), vec_z(1), vec_z(2), vec_boa.dot(vec_z),
242 Eigen::Matrix3d c_inverse;
243 Eigen::Vector3d rpy_velocity;
245 c_inverse << 1, sin(rpy_vector(0))*tan(rpy_vector(1)), cos(rpy_vector(0))*tan(rpy_vector(1)),
246 0, cos(rpy_vector(0)), -sin(rpy_vector(0)),
247 0, sin(rpy_vector(0))/cos(rpy_vector(1)), cos(rpy_vector(0))/cos(rpy_vector(1));
249 rpy_velocity = c_inverse * omega;
256 Eigen::Vector3d omega;
258 c << 1, 0, -sin(rpy_vector(1)),
259 0, cos(rpy_vector(0)), sin(rpy_vector(0))*cos(rpy_vector(1)),
260 0, -sin(rpy_vector(0)), cos(rpy_vector(0))*cos(rpy_vector(1));
262 omega = c * rpy_velocity;
268 Eigen::Vector3d c_dot;
269 Eigen::Matrix3d c_inverse;
270 Eigen::Vector3d rpy_acceleration;
272 c_dot << -cos(rpy_vector[1]) * rpy_velocity[1] * rpy_velocity[2],
273 -sin(rpy_vector[0]) * rpy_velocity[0] * rpy_velocity[1] - sin(rpy_vector[0]) * sin(rpy_vector[1]) * rpy_velocity[1] * rpy_velocity[2] + cos(rpy_vector[0]) * cos(rpy_vector[1]) * rpy_velocity[0] * rpy_velocity[2],
274 -cos(rpy_vector[0]) * rpy_velocity[0] * rpy_velocity[1] - sin(rpy_vector[0]) * cos(rpy_vector[1]) * rpy_velocity[0] * rpy_velocity[2] - cos(rpy_vector[0]) * sin(rpy_vector[1]) * rpy_velocity[1] * rpy_velocity[2];
276 c_inverse << 1, sin(rpy_vector(0))*tan(rpy_vector(1)), cos(rpy_vector(0))*tan(rpy_vector(1)),
277 0, cos(rpy_vector(0)), -sin(rpy_vector(0)),
278 0, sin(rpy_vector(0))/cos(rpy_vector(1)), cos(rpy_vector(0))/cos(rpy_vector(1));
280 rpy_acceleration = c_inverse * (omega_dot - c_dot);
281 return rpy_acceleration;
286 Eigen::Vector3d c_dot;
288 Eigen::Vector3d omega_dot;
290 c_dot << -cos(rpy_vector[1]) * rpy_velocity[1] * rpy_velocity[2],
291 -sin(rpy_vector[0]) * rpy_velocity[0] * rpy_velocity[1] - sin(rpy_vector[0]) * sin(rpy_vector[1]) * rpy_velocity[1] * rpy_velocity[2] + cos(rpy_vector[0]) * cos(rpy_vector[1]) * rpy_velocity[0] * rpy_velocity[2],
292 -cos(rpy_vector[0]) * rpy_velocity[0] * rpy_velocity[1] - sin(rpy_vector[0]) * cos(rpy_vector[1]) * rpy_velocity[0] * rpy_velocity[2] - cos(rpy_vector[0]) * sin(rpy_vector[1]) * rpy_velocity[1] * rpy_velocity[2];
294 c << 1, 0, -sin(rpy_vector(1)),
295 0, cos(rpy_vector(0)), sin(rpy_vector(0))*cos(rpy_vector(1)),
296 0, -sin(rpy_vector(0)), cos(rpy_vector(0))*cos(rpy_vector(1));
298 omega_dot = c_dot + c * rpy_acceleration;
320 Eigen::Matrix3d R = rotation_matrix;
321 Eigen::Vector3d l = Eigen::Vector3d::Zero();
322 Eigen::Vector3d rotation_vector = Eigen::Vector3d::Zero();
326 bool diagonal_matrix = R.isDiagonal();
328 l << R(2, 1) - R(1, 2),
331 theta = atan2(l.norm(), R(0, 0) + R(1, 1) + R(2, 2) - 1);
336 rotation_vector.setZero(3);
337 return rotation_vector;
340 if (diagonal_matrix ==
true)
342 rotation_vector << R(0, 0) + 1, R(1, 1) + 1, R(2, 2) + 1;
343 rotation_vector = rotation_vector * M_PI_2;
347 rotation_vector = theta * (l / l.norm());
349 return rotation_vector;
354 Eigen::Matrix3d skew_symmetric_matrix = Eigen::Matrix3d::Zero();
355 skew_symmetric_matrix << 0, -v(2), v(1),
358 return skew_symmetric_matrix;
363 Eigen::Matrix3d skew_symmetric_matrix = Eigen::Matrix3d::Zero();
364 Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Zero();
365 Eigen::Matrix3d Identity_matrix = Eigen::Matrix3d::Identity();
368 rotation_matrix = Identity_matrix +
369 skew_symmetric_matrix * sin(angle) +
370 skew_symmetric_matrix * skew_symmetric_matrix * (1 - cos(angle));
371 return rotation_matrix;
376 Eigen::Vector3d position_difference;
377 position_difference = desired_position - present_position;
379 return position_difference;
384 Eigen::Vector3d orientation_difference;
387 return orientation_difference;
391 Eigen::Matrix3d desired_orientation, Eigen::Matrix3d present_orientation)
393 Eigen::Vector3d position_difference;
394 Eigen::Vector3d orientation_difference;
395 Eigen::VectorXd pose_difference(6);
399 pose_difference << position_difference(0), position_difference(1), position_difference(2),
400 orientation_difference(0), orientation_difference(1), orientation_difference(2);
402 return pose_difference;
Eigen::Matrix4d convertRPYToTransformationMatrix(double roll, double pitch, double yaw)
convertRPYToTransformationMatrix
double sign(double value)
sign
Eigen::Matrix3d inertiaMatrix(double ixx, double ixy, double ixz, double iyy, double iyz, double izz)
inertiaMatrix
Eigen::Vector3d convertQuaternionToRPYVector(const Eigen::Quaterniond &quaternion)
convertQuaternionToRPYVector
Eigen::Vector3d convertRPYVelocityToOmega(Eigen::Vector3d rpy_vector, Eigen::Vector3d rpy_velocity)
convertRPYVelocityToOmega
Eigen::VectorXd poseDifference(Eigen::Vector3d desired_position, Eigen::Vector3d present_position, Eigen::Matrix3d desired_orientation, Eigen::Matrix3d present_orientation)
poseDifference
Eigen::Matrix3d matrix3(double m11, double m12, double m13, double m21, double m22, double m23, double m31, double m32, double m33)
matrix3
Eigen::Vector3d convertRPYAccelerationToOmegaDot(Eigen::Vector3d rpy_vector, Eigen::Vector3d rpy_velocity, Eigen::Vector3d rpy_acceleration)
convertRPYAccelerationToOmegaDot
Eigen::Matrix3d convertQuaternionToRotationMatrix(const Eigen::Quaterniond &quaternion)
convertQuaternionToRotationMatrix
Eigen::Matrix4d inverseTransformationMatrix(const Eigen::MatrixXd &transformation_matrix)
inverseTransformationMatrix
Eigen::Vector3d orientationDifference(Eigen::Matrix3d desired_orientation, Eigen::Matrix3d present_orientation)
orientationDifference
Eigen::Vector3d vector3(double v1, double v2, double v3)
vector3
Eigen::Vector3d convertRotationMatrixToOmega(const Eigen::Matrix3d &rotation_matrix)
convertRotationMatrixToOmega
Eigen::Matrix3d skewSymmetricMatrix(Eigen::Vector3d v)
skewSymmetricMatrix
Eigen::Vector3d positionDifference(Eigen::Vector3d desired_position, Eigen::Vector3d present_position)
positionDifference
Eigen::Vector3d matrixLogarithm(Eigen::Matrix3d rotation_matrix)
matrixLogarithm
Eigen::Matrix3d rodriguesRotationMatrix(Eigen::Vector3d axis, double angle)
rodriguesRotationMatrix
Eigen::Matrix4d convertXYZRPYToTransformationMatrix(double x, double y, double z, double roll, double pitch, double yaw)
convertXYZRPYToTransformationMatrix
Eigen::Vector3d convertOmegaToRPYVelocity(Eigen::Vector3d rpy_vector, Eigen::Vector3d omega)
convertOmegaToRPYVelocity
Eigen::Vector3d convertXYZToVector(double x, double y, double z)
convertXYZToVector
Eigen::Quaterniond convertRPYToQuaternion(double roll, double pitch, double yaw)
convertRPYToQuaternion
Eigen::Matrix3d convertRollAngleToRotationMatrix(double angle)
convertRollAngleToRotationMatrix
Eigen::Vector3d convertRotationMatrixToRPYVector(const Eigen::Matrix3d &rotation_matrix)
convertRotationMatrixToRPYVector
Eigen::Vector3d convertOmegaDotToRPYAcceleration(Eigen::Vector3d rpy_vector, Eigen::Vector3d rpy_velocity, Eigen::Vector3d omega_dot)
convertOmegaDotToRPYAcceleration
Eigen::Matrix3d convertYawAngleToRotationMatrix(double angle)
convertYawAngleToRotationMatrix
Eigen::Matrix3d convertPitchAngleToRotationMatrix(double angle)
convertPitchAngleToRotationMatrix
Eigen::Matrix3d convertRPYToRotationMatrix(double roll, double pitch, double yaw)
convertRPYToRotationMatrix
Eigen::Matrix4d convertXYZToTransformationMatrix(double x, double y, double z)
convertXYZToTransformationMatrix
Eigen::Quaterniond convertRotationMatrixToQuaternion(const Eigen::Matrix3d &rotation_matrix)
convertRotationMatrixToQuaternion