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.

https://github.com/frankaemika/franka_ros/blob/b9b60e8a8316593b6a82d3bfa48a5b89155f42a5/franka_example_controllers/src/force_example_controller.cpp#L115)

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.

https://github.com/frankaemika/franka_ros/blob/1922fc9d24e37b2a8226c522e736eaa9a65d66ca/franka_gazebo/src/franka_hw_sim.cpp#L310-L317

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.

https://github.com/frankaemika/franka_ros/blob/1922fc9d24e37b2a8226c522e736eaa9a65d66ca/franka_gazebo/src/franka_hw_sim.cpp#L310-L317

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.

https://github.com/frankaemika/franka_ros/blob/1922fc9d24e37b2a8226c522e736eaa9a65d66ca/franka_description/robots/panda_arm.xacro#L102

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