Commit 98a98718 authored by Sébastien Kleff's avatar Sébastien Kleff
Browse files

added properties in action lpf bindings

parent d642fc7c
......@@ -46,9 +46,10 @@ 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& is_terminal = false);
const bool& with_cost_residual = true,
const Scalar& fc = 0,
const bool& tau_plus_integration = true,
const int& filter = 0);
virtual ~IntegratedActionModelLPFTpl();
virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
......
......@@ -17,10 +17,12 @@ using namespace crocoddyl;
template <typename Scalar>
IntegratedActionModelLPFTpl<Scalar>::IntegratedActionModelLPFTpl(
boost::shared_ptr<DifferentialActionModelAbstract> model,
std::vector<std::string> lpf_joint_names, const Scalar& time_step,
const bool& with_cost_residual, const Scalar& fc,
const bool& tau_plus_integration, const int& filter,
const bool& is_terminal)
std::vector<std::string> lpf_joint_names,
const Scalar& time_step,
const bool& with_cost_residual,
const Scalar& fc,
const bool& tau_plus_integration,
const int& filter)
: Base(model->get_state(), model->get_nu(),
model->get_nr() + 2 * lpf_joint_names.size()),
differential_(model),
......@@ -30,10 +32,8 @@ IntegratedActionModelLPFTpl<Scalar>::IntegratedActionModelLPFTpl(
fc_(fc),
nw_(model->get_nu()),
tau_plus_integration_(tau_plus_integration),
// ny_(model->get_state()->get_nx() + lpf_joint_names.size()),
// enable_integration_(true),
filter_(filter),
is_terminal_(is_terminal) {
filter_(filter)
{
// Downcast DAM state (abstract --> multibody)
boost::shared_ptr<StateMultibody> state =
boost::static_pointer_cast<StateMultibody>(model->get_state());
......
......@@ -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,44 @@ 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,
......
......@@ -46,8 +46,7 @@ boost::shared_ptr<sobec::IntegratedActionModelLPF>
ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
DifferentialActionModelTypes::Type dam_type,
PinocchioReferenceTypes::Type ref_type,
ContactModelMaskTypes::Type mask_type,
bool is_terminal) const {
ContactModelMaskTypes::Type mask_type) const {
// LPFJointMaskType lpf_mask_type) const {
boost::shared_ptr<sobec::IntegratedActionModelLPF> iam;
boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dam =
......@@ -68,8 +67,13 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
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);
// set hard-coded costs on unfiltered torque
double tauReg_weight = 1e-6 + (double) (rand()) /( (double)(RAND_MAX/(1e-1-1e-3)));
Eigen::VectorXd tauReg_ref = Eigen::VectorXd::Zero(iam->get_ntau());
......@@ -78,6 +82,7 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
iam->set_control_lim_cost(tauLim_weight);
break;
}
case ActionModelLPFTypes::IntegratedActionModelLPF_RAND: {
double time_step = 1e-3;
bool with_cost_residual = true;
......@@ -94,8 +99,13 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
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);
// set hard-coded costs on unfiltered torque
double tauReg_weight = 1e-6 + (double) (rand()) /( (double)(RAND_MAX/(1e-1-1e-3)));
Eigen::VectorXd tauReg_ref = Eigen::VectorXd::Zero(iam->get_ntau());
......@@ -104,6 +114,7 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
iam->set_control_lim_cost(tauLim_weight);
break;
}
case ActionModelLPFTypes::IntegratedActionModelLPF_NONE: {
double time_step = 1e-3;
bool with_cost_residual = true;
......@@ -119,8 +130,13 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
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);
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 = 1e-6 + (double) (rand()) /( (double)(RAND_MAX/(1e-1-1e-3)));
......@@ -128,6 +144,7 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
iam->set_control_lim_cost(tauLim_weight);
break;
}
case ActionModelLPFTypes::IntegratedActionModelLPF_alpha0: {
double time_step = 1e-3;
bool with_cost_residual = true;
......@@ -143,8 +160,13 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
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);
// set hard-coded costs on unfiltered torque
iam->set_alpha(0.);
break;
......
......@@ -52,8 +52,7 @@ class ActionModelLPFFactory {
ActionModelLPFTypes::Type iam_type,
DifferentialActionModelTypes::Type dam_type,
PinocchioReferenceTypes::Type ref_type = PinocchioReferenceTypes::LOCAL,
ContactModelMaskTypes::Type mask_type = ContactModelMaskTypes::Z,
bool is_terminal = false) const;
ContactModelMaskTypes::Type mask_type = ContactModelMaskTypes::Z) const;
// LPFJointMaskType lpf_mask_type = LPFJointMaskType::ALL) const;
};
......
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