Unverified Commit 070ea454 authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Merge pull request #53 from skleff1994/skleff_devel

Add mask on joint dimensions for LPF
parents 8a7d0883 9e734eb8
Pipeline #20032 passed with stage
in 13 minutes and 3 seconds
......@@ -44,6 +44,7 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
IntegratedActionModelLPFTpl(
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,
......@@ -70,6 +71,14 @@ 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_;
};
void set_dt(const Scalar& dt);
void set_fc(const Scalar& fc);
void set_differential(
......@@ -90,14 +99,14 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
using Base::u_lb_; //!< Lower control limits
using Base::u_ub_; //!< Upper control limits
using Base::unone_; //!< Neutral state
std::size_t nw_; //!< Unfiltered torque dimension
std::size_t ny_; //!< Augmented state dimension
using Base::state_; //!< Model of the state
// boost::shared_ptr<StateLPF> 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
public:
boost::shared_ptr<ActivationModelQuadraticBarrier>
activation_model_w_lim_; //!< for lim cost
activation_model_tauLim_; //!< for lim cost
private:
boost::shared_ptr<DifferentialActionModelAbstract> differential_;
......@@ -107,17 +116,28 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
Scalar alpha_;
bool with_cost_residual_;
bool enable_integration_;
Scalar wreg_; //!< Cost weight for unfiltered torque regularization
VectorXs wref_; //!< Cost reference for unfiltered torque regularization
// bool gravity_reg_; //!< Use gravity torque for
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 wlim_; //!< Cost weight for unfiltered torque limits
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
};
template <typename _Scalar>
......@@ -137,13 +157,14 @@ struct IntegratedActionDataLPFTpl : public ActionDataAbstractTpl<_Scalar> {
template <template <typename Scalar> class Model>
explicit IntegratedActionDataLPFTpl(Model<Scalar>* const model)
: Base(model) {
: 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();
dy = VectorXs::Zero(ndy);
// for wlim cost
activation = boost::static_pointer_cast<ActivationDataQuadraticBarrier>(
model->activation_model_w_lim_->createData());
model->activation_model_tauLim_->createData());
}
virtual ~IntegratedActionDataLPFTpl() {}
......@@ -156,6 +177,7 @@ struct IntegratedActionDataLPFTpl : public ActionDataAbstractTpl<_Scalar> {
using Base::cost;
using Base::r;
VectorXs tau_tmp;
// use refs to "alias" base class member names
VectorXs& ynext = Base::xnext;
MatrixXs& Fy = Base::Fx;
......
......@@ -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::size_t nu = 0);
std::vector<int> lpf_joint_ids);
virtual ~StateLPFTpl();
virtual VectorXs zero() const;
......@@ -57,7 +57,7 @@ class StateLPFTpl : public StateAbstractTpl<_Scalar> {
const Jcomponent firstsecond) const;
const boost::shared_ptr<pinocchio::ModelTpl<Scalar> >& get_pinocchio() const;
const std::size_t& get_nw() const;
const std::size_t& get_ntau() const;
const std::size_t& get_ny() const;
const std::size_t& get_ndy() const;
......@@ -69,7 +69,7 @@ class StateLPFTpl : public StateAbstractTpl<_Scalar> {
using Base::nv_;
using Base::nx_;
using Base::ub_;
std::size_t nw_;
std::size_t ntau_;
std::size_t ny_;
std::size_t ndy_;
......
......@@ -16,21 +16,23 @@ using namespace crocoddyl;
template <typename Scalar>
StateLPFTpl<Scalar>::StateLPFTpl(
boost::shared_ptr<pinocchio::ModelTpl<Scalar> > model, std::size_t nu)
: Base(model->nq + model->nv + nu, 2 * model->nv + nu),
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),
y0_(VectorXs::Zero(model->nq + model->nv + nu)) {
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
ny_ = nq_ + nv_ + nu; // augmented state dimension
ndy_ = 2 * nv_ + nu; // augmented state tangent space dimension
nw_ = nu; // unfiltered input dimension (nb of actuated joints)
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
// Define internally the limits of the first joint
const std::size_t nq0 = model->joints[1].nq();
......@@ -41,20 +43,36 @@ StateLPFTpl<Scalar>::StateLPFTpl(
ub_.segment(nq0, nq_ - nq0) = pinocchio_->upperPositionLimit.tail(nq_ - nq0);
lb_.segment(nq_, nv_) = -pinocchio_->velocityLimit;
ub_.segment(nq_, nv_) = pinocchio_->velocityLimit;
lb_.tail(nw_) = -pinocchio_->effortLimit.tail(nw_);
ub_.tail(nw_) = pinocchio_->effortLimit.tail(nw_);
// 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_)(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();
y0_.head(nq_) = pinocchio::neutral(*pinocchio_.get());
y0_.tail(nv_ + nu) = VectorXs::Zero(nv_ + nu);
y0_.tail(nv_ + ntau_) = VectorXs::Zero(nv_ + ntau_);
}
template <typename Scalar>
StateLPFTpl<Scalar>::~StateLPFTpl() {}
template <typename Scalar>
const std::size_t& StateLPFTpl<Scalar>::get_nw() const {
return nw_;
const std::size_t& StateLPFTpl<Scalar>::get_ntau() const {
return ntau_;
}
template <typename Scalar>
......@@ -102,7 +120,7 @@ void StateLPFTpl<Scalar>::diff(const Eigen::Ref<const VectorXs>& y0,
pinocchio::difference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_),
dyout.head(nv_));
dyout.segment(nv_, nv_) = y1.segment(nq_, nv_) - y0.segment(nq_, nv_);
dyout.tail(nw_) = y1.tail(nw_) - y0.tail(nw_);
dyout.tail(ntau_) = y1.tail(ntau_) - y0.tail(ntau_);
}
template <typename Scalar>
......@@ -128,7 +146,7 @@ void StateLPFTpl<Scalar>::integrate(const Eigen::Ref<const VectorXs>& y,
pinocchio::integrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
yout.head(nq_));
yout.segment(nq_, nv_) = y.segment(nq_, nv_) + dy.segment(nv_, nv_);
yout.tail(nw_) = y.tail(nw_) + dy.tail(nw_);
yout.tail(ntau_) = y.tail(ntau_) + dy.tail(ntau_);
}
template <typename Scalar>
......@@ -163,7 +181,7 @@ void StateLPFTpl<Scalar>::Jdiff(const Eigen::Ref<const VectorXs>& y0,
pinocchio::dDifference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_),
Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0);
Jfirst.block(nv_, nv_, nv_, nv_).diagonal().array() = (Scalar)-1;
Jfirst.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)-1;
Jfirst.bottomRightCorner(ntau_, ntau_).diagonal().array() = (Scalar)-1;
} else if (firstsecond == second) {
if (static_cast<std::size_t>(Jsecond.rows()) != ndy_ ||
static_cast<std::size_t>(Jsecond.cols()) != ndy_) {
......@@ -175,7 +193,7 @@ void StateLPFTpl<Scalar>::Jdiff(const Eigen::Ref<const VectorXs>& y0,
pinocchio::dDifference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_),
Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1);
Jsecond.block(nv_, nv_, nv_, nv_).diagonal().array() = (Scalar)1;
Jsecond.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)1;
Jsecond.bottomRightCorner(ntau_, ntau_).diagonal().array() = (Scalar)1;
} else { // computing both
if (static_cast<std::size_t>(Jfirst.rows()) != ndy_ ||
static_cast<std::size_t>(Jfirst.cols()) != ndy_) {
......@@ -196,9 +214,9 @@ void StateLPFTpl<Scalar>::Jdiff(const Eigen::Ref<const VectorXs>& y0,
pinocchio::dDifference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_),
Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1);
Jfirst.block(nv_, nv_, nv_, nv_).diagonal().array() = (Scalar)-1;
Jfirst.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)-1;
Jfirst.bottomRightCorner(ntau_, ntau_).diagonal().array() = (Scalar)-1;
Jsecond.block(nv_, nv_, nv_, nv_).diagonal().array() = (Scalar)1;
Jsecond.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)1;
Jsecond.bottomRightCorner(ntau_, ntau_).diagonal().array() = (Scalar)1;
}
}
......@@ -228,21 +246,21 @@ void StateLPFTpl<Scalar>::Jintegrate(const Eigen::Ref<const VectorXs>& y,
Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
pinocchio::SETTO);
Jfirst.block(nv_, nv_, nv_, nv_).diagonal().array() = (Scalar)1;
Jfirst.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)1;
Jfirst.bottomRightCorner(ntau_, ntau_).diagonal().array() = (Scalar)1;
break;
case addto:
pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
pinocchio::ADDTO);
Jfirst.block(nv_, nv_, nv_, nv_).diagonal().array() += (Scalar)1;
Jfirst.bottomRightCorner(nw_, nw_).diagonal().array() += (Scalar)1;
Jfirst.bottomRightCorner(ntau_, ntau_).diagonal().array() += (Scalar)1;
break;
case rmfrom:
pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
pinocchio::RMTO);
Jfirst.block(nv_, nv_, nv_, nv_).diagonal().array() -= (Scalar)1;
Jfirst.bottomRightCorner(nw_, nw_).diagonal().array() -= (Scalar)1;
Jfirst.bottomRightCorner(ntau_, ntau_).diagonal().array() -= (Scalar)1;
break;
default:
throw_pretty(
......@@ -264,21 +282,21 @@ void StateLPFTpl<Scalar>::Jintegrate(const Eigen::Ref<const VectorXs>& y,
Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
pinocchio::SETTO);
Jsecond.block(nv_, nv_, nv_, nv_).diagonal().array() = (Scalar)1;
Jsecond.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)1;
Jsecond.bottomRightCorner(ntau_, ntau_).diagonal().array() = (Scalar)1;
break;
case addto:
pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
pinocchio::ADDTO);
Jsecond.block(nv_, nv_, nv_, nv_).diagonal().array() += (Scalar)1;
Jsecond.bottomRightCorner(nw_, nw_).diagonal().array() += (Scalar)1;
Jsecond.bottomRightCorner(ntau_, ntau_).diagonal().array() += (Scalar)1;
break;
case rmfrom:
pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
pinocchio::RMTO);
Jsecond.block(nv_, nv_, nv_, nv_).diagonal().array() -= (Scalar)1;
Jsecond.bottomRightCorner(nw_, nw_).diagonal().array() -= (Scalar)1;
Jsecond.bottomRightCorner(ntau_, ntau_).diagonal().array() -= (Scalar)1;
break;
default:
throw_pretty(
......
......@@ -18,6 +18,7 @@ void exposeDesigner();
void exposeHorizonManager();
void exposeModelFactory();
void exposeIntegratedActionLPF();
void exposeStateLPF();
void exposeDAMContactFwdDyn();
void exposeResidualContactForce();
void exposeWBC();
......
......@@ -10,6 +10,7 @@ set(${PY_NAME}_SOURCES
crocomplements/residual-fly-high.cpp
crocomplements/activation-quad-ref.cpp
crocomplements/lowpassfilter/action.cpp
crocomplements/lowpassfilter/state.cpp
crocomplements/contact/contact3d.cpp
crocomplements/contact/contact1d.cpp
crocomplements/contact/multiple-contacts.cpp
......
......@@ -31,11 +31,14 @@ 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<double, bool, double, bool, int, bool> >(
bp::args("self", "diffModel", "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"
":param stepTime: step time\n"
":param withCostResidual: includes the cost residuals and "
"derivatives\n"
......@@ -101,6 +104,23 @@ 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)")
.def("set_control_reg_cost",
&IntegratedActionModelLPF::set_control_reg_cost,
bp::args("self", "weight", "ref"),
......
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
#include "sobec/crocomplements/lowpassfilter/state.hpp"
#include <boost/python.hpp>
#include <boost/python/enum.hpp>
#include <eigenpy/eigenpy.hpp>
namespace sobec {
namespace python {
namespace bp = boost::python;
void exposeStateLPF() {
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::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>()])
.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")
.def("rand", &sobec::StateLPF::rand, bp::args("self"),
"Return a random reference state.\n\n"
":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"
"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"),
"Operator that integrates the current robot state.\n\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"
"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("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"
":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");
}
} // namespace python
} // namespace sobec
......@@ -21,6 +21,7 @@ BOOST_PYTHON_MODULE(sobec_pywrap) {
sobec::python::exposeModelFactory();
sobec::python::exposeFlex();
sobec::python::exposeIntegratedActionLPF();
sobec::python::exposeStateLPF();
sobec::python::exposeWBC();
sobec::python::exposeOCPWalk();
sobec::python::exposeMPCWalk();
......
......@@ -12,6 +12,7 @@ from .sobec_pywrap import (
ResidualModelFeetCollision,
ResidualModelFlyHigh,
IntegratedActionModelLPF,
StateLPF,
ContactModel3D,
ContactModel1D,
ContactModelMultiple,
......
......@@ -18,8 +18,14 @@ const std::vector<ActionModelLPFTypes::Type> ActionModelLPFTypes::all(
std::ostream& operator<<(std::ostream& os, ActionModelLPFTypes::Type type) {
switch (type) {
case ActionModelLPFTypes::IntegratedActionModelLPF:
os << "IntegratedActionModelLPF";
case ActionModelLPFTypes::IntegratedActionModelLPF_ALL:
os << "IntegratedActionModelLPF_ALL";
break;
case ActionModelLPFTypes::IntegratedActionModelLPF_RAND:
os << "IntegratedActionModelLPF_RAND";
break;
case ActionModelLPFTypes::IntegratedActionModelLPF_NONE:
os << "IntegratedActionModelLPF_NONE";
break;
case ActionModelLPFTypes::NbActionModelLPFTypes:
os << "NbActionModelLPFTypes";
......@@ -38,26 +44,87 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
DifferentialActionModelTypes::Type dam_type,
PinocchioReferenceTypes::Type ref_type,
ContactModelMaskTypes::Type 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);
switch (iam_type) {
case ActionModelLPFTypes::IntegratedActionModelLPF: {
case ActionModelLPFTypes::IntegratedActionModelLPF_ALL: {
double time_step = 1e-6;
bool with_cost_residual = true;
double fc = 5;
bool tau_plus_integration = false;
int filter = 1;
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);
iam = boost::make_shared<sobec::IntegratedActionModelLPF>(
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());
double tauLim_weight = 1.;
iam->set_control_reg_cost(tauReg_weight, tauReg_ref);
iam->set_control_lim_cost(tauLim_weight);
break;
}
case ActionModelLPFTypes::IntegratedActionModelLPF_RAND: {
double time_step = 1e-6;
bool with_cost_residual = true;
double fc = 5;
bool tau_plus_integration = false;
int filter = 1;
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);
iam = boost::make_shared<sobec::IntegratedActionModelLPF>(
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());
double tauLim_weight = 1.;
iam->set_control_reg_cost(tauReg_weight, tauReg_ref);
iam->set_control_lim_cost(tauLim_weight);
break;
}
case ActionModelLPFTypes::IntegratedActionModelLPF_NONE: {
double time_step = 1e-6;
bool with_cost_residual = true;
double fc = 5;
bool tau_plus_integration = false;
int filter = 1;
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);