-
Notifications
You must be signed in to change notification settings - Fork 38
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Implement AngularMomentum task in the IK component #443
Merged
Merged
Changes from all commits
Commits
Show all changes
3 commits
Select commit
Hold shift + click to select a range
File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
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
26 changes: 26 additions & 0 deletions
26
bindings/python/IK/include/BipedalLocomotion/bindings/IK/AngularMomentumTask.h
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,26 @@ | ||
| /** | ||
| * @file AngularMomentumTask.h | ||
| * @authors Giulio Romualdi | ||
| * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and | ||
| * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. | ||
| */ | ||
|
|
||
| #ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_ANGULAR_MOMENTUM_TASK_H | ||
| #define BIPEDAL_LOCOMOTION_BINDINGS_IK_ANGULAR_MOMENTUM_TASK_H | ||
|
|
||
| #include <pybind11/pybind11.h> | ||
|
|
||
| namespace BipedalLocomotion | ||
| { | ||
| namespace bindings | ||
| { | ||
| namespace IK | ||
| { | ||
|
|
||
| void CreateAngularMomentumTask(pybind11::module& module); | ||
|
|
||
| } // namespace IK | ||
| } // namespace bindings | ||
| } // namespace BipedalLocomotion | ||
|
|
||
| #endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_ANGULAR_MOMENTUM_TASK_H |
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,38 @@ | ||
| /** | ||
| * @file AngularMomentumTask.cpp | ||
| * @authors Giulio Romualdi | ||
| * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and | ||
| * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. | ||
| */ | ||
|
|
||
| #include <pybind11/eigen.h> | ||
| #include <pybind11/pybind11.h> | ||
| #include <pybind11/stl.h> | ||
|
|
||
| #include <BipedalLocomotion/IK/IKLinearTask.h> | ||
| #include <BipedalLocomotion/IK/AngularMomentumTask.h> | ||
| #include <BipedalLocomotion/bindings/IK/AngularMomentumTask.h> | ||
|
|
||
| namespace BipedalLocomotion | ||
| { | ||
| namespace bindings | ||
| { | ||
| namespace IK | ||
| { | ||
|
|
||
| void CreateAngularMomentumTask(pybind11::module& module) | ||
| { | ||
| namespace py = ::pybind11; | ||
| using namespace BipedalLocomotion::IK; | ||
|
|
||
| py::class_<AngularMomentumTask, // | ||
| std::shared_ptr<AngularMomentumTask>, | ||
| IKLinearTask>(module, "AngularMomentumTask") | ||
| .def(py::init()) | ||
| .def("set_kin_dyn", &AngularMomentumTask::setKinDyn, py::arg("kin_dyn")) | ||
| .def("set_set_point", &AngularMomentumTask::setSetPoint, py::arg("angular_momentum")); | ||
| } | ||
|
|
||
| } // namespace IK | ||
| } // namespace bindings | ||
| } // namespace BipedalLocomotion |
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
128 changes: 128 additions & 0 deletions
128
src/IK/include/BipedalLocomotion/IK/AngularMomentumTask.h
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,128 @@ | ||
| /** | ||
| * @file AngularMomentumTask.h | ||
| * @authors Giulio Romualdi | ||
| * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and | ||
| * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. | ||
| */ | ||
|
|
||
| #ifndef BIPEDAL_LOCOMOTION_IK_ANGULAR_MOMENTUM_TASK_H | ||
| #define BIPEDAL_LOCOMOTION_IK_ANGULAR_MOMENTUM_TASK_H | ||
|
|
||
| #include <memory> | ||
|
|
||
| #include <BipedalLocomotion/IK/IKLinearTask.h> | ||
|
|
||
| #include <iDynTree/KinDynComputations.h> | ||
|
|
||
| namespace BipedalLocomotion | ||
| { | ||
| namespace IK | ||
| { | ||
|
|
||
| /** | ||
| * AngularMomentumTask is a concrete implementation of the Task. Please use this element if you | ||
| * want to control robot centroidal angular momentum. | ||
| * The task assumes perfect control of the robot velocity \f$\nu\f$ that contains the base | ||
| * linear and angular velocity expressed in mixed representation and the joint velocities. | ||
| * The task represents the following equation | ||
| * \f[ | ||
| * A_{cmm_\omega} \nu = {}^{G[A]} h_\omega ^* | ||
| * \f] | ||
| * where \f$A_{cmm_\omega}\f$ is the angular part of the robot centroidal momentum matrix | ||
| * \f${}^G[A} h_\omega ^*$ is the desired centroidal angular momentum. | ||
| * @note AngularMomentumTask can be used to control also a subset of element of the AngularMomentum. | ||
| * Please refer to `mask` parameter in IK::AngularMomentumTask::initialize method. | ||
| */ | ||
| class AngularMomentumTask : public IKLinearTask | ||
| { | ||
| System::VariablesHandler::VariableDescription m_robotVelocityVariable; /**< Variable | ||
| describing the | ||
| robot velocity | ||
| (base + joint) */ | ||
|
|
||
| static constexpr std::size_t m_angularVelocitySize{3}; /**< Size of the angular velocity vector. | ||
| */ | ||
| static constexpr std::size_t m_spatialVelocitySize{6}; /**< Size of the spatial velocity vector. | ||
| */ | ||
|
|
||
| bool m_isInitialized{false}; /**< True if the task has been initialized. */ | ||
| bool m_isValid{false}; /**< True if the task is valid. */ | ||
|
|
||
| std::shared_ptr<iDynTree::KinDynComputations> m_kinDyn; /**< Pointer to a KinDynComputations | ||
| object */ | ||
|
|
||
| Eigen::MatrixXd m_centroidalMomentumMatrix; | ||
| std::size_t m_angularMomentumTaskSize{m_angularVelocitySize}; /**< DoFs associated to the linear task */ | ||
| std::array<bool, m_angularVelocitySize> m_mask{true, true, true}; | ||
| public: | ||
|
|
||
| /** | ||
| * Initialize the task. | ||
| * @param paramHandler pointer to the parameters handler. | ||
| * @note the following parameters are required by the class | ||
| * | Parameter Name | Type | Description | Mandatory | | ||
| * |:----------------------------------:|:--------:|:--------------------------------------------------------------------------------------:|:---------:| | ||
| * | `robot_velocity_variable_name` | `string` | Name of the variable contained in `VariablesHandler` describing the robot velocity | Yes | | ||
| * | `mask` | `vector<bool>` | Mask representing the angular momentum coordinates controlled. E.g. [1,0,1] will enable the control on the x and z coordinates only and the angular part. (Default value, [1,1,1]) | No | | ||
| * Where the generalized robot velocity is a vector containing the base spatial-velocity | ||
| * (expressed in mixed representation) and the joint velocities. | ||
| * @return True in case of success, false otherwise. | ||
| */ | ||
| bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override; | ||
|
|
||
| /** | ||
| * Set the kinDynComputations object. | ||
| * @param kinDyn pointer to a kinDynComputations object. | ||
| * @return True in case of success, false otherwise. | ||
| */ | ||
| bool setKinDyn(std::shared_ptr<iDynTree::KinDynComputations> kinDyn); | ||
|
|
||
| /** | ||
| * Set the set of variables required by the task. The variables are stored in the | ||
| * System::VariablesHandler. | ||
| * @param variablesHandler reference to a variables handler. | ||
| * @note The handler must contain a variable named as the parameter | ||
| * `robot_velocity_variable_name` stored in the parameter handler. The variable represents the | ||
| * generalized velocity of the robot. Where the generalized robot velocity is a vector | ||
| * containing the base spatial-velocity (expressed in mixed representation) and the joints | ||
| * velocity. | ||
| * @return True in case of success, false otherwise. | ||
| */ | ||
| bool setVariablesHandler(const System::VariablesHandler& variablesHandler) override; | ||
|
|
||
| /** | ||
| * Update the content of the element. | ||
| * @return True in case of success, false otherwise. | ||
| */ | ||
| bool update() override; | ||
|
|
||
| /** | ||
| * Set the desired reference trajectory. | ||
| * @param desired angular momentum | ||
| * @return True in case of success, false otherwise. | ||
| */ | ||
| bool setSetPoint(Eigen::Ref<const Eigen::Vector3d> desiredAngularMomentumComponents); | ||
|
|
||
| /** | ||
| * Get the size of the task. (I.e the number of rows of the vector b) | ||
| * @return the size of the task. | ||
| */ | ||
| std::size_t size() const override; | ||
|
|
||
| /** | ||
| * The AngularMomentumTask is an equality task. | ||
| * @return the size of the task. | ||
| */ | ||
| Type type() const override; | ||
|
|
||
| /** | ||
| * Determines the validity of the objects retrieved with getA() and getB() | ||
| * @return True if the objects are valid, false otherwise. | ||
| */ | ||
| bool isValid() const override; | ||
| }; | ||
|
|
||
| } // namespace IK | ||
| } // namespace BipedalLocomotion | ||
|
|
||
| #endif // BIPEDAL_LOCOMOTION_IK_ANGULAR_MOMENTUM_TASK_H |
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Perhaps you may want to get a fixed release to avoid that a change in icub-models breaks this test?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think that as soon as we can start using
pybind11iDynTreebindings we can switch toiDynTree.get_random_chain(10)There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
In that case, we need to remind to set a fixed seed.