def cartpole_model():
cartpoleDAM = DifferentialActionModelCartpole()
cartpoleData = cartpoleDAM.createData()
x = cartpoleDAM.state.rand()
u = np.zeros(1)
cartpoleDAM.calc(cartpoleData, x, u)
cartpoleND = crocoddyl.DifferentialActionModelNumDiff(cartpoleDAM, True)
timeStep = 5e-4
cartpoleIAM = crocoddyl.IntegratedActionModelEuler(cartpoleND, timeStep)
terminalCartpole = DifferentialActionModelCartpole()
terminalCartpoleDAM = crocoddyl.DifferentialActionModelNumDiff(
terminalCartpole, True)
terminalCartpoleIAM = crocoddyl.IntegratedActionModelEuler(
terminalCartpoleDAM)
terminalCartpole.costWeights[0] = 1
terminalCartpole.costWeights[1] = 100
terminalCartpole.costWeights[2] = 1.
terminalCartpole.costWeights[3] = 0.1
terminalCartpole.costWeights[4] = 0.01
terminalCartpole.costWeights[5] = 0.0001
return cartpoleIAM
def cartpole_data(ntraj: int = 1):
cartpoleDAM = DifferentialActionModelCartpole()
cartpoleData = cartpoleDAM.createData()
x = cartpoleDAM.state.rand()
u = np.zeros(1)
cartpoleDAM.calc(cartpoleData, x, u)
cartpoleND = crocoddyl.DifferentialActionModelNumDiff(cartpoleDAM, True)
timeStep = 5e-2
cartpoleIAM = crocoddyl.IntegratedActionModelEuler(cartpoleND, timeStep)
T = 50
data = []
for _ in range(ntraj):
x0 = [
random.uniform(0, 1),
random.uniform(-3.14, 3.14),
random.uniform(0., 1), 0.
]
problem = crocoddyl.ShootingProblem(
np.array(x0).T, [cartpoleIAM] * T, cartpoleIAM)
ddp = crocoddyl.SolverDDP(problem)
ddp.solve([], [], 1000)
if ddp.iter < 1000:
ddp_xs = np.array(ddp.xs)
# Append value function to initial state
x0.append(ddp.cost)
# get us, xs for T timesteps
us = np.array(ddp.us)
xs = ddp_xs[1:, :]
cost = []
for d in ddp.datas():
cost.append(d.cost)
# Remove the first value
cost.pop(0)
cost = np.array(cost).reshape(-1, 1)
info = np.hstack((xs, us, cost))
info = info.ravel()
state = np.concatenate((x0, info))
data.append(state)
data = np.array(data)
print(
"Shape x_train -> 0 : 4, value function = index 4, then (x, y, z, theta, control, cost)..repeated"
)
print("T -> 50, so T x 6 = 300, + 5 ")
#np.savetxt("foo.csv", data, delimiter=",")
return data
def cartpole_data(ntraj: int = 1000):
cartpoleDAM = DifferentialActionModelCartpole()
cartpoleData = cartpoleDAM.createData()
x = cartpoleDAM.state.rand()
u = np.zeros(1)
cartpoleDAM.calc(cartpoleData, x, u)
cartpoleND = crocoddyl.DifferentialActionModelNumDiff(cartpoleDAM, True)
timeStep = 5e-2
cartpoleIAM = crocoddyl.IntegratedActionModelEuler(cartpoleND, timeStep)
T = 50
data = []
for _ in range(ntraj):
cost = []
x0 = np.array([
random.uniform(0, 1),
random.uniform(-3.14, 3.14),
random.uniform(0., 1), 0.
])
problem = crocoddyl.ShootingProblem(x0.T, [cartpoleIAM] * T,
cartpoleIAM)
ddp = crocoddyl.SolverDDP(problem)
ddp.solve([], [], 1000)
if ddp.iter < 1000:
xs = np.array(ddp.xs)
us = np.array(ddp.us)
for d in ddp.datas():
cost.append(d.cost)
data.append(xs.ravel())
data = np.array(data)
return data
pinocchio.utils.rand(2))
CONTACTS.addContact("r_sole_contact", CONTACT_6D_1)
CONTACTS.addContact("l_sole_contact", CONTACT_6D_2)
COSTS = crocoddyl.CostModelSum(ROBOT_STATE, ACTUATION.nu, False)
COSTS.addCost(
"force",
crocoddyl.CostModelContactForce(
ROBOT_STATE,
crocoddyl.FrameForce(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.Force.Random()), ACTUATION.nu), 1.)
MODEL = crocoddyl.DifferentialActionModelContactFwdDynamics(
ROBOT_STATE, ACTUATION, CONTACTS, COSTS, 0., True)
DATA = MODEL.createData()
# Created DAM numdiff
MODEL_ND = crocoddyl.DifferentialActionModelNumDiff(MODEL)
dnum = MODEL_ND.createData()
x = ROBOT_STATE.rand()
u = pinocchio.utils.rand(ACTUATION.nu)
MODEL.calcDiff(DATA, x, u)
MODEL_ND.calcDiff(dnum, x, u)
assertNumDiff(
DATA.Fx, dnum.Fx, NUMDIFF_MODIFIER * MODEL_ND.disturbance
) # threshold was 2.7e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(DATA.Fu, dnum.Fu, NUMDIFF_MODIFIER *
MODEL_ND.disturbance) # threshold was 7e-3, is now 2.11e-4 (se
assertNumDiff(
DATA.Lx, dnum.Lx, NUMDIFF_MODIFIER * MODEL_ND.disturbance
) # threshold was 2.7e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(DATA.Lu, dnum.Lu, NUMDIFF_MODIFIER *
control_array = warm[:, 4]
for state in state_array:
state = np.matrix(state).T
init_xs.append(state)
for control in control_array:
control = np.matrix(control).T
init_us.append(control)
cartpoleDAM = DifferentialActionModelCartpole()
cartpoleData = cartpoleDAM.createData()
x = cartpoleDAM.state.rand()
u = np.zeros(1)
cartpoleDAM.calc(cartpoleData, x, u)
cartpoleND = crocoddyl.DifferentialActionModelNumDiff(
cartpoleDAM, True)
timeStep = 5e-2
cartpoleIAM = crocoddyl.IntegratedActionModelEuler(
cartpoleND, timeStep)
T = 50
problem = crocoddyl.ShootingProblem(
np.array(x0).T, [cartpoleIAM] * T, cartpoleIAM)
ddp = crocoddyl.SolverDDP(problem)
cartpoleDAM2 = DifferentialActionModelCartpole()
cartpoleData2 = cartpoleDAM2.createData()
x2 = cartpoleDAM2.state.rand()
u2 = np.zeros(1)
cartpoleDAM2.calc(cartpoleData2, x2, u2)
cartpoleND2 = crocoddyl.DifferentialActionModelNumDiff(
cartpoleDAM2, True)
data.cost = .5 * np.asscalar(sum(np.asarray(data.r)**2))
def calcDiff(self, data, x, u=None):
# Advance user might implement the derivatives
pass
# Creating the DAM for the cartpole
cartpoleDAM = DifferentialActionModelCartpole()
cartpoleData = cartpoleDAM.createData()
cartpoleDAM = model = DifferentialActionModelCartpole()
# Using NumDiff for computing the derivatives. We specify the
# withGaussApprox=True to have approximation of the Hessian based on the
# Jacobian of the cost residuals.
cartpoleND = crocoddyl.DifferentialActionModelNumDiff(cartpoleDAM, True)
# Getting the IAM using the simpletic Euler rule
timeStep = 5e-2
cartpoleIAM = crocoddyl.IntegratedActionModelEuler(cartpoleND, timeStep)
# Creating the shooting problem
x0 = np.array([0., 3.14, 0., 0.])
T = 50
terminalCartpole = DifferentialActionModelCartpole()
terminalCartpoleDAM = crocoddyl.DifferentialActionModelNumDiff(
terminalCartpole, True)
terminalCartpoleIAM = crocoddyl.IntegratedActionModelEuler(terminalCartpoleDAM)
terminalCartpole.costWeights[0] = 100
])
data.cost = .5 * sum(np.asarray(data.r)**2).item()
init_state = np.array([0, 0, np.pi / 2, 0])
target = np.array([4, 4, np.pi / 2, 0])
ex_m = DifferentialActionModelTT(target)
ex_d = ex_m.createData()
x = ex_m.state.rand()
u = np.zeros(2)
a = ex_m.calc(ex_d, x, u)
print(ex_d.xout)
ex_nd = crocoddyl.DifferentialActionModelNumDiff(ex_m, True)
ex_d = ex_nd.createData()
ex_nd.calc(ex_d, x, u)
print('h')
ex_nd.calcDiff(ex_d, x, u)
print(ex_d.Fx)
timeStep = 5e-2
ex_iam = crocoddyl.IntegratedActionModelEuler(ex_nd, timeStep)
x0 = np.matrix([0., 0, np.pi / 2., 0.]).T
T = 50
problem = crocoddyl.ShootingProblem(x0, [ex_iam] * T, ex_iam)
us = [pinocchio.utils.zero(ex_iam.differential.nu)] * T
xs = problem.rollout(us)
# xs=ddp.xs
# us=ddp.us
# ddp2 = crocoddyl.SolverBoxFDDP(problem_with_contact)
# ddp2.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(),])
# ddp2.th_stop = 1e-6
# ddp2.solve(xs, us, maxiter = int(2e2))
# ddp2.robot_model = robot_model
# plotOCSolution(ddp2)
# plotConvergence(ddp2)
# plot_power(ddp2)
# animateMonoped(ddp2)
# CHECKING THE PARTIAL DERIVATIVES
# runningModel.differential.costs.removeCost('joule_dissipation')
mnd = crocoddyl.DifferentialActionModelNumDiff(runningModel.differential)
dnd = mnd.createData()
m=runningModel.differential
d=m.createData()
x=ddp.xs[3];u=ddp.us[3]
cm=m.costs.costs['joule_dissipation'].cost
#cm.gamma=1
#cm.T_mu=1
#cm.n[:]=1
#cm.K[:,:]=1
# CHECK THE GRADIENT AND THE HESSIAN
n_joints = conf.n_links
x=.5*np.ones(2 * (n_joints + 1))
u=np.ones(n_joints)