Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Physics: remove *VelocityCmd at each time step #2228

Merged
merged 10 commits into from
Apr 9, 2024
Merged

Physics: remove *VelocityCmd at each time step #2228

Show file tree
Hide file tree
Changes from 9 commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
4070cac
Physics: remove *VelocityCmd at each time step
scpeters Nov 4, 2023
740071a
Fix pose syntax error in quadcopter test worlds
scpeters Nov 5, 2023
ac58022
MulticopterMotorModel: handle missing component
scpeters Nov 5, 2023
c963956
wheel_slip: use SetComponentData
scpeters Nov 5, 2023
f6ecc61
Fix gz-sensors branch in custom sensor example
scpeters Nov 5, 2023
b145c05
Use SetComponentData to improve coverage, simplify
scpeters Nov 6, 2023
7dc33fe
Use SetComponentData in MecanumDrive as well
scpeters Nov 11, 2023
120e5fe
TrajectoryFollower: don't need to remove commands
scpeters Nov 11, 2023
be5214f
Merge branch 'main' into scpeters/physics_remove_velocity_cmds
scpeters Apr 5, 2024
f4ffd4a
Update Migration.md
scpeters Apr 7, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions Migration.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,13 @@ 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**:
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for adding this!

+ In the Physics system, all `*VelocityCmd` components are deleted after
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

mention that previously it was setting them to zero?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

f4ffd4a

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
Open in desktop
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;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we need to preserve the doUpdateForcesAndMoments = false;?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

no, the other places that variable is set to false is when a physics state / sensor reading component is unavailable that is needed for the control calculation, but JointVelocityCmd is an actuator command written by this system, so it's ok if it's missing

}

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
Open in desktop
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
Open in desktop
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
Open in desktop
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>
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I realized last week while trying to fix another test that the quadcopter test is passing even though this SDF was invalid. I'll be making the same changes in other branches.

<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
Open in desktop
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
Loading