def __init__(self):
self.robot = load('solo12')
#Inputs to be modified bu the user before calling .compute
self.feet_position_ref = [
np.array([0.1946, 0.16891, 0.0191028]),
np.array([0.1946, -0.16891, 0.0191028]),
np.array([-0.1946, 0.16891, 0.0191028]),
np.array([-0.1946, -0.16891, 0.0191028])
]
self.feet_velocity_ref = [
np.array([0, 0, 0]),
np.array([0, 0, 0]),
np.array([0, 0, 0]),
np.array([0, 0, 0])
]
self.feet_acceleration_ref = [
np.array([0, 0, 0]),
np.array([0, 0, 0]),
np.array([0, 0, 0]),
np.array([0, 0, 0])
]
self.flag_in_contact = np.array([0, 1, 0, 1])
self.base_orientation_ref = pin.utils.rpyToMatrix(0, 0, np.pi / 6)
self.base_angularvelocity_ref = np.array([0, 0, 0])
self.base_angularacceleration_ref = np.array([0, 0, 0])
self.base_position_ref = np.array([0, 0, 0.235])
self.base_linearvelocity_ref = np.array([0, 0, 0])
self.base_linearacceleration_ref = np.array([0, 0, 0])
self.Kp_base_orientation = 100
self.Kd_base_orientation = 2 * np.sqrt(self.Kp_base_orientation)
self.Kp_base_position = 100
self.Kd_base_position = 2 * np.sqrt(self.Kp_base_position)
self.Kp_flyingfeet = 100
self.Kd_flyingfeet = 2 * np.sqrt(self.Kp_flyingfeet)
#Get frame IDs
FL_FOOT_ID = self.robot.model.getFrameId('FL_FOOT')
FR_FOOT_ID = self.robot.model.getFrameId('FR_FOOT')
HL_FOOT_ID = self.robot.model.getFrameId('HL_FOOT')
HR_FOOT_ID = self.robot.model.getFrameId('HR_FOOT')
self.BASE_ID = self.robot.model.getFrameId('base_link')
self.foot_ids = np.array(
[FL_FOOT_ID, FR_FOOT_ID, HL_FOOT_ID, HR_FOOT_ID])
def dinv(J, damping=1e-2):
''' Damped inverse '''
U, S, V = np.linalg.svd(J)
if damping == 0:
Sinv = 1 / S
else:
Sinv = S / (S**2 + damping**2)
return (V.T * Sinv) @ U.T
self.rmodel = self.robot.model
self.rdata = self.robot.data
self.i = 0
class TalosArmShootingTest(ShootingProblemTestCase):
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)
DIFF_MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(
STATE, ACTUATION, COST_SUM)
DIFF_MODEL_DER = DifferentialFreeFwdDynamicsModelDerived(
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.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)
MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(
STATE, ACTUATION, COST_SUM)
MODEL_DER = DifferentialFreeFwdDynamicsModelDerived(
STATE, ACTUATION, COST_SUM)
MODEL.armature = 0.1 * np.ones(ROBOT_MODEL.nv)
MODEL_DER.set_armature(0.1 * np.ones(ROBOT_MODEL.nv))
class FramePlacementCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
COST = crocoddyl.CostModelResidual(
ROBOT_STATE,
crocoddyl.ResidualModelFramePlacement(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'), pinocchio.SE3.Random()))
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.IntegratedActionModelRK(DIFFERENTIAL,
crocoddyl.RKType.four, 1e-3)
MODEL_DER = IntegratedActionModelRK4Derived(DIFFERENTIAL, 1e-3)
class FrameRotationCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
Rref = crocoddyl.FrameRotation(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.SE3.Random().rotation)
COST = crocoddyl.CostModelFrameRotation(ROBOT_STATE, Rref)
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 CoMPositionCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
cref = pinocchio.utils.rand(3)
COST = crocoddyl.CostModelCoMPosition(ROBOT_STATE, cref)
COST_DER = CoMPositionCostModelDerived(ROBOT_STATE, cref=cref)
class FrameVelocityCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
vref = crocoddyl.FrameMotion(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.Motion.Random())
COST = crocoddyl.CostModelFrameVelocity(ROBOT_STATE, vref)
class Impulse6DTest(ImpulseModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
frame = ROBOT_MODEL.getFrameId('r_sole')
IMPULSE = crocoddyl.ImpulseModel6D(ROBOT_STATE, frame)
IMPULSE_DER = Impulse6DModelDerived(ROBOT_STATE, frame)
class FramePlacementCostTest(CostModelAbstractTestCase):
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)
COST_DER = FramePlacementCostModelDerived(ROBOT_STATE, Mref=Mref)
class FrameRotationCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
Rref = pinocchio.SE3.Random().rotation
COST = crocoddyl.CostModelResidual(
ROBOT_STATE, crocoddyl.ResidualModelFrameRotation(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'), Rref))
COST_DER = FrameRotationCostModelDerived(ROBOT_STATE, frame_id=ROBOT_MODEL.getFrameId('r_sole'), rotation=Rref)
class Impulse3DTest(ImpulseModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('hyq').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
# gains = pinocchio.utils.rand(2)
frame = ROBOT_MODEL.getFrameId('lf_foot')
IMPULSE = crocoddyl.ImpulseModel3D(ROBOT_STATE, frame)
IMPULSE_DER = Impulse3DModelDerived(ROBOT_STATE, frame)
class Contact3DTest(ContactModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('hyq').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 = Contact3DModelDerived(ROBOT_STATE, xref, gains)
class Contact6DTest(ContactModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').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 = Contact6DModelDerived(ROBOT_STATE, Mref, gains)
class FrameVelocityCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
vref = pinocchio.Motion.Random()
COST = crocoddyl.CostModelResidual(
ROBOT_STATE,
crocoddyl.ResidualModelFrameVelocity(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'), vref, pinocchio.LOCAL))
COST_DER = FrameVelocityCostModelDerived(ROBOT_STATE, frame_id=ROBOT_MODEL.getFrameId('r_sole'), velocity=vref)
class Impulse6DMultipleTest(ImpulseModelMultipleAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
gains = pinocchio.utils.rand(2)
IMPULSES = collections.OrderedDict(
sorted({
'l_sole': crocoddyl.ImpulseModel6D(ROBOT_STATE, ROBOT_MODEL.getFrameId('l_sole')),
'r_sole': crocoddyl.ImpulseModel6D(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'))
}.items()))
class FrameTranslationCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
xref = pinocchio.utils.rand(3)
COST = crocoddyl.CostModelResidual(
ROBOT_STATE, crocoddyl.ResidualModelFrameTranslation(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'), xref))
COST_DER = FrameTranslationCostModelDerived(ROBOT_STATE,
frame_id=ROBOT_MODEL.getFrameId('r_sole'),
translation=xref)
class AnymalFreeFwdDynamicsTest(ActionModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('anymal').model
STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
ACTUATION = crocoddyl.ActuationModelFloatingBase(STATE)
COST_SUM = crocoddyl.CostModelSum(STATE, ACTUATION.nu)
COST_SUM.addCost("xReg", crocoddyl.CostModelResidual(STATE, crocoddyl.ResidualModelState(STATE, ACTUATION.nu)),
1e-7)
COST_SUM.addCost("uReg", crocoddyl.CostModelResidual(STATE, crocoddyl.ResidualModelControl(STATE, ACTUATION.nu)),
1e-7)
MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, ACTUATION, COST_SUM)
MODEL_DER = DifferentialFreeFwdDynamicsModelDerived(STATE, ACTUATION, COST_SUM)
class Contact6DMultipleTest(ContactModelMultipleAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').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 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 createRobotWithObstacles(robotname='ur5'):
### Robot
# Load the robot
robot = robex.load(robotname)
### Obstacle map
# Capsule obstacles will be placed at these XYZ-RPY parameters
oMobs = [[0.40, 0., 0.30, np.pi / 2, 0, 0],
[-0.08, -0., 0.69, np.pi / 2, 0, 0],
[0.23, -0., 0.04, np.pi / 2, 0, 0],
[-0.32, 0., -0.08, np.pi / 2, 0, 0]]
# Load visual objects and add them in collision/visual models
color = [1.0, 0.2, 0.2, 1.0] # color of the capsules
rad, length = .1, 0.4 # radius and length of capsules
for i, xyzrpy in enumerate(oMobs):
obs = pin.GeometryObject.CreateCapsule(
rad, length) # Pinocchio obstacle object
obs.meshColor = np.array(
[1.0, 0.2, 0.2,
1.0]) # Don't forget me, otherwise I am transparent ...
obs.name = "obs%d" % i # Set object name
obs.parentJoint = 0 # Set object parent = 0 = universe
obs.placement = XYZRPYtoSE3(xyzrpy) # Set object placement wrt parent
robot.collision_model.addGeometryObject(
obs) # Add object to collision model
robot.visual_model.addGeometryObject(obs) # Add object to visual model
### Collision pairs
nobs = len(oMobs)
nbodies = robot.collision_model.ngeoms - nobs
robotBodies = range(nbodies)
envBodies = range(nbodies, nbodies + nobs)
for a, b in itertools.product(robotBodies, envBodies):
robot.collision_model.addCollisionPair(pin.CollisionPair(a, b))
### Geom data
# Collision/visual models have been modified => re-generate corresponding data.
robot.collision_data = pin.GeometryData(robot.collision_model)
robot.visual_data = pin.GeometryData(robot.visual_model)
return robot
class Contact3DMultipleTest(ContactModelMultipleAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('hyq').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()))
def init_robot(q_init, enable_viewer):
"""Load the solo model and initialize the Gepetto viewer if it is enabled
Args:
q_init (array): the default position of the robot actuators
enable_viewer (bool): if the Gepetto viewer is enabled or not
"""
# Load robot model and data
# Initialisation of the Gepetto viewer
print(enable_viewer)
solo = load('solo12', display=enable_viewer)
q = solo.q0.reshape((-1, 1))
q[7:, 0] = q_init
"""if enable_viewer:
solo.initViewer(loadModel=True)
if ('viewer' in solo.viz.__dict__):
solo.viewer.gui.addFloor('world/floor')
solo.viewer.gui.setRefreshIsSynchronous(False)"""
if enable_viewer:
solo.display(q)
print("PASS")
# Initialisation of model quantities
pin.centerOfMass(solo.model, solo.data, q, np.zeros((18, 1)))
pin.updateFramePlacements(solo.model, solo.data)
pin.crba(solo.model, solo.data, solo.q0)
# Initialisation of the position of footsteps
fsteps_init = np.zeros((3, 4))
indexes = [10, 18, 26, 34]
for i in range(4):
fsteps_init[:, i] = solo.data.oMf[indexes[i]].translation
h_init = (solo.data.oMf[1].translation -
solo.data.oMf[indexes[0]].translation)[2]
fsteps_init[2, :] = 0.0
return solo, fsteps_init, h_init
[o_R_i @ i_omg_oi for o_R_i, i_omg_oi in zip(o_R_i_arr, i_omg_oi_arr)])
o_domg_i_arr = signal.savgol_filter(o_omg_i_arr,
window_length=25,
polyorder=2,
deriv=1,
delta=dt,
mode='mirror',
axis=0)
i_domg_i_arr = np.array(
[o_R_i.T @ o_domg_i for o_R_i, o_domg_i in zip(o_R_i_arr, o_domg_i_arr)])
rpy_arr = np.array([pin.rpy.matrixToRpy(o_R_i) for o_R_i in o_R_i_arr])
# Now compute the forces using the robot model
robot = load('solo12')
robot.model.gravity.linear = np.array([0, 0, -9.806])
LEGS = ['FL', 'FR', 'HL', 'HR']
nb_feet = len(LEGS)
contact_frame_names = [leg + '_ANKLE' for leg in LEGS]
contact_frame_ids = [
robot.model.getFrameId(leg_name) for leg_name in contact_frame_names
]
cforces_est = ContactForcesEstimator(robot, contact_frame_ids, dt)
o_forces_arr = np.zeros((N, 12))
l_forces_arr = np.zeros((N, 12))
o_forces_sum = np.zeros((N, 3))
detect_arr = np.zeros((N, 4))
for i in range(N):
import os
import sys
import numpy as np
import crocoddyl
import example_robot_data
import pinocchio
from crocoddyl.utils.biped import SimpleBipedGaitProblem, plotSolution
WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ
WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ
# Creating the lower-body part of Talos
talos_legs = example_robot_data.load('talos_legs')
# Defining the initial state of the robot
q0 = talos_legs.model.referenceConfigurations['half_sitting'].copy()
v0 = pinocchio.utils.zero(talos_legs.model.nv)
x0 = np.concatenate([q0, v0])
# Setting up the 3d walking problem
rightFoot = 'right_sole_link'
leftFoot = 'left_sole_link'
gait = SimpleBipedGaitProblem(talos_legs.model, rightFoot, leftFoot)
# Setting up all tasks
GAITPHASES = \
[{'walking': {'stepLength': 0.6, 'stepHeight': 0.1,
'timeStep': 0.03, 'stepKnots': 35, 'supportKnots': 10}},
{'walking': {'stepLength': 0.6, 'stepHeight': 0.1,
class ControlCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
COST = crocoddyl.CostModelControl(ROBOT_STATE)
class ControlCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
COST = crocoddyl.CostModelControl(ROBOT_STATE)
COST_DER = ControlCostModelDerived(ROBOT_STATE)
import os
import sys
import crocoddyl
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
hector = example_robot_data.load('hector')
robot_model = hector.model
target_pos = np.array([1., 0., 1.])
target_quat = pinocchio.Quaternion(1., 0., 0., 0.)
state = crocoddyl.StateMultibody(robot_model)
d_cog, cf, cm, u_lim, l_lim = 0.1525, 6.6e-5, 1e-6, 5., 0.1
tau_f = np.array([[0., 0., 0., 0.], [0., 0., 0., 0.], [1., 1., 1., 1.], [0., d_cog, 0., -d_cog],
[-d_cog, 0., d_cog, 0.], [-cm / cf, cm / cf, -cm / cf, cm / cf]])
actuation = crocoddyl.ActuationModelMultiCopterBase(state, tau_f)
nu = actuation.nu
runningCostModel = crocoddyl.CostModelSum(state, nu)
terminalCostModel = crocoddyl.CostModelSum(state, nu)
# Costs
xResidual = crocoddyl.ResidualModelState(state, state.zero(), nu)
xActivation = crocoddyl.ActivationModelWeightedQuad(np.array([0.1] * 3 + [1000.] * 3 + [1000.] * robot_model.nv))
import os
import sys
import crocoddyl
from crocoddyl.utils.biped import plotSolution
import numpy as np
import example_robot_data
import pinocchio
WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ
WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ
# Load robot
robot = example_robot_data.load('talos')
rmodel = robot.model
lims = rmodel.effortLimit
# lims[19:] *= 0.5 # reduced artificially the torque limits
rmodel.effortLimit = lims
# Create data structures
rdata = rmodel.createData()
state = crocoddyl.StateMultibody(rmodel)
actuation = crocoddyl.ActuationModelFloatingBase(state)
# Set integration time
DT = 5e-2
T = 60
target = np.array([0.4, 0, 1.2])
# Initialize reference state, target and reference CoM
rightFoot = 'right_sole_link'
