def createProblem(gait_phase): robot_model = example_robot_data.loadTalosLegs().model rightFoot, leftFoot = 'right_sole_link', 'left_sole_link' gait = SimpleBipedGaitProblem(robot_model, rightFoot, leftFoot) q0 = robot_model.referenceConfigurations['half_sitting'].copy() v0 = pinocchio.utils.zero(robot_model.nv) x0 = np.concatenate([q0, v0]) type_of_gait = list(gait_phase.keys())[0] value = gait_phase[type_of_gait] if type_of_gait == 'walking': # Creating a walking problem problem = gait.createWalkingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'], value['stepKnots'], value['supportKnots']) xs = [robot_model.defaultState] * (len(problem.runningModels) + 1) us = [ m.quasiStatic(d, robot_model.defaultState) for m, d in list(zip(problem.runningModels, problem.runningDatas)) ] return xs, us, problem
WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ # Creating the lower-body part of Talos talos_legs = example_robot_data.load('talos_legs') # Defining the initial state of the robot q0 = talos_legs.model.referenceConfigurations['half_sitting'].copy() v0 = pinocchio.utils.zero(talos_legs.model.nv) x0 = np.concatenate([q0, v0]) # Setting up the 3d walking problem rightFoot = 'right_sole_link' leftFoot = 'left_sole_link' gait = SimpleBipedGaitProblem(talos_legs.model, rightFoot, leftFoot) # Setting up all tasks GAITPHASES = \ [{'walking': {'stepLength': 0.6, 'stepHeight': 0.1, 'timeStep': 0.03, 'stepKnots': 35, 'supportKnots': 10}}, {'walking': {'stepLength': 0.6, 'stepHeight': 0.1, 'timeStep': 0.03, 'stepKnots': 35, 'supportKnots': 10}}, {'walking': {'stepLength': 0.6, 'stepHeight': 0.1, 'timeStep': 0.03, 'stepKnots': 35, 'supportKnots': 10}}, {'walking': {'stepLength': 0.6, 'stepHeight': 0.1, 'timeStep': 0.03, 'stepKnots': 35, 'supportKnots': 10}}] cameraTF = [3., 3.68, 0.84, 0.2, 0.62, 0.72, 0.22] ddp = [None] * len(GAITPHASES) for i, phase in enumerate(GAITPHASES):