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

Merge pull request #57 from skleff1994/skleff_devel

Added more unittest for mask on LPF action model
parents b6713925 c09cd0f6
Pipeline #20080 passed with stage
in 38 minutes and 38 seconds
......@@ -47,16 +47,23 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
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& is_terminal = false);
const bool& tau_plus_integration = true, const int& filter = 0);
virtual ~IntegratedActionModelLPFTpl();
virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& y,
const Eigen::Ref<const VectorXs>& w);
virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& y);
virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& y,
const Eigen::Ref<const VectorXs>& w);
virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& y);
virtual boost::shared_ptr<ActionDataAbstract> createData();
virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
......@@ -70,6 +77,7 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
const;
const Scalar& get_dt() const;
const Scalar& get_fc() const;
const Scalar& get_alpha() const { return alpha_; };
const std::size_t& get_nw() const { return nw_; };
const std::size_t& get_ntau() const { return ntau_; };
......@@ -82,6 +90,9 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
const std::vector<int>& get_lpf_torque_ids() const {
return lpf_torque_ids_;
};
const std::vector<int>& get_non_lpf_torque_ids() const {
return non_lpf_torque_ids_;
};
void set_dt(const Scalar& dt);
void set_fc(const Scalar& fc);
......@@ -120,7 +131,7 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
Scalar alpha_;
bool with_cost_residual_;
Scalar fc_;
bool enable_integration_;
// bool enable_integration_;
Scalar tauReg_weight_; //!< Cost weight for unfiltered torque regularization
VectorXs tauReg_reference_; //!< Cost reference for unfiltered torque
//!< regularization
......@@ -139,10 +150,15 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
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
// std::vector<std::string> non_lpf_joint_names_; //!< Vector of joint
// names that are NOT low-pass filtered
std::vector<int> non_lpf_joint_ids_; //!< Vector of joint ids that are NOT
//!< low-pass filtered
std::vector<int> non_lpf_torque_ids_; //!< Vector of torque ids that are NOT
//!< low-passs filtered
};
template <typename _Scalar>
......
......@@ -32,10 +32,9 @@ void exposeIntegratedActionLPF() {
"dt, [alpha*tau + (1-alpha)*w]).",
bp::init<boost::shared_ptr<DifferentialActionModelAbstract>,
bp::optional<std::vector<std::string>, double, bool, double,
bool, int, bool> >(
bool, int> >(
bp::args("self", "diffModel", "LPFJointNames", "stepTime",
"withCostResidual", "fc", "tau_plus_integration", "filter",
"is_terminal"),
"withCostResidual", "fc", "tau_plus_integration", "filter"),
"Initialize the sympletic Euler integrator.\n\n"
":param diffModel: differential action model\n"
":param LPFJointNames: names of joints that are low-pass filtered\n"
......@@ -47,8 +46,7 @@ void exposeIntegratedActionLPF() {
":param tau_plus_integration: use tau+=LPF(tau,w) in acceleration "
"computation, or tau\n"
":param filter: type of low-pass filter (0 = Expo Moving Avg, 1 = "
"Classical, 2 = Exact)\n"
":param is_terminal: terminal model or not."))
"Classical, 2 = Exact)"))
.def<void (IntegratedActionModelLPF::*)(
const boost::shared_ptr<ActionDataAbstract>&,
const Eigen::Ref<const Eigen::VectorXd>&,
......@@ -103,23 +101,45 @@ void exposeIntegratedActionLPF() {
bp::return_value_policy<bp::return_by_value>()),
&IntegratedActionModelLPF::set_fc,
"cut-off frequency of low-pass filter")
.add_property(
"alpha",
bp::make_function(&IntegratedActionModelLPF::get_alpha,
bp::return_value_policy<bp::return_by_value>()),
&IntegratedActionModelLPF::set_alpha,
"discrete parameter of the 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_function(&IntegratedActionModelLPF::get_nw,
bp::return_value_policy<bp::return_by_value>()),
"torque actuation dimension (nu)")
.add_property(
"ntau",
bp::make_function(&IntegratedActionModelLPF::get_ntau,
bp::return_value_policy<bp::return_by_value>()),
"low-pass filtered actuation dimension")
.add_property(
"ny",
bp::make_function(&IntegratedActionModelLPF::get_ny,
bp::return_value_policy<bp::return_by_value>()),
"augmented state dimension (nx+ntau)")
.add_property(
"lpf_joint_names",
bp::make_function(&IntegratedActionModelLPF::get_lpf_joint_names,
bp::return_value_policy<bp::return_by_value>()),
"names of the joints that are low-pass filtered")
.add_property(
"lpf_torque_ids",
bp::make_function(&IntegratedActionModelLPF::get_lpf_torque_ids,
bp::return_value_policy<bp::return_by_value>()),
"ids in the torque vector of dimensions that are low-pass filtered")
.add_property(
"non_lpf_torque_ids",
bp::make_function(&IntegratedActionModelLPF::get_non_lpf_torque_ids,
bp::return_value_policy<bp::return_by_value>()),
"ids in the torque vector of dimensions that are NOT low-pass "
"filtered (perfect actuators)")
.def("set_control_reg_cost",
&IntegratedActionModelLPF::set_control_reg_cost,
......
......@@ -53,12 +53,11 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
DifferentialActionModelFactory().create(dam_type, ref_type, mask_type);
switch (iam_type) {
case ActionModelLPFTypes::IntegratedActionModelLPF_ALL: {
double time_step = 1e-6;
double time_step = 1e-3;
bool with_cost_residual = true;
double fc = 5;
double fc = 1 + (double)(rand()) / ((double)(RAND_MAX / (50 - 1)));
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>(
......@@ -69,22 +68,26 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
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);
tau_plus_integration, filter);
// set hard-coded costs on unfiltered torque
double tauReg_weight = 0.02;
double tauReg_weight =
1e-6 + (double)(rand()) / ((double)(RAND_MAX / (1e-1 - 1e-3)));
Eigen::VectorXd tauReg_ref = Eigen::VectorXd::Zero(iam->get_ntau());
double tauLim_weight = 1.;
double tauLim_weight =
1e-6 + (double)(rand()) / ((double)(RAND_MAX / (1e-1 - 1e-3)));
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;
double time_step = 1e-3;
bool with_cost_residual = true;
double fc = 5;
// double r3 = 1 + static_cast <double> (rand()) /( static_cast <double>
// (RAND_MAX/(50-1)));
double fc = 1 + (double)(rand()) / ((double)(RAND_MAX / (50 - 1)));
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>(
......@@ -95,22 +98,24 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
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);
tau_plus_integration, filter);
// set hard-coded costs on unfiltered torque
double tauReg_weight = 0.02;
double tauReg_weight =
1e-6 + (double)(rand()) / ((double)(RAND_MAX / (1e-1 - 1e-3)));
Eigen::VectorXd tauReg_ref = Eigen::VectorXd::Zero(iam->get_ntau());
double tauLim_weight = 1.;
double tauLim_weight =
1e-6 + (double)(rand()) / ((double)(RAND_MAX / (1e-1 - 1e-3)));
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;
double time_step = 1e-3;
bool with_cost_residual = true;
double fc = 5;
double fc = rand() % (50) + 1;
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>(
......@@ -121,23 +126,23 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
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);
// set hard-coded costs on unfiltered torque
double tauReg_weight = 0.02;
tau_plus_integration, filter);
double tauReg_weight =
1e-6 + (double)(rand()) / ((double)(RAND_MAX / (1e-1 - 1e-3)));
Eigen::VectorXd tauReg_ref = Eigen::VectorXd::Zero(iam->get_ntau());
double tauLim_weight = 1.;
double tauLim_weight =
1e-6 + (double)(rand()) / ((double)(RAND_MAX / (1e-1 - 1e-3)));
iam->set_control_reg_cost(tauReg_weight, tauReg_ref);
iam->set_control_lim_cost(tauLim_weight);
break;
}
case ActionModelLPFTypes::IntegratedActionModelLPF_alpha0: {
double time_step = 1e-6;
double time_step = 1e-3;
bool with_cost_residual = true;
double alpha = 0.;
double fc = 50000;
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>(
......@@ -148,14 +153,9 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
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);
tau_plus_integration, filter);
// set hard-coded costs on unfiltered torque
double tauReg_weight = 0.;
Eigen::VectorXd tauReg_ref = Eigen::VectorXd::Zero(iam->get_ntau());
double tauLim_weight = 0.;
iam->set_control_reg_cost(tauReg_weight, tauReg_ref);
iam->set_control_lim_cost(tauLim_weight);
iam->set_alpha(alpha);
iam->set_alpha(0.);
break;
}
default:
......
This diff is collapsed.
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment