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)
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 crocoddyl.switchToNumpyMatrix() # In this example test, we will solve the reaching-goal task with the Talos arm. # For that, we use the forward dynamics (with its analytical derivatives) # developed inside crocoddyl; it describes inside DifferentialActionModelFullyActuated class. # Finally, we use an Euler sympletic integration scheme. # 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]])))
import crocoddyl from crocoddyl.utils import DifferentialFreeFwdDynamicsDerived import pinocchio import example_robot_data import numpy as np import os import sys import time import subprocess # First, let's load the Pinocchio model for the Talos arm. ROBOT = example_robot_data.loadTalosArm() N = 100 # number of nodes T = int(sys.argv[1]) if (len(sys.argv) > 1) else int(5e3) # number of trials MAXITER = 1 CALLBACKS = False 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"),
class TalosArmFullActuationTest(ActuationModelAbstractTestCase): STATE = crocoddyl.StateMultibody(example_robot_data.loadTalosArm().model) ACTUATION = crocoddyl.ActuationModelFull(STATE) ACTUATION_DER = FullActuationDerived(STATE)
class StateMultibodyTalosArmTest(StateAbstractTestCase): MODEL = example_robot_data.loadTalosArm().model NX = MODEL.nq + MODEL.nv NDX = 2 * MODEL.nv STATE = crocoddyl.StateMultibody(MODEL) STATE_DER = StateMultibodyDerived(MODEL)
class TalosArmFloatingTest(RobotTestCase): RobotTestCase.ROBOT = example_robot_data.loadTalosArm() RobotTestCase.NQ = 14 RobotTestCase.NV = 13
class TalosArmTest(RobotTestCase): RobotTestCase.ROBOT = example_robot_data.loadTalosArm() RobotTestCase.NQ = 7 RobotTestCase.NV = 7
# developed inside crocoddyl; it describes inside DifferentialActionModelFullyActuated class. # Finally, we use an Euler sympletic integration scheme. ''' import sys WITHDISPLAY = 'display' in sys.argv WITHPLOT = 'plot' in sys.argv import crocoddyl crocoddyl.switchToNumpyMatrix() import pinocchio import numpy as np import example_robot_data # First, let's load the Pinocchio model for the Talos arm. robotWrapper = example_robot_data.loadTalosArm() robotWrapper.initViewer(loadModel=True) viewer = robotWrapper.viewer.gui # Set robot model robot_model = robotWrapper.model robot_model.armature = np.matrix([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.]).T * 5 robot_model.q0 = np.matrix([3.5, 2, 2, 0, 0, 0, 0]).T robot_model.x0 = np.concatenate( [robot_model.q0, pinocchio.utils.zero(robot_model.nv)]) robot_model.gravity *= 0 # Configure task #FRAME_TIP = robot_model.getFrameId("gripper_left_joint") FRAME_TIP = robot_model.getFrameId("gripper_left_fingertip_3_link") goal = np.matrix([.2, 0.5, .5]).T