Gazebo Msgs

API Reference

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