-
Notifications
You must be signed in to change notification settings - Fork 33
/
Simulator.tpp
155 lines (131 loc) · 5.55 KB
/
Simulator.tpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
/**
* @file Simulator.tpp
* @authors Giulio Romualdi
* @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
*/
#ifndef BIPEDAL_LOCOMOTION_CONTROLLERS_SIMULATOR_SIMULATOR_TPP
#define BIPEDAL_LOCOMOTION_CONTROLLERS_SIMULATOR_SIMULATOR_TPP
#include <BipedalLocomotionControllers/Simulator/Simulator.h>
namespace BipedalLocomotionControllers
{
namespace Simulator
{
template <class Derived>
bool Simulator::initialize(std::weak_ptr<ParametersHandler::IParametersHandler<Derived>> handlerWeak)
{
auto handler = handlerWeak.lock();
if (handler == nullptr)
{
std::cerr << "[Simulator::initialize] The parameter handler is not valid." << std::endl;
return false;
}
if (m_state != State::NotInitialized)
{
std::cerr << "[Simulator::initialize] The simulator has been already initialized."
<< std::endl;
return false;
}
// get contact models parameters
if (!handler->getParameter("length", m_length) || !handler->getParameter("width", m_width)
|| !handler->getParameter("spring_coeff", m_springCoeff)
|| !handler->getParameter("damper_coeff", m_damperCoeff))
{
std::cerr << "[Simulator::initialize] Unable to get the contact parameters." << std::endl;
return false;
}
const std::unordered_map<std::string, std::any> parameters({{"length", m_length},
{"width", m_width},
{"spring_coeff", m_springCoeff},
{"damper_coeff", m_damperCoeff}});
m_leftContact.model = std::make_unique<ContactModels::ContinuousContactModel>(parameters);
m_rightContact.model = std::make_unique<ContactModels::ContinuousContactModel>(parameters);
// get the contact frames
std::string footFrame;
if (!handler->getParameter("left_foot_frame", footFrame))
{
std::cerr << "[Simulator::initialize] Unable to get the frame name." << std::endl;
return false;
}
m_leftContact.indexInTheModel = m_kinDyn.model().getFrameIndex(footFrame);
if (m_leftContact.indexInTheModel == iDynTree::FRAME_INVALID_INDEX)
{
std::cerr << "[Simulator::initialize] Unable to find the frame named: " << footFrame
<< std::endl;
return false;
}
if (!handler->getParameter("right_foot_frame", footFrame))
{
std::cerr << "[Simulator::initialize] Unable to get the frame name." << std::endl;
return false;
}
m_rightContact.indexInTheModel = m_kinDyn.model().getFrameIndex(footFrame);
if (m_rightContact.indexInTheModel == iDynTree::FRAME_INVALID_INDEX)
{
std::cerr << "[Simulator::initialize] Unable to find the frame named: " << footFrame
<< std::endl;
return false;
}
if (!setBaseFrame(m_leftContact.indexInTheModel, "left_foot"))
{
std::cerr << "[Simulator::initialize] Unable to set the leftFootFrame." << std::endl;
return false;
}
// set the right foot frame
if (!setBaseFrame(m_rightContact.indexInTheModel, "right_foot"))
{
std::cerr << "[Simulator::initialize] Unable to set the rightFootFrame." << std::endl;
return false;
}
// get the base frame
std::string baseFrame;
if (!handler->getParameter("base_frame", baseFrame))
{
std::cerr << "[Simulator::initialize] Unable to get the frame name." << std::endl;
return false;
}
m_baseFrame = m_kinDyn.model().getFrameIndex(baseFrame);
if (!setBaseFrame(m_baseFrame, "root"))
{
std::cerr << "[Simulator::initialize] Unable to set the root frame." << std::endl;
return false;
}
if (!handler->getParameter("sampling_time", m_dT))
{
std::cerr << "[Simulator::initialize] Unable to get the sampling time." << std::endl;
return false;
}
std::string controlMode;
if (!handler->getParameter("control_mode", controlMode))
{
std::cerr << "[Simulator::initialize] Unable to get the control mode." << std::endl;
return false;
}
if (controlMode == "torque")
m_controlMode = ControlMode::Torque;
else if (controlMode == "acceleration")
m_controlMode = ControlMode::Acceleration;
else if (controlMode == "velocity")
m_controlMode = ControlMode::Velocity;
else
{
std::cerr << "[Simulator::initialize] The control mode selected is not supported. The "
"supported control mode are: \"torque\", \"acceleration\" and \"velocity\" "
<< std::endl;
return false;
}
// instantiate the integrates
m_jointVelocityIntegrator = std::make_unique<Integrator<iDynTree::VectorDynSize>>(m_dT);
m_jointPositionIntegrator = std::make_unique<Integrator<iDynTree::VectorDynSize>>(m_dT);
m_baseLinearVelocityIntegrator
= std::make_unique<Integrator<iDynTree::LinearMotionVector3>>(m_dT);
m_baseAngularVelocityIntegrator
= std::make_unique<Integrator<iDynTree::AngularMotionVector3>>(m_dT);
m_basePositionIntegrator = std::make_unique<Integrator<iDynTree::Vector3>>(m_dT);
m_baseRotationIntegrator = std::make_unique<Integrator<iDynTree::Matrix3x3>>(m_dT);
m_state = State::Initialized;
return true;
}
} // namespace Simulator
} // namespace BipedalLocomotionControllers
#endif // BIPEDAL_LOCOMOTION_CONTROLLERS_SIMULATOR_SIMULATOR_TPP