robotis_linear_algebra.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2018 ROBOTIS CO., LTD.
3 *
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
9 *
10 * Unless required by applicable law or agreed to in writing, software
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *******************************************************************************/
16
17 /*
18  * robotis_linear_algebra.cpp
19  *
20  * Created on: June 7, 2016
21  * Author: SCH
22  */
23
25
26 namespace robotis_framework
27 {
28
29 Eigen::Vector3d getTransitionXYZ(double position_x, double position_y, double position_z)
30 {
31  Eigen::Vector3d position;
32
33  position <<
34  position_x,
35  position_y,
36  position_z;
37
38  return position;
39 }
40
41 Eigen::Matrix4d getTransformationXYZRPY(double position_x, double position_y, double position_z , double roll , double pitch , double yaw)
42 {
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;
47
48  return transformation;
49 }
50
51 Eigen::Matrix4d getInverseTransformation(const Eigen::MatrixXd& transform)
52 {
53  // If T is Transform Matrix A from B, the BOA is translation component coordi. B to coordi. A
54
55  Eigen::Vector3d vec_boa;
56  Eigen::Vector3d vec_x, vec_y, vec_z;
57  Eigen::Matrix4d inv_t;
58
59  vec_boa(0) = -transform(0,3);
60  vec_boa(1) = -transform(1,3);
61  vec_boa(2) = -transform(2,3);
62
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);
66
67  inv_t <<
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),
71  0, 0, 0, 1;
72
73  return inv_t;
74 }
75
76 Eigen::Matrix3d getInertiaXYZ(double ixx, double ixy, double ixz , double iyy , double iyz, double izz)
77 {
78  Eigen::Matrix3d inertia;
79
80  inertia <<
81  ixx, ixy, ixz,
82  ixy, iyy, iyz,
83  ixz, iyz, izz;
84
85  return inertia;
86 }
87
88 Eigen::Matrix3d getRotationX(double angle)
89 {
90  Eigen::Matrix3d rotation(3,3);
91
92  rotation <<
93  1.0, 0.0, 0.0,
94  0.0, cos(angle), -sin(angle),
95  0.0, sin(angle), cos(angle);
96
97  return rotation;
98 }
99
100 Eigen::Matrix3d getRotationY(double angle)
101 {
102  Eigen::Matrix3d rotation(3,3);
103
104  rotation <<
105  cos(angle), 0.0, sin(angle),
106  0.0, 1.0, 0.0,
107  -sin(angle), 0.0, cos(angle);
108
109  return rotation;
110 }
111
112 Eigen::Matrix3d getRotationZ(double angle)
113 {
114  Eigen::Matrix3d rotation(3,3);
115
116  rotation <<
117  cos(angle), -sin(angle), 0.0,
118  sin(angle), cos(angle), 0.0,
119  0.0, 0.0, 1.0;
120
121  return rotation;
122 }
123
124 Eigen::Matrix4d getRotation4d(double roll, double pitch, double yaw )
125 {
126  double sr = sin(roll), cr = cos(roll);
127  double sp = sin(pitch), cp = cos(pitch);
128  double sy = sin(yaw), cy = cos(yaw);
129
130  Eigen::Matrix4d mat_roll;
131  Eigen::Matrix4d mat_pitch;
132  Eigen::Matrix4d mat_yaw;
133
134  mat_roll <<
135  1, 0, 0, 0,
136  0, cr, -sr, 0,
137  0, sr, cr, 0,
138  0, 0, 0, 1;
139
140  mat_pitch <<
141  cp, 0, sp, 0,
142  0, 1, 0, 0,
143  -sp, 0, cp, 0,
144  0, 0, 0, 1;
145
146  mat_yaw <<
147  cy, -sy, 0, 0,
148  sy, cy, 0, 0,
149  0, 0, 1, 0,
150  0, 0, 0, 1;
151
152  Eigen::Matrix4d mat_rpy = (mat_yaw*mat_pitch)*mat_roll;
153
154  return mat_rpy;
155 }
156
157 Eigen::Matrix4d getTranslation4D(double position_x, double position_y, double position_z)
158 {
159  Eigen::Matrix4d mat_translation;
160
161  mat_translation <<
162  1, 0, 0, position_x,
163  0, 1, 0, position_y,
164  0, 0, 1, position_z,
165  0, 0, 0, 1;
166
167  return mat_translation;
168 }
169
170 Eigen::Vector3d convertRotationToRPY(const Eigen::Matrix3d& rotation)
171 {
172  Eigen::Vector3d rpy;// = Eigen::MatrixXd::Zero(3,1);
173
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));
177
178  return rpy;
179 }
180
181 Eigen::Matrix3d convertRPYToRotation(double roll, double pitch, double yaw)
182 {
183  Eigen::Matrix3d rotation = getRotationZ(yaw)*getRotationY(pitch)*getRotationX(roll);
184
185  return rotation;
186 }
187
188 Eigen::Quaterniond convertRPYToQuaternion(double roll, double pitch, double yaw)
189 {
190  Eigen::Quaterniond quaternion;
191  quaternion = convertRPYToRotation(roll,pitch,yaw);
192
193  return quaternion;
194 }
195
196 Eigen::Quaterniond convertRotationToQuaternion(const Eigen::Matrix3d& rotation)
197 {
198  Eigen::Quaterniond quaternion;
199  quaternion = rotation;
200
201  return quaternion;
202 }
203
204 Eigen::Vector3d convertQuaternionToRPY(const Eigen::Quaterniond& quaternion)
205 {
206  Eigen::Vector3d rpy = convertRotationToRPY(quaternion.toRotationMatrix());
207
208  return rpy;
209 }
210
211 Eigen::Matrix3d convertQuaternionToRotation(const Eigen::Quaterniond& quaternion)
212 {
213  Eigen::Matrix3d rotation = quaternion.toRotationMatrix();
214
215  return rotation;
216 }
217
218 Eigen::Matrix3d calcHatto(const Eigen::Vector3d& matrix3d)
219 {
220  Eigen::MatrixXd hatto(3,3);
221
222  hatto <<
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;
226
227  return hatto;
228 }
229
230 Eigen::Matrix3d calcRodrigues(const Eigen::Matrix3d& hat_matrix, double angle)
231 {
232 // Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(3,3);
233 // Eigen::MatrixXd rodrigues = identity+hat_matrix*sin(angle)+hat_matrix*hat_matrix*(1-cos(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;
238
239  return rodrigues;
240 }
241
242 Eigen::Vector3d convertRotToOmega(const Eigen::Matrix3d& rotation)
243 {
244  double eps = 1e-10;
245
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 );
248
249  Eigen::Vector3d rot_to_omega;
250
251  if( alpha_dash < eps )
252  {
253  rot_to_omega <<
254  0.0,
255  0.0,
256  0.0;
257  }
258  else
259  {
260  double theta = acos(alpha);
261
262  rot_to_omega <<
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);
266
267  rot_to_omega = 0.5*(theta/sin(theta))*rot_to_omega;
268  }
269
270  return rot_to_omega;
271 }
272
273 Eigen::Vector3d calcCross(const Eigen::Vector3d& vector3d_a, const Eigen::Vector3d& vector3d_b)
274 {
275  Eigen::Vector3d cross;
276
277  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);
281
282  return cross;
283 }
284
285 double calcInner(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b)
286 {
287  return vector3d_a.dot(vector3d_b);
288 }
289
290 Pose3D getPose3DfromTransformMatrix(const Eigen::Matrix4d& transform)
291 {
292  Pose3D pose_3d;
293
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));
300
301  return pose_3d;
302 }
303
304 }
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)

robotis_math
Author(s): SCH , Kayman , Jay Song
autogenerated on Fri Jul 17 2020 03:17:50