Skip to content

Recreate vel cmd components each cycle #141

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

Open
wants to merge 5 commits into
base: main
Choose a base branch
from
Open

Recreate vel cmd components each cycle #141

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
9535dd3
Recreate vel cmd components each cycle
mxgrey Apr 25, 2025
aadab42
Debugging elevator free-falling issue
mxgrey Apr 29, 2025
e64c8b8
Fix lift behavior
mxgrey Apr 30, 2025
1f87f57
Minor cleanup
mxgrey Apr 30, 2025
d1e0b92
Remove style tests
mxgrey Apr 30, 2025
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
22 changes: 0 additions & 22 deletions .github/workflows/style.yaml
View file
Open in desktop

This file was deleted.

6 changes: 2 additions & 4 deletions rmf_building_sim_gz_plugins/src/door.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
#include <gz/sim/components/Joint.hh>
#include <gz/sim/components/JointAxis.hh>
#include <gz/sim/components/JointPosition.hh>
#include <gz/sim/components/JointPositionReset.hh>
#include <gz/sim/components/JointVelocityCmd.hh>
#include <gz/sim/components/Name.hh>

#include <rclcpp/rclcpp.hpp>
Expand Down Expand Up @@ -136,9 +136,7 @@ class DoorPlugin
auto target_vel = calculate_target_velocity(target_pos, cur_pos,
_last_cmd_vel[joint_entity],
dt, door.params);
ecm.CreateComponent<components::JointPositionReset>(joint_entity,
components::JointPositionReset(
{cur_pos + target_vel * dt}));
ecm.SetComponentData<components::JointVelocityCmd>(joint_entity, {target_vel});
_last_cmd_vel[joint_entity] = target_vel;
}
}
Expand Down
83 changes: 59 additions & 24 deletions rmf_building_sim_gz_plugins/src/lift.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,9 +8,11 @@
#include <gz/sim/components/Pose.hh>
#include <gz/sim/components/Static.hh>
#include <gz/sim/components/AxisAlignedBox.hh>
#include <gz/sim/components/JointForceCmd.hh>
#include <gz/sim/components/JointPosition.hh>
#include <gz/sim/components/JointVelocityCmd.hh>
#include <gz/sim/components/JointPositionReset.hh>
#include <gz/sim/components/JointVelocity.hh>
#include <gz/sim/components/JointVelocityCmd.hh>
#include <gz/sim/components/LinearVelocityCmd.hh>
#include <gz/sim/components/AngularVelocityCmd.hh>
#include <gz/sim/components/PoseCmd.hh>
Expand Down Expand Up @@ -106,11 +108,10 @@ class LiftPlugin
{
const auto& lift = lift_comp->Data();
auto cabin_joint_entity =
Model(entity).JointByName(ecm, lift.cabin_joint);
Model(entity).JointByName(ecm, lift.cabin_joint);
enableComponent<components::AxisAlignedBox>(ecm, entity);
enableComponent<components::JointPosition>(ecm, cabin_joint_entity);
ecm.CreateComponent<components::JointVelocityCmd>(cabin_joint_entity,
components::JointVelocityCmd({0.0}));
ecm.SetComponentData<components::JointPosition>(cabin_joint_entity, {0.0});
ecm.SetComponentData<components::JointVelocity>(cabin_joint_entity, {0.0});

LiftCommand lift_command;
lift_command.request_type = LiftRequest::REQUEST_AGV_MODE;
Expand Down Expand Up @@ -203,7 +204,7 @@ class LiftPlugin
}

const auto lift_pos =
ecm.Component<components::JointPosition>(joint_entity)->Data()[0];
ecm.Component<components::Pose>(entity)->Data().Z();

double smallest_error = std::numeric_limits<double>::infinity();
std::string closest_floor_name;
Expand Down Expand Up @@ -284,8 +285,13 @@ class LiftPlugin
return LiftState::MOTION_STOPPED;
}

const auto lift_pos =
ecm.Component<components::JointPosition>(joint_entity)->Data()[0];
auto* lift_pos_component = ecm.Component<components::JointPosition>(joint_entity);
if (!lift_pos_component)
{
return LiftState::MOTION_STOPPED;
}

const auto lift_pos = lift_pos_component->Data()[0];
const auto target_it = lift.floors.find(destination_floor);

if (target_it != lift.floors.end())
Expand Down Expand Up @@ -365,15 +371,22 @@ class LiftPlugin
const LiftData& lift, double dt, double target_elevation,
double cur_elevation)
{
for (auto joint : Model(entity).Joints(ecm))
{
ecm.RemoveComponent<components::JointForceCmd>(joint);
}

auto joint_entity = Model(entity).JointByName(ecm, lift.cabin_joint);
auto target_vel = 0.0;
if (joint_entity != kNullEntity)
{
target_vel = calculate_target_velocity(target_elevation, cur_elevation,
_last_cmd_vel[joint_entity], dt,
lift.params);
ecm.Component<components::JointVelocityCmd>(joint_entity)->Data() =
{target_vel};

ecm.SetComponentData<components::JointVelocityCmd>(joint_entity, {target_vel});
ecm.RemoveComponent<components::JointForceCmd>(joint_entity);

_last_cmd_vel[joint_entity] = target_vel;
}
return target_vel;
Expand Down Expand Up @@ -533,21 +546,41 @@ class LiftPlugin
const double dt = to_seconds(info.dt);
// Command lifts
ecm.Each<components::Lift,
components::Pose,
components::LiftCmd>([&](const Entity& entity,
components::Pose>([&](const Entity& entity,
const components::Lift* lift_comp,
const components::Pose* pose_comp,
const components::LiftCmd* lift_cmd_comp) -> bool
const components::Pose* pose_comp) -> bool
{
const components::LiftCmd* lift_cmd_comp = ecm.Component<const components::LiftCmd>(entity);
const LiftCommand* lift_cmd = nullptr;
if (lift_cmd_comp)
{
lift_cmd = &lift_cmd_comp->Data();
}
else
{
const auto it = _last_lift_command.find(entity);
if (it != _last_lift_command.end())
{
lift_cmd = &it->second;
}
else
{
// We have no indication of where this lift should go, so
// just try to keep it in place.
auto cabin_joint_entity = Model(entity).JointByName(ecm, lift_comp->Data().cabin_joint);
ecm.SetComponentData<components::JointVelocityCmd>(cabin_joint_entity, {0.0});

return true;
}
}

const auto& lift = lift_comp->Data();
const auto& pose = pose_comp->Data();

const auto& lift_cmd = lift_cmd_comp->Data();

const auto& destination_floor = lift_cmd.destination_floor;
const auto& destination_floor = lift_cmd->destination_floor;
const double target_elevation = lift.floors.at(
destination_floor).elevation;
const auto target_door_state = lift_cmd.door_state;
const auto target_door_state = lift_cmd->door_state;
const std::string cur_floor = get_current_floor(entity, ecm, lift);

const auto doors = get_floor_doors(ecm, lift, cur_floor);
Expand All @@ -558,8 +591,10 @@ class LiftPlugin
command_doors(ecm, doors, target_door_state);
// Clear the command if it was finished
if (destination_floor == cur_floor &&
all_doors_at_state(ecm, doors, target_door_state))
all_doors_at_state(ecm, doors, target_door_state))
{
finished_cmds.insert(entity);
}
}
else
{
Expand All @@ -568,18 +603,18 @@ class LiftPlugin
if (all_doors_at_state(ecm, doors, DoorModeCmp::CLOSE))
{
auto target_velocity =
command_lift(entity, ecm, lift, dt, target_elevation, pose.Z());
command_lift(entity, ecm, lift, dt, target_elevation, pose.Z());

// Move payloads as well
if (target_velocity != 0.0)
{
auto payloads = get_payloads(ecm, entity);

for (const Entity& payload : payloads)
{
if (ecm.EntityHasComponentType(payload,
components::LinearVelocityCmd().TypeId()))
if (ecm.EntityHasComponentType(payload, components::LinearVelocityCmd().TypeId()))
{
auto lin_vel_cmd =
ecm.Component<components::LinearVelocityCmd>(payload);
auto lin_vel_cmd = ecm.Component<components::LinearVelocityCmd>(payload);
lin_vel_cmd->Data()[2] = target_velocity;
}
}
Expand Down
30 changes: 20 additions & 10 deletions rmf_robot_sim_gz_plugins/src/slotcar.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@ using namespace gz::sim;
class GZ_SIM_VISIBLE SlotcarPlugin
: public System,
public ISystemConfigure,
public ISystemConfigurePriority,
public ISystemPreUpdate
{
public:
Expand All @@ -43,6 +44,7 @@ class GZ_SIM_VISIBLE SlotcarPlugin
void Configure(const Entity& entity,
const std::shared_ptr<const sdf::Element>& sdf,
EntityComponentManager& ecm, EventManager& eventMgr) override;
int32_t ConfigurePriority() override;
void PreUpdate(const UpdateInfo& info, EntityComponentManager& ecm) override;

private:
Expand Down Expand Up @@ -211,9 +213,6 @@ void SlotcarPlugin::Configure(const Entity& entity,
enableComponent<components::Pose>(ecm, entity);
// Initialize Bounding Box component
enableComponent<components::AxisAlignedBox>(ecm, entity);
// Initialize Linear/AngularVelocityCmd components to drive slotcar
enableComponent<components::LinearVelocityCmd>(ecm, entity);
enableComponent<components::AngularVelocityCmd>(ecm, entity);

// Respond to requests asking for height (e.g. for dispenser to dispense object)
const std::string height_srv_name =
Expand All @@ -239,11 +238,6 @@ void SlotcarPlugin::send_control_signals(EntityComponentManager& ecm,
const double target_linear_speed_destination,
const std::optional<double>& max_linear_velocity)
{
auto lin_vel_cmd =
ecm.Component<components::LinearVelocityCmd>(_entity);
auto ang_vel_cmd =
ecm.Component<components::AngularVelocityCmd>(_entity);

// Open loop control
double v_robot = _prev_v_command;
double w_robot = _prev_w_command;
Expand All @@ -252,8 +246,13 @@ void SlotcarPlugin::send_control_signals(EntityComponentManager& ecm,
displacements, dt, target_linear_speed_now,
target_linear_speed_destination, max_linear_velocity);

lin_vel_cmd->Data()[0] = target_vels[0];
ang_vel_cmd->Data()[2] = target_vels[1];
gz::math::Vector3d lin_vel_cmd(0, 0, 0);
lin_vel_cmd[0] = target_vels[0];
gz::math::Vector3d ang_vel_cmd(0, 0, 0);
ang_vel_cmd[2] = target_vels[1];

ecm.SetComponentData<components::LinearVelocityCmd>(_entity, lin_vel_cmd);
ecm.SetComponentData<components::AngularVelocityCmd>(_entity, ang_vel_cmd);

// Update previous velocities
_prev_v_command = target_vels[0];
Expand Down Expand Up @@ -454,6 +453,16 @@ void SlotcarPlugin::draw_lookahead_marker()
_gz_node.Request("/marker", marker_msg);
}

int32_t SlotcarPlugin::ConfigurePriority()
{
// Set the priority down by one so this runs before the infrastructure plugin
// which should have a default priority of 0. This is important for ensuring
// that the linear velocity command component is created before the
// infrastructure plugin runs since the lift needs to adjust its z value, and
// the physics plugin removes the component on every update.
return -1;
}

void SlotcarPlugin::PreUpdate(const UpdateInfo& info,
EntityComponentManager& ecm)
{
Expand Down Expand Up @@ -523,6 +532,7 @@ GZ_ADD_PLUGIN(
SlotcarPlugin,
System,
SlotcarPlugin::ISystemConfigure,
SlotcarPlugin::ISystemConfigurePriority,
SlotcarPlugin::ISystemPreUpdate)

GZ_ADD_PLUGIN_ALIAS(SlotcarPlugin, "slotcar")