[cob_twist_controller] Investigate JLA constraints behavior

[fmessmer]

Needs to be investigated further, but here are some details I figured out so far:
On schunk_lwa4d with current JointState (see below), sending Zero-Twist results in a constant velocity command (see rqt_plot), TwistController conifg (see below):

fxm@sony-vaio:~$ rostopic echo /arm/joint_states -n 1
header: 
  seq: 71361
  stamp: 
    secs: 1443087826
    nsecs: 351550793
  frame_id: ''
name: ['arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint', 'arm_5_joint', 'arm_6_joint', 'arm_7_joint']
position: [-0.6311459641061895, 1.0047162372030558, -0.021048670779051617, -1.6828988246504923, -1.7275268936239874, 0.7537902506438311, 1.8116168569850744]
velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

---

joint_limits:

<property name="safety_offset" value="0.02"/>
<limit effort="216" velocity="0.43633" lower="${-M_PI+safety_offset}" upper="${M_PI-safety_offset}"/>
<limit effort="216" velocity="0.43633" lower="${-2.14 + safety_offset}" upper="${2.14 - safety_offset}"/>
<limit effort="81.5" velocity="0.4189" lower="${-M_PI + safety_offset}" upper="${M_PI - safety_offset}"/>
<limit effort="81.5" velocity="0.4189" lower="${-2.18 + safety_offset}" upper="${2.18 - safety_offset}"/>
<limit effort="20.7" velocity="0.43633" lower="${-M_PI + safety_offset}" upper="${M_PI - safety_offset}"/>
<limit effort="15" velocity="1.2566" lower="${-2.09 + safety_offset}" upper="${2.09 - safety_offset}"/>
<limit effort="15" velocity="1.2566" lower="${-2.96 + safety_offset}" upper="${2.96 - safety_offset}"/>

rviz screenshot:

rqt_plot screenshot:

From left to right, I swtched JLA_OFF, JLA_ON, JLA_MID, JLA_INEQ.

arm/twist_controller config:

fxm@sony-vaio:~$ rosparam get /arm
arm_1_joint_position_controller: {joint: arm_1_joint, required_drive_mode: 1, type: position_controllers/JointPositionController}
arm_1_joint_velocity_controller: {joint: arm_1_joint, required_drive_mode: 2, type: velocity_controllers/JointVelocityController}
arm_2_joint_position_controller: {joint: arm_2_joint, required_drive_mode: 1, type: position_controllers/JointPositionController}
arm_2_joint_velocity_controller: {joint: arm_2_joint, required_drive_mode: 2, type: velocity_controllers/JointVelocityController}
arm_3_joint_position_controller: {joint: arm_3_joint, required_drive_mode: 1, type: position_controllers/JointPositionController}
arm_3_joint_velocity_controller: {joint: arm_3_joint, required_drive_mode: 2, type: velocity_controllers/JointVelocityController}
arm_4_joint_position_controller: {joint: arm_4_joint, required_drive_mode: 1, type: position_controllers/JointPositionController}
arm_4_joint_velocity_controller: {joint: arm_4_joint, required_drive_mode: 2, type: velocity_controllers/JointVelocityController}
arm_5_joint_position_controller: {joint: arm_5_joint, required_drive_mode: 1, type: position_controllers/JointPositionController}
arm_5_joint_velocity_controller: {joint: arm_5_joint, required_drive_mode: 2, type: velocity_controllers/JointVelocityController}
arm_6_joint_position_controller: {joint: arm_6_joint, required_drive_mode: 1, type: position_controllers/JointPositionController}
arm_6_joint_velocity_controller: {joint: arm_6_joint, required_drive_mode: 2, type: velocity_controllers/JointVelocityController}
arm_7_joint_position_controller: {joint: arm_7_joint, required_drive_mode: 1, type: position_controllers/JointPositionController}
arm_7_joint_velocity_controller: {joint: arm_7_joint, required_drive_mode: 2, type: velocity_controllers/JointVelocityController}
chain_base_link: arm_podest_link
chain_tip_link: arm_7_link
driver:
  bus: {device: can0}
  defaults:
    dcf_overlay: {604Csub1: '1', 604Csub2: '24000'}
    eds_file: config/Schunk_0_63.dcf
    eds_pkg: schunk_lwa4d
    vel_to_device: rint(rad2deg(vel)*250)
  name: arm
  nodes:
    arm_1_joint: {id: 3}
    arm_2_joint: {id: 4}
    arm_3_joint: {id: 5}
    arm_4_joint: {id: 6}
    arm_5_joint: {id: 7}
    arm_6_joint: {id: 8}
    arm_7_joint: {id: 9}
  sync: {interval_ms: 10, overflow: 0}
frame_tracker:
  cart_min_dist_threshold_lin: 0.1
  cart_min_dist_threshold_rot: 0.1
  marker_scale: 0.3
  movable_rot: true
  movable_trans: true
  pid_rot_x: {d: 0.0, i: 0.0, i_clamp: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, p: 1.0}
  pid_rot_y: {d: 0.0, i: 0.0, i_clamp: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, p: 1.0}
  pid_rot_z: {d: 0.0, i: 0.0, i_clamp: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, p: 1.0}
  pid_trans_x: {d: 0.0, i: 0.0, i_clamp: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0,
    p: 1.0}
  pid_trans_y: {d: 0.0, i: 0.0, i_clamp: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0,
    p: 1.0}
  pid_trans_z: {d: 0.0, i: 0.0, i_clamp: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0,
    p: 1.0}
  tracking_frame: arm_7_target
  twist_dead_threshold_lin: 0.05
  twist_dead_threshold_rot: 0.05
  twist_deviation_threshold_lin: 0.5
  twist_deviation_threshold_rot: 0.5
  update_rate: 50.0
joint_group_interpol_position_controller:
  joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint,
    arm_7_joint]
  required_drive_mode: 7
  type: position_controllers/JointGroupPositionController
joint_group_position_controller:
  joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint,
    arm_7_joint]
  required_drive_mode: 1
  type: position_controllers/JointGroupPositionController
joint_group_velocity_controller:
  joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint,
    arm_7_joint]
  required_drive_mode: 2
  type: velocity_controllers/JointGroupVelocityController
joint_limits:
  arm_1_joint: {has_acceleration_limits: true, has_effort_limits: true, has_jerk_limits: true,
    has_velocity_limits: true, max_acceleration: 0.4, max_effort: 5.0, max_jerk: 100.0,
    max_velocity: 0.4}
  arm_2_joint: {has_acceleration_limits: true, has_effort_limits: true, has_jerk_limits: true,
    has_velocity_limits: true, max_acceleration: 0.4, max_effort: 5.0, max_jerk: 100.0,
    max_velocity: 0.4}
  arm_3_joint: {has_acceleration_limits: true, has_effort_limits: true, has_jerk_limits: true,
    has_velocity_limits: true, max_acceleration: 0.4, max_effort: 5.0, max_jerk: 100.0,
    max_velocity: 0.4}
  arm_4_joint: {has_acceleration_limits: true, has_effort_limits: true, has_jerk_limits: true,
    has_velocity_limits: true, max_acceleration: 0.4, max_effort: 5.0, max_jerk: 100.0,
    max_velocity: 0.4}
  arm_5_joint: {has_acceleration_limits: true, has_effort_limits: true, has_jerk_limits: true,
    has_velocity_limits: true, max_acceleration: 0.4, max_effort: 5.0, max_jerk: 100.0,
    max_velocity: 0.4}
  arm_6_joint: {has_acceleration_limits: true, has_effort_limits: true, has_jerk_limits: true,
    has_velocity_limits: true, max_acceleration: 1.0, max_effort: 5.0, max_jerk: 100.0,
    max_velocity: 1.0}
  arm_7_joint: {has_acceleration_limits: true, has_effort_limits: true, has_jerk_limits: true,
    has_velocity_limits: true, max_acceleration: 1.0, max_effort: 5.0, max_jerk: 100.0,
    max_velocity: 1.0}
joint_names: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint,
  arm_7_joint]
joint_state_controller: {publish_rate: 50, type: joint_state_controller/JointStateController}
joint_trajectory_controller:
  action_monitor_rate: 10
  constraints:
    arm_1_joint: {goal: 0.1, trajectory: 0.1}
    arm_2_joint: {goal: 0.1, trajectory: 0.1}
    arm_3_joint: {goal: 0.1, trajectory: 0.1}
    arm_4_joint: {goal: 0.1, trajectory: 0.1}
    arm_5_joint: {goal: 0.1, trajectory: 0.1}
    arm_6_joint: {goal: 0.1, trajectory: 0.1}
    arm_7_joint: {goal: 0.1, trajectory: 0.1}
    goal_time: 0.6
    stopped_velocity_tolerance: 0.05
  joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint,
    arm_7_joint]
  required_drive_mode: 7
  state_publish_rate: 25
  stop_trajectory_duration: 0.5
  type: position_controllers/JointTrajectoryController
max_command_silence: 0.5
robot_state_publisher: {publish_frequency: 50.0, tf_prefix: ''}
root_frame: world
twist_controller:
  activation_buffer_ca: 50.0
  activation_buffer_jla: 300.0
  activation_threshold_ca: 0.1
  activation_threshold_jla: 10.0
  base_ratio: 0.05
  beta: 0.005
  collision_check_links: [arm_1_link, arm_2_link, arm_3_link, arm_4_link, arm_5_link,
    arm_6_link, arm_7_link]
  constraint_ca: 0
  constraint_jla: 3
  controller_interface: 0
  critical_threshold_ca: 0.025
  critical_threshold_jla: 5.0
  damping_ca: 1.0e-06
  damping_factor: 0.01
  damping_jla: 0.0001
  damping_method: 2
  enforce_pos_limits: true
  enforce_vel_limits: true
  eps_damping: 0.003
  eps_truncation: 0.001
  k_H: 1.0
  k_H_ca: -2.0
  k_H_jla: -1.0
  keep_direction: true
  kinematic_extension: 0
  lambda_max: 0.1
  max_vel_lin_base: 0.5
  max_vel_rot_base: 0.5
  mu: -2.0
  numerical_filtering: false
  priority: 500
  priority_ca: 100
  priority_jla: 50
  solver: 2
  tolerance: 5.0
  w_threshold: 0.005

@fmessmer FYI

[fmessmer]

I think the main cause for this behavior is #57
But I'll leave it open until we are fully sure...

[fmessmer]

#57 is not (necessarily) related to the observed behavior (i.e. constant offset/command).
However, the implementation of JLA constrained should be investigated further...

[fmessmer]

We have seen some "drift" on the cob4-1 torso (torso_2_joint) when doing tests related to Elmo-VelocityMode-JointLimits-Tests, i.e. rotating torso_3_joint towards its limit....

• Re-Do these tests with particular attention to the behavior of the cob_twist_controller Limiters and JLA constraint!

The "drift" might also be caused by the issue described in #65

[fmessmer]

Drift still observed on LWA4D after #78 being merged and thus #65 being resolved...
JLA_MID and JLA_INEQ sometimes even try to go towards the limit...rather than away from it!
👉 JLA (all variants) need to be verified!

[fmessmer]

Just for the record:
Investigating the scaling limiters in #97 proved a smooth and satisfying behavior!

Still the JLA-variants need to be investigated further...

• verify behavior of JLA variants - using the various solvers (GPM, WLN)

Also, we were still running into the JointLimits in case JLA is used together with CA enabled when close to an obstacle....maybe the Priority order is not correct....I guess JLA should have higher priority than CA!

• verify priority order and behavior of JLA when used toghether with CA enabled