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
