Skip to content
Snippets Groups Projects

Muscodwalkin. Closes #72

Merged Rohan Budhiraja requested to merge proyan/crocoddyl:muscodwalkin into devel
Files
2
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):
Loading