def setUp(self): # Creating the model data self.model = self.MODEL self.data = self.model.createData() # Creating shooting problem self.problem = ShootingProblem(self.model.State.zero(), [self.model, self.model], self.model)
def createProblem(self, x0, comGoTo, timeStep, numKnots): # Compute the current foot positions q0 = a2m(x0[:self.rmodel.nq]) pinocchio.forwardKinematics(self.rmodel, self.rdata, q0) pinocchio.updateFramePlacements(self.rmodel, self.rdata) com0 = m2a(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)) # Defining the action models along the time instances comModels = [] # Creating the action model for the CoM task comForwardModels = [ self.createModels( timeStep, [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId], ) for k in range(numKnots) ] comForwardTermModel = self.createModels( timeStep, [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId], com0 + [comGoTo, 0., 0.]) comForwardTermModel.differential.costs['comTrack'].weight = 1e6 # Adding the CoM tasks comModels += comForwardModels + [comForwardTermModel] # Defining the shooting problem problem = ShootingProblem(x0, comModels, comModels[-1]) return problem
def setUp(self): # Creating the model self.model = self.MODEL self.data = self.model.createData() # Defining the shooting problem self.problem = ShootingProblem(self.model.State.zero(), [self.model, self.model], self.model) # Creating the KKT solver self.kkt = SolverKKT(self.problem) # Setting up a warm-point xs = [m.State.zero() for m in self.problem.runningModels + [self.problem.terminalModel]] us = [np.zeros(m.nu) for m in self.problem.runningModels] self.kkt.setCandidate(xs, us)
rdata = robot.data rmodel.defaultState = np.concatenate([m2a(robot.q0), np.zeros(rmodel.nv)]) # ----------------Load Contact Phases and PostProcess----------------------- cs = ContactSequenceHumanoid(0) cs.loadFromXML(conf.MUSCOD_CS_OUTPUT_FILENAME, conf.CONTACT_SEQUENCE_XML_TAG) csw = ContactSequenceWrapper(cs, conf.contact_patches) csw.createCentroidalPhi(rmodel, rdata) csw.createEESplines(rmodel, rdata, conf.X_init, 0.005) # ----------------Define Problem---------------------------- models = createMultiphaseShootingProblem(rmodel, rdata, csw, 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: if hasattr( d, "differential" ): # Because we also have the impact models without differntial. for (patchname, contactData) in d.differential.contact.contacts.iteritems(): if "forces_" + patchname in d.differential.costs.costs: d.differential.costs["forces_" + patchname].contact = contactData # ---------Ugliness over------------------------
models += [ runningModel([rightId], {leftId: left0 + [0, 0, 0.1]}, com=com0 + [-0.1, 0, 0], integrationStep=5e-2) ] pass1 = models[10] pass2 = models[21] pass1.differential.costs['com'].weight = 100000 pass2.differential.costs['com'].weight = 100000 pass2.differential.costs['track16'].weight = 100000 # --- DDP problem = ShootingProblem(initialState=x0, runningModels=models[:-1], terminalModel=models[-1]) ddp = SolverDDP(problem) # ddp.callback = [ CallbackDDPLogger(), CallbackDDPVerbose() ] ddp.th_stop = 1e-6 ddp.setCandidate() m = models[0] d = m.createData() m = m.differential d = d.differential # m=m.differential.contact['contact30'] # d=d.differential.contact['contact30'] m.calcDiff(d, ddp.xs[0], ddp.us[0])
xref = fix.State.rand() xref[fix.rmodel.nq:] = 0 fix.calc(xref) f = ff f.u[:] = ( 0 * pinocchio.rnea(fix.rmodel, fix.rdata, fix.q, fix.v * 0, fix.v * 0)).flat f.v[:] = 0 f.x[f.rmodel.nq:] = f.v.flat # f.u[:] = np.zeros(f.model.nu) f.model.differential.costs['pos'].weight = 1 f.model.differential.costs['regx'].weight = 0.01 f.model.differential.costs['regu'].weight = 0.0001 fterm = f.__class__() fterm.model.differential.costs['pos'].weight = 1000 fterm.model.differential.costs['regx'].weight = 1 fterm.model.differential.costs['regu'].weight = 0.01 problem = ShootingProblem(f.x, [f.model] * T, fterm.model) u0s = [f.u] * T x0s = problem.rollout(u0s) # disp = lambda xs: disptraj(f.robot, xs) ddp = SolverDDP(problem) # ddp.callback = [ CallbackDDPLogger(), CallbackDDPVerbose() ] ddp.th_stop = 1e-18 ddp.solve(maxiter=1000)
assertNumDiff(data.Fu, dnum.Fu, NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff(data.Lx, dnum.Lx, NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff(data.Lu, dnum.Lu, NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff(dnum.Lxx, data.Lxx, NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff(dnum.Lxu, data.Lxu, NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff(dnum.Luu, data.Luu, NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__) # ------------------------------------------------------------------------------- # ------------------------------------------------------------------------------- # ------------------------------------------------------------------------------- # --- DDP FOR THE ARM --- dmodel = DifferentialActionModelPositioning(rmodel) model = IntegratedActionModelEuler(dmodel) problem = ShootingProblem(model.State.zero() + 1, [model], model) kkt = SolverKKT(problem) kkt.th_stop = 1e-18 xkkt, ukkt, dkkt = kkt.solve() ddp = SolverDDP(problem) ddp.th_stop = 1e-18 xddp, uddp, dddp = ddp.solve() assert (norm(uddp[0] - ukkt[0]) < 1e-6)
model.timeStep = 1e-1 dmodel.costs['pos'].weight = 1 dmodel.costs['regx'].weight = 0 dmodel.costs['regu'].weight = 0 # Choose a cost that is reachable. x0 = model.State.rand() xref = model.State.rand() xref[:7] = x0[:7] xref[3:7] = [0, 0, 0, 1] # TODO: remove this after adding assertion to include any case pinocchio.forwardKinematics(rmodel, rdata, a2m(xref[:rmodel.nq])) pinocchio.updateFramePlacements(rmodel, rdata) c1.ref[:] = m2a(rdata.oMf[c1.frame].translation.copy()) problem = ShootingProblem(x0, [model], model) ddp = SolverDDP(problem) ddp.callback = [CallbackDDPLogger()] ddp.th_stop = 1e-18 xddp, uddp, doneddp = ddp.solve(maxiter=400) assert (doneddp) assert (norm(ddp.datas()[-1].differential.costs['pos'].residuals) < 1e-3) assert (norm( m2a(ddp.datas()[-1].differential.costs['pos'].pinocchio.oMf[ c1.frame].translation) - c1.ref) < 1e-3) u0 = np.zeros(model.nu) x1 = model.calc(data, problem.initialState, u0)[0] x0s = [problem.initialState.copy(), x1]
# --------------------------------------------------- # --------------------------------------------------- np.set_printoptions(linewidth=400, suppress=True) np.random.seed(220) nq = 4 nu = 2 nv = nq dmodel = DifferentialActionModelLQR(nq, nu) model = IntegratedActionModelEuler(dmodel, withCostResiduals=False) nx = model.nx nu = model.nu T = 1 problem = ShootingProblem(model.State.zero() + 2, [model] * T, model) xs = [m.State.zero() for m in problem.runningModels + [problem.terminalModel]] us = [np.zeros(m.nu) for m in problem.runningModels] x0ref = problem.initialState xs[0] = np.random.rand(nx) xs[1] = np.random.rand(nx) us[0] = np.random.rand(nu) # xs[1] = model.calc(data,xs[0],us[0])[0].copy() ddpbox = SolverBoxDDP(problem) ddpbox.qpsolver = quadprogWrapper ddp = SolverDDP(problem) ddp.setCandidate(xs, us) ddp.computeDirection()
# --- TEST KKT --- # --------------------------------------------------- # --------------------------------------------------- # --------------------------------------------------- NX = 1 NU = 1 model = ActionModelUnicycle() data = model.createData() LQR = isinstance(model, ActionModelLQR) x = model.State.rand() u = np.zeros([model.nu]) problem = ShootingProblem(model.State.zero(), [model, model], model) kkt = SolverKKT(problem) xs = [m.State.zero() for m in problem.runningModels + [problem.terminalModel]] us = [np.zeros(m.nu) for m in problem.runningModels] # Test dimensions of KKT calc. kkt.setCandidate(xs, us) # Test that the solution respect the dynamics (or linear approx of it). dxs, dus, ls = kkt.computeDirection() x0, x1, x2 = dxs u0, u1 = dus l0, l1, l2 = ls # If LQR. test that a random solution respect the dynamics xs = [m.State.rand() for m in problem.runningModels + [problem.terminalModel]]