Fix error joint velocity in the first step of simulation

qiayuanl · Feb 1, 2024 · 715cc53 · 715cc53
1 parent b3864d4
commit 715cc53
Showing 1 changed file with 3 additions and 0 deletions.
diff --git a/legged_gazebo/src/LeggedHWSim.cpp b/legged_gazebo/src/LeggedHWSim.cpp
@@ -80,6 +80,9 @@ void LeggedHWSim::readSim(ros::Time time, ros::Duration period) {
     double position = sim_joints_[j]->Position(0);
 
     joint_velocity_[j] = (position - joint_position_[j]) / period.toSec();
+    if (time == ros::Time(period.toSec())) {
+      joint_velocity_[j] = 0;
+    }
     if (joint_types_[j] == urdf::Joint::PRISMATIC) {
       joint_position_[j] = position;
     } else {