Corrected signs

Author: EnricoMingo

bindings/python/examples/lipm_ms.py

@@ -90,7 +90,7 @@ def euler(x0, xdot0, x1, dt):
 
 x_tasks = list()
 for i in range(Ns+1):
-    min_xi = GenericTask("min_x", variables.getVariable(f"x{i}").getM(), variables.getVariable(f"x{i}").getq())
+    min_xi = GenericTask("min_x", variables.getVariable(f"x{i}").getM(), -variables.getVariable(f"x{i}").getq())
     Q = 1e-3 * np.array([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
     if i == Ns:
         Q = 1e6 * np.array([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])

bindings/python/examples/lipm_ss.py

@@ -94,13 +94,13 @@ def update(self):
 
 
 xinit = variables.getVariable("x0") + np.array([0., 0., 0, 0.])
-initial_state = GenericTask("initial_state", xinit.getM(), xinit.getq())
+initial_state = GenericTask("initial_state", xinit.getM(), -xinit.getq())
 xNs = state(Ns, variables, h, dt)
-min_rdot_final = GenericTask("min_rdot_final", xNs[2:].getM(), xNs[2:].getq())
+min_rdot_final = GenericTask("min_rdot_final", xNs[2:].getM(), -xNs[2:].getq())
 
 zmp_tasks = list()
 for i in range(Ns):
-    min_ui = GenericTask("zmp_tracking", variables.getVariable(f"u{i}").getM(), variables.getVariable(f"u{i}").getq())
+    min_ui = GenericTask("zmp_tracking", variables.getVariable(f"u{i}").getM(), -variables.getVariable(f"u{i}").getq())
     min_ui.setWeight(1e1 * np.array([[1, 0], [0, 1]]))
     zmp_tasks.append(min_ui)
 zmp_tracking_task = AggregatedTask(zmp_tasks, variables.getSize())

bindings/python/examples/panda_mpc.py

@@ -242,7 +242,7 @@ def create(cls, vee, v):
 min_u_list = list()
 for i in range(Ns):
     u_i = variables.getVariable(f"u{i}")
-    min_ui = GenericTask(f"min_u{i}", u_i.getM(), u_i.getq())
+    min_ui = GenericTask(f"min_u{i}", u_i.getM(), -u_i.getq())
     min_ui.setWeight(1e-3 * np.eye(u_i.getM().shape[0]))
     min_ui.update()
     min_u_list.append(min_ui)

bindings/python/examples/wblipm_ms.py

@@ -238,26 +238,26 @@ def create(cls, r0, rddot0, u0, h):
     xdot0 = AffineHelper.pile(rdot, rddot)
 
     integration_ = euler(x0, xdot0, x1, dt)
-    integration.append(GenericTask(f"integration_{i}", integration_.getM(), integration_.getq()))
+    integration.append(GenericTask(f"integration_{i}", integration_.getM(), -integration_.getq()))
 
 
 integration_constraint = AggregatedTask(integration, variables.getSize()) + integration_0_constraint
 #plt.spy(integration_constraint.getA(), markersize=5)
 #plt.show()
 
-xinit = variables.getVariable("x0") + np.hstack((model.getCOM()[0:2], model.getCOMVelocity()[0:2]))
-initial_state = GenericTask("initial_state", xinit.getM(), xinit.getq())
+xinit = variables.getVariable("x0") - np.hstack((model.getCOM()[0:2], model.getCOMVelocity()[0:2]))
+initial_state = GenericTask("initial_state", xinit.getM(), -xinit.getq())
 
 zmp_tasks = list()
 for i in range(Ns):
-    min_ui = GenericTask("zmp_tracking", variables.getVariable(f"u{i}").getM(), variables.getVariable(f"u{i}").getq())
+    min_ui = GenericTask("zmp_tracking", variables.getVariable(f"u{i}").getM(), -variables.getVariable(f"u{i}").getq())
     min_ui.setWeight(1e6 * np.array([[1, 0], [0, 1]]))
     zmp_tasks.append(min_ui)
 zmp_tracking_task = AggregatedTask(zmp_tasks, variables.getSize())
 
 x_tasks = list()
 for i in range(Ns+1):
-    min_xi = GenericTask("min_x", variables.getVariable(f"x{i}").getM(), variables.getVariable(f"x{i}").getq())
+    min_xi = GenericTask("min_x", variables.getVariable(f"x{i}").getM(), -variables.getVariable(f"x{i}").getq())
     Q = 1e-3 * np.array([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
     if i == Ns:
         Q = 2e2 * np.array([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
@@ -285,7 +285,7 @@ def create(cls, r0, rddot0, u0, h):
 pelvis = Cartesian("pelvis", model, "pelvis", "world", variables.getVariable("acc0"))
 
 # Create the stack
-min_acc = GenericTask("min_acc", variables.getVariable("acc0").getM(), variables.getVariable("acc0").getq())
+min_acc = GenericTask("min_acc", variables.getVariable("acc0").getM(), -variables.getVariable("acc0").getq())
 cost = min_xdot_task + zmp_tracking_task + 1e-3*min_acc + com[2] + 1e-3 * postural[18:] + 0.1 * amom + 0.1 * pelvis[3:]
 for foot_frame in foot_frames:
     cost = cost + contact_tasks[foot_frame]

bindings/python/examples/wblipm_ss.py

@@ -208,7 +208,7 @@ def update(self):
 xNs = state(Ns, variables, rddot0, h, dt)
 xNs.update()
 print(f"xNs: {xNs}")
-min_rdot_final = GenericTask("min_rdot_final", xNs[2:].getM(), xNs[2:].getq())
+min_rdot_final = GenericTask("min_rdot_final", xNs[2:].getM(), -xNs[2:].getq())
 
 
 
@@ -234,12 +234,12 @@ def create(cls, r0, rddot0, u0, h):
 lipmc = lipm_constraint.create(variables.getVariable("x0")[0:2], rddot0, variables.getVariable("u0"), h)
 
 
-xinit = variables.getVariable("x0") + np.hstack((model.getCOM()[0:2], model.getCOMVelocity()[0:2]))
-initial_state = GenericTask("initial_state", xinit.getM(), xinit.getq())
+xinit = variables.getVariable("x0") - np.hstack((model.getCOM()[0:2], model.getCOMVelocity()[0:2]))
+initial_state = GenericTask("initial_state", xinit.getM(), -xinit.getq())
 
 zmp_tasks = list()
 for i in range(Ns):
-    min_ui = GenericTask("zmp_tracking", variables.getVariable(f"u{i}").getM(), variables.getVariable(f"u{i}").getq())
+    min_ui = GenericTask("zmp_tracking", variables.getVariable(f"u{i}").getM(), -variables.getVariable(f"u{i}").getq())
     min_ui.setWeight(1e4 * np.array([[1, 0], [0, 1]]))
     zmp_tasks.append(min_ui)
 zmp_tracking_task = AggregatedTask(zmp_tasks, variables.getSize())
@@ -264,7 +264,7 @@ def create(cls, r0, rddot0, u0, h):
 
 
 # Create the stack
-min_acc = GenericTask("min_acc", variables.getVariable("acc0").getM(), variables.getVariable("acc0").getq())
+min_acc = GenericTask("min_acc", variables.getVariable("acc0").getM(), -variables.getVariable("acc0").getq())
 cost = 10.*min_rdot_final + zmp_tracking_task + com[2] + 1e-3 * min_acc + .1 * pelvis[3:] + 1. * postural[18:] + 1e-1 * amom
 for foot_frame in foot_frames:
     cost = cost + 1. * contact_tasks[foot_frame]