From 97bd5f63d6757d25899b1416f60d9fffb13792cb Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Mon, 21 Jun 2021 18:06:55 +0200 Subject: [PATCH] *= operator should not be deprecated YET --- .../space/cartesian/CartesianPose.hpp | 2 +- .../space/cartesian/CartesianState.hpp | 2 +- .../space/cartesian/CartesianTwist.hpp | 2 +- .../space/cartesian/CartesianWrench.hpp | 4 +-- .../src/space/cartesian/CartesianState.cpp | 36 +++++++++---------- 5 files changed, 21 insertions(+), 25 deletions(-) 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 3adf5ea0a..d76b34132 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp @@ -150,7 +150,7 @@ class CartesianPose : public CartesianState { * @param pose CartesianPose to multiply with * @return the current CartesianPose multiplied by the CartesianPose given in argument */ - [[deprecated]] CartesianPose& operator*=(const CartesianPose& pose); + CartesianPose& operator*=(const CartesianPose& pose); /** * @brief Overload the * operator diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp index 6895deff1..c6b054936 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp @@ -379,7 +379,7 @@ class CartesianState : public SpatialState { * @param state the state to compose with corresponding to b_S_c * @return the CartesianState corresponding f_S_c = f_S_b * b_S_c (assuming this is f_S_b) */ - [[deprecated]] CartesianState& operator*=(const CartesianState& state); + CartesianState& operator*=(const CartesianState& state); /** * @brief Overload the * operator with another state by deriving the equations of motions 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 893c84ef8..b8faf2375 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp @@ -127,7 +127,7 @@ class CartesianTwist : public CartesianState { * @param twist CartesianTwist to multiply with * @return the current CartesianTwist multiplied by the CartesianTwist given in argument */ - [[deprecated]] CartesianTwist& operator*=(const CartesianTwist& twist); + CartesianTwist& operator*=(const CartesianTwist& twist); /** * @brief Overload the * operator with a twist 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 dd23a24fe..568cb6546 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp @@ -121,7 +121,7 @@ class CartesianWrench : public CartesianState { * @param wrench CartesianWrench to multiply with * @return the current CartesianWrench multiplied by the CartesianWrench given in argument */ - [[deprecated]] CartesianWrench& operator*=(const CartesianWrench& wrench); + CartesianWrench& operator*=(const CartesianWrench& wrench); /** * @brief Overload the * operator with a wrench @@ -269,7 +269,7 @@ class CartesianWrench : public CartesianState { /** * @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/CartesianState.cpp b/source/state_representation/src/space/cartesian/CartesianState.cpp index ed4660abd..7d41f38a4 100644 --- a/source/state_representation/src/space/cartesian/CartesianState.cpp +++ b/source/state_representation/src/space/cartesian/CartesianState.cpp @@ -84,12 +84,6 @@ Eigen::ArrayXd CartesianState::array() const { } CartesianState& CartesianState::operator*=(const CartesianState& state) { - *this = *this * state; - return (*this); -} - -CartesianState CartesianState::operator*(const CartesianState& state) const { - CartesianState result(*this); // sanity check if (this->is_empty()) { throw EmptyStateException(this->get_name() + " state is empty"); @@ -100,7 +94,7 @@ CartesianState CartesianState::operator*(const CartesianState& state) const { if (this->get_name() != state.get_reference_frame()) { throw IncompatibleReferenceFramesException("Expected " + this->get_name() + ", got " + state.get_reference_frame()); } - result.set_name(state.get_name()); + this->set_name(state.get_name()); // intermediate variables for f_S_b Eigen::Vector3d f_P_b = this->get_position(); Eigen::Quaterniond f_R_b = this->get_orientation(); @@ -117,24 +111,26 @@ CartesianState CartesianState::operator*(const CartesianState& state) const { Eigen::Vector3d b_a_c = state.get_linear_acceleration(); Eigen::Vector3d b_alpha_c = state.get_angular_acceleration(); // pose - result.set_position(f_P_b + f_R_b * b_P_c); - result.set_orientation(f_R_b * b_R_c); + this->set_position(f_P_b + f_R_b * b_P_c); + this->set_orientation(f_R_b * b_R_c); // twist - result.set_linear_velocity(f_v_b + f_R_b * b_v_c + f_omega_b.cross(f_R_b * b_P_c)); - result.set_angular_velocity(f_omega_b + f_R_b * b_omega_c); + this->set_linear_velocity(f_v_b + f_R_b * b_v_c + f_omega_b.cross(f_R_b * b_P_c)); + this->set_angular_velocity(f_omega_b + f_R_b * b_omega_c); // acceleration - result.set_linear_acceleration(f_a_b + f_R_b * b_a_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))); - result.set_angular_acceleration(f_alpha_b + f_R_b * b_alpha_c + f_omega_b.cross(f_R_b * b_omega_c)); + this->set_linear_acceleration(f_a_b + f_R_b * b_a_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)); // wrench //TODO - return result; + return (*this); +} -// CartesianState result(*this); -// result *= state; -// return result; +CartesianState CartesianState::operator*(const CartesianState& state) const { + CartesianState result(*this); + result *= state; + return result; } CartesianState& CartesianState::operator+=(const CartesianState& state) {