Def.hh
Go to the documentation of this file.
1 // Copyright (c) 2016 The UUV Simulator Authors.
2 // All rights reserved.
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 
18 
19 #ifndef __UUV_GAZEBO_PLUGINS_DEF_HH__
20 #define __UUV_GAZEBO_PLUGINS_DEF_HH__
21 
22 #include <vector>
23 #include <string>
24 #include <map>
25 
26 #include <gazebo/gazebo.hh>
27 
28 #include <eigen3/Eigen/Core>
29 #include <eigen3/Eigen/Geometry>
30 
32 #define RESTORING_FORCE "restoring_force"
33 #define UUV_DAMPING_FORCE "damping_force"
34 #define UUV_DAMPING_TORQUE "damping_torque"
35 #define UUV_ADDED_CORIOLIS_FORCE "coriolis_force"
36 #define UUV_ADDED_CORIOLIS_TORQUE "coriolis_torque"
37 #define UUV_ADDED_MASS_FORCE "added_mass_force"
38 #define UUV_ADDED_MASS_TORQUE "added_mass_torque"
39 
40 namespace Eigen
41 {
43 typedef Eigen::Matrix<double, 6, 6> Matrix6d;
44 
46 typedef Eigen::Matrix<double, 6, 1> Vector6d;
47 }
48 
49 namespace gazebo
50 {
51 // Pi
52 #define PI 3.14159265359
53 
55 inline std::vector<double> Str2Vector(std::string _input)
56 {
57  std::vector<double> output;
58  std::string buf;
59  std::stringstream ss(_input);
60  while (ss >> buf)
61  output.push_back(std::stod(buf));
62  return output;
63 }
64 
67 inline Eigen::Matrix3d CrossProductOperator(Eigen::Vector3d _x)
68 {
69  Eigen::Matrix3d output;
70  output << 0.0, -_x[2], _x[1], _x[2], 0.0, -_x[0], -_x[1], _x[0], 0.0;
71  return output;
72 }
73 
76 inline Eigen::Matrix3d CrossProductOperator(ignition::math::Vector3d _x)
77 {
78  Eigen::Matrix3d output;
79  output << 0.0, -_x[2], _x[1], _x[2], 0.0, -_x[0], -_x[1], _x[0], 0.0;
80  return output;
81 }
82 
83 inline Eigen::Vector3d ToEigen(const ignition::math::Vector3d &_x)
84 {
85  return Eigen::Vector3d(_x[0], _x[1], _x[2]);
86 }
87 
88 inline Eigen::Matrix3d ToEigen(const ignition::math::Matrix3d &_x)
89 {
90  Eigen::Matrix3d m;
91  m << _x(0, 0), _x(0, 1), _x(0, 2),
92  _x(1, 0), _x(1, 1), _x(1, 2),
93  _x(2, 0), _x(2, 1), _x(2, 2);
94  return m;
95 }
96 
97 inline Eigen::Vector6d EigenStack(const ignition::math::Vector3d &_x,
98  const ignition::math::Vector3d &_y)
99 {
100  Eigen::Vector3d xe = ToEigen(_x);
101  Eigen::Vector3d ye = ToEigen(_y);
102  Eigen::Vector6d out;
103  out << xe, ye;
104  return out;
105 }
106 
107 inline ignition::math::Vector3d Vec3dToGazebo(const Eigen::Vector3d &_x)
108 {
109  return ignition::math::Vector3d(_x[0], _x[1], _x[2]);
110 }
111 
112 inline ignition::math::Matrix3d Mat3dToGazebo(const Eigen::Matrix3d &_x)
113 {
114  return ignition::math::Matrix3d(_x(0, 0), _x(0, 1), _x(0, 2),
115  _x(1, 0), _x(1, 1), _x(1, 2),
116  _x(2, 0), _x(2, 1), _x(2, 2));
117 }
118 
119 }
120 
121 #endif
Definition: Def.hh:40
Eigen::Matrix3d ToEigen(const ignition::math::Matrix3d &_x)
Definition: Def.hh:88
ignition::math::Vector3d Vec3dToGazebo(const Eigen::Vector3d &_x)
Definition: Def.hh:107
Eigen::Matrix< double, 6, 1 > Vector6d
Definition of a 6 element Eigen vector.
Definition: Def.hh:46
Eigen::Matrix3d CrossProductOperator(ignition::math::Vector3d _x)
Returns the cross product operator matrix for Gazebo vectors.
Definition: Def.hh:76
Eigen::Vector6d EigenStack(const ignition::math::Vector3d &_x, const ignition::math::Vector3d &_y)
Definition: Def.hh:97
ignition::math::Matrix3d Mat3dToGazebo(const Eigen::Matrix3d &_x)
Definition: Def.hh:112
Eigen::Matrix< double, 6, 6 > Matrix6d
Definition of a 6x6 Eigen matrix.
Definition: Def.hh:43
std::vector< double > Str2Vector(std::string _input)
Conversion of a string to a double vector.
Definition: Def.hh:55


uuv_gazebo_plugins
Author(s): Musa Morena Marcusso Manhaes , Sebastian Scherer , Luiz Ricardo Douat
autogenerated on Thu Jun 18 2020 03:28:24