Gazebo model joints move by themselves

[saalimperator]

Hello!

I'm currently working with your Gazebo model to test my controller. To do this, I modified one of your example controllers so that all joints except the first one keep their current angle. The first one is waiting for input from my controller, which is applied directly to the joint using the ".setCommand(tau)" function.

Now for my problem: when I start Gazebo, it seems to maintain the initial state, but after a few seconds the first joint starts to move slightly counterclockwise without any input from me. Is this normal behavior of this joint when the input is set to zero? Or is there something wrong with the inertia, friction and damping of this joint. When I send some inputs to this joint, it starts to move, but it is not slowed down by friction or damping and just bounces back when it hits the joint limits.

Can you give me a hint where I am doing something wrong?

[rickstaa]

@saalimperator Thanks for creating this issue.

It might not be related to your problem but I also noticed this behaviour even when no controllers are loaded. In that case, one would expect the robot to just fall down and stay there. The behaviour that occurs is however very strange (See gif 1 below). The first time I saw this, I thought it was because of gravity compensation, but there is no code for this the franka_state_controller or franka_gripper controllers.

This behaviour is also present when using the force_example_controller roslaunch franka_gazebo panda.launch controller:=force_example_controller which does contain gravity compensation. Probably, since the PID gains start at 0.

franka_ros/franka_example_controllers/src/force_example_controller.cpp

Line 115 in b9b60e8

// FF + PI control (PI gains are initially all 0)

)

The other strange thing is that the path that the arm takes with not controllers looks the same every time the simulation is started.

It could be related to the unstable dynamics that is described in justagist/franka_panda_description#5.

[gollth]

In that case, one would expect the robot to just fall down and stay there.

Not really. The franka_hw_sim plugin simulates the real robot in that it compensates the gravity of arm when you send zero (or no) commands from the controller.

franka_ros/franka_gazebo/src/franka_hw_sim.cpp

Lines 310 to 317 in 1922fc9

	auto command = joint->command;
	// Check if this joint is affected by gravity compensation
	std::string prefix = this->arm_id_ + "_joint";
	if (pair.first.rfind(prefix, 0) != std::string::npos) {
	int i = std::stoi(pair.first.substr(prefix.size())) - 1;
	command += g.at(i);
	}

Imagine this on the real robot. When you have a ROS controller which sends zero torque, the robot won't fall to the ground either but behaves like in "free space" essentially. This is also what happens when no controller is running.

Why the robot is drifting is unclear to me, though. I suspect numerical instabilities somewhere between franka_hw_sim and Gazebo.

when I start Gazebo, it seems to maintain the initial state, but after a few seconds the first joint starts to move slightly counterclockwise without any input from me.

@saalimperator a possible solution would be to have the controller active at all times. So while you are waiting for input, generate commands in your controller that hold the robot at a defined configuration. This should stop the drifting

[rickstaa]

In that case, one would expect the robot to just fall down and stay there.

Not really. The franka_hw_sim plugin simulates the real robot in that it compensates the gravity of arm when you send zero (or no) commands from the controller.

franka_ros/franka_gazebo/src/franka_hw_sim.cpp

Lines 310 to 317 in 1922fc9

	auto command = joint->command;
	// Check if this joint is affected by gravity compensation
	std::string prefix = this->arm_id_ + "_joint";
	if (pair.first.rfind(prefix, 0) != std::string::npos) {
	int i = std::stoi(pair.first.substr(prefix.size())) - 1;
	command += g.at(i);
	}

Imagine this on the real robot. When you have a ROS controller which sends zero torque, the robot won't fall to the ground either but behaves like in "free space" essentially. This is also what happens when no controller is running.

Why the robot is drifting is unclear to me, though. I suspect numerical instabilities somewhere between franka_hw_sim and Gazebo.

when I start Gazebo, it seems to maintain the initial state, but after a few seconds the first joint starts to move slightly counterclockwise without any input from me.

@saalimperator a possible solution would be to have the controller active at all times. So while you are waiting for input, generate commands in your controller that hold the robot at a defined configuration. This should stop the drifting

@gollth At the time of writing, I forgot to check the franka_hw_sim for any gravity compensation. Thanks for pointing me to the relevant code. In that case, numerical instabilities could indeed cause this drifting. I have seen the same behaviour in other simulated robots.

For now, in my experiments, I will use your workaround and try to send control commands to mitigate the drift.

[saalimperator]

@saalimperator a possible solution would be to have the controller active at all times. So while you are waiting for input, generate commands in your controller that hold the robot at a defined configuration. This should stop the drifting

Thanks for the answer. I already expected something like this. Unfortunately, your proposed solution does not work for my project, because I am trying to determine the complete dynamics of the robot by means of an experiment. I have already found a better simulation environment for my case. Much success with the programming!

[rickstaa]

@saalimperator Just out of interest, may I ask which simulation environment you switched?

@gollth As this bug leads to strange behaviour, I was wondering if there are plans to fix this bug in the future?

[saalimperator]

Sorry I got that wrong, I didn't switch to another environment I switched the model. I am now working with an UR in Gazebo. But what I want to do is rather special. My programm communication looks like this:
Matlab<->Simulink<->ROS<->Gazebo

[rickstaa]

@saalimperator Thanks for your response! Ah, I see colleagues of my have the same communication scheme.

Since the issue you described above still exists in the franka_gazebo package, maybe let's re-open it for now? In this way, it can serve as a bug report.

[rickstaa]

@gollth I managed to improve the behaviour by passing the gravity vector from the Gazebo simulator to the gravity compensation function. See #177.

After applying this pull request, the behaviour becomes more stable, but the robot still drifts a bit. I, however, could not find anything wrong with the current implementation. Do you maybe know other factors that can cause this drift?

Things I checked

• Is there a delay between the sensor data and the control?: There appears to be no delay between the readSim and writeSim command, since they are both contained in the gazebo_ros_control_plugin.update() method. Also, due to this, the rate of control is equal to the simulation rate.
• Is the floating-point precision of the involved parameters decreased during execution?: To my knowledge, all the parameters are defined as doubles and stay this during the update.
• Are the effort limits defined in the robot description high enough to apply for the gravity compensation?: Since the effort limit is defined as Nm, I think all the joints are well able to perform the gravity compensation.

franka_ros/franka_description/robots/panda_arm.xacro

Line 102 in 1922fc9

<limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>

Other causes

• Are there other physics parameters that are needed that are currently hardcoded?
• Are there any round off errors during the gravity compensation?
• Is there any other component that sets joints commands?

[rickstaa]

@gollth Are there any updates on this?

I tried using your suggestion, but for my use case, sending control commands at every time step are not optional. I'm training a RL algorithm to try to control the robot arm. When gravity control is not working, the task becomes a lot harder, since it needs to be able to send control commands and thus do inference at a high frequency.

[gollth]

I'm currently investigating the issue

[rickstaa]

Great! I'm curious about what causes the drift that is still there. Based on the gazebo gravity compensation tutorial I think this drift should not be there. 🤔

[gollth]

Okay I think I have an idea where the problem comes from. It's part of a systematic error in the current calculation of the efforts coming from Gazebo. It's not only related to joints moving on their own without controller, but will affect any controller!

Let me try to explain what I think is going on:

Hypothesis

There is a bug in the way Gazebo calculates torques if the joint origin is rotated. The critical function is gazebo::physics::ODEJoint::GetForceTorque()

Bug Reproduction

Nominal Case

To see more clearly what is going on, let's create a simplified URDF containing only equally long sticks (length 10cm) with equal masses (5kg), where joints have zero rotation (i.e. <origin rpy="0 0 0" />) and bring the robot in the following configuration:

Let's focus on joint6 individually. We would expect a (negative) torque around joint6's Y axis because of gravity of link 6 and 7. If we print some values from during the writeSim() step, we see this confirmed:

Quantity	Value	Source
Effort acting on joint	-0.750765	body2Torque.dot(joint->axis)
KDL Gravity Term	0.750765	model->gravity(q)
Torque calculated by Gazebo	[-0.000793, -0.750765, -0.225921]	GetForceTorque(0).body2Torque
Joint Axis in World frame	[0.000000, 1.000000, 0.000000]	joint->GetGlobalAxis()
Joint Axis in Local frame	[0.000000, 1.000000, 0.000000]	joint->GetLocalAxis()
Joint Axis defined in URDF	[0, 1, 0]	joint->axis_

As expected Gazebo measures a torque around the Y-axis of the joint, which is the degree of freedom, and some stray torques in other directions. The magnitue also seems to match. We have the torque from link6 (5kg × 0.05m ≈ 0.25Nm) plus the torque from link7 (5kg × 0.1m ≈ 0.5Nm). This matches the torque which KDL calculates for its gravity term.

Bugged Case

Now let's do the same thing, but rotate the joint's origin by 90 degrees, i.e. <origin rpy="${pi/2} 0 0" />. Bringing the robot in a similar configuration should now make it experience (negative) torque, this time only coming from the weight of link7 alone, since joint6 is now rotating around Z.

Again we look at the interessting values:

Quantity	Value	Source
Effort acting on joint	-0.507839	body2Torque.dot(joint->axis)
KDL Gravity Term	0.250255	model->gravity(q)
Torque calculated by Gazebo	[-0.000254, -0.507839, 0.000003]	GetForceTorque(0).body2Torque
Joint Axis in World frame	[0.000000, -1.000000, 0.000000]	joint->GetGlobalAxis()
Joint Axis in Local frame	[0.000000, -1.000000, 0.000000]	joint->GetLocalAxis()
Joint Axis defined in URDF	[0, 1, 0]	joint->axis_

Here something strange is happening! KDL correctly calculates that the gravity term acting on the joint should only come from link7 (5kg × 0.05m ≈ 0.25Nm). But the result of GetForceTorque() yields a different result namely 0.5Nm. It seems that this is the result of the torque before rotation? Also the signs don't match.

Panda URDF

For the original Panda URDF this get's of course more complicated. When we plot the difference between the gravity term of KDL and the torque measured by Gazebo over time we see that this is really a systematic error and not (only) related to numerical noise as initially assumed:

Solutions

1. Create a bug report in osrf/gazebo, however there are 1.4k other issues already waiting...
2. Transform meshes/frames/inertias/axis such that the joint in the URDF do not contain any rotation.
3. Hope anybody has an idea how to correctly transform the result of GetForceTorque(). Would appreciate any further ideas or insights 😕

[gavanderhoorn]

1. Create a bug report in osrf/gazebo, however there are 1.4k other issues already waiting...

I would suggest to always do this, if you believe there is no other bug report already open about this.

The osrf/gazebo issue tracker is sort-of also used as a (public) todo-list, so don't let that high nr of open issues dissuade you from reporting something you believe is wrong.

Getting this fixed -- provided the analysis is correct -- would be tremendously valuable.

---

Edit: what would be good though perhaps would be to check whether Ignition has this same problem.

As Ignition has developer focus, finding the same problem there would probably make it easier to dedicate engineering resources to fix it, and back-porting it to Gazebo should then not be too difficult.

---

Edit 2:

Transform meshes/frames/inertias/axis such that the joint in the URDF do not contain any rotation.

no, that would be a work-around. Not a solution.

And would also not be very nice to those other 1000s of users who cannot do this (with their models).

[rickstaa]

@gollth Thanks a lot for finding the root cause of this issue and for creating the steps to reproduce it. Can you maybe push a small example repo to GitHub with the files you used to debug this problem? This will save me time when repeating the steps you describe above.

I agree with @gavanderhoorn this problem is likely affecting multiple repositories and users and should be fixed on osrf/gazebo. I think @gavanderhoorn's suggestion to check if the problem exists in ignition would be an excellent first step. If it does, we can create an issue there and backport the solution. If it does not, we can check what ignition does differently.

I will try to reproduce your example and then check if I can find more information about what causes the issue inside the gazebo/ignition code.

---

Edit1: I will first fix #177

[gollth]

Hey @rickstaa, thanks for taking a look. You can find the Stick URDF and the prints in this commit. Also look at its commit message to see how to reproduce the above described behavior.

[rickstaa]

@gollth I will start my investigation of this issue. I will keep this comment as a notepad.

TODOS

• Reproduce the problem in gazebo using the steps given by @gollth.
• Check if the bug is present in the ignition simulator.
• Inspect what is causing the bug in the Gazebo source code.

Reproduce the bug in gazebo

1. Clone the gazebo-torque-calculation-bug repository using the --recursive. flag.
2. Install the ROS dependencies.
3. Build the catkin workspace.
4. Start the example launch file using roslaunch gazebo_torque_calculation_bug stick.launch load_two_sticks:=true
5. Check the plot to see that the torque of stick 2 is incorrect.

I can not reproduce the bug in the Gazebo v11 simulator.

Check if the bug is present in the ignition simulator

I do not see the bug in the Ignition v6.0 simulator

Possibly related issues

• ForceTorque sensor visualization seems to ignore <pose> gazebosim/gazebo-classic#2476
• force torque sensor reports wild readings at joint limits using ODE gazebosim/gazebo-classic#2209

[rickstaa]

@gollth quick question. I think

$(calc 'deg2rad(90)')

is a syntax that is specific to your setup? I think calc is referring to the calc-common package but I could not find the deg2rad function. I used hardcoded values for now but I think these methods are very useful when having to deal with non 0/30/45/60/90 degree angles.

[rickstaa]

@gollth Based on your commit, I created a minimal example repository (i.e. no dependency on franka_ros) we can use in our Gazebo issue. This repository can be found here. I will make sure an RQT plot displays the torque, the joints can be controlled through a GUI, and some information is printed to the cmd line. Further, I will also add an Ignition launch file to this repo. I will keep you posted.

[gollth]

@gollth quick question. I think

$(calc 'deg2rad(90)')

is a syntax that is specific to your setup? I think calc is referring to the calc-common package but I could not find the deg2rad function. I used hardcoded values for now but I think these methods are very useful when having to deal with non 0/30/45/60/90 degree angles.

Hi @rickstaa,

thanks for looking into the issue. No calc is way dirtier than that, and comes from my local setup:

calc () {
  expression=$@ 
  python -c "from numpy import *; print($expression)"
}

Feel free to use whatever else you find suitable

[rickstaa]

Ha nice little trick! I mostly write bash scripts but this is a lot faster and versatile.

[rickstaa]

@gollth I investigated your issue, but I'm not able to reproduce it using your steps provided in #160 (comment). First, it looks like the figure included in the Bugged Case does not correspond with the data shown in the table. In the figure, the local URDF axis is 0 0 1 where the table shows 0 1 0. Further, I cannot reproduce the error between the KDL GravComp and Total Effort using your example; I also do not see any problems using my example.

Your example

I used a modified version of your example that I included as a submodule in my example repository. The franka_ros submodule that is in there includes your commit. The only thing I did was to add a rotated launch file argument to make it easier to inspect both cases (see c8b153b) and published the debug info to topics (see 01c8011). But the error you described above also doesn't show up on my system when I revert these commits and test your original example.

Results

Nominal case

roslaunch franka_gazebo panda.launch \
  use_gripper:=false \
  headless:=false \
  paused:=true \
  rotated:=false \
  initial_joint_positions:="-J panda_joint6 $(calc 'deg2rad(-90)')"

initial result

panda_joint6: Error:                 0.750765
panda_joint6: Total Effort:          0.000000
panda_joint6: KDL GravComp:          0.750765
panda_joint6: KDL GravComp flipped:  -0.750765
panda_joint6: Gazebo body2Torque:   [0.000000, 0.000000, 0.000000]
panda_joint6: Desired joint command: 0.000000
panda_joint6: Joint command:         0.750765
panda_joint6: Global Axis: [0.000000, 1.000000, 0.000000]
panda_joint6: Local Axis:  [0.000000, 1.000000, 0.000000]
panda_joint6: URDF Axis:   [0, 1, 0]
panda_joint6: Joint velocity:        0.000000

after one step

panda_joint6: Error:                 0.000000
panda_joint6: Total Effort:          -0.750765
panda_joint6: KDL GravComp:          0.750765
panda_joint6: KDL GravComp flipped:  -0.750765
panda_joint6: Gazebo body2Torque:   [-0.000793, -0.750765, -0.225921]
panda_joint6: Desired joint command: 0.000000
panda_joint6: Joint command:         0.750765
panda_joint6: Global Axis: [0.000000, 1.000000, 0.000000]
panda_joint6: Local Axis:  [0.000000, 1.000000, 0.000000]
panda_joint6: URDF Axis:   [0, 1, 0]
panda_joint6: Joint velocity:        0.000277

All looks correct to me:

• Effort is caused by the gravity of both link_6 and link_7 (i.e. 0.5 kg * -9,8 kg/m × 0.1m + 0.5kg * -9.8 kg/m * 0.05m = -0.735 N/m)
• The torques and efforts' signs are also correct when using the right-hand rule.

Bugged case

roslaunch franka_gazebo panda.launch \
  use_gripper:=false \
  headless:=false \
  paused:=true \
  rotated:=true \
  initial_joint_positions:="-J panda_joint6 $(calc 'deg2rad(90)')"

initial result

panda_joint6: Error:                 0.250255
panda_joint6: Total Effort:          0.000000
panda_joint6: KDL GravComp:          0.250255
panda_joint6: KDL GravComp flipped:  -0.250255
panda_joint6: Gazebo body2Torque:   [0.000000, 0.000000, 0.000000]
panda_joint6: Desired joint command: 0.000000
panda_joint6: Joint command:         0.250255
panda_joint6: Global Axis: [-0.000000, -1.000000, -0.000004]
panda_joint6: Local Axis:  [0.000000, 0.000000, 1.000000]
panda_joint6: URDF Axis:   [0, 0, 1]
panda_joint6: Joint velocity:        0.000000

after two steps

panda_joint6: Error:                 0.000000
panda_joint6: Total Effort:          -0.250255
panda_joint6: KDL GravComp:          0.250255
panda_joint6: KDL GravComp flipped:  -0.250255
panda_joint6: Gazebo body2Torque:   [-0.000317, -0.780290, -0.250255]
panda_joint6: Desired joint command: 0.000000
panda_joint6: Joint command:         0.250255
panda_joint6: Global Axis: [0.000000, -1.000000, -0.000004]
panda_joint6: Local Axis:  [0.000000, 0.000000, 1.000000]
panda_joint6: URDF Axis:   [0, 0, 1]
panda_joint6: Joint velocity:        0.000022

It also looks correct to me:

• Effort is caused by the gravity of both link_6 and link_7 (i.e. 0.5 kg * -9,8 kg/m × 0.05m = -0.245 N/m)
• The torques and efforts' signs are also correct when using the right-hand rule.

How to use my example

You can find a guide on using my example in the README. To quickly start, run the following command after building the catkin workspace:

roslaunch gazebo_torque_calculation_bug stick.launch initial_joint_positions:="-J stick_joint1 $(calc 'deg2rad(-90)')"

or for the bugged case

roslaunch gazebo_torque_calculation_bug stick.launch rotated:=true initial_joint_positions:="-J stick_joint1 $(calc 'deg2rad(90)')"

In my example, I got the same results as your example. Maybe I misunderstood your example, or I overlooked something. I will investigate further.

[rickstaa]

@gollth The only two things that I have found so far from Gazebos side that could perturb your gravity compensation are:

• There is a startup delay of 0.2 s for the Gazebo effort calculation. This delay is present in both the GetForce and GetForceTorque(0).body2Torque methods but not for the gravity component that is calculated using the orocos KDL library. As the delay is small and subsides quickly, it should not influence the gravity compensation controller much.

• There also is a minor delay; see the peak in the gif below when we control the robot. As the difference subsides quickly, this shouldn't influence the drift that is seen when the robot is stable.

I also saw these effects in the Ignition simulator.

EDIT:

When we add gravity compensation the initial delay only exists for a one-time step since the compensation applies an effort in the first time step:

Therefore this should not have an effect. Also, the control delay becomes smaller when gravity compensation is enabled

[rickstaa]

I also tried to migrate the example to the Ignition simulator see gazebo-torque-calculation-bug#ignition-gazebo. I, however, think that this requires a bit more work since the ros-simulation/gazebo_ros_pkgs is not ignition compatible. I could not find an Ignition equivalent in the ros_ign.

I think testing only the bug in the ignition simulator requires us to create a small sensor plugin since I saw no ignition topic that contained the joint effort. I will check if we can use the ForceTorque sensor that is contained in the ign-sensors package. The nominal and bugged configurations are now hardcoded into the stick_description xacro file since the ros_ign_gazebo/create.cpp method does not yet take joint angles. I further changed the revolute joint to a fixed joint so that we don't need to implement a control plugin.

As I have no experience with the ignition simulator, I am not sure the easiest way to test this bug. Maybe @gavanderhoorn has some suggestions. However, let's first find the proper steps to reproduce the bug in Gazebo.

[gavanderhoorn]

I'm not sure I completely understand what you're looking for, but perhaps ros-controls/gz_ros2_control#1 helps?

[rickstaa]

@gollth, @marcbone I just noticed that I made an error while looking at the safety controller. It is not the safety controller that causes the starting drift but the addition of the hand. It however does cause unstable behaviour (see #160 (comment)).

I updated #160 (comment) so that it reflects the right conclusion.

[rickstaa]

@gollth, @marcbone, I think I found the cause of the drift experienced when the hand is attached. It looks that your gravity compensation is not taking into account the physical properties of the attached hand:

franka_ros/franka_gazebo/src/model_kdl.cpp

Line 277 in cbe64f9

KDL::Chain chain = this->chain_; // copy

The currently used KDL::Chain starts from panda_link0 and ends at panda_link8. Can you verify that this is indeed the case?

I did not create a pull request since it requires me to rewrite a significant part of your codebase. I, therefore, first wanted to discuss the issue with you.

I updated #160 (comment) to include the new cause. To summarize, the causes I found for the drift, ordered from most serious to less severe, are:

• The hand physical properties are not considered when performing the gravity compensation (cause a starting drift when the hand is attached).
• Physics engine inaccuracies (i.e. most minor in the dart physics engine).
• The soft and hard limits are not considered when performing the gravity compensation (lead to instabilities).
• Other model physical property inaccuracies.
• Other numerical instabilities.

[rickstaa]

@gollth, @marcbone sorry for the enormous amount of information, but I just noticed I forgot something in my previous conclusion #160 (comment). It did not include the fact that although the starting drift is not caused by the safety_controller it does cause some unstable behaviour at the soft joint limits. I hope I have given you a good picture of all the problems that are currently present in the gravity compensation. I will leave the further investigation to you now. Please let me know when things are unclear.

[gollth]

It looks that your gravity compensation is not taking into account the physical properties of the attached hand:

Yes it does (kind of), directly with the line after your snippets:

franka_ros/franka_gazebo/src/model_kdl.cpp

Lines 277 to 278 in 8efef62

	KDL::Chain chain = this->chain_; // copy
	augmentFrame("load", F_x_Ctotal, m_total, {1, 0, 0, 0, 1, 0, 0, 0, 1}, chain);

This appends a load frame to the end of the chain. This load frame is defined by the mass of the EE (m_total) and the CoM (F_x_Ctotal) which both come ultimately from the URDF (unless the user sets custom values via ROS parameters):

franka_ros/franka_gazebo/src/franka_hw_sim.cpp

Lines 440 to 455 in 8efef62

	if (not nh.hasParam("F_x_Cee") and hand != nullptr) {
	if (hand->inertial == nullptr) {
	throw std::invalid_argument("Trying to use inertia of " + hand_link +
	" but this link has no <inertial> tag defined in it.");
	}
	def_f_x_cee = std::to_string(hand->inertial->origin.position.x) + " " +
	std::to_string(hand->inertial->origin.position.y) + " " +
	std::to_string(hand->inertial->origin.position.z);
	}
	std::string F_x_Cee; // NOLINT [readability-identifier-naming]
	nh.param<std::string>("F_x_Cee", F_x_Cee, def_f_x_cee);
	this->robot_state_.F_x_Cee = readArray<3>(F_x_Cee, "F_x_Cee");
	}
	void FrankaHWSim::updateRobotStateDynamics() {
	this->robot_state_.m_total = this->robot_state_.m_ee + this->robot_state_.m_load;

Although you are right, the inertia tensor of this augmented link is still identity

[gollth]

Thanks for all your investigation @rickstaa

[rickstaa]

@gollth Thanks a lot for your response. You are right thanks for clarifying that! Based on the naming (i.e. load), I assumed that the augmented frame only accounted for the external load. I quickly checked the code, and overall, it looks correct. I, however, have some concerns about the current implementation:

• First, it looks like your not getting the hand and finger masses from the URDF (see franka_hw_sim.cpp#L392-L452). Instead, you set the m_ee parameter in the panda.launch file. It would make more sense to remove this line and get the mass of both the fingers and hand directly from the URDF.

franka_ros/franka_gazebo/launch/panda.launch

Line 54 in 8efef62

<param name="m_ee" value="0.76" if="$(arg use_gripper)" />

• Second, when the m_ee parameter is not supplied, you only retrieve the mass of the hand and ignore the mass of the fingers:

franka_ros/franka_gazebo/src/franka_hw_sim.cpp

Line 421 in 8efef62

def_m_ee = hand->inertial->mass;

• Same for the inertias and COM's:

franka_ros/franka_gazebo/src/franka_hw_sim.cpp

Lines 431 to 434 in 8efef62

	def_i_ee = std::to_string(hand->inertial->ixx) + " " + std::to_string(hand->inertial->ixy) + " " + std::to_string(hand->inertial->ixz) + " "
	+ std::to_string(hand->inertial->ixy) + " " + std::to_string(hand->inertial->iyy) + " " + std::to_string(hand->inertial->iyz) + " "
	+ std::to_string(hand->inertial->ixz) + " " + std::to_string(hand->inertial->iyz) + " " + std::to_string(hand->inertial->izz);
	// clang-format on

franka_ros/franka_gazebo/src/franka_hw_sim.cpp

Lines 445 to 447 in 8efef62

	def_f_x_cee = std::to_string(hand->inertial->origin.position.x) + " " +
	std::to_string(hand->inertial->origin.position.y) + " " +
	std::to_string(hand->inertial->origin.position.z);

I think adding the fingers to the chain might be more correct. I, however, am not sure if KDL allows for parallel links in a chain. I vaguely remember it does not, but I haven't looked at it for a while. If adding parallel links is not allowed, we could use a single link that contains the combined COM and inertia matrix of both the hand and the two fingers. Lastly

• Lastly, it looks that you are using the initialized value of F_x_Ctotal (i.e. 0, 0, 0) and do not update it anywhere in the code. Maybe this is as intended, or I overlooked the part where you set this parameter, but I would have expected it to be equal to the hand and arm's combined COM position.

franka_ros/franka_gazebo/src/franka_hw_sim.cpp

Line 315 in 8efef62

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

franka_ros/franka_gazebo/src/model_kdl.cpp

Line 271 in 8efef62

const std::array<double, 3>& F_x_Ctotal, // NOLINT(readability-identifier-naming)

I hope we can find the cause of this drift. Please let me know if you need help from my side.

[rickstaa]

@gollth I created an issue on the gazebo repository for the drift difference that is observed between the ODE and DART physics engine (see gazebosim/gazebo-classic#3149). Did you already create an issue for the ROS melodic torque calculation problem? In my issue, I quickly mention the melodic torque problem but I currently link it to #160 (comment).

[justagist]

Although franka_hw_sim_plugin handles the gravity compensation for the arm, there is no gravity compensation in franka_gripper_sim. This causes the gripper commands to not work correctly when the gripper fingers are not in the same horizontal plane.
I had to completely disable gravity in the simulation to make the gripper work in such configurations.

[rickstaa]

@gollth, @marcbone do you have any updates on what goes wrong with the gravity compensation when the gripper is attached?

I think using the DART simulator is a good workaround for now when the gripper is not attached. I can successfully train my RL algorithm using the DART simulator as the gravity compensation seems stable. I am unsure what causes the drift when the ODE physics engine is used. I did not have time to dive deeper into the Gazebo codebase, and the gazebo team has not yet responded to the issue I opened about this drift. I, therefore, think for now it might be best to merge #211 since this gives users the ability to use the DART physics engine when using effort control.

I, however, found one problem when using the DART physics engine. See the section below. Nevertheless, the gravity compensation does not seem to work both in the ODE and DART physics engine, the panda robot keeps drifting, so I think this should be fixed first.

Grasp action DART crash

When the gripper is attached and the franka_gripper/grasp/goal, franka_gripper/move/goal or franka_gripper/gripper_action/goal actions are used the simulator sometimes crashes with the following error message:

Show error message

Error [BoxedLcpConstraintSolver.cpp:291] [BoxedLcpConstraintSolver] The solution of LCP includes NAN values: -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan  nan  nan  nan  nan  nan  nan  nan -nan -nan. We're setting it zero for safety. Consider using more robust solver such as PGS as a secondary solver. If this happens even with PGS solver, please report this as a bug.
[ WARN] [1643023521.425695656, 90.305000000]: Command for panda_joint1is NaN, won't send to robot
[ WARN] [1643023521.425777956, 90.305000000]: Command for panda_joint2is NaN, won't send to robot
[ WARN] [1643023521.425795056, 90.305000000]: Command for panda_joint3is NaN, won't send to robot
[ WARN] [1643023521.425808456, 90.305000000]: Command for panda_joint4is NaN, won't send to robot
[ WARN] [1643023521.425855257, 90.305000000]: Command for panda_joint5is NaN, won't send to robot
[ WARN] [1643023521.426084258, 90.305000000]: Command for panda_joint6is NaN, won't send to robot
[ WARN] [1643023521.426113458, 90.305000000]: Command for panda_joint7is NaN, won't send to robot
[ WARN] [1643023522.488360928, 90.307000000]: Command for panda_joint1is NaN, won't send to robot
[ WARN] [1643023522.488449328, 90.307000000]: Command for panda_joint2is NaN, won't send to robot
[ WARN] [1643023522.488477629, 90.307000000]: Command for panda_joint3is NaN, won't send to robot
[ WARN] [1643023522.488506729, 90.307000000]: Command for panda_joint4is NaN, won't send to robot
[ WARN] [1643023522.488532129, 90.307000000]: Command for panda_joint5is NaN, won't send to robot
[ WARN] [1643023522.488556229, 90.307000000]: Command for panda_joint6is NaN, won't send to robot
[ WARN] [1643023522.488580929, 90.307000000]: Command for panda_joint7is NaN, won't send to robot
[ WARN] [1643023523.534044389, 90.307000000]: Command for panda_joint1is NaN, won't send to robot
[ WARN] [1643023523.534135489, 90.307000000]: Command for panda_joint2is NaN, won't send to robot
[ WARN] [1643023523.534164089, 90.307000000]: Command for panda_joint3is NaN, won't send to robot
[ WARN] [1643023523.534194190, 90.307000000]: Command for panda_joint4is NaN, won't send to robot
[ WARN] [1643023523.534219690, 90.307000000]: Command for panda_joint5is NaN, won't send to robot
[ WARN] [1643023523.534244590, 90.307000000]: Command for panda_joint6is NaN, won't send to robot
[ WARN] [1643023523.534268890, 90.307000000]: Command for panda_joint7is NaN, won't send to robot
Segmentation fault (core dumped)
franka_gripper_sim_test: /usr/include/boost/smart_ptr/shared_ptr.hpp:734: typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const [with T = const sensor_msgs::JointState_<std::allocator<void> >; typename boost::detail::sp_member_access<T>::type = const sensor_msgs::JointState_<std::allocator<void> >*]: Assertion `px != 0' failed.

See the problem in action

Branch used for the test: https://github.com/rickstaa/franka_ros/tree/dart_engine_tests

Funny enough, this issue showed that the new tests you introduced are working (see https://github.com/frankaemika/franka_ros/runs/4920586653?check_suite_focus=true) 🚀 .

This only seems to happen when the gripper is controlled to the fully closed and fully opened position (i.e. 0.0 and 0.08). I, therefore, think this is caused by a bug or the instability of the gripper control logic, which causes some numbers to become singular. It might be closely related to the problems I pointed out in #173 (comment). However, I did not yet investigate since the gravity compensator for when the gripper is attached is not working yet, which might also cause the crash. However, this hypothesis is strengthened since it does not seem to happen when I stabilize the panda arm by putting it down on the ground (see figure below). I know that getting the gripper to work in simulation is not easy, but I just wanted to make you aware of all the problems I found.

See stabilized position

[rickstaa]

Although franka_hw_sim_plugin handles the gravity compensation for the arm, there is no gravity compensation in franka_gripper_sim. This causes the gripper commands to not work correctly when the gripper fingers are not in the same horizontal plane. I had to completely disable gravity in the simulation to make the gripper work in such configurations.

@justagist Thanks for your comment! @marcbone I think this is closely related to my comment in #173 (comment). The gripper PID gains might be easier to tune when the gravity of the fingers is also compensated! I however do not think that this is the major cause of the drift we observe when the hand is attached when using the DART physics engine (see figure below).

See gif

[gollth]

For reference, the movement of the joints should now be gone also in Melodic with the new feature from @rickstaa & @marcbone: #181 (comment) (although it does only mask the underlying real problem in Gazebo).

@rickstaa what do you think, shall we close this ticket or do you want to keep it open for reference?

[rickstaa]

@gollth Although, I agree with you that the problem as it was originally described by @sallimperator has been fixed the underlying problem still is present. I however think we can close #160 and continue the discussion on this problem in #233.

[rickstaa]

Although franka_hw_sim_plugin handles the gravity compensation for the arm, there is no gravity compensation in franka_gripper_sim. This causes the gripper commands to not work correctly when the gripper fingers are not in the same horizontal plane. I had to completely disable gravity in the simulation to make the gripper work in such configurations.

@justagist Thanks for your comment! @marcbone I think this is closely related to my comment in #173 (comment). The gripper PID gains might be easier to tune when the gravity of the fingers is also compensated! I however do not think that this is the major cause of the drift we observe when the hand is attached when using the DART physics engine (see figure below).

See gif

I just double-checked and for the gripper joints the gravity is being compensated:

franka_ros/franka_gazebo/src/franka_hw_sim.cpp

Lines 365 to 377 in 30dc6b8

	auto g = this->model_->gravity(this->robot_state_, this->gravity_earth_);
	for (auto& pair : this->joints_) {
	auto joint = pair.second;
	// Retrieve effort control command
	double effort = 0;
	// Check if this joint is affected by gravity compensation
	std::string prefix = this->arm_id_ + "_joint";
	if (pair.first.rfind(prefix, 0) != std::string::npos) {
	int i = std::stoi(pair.first.substr(prefix.size())) - 1;
	joint->gravity = g.at(i);
	}

Could still be that the component of the gravity of the finger and hand on the other arm joints is not being handled (see #160 (comment)).