Muscodwalkin. Closes #72

Loads references from the centroidal solver, and runs ddp to track those references.

Some work still needs to be done to define properly the tasks (e.g., to add high cost velocity tracking on foot during contact transitions).

The current framework is meant to deal with multi-contact locomotion (and not just bipedal walking)

locomotion/centroidal_utils.py

@@ -81,8 +81,9 @@ def createMultiphaseShootingProblem(rmodel, rdata, patch_name_map, cs, phi_c, sw
 
   #Define Cost weights
   w = EmptyClass()
-  w.com = 1e3;    w.state = 1e-1;    w.control = 1e-3;
-  w.swing_patch = 1e3; w.forces = 1e-4;
+  w.com = 1e2;    w.state = 1e-1;    w.control = 1e-3;
+  w.swing_patch = 1e4; w.forces = 1e-4;
+  w.swingv = 1e4
 
   #Define state cost vector for WeightedActivation
   w.xweight = np.array([0]*6+[0.01]*(rmodel.nv-6)+[10.]*rmodel.nv)
@@ -141,12 +142,20 @@ def createMultiphaseShootingProblem(rmodel, rdata, patch_name_map, cs, phi_c, sw
       #Control Regularization
       cost_regu = CostModelControl(rmodel, nu = actuationff.nu)
       cost_model.addCost("regu", cost_regu, w.control)
-      
+
       dmodel = DifferentialActionModelFloatingInContact(rmodel, actuationff,
                                                         contact_model, cost_model)
       imodel = IntegratedActionModelEuler(dmodel)
       problem_models.append(imodel)
 
+    #for the last model of the phase, add velocity cost on swing limbs.
+    for patch in swing_patch:
+      cost_vswing = CostModelFrameVelocity(rmodel,
+                                           frame=rmodel.getFrameId(patch_name_map[patch]),
+                                           ref=m2a(pinocchio.Motion.Zero().vector),
+                                           nu=actuationff.nu)
+      problem_models[-1].differential.costs.addCost("swingv_"+patch, cost_swing, w.swingv)
+      
   #Create Terminal Model.
   contact_model = ContactModelMultiple(rmodel)
   # Add contact constraints for the active contact patches.