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

+import pinocchio
 import numpy as np
+from collections import OrderedDict
 from locomote import CubicHermiteSpline
-import pinocchio
 from crocoddyl import m2a, a2m
-from locomote import CubicHermiteSpline
+from crocoddyl import CostModelSum,CostModelState,CostModelControl,CostModelForce,\
+  CostModelFrameVelocity,CostModelFramePlacement,CostModelCoM,\
+  ImpulseModelMultiple,DifferentialActionModelFloatingInContact,IntegratedActionModelEuler,\
+  StatePinocchio,ContactModel6D,ContactModelMultiple,\
+  ActuationModelFreeFloating, ActivationModelWeightedQuad
 
-class CentroidalPhi():
+class EESplines(OrderedDict):
+  def __add__(self,other):
+    return EESplines([[patch, self[patch] + other[patch]] for patch in self.keys()]);
+  def __sub__(self,other):
+    return EESplines([[patch, self[patch] - other[patch]] for patch in self.keys()]);
+
+class CentroidalPhi:
 
   class zero():
     def __init__(self, dim):
       self.dim = dim
     def eval(self, t): return (np.matrix(np.zeros((self.dim,1))),
                                np.matrix(np.zeros((self.dim,1))))
-    
-  def __init__(self, com_vcom=None, hg=None, forces=None):
+
+  def __init__(self, patch_names, com_vcom=None, hg=None, forces=None):
     if com_vcom is None: self.com_vcom = self.zero(6)
-    else:                  self.com_vcom = com_vcom
+    else:                self.com_vcom = com_vcom
     if hg is None:       self.hg = self.zero(6)
-    else:                  self.hg = hg      
-    if forces is None:   self.forces = self.zero(12)
-    else:                  self.forces = forces
-      
+    else:                self.hg = hg
+    if forces is None:   self.forces = EESplines()
+    else:                self.forces = forces
+    
   def __add__(self,other):
     if isinstance(self.com_vcom,self.zero):
       return CentroidalPhi(other.com_vcom, other.hg, other.forces)
     if isinstance(other.com_vcom,other.zero):
       return CentroidalPhi(self.com_vcom, self.hg, self.forces)
     return CentroidalPhi(self.com_vcom+other.com_vcom,self.hg+other.hg,
-                      self.forces+other.forces)
+                         self.forces+other.forces)
   def __sub__(self,other):
     if isinstance(self.com_vcom,self.zero):
       return NotImplementedError
@@ -35,8 +46,153 @@ class CentroidalPhi():
     return CentroidalPhi(self.com_vcom-other.com_vcom,self.hg-other.hg,
                       self.forces-other.forces)
 
+def createSwingTrajectories(rmodel, rdata, x, contact_patches, dt):
+  p = OrderedDict(); m = OrderedDict()
+  N = len(x)
+  abscissa = a2m(np.linspace(0.,dt*(N-1), N))
+  swing_ref = EESplines()
+  for patch in contact_patches.keys():
+    p = np.zeros((3,N));  m = np.zeros((3,N))
+    for i in xrange(N):
+      q = a2m(x[i][:rmodel.nq])
+      v = a2m(x[i][-rmodel.nv:])
+      pinocchio.forwardKinematics(rmodel, rdata, q, v)
+      p[:,i] = m2a(pinocchio.updateFramePlacement(rmodel, rdata,
+                                  rmodel.getFrameId(contact_patches[patch])).translation)
+      m[:,i] = m2a(pinocchio.getFrameVelocity(rmodel, rdata,
+                                  rmodel.getFrameId(contact_patches[patch])).linear)
+    swing_ref.update([[patch, CubicHermiteSpline(abscissa, p, m)]])
+  return swing_ref
+
+def createMultiphaseShootingProblem(rmodel, rdata, patch_name_map, cs, phi_c, swing_ref, dt):
+  """
+  Create a Multiphase Shooting problem from the output of the centroidal solver.
+  
+  :params rmodel: robot model of type pinocchio::model
+  :params rdata: robot data of type pinocchio::data
+  :params patch_name_map: dictionary mapping of contact_patch->robot_framename. e.g. "LF_Patch":leg_6_joint
+  :params cs: contact sequence of type locomote::ContactSequenceHumanoid
+  :params phi_c: centroidal reference of type CentroidalPhi
+  :params swing_ref: end-effector trajectories of type EESplines
+  :params dt: Scalar timestep between nodes.
+
+  :returns list of IntegratedActionModels
+  """
+
+  #Define Cost weights
+  w = lambda t: 0
+  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)
+  #for patch in swing_patch:  w.swing_patch.append(100.);
+
+  problem_models = []
+  actuationff = ActuationModelFreeFloating(rmodel)
+  State = StatePinocchio(rmodel)  
+
+  for phase in cs.contact_phases:
+    t0 = phase.time_trajectory[0];    t1 = phase.time_trajectory[-1]
+    N = int(round((t1-t0)/dt))+1
+    contact_model = ContactModelMultiple(rmodel)
+
+    # Add contact constraints for the active contact patches.
+    # Add SE3 cost for the non-active contact patches.
+    swing_patch = [];
+    for patch in patch_name_map.keys():
+      if getattr(phase, patch).active:
+        active_contact = ContactModel6D(rmodel,
+                                        frame=rmodel.getFrameId(patch_name_map[patch]),
+                                        ref = getattr(phase,patch).placement)
+        contact_model.addContact(patch, active_contact)
+      else:
+        swing_patch.append(patch)
+
+    # Define the cost and action models for each timestep in the contact phase.
+    for t in np.linspace(t0,t1,N)[:-1]:
+      cost_model = CostModelSum(rmodel, actuationff.nu);
+      #CoM Cost
+      cost_com = CostModelCoM(rmodel, ref=m2a(phi_c.com_vcom.eval(t)[0][:3,:]),
+                              nu = actuationff.nu);
+      cost_model.addCost("CoM",cost_com, w.com)
+
+      #Forces Cost
+      for patch in contact_model.contacts.keys():
+        cost_force = CostModelForce(rmodel,
+                                    contactModel=contact_model.contacts[patch],
+                                    ref =m2a(phi_c.forces[patch].eval(t)[0]),
+                                    nu = actuationff.nu);
+        cost_model.addCost("forces_"+patch,cost_force, w.forces)
+      #Swing patch cost
+      for patch in swing_patch:
+        cost_swing = CostModelFramePlacement(rmodel,
+                                             frame=rmodel.getFrameId(patch_name_map[patch]),
+                                             ref=pinocchio.SE3(np.identity(3),
+                                                               swing_ref[patch].eval(t)[0]),
+                                             nu=actuationff.nu)
+        cost_model.addCost("swing_"+patch, cost_swing, w.swing_patch)
+
+      #State Regularization
+      cost_regx = CostModelState(rmodel, State, ref=rmodel.defaultState,
+                                 nu = actuationff.nu,
+                                 activation=ActivationModelWeightedQuad(w.xweight**2))
+      cost_model.addCost("regx", cost_regx, w.state)
+      #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.
+  swing_patch = [];  t=t1;
+  for patch in patch_name_map.keys():
+    if getattr(phase, patch).active:
+      active_contact = ContactModel6D(rmodel,
+                                      frame=rmodel.getFrameId(patch_name_map[patch]),
+                                      ref = getattr(phase,patch).placement)
+      contact_model.addContact(patch, active_contact)
+  cost_model = CostModelSum(rmodel, actuationff.nu);
+  #CoM Cost
+  cost_com = CostModelCoM(rmodel, ref=m2a(phi_c.com_vcom.eval(t)[0][:3,:]),
+                          nu = actuationff.nu);
+  cost_model.addCost("CoM",cost_com, w.com)
+
+  #State Regularization
+  cost_regx = CostModelState(rmodel, State, ref=rmodel.defaultState,
+                             nu = actuationff.nu,
+                             activation=ActivationModelWeightedQuad(w.xweight**2))
+  cost_model.addCost("regx", cost_regx, w.state)
+
+  dmodel = DifferentialActionModelFloatingInContact(rmodel, actuationff,
+                                                    contact_model, cost_model)
+  imodel = IntegratedActionModelEuler(dmodel)
+  problem_models.append(imodel)  
+  problem_models.append
+  return problem_models
+
+def createPhiFromContactSequence(rmodel, rdata, cs, patch_names):
+  #Note that centroidal plannar returns the forces in the sequence RF,LF,RH,LH.
+  range_def = OrderedDict()
+  range_def.update([["RF_patch", range(0,6)]]);
+  range_def.update([["LF_patch", range(6,12)]])
+  range_def.update([["RH_patch", range(12,18)]])
+  range_def.update([["LH_patch", range(18,24)]])
   
-def createPhiFromContactSequence(rmodel, rdata, cs):
   mass = pinocchio.crba(rmodel, rdata, pinocchio.neutral(rmodel))[0,0]
   t_traj =None
   forces_traj =None
@@ -51,13 +207,16 @@ def createPhiFromContactSequence(rmodel, rdata, cs):
   N = len(t_traj)
 
   #------Get values of state and control--------------
-  cc = lambda t:0
-  cc.f = np.zeros((N, 12)); cc.df = np.zeros((N,12));
-  cc.com_vcom = np.zeros((N,6)); cc.vcom_acom =np.zeros((N, 6));
-  cc.hg = np.zeros((N, 6));      cc.dhg = np.zeros((N, 6));
+  phi_c = lambda t: 0
+  phi_c.f = OrderedDict(); phi_c.df = OrderedDict();
+  for patch in patch_names:
+    phi_c.f.update([[patch, np.zeros((N,6))]])
+    phi_c.df.update([[patch, np.zeros((N,6))]])
+  
+  phi_c.com_vcom = np.zeros((N,6)); phi_c.vcom_acom =np.zeros((N, 6));
+  phi_c.hg = np.zeros((N, 6));      phi_c.dhg = np.zeros((N, 6));
 
   n = 0;
-  f_rearrange = range(6,12)+range(0,6)
   for i,spl in enumerate(cs.ms_interval_data[:-1]):
     x = m2a(spl.state_trajectory)
     dx = m2a(spl.dot_state_trajectory)
@@ -65,27 +224,30 @@ def createPhiFromContactSequence(rmodel, rdata, cs):
     nt = len(x)
 
     tt = t_traj[n:n+nt]
-    cc.com_vcom[n:n+nt,:] = x[:,:6];     cc.vcom_acom[n:n+nt,:] = dx[:,:6]
-    cc.hg[n:n+nt, 3:] = x[:,-3:];        cc.dhg[n:n+nt, 3:] = dx[:,-3:]
-    cc.hg[n:n+nt, :3] = mass*x[:,3:6];   cc.dhg[n:n+nt, :3] = mass*dx[:,3:6]
+    phi_c.com_vcom[n:n+nt,:] = x[:,:6];     phi_c.vcom_acom[n:n+nt,:] = dx[:,:6]
+    phi_c.hg[n:n+nt, 3:] = x[:,-3:];        phi_c.dhg[n:n+nt, 3:] = dx[:,-3:]
+    phi_c.hg[n:n+nt, :3] = mass*x[:,3:6];   phi_c.dhg[n:n+nt, :3] = mass*dx[:,3:6]
 
     #--Control output of MUSCOD is a discretized piecewise polynomial.
     #------Convert the one piece to Points and Derivatives.
     poly_u, dpoly_u = polyfitND(tt, u, deg=3, full=True, eps=1e-5)
-    #Note that centroidal plannar returns the forces in the sequence RF,LF,RH,LH.
-    #The wholebody solver follows the urdf parsing given by LF,RF,LH,RH
-    f_poly = lambda t: np.array([poly_u[i](t) for i in f_rearrange])
-    f_dpoly = lambda t: np.array([dpoly_u[i](t) for i in f_rearrange])
-    cc.f[n:n+nt,:] = np.array([f_poly(t) for t in tt])
-    cc.df[n:n+nt,:] = np.array([f_dpoly(t) for t in tt])
-    
+
+    f_poly = lambda t,r: np.array([poly_u[i](t) for i in r])
+    f_dpoly = lambda t,r: np.array([dpoly_u[i](t) for i in r])
+    for patch in patch_names:
+      phi_c.f[patch][n:n+nt,:]  = np.array([f_poly(t,range_def[patch]) for t in tt])
+      phi_c.df[patch][n:n+nt,:] = np.array([f_dpoly(t,range_def[patch]) for t in tt])
+
     n += nt
   
-  centroidal = CentroidalPhi()
-  centroidal.com_vcom = CubicHermiteSpline(a2m(t_traj), a2m(cc.com_vcom), a2m(cc.vcom_acom))
-  centroidal.hg = CubicHermiteSpline(a2m(t_traj), a2m(cc.hg), a2m(cc.dhg))
-  centroidal.forces = CubicHermiteSpline(a2m(t_traj), a2m(cc.f), a2m(cc.df))
+  centroidal = CentroidalPhi(patch_names)
+  centroidal.com_vcom = CubicHermiteSpline(a2m(t_traj), a2m(phi_c.com_vcom),
+                                           a2m(phi_c.vcom_acom))
+  centroidal.hg = CubicHermiteSpline(a2m(t_traj), a2m(phi_c.hg), a2m(phi_c.dhg))
 
+  for patch in patch_names:
+    centroidal.forces[patch] = CubicHermiteSpline(a2m(t_traj), a2m(phi_c.f[patch]),
+                                                  a2m(phi_c.df[patch]))
   return centroidal
 
 def polyfitND(x, y, deg, eps, rcond=None, full=False, w=None, cov=False):