class TalosArmShootingTest(ShootingProblemTestCase): 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) DIFF_MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, ACTUATION, COST_SUM) DIFF_MODEL_DER = DifferentialFreeFwdDynamicsDerived(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.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) MODEL.armature = 0.1 * np.matrix(np.ones(ROBOT_MODEL.nv)).T MODEL_DER.set_armature(0.1 * np.matrix(np.ones(ROBOT_MODEL.nv)).T)
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.IntegratedActionModelRK4(DIFFERENTIAL, 1e-3) MODEL_DER = IntegratedActionModelRK4Derived(DIFFERENTIAL, 1e-3)
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) dt = 1e-3 runningModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, runningCostModel), dt) runningModel.differential.armature = np.matrix( [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T terminalModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, terminalCostModel), 0.) 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 250 knots (or running action # models) plus a terminal knot T = 250
class TalosArmFullActuationTest(ActuationModelAbstractTestCase): STATE = crocoddyl.StateMultibody(example_robot_data.loadTalosArm().model) ACTUATION = crocoddyl.ActuationModelFull(STATE) ACTUATION_DER = FullActuationDerived(STATE)
class FullActuationTest(ActuationModelAbstractTestCase): STATE = crocoddyl.StateMultibody(pinocchio.buildSampleModelManipulator()) ACTUATION = crocoddyl.ActuationModelFull(STATE) ACTUATION_DER = FullActuationDerived(STATE)