[bug] Gazebo simulation effort/velocity control gravity compensation contains drift

[rickstaa]

This issue is a continuation of #160. Although, as of 2514ae1, the random joint motion described in #160 is now gone when starting the simulation, the underlying problem that caused this motion still exists. The only difference is that the real issue now is mitigated by the position controller that is started by default. When we change the control back to effort or velocity control, the robot again starts to drift. Because of this, we can see that the gravity compensation is still malfunctioning. Below, I will briefly overview the problems we discussed in #160. For the whole discussion, people are directed to #160.

Problem description

Since v0.8.0 a Franka simulation (i.e. franka_gazebo) has been added to the franka_ros package. This simulator is a big improvement compared to third-party simulations that people were forced to use before v0.8.0. Since its release, this simulator has matured, and initial problems have been fixed.

This simulator has helped do simulation research using position/cartesian impedance and trajectory controllers. However, as discussed in #160, the simulator is useless when doing effort/velocity control research. The reason this is the case is that the gravity compensation that has been implemented in the FrankaHWSim:

franka_ros/franka_gazebo/src/franka_hw_sim.cpp

Lines 379 to 411 in 30dc6b8

	if (control_method == EFFORT) {
	effort = joint->command + joint->gravity;
	} else if (control_method == POSITION) {
	// Use position motion generator
	double error;
	const double kJointLowerLimit = joint->limits.min_position;
	const double kJointUpperLimit = joint->limits.max_position;
	switch (joint->type) {
	case urdf::Joint::REVOLUTE:
	angles::shortest_angular_distance_with_limits(joint->position, joint->desired_position,
	kJointLowerLimit, kJointUpperLimit, error);
	break;
	default:
	std::string error_message =
	"Only revolute joints are allowed for position control right now";
	ROS_FATAL("%s", error_message.c_str());
	throw std::invalid_argument(error_message);
	}
	const double kEffortLimit = joint->limits.max_effort;
	effort = boost::algorithm::clamp(
	position_pid_controllers_[joint->name].computeCommand(error, period),
	-kEffortLimit, kEffortLimit) +
	joint->gravity;
	} else if (control_method == VELOCITY) {
	// Use velocity motion generator
	const double kError = joint->desired_velocity - joint->velocity;
	const double kEffortLimit = joint->limits.max_effort;
	effort = boost::algorithm::clamp(
	velocity_pid_controllers_[joint->name].computeCommand(kError, period),
	-kEffortLimit, kEffortLimit) +
	joint->gravity;
	}

cannot keep the robot still when no effort/velocity control commands are sent (see gif below).

@gollth and I have looked at this issue some months ago (see the treat in #160). From our investigations, both problems with the simulator and the franka_ros package were found:

Gazebo problems

• For Gazebo Melodic, there appears to be a problem with the current calculation of the efforts coming from Gazebo (see Gazebo model joints move by themselves #160 (comment) ). This problem is not present in the ROS Noetic version of Gazebo (see Gazebo model joints move by themselves #160 (comment)).
  • Although I use Noetic, it might be beneficial to create a minor GitHub issue in the
    https://github.com/osrf/gazebo/issues repository to make the maintainers of Gazebo aware of this issue so that they can back-port the fix in Noetic to Melodic. @gollth, since you found the error, can you create an issue for this?
• However, there also appears to be a problem in ROS Noetic when the ODE physics engine is used. When this physics engine is used, the robot arm drifts when the simulation starts. Numerical instabilities and the presence of initial velocities likely cause this. I created Non-zero initial velocities in the ODE physics engine gazebosim/gazebo-classic#3149 to inform the Gazebo team about this behaviour, but I am still awaiting a response. This is because most of the development is currently being done on Gazebo Ignition. Maybe it is better to create an issue at the repository of the underlying physics engine https://bitbucket.org/odedevs/ode/src? @gavanderhoorn Do you perhaps suggest how we should proceed with this issue?

The problems above are not present when changing the physics engine to DART (see #160 (comment)). Therefore, merging #211 and switching to this engine when doing effort/velocity control research is a good workaround until the problems above have been fixed.

Problems caused by the franka_ros package

When we use the DART physics engine, and don't attach the hand, we can correctly do effort/velocity research as the drift is gone (i.e. It now takes several minutes for the robot arm to drift from its initial position). However, when the gripper is attached, there still seem to be problems with the gravity compensation:

I think this problem still has to be fixed by the @frankaemika team (see @justagist's comment in #160 (comment)) because I believe it stems from the gravity compensation of the hand and fingers to be incorrect. It could still be a problem with Gazebo, but we should determine what causes this problem and rule out that the franka_ros codebase causes it.