Add a JointTrajectoryController config

[rhaschke]

Usage:
roslaunch franka_gazebo panda.launch controller:=effort_joint_trajectory_controller

Instead of stating the transmission type in the controller name, I suggest using just joint_trajectory_controller, here as well as in default_controllers.yaml:

franka_ros/franka_control/config/default_controllers.yaml

Line 1 in 041d3eb

position_joint_trajectory_controller:

[rickstaa]

@rhaschke, @gollth I re-tested the gains you provided for the simulation on the real robot and they seem to work. I think the errors and problems I experienced earlier were unrelated to the PID gains. I, therefore, think we can safely add the effort_joint_trajectory_controller to the repository. By doing this people can switch between the joint_trajectory_controller (position_based) and the effort_joint_trajectory_controller (effort based).

effort_joint_trajectory_controller:
  type: effort_controllers/JointTrajectoryController
  joints:
    - panda_joint1
    - panda_joint2
    - panda_joint3
    - panda_joint4
    - panda_joint5
    - panda_joint6
    - panda_joint7
  gains:
    panda_joint1: { p: 600, d: 30, i: 0 }
    panda_joint2: { p: 600, d: 30, i: 0 }
    panda_joint3: { p: 600, d: 30, i: 0 }
    panda_joint4: { p: 600, d: 30, i: 0 }
    panda_joint5: { p: 250, d: 10, i: 0 }
    panda_joint6: { p: 150, d: 10, i: 0 }
    panda_joint7: { p: 50, d: 5, i: 0 }

[rhaschke]

I'm not sure we should keep the detailed controller names indicating the HW interface used, i.e. effort_joint_trajectory_controller vs. position_joint_trajectory_controller. To facilitate selecting the default trajectory controller, as a user I prefer to have the name joint_trajectory_controller available, pointing to the default controller.

[rickstaa]

I have no real preference other than the user being able to switch between multiple controllers when working with the real robot. I checked #188 and for the simulation the following code is fine:

franka_ros/franka_gazebo/config/sim_controllers.yaml

Lines 14 to 31 in 7813b3f

	joint_trajectory_controller:
	type: $(arg transmission)_controllers/JointTrajectoryController
	joints:
	- $(arg arm_id)_joint1
	- $(arg arm_id)_joint2
	- $(arg arm_id)_joint3
	- $(arg arm_id)_joint4
	- $(arg arm_id)_joint5
	- $(arg arm_id)_joint6
	- $(arg arm_id)_joint7
	gains:
	$(arg arm_id)_joint1: { p: 600, d: 30, i: 0 }
	$(arg arm_id)_joint2: { p: 600, d: 30, i: 0 }
	$(arg arm_id)_joint3: { p: 600, d: 30, i: 0 }
	$(arg arm_id)_joint4: { p: 600, d: 30, i: 0 }
	$(arg arm_id)_joint5: { p: 250, d: 10, i: 0 }
	$(arg arm_id)_joint6: { p: 150, d: 10, i: 0 }
	$(arg arm_id)_joint7: { p: 50, d: 5, i: 0 }

When working with the real robot you now use the following code:

franka_ros/franka_control/config/default_controllers.yaml

Lines 1 to 26 in 7813b3f

	joint_trajectory_controller:
	type: position_controllers/JointTrajectoryController
	joints:
	- panda_joint1
	- panda_joint2
	- panda_joint3
	- panda_joint4
	- panda_joint5
	- panda_joint6
	- panda_joint7
	constraints:
	goal_time: 0.5
	panda_joint1:
	goal: 0.05
	panda_joint2:
	goal: 0.05
	panda_joint3:
	goal: 0.05
	panda_joint4:
	goal: 0.05
	panda_joint5:
	goal: 0.05
	panda_joint6:
	goal: 0.05
	panda_joint7:
	goal: 0.05

Here I think we can change the joint_trajectory_controller to the effort controller and add the parameters of the position_joint_trajectory_controller controller to that file.

Alternatively, you can add code similar to the code found in the franka_control/config/default_controllers.yaml for the real robot. I however remembered that @gollth wanted to use the effort controllers by default on the real robot.

[gollth]

Hmm, I'm not sure. If we offer multiple joint_trajectory_controllers to me there is no clear "default". What's the problem with making the naming explicit?

[rhaschke]

With explicit, but different names for real and sim (position_joint_trajectory_controller vs. effort_joint_trajectory_controller),
we would also need different MoveIt configs referring to the different names.

[gollth]

The name in franka_control/config/default_controllers.yaml used to be position_joint_trajectory_controller anyways. Then the names won't be different in reality and simulation

[rhaschke]

position_joint_trajectory_controller from default_controllers.yaml is used for real.
However, an effort_joint_trajectory_controller is needed for Gazebo sim.
-> two different names, isn't it?

[rickstaa]

Since the PID gains for the effort_controllers/JointTrajectoryController also work on the real robot (see #186 (comment)) we can use the effort controller for both the position and real robot.

However, my preference is to use the general joint_trajectory_controller since it leads to clean code and, like @rhaschke pointed out, would not require us to write multiple launch files at the MoveIt side.

franka_ros/franka_gazebo/config/sim_controllers.yaml

Lines 14 to 31 in 7813b3f

	joint_trajectory_controller:
	type: $(arg transmission)_controllers/JointTrajectoryController
	joints:
	- $(arg arm_id)_joint1
	- $(arg arm_id)_joint2
	- $(arg arm_id)_joint3
	- $(arg arm_id)_joint4
	- $(arg arm_id)_joint5
	- $(arg arm_id)_joint6
	- $(arg arm_id)_joint7
	gains:
	$(arg arm_id)_joint1: { p: 600, d: 30, i: 0 }
	$(arg arm_id)_joint2: { p: 600, d: 30, i: 0 }
	$(arg arm_id)_joint3: { p: 600, d: 30, i: 0 }
	$(arg arm_id)_joint4: { p: 600, d: 30, i: 0 }
	$(arg arm_id)_joint5: { p: 250, d: 10, i: 0 }
	$(arg arm_id)_joint6: { p: 150, d: 10, i: 0 }
	$(arg arm_id)_joint7: { p: 50, d: 5, i: 0 }

In that case we can use the joint_trajectory_controller to point to the effort based controller in the franka_control/config/default_controllers.yaml file (you might want to document this):

effort_joint_trajectory_controller:
  type: effort_controllers/JointTrajectoryController
  joints:
    - panda_joint1
    - panda_joint2
    - panda_joint3
    - panda_joint4
    - panda_joint5
    - panda_joint6
    - panda_joint7
  gains:
    panda_joint1: { p: 600, d: 30, i: 0 }
    panda_joint2: { p: 600, d: 30, i: 0 }
    panda_joint3: { p: 600, d: 30, i: 0 }
    panda_joint4: { p: 600, d: 30, i: 0 }
    panda_joint5: { p: 250, d: 10, i: 0 }
    panda_joint6: { p: 150, d: 10, i: 0 }
    panda_joint7: { p: 50, d: 5, i: 0 }
  constraints: 
     goal_time: 0.5 
     panda_joint1: 
       goal: 0.05 
     panda_joint2: 
       goal: 0.05 
     panda_joint3: 
       goal: 0.05 
     panda_joint4: 
       goal: 0.05 
     panda_joint5: 
       goal: 0.05 
     panda_joint6: 
       goal: 0.05 
     panda_joint7: 
       goal: 0.05 

We can then additionally add the position and velocity controllers:

position_joint_trajectory_controller: 
   type: position_controllers/JointTrajectoryController 
   joints: 
     - panda_joint1 
     - panda_joint2 
     - panda_joint3 
     - panda_joint4 
     - panda_joint5 
     - panda_joint6 
     - panda_joint7 
   constraints: 
     goal_time: 0.5 
     panda_joint1: 
       goal: 0.05 
     panda_joint2: 
       goal: 0.05 
     panda_joint3: 
       goal: 0.05 
     panda_joint4: 
       goal: 0.05 
     panda_joint5: 
       goal: 0.05 
     panda_joint6: 
       goal: 0.05 
     panda_joint7: 
       goal: 0.05 

velocity_joint_trajectory_controller: 
   type: velocity_controllers/JointTrajectoryController 
   joints: 
     - panda_joint1 
     - panda_joint2 
     - panda_joint3 
     - panda_joint4 
     - panda_joint5 
     - panda_joint6 
     - panda_joint7 
   constraints: 
     goal_time: 0.5 
     panda_joint1: 
       goal: 0.05 
     panda_joint2: 
       goal: 0.05 
     panda_joint3: 
       goal: 0.05 
     panda_joint4: 
       goal: 0.05 
     panda_joint5: 
       goal: 0.05 
     panda_joint6: 
       goal: 0.05 
     panda_joint7: 
       goal: 0.05 

[rickstaa]

@gollth Here are some details about why we need more changes on the panda_moveit_config side when explicit controller names are used.

We currently load the controller configuration of the robot to MoveIt in the simple_moveit_controller_manager.launch.xml launch file. This is done by loading the parameters in the simple_moveit_controllers.yaml simple_moveit_controllers.yaml onto the move_group ns. When we use explicit names for the controllers, we need to create to:

• Create at least two configuration files (i.e. effort and position).
• We need to add logic to switch between these configuration files in the the simple_moveit_controller_manager.launch.xml launch file.
• We need to pass a variable specifying which controller we should load (i.e. transmission) from the franka_gazebo.launch down to the the simple_moveit_controller_manager.launch.xml launch file.

[rhaschke]

Using YAML anchors + references, I was able to define the commonly named default joint_trajectory_controller, while keeping the previous names as well.

[gollth]

I'm still not convinced to drop the transmissions from the controller names. To my understanding MoveIT should be controller independent. Therefore it should run with both position_joint_trajectory_controller as well as effort_joint_trajectory_controller and not rely on having a default joint_trajectory_controller, even if this means a little more overhead in terms of configuration here.

Technically ROS provides a lot of tools which can solve the issues @rickstaa you mentioned in a generic way.

• Create at least two configuration files (i.e. effort and position).

@rickstaa which files exactly are you referring to? Could this be solved with ROS launch arguments?

• We need to add logic to switch between these configuration files in the the simple_moveit_controller_manager.launch.xml launch file.

Not necessarily. Again with launch args and subst_value="true" we could do something like:

controller_list:
  - name: $(arg transmission)_joint_trajectory_controller
    action_ns: follow_joint_trajectory
    ...

• We need to pass a variable specifying which controller we should load (i.e. transmission) from the franka_gazebo.launch down to the the simple_moveit_controller_manager.launch.xml launch file.

Does that pose a problem?

[gollth]

position_joint_trajectory_controller from default_controllers.yaml is used for real.
However, an effort_joint_trajectory_controller is needed for Gazebo sim.
-> two different names, isn't it?

@rhaschke my suggestion is to add the effort_joint_trajectory_controller to the franka_control. The we have a coherent naming both on the real hw and in simulation ;)

[rhaschke]

Of course, you are right @gollth. One could configure the desired controller name via substitution on the panda_moveit_config side. However, this creates a config dependency between franka_ros and panda_moveit_config: If the default controller changes in franka_ros, one needs to adapt panda_moveit_config appropriately. On the other hand, if we have a default specified here as suggested in the last commit, it is sufficient to reference another controller here in franka_ros.

@rhaschke my suggestion is to add the effort_joint_trajectory_controller to the franka_control. Then we have a coherent naming both on the real hw and in simulation

That's not really a solution, because that would enforce people to use the effort controller. I think we want to keep the flexibility of switching controllers.
We can also add an additional argument arm_controller argument to panda_moveit_config to switch controllers from there.
However, having a default as proposed here, would be good in any case. Maybe we should rename joint_trajectory_controller to default_joint_trajectory_controller to clarify its meaning.

[gollth]

However, this creates a config dependency between franka_ros and panda_moveit_config: If the default controller changes in franka_ros, one needs to adapt panda_moveit_config appropriately.

Not necessarily, when you make this configurable via substitutation/launch args

That's not really a solution, because that would enforce people to use the effort controller.

Again not really. The final end user (of panda_moveit_config) would be able to specify which controller she prefers. Maybe there is a misunderstanding happening here, let me show you in code what I had in mind:

rhaschke/panda_moveit_config#16

(I'm not entirely sure which is the latest branch, so I create that PR on melodic-devel but I think you will get my point regardless)

This way you can do:

# Same as before, will take the 'effort_joint_trajectory_controller' as default
roslaunch panda_moveit_config move_group.launch 

# Alternatively once we change controllers or add new ones:
roslaunch panda_moveit_config move_group.launch transmission:=position

The config dependency will be introduced with the "default" naming scheme IMO (but even worse, it will be unspecified). Should we decide to update the default controller from, say, effort_joint_trajectory_controller to position_joint_trajectory_controller users will experience a breaking change. Let's avoid that at all cost!

Maybe we should rename joint_trajectory_controller to default_joint_trajectory_controller to clarify its meaning.

Please no.

[rhaschke]

I got this idea before. I stated:

I can also add an additional argument arm_controller to panda_moveit_config to switch controllers from there.

However, this way, the default specified in franka_ros and the one in panda_moveit_config (in this line) are coupled.
If the default is changed in franka_ros, the users cannot rely on the default value specified in panda_moveit_config anymore, because it became wrong! Thus they are enforced to provide the transmission arg on the cmdline.
With my proposed solution, we would always have a consistent default.

[rhaschke]

Maybe, I had a fundamental misunderstanding of your launch process. So far, I was assuming that you load (and start) a specific trajectory controller by default when launching either the Gazebo sim or the real robot. However, in Gazebo, the default is empty:

franka_ros/franka_gazebo/launch/panda.launch

Line 14 in f87ea8d

<arg name="controller" default=" " doc="Which example controller should be started? (One of {cartesian_impedance,model,force}_example_controller)" />

franka_ros/franka_gazebo/launch/panda.launch

Line 75 in f87ea8d

args="franka_state_controller $(arg controller)"

and for the real robot, you don't even provide an argument to load/start a controller, but just start the state controller:

franka_ros/franka_control/launch/franka_control.launch

Line 18 in f87ea8d

<node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="franka_state_controller"/>

Hence, given the fact that the user needs to specify the controller manually anyway, I agree that he can do so from panda_moveit_config.

[rhaschke]

Closing in favour of #200.