Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add inverse for CartesianPose and define operators explicitely #158

Merged
merged 9 commits into from
Jul 3, 2021
Merged

Add inverse for CartesianPose and define operators explicitely #158

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
a016f7e
Remove the * and *= operators because they don't make sense
Jun 18, 2021
d3e31dc
Explicitely define * operators and add inverse function
Jun 18, 2021
a5d16fb
Fix names
Jun 18, 2021
d75c356
Make operators deprecated and hide inverse functions
Jun 18, 2021
bc1d463
Mark more operators as deprecated
Jun 19, 2021
da46390
Change the * and *= operator
Jun 19, 2021
8cca183
Remove unnecessary casting
Jun 21, 2021
97bd5f6
*= operator should not be deprecated YET
Jun 21, 2021
b8822f4
Update changelog
Jun 23, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 10 additions & 0 deletions CHANGELOG.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
38 changes: 37 additions & 1 deletion source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The CartesianState inverse() const is not deleted in this header, which means this new declaration is hiding the base method. It would be safer to delete the base inverse method in this header as well.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If I add the line

  CartesianState inverse() const = delete;

to the CartesianPose header, I cannot build state_representation anymore

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ah ok, my bad. Then instead you should mark the CartesianState::inverse as virtual and explicitly override the derived version here.

// in CartesianState.hpp
virtual CartesianState CartesianState::inverse() const;

// in CartesianPose.hpp
CartesianPose CartesianPose::inverse() const override;

This should work because the return type of the derived function is a co-variant (the derived class) of the base function (the base class).

Copy link
Contributor Author

@domire8 domire8 Jun 18, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No, that doesn't work unfortunately... Even though it should...

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Actually, there are other functions like that (the copy one) that are not defined virtual or deleted but that hide the CartesianState function


/**
* @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
Expand All @@ -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);

Expand Down
38 changes: 37 additions & 1 deletion source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

why do you need to deprecate this one? It was actually never implemented explicitly before. I think you can just deprecate the one with state and leave off the other ones otherwise you introduce also some forward declaration for wrench that is not needed

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ok I think I understand that you need those to mark them as removed, as you don't want to deprecate the operator* which will be called if those are not implemented. Is that correct? Then yes I guess no other choices and forget the other comments.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes indeed, thats what you also said, if we explicitly implement one, we need to do all of them :)


/**
* @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;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

same as above


/**
* @brief Overload the += operator
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down
42 changes: 40 additions & 2 deletions source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

same as for the twist so I don't think this is needed

/**
* @class CartesianWrench
* @brief Class to define wrench in cartesian space as 3D force and torque vectors
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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);
};
Expand Down
20 changes: 20 additions & 0 deletions source/state_representation/src/space/cartesian/CartesianPose.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -123,6 +135,10 @@ Eigen::VectorXd CartesianPose::data() const {
return this->get_pose();
}

CartesianPose CartesianPose::inverse() const {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

why not use the CartesianState::inverse() and then downcast the return back to a pose? it would prevent some code duplication, and I would say the few extra linear operations are not hugely costly. It's also how the other operators are working anyway

return this->CartesianState::inverse();
}

std::ostream& operator<<(std::ostream& os, const CartesianPose& pose) {
if (pose.is_empty()) {
os << "Empty CartesianPose";
Expand All @@ -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;
}
Expand Down
5 changes: 2 additions & 3 deletions source/state_representation/src/space/cartesian/CartesianState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down Expand Up @@ -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();
Expand Down
22 changes: 22 additions & 0 deletions source/state_representation/src/space/cartesian/CartesianTwist.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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";
Expand All @@ -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;
}
Expand Down
22 changes: 22 additions & 0 deletions source/state_representation/src/space/cartesian/CartesianWrench.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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";
Expand All @@ -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;
}
Expand Down