diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py index 961e673184bc4016a34446a4189a9974a5e813be..1d1d16a15970ecbd5408a3c9a7c00e4830726092 100644 --- a/python/quadruped_reactive_walking/Controller.py +++ b/python/quadruped_reactive_walking/Controller.py @@ -122,7 +122,7 @@ class Controller: # self.result.P = np.array(self.params.Kp_main.tolist() * 4) self.result.P = np.array([5] * 3 + [1] * 3 + [5] * 6) # self.result.D = np.array(self.params.Kd_main.tolist() * 4) - self.result.D = np.array([0.3] * 3 + [0.5] * 3 + [0.3] * 6) + self.result.D = np.array([0.3] * 3 + [0.01] * 3 + [0.3] * 6) tauFF = self.mpc_result.u[0] self.result.FF = self.params.Kff_main * np.array( [0] * 3 + list(tauFF) + [0] * 6 @@ -185,7 +185,7 @@ class Controller: elif (np.abs(m["vj_m"]) > 1000 * np.pi / 180).any(): print("-- VELOCITY TOO HIGH ERROR --") print(m["vj_m"]) - print(np.abs(m["vj_m"]) > 500 * np.pi / 180) + print(np.abs(m["vj_m"]) > 1000 * np.pi / 180) self.error = True elif (np.abs(self.result.FF) > 3.2).any(): print("-- FEEDFORWARD TORQUES TOO HIGH ERROR --") diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py index 3c44ceaf8cb320e212c6fdc2410a0a89ff2e2abe..a7c25043d53a8f0e586441fc1e0d407c49bec70e 100644 --- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py +++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py @@ -117,7 +117,7 @@ class ProblemDataFull(problemDataAbstract): #self.friction_cone_w = 1e3 * 0 self.control_bound_w = 1e3 self.control_reg_w = 1e0 - self.state_reg_w = np.array([1e-3] * 3 + [2*1e-1]*3) + self.state_reg_w = np.array([1e-5] * 3 + [ 3* 1e-1]*3) self.terminal_velocity_w = np.array([0] * 3 + [1e3] * 3 ) self.q0_reduced = self.q0[10:13] diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py index 881e081f7ebe8a14845ffc4af599e4d95291f989..f86a80afa26837ac6f6ab4391686bf843c804a38 100644 --- a/python/quadruped_reactive_walking/WB_MPC/Target.py +++ b/python/quadruped_reactive_walking/WB_MPC/Target.py @@ -29,9 +29,9 @@ class Target: pin.updateFramePlacements(pd.model, pd.rdata) self.FR_foot0 = pd.rdata.oMf[pd.rfFootId].translation.copy() self.A = np.array([0, 0.03, 0.03]) - self.offset = np.array([0.08, 0, 0.06]) - self.freq = np.array([0, 1, 1]) - self.phase = np.array([0, 0, np.pi / 2]) + self.offset = np.array([0.04, 0, 0.06]) + self.freq = np.array([0, 0.5, 0.5]) + self.phase = np.array([0, np.pi / 2, 0]) def patternToId(self, gait): return tuple(self.pd.allContactIds[i] for i, c in enumerate(gait) if c == 1)