Add inverse for CartesianPose and define operators explicitely (#158)

* Remove the * and *= operators because they don't make sense

* Explicitely define * operators and add inverse function

* Fix names

* Make operators deprecated and hide inverse functions

* Mark more operators as deprecated

* Change the * and *= operator

* Remove unnecessary casting

* *= operator should not be deprecated YET

* Update changelog

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
 

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<double> 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);
 

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

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);
 };

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;
 }

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();

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;
 }

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;
 }