JointPositionController and JointVelocityController misbehave when stopped and started

[goretkin]

If these controllers are stopped and then started later (without unloading and loading), then its update function is called and within it, the dt_ passed to the updatePid function is a very large value, which means that if there is any error at all, the integrator will likely become saturated in one step.

What happens is that the commanded effort will jump when you start the controllers. At first I thought perhaps the controller manager was still calling the update loop, causing the integrator to accumulate error, but the controller manager is working properly.

[goretkin]

Regarding the velocity controller

To illustrate the error this causes, I've only been able to use a python script, not only bash. This is probably due to timing. The demonstration below relies on a race condition so that command_ is changed between the time that the controller manager calls starting (which sets command_ to zero) and update.

The point is that, if update is called after a long period of not being called (because the controller was stopped), and the error is non-zero for either of the following reasons

• the set-point command_ is set at zero, but someone is rotating the wrist,
• the set-point command_ is set to non-zero (how this comment demonstrates it)

then dt_ multiplies this non-zero error.

At least on my setup, this reproduces the error reliably, but it does seem to rely on specific timing of a race condition, so perhaps run it a few times.

here are the steps

1. Start up the robot

2. launch the following file

   <launch>
    <node name="test_unspawn_arms"
          pkg="pr2_controller_manager" type="unspawner"
          args="r_arm_controller" />
   
    <rosparam file="$(find pr2_controller_configuration)/pr2_joint_velocity_controllers.yaml" command="load" />
    <rosparam file="$(find pr2_controller_configuration)/pr2_joint_position_controllers.yaml" command="load" />
   
    <node pkg="pr2_controller_manager" type="spawner" args="--stopped r_wrist_roll_velocity_controller r_wrist_roll_position_controller" 
          name="test_spawn_wrist" />        
   </launch>

3. finally run the python script below [1].

You will see the wrist rotate in the positive direction for 5 secs, then it will quickly spin to joint angle 0.0 (unless it is on it OR if the timing is such that controller initializes the set point to the current position [2]). It will then quickly spin in the negative direction until the integrator is back to zero and then it will resume the actual pace in the negative direction. This is the bug. The joint shouldn't speed up and then slow down.

[1]

#!/usr/bin/python
import rospy

from pr2_mechanism_msgs.srv import LoadController, SwitchController, UnloadController, SwitchControllerRequest
import std_msgs.msg 
import time

rospy.init_node("show_velocity_bug")

pos_ctrlr_name = "r_wrist_roll_position_controller"
vel_ctrlr_name = "r_wrist_roll_velocity_controller"

vel_publisher = rospy.Publisher("/"+vel_ctrlr_name+"/command", std_msgs.msg.Float64)
pos_publisher = rospy.Publisher("/"+pos_ctrlr_name+"/command", std_msgs.msg.Float64)

switch_controller = rospy.ServiceProxy('pr2_controller_manager/switch_controller', SwitchController)

def set_velocity(vel):
    print("Set velocity to %f"%vel)
    assert(
    switch_controller( start_controllers=[vel_ctrlr_name],
                            stop_controllers=[pos_ctrlr_name],
                            strictness=SwitchControllerRequest.STRICT)
    )

    vel_publisher.publish(vel)

def set_position(pos):
    print("Set position to %f"%pos)
    assert(
    switch_controller( start_controllers=[pos_ctrlr_name],
                            stop_controllers=[vel_ctrlr_name],
                            strictness=SwitchControllerRequest.STRICT)
    )

    pos_publisher.publish(pos)

#weirdly the first set_velocity command doesn't work. 
set_velocity(0.0)
time.sleep(1.0)

set_velocity(1.0)
time.sleep(5.0)

set_position(0.0)
#wait some time for `dt_` to get huge. 
time.sleep(5.0)

set_velocity(-1.0)
time.sleep(10.0)

[2] which happens here: https://github.com/PR2/pr2_controllers/blob/02376d1a1899b71f561dc0e669786cfbc4d3d512/robot_mechanism_controllers/src/joint_position_controller.cpp#L147