diff --git a/CHANGELOG.md b/CHANGELOG.md index 0610819a8..8d9df035c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,6 +13,16 @@ Release Versions: - Add CI workflow for Python bindings (#159) - Add emtpy constructors for Circular and Ring DS (#154) - Update ROS1 example because simulation is more developed (#160) +- Define `inverse` and `*` operators for Cartesian types explicitly (#158) + +## Important TODOs + +- Revise `*=` and `*` operators in Cartesian types before the next release with + breaking changes (some are marked *deprecated*, and some are left as is, but + they should be deleted). See issue #156 +- Add the wrench computation in the `*` operator and `inverse` function (#134) +- Refactor and improve unittests for state_representation (especially JointState + and CartesianState) ## 3.0.0 diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp index f2336fdba..d76b34132 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp @@ -7,9 +7,11 @@ #include "state_representation/space/cartesian/CartesianState.hpp" #include "state_representation/space/cartesian/CartesianTwist.hpp" +#include "state_representation/space/cartesian/CartesianWrench.hpp" namespace state_representation { class CartesianTwist; +class CartesianWrench; /** * @class CartesianPose @@ -157,6 +159,27 @@ class CartesianPose : public CartesianState { */ CartesianPose operator*(const CartesianPose& pose) const; + /** + * @brief Overload the * operator + * @param state CartesianState to multiply with + * @return the current CartesianPose multiplied by the CartesianState given in argument + */ + CartesianState operator*(const CartesianState& state) const; + + /** + * @brief Overload the * operator + * @param twist CartesianTwist to multiply with + * @return the current CartesianPose multiplied by the CartesianTwist given in argument + */ + CartesianTwist operator*(const CartesianTwist& twist) const; + + /** + * @brief Overload the * operator + * @param wrench CartesianWrench to multiply with + * @return the current CartesianPose multiplied by the CartesianWrench given in argument + */ + CartesianWrench operator*(const CartesianWrench& wrench) const; + /** * @brief Overload the *= operator with a scalar * @param lambda the scalar to multiply with @@ -225,6 +248,12 @@ class CartesianPose : public CartesianState { */ std::vector norms(const CartesianStateVariable& state_variable_type = CartesianStateVariable::POSE) const override; + /** + * @brief Compute the inverse of the current CartesianPose + * @return the inverse corresponding to b_S_f (assuming this is f_S_b) + */ + CartesianPose inverse() const; + /** * @brief Compute the normalized pose at the state variable given in argument (default is full pose) * @param state_variable_type the type of state variable to compute the norms on @@ -240,10 +269,17 @@ class CartesianPose : public CartesianState { */ friend std::ostream& operator<<(std::ostream& os, const CartesianPose& pose); + /** + * @brief Overload the * operator with a CartesianState + * @param state the state to multiply with + * @return the CartesianPose provided multiplied by the state + */ + friend CartesianPose operator*(const CartesianState& state, const CartesianPose& pose); + /** * @brief Overload the * operator with a scalar * @param lambda the scalar to multiply with - * @return the CartesianPose provided multiply by lambda + * @return the CartesianPose provided multiplied by lambda */ friend CartesianPose operator*(double lambda, const CartesianPose& pose); diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp index 71f7ceb15..b8faf2375 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp @@ -7,9 +7,11 @@ #include "state_representation/space/cartesian/CartesianPose.hpp" #include "state_representation/space/cartesian/CartesianState.hpp" +#include "state_representation/space/cartesian/CartesianWrench.hpp" namespace state_representation { class CartesianPose; +class CartesianWrench; /** * @class CartesianTwist @@ -132,7 +134,28 @@ class CartesianTwist : public CartesianState { * @param twist CartesianTwist to multiply with * @return the current CartesianTwist multiplied by the CartesianTwist given in argument */ - CartesianTwist operator*(const CartesianTwist& twist) const; + [[deprecated]] CartesianTwist operator*(const CartesianTwist& twist) const; + + /** + * @brief Overload the * operator + * @param state CartesianState to multiply with + * @return the current CartesianTwist multiplied by the CartesianState given in argument + */ + [[deprecated]] CartesianState operator*(const CartesianState& state) const; + + /** + * @brief Overload the * operator + * @param state CartesianPose to multiply with + * @return the current CartesianTwist multiplied by the CartesianPose given in argument + */ + [[deprecated]] CartesianPose operator*(const CartesianPose& pose) const; + + /** + * @brief Overload the * operator + * @param state CartesianWrench to multiply with + * @return the current CartesianTwist multiplied by the CartesianWrench given in argument + */ + [[deprecated]] CartesianWrench operator*(const CartesianWrench& wrench) const; /** * @brief Overload the += operator @@ -228,6 +251,12 @@ class CartesianTwist : public CartesianState { */ Eigen::VectorXd data() const; + /** + * @brief Compute the inverse of the current CartesianTwist + * @return the inverse corresponding to b_S_f (assuming this is f_S_b) + */ + CartesianTwist inverse() const; + /** * @brief Compute the norms of the state variable specified by the input type (default is full twist) * @param state_variable_type the type of state variable to compute the norms on @@ -250,6 +279,13 @@ class CartesianTwist : public CartesianState { */ friend std::ostream& operator<<(std::ostream& os, const CartesianTwist& twist); + /** + * @brief Overload the * operator with a CartesianState + * @param state the state to multiply with + * @return the CartesianTwist provided multiplied by the state + */ + friend CartesianTwist operator*(const CartesianState& state, const CartesianTwist& twist); + /** * @brief Overload the * operator with a scalar * @param lambda the scalar to multiply with diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp index 663f9773b..568cb6546 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp @@ -6,8 +6,12 @@ #pragma once #include "state_representation/space/cartesian/CartesianState.hpp" +#include "state_representation/space/cartesian/CartesianPose.hpp" +#include "state_representation/space/cartesian/CartesianTwist.hpp" namespace state_representation { +class CartesianPose; +class CartesianTwist; /** * @class CartesianWrench * @brief Class to define wrench in cartesian space as 3D force and torque vectors @@ -124,7 +128,28 @@ class CartesianWrench : public CartesianState { * @param wrench CartesianWrench to multiply with * @return the current CartesianWrench multiplied by the CartesianWrench given in argument */ - CartesianWrench operator*(const CartesianWrench& wrench) const; + [[deprecated]] CartesianWrench operator*(const CartesianWrench& wrench) const; + + /** + * @brief Overload the * operator + * @param state CartesianState to multiply with + * @return the current CartesianWrench multiplied by the CartesianState given in argument + */ + [[deprecated]] CartesianState operator*(const CartesianState& state) const; + + /** + * @brief Overload the * operator + * @param state CartesianPose to multiply with + * @return the current CartesianWrench multiplied by the CartesianPose given in argument + */ + [[deprecated]] CartesianPose operator*(const CartesianPose& pose) const; + + /** + * @brief Overload the * operator + * @param state CartesianWrench to multiply with + * @return the current CartesianWrench multiplied by the CartesianTwist given in argument + */ + [[deprecated]] CartesianTwist operator*(const CartesianTwist& twist) const; /** * @brief Overload the += operator @@ -206,6 +231,12 @@ class CartesianWrench : public CartesianState { */ Eigen::VectorXd data() const; + /** + * @brief Compute the inverse of the current CartesianWrench + * @return the inverse corresponding to b_S_f (assuming this is f_S_b) + */ + CartesianWrench inverse() const; + /** * @brief Compute the norms of the state variable specified by the input type (default is full wrench) * @param state_variable_type the type of state variable to compute the norms on @@ -228,10 +259,17 @@ class CartesianWrench : public CartesianState { */ friend std::ostream& operator<<(std::ostream& os, const CartesianWrench& wrench); + /** + * @brief Overload the * operator with a CartesianState + * @param state the state to multiply with + * @return the CartesianWrench provided multiplied by the state + */ + friend CartesianWrench operator*(const CartesianState& state, const CartesianWrench& wrench); + /** * @brief Overload the * operator with a scalar * @param lambda the scalar to multiply with - * @return the CartesianWrench provided multiply by lambda + * @return the CartesianWrench provided multiplied by lambda */ friend CartesianWrench operator*(double lambda, const CartesianWrench& wrench); }; diff --git a/source/state_representation/src/space/cartesian/CartesianPose.cpp b/source/state_representation/src/space/cartesian/CartesianPose.cpp index 662c69b9e..debb6c019 100644 --- a/source/state_representation/src/space/cartesian/CartesianPose.cpp +++ b/source/state_representation/src/space/cartesian/CartesianPose.cpp @@ -66,6 +66,18 @@ CartesianPose CartesianPose::operator*(const CartesianPose& pose) const { return this->CartesianState::operator*(pose); } +CartesianState CartesianPose::operator*(const CartesianState& state) const { + return this->CartesianState::operator*(state); +} + +CartesianTwist CartesianPose::operator*(const CartesianTwist& twist) const { + return this->CartesianState::operator*(twist); +} + +CartesianWrench CartesianPose::operator*(const CartesianWrench& wrench) const { + return this->CartesianState::operator*(wrench); +} + CartesianPose& CartesianPose::operator*=(double lambda) { this->CartesianState::operator*=(lambda); return (*this); @@ -123,6 +135,10 @@ Eigen::VectorXd CartesianPose::data() const { return this->get_pose(); } +CartesianPose CartesianPose::inverse() const { + return this->CartesianState::inverse(); +} + std::ostream& operator<<(std::ostream& os, const CartesianPose& pose) { if (pose.is_empty()) { os << "Empty CartesianPose"; @@ -144,6 +160,10 @@ std::ostream& operator<<(std::ostream& os, const CartesianPose& pose) { return os; } +CartesianPose operator*(const CartesianState& state, const CartesianPose& pose) { + return state.operator*(pose); +} + CartesianPose operator*(double lambda, const CartesianPose& pose) { return pose * lambda; } diff --git a/source/state_representation/src/space/cartesian/CartesianState.cpp b/source/state_representation/src/space/cartesian/CartesianState.cpp index b99c2aed5..7d41f38a4 100644 --- a/source/state_representation/src/space/cartesian/CartesianState.cpp +++ b/source/state_representation/src/space/cartesian/CartesianState.cpp @@ -118,7 +118,7 @@ CartesianState& CartesianState::operator*=(const CartesianState& state) { this->set_angular_velocity(f_omega_b + f_R_b * b_omega_c); // acceleration this->set_linear_acceleration(f_a_b + f_R_b * b_a_c - + f_alpha_b.cross(f_R_b * b_P_c) + + f_alpha_b.cross(f_R_b * b_P_c) + 2 * f_omega_b.cross(f_R_b * b_v_c) + f_omega_b.cross(f_omega_b.cross(f_R_b * b_P_c))); this->set_angular_acceleration(f_alpha_b + f_R_b * b_alpha_c + f_omega_b.cross(f_R_b * b_omega_c)); @@ -201,8 +201,7 @@ CartesianState CartesianState::inverse() const { CartesianState result(*this); // inverse name and reference frame std::string ref = result.get_reference_frame(); - std::string name = result.get_name(); - result.set_reference_frame(name); + result.set_reference_frame(result.get_name()); result.set_name(ref); // intermediate variables for f_S_b Eigen::Vector3d f_P_b = this->get_position(); diff --git a/source/state_representation/src/space/cartesian/CartesianTwist.cpp b/source/state_representation/src/space/cartesian/CartesianTwist.cpp index 96523b575..a9544b1ab 100644 --- a/source/state_representation/src/space/cartesian/CartesianTwist.cpp +++ b/source/state_representation/src/space/cartesian/CartesianTwist.cpp @@ -60,6 +60,20 @@ CartesianTwist CartesianTwist::operator*(const CartesianTwist& twist) const { return this->CartesianState::operator*(twist); } +CartesianState CartesianTwist::operator*(const CartesianState& state) const { + return this->CartesianState::operator*(state); +} + + +CartesianPose CartesianTwist::operator*(const CartesianPose& pose) const { + return this->CartesianState::operator*(pose); +} + + +CartesianWrench CartesianTwist::operator*(const CartesianWrench& wrench) const { + return this->CartesianState::operator*(wrench); +} + CartesianTwist& CartesianTwist::operator+=(const CartesianTwist& twist) { this->CartesianState::operator+=(twist); return (*this); @@ -149,6 +163,10 @@ Eigen::VectorXd CartesianTwist::data() const { return this->get_twist(); } +CartesianTwist CartesianTwist::inverse() const { + return this->CartesianState::inverse(); +} + std::ostream& operator<<(std::ostream& os, const CartesianTwist& twist) { if (twist.is_empty()) { os << "Empty CartesianTwist"; @@ -164,6 +182,10 @@ std::ostream& operator<<(std::ostream& os, const CartesianTwist& twist) { return os; } +CartesianTwist operator*(const CartesianState& state, const CartesianTwist& twist) { + return state.operator*(twist); +} + CartesianTwist operator*(double lambda, const CartesianTwist& twist) { return twist * lambda; } diff --git a/source/state_representation/src/space/cartesian/CartesianWrench.cpp b/source/state_representation/src/space/cartesian/CartesianWrench.cpp index 134cd09c9..930f25873 100644 --- a/source/state_representation/src/space/cartesian/CartesianWrench.cpp +++ b/source/state_representation/src/space/cartesian/CartesianWrench.cpp @@ -54,6 +54,20 @@ CartesianWrench CartesianWrench::operator*(const CartesianWrench& wrench) const return this->CartesianState::operator*(wrench); } +CartesianState CartesianWrench::operator*(const CartesianState& state) const { + return this->CartesianState::operator*(state); +} + + +CartesianPose CartesianWrench::operator*(const CartesianPose& pose) const { + return this->CartesianState::operator*(pose); +} + + +CartesianTwist CartesianWrench::operator*(const CartesianTwist& twist) const { + return this->CartesianState::operator*(twist); +} + CartesianWrench& CartesianWrench::operator+=(const CartesianWrench& wrench) { this->CartesianState::operator+=(wrench); return (*this); @@ -106,6 +120,10 @@ Eigen::VectorXd CartesianWrench::data() const { return this->get_wrench(); } +CartesianWrench CartesianWrench::inverse() const { + return this->CartesianState::inverse(); +} + std::ostream& operator<<(std::ostream& os, const CartesianWrench& wrench) { if (wrench.is_empty()) { os << "Empty CartesianWrench"; @@ -122,6 +140,10 @@ std::ostream& operator<<(std::ostream& os, const CartesianWrench& wrench) { return os; } +CartesianWrench operator*(const CartesianState& state, const CartesianWrench& wrench) { + return state.operator*(wrench); +} + CartesianWrench operator*(double lambda, const CartesianWrench& wrench) { return wrench * lambda; }