Skip to content

Commit

Permalink
Physics: remove *VelocityCmd at each time step (#2228)
Browse files Browse the repository at this point in the history 
This implements a suggestion from #1926 to delete
all `*VelocityCmd` components after each time step.
This also updates the logic for the following two systems
to handle this change:

* MulticopterMotorModel: handle missing component

Since JointVelocityCmd components are deleted after each
timestep, don't skip updating forces and moments if
the component does not exist, and use the
SetComponent API to more cleanly handle the component
creation logic. A syntax error in the the quadcopter test
worlds was fixed as well.

* TrajectoryFollower: don't need to remove commands

Now that the physics system is removing AngularVelocityCmd
components at every timestep, that logic can be removed
from the trajectory follower system.

Signed-off-by: Steve Peters <[email protected]>
  • Loading branch information
@scpeters
scpeters authored Apr 9, 2024
1 parent 301832d commit 77f4c00
Show file tree
Hide file tree
Showing 6 changed files with 66 additions and 61 deletions.
8 changes: 8 additions & 0 deletions Migration.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,14 @@ Deprecated code produces compile-time warnings. These warning serve as
notification to users that their code should be upgraded. The next major
release will remove the deprecated code.

## Gazebo Sim 8.x to 9.0

* **Modified**:
+ In the Physics system, all `*VelocityCmd` components are now deleted after
each time step, whereas previously the component values were set to `0`
after each time step. Persistent velocity commands should be reapplied at
each time step.

## Gazebo Sim 7.x to 8.0
* **Deprecated**
+ `gz::sim::components::Factory::Register(const std::string &_type, ComponentDescriptorBase *_compDesc)` and
Expand Down
17 changes: 4 additions & 13 deletions src/systems/multicopter_motor_model/MulticopterMotorModel.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -447,14 +447,6 @@ void MulticopterMotorModel::PreUpdate(const UpdateInfo &_info,
doUpdateForcesAndMoments = false;
}

if (!_ecm.Component<components::JointVelocityCmd>(
this->dataPtr->jointEntity))
{
_ecm.CreateComponent(this->dataPtr->jointEntity,
components::JointVelocityCmd({0}));
doUpdateForcesAndMoments = false;
}

if (!_ecm.Component<components::WorldPose>(this->dataPtr->linkEntity))
{
_ecm.CreateComponent(this->dataPtr->linkEntity, components::WorldPose());
Expand Down Expand Up @@ -682,11 +674,10 @@ void MulticopterMotorModelPrivate::UpdateForcesAndMoments(
refMotorRotVel = this->rotorVelocityFilter->UpdateFilter(
this->refMotorInput, this->samplingTime);

const auto jointVelCmd = _ecm.Component<components::JointVelocityCmd>(
this->jointEntity);
*jointVelCmd = components::JointVelocityCmd(
{this->turningDirection * refMotorRotVel
/ this->rotorVelocitySlowdownSim});
_ecm.SetComponentData<components::JointVelocityCmd>(
this->jointEntity,
{this->turningDirection * refMotorRotVel
/ this->rotorVelocitySlowdownSim});
}
}
}
Expand Down
62 changes: 43 additions & 19 deletions src/systems/physics/Physics.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3628,34 +3628,58 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm,
return true;
});

_ecm.Each<components::JointVelocityCmd>(
[&](const Entity &, components::JointVelocityCmd *_vel) -> bool
{
std::fill(_vel->Data().begin(), _vel->Data().end(), 0.0);
return true;
});
{
std::vector<Entity> entitiesJointVelocityCmd;
_ecm.Each<components::JointVelocityCmd>(
[&](const Entity &_entity, components::JointVelocityCmd *) -> bool
{
entitiesJointVelocityCmd.push_back(_entity);
return true;
});

for (const auto entity : entitiesJointVelocityCmd)
{
_ecm.RemoveComponent<components::JointVelocityCmd>(entity);
}
}

_ecm.Each<components::SlipComplianceCmd>(
[&](const Entity &, components::SlipComplianceCmd *_slip) -> bool
{
std::fill(_slip->Data().begin(), _slip->Data().end(), 0.0);
return true;
});
GZ_PROFILE_END();

_ecm.Each<components::AngularVelocityCmd>(
[&](const Entity &, components::AngularVelocityCmd *_vel) -> bool
{
_vel->Data() = math::Vector3d::Zero;
return true;
});
{
std::vector<Entity> entitiesAngularVelocityCmd;
_ecm.Each<components::AngularVelocityCmd>(
[&](const Entity &_entity, components::AngularVelocityCmd *) -> bool
{
entitiesAngularVelocityCmd.push_back(_entity);
return true;
});

_ecm.Each<components::LinearVelocityCmd>(
[&](const Entity &, components::LinearVelocityCmd *_vel) -> bool
{
_vel->Data() = math::Vector3d::Zero;
return true;
});
for (const auto entity : entitiesAngularVelocityCmd)
{
_ecm.RemoveComponent<components::AngularVelocityCmd>(entity);
}
}

{
std::vector<Entity> entitiesLinearVelocityCmd;
_ecm.Each<components::LinearVelocityCmd>(
[&](const Entity &_entity, components::LinearVelocityCmd *) -> bool
{
entitiesLinearVelocityCmd.push_back(_entity);
return true;
});

for (const auto entity : entitiesLinearVelocityCmd)
{
_ecm.RemoveComponent<components::LinearVelocityCmd>(entity);
}
}
GZ_PROFILE_END();

// Update joint positions
GZ_PROFILE_BEGIN("Joints");
Expand Down
24 changes: 3 additions & 21 deletions src/systems/trajectory_follower/TrajectoryFollower.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,9 +111,6 @@ class gz::sim::systems::TrajectoryFollowerPrivate
/// \brief Whether the trajectory follower behavior should be paused or not.
public: bool paused = false;

/// \brief Angular velocity set to zero
public: bool zeroAngVelSet = false;

/// \brief Force angular velocity to be zero when bearing is reached
public: bool forceZeroAngVel = false;
};
Expand Down Expand Up @@ -390,37 +387,22 @@ void TrajectoryFollower::PreUpdate(
// Transform the force and torque to the world frame.
// Move commands. The vehicle always move forward (X direction).
gz::math::Vector3d forceWorld;
gz::math::Vector3d torqueWorld;
if (std::abs(bearing.Degree()) <= this->dataPtr->bearingTolerance)
{
forceWorld = (*comPose).Rot().RotateVector(
gz::math::Vector3d(this->dataPtr->forceToApply, 0, 0));

// force angular velocity to be zero when bearing is reached
if (this->dataPtr->forceZeroAngVel && !this->dataPtr->zeroAngVelSet &&
if (this->dataPtr->forceZeroAngVel &&
math::equal (std::abs(bearing.Degree()), 0.0,
this->dataPtr->bearingTolerance * 0.5))
{
this->dataPtr->link.SetAngularVelocity(_ecm, math::Vector3d::Zero);
this->dataPtr->zeroAngVelSet = true;
}
}
gz::math::Vector3d torqueWorld;
if (std::abs(bearing.Degree()) > this->dataPtr->bearingTolerance)
else
{
// remove angular velocity component otherwise the physics system will set
// the zero ang vel command every iteration
if (this->dataPtr->forceZeroAngVel && this->dataPtr->zeroAngVelSet)
{
auto angVelCmdComp = _ecm.Component<components::AngularVelocityCmd>(
this->dataPtr->link.Entity());
if (angVelCmdComp)
{
_ecm.RemoveComponent<components::AngularVelocityCmd>(
this->dataPtr->link.Entity());
this->dataPtr->zeroAngVelSet = false;
}
}

int sign = static_cast<int>(std::abs(bearing.Degree()) / bearing.Degree());
torqueWorld = (*comPose).Rot().RotateVector(
gz::math::Vector3d(0, 0, sign * this->dataPtr->torqueToApply));
Expand Down
8 changes: 4 additions & 4 deletions test/worlds/quadcopter.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,7 @@
</geometry>
</collision>
<visual name="rotor_0_visual">
<pose>0 0 0 1.57 0 0 0</pose>
<pose>0 0 0 1.57 0 0</pose>
<geometry>
<cylinder>
<length>0.2</length>
Expand Down Expand Up @@ -136,7 +136,7 @@
</geometry>
</collision>
<visual name="rotor_1_visual">
<pose>0 0 0 1.57 0 0 0</pose>
<pose>0 0 0 1.57 0 0</pose>
<geometry>
<cylinder>
<length>0.2</length>
Expand Down Expand Up @@ -181,7 +181,7 @@
</geometry>
</collision>
<visual name="rotor_2_visual">
<pose>0 0 0 1.57 0 0 0</pose>
<pose>0 0 0 1.57 0 0</pose>
<geometry>
<cylinder>
<length>0.2</length>
Expand Down Expand Up @@ -226,7 +226,7 @@
</geometry>
</collision>
<visual name="rotor_3_visual">
<pose>0 0 0 1.57 0 0 0</pose>
<pose>0 0 0 1.57 0 0</pose>
<geometry>
<cylinder>
<length>0.2</length>
Expand Down
8 changes: 4 additions & 4 deletions test/worlds/quadcopter_velocity_control.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,7 @@
</geometry>
</collision>
<visual name="rotor_0_visual">
<pose>0 0 0 1.57 0 0 0</pose>
<pose>0 0 0 1.57 0 0</pose>
<geometry>
<cylinder>
<length>0.2</length>
Expand Down Expand Up @@ -136,7 +136,7 @@
</geometry>
</collision>
<visual name="rotor_1_visual">
<pose>0 0 0 1.57 0 0 0</pose>
<pose>0 0 0 1.57 0 0</pose>
<geometry>
<cylinder>
<length>0.2</length>
Expand Down Expand Up @@ -181,7 +181,7 @@
</geometry>
</collision>
<visual name="rotor_2_visual">
<pose>0 0 0 1.57 0 0 0</pose>
<pose>0 0 0 1.57 0 0</pose>
<geometry>
<cylinder>
<length>0.2</length>
Expand Down Expand Up @@ -226,7 +226,7 @@
</geometry>
</collision>
<visual name="rotor_3_visual">
<pose>0 0 0 1.57 0 0 0</pose>
<pose>0 0 0 1.57 0 0</pose>
<geometry>
<cylinder>
<length>0.2</length>
Expand Down

0 comments on commit 77f4c00

Please sign in to comment.