*= operator should not be deprecated YET

epfl-lasa · Jun 21, 2021 · c2f8c32 · c2f8c32
1 parent fa4bcf9
commit c2f8c32
Show file tree

Hide file tree

Showing 5 changed files with 21 additions and 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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) {