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
import os
import sys
import numpy as np
import crocoddyl
import example_robot_data
import pinocchio
from crocoddyl.utils.biped import SimpleBipedGaitProblem, plotSolution
# Creating the lower-body part of Talos
talos_legs = example_robot_data.loadTalosLegs()
# 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': 25, 'supportKnots': 1}},
{'walking': {'stepLength': 0.6, 'stepHeight': 0.1,
'timeStep': 0.03, 'stepKnots': 25, 'supportKnots': 1}},
{'walking': {'stepLength': 0.6, 'stepHeight': 0.1,
class TalosLegsTest(RobotTestCase):
RobotTestCase.ROBOT = example_robot_data.loadTalosLegs()
RobotTestCase.NQ = 19
RobotTestCase.NV = 18