def createJumpingProblem(self, x0, jumpHeight, jumpLength, timeStep, groundKnots, flyingKnots): q0 = x0[:self.rmodel.nq] pinocchio.forwardKinematics(self.rmodel, self.rdata, q0) pinocchio.updateFramePlacements(self.rmodel, self.rdata) rfFootPos0 = self.rdata.oMf[self.rfFootId].translation rhFootPos0 = self.rdata.oMf[self.rhFootId].translation lfFootPos0 = self.rdata.oMf[self.lfFootId].translation lhFootPos0 = self.rdata.oMf[self.lhFootId].translation df = jumpLength[2] - rfFootPos0[2] rfFootPos0[2] = 0. rhFootPos0[2] = 0. lfFootPos0[2] = 0. lhFootPos0[2] = 0. comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4 comRef[2] = np.asscalar(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2]) loco3dModel = [] takeOff = [ self.createSwingFootModel( timeStep, [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId], ) for k in range(groundKnots) ] flyingUpPhase = [ self.createSwingFootModel( timeStep, [], np.array([jumpLength[0], jumpLength[1], jumpLength[2] + jumpHeight]) * (k + 1) / flyingKnots + comRef) for k in range(flyingKnots) ] flyingDownPhase = [] for k in range(flyingKnots): flyingDownPhase += [self.createSwingFootModel(timeStep, [])] f0 = jumpLength footTask = [ crocoddyl.FramePlacement(self.lfFootId, pinocchio.SE3(np.eye(3), lfFootPos0 + f0)), crocoddyl.FramePlacement(self.rfFootId, pinocchio.SE3(np.eye(3), rfFootPos0 + f0)), crocoddyl.FramePlacement(self.lhFootId, pinocchio.SE3(np.eye(3), lhFootPos0 + f0)), crocoddyl.FramePlacement(self.rhFootId, pinocchio.SE3(np.eye(3), rhFootPos0 + f0)) ] landingPhase = [ self.createFootSwitchModel([self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId], footTask, False) ] f0[2] = df landed = [ self.createSwingFootModel(timeStep, [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId], comTask=comRef + f0) for k in range(groundKnots) ] loco3dModel += takeOff loco3dModel += flyingUpPhase loco3dModel += flyingDownPhase loco3dModel += landingPhase loco3dModel += landed problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1]) return problem
class FramePlacementCostSumTest(CostModelSumTestCase): 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)
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 swingFootTask: swinging foot task :return pseudo-impulse differential action model """ # Creating a 6D multi-contact model, and then including the supporting # foot contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: Mref = crocoddyl.FramePlacement(i, pinocchio.SE3.Identity()) supportContactModel = crocoddyl.ContactModel6D( self.state, Mref, self.actuation.nu, np.array([0., 0.])) 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.WrenchCone(np.identity(3), self.mu, np.array([0.1, 0.05])) wrenchCone = crocoddyl.CostModelContactWrenchCone( self.state, crocoddyl.ActivationModelQuadraticBarrier( crocoddyl.ActivationBounds(cone.lb, cone.ub)), crocoddyl.FrameWrenchCone(i, cone), self.actuation.nu) costModel.addCost(self.rmodel.frames[i].name + "_wrenchCone", wrenchCone, 1e1) if swingFootTask is not None: for i in swingFootTask: footTrack = crocoddyl.CostModelFramePlacement( self.state, i, self.actuation.nu) costModel.addCost(self.rmodel.frames[i.id].name + "_footTrack", footTrack, 1e8) footVel = crocoddyl.FrameMotion(i.id, pinocchio.Motion.Zero()) impulseFootVelCost = crocoddyl.CostModelFrameVelocity( self.state, footVel, self.actuation.nu) costModel.addCost( self.rmodel.frames[i.id].name + "_impulseVel", impulseFootVelCost, 1e6) stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.state.nv - 6) + [10] * self.state.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
class FramePlacementCostSumTest(CostModelSumTestCase): ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom() ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random()) COST = crocoddyl.CostModelFramePlacement(ROBOT_STATE, Mref)
class Contact6DMultipleTest(ContactModelMultipleAbstractTestCase): ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom() ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) gains = pinocchio.utils.rand(2) Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random()) CONTACT = crocoddyl.ContactModel6D(ROBOT_STATE, Mref, gains)
class FramePlacementCostTest(CostModelAbstractTestCase): ROBOT_MODEL = example_robot_data.loadICub().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)
def runBenchmark(model): robot_model = ROBOT.model q0 = np.matrix([0.173046, 1., -0.52366, 0., 0., 0.1, -0.005]).T x0 = np.vstack([q0, np.zeros((robot_model.nv, 1))]) # 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. state = crocoddyl.StateMultibody(robot_model) Mref = crocoddyl.FramePlacement( robot_model.getFrameId("gripper_left_joint"), pinocchio.SE3(np.eye(3), np.matrix([[.0], [.0], [.4]]))) goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref) xRegCost = crocoddyl.CostModelState(state) uRegCost = crocoddyl.CostModelControl(state) # Create a cost model per the running and terminal action model. runningCostModel = crocoddyl.CostModelSum(state) terminalCostModel = crocoddyl.CostModelSum(state) # Then let's added the running and terminal cost functions runningCostModel.addCost("gripperPose", goalTrackingCost, 1e-3) runningCostModel.addCost("xReg", xRegCost, 1e-7) runningCostModel.addCost("uReg", uRegCost, 1e-7) terminalCostModel.addCost("gripperPose", goalTrackingCost, 1) # Next, we need to create an action model for running and terminal knots. The # forward dynamics (computed using ABA) are implemented # inside DifferentialActionModelFullyActuated. runningModel = crocoddyl.IntegratedActionModelEuler( model(state, runningCostModel), 1e-3) runningModel.differential.armature = np.matrix( [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T terminalModel = crocoddyl.IntegratedActionModelEuler( model(state, terminalCostModel), 1e-3) terminalModel.differential.armature = np.matrix( [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T # For this optimal control problem, we define 100 knots (or running action # models) plus a terminal knot problem = crocoddyl.ShootingProblem(x0, [runningModel] * N, terminalModel) # Creating the DDP solver for this OC problem, defining a logger ddp = crocoddyl.SolverDDP(problem) duration = [] for i in range(T): c_start = time.time() ddp.solve([], [], MAXITER) 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
class Contact6DMultipleTest(ContactModelMultipleAbstractTestCase): ROBOT_MODEL = example_robot_data.loadICub().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 Contact6DTest(ContactModelAbstractTestCase): ROBOT_MODEL = example_robot_data.loadICub().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 = Contact6DDerived(ROBOT_STATE, Mref, gains)
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 6D multi-contact model, and then including the supporting # foot contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: Mref = crocoddyl.FramePlacement(i, pinocchio.SE3.Identity()) supportContactModel = \ crocoddyl.ContactModel6D(self.state, Mref, self.actuation.nu, np.matrix([0., 0.]).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) if swingFootTask is not None: for i in swingFootTask: footTrack = crocoddyl.CostModelFramePlacement( self.state, i, self.actuation.nu) costModel.addCost( self.rmodel.frames[i.frame].name + "_footTrack", footTrack, 1e6) stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.state.nv - 6) + [10] * self.state.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-1) # 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 createFootstepModels(self, comPos0, feetPos0, stepLength, stepHeight, timeStep, numKnots, supportFootIds, swingFootIds): """ Action models for a footstep phase. :param comPos0, initial CoM position :param feetPos0: initial position of the swinging feet :param stepLength: step length :param stepHeight: step height :param timeStep: time step :param numKnots: number of knots for the footstep phase :param supportFootIds: Ids of the supporting feet :param swingFootIds: Ids of the swinging foot :return footstep action models """ numLegs = len(supportFootIds) + len(swingFootIds) comPercentage = float(len(swingFootIds)) / numLegs # Action models for the foot swing footSwingModel = [] for k in range(numKnots): swingFootTask = [] for i, p in zip(swingFootIds, feetPos0): # Defining a foot swing task given the step length. The swing task # is decomposed on two phases: swing-up and swing-down. We decide # deliveratively to allocated the same number of nodes (i.e. phKnots) # in each phase. With this, we define a proper z-component for the # swing-leg motion. phKnots = numKnots / 2 if k < phKnots: dp = np.matrix([[stepLength * (k + 1) / numKnots, 0., stepHeight * k / phKnots]]).T elif k == phKnots: dp = np.matrix([[stepLength * (k + 1) / numKnots, 0., stepHeight]]).T else: dp = np.matrix( [[stepLength * (k + 1) / numKnots, 0., stepHeight * (1 - float(k - phKnots) / phKnots)]]).T tref = np.asmatrix(p + dp) swingFootTask += [crocoddyl.FramePlacement(i, pinocchio.SE3(np.eye(3), tref))] comTask = np.matrix([stepLength * (k + 1) / numKnots, 0., 0.]).T * comPercentage + comPos0 footSwingModel += [ self.createSwingFootModel(timeStep, supportFootIds, comTask=comTask, swingFootTask=swingFootTask) ] # Action model for the foot switch footSwitchModel = \ self.createFootSwitchModel(supportFootIds, swingFootTask) # Updating the current foot position for next step comPos0 += np.matrix([stepLength * comPercentage, 0., 0.]).T for p in feetPos0: p += np.matrix([[stepLength, 0., 0.]]).T return footSwingModel + [footSwitchModel]
class FreeFwdDynamicsTest(ActionModelAbstractTestCase): ROBOT_MODEL = pinocchio.buildSampleModelManipulator() STATE = crocoddyl.StateMultibody(ROBOT_MODEL) COST_SUM = crocoddyl.CostModelSum(STATE, ROBOT_MODEL.nv) COST_SUM.addCost('xReg', crocoddyl.CostModelState(STATE), 1.) COST_SUM.addCost( 'frTrack', crocoddyl.CostModelFramePlacement( STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId("effector_body"), pinocchio.SE3.Random())), 1.) MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM) MODEL_DER = DifferentialFreeFwdDynamicsDerived(STATE, COST_SUM)
def createProblem(model): robot_model = ROBOT.model q0 = np.matrix([0.173046, 1., -0.52366, 0., 0., 0.1, -0.005]).T x0 = np.vstack([q0, np.zeros((robot_model.nv, 1))]) # 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. state = crocoddyl.StateMultibody(robot_model) Mref = crocoddyl.FramePlacement( robot_model.getFrameId("gripper_left_joint"), pinocchio.SE3(np.eye(3), np.matrix([[.0], [.0], [.4]]))) goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref) xRegCost = crocoddyl.CostModelState(state) uRegCost = crocoddyl.CostModelControl(state) # Create a cost model per the running and terminal action model. runningCostModel = crocoddyl.CostModelSum(state) terminalCostModel = crocoddyl.CostModelSum(state) # Then let's added the running and terminal cost functions runningCostModel.addCost("gripperPose", goalTrackingCost, 1) runningCostModel.addCost("xReg", xRegCost, 1e-4) runningCostModel.addCost("uReg", uRegCost, 1e-4) terminalCostModel.addCost("gripperPose", goalTrackingCost, 1) # Next, we need to create an action model for running and terminal knots. The # forward dynamics (computed using ABA) are implemented # inside DifferentialActionModelFullyActuated. actuation = crocoddyl.ActuationModelFull(state) runningModel = crocoddyl.IntegratedActionModelEuler( model(state, actuation, runningCostModel), 1e-3) runningModel.differential.armature = np.matrix( [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T terminalModel = crocoddyl.IntegratedActionModelEuler( model(state, actuation, terminalCostModel), 1e-3) terminalModel.differential.armature = np.matrix( [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T # For this optimal control problem, we define 100 knots (or running action # models) plus a terminal knot problem = crocoddyl.ShootingProblem(x0, [runningModel] * N, terminalModel) xs = [x0] * (len(problem.runningModels) + 1) us = [ m.quasiStatic(d, x0) for m, d in list(zip(problem.runningModels, problem.runningDatas)) ] return xs, us, problem
class TalosArmFreeFwdDynamicsTest(ActionModelAbstractTestCase): ROBOT_MODEL = example_robot_data.loadTalosArm().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) MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, ACTUATION, COST_SUM) MODEL_DER = DifferentialFreeFwdDynamicsDerived(STATE, ACTUATION, COST_SUM)
def createFootstepModels(self, comPos0, feetPos0, stepLength, stepHeight, timeStep, numKnots, supportFootIds, swingFootIds): """ Action models for a footstep phase. :param comPos0, initial CoM position :param feetPos0: initial position of the swinging feet :param stepLength: step length :param stepHeight: step height :param timeStep: time step :param numKnots: number of knots for the footstep phase :param supportFootIds: Ids of the supporting feet :param swingFootIds: Ids of the swinging foot :return footstep action models """ numLegs = len(supportFootIds) + len(swingFootIds) comPercentage = float(len(swingFootIds)) / numLegs # Action models for the foot swing footSwingModel = [] for k in range(numKnots): swingFootTask = [] for i, p in zip(swingFootIds, feetPos0): # Defining a foot swing task given the step length # resKnot = numKnots % 2 phKnots = numKnots / 2 if k < phKnots: dp = np.array([stepLength * (k + 1) / numKnots, 0., stepHeight * k / phKnots]) elif k == phKnots: dp = np.array([stepLength * (k + 1) / numKnots, 0., stepHeight]) else: dp = np.array( [stepLength * (k + 1) / numKnots, 0., stepHeight * (1 - float(k - phKnots) / phKnots)]) tref = p + dp swingFootTask += [crocoddyl.FramePlacement(i, pinocchio.SE3(np.eye(3), tref))] comTask = np.array([stepLength * (k + 1) / numKnots, 0., 0.]) * comPercentage + comPos0 footSwingModel += [ self.createSwingFootModel(timeStep, supportFootIds, comTask=comTask, swingFootTask=swingFootTask) ] # Action model for the foot switch footSwitchModel = self.createFootSwitchModel(supportFootIds, swingFootTask) # Updating the current foot position for next step comPos0 += [stepLength * comPercentage, 0., 0.] for p in feetPos0: p += [stepLength, 0., 0.] return footSwingModel + [footSwitchModel]
class ManipulatorDDPTest(SolverAbstractTestCase): ROBOT_MODEL = pinocchio.buildSampleModelManipulator() STATE = crocoddyl.StateMultibody(ROBOT_MODEL) COST_SUM = crocoddyl.CostModelSum(STATE, ROBOT_MODEL.nv) COST_SUM.addCost('xReg', crocoddyl.CostModelState(STATE), 1e-7) COST_SUM.addCost('uReg', crocoddyl.CostModelControl(STATE), 1e-7) COST_SUM.addCost( 'frTrack', crocoddyl.CostModelFramePlacement( STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId("effector_body"), pinocchio.SE3.Random())), 1.) DIFF_MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM) MODEL = crocoddyl.IntegratedActionModelEuler(crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM), 1e-3) SOLVER = crocoddyl.SolverDDP SOLVER_DER = DDPDerived
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 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 6D multi-contact model, and then including the supporting # foot contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: Mref = crocoddyl.FramePlacement(i, pinocchio.SE3.Identity()) supportContactModel = crocoddyl.ContactModel6D(self.state, Mref, 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: footTrack = crocoddyl.CostModelFramePlacement(self.state, i, self.actuation.nu) costModel.addCost("footTrack_" + str(i), footTrack, 1e8) footVel = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero()) impactFootVelCost = crocoddyl.CostModelFrameVelocity(self.state, footVel, self.actuation.nu) costModel.addCost('impactVel_' + str(i.frame), impactFootVelCost, 1e6) stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.state.nv - 6) + [10] * self.state.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) # 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
# First, let's load the Pinocchio model for the Talos arm. talos_arm = example_robot_data.loadTalosArm() robot_model = talos_arm.model # 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( robot_model.getFrameId("gripper_left_joint"), pinocchio.SE3(np.eye(3), np.matrix([[.0], [.0], [.4]]))) goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref) xRegCost = crocoddyl.CostModelState(state) uRegCost = crocoddyl.CostModelControl(state) # Then let's added the running and terminal cost functions runningCostModel.addCost("gripperPose", goalTrackingCost, 1) runningCostModel.addCost("xReg", xRegCost, 1e-4) runningCostModel.addCost("uReg", uRegCost, 1e-4) terminalCostModel.addCost("gripperPose", goalTrackingCost, 1) # Next, we need to create an action model for running and terminal knots. The # forward dynamics (computed using ABA) are implemented # inside DifferentialActionModelFullyActuated. actuationModel = crocoddyl.ActuationModelFull(state)
state = crocoddyl.StateMultibody(robot_model) d_cog = 0.1525 cf = 6.6e-5 cm = 1e-6 u_lim = 5 l_lim = 0.1 tau_f = np.array([[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [1.0, 1.0, 1.0, 1.0], [0.0, d_cog, 0.0, -d_cog], [-d_cog, 0.0, d_cog, 0.0], [-cm / cf, cm / cf, -cm / cf, cm / cf]]) actModel = crocoddyl.ActuationModelMultiCopterBase(state, 4, tau_f) runningCostModel = crocoddyl.CostModelSum(state, actModel.nu) terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu) # Needed objects to create the costs Mref = crocoddyl.FramePlacement(robot_model.getFrameId("base_link"), pinocchio.SE3(target_quat.matrix(), target_pos)) wBasePos, wBaseOri, wBaseVel = 0.1, 1000, 1000 stateWeights = np.array([wBasePos] * 3 + [wBaseOri] * 3 + [wBaseVel] * robot_model.nv) # Costs goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref, actModel.nu) xRegCost = crocoddyl.CostModelState(state, crocoddyl.ActivationModelWeightedQuad(stateWeights), state.zero(), actModel.nu) uRegCost = crocoddyl.CostModelControl(state, actModel.nu) runningCostModel.addCost("xReg", xRegCost, 1e-6) runningCostModel.addCost("uReg", uRegCost, 1e-6) runningCostModel.addCost("trackPose", goalTrackingCost, 1e-2) terminalCostModel.addCost("goalPose", goalTrackingCost, 100) dt = 3e-2 runningModel = crocoddyl.IntegratedActionModelEuler(
#viewer.addSphere('world/goal',.1,[1.,0,0,1]) viewer.addXYZaxis('world/goal', [0, 1, 0, 1], .03, .1) 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)
comRef = (rfPos0 + lfPos0) / 2 comRef[2] = np.asscalar(pinocchio.centerOfMass(rmodel, rdata, q0)[2]) # Initialize Gepetto viewer if WITHDISPLAY: display = crocoddyl.GepettoDisplay(robot, frameNames=[rightFoot, leftFoot]) display.robot.viewer.gui.addSphere( 'world/point', .05, [1., 0., 0., 1.]) # radius = .1, RGBA=1001 display.robot.viewer.gui.applyConfiguration( 'world/point', target.tolist() + [0., 0., 0., 1.]) # xyz+quaternion # Create two contact models used along the motion contactModel1Foot = crocoddyl.ContactModelMultiple(state, actuation.nu) contactModel2Feet = crocoddyl.ContactModelMultiple(state, actuation.nu) framePlacementLeft = crocoddyl.FramePlacement(leftFootId, pinocchio.SE3.Identity()) framePlacementRight = crocoddyl.FramePlacement(rightFootId, pinocchio.SE3.Identity()) supportContactModelLeft = crocoddyl.ContactModel6D(state, framePlacementLeft, actuation.nu, np.array([0, 40])) supportContactModelRight = crocoddyl.ContactModel6D(state, framePlacementRight, actuation.nu, np.array([0, 40])) contactModel1Foot.addContact(rightFoot + "_contact", supportContactModelRight) contactModel2Feet.addContact(leftFoot + "_contact", supportContactModelLeft) contactModel2Feet.addContact(rightFoot + "_contact", supportContactModelRight) # Cost for self-collision maxfloat = sys.float_info.max xlb = np.concatenate([
# First, let's load the Pinocchio model for the Talos arm. talos_arm = example_robot_data.loadTalosArm() robot_model = talos_arm.model # 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( robot_model.getFrameId("gripper_left_joint"), pinocchio.SE3(np.eye(3), np.array([.0, .0, .4]))) goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref) xRegCost = crocoddyl.CostModelState(state) uRegCost = crocoddyl.CostModelControl(state) # Then let's added the running and terminal cost functions runningCostModel.addCost("gripperPose", goalTrackingCost, 1) runningCostModel.addCost("xReg", xRegCost, 1e-4) runningCostModel.addCost("uReg", uRegCost, 1e-4) terminalCostModel.addCost("gripperPose", goalTrackingCost, 1) # Next, we need to create an action model for running and terminal knots. The # forward dynamics (computed using ABA) are implemented # inside DifferentialActionModelFullyActuated. actuationModel = crocoddyl.ActuationModelFull(state)
from test_utils import NUMDIFF_MODIFIER, assertNumDiff crocoddyl.switchToNumpyMatrix() # 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_1 = crocoddyl.ContactModel6D( ROBOT_STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.SE3.Random()), ACTUATION.nu, pinocchio.utils.rand(2)) CONTACT_6D_2 = crocoddyl.ContactModel6D( ROBOT_STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('l_sole'), pinocchio.SE3.Random()), ACTUATION.nu, pinocchio.utils.rand(2)) CONTACTS.addContact("r_sole_contact", CONTACT_6D_1) CONTACTS.addContact("l_sole_contact", CONTACT_6D_2) COSTS = crocoddyl.CostModelSum(ROBOT_STATE, ACTUATION.nu, False) COSTS.addCost( "force", crocoddyl.CostModelContactForce( ROBOT_STATE, crocoddyl.FrameForce(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.Force.Random()), ACTUATION.nu), 1.)
refGripper = rdata.oMf[rmodel.getFrameId("gripper_left_joint")].translation comRef = (rfPos0 + lfPos0) / 2 comRef[2] = np.asscalar(pinocchio.centerOfMass(rmodel, rdata, q0)[2]) # Initialize Gepetto viewer if WITHDISPLAY: display = crocoddyl.GepettoDisplay(robot, frameNames=[rightFoot, leftFoot]) display.robot.viewer.gui.addSphere( 'world/point', .05, [1., 0., 0., 1.]) # radius = .1, RGBA=1001 display.robot.viewer.gui.applyConfiguration( 'world/point', target.tolist() + [0., 0., 0., 1.]) # xyz+quaternion # Add contact to the model contactModel = crocoddyl.ContactModelMultiple(state, actuation.nu) framePlacementLeft = crocoddyl.FramePlacement(leftFootId, pinocchio.SE3.Identity()) supportContactModelLeft = crocoddyl.ContactModel6D(state, framePlacementLeft, actuation.nu, np.array([0, 0])) contactModel.addContact(leftFoot + "_contact", supportContactModelLeft) framePlacementRight = crocoddyl.FramePlacement(rightFootId, pinocchio.SE3.Identity()) supportContactModelRight = crocoddyl.ContactModel6D(state, framePlacementRight, actuation.nu, np.array([0, 0])) contactModel.addContact(rightFoot + "_contact", supportContactModelRight) contactData = contactModel.createData(rdata) # Cost for self-collision maxfloat = sys.float_info.max xlb = np.concatenate([