-
Notifications
You must be signed in to change notification settings - Fork 25
/
IgnitionRobot.cpp
703 lines (565 loc) · 22.8 KB
/
IgnitionRobot.cpp
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
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
/*
* Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT)
* All rights reserved.
*
* This software may be modified and distributed under the terms of the
* GNU Lesser General Public License v2.1 or any later version.
*/
#include "gympp/gazebo/IgnitionRobot.h"
#include "gympp/Log.h"
#include "gympp/gazebo/RobotSingleton.h"
#include "gympp/gazebo/components/JointPositionReset.h"
#include "gympp/gazebo/components/JointVelocityReset.h"
#include <ignition/gazebo/Model.hh>
#include <ignition/gazebo/components/Joint.hh>
#include <ignition/gazebo/components/JointForceCmd.hh>
#include <ignition/gazebo/components/JointPosition.hh>
#include <ignition/gazebo/components/JointType.hh>
#include <ignition/gazebo/components/JointVelocity.hh>
#include <ignition/gazebo/components/Link.hh>
#include <ignition/gazebo/components/Name.hh>
#include <ignition/gazebo/components/ParentEntity.hh>
#include <ignition/gazebo/components/Pose.hh>
#include <ignition/math/PID.hh>
#include <sdf/Joint.hh>
#include <cassert>
#include <chrono>
#include <map>
#include <ostream>
#include <string>
using namespace gympp::gazebo;
using LinkName = std::string;
using JointName = std::string;
using LinkEntity = ignition::gazebo::Entity;
using JointEntity = ignition::gazebo::Entity;
const ignition::math::PID DefaultPID(1, 0.1, 0.01, 1, -1, 10000, -10000);
enum class ControlType
{
Position,
Velocity
};
struct PIDData
{
ControlType type;
ignition::math::PID pid;
};
struct Buffers
{
struct
{
gympp::Robot::JointPositions positions;
gympp::Robot::JointVelocities velocities;
std::map<JointName, double> references;
std::map<JointName, double> appliedForces;
std::map<JointName, PIDData> pidData;
} joints;
};
class IgnitionRobot::Impl
{
public:
std::string name;
ignition::gazebo::EntityComponentManager* ecm = nullptr;
ignition::gazebo::Model model;
std::chrono::duration<double> dt;
std::chrono::duration<double> prevUpdateTime = std::chrono::duration<double>(0.0);
Buffers buffers;
std::map<LinkName, LinkEntity> links;
std::map<JointName, JointEntity> joints;
inline bool jointExists(const JointName& jointName) const
{
return joints.find(jointName) != joints.end();
}
inline bool pidExists(const JointName& jointName) const
{
return buffers.joints.pidData.find(jointName) != buffers.joints.pidData.end();
}
inline bool linkExists(const LinkName& linkName) const
{
return links.find(linkName) != links.end();
}
JointEntity getLinkEntity(const LinkName& linkName);
JointEntity getJointEntity(const JointName& jointName);
template <typename ComponentType>
ComponentType& getOrCreateComponent(const ignition::gazebo::Entity entity);
};
LinkEntity IgnitionRobot::Impl::getLinkEntity(const LinkName& linkName)
{
if (!ecm) {
gymppError << "Failed to get the entity-component mananger" << std::endl;
return ignition::gazebo::kNullEntity;
}
if (!linkExists(linkName)) {
gymppError << "Link '" << linkName << "' not found" << std::endl;
return ignition::gazebo::kNullEntity;
}
if (links[linkName] == ignition::gazebo::kNullEntity) {
gymppError << "The entity associated to link '" << linkName
<< "' has not been properly stored" << std::endl;
return ignition::gazebo::kNullEntity;
}
// Return the link entity
return links[linkName];
}
JointEntity IgnitionRobot::Impl::getJointEntity(const JointName& jointName)
{
if (!ecm) {
gymppError << "Failed to get the entity-component mananger" << std::endl;
return ignition::gazebo::kNullEntity;
}
if (!jointExists(jointName)) {
gymppError << "Joint '" << jointName << "' not found" << std::endl;
return ignition::gazebo::kNullEntity;
}
if (joints[jointName] == ignition::gazebo::kNullEntity) {
gymppError << "The entity associated to joint '" << jointName
<< "' has not been properly stored" << std::endl;
return ignition::gazebo::kNullEntity;
}
// Return the joint entity
return joints[jointName];
}
template <typename ComponentTypeT>
ComponentTypeT& IgnitionRobot::Impl::getOrCreateComponent(const ignition::gazebo::Entity entity)
{
auto* component = ecm->Component<ComponentTypeT>(entity);
if (!component) {
ecm->CreateComponent(entity, ComponentTypeT());
component = ecm->Component<ComponentTypeT>(entity);
}
return *component;
}
// ==============
// IGNITION ROBOT
// ==============
IgnitionRobot::IgnitionRobot()
: pImpl{new Impl(), [](Impl* impl) { delete impl; }}
{}
IgnitionRobot::~IgnitionRobot() = default;
bool IgnitionRobot::configureECM(const ignition::gazebo::Entity& entity,
const std::shared_ptr<const sdf::Element>& sdf,
ignition::gazebo::EntityComponentManager& ecm)
{
// Store the address of the entity-component manager
pImpl->ecm = &ecm;
// Create the model
pImpl->model = ignition::gazebo::Model(entity);
// Check that the model is valid
if (!pImpl->model.Valid(ecm)) {
// Create a label to identify the sdf element
std::string sdfElementString = "<" + sdf->GetName();
for (size_t i = 0; i < sdf->GetAttributeCount(); ++i) {
sdfElementString += " attr='" + sdf->GetAttribute(i)->GetAsString() + "'";
}
sdfElementString += ">";
gymppError << "The model associated to sdf element '" << sdfElementString << "is not valid"
<< std::endl;
return false;
}
gymppDebug << "Processing model '" << pImpl->model.Name(ecm) << "'" << std::endl;
// Get all the model joints
ecm.Each<ignition::gazebo::components::Joint,
ignition::gazebo::components::Name,
ignition::gazebo::components::JointType,
ignition::gazebo::components::ParentEntity>(
[&](const ignition::gazebo::Entity& entity,
ignition::gazebo::components::Joint* /*joint*/,
ignition::gazebo::components::Name* name,
ignition::gazebo::components::JointType* type,
ignition::gazebo::components::ParentEntity* parentEntityComponent) -> bool {
// Skip all the joints not belonging to this model
if (parentEntityComponent->Data() != pImpl->model.Entity()) {
return true;
}
gymppDebug << " Found joint: " << pImpl->model.Name(ecm) << "::" << name->Data()
<< " [" << entity << "]" << std::endl;
// Find the entity of the joint in the ecm
auto jointEntity = pImpl->model.JointByName(ecm, name->Data());
if (jointEntity == ignition::gazebo::kNullEntity) {
gymppError << "Failed to find entity for joint '" << pImpl->model.Name(ecm)
<< "::" << name->Data() << "'" << std::endl;
return false;
}
// Ignore fixed joints
if (type->Data() == sdf::JointType::FIXED) {
gymppDebug << " Skipping fixed joint '" << pImpl->model.Name(ecm)
<< "::" << name->Data() << "'" << std::endl;
return true;
}
// Create the joint position and velocity components.
// In this way this data is stored in these components after the physics step.
ecm.CreateComponent(entity, ignition::gazebo::components::JointPosition());
ecm.CreateComponent(entity, ignition::gazebo::components::JointVelocity());
// Store the joint entity
pImpl->joints[name->Data()] = jointEntity;
return true;
});
// Get all the model links
ecm.Each<ignition::gazebo::components::Link,
ignition::gazebo::components::Name,
ignition::gazebo::components::Pose,
ignition::gazebo::components::ParentEntity>(
[&](const ignition::gazebo::Entity& entity,
ignition::gazebo::components::Link* /*link*/,
ignition::gazebo::components::Name* name,
ignition::gazebo::components::Pose* /*pose*/,
ignition::gazebo::components::ParentEntity* parentEntityComponent) -> bool {
// Skip all the joints not belonging to this model
if (parentEntityComponent->Data() != pImpl->model.Entity()) {
return true;
}
gymppDebug << " Found link: " << pImpl->model.Name(ecm) << "::" << name->Data() << " ["
<< entity << "]" << std::endl;
// TODO: there is an extra link 'link', I suspect related to the <include><pose>
if (name->Data() == "link") {
gymppDebug << " Skipping dummy link 'link'" << std::endl;
return true;
}
// Find the entity of the link in the ecm
auto linkEntity = pImpl->model.LinkByName(ecm, name->Data());
if (linkEntity == ignition::gazebo::kNullEntity) {
gymppError << "Failed to find entity for link '" << pImpl->model.Name(ecm)
<< "::" << name->Data() << "'" << std::endl;
return false;
}
// Store the link entity
pImpl->links[name->Data()] = linkEntity;
return true;
});
// Check that the created object is valid
if (!valid()) {
gymppError << "The IgnitionRobot object for model '" << pImpl->model.Name(ecm)
<< "' is not valid" << std::endl;
return false;
}
// Store the name of the robot
pImpl->name = pImpl->model.Name(ecm);
if (pImpl->name.empty()) {
gymppError << "The model entity has an empty name component" << std::endl;
return false;
}
// Initialize the buffers
pImpl->buffers.joints.positions.resize(pImpl->joints.size());
pImpl->buffers.joints.velocities.resize(pImpl->joints.size());
return true;
}
bool IgnitionRobot::valid() const
{
// TODO: find the proper logic to check if this object is valid
if (!pImpl->ecm) {
return false;
}
if (pImpl->joints.size() == 0) {
return false;
}
if (pImpl->links.size() == 0) {
return false;
}
return true;
}
// ===========
// GET METHODS
// ===========
gympp::Robot::RobotName IgnitionRobot::name() const
{
return pImpl->name;
}
gympp::Robot::JointNames IgnitionRobot::jointNames() const
{
JointNames names;
names.reserve(pImpl->joints.size());
for (const auto& [jointName, _] : pImpl->joints) {
names.push_back(jointName);
}
return names;
}
double IgnitionRobot::jointPosition(const gympp::Robot::JointName& jointName) const
{
JointEntity jointEntity = pImpl->getJointEntity(jointName);
assert(jointEntity != ignition::gazebo::kNullEntity);
// Get the joint position component
auto jointPositionComponent =
pImpl->ecm->Component<ignition::gazebo::components::JointPosition>(jointEntity);
if (!jointPositionComponent) {
gymppError << "Position for joint '" << jointName << "' not found in the ecm" << std::endl;
return {};
}
if (jointPositionComponent->Data().size() <= 0) {
gymppWarning << "The joint position component exists but it does not have yet any data"
<< std::endl;
return {};
}
assert(jointPositionComponent->Data().size() == 1);
return jointPositionComponent->Data()[0];
}
double IgnitionRobot::jointVelocity(const gympp::Robot::JointName& jointName) const
{
JointEntity jointEntity = pImpl->getJointEntity(jointName);
assert(jointEntity != ignition::gazebo::kNullEntity);
// Get the joint velocity component
auto jointVelocityComponent =
pImpl->ecm->Component<ignition::gazebo::components::JointVelocity>(jointEntity);
if (!jointVelocityComponent) {
gymppError << "Velocity for joint '" << jointName << "' not found in the ecm" << std::endl;
return {};
}
if (jointVelocityComponent->Data().size() <= 0) {
gymppWarning << "The joint velocity component exists but it does not have yet any data"
<< std::endl;
return {};
}
assert(jointVelocityComponent->Data().size() == 1);
return jointVelocityComponent->Data()[0];
}
gympp::Robot::JointPositions IgnitionRobot::jointPositions() const
{
size_t i = 0;
for (const auto& [jointName, _] : pImpl->joints) {
pImpl->buffers.joints.positions[i++] = jointPosition(jointName);
}
return pImpl->buffers.joints.positions;
}
gympp::Robot::JointVelocities IgnitionRobot::jointVelocities() const
{
size_t i = 0;
for (const auto& [jointName, _] : pImpl->joints) {
pImpl->buffers.joints.velocities[i++] = jointVelocity(jointName);
}
return pImpl->buffers.joints.velocities;
}
gympp::Robot::StepSize IgnitionRobot::dt() const
{
return pImpl->dt;
}
IgnitionRobot::PID IgnitionRobot::jointPID(const gympp::Robot::JointName& jointName) const
{
assert(pImpl->jointExists(jointName));
assert(pImpl->pidExists(jointName));
auto& pid = pImpl->buffers.joints.pidData[jointName].pid;
return PID(pid.PGain(), pid.IGain(), pid.DGain());
}
bool IgnitionRobot::setdt(const gympp::Robot::StepSize& stepSize)
{
pImpl->dt = stepSize;
return true;
}
// ===========
// SET METHODS
// ===========
bool IgnitionRobot::setJointForce(const gympp::Robot::JointName& jointName, const double jointForce)
{
JointEntity jointEntity = pImpl->getJointEntity(jointName);
if (jointEntity == ignition::gazebo::kNullEntity) {
return false;
}
// Get the JointForce component
auto& forceComponent =
pImpl->getOrCreateComponent<ignition::gazebo::components::JointForceCmd>(jointEntity);
// Set the joint force
forceComponent = ignition::gazebo::components::JointForceCmd({jointForce});
return true;
}
bool IgnitionRobot::setJointPositionTarget(const gympp::Robot::JointName& jointName,
const double jointPositionReference)
{
// The controller period must have been set in order to set references
if (pImpl->dt == std::chrono::duration<double>(0.0)) {
gymppError << "The update time of the controlled was not set" << std::endl;
return false;
}
JointEntity jointEntity = pImpl->getJointEntity(jointName);
if (jointEntity == ignition::gazebo::kNullEntity) {
return false;
}
// Create the PID if it does not exist
if (!pImpl->pidExists(jointName)) {
pImpl->buffers.joints.pidData[jointName] = {ControlType::Position, DefaultPID};
}
// Check the control type and switch to position control if the joint was controlled differently
if (pImpl->buffers.joints.pidData[jointName].type != ControlType::Position) {
gymppDebug << "Switching joint '" << jointName << "' to Position control" << std::endl;
pImpl->buffers.joints.pidData[jointName].type = ControlType::Position;
// Reset the PID
assert(pImpl->pidExists(jointName));
pImpl->buffers.joints.pidData[jointName].pid.Reset();
}
// Update the joint reference
pImpl->buffers.joints.references[jointName] = jointPositionReference;
return true;
}
bool IgnitionRobot::setJointVelocityTarget(const gympp::Robot::JointName& jointName,
const double jointVelocityReference)
{
// The controller period must have been set in order to set references
if (pImpl->dt == std::chrono::duration<double>(0.0)) {
gymppError << "The update time of the controlled was not set" << std::endl;
return false;
}
JointEntity jointEntity = pImpl->getJointEntity(jointName);
if (jointEntity == ignition::gazebo::kNullEntity) {
return false;
}
// Create the PID if it does not exist
if (!pImpl->pidExists(jointName)) {
pImpl->buffers.joints.pidData[jointName] = {ControlType::Velocity, DefaultPID};
}
// Check the control type and switch to velocity control if the joint was controlled differently
if (pImpl->buffers.joints.pidData[jointName].type != ControlType::Velocity) {
gymppDebug << "Switching joint '" << jointName << "' to Velocity control" << std::endl;
pImpl->buffers.joints.pidData[jointName].type = ControlType::Velocity;
// Reset the PID
assert(pImpl->pidExists(jointName));
pImpl->buffers.joints.pidData[jointName].pid.Reset();
}
// Update the joint reference
pImpl->buffers.joints.references[jointName] = jointVelocityReference;
return true;
}
bool IgnitionRobot::setJointPosition(const gympp::Robot::JointName& jointName,
const double jointPosition)
{
JointEntity jointEntity = pImpl->getJointEntity(jointName);
if (jointEntity == ignition::gazebo::kNullEntity) {
return false;
}
// Reset the position
auto& jointPosResetComponent =
pImpl->getOrCreateComponent<ignition::gazebo::components::JointPositionReset>(jointEntity);
jointPosResetComponent = ignition::gazebo::components::JointPositionReset({jointPosition});
// Store the new position in the ECM
auto& jointPosComponent =
pImpl->getOrCreateComponent<ignition::gazebo::components::JointPosition>(jointEntity);
jointPosComponent = ignition::gazebo::components::JointPosition({jointPosition});
return true;
}
bool IgnitionRobot::setJointVelocity(const gympp::Robot::JointName& jointName,
const double jointVelocity)
{
JointEntity jointEntity = pImpl->getJointEntity(jointName);
if (jointEntity == ignition::gazebo::kNullEntity) {
return false;
}
// Reset the velocity
auto& jointVelResetComponent =
pImpl->getOrCreateComponent<ignition::gazebo::components::JointVelocityReset>(jointEntity);
jointVelResetComponent = ignition::gazebo::components::JointVelocityReset({jointVelocity});
// Store the new velocity in the ECM
auto& jointVelComponent =
pImpl->getOrCreateComponent<ignition::gazebo::components::JointVelocity>(jointEntity);
jointVelComponent = ignition::gazebo::components::JointVelocity({jointVelocity});
return true;
}
bool IgnitionRobot::setJointPID(const gympp::Robot::JointName& jointName, const PID& pid)
{
JointEntity jointEntity = pImpl->getJointEntity(jointName);
if (jointEntity == ignition::gazebo::kNullEntity) {
return false;
}
if (!pImpl->pidExists(jointName)) {
gymppDebug << "Creating new PID for joint " << jointName << std::endl;
pImpl->buffers.joints.pidData[jointName] = {ControlType::Position, DefaultPID};
}
else {
pImpl->buffers.joints.pidData[jointName].pid.Reset();
}
// Update the gains. The other PID parameters do not change.
pImpl->buffers.joints.pidData[jointName].pid.SetPGain(pid.p);
pImpl->buffers.joints.pidData[jointName].pid.SetIGain(pid.i);
pImpl->buffers.joints.pidData[jointName].pid.SetDGain(pid.d);
return true;
}
bool IgnitionRobot::resetJoint(const gympp::Robot::JointName& jointName,
const double jointPosition,
const double jointVelocity)
{
JointEntity jointEntity = pImpl->getJointEntity(jointName);
if (jointEntity == ignition::gazebo::kNullEntity) {
return false;
}
// Reset the joint position
if (!setJointPosition(jointName, jointPosition)) {
gymppError << "Failed to reset the joint position of joint '" << jointName << "'"
<< std::endl;
return false;
}
// Reset the joint velocity
if (!setJointVelocity(jointName, jointVelocity)) {
gymppError << "Failed to reset the joint velocity of joint '" << jointName << "'"
<< std::endl;
return false;
}
// Reset the PID
if (pImpl->pidExists(jointName)) {
pImpl->buffers.joints.pidData[jointName].pid.Reset();
pImpl->buffers.joints.pidData[jointName].type = ControlType::Position;
}
// Clean the joint controlling storage
pImpl->buffers.joints.references.erase(jointName);
return true;
}
bool IgnitionRobot::update(const std::chrono::duration<double> time)
{
// Return if there are no references to actuate
if (pImpl->buffers.joints.references.empty()) {
return true;
}
// The controller period must have been set in order to use PIDs
if (pImpl->dt == std::chrono::duration<double>(0.0)) {
gymppError << "The update time of the controlled was not set" << std::endl;
return false;
}
// Update the controller only if enough time is passed
std::chrono::duration<double> stepTime = time - pImpl->prevUpdateTime;
// Handle first iteration
if (pImpl->prevUpdateTime == std::chrono::duration<double>(0.0)) {
stepTime = pImpl->dt;
}
// If enough time is passed, store the time of this actuation step. In this case the state of
// the robot is read and new force references are computed and actuated.
// Otherwise, the same force of the last step is actuated.
bool updateCurrentState;
if (stepTime >= pImpl->dt) {
// Store the current update time
pImpl->prevUpdateTime = time;
// Enable using the PID to compute the new force
updateCurrentState = true;
}
else {
// Disable the PID and send the same force reference as last update
updateCurrentState = false;
}
// Actuate the references
// The references can be either position or velocity references
for (auto& [jointName, reference] : pImpl->buffers.joints.references) {
assert(pImpl->pidExists(jointName));
// Use the PID the compute the new force
if (updateCurrentState) {
double force = 0;
// Get the PID
auto& pid = pImpl->buffers.joints.pidData[jointName].pid;
// Use the PID to get the reference
switch (pImpl->buffers.joints.pidData[jointName].type) {
case ControlType::Position:
force = pid.Update(jointPosition(jointName) - reference, stepTime);
break;
case ControlType::Velocity:
force = pid.Update(jointVelocity(jointName) - reference, stepTime);
break;
}
// Store the force
pImpl->buffers.joints.appliedForces[jointName] = force;
}
// Break if there is no force to actuate for this joint
if (pImpl->buffers.joints.appliedForces.find(jointName)
== pImpl->buffers.joints.appliedForces.end()) {
break;
}
// Get the force
auto force = pImpl->buffers.joints.appliedForces[jointName];
// Actuate the force
if (!setJointForce(jointName, force)) {
gymppError << "Failed to set force to joint '" << jointName << "'" << std::endl;
return false;
}
}
return true;
}