Robotis Manipulator  1.0.0
Functions
robotis_manipulator::math Namespace Reference

Functions

Eigen::Vector3d vector3 (double v1, double v2, double v3)
 vector3 More...
 
Eigen::Matrix3d matrix3 (double m11, double m12, double m13, double m21, double m22, double m23, double m31, double m32, double m33)
 matrix3 More...
 
Eigen::Matrix3d inertiaMatrix (double ixx, double ixy, double ixz, double iyy, double iyz, double izz)
 inertiaMatrix More...
 
Eigen::Vector3d convertXYZToVector (double x, double y, double z)
 convertXYZToVector More...
 
Eigen::Matrix3d convertRollAngleToRotationMatrix (double angle)
 convertRollAngleToRotationMatrix More...
 
Eigen::Matrix3d convertPitchAngleToRotationMatrix (double angle)
 convertPitchAngleToRotationMatrix More...
 
Eigen::Matrix3d convertYawAngleToRotationMatrix (double angle)
 convertYawAngleToRotationMatrix More...
 
Eigen::Vector3d convertRotationMatrixToRPYVector (const Eigen::Matrix3d &rotation_matrix)
 convertRotationMatrixToRPYVector More...
 
Eigen::Matrix3d convertRPYToRotationMatrix (double roll, double pitch, double yaw)
 convertRPYToRotationMatrix More...
 
Eigen::Quaterniond convertRPYToQuaternion (double roll, double pitch, double yaw)
 convertRPYToQuaternion More...
 
Eigen::Quaterniond convertRotationMatrixToQuaternion (const Eigen::Matrix3d &rotation_matrix)
 convertRotationMatrixToQuaternion More...
 
Eigen::Vector3d convertQuaternionToRPYVector (const Eigen::Quaterniond &quaternion)
 convertQuaternionToRPYVector More...
 
Eigen::Matrix3d convertQuaternionToRotationMatrix (const Eigen::Quaterniond &quaternion)
 convertQuaternionToRotationMatrix More...
 
Eigen::Vector3d convertRotationMatrixToOmega (const Eigen::Matrix3d &rotation_matrix)
 convertRotationMatrixToOmega More...
 
Eigen::Matrix4d convertXYZRPYToTransformationMatrix (double x, double y, double z, double roll, double pitch, double yaw)
 convertXYZRPYToTransformationMatrix More...
 
Eigen::Matrix4d convertXYZToTransformationMatrix (double x, double y, double z)
 convertXYZToTransformationMatrix More...
 
Eigen::Matrix4d convertRPYToTransformationMatrix (double roll, double pitch, double yaw)
 convertRPYToTransformationMatrix More...
 
Eigen::Vector3d convertOmegaToRPYVelocity (Eigen::Vector3d rpy_vector, Eigen::Vector3d omega)
 convertOmegaToRPYVelocity More...
 
Eigen::Vector3d convertRPYVelocityToOmega (Eigen::Vector3d rpy_vector, Eigen::Vector3d rpy_velocity)
 convertRPYVelocityToOmega More...
 
Eigen::Vector3d convertOmegaDotToRPYAcceleration (Eigen::Vector3d rpy_vector, Eigen::Vector3d rpy_velocity, Eigen::Vector3d omega_dot)
 convertOmegaDotToRPYAcceleration More...
 
Eigen::Vector3d convertRPYAccelerationToOmegaDot (Eigen::Vector3d rpy_vector, Eigen::Vector3d rpy_velocity, Eigen::Vector3d rpy_acceleration)
 convertRPYAccelerationToOmegaDot More...
 
double sign (double value)
 sign More...
 
Eigen::Matrix4d inverseTransformationMatrix (const Eigen::MatrixXd &transformation_matrix)
 inverseTransformationMatrix More...
 
Eigen::Vector3d matrixLogarithm (Eigen::Matrix3d rotation_matrix)
 matrixLogarithm More...
 
Eigen::Matrix3d skewSymmetricMatrix (Eigen::Vector3d v)
 skewSymmetricMatrix More...
 
Eigen::Matrix3d rodriguesRotationMatrix (Eigen::Vector3d axis, double angle)
 rodriguesRotationMatrix More...
 
Eigen::Vector3d positionDifference (Eigen::Vector3d desired_position, Eigen::Vector3d present_position)
 positionDifference More...
 
Eigen::Vector3d orientationDifference (Eigen::Matrix3d desired_orientation, Eigen::Matrix3d present_orientation)
 orientationDifference More...
 
Eigen::VectorXd poseDifference (Eigen::Vector3d desired_position, Eigen::Vector3d present_position, Eigen::Matrix3d desired_orientation, Eigen::Matrix3d present_orientation)
 poseDifference More...
 

Function Documentation

Eigen::Vector3d robotis_manipulator::math::convertOmegaDotToRPYAcceleration ( Eigen::Vector3d  rpy_vector,
Eigen::Vector3d  rpy_velocity,
Eigen::Vector3d  omega_dot 
)

convertOmegaDotToRPYAcceleration

Parameters
rpy_vector
rpy_velocity
omega_dot
Returns

Definition at line 266 of file robotis_manipulator_math.cpp.

267 {
268  Eigen::Vector3d c_dot;
269  Eigen::Matrix3d c_inverse;
270  Eigen::Vector3d rpy_acceleration;
271 
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];
275 
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));
279 
280  rpy_acceleration = c_inverse * (omega_dot - c_dot);
281  return rpy_acceleration;
282 }

Here is the caller graph for this function:

Eigen::Vector3d robotis_manipulator::math::convertOmegaToRPYVelocity ( Eigen::Vector3d  rpy_vector,
Eigen::Vector3d  omega 
)

convertOmegaToRPYVelocity

Parameters
rpy_vector
omega
Returns

Definition at line 240 of file robotis_manipulator_math.cpp.

241 {
242  Eigen::Matrix3d c_inverse;
243  Eigen::Vector3d rpy_velocity;
244 
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));
248 
249  rpy_velocity = c_inverse * omega;
250  return rpy_velocity;
251 }

Here is the caller graph for this function:

Eigen::Matrix3d robotis_manipulator::math::convertPitchAngleToRotationMatrix ( double  angle)

convertPitchAngleToRotationMatrix

Parameters
angle
Returns

Definition at line 83 of file robotis_manipulator_math.cpp.

84 {
85  Eigen::Matrix3d rotation(3,3);
86  rotation <<
87  cos(angle), 0.0, sin(angle),
88  0.0, 1.0, 0.0,
89  -sin(angle), 0.0, cos(angle);
90 
91  return rotation;
92 }

Here is the caller graph for this function:

Eigen::Matrix3d robotis_manipulator::math::convertQuaternionToRotationMatrix ( const Eigen::Quaterniond &  quaternion)

convertQuaternionToRotationMatrix

Parameters
quaternion
Returns

Definition at line 145 of file robotis_manipulator_math.cpp.

146 {
147  Eigen::Matrix3d rotation = quaternion.toRotationMatrix();
148 
149  return rotation;
150 }
Eigen::Vector3d robotis_manipulator::math::convertQuaternionToRPYVector ( const Eigen::Quaterniond &  quaternion)

convertQuaternionToRPYVector

Parameters
quaternion
Returns

Definition at line 138 of file robotis_manipulator_math.cpp.

139 {
140  Eigen::Vector3d rpy = robotis_manipulator::math::convertRotationMatrixToRPYVector(quaternion.toRotationMatrix());
141 
142  return rpy;
143 }
robotis_manipulator::math::convertRotationMatrixToRPYVector
Eigen::Vector3d convertRotationMatrixToRPYVector(const Eigen::Matrix3d &rotation_matrix)
convertRotationMatrixToRPYVector
Definition: robotis_manipulator_math.cpp:105

Here is the call graph for this function:

Eigen::Matrix3d robotis_manipulator::math::convertRollAngleToRotationMatrix ( double  angle)

convertRollAngleToRotationMatrix

Parameters
angle
Returns

Definition at line 72 of file robotis_manipulator_math.cpp.

73 {
74  Eigen::Matrix3d rotation(3,3);
75  rotation <<
76  1.0, 0.0, 0.0,
77  0.0, cos(angle), -sin(angle),
78  0.0, sin(angle), cos(angle);
79 
80  return rotation;
81 }

Here is the caller graph for this function:

Eigen::Vector3d robotis_manipulator::math::convertRotationMatrixToOmega ( const Eigen::Matrix3d &  rotation_matrix)

convertRotationMatrixToOmega

Parameters
rotation_matrix
Returns

Definition at line 152 of file robotis_manipulator_math.cpp.

153 {
154  return robotis_manipulator::math::matrixLogarithm(rotation_matrix);
155 }
robotis_manipulator::math::matrixLogarithm
Eigen::Vector3d matrixLogarithm(Eigen::Matrix3d rotation_matrix)
matrixLogarithm
Definition: robotis_manipulator_math.cpp:318

Here is the call graph for this function:

Eigen::Quaterniond robotis_manipulator::math::convertRotationMatrixToQuaternion ( const Eigen::Matrix3d &  rotation_matrix)

convertRotationMatrixToQuaternion

Parameters
rotation_matrix
Returns

Definition at line 130 of file robotis_manipulator_math.cpp.

131 {
132  Eigen::Quaterniond quaternion;
133  quaternion = rotation;
134 
135  return quaternion;
136 }
Eigen::Vector3d robotis_manipulator::math::convertRotationMatrixToRPYVector ( const Eigen::Matrix3d &  rotation_matrix)

convertRotationMatrixToRPYVector

Parameters
rotation_matrix
Returns

Definition at line 105 of file robotis_manipulator_math.cpp.

106 {
107  Eigen::Vector3d rpy;// = Eigen::MatrixXd::Zero(3,1);
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));
111 
112  return rpy;
113 }

Here is the caller graph for this function:

Eigen::Vector3d robotis_manipulator::math::convertRPYAccelerationToOmegaDot ( Eigen::Vector3d  rpy_vector,
Eigen::Vector3d  rpy_velocity,
Eigen::Vector3d  rpy_acceleration 
)

convertRPYAccelerationToOmegaDot

Parameters
rpy_vector
rpy_velocity
rpy_acceleration
Returns

Definition at line 284 of file robotis_manipulator_math.cpp.

285 {
286  Eigen::Vector3d c_dot;
287  Eigen::Matrix3d c;
288  Eigen::Vector3d omega_dot;
289 
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];
293 
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));
297 
298  omega_dot = c_dot + c * rpy_acceleration;
299  return omega_dot;
300 }

Here is the caller graph for this function:

Eigen::Quaterniond robotis_manipulator::math::convertRPYToQuaternion ( double  roll,
double  pitch,
double  yaw 
)

convertRPYToQuaternion

Parameters
roll
pitch
yaw
Returns

Definition at line 122 of file robotis_manipulator_math.cpp.

123 {
124  Eigen::Quaterniond quaternion;
125  quaternion = robotis_manipulator::math::convertRPYToRotationMatrix(roll,pitch,yaw);
126 
127  return quaternion;
128 }
robotis_manipulator::math::convertRPYToRotationMatrix
Eigen::Matrix3d convertRPYToRotationMatrix(double roll, double pitch, double yaw)
convertRPYToRotationMatrix
Definition: robotis_manipulator_math.cpp:115

Here is the call graph for this function:

Eigen::Matrix3d robotis_manipulator::math::convertRPYToRotationMatrix ( double  roll,
double  pitch,
double  yaw 
)

convertRPYToRotationMatrix

Parameters
roll
pitch
yaw
Returns

Definition at line 115 of file robotis_manipulator_math.cpp.

116 {
117  Eigen::Matrix3d rotation = robotis_manipulator::math::convertYawAngleToRotationMatrix(yaw)*robotis_manipulator::math::convertPitchAngleToRotationMatrix(pitch)*robotis_manipulator::math::convertRollAngleToRotationMatrix(roll);
118 
119  return rotation;
120 }
robotis_manipulator::math::convertRollAngleToRotationMatrix
Eigen::Matrix3d convertRollAngleToRotationMatrix(double angle)
convertRollAngleToRotationMatrix
Definition: robotis_manipulator_math.cpp:72
robotis_manipulator::math::convertYawAngleToRotationMatrix
Eigen::Matrix3d convertYawAngleToRotationMatrix(double angle)
convertYawAngleToRotationMatrix
Definition: robotis_manipulator_math.cpp:94
robotis_manipulator::math::convertPitchAngleToRotationMatrix
Eigen::Matrix3d convertPitchAngleToRotationMatrix(double angle)
convertPitchAngleToRotationMatrix
Definition: robotis_manipulator_math.cpp:83

Here is the call graph for this function:

Here is the caller graph for this function:

Eigen::Matrix4d robotis_manipulator::math::convertRPYToTransformationMatrix ( double  roll,
double  pitch,
double  yaw 
)

convertRPYToTransformationMatrix

Parameters
roll
pitch
yaw
Returns

Definition at line 181 of file robotis_manipulator_math.cpp.

182 {
183  double sr = sin(roll), cr = cos(roll);
184  double sp = sin(pitch), cp = cos(pitch);
185  double sy = sin(yaw), cy = cos(yaw);
186 
187  Eigen::Matrix4d mat_roll;
188  Eigen::Matrix4d mat_pitch;
189  Eigen::Matrix4d mat_yaw;
190 
191  mat_roll <<
192  1, 0, 0, 0,
193  0, cr, -sr, 0,
194  0, sr, cr, 0,
195  0, 0, 0, 1;
196 
197  mat_pitch <<
198  cp, 0, sp, 0,
199  0, 1, 0, 0,
200  -sp, 0, cp, 0,
201  0, 0, 0, 1;
202 
203  mat_yaw <<
204  cy, -sy, 0, 0,
205  sy, cy, 0, 0,
206  0, 0, 1, 0,
207  0, 0, 0, 1;
208 
209  Eigen::Matrix4d mat_rpy = (mat_yaw*mat_pitch)*mat_roll;
210 
211  return mat_rpy;
212 }

Here is the caller graph for this function:

Eigen::Vector3d robotis_manipulator::math::convertRPYVelocityToOmega ( Eigen::Vector3d  rpy_vector,
Eigen::Vector3d  rpy_velocity 
)

convertRPYVelocityToOmega

Parameters
rpy_vector
rpy_velocity
Returns

Definition at line 253 of file robotis_manipulator_math.cpp.

254 {
255  Eigen::Matrix3d c;
256  Eigen::Vector3d omega;
257 
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));
261 
262  omega = c * rpy_velocity;
263  return omega;
264 }

Here is the caller graph for this function:

Eigen::Matrix4d robotis_manipulator::math::convertXYZRPYToTransformationMatrix ( double  x,
double  y,
double  z,
double  roll,
double  pitch,
double  yaw 
)

convertXYZRPYToTransformationMatrix

Parameters
x
y
z
roll
pitch
yaw
Returns

Definition at line 158 of file robotis_manipulator_math.cpp.

159 {
160  Eigen::Matrix4d transformation = robotis_manipulator::math::convertRPYToTransformationMatrix(roll, pitch, yaw);
161  transformation.coeffRef(0,3) = position_x;
162  transformation.coeffRef(1,3) = position_y;
163  transformation.coeffRef(2,3) = position_z;
164 
165  return transformation;
166 }
robotis_manipulator::math::convertRPYToTransformationMatrix
Eigen::Matrix4d convertRPYToTransformationMatrix(double roll, double pitch, double yaw)
convertRPYToTransformationMatrix
Definition: robotis_manipulator_math.cpp:181

Here is the call graph for this function:

Eigen::Matrix4d robotis_manipulator::math::convertXYZToTransformationMatrix ( double  x,
double  y,
double  z 
)

convertXYZToTransformationMatrix

Parameters
x
y
z
Returns

Definition at line 168 of file robotis_manipulator_math.cpp.

169 {
170  Eigen::Matrix4d mat_translation;
171 
172  mat_translation <<
173  1, 0, 0, position_x,
174  0, 1, 0, position_y,
175  0, 0, 1, position_z,
176  0, 0, 0, 1;
177 
178  return mat_translation;
179 }
Eigen::Vector3d robotis_manipulator::math::convertXYZToVector ( double  x,
double  y,
double  z 
)

convertXYZToVector

Parameters
x
y
z
Returns

Definition at line 63 of file robotis_manipulator_math.cpp.

64 {
65  Eigen::Vector3d position;
66  position << x, y, z;
67 
68  return position;
69 }
Eigen::Matrix3d robotis_manipulator::math::convertYawAngleToRotationMatrix ( double  angle)

convertYawAngleToRotationMatrix

Parameters
angle
Returns

Definition at line 94 of file robotis_manipulator_math.cpp.

95 {
96  Eigen::Matrix3d rotation(3,3);
97  rotation <<
98  cos(angle), -sin(angle), 0.0,
99  sin(angle), cos(angle), 0.0,
100  0.0, 0.0, 1.0;
101 
102  return rotation;
103 }

Here is the caller graph for this function:

Eigen::Matrix3d robotis_manipulator::math::inertiaMatrix ( double  ixx,
double  ixy,
double  ixz,
double  iyy,
double  iyz,
double  izz 
)

inertiaMatrix

Parameters
ixx
ixy
ixz
iyy
iyz
izz
Returns

Definition at line 47 of file robotis_manipulator_math.cpp.

48 {
49  Eigen::Matrix3d inertia;
50  inertia <<
51  ixx, ixy, ixz,
52  ixy, iyy, iyz,
53  ixz, iyz, izz;
54 
55  return inertia;
56 }
Eigen::Matrix4d robotis_manipulator::math::inverseTransformationMatrix ( const Eigen::MatrixXd &  transformation_matrix)

inverseTransformationMatrix

Parameters
transformation_matrix
Returns

Definition at line 214 of file robotis_manipulator_math.cpp.

215 {
216  // If T is Transform Matrix A from B, the BOA is translation component coordi. B to coordi. A
217 
218  Eigen::Vector3d vec_boa;
219  Eigen::Vector3d vec_x, vec_y, vec_z;
220  Eigen::Matrix4d inv_t;
221 
222  vec_boa(0) = -transform(0,3);
223  vec_boa(1) = -transform(1,3);
224  vec_boa(2) = -transform(2,3);
225 
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);
229 
230  inv_t <<
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),
234  0, 0, 0, 1;
235 
236  return inv_t;
237 }
Eigen::Matrix3d robotis_manipulator::math::matrix3 ( double  m11,
double  m12,
double  m13,
double  m21,
double  m22,
double  m23,
double  m31,
double  m32,
double  m33 
)

matrix3

Parameters
m11
m12
m13
m21
m22
m23
m31
m32
m33
Returns

Definition at line 38 of file robotis_manipulator_math.cpp.

41 {
42  Eigen::Matrix3d temp;
43  temp << m11, m12, m13, m21, m22, m23, m31, m32, m33;
44  return temp;
45 }
Eigen::Vector3d robotis_manipulator::math::matrixLogarithm ( Eigen::Matrix3d  rotation_matrix)

matrixLogarithm

Parameters
rotation_matrix
Returns

Definition at line 318 of file robotis_manipulator_math.cpp.

319 {
320  Eigen::Matrix3d R = rotation_matrix;
321  Eigen::Vector3d l = Eigen::Vector3d::Zero();
322  Eigen::Vector3d rotation_vector = Eigen::Vector3d::Zero();
323 
324  double theta = 0.0;
325  // double diag = 0.0;
326  bool diagonal_matrix = R.isDiagonal();
327 
328  l << R(2, 1) - R(1, 2),
329  R(0, 2) - R(2, 0),
330  R(1, 0) - R(0, 1);
331  theta = atan2(l.norm(), R(0, 0) + R(1, 1) + R(2, 2) - 1);
332  // diag = R.determinant();
333 
334  if (R.isIdentity())
335  {
336  rotation_vector.setZero(3);
337  return rotation_vector;
338  }
339 
340  if (diagonal_matrix == true)
341  {
342  rotation_vector << R(0, 0) + 1, R(1, 1) + 1, R(2, 2) + 1;
343  rotation_vector = rotation_vector * M_PI_2;
344  }
345  else
346  {
347  rotation_vector = theta * (l / l.norm());
348  }
349  return rotation_vector;
350 }

Here is the caller graph for this function:

Eigen::Vector3d robotis_manipulator::math::orientationDifference ( Eigen::Matrix3d  desired_orientation,
Eigen::Matrix3d  present_orientation 
)

orientationDifference

Parameters
desired_orientation
present_orientation
Returns

Definition at line 382 of file robotis_manipulator_math.cpp.

383 {
384  Eigen::Vector3d orientation_difference;
385  orientation_difference = present_orientation * robotis_manipulator::math::matrixLogarithm(present_orientation.transpose() * desired_orientation);
386 
387  return orientation_difference;
388 }
robotis_manipulator::math::matrixLogarithm
Eigen::Vector3d matrixLogarithm(Eigen::Matrix3d rotation_matrix)
matrixLogarithm
Definition: robotis_manipulator_math.cpp:318

Here is the call graph for this function:

Here is the caller graph for this function:

Eigen::VectorXd robotis_manipulator::math::poseDifference ( Eigen::Vector3d  desired_position,
Eigen::Vector3d  present_position,
Eigen::Matrix3d  desired_orientation,
Eigen::Matrix3d  present_orientation 
)

poseDifference

Parameters
desired_position
present_position
desired_orientation
present_orientation
Returns

Definition at line 390 of file robotis_manipulator_math.cpp.

392 {
393  Eigen::Vector3d position_difference;
394  Eigen::Vector3d orientation_difference;
395  Eigen::VectorXd pose_difference(6);
396 
397  position_difference = robotis_manipulator::math::positionDifference(desired_position, present_position);
398  orientation_difference = robotis_manipulator::math::orientationDifference(desired_orientation, present_orientation);
399  pose_difference << position_difference(0), position_difference(1), position_difference(2),
400  orientation_difference(0), orientation_difference(1), orientation_difference(2);
401 
402  return pose_difference;
403 }
robotis_manipulator::math::orientationDifference
Eigen::Vector3d orientationDifference(Eigen::Matrix3d desired_orientation, Eigen::Matrix3d present_orientation)
orientationDifference
Definition: robotis_manipulator_math.cpp:382
robotis_manipulator::math::positionDifference
Eigen::Vector3d positionDifference(Eigen::Vector3d desired_position, Eigen::Vector3d present_position)
positionDifference
Definition: robotis_manipulator_math.cpp:374

Here is the call graph for this function:

Eigen::Vector3d robotis_manipulator::math::positionDifference ( Eigen::Vector3d  desired_position,
Eigen::Vector3d  present_position 
)

positionDifference

Parameters
desired_position
present_position
Returns

Definition at line 374 of file robotis_manipulator_math.cpp.

375 {
376  Eigen::Vector3d position_difference;
377  position_difference = desired_position - present_position;
378 
379  return position_difference;
380 }

Here is the caller graph for this function:

Eigen::Matrix3d robotis_manipulator::math::rodriguesRotationMatrix ( Eigen::Vector3d  axis,
double  angle 
)

rodriguesRotationMatrix

Parameters
axis
angle
Returns

Definition at line 361 of file robotis_manipulator_math.cpp.

362 {
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();
366 
367  skew_symmetric_matrix = robotis_manipulator::math::skewSymmetricMatrix(axis);
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;
372 }
robotis_manipulator::math::skewSymmetricMatrix
Eigen::Matrix3d skewSymmetricMatrix(Eigen::Vector3d v)
skewSymmetricMatrix
Definition: robotis_manipulator_math.cpp:352

Here is the call graph for this function:

double robotis_manipulator::math::sign ( double  value)

sign

Parameters
value
Returns

Definition at line 306 of file robotis_manipulator_math.cpp.

307 {
308  if (value >= 0.0)
309  {
310  return 1.0;
311  }
312  else
313  {
314  return -1.0;
315  }
316 }
Eigen::Matrix3d robotis_manipulator::math::skewSymmetricMatrix ( Eigen::Vector3d  v)

skewSymmetricMatrix

Parameters
v
Returns

Definition at line 352 of file robotis_manipulator_math.cpp.

353 {
354  Eigen::Matrix3d skew_symmetric_matrix = Eigen::Matrix3d::Zero();
355  skew_symmetric_matrix << 0, -v(2), v(1),
356  v(2), 0, -v(0),
357  -v(1), v(0), 0;
358  return skew_symmetric_matrix;
359 }

Here is the caller graph for this function:

Eigen::Vector3d robotis_manipulator::math::vector3 ( double  v1,
double  v2,
double  v3 
)

vector3

Parameters
v1
v2
v3
Returns

Definition at line 31 of file robotis_manipulator_math.cpp.

32 {
33  Eigen::Vector3d temp;
34  temp << v1, v2, v3;
35  return temp;
36 }