forked from loco-3d/crocoddyl
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Adding Low-Pass Filter (LPF) model (#13)
* added lpf IAM and state LPF files from Gabriele branch * added bindings for IAMLPF * renamed x into y in state LPF * implemented diff, integrate, Jdiff, Jintegrate operations for state LPF * binding stateLPF * implemented calc in stateLPF using new integration rules + bindings (still need to test out + do calcDiff * implemented calcDiff + renamed internally IAMLPF class members * fixed bug * still need to debug y0 size in stateLPF diff * fixed bug in state dimensions * minor fixes * corrected errors in lpf calcDiff and started to add optional tau / tau+ integration * implemented tau integration + corrected mistake in partials * added different smoothing factors for LPF + cost gravity on the unfiltered control inut w * implemented limit cost for w in IAM lpf + apparition of eigen bug at run time when constructing python IAMLPF * added reg and lim on w , debugged eigen issue * debugged w cost issue in terminal model lpf * fixed bug in terminal model lpf * debugged derivatives + calc and calcDiff at tau instead of tau_plus when model is terminal * fixed bugs in tau integration, added gravity reg second order terms * fixed stateLPF Jintegrate and partials in IAM LPF * fixed peaks at end of trajectory by increasing size of residual from nw to 2*nw in IAM LPF * cleaned up IAM LPF Co-authored-by: Sebastien Kleff <[email protected]>
- Loading branch information
1 parent
0500e39
commit fb66785
Showing
17 changed files
with
1,182 additions
and
3 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1 @@ | ||
| --- |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,95 @@ | ||
| /////////////////////////////////////////////////////////////////////////////// | ||
| // BSD 3-Clause License | ||
| // | ||
| // Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh | ||
| // Copyright note valid unless otherwise stated in individual files. | ||
| // All rights reserved. | ||
| /////////////////////////////////////////////////////////////////////////////// | ||
|
|
||
| #include <pinocchio/fwd.hpp> // to avoid compilation error (https://github.com/loco-3d/crocoddyl/issues/205) | ||
|
|
||
| #include "python/crocoddyl/core/core.hpp" | ||
| #include "python/crocoddyl/core/action-base.hpp" | ||
| #include "crocoddyl/core/integrator/lpf.hpp" | ||
|
|
||
| namespace crocoddyl { | ||
| namespace python { | ||
|
|
||
| void exposeIntegratedActionLPF() { | ||
| bp::class_<IntegratedActionModelLPF, bp::bases<ActionModelAbstract> >( | ||
| "IntegratedActionModelLPF", | ||
| "Sympletic Euler integrator for differential action models.\n\n" | ||
| "This class implements a sympletic Euler integrator (a.k.a semi-implicit\n" | ||
| "integrator) give a differential action model, i.e.:\n" | ||
| " [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, double, Eigen::VectorXd, bool, double, bool, int, bool> >( | ||
| bp::args("self", "diffModel", "stepTime", "withCostResidual", | ||
| "fc", "cost_weight_w_reg", "cost_ref_w_reg", "w_gravity_reg", "cost_weight_w_lim", "tau_plus_integration", "filter", "is_terminal"), | ||
| "Initialize the sympletic Euler integrator.\n\n" | ||
| ":param diffModel: differential action model\n" | ||
| ":param stepTime: step time\n" | ||
| ":param withCostResidual: includes the cost residuals and derivatives\n" | ||
| ":param fc: LPF parameter depending on cut-off frequency alpha=1/(1+2*Pi*dt*fc)\n" | ||
| ":param cost_weight_w_reg: cost weight on unfiltered torque regularization\n" | ||
| ":param cost_ref_w_reg: cost reference on unfiltered torque regularization\n" | ||
| ":param w_gravity_reg: use gravity regularization on unfiltered torque (True) or user-provided constant reference (False) " | ||
| ":param cost_weight_w_lim: cost weight on unfiltered torque limit\n" | ||
| ":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.")) | ||
| .def<void (IntegratedActionModelLPF::*)(const boost::shared_ptr<ActionDataAbstract>&, | ||
| const Eigen::Ref<const Eigen::VectorXd>&, | ||
| const Eigen::Ref<const Eigen::VectorXd>&)>( | ||
| "calc", &IntegratedActionModelLPF::calc, bp::args("self", "data", "x", "u"), | ||
| "Compute the time-discrete evolution of a differential action model.\n\n" | ||
| "It describes the time-discrete evolution of action model.\n" | ||
| ":param data: action data\n" | ||
| ":param x: state vector\n" | ||
| ":param u: control input") | ||
| .def<void (IntegratedActionModelLPF::*)(const boost::shared_ptr<ActionDataAbstract>&, | ||
| const Eigen::Ref<const Eigen::VectorXd>&)>( | ||
| "calc", &ActionModelAbstract::calc, bp::args("self", "data", "x")) | ||
| .def<void (IntegratedActionModelLPF::*)(const boost::shared_ptr<ActionDataAbstract>&, | ||
| const Eigen::Ref<const Eigen::VectorXd>&, | ||
| const Eigen::Ref<const Eigen::VectorXd>&)>( | ||
| "calcDiff", &IntegratedActionModelLPF::calcDiff, bp::args("self", "data", "x", "u"), | ||
| "Computes the derivatives of the integrated action model wrt state and control. \n\n" | ||
| "This function builds a quadratic approximation of the\n" | ||
| "action model (i.e. dynamical system and cost function).\n" | ||
| "It assumes that calc has been run first.\n" | ||
| ":param data: action data\n" | ||
| ":param x: state vector\n" | ||
| ":param u: control input\n") | ||
| .def<void (IntegratedActionModelLPF::*)(const boost::shared_ptr<ActionDataAbstract>&, | ||
| const Eigen::Ref<const Eigen::VectorXd>&)>( | ||
| "calcDiff", &ActionModelAbstract::calcDiff, bp::args("self", "data", "x")) | ||
| .def("createData", &IntegratedActionModelLPF::createData, bp::args("self"), | ||
| "Create the Euler integrator data.") | ||
| .add_property("differential", | ||
| bp::make_function(&IntegratedActionModelLPF::get_differential, | ||
| bp::return_value_policy<bp::return_by_value>()), | ||
| &IntegratedActionModelLPF::set_differential, "differential action model") | ||
| .add_property( | ||
| "dt", bp::make_function(&IntegratedActionModelLPF::get_dt, bp::return_value_policy<bp::return_by_value>()), | ||
| &IntegratedActionModelLPF::set_dt, "step time") | ||
| .add_property( | ||
| "fc", bp::make_function(&IntegratedActionModelLPF::get_fc, bp::return_value_policy<bp::return_by_value>()), | ||
| &IntegratedActionModelLPF::set_fc, "cut-off frequency of low-pass filter"); | ||
|
|
||
| bp::register_ptr_to_python<boost::shared_ptr<IntegratedActionDataLPF> >(); | ||
|
|
||
| bp::class_<IntegratedActionDataLPF, bp::bases<ActionDataAbstract> >( | ||
| "IntegratedActionDataLPF", "Sympletic Euler integrator data.", | ||
| bp::init<IntegratedActionModelLPF*>(bp::args("self", "model"), | ||
| "Create sympletic Euler integrator data.\n\n" | ||
| ":param model: sympletic Euler integrator model")) | ||
| .add_property( | ||
| "differential", | ||
| bp::make_getter(&IntegratedActionDataLPF::differential, bp::return_value_policy<bp::return_by_value>()), | ||
| "differential action data") | ||
| .add_property("dy", bp::make_getter(&IntegratedActionDataLPF::dy, bp::return_internal_reference<>()), | ||
| "state rate."); | ||
| } | ||
|
|
||
| } // namespace python | ||
| } // namespace crocoddyl |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,86 @@ | ||
| /////////////////////////////////////////////////////////////////////////////// | ||
| // BSD 3-Clause License | ||
| // | ||
| // Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh | ||
| // Copyright note valid unless otherwise stated in individual files. | ||
| // All rights reserved. | ||
| /////////////////////////////////////////////////////////////////////////////// | ||
|
|
||
| #include "python/crocoddyl/multibody/multibody.hpp" | ||
| #include "python/crocoddyl/core/state-base.hpp" | ||
| #include "crocoddyl/multibody/states/statelpf.hpp" | ||
|
|
||
| namespace crocoddyl { | ||
| namespace python { | ||
|
|
||
| void exposeStateLPF() { | ||
| bp::class_<StateLPF, bp::bases<StateAbstract> >( | ||
| "StateLPF", | ||
| "LPF state defined using Pinocchio.\n\n" | ||
| "Pinocchio defines operators for integrating or differentiating the robot's\n" | ||
| "configuration space. And here we assume that the state is defined by the\n" | ||
| "robot's configuration, joint velocities and torques (y=[q,v,u]). Generally speaking,\n" | ||
| "q lies on the manifold configuration manifold (M) and v in its tangent space\n" | ||
| "(Ty M). Additionally the Pinocchio allows us to compute analytically the\n" | ||
| "Jacobians for the differentiate and integrate operators. Note that this code\n" | ||
| "can be reused in any robot that is described through its Pinocchio model.", | ||
| bp::init<boost::shared_ptr<pinocchio::Model>, bp::optional<std::size_t> >( | ||
| bp::args("self", "pinocchioModel", "nu"), | ||
| "Initialize the multibody state given a Pinocchio model.\n\n" | ||
| ":param pinocchioModel: pinocchio model (i.e. multibody model)\n" | ||
| ":param nu: size of control vector.")[bp::with_custodian_and_ward<1, 2>()]) | ||
| .def("zero", &StateLPF::zero, bp::args("self"), | ||
| "Return the neutral robot configuration with zero velocity.\n\n" | ||
| ":return neutral robot configuration with zero velocity") | ||
| .def("rand", &StateLPF::rand, bp::args("self"), | ||
| "Return a random reference state.\n\n" | ||
| ":return random reference state") | ||
| .def("diff", &StateLPF::diff_dx, bp::args("self", "y0", "y1"), | ||
| "Operator that differentiates the two robot states.\n\n" | ||
| "It returns the value of y1 [-] y0 operation. This operator uses the Lie\n" | ||
| "algebra since the robot's root could lie in the SE(3) manifold.\n" | ||
| ":param y0: current state (dim state.ny()).\n" | ||
| ":param y1: next state (dim state.ny()).\n" | ||
| ":return y1 - y0 value (dim state.ny()).") | ||
| .def("integrate", &StateLPF::integrate_x, bp::args("self", "y", "dy"), | ||
| "Operator that integrates the current robot state.\n\n" | ||
| "It returns the value of y [+] dy 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. dy = v*dt), note this if you're\n" | ||
| "integrating a velocity v during an interval dt.\n" | ||
| ":param y: current state (dim state.ny()).\n" | ||
| ":param dy: displacement of the state (dim state.ndy()).\n" | ||
| ":return y + dy value (dim state.ny()).") | ||
| .def("Jdiff", &StateLPF::Jdiff_Js, | ||
| Jdiffs(bp::args("self", "y0", "y1", "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 y0: current state (dim state.ny()).\n" | ||
| ":param y1: next state (dim state.ny()).\n" | ||
| ":param firstsecond: desired partial derivative\n" | ||
| ":return the partial derivative(s) of the diff(y0, y1) function")) | ||
| .def("Jintegrate", &StateLPF::Jintegrate_Js, | ||
| Jintegrates(bp::args("self", "y", "dy", "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.ny()).\n" | ||
| ":param dy: displacement of the state (dim state.ndy()).\n" | ||
| ":param firstsecond: desired partial derivative\n" | ||
| ":return the partial derivative(s) of the integrate(x, dy) function")) | ||
| .add_property("pinocchio", | ||
| bp::make_function(&StateLPF::get_pinocchio, bp::return_value_policy<bp::return_by_value>()), | ||
| "pinocchio model"); | ||
| // .add_property("ny", bp::make_getter(&StateLPF::ny_, bp::return_internal_reference<>())); | ||
| // .add_property("nw", bp::make_getter(&StateLPF::ny_, bp::return_internal_reference<>())); | ||
| } | ||
|
|
||
| } // namespace python | ||
| } // namespace crocoddyl |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.