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)]
