Switch Controller Failed

[haokun-wang]

Hi, justagist,
Thank you for your nice work of franka arm!
I meet a problem when I try to send a joints position command after a joints velocity command.
There was an error that "robot still moving", and the following position command failed.

I found a notice in franka FCI website, which says "When using velocity interfaces, do not simply command zero velocity in stopping. Since it might be called while the robot is still moving, it would be equivalent to commanding a jump in velocity leading to very high resulting joint-level torques."
It seems that I need to call stopping function to stop velocity controller before I start position controller.

Then, I try to stop the velocity controller and switch to position controller, but the position command failed too. My code looks like that:

robot.exec_velocity_cmd(joints_velocity) # send a velocity command
robot.exit_control_mode() # exit currernt control mode
robot.move_to_joint_positions(joints_positions) # send a position command, but failed

Could you please give some suggestions about that?
Thank you so much!

[justagist]

Hi,

Yes, there is a problem sometimes when switching controllers very quickly, which happens when the first controller is not fully stopped. I am planning to work on a fix for this soon.
In the meantime, the previous controller will automatically stop if you don't send commands for a few milliseconds (you can change the default value for this timeout using robot.set_command_timeout). So a simple workaround, for now, would be to put a delay between the commands when you want to use a different controller.
Eg:

robot.exec_velocity_cmd(joints_velocity) # send a velocity command
rospy.sleep(0.5) # add a delay of 0.5 sec
robot.move_to_joint_positions(joints_positions) # uses position controller (joint_trajectory_controller)

Hope this helps.

-- Saif

[haokun-wang]

Hi, Saif,

Thank you for your reply! I add a delay of 0.5 second between two commands, but it raises an error that "Controller position_joint_trajectory_controller failed with error GOAL_TOLERANCE_VIOLATED". And robot just stopped. My solution is delete the current INTERFACE instance, and create a new one, like that:

robot.exec_velocity_cmd(joints_velocity)
robot.exit_control_mode()
del robot
robot = PandaArm()
robot.move_to_joint_positions(joints_positions)

It works for me, but restarting is time consuming.

Following please find the execution log of franka.sh in master mode, and I hope it can provide more information about this problem.

You can start planning now!

[ INFO] [1617242170.523096108]: FrankaHW: Prepared switching controllers to <none> with parameters limit_rate=1, cutoff_frequency=100, internal_controller=joint_impedance
[ INFO] [1617242186.365964091]: FrankaHW: Prepared switching controllers to joint_velocity with parameters limit_rate=1, cutoff_frequency=100, internal_controller=joint_impedance
[ INFO] [1617242186.366237520]: MotionControllerInterface: Controller franka_ros_interface/velocity_joint_velocity_controller started; Controllers franka_ros_interface/position_joint_position_controller, franka_ros_interface/effort_joint_impedance_controller, franka_ros_interface/effort_joint_torque_controller, position_joint_trajectory_controller stopped.
[ WARN] [1617242214.014615679]: MotionControllerInterface: Command timeout violated: Switching to Default control mode.position_joint_trajectory_controller
[ INFO] [1617242214.014847343]: FrankaHW: Prepared switching controllers to joint_position with parameters limit_rate=1, cutoff_frequency=100, internal_controller=joint_impedance
[ERROR] [1617242214.220702760]: Motion finished commanded, but the robot is still moving! ["joint_motion_generator_velocity_discontinuity", "joint_motion_generator_acceleration_discontinuity"]
control_command_success_rate: 0.98
[ INFO] [1617242214.221702399]: MotionControllerInterface: Controller position_joint_trajectory_controller started; Controllers franka_ros_interface/position_joint_position_controller, franka_ros_interface/effort_joint_torque_controller, franka_ros_interface/effort_joint_impedance_controller, franka_ros_interface/velocity_joint_velocity_controller stopped.
[ INFO] [1617242214.933622318]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1617242214.933892185]: Planning attempt 1 of at most 1
[ INFO] [1617242214.936442792]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1617242214.937333200]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1617242214.949242620]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1617242214.949324125]: Solution found in 0.012267 seconds
[ INFO] [1617242214.953775535]: SimpleSetup: Path simplification took 0.004270 seconds and changed from 3 to 2 states
[ INFO] [1617242214.955471633]: Disabling trajectory recording
[ WARN] [1617242222.459716908]: Controller position_joint_trajectory_controller failed with error GOAL_TOLERANCE_VIOLATED: 
[ WARN] [1617242222.459888508]: Controller handle position_joint_trajectory_controller reports status ABORTED
[ INFO] [1617242222.459961283]: Completed trajectory execution with status ABORTED ...
[ INFO] [1617242222.469554694]: Received event 'stop'
[ INFO] [1617242222.983184141]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1617242222.983282897]: Planning attempt 1 of at most 1
[ INFO] [1617242222.984038301]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1617242222.984329121]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1617242222.995263067]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1617242222.995432991]: Solution found in 0.011220 seconds
[ INFO] [1617242223.003511822]: SimpleSetup: Path simplification took 0.007997 seconds and changed from 3 to 2 states

[justagist]

Hi,
Adding the delay should normally work. It works for me at least. It is possible that the robot has not fully come to rest in 0.5 seconds. Try increasing the delay to make sure that the robot is fully stationary before switching controllers. The reason it works for you when you reinitialise the robot instance is probably because of the robot has time to fully come to rest by the time the interface reloads.

[justagist]

Also I would suggest not to use exit_control_mode for now because it is not implemented properly for handling immediate switching.

[haokun-wang]

Hi, Saif,
Thank you for your suggestion! I'll check the delay time. If there is any improvement, I'll share it here. Thanks a lot!

Hank