Bad precision on positionning control

Hello,

I use a Frama Emika with Moveit. I use Desk and Moveit to compare the position control precision.

I had to do a vertical insertion on a tool. On Desk software, I can easily do a vertical up and down. But my MoveIt and Franka state controller, the robot arm seems to shift and don't have repeatable position precision.

I use Franka_control.launch and I don't modify it:

<?xml version="1.0" ?>
<launch>
  <arg name="robot_ip" />
  <arg name="load_gripper" />

  <param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm.urdf.xacro hand:=$(arg load_gripper)" />

  <include file="$(find franka_gripper)/launch/franka_gripper.launch" if="$(arg load_gripper)">
    <arg name="robot_ip" value="$(arg robot_ip)" />
  </include>

  <node name="franka_control" pkg="franka_control" type="franka_control_node" output="screen" required="true">
    <rosparam command="load" file="$(find franka_control)/config/franka_control_node.yaml" />
    <param name="robot_ip" value="$(arg robot_ip)" />
  </node>

  <rosparam command="load" file="$(find franka_control)/config/default_controllers.yaml" />
  <node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="franka_state_controller"/>
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/>
  <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
    <rosparam if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
    <rosparam unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states] </rosparam>
    <param name="rate" value="30"/>
  </node>
</launch>

It seems to use the franka_control_node.yaml and default_controllers.yaml that I don't modify:

joint_names:
  - panda_joint1
  - panda_joint2
  - panda_joint3
  - panda_joint4
  - panda_joint5
  - panda_joint6
  - panda_joint7
arm_id: panda
# Configure the threshold angle for printing joint limit warnings.
joint_limit_warning_threshold: 5 # [rad]
# Activate rate limiter? [true|false]
rate_limiting: true
# Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
cutoff_frequency: 100
# Internal controller for motion generators [joint_impedance|cartesian_impedance]
internal_controller: joint_impedance
# Used to decide whether to enforce realtime mode [enforce|ignore]
realtime_config: enforce
# Configure the initial defaults for the collision behavior reflexes.
collision_config:
  lower_torque_thresholds_acceleration: [104.0, 104.0, 96.0, 96.0, 84.0, 72.0, 64.0]  # [Nm]
  upper_torque_thresholds_acceleration: [104.0, 104.0, 96.0, 96.0, 84.0, 72.0, 64.0]  # [Nm]
  lower_torque_thresholds_nominal: [104.0, 104.0, 96.0, 96.0, 84.0, 72.0, 64.0]  # [Nm]
  upper_torque_thresholds_nominal: [104.0, 104.0, 96.0, 96.0, 84.0, 72.0, 64.0]  # [Nm]
  lower_force_thresholds_acceleration: [80.0, 80.0, 80.0, 100.0, 100.0, 100.0]  # [N, N, N, Nm, Nm, Nm]
  upper_force_thresholds_acceleration: [80.0, 80.0, 80.0, 100.0, 100.0, 100.0]  # [N, N, N, Nm, Nm, Nm]
  lower_force_thresholds_nominal: [80.0, 80.0, 80.0, 100.0, 100.0, 100.0]  # [N, N, N, Nm, Nm, Nm]
  upper_force_thresholds_nominal: [80.0, 80.0, 80.0, 100.0, 100.0, 100.0]  # [N, N, N, Nm, Nm, Nm]

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}

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}

franka_state_controller:
  type: franka_control/FrankaStateController
  publish_rate: 30  # [Hz]
  joint_names:
    - panda_joint1
    - panda_joint2
    - panda_joint3
    - panda_joint4
    - panda_joint5
    - panda_joint6
    - panda_joint7
  arm_id: panda

In franka_control_node the internal_controller is joint_impedance. What is the deference between joint_impedance and cartesian_cartesian_impedance ?

How can I improve my position control ?

Edit: The position static error seems to come from the variable load_gripper. When the load_gripper value is at false, the robot arm gets bag position repeatability and even the end robot part is slowly shifting.

frankaemika / franka_ros

Bad precision on positionning control #286