Skip to content

Commit 707aa69

Browse files
iche033iche033
authored
bullet-featherstone: Enforce joint velocity and effort limits for velocity control commands (#658)
Signed-off-by: Ian Chen <ichen@openrobotics.org>
1 parent f54d4de commit 707aa69

10 files changed

+270
-35
lines changed

‎bullet-featherstone/README.md

+6-3
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22

33
This component enables the use of this [Bullet Physics](https://github.com/bulletphysics/bullet3) `btMultiBody` Featherstone implementation.
44
The Featherstone uses minimal coordinates to represent articulated bodies and efficiently simulate them.
5-
5+
66
# Features
77

88
Originally tracked via: [Bullet featherstone meta-ticket](https://github.com/gazebosim/gz-physics/issues/423)
@@ -11,7 +11,7 @@ Originally tracked via: [Bullet featherstone meta-ticket](https://github.com/gaz
1111

1212
* Constructing SDF Models
1313
* Constructing SDF Worlds
14-
* Joint Types:
14+
* Joint Types:
1515
* Fixed
1616
* Prismatic
1717
* Revolute
@@ -25,7 +25,7 @@ Originally tracked via: [Bullet featherstone meta-ticket](https://github.com/gaz
2525

2626
These are the specific physics API features implemented.
2727

28-
* Entity Management Features
28+
* Entity Management Features
2929
* ConstructEmptyWorldFeature
3030
* GetEngineInfo
3131
* GetJointFromModel
@@ -47,6 +47,9 @@ These are the specific physics API features implemented.
4747
* SetBasicJointState
4848
* GetBasicJointProperties
4949
* SetJointVelocityCommandFeature
50+
* SetJointPositionLimitsFeature,
51+
* SetJointVelocityLimitsFeature,
52+
* SetJointEffortLimitsFeature,
5053
* SetJointTransformFromParentFeature
5154
* AttachFixedJointFeature
5255
* DetachJointFeature

‎bullet-featherstone/src/Base.hh

+11-1
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,8 @@ struct WorldInfo
7676
std::unordered_map<std::string, std::size_t> modelNameToEntityId;
7777
int nextModelIndex = 0;
7878

79+
double stepSize = 0.001;
80+
7981
explicit WorldInfo(std::string name);
8082
};
8183

@@ -189,7 +191,15 @@ struct JointInfo
189191
Identity model;
190192
// This field gets set by AddJoint
191193
std::size_t indexInGzModel = 0;
192-
btScalar effort = 0;
194+
195+
// joint limits
196+
// \todo(iche033) Extend to support joints with more than 1 dof
197+
double minEffort = 0.0;
198+
double maxEffort = 0.0;
199+
double minVelocity = 0.0;
200+
double maxVelocity = 0.0;
201+
double axisLower = 0.0;
202+
double axisUpper = 0.0;
193203

194204
std::shared_ptr<btMultiBodyJointMotor> motor = nullptr;
195205
std::shared_ptr<btMultiBodyJointLimitConstraint> jointLimits = nullptr;

‎bullet-featherstone/src/JointFeatures.cc

+167-4
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,8 @@
1818
#include "JointFeatures.hh"
1919

2020
#include <algorithm>
21+
#include <cmath>
22+
#include <cstddef>
2123
#include <memory>
2224
#include <unordered_map>
2325

@@ -28,6 +30,29 @@ namespace gz {
2830
namespace physics {
2931
namespace bullet_featherstone {
3032

33+
/////////////////////////////////////////////////
34+
void recreateJointLimitConstraint(JointInfo *_jointInfo, ModelInfo *_modelInfo,
35+
WorldInfo *_worldInfo)
36+
{
37+
const auto *identifier = std::get_if<InternalJoint>(&_jointInfo->identifier);
38+
if (!identifier)
39+
return;
40+
41+
if (_jointInfo->jointLimits)
42+
{
43+
_worldInfo->world->removeMultiBodyConstraint(_jointInfo->jointLimits.get());
44+
_jointInfo->jointLimits.reset();
45+
}
46+
47+
_jointInfo->jointLimits =
48+
std::make_shared<btMultiBodyJointLimitConstraint>(
49+
_modelInfo->body.get(), identifier->indexInBtModel,
50+
static_cast<btScalar>(_jointInfo->axisLower),
51+
static_cast<btScalar>(_jointInfo->axisUpper));
52+
53+
_worldInfo->world->addMultiBodyConstraint(_jointInfo->jointLimits.get());
54+
}
55+
3156
/////////////////////////////////////////////////
3257
void makeColliderStatic(LinkInfo *_linkInfo)
3358
{
@@ -277,8 +302,13 @@ void JointFeatures::SetJointForce(
277302
return;
278303

279304
const auto *model = this->ReferenceInterface<ModelInfo>(joint->model);
305+
306+
// clamp the values
307+
double force = std::clamp(_value,
308+
joint->minEffort, joint->maxEffort);
309+
280310
model->body->getJointTorqueMultiDof(
281-
identifier->indexInBtModel)[_dof] = static_cast<btScalar>(_value);
311+
identifier->indexInBtModel)[_dof] = static_cast<btScalar>(force);
282312
model->body->wakeUp();
283313
}
284314

@@ -409,20 +439,153 @@ void JointFeatures::SetJointVelocityCommand(
409439
auto modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
410440
if (!jointInfo->motor)
411441
{
442+
auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
443+
// \todo(iche033) The motor constraint is created with a max impulse
444+
// computed by maxEffort * stepsize. However, our API supports
445+
// stepping sim with varying dt. We should recompute max impulse
446+
// if stepSize changes.
412447
jointInfo->motor = std::make_shared<btMultiBodyJointMotor>(
413448
modelInfo->body.get(),
414449
std::get<InternalJoint>(jointInfo->identifier).indexInBtModel,
415450
0,
416451
static_cast<btScalar>(0),
417-
static_cast<btScalar>(jointInfo->effort));
418-
auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
452+
static_cast<btScalar>(jointInfo->maxEffort * world->stepSize));
419453
world->world->addMultiBodyConstraint(jointInfo->motor.get());
420454
}
421455

422-
jointInfo->motor->setVelocityTarget(static_cast<btScalar>(_value));
456+
// clamp the values
457+
double velocity = std::clamp(_value,
458+
jointInfo->minVelocity, jointInfo->maxVelocity);
459+
460+
jointInfo->motor->setVelocityTarget(static_cast<btScalar>(velocity));
423461
modelInfo->body->wakeUp();
424462
}
425463

464+
/////////////////////////////////////////////////
465+
void JointFeatures::SetJointMinPosition(
466+
const Identity &_id, std::size_t _dof, double _value)
467+
{
468+
auto *jointInfo = this->ReferenceInterface<JointInfo>(_id);
469+
if (std::isnan(_value))
470+
{
471+
gzerr << "Invalid minimum joint position value [" << _value
472+
<< "] commanded on joint [" << jointInfo->name << " DOF " << _dof
473+
<< "]. The command will be ignored\n";
474+
return;
475+
}
476+
jointInfo->axisLower = _value;
477+
478+
auto *modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
479+
auto *worldInfo = this->ReferenceInterface<WorldInfo>(modelInfo->world);
480+
recreateJointLimitConstraint(jointInfo, modelInfo, worldInfo);
481+
}
482+
483+
/////////////////////////////////////////////////
484+
void JointFeatures::SetJointMaxPosition(
485+
const Identity &_id, std::size_t _dof, double _value)
486+
{
487+
auto *jointInfo = this->ReferenceInterface<JointInfo>(_id);
488+
if (std::isnan(_value))
489+
{
490+
gzerr << "Invalid maximum joint position value [" << _value
491+
<< "] commanded on joint [" << jointInfo->name << " DOF " << _dof
492+
<< "]. The command will be ignored\n";
493+
return;
494+
}
495+
496+
jointInfo->axisUpper = _value;
497+
498+
auto *modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
499+
auto *worldInfo = this->ReferenceInterface<WorldInfo>(modelInfo->world);
500+
recreateJointLimitConstraint(jointInfo, modelInfo, worldInfo);
501+
}
502+
503+
/////////////////////////////////////////////////
504+
void JointFeatures::SetJointMinVelocity(
505+
const Identity &_id, std::size_t _dof, double _value)
506+
{
507+
auto *jointInfo = this->ReferenceInterface<JointInfo>(_id);
508+
if (std::isnan(_value))
509+
{
510+
gzerr << "Invalid minimum joint velocity value [" << _value
511+
<< "] commanded on joint [" << jointInfo->name << " DOF " << _dof
512+
<< "]. The command will be ignored\n";
513+
return;
514+
}
515+
516+
jointInfo->minVelocity = _value;
517+
}
518+
519+
/////////////////////////////////////////////////
520+
void JointFeatures::SetJointMaxVelocity(
521+
const Identity &_id, std::size_t _dof, double _value)
522+
{
523+
auto *jointInfo = this->ReferenceInterface<JointInfo>(_id);
524+
if (std::isnan(_value))
525+
{
526+
gzerr << "Invalid maximum joint velocity value [" << _value
527+
<< "] commanded on joint [" << jointInfo->name << " DOF " << _dof
528+
<< "]. The command will be ignored\n";
529+
return;
530+
}
531+
532+
jointInfo->maxVelocity = _value;
533+
}
534+
535+
/////////////////////////////////////////////////
536+
void JointFeatures::SetJointMinEffort(
537+
const Identity &_id, std::size_t _dof, double _value)
538+
{
539+
auto *jointInfo = this->ReferenceInterface<JointInfo>(_id);
540+
if (std::isnan(_value))
541+
{
542+
gzerr << "Invalid minimum joint effort value [" << _value
543+
<< "] commanded on joint [" << jointInfo->name << " DOF " << _dof
544+
<< "]. The command will be ignored\n";
545+
546+
return;
547+
}
548+
549+
jointInfo->minEffort = _value;
550+
}
551+
552+
/////////////////////////////////////////////////
553+
void JointFeatures::SetJointMaxEffort(
554+
const Identity &_id, std::size_t _dof, double _value)
555+
{
556+
auto *jointInfo = this->ReferenceInterface<JointInfo>(_id);
557+
if (std::isnan(_value))
558+
{
559+
gzerr << "Invalid maximum joint effort value [" << _value
560+
<< "] commanded on joint [" << jointInfo->name << " DOF " << _dof
561+
<< "]. The command will be ignored\n";
562+
return;
563+
}
564+
565+
const auto *identifier = std::get_if<InternalJoint>(&jointInfo->identifier);
566+
if (!identifier)
567+
return;
568+
569+
jointInfo->maxEffort = _value;
570+
571+
auto *modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
572+
auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
573+
574+
if (jointInfo->motor)
575+
{
576+
world->world->removeMultiBodyConstraint(jointInfo->motor.get());
577+
jointInfo->motor.reset();
578+
}
579+
580+
jointInfo->motor = std::make_shared<btMultiBodyJointMotor>(
581+
modelInfo->body.get(),
582+
std::get<InternalJoint>(jointInfo->identifier).indexInBtModel,
583+
0,
584+
static_cast<btScalar>(0),
585+
static_cast<btScalar>(jointInfo->maxEffort * world->stepSize));
586+
world->world->addMultiBodyConstraint(jointInfo->motor.get());
587+
}
588+
426589
/////////////////////////////////////////////////
427590
Identity JointFeatures::AttachFixedJoint(
428591
const Identity &_childID,

‎bullet-featherstone/src/JointFeatures.hh

+27
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,9 @@ struct JointFeatureList : FeatureList<
3737
GetBasicJointProperties,
3838

3939
SetJointVelocityCommandFeature,
40+
SetJointPositionLimitsFeature,
41+
SetJointVelocityLimitsFeature,
42+
SetJointEffortLimitsFeature,
4043

4144
SetJointTransformFromParentFeature,
4245
AttachFixedJointFeature,
@@ -127,6 +130,30 @@ class JointFeatures :
127130
const Identity &_id, const std::size_t _dof,
128131
const double _value) override;
129132

133+
public: void SetJointMinPosition(
134+
const Identity &_id, std::size_t _dof,
135+
double _value) override;
136+
137+
public: void SetJointMaxPosition(
138+
const Identity &_id, std::size_t _dof,
139+
double _value) override;
140+
141+
public: void SetJointMinVelocity(
142+
const Identity &_id, std::size_t _dof,
143+
double _value) override;
144+
145+
public: void SetJointMaxVelocity(
146+
const Identity &_id, std::size_t _dof,
147+
double _value) override;
148+
149+
public: void SetJointMinEffort(
150+
const Identity &_id, std::size_t _dof,
151+
double _value) override;
152+
153+
public: void SetJointMaxEffort(
154+
const Identity &_id, std::size_t _dof,
155+
double _value) override;
156+
130157
// ----- AttachFixedJointFeature -----
131158
public: Identity AttachFixedJoint(
132159
const Identity &_childID,

‎bullet-featherstone/src/SDFFeatures.cc

+18-1
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@
3333
#include <sdf/JointAxis.hh>
3434
#include <sdf/Link.hh>
3535
#include <sdf/Mesh.hh>
36+
#include <sdf/Physics.hh>
3637
#include <sdf/Plane.hh>
3738
#include <sdf/Sphere.hh>
3839
#include <sdf/Surface.hh>
@@ -89,6 +90,12 @@ Identity SDFFeatures::ConstructSdfWorld(
8990

9091
worldInfo->world->setGravity(convertVec(_sdfWorld.Gravity()));
9192

93+
const ::sdf::Physics *physics = _sdfWorld.PhysicsByIndex(0);
94+
if (physics)
95+
worldInfo->stepSize = physics->MaxStepSize();
96+
else
97+
worldInfo->stepSize = 0.001;
98+
9299
for (std::size_t i = 0; i < _sdfWorld.ModelCount(); ++i)
93100
{
94101
const ::sdf::Model *model = _sdfWorld.ModelByIndex(i);
@@ -770,6 +777,10 @@ Identity SDFFeatures::ConstructSdfModelImpl(
770777
if (::sdf::JointType::PRISMATIC == joint->Type() ||
771778
::sdf::JointType::REVOLUTE == joint->Type())
772779
{
780+
// Note: These m_joint* properties below are currently not supported by
781+
// bullet-featherstone and so setting them does not have any effect.
782+
// The lower and uppper limit is supported via the
783+
// btMultiBodyJointLimitConstraint.
773784
model->body->getLink(i).m_jointLowerLimit =
774785
static_cast<btScalar>(joint->Axis()->Lower());
775786
model->body->getLink(i).m_jointUpperLimit =
@@ -782,7 +793,13 @@ Identity SDFFeatures::ConstructSdfModelImpl(
782793
static_cast<btScalar>(joint->Axis()->MaxVelocity());
783794
model->body->getLink(i).m_jointMaxForce =
784795
static_cast<btScalar>(joint->Axis()->Effort());
785-
jointInfo->effort = static_cast<btScalar>(joint->Axis()->Effort());
796+
797+
jointInfo->minEffort = -joint->Axis()->Effort();
798+
jointInfo->maxEffort = joint->Axis()->Effort();
799+
jointInfo->minVelocity = -joint->Axis()->MaxVelocity();
800+
jointInfo->maxVelocity = joint->Axis()->MaxVelocity();
801+
jointInfo->axisLower = joint->Axis()->Lower();
802+
jointInfo->axisUpper = joint->Axis()->Upper();
786803

787804
jointInfo->jointLimits =
788805
std::make_shared<btMultiBodyJointLimitConstraint>(

‎bullet-featherstone/src/SimulationFeatures.cc

+7-2
Original file line numberDiff line numberDiff line change
@@ -37,14 +37,19 @@ void SimulationFeatures::WorldForwardStep(
3737
const auto worldInfo = this->ReferenceInterface<WorldInfo>(_worldID);
3838
auto *dtDur =
3939
_u.Query<std::chrono::steady_clock::duration>();
40+
double stepSize = 0.001;
4041
if (dtDur)
4142
{
4243
std::chrono::duration<double> dt = *dtDur;
4344
stepSize = dt.count();
4445
}
4546

46-
worldInfo->world->stepSimulation(static_cast<btScalar>(this->stepSize), 1,
47-
static_cast<btScalar>(this->stepSize));
47+
// \todo(iche033) Stepping sim with varying dt may not work properly.
48+
// One example is the motor constraint that's created in
49+
// JointFeatures::SetJointVelocityCommand which assumes a fixed step
50+
// size.
51+
worldInfo->world->stepSimulation(static_cast<btScalar>(stepSize), 1,
52+
static_cast<btScalar>(stepSize));
4853

4954
this->WriteRequiredData(_h);
5055
this->Write(_h.Get<ChangedWorldPoses>());

‎bullet-featherstone/src/SimulationFeatures.hh

-2
Original file line numberDiff line numberDiff line change
@@ -58,8 +58,6 @@ class SimulationFeatures :
5858
public: std::vector<ContactInternal> GetContactsFromLastStep(
5959
const Identity &_worldID) const override;
6060

61-
private: double stepSize = 0.001;
62-
6361
/// \brief link poses from the most recent pose change/update.
6462
/// The key is the link's ID, and the value is the link's pose
6563
private: mutable std::unordered_map<std::size_t, math::Pose3d> prevLinkPoses;

0 commit comments

Comments
 (0)