def calcDiff(self, data, x, u=None, recalc=True): if u is None: u = self.unone if recalc: xout, cost = self.calc(data, x, u) nq, nv = self.nq, self.nv q = a2m(x[:nq]) v = a2m(x[-nv:]) tauq = a2m(data.actuation.a) pinocchio.computeABADerivatives(self.pinocchio, data.pinocchio, q, v, tauq) da_dq = data.pinocchio.ddq_dq da_dv = data.pinocchio.ddq_dv da_dact = data.pinocchio.Minv dact_dx = data.actuation.Ax dact_du = data.actuation.Au data.Fx[:, :nv] = da_dq data.Fx[:, nv:] = da_dv data.Fx += np.dot(da_dact, dact_dx) data.Fu[:, :] = np.dot(da_dact, dact_du) pinocchio.computeJointJacobians(self.pinocchio, data.pinocchio, q) pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio) self.costs.calcDiff(data.costs, x, u, recalc=False) return data.xout, data.cost
def calc(self, data, x, u=None): if u is None: u = self.unone nq, nv = self.nq, self.nv q = a2m(x[:nq]) v = a2m(x[-nv:]) tauq = a2m(self.actuation.calc(data.actuation, x, u)) data.xout[:] = pinocchio.aba(self.pinocchio, data.pinocchio, q, v, tauq).flat pinocchio.forwardKinematics(self.pinocchio, data.pinocchio, q, v) pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio) data.cost = self.costs.calc(data.costs, x, u) return data.xout, data.cost
def __init__(self): robot = self.robot = loadTalosArm(freeFloating=True) rmodel = self.rmodel = robot.model qmin = rmodel.lowerPositionLimit qmin[:7] = -1 rmodel.lowerPositionLimit = qmin qmax = rmodel.upperPositionLimit qmax[:7] = 1 rmodel.upperPositionLimit = qmax State = self.State = StatePinocchio(rmodel) actModel = self.actModel = ActuationModelFreeFloating(rmodel) contactModel = self.contactModel = ContactModelMultiple(rmodel) contact6 = ContactModel6D(rmodel, rmodel.getFrameId(contactName), ref=pinocchio.SE3.Identity(), gains=[0., 0.]) contactModel.addContact(name='contact', contact=contact6) costModel = self.costModel = CostModelSum(rmodel, nu=actModel.nu) self.cost1 = CostModelFrameTranslation( rmodel, nu=actModel.nu, frame=rmodel.getFrameId(opPointName), ref=np.array([.5, .4, .3])) stateWeights = np.array([0] * 6 + [0.01] * (rmodel.nv - 6) + [10] * rmodel.nv) self.cost2 = CostModelState(rmodel, State, ref=State.zero(), nu=actModel.nu, activation=ActivationModelWeightedQuad( stateWeights**2)) self.cost3 = CostModelControl(rmodel, nu=actModel.nu) costModel.addCost(name="pos", weight=10, cost=self.cost1) costModel.addCost(name="regx", weight=0.1, cost=self.cost2) costModel.addCost(name="regu", weight=0.01, cost=self.cost3) self.dmodel = DifferentialActionModelFloatingInContact( rmodel, actModel, contactModel, costModel) self.model = IntegratedActionModelEuler(self.dmodel) self.data = self.model.createData() self.cd1 = self.data.differential.costs.costs['pos'] self.cd2 = self.data.differential.costs.costs['regx'] self.cd3 = self.data.differential.costs.costs['regu'] self.ddata = self.data.differential self.rdata = self.data.differential.pinocchio self.x = self.State.rand() self.q = a2m(self.x[:rmodel.nq]) self.v = a2m(self.x[rmodel.nq:]) self.u = np.random.rand(self.model.nu)
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 createEESplines(self, rmodel, rdata, xs, t_sampling=0.005): N = len(xs) abscissa = a2m(np.linspace(0., t_sampling * (N - 1), N)) self.ee_splines = EESplines() for patch in self.ee_map.keys(): p = np.zeros((3, N)) m = np.zeros((3, N)) for i in range(N): q = a2m(xs[i][:rmodel.nq]) v = a2m(xs[i][-rmodel.nv:]) pinocchio.forwardKinematics(rmodel, rdata, q, v) p[:, i] = m2a( pinocchio.updateFramePlacement(rmodel, rdata, rmodel.getFrameId(self.ee_map[patch])).translation) m[:, i] = m2a(pinocchio.getFrameVelocity(rmodel, rdata, rmodel.getFrameId(self.ee_map[patch])).linear) self.ee_splines.update([[patch, CubicHermiteSpline(abscissa, p, m)]]) return
def __init__(self): robot = self.robot = loadTalosArm() rmodel = self.rmodel = robot.model rmodel.armature = np.matrix([0] * rmodel.nv).T for j in rmodel.joints[1:]: if j.shortname() != 'JointModelFreeFlyer': rmodel.armature[j.idx_v:j.idx_v + j.nv] = 1 State = self.State = StatePinocchio(rmodel) self.cost1 = CostModelFrameTranslation( rmodel, nu=rmodel.nv, frame=rmodel.getFrameId(opPointName), ref=np.array([.5, .4, .3])) self.cost2 = CostModelState(rmodel, State, ref=State.zero(), nu=rmodel.nv) self.cost3 = CostModelControl(rmodel, nu=rmodel.nv) costModel = CostModelSum(rmodel) costModel.addCost(name="pos", weight=10, cost=self.cost1) costModel.addCost(name="regx", weight=0.1, cost=self.cost2) costModel.addCost(name="regu", weight=0.01, cost=self.cost3) self.dmodel = DifferentialActionModelFullyActuated(rmodel, costModel) self.model = IntegratedActionModelEuler(self.dmodel) self.data = self.model.createData() self.cd1 = self.data.differential.costs.costs['pos'] self.cd2 = self.data.differential.costs.costs['regx'] self.cd3 = self.data.differential.costs.costs['regu'] self.ddata = self.data.differential self.rdata = self.data.differential.pinocchio self.x = self.State.rand() self.q = a2m(self.x[:rmodel.nq]) self.v = a2m(self.x[rmodel.nq:]) self.u = np.random.rand(self.model.nu)
def runningModel(contactIds, effectors, com=None, integrationStep=1e-2): ''' Creating the action model for floating-base systems. A walker system is by default a floating-base system. contactIds is a list of frame Ids of points that should be in contact. effectors is a dict of key frame ids and SE3 values of effector references. ''' actModel = ActuationModelFreeFloating(rmodel) State = StatePinocchio(rmodel) # Creating a 6D multi-contact model, and then including the supporting foot contactModel = ContactModelMultiple(rmodel) for cid in contactIds: contactModel.addContact('contact%d' % cid, ContactModel6D(rmodel, cid, ref=None)) # Creating the cost model for a contact phase costModel = CostModelSum(rmodel, actModel.nu) wx = np.array([0] * 6 + [.1] * (rmodel.nv - 6) + [10] * rmodel.nv) costModel.addCost('xreg', weight=1e-1, cost=CostModelState( rmodel, State, ref=rmodel.defaultState, nu=actModel.nu, activation=ActivationModelWeightedQuad(wx))) costModel.addCost('ureg', weight=1e-4, cost=CostModelControl(rmodel, nu=actModel.nu)) for fid, ref in effectors.items(): if not isinstance(ref, SE3): ref = SE3(eye(3), a2m(ref)) costModel.addCost("track%d" % fid, weight=100., cost=CostModelFramePlacement(rmodel, fid, ref, actModel.nu)) if com is not None: costModel.addCost("com", weight=100., cost=CostModelCoM(rmodel, ref=com, nu=actModel.nu)) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme dmodel = DifferentialActionModelFloatingInContact(rmodel, actModel, contactModel, costModel) model = IntegratedActionModelEuler(dmodel) model.timeStep = integrationStep return model
contactName = 'arm_left_7_joint' contactName = 'gripper_left_fingertip_1_link' # opPointName,contactName = contactName,opPointName CONTACTFRAME = rmodel.getFrameId(contactName) OPPOINTFRAME = rmodel.getFrameId(opPointName) impulseModel = ImpulseModel6D(rmodel, rmodel.getFrameId(contactName)) costModel = CostModelImpactWholeBody(rmodel) model = ActionModelImpact(rmodel, impulseModel, costModel) model.impulseWeight = 1. data = model.createData() x = model.State.rand() # x[7:13] = 0 q = a2m(x[:model.nq]) v = a2m(x[model.nq:]) model.calc(data, x) model.calcDiff(data, x) mnum = ActionModelNumDiff(model, withGaussApprox=True) dnum = mnum.createData() nx, ndx, nq, nv, nu = model.nx, model.ndx, model.nq, model.nv, model.nu mnum.calcDiff(dnum, x, None) assertNumDiff( dnum.Fx[:nv, :nv], data.Fx[:nv, :nv], NUMDIFF_MODIFIER * mnum.disturbance ) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff(
left0 = m2a(rdata.oMf[leftId].translation) com0 = m2a(pinocchio.centerOfMass(rmodel, rdata, q0)) KT = 3 KS = 8 models = [ runningModel([rightId, leftId], {}, integrationStep=stanceDurantion / KT) for i in range(KT) ] models += [ runningModel([rightId], {}, integrationStep=swingDuration / KS) for i in range(KS) ] models += [ impactModel([leftId, rightId], {leftId: SE3(eye(3), a2m(left0 + [stepLength, 0, 0]))}) ] models += [ runningModel([rightId, leftId], {}, integrationStep=stanceDurantion / KT) for i in range(KT) ] models += [ runningModel([leftId], {}, integrationStep=swingDuration / KS) for i in range(KS) ] models += [ impactModel([leftId, rightId], {rightId: SE3(eye(3), a2m(right0 + [stepLength, 0, 0]))}) ] models += [ runningModel([rightId, leftId], {}, integrationStep=stanceDurantion / KT)
model.calc(data, x, m2a(u)) return a2m(data.xnext) # .copy() def get_au(u): a, _ = model.differential.calc(data.differential[0], x, m2a(u)) return a2m(a) def get_y(q, v): x_ = np.vstack([q, v]) model.calc(data, m2a(x_), u) return [a2m(y) for y in data.y] dxn_du = df_dx(lambda _u: get_xn(_u), a2m(u)) def dk_du(i): return df_dx(lambda _u: get_ku(_u)[i], a2m(u)) def dk_dq(i): return df_dx(lambda _q: get_k(_q, a2m(x[nq:]))[i], a2m(x[:nq])) def dk_dv(i): return df_dx(lambda _v: get_k(a2m(x[:nq]), _v)[i], a2m(x[nq:])) def dy_dq(i):
def returna_at0(q, v): x = np.vstack([q, v]).flat pinocchio.computeAllTerms(rmodel, rdata2, q, v) pinocchio.updateFramePlacements(rmodel, rdata2) contactModel.calc(contactData2, x) return a2m(contactData2.a0) # .copy()
np.array([.5, .4, .3])) costData = costModel.createData(rdata) pinocchio.forwardKinematics(rmodel, rdata, q, v) pinocchio.computeJointJacobians(rmodel, rdata, q) pinocchio.updateFramePlacements(rmodel, rdata) costModel.calcDiff(costData, x, u) costModelND = CostModelNumDiff( costModel, StatePinocchio(rmodel), withGaussApprox=True, reevals=[ lambda m, d, x, u: pinocchio.forwardKinematics(m, d, a2m(x[:rmodel.nq]), a2m(x[rmodel.nq:])), lambda m, d, x, u: pinocchio.computeJointJacobians(m, d, a2m(x[:rmodel.nq])), lambda m, d, x, u: pinocchio.updateFramePlacements(m, d) ]) costDataND = costModelND.createData(rdata) costModelND.calcDiff(costDataND, x, u) assertNumDiff(costData.Lx, costDataND.Lx, NUMDIFF_MODIFIER * costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff(costData.Lu, costDataND.Lu, NUMDIFF_MODIFIER * costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff(costData.Lxx, costDataND.Lxx, NUMDIFF_MODIFIER * costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff(costData.Lxu, costDataND.Lxu, NUMDIFF_MODIFIER * costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
rmodel.getFrameId('gripper_left_fingertip_2_link'), ref=np.random.rand(3), gains=[4., 4.]) rmodel.frames[contactModel3.frame].placement = pinocchio.SE3.Random() contactModel = ContactModelMultiple(rmodel) contactModel.addContact(name='fingertip', contact=contactModel3) model = DifferentialActionModelFloatingInContact(rmodel, actModel, contactModel, CostModelSum(rmodel)) data = model.createData() model.calc(data, x, u) assert (len(list(filter(lambda x: x > 0, eig(data.K)[0]))) == model.nv) assert (len(list(filter(lambda x: x < 0, eig(data.K)[0]))) == model.ncontact) _taucheck = pinocchio.rnea(rmodel, rdata, q, v, a2m(data.a), data.contact.forces) _taucheck.flat[:] += rmodel.armature.flat * data.a assert (absmax(_taucheck[:6]) < 1e-6) assert (absmax(m2a(_taucheck[6:]) - u) < 1e-6) model.calcDiff(data, x, u) mnum = DifferentialActionModelNumDiff(model, withGaussApprox=False) dnum = mnum.createData() mnum.calcDiff(dnum, x, u) assertNumDiff( data.Fx, dnum.Fx, NUMDIFF_MODIFIER * mnum.disturbance ) # threshold was 2.7e-2, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff( data.Fu, dnum.Fu, NUMDIFF_MODIFIER * mnum.disturbance
def calcForces(q_, v_, u_): model.calc(data, np.concatenate([m2a(q_), m2a(v_)]), m2a(u_)) return a2m(data.f)
actModel = ActuationModelFreeFloating(rmodel) contactModel3 = ContactModel3D(rmodel, rmodel.getFrameId('gripper_left_fingertip_2_link'), ref=np.random.rand(3), gains=[4., 4.]) rmodel.frames[contactModel3.frame].placement = pinocchio.SE3.Random() contactModel = ContactModelMultiple(rmodel) contactModel.addContact(name='fingertip', contact=contactModel3) model = DifferentialActionModelFloatingInContact(rmodel, actModel, contactModel, CostModelSum(rmodel)) data = model.createData() model.calc(data, x, u) assert (len(list(filter(lambda x: x > 0, eig(data.K)[0]))) == model.nv) assert (len(list(filter(lambda x: x < 0, eig(data.K)[0]))) == model.ncontact) _taucheck = pinocchio.rnea(rmodel, rdata, q, v, a2m(data.a), data.contact.forces) _taucheck.flat[:] += rmodel.armature.flat * data.a assert (absmax(_taucheck[:6]) < 1e-6) assert (absmax(m2a(_taucheck[6:]) - u) < 1e-6) model.calcDiff(data, x, u) mnum = DifferentialActionModelNumDiff(model, withGaussApprox=False) dnum = mnum.createData() mnum.calcDiff(dnum, x, u) assertNumDiff(data.Fx, dnum.Fx, NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 2.7e-2, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff(data.Fu, dnum.Fu, NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 7e-3, is now 2.11e-4 (see assertNumDiff.__doc__) # ------------------------------------------------
def dy_dv(i): return df_dx(lambda _v: get_y(a2m(x[:nq]), _v)[i], a2m(x[nq:]))
# 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) assert (norm(comT - com0 - np.matrix([[comGoTo], [0.], [0.]])) < 1e-3)
def createCentroidalPhi(self, rmodel, rdata): # centroidal planar (muscod) returns the forces in the sequence RF,LF,RH,LH. # TODO: make more generic 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)]]) patch_names = self.ee_map.keys() mass = pinocchio.crba(rmodel, rdata, pinocchio.neutral(rmodel))[0, 0] t_traj = None # -----Get Length of Timeline------------------------ t_traj = [] for spl in self.cs.ms_interval_data[:-1]: t_traj += list(spl.time_trajectory) t_traj = np.array(t_traj) N = len(t_traj) # ------Get values of state and control-------------- class PhiC: f = OrderedDict() df = OrderedDict() for patch in patch_names: f.update([[patch, np.zeros((N, 6))]]) df.update([[patch, np.zeros((N, 6))]]) com_vcom = np.zeros((N, 6)) vcom_acom = np.zeros((N, 6)) hg = np.zeros((N, 6)) dhg = np.zeros((N, 6)) phi_c_ = PhiC() n = 0 for i, spl in enumerate(self.cs.ms_interval_data[:-1]): x = m2a(spl.state_trajectory) dx = m2a(spl.dot_state_trajectory) u = m2a(spl.control_trajectory) nt = len(x) tt = t_traj[n:n + nt] 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) def f_poly(t, r): return np.array([poly_u[i](t) for i in r]) def f_dpoly(t, r): return 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 duplicates = findDuplicates(t_traj) class PhiC2: f = OrderedDict() df = OrderedDict() for patch in patch_names: f.update([[patch, removeDuplicates(phi_c_.f[patch], duplicates)]]) df.update([[patch, removeDuplicates(phi_c_.df[patch], duplicates)]]) com_vcom = removeDuplicates(phi_c_.com_vcom, duplicates) vcom_acom = removeDuplicates(phi_c_.vcom_acom, duplicates) hg = removeDuplicates(phi_c_.hg, duplicates) dhg = removeDuplicates(phi_c_.dhg, duplicates) phi_c_2 = PhiC2() t_traj = removeDuplicates(t_traj, duplicates) self.phi_c.com_vcom = CubicHermiteSpline(a2m(t_traj), a2m(phi_c_2.com_vcom), a2m(phi_c_2.vcom_acom)) self.phi_c.hg = CubicHermiteSpline(a2m(t_traj), a2m(phi_c_2.hg), a2m(phi_c_2.dhg)) for patch in patch_names: self.phi_c.forces[patch] = CubicHermiteSpline(a2m(t_traj), a2m(phi_c_2.f[patch]), a2m(phi_c_2.df[patch])) return
def get_au(u): a, _ = model.differential.calc(data.differential[0], x, m2a(u)) return a2m(a)
def get_xn(u): model.calc(data, x, m2a(u)) return a2m(data.xnext) # .copy()
def get_ku(u): model.calc(data, x, m2a(u)) return [a2m(ki) for ki in data.ki]
def get_k(q, v): x_ = np.vstack([q, v]) model.calc(data, m2a(x_), u) return [a2m(ki) for ki in data.ki]
def get_attr_analytical(x, u, attr): _u = m2a(u) _x = m2a(x) model.calcDiff(data, _x, _u) return a2m(getattr(data, attr)) # .copy()
def get_y(q, v): x_ = np.vstack([q, v]) model.calc(data, m2a(x_), u) return [a2m(y) for y in data.y]
np.array([.5, .4, .3])) costData = costModel.createData(rdata) pinocchio.forwardKinematics(rmodel, rdata, q, v) pinocchio.computeJointJacobians(rmodel, rdata, q) pinocchio.updateFramePlacements(rmodel, rdata) costModel.calcDiff(costData, x, u) costModelND = CostModelNumDiff( costModel, StatePinocchio(rmodel), withGaussApprox=True, reevals=[ lambda m, d, x, u: pinocchio.forwardKinematics(m, d, a2m( x[:rmodel.nq]), a2m(x[rmodel.nq:])), lambda m, d, x, u: pinocchio.computeJointJacobians( m, d, a2m(x[:rmodel.nq])), lambda m, d, x, u: pinocchio.updateFramePlacements(m, d) ]) costDataND = costModelND.createData(rdata) costModelND.calcDiff(costDataND, x, u) assertNumDiff( costData.Lx, costDataND.Lx, NUMDIFF_MODIFIER * costModelND.disturbance ) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff( costData.Lu, costDataND.Lu, NUMDIFF_MODIFIER * costModelND.disturbance ) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff(
def dk_du(i): return df_dx(lambda _u: get_ku(_u)[i], a2m(u))
def dy_dq(i): return df_dx(lambda _q: get_y(_q, a2m(x[nq:]))[i], a2m(x[:nq]))
# ---------Ugliness over------------------------ # -----------Create inital trajectory--------------- class Init: X = [] U = [] init = Init() x_tsid = np.matrix(conf.X_init).T conf.ddq_init.append(np.zeros(rmodel.nv)) dx_tsid = np.vstack([x_tsid[rmodel.nv:, :], np.matrix(conf.ddq_init).T]) t_tsid = np.linspace(0., cs.contact_phases[-1].time_trajectory[-1], len(conf.ddq_init) + 1) x_spl = CubicHermiteSpline(a2m(t_tsid), x_tsid, dx_tsid) state = StatePinocchio(rmodel) dt = conf.DT def x_eval(t): return m2a(x_spl.eval(t)[0]) for i, (m, d) in enumerate(zip(problem.runningModels, problem.runningDatas)): # Impact models have zero timestep, thus they are copying the same state as the beginning of the next action model # State and control are defined at the beginning of the time step. if isinstance(m, ActionModelImpact): init.X.append(x_eval((i + 1) * dt)) init.U.append(np.zeros(m.nu))
ddp.x_reg = 0 ddp.u_reg = 0 ddp.setCandidate() ddp.computeDirection() ddp.tryStep(1) for t in range(T): assert (np.allclose(ddp.us_try[t], kkt.us[t])) assert (np.allclose(ddp.xs_try[t + 1], kkt.xs[t + 1])) # assert(np.isclose(-dot(ddp.Vx[0],x0)+ddp.expectedImprovement()[0],kkt.expectedImprovement()[0])) datas = ddp.datas() Fu = [np.matrix(d.Fu) for d in datas] Fx = [np.matrix(d.Fx) for d in datas] Lu = [a2m(d.Lu) for d in datas] Lx = [a2m(d.Lx) for d in datas] Lxx = [np.matrix(d.Lxx) for d in datas] Quu = [np.matrix(Q) for Q in ddp.Quu] Vx = [a2m(v) for v in ddp.Vx] Qx = [a2m(q) for q in ddp.Qx] Qu = [a2m(q) for q in ddp.Qu] k = [a2m(k) for k in ddp.k] K = [np.matrix(K) for K in ddp.K] Vxx = [np.matrix(Vxx) for Vxx in ddp.Vxx] f = [a2m(f) for f in ddp.gaps] x = [a2m(x) for x in kkt.xs] u = [a2m(u) for u in kkt.us] FuK = [-_Fu * _K for _Fu, _K in zip(Fu[:-1], K)]