acc = dx[rmodel.nv:] / dt
u = pinocchio.rnea(rmodel, rdata, a2m(xp[:rmodel.nq]),
a2m(xp[rmodel.nq:]), a2m(acc))
m.differential.calc(d.differential, init.X[-1])
contactJ = d.differential.contact.J
f = np.dot(np.linalg.pinv(contactJ.T[:6, :]), u[:6])
u -= (np.dot(contactJ.transpose(), f))
init.U.append(np.array(u[6:]).squeeze().copy())
init.X.append(conf.X_init[-1])
# ---------------Display Initial Trajectory--------------
if conf.DISPLAY:
robot.initDisplay(loadModel=True)
if conf.DISPLAY:
for x in init.X:
robot.display(a2m(x[:robot.nq]))
# sleep(0.005)
# ----------------------
ddp = SolverDDP(problem)
ddp.callback = [CallbackDDPVerbose()] # CallbackSolverTimer()]
if conf.RUNTIME_DISPLAY:
ddp.callback.append(CallbackSolverDisplay(robot, 4))
ddp.th_stop = 1e-9
ddp.solve(maxiter=1000, regInit=0.1, init_xs=init.X, init_us=init.U)
# ---------------Display Final Trajectory--------------
if conf.DISPLAY:
for x in init.X:
robot.display(a2m(x[:robot.nq]))
# sleep(0.005)
# ----------------------
x0 = m2a(np.concatenate([q0, v0]))
# Setting up the 3d walking problem
lfFoot = 'lf_foot'
rfFoot = 'rf_foot'
lhFoot = 'lh_foot'
rhFoot = 'rh_foot'
walk = SimpleQuadrupedProblem(rmodel, lfFoot, rfFoot, lhFoot, rhFoot)
# Setting up the walking variables
comGoTo = 0.1 # meters
timeStep = 5e-2 # seconds
supportKnots = 2
# Creating the CoM problem and solving it
ddp = SolverDDP(walk.createProblem(x0, comGoTo, timeStep, supportKnots))
# ddp.callback = [ CallbackDDPVerbose() ]
ddp.th_stop = 1e-9
ddp.solve(maxiter=1000,
regInit=.1,
init_xs=[rmodel.defaultState] * len(ddp.models()))
# Checking the CoM displacement
x0 = ddp.xs[0]
xT = ddp.xs[-1]
q0 = a2m(x0[:rmodel.nq])
qT = a2m(xT[:rmodel.nq])
data0 = rmodel.createData()
dataT = rmodel.createData()
comT = pinocchio.centerOfMass(rmodel, dataT, qT)
com0 = pinocchio.centerOfMass(rmodel, data0, q0)