robotis_linear_algebra.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2018 ROBOTIS CO., LTD.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
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