From 715cc5327e7f4af0531b22e83844dcf569919126 Mon Sep 17 00:00:00 2001
From: qiayuanl <liaoqiayuan@gmail.com>
Date: Wed, 31 Jan 2024 18:24:44 -0800
Subject: [PATCH] Fix error joint velocity in the first step of simulation

---
 legged_gazebo/src/LeggedHWSim.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/legged_gazebo/src/LeggedHWSim.cpp b/legged_gazebo/src/LeggedHWSim.cpp
index 7af11eb9..aa346d4c 100644
--- 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 {