example working on real robot

mpc/mpc_box_to_table.py

@@ -66,13 +66,13 @@ x0= np.array([0,
               0,
               0,
               0,
-    -0.9933422744148654,
-      0.8247008208170289,
-    -0.14514078432434913,
-    -2.1488552764210156,
-    -1.1443090179703352,
-     1.59336082303524,
-    0.6099733325640361,
+    -0.993,
+      0.825,
+    -0.145,
+    -2.15,
+    -1.14,
+     1.59,
+    0.61,
            0,
            0,
            0,
@@ -111,6 +111,9 @@ for i in range(100):
         xs_sol = list(xs_sol)
         us_sol = list(us_sol)
 
+        print("Xs_sol = ", xs_sol)
+        print("Us_sol = ", us_sol)
+
         for ii in range(len(xs_sol)):
             q_list.append(xs_sol[ii][:nq])
 
@@ -121,6 +124,9 @@ for i in range(100):
         xs_sol = list(xs_sol)
         us_sol = list(us_sol)
 
+        print("Xs_sol = ", xs_sol)
+        print("Us_sol = ", us_sol)
+
         if i >= T:
             q_list.append(xs_sol[-1][:nq])
 

mpc/ocp_box_to_table.py

@@ -18,7 +18,7 @@ class OCP_Crocoddyl:
         T=50,
         dt=1e-2,
         WEIGHT_fREG=1000,
-        WEIGHT_xREG=1e3,
+        WEIGHT_xREG=1000,
         WEIGHT_xREG_TERM=1,
         WEIGHT_uREG=1e-6,
         ):
@@ -81,6 +81,8 @@ class OCP_Crocoddyl:
         forceCost = crocoddyl.CostModelResidual(state,forceRes)
         runningCostModel.addCost("force", forceCost, 1000)
 
+        print(runningCostModel)
+
         # Next, we need to create the running and terminal action model.
         # The forward dynamics (computed using ABA) are implemented
         # inside DifferentialActionModel.
@@ -99,6 +101,8 @@ class OCP_Crocoddyl:
         terminalModel = crocoddyl.IntegratedActionModelEuler(terminalDam, 0.)
         terminalModel.differential.armature = self._rmodel.armature
 
+        print(terminalCostModel)
+
         # Define the optimal control problem.
         # For this optimal control problem, we define HORIZON_LENGTH knots (or running action
         # models) plus a terminal knot