Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Switch Controller Failed #27

Open
haokun-wang opened this issue Mar 31, 2021 · 5 comments
Open

Switch Controller Failed #27

haokun-wang opened this issue Mar 31, 2021 · 5 comments
Labels
bug Something isn't working good first issue Good for newcomers

Comments

@haokun-wang
Copy link

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
Copy link
Owner

justagist commented Mar 31, 2021

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
Copy link
Author

haokun-wang commented Apr 1, 2021

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
Copy link
Owner

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
Copy link
Owner

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

@haokun-wang
Copy link
Author

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

@justagist justagist added bug Something isn't working good first issue Good for newcomers labels May 12, 2021
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
bug Something isn't working good first issue Good for newcomers
Projects
None yet
Development

No branches or pull requests

2 participants