class Impulse3DTest(ImpulseModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadHyQ().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
# gains = pinocchio.utils.rand(2)
frame = ROBOT_MODEL.getFrameId('lf_foot')
IMPULSE = crocoddyl.ImpulseModel3D(ROBOT_STATE, frame)
IMPULSE_DER = Impulse3DDerived(ROBOT_STATE, frame)
class Contact3DTest(ContactModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadHyQ().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
gains = pinocchio.utils.rand(2)
xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('lf_foot'),
pinocchio.SE3.Random().translation)
CONTACT = crocoddyl.ContactModel3D(ROBOT_STATE, xref, gains)
CONTACT_DER = Contact3DDerived(ROBOT_STATE, xref, gains)
class Impulse3DMultipleTest(ImpulseModelMultipleAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadHyQ().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
gains = pinocchio.utils.rand(2)
IMPULSES = collections.OrderedDict(
sorted({
'lf_foot': crocoddyl.ImpulseModel3D(ROBOT_STATE, ROBOT_MODEL.getFrameId('lf_foot')),
'rh_foot': crocoddyl.ImpulseModel3D(ROBOT_STATE, ROBOT_MODEL.getFrameId('rh_foot'))
}.items()))
def createProblem(gait_phase):
robot_model = example_robot_data.loadHyQ().model
lfFoot, rfFoot, lhFoot, rhFoot = 'lf_foot', 'rf_foot', 'lh_foot', 'rh_foot'
gait = SimpleQuadrupedalGaitProblem(robot_model, lfFoot, rfFoot, lhFoot,
rhFoot)
q0 = robot_model.referenceConfigurations['standing'].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'])
elif type_of_gait == 'trotting':
# Creating a trotting problem
problem = gait.createTrottingProblem(x0, value['stepLength'],
value['stepHeight'],
value['timeStep'],
value['stepKnots'],
value['supportKnots'])
elif type_of_gait == 'pacing':
# Creating a pacing problem
problem = gait.createPacingProblem(x0, value['stepLength'],
value['stepHeight'],
value['timeStep'],
value['stepKnots'],
value['supportKnots'])
elif type_of_gait == 'bounding':
# Creating a bounding problem
problem = gait.createBoundingProblem(x0, value['stepLength'],
value['stepHeight'],
value['timeStep'],
value['stepKnots'],
value['supportKnots'])
elif type_of_gait == 'jumping':
# Creating a jumping problem
problem = gait.createJumpingProblem(x0, value['jumpHeight'],
value['jumpLength'],
value['timeStep'],
value['groundKnots'],
value['flyingKnots'])
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
class Contact3DMultipleTest(ContactModelMultipleAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadHyQ().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
gains = pinocchio.utils.rand(2)
CONTACTS = collections.OrderedDict(
sorted({
'lf_foot':
crocoddyl.ContactModel3D(
ROBOT_STATE,
crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('lf_foot'),
pinocchio.SE3.Random().translation),
gains),
'rh_foot':
crocoddyl.ContactModel3D(
ROBOT_STATE,
crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('rh_foot'),
pinocchio.SE3.Random().translation),
gains)
}.items()))
class HyQFloatingBaseActuationTest(ActuationModelAbstractTestCase):
STATE = crocoddyl.StateMultibody(example_robot_data.loadHyQ().model)
ACTUATION = crocoddyl.ActuationModelFloatingBase(STATE)
ACTUATION_DER = FreeFloatingActuationDerived(STATE)
class StateMultibodyHyQTest(StateAbstractTestCase):
MODEL = example_robot_data.loadHyQ().model
NX = MODEL.nq + MODEL.nv
NDX = 2 * MODEL.nv
STATE = crocoddyl.StateMultibody(MODEL)
STATE_DER = StateMultibodyDerived(MODEL)
class HyQTest(RobotTestCase):
RobotTestCase.ROBOT = example_robot_data.loadHyQ()
RobotTestCase.NQ = 19
RobotTestCase.NV = 18
import pinocchio as pin import numpy as np import example_robot_data from pinocchio.utils import * #rand from pinocchio.robot_wrapper import RobotWrapper from pinocchio.utils import * # console print options to see matrix nicely np.set_printoptions(precision = 3, linewidth = 200, suppress = True) np.set_printoptions(threshold=np.inf) # Loading a robot model model = example_robot_data.loadHyQ().model data = model.createData() #start configuration v = np.array([0.0 , 0.0 , 0.0, 0.0, 0.0, 0.0, #underactuated 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) #actuated q = example_robot_data.loadHyQ().q0 # Update the joint and frame placements pin.forwardKinematics(model,data,q,v) pin.updateFramePlacements(model,data) M = pin.crba(model, data, q) H = pin.nonLinearEffects(model, data, q, v) G = pin.computeGeneralizedGravity(model,data, q) #EXERCISE 1: Compute the com of the robot (in WF)
def runBenchmark(gait_phase):
robot_model = example_robot_data.loadHyQ().model
lfFoot, rfFoot, lhFoot, rhFoot = 'lf_foot', 'rf_foot', 'lh_foot', 'rh_foot'
gait = SimpleQuadrupedalGaitProblem(robot_model, lfFoot, rfFoot, lhFoot,
rhFoot)
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
ddp = crocoddyl.SolverDDP(
gait.createWalkingProblem(x0, value['stepLength'],
value['stepHeight'], value['timeStep'],
value['stepKnots'],
value['supportKnots']))
elif type_of_gait == 'trotting':
# Creating a trotting problem
ddp = crocoddyl.SolverDDP(
gait.createTrottingProblem(x0, value['stepLength'],
value['stepHeight'], value['timeStep'],
value['stepKnots'],
value['supportKnots']))
elif type_of_gait == 'pacing':
# Creating a pacing problem
ddp = crocoddyl.SolverDDP(
gait.createPacingProblem(x0, value['stepLength'],
value['stepHeight'], value['timeStep'],
value['stepKnots'],
value['supportKnots']))
elif type_of_gait == 'bounding':
# Creating a bounding problem
ddp = crocoddyl.SolverDDP(
gait.createBoundingProblem(x0, value['stepLength'],
value['stepHeight'], value['timeStep'],
value['stepKnots'],
value['supportKnots']))
elif type_of_gait == 'jumping':
# Creating a jumping problem
ddp = crocoddyl.SolverDDP(
gait.createJumpingProblem(x0, value['jumpHeight'],
value['timeStep']))
duration = []
xs = [robot_model.defaultState] * len(ddp.models())
us = [
m.quasicStatic(d, robot_model.defaultState)
for m, d in list(zip(ddp.models(), ddp.datas()))[:-1]
]
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
import sys
import numpy as np
import crocoddyl
import example_robot_data
import pinocchio
from crocoddyl.utils.quadruped import SimpleQuadrupedalGaitProblem, plotSolution
WITHDISPLAY = 'display' in sys.argv
WITHPLOT = 'plot' in sys.argv
# Loading the HyQ model
hyq = example_robot_data.loadHyQ()
# Defining the initial state of the robot
q0 = hyq.model.referenceConfigurations['half_sitting'].copy()
v0 = pinocchio.utils.zero(hyq.model.nv)
x0 = np.concatenate([q0, v0])
# Setting up the 3d walking problem
lfFoot = 'lf_foot'
rfFoot = 'rf_foot'
lhFoot = 'lh_foot'
rhFoot = 'rh_foot'
gait = SimpleQuadrupedalGaitProblem(hyq.model, lfFoot, rfFoot, lhFoot, rhFoot)
# Setting up all tasks
GAITPHASES = [{
'walking': {
'stepLength': 0.15,
