Fix unlock base for pybullet

Author: EtienneAr

go2_simulation/bullet_wrapper.py

@@ -96,6 +96,9 @@ def __init__(self, node, timestep):
         # gravity and feet friction
         pybullet.setGravity(0, 0, -9.81)
 
+        # Locked base constraint ID
+        self.fixed_base_constraint = None
+
         # Finite differences to compute acceleration
         self.dt = timestep
         self.v_last = None
@@ -121,20 +124,23 @@ def reset(self):
 
         # Lock torso (as if the robot was hanged)
         pos, orn = pybullet.getBasePositionAndOrientation(self.robot)
-        self.fixed_base_constraint = pybullet.createConstraint(
-            parentBodyUniqueId=self.robot,
-            parentLinkIndex=-1,
-            childBodyUniqueId=-1,
-            childLinkIndex=-1,
-            jointType=pybullet.JOINT_FIXED,
-            jointAxis=[0, 0, 0],
-            parentFramePosition=[0, 0, 0],
-            childFramePosition=pos,
-            childFrameOrientation=orn,
-        )
+        if self.fixed_base_constraint is None:
+            self.fixed_base_constraint = pybullet.createConstraint(
+                parentBodyUniqueId=self.robot,
+                parentLinkIndex=-1,
+                childBodyUniqueId=-1,
+                childLinkIndex=-1,
+                jointType=pybullet.JOINT_FIXED,
+                jointAxis=[0, 0, 0],
+                parentFramePosition=[0, 0, 0],
+                childFramePosition=pos,
+                childFrameOrientation=orn,
+            )
 
     def unlock_base(self):
-        pybullet.removeConstraint(self.fixed_base_constraint)
+        if self.fixed_base_constraint is not None:
+            pybullet.removeConstraint(self.fixed_base_constraint)
+            self.fixed_base_constraint = None
 
     def get_joint_id(self, joint_name):
         """

go2_simulation/simulation_node.py

@@ -54,9 +54,9 @@ def __init__(self):
             exit()
 
         if self.unlock_base_default:
-            self.simulator.unlock_base()
+            self.unlock_base()
         else:
-            self.create_subscription(Empty, "/unlock_base", lambda msg: self.simulator.unlock_base(), 1)
+            self.create_subscription(Empty, "/unlock_base", lambda msg: self.unlock_base(), 1)
 
         self.create_subscription(Empty, "/reset", lambda msg: self.reset(), 1)
 
@@ -172,6 +172,10 @@ def reset(self):
         if self.unlock_base_default:
             self.simulator.unlock_base()
 
+    def unlock_base(self):
+        self.get_logger().info("Unlocking robot base")
+        self.simulator.unlock_base()
+
 
 def main(args=None):
     rclpy.init(args=args)