Unverified Commit 919ee974 authored by Carlos Mastalli's avatar Carlos Mastalli Committed by GitHub
Browse files

Merge pull request #1033 from JaehyunShim/topic/control-python

Added createData bindings
parents 408da91b 5e546b14
......@@ -29,6 +29,7 @@ def rotationMatrixFromTwoVectors(a, b):
class DisplayAbstract:
def __init__(self, rate=-1, freq=1):
self.rate = rate
self.freq = freq
......@@ -63,6 +64,7 @@ class DisplayAbstract:
class GepettoDisplay(DisplayAbstract):
def __init__(self, robot, rate=-1, freq=1, cameraTF=None, floor=True, frameNames=[], visibility=False):
DisplayAbstract.__init__(self, rate, freq)
self.robot = robot
......@@ -336,6 +338,7 @@ class GepettoDisplay(DisplayAbstract):
class MeshcatDisplay(DisplayAbstract):
def __init__(self, robot, rate=-1, freq=1, openWindow=True):
DisplayAbstract.__init__(self, rate, freq)
self.robot = robot
......@@ -362,6 +365,7 @@ class MeshcatDisplay(DisplayAbstract):
class CallbackDisplay(libcrocoddyl_pywrap.CallbackAbstract):
def __init__(self, display):
libcrocoddyl_pywrap.CallbackAbstract.__init__(self)
self.visualization = display
......@@ -373,6 +377,7 @@ class CallbackDisplay(libcrocoddyl_pywrap.CallbackAbstract):
class CallbackLogger(libcrocoddyl_pywrap.CallbackAbstract):
def __init__(self):
libcrocoddyl_pywrap.CallbackAbstract.__init__(self)
self.xs = []
......
......@@ -40,6 +40,8 @@ void exposeControlParametrizationPolyOne() {
":param data: control-parametrization data\n"
":param t: normalized time in [0, 1]\n"
":param u: control parameters (dim control.nu)")
.def("createData", &ControlParametrizationModelPolyOne::createData, bp::args("self"),
"Create the poly-one data.")
.def<void (ControlParametrizationModelPolyOne::*)(const boost::shared_ptr<ControlParametrizationDataAbstract>&,
double, const Eigen::Ref<const Eigen::VectorXd>&) const>(
"params", &ControlParametrizationModelPolyOne::params, bp::args("self", "data", "t", "w"),
......
......@@ -43,6 +43,8 @@ void exposeControlParametrizationPolyTwoRK() {
":param data: poly-two-rk data\n"
":param t: normalized time in [0, 1]\n"
":param u: control parameters (dim control.nu)")
.def("createData", &ControlParametrizationModelPolyTwoRK::createData, bp::args("self"),
"Create the poly-two-rk data.")
.def<void (ControlParametrizationModelPolyTwoRK::*)(const boost::shared_ptr<ControlParametrizationDataAbstract>&,
double, const Eigen::Ref<const Eigen::VectorXd>&) const>(
"params", &ControlParametrizationModelPolyTwoRK::params, bp::args("self", "data", "t", "w"),
......
......@@ -39,6 +39,8 @@ void exposeControlParametrizationPolyZero() {
":param data: control-parametrization data\n"
":param t: normalized time in [0, 1]\n"
":param u: control parameters (dim control.nu)")
.def("createData", &ControlParametrizationModelPolyZero::createData, bp::args("self"),
"Create the poly-zero data.")
.def<void (ControlParametrizationModelPolyZero::*)(const boost::shared_ptr<ControlParametrizationDataAbstract>&,
double, const Eigen::Ref<const Eigen::VectorXd>&) const>(
"params", &ControlParametrizationModelPolyZero::params, bp::args("self", "data", "t", "u"),
......
......@@ -12,10 +12,12 @@ def deprecated(instructions):
instructions: A human-friendly string of instructions, such
as: 'Please migrate to add_proxy() ASAP.'
"""
def decorator(func):
'''This is a decorator which can be used to mark functions
as deprecated. It will result in a warning being emitted
when the function is used.'''
@functools.wraps(func)
def wrapper(*args, **kwargs):
message = 'Call to deprecated function {}. {}'.format(func.__name__, instructions)
......@@ -33,6 +35,7 @@ def deprecated(instructions):
class DeprecationHelper(object):
def __init__(self, new_target, old_name):
self.new_target = new_target
self.warning_str = '%s is deprecated: Use %s' % (old_name, new_target.__name__)
......
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
......@@ -28,16 +28,16 @@ void exposeImpulseMultiple() {
typedef boost::shared_ptr<ImpulseItem> ImpulseItemPtr;
typedef boost::shared_ptr<ImpulseDataAbstract> ImpulseDataPtr;
StdMapPythonVisitor<std::string, ImpulseItemPtr, std::less<std::string>,
std::allocator<std::pair<const std::string, ImpulseItemPtr> >,
std::allocator<std::pair<const std::string, ImpulseItemPtr>>,
true>::expose("StdMap_ImpulseItem");
StdMapPythonVisitor<std::string, ImpulseDataPtr, std::less<std::string>,
std::allocator<std::pair<const std::string, ImpulseDataPtr> >,
std::allocator<std::pair<const std::string, ImpulseDataPtr>>,
true>::expose("StdMap_ImpulseData");
bp::register_ptr_to_python<boost::shared_ptr<ImpulseItem> >();
bp::register_ptr_to_python<boost::shared_ptr<ImpulseItem>>();
bp::class_<ImpulseItem>("ImpulseItem", "Describe a impulse item.\n\n",
bp::init<std::string, boost::shared_ptr<ImpulseModelAbstract>, bp::optional<bool> >(
bp::init<std::string, boost::shared_ptr<ImpulseModelAbstract>, bp::optional<bool>>(
bp::args("self", "name", "impulse", "active"),
"Initialize the impulse item.\n\n"
":param name: impulse name\n"
......@@ -50,9 +50,9 @@ void exposeImpulseMultiple() {
.def(PrintableVisitor<ImpulseItem>());
;
bp::register_ptr_to_python<boost::shared_ptr<ImpulseModelMultiple> >();
bp::register_ptr_to_python<boost::shared_ptr<ImpulseModelMultiple>>();
bp::class_<ImpulseModelMultiple>("ImpulseModelMultiple", bp::init<boost::shared_ptr<StateMultibody> >(
bp::class_<ImpulseModelMultiple>("ImpulseModelMultiple", bp::init<boost::shared_ptr<StateMultibody>>(
bp::args("self", "state"),
"Initialize the multiple impulse model.\n\n"
":param state: state of the multibody system"))
......@@ -118,20 +118,30 @@ void exposeImpulseMultiple() {
.add_property("ni_total",
bp::make_function(&ImpulseModelMultiple::get_nc_total, deprecated<>("Deprecated. Use nc_total.")),
"dimension of the total impulse vector")
.add_property("active",
bp::make_function(&ImpulseModelMultiple::get_active,
deprecated<bp::return_value_policy<bp::return_by_value>>(
"Deprecated. Use property active_set")),
"list of names of active contact items")
.add_property("inactive",
bp::make_function(&ImpulseModelMultiple::get_inactive,
deprecated<bp::return_value_policy<bp::return_by_value>>(
"Deprecated. Use property inactive_set")),
"list of names of inactive contact items")
.add_property(
"active",
bp::make_function(&ImpulseModelMultiple::get_active, bp::return_value_policy<bp::return_by_value>()),
"name of active impulse items")
"active_set",
bp::make_function(&ImpulseModelMultiple::get_active_set, bp::return_value_policy<bp::return_by_value>()),
"set of names of active contact items")
.add_property(
"inactive",
bp::make_function(&ImpulseModelMultiple::get_inactive, bp::return_value_policy<bp::return_by_value>()),
"name of inactive impulse items")
"inactive_set",
bp::make_function(&ImpulseModelMultiple::get_inactive_set, bp::return_value_policy<bp::return_by_value>()),
"set of names of inactive contact items")
.def("getImpulseStatus", &ImpulseModelMultiple::getImpulseStatus, bp::args("self", "name"),
"Return the impulse status of a given impulse name.\n\n"
":param name: impulse name")
.def(PrintableVisitor<ImpulseModelMultiple>());
bp::register_ptr_to_python<boost::shared_ptr<ImpulseDataMultiple> >();
bp::register_ptr_to_python<boost::shared_ptr<ImpulseDataMultiple>>();
bp::class_<ImpulseDataMultiple>(
"ImpulseDataMultiple", "Data class for multiple impulses.\n\n",
......@@ -139,7 +149,7 @@ void exposeImpulseMultiple() {
bp::args("self", "model", "data"),
"Create multi-impulse data.\n\n"
":param model: multi-impulse model\n"
":param data: Pinocchio data")[bp::with_custodian_and_ward<1, 2, bp::with_custodian_and_ward<1, 3> >()])
":param data: Pinocchio data")[bp::with_custodian_and_ward<1, 2, bp::with_custodian_and_ward<1, 3>>()])
.add_property("Jc", bp::make_getter(&ImpulseDataMultiple::Jc, bp::return_internal_reference<>()),
bp::make_setter(&ImpulseDataMultiple::Jc), "Jacobian for all impulses (active and inactive)")
.add_property("dv0_dq", bp::make_getter(&ImpulseDataMultiple::dv0_dq, bp::return_internal_reference<>()),
......
......@@ -28,6 +28,7 @@ def raiseIfNan(A, error=None):
class StateVectorDerived(crocoddyl.StateAbstract):
def __init__(self, nx):
crocoddyl.StateAbstract.__init__(self, nx, nx)
......@@ -64,6 +65,7 @@ class StateVectorDerived(crocoddyl.StateAbstract):
class StateMultibodyDerived(crocoddyl.StateAbstract):
def __init__(self, pinocchioModel):
crocoddyl.StateAbstract.__init__(self, pinocchioModel.nq + pinocchioModel.nv, 2 * pinocchioModel.nv)
self.model = pinocchioModel
......@@ -128,6 +130,7 @@ class StateMultibodyDerived(crocoddyl.StateAbstract):
class SquashingSmoothSatDerived(crocoddyl.SquashingModelAbstract):
def __init__(self, u_lb, u_ub, ns):
self.u_lb = u_lb
self.u_ub = u_ub
......@@ -148,6 +151,7 @@ class SquashingSmoothSatDerived(crocoddyl.SquashingModelAbstract):
class FreeFloatingActuationDerived(crocoddyl.ActuationModelAbstract):
def __init__(self, state):
assert (state.pinocchio.joints[1].shortname() == 'JointModelFreeFlyer')
crocoddyl.ActuationModelAbstract.__init__(self, state, state.nv - 6)
......@@ -160,6 +164,7 @@ class FreeFloatingActuationDerived(crocoddyl.ActuationModelAbstract):
class FullActuationDerived(crocoddyl.ActuationModelAbstract):
def __init__(self, state):
assert (state.pinocchio.joints[1].shortname() != 'JointModelFreeFlyer')
crocoddyl.ActuationModelAbstract.__init__(self, state, state.nv)
......@@ -172,6 +177,7 @@ class FullActuationDerived(crocoddyl.ActuationModelAbstract):
class UnicycleModelDerived(crocoddyl.ActionModelAbstract):
def __init__(self):
crocoddyl.ActionModelAbstract.__init__(self, crocoddyl.StateVector(3), 2, 5)
self.dt = .1
......@@ -214,6 +220,7 @@ class UnicycleModelDerived(crocoddyl.ActionModelAbstract):
class UnicycleDataDerived(crocoddyl.ActionDataAbstract):
def __init__(self, model):
crocoddyl.ActionDataAbstract.__init__(self, model)
nx, nu = model.state.nx, model.nu
......@@ -225,6 +232,7 @@ class UnicycleDataDerived(crocoddyl.ActionDataAbstract):
class LQRModelDerived(crocoddyl.ActionModelAbstract):
def __init__(self, nx, nu, driftFree=True):
crocoddyl.ActionModelAbstract.__init__(self, crocoddyl.StateVector(nx), nu)
......@@ -258,6 +266,7 @@ class LQRModelDerived(crocoddyl.ActionModelAbstract):
class LQRDataDerived(crocoddyl.ActionDataAbstract):
def __init__(self, model):
crocoddyl.ActionDataAbstract.__init__(self, model)
self.Fx[:, :] = model.Fx
......@@ -268,6 +277,7 @@ class LQRDataDerived(crocoddyl.ActionDataAbstract):
class DifferentialLQRModelDerived(crocoddyl.DifferentialActionModelAbstract):
def __init__(self, nq, nu, driftFree=True):
crocoddyl.DifferentialActionModelAbstract.__init__(self, crocoddyl.StateVector(2 * nq), nu)
......@@ -303,6 +313,7 @@ class DifferentialLQRModelDerived(crocoddyl.DifferentialActionModelAbstract):
class DifferentialLQRDataDerived(crocoddyl.DifferentialActionDataAbstract):
def __init__(self, model):
crocoddyl.DifferentialActionDataAbstract.__init__(self, model)
self.Lxx[:, :] = model.Lxx
......@@ -313,6 +324,7 @@ class DifferentialLQRDataDerived(crocoddyl.DifferentialActionDataAbstract):
class DifferentialFreeFwdDynamicsModelDerived(crocoddyl.DifferentialActionModelAbstract):
def __init__(self, state, actuationModel, costModel):
crocoddyl.DifferentialActionModelAbstract.__init__(self, state, actuationModel.nu, costModel.nr)
self.actuation = actuationModel
......@@ -379,6 +391,7 @@ class DifferentialFreeFwdDynamicsModelDerived(crocoddyl.DifferentialActionModelA
class DifferentialFreeFwdDynamicsDataDerived(crocoddyl.DifferentialActionDataAbstract):
def __init__(self, model):
crocoddyl.DifferentialActionDataAbstract.__init__(self, model)
self.pinocchio = pinocchio.Model.createData(model.state.pinocchio)
......@@ -390,6 +403,7 @@ class DifferentialFreeFwdDynamicsDataDerived(crocoddyl.DifferentialActionDataAbs
class IntegratedActionModelEulerDerived(crocoddyl.ActionModelAbstract):
def __init__(self, diffModel, timeStep=1e-3, withCostResiduals=True):
crocoddyl.ActionModelAbstract.__init__(self, diffModel.state, diffModel.nv, diffModel.nr)
self.differential = diffModel
......@@ -432,12 +446,14 @@ class IntegratedActionModelEulerDerived(crocoddyl.ActionModelAbstract):
class IntegratedActionDataEulerDerived(crocoddyl.ActionDataAbstract):
def __init__(self, model):
crocoddyl.ActionDataAbstract.__init__(self, model)
self.differential = model.differential.createData(self)
class IntegratedActionModelRK4Derived(crocoddyl.ActionModelAbstract):
def __init__(self, diffModel, timeStep=1e-3, withCostResiduals=True):
crocoddyl.ActionModelAbstract.__init__(self, diffModel.state, diffModel.nu, diffModel.nr)
self.differential = diffModel
......@@ -553,6 +569,7 @@ class IntegratedActionModelRK4Derived(crocoddyl.ActionModelAbstract):
class IntegratedActionDataRK4Derived(crocoddyl.ActionDataAbstract):
def __init__(self, model):
crocoddyl.ActionDataAbstract.__init__(self, model)
......@@ -600,6 +617,7 @@ class IntegratedActionDataRK4Derived(crocoddyl.ActionDataAbstract):
class StateCostModelDerived(crocoddyl.CostModelAbstract):
def __init__(self, state, activation=None, xref=None, nu=None):
activation = activation if activation is not None else crocoddyl.ActivationModelQuad(state.ndx)
self.xref = xref if xref is not None else state.zero()
......@@ -621,6 +639,7 @@ class StateCostModelDerived(crocoddyl.CostModelAbstract):
class ControlCostModelDerived(crocoddyl.CostModelAbstract):
def __init__(self, state, activation=None, uref=None, nu=None):
nu = nu if nu is not None else state.nv
activation = activation if activation is not None else crocoddyl.ActivationModelQuad(nu)
......@@ -639,6 +658,7 @@ class ControlCostModelDerived(crocoddyl.CostModelAbstract):
class CoMPositionCostModelDerived(crocoddyl.CostModelAbstract):
def __init__(self, state, activation=None, cref=None, nu=None):
activation = activation if activation is not None else crocoddyl.ActivationModelQuad(3)
if nu is None:
......@@ -668,6 +688,7 @@ class CoMPositionCostModelDerived(crocoddyl.CostModelAbstract):
class FramePlacementCostModelDerived(crocoddyl.CostModelAbstract):
def __init__(self, state, activation=None, frame_id=None, placement=None, nu=None):
activation = activation if activation is not None else crocoddyl.ActivationModelQuad(6)
if nu is None:
......@@ -705,6 +726,7 @@ class FramePlacementCostModelDerived(crocoddyl.CostModelAbstract):
class FramePlacementCostDataDerived(crocoddyl.CostDataAbstract):
def __init__(self, model, collector):
crocoddyl.CostDataAbstract.__init__(self, model, collector)
self.rMf = pinocchio.SE3.Identity()
......@@ -715,6 +737,7 @@ class FramePlacementCostDataDerived(crocoddyl.CostDataAbstract):
class FrameTranslationCostModelDerived(crocoddyl.CostModelAbstract):
def __init__(self, state, activation=None, frame_id=None, translation=None, nu=None):
activation = activation if activation is not None else crocoddyl.ActivationModelQuad(3)
if nu is None:
......@@ -752,6 +775,7 @@ class FrameTranslationCostModelDerived(crocoddyl.CostModelAbstract):
class FrameTranslationDataDerived(crocoddyl.CostDataAbstract):
def __init__(self, model, collector):
crocoddyl.CostDataAbstract.__init__(self, model, collector)
self.R = np.eye(3)
......@@ -759,6 +783,7 @@ class FrameTranslationDataDerived(crocoddyl.CostDataAbstract):
class FrameRotationCostModelDerived(crocoddyl.CostModelAbstract):
def __init__(self, state, activation=None, frame_id=None, rotation=None, nu=None):
activation = activation if activation is not None else crocoddyl.ActivationModelQuad(3)
if nu is None:
......@@ -796,6 +821,7 @@ class FrameRotationCostModelDerived(crocoddyl.CostModelAbstract):
class FrameRotationCostDataDerived(crocoddyl.CostDataAbstract):
def __init__(self, model, collector):
crocoddyl.CostDataAbstract.__init__(self, model, collector)
self.rRf = np.eye(3)
......@@ -805,6 +831,7 @@ class FrameRotationCostDataDerived(crocoddyl.CostDataAbstract):
class FrameVelocityCostModelDerived(crocoddyl.CostModelAbstract):
def __init__(self, state, activation=None, frame_id=None, velocity=None, nu=None):
activation = activation if activation is not None else crocoddyl.ActivationModelQuad(6)
if nu is None:
......@@ -836,6 +863,7 @@ class FrameVelocityCostModelDerived(crocoddyl.CostModelAbstract):
class FrameVelocityCostDataDerived(crocoddyl.CostDataAbstract):
def __init__(self, model, collector):
crocoddyl.CostDataAbstract.__init__(self, model, collector)
self.fXj = model.state.pinocchio.frames[model._frame_id].placement.inverse().action
......@@ -843,6 +871,7 @@ class FrameVelocityCostDataDerived(crocoddyl.CostDataAbstract):
class Contact3DModelDerived(crocoddyl.ContactModelAbstract):
def __init__(self, state, xref, gains=[0., 0.]):
crocoddyl.ContactModelAbstract.__init__(self, state, 3)
self.xref = xref
......@@ -899,6 +928,7 @@ class Contact3DModelDerived(crocoddyl.ContactModelAbstract):
class Contact3DDataDerived(crocoddyl.ContactDataAbstract):
def __init__(self, model, data):
crocoddyl.ContactDataAbstract.__init__(self, model, data)
self.fXj = model.state.pinocchio.frames[model.xref.id].placement.inverse().action
......@@ -910,6 +940,7 @@ class Contact3DDataDerived(crocoddyl.ContactDataAbstract):
class Contact6DModelDerived(crocoddyl.ContactModelAbstract):
def __init__(self, state, Mref, gains=[0., 0.]):
crocoddyl.ContactModelAbstract.__init__(self, state, 6)
self.Mref = Mref
......@@ -947,6 +978,7 @@ class Contact6DModelDerived(crocoddyl.ContactModelAbstract):
class Contact6DDataDerived(crocoddyl.ContactDataAbstract):
def __init__(self, model, data):
crocoddyl.ContactDataAbstract.__init__(self, model, data)
self.fXj = model.state.pinocchio.frames[model.Mref.id].placement.inverse().action
......@@ -957,6 +989,7 @@ class Contact6DDataDerived(crocoddyl.ContactDataAbstract):
class Impulse3DModelDerived(crocoddyl.ImpulseModelAbstract):
def __init__(self, state, frame):
crocoddyl.ImpulseModelAbstract.__init__(self, state, 3)
self.frame = frame
......@@ -976,6 +1009,7 @@ class Impulse3DModelDerived(crocoddyl.ImpulseModelAbstract):
class Impulse3DDataDerived(crocoddyl.ImpulseDataAbstract):
def __init__(self, model, data):
crocoddyl.ImpulseDataAbstract.__init__(self, model, data)
self.fXj = model.state.pinocchio.frames[model.frame].placement.inverse().action
......@@ -983,6 +1017,7 @@ class Impulse3DDataDerived(crocoddyl.ImpulseDataAbstract):
class Impulse6DModelDerived(crocoddyl.ImpulseModelAbstract):
def __init__(self, state, frame):
crocoddyl.ImpulseModelAbstract.__init__(self, state, 6)
self.frame = frame
......@@ -1002,6 +1037,7 @@ class Impulse6DModelDerived(crocoddyl.ImpulseModelAbstract):
class Impulse6DDataDerived(crocoddyl.ImpulseDataAbstract):
def __init__(self, model, data):
crocoddyl.ImpulseDataAbstract.__init__(self, model, data)
self.fXj = model.state.pinocchio.frames[model.frame].placement.inverse().action
......@@ -1009,6 +1045,7 @@ class Impulse6DDataDerived(crocoddyl.ImpulseDataAbstract):
class DDPDerived(crocoddyl.SolverAbstract):
def __init__(self, shootingProblem):
crocoddyl.SolverAbstract.__init__(self, shootingProblem)
self.allocateData() # TODO remove it?
......@@ -1211,6 +1248,7 @@ class DDPDerived(crocoddyl.SolverAbstract):
class FDDPDerived(DDPDerived):
def __init__(self, shootingProblem):
DDPDerived.__init__(self, shootingProblem)
......
......@@ -13,6 +13,7 @@ class SimpleBipedGaitProblem:
through a strict process of deprecation.
Thus, we advice any user to DO NOT develop their application based on this class.
"""
def __init__(self, rmodel, rightFoot, leftFoot, integrator='euler', control='zero'):
""" Construct biped-gait problem.
......
......@@ -3,6 +3,7 @@ import numpy as np
class CostModelDoublePendulum(crocoddyl.CostModelAbstract):
def __init__(self, state, activation, nu):
activation = activation if activation is not None else crocoddyl.ActivationModelQuad(state.ndx)
crocoddyl.CostModelAbstract.__init__(self, state, activation, nu=nu)
......@@ -36,12 +37,14 @@ class CostModelDoublePendulum(crocoddyl.CostModelAbstract):
class CostDataDoublePendulum(crocoddyl.CostDataAbstract):
def __init__(self, model, collector):
crocoddyl.CostDataAbstract.__init__(self, model, collector)
self.Rxx = np.zeros((6, 4))
class ActuationModelDoublePendulum(crocoddyl.ActuationModelAbstract):
def __init__(self, state, actLink):
crocoddyl.ActuationModelAbstract.__init__(self, state, 1)
self.nv = state.nv
......@@ -59,6 +62,7 @@ class ActuationModelDoublePendulum(crocoddyl.ActuationModelAbstract):
class ActuationDataDoublePendulum(crocoddyl.ActuationDataAbstract):
def __init__(self, model):
crocoddyl.ActuationDataAbstract.__init__(self, model)
if model.nu == 1:
......
......@@ -13,6 +13,7 @@ class SimpleQuadrupedalGaitProblem:
through a strict process of deprecation.
Thus, we advice any user to DO NOT develop their application based on this class.
"""
def __init__(self, rmodel, lfFoot, rfFoot, lhFoot, rhFoot, integrator='euler', control='zero'):
""" Construct quadrupedal-gait problem.
......
......@@ -7,6 +7,7 @@ import crocoddyl
class DifferentialActionModelCartpole(crocoddyl.DifferentialActionModelAbstract):
def __init__(self):
crocoddyl.DifferentialActionModelAbstract.__init__(self, crocoddyl.StateVector(4), 1, 6) # nu = 1; nr = 6
self.unone = np.zeros(self.nu)
......
......@@ -447,7 +447,7 @@ void ShootingProblemTpl<Scalar>::set_runningModels(
<< "ndx in " << i << " node is not consistent with the other nodes")
}
}
is_updated_ = false;
is_updated_ = true;
T_ = models.size();
running_models_.clear();
running_datas_.clear();
......@@ -467,7 +467,7 @@ void ShootingProblemTpl<Scalar>::set_terminalModel(boost::shared_ptr<ActionModel
throw_pretty("Invalid argument: "
<< "ndx is not consistent with the other nodes")
}
is_updated_ = false;
is_updated_ = true;
terminal_model_ = model;
terminal_data_ = terminal_model_->createData();
}
......
......@@ -27,6 +27,7 @@ class SolverBoxFDDP : public SolverFDDP {
virtual void allocateData();
virtual void computeGains(const std::size_t t);
virtual void forwardPass(const double steplength);
virtual void resizeData();
const std::vector<Eigen::MatrixXd>& get_Quu_inv() const;
......
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
......@@ -227,7 +227,7 @@ class ContactModelMultipleTpl {
const std::vector<std::string>& get_inactive() {
inactive_.clear();
inactive_.reserve(inactive_set_.size());
for (const auto& contact : active_set_) {
for (const auto& contact : inactive_set_) {
inactive_.push_back(contact);
}
return inactive_;
......
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
......
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
......@@ -11,6 +11,7 @@
#include <string>
#include <map>
#include <set>
#include <utility>
#include "crocoddyl/multibody/fwd.hpp"
......@@ -189,14 +190,34 @@ class ImpulseModelMultipleTpl {
DEPRECATED("Use get_nc_total().", std::size_t get_ni_total() const;)
/**
* @brief Return the names of the active impulses