Gazebo Msgs

API Reference

10.3.0
Inertial.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2023 Open Source Robotics Foundation
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 #ifndef GZ_MSGS_CONVERT_INERTIAL_HH_
18 #define GZ_MSGS_CONVERT_INERTIAL_HH_
19 
20 #include <gz/msgs/convert/Pose.hh>
21 #include <gz/msgs/convert/Vector3.hh>
22 
23 // Message Headers
24 #include "gz/msgs/inertial.pb.h"
25 
26 // Data Headers
27 #include <gz/math/Inertial.hh>
28 #include <gz/math/MassMatrix3.hh>
29 
30 namespace gz::msgs {
31 // Inline bracket to help doxygen filtering.
32 inline namespace GZ_MSGS_VERSION_NAMESPACE {
33 
35 inline void Set(gz::msgs::Inertial *_msg, const gz::math::MassMatrix3d &_data)
36 {
37  _msg->set_mass(_data.Mass());
38  _msg->set_ixx(_data.Ixx());
39  _msg->set_iyy(_data.Iyy());
40  _msg->set_izz(_data.Izz());
41  _msg->set_ixy(_data.Ixy());
42  _msg->set_ixz(_data.Ixz());
43  _msg->set_iyz(_data.Iyz());
44  _msg->mutable_pose()->mutable_orientation()->set_w(1);
45 }
46 
47 inline void Set(gz::math::MassMatrix3d *_data, const gz::msgs::Inertial &_msg)
48 {
49  _data->SetMass(_msg.mass());
50  _data->SetIxx(_msg.ixx());
51  _data->SetIyy(_msg.iyy());
52  _data->SetIzz(_msg.izz());
53  _data->SetIxy(_msg.ixy());
54  _data->SetIxz(_msg.ixz());
55  _data->SetIyz(_msg.iyz());
56 }
57 
58 inline gz::msgs::Inertial Convert(const gz::math::MassMatrix3d &_data)
59 {
60  gz::msgs::Inertial ret;
61  Set(&ret, _data);
62  return ret;
63 }
64 
66 inline void Set(gz::msgs::Inertial *_msg, const gz::math::Inertiald &_data)
67 {
68  Set(_msg, _data.MassMatrix());
69  Set(_msg->mutable_pose(), _data.Pose());
70 
71  if (_data.FluidAddedMass().has_value())
72  {
73  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(0, 0));
74  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(0, 1));
75  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(0, 2));
76  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(0, 3));
77  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(0, 4));
78  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(0, 5));
79  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(1, 1));
80  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(1, 2));
81  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(1, 3));
82  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(1, 4));
83  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(1, 5));
84  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(2, 2));
85  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(2, 3));
86  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(2, 4));
87  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(2, 5));
88  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(3, 3));
89  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(3, 4));
90  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(3, 5));
91  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(4, 4));
92  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(4, 5));
93  _msg->add_fluid_added_mass(_data.FluidAddedMass().value()(5, 5));
94  }
95 }
96 
97 inline void Set(gz::math::Inertiald *_data, const gz::msgs::Inertial &_msg)
98 {
99  gz::math::MassMatrix3d to_set = _data->MassMatrix();
100  Set(&to_set, _msg);
101  _data->SetMassMatrix(to_set);
102 
103  gz::math::Pose3d pose_to_set = _data->Pose();
104  Set(&pose_to_set, _msg.pose());
105  _data->SetPose(pose_to_set);
106 
107  if (_msg.fluid_added_mass_size() == 21)
108  {
109  math::Matrix6d addedMass{
110  _msg.fluid_added_mass(0),
111  _msg.fluid_added_mass(1),
112  _msg.fluid_added_mass(2),
113  _msg.fluid_added_mass(3),
114  _msg.fluid_added_mass(4),
115  _msg.fluid_added_mass(5),
116 
117  _msg.fluid_added_mass(1),
118  _msg.fluid_added_mass(6),
119  _msg.fluid_added_mass(7),
120  _msg.fluid_added_mass(8),
121  _msg.fluid_added_mass(9),
122  _msg.fluid_added_mass(10),
123 
124  _msg.fluid_added_mass(2),
125  _msg.fluid_added_mass(7),
126  _msg.fluid_added_mass(11),
127  _msg.fluid_added_mass(12),
128  _msg.fluid_added_mass(13),
129  _msg.fluid_added_mass(14),
130 
131  _msg.fluid_added_mass(3),
132  _msg.fluid_added_mass(8),
133  _msg.fluid_added_mass(12),
134  _msg.fluid_added_mass(15),
135  _msg.fluid_added_mass(16),
136  _msg.fluid_added_mass(17),
137 
138  _msg.fluid_added_mass(4),
139  _msg.fluid_added_mass(9),
140  _msg.fluid_added_mass(13),
141  _msg.fluid_added_mass(16),
142  _msg.fluid_added_mass(18),
143  _msg.fluid_added_mass(19),
144 
145  _msg.fluid_added_mass(5),
146  _msg.fluid_added_mass(10),
147  _msg.fluid_added_mass(14),
148  _msg.fluid_added_mass(17),
149  _msg.fluid_added_mass(19),
150  _msg.fluid_added_mass(20)
151  };
152  _data->SetFluidAddedMass(addedMass);
153  }
154 }
155 
156 inline gz::msgs::Inertial Convert(const gz::math::Inertiald &_data)
157 {
158  gz::msgs::Inertial ret;
159  Set(&ret, _data);
160  return ret;
161 }
162 
163 inline gz::math::Inertiald Convert(const gz::msgs::Inertial &_msg)
164 {
165  gz::math::Inertiald ret;
166  Set(&ret, _msg);
167  return ret;
168 }
169 } // namespce
170 } // namespace gz::msgs
171 #endif // GZ_MSGS_CONVERT_VECTOR3_HH_