Switch to open loop control for lifts

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
open-rmf · Feb 2, 2024 · b2ed3f4 · b2ed3f4
1 parent 0c80c7e
commit b2ed3f4
Showing 1 changed file with 3 additions and 6 deletions.
diff --git a/rmf_building_sim_gz_plugins/src/lift.cpp b/rmf_building_sim_gz_plugins/src/lift.cpp
@@ -9,7 +9,6 @@
 #include <gz/sim/components/Static.hh>
 #include <gz/sim/components/AxisAlignedBox.hh>
 #include <gz/sim/components/JointPosition.hh>
-#include <gz/sim/components/JointVelocityCmd.hh>
 #include <gz/sim/components/JointPositionReset.hh>
 #include <gz/sim/components/LinearVelocityCmd.hh>
 #include <gz/sim/components/AngularVelocityCmd.hh>
@@ -106,9 +105,6 @@ class GZ_SIM_VISIBLE LiftPlugin
         Model(entity).JointByName(ecm, lift.cabin_joint);
         enableComponent<components::AxisAlignedBox>(ecm, entity);
         enableComponent<components::JointPosition>(ecm, cabin_joint_entity);
-        enableComponent<components::JointVelocityCmd>(ecm, cabin_joint_entity);
-        ecm.CreateComponent<components::JointVelocityCmd>(cabin_joint_entity,
-        components::JointVelocityCmd({0.0}));
 
         LiftCommand lift_command;
         lift_command.request_type = LiftRequest::REQUEST_AGV_MODE;
@@ -352,8 +348,9 @@ class GZ_SIM_VISIBLE LiftPlugin
       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};
+      std::vector<double> joint_position = {cur_elevation + target_vel * dt};
+      ecm.CreateComponent<components::JointPositionReset>(joint_entity,
+        components::JointPositionReset{joint_position});
       _last_cmd_vel[joint_entity] = target_vel;
     }
     return target_vel;