def createPseudoImpulseModel(self, supportFootIds, swingFootTask): """ Action model for pseudo-impulse models. A pseudo-impulse model consists of adding high-penalty cost for the contact velocities. :param supportFootIds: Ids of the constrained feet :param swingFootTask: swinging foot task :return pseudo-impulse differential action model """ # Creating a 3D multi-contact model, and then including the supporting # foot contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: xref = crocoddyl.FrameTranslation(i, np.array([0., 0., 0.])) supportContactModel = crocoddyl.ContactModel3D( self.state, xref, self.actuation.nu, np.array([0., 50.])) contactModel.addContact(self.rmodel.frames[i].name + "_contact", supportContactModel) # Creating the cost model for a contact phase costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu) for i in supportFootIds: cone = crocoddyl.FrictionCone(self.nsurf, self.mu, 4, False) frictionCone = crocoddyl.CostModelContactFrictionCone( self.state, crocoddyl.ActivationModelQuadraticBarrier( crocoddyl.ActivationBounds(cone.lb, cone.ub)), crocoddyl.FrameFrictionCone(i, cone), self.actuation.nu) costModel.addCost(self.rmodel.frames[i].name + "_frictionCone", frictionCone, 1e1) if swingFootTask is not None: for i in swingFootTask: xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation) vref = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero()) footTrack = crocoddyl.CostModelFrameTranslation( self.state, xref, self.actuation.nu) impulseFootVelCost = crocoddyl.CostModelFrameVelocity( self.state, vref, self.actuation.nu) costModel.addCost( self.rmodel.frames[i.frame].name + "_footTrack", footTrack, 1e7) costModel.addCost( self.rmodel.frames[i.frame].name + "_impulseVel", impulseFootVelCost, 1e6) stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * self.rmodel.nv) stateReg = crocoddyl.CostModelState( self.state, crocoddyl.ActivationModelWeightedQuad(stateWeights**2), self.rmodel.defaultState, self.actuation.nu) ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu) costModel.addCost("stateReg", stateReg, 1e1) costModel.addCost("ctrlReg", ctrlReg, 1e-3) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics( self.state, self.actuation, contactModel, costModel, 0., True) model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.) return model
def createSwingFootModel(self, timeStep, supportFootIds, comTask=None, swingFootTask=None): """ Action model for a swing foot phase. :param timeStep: step duration of the action model :param supportFootIds: Ids of the constrained feet :param comTask: CoM task :param swingFootTask: swinging foot task :return action model for a swing foot phase """ # Creating a 3D multi-contact model, and then including the supporting # foot contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: xref = crocoddyl.FrameTranslation(i, np.matrix([0., 0., 0.]).T) supportContactModel = crocoddyl.ContactModel3D(self.state, xref, self.actuation.nu, np.matrix([0., 50.]).T) contactModel.addContact(self.rmodel.frames[i].name + "_contact", supportContactModel) # Creating the cost model for a contact phase costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu) if isinstance(comTask, np.ndarray): comTrack = crocoddyl.CostModelCoMPosition(self.state, comTask, self.actuation.nu) costModel.addCost("comTrack", comTrack, 1e6) for i in supportFootIds: cone = crocoddyl.FrictionCone(self.nsurf, self.mu, 4, False) frictionCone = crocoddyl.CostModelContactFrictionCone( self.state, crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(cone.lb, cone.ub)), cone, i, self.actuation.nu) costModel.addCost(self.rmodel.frames[i].name + "_frictionCone", frictionCone, 1e1) if swingFootTask is not None: for i in swingFootTask: xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation) footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, self.actuation.nu) costModel.addCost(self.rmodel.frames[i.frame].name + "_footTrack", footTrack, 1e6) stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * 6 + [1.] * (self.rmodel.nv - 6)) stateReg = crocoddyl.CostModelState(self.state, crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeights**2).T), self.rmodel.defaultState, self.actuation.nu) ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu) costModel.addCost("stateReg", stateReg, 1e1) costModel.addCost("ctrlReg", ctrlReg, 1e-1) lb = self.state.diff(0 * self.state.lb, self.state.lb) ub = self.state.diff(0 * self.state.ub, self.state.ub) stateBounds = crocoddyl.CostModelState( self.state, crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(lb, ub)), 0 * self.rmodel.defaultState, self.actuation.nu) costModel.addCost("stateBounds", stateBounds, 1e3) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(self.state, self.actuation, contactModel, costModel, 0., True) model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep) return model
def createSwingFootModel(self, timeStep, supportFootIds, comTask=None, swingFootTask=None): """ Action model for a swing foot phase. :param timeStep: step duration of the action model :param supportFootIds: Ids of the constrained feet :param comTask: CoM task :param swingFootTask: swinging foot task :return action model for a swing foot phase """ # Creating a 3D multi-contact model, and then including the supporting # foot contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: xref = crocoddyl.FrameTranslation(i, np.matrix([0., 0., 0.]).T) supportContactModel = crocoddyl.ContactModel3D( self.state, xref, self.actuation.nu, np.matrix([0., 0.]).T) contactModel.addContact('contact_' + str(i), supportContactModel) # Creating the cost model for a contact phase costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu) if isinstance(comTask, np.ndarray): comTrack = crocoddyl.CostModelCoMPosition(self.state, comTask, self.actuation.nu) costModel.addCost("comTrack", comTrack, 1e4) if swingFootTask is not None: for i in swingFootTask: xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation) footTrack = crocoddyl.CostModelFrameTranslation( self.state, xref, self.actuation.nu) costModel.addCost("footTrack_" + str(i), footTrack, 1e4) stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10] * self.rmodel.nv) stateReg = crocoddyl.CostModelState( self.state, crocoddyl.ActivationModelWeightedQuad( np.matrix(stateWeights**2).T), self.rmodel.defaultState, self.actuation.nu) ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu) costModel.addCost("stateReg", stateReg, 1e-1) costModel.addCost("ctrlReg", ctrlReg, 1e-4) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics( self.state, self.actuation, contactModel, costModel) model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep) return model
def createFootSwitchModel(self, supportFootIds, swingFootTask): """ Action model for a foot switch phase. :param supportFootIds: Ids of the constrained feet :param swingFootTask: swinging foot task :return action model for a foot switch phase """ # Creating a 3D multi-contact model, and then including the supporting # foot contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: xref = crocoddyl.FrameTranslation(i, np.matrix([0., 0., 0.]).T) supportContactModel = crocoddyl.ContactModel3D( self.state, xref, self.actuation.nu, np.matrix([0., 0.]).T) contactModel.addContact('contact_' + str(i), supportContactModel) # Creating the cost model for a contact phase costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu) if swingFootTask is not None: for i in swingFootTask: xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation) footTrack = crocoddyl.CostModelFrameTranslation( self.state, xref, self.actuation.nu) costModel.addCost("footTrack_" + str(i), footTrack, 1e7) stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10] * self.rmodel.nv) stateReg = crocoddyl.CostModelState( self.state, crocoddyl.ActivationModelWeightedQuad( np.matrix(stateWeights**2).T), self.rmodel.defaultState, self.actuation.nu) ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu) costModel.addCost("stateReg", stateReg, 1e1) costModel.addCost("ctrlReg", ctrlReg, 1e-3) for i in swingFootTask: vref = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero()) impactFootVelCost = crocoddyl.CostModelFrameVelocity( self.state, vref, self.actuation.nu) costModel.addCost('impactVel_' + str(i.frame), impactFootVelCost, 1e6) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics( self.state, self.actuation, contactModel, costModel) model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.) return model
def createImpulseModel(self, supportFootIds, swingFootTask, JMinvJt_damping=1e-12, r_coeff=0.0): """ Action model for impulse models. An impulse model consists of describing the impulse dynamics against a set of contacts. :param supportFootIds: Ids of the constrained feet :param swingFootTask: swinging foot task :return impulse action model """ # Creating a 3D multi-contact model, and then including the supporting foot impulseModel = crocoddyl.ImpulseModelMultiple(self.state) for i in supportFootIds: supportContactModel = crocoddyl.ImpulseModel3D(self.state, i) impulseModel.addImpulse(self.rmodel.frames[i].name + "_impulse", supportContactModel) # Creating the cost model for a contact phase costModel = crocoddyl.CostModelSum(self.state, 0) if swingFootTask is not None: for i in swingFootTask: xref = crocoddyl.FrameTranslation(i.id, i.placement.translation) footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, 0) costModel.addCost(self.rmodel.frames[i.id].name + "_footTrack", footTrack, 1e7) stateWeights = np.array([1.] * 6 + [10.] * (self.rmodel.nv - 6) + [10.] * self.rmodel.nv) stateReg = crocoddyl.CostModelState(self.state, crocoddyl.ActivationModelWeightedQuad(stateWeights**2), self.rmodel.defaultState, 0) costModel.addCost("stateReg", stateReg, 1e1) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme model = crocoddyl.ActionModelImpulseFwdDynamics(self.state, impulseModel, costModel) model.JMinvJt_damping = JMinvJt_damping model.r_coeff = r_coeff return model
class FrameTranslationCostSumTest(CostModelSumTestCase): ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom() ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.utils.rand(3)) COST = crocoddyl.CostModelFrameTranslation(ROBOT_STATE, xref)
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 Contact3DMultipleTest(ContactModelMultipleAbstractTestCase): ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom() ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) gains = pinocchio.utils.rand(2) xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random().translation) CONTACT = crocoddyl.ContactModel3D(ROBOT_STATE, xref, gains)
class FrameTranslationCostTest(CostModelAbstractTestCase): ROBOT_MODEL = example_robot_data.loadICub().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) COST_DER = FrameTranslationCostModelDerived(ROBOT_STATE, xref=xref)
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 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()))
from test_utils import NUMDIFF_MODIFIER, assertNumDiff # Create robot model and data ROBOT_MODEL = example_robot_data.loadICub().model ROBOT_DATA = ROBOT_MODEL.createData() ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) # Contact force cost # Create differential action model and data; link the contact data ACTUATION = crocoddyl.ActuationModelFloatingBase(ROBOT_STATE) CONTACTS = crocoddyl.ContactModelMultiple(ROBOT_STATE, ACTUATION.nu) CONTACT_6D_1 = crocoddyl.ContactModel6D( ROBOT_STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.SE3.Random()), ACTUATION.nu, pinocchio.utils.rand(2)) CONTACT_3D_2 = crocoddyl.ContactModel3D( ROBOT_STATE, crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('l_sole'), pinocchio.SE3.Random().translation), ACTUATION.nu, pinocchio.utils.rand(2)) CONTACTS.addContact("r_sole_contact", CONTACT_6D_1) CONTACTS.addContact("l_sole_contact", CONTACT_3D_2) COSTS = crocoddyl.CostModelSum(ROBOT_STATE, ACTUATION.nu) COSTS.addCost( "force_6d", crocoddyl.CostModelContactForce(ROBOT_STATE, crocoddyl.FrameForce(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.Force.Random()), 6, ACTUATION.nu), 1.) COSTS.addCost( "force_3d", crocoddyl.CostModelContactForce(ROBOT_STATE, crocoddyl.FrameForce(ROBOT_MODEL.getFrameId('l_sole'), pinocchio.Force.Random()), 3, ACTUATION.nu), 1.) MODEL = crocoddyl.DifferentialActionModelContactFwdDynamics(ROBOT_STATE, ACTUATION, CONTACTS, COSTS, 0., True) DATA = MODEL.createData()
viewer.applyConfiguration('world/goal', [0.2, 0.5, .5, 0, 0, 0, 1]) viewer.refresh() # Create a cost model per the running and terminal action model. state = crocoddyl.StateMultibody(robot_model) runningCostModel = crocoddyl.CostModelSum(state) terminalCostModel = crocoddyl.CostModelSum(state) # Note that we need to include a cost model (i.e. set of cost functions) in # order to fully define the action model for our optimal control problem. # For this particular example, we formulate three running-cost functions: # goal-tracking cost, state and control regularization; and one terminal-cost: # goal cost. First, let's create the common cost functions. Mref = crocoddyl.FramePlacement(FRAME_TIP, pinocchio.SE3(np.eye(3), goal)) #goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref) pref = crocoddyl.FrameTranslation(FRAME_TIP, goal) goalTrackingCost = crocoddyl.CostModelFrameTranslation(state, pref) weights = crocoddyl.ActivationModelWeightedQuad( np.array([1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2.])) xRegCost = crocoddyl.CostModelState(state, weights, robot_model.x0) uRegCost = crocoddyl.CostModelControl(state) weightsT = crocoddyl.ActivationModelWeightedQuad( np.array([.01, .01, .01, .01, .01, .01, .01, 1, 1, 1, 1, 2, 2, 2.])) xRegCostT = crocoddyl.CostModelState(state, weights, robot_model.x0) # Then let's added the running and terminal cost functions runningCostModel.addCost("gripperPose", goalTrackingCost, .001) runningCostModel.addCost("xReg", xRegCost, 1e-3) runningCostModel.addCost("uReg", uRegCost, 1e-6) terminalCostModel.addCost("gripperPose", goalTrackingCost, 10) terminalCostModel.addCost("xReg", xRegCostT, .01)
# Setting the final position goal with variable angle # angle = np.pi/2 # s = np.sin(angle) # c = np.cos(angle) # R = np.matrix([ [c, 0, s], # [0, 1, 0], # [-s, 0, c] # ]) # target = np.array([np.sin(angle), 0, np.cos(angle)])) runningCostModel = crocoddyl.CostModelSum(state, actuation.nu) terminalCostModel = crocoddyl.CostModelSum(state, actuation.nu) target = np.array(conf.target) footName = 'foot' footFrameID = robot_model.getFrameId(footName) assert (robot_model.existFrame(footName)) Pref = crocoddyl.FrameTranslation(footFrameID, target) # If also the orientation is useful for the task use # Mref = crocoddyl.FramePlacement(footFrameID, # pinocchio.SE3(R, conf.n_links * np.matrix([[np.sin(angle)], [0], [np.cos(angle)]]))) footTrackingCost = crocoddyl.CostModelFrameTranslation(state, Pref, actuation.nu) Vref = crocoddyl.FrameMotion(footFrameID, pinocchio.Motion(np.zeros(6))) footFinalVelocity = crocoddyl.CostModelFrameVelocity(state, Vref, actuation.nu) # simulating the cost on the power with a cost on the control power_act = crocoddyl.ActivationModelQuad(conf.n_links) u2 = crocoddyl.CostModelControl( state, power_act, actuation.nu) # joule dissipation cost without friction, for benchmarking stateAct = crocoddyl.ActivationModelWeightedQuad( np.concatenate([np.zeros(state.nq + 1), np.ones(state.nv - 1)]))
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 WITHDISPLAY = True robot = example_robot_data.load('tiago_no_hand_head_fixed') #robot = example_robot_data.loadTiago(True,'wsg') robot_model = robot.model DT = 1e-3 T = 250 target = np.array([3.8, 0.1, 1.1]) # Create the cost functions Mref = crocoddyl.FrameTranslation(robot_model.getFrameId("wrist_tool_joint"), target) state = crocoddyl.StateMultibody(robot.model) goalTrackingCost = crocoddyl.CostModelFrameTranslation(state, Mref) xRegCost = crocoddyl.CostModelState(state) uRegCost = crocoddyl.CostModelControl(state) # Create cost model per each action model runningCostModel = crocoddyl.CostModelSum(state) terminalCostModel = crocoddyl.CostModelSum(state) # Then let's added the running and terminal cost functions runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2) runningCostModel.addCost("stateReg", xRegCost, 1e-4) runningCostModel.addCost("ctrlReg", uRegCost, 1e-7) terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e5) terminalCostModel.addCost("stateReg", xRegCost, 1e-4)
# Create robot model and data ROBOT_MODEL = example_robot_data.loadICub().model ROBOT_DATA = ROBOT_MODEL.createData() # Create differential action model and data; link the contact data ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) ACTUATION = crocoddyl.ActuationModelFloatingBase(ROBOT_STATE) CONTACTS = crocoddyl.ContactModelMultiple(ROBOT_STATE, ACTUATION.nu) CONTACT_6D = crocoddyl.ContactModel6D( ROBOT_STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.SE3.Random()), ACTUATION.nu, pinocchio.utils.rand(2)) CONTACT_3D = crocoddyl.ContactModel3D( ROBOT_STATE, crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('l_sole'), pinocchio.SE3.Random().translation), ACTUATION.nu, pinocchio.utils.rand(2)) CONTACTS.addContact("r_sole_contact", CONTACT_6D) CONTACTS.addContact("l_sole_contact", CONTACT_3D) COSTS = crocoddyl.CostModelSum(ROBOT_STATE, ACTUATION.nu, True) frictionCone = crocoddyl.FrictionCone(np.matrix([0., 0., 1.]).T, 0.7, 4, False) activation = crocoddyl.ActivationModelQuadraticBarrier( crocoddyl.ActivationBounds(frictionCone.lb, frictionCone.ub)) COSTS.addCost( "r_sole_friction_cone", crocoddyl.CostModelContactFrictionCone(ROBOT_STATE, activation, frictionCone, ROBOT_MODEL.getFrameId('r_sole'), ACTUATION.nu), 1.) COSTS.addCost(
gv.addSphere('world/point%d' % i, .05, colors[i]) gv.applyConfiguration('world/point%d' % i, p.tolist() + [0., 0., 0., 1.]) gv.refresh() # State and control regularization costs xRegCost = crocoddyl.CostModelState(state) uRegCost = crocoddyl.CostModelControl(state) # Then let's added the running and terminal cost functions per each action # model runningModels = [] terminalModels = [] for p in ps: # Create the tracking cost Mref = crocoddyl.FrameTranslation( robot_model.getFrameId("gripper_left_joint"), np.matrix(p).T) goalTrackingCost = crocoddyl.CostModelFrameTranslation(state, Mref) actuation = crocoddyl.ActuationModelFull(state) # Create the running action model runningCostModel = crocoddyl.CostModelSum(state) runningCostModel.addCost("gripperPose", goalTrackingCost, 1.) runningCostModel.addCost("xReg", xRegCost, 1e-4) runningCostModel.addCost("uReg", uRegCost, 1e-7) runningModels += [ crocoddyl.DifferentialActionModelFreeFwdDynamics( state, actuation, runningCostModel) ]