Skip to content
Snippets Groups Projects

Muscodwalkin. Closes #72

Merged Rohan Budhiraja requested to merge proyan/crocoddyl:muscodwalkin into devel
All threads resolved!
Files
2
 
import pinocchio
import numpy as np
import numpy as np
 
from collections import OrderedDict
from locomote import CubicHermiteSpline
from locomote import CubicHermiteSpline
import pinocchio
from crocoddyl import m2a, a2m
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():
class zero():
def __init__(self, dim):
def __init__(self, dim):
self.dim = dim
self.dim = dim
def eval(self, t): return (np.matrix(np.zeros((self.dim,1))),
def eval(self, t): return (np.matrix(np.zeros((self.dim,1))),
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)
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)
if hg is None: self.hg = self.zero(6)
else: self.hg = hg
else: self.hg = hg
if forces is None: self.forces = self.zero(12)
if forces is None: self.forces = EESplines()
else: self.forces = forces
else: self.forces = forces
def __add__(self,other):
def __add__(self,other):
if isinstance(self.com_vcom,self.zero):
if isinstance(self.com_vcom,self.zero):
return CentroidalPhi(other.com_vcom, other.hg, other.forces)
return CentroidalPhi(other.com_vcom, other.hg, other.forces)
if isinstance(other.com_vcom,other.zero):
if isinstance(other.com_vcom,other.zero):
return CentroidalPhi(self.com_vcom, self.hg, self.forces)
return CentroidalPhi(self.com_vcom, self.hg, self.forces)
return CentroidalPhi(self.com_vcom+other.com_vcom,self.hg+other.hg,
return CentroidalPhi(self.com_vcom+other.com_vcom,self.hg+other.hg,
self.forces+other.forces)
self.forces+other.forces)
def __sub__(self,other):
def __sub__(self,other):
if isinstance(self.com_vcom,self.zero):
if isinstance(self.com_vcom,self.zero):
return NotImplementedError
return NotImplementedError
@@ -35,8 +46,153 @@ class CentroidalPhi():
@@ -35,8 +46,153 @@ class CentroidalPhi():
return CentroidalPhi(self.com_vcom-other.com_vcom,self.hg-other.hg,
return CentroidalPhi(self.com_vcom-other.com_vcom,self.hg-other.hg,
self.forces-other.forces)
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]
mass = pinocchio.crba(rmodel, rdata, pinocchio.neutral(rmodel))[0,0]
t_traj =None
t_traj =None
forces_traj =None
forces_traj =None
@@ -51,13 +207,16 @@ def createPhiFromContactSequence(rmodel, rdata, cs):
@@ -51,13 +207,16 @@ def createPhiFromContactSequence(rmodel, rdata, cs):
N = len(t_traj)
N = len(t_traj)
#------Get values of state and control--------------
#------Get values of state and control--------------
cc = lambda t:0
phi_c = lambda t: 0
cc.f = np.zeros((N, 12)); cc.df = np.zeros((N,12));
phi_c.f = OrderedDict(); phi_c.df = OrderedDict();
cc.com_vcom = np.zeros((N,6)); cc.vcom_acom =np.zeros((N, 6));
for patch in patch_names:
cc.hg = np.zeros((N, 6)); cc.dhg = np.zeros((N, 6));
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;
n = 0;
f_rearrange = range(6,12)+range(0,6)
for i,spl in enumerate(cs.ms_interval_data[:-1]):
for i,spl in enumerate(cs.ms_interval_data[:-1]):
x = m2a(spl.state_trajectory)
x = m2a(spl.state_trajectory)
dx = m2a(spl.dot_state_trajectory)
dx = m2a(spl.dot_state_trajectory)
@@ -65,27 +224,30 @@ def createPhiFromContactSequence(rmodel, rdata, cs):
@@ -65,27 +224,30 @@ def createPhiFromContactSequence(rmodel, rdata, cs):
nt = len(x)
nt = len(x)
tt = t_traj[n:n+nt]
tt = t_traj[n:n+nt]
cc.com_vcom[n:n+nt,:] = x[:,:6]; cc.vcom_acom[n:n+nt,:] = dx[:,:6]
phi_c.com_vcom[n:n+nt,:] = x[:,:6]; phi_c.vcom_acom[n:n+nt,:] = dx[:,:6]
cc.hg[n:n+nt, 3:] = x[:,-3:]; cc.dhg[n:n+nt, 3:] = dx[:,-3:]
phi_c.hg[n:n+nt, 3:] = x[:,-3:]; phi_c.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.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.
#--Control output of MUSCOD is a discretized piecewise polynomial.
#------Convert the one piece to Points and Derivatives.
#------Convert the one piece to Points and Derivatives.
poly_u, dpoly_u = polyfitND(tt, u, deg=3, full=True, eps=1e-5)
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,r: np.array([poly_u[i](t) for i in r])
f_poly = lambda t: np.array([poly_u[i](t) for i in f_rearrange])
f_dpoly = lambda t,r: np.array([dpoly_u[i](t) for i in r])
f_dpoly = lambda t: np.array([dpoly_u[i](t) for i in f_rearrange])
for patch in patch_names:
cc.f[n:n+nt,:] = np.array([f_poly(t) for t in tt])
phi_c.f[patch][n:n+nt,:] = np.array([f_poly(t,range_def[patch]) for t in tt])
cc.df[n:n+nt,:] = np.array([f_dpoly(t) 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
n += nt
centroidal = CentroidalPhi()
centroidal = CentroidalPhi(patch_names)
centroidal.com_vcom = CubicHermiteSpline(a2m(t_traj), a2m(cc.com_vcom), a2m(cc.vcom_acom))
centroidal.com_vcom = CubicHermiteSpline(a2m(t_traj), a2m(phi_c.com_vcom),
centroidal.hg = CubicHermiteSpline(a2m(t_traj), a2m(cc.hg), a2m(cc.dhg))
a2m(phi_c.vcom_acom))
centroidal.forces = CubicHermiteSpline(a2m(t_traj), a2m(cc.f), a2m(cc.df))
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
return centroidal
def polyfitND(x, y, deg, eps, rcond=None, full=False, w=None, cov=False):
def polyfitND(x, y, deg, eps, rcond=None, full=False, w=None, cov=False):
Loading