for key, value in phase.items(): if key == 'walking': # Creating a walking problem ddp[i] = crocoddyl.SolverDDP( 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) ddp[i].solve(xs, us, 100, False, 0.1)
# For this optimal control problem, we define 250 knots (or running action # models) plus a terminal knot T = 250 q0 = np.matrix([0.173046, 1., -0.52366, 0., 0., 0.1, -0.005]).T x0 = np.concatenate([q0, pinocchio.utils.zero(robot_model.nv)]) problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel) # Creating the DDP solver for this OC problem, defining a logger ddp = crocoddyl.SolverDDP(problem) cameraTF = [2., 2.68, 0.54, 0.2, 0.62, 0.72, 0.22] if WITHDISPLAY and WITHPLOT: display = crocoddyl.GepettoDisplay(talos_arm, 4, 4, cameraTF) ddp.setCallbacks([ crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display) ]) elif WITHDISPLAY: display = crocoddyl.GepettoDisplay(talos_arm, 4, 4, cameraTF) ddp.setCallbacks( [crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)]) elif WITHPLOT: ddp.setCallbacks([ crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), ]) else: ddp.setCallbacks([crocoddyl.CallbackVerbose()]) # Solving it with the DDP algorithm
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()]) # Solving the problem with the FDDP solver solver.solve() # Plotting the entire motion if WITHPLOT: log = solver.getCallbacks()[0] crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=False)
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.SolverBoxFDDP(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])) ]) elif WITHPLOT: solver.setCallbacks( [crocoddyl.CallbackLogger(),
x0 = np.concatenate([q0, v0]) # Defining the walking gait parameters walking_gait = {'stepLength': 0.25, 'stepHeight': 0.25, 'timeStep': 1e-2, 'stepKnots': 25, 'supportKnots': 2} # Setting up the control-limited DDP solver boxddp = crocoddyl.SolverBoxDDP( gait.createWalkingProblem(x0, walking_gait['stepLength'], walking_gait['stepHeight'], walking_gait['timeStep'], walking_gait['stepKnots'], walking_gait['supportKnots'])) # Add the callback functions print('*** SOLVE ***') cameraTF = [2., 2.68, 0.84, 0.2, 0.62, 0.72, 0.22] if WITHDISPLAY and WITHPLOT: display = crocoddyl.GepettoDisplay(anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot]) boxddp.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)]) elif WITHDISPLAY: display = crocoddyl.GepettoDisplay(anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot]) boxddp.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)]) elif WITHPLOT: boxddp.setCallbacks([ crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), ]) else: boxddp.setCallbacks([crocoddyl.CallbackVerbose()]) xs = [robot_model.defaultState] * (boxddp.problem.T + 1) us = boxddp.problem.quasiStatic([anymal.model.defaultState] * boxddp.problem.T) # Solve the DDP problem
def generateTrajectory(self, **kwargs): import time import crocoddyl from crocoddyl.utils.quadruped import SimpleQuadrupedalGaitProblem, plotSolution # Load parameters of the trajectory self.setParametersFromDict(**kwargs) # Loading the solo model solo = loadSolo(False) robot_model = solo.model # Set limits of actuators speeds and efforts robot_model.effortLimit[6:] = np.full(12, self.getParameter('torque_lim')) robot_model.velocityLimit[6:] = np.tile( np.deg2rad(self.getParameter('speed_lim')), 4) # Setting up CoM problem lfFoot, rfFoot, lhFoot, rhFoot = 'FL_FOOT', 'FR_FOOT', 'HL_FOOT', 'HR_FOOT' gait = SimpleQuadrupedalGaitProblem(robot_model, lfFoot, rfFoot, lhFoot, rhFoot) # Defining the initial state of the robot q0 = robot_model.referenceConfigurations['standing'].copy() v0 = pin.utils.zero(robot_model.nv) x0 = np.concatenate([q0, v0]) # Defining the CoM gait parameters Jumping_gait = {} Jumping_gait['jumpHeight'] = self.getParameter('height') Jumping_gait['jumpLength'] = [ self.getParameter('dx'), self.getParameter('dy'), self.getParameter('dz') ] Jumping_gait['timeStep'] = self.getParameter('dt') Jumping_gait['groundKnots'] = self.getParameter('groundKnots') Jumping_gait['flyingKnots'] = self.getParameter('flyingKnots') # Setting up the control-limited DDP solver boxddp = crocoddyl.SolverBoxDDP( gait.createJumpingProblem(x0, Jumping_gait['jumpHeight'], Jumping_gait['jumpLength'], Jumping_gait['timeStep'], Jumping_gait['groundKnots'], Jumping_gait['flyingKnots'])) # Print debug info if requiered t0 = time.time() if self.getParameter('debug'): print("Computing Trajectory...") self.printInfo() # Add the callback functions if requiered if self.getParameter('verbose'): boxddp.setCallbacks([crocoddyl.CallbackVerbose()]) # Setup viewer if requiered cameraTF = [2., 2.68, 0.84, 0.2, 0.62, 0.72, 0.22] if self.getParameter('gepetto_viewer'): display = crocoddyl.GepettoDisplay( solo, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot]) boxddp.setCallbacks([ crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display) ]) xs = [robot_model.defaultState] * (boxddp.problem.T + 1) us = boxddp.problem.quasiStatic([solo.model.defaultState] * boxddp.problem.T) # Solve the DDP problem boxddp.solve(xs, us, self.getParameter('nb_it'), False, 0.1) # Display the entire motion if self.getParameter('gepetto_viewer'): display = crocoddyl.GepettoDisplay( solo, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot]) while True: display.displayFromSolver(boxddp) # Get results from solver xs, us = boxddp.xs, boxddp.us nx, nq, nu = xs[0].shape[0], robot_model.nq, us[0].shape[0] X = [0.] * nx U = [0.] * nu for i in range(nx): X[i] = [np.asscalar(x[i]) for x in xs] for i in range(nu): U[i] = [np.asscalar(u[i]) if u.shape[0] != 0 else 0 for u in us] qa = np.array(X[7:nq])[:, :-1] qa_dot = np.array(X[nq + 6:2 * nq - 1])[:, :-1] torques = np.array(U[:]) t_traj = np.arange(0, qa.shape[1]) * self.getParameter('dt') # Print execution time if requiered if self.getParameter('debug'): print("Done in", time.time() - t0, "s") # Define trajectory for return traj = ActuatorsTrajectory() traj.addElement('t', t_traj) traj.addElement('q', qa) traj.addElement('q_dot', qa_dot) traj.addElement('torques', torques) return traj
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)