# 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) class extractDDPLPF(): def __init__(self, ddp, nu): self.xs = np.array(ddp.xs)[:, :-nu]
jumping_gait['groundKnots'], jumping_gait['flyingKnots'])) boxddp = crocoddyl.SolverBoxDDP( gait.createJumpingProblem(x0, jumping_gait['jumpHeight'], jumping_gait['jumpLength'], jumping_gait['timeStep'], jumping_gait['groundKnots'], jumping_gait['flyingKnots'])) # Added the callback functions if WITHDISPLAY and WITHPLOT: display = crocoddyl.GepettoDisplay( anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot]) boxfddp.setCallbacks([ crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display) ]) boxddp.setCallbacks([ crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display) ]) elif WITHDISPLAY: display = crocoddyl.GepettoDisplay( anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot]) boxfddp.setCallbacks( [crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)]) boxddp.setCallbacks( [crocoddyl.CallbackVerbose(),
for i, phase in enumerate(GAITPHASES): for key, value in phase.items(): if key == 'walking': # Creating a walking problem ddp[i] = crocoddyl.SolverBoxFDDP( gait.createWalkingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'], value['stepKnots'], value['supportKnots'])) ddp[i].th_stop = 1e-7 # Added the callback functions print('*** SOLVE ' + key + ' ***') if WITHDISPLAY and WITHPLOT: display = crocoddyl.GepettoDisplay(talos_legs, 4, 4, cameraTF, frameNames=[rightFoot, leftFoot]) ddp[i].setCallbacks( [crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)]) elif WITHDISPLAY: display = crocoddyl.GepettoDisplay(talos_legs, 4, 4, cameraTF, frameNames=[rightFoot, leftFoot]) ddp[i].setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)]) elif WITHPLOT: ddp[i].setCallbacks([ crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), ]) else: ddp[i].setCallbacks([crocoddyl.CallbackVerbose()]) # Solving the problem with the DDP solver xs = [talos_legs.model.defaultState] * (ddp[i].problem.T + 1) us = ddp[i].problem.quasiStatic([talos_legs.model.defaultState] * ddp[i].problem.T)
value['timeStep'], value['groundKnots'], value['flyingKnots'])) # Added the callback functions print('*** SOLVE ' + key + ' ***') if WITHDISPLAY and WITHPLOT: display = crocoddyl.GepettoDisplay( anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot]) ddp[i].setCallbacks([ crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display) ]) elif WITHDISPLAY: display = crocoddyl.GepettoDisplay( anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot]) ddp[i].setCallbacks( [crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)]) elif WITHPLOT: ddp[i].setCallbacks([ crocoddyl.CallbackLogger(),
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, actModel, runningCostModel), dt) terminalModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actModel, 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) fddp = crocoddyl.SolverFDDP(problem) fddp.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) fddp.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)]) elif WITHDISPLAY: display = crocoddyl.GepettoDisplay(hector, 4, 4, cameraTF) fddp.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)]) elif WITHPLOT: fddp.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()]) else: fddp.setCallbacks([crocoddyl.CallbackVerbose()]) # Solving the problem with the FDDP solver fddp.solve()
terminalCostModel.addCost("ctrlReg", uRegCost, 1e-7) # Create the action model runningModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics(state, runningCostModel), DT) terminalModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics(state, terminalCostModel)) #runningModel.differential.armature = 0.2 * np.matrix(np.ones(state.nv)).T #terminalModel.differential.armature = 0.2 * np.matrix(np.ones(state.nv)).T # Create the problem q0 = np.matrix([2., 1.5, -2., 0., 0., 0., 0.]).T x0 = np.concatenate([q0, pinocchio.utils.zero(state.nv)]) problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel) # Creating the DDP solver for this OC problem, defining a logger ddp = crocoddyl.SolverDDP(problem) ddp.setCallbacks([crocoddyl.CallbackVerbose()]) # Solving it with the DDP algorithm ddp.solve() # Visualizing the solution in gepetto-viewer crocoddyl.displayTrajectory(robot, ddp.xs, runningModel.dt) robot_data = robot_model.createData() xT = ddp.xs[-1] pinocchio.forwardKinematics(robot_model, robot_data, xT[:state.nq]) pinocchio.updateFramePlacements(robot_model, robot_data) print('Finally reached = ', robot_data.oMf[robot_model.getFrameId("gripper_left_joint")].translation.T)
model = crocoddyl.ActionModelUnicycle() # Setting up the cost weights model.r = [ 10., # state weight 1. # control weight ] # Formulating the optimal control problem T = 20 # number of knots x0 = np.matrix([-1., -1., 1.]).T #x,y,theta problem = crocoddyl.ShootingProblem(x0, [model] * T, model) # Creating the DDP solver for this OC problem, defining a logger ddp = crocoddyl.SolverDDP(problem) ddp.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()]) # Solving it with the DDP algorithm ddp.solve() # Plotting the solution, solver convergence and unicycle motion log = ddp.getCallbacks()[0] crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=False) crocoddyl.plotConvergence(log.costs, log.control_regs, log.state_regs, log.gm_stops, log.th_stops, log.steps, figIndex=2, show=False)
state, actuation, contactModel, runningCostModel) dmodelTerminal = crocoddyl.DifferentialActionModelContactFwdDynamics( state, actuation, contactModel, terminalCostModel) runningModel = crocoddyl.IntegratedActionModelEuler(dmodelRunning, DT) terminalModel = crocoddyl.IntegratedActionModelEuler(dmodelTerminal, 0) # Problem definition x0 = np.concatenate([q0, pinocchio.utils.zero(state.nv)]) problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel) # Creating the DDP solver for this OC problem, defining a logger solver = crocoddyl.SolverFDDP(problem) if WITHDISPLAY and WITHPLOT: solver.setCallbacks([ crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay( crocoddyl.GepettoDisplay(robot, 4, 4, frameNames=[rightFoot, leftFoot])) ]) elif WITHDISPLAY: solver.setCallbacks([ crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay( crocoddyl.GepettoDisplay(robot, 4, 4, frameNames=[rightFoot, leftFoot])) ])