fix: adds gazebo gravity vector to the gravity calculation #177

rickstaa · 2021-10-08T14:43:04Z

This pull request makes sure that the gravity calculation uses the gravity vector that comes from the Gazebo simulation.

In the old version the default value set in the model_base.h class was used (i.e. [0, 0, -9.81]) while gazebo uses the following gravity vector [0, 0, -9.8000000000000007].

franka_ros/franka_hw/include/franka_hw/model_base.h

Lines 236 to 238 in 1922fc9

    
           std::array<double, 7> gravity(const franka::RobotState& robot_state) const { 
        
             return gravity(robot_state.q, robot_state.m_total, robot_state.F_x_Ctotal, {0, 0, -9.81}); 
        
           }

This is because the gravity_earth argument is not used.

franka_ros/franka_gazebo/src/franka_hw_sim.cpp

Line 306 in 1922fc9

auto g = this->model_->gravity(this->robot_state_);

Other todos

Update documentation.

This commit adds a temporary gravity compensation hotfix for the controllering the `franka_gazebo` simulation (see frankaemika/franka_ros#160). Can be removed if frankaemika/franka_ros#177 is merged.

gollth

Thank you for this improvement @rickstaa, definitely makes sense!

I could imagine use cases, though, where you want to provide custom directions/strengths for the gravity vector. For example one might want to simulate their controller if the robot is in free space. For that the gravity in Gazebo would be set to zero. The Master Controller of the real robot always compensates the gravity by ~9.81 m/s^2, which the controller of such user would need to account for.

Other use cases from the top of my head:

Simulation of the Robot mounted non-tabletop, say on a wall
Simulation of the robot on different places with different strengths of gravity vectors (poles, equator...)

To encompass these use cases as well, we need a way to specify if franka_hw_sim should use the gravity vector from Gazebo or a custom one.

My suggestion would be to make this configurable by ROS parameter. When the parameter is set, then this would be taken as gravity vector. If it is missing, then we could read it from Gazebo. This would also make a good default IMO.

If you agree with this approach, I would like to ask you to implement this. For reference you can have a look at FrankaHWSim::readParameter() (probably the place, where this logic belongs as well):

https://github.com/frankaemika/franka_ros/blob/develop/franka_gazebo/src/franka_hw_sim.cpp#L334-L380

franka_gazebo/src/franka_hw_sim.cpp

rickstaa · 2021-11-03T14:39:21Z

Thank you for this improvement @rickstaa, definitely makes sense!

I could imagine use cases, though, where you want to provide custom directions/strengths for the gravity vector. For example one might want to simulate their controller if the robot is in free space. For that the gravity in Gazebo would be set to zero. The Master Controller of the real robot always compensates the gravity by ~9.81 m/s^2, which the controller of such user would need to account for.

Other use cases from the top of my head:
* Simulation of the Robot mounted non-tabletop, say on a wall

* Simulation of the robot on different places with different strengths of gravity vectors (poles, equator...)
To encompass these use cases as well, we need a way to specify if franka_hw_sim should use the gravity vector from Gazebo or a custom one.

My suggestion would be to make this configurable by ROS parameter. When the parameter is set, then this would be taken as gravity vector. If it is missing, then we could read it from Gazebo. This would also make a good default IMO.

If you agree with this approach, I would like to ask you to implement this. For reference, you can have a look at FrankaHWSim::readParameter() (probably the place, where this logic belongs as well):

https://github.com/frankaemika/franka_ros/blob/develop/franka_gazebo/src/franka_hw_sim.cpp#L334-L380

I like your suggestion. I will add it to this pull request.

CHANGELOG.md

rickstaa · 2021-11-23T10:40:35Z

franka_gazebo/src/franka_hw_sim.cpp

@@ -331,7 +336,7 @@ void FrankaHWSim::writeSim(ros::Time /*time*/, ros::Duration /*period*/) {

 void FrankaHWSim::eStopActive(bool /* active */) {}

-bool FrankaHWSim::readParameters(const ros::NodeHandle& nh, const urdf::Model& urdf) {
+auto FrankaHWSim::readParameters(const ros::NodeHandle& nh, const urdf::Model& urdf) -> bool {


Automatically applied by clang-tidy.

the const in the for loop can stay, however the trailing return types, look just look ugly imo. We currently use clang-tidy 7 in the CI. There are lots of things that could be fixed with a newer version of clang-tidy, however there are other lints like modernize-use-trailing-return-type or readability-function-cognitive-complexity that you do not want to have. Therefore, we currently stick to clang-tidy 7.

Strange that the extension does not pick up the right clang-tidy config. I reverted the clang-tidy changes manually.

It is because we include modernize* and readability*, so if new checks appear they will be included by default. Thats why you need to choose the correct clang-tidy version, so just use clang-tidy 7.

Ah over overlooked that in your previous answer. I'm on clang-tidy 10.

rickstaa · 2021-11-23T10:40:50Z

franka_gazebo/src/franka_hw_sim.cpp

                          ros::NodeHandle model_nh,
                          gazebo::physics::ModelPtr parent,
                          const urdf::Model* const urdf,
-                          std::vector<transmission_interface::TransmissionInfo> transmissions) {
+                          std::vector<transmission_interface::TransmissionInfo> transmissions) -> bool {


Automatically applied by clang-tidy. I can remove the automatic clang-tidy changes if you want.

rickstaa · 2021-11-23T10:41:18Z

franka_gazebo/src/franka_hw_sim.cpp

@@ -200,7 +205,7 @@ void FrankaHWSim::initFrankaModelHandle(
        " joints were found beneath the <transmission> tag, but 2 are required.");
  }

-  for (auto& joint : transmission.joints_) {
+  for (const auto & joint : transmission.joints_) {


Automatically applied by clang-tidy.

There is a space between the auto and the &. Actually this should not pass the CI 🤔

Ah, that's strange.

I also noticed that the clang-tidy vscode doesn't take that rule into account when formatting. This can of course be a bug with the extensions and not the .clang-tidy config. If it is a bug in the extension it will probably be fixed when the C++ extension adds clang-tidy support (see microsoft/vscode-cpptools#2908).

rickstaa · 2021-11-23T10:42:46Z

@gollth, @marcbone I implemented the requested changes and rebased on the develop branch. I used g_vector for the gravity ROS parameter, but I can change it to your liking.

gollth · 2021-11-23T17:17:53Z

I vote for gravity_vector for clarity, other than that, LGTM

This commit adds the 'gravity_vector' ROS parameter. This parameter can be used to set the gravity vector used in the FrankaHWSim. If the 'g_vector' parameter is not set the gravity vector will be retrieved from Gazebo.

rickstaa · 2021-11-23T17:22:37Z

Great I updated the code to reflect that change.

rickstaa · 2021-11-24T15:43:30Z

Thanks a lot for merging this!

…o develop * commit 'bcd58208ce0b003e6621ad211139b2e9b545fecb': add catkin_install_python to CMakeLists.txt to generate proper shebang headers

rickstaa mentioned this pull request Oct 8, 2021

Gazebo model joints move by themselves #160

Open

rickstaa mentioned this pull request Oct 22, 2021

Noetic release moveit/panda_moveit_config#89

Closed

26 tasks

gollth reviewed Nov 3, 2021

View reviewed changes

franka_gazebo/src/franka_hw_sim.cpp Outdated Show resolved Hide resolved

rickstaa force-pushed the fix_gazebo_gravity_compensation branch 4 times, most recently from 87a7864 to f35b14a Compare November 3, 2021 14:06

gollth reviewed Nov 3, 2021

View reviewed changes

CHANGELOG.md Outdated Show resolved Hide resolved

rickstaa mentioned this pull request Nov 23, 2021

Adds the 'transmission' arg to the 'panda_arm.urdf.xacro' file #157

Closed

rickstaa force-pushed the fix_gazebo_gravity_compensation branch 4 times, most recently from b90980e to 6ba5602 Compare November 23, 2021 10:39

rickstaa commented Nov 23, 2021

View reviewed changes

rickstaa force-pushed the fix_gazebo_gravity_compensation branch 3 times, most recently from 5f6cacd to e49dc35 Compare November 23, 2021 13:35

rickstaa requested a review from marcbone November 23, 2021 14:05

rickstaa force-pushed the fix_gazebo_gravity_compensation branch from e49dc35 to 211ff3d Compare November 23, 2021 17:21

fix(FrankaHWSim): add 'gravity_vector' gravity ROS parameter

53a9be0

This commit adds the 'gravity_vector' ROS parameter. This parameter can be used to set the gravity vector used in the FrankaHWSim. If the 'g_vector' parameter is not set the gravity vector will be retrieved from Gazebo.

rickstaa force-pushed the fix_gazebo_gravity_compensation branch from 211ff3d to 53a9be0 Compare November 23, 2021 17:21

rickstaa requested a review from gollth November 23, 2021 17:22

gollth approved these changes Nov 24, 2021

View reviewed changes

gollth merged commit 4fc4042 into frankaemika:develop Nov 24, 2021

rickstaa deleted the fix_gazebo_gravity_compensation branch November 24, 2021 15:43

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fix: adds gazebo gravity vector to the gravity calculation #177

fix: adds gazebo gravity vector to the gravity calculation #177

rickstaa commented Oct 8, 2021 •

edited

Loading

gollth left a comment

rickstaa commented Nov 3, 2021

rickstaa Nov 23, 2021 •

edited

Loading

marcbone Nov 23, 2021

rickstaa Nov 23, 2021

marcbone Nov 24, 2021

rickstaa Nov 24, 2021

rickstaa Nov 23, 2021 •

edited

Loading

rickstaa Nov 23, 2021

marcbone Nov 23, 2021

rickstaa Nov 23, 2021

rickstaa commented Nov 23, 2021

gollth commented Nov 23, 2021

rickstaa commented Nov 23, 2021

rickstaa commented Nov 24, 2021

	std::array<double, 7> gravity(const franka::RobotState& robot_state) const {
	return gravity(robot_state.q, robot_state.m_total, robot_state.F_x_Ctotal, {0, 0, -9.81});
	}

fix: adds gazebo gravity vector to the gravity calculation #177

fix: adds gazebo gravity vector to the gravity calculation #177

Conversation

rickstaa commented Oct 8, 2021 • edited Loading

Other todos

gollth left a comment

Choose a reason for hiding this comment

rickstaa commented Nov 3, 2021

rickstaa Nov 23, 2021 • edited Loading

Choose a reason for hiding this comment

marcbone Nov 23, 2021

Choose a reason for hiding this comment

rickstaa Nov 23, 2021

Choose a reason for hiding this comment

marcbone Nov 24, 2021

Choose a reason for hiding this comment

rickstaa Nov 24, 2021

Choose a reason for hiding this comment

rickstaa Nov 23, 2021 • edited Loading

Choose a reason for hiding this comment

rickstaa Nov 23, 2021

Choose a reason for hiding this comment

marcbone Nov 23, 2021

Choose a reason for hiding this comment

rickstaa Nov 23, 2021

Choose a reason for hiding this comment

rickstaa commented Nov 23, 2021

gollth commented Nov 23, 2021

rickstaa commented Nov 23, 2021

rickstaa commented Nov 24, 2021

rickstaa commented Oct 8, 2021 •

edited

Loading

rickstaa Nov 23, 2021 •

edited

Loading

rickstaa Nov 23, 2021 •

edited

Loading