From 76e83d41fbc301a0b1aa8e472de16aff2ba22170 Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Mon, 8 Feb 2021 16:03:01 +0100
Subject: [PATCH] Add actuators position and velocity update in SendCommand to
handle case in which PyBullet simulator is run faster than the control loop
---
scripts/PyBulletSimulator.py | 5 +++++
1 file changed, 5 insertions(+)
diff --git a/scripts/PyBulletSimulator.py b/scripts/PyBulletSimulator.py
index 9754f408..d97d3f29 100644
--- a/scripts/PyBulletSimulator.py
+++ b/scripts/PyBulletSimulator.py
@@ -676,6 +676,11 @@ class PyBulletSimulator():
WaitEndOfCycle (bool): wait to have simulation time = real time
"""
+ # Position and velocity of actuators
+ jointStates = pyb.getJointStates(self.pyb_sim.robotId, self.revoluteJointIndices) # State of all joints
+ self.q_mes[:] = np.array([state[0] for state in jointStates])
+ self.v_mes[:] = np.array([state[1] for state in jointStates])
+
# Compute PD torques
tau_pd = self.P * (self.q_des - self.q_mes) + self.D * (self.v_des - self.v_mes)
--
GitLab