fixing reflex z height in case of vision

Author: giulioturrisi

quadruped_pympc/config.py

@@ -200,8 +200,8 @@
 
     # This is used to activate or deactivate the reflexes upon contact detection
     'reflex_trigger_mode':       'tracking', # 'tracking', 'geom_contact', False
-    'reflex_height_enhancement': True,
-    'velocity_modulator': False,
+    'reflex_height_enhancement': False,
+    'velocity_modulator': True,
 
     # velocity mode: human will give you the possibility to use the keyboard, the other are
     # forward only random linear-velocity, random will give you random linear-velocity and yaw-velocity

quadruped_pympc/helpers/early_stance_detector.py

@@ -1,9 +1,7 @@
 import numpy as np
-from quadruped_pympc import config as cfg
 from gym_quadruped.utils.quadruped_utils import LegsAttr
 
-
-
+from quadruped_pympc import config as cfg
 
 class EarlyStanceDetector:
     def __init__(self, feet_geom_id : LegsAttr = None,
@@ -31,7 +29,7 @@ def __init__(self, feet_geom_id : LegsAttr = None,
         self.absolute_min_distance_error_threshold = 0.1
 
         self.gait_cycles_after_step_height_enanchement = -1
-        self.use_height_enhancement = cfg.simulation_params['reflex_trigger_mode']
+        self.use_height_enhancement = cfg.simulation_params['reflex_height_enhancement']
         self.max_gait_cycles_height_enhancement = 6
 
 

quadruped_pympc/helpers/swing_generators/scipy_swing_trajectory_generator.py

@@ -2,6 +2,7 @@
 import numpy as np
 from scipy.interpolate import CubicSpline
 
+from quadruped_pympc import config as cfg
 
 class SwingTrajectoryGenerator:
     def __init__(self, step_height: float, swing_period: float) -> None:
@@ -14,6 +15,11 @@ def __init__(self, step_height: float, swing_period: float) -> None:
         self.stepHeight = step_height
         self.step_height_enhancement = False
 
+        if(cfg.simulation_params['visual_foothold_adaptation'] == 'blind'):
+            self.z_height_enhancement = True
+        else:
+            self.z_height_enhancement = False
+
     def createCurve(self, x0, xf, early_stance_hitmoment = -1):
 
         scaling_factor = 1.5
@@ -29,7 +35,10 @@ def createCurve(self, x0, xf, early_stance_hitmoment = -1):
 
             x = np.array([x0[0], p1[0], p2[0], p3[0], xf[0]])
             y = np.array([x0[1], p1[1], p2[1], p3[1], xf[1]])
-            z = np.array([x0[2], p1[2], p2[2], p3[2], xf[2] + reflex_maximum_height / (scaling_factor+0.5)])
+            if(self.z_height_enhancement):
+                z = np.array([x0[2], p1[2], p2[2], p3[2], xf[2] + reflex_maximum_height / (scaling_factor+0.5)])
+            else:
+                z = np.array([x0[2], p1[2], p2[2], p3[2], xf[2]])
 
             updated_swing_period = self.swing_period - early_stance_hitmoment
             t = np.array([early_stance_hitmoment, early_stance_hitmoment+updated_swing_period/4, early_stance_hitmoment+updated_swing_period/2, early_stance_hitmoment+updated_swing_period*3/4, self.swing_period])

quadruped_pympc/helpers/velocity_modulator.py

@@ -29,6 +29,10 @@ def modulate_velocities(self, ref_base_lin_vel, ref_base_ang_vel, feet_pos, hip_
             np.square(feet_pos.RR[0] - hip_pos.RR[0]) + np.square(feet_pos.RR[1] - hip_pos.RR[1])
         )
 
+        if(ref_base_lin_vel[0] < 0.01 and ref_base_lin_vel[1] < 0.01):
+            # If the robot is not moving, we don't need to modulate the velocities
+            return ref_base_lin_vel, ref_base_ang_vel
+
         if (
             distance_FL_to_hip_xy > self.max_distance
             or distance_FR_to_hip_xy > self.max_distance

simulation/simulation.py

@@ -46,7 +46,7 @@ def run_simulation(
     simulation_dt = qpympc_cfg.simulation_params["dt"]
 
     # Save all observables available.
-    state_obs_names = list(QuadrupedEnv.ALL_OBS)  # + list(IMU.ALL_OBS)
+    state_obs_names = [] #list(QuadrupedEnv.ALL_OBS)  # + list(IMU.ALL_OBS)
 
     # Create the quadruped robot environment -----------------------------------------------------------
     env = QuadrupedEnv(