Skip to content
Snippets Groups Projects

Muscodwalkin. Closes #72

Merged Rohan Budhiraja requested to merge proyan/crocoddyl:muscodwalkin into devel
All threads resolved!
@@ -2,20 +2,49 @@ import pinocchio
@@ -2,20 +2,49 @@ import pinocchio
import conf_talos_warm_start as conf
import conf_talos_warm_start as conf
from time import sleep
from time import sleep
from locomote import ContactSequenceHumanoid
from locomote import ContactSequenceHumanoid
from centroidal_utils import createPhiFromContactSequence
from centroidal_utils import createPhiFromContactSequence, createMultiphaseShootingProblem, createSwingTrajectories
 
from crocoddyl import m2a, a2m
 
from crocoddyl import ShootingProblem, SolverDDP
 
from crocoddyl import CallbackDDPVerbose, CallbackSolverDisplay, CallbackDDPLogger
 
import numpy as np
 
np.set_printoptions(linewidth=400, suppress=True)
robot = conf.robot
robot = conf.robot
rmodel = robot.model
rmodel = robot.model
rdata = robot.data
rdata = robot.data
 
rmodel.defaultState = np.concatenate([m2a(rmodel.neutralConfiguration),np.zeros(rmodel.nv)])
#---------------Display Initial Trajectory--------------
#---------------Display Initial Trajectory--------------
if conf.DISPLAY:
if conf.DISPLAY and False:
robot.initDisplay(loadModel=True)
robot.initDisplay(loadModel=True)
for x in conf.X_init:
for x in conf.X_init:
robot.display(x[:robot.nq])
robot.display(a2m(x[:robot.nq]))
sleep(0.005)
#sleep(0.005)
#----------------Load Contact Phases-----------------------
#----------------Load Contact Phases-----------------------
cs = ContactSequenceHumanoid(0)
cs = ContactSequenceHumanoid(0)
cs.loadFromXML(conf.MUSCOD_CS_OUTPUT_FILENAME, conf.CONTACT_SEQUENCE_XML_TAG)
cs.loadFromXML(conf.MUSCOD_CS_OUTPUT_FILENAME, conf.CONTACT_SEQUENCE_XML_TAG)
cc = createPhiFromContactSequence(rmodel, rdata, cs)
#----------------Define References-------------------------
 
 
swing_ref = createSwingTrajectories(rmodel, rdata, conf.X_init, conf.contact_patches, conf.DT)
 
phi_c = createPhiFromContactSequence(rmodel, rdata, cs, conf.contact_patches.keys())
 
 
#----------------Define Problem----------------------------
 
models = createMultiphaseShootingProblem(rmodel, rdata, conf.contact_patches,
 
cs, phi_c, swing_ref, conf.DT)
 
 
disp = lambda xs: disptraj(robot,xs)
 
 
problem = ShootingProblem(m2a(conf.X_init[0]), models[:-1], models[-1])
 
 
#Set contacts in the data elements. Ugly.
 
#This is defined for IAMEuler. If using IAMRK4, differential is a list. so we need to change.
 
for d in problem.runningDatas:
 
for (patchname, contactData) in d.differential.contact.contacts.iteritems():
 
d.differential.costs["forces_"+patchname].contact = contactData
 
#---------Ugliness over------------------------
 
 
ddp = SolverDDP(problem)
 
ddp.callback = [CallbackDDPVerbose()]#, CallbackSolverDisplay(robot,4)]
 
ddp.th_stop = 1e-9
 
ddp.solve(maxiter=1000,regInit=0.1,init_xs=conf.X_init,init_us=conf.U_init)
Loading