def getTrainingData(ntraj:int = 10000):
trajectory = []
for _ in range(ntraj):
initial_config = [random.uniform(-1.99, 1.99), random.uniform(-1.99, 1.99), 0]
model = crocoddyl.ActionModelUnicycle()
model.costWeights = np.matrix([1, 0.3]).T
problem = crocoddyl.ShootingProblem(np.matrix(initial_config).T, [ model ] * 30, model)
ddp = crocoddyl.SolverFDDP(problem)
ddp.solve([], [], 1000)
if ddp.iter < 1000:
# Store trajectory(ie x, y)
a = np.delete(np.array(ddp.xs), 2, 1)
a = np.array(a.flatten())
b = np.append(a, sum(d.cost for d in ddp.datas()))
trajectory.append(b)
#trajectory.append(sum(d.cost for d in ddp.datas()))
# Dataset 2: Shape (number of init_points, 63)..columns are recurring (x,y).
# The first two columns will form train. The last column is the cost associated with each trajectory
trajectory = np.array(trajectory)
print(trajectory.shape)
df_trajectory = pd.DataFrame(trajectory[0:,0:])
return df_trajectory
def get_data(ntraj: int = 1):
data = []
T = 100
for _ in range(ntraj):
x0 = [
random.uniform(0, 1),
random.uniform(-3.14, 3.14),
random.uniform(0., 0.99),
random.uniform(0., .99)
]
runningModel, terminalModel = cartpole()
problem = crocoddyl.ShootingProblem(
np.matrix(x0).T, [runningModel] * T, terminalModel)
ddp = crocoddyl.SolverFDDP(problem)
ddp.solve()
if ddp.iter < 1000:
position = []
# Attach state four vectors
position.extend(float(i) for i in ddp.xs[0])
# Attach control
position.extend(float(i) for i in ddp.us[0])
# Attach cost
position.append(sum(d.cost for d in ddp.datas()))
# Attach the number of iterations
position.append(ddp.iter)
data.append(position)
data = np.array(data)
def stateData(nTraj: int = 10000,
modelWeight1: float = 1,
modelWeight2: float = 0.4,
timeHorizon=30,
initialTheta: float = 0.,
fddp: bool = False,
varyingInitialTheta: bool = False,
saveData: bool = False):
trajectory = []
model = crocoddyl.ActionModelUnicycle()
for _ in range(nTraj):
if varyingInitialTheta:
initial_config = [
random.uniform(-1.99, 1.99),
random.uniform(-1.99, 1.99),
random.uniform(0, 1)
]
else:
initial_config = [
random.uniform(-1.99, 1.99),
random.uniform(-1.99, 1.99), initialTheta
]
model.costWeights = np.matrix([modelWeight1, modelWeight2]).T
problem = crocoddyl.ShootingProblem(
np.matrix(initial_config).T, [model] * timeHorizon, model)
if fddp:
ddp = crocoddyl.SolverFDDP(problem)
else:
ddp = crocoddyl.SolverDDP(problem)
# Will need to include a assertion check for done in more complicated examples
ddp.solve([], [], 1000)
if ddp.iter < 1000:
# Store trajectory(ie x, y, theta)
a = np.array(ddp.xs)
# trajectory -> (x, y, theta)..(x, y, theta)..
a = np.array(a.flatten())
# append cost at the end of each trajectory
b = np.append(a, sum(d.cost for d in ddp.datas()))
trajectory.append(b)
trajectory_ = np.array(trajectory)
if saveData:
df = pd.DataFrame(trajectory_[0:, 0:])
df.to_csv("trajectories.csv")
print(trajectory_.shape)
print(
"# Dataset 2: Shape (nTraj, 3 X TimeWindow + 1)..columns are recurring ie (x,y, theta),(x,y, theta).... \n\
# The first two columns will form train. The last column is the cost associated with each trajectory"
)
return trajectory_
def getTrainingData(nTraj: int = 10000,
thetaVal: bool = False,
fddp: bool = False):
model = crocoddyl.ActionModelUnicycle()
trajectory = []
for _ in range(nTraj):
if thetaVal:
initial_config = [
random.uniform(-1.99, 1.99),
random.uniform(-1.99, 1.99),
random.uniform(0., 1.)
]
else:
initial_config = [
random.uniform(-1.99, 1.99),
random.uniform(-1.99, 1.99), 0.0
]
model.costWeights = np.matrix([1, 0.3]).T
problem = crocoddyl.ShootingProblem(
np.matrix(initial_config).T, [model] * 30, model)
if fddp:
ddp = crocoddyl.SolverFDDP(problem)
else:
ddp = crocoddyl.SolverDDP(problem)
ddp.solve([], [], 1000)
if ddp.iter < 1000:
state = []
# Attach x, y, theta. this will form the columns in x_training... index 0, 1, 2
state.extend(i for i in ddp.xs[0])
# Attach control: linear_velocity, angular_velocty.....index 3, 4
state.extend(i for i in ddp.us[0])
# Attach value function: cost....index 5
state.append(sum(d.cost for d in ddp.datas()))
trajectory_control = np.hstack(
(np.array(ddp.xs[1:]), np.array(ddp.us)))
state.extend(i for i in trajectory_control.flatten()
) #..............index 6 to -1.....30 X 5
trajectory.append(state)
data = np.array(trajectory)
df = pd.DataFrame(data)
#....................................
# x_train = data[:,0:3] x,y,z
# y_train = data[:,3:] lv, av, cost, (x, y, theta, control1, control2)
#....................................
return data
def runDDPSolveBenchmark(xs, us, problem):
ddp = crocoddyl.SolverFDDP(problem)
if CALLBACKS:
ddp.setCallbacks([crocoddyl.CallbackVerbose()])
duration = []
for i in range(T):
c_start = time.time()
ddp.solve(xs, us, MAXITER, False, 0.1)
c_end = time.time()
duration.append(1e3 * (c_end - c_start))
avrg_duration = sum(duration) / len(duration)
min_duration = min(duration)
max_duration = max(duration)
return avrg_duration, min_duration, max_duration
def solver(starting_condition, T=30, precision=1e-9, maxiters=1000):
"""Solve one pendulum problem"""
robot = example_robot_data.loadDoublePendulum()
robot_model = robot.model
state = crocoddyl.StateMultibody(robot_model)
actModel = ActuationModelDoublePendulum(state, actLink=1)
weights = np.array([1.5, 1.5, 1, 1] + [0.1] * 2)
runningCostModel = crocoddyl.CostModelSum(state, actModel.nu)
dt = 1e-2
xRegCost = crocoddyl.CostModelState(
state, crocoddyl.ActivationModelQuad(state.ndx), state.zero(),
actModel.nu)
uRegCost = crocoddyl.CostModelControl(state,
crocoddyl.ActivationModelQuad(1),
actModel.nu)
xPendCost = CostModelDoublePendulum(
state, crocoddyl.ActivationModelWeightedQuad(weights), actModel.nu)
runningCostModel.addCost("uReg", uRegCost, 1e-4 / dt)
runningCostModel.addCost("xGoal", xPendCost, 1e-5 / dt)
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(
state, actModel, runningCostModel), dt)
terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu)
terminalCostModel.addCost("xGoal", xPendCost, 1e4)
terminal_model = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(
state, actModel, terminalCostModel), dt)
problem = crocoddyl.ShootingProblem(starting_condition, [runningModel] * T,
terminal_model)
fddp = crocoddyl.SolverFDDP(problem)
fddp.th_stop = precision
fddp.solve([], [], maxiters)
def data(nTraj: int = 1000, fddp: bool = False):
model = crocoddyl.ActionModelUnicycle()
x_data = []
y_data = []
for _ in range(nTraj):
if thetaVal:
initial_config = [
random.uniform(-1.99, 1.99),
random.uniform(-1.99, 1.99),
random.uniform(0., 1.)
]
else:
initial_config = [
random.uniform(-1.99, 1.99),
random.uniform(-1.99, 1.99), 0.0
]
model.costWeights = np.matrix([1, 0.3]).T
problem = crocoddyl.ShootingProblem(
np.matrix(initial_config).T, [model] * 30, model)
if fddp:
ddp = crocoddyl.SolverFDDP(problem)
else:
ddp = crocoddyl.SolverDDP(problem)
ddp.solve([], [], 1000)
if ddp.iter < 1000:
xs = np.array(ddp.xs)
cost = []
for d in ddp.data:
cost.append(d.cost)
cost = np.array(cost).reshape(31, 1)
state = np.hstack((cs, cost))
y_data.append(state.ravel())
x_data.append(initial_config)
x_data = np.array(x_data)
y_data = np.array(y_data)
def testData(ntraj: int = 50):
"""
Data is in the form of (x, y, theta, cost)
"""
model = crocoddyl.ActionModelUnicycle()
cost_trajectory = []
for _ in range(ntraj):
x, y = point(0, 0, 2.1)
initial_config = [x, y, 0]
model.costWeights = np.matrix([1, 0.3]).T
problem = crocoddyl.ShootingProblem(
np.matrix(initial_config).T, [model] * 30, model)
ddp = crocoddyl.SolverFDDP(problem)
ddp.solve([], [], 1000)
if ddp.iter < 1000:
a_ = np.array(ddp.xs)
a = np.array(a_)
b = a.flatten()
c = np.append(a, sum(d.cost for d in ddp.datas()))
cost_trajectory.append(c)
return np.array(cost_trajectory)
goalTrackingCost = crocoddyl.CostModelResidual(state, goalTrackingResidual)
runningCostModel.addCost("xReg", xRegCost, 1e-6)
runningCostModel.addCost("uReg", uRegCost, 1e-6)
runningCostModel.addCost("trackPose", goalTrackingCost, 1e-2)
terminalCostModel.addCost("goalPose", goalTrackingCost, 100.)
dt = 3e-2
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, runningCostModel), dt)
terminalModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, terminalCostModel), dt)
# Creating the shooting problem and the FDDP solver
T = 33
problem = crocoddyl.ShootingProblem(np.concatenate([hector.q0, np.zeros(state.nv)]), [runningModel] * T, terminalModel)
solver = crocoddyl.SolverFDDP(problem)
solver.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()])
cameraTF = [-0.03, 4.4, 2.3, -0.02, 0.56, 0.83, -0.03]
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(hector, 4, 4, cameraTF)
solver.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(hector, 4, 4, cameraTF)
solver.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
solver.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()])
else:
solver.setCallbacks([crocoddyl.CallbackVerbose()])
def optimalSolution(init_states: Union[list, np.ndarray, torch.Tensor],
terminal_model: crocoddyl.ActionModelAbstract = None,
horizon: int = 150,
precision: float = 1e-9,
maxiters: int = 1000,
use_fddp: bool = True):
"""Solve double pendulum problem with the given terminal model for the given position
Parameters
----------
init_states : list or array or tensor
These are the initial, starting configurations for the double pendulum
terminal_model: crocoddyl.ActionModelAbstract
The terminal model to be used to solve the problem
horizon : int
Time horizon for the running model
precision : float
precision for ddp.th_stop
maxiters : int
Maximum iterations allowed for the problem
use_fddp : boolean
Solve using ddp or fddp
Returns
--------
ddp : crocoddyl.Solverddp
the optimal ddp or fddp of the prblem
"""
if isinstance(init_states, torch.Tensor):
init_states = init_states.numpy()
init_states = np.atleast_2d(init_states)
solutions = []
for init_state in init_states:
robot = example_robot_data.loadDoublePendulum()
robot_model = robot.model
state = crocoddyl.StateMultibody(robot_model)
actModel = ActuationModelDoublePendulum(state, actLink=1)
weights = np.array([1.5, 1.5, 1, 1] + [0.1] * 2)
runningCostModel = crocoddyl.CostModelSum(state, actModel.nu)
dt = 1e-2
xRegCost = crocoddyl.CostModelState(
state, crocoddyl.ActivationModelQuad(state.ndx), state.zero(),
actModel.nu)
uRegCost = crocoddyl.CostModelControl(
state, crocoddyl.ActivationModelQuad(1), actModel.nu)
xPendCost = CostModelDoublePendulum(
state, crocoddyl.ActivationModelWeightedQuad(weights),
actModel.nu)
runningCostModel.addCost("uReg", uRegCost, 1e-4 / dt)
runningCostModel.addCost("xGoal", xPendCost, 1e-5 / dt)
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(
state, actModel, runningCostModel), dt)
if terminal_model is None:
terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu)
terminalCostModel.addCost("xGoal", xPendCost, 1e4)
terminal_model = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(
state, actModel, terminalCostModel), dt)
problem = crocoddyl.ShootingProblem(init_state,
[runningModel] * horizon,
terminal_model)
if use_fddp:
fddp = crocoddyl.SolverFDDP(problem)
else:
fddp = crocoddyl.SolverDDP(problem)
fddp.th_stop = precision
fddp.solve([], [], maxiters)
solutions.append(fddp)
return solutions
'jumpLength': [0.0, 0.3, 0.],
'timeStep': 1e-2,
'groundKnots': 10,
'flyingKnots': 20
}
}]
cameraTF = [2., 2.68, 0.84, 0.2, 0.62, 0.72, 0.22]
ddp = [None] * len(GAITPHASES)
for i, phase in enumerate(GAITPHASES):
for key, value in phase.items():
if key == 'walking':
# Creating a walking problem
ddp[i] = crocoddyl.SolverFDDP(
gait.createWalkingProblem(x0, value['stepLength'],
value['stepHeight'],
value['timeStep'],
value['stepKnots'],
value['supportKnots']))
elif key == 'trotting':
# Creating a trotting problem
ddp[i] = crocoddyl.SolverFDDP(
gait.createTrottingProblem(x0, value['stepLength'],
value['stepHeight'],
value['timeStep'],
value['stepKnots'],
value['supportKnots']))
elif key == 'pacing':
# Creating a pacing problem
ddp[i] = crocoddyl.SolverFDDP(
gait.createPacingProblem(x0, value['stepLength'],
value['stepHeight'],
def solve_problem(terminal_model=None,
initial_configuration=None,
horizon: int = 100,
precision: float = 1e-9,
maxiters: int = 1000):
"""
Solve the problem for a given initial_position.
@params:
1: terminal_model = Terminal model with neural network inside it, for the crocoddyl problem.
If none, then Crocoddyl Integrated Action Model will be used as terminal model.
2: initial_configuration = initial position for the unicycle,
either a list or a numpy array or a tensor.
3: horizon = Time horizon for the unicycle. Defaults to 100
4: stop = ddp.th_stop. Defaults to 1e-9
5: maxiters = maximum iterations allowed for crocoddyl.Defaults to 1000
@returns:
ddp
"""
if isinstance(initial_configuration, list):
initial_configuration = np.array(initial_configuration)
elif isinstance(initial_configuration, torch.Tensor):
initial_configuration = initial_configuration.numpy()
# Loading the double pendulum model
robot = example_robot_data.loadDoublePendulum()
robot_model = robot.model
state = crocoddyl.StateMultibody(robot_model)
actModel = ActuationModelDoublePendulum(state, actLink=1)
weights = np.array([1, 1, 1, 1] + [0.1] * 2)
runningCostModel = crocoddyl.CostModelSum(state, actModel.nu)
xRegCost = crocoddyl.CostModelState(
state, crocoddyl.ActivationModelQuad(state.ndx), state.zero(),
actModel.nu)
uRegCost = crocoddyl.CostModelControl(state,
crocoddyl.ActivationModelQuad(1),
actModel.nu)
xPendCost = CostModelDoublePendulum(
state, crocoddyl.ActivationModelWeightedQuad(np.matrix(weights).T),
actModel.nu)
dt = 1e-2
runningCostModel.addCost("uReg", uRegCost, 1e-4 / dt)
runningCostModel.addCost("xGoal", xPendCost, 1e-5 / dt)
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(
state, actModel, runningCostModel), dt)
if terminal_model is None:
terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu)
terminalCostModel.addCost("xGoal", xPendCost, 1e4)
terminal_model = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(
state, actModel, terminalCostModel), dt)
# Creating the shooting problem and the FDDP solver
problem = crocoddyl.ShootingProblem(initial_configuration.T,
[runningModel] * horizon,
terminal_model)
fddp = crocoddyl.SolverFDDP(problem)
fddp.th_stop = precision
fddp.solve([], [], maxiters)
return fddp
def stateData(nTraj:int = 10,
modelWeight1:float = 1,
modelWeight2:float = 0.4,
timeHorizon = 30,
initialTheta:float = 0.,
fddp:bool = False,
varyingInitialTheta:bool = False,
saveData:bool = False
):
data = []
trajectory = []
model = crocoddyl.ActionModelUnicycle()
for _ in range(nTraj):
if varyingInitialTheta:
initial_config = [random.uniform(-1.99, 1.99), random.uniform(-1.99, 1.99), initialTheta]
else:
initial_config = [random.uniform(-1.99, 1.99), random.uniform(-1.99, 1.99), initialTheta]
model.costWeights = np.matrix([modelWeight1, modelWeight2]).T
problem = crocoddyl.ShootingProblem(np.matrix(initial_config).T, [ model ] * timeHorizon, model)
if fddp:
ddp = crocoddyl.SolverFDDP(problem)
else:
ddp = crocoddyl.SolverDDP(problem)
# Will need to include a assertion check for done in more complicated examples
ddp.solve([], [], 1000)
if ddp.iter < 1000:
state = []
# Attach x, y, theta
state.extend(i for i in ddp.xs[0])
# Attach control: linear_velocity, angular_velocty
state.extend(i for i in ddp.us[0])
# Attach value function: cost
state.append(sum(d.cost for d in ddp.datas()))
# Attach the number of iterations
state.append(ddp.iter)
data.append(state)
# Store trajectory(ie x, y)
a = np.delete(np.array(ddp.xs), 2, 1)
a = np.array(a.flatten())
b = np.append(a, sum(d.cost for d in ddp.datas()))
trajectory.append(b)
# Dataset 1: x, y, z, linear_velocity, angular_velocity, value_function, iterations
data = np.array(data)
df = pd.DataFrame(data[0:,0:], columns = ["x_position", "y_position",
"z_position", "linear_velocity",
"angular_velocity", "value_function",
"iterations"])
# Dataset 2: Shape (number of init_points, 63)..columns are recurring (x,y).
# The first two columns will form train.
# The last column is the cost associated with each trajectory
trajectory = np.array(trajectory)
print(trajectory.shape)
print("# Dataset 2: Shape (number of init_points, 63)..columns are recurring ie (x,y),(x,y).... \n\
# The first two columns will form train. The last column is the cost associated with each trajectory")
df_trajectory = pd.DataFrame(trajectory[0:,0:])
if saveData:
df.to_csv("initialStates.csv")
df_trajectory.to_csv("trajectory.csv")
print(f"\nReturning {list(df.columns) } for {nTraj} trajectories.")
print("\nReturing trajectories.")
return df, df_trajectory
def run():
# Loading the anymal model
anymal = example_robot_data.load('anymal')
# nq is the dimension fo the configuration vector representation
# nv dimension of the velocity vector space
# Defining the initial state of the robot
q0 = anymal.model.referenceConfigurations['standing'].copy()
v0 = pinocchio.utils.zero(anymal.model.nv)
x0 = np.concatenate([q0, v0])
# Setting up the 3d walking problem
lfFoot, rfFoot, lhFoot, rhFoot = 'LF_FOOT', 'RF_FOOT', 'LH_FOOT', 'RH_FOOT'
gait = SimpleQuadrupedalGaitProblem(anymal.model, lfFoot, rfFoot, lhFoot, rhFoot)
cameraTF = [2., 2.68, 0.84, 0.2, 0.62, 0.72, 0.22]
walking = {
'stepLength': 0.25,
'stepHeight': 0.15,
'timeStep': 1e-2,
'stepKnots': 100,
'supportKnots': 2
}
# Creating a walking problem
ddp = crocoddyl.SolverFDDP(
gait.createWalkingProblem(x0, walking['stepLength'], walking['stepHeight'], walking['timeStep'],
walking['stepKnots'], walking['supportKnots']))
plot = False
display = False
if display:
# Added the callback functions
display = crocoddyl.GepettoDisplay(anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
ddp.setCallbacks(
[crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)])
# Solving the problem with the DDP solver
xs = [anymal.model.defaultState] * (ddp.problem.T + 1)
us = ddp.problem.quasiStatic([anymal.model.defaultState] * ddp.problem.T)
ddp.solve(xs, us, 100, False, 0.1)
if display:
# Defining the final state as initial one for the next phase
# Display the entire motion
display = crocoddyl.GepettoDisplay(anymal, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
display.displayFromSolver(ddp)
# Plotting the entire motion
if plot:
plotSolution(ddp, figIndex=1, show=False)
log = ddp.getCallbacks()[0]
crocoddyl.plotConvergence(log.costs,
log.u_regs,
log.x_regs,
log.grads,
log.stops,
log.steps,
figTitle='walking',
figIndex=3,
show=True)
terminalModel = IntegratedActionModelLPF(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation,
terminalCostModel), 0.)
# Setting the nodes of the problem with a sliding variable
ratioContactTotal = 0.4 / (conf.dt * T) # expressed as ratio in [s]
contactNodes = int(conf.T * ratioContactTotal)
flyingNodes = conf.T - contactNodes
problem_with_contact = crocoddyl.ShootingProblem(
x0, [contactPhase] * contactNodes + [runningModel] * flyingNodes,
terminalModel)
problem_without_contact = crocoddyl.ShootingProblem(x0, [runningModel] * T,
terminalModel)
# SOLVE
ddp = crocoddyl.SolverFDDP(problem_with_contact)
ddp.setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
])
ddp.lpf = True
# Additionally also modify ddp.th_stop and ddp.th_grad
ddp.th_stop = 1e-6
ddp.solve([], [], maxiter=int(1e3))
ddp.robot_model = robot_model
# SHOWING THE RESULTS
plotOCSolution(ddp)
plotConvergence(ddp)