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,