-
Notifications
You must be signed in to change notification settings - Fork 38
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add FloatingBaseSystemAccelerationKinematics class and update CMakeLi…
…sts.txt
- Loading branch information
1 parent
9a0daac
commit 845dff1
Showing
3 changed files
with
228 additions
and
2 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
137 changes: 137 additions & 0 deletions
137
...de/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemAccelerationKinematics.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,137 @@ | ||
| /** | ||
| * @file FloatingBaseSystemAccelerationKinematics.h | ||
| * @authors Giulio Romualdi | ||
| * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and | ||
| * distributed under the terms of the BSD-3-Clause license. | ||
| */ | ||
|
|
||
| #ifndef BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FLOATING_BASE_SYSTEM_ACCELERATION_KINEMATICS_H | ||
| #define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FLOATING_BASE_SYSTEM_ACCELERATION_KINEMATICS_H | ||
|
|
||
| #include <memory> | ||
| #include <vector> | ||
|
|
||
| #include <BipedalLocomotion/ContinuousDynamicalSystem/DynamicalSystem.h> | ||
| #include <BipedalLocomotion/GenericContainer/NamedTuple.h> | ||
| #include <BipedalLocomotion/ParametersHandler/IParametersHandler.h> | ||
|
|
||
| #include <manif/SO3.h> | ||
|
|
||
| #include <Eigen/Dense> | ||
|
|
||
| namespace BipedalLocomotion | ||
| { | ||
| namespace ContinuousDynamicalSystem | ||
| { | ||
|
|
||
| // Forward declare for type traits specialization | ||
| class FloatingBaseSystemAccelerationKinematics; | ||
| } // namespace ContinuousDynamicalSystem | ||
| } // namespace BipedalLocomotion | ||
|
|
||
| namespace BipedalLocomotion::ContinuousDynamicalSystem::internal | ||
| { | ||
| template <> struct traits<FloatingBaseSystemAccelerationKinematics> | ||
| { | ||
| using Twist = Eigen::Matrix<double, 6, 1>; | ||
| using State = GenericContainer::named_tuple<BLF_NAMED_PARAM(p, Eigen::Vector3d), | ||
| BLF_NAMED_PARAM(R, manif::SO3d), | ||
| BLF_NAMED_PARAM(s, Eigen::VectorXd), | ||
| BLF_NAMED_PARAM(dp, Eigen::Vector3d), | ||
| BLF_NAMED_PARAM(omega, manif::SO3d::Tangent), | ||
| BLF_NAMED_PARAM(ds, Eigen::VectorXd)>; | ||
| using StateDerivative | ||
| = GenericContainer::named_tuple<BLF_NAMED_PARAM(dp, Eigen::Vector3d), | ||
| BLF_NAMED_PARAM(omega, manif::SO3d::Tangent), | ||
| BLF_NAMED_PARAM(ds, Eigen::VectorXd), | ||
| BLF_NAMED_PARAM(ddp, Eigen::Vector3d), | ||
| BLF_NAMED_PARAM(domega, manif::SO3d::Tangent), | ||
| BLF_NAMED_PARAM(dds, Eigen::VectorXd)>; | ||
| using Input = GenericContainer::named_tuple<BLF_NAMED_PARAM(dtwist, Twist), | ||
| BLF_NAMED_PARAM(dds, Eigen::VectorXd)>; | ||
| using DynamicalSystem = FloatingBaseSystemAccelerationKinematics; | ||
| }; | ||
| } // namespace BipedalLocomotion::ContinuousDynamicalSystem::internal | ||
|
|
||
| namespace BipedalLocomotion | ||
| { | ||
| namespace ContinuousDynamicalSystem | ||
| { | ||
|
|
||
| /** | ||
| * FloatingBaseSystemAccelerationKinematics describes a floating base system kinematics. | ||
| * The FloatingBaseSystemAccelerationKinematics inherits from a generic DynamicalSystem where the State | ||
| * is described by a BipedalLocomotion::GenericContainer::named_tuple | Name | Type | | ||
| * Description | | ||
| * |:----:|:-----------------:|:--------------------------------------------------------------------------------------------------------------------------------------------:| | ||
| * | `p` | `Eigen::Vector3d` | Position of the base | ||
| * w.r.t. the inertial frame | | `R` | `manif::SO3d` | ||
| * | Rotation matrix \f${} ^ I R _ {b}\f$. Matrix that transform a vector whose coordinates are | ||
| * expressed in the base frame in the inertial frame | | `s` | `Eigen::VectorXd` | Joint positions | ||
| * [in rad] | | ||
| * | ||
| * The `StateDerivative` is described by a BipedalLocomotion::GenericContainer::named_tuple | ||
| * | Name | Type | Description | | ||
| * |:-------:|:----------------------:|:---------------------------------------------------------------------------------------------------------------------------:| | ||
| * | `dp` | `Eigen::Vector3d` | Linear Acceleration of the origin of the base link whose | ||
| * coordinates are expressed in the Inertial frame (MIXED RERPESENTATION) | | `omega` | | ||
| * `manif::SO3d::Tangent` | base angular Acceleration whose coordinates are expressed in | ||
| * the inertial frame (Left trivialized) | | `ds` | `Eigen::VectorXd` | Joint | ||
| * velocities [in rad/s] | | ||
| * | ||
| * The `Input` is described by a BipedalLocomotion::GenericContainer::named_tuple | ||
| * | Name | Type | Description | | ||
| * |:-------:|:-----------------------------:|:--------------------------------------------:| | ||
| * | `twist` | `Eigen::Matrix<double, 6, 1>` | Base twist expressed in mixed representation | | ||
| * | `ds` | `Eigen::VectorXd` | Joint velocities [in rad/s] | | ||
| */ | ||
| class FloatingBaseSystemAccelerationKinematics | ||
| : public DynamicalSystem<FloatingBaseSystemAccelerationKinematics> | ||
| { | ||
| State m_state; | ||
| Input m_controlInput; | ||
|
|
||
| public: | ||
| /** | ||
| * Initialize the Dynamical system. | ||
| * @param handler pointer to the parameter handler. | ||
| * @return true in case of success/false otherwise. | ||
| * @note This function does nothing but it is required for CRTP. | ||
| */ | ||
| bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler); | ||
|
|
||
| /** | ||
| * Set the state of the dynamical system. | ||
| * @param state tuple containing a const reference to the state elements. | ||
| * @return true in case of success, false otherwise. | ||
| */ | ||
| bool setState(const State& state); | ||
|
|
||
| /** | ||
| * Get the state to the dynamical system. | ||
| * @return the current state of the dynamical system | ||
| */ | ||
| const State& getState() const; | ||
|
|
||
| /** | ||
| * Set the control input to the dynamical system. | ||
| * @param controlInput the value of the control input used to compute the system dynamics. | ||
| * @return true in case of success, false otherwise. | ||
| */ | ||
| bool setControlInput(const Input& controlInput); | ||
|
|
||
| /** | ||
| * Computes the floating based system dynamics. It return \f$f(x, u, t)\f$. | ||
| * @note The control input and the state have to be set separately with the methods | ||
| * setControlInput and setState. | ||
| * @param time the time at witch the dynamics is computed. | ||
| * @param stateDynamics tuple containing a reference to the element of the state derivative | ||
| * @return true in case of success, false otherwise. | ||
| */ | ||
| bool dynamics(const std::chrono::nanoseconds& time, StateDerivative& stateDerivative); | ||
| }; | ||
|
|
||
| } // namespace ContinuousDynamicalSystem | ||
| } // namespace BipedalLocomotion | ||
|
|
||
| #endif // BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FLOATING_BASE_SYSTEM_ACCELERATION_KINEMATICS_H |
89 changes: 89 additions & 0 deletions
89
src/ContinuousDynamicalSystem/src/FloatingBaseSystemAccelerationKinematics.cpp
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,89 @@ | ||
| /** | ||
| * @file FloatingBaseSystemAccelerationKinematics.cpp | ||
| * @authors Giulio Romualdi | ||
| * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and | ||
| * distributed under the terms of the BSD-3-Clause license. | ||
| */ | ||
|
|
||
| #include <BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemAccelerationKinematics.h> | ||
| #include <BipedalLocomotion/TextLogging/Logger.h> | ||
|
|
||
| using namespace BipedalLocomotion::ContinuousDynamicalSystem; | ||
| using namespace BipedalLocomotion::ParametersHandler; | ||
|
|
||
| bool FloatingBaseSystemAccelerationKinematics::initialize( | ||
| std::weak_ptr<const IParametersHandler> handler) | ||
| { | ||
| return true; | ||
| } | ||
|
|
||
| bool FloatingBaseSystemAccelerationKinematics::dynamics(const std::chrono::nanoseconds& time, | ||
| StateDerivative& stateDerivative) | ||
| { | ||
| using namespace BipedalLocomotion::GenericContainer::literals; | ||
|
|
||
| // get the state | ||
| const Eigen::Vector3d& basePositionState = m_state.get_from_hash<"p"_h>(); | ||
| const manif::SO3d& baseRotationState = m_state.get_from_hash<"R"_h>(); | ||
| const Eigen::VectorXd& jointPositionsState = m_state.get_from_hash<"s"_h>(); | ||
| const Eigen::Vector3d& baseLinearVelocityState = m_state.get_from_hash<"dp"_h>(); | ||
| const manif::SO3d::Tangent& baseAngularVelocityState = m_state.get_from_hash<"omega"_h>(); | ||
| const Eigen::VectorXd& jointVelocitiesState = m_state.get_from_hash<"ds"_h>(); | ||
|
|
||
| // get the state derivative | ||
| Eigen::Vector3d& baseLinearVelocityStateDerivative = stateDerivative.get_from_hash<"dp"_h>(); | ||
| manif::SO3d::Tangent& baseAngularVelocityStateDerivative | ||
| = stateDerivative.get_from_hash<"omega"_h>(); | ||
| Eigen::VectorXd& jointVelocitiesStateDerivative = stateDerivative.get_from_hash<"ds"_h>(); | ||
| Eigen::Vector3d& baseLinearAccelerationStateDerivative | ||
| = stateDerivative.get_from_hash<"ddp"_h>(); | ||
| manif::SO3d::Tangent& baseAngularAccelerationStateDerivative | ||
| = stateDerivative.get_from_hash<"domega"_h>(); | ||
| Eigen::VectorXd& jointAccelerationsStateDerivative = stateDerivative.get_from_hash<"dds"_h>(); | ||
|
|
||
| // get the control input | ||
| const Eigen::Matrix<double, 6, 1>& baseAcceleration | ||
| = m_controlInput.get_from_hash<"dtwist"_h>(); | ||
| const Eigen::VectorXd& jointAcceleration = m_controlInput.get_from_hash<"dds"_h>(); | ||
|
|
||
| // check the size of the joint vectors | ||
| if (jointAcceleration.size() != jointPositionsState.size() | ||
| || jointVelocitiesState.size() != jointPositionsState.size() | ||
| || jointVelocitiesStateDerivative.size() != jointPositionsState.size() | ||
| || jointAccelerationsStateDerivative.size() != jointPositionsState.size()) | ||
| { | ||
| log()->error("[FloatingBaseSystemAccelerationKinematics::dynamics] Wrong size of the " | ||
| "vectors."); | ||
| return false; | ||
| } | ||
|
|
||
| baseLinearVelocityStateDerivative = baseLinearVelocityState; | ||
| baseAngularVelocityStateDerivative = baseAngularVelocityState; | ||
| jointVelocitiesStateDerivative = jointVelocitiesState; | ||
|
|
||
| // we assume the base acceleration is expressed in the mixed representation | ||
| baseLinearAccelerationStateDerivative = baseAcceleration.head<3>(); | ||
| baseAngularAccelerationStateDerivative = baseAcceleration.tail<3>(); | ||
| jointAccelerationsStateDerivative = jointAcceleration; | ||
|
|
||
| return true; | ||
| } | ||
|
|
||
| bool FloatingBaseSystemAccelerationKinematics::setState( | ||
| const FloatingBaseSystemAccelerationKinematics::State& state) | ||
| { | ||
| m_state = state; | ||
| return true; | ||
| } | ||
|
|
||
| const FloatingBaseSystemAccelerationKinematics::State& | ||
| FloatingBaseSystemAccelerationKinematics::getState() const | ||
| { | ||
| return m_state; | ||
| } | ||
|
|
||
| bool FloatingBaseSystemAccelerationKinematics::setControlInput(const Input& controlInput) | ||
| { | ||
| m_controlInput = controlInput; | ||
| return true; | ||
| } |