added friction model to the mujoco simulator.

Go2Py/robot/model.py

@@ -5,6 +5,22 @@ import numpy as np
 urdf_path = os.path.join(ASSETS_PATH, 'urdf/go2.urdf')
 urdf_root_path = os.path.join(ASSETS_PATH, 'urdf')
 
+class FrictionModel:
+    def __init__(self, mu_v=0.1, Fs=0.3, Vs=0.5, temperature=0.1):
+        self.mu_v = mu_v
+        self.Fs = Fs
+        self.Vs = Vs 
+        self.temperature = temperature
+
+    def __call__(self, dq):
+        # tau_sticktion = self.Fs*np.exp(-(np.abs(dq)/self.Vs)**2)*self.softSign(dq, temperature=self.temperature)
+        tau_sticktion = self.Fs*self.softSign(dq, temperature=self.temperature)
+        tau_viscose = self.mu_v*dq
+        return tau_sticktion+tau_viscose
+
+    def softSign(self, u, temperature=0.1):
+        return np.tanh(u/temperature)
+    
 
 class Go2Model:
     """

Go2Py/sim/mujoco.py

@@ -242,7 +242,9 @@ class Go2Sim:
                  xml_path=None,
                  camera_name = "front_camera", 
                  camera_resolution = (640, 480),
-                 camera_depth_range = (0.35, 3.0)):
+                 camera_depth_range = (0.35, 3.0), 
+                 friction_model = None,
+                 ):
 
         if xml_path is None:
             self.model = mujoco.MjModel.from_xml_path(
@@ -256,7 +258,7 @@ class Go2Sim:
                 self.updateHeightMap(height_map)
             except:
                 raise Exception('Could not set height map. Are you sure the XML contains the required asset?')
-        
+        self.friction_model = friction_model
         self.simulated = True
         self.data = mujoco.MjData(self.model)
         self.dt = dt
@@ -405,6 +407,9 @@ class Go2Sim:
         q, dq = state['q'], state['dq']
         tau = np.diag(self.kp) @ (self.q_des - q).reshape(12, 1) + \
             np.diag(self.kv) @ (self.dq_des - dq).reshape(12, 1) + self.tau_ff.reshape(12, 1)
+        # Apply the friction model if it is provided to the simulator
+        if self.friction_model is not None:
+            tau = tau.squeeze()-self.friction_model(dq)
         self.actuator_tau = tau
         self.data.ctrl[:] = tau.squeeze()