19 #include "../../include/robotis_manipulator/robotis_manipulator_math.h" 32 double m21,
double m22,
double m23,
33 double m31,
double m32,
double m33)
36 temp << m11, m12, m13, m21, m22, m23, m31, m32, m33;
42 Eigen::Matrix3d inertia;
58 Eigen::Vector3d position;
67 Eigen::Matrix3d rotation(3,3);
70 0.0, cos(angle), -sin(angle),
71 0.0, sin(angle), cos(angle);
78 Eigen::Matrix3d rotation(3,3);
80 cos(angle), 0.0, sin(angle),
82 -sin(angle), 0.0, cos(angle);
89 Eigen::Matrix3d rotation(3,3);
91 cos(angle), -sin(angle), 0.0,
92 sin(angle), cos(angle), 0.0,
101 rpy.coeffRef(0,0) = atan2(rotation.coeff(2,1), rotation.coeff(2,2));
102 rpy.coeffRef(1,0) = atan2(-rotation.coeff(2,0), sqrt(pow(rotation.coeff(2,1), 2) + pow(rotation.coeff(2,2),2)));
103 rpy.coeffRef(2,0) = atan2 (rotation.coeff(1,0), rotation.coeff(0,0));
117 Eigen::Quaterniond quaternion;
125 Eigen::Quaterniond quaternion;
126 quaternion = rotation;
140 Eigen::Matrix3d rotation = quaternion.toRotationMatrix();
154 transformation.coeffRef(0,3) = position_x;
155 transformation.coeffRef(1,3) = position_y;
156 transformation.coeffRef(2,3) = position_z;
158 return transformation;
163 Eigen::Matrix4d mat_translation;
171 return mat_translation;
176 double sr = sin(roll), cr = cos(roll);
177 double sp = sin(pitch), cp = cos(pitch);
178 double sy = sin(yaw), cy = cos(yaw);
180 Eigen::Matrix4d mat_roll;
181 Eigen::Matrix4d mat_pitch;
182 Eigen::Matrix4d mat_yaw;
202 Eigen::Matrix4d mat_rpy = (mat_yaw*mat_pitch)*mat_roll;
211 Eigen::Vector3d vec_boa;
212 Eigen::Vector3d vec_x, vec_y, vec_z;
213 Eigen::Matrix4d inv_t;
215 vec_boa(0) = -transform(0,3);
216 vec_boa(1) = -transform(1,3);
217 vec_boa(2) = -transform(2,3);
219 vec_x(0) = transform(0,0); vec_x(1) = transform(1,0); vec_x(2) = transform(2,0);
220 vec_y(0) = transform(0,1); vec_y(1) = transform(1,1); vec_y(2) = transform(2,1);
221 vec_z(0) = transform(0,2); vec_z(1) = transform(1,2); vec_z(2) = transform(2,2);
224 vec_x(0), vec_x(1), vec_x(2), vec_boa.dot(vec_x),
225 vec_y(0), vec_y(1), vec_y(2), vec_boa.dot(vec_y),
226 vec_z(0), vec_z(1), vec_z(2), vec_boa.dot(vec_z),
235 Eigen::Matrix3d c_inverse;
236 Eigen::Vector3d rpy_velocity;
238 c_inverse << 1, sin(rpy_vector(0))*tan(rpy_vector(1)), cos(rpy_vector(0))*tan(rpy_vector(1)),
239 0, cos(rpy_vector(0)), -sin(rpy_vector(0)),
240 0, sin(rpy_vector(0))/cos(rpy_vector(1)), cos(rpy_vector(0))/cos(rpy_vector(1));
242 rpy_velocity = c_inverse * omega;
249 Eigen::Vector3d omega;
251 c << 1, 0, -sin(rpy_vector(1)),
252 0, cos(rpy_vector(0)), sin(rpy_vector(0))*cos(rpy_vector(1)),
253 0, -sin(rpy_vector(0)), cos(rpy_vector(0))*cos(rpy_vector(1));
255 omega = c * rpy_velocity;
261 Eigen::Vector3d c_dot;
262 Eigen::Matrix3d c_inverse;
263 Eigen::Vector3d rpy_acceleration;
265 c_dot << -cos(rpy_vector[1]) * rpy_velocity[1] * rpy_velocity[2],
266 -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],
267 -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];
269 c_inverse << 1, sin(rpy_vector(0))*tan(rpy_vector(1)), cos(rpy_vector(0))*tan(rpy_vector(1)),
270 0, cos(rpy_vector(0)), -sin(rpy_vector(0)),
271 0, sin(rpy_vector(0))/cos(rpy_vector(1)), cos(rpy_vector(0))/cos(rpy_vector(1));
273 rpy_acceleration = c_inverse * (omega_dot - c_dot);
274 return rpy_acceleration;
279 Eigen::Vector3d c_dot;
281 Eigen::Vector3d omega_dot;
283 c_dot << -cos(rpy_vector[1]) * rpy_velocity[1] * rpy_velocity[2],
284 -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],
285 -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];
287 c << 1, 0, -sin(rpy_vector(1)),
288 0, cos(rpy_vector(0)), sin(rpy_vector(0))*cos(rpy_vector(1)),
289 0, -sin(rpy_vector(0)), cos(rpy_vector(0))*cos(rpy_vector(1));
291 omega_dot = c_dot + c * rpy_acceleration;
313 Eigen::Matrix3d R = rotation_matrix;
314 Eigen::Vector3d l = Eigen::Vector3d::Zero();
315 Eigen::Vector3d rotation_vector = Eigen::Vector3d::Zero();
319 bool diagonal_matrix = R.isDiagonal();
321 l << R(2, 1) - R(1, 2),
324 theta = atan2(l.norm(), R(0, 0) + R(1, 1) + R(2, 2) - 1);
329 rotation_vector.setZero(3);
330 return rotation_vector;
333 if (diagonal_matrix ==
true)
335 rotation_vector << R(0, 0) + 1, R(1, 1) + 1, R(2, 2) + 1;
336 rotation_vector = rotation_vector * M_PI_2;
340 rotation_vector = theta * (l / l.norm());
342 return rotation_vector;
347 Eigen::Matrix3d skew_symmetric_matrix = Eigen::Matrix3d::Zero();
348 skew_symmetric_matrix << 0, -v(2), v(1),
351 return skew_symmetric_matrix;
356 Eigen::Matrix3d skew_symmetric_matrix = Eigen::Matrix3d::Zero();
357 Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Zero();
358 Eigen::Matrix3d Identity_matrix = Eigen::Matrix3d::Identity();
361 rotation_matrix = Identity_matrix +
362 skew_symmetric_matrix * sin(angle) +
363 skew_symmetric_matrix * skew_symmetric_matrix * (1 - cos(angle));
364 return rotation_matrix;
369 Eigen::Vector3d position_difference;
370 position_difference = desired_position - present_position;
372 return position_difference;
377 Eigen::Vector3d orientation_difference;
380 return orientation_difference;
384 Eigen::Matrix3d desired_orientation, Eigen::Matrix3d present_orientation)
386 Eigen::Vector3d position_difference;
387 Eigen::Vector3d orientation_difference;
388 Eigen::VectorXd pose_difference(6);
392 pose_difference << position_difference(0), position_difference(1), position_difference(2),
393 orientation_difference(0), orientation_difference(1), orientation_difference(2);
395 return pose_difference;
400 Eigen::Vector3d linear_difference = desired_linear_velocity - present_linear_velocity;
401 Eigen::Vector3d angular_difference = desired_angular_velocity - present_angular_velocity;
402 Eigen::VectorXd dynamic_pose_difference(6);
403 dynamic_pose_difference << linear_difference(0), linear_difference(1), linear_difference(2),
404 angular_difference(0), angular_difference(1), angular_difference(2);
406 return dynamic_pose_difference;
Eigen::Matrix4d convertRPYToTransformationMatrix(double roll, double pitch, double yaw)
double sign(double value)
Eigen::Matrix3d inertiaMatrix(double ixx, double ixy, double ixz, double iyy, double iyz, double izz)
Eigen::VectorXd dynamicPoseDifference(Eigen::Vector3d desired_linear_velocity, Eigen::Vector3d present_linear_velocity, Eigen::Vector3d desired_angular_velocity, Eigen::Vector3d present_angular_velocity)
Eigen::Vector3d convertQuaternionToRPYVector(const Eigen::Quaterniond &quaternion)
Eigen::Vector3d convertRPYVelocityToOmega(Eigen::Vector3d rpy_vector, Eigen::Vector3d rpy_velocity)
Eigen::VectorXd poseDifference(Eigen::Vector3d desired_position, Eigen::Vector3d present_position, Eigen::Matrix3d desired_orientation, Eigen::Matrix3d present_orientation)
Eigen::Matrix3d matrix3(double m11, double m12, double m13, double m21, double m22, double m23, double m31, double m32, double m33)
Eigen::Vector3d convertRPYAccelerationToOmegaDot(Eigen::Vector3d rpy_vector, Eigen::Vector3d rpy_velocity, Eigen::Vector3d rpy_acceleration)
Eigen::Matrix3d convertQuaternionToRotationMatrix(const Eigen::Quaterniond &quaternion)
Eigen::Matrix4d inverseTransformationMatrix(const Eigen::MatrixXd &transformation_matrix)
Eigen::Vector3d orientationDifference(Eigen::Matrix3d desired_orientation, Eigen::Matrix3d present_orientation)
Eigen::Vector3d vector3(double v1, double v2, double v3)
Eigen::Vector3d convertRotationMatrixToOmega(const Eigen::Matrix3d &rotation_matrix)
Eigen::Matrix3d skewSymmetricMatrix(Eigen::Vector3d v)
Eigen::Vector3d positionDifference(Eigen::Vector3d desired_position, Eigen::Vector3d present_position)
Eigen::Vector3d matrixLogarithm(Eigen::Matrix3d rotation_matrix)
Eigen::Matrix3d rodriguesRotationMatrix(Eigen::Vector3d axis, double angle)
Eigen::Matrix4d convertXYZRPYToTransformationMatrix(double x, double y, double z, double roll, double pitch, double yaw)
Eigen::Vector3d convertOmegaToRPYVelocity(Eigen::Vector3d rpy_vector, Eigen::Vector3d omega)
Eigen::Vector3d convertXYZToVector(double x, double y, double z)
Eigen::Quaterniond convertRPYToQuaternion(double roll, double pitch, double yaw)
Eigen::Matrix3d convertRollAngleToRotationMatrix(double angle)
Eigen::Vector3d convertRotationMatrixToRPYVector(const Eigen::Matrix3d &rotation_matrix)
Eigen::Vector3d convertOmegaDotToRPYAcceleration(Eigen::Vector3d rpy_vector, Eigen::Vector3d rpy_velocity, Eigen::Vector3d omega_dot)
Eigen::Matrix3d convertYawAngleToRotationMatrix(double angle)
Eigen::Matrix3d convertPitchAngleToRotationMatrix(double angle)
Eigen::Matrix3d convertRPYToRotationMatrix(double roll, double pitch, double yaw)
Eigen::Matrix4d convertXYZToTransformationMatrix(double x, double y, double z)
Eigen::Quaterniond convertRotationMatrixToQuaternion(const Eigen::Matrix3d &rotation_matrix)