Commit ab3f3a0f authored by pre-commit-ci[bot]'s avatar pre-commit-ci[bot]
Browse files

[pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
parent 7cc4c43c
Pipeline #20027 failed with stage
in 18 minutes and 24 seconds
......@@ -46,10 +46,8 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
boost::shared_ptr<DifferentialActionModelAbstract> model,
std::vector<std::string> lpf_joint_names = {},
const Scalar& time_step = Scalar(1e-3),
const bool& with_cost_residual = true,
const Scalar& fc = 0,
const bool& tau_plus_integration = true,
const int& filter = 0,
const bool& with_cost_residual = true, const Scalar& fc = 0,
const bool& tau_plus_integration = true, const int& filter = 0,
const bool& is_terminal = false);
virtual ~IntegratedActionModelLPFTpl();
......@@ -73,11 +71,13 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
const Scalar& get_dt() const;
const Scalar& get_fc() const;
const std::size_t& get_nw() const {return nw_;};
const std::size_t& get_ntau() const {return ntau_;};
const std::size_t& get_ny() const {return ny_;};
const std::vector<std::string>& get_lpf_joint_names() const {return lpf_joint_names_;};
const std::size_t& get_nw() const { return nw_; };
const std::size_t& get_ntau() const { return ntau_; };
const std::size_t& get_ny() const { return ny_; };
const std::vector<std::string>& get_lpf_joint_names() const {
return lpf_joint_names_;
};
void set_dt(const Scalar& dt);
void set_fc(const Scalar& fc);
......@@ -100,9 +100,9 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
using Base::u_ub_; //!< Upper control limits
using Base::unone_; //!< Neutral state
std::size_t nw_; //!< Input torque dimension (unfiltered)
std::size_t ntau_; //!< Filtered torque dimension ("lpf" dimension)
std::size_t ny_; //!< Augmented state dimension : nq+nv+ntau
using Base::state_; //!< Model of the state
std::size_t ntau_; //!< Filtered torque dimension ("lpf" dimension)
std::size_t ny_; //!< Augmented state dimension : nq+nv+ntau
using Base::state_; //!< Model of the state
public:
boost::shared_ptr<ActivationModelQuadraticBarrier>
......@@ -116,23 +116,28 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
Scalar alpha_;
bool with_cost_residual_;
bool enable_integration_;
Scalar tauReg_weight_; //!< Cost weight for unfiltered torque regularization
VectorXs tauReg_reference_; //!< Cost reference for unfiltered torque regularization
VectorXs tauReg_residual_, tauLim_residual_; //!< Residuals for LPF torques reg and lim
Scalar tauReg_weight_; //!< Cost weight for unfiltered torque regularization
VectorXs tauReg_reference_; //!< Cost reference for unfiltered torque
//!< regularization
VectorXs tauReg_residual_,
tauLim_residual_; //!< Residuals for LPF torques reg and lim
// bool gravity_reg_; //!< Use gravity torque for
// unfiltered torque reg, or user-provided reference?
Scalar tauLim_weight_; //!< Cost weight for unfiltered torque limits
bool tau_plus_integration_; //!< Use tau+ = LPF(tau,w) in acceleration
//!< computation, or tau
int filter_; //!< Type of LPF used>
Scalar tauLim_weight_; //!< Cost weight for unfiltered torque limits
bool tau_plus_integration_; //!< Use tau+ = LPF(tau,w) in acceleration
//!< computation, or tau
int filter_; //!< Type of LPF used>
boost::shared_ptr<PinocchioModel> pin_model_; //!< for reg cost
bool is_terminal_; //!< is it a terminal model or not ? (deactivate cost on w
//!< if true)
std::vector<std::string> lpf_joint_names_; //!< Vector of joint names that are low-pass filtered
std::vector<int> lpf_joint_ids_; //!< Vector of joint ids that are low-pass filtered
// std::vector<int> nonlpf_joint_ids_; //!< Vector of joint ids that are NOT low-pass filtered
std::vector<int> lpf_torque_ids_; //!< Vector of torque ids that are low-passs filtered
bool is_terminal_; //!< is it a terminal model or not ? (deactivate cost on w
//!< if true)
std::vector<std::string>
lpf_joint_names_; //!< Vector of joint names that are low-pass filtered
std::vector<int>
lpf_joint_ids_; //!< Vector of joint ids that are low-pass filtered
// std::vector<int> nonlpf_joint_ids_; //!< Vector of joint ids that are
// NOT low-pass filtered
std::vector<int>
lpf_torque_ids_; //!< Vector of torque ids that are low-passs filtered
};
template <typename _Scalar>
......@@ -152,8 +157,7 @@ struct IntegratedActionDataLPFTpl : public ActionDataAbstractTpl<_Scalar> {
template <template <typename Scalar> class Model>
explicit IntegratedActionDataLPFTpl(Model<Scalar>* const model)
: Base(model),
tau_tmp(model->get_nu()) {
: Base(model), tau_tmp(model->get_nu()) {
tau_tmp.setZero();
differential = model->get_differential()->createData();
const std::size_t& ndy = model->get_state()->get_ndx();
......
......@@ -29,7 +29,7 @@ class StateLPFTpl : public StateAbstractTpl<_Scalar> {
enum JointType { FreeFlyer = 0, Spherical, Simple };
explicit StateLPFTpl(boost::shared_ptr<pinocchio::ModelTpl<Scalar> > model,
std::vector<int> lpf_joint_ids );
std::vector<int> lpf_joint_ids);
virtual ~StateLPFTpl();
virtual VectorXs zero() const;
......
......@@ -16,19 +16,21 @@ using namespace crocoddyl;
template <typename Scalar>
StateLPFTpl<Scalar>::StateLPFTpl(
boost::shared_ptr<pinocchio::ModelTpl<Scalar> > model, std::vector<int> lpf_joint_ids)
: Base(model->nq + model->nv + lpf_joint_ids.size(), 2 * model->nv + lpf_joint_ids.size()),
pinocchio_(model),
ntau_(lpf_joint_ids.size()),
y0_(VectorXs::Zero(model->nq + model->nv + lpf_joint_ids.size())) {
boost::shared_ptr<pinocchio::ModelTpl<Scalar> > model,
std::vector<int> lpf_joint_ids)
: Base(model->nq + model->nv + lpf_joint_ids.size(),
2 * model->nv + lpf_joint_ids.size()),
pinocchio_(model),
ntau_(lpf_joint_ids.size()),
y0_(VectorXs::Zero(model->nq + model->nv + lpf_joint_ids.size())) {
// In a multibody system, we could define the first joint using Lie groups.
// The current cases are free-flyer (SE3) and spherical (S03).
// Instead simple represents any joint that can model within the Euclidean
// manifold. The rest of joints use Euclidean algebra. We use this fact for
// computing Jdiff.
nv_ = model->nv; // tangent configuration dimension
nq_ = model->nq; // configuration dimension
nv_ = model->nv; // tangent configuration dimension
nq_ = model->nq; // configuration dimension
ny_ = nq_ + nv_ + ntau_; // augmented state dimension
ndy_ = 2 * nv_ + ntau_; // augmented state tangent space dimension
......@@ -42,14 +44,22 @@ StateLPFTpl<Scalar>::StateLPFTpl(
lb_.segment(nq_, nv_) = -pinocchio_->velocityLimit;
ub_.segment(nq_, nv_) = pinocchio_->velocityLimit;
// Effort limit (only for LPF joints)
// lb_.tail(ntau_) = -pinocchio_->effortLimit.tail[model->idx_vs[lpf_joint_ids[i]]];
// ub_.tail(ntau_) = pinocchio_->effortLimit.tail(nw_)[model->idx_vs[lpf_joint_ids[i]]];
for(int i=0; i<lpf_joint_ids.size(); i++){
if((int)model->nvs[lpf_joint_ids[i]] != (int)1 ){
throw_pretty("Invalid argument: " << "Joint " << lpf_joint_ids[i] << " has nv=" << model->nvs[lpf_joint_ids[i]] << ". LPF joints list can only contain joints with nv=1 (i.e. free-flyer joint is forbidden) ");
// lb_.tail(ntau_) =
// -pinocchio_->effortLimit.tail[model->idx_vs[lpf_joint_ids[i]]];
// ub_.tail(ntau_) =
// pinocchio_->effortLimit.tail(nw_)[model->idx_vs[lpf_joint_ids[i]]];
for (int i = 0; i < lpf_joint_ids.size(); i++) {
if ((int)model->nvs[lpf_joint_ids[i]] != (int)1) {
throw_pretty("Invalid argument: "
<< "Joint " << lpf_joint_ids[i]
<< " has nv=" << model->nvs[lpf_joint_ids[i]]
<< ". LPF joints list can only contain joints with nv=1 "
"(i.e. free-flyer joint is forbidden) ");
}
lb_.tail(ntau_)(i) = -pinocchio_->effortLimit[model->idx_vs[lpf_joint_ids[i]]];
ub_.tail(ntau_)(i) = pinocchio_->effortLimit[model->idx_vs[lpf_joint_ids[i]]];
lb_.tail(ntau_)(i) =
-pinocchio_->effortLimit[model->idx_vs[lpf_joint_ids[i]]];
ub_.tail(ntau_)(i) =
pinocchio_->effortLimit[model->idx_vs[lpf_joint_ids[i]]];
}
Base::update_has_limits();
......
......@@ -31,9 +31,11 @@ void exposeIntegratedActionLPF() {
" [q+, v+, tau+] = StateLPF.integrate([q, v], [v + a * dt, a * dt] * "
"dt, [alpha*tau + (1-alpha)*w]).",
bp::init<boost::shared_ptr<DifferentialActionModelAbstract>,
bp::optional<std::vector<std::string>, double, bool, double, bool, int, bool> >(
bp::args("self", "diffModel", "LPFJointNames", "stepTime", "withCostResidual", "fc",
"tau_plus_integration", "filter", "is_terminal"),
bp::optional<std::vector<std::string>, double, bool, double,
bool, int, bool> >(
bp::args("self", "diffModel", "LPFJointNames", "stepTime",
"withCostResidual", "fc", "tau_plus_integration", "filter",
"is_terminal"),
"Initialize the sympletic Euler integrator.\n\n"
":param diffModel: differential action model\n"
":param LPFJointNames: names of joints that are low-pass filtered\n"
......@@ -102,22 +104,22 @@ void exposeIntegratedActionLPF() {
&IntegratedActionModelLPF::set_fc,
"cut-off frequency of low-pass filter")
// .add_property(
// "nw",
// bp::make_getter(&IntegratedActionModelLPF::get_nw,
// bp::return_value_policy<bp::return_by_value>()),
// "Dimension of the unfiltered input")
// .add_property(
// "ntau",
// bp::make_getter(&IntegratedActionModelLPF::get_ntau,
// bp::return_value_policy<bp::return_by_value>()),
// "Dimension of filtered torques")
// .add_property(
// "ny",
// bp::make_getter(&IntegratedActionModelLPF::get_ny,
// bp::return_value_policy<bp::return_by_value>()),
// "Dimension of augmented state (position, velocity and filtered torque)")
// .add_property(
// "nw",
// bp::make_getter(&IntegratedActionModelLPF::get_nw,
// bp::return_value_policy<bp::return_by_value>()),
// "Dimension of the unfiltered input")
// .add_property(
// "ntau",
// bp::make_getter(&IntegratedActionModelLPF::get_ntau,
// bp::return_value_policy<bp::return_by_value>()),
// "Dimension of filtered torques")
// .add_property(
// "ny",
// bp::make_getter(&IntegratedActionModelLPF::get_ny,
// bp::return_value_policy<bp::return_by_value>()),
// "Dimension of augmented state (position, velocity and filtered
// torque)")
.def("set_control_reg_cost",
&IntegratedActionModelLPF::set_control_reg_cost,
......
......@@ -17,17 +17,16 @@ namespace python {
namespace bp = boost::python;
void exposeStateLPF() {
bp::register_ptr_to_python<
boost::shared_ptr<sobec::StateLPF> >();
bp::register_ptr_to_python<boost::shared_ptr<sobec::StateLPF>>();
bp::class_<sobec::StateLPF, bp::bases<crocoddyl::StateAbstract> >(
"StateLPF",
"State model for low-pass filtered joints",
bp::class_<sobec::StateLPF, bp::bases<crocoddyl::StateAbstract>>(
"StateLPF", "State model for low-pass filtered joints",
bp::init<boost::shared_ptr<pinocchio::Model>, std::vector<int>>(
bp::args("self", "model", "lpf_joint_ids"),
"Initialize the state lpf model.\n\n"
":param model: pinocchio model\n"
":param lpf_joint_names: list of joints names that are low-pass filtered\n")[bp::with_custodian_and_ward<1, 2>()])
":param lpf_joint_names: list of joints names that are low-pass "
"filtered\n")[bp::with_custodian_and_ward<1, 2>()])
.def("zero", &sobec::StateLPF::zero, bp::args("self"),
"Return the neutral robot configuration with zero velocity.\n\n"
":return neutral robot configuration with zero velocity")
......@@ -36,56 +35,72 @@ void exposeStateLPF() {
":return random reference state")
.def("diff", &sobec::StateLPF::diff_dx, bp::args("self", "x0", "x1"),
"Operator that differentiates the two robot states.\n\n"
"It returns the value of x1 [-] x0 operation. This operator uses the Lie\n"
"It returns the value of x1 [-] x0 operation. This operator uses "
"the Lie\n"
"algebra since the robot's root could lie in the SE(3) manifold.\n"
":param x0: current state (dim state.nx()).\n"
":param x1: next state (dim state.nx()).\n"
":return x1 - x0 value (dim state.nx()).")
.def("integrate", &sobec::StateLPF::integrate_x, bp::args("self", "x", "dx"),
.def("integrate", &sobec::StateLPF::integrate_x,
bp::args("self", "x", "dx"),
"Operator that integrates the current robot state.\n\n"
"It returns the value of x [+] dx operation. This operator uses the Lie\n"
"It returns the value of x [+] dx operation. This operator uses the "
"Lie\n"
"algebra since the robot's root could lie in the SE(3) manifold.\n"
"Futhermore there is no timestep here (i.e. dx = v*dt), note this if you're\n"
"Futhermore there is no timestep here (i.e. dx = v*dt), note this "
"if you're\n"
"integrating a velocity v during an interval dt.\n"
":param x: current state (dim state.nx()).\n"
":param dx: displacement of the state (dim state.ndx()).\n"
":return x + dx value (dim state.nx()).")
.def("Jdiff", &sobec::StateLPF::Jdiff_Js, bp::args("self", "x0", "x1", "firstsecond"),
"Compute the partial derivatives of the diff operator.\n\n"
"Both Jacobian matrices are represented throught an identity matrix, with the exception\n"
"that the robot's root is defined as free-flying joint (SE(3)). By default, this\n"
"function returns the derivatives of the first and second argument (i.e.\n"
"firstsecond='both'). However we ask for a specific partial derivative by setting\n"
"firstsecond='first' or firstsecond='second'.\n"
":param x0: current state (dim state.nx()).\n"
":param x1: next state (dim state.nx()).\n"
":param firstsecond: derivative w.r.t x0 or x1 or both\n"
":return the partial derivative(s) of the diff(x0, x1) function")
.def("Jintegrate", &sobec::StateLPF::Jintegrate_Js, bp::args("self", "x", "dx", "firstsecond"),
"Compute the partial derivatives of arithmetic addition.\n\n"
"Both Jacobian matrices are represented throught an identity matrix. with the exception\n"
"that the robot's root is defined as free-flying joint (SE(3)). By default, this\n"
"function returns the derivatives of the first and second argument (i.e.\n"
"firstsecond='both'). However we ask for a specific partial derivative by setting\n"
"firstsecond='first' or firstsecond='second'.\n"
":param x: current state (dim state.nx()).\n"
":param dx: displacement of the state (dim state.ndx()).\n"
":param firstsecond: derivative w.r.t x or dx or both\n"
":return the partial derivative(s) of the integrate(x, dx) function")
.def("Jdiff", &sobec::StateLPF::Jdiff_Js,
bp::args("self", "x0", "x1", "firstsecond"),
"Compute the partial derivatives of the diff operator.\n\n"
"Both Jacobian matrices are represented throught an identity "
"matrix, with the exception\n"
"that the robot's root is defined as free-flying joint (SE(3)). By "
"default, this\n"
"function returns the derivatives of the first and second argument "
"(i.e.\n"
"firstsecond='both'). However we ask for a specific partial "
"derivative by setting\n"
"firstsecond='first' or firstsecond='second'.\n"
":param x0: current state (dim state.nx()).\n"
":param x1: next state (dim state.nx()).\n"
":param firstsecond: derivative w.r.t x0 or x1 or both\n"
":return the partial derivative(s) of the diff(x0, x1) function")
.def("Jintegrate", &sobec::StateLPF::Jintegrate_Js,
bp::args("self", "x", "dx", "firstsecond"),
"Compute the partial derivatives of arithmetic addition.\n\n"
"Both Jacobian matrices are represented throught an identity "
"matrix. with the exception\n"
"that the robot's root is defined as free-flying joint (SE(3)). By "
"default, this\n"
"function returns the derivatives of the first and second argument "
"(i.e.\n"
"firstsecond='both'). However we ask for a specific partial "
"derivative by setting\n"
"firstsecond='first' or firstsecond='second'.\n"
":param x: current state (dim state.nx()).\n"
":param dx: displacement of the state (dim state.ndx()).\n"
":param firstsecond: derivative w.r.t x or dx or both\n"
":return the partial derivative(s) of the integrate(x, dx) function")
.def("JintegrateTransport", &sobec::StateLPF::JintegrateTransport,
bp::args("self", "x", "dx", "Jin", "firstsecond"),
"Parallel transport from integrate(x, dx) to x.\n\n"
"This function performs the parallel transportation of an input matrix whose columns\n"
"are expressed in the tangent space at integrate(x, dx) to the tangent space at x point\n"
"This function performs the parallel transportation of an input "
"matrix whose columns\n"
"are expressed in the tangent space at integrate(x, dx) to the "
"tangent space at x point\n"
":param x: state point (dim. state.nx).\n"
":param dx: velocity vector (dim state.ndx).\n"
":param Jin: input matrix (number of rows = state.nv).\n"
":param firstsecond: derivative w.r.t x or dx")
.add_property("pinocchio",
bp::make_function(&StateMultibody::get_pinocchio, bp::return_value_policy<bp::return_by_value>()),
"pinocchio model");
.add_property(
"pinocchio",
bp::make_function(&StateMultibody::get_pinocchio,
bp::return_value_policy<bp::return_by_value>()),
"pinocchio model");
}
} // namespace python
......
......@@ -44,7 +44,7 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
DifferentialActionModelTypes::Type dam_type,
PinocchioReferenceTypes::Type ref_type,
ContactModelMaskTypes::Type mask_type) const {
// LPFJointMaskType lpf_mask_type) const {
// LPFJointMaskType lpf_mask_type) const {
boost::shared_ptr<sobec::IntegratedActionModelLPF> iam;
boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dam =
DifferentialActionModelFactory().create(dam_type, ref_type, mask_type);
......@@ -55,20 +55,18 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
double fc = 5;
bool tau_plus_integration = false;
int filter = 1;
bool is_terminal = false;
bool is_terminal = false;
// Select LPF joints
boost::shared_ptr<crocoddyl::StateMultibody> stateMultibody = boost::static_pointer_cast<crocoddyl::StateMultibody>(dam->get_state());
boost::shared_ptr<pinocchio::Model> model = stateMultibody->get_pinocchio();
std::vector<std::string> lpf_joint_names = LPFJointListFactory().create_names(model, LPFJointMaskType::ALL);
boost::shared_ptr<crocoddyl::StateMultibody> stateMultibody =
boost::static_pointer_cast<crocoddyl::StateMultibody>(
dam->get_state());
boost::shared_ptr<pinocchio::Model> model =
stateMultibody->get_pinocchio();
std::vector<std::string> lpf_joint_names =
LPFJointListFactory().create_names(model, LPFJointMaskType::ALL);
iam = boost::make_shared<sobec::IntegratedActionModelLPF>(
dam,
lpf_joint_names,
time_step,
with_cost_residual,
fc,
tau_plus_integration,
filter,
is_terminal);
dam, lpf_joint_names, time_step, with_cost_residual, fc,
tau_plus_integration, filter, is_terminal);
// set hard-coded costs on unfiltered torque
double tauReg_weight = 0.02;
Eigen::VectorXd tauReg_ref = Eigen::VectorXd::Zero(iam->get_ntau());
......@@ -83,20 +81,18 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
double fc = 5;
bool tau_plus_integration = false;
int filter = 1;
bool is_terminal = false;
bool is_terminal = false;
// Select LPF joints
boost::shared_ptr<crocoddyl::StateMultibody> stateMultibody = boost::static_pointer_cast<crocoddyl::StateMultibody>(dam->get_state());
boost::shared_ptr<pinocchio::Model> model = stateMultibody->get_pinocchio();
std::vector<std::string> lpf_joint_names = LPFJointListFactory().create_names(model, LPFJointMaskType::RAND);
boost::shared_ptr<crocoddyl::StateMultibody> stateMultibody =
boost::static_pointer_cast<crocoddyl::StateMultibody>(
dam->get_state());
boost::shared_ptr<pinocchio::Model> model =
stateMultibody->get_pinocchio();
std::vector<std::string> lpf_joint_names =
LPFJointListFactory().create_names(model, LPFJointMaskType::RAND);
iam = boost::make_shared<sobec::IntegratedActionModelLPF>(
dam,
lpf_joint_names,
time_step,
with_cost_residual,
fc,
tau_plus_integration,
filter,
is_terminal);
dam, lpf_joint_names, time_step, with_cost_residual, fc,
tau_plus_integration, filter, is_terminal);
// set hard-coded costs on unfiltered torque
double tauReg_weight = 0.02;
Eigen::VectorXd tauReg_ref = Eigen::VectorXd::Zero(iam->get_ntau());
......@@ -111,20 +107,18 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
double fc = 5;
bool tau_plus_integration = false;
int filter = 1;
bool is_terminal = false;
bool is_terminal = false;
// Select LPF joints
boost::shared_ptr<crocoddyl::StateMultibody> stateMultibody = boost::static_pointer_cast<crocoddyl::StateMultibody>(dam->get_state());
boost::shared_ptr<pinocchio::Model> model = stateMultibody->get_pinocchio();
std::vector<std::string> lpf_joint_names = LPFJointListFactory().create_names(model, LPFJointMaskType::NONE);
boost::shared_ptr<crocoddyl::StateMultibody> stateMultibody =
boost::static_pointer_cast<crocoddyl::StateMultibody>(
dam->get_state());
boost::shared_ptr<pinocchio::Model> model =
stateMultibody->get_pinocchio();
std::vector<std::string> lpf_joint_names =
LPFJointListFactory().create_names(model, LPFJointMaskType::NONE);
iam = boost::make_shared<sobec::IntegratedActionModelLPF>(
dam,
lpf_joint_names,
time_step,
with_cost_residual,
fc,
tau_plus_integration,
filter,
is_terminal);
dam, lpf_joint_names, time_step, with_cost_residual, fc,
tau_plus_integration, filter, is_terminal);
// set hard-coded costs on unfiltered torque
double tauReg_weight = 0.02;
Eigen::VectorXd tauReg_ref = Eigen::VectorXd::Zero(iam->get_ntau());
......
......@@ -15,7 +15,6 @@
#include "crocoddyl/core/numdiff/action.hpp"
#include "diff-action.hpp"
#include "sobec/crocomplements/lowpassfilter/action.hpp"
#include "statelpf.hpp"
namespace sobec {
namespace unittest {
......@@ -53,8 +52,8 @@ class ActionModelLPFFactory {
ActionModelLPFTypes::Type iam_type,
DifferentialActionModelTypes::Type dam_type,
PinocchioReferenceTypes::Type ref_type = PinocchioReferenceTypes::LOCAL,
ContactModelMaskTypes::Type mask_type = ContactModelMaskTypes::Z) const;
// LPFJointMaskType lpf_mask_type = LPFJointMaskType::ALL) const;
ContactModelMaskTypes::Type mask_type = ContactModelMaskTypes::Z) const;
// LPFJointMaskType lpf_mask_type = LPFJointMaskType::ALL) const;
};
} // namespace unittest
......
......@@ -24,27 +24,29 @@ namespace sobec {
namespace unittest {
using namespace crocoddyl;
LPFJointListFactory::LPFJointListFactory() {}
LPFJointListFactory::~LPFJointListFactory() {}
std::vector<std::string> LPFJointListFactory::create_names(boost::shared_ptr<pinocchio::Model> model,
LPFJointMaskType lpf_mask_type) const {
std::vector<std::string> LPFJointListFactory::create_names(
boost::shared_ptr<pinocchio::Model> model,
LPFJointMaskType lpf_mask_type) const {
std::vector<std::string> lpf_joint_names = {};
switch (lpf_mask_type) {
case LPFJointMaskType::ALL: {
for(std::vector<std::string>::iterator iter = model->names.begin(); iter != model->names.end(); ++iter){
if(model->getJointId(*iter) < model->njoints && model->nvs[model->getJointId(*iter)] == 1){
for (std::vector<std::string>::iterator iter = model->names.begin();
iter != model->names.end(); ++iter) {
if (model->getJointId(*iter) < model->njoints &&
model->nvs[model->getJointId(*iter)] == 1) {
lpf_joint_names.push_back(*iter);
}
}
break;
}
case LPFJointMaskType::NONE:{
}
case LPFJointMaskType::NONE: {
break;
}
case LPFJointMaskType::RAND:{
int maxJointId(model->njoints-1), minJointId(2);
int randJointId = rand()%(maxJointId-minJointId+1) + minJointId;
case LPFJointMaskType::RAND: {
int maxJointId(model->njoints - 1), minJointId(2);
int randJointId = rand() % (maxJointId - minJointId + 1) + minJointId;
lpf_joint_names.push_back(model->names[randJointId]);
break;
}
......@@ -52,24 +54,27 @@ std::vector<std::string> LPFJointListFactory::create_names(boost::shared_ptr<pin
return lpf_joint_names;
}
std::vector<int> LPFJointListFactory::create_ids(boost::shared_ptr<pinocchio::Model> model,
LPFJointMaskType lpf_mask_type) const {
std::vector<int> LPFJointListFactory::create_ids(
boost::shared_ptr<pinocchio::Model> model,
LPFJointMaskType lpf_mask_type) const {
std::vector<int> lpf_joint_ids = {};
switch (lpf_mask_type) {
case LPFJointMaskType::ALL: {
for(std::vector<std::string>::iterator iter = model->names.begin(); iter != model->names.end(); ++iter){
if(model->getJointId(*iter) < model->njoints && model->nvs[model->getJointId(*iter)] == 1){
for (std::vector<std::string>::iterator iter = model->names.begin();
iter != model->names.end(); ++iter) {
if (model->getJointId(*iter) < model->njoints &&
model->nvs[model->getJointId(*iter)] == 1) {
lpf_joint_ids.push_back(model->getJointId(*iter));
}
}
break;
}
case LPFJointMaskType::NONE:{
}
case LPFJointMaskType::NONE: {
break;
}
case LPFJointMaskType::RAND:{
int maxJointId(model->njoints-1), minJointId(2);
int randJointId = rand()%(maxJointId-minJointId+1) + minJointId;
case LPFJointMaskType::RAND: {
int maxJointId(model->njoints - 1), minJointId(2);
int randJointId = rand() % (maxJointId - minJointId + 1) + minJointId;
lpf_joint_ids.push_back(randJointId);
break;
}
......@@ -77,7 +82,6 @@ std::vector<int> LPFJointListFactory::create_ids(boost::shared_ptr<pinocchio::Mo
return lpf_joint_ids;
}
const std::vector<StateLPFModelTypes::Type> StateLPFModelTypes::all(
StateLPFModelTypes::init_all());
......@@ -108,8 +112,7 @@ StateLPFModelFactory::StateLPFModelFactory() {}
StateLPFModelFactory::~StateLPFModelFactory() {}
boost::shared_ptr<sobec::StateLPF> StateLPFModelFactory::create(
StateLPFModelTypes::Type state_type,
LPFJointMaskType lpf_mask_type) const {
StateLPFModelTypes::Type state_type, LPFJointMaskType lpf_mask_type) const {
boost::shared_ptr<pinocchio::Model> model;
boost::shared_ptr<sobec::StateLPF> state;
switch (state_type) {
......@@ -119,8 +122,9 @@ boost::shared_ptr<sobec::StateLPF> StateLPFModelFactory::create(
boost::make_shared<crocoddyl::ActuationModelFull>(
StateModelFactory().create(
StateModelTypes::StateMultibody_TalosArm));
std::vector<int> lpf_joint_ids = LPFJointListFactory().create_ids(model, lpf_mask_type);
state = boost::make_shared<sobec::StateLPF>(model, lpf_joint_ids);
std::vector<int> lpf_joint_ids =
LPFJointListFactory().create_ids(model, lpf_mask_type);
state = boost::make_shared<sobec::StateLPF>(model, lpf_joint_ids);
break;
}
......@@ -131,8 +135,9 @@ boost::shared_ptr<sobec::StateLPF> StateLPFModelFactory::create(
boost::static_pointer_cast<crocoddyl::StateMultibody>(
StateModelFactory().create(
StateModelTypes::StateMultibody_HyQ)));
std::vector<int> lpf_joint_ids = LPFJointListFactory().create_ids(model, lpf_mask_type);
state = boost::make_shared<sobec::StateLPF>(model, lpf_joint_ids);
std::vector<int> lpf_joint_ids =