Takeuchi Issei / Mbed 2 deprecated 4Omunisleeve3

メカナムのコード

Dependencies:   mbed Eigen

shared/EigenCalculation/EigenCalculation.cpp@0:ee7e9405e1c7, 2021-04-14 (annotated)

Committer:
e2011220
Date:
Wed Apr 14 07:26:19 2021 +0000
Revision:
0:ee7e9405e1c7
first

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e2011220 0:ee7e9405e1c7 1 #include "EigenCalculation.h"
e2011220 0:ee7e9405e1c7 2
e2011220 0:ee7e9405e1c7 3 void EigenCalculation::invert_matrix(Eigen::Matrix3d *matrix3d)
e2011220 0:ee7e9405e1c7 4 {
e2011220 0:ee7e9405e1c7 5 Eigen::FullPivLU< Eigen::Matrix3d > lu(*matrix3d);
e2011220 0:ee7e9405e1c7 6 (*matrix3d) = lu.inverse();
e2011220 0:ee7e9405e1c7 7 }
e2011220 0:ee7e9405e1c7 8
e2011220 0:ee7e9405e1c7 9 void EigenCalculation::yawling_correct(Eigen::Vector3d *vector3d)
e2011220 0:ee7e9405e1c7 10 {
e2011220 0:ee7e9405e1c7 11 Eigen::Vector3d vector_arc;
e2011220 0:ee7e9405e1c7 12 double arc, vector_angle, radius;
e2011220 0:ee7e9405e1c7 13 double x, y, yaw;
e2011220 0:ee7e9405e1c7 14
e2011220 0:ee7e9405e1c7 15 x = (*vector3d)(EigenCalculation::X);
e2011220 0:ee7e9405e1c7 16 y = (*vector3d)(EigenCalculation::Y);
e2011220 0:ee7e9405e1c7 17 yaw = (*vector3d)(EigenCalculation::Yaw);
e2011220 0:ee7e9405e1c7 18
e2011220 0:ee7e9405e1c7 19 if(yaw == 0.0)
e2011220 0:ee7e9405e1c7 20 return;
e2011220 0:ee7e9405e1c7 21
e2011220 0:ee7e9405e1c7 22 arc = sqrt(x * x + y * y);
e2011220 0:ee7e9405e1c7 23 vector_angle = atan2(y, x);
e2011220 0:ee7e9405e1c7 24
e2011220 0:ee7e9405e1c7 25 radius = arc / yaw;
e2011220 0:ee7e9405e1c7 26
e2011220 0:ee7e9405e1c7 27 vector_arc(EigenCalculation::X) = radius * (1.0 - cos(yaw));
e2011220 0:ee7e9405e1c7 28 vector_arc(EigenCalculation::Y) = radius * sin(yaw);
e2011220 0:ee7e9405e1c7 29 vector_arc(EigenCalculation::Yaw) = yaw;
e2011220 0:ee7e9405e1c7 30
e2011220 0:ee7e9405e1c7 31 rotate_coordinate( &vector_arc, -(0.5 * M_PI - vector_angle) );
e2011220 0:ee7e9405e1c7 32
e2011220 0:ee7e9405e1c7 33 (*vector3d) = vector_arc;
e2011220 0:ee7e9405e1c7 34 }
e2011220 0:ee7e9405e1c7 35
e2011220 0:ee7e9405e1c7 36 void EigenCalculation::rotate_coordinate(Eigen::Vector3d *vector3d, double yaw)
e2011220 0:ee7e9405e1c7 37 {
e2011220 0:ee7e9405e1c7 38 Eigen::Vector3d rotated_vector;
e2011220 0:ee7e9405e1c7 39 Eigen::Vector3d axis; axis << 0,0,1;
e2011220 0:ee7e9405e1c7 40 Eigen::Matrix3d rotation3d; rotation3d = Eigen::AngleAxisd(yaw, axis);
e2011220 0:ee7e9405e1c7 41
e2011220 0:ee7e9405e1c7 42 rotated_vector = rotation3d * (*vector3d);
e2011220 0:ee7e9405e1c7 43
e2011220 0:ee7e9405e1c7 44 (*vector3d) = rotated_vector;
e2011220 0:ee7e9405e1c7 45 }