def __init__(self): self.robot = load('solo12') #Inputs to be modified bu the user before calling .compute self.feet_position_ref = [ np.array([0.1946, 0.16891, 0.0191028]), np.array([0.1946, -0.16891, 0.0191028]), np.array([-0.1946, 0.16891, 0.0191028]), np.array([-0.1946, -0.16891, 0.0191028]) ] self.feet_velocity_ref = [ np.array([0, 0, 0]), np.array([0, 0, 0]), np.array([0, 0, 0]), np.array([0, 0, 0]) ] self.feet_acceleration_ref = [ np.array([0, 0, 0]), np.array([0, 0, 0]), np.array([0, 0, 0]), np.array([0, 0, 0]) ] self.flag_in_contact = np.array([0, 1, 0, 1]) self.base_orientation_ref = pin.utils.rpyToMatrix(0, 0, np.pi / 6) self.base_angularvelocity_ref = np.array([0, 0, 0]) self.base_angularacceleration_ref = np.array([0, 0, 0]) self.base_position_ref = np.array([0, 0, 0.235]) self.base_linearvelocity_ref = np.array([0, 0, 0]) self.base_linearacceleration_ref = np.array([0, 0, 0]) self.Kp_base_orientation = 100 self.Kd_base_orientation = 2 * np.sqrt(self.Kp_base_orientation) self.Kp_base_position = 100 self.Kd_base_position = 2 * np.sqrt(self.Kp_base_position) self.Kp_flyingfeet = 100 self.Kd_flyingfeet = 2 * np.sqrt(self.Kp_flyingfeet) #Get frame IDs FL_FOOT_ID = self.robot.model.getFrameId('FL_FOOT') FR_FOOT_ID = self.robot.model.getFrameId('FR_FOOT') HL_FOOT_ID = self.robot.model.getFrameId('HL_FOOT') HR_FOOT_ID = self.robot.model.getFrameId('HR_FOOT') self.BASE_ID = self.robot.model.getFrameId('base_link') self.foot_ids = np.array( [FL_FOOT_ID, FR_FOOT_ID, HL_FOOT_ID, HR_FOOT_ID]) def dinv(J, damping=1e-2): ''' Damped inverse ''' U, S, V = np.linalg.svd(J) if damping == 0: Sinv = 1 / S else: Sinv = S / (S**2 + damping**2) return (V.T * Sinv) @ U.T self.rmodel = self.robot.model self.rdata = self.robot.data self.i = 0
class TalosArmShootingTest(ShootingProblemTestCase): ROBOT_MODEL = example_robot_data.load('talos_arm').model STATE = crocoddyl.StateMultibody(ROBOT_MODEL) ACTUATION = crocoddyl.ActuationModelFull(STATE) COST_SUM = crocoddyl.CostModelSum(STATE) COST_SUM.addCost( 'gripperPose', crocoddyl.CostModelResidual( STATE, crocoddyl.ResidualModelFramePlacement( STATE, ROBOT_MODEL.getFrameId("gripper_left_joint"), pinocchio.SE3.Random())), 1e-3) COST_SUM.addCost( "xReg", crocoddyl.CostModelResidual(STATE, crocoddyl.ResidualModelState(STATE)), 1e-7) COST_SUM.addCost( "uReg", crocoddyl.CostModelResidual(STATE, crocoddyl.ResidualModelControl(STATE)), 1e-7) DIFF_MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics( STATE, ACTUATION, COST_SUM) DIFF_MODEL_DER = DifferentialFreeFwdDynamicsModelDerived( STATE, ACTUATION, COST_SUM) MODEL = crocoddyl.IntegratedActionModelEuler(DIFF_MODEL, 1e-3) MODEL_DER = crocoddyl.IntegratedActionModelEuler(DIFF_MODEL_DER, 1e-3)
class TalosArmFreeFwdDynamicsWithArmatureTest(ActionModelAbstractTestCase): ROBOT_MODEL = example_robot_data.load('talos_arm').model STATE = crocoddyl.StateMultibody(ROBOT_MODEL) ACTUATION = crocoddyl.ActuationModelFull(STATE) COST_SUM = crocoddyl.CostModelSum(STATE) COST_SUM.addCost( 'gripperPose', crocoddyl.CostModelResidual( STATE, crocoddyl.ResidualModelFramePlacement( STATE, ROBOT_MODEL.getFrameId("gripper_left_joint"), pinocchio.SE3.Random())), 1e-3) COST_SUM.addCost( "xReg", crocoddyl.CostModelResidual(STATE, crocoddyl.ResidualModelState(STATE)), 1e-7) COST_SUM.addCost( "uReg", crocoddyl.CostModelResidual(STATE, crocoddyl.ResidualModelControl(STATE)), 1e-7) MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics( STATE, ACTUATION, COST_SUM) MODEL_DER = DifferentialFreeFwdDynamicsModelDerived( STATE, ACTUATION, COST_SUM) MODEL.armature = 0.1 * np.ones(ROBOT_MODEL.nv) MODEL_DER.set_armature(0.1 * np.ones(ROBOT_MODEL.nv))
class FramePlacementCostSumTest(CostModelSumTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) COST = crocoddyl.CostModelResidual( ROBOT_STATE, crocoddyl.ResidualModelFramePlacement(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'), pinocchio.SE3.Random()))
class TalosArmIntegratedRK4Test(ActionModelAbstractTestCase): ROBOT_MODEL = example_robot_data.load('talos_arm').model STATE = crocoddyl.StateMultibody(ROBOT_MODEL) ACTUATION = crocoddyl.ActuationModelFull(STATE) COST_SUM = crocoddyl.CostModelSum(STATE) COST_SUM.addCost( 'gripperPose', crocoddyl.CostModelResidual( STATE, crocoddyl.ResidualModelFramePlacement( STATE, ROBOT_MODEL.getFrameId("gripper_left_joint"), pinocchio.SE3.Random())), 1e-3) COST_SUM.addCost( "xReg", crocoddyl.CostModelResidual(STATE, crocoddyl.ResidualModelState(STATE)), 1e-7) COST_SUM.addCost( "uReg", crocoddyl.CostModelResidual(STATE, crocoddyl.ResidualModelControl(STATE)), 1e-7) DIFFERENTIAL = crocoddyl.DifferentialActionModelFreeFwdDynamics( STATE, ACTUATION, COST_SUM) MODEL = crocoddyl.IntegratedActionModelRK(DIFFERENTIAL, crocoddyl.RKType.four, 1e-3) MODEL_DER = IntegratedActionModelRK4Derived(DIFFERENTIAL, 1e-3)
class FrameRotationCostSumTest(CostModelSumTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) Rref = crocoddyl.FrameRotation(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.SE3.Random().rotation) COST = crocoddyl.CostModelFrameRotation(ROBOT_STATE, Rref)
class FrameTranslationCostSumTest(CostModelSumTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.utils.rand(3)) COST = crocoddyl.CostModelFrameTranslation(ROBOT_STATE, xref)
class CoMPositionCostTest(CostModelAbstractTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) cref = pinocchio.utils.rand(3) COST = crocoddyl.CostModelCoMPosition(ROBOT_STATE, cref) COST_DER = CoMPositionCostModelDerived(ROBOT_STATE, cref=cref)
class FrameVelocityCostSumTest(CostModelSumTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) vref = crocoddyl.FrameMotion(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.Motion.Random()) COST = crocoddyl.CostModelFrameVelocity(ROBOT_STATE, vref)
class Impulse6DTest(ImpulseModelAbstractTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) frame = ROBOT_MODEL.getFrameId('r_sole') IMPULSE = crocoddyl.ImpulseModel6D(ROBOT_STATE, frame) IMPULSE_DER = Impulse6DModelDerived(ROBOT_STATE, frame)
class FramePlacementCostTest(CostModelAbstractTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.SE3.Random()) COST = crocoddyl.CostModelFramePlacement(ROBOT_STATE, Mref) COST_DER = FramePlacementCostModelDerived(ROBOT_STATE, Mref=Mref)
class FrameRotationCostTest(CostModelAbstractTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) Rref = pinocchio.SE3.Random().rotation COST = crocoddyl.CostModelResidual( ROBOT_STATE, crocoddyl.ResidualModelFrameRotation(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'), Rref)) COST_DER = FrameRotationCostModelDerived(ROBOT_STATE, frame_id=ROBOT_MODEL.getFrameId('r_sole'), rotation=Rref)
class Impulse3DTest(ImpulseModelAbstractTestCase): ROBOT_MODEL = example_robot_data.load('hyq').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 = Impulse3DModelDerived(ROBOT_STATE, frame)
class Contact3DTest(ContactModelAbstractTestCase): ROBOT_MODEL = example_robot_data.load('hyq').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 = Contact3DModelDerived(ROBOT_STATE, xref, gains)
class Contact6DTest(ContactModelAbstractTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) gains = pinocchio.utils.rand(2) Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.SE3.Random()) CONTACT = crocoddyl.ContactModel6D(ROBOT_STATE, Mref, gains) CONTACT_DER = Contact6DModelDerived(ROBOT_STATE, Mref, gains)
class FrameVelocityCostTest(CostModelAbstractTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) vref = pinocchio.Motion.Random() COST = crocoddyl.CostModelResidual( ROBOT_STATE, crocoddyl.ResidualModelFrameVelocity(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'), vref, pinocchio.LOCAL)) COST_DER = FrameVelocityCostModelDerived(ROBOT_STATE, frame_id=ROBOT_MODEL.getFrameId('r_sole'), velocity=vref)
class Impulse6DMultipleTest(ImpulseModelMultipleAbstractTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) gains = pinocchio.utils.rand(2) IMPULSES = collections.OrderedDict( sorted({ 'l_sole': crocoddyl.ImpulseModel6D(ROBOT_STATE, ROBOT_MODEL.getFrameId('l_sole')), 'r_sole': crocoddyl.ImpulseModel6D(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole')) }.items()))
class FrameTranslationCostTest(CostModelAbstractTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) xref = pinocchio.utils.rand(3) COST = crocoddyl.CostModelResidual( ROBOT_STATE, crocoddyl.ResidualModelFrameTranslation(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'), xref)) COST_DER = FrameTranslationCostModelDerived(ROBOT_STATE, frame_id=ROBOT_MODEL.getFrameId('r_sole'), translation=xref)
class AnymalFreeFwdDynamicsTest(ActionModelAbstractTestCase): ROBOT_MODEL = example_robot_data.load('anymal').model STATE = crocoddyl.StateMultibody(ROBOT_MODEL) ACTUATION = crocoddyl.ActuationModelFloatingBase(STATE) COST_SUM = crocoddyl.CostModelSum(STATE, ACTUATION.nu) COST_SUM.addCost("xReg", crocoddyl.CostModelResidual(STATE, crocoddyl.ResidualModelState(STATE, ACTUATION.nu)), 1e-7) COST_SUM.addCost("uReg", crocoddyl.CostModelResidual(STATE, crocoddyl.ResidualModelControl(STATE, ACTUATION.nu)), 1e-7) MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, ACTUATION, COST_SUM) MODEL_DER = DifferentialFreeFwdDynamicsModelDerived(STATE, ACTUATION, COST_SUM)
class Contact6DMultipleTest(ContactModelMultipleAbstractTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) gains = pinocchio.utils.rand(2) CONTACTS = collections.OrderedDict( sorted({ 'l_foot': crocoddyl.ContactModel6D( ROBOT_STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('l_sole'), pinocchio.SE3.Random()), gains), 'r_foot': crocoddyl.ContactModel6D( ROBOT_STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.SE3.Random()), gains) }.items()))
class TalosArmFDDPTest(SolverAbstractTestCase): ROBOT_MODEL = example_robot_data.load('talos_arm').model STATE = crocoddyl.StateMultibody(ROBOT_MODEL) ACTUATION = crocoddyl.ActuationModelFull(STATE) COST_SUM = crocoddyl.CostModelSum(STATE) COST_SUM.addCost( 'gripperPose', crocoddyl.CostModelFramePlacement( STATE, crocoddyl.FramePlacement( ROBOT_MODEL.getFrameId("gripper_left_joint"), pinocchio.SE3.Random())), 1e-3) COST_SUM.addCost("xReg", crocoddyl.CostModelState(STATE), 1e-7) COST_SUM.addCost("uReg", crocoddyl.CostModelControl(STATE), 1e-7) DIFF_MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics( STATE, ACTUATION, COST_SUM) MODEL = crocoddyl.IntegratedActionModelEuler(DIFF_MODEL, 1e-3) SOLVER = crocoddyl.SolverFDDP SOLVER_DER = FDDPDerived
def createRobotWithObstacles(robotname='ur5'): ### Robot # Load the robot robot = robex.load(robotname) ### Obstacle map # Capsule obstacles will be placed at these XYZ-RPY parameters oMobs = [[0.40, 0., 0.30, np.pi / 2, 0, 0], [-0.08, -0., 0.69, np.pi / 2, 0, 0], [0.23, -0., 0.04, np.pi / 2, 0, 0], [-0.32, 0., -0.08, np.pi / 2, 0, 0]] # Load visual objects and add them in collision/visual models color = [1.0, 0.2, 0.2, 1.0] # color of the capsules rad, length = .1, 0.4 # radius and length of capsules for i, xyzrpy in enumerate(oMobs): obs = pin.GeometryObject.CreateCapsule( rad, length) # Pinocchio obstacle object obs.meshColor = np.array( [1.0, 0.2, 0.2, 1.0]) # Don't forget me, otherwise I am transparent ... obs.name = "obs%d" % i # Set object name obs.parentJoint = 0 # Set object parent = 0 = universe obs.placement = XYZRPYtoSE3(xyzrpy) # Set object placement wrt parent robot.collision_model.addGeometryObject( obs) # Add object to collision model robot.visual_model.addGeometryObject(obs) # Add object to visual model ### Collision pairs nobs = len(oMobs) nbodies = robot.collision_model.ngeoms - nobs robotBodies = range(nbodies) envBodies = range(nbodies, nbodies + nobs) for a, b in itertools.product(robotBodies, envBodies): robot.collision_model.addCollisionPair(pin.CollisionPair(a, b)) ### Geom data # Collision/visual models have been modified => re-generate corresponding data. robot.collision_data = pin.GeometryData(robot.collision_model) robot.visual_data = pin.GeometryData(robot.visual_model) return robot
class Contact3DMultipleTest(ContactModelMultipleAbstractTestCase): ROBOT_MODEL = example_robot_data.load('hyq').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()))
def init_robot(q_init, enable_viewer): """Load the solo model and initialize the Gepetto viewer if it is enabled Args: q_init (array): the default position of the robot actuators enable_viewer (bool): if the Gepetto viewer is enabled or not """ # Load robot model and data # Initialisation of the Gepetto viewer print(enable_viewer) solo = load('solo12', display=enable_viewer) q = solo.q0.reshape((-1, 1)) q[7:, 0] = q_init """if enable_viewer: solo.initViewer(loadModel=True) if ('viewer' in solo.viz.__dict__): solo.viewer.gui.addFloor('world/floor') solo.viewer.gui.setRefreshIsSynchronous(False)""" if enable_viewer: solo.display(q) print("PASS") # Initialisation of model quantities pin.centerOfMass(solo.model, solo.data, q, np.zeros((18, 1))) pin.updateFramePlacements(solo.model, solo.data) pin.crba(solo.model, solo.data, solo.q0) # Initialisation of the position of footsteps fsteps_init = np.zeros((3, 4)) indexes = [10, 18, 26, 34] for i in range(4): fsteps_init[:, i] = solo.data.oMf[indexes[i]].translation h_init = (solo.data.oMf[1].translation - solo.data.oMf[indexes[0]].translation)[2] fsteps_init[2, :] = 0.0 return solo, fsteps_init, h_init
[o_R_i @ i_omg_oi for o_R_i, i_omg_oi in zip(o_R_i_arr, i_omg_oi_arr)]) o_domg_i_arr = signal.savgol_filter(o_omg_i_arr, window_length=25, polyorder=2, deriv=1, delta=dt, mode='mirror', axis=0) i_domg_i_arr = np.array( [o_R_i.T @ o_domg_i for o_R_i, o_domg_i in zip(o_R_i_arr, o_domg_i_arr)]) rpy_arr = np.array([pin.rpy.matrixToRpy(o_R_i) for o_R_i in o_R_i_arr]) # Now compute the forces using the robot model robot = load('solo12') robot.model.gravity.linear = np.array([0, 0, -9.806]) LEGS = ['FL', 'FR', 'HL', 'HR'] nb_feet = len(LEGS) contact_frame_names = [leg + '_ANKLE' for leg in LEGS] contact_frame_ids = [ robot.model.getFrameId(leg_name) for leg_name in contact_frame_names ] cforces_est = ContactForcesEstimator(robot, contact_frame_ids, dt) o_forces_arr = np.zeros((N, 12)) l_forces_arr = np.zeros((N, 12)) o_forces_sum = np.zeros((N, 3)) detect_arr = np.zeros((N, 4)) for i in range(N):
import os import sys import numpy as np import crocoddyl import example_robot_data import pinocchio from crocoddyl.utils.biped import SimpleBipedGaitProblem, plotSolution 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,
class ControlCostSumTest(CostModelSumTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) COST = crocoddyl.CostModelControl(ROBOT_STATE)
class ControlCostTest(CostModelAbstractTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) COST = crocoddyl.CostModelControl(ROBOT_STATE) COST_DER = ControlCostModelDerived(ROBOT_STATE)
import os import sys import crocoddyl import pinocchio import numpy as np import example_robot_data WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ hector = example_robot_data.load('hector') robot_model = hector.model target_pos = np.array([1., 0., 1.]) target_quat = pinocchio.Quaternion(1., 0., 0., 0.) state = crocoddyl.StateMultibody(robot_model) d_cog, cf, cm, u_lim, l_lim = 0.1525, 6.6e-5, 1e-6, 5., 0.1 tau_f = np.array([[0., 0., 0., 0.], [0., 0., 0., 0.], [1., 1., 1., 1.], [0., d_cog, 0., -d_cog], [-d_cog, 0., d_cog, 0.], [-cm / cf, cm / cf, -cm / cf, cm / cf]]) actuation = crocoddyl.ActuationModelMultiCopterBase(state, tau_f) nu = actuation.nu runningCostModel = crocoddyl.CostModelSum(state, nu) terminalCostModel = crocoddyl.CostModelSum(state, nu) # Costs xResidual = crocoddyl.ResidualModelState(state, state.zero(), nu) xActivation = crocoddyl.ActivationModelWeightedQuad(np.array([0.1] * 3 + [1000.] * 3 + [1000.] * robot_model.nv))
import os import sys import crocoddyl from crocoddyl.utils.biped import plotSolution import numpy as np import example_robot_data import pinocchio WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ # Load robot robot = example_robot_data.load('talos') rmodel = robot.model lims = rmodel.effortLimit # lims[19:] *= 0.5 # reduced artificially the torque limits rmodel.effortLimit = lims # Create data structures rdata = rmodel.createData() state = crocoddyl.StateMultibody(rmodel) actuation = crocoddyl.ActuationModelFloatingBase(state) # Set integration time DT = 5e-2 T = 60 target = np.array([0.4, 0, 1.2]) # Initialize reference state, target and reference CoM rightFoot = 'right_sole_link'