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 supportFootIds: Ids of the constrained feet
:param swingFootTask: swinging foot task
:return pseudo-impulse differential action model
"""
# Creating a 3D multi-contact model, and then including the supporting
# foot
contactModel = crocoddyl.ContactModelMultiple(self.state,
self.actuation.nu)
for i in supportFootIds:
xref = crocoddyl.FrameTranslation(i, np.array([0., 0., 0.]))
supportContactModel = crocoddyl.ContactModel3D(
self.state, xref, self.actuation.nu, np.array([0., 50.]))
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.FrictionCone(self.nsurf, self.mu, 4, False)
frictionCone = crocoddyl.CostModelContactFrictionCone(
self.state,
crocoddyl.ActivationModelQuadraticBarrier(
crocoddyl.ActivationBounds(cone.lb, cone.ub)),
crocoddyl.FrameFrictionCone(i, cone), self.actuation.nu)
costModel.addCost(self.rmodel.frames[i].name + "_frictionCone",
frictionCone, 1e1)
if swingFootTask is not None:
for i in swingFootTask:
xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation)
vref = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero())
footTrack = crocoddyl.CostModelFrameTranslation(
self.state, xref, self.actuation.nu)
impulseFootVelCost = crocoddyl.CostModelFrameVelocity(
self.state, vref, self.actuation.nu)
costModel.addCost(
self.rmodel.frames[i.frame].name + "_footTrack", footTrack,
1e7)
costModel.addCost(
self.rmodel.frames[i.frame].name + "_impulseVel",
impulseFootVelCost, 1e6)
stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] *
(self.rmodel.nv - 6) + [10.] * self.rmodel.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
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 3D multi-contact model, and then including the supporting
# foot
contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu)
for i in supportFootIds:
xref = crocoddyl.FrameTranslation(i, np.matrix([0., 0., 0.]).T)
supportContactModel = crocoddyl.ContactModel3D(self.state, xref, self.actuation.nu, np.matrix([0., 50.]).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)
for i in supportFootIds:
cone = crocoddyl.FrictionCone(self.nsurf, self.mu, 4, False)
frictionCone = crocoddyl.CostModelContactFrictionCone(
self.state, crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(cone.lb, cone.ub)),
cone, i, self.actuation.nu)
costModel.addCost(self.rmodel.frames[i].name + "_frictionCone", frictionCone, 1e1)
if swingFootTask is not None:
for i in swingFootTask:
xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation)
footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, self.actuation.nu)
costModel.addCost(self.rmodel.frames[i.frame].name + "_footTrack", footTrack, 1e6)
stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * 6 + [1.] *
(self.rmodel.nv - 6))
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)
lb = self.state.diff(0 * self.state.lb, self.state.lb)
ub = self.state.diff(0 * self.state.ub, self.state.ub)
stateBounds = crocoddyl.CostModelState(
self.state, crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(lb, ub)),
0 * self.rmodel.defaultState, self.actuation.nu)
costModel.addCost("stateBounds", stateBounds, 1e3)
# 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, timeStep)
return model
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 3D multi-contact model, and then including the supporting
# foot
contactModel = crocoddyl.ContactModelMultiple(self.state,
self.actuation.nu)
for i in supportFootIds:
xref = crocoddyl.FrameTranslation(i, np.matrix([0., 0., 0.]).T)
supportContactModel = crocoddyl.ContactModel3D(
self.state, xref, 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 isinstance(comTask, np.ndarray):
comTrack = crocoddyl.CostModelCoMPosition(self.state, comTask,
self.actuation.nu)
costModel.addCost("comTrack", comTrack, 1e4)
if swingFootTask is not None:
for i in swingFootTask:
xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation)
footTrack = crocoddyl.CostModelFrameTranslation(
self.state, xref, self.actuation.nu)
costModel.addCost("footTrack_" + str(i), footTrack, 1e4)
stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] *
(self.rmodel.nv - 6) + [10] * self.rmodel.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, 1e-1)
costModel.addCost("ctrlReg", ctrlReg, 1e-4)
# 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 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 3D multi-contact model, and then including the supporting
# foot
contactModel = crocoddyl.ContactModelMultiple(self.state,
self.actuation.nu)
for i in supportFootIds:
xref = crocoddyl.FrameTranslation(i, np.matrix([0., 0., 0.]).T)
supportContactModel = crocoddyl.ContactModel3D(
self.state, xref, 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:
xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation)
footTrack = crocoddyl.CostModelFrameTranslation(
self.state, xref, self.actuation.nu)
costModel.addCost("footTrack_" + str(i), footTrack, 1e7)
stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] *
(self.rmodel.nv - 6) + [10] * self.rmodel.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)
for i in swingFootTask:
vref = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero())
impactFootVelCost = crocoddyl.CostModelFrameVelocity(
self.state, vref, self.actuation.nu)
costModel.addCost('impactVel_' + str(i.frame), impactFootVelCost,
1e6)
# 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
def createImpulseModel(self, supportFootIds, swingFootTask, JMinvJt_damping=1e-12, r_coeff=0.0):
""" Action model for impulse models.
An impulse model consists of describing the impulse dynamics against a set of contacts.
:param supportFootIds: Ids of the constrained feet
:param swingFootTask: swinging foot task
:return impulse action model
"""
# Creating a 3D multi-contact model, and then including the supporting foot
impulseModel = crocoddyl.ImpulseModelMultiple(self.state)
for i in supportFootIds:
supportContactModel = crocoddyl.ImpulseModel3D(self.state, i)
impulseModel.addImpulse(self.rmodel.frames[i].name + "_impulse", supportContactModel)
# Creating the cost model for a contact phase
costModel = crocoddyl.CostModelSum(self.state, 0)
if swingFootTask is not None:
for i in swingFootTask:
xref = crocoddyl.FrameTranslation(i.id, i.placement.translation)
footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, 0)
costModel.addCost(self.rmodel.frames[i.id].name + "_footTrack", footTrack, 1e7)
stateWeights = np.array([1.] * 6 + [10.] * (self.rmodel.nv - 6) + [10.] * self.rmodel.nv)
stateReg = crocoddyl.CostModelState(self.state, crocoddyl.ActivationModelWeightedQuad(stateWeights**2),
self.rmodel.defaultState, 0)
costModel.addCost("stateReg", stateReg, 1e1)
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
model = crocoddyl.ActionModelImpulseFwdDynamics(self.state, impulseModel, costModel)
model.JMinvJt_damping = JMinvJt_damping
model.r_coeff = r_coeff
return model
class FrameTranslationCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('rleg5_joint'),
pinocchio.utils.rand(3))
COST = crocoddyl.CostModelFrameTranslation(ROBOT_STATE, xref)
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 Contact3DMultipleTest(ContactModelMultipleAbstractTestCase):
ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
gains = pinocchio.utils.rand(2)
xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random().translation)
CONTACT = crocoddyl.ContactModel3D(ROBOT_STATE, xref, gains)
class FrameTranslationCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadICub().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)
COST_DER = FrameTranslationCostModelDerived(ROBOT_STATE, xref=xref)
class Contact3DTest(ContactModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadHyQ().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 = Contact3DDerived(ROBOT_STATE, xref, gains)
class Contact3DMultipleTest(ContactModelMultipleAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadHyQ().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()))
from test_utils import NUMDIFF_MODIFIER, assertNumDiff
# Create robot model and data
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_DATA = ROBOT_MODEL.createData()
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
# Contact force cost
# Create differential action model and data; link the contact data
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_3D_2 = crocoddyl.ContactModel3D(
ROBOT_STATE, crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('l_sole'),
pinocchio.SE3.Random().translation), ACTUATION.nu, pinocchio.utils.rand(2))
CONTACTS.addContact("r_sole_contact", CONTACT_6D_1)
CONTACTS.addContact("l_sole_contact", CONTACT_3D_2)
COSTS = crocoddyl.CostModelSum(ROBOT_STATE, ACTUATION.nu)
COSTS.addCost(
"force_6d",
crocoddyl.CostModelContactForce(ROBOT_STATE,
crocoddyl.FrameForce(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.Force.Random()),
6, ACTUATION.nu), 1.)
COSTS.addCost(
"force_3d",
crocoddyl.CostModelContactForce(ROBOT_STATE,
crocoddyl.FrameForce(ROBOT_MODEL.getFrameId('l_sole'), pinocchio.Force.Random()),
3, ACTUATION.nu), 1.)
MODEL = crocoddyl.DifferentialActionModelContactFwdDynamics(ROBOT_STATE, ACTUATION, CONTACTS, COSTS, 0., True)
DATA = MODEL.createData()
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)
terminalCostModel.addCost("gripperPose", goalTrackingCost, 10)
terminalCostModel.addCost("xReg", xRegCostT, .01)
# Setting the final position goal with variable angle
# angle = np.pi/2
# s = np.sin(angle)
# c = np.cos(angle)
# R = np.matrix([ [c, 0, s],
# [0, 1, 0],
# [-s, 0, c]
# ])
# target = np.array([np.sin(angle), 0, np.cos(angle)]))
runningCostModel = crocoddyl.CostModelSum(state, actuation.nu)
terminalCostModel = crocoddyl.CostModelSum(state, actuation.nu)
target = np.array(conf.target)
footName = 'foot'
footFrameID = robot_model.getFrameId(footName)
assert (robot_model.existFrame(footName))
Pref = crocoddyl.FrameTranslation(footFrameID, target)
# If also the orientation is useful for the task use
# Mref = crocoddyl.FramePlacement(footFrameID,
# pinocchio.SE3(R, conf.n_links * np.matrix([[np.sin(angle)], [0], [np.cos(angle)]])))
footTrackingCost = crocoddyl.CostModelFrameTranslation(state, Pref,
actuation.nu)
Vref = crocoddyl.FrameMotion(footFrameID, pinocchio.Motion(np.zeros(6)))
footFinalVelocity = crocoddyl.CostModelFrameVelocity(state, Vref, actuation.nu)
# simulating the cost on the power with a cost on the control
power_act = crocoddyl.ActivationModelQuad(conf.n_links)
u2 = crocoddyl.CostModelControl(
state, power_act,
actuation.nu) # joule dissipation cost without friction, for benchmarking
stateAct = crocoddyl.ActivationModelWeightedQuad(
np.concatenate([np.zeros(state.nq + 1),
np.ones(state.nv - 1)]))
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
WITHDISPLAY = True
robot = example_robot_data.load('tiago_no_hand_head_fixed')
#robot = example_robot_data.loadTiago(True,'wsg')
robot_model = robot.model
DT = 1e-3
T = 250
target = np.array([3.8, 0.1, 1.1])
# Create the cost functions
Mref = crocoddyl.FrameTranslation(robot_model.getFrameId("wrist_tool_joint"),
target)
state = crocoddyl.StateMultibody(robot.model)
goalTrackingCost = crocoddyl.CostModelFrameTranslation(state, Mref)
xRegCost = crocoddyl.CostModelState(state)
uRegCost = crocoddyl.CostModelControl(state)
# Create cost model per each action model
runningCostModel = crocoddyl.CostModelSum(state)
terminalCostModel = crocoddyl.CostModelSum(state)
# Then let's added the running and terminal cost functions
runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
runningCostModel.addCost("stateReg", xRegCost, 1e-4)
runningCostModel.addCost("ctrlReg", uRegCost, 1e-7)
terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e5)
terminalCostModel.addCost("stateReg", xRegCost, 1e-4)
# 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 = crocoddyl.ContactModel6D(
ROBOT_STATE,
crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.SE3.Random()), ACTUATION.nu,
pinocchio.utils.rand(2))
CONTACT_3D = crocoddyl.ContactModel3D(
ROBOT_STATE,
crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('l_sole'),
pinocchio.SE3.Random().translation),
ACTUATION.nu, pinocchio.utils.rand(2))
CONTACTS.addContact("r_sole_contact", CONTACT_6D)
CONTACTS.addContact("l_sole_contact", CONTACT_3D)
COSTS = crocoddyl.CostModelSum(ROBOT_STATE, ACTUATION.nu, True)
frictionCone = crocoddyl.FrictionCone(np.matrix([0., 0., 1.]).T, 0.7, 4, False)
activation = crocoddyl.ActivationModelQuadraticBarrier(
crocoddyl.ActivationBounds(frictionCone.lb, frictionCone.ub))
COSTS.addCost(
"r_sole_friction_cone",
crocoddyl.CostModelContactFrictionCone(ROBOT_STATE, activation,
frictionCone,
ROBOT_MODEL.getFrameId('r_sole'),
ACTUATION.nu), 1.)
COSTS.addCost(
gv.addSphere('world/point%d' % i, .05, colors[i])
gv.applyConfiguration('world/point%d' % i, p.tolist() + [0., 0., 0., 1.])
gv.refresh()
# State and control regularization costs
xRegCost = crocoddyl.CostModelState(state)
uRegCost = crocoddyl.CostModelControl(state)
# Then let's added the running and terminal cost functions per each action
# model
runningModels = []
terminalModels = []
for p in ps:
# Create the tracking cost
Mref = crocoddyl.FrameTranslation(
robot_model.getFrameId("gripper_left_joint"),
np.matrix(p).T)
goalTrackingCost = crocoddyl.CostModelFrameTranslation(state, Mref)
actuation = crocoddyl.ActuationModelFull(state)
# Create the running action model
runningCostModel = crocoddyl.CostModelSum(state)
runningCostModel.addCost("gripperPose", goalTrackingCost, 1.)
runningCostModel.addCost("xReg", xRegCost, 1e-4)
runningCostModel.addCost("uReg", uRegCost, 1e-7)
runningModels += [
crocoddyl.DifferentialActionModelFreeFwdDynamics(
state, actuation, runningCostModel)
]
