Skip to content
Snippets Groups Projects
Commit 6ab7b5b2 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by nmansard
Browse files

First MPC, dirty copied from walk.py.

parent 9505cd14
Branches main
No related tags found
No related merge requests found
Pipeline #19269 failed with stage
in 7 minutes and 26 seconds
import pinocchio as pin
import crocoddyl as croc
import numpy as np
import example_robot_data as robex
import matplotlib.pyplot as plt; #plt.ion()
from numpy.linalg import norm,inv,pinv,svd,eig
# Local imports
import talos_low
from weight_share import computeReferenceForces
import sobec
import time
pin.SE3.__repr__=pin.SE3.__str__
np.set_printoptions(precision=2, linewidth=300, suppress=True,threshold=10000)
### HYPER PARAMETERS
# Hyperparameters defining the optimal control problem.
T_START = 30
T_SINGLE = 50
T_DOUBLE = 11
T_END = 30
### LOAD AND DISPLAY SOLO
# Load the robot model from example robot data and display it if possible in Gepetto-viewer
robot = talos_low.load()
contactIds = [ i for i,f in enumerate(robot.model.frames) if "sole_link" in f.name ]
ankleToTow=0.1
ankleToHeel=-0.1
for cid in contactIds:
f = robot.model.frames[cid]
robot.model.addFrame(
pin.Frame(f'{f.name}_tow',f.parent,f.previousFrame,
f.placement*pin.SE3(np.eye(3),np.array([ankleToTow,0,0])),pin.FrameType.OP_FRAME))
robot.model.addFrame(
pin.Frame(f'{f.name}_heel',f.parent,f.previousFrame,
f.placement*pin.SE3(np.eye(3),np.array([ankleToHeel,0,0])),pin.FrameType.OP_FRAME))
try:
viz = pin.visualize.GepettoVisualizer(robot.model,robot.collision_model,robot.visual_model)
viz.initViewer()
viz.loadViewerModel()
gv = viz.viewer.gui
except:
print("No viewer" )
# The pinocchio model is what we are really interested by.
model = robot.model
model.q0 = robot.q0
data = model.createData()
# Initial config, also used for warm start
x0 = np.concatenate([model.q0,np.zeros(model.nv)])
# Some key elements of the model
towIds = { idf: model.getFrameId(f'{model.frames[idf].name}_tow') for idf in contactIds }
heelIds = { idf: model.getFrameId(f'{model.frames[idf].name}_heel') for idf in contactIds }
baseId = model.getFrameId('root_joint')
robotweight = -sum([Y.mass for Y in model.inertias]) * model.gravity.linear[2]
com0 = pin.centerOfMass(model,data,model.q0)
pin.framesForwardKinematics(model,data,x0[:model.nq])
###################################################################################################
### TUNING ########################################################################################
###################################################################################################
# In the code, cost terms with 0 weight are commented for reducing execution cost
# An example of working weight value is then given as comment at the end of the line.
# When setting them to >0, take care to uncomment the corresponding line.
# All these lines are marked with the tag ##0##.
from mpcparams import *
assert(len(STATE_WEIGHT)==model.nv*2)
# Contact are specified with the order chosen in <contactIds>
# contactPattern = [] \
# + [ [ 1,1 ] ] * 40 \
# + [ [ 1,0 ] ] * 50 \
# + [ [ 1,1 ] ] * 11 \
# + [ [ 0,1 ] ] * 50 \
# + [ [ 1,1 ] ] * 11 \
# + [ [ 1,1 ] ] * 40 \
# + [ [ 1,1 ] ]
contactPattern = [] \
+ [ [ 1,1 ] ] * T_START \
+ [ [ 1,1 ] ] * T_DOUBLE \
+ [ [ 1,0 ] ] * T_SINGLE \
+ [ [ 1,1 ] ] * T_DOUBLE \
+ [ [ 0,1 ] ] * T_SINGLE \
+ [ [ 1,1 ] ] * T_DOUBLE \
+ [ [ 1,1 ] ] * T_END \
+ [ [ 1,1 ] ]
T = len(contactPattern)-1
def patternToId(pattern):
'''Return the tuple of active contact from a pattern like [0,1], [1,0] or [1,1].'''
return tuple( contactIds[i] for i,c in enumerate(pattern) if c==1 )
# ### REF FORCES ######################################################################
# The force costs are defined using a reference (smooth) force.
# # Search the contact phase of minimal duration (typically double support)
# contactState=[]
# dur=mindur=len(contactPattern)
# for t,s in enumerate(contactPattern):
# dur+=1
# if s!=contactState:
# contactState=s
# mindur=min(mindur,dur)
# dur=0
# # Select the smoothing transition to be smaller than half of the minimal duration.
# transDuration=(mindur-1)//2
# # Compute contact importance, ie how much of the weight should be supported by each
# # foot at each time.
# contactImportance = weightShareSmoothProfile(contactPattern,transDuration,switch=switch_linear)
# # Contact reference forces are set to contactimportance*weight
# weightReaction = np.array([0,0,robotweight,0,0,0])
# referenceForces = [
# [ weightReaction*contactImportance[t,cid] for cid,__c in enumerate(pattern) ]
# for t,pattern in enumerate(contactPattern) ]
# # Take care, we suppose here that foot normal is vertical.
referenceForces = computeReferenceForces(contactPattern,robotweight)
# #################################################################################################
# #################################################################################################
# #################################################################################################
models = []
# #################################################################################################
for t,pattern in enumerate(contactPattern[:-1]):
#print(f'time t={t}')
# Basics
state = croc.StateMultibody(model)
actuation = croc.ActuationModelFloatingBase(state)
# Contacts
contacts = croc.ContactModelMultiple(state, actuation.nu)
for k,cid in enumerate(contactIds):
if not pattern[k]: continue
contact = croc.ContactModel6D(state, cid, pin.SE3.Identity(), actuation.nu, baumgartGains)
contacts.addContact(model.frames[cid].name + "_contact", contact)
# Costs
costs = croc.CostModelSum(state, actuation.nu)
xRegResidual = croc.ResidualModelState(state, x0, actuation.nu)
xRegCost = croc.CostModelResidual(state, croc.ActivationModelWeightedQuad(STATE_WEIGHT**2),xRegResidual)
costs.addCost("stateReg", xRegCost, refStateWeight)
uResidual = croc.ResidualModelControl(state, actuation.nu)
uRegCost = croc.CostModelResidual(state, croc.ActivationModelWeightedQuad(np.array(CONTROL_WEIGHT**2)), uResidual)
costs.addCost("ctrlReg", uRegCost, refTorqueWeight)
comVelResidual = sobec.ResidualModelCoMVelocity(state, VCOM_TARGET, actuation.nu)
comVelCost = croc.CostModelResidual( state,comVelResidual )
costs.addCost("comVelCost", comVelCost, vcomWeight)
# Contact costs
for k,cid in enumerate(contactIds):
if not pattern[k]: continue
copResidual = sobec.ResidualModelCenterOfPressure(state,cid,actuation.nu)
copAct = croc.ActivationModelWeightedQuad(np.array([ 1/FOOT_SIZE**2 ]*2))
copCost = croc.CostModelResidual( state,copAct,copResidual)
costs.addCost(f'{model.frames[cid].name}_cop',copCost,copWeight)
# Cone with enormous friction (Assuming the robot will barely ever slide).
# FOOT_SIZE is the allowed area size, while cone expects the corner coordinates => x2
cone = croc.WrenchCone(np.eye(3), 1000, np.array([ FOOT_SIZE*2 ]*2),4, True,1,10000)
coneCost = croc.ResidualModelContactWrenchCone(state,cid,cone,actuation.nu)
ub = cone.ub.copy()
ub[:4] = np.inf
#ub[5:] = np.inf ### DEBUG
ub[-8:] = np.inf
coneAct = croc.ActivationModelQuadraticBarrier(croc.ActivationBounds(cone.lb,ub))
coneCost = croc.CostModelResidual(state,coneAct,coneCost)
costs.addCost(f'{model.frames[cid].name}_cone',coneCost,conePenaltyWeight)
# Penalize the distance to the central axis of the cone ...
# ... using normalization weights depending on the axis.
# The weights are squared to match the tuning of the CASADI formulation.
coneAxisResidual = croc.ResidualModelContactForce(state,cid,pin.Force.Zero(),6,actuation.nu);
w = np.array(forceImportance**2); w[2] = 0
coneAxisAct = croc.ActivationModelWeightedQuad(w)
coneAxisCost = croc.CostModelResidual(state,coneAxisAct,coneAxisResidual);
costs.addCost(f'{model.frames[cid].name}_coneaxis',coneAxisCost,coneAxisWeight)
# Follow reference (smooth) contact forces
forceRefResidual = croc.ResidualModelContactForce(state,cid,pin.Force(referenceForces[t][k]),6,actuation.nu)
forceRefCost = croc.CostModelResidual(state,forceRefResidual)
costs.addCost(f'{model.frames[cid].name}_forceref',forceRefCost,refForceWeight/robotweight**2)
# IMPACT
for k,cid in enumerate(contactIds):
if t>0 and not contactPattern[t-1][k] and pattern[k]:
# REMEMBER TO divide the weight by DT, as impact should be independant of the node duration
# (at least, that s how weights are tuned in casadi).
print(f'Impact {cid} at time {t}')
if 'left' in model.frames[cid].name:
itarget = np.array([0,.1,0])
else:
itarget = np.array([0,-.1,0])
impactResidual = croc.ResidualModelFrameTranslation(state,cid,itarget,actuation.nu)
impactAct = croc.ActivationModelWeightedQuad(np.array([0,100,1]))
impactCost = croc.CostModelResidual(state,impactAct,impactResidual)
costs.addCost(f'{model.frames[cid].name}_atitudeimpact',impactCost,impactAltitudeWeight/DT)
impactVelResidual = croc.ResidualModelFrameVelocity(state,cid,pin.Motion.Zero(),pin.ReferenceFrame.LOCAL,actuation.nu)
impactVelCost = croc.CostModelResidual(state,impactVelResidual)
costs.addCost(f'{model.frames[cid].name}_velimpact',impactVelCost,impactVelocityWeight/DT)
impactRotResidual = croc.ResidualModelFrameRotation(state,cid,np.eye(3),actuation.nu)
impactRotAct = croc.ActivationModelWeightedQuad(np.array([1,1,0]))
impactRotCost = croc.CostModelResidual(state,impactRotAct,impactRotResidual)
costs.addCost(f'{model.frames[cid].name}_rotimpact',impactRotCost,impactRotationWeight/DT)
impactRefJointsResidual = croc.ResidualModelState(state,x0,actuation.nu)
jselec = np.zeros(model.nv*2)
jselec[[ model.joints[model.getJointId(name)].idx_v for name in MAIN_JOINTS ]]=1
impactRefJointsAct = croc.ActivationModelWeightedQuad(jselec)
impactRefJointCost = croc.CostModelResidual(state,impactRefJointsAct,impactRefJointsResidual)
costs.addCost('impactRefJoint',impactRefJointCost,refMainJointsAtImpactWeight/DT)
# Flying foot
for k,cid in enumerate(contactIds):
if pattern[k]: continue
verticalFootVelResidual = croc.ResidualModelFrameVelocity(state,cid,pin.Motion.Zero(),
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED,actuation.nu)
verticalFootVelAct = croc.ActivationModelWeightedQuad(np.array([0,0,1,0,0,0]))
verticalFootVelCost = croc.CostModelResidual(state,verticalFootVelAct,verticalFootVelResidual)
costs.addCost(f'{model.frames[cid].name}_vfoot_vel',verticalFootVelCost,verticalFootVelWeight)
# Slope is /2 since it is squared in casadi (je me comprends)
flyHighResidual = sobec.ResidualModelFlyHigh(state,cid,flyHighSlope/2,actuation.nu)
flyHighCost = croc.CostModelResidual(state,flyHighResidual)
costs.addCost(f'{model.frames[cid].name}_flyhigh',flyHighCost,flyWeight)
groundColRes = croc.ResidualModelFrameTranslation(state,cid,np.zeros(3),actuation.nu)
#groundColBounds = croc.ActivationBounds(np.array([-np.inf,-np.inf,0.01]),np.array([np.inf,np.inf,np.inf]))
# np.inf introduces an error on lb[2] ... why? TODO ... patch by replacing np.inf with 1000
groundColBounds = croc.ActivationBounds(np.array([-1000,-1000,0.0]),np.array([1000,1000,1000]))
groundColAct = croc.ActivationModelQuadraticBarrier(groundColBounds)
groundColCost = croc.CostModelResidual(state,groundColAct,groundColRes)
costs.addCost(f'{model.frames[cid].name}_groundcol',groundColCost,groundColWeight)
# Action
damodel = croc.DifferentialActionModelContactFwdDynamics(
state, actuation, contacts, costs, kktDamping, True)
amodel = croc.IntegratedActionModelEuler(damodel, DT)
models.append(amodel)
#stophere
# #################################################################################################
pattern = contactPattern[-1]
state = croc.StateMultibody(model)
actuation = croc.ActuationModelFloatingBase(state)
# Contacts
contacts = croc.ContactModelMultiple(state, actuation.nu)
for k,cid in enumerate(contactIds):
if not pattern[k]: continue
contact = croc.ContactModel6D(state, cid, pin.SE3.Identity(), actuation.nu, baumgartGains)
contacts.addContact(model.frames[cid].name + "_contact", contact)
# Costs
costs = croc.CostModelSum(state, actuation.nu)
xRegResidual = croc.ResidualModelState(state, x0, actuation.nu)
xRegCost = croc.CostModelResidual(state, croc.ActivationModelWeightedQuad(np.array([0]*model.nv+[1]*model.nv)),xRegResidual)
costs.addCost("stateReg", xRegCost, terminalNoVelocityWeight)
damodel = croc.DifferentialActionModelContactFwdDynamics(
state, actuation, contacts, costs, kktDamping, True)
termmodel = croc.IntegratedActionModelEuler(damodel, DT)
# #################################################################################################
#GUESS_FILE = '/tmp/sol.npy'
#guess = np.load(GUESS_FILE,allow_pickle=True)[()]
#print(f'Load "{GUESS_FILE}"!')
# #################################################################################################
problem = croc.ShootingProblem(x0,models,termmodel)
ddp = croc.SolverFDDP(problem)
x0s = [x0.copy()]*(len(models)+1)
u0s = [ m.quasiStatic(d,x) for m,d,x in zip(problem.runningModels,problem.runningDatas,x0s) ]
ddp.setCallbacks([croc.CallbackVerbose()])
#x0s = [x for x in guess['xs']]
#u0s = [u for u in guess['us']]
# l = pin.StdVec_Double()
# l.append(2)
# ddp.alphas = l
#ddp.th_acceptStep = 0.1
ddp.th_stop = 1e-3
ddp.solve(x0s,u0s,200)
# ### MPC #########################################################################################
Tmpc = 100
mpcProblem = croc.ShootingProblem(x0,models[:Tmpc],termmodel)
mpcSolver = croc.SolverFDDP(mpcProblem)
#mpcSolver.setCallbacks([croc.CallbackVerbose()])
mpcSolver.th_stop = 1e-3
mpcSolver.solve(ddp.xs[:Tmpc+1],ddp.us[:Tmpc],10,isFeasible=True)
x = mpcSolver.xs[1].copy()
def contact2car(contacts):
if len(contacts)==2: return '='
if len(contacts)==0: return ' '
if 'right' in next(iter(contacts)).key(): return '_'
if 'left' in next(iter(contacts)).key(): return ''
error
def dispocp(pb):
return ''.join([contact2car(r.differential.contacts.contacts) for r in pb.runningModels ])
hx = [ x ]
hiter = [ mpcSolver.iter ]
for t in range(1,10000):
#tlast = t+Tmpc
tlast = T_START+((t+Tmpc-T_START)%(2*T_SINGLE+2*T_DOUBLE))
#print(f't={t} ... last is {tlast}')
mpcProblem.circularAppend(problem.runningModels[tlast],problem.runningDatas[tlast])
mpcProblem.x0 = x.copy()
print(dispocp(mpcProblem))
#assert(mpcProblem.runningModels[0] == problem.runningModels[t])
xg = list(mpcSolver.xs)[1:]+[mpcSolver.xs[-1]]
ug = list(mpcSolver.us)[1:]+[mpcSolver.us[-1]]
mpcSolver.solve(xg,ug)
x = mpcSolver.xs[1].copy()
hx.append(x)
hiter.append(mpcSolver.iter)
if not t%10:
viz.display(x[:model.nq])
time.sleep(DT)
def savempc():
SOLU_FILE = "/tmp/mpc.npy"
np.save(open(SOLU_FILE, "wb"),
{
"xs": np.array(hx),
})
print(f'Save "{SOLU_FILE}"!')
# ### PLOT ######################################################################
# ### PLOT ######################################################################
# ### PLOT ######################################################################
xs_sol = np.array(ddp.xs)
us_sol = np.array(ddp.us)
acs_sol = np.array([ d.differential.xout for d in problem.runningDatas ])
fs_sol = [ [ (cd.data().jMf.inverse()*cd.data().f).vector for cd in d.differential.multibody.contacts.contacts ]
for d in problem.runningDatas ]
fs_sol0 = [ np.concatenate([
(d.differential.multibody.contacts.contacts[f'{model.frames[cid].name}_contact'].jMf.inverse()*
d.differential.multibody.contacts.contacts[f'{model.frames[cid].name}_contact'].f).vector
if f'{model.frames[cid].name}_contact' in d.differential.multibody.contacts.contacts
else np.zeros(6)
for cid in contactIds ])
for m,d in zip(problem.runningModels,problem.runningDatas) ]
from walk_plotter import WalkPlotter
plotter = WalkPlotter(model,contactIds)
plotter.setData(contactPattern,xs_sol,us_sol,fs_sol0)
plotter.plotBasis(X_TARGET)
plotter.plotTimeCop()
plotter.plotCopAndFeet(FOOT_SIZE,.6)
plotter.plotForces(referenceForces)
plotter.plotCom(com0)
plotter.plotFeet()
plotter.plotFootCollision(footMinimalDistance)
# ### SAVE #####################################################################
def save():
SOLU_FILE = "/tmp/ddp.npy"
np.save(open(SOLU_FILE, "wb"),
{
"xs": xs_sol,
"us": us_sol,
"acs": acs_sol,
"fs": np.array(fs_sol0),
})
print(f'Save "{SOLU_FILE}"!')
'''
-B < tau/f < B
tau < Bf
Bf - tau >=0
tau >= -Bf
Bf + tau >= 0
'''
### DEBUG
# dmodel = problem.runningModels[0].differential
# ddata = problem.runningDatas[0].differential
# contactmodell = dmodel.contacts.contacts['left_sole_link_contact'].contact
# contactmodelr = dmodel.contacts.contacts['right_sole_link_contact'].contact
# contactdatal = ddata.multibody.contacts.contacts['left_sole_link_contact']
# contactdatar = ddata.multibody.contacts.contacts['right_sole_link_contact']
# copcostmodell = dmodel.costs.costs['left_sole_link_cop'].cost
# copcostmodelr = dmodel.costs.costs['right_sole_link_cop'].cost
# copcostdatal = ddata.costs.costs['left_sole_link_cop']
# copcostdatar = ddata.costs.costs['right_sole_link_cop']
t = 60; fid=48
t=119; fid=34
t=90; cid=48 # impact
xs=guess['xs']
us=guess['us']
fs0=guess['fs']
acs=guess['acs']
dadata=problem.runningDatas[t].differential
damodel=problem.runningModels[t].differential
damodel.calc(dadata,xs[t],us[t])
damodel.calcDiff(dadata,xs[t],us[t])
cosname='left_sole_link_cone'
cosname='right_sole_link_cone'
#cosname='altitudeImpact'
cosname='right_sole_link_vfoot_vel' # t = 60
cosname='right_sole_link_flyhigh'
cosname=f'{model.frames[fid].name}_flyhigh'
cosname=f'{model.frames[fid].name}_groundcol'
cosname=f'{model.frames[cid].name}_velimpact'
cosname='impactRefJoint'
cosdata = dadata.costs.costs[cosname]
cosmodel = damodel.costs.costs[cosname].cost
np.set_printoptions(precision=6, linewidth=300, suppress=True,threshold=10000)
from params import *
terminalNoVelocityWeight = 20
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment