Controller is taking too long to execute trajectory

[Patrick Goebel]

Hello,

I am now using MoveIt to control a real arm that uses Dynamixel servos.  Everything is working perfectly so far except that in about one in three planning requests I get an error like the following:

[ERROR] [1370828683.408684569]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 2.138772 seconds). Stopping trajectory.

Consequently the arm either doesn't move at all or it stops partway through the trajectory.  I could not find a setting in the MoveGroupCommander Python API that sets a timeout for trajectory *execution*.  Is this something that is set by MoveIt or is it up to the hardware controller?  (I am using the dynamixel_motor stack.)

Thanks,
patrick

[Ioan Sucan]

Hello Patrick,

This is something that should become a parameter:
https://github.com/ros-planning/moveit_ros/issues/256

I will work on this soon. In the meantime, you can hack trajectory_execution_manager.cpp -- there is a multiplicative parameter defined at the top of the file; If you increase the value of that, things should work better.

Ioan

[Michael Ferguson]

Patrick,

One thing I would definitely do is make sure you set velocity and acceleration limits for your joints in config/joint_limits.yaml of your moveit config package. The setup assistant does not pull these from your URDF, so if you have not set them, they are wrong (and default to 1.0, which in most cases is MUCH faster than the limits you've specified for your dynamixels). I've got proper velocity and acceleration limits to match my actual robot, and have never actually seen this error pop up (and, it probably also makes the trajectories look better on the robot, since all joints follow the trajectory as planned not just those commanded at slow speeds).

-Fergs

[Michael Ferguson]

And, although there is no way to do acceleration limits (they aren't in a URDF), I just created a ticket for velocity limits here: https://github.com/ros-planning/moveit_setup_assistant/issues/44

-Fergs

[Patrick Goebel]

OK, cool.  But see also my reply to Fergs.

--patrick

[Patrick Goebel]

Thanks Fergs.  If I set the velocity limits much lower than 1.0 in the joint_limits.yaml file (e.g 0.5) the arm moves with a jerky motion.  But I'm using the dynamixel_motor package at the moment and I imagine you're using your ArbotiX driver.  So I played around and found another interim solution.  I left the velocity limits set to 1.0 in the joint_limits.yaml file but set the trajectory goal time to 0.1 seconds in the dynamixel_motor config file.  This is counterintuitive to me since I thought I would have to set the goal time to a higher value like 5.0 seconds, but it seems to work so I'm not complaining since now I am getting smooth motion without the execution timeout error.

--patrick

[mfr...@vt.edu]

Hey,

I am running in to the same problem as Patrick was. I currently use ros_controller hardware controllers to communicate with the moveit side. The error I get is after I try to send a trajectory it fails:

[ INFO] [1396539042.134775576]: Ready to take MoveGroup commands for group arm.

[ INFO] [1396539042.134906929]: Looking around: no

[ INFO] [1396539042.134953729]: Replanning: no

[ INFO] [1396539114.846604287]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.

[ WARN] [1396539114.846806676]: Execution of motions should always start at the robot's current state. Ignoring the state supplied as start state in the motion planning request

[ INFO] [1396539114.846870269]: Planning attempt 1 of at most 1

[ INFO] [1396539114.857525271]: No planner specified. Using default.

[ INFO] [1396539114.857824327]: LBKPIECE1: Attempting to use default projection.

[ INFO] [1396539114.861084064]: LBKPIECE1: Starting with 1 states

[ INFO] [1396539115.027256042]: LBKPIECE1: Created 26 (13 start + 13 goal) states in 23 cells (12 start (12 on boundary) + 11 goal (11 on boundary))

[ INFO] [1396539115.027331717]: Solution found in 0.169262 seconds

[ INFO] [1396539115.031389858]: Path simplification took 0.003965 seconds

[ERROR] [1396539116.919760884]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 1.867523 seconds). Stopping trajectory.

[ INFO] [1396539116.919829619]: MoveitSimpleControllerManager: Cancelling execution for atlasarm/arm_controller

[ INFO] [1396539117.136201842]: ABORTED: Timeout reached

Is this a problem with Moveit, or my ros_controller hardware controllers. I tried taking Ioans advice and updating the multiplcative parameters in tracjectory_execution_manager.cpp, but I saw three vlaues at the top of file and didnt know which one to use , so I increased all three of them. 

static const ros::Duration DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE(3.0);

static const double DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN = 3.5; // allow 0.5s more than the expected execution time before triggering a trajectory cancel (applied after scaling)

static const double DEFAULT_CONTROLLER_GOAL_DURATION_SCALING = 3.1; // allow the execution of a trajectory to take more time than expected (scaled by a value > 1)

Still the error occurred. Any help to solving this would be greatly appreciated .

Thanks 

Mike

[Adolfo Rodríguez Tsouroukdissian]

On Thu, Apr 3, 2014 at 5:50 PM, <mfr...@vt.edu> wrote:

Hey,

I am running in to the same problem as Patrick was. I currently use ros_controller hardware controllers to communicate with the moveit side. The error I get is after I try to send a trajectory it fails:

[ INFO] [1396539042.134775576]: Ready to take MoveGroup commands for group arm.

[ INFO] [1396539042.134906929]: Looking around: no

[ INFO] [1396539042.134953729]: Replanning: no

[ INFO] [1396539114.846604287]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.

[ WARN] [1396539114.846806676]: Execution of motions should always start at the robot's current state. Ignoring the state supplied as start state in the motion planning request

[ INFO] [1396539114.846870269]: Planning attempt 1 of at most 1

[ INFO] [1396539114.857525271]: No planner specified. Using default.

[ INFO] [1396539114.857824327]: LBKPIECE1: Attempting to use default projection.

[ INFO] [1396539114.861084064]: LBKPIECE1: Starting with 1 states

[ INFO] [1396539115.027256042]: LBKPIECE1: Created 26 (13 start + 13 goal) states in 23 cells (12 start (12 on boundary) + 11 goal (11 on boundary))

[ INFO] [1396539115.027331717]: Solution found in 0.169262 seconds

[ INFO] [1396539115.031389858]: Path simplification took 0.003965 seconds

[ERROR] [1396539116.919760884]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 1.867523 seconds). Stopping trajectory.

[ INFO] [1396539116.919829619]: MoveitSimpleControllerManager: Cancelling execution for atlasarm/arm_controller

[ INFO] [1396539117.136201842]: ABORTED: Timeout reached

Is this a problem with Moveit, or my ros_controller hardware controllers. I tried taking Ioans advice and updating the multiplcative parameters in tracjectory_execution_manager.cpp, but I saw three vlaues at the top of file and didnt know which one to use , so I increased all three of them. 

static const ros::Duration DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE(3.0);

static const double DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN = 3.5; // allow 0.5s more than the expected execution time before triggering a trajectory cancel (applied after scaling)

static const double DEFAULT_CONTROLLER_GOAL_DURATION_SCALING = 3.1; // allow the execution of a trajectory to take more time than expected (scaled by a value > 1)

Still the error occurred. Any help to solving this would be greatly appreciated .

If trajectory execution is not finishing on time, it could be either a very small goal time tolerance (which you seem to have addressed), or unrealistic joint limits (larger max velocities and/or accelerations than what your robot can achieve). Have you checked that the latter make sense?. In the same vein, how long is the total trajectory duration sent to the controllers?. Since you are using ros_control, you can check this by inspecting the controller goal topic:

rostopic echo foo_controller/follow_joint_trajectory/goal

and get the 'time_from_start' field for the last waypoint. You can also see the waypoint velocoties and accelerations, to verify that they are within bounds.

It could also be that your robot has bad control and cannot track the desired trajectory.

Adolfo.

--
Adolfo Rodríguez Tsouroukdissian
Senior robotics engineer
adolfo.r...@pal-robotics.com
http://www.pal-robotics.com

PAL ROBOTICS S.L
c/ Pujades 77-79, 4º4ª
08005 Barcelona, Spain.
Tel. +34.93.414.53.47
Fax.+34.93.209.11.09
Skype: adolfo.pal-robotics
Facebook - Twitter - PAL Robotics YouTube Channel

AVISO DE CONFIDENCIALIDAD: Este mensaje y sus documentos adjuntos, pueden contener información privilegiada y/o confidencial que está dirigida exclusivamente a su destinatario. Si usted recibe este mensaje y no es el destinatario indicado, o el empleado encargado de su entrega a dicha persona, por favor, notifíquelo inmediatamente y remita el mensaje original a la dirección de correo electrónico indicada. Cualquier copia, uso o distribución no autorizados de esta comunicación queda estrictamente prohibida.

CONFIDENTIALITY NOTICE: This e-mail and the accompanying document(s) may contain confidential information which is privileged and intended only for the individual or entity to whom they are addressed.  If you are not the intended recipient, you are hereby notified that any disclosure, copying, distribution or use of this e-mail and/or accompanying document(s) is strictly prohibited.  If you have received this e-mail in error, please immediately notify the sender at the above e-mail address.

[mfr...@vt.edu]

Hey Adolfo,

If trajectory execution is not finishing on time, it could be either a very small goal time tolerance (which you seem to have addressed), or unrealistic joint limits (larger max velocities and/or accelerations than what your robot can achieve). Have you checked that the latter make sense?. In the same vein, how long is the total trajectory duration sent to the controllers?. Since you are using ros_control, you can check this by inspecting the controller goal topic:

The trajectory duration time based on tome from start was
time_from_start:
          secs: 1
          nsecs: 796378092


On RViz side I got this
[ERROR] [1396882317.129955727]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 2.476016 seconds). Stopping trajectory.

Should both times be the same?

It could also be that your robot has bad control and cannot track the desired trajectory.

I do not think this is the case, because when I use fake_joint_controllers I am able to plan&execute just fine.

On a side note I have attached a text  file that shows a trajectory that I  saw through the /goal ros topic, if you see anything that looks out of the ordinary please  advice.

Again thanks for all the help
Mike

[Adolfo Rodríguez Tsouroukdissian]

On Mon, Apr 7, 2014 at 5:12 PM, <mfr...@vt.edu> wrote:

Hey Adolfo,

If trajectory execution is not finishing on time, it could be either a very small goal time tolerance (which you seem to have addressed), or unrealistic joint limits (larger max velocities and/or accelerations than what your robot can achieve). Have you checked that the latter make sense?. In the same vein, how long is the total trajectory duration sent to the controllers?. Since you are using ros_control, you can check this by inspecting the controller goal topic:

The trajectory duration time based on tome from start was
time_from_start:
          secs: 1
          nsecs: 796378092


On RViz side I got this
[ERROR] [1396882317.129955727]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 2.476016 seconds). Stopping trajectory.

Should both times be the same?

No, the upper bound should be larger that the nominal time, that's what the goal time tolerance is for.

It could also be that your robot has bad control and cannot track the desired trajectory.

I do not think this is the case, because when I use fake_joint_controllers I am able to plan&execute just fine.

I don't think there's any control going on in the fake_controllers scenario. I haven't checked the implementation, as I don't use it, but it sounds like a simple echoing of commands to current state. So it could still be the case that your controllers are not tuned. Have you tried to manually send simple raw trajectories (no moveit, just move a single joint) to your robot and see if it properly moves?.

If you instead spy the controller result topic, what error code do you get, does it succeed?.

rostopic echo /foo_controller/follow_joint_trajectory/result

You can also use rqt_plot to compare desired and actual joint trajectories. The /foo_controller/state topic can be used to conveniently extract this data.

You never said, are you working in simulation or with real hardware?.

Adolfo.

 

On a side note I have attached a text  file that shows a trajectory that I  saw through the /goal ros topic, if you see anything that looks out of the ordinary please  advice.

I don't know what your joint limits are, but at least velocities and accelerations look sane, they are are small-ish.

[mfr...@vt.edu]

 I am working with RVIZ, and not the actual hardware. However I have the capability of sending the position,velocity, and acceleration to my hardware, but I am still learning what commands the hardware requires in order to actually move the arm based on those three values the trajectory way point gives me. I only wanted to know why moveit was giving me the error because I wasnt sure if there was a problem with the trajectory that was created, or if my configuration information is invalid,  or something else.

As for the joint limits in example.txt I am able to confirm that the values being sent are within bounds of what I set in my urdf.

is the only way then to confirm whether the trajectory is going to work is be testing it on the actual hardware?

[Adolfo Rodríguez Tsouroukdissian]

On Mon, Apr 7, 2014 at 5:50 PM, <mfr...@vt.edu> wrote:

 I am working with RVIZ, and not the actual hardware. However I have the capability of sending the position,velocity, and acceleration to my hardware, but I am still learning what commands the hardware requires in order to actually move the arm based on those three values the trajectory way point gives me. I only wanted to know why moveit was giving me the error because I wasnt sure if there was a problem with the trajectory that was created, or if my configuration information is invalid,  or something else.

As for the joint limits in example.txt I am able to confirm that the values being sent are within bounds of what I set in my urdf.

is the only way then to confirm whether the trajectory is going to work is be testing it on the actual hardware?

I advise trying on a simulator first. And also validate that your controllers work well before actually testing MoveIt!. Once you make sure the lower level stuff is working as expected, move to higher level functionality.

Adolfo.

[mfr...@vt.edu]

Ok so I am using the joint_trajectory controller from ros_controllers as the controller that would interact with RobotHW which would communiate with the arm using serial commands. When you say low level are you implying what I said in the first sentence?  I know for sure that the actual external controller(controls arm) that I send the serial commands to is working. I am just trying to get a motion plan sent to it. 

My next question is , if the upper bound is larger than the nominal time, then why is the error showing up?  

I tired seeing what rostopic echo /foo_controller/follow_joint_trajectory/result would show me , but after the execution of the trajectory was attempted nothing showed up in for this rostopic. I did see that when I didnt have the controller started I would get this error: 

header: 

  seq: 0

  stamp: 

    secs: 1397060145

    nsecs: 396215258

  frame_id: ''

status: 

  goal_id: 

    stamp: 

      secs: 1397060145

      nsecs: 395121292

    id: /move_group-1-1397060145.395121292

  status: 5

  text: ''

result: 

  error_code: -1

---

So this tells me that the "Result" ros topic  might not get information when it fails to execute the trajectory. 

Mike,

[mfr...@vt.edu]

Well I figured out my problem about why the moveit trajectory kept failing to execute. I feel pretty dumb, but basically I forgot to load up the joint_state_controller from  "ros_controller" meta package in my code. Once this was done the arm was able atleast execute the trajectory. Now the problem seems to be that I have some joints going out of bounds. Hopefully I should be able to fix that on my own. But again Adolfo thanks for the help I am really learning alot about ros_control and moveit!

Mike

[Adolfo Rodríguez Tsouroukdissian]

On Wed, Apr 9, 2014 at 9:12 PM, <mfr...@vt.edu> wrote:

Well I figured out my problem about why the moveit trajectory kept failing to execute. I feel pretty dumb, but basically I forgot to load up the joint_state_controller from  "ros_controller" meta package in my code. Once this was done the arm was able atleast execute the trajectory. Now the problem seems to be that I have some joints going out of bounds. Hopefully I should be able to fix that on my own. But again Adolfo thanks for the help I am really learning alot about ros_control and moveit!

I'm happy to hear that your issues are resolved.

I'd still like to understand what was going on. The joint_trajectory_controller is fine if no /joint_states topic is being published, as it does not use it, but I'd expect the MoveIt! initialization to somehow warn that the joint state info is absent (or stale). I'm pretty sure arm_navigation allowed you to specify the duration of the joint_states buffer that is considered invalid, so MoveIt devs, does this still exist?. If so, it could make sense to have the setup assistant set some default value, or otherwise add some logic that complains if joint state info is required but has never been set.

Adolfo.

[mfr...@vt.edu]

So I wasnt getting any error specifying that the joint_state_controller was not being used to publish the joint poistion information to /joint_states, however the robot_state_publisher on moveit side does subscribe to it, so the rostopic /joint_states exsited.  The joint_trajectory_controller was receiving the trajectory message from moveit, however I was not sending the acutal position, velocity, and acceleration way points to the acutal arm. Therefore the joint_state_controller (when I got it started) was not receiving updates from the actual arm in terms in of positions, so it kept publishing 0's for all the joint positions. Which is why moveit kept giving the error that the trajectory took too long. Hope that makes sense.

Mike

[Angela Faragasso]

Hello Patrick,

I'm working with the schunk lwap4... I'm having the same problem...

I tried all the suggested solutions: checking of the joint_limit in the yaml file; hacking of the trajectory_excecution_manager (if DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN is more the one I get another error: "Current control error exceeds limit"...) So far nothing make the robot execute the plan that the simulated one performs in RViz... 

Can be this due to the controller that I defined?(See the attachment)...I just started with moveit and I still don't know which one I should use and how exactly they work.....

Thanks,
Angela

[Patrick Goebel]

Hi Angela,

Have you tried bringing up rqt_reconfigure while running your MoveIt nodes and then selecting the Trajectory Execution category under the move_group section?  There are three parameters here related to trajectory execution.  I haven't played with them myself, but I'm wondering if these will help your situation.

--patrick

P.S. I don't think there is anything wrong with your kinematics.yaml file.

[sharon temtsin]

Hi Angela,

This is a moveit problem. you just need to enlarge the 'allowed_execution_duration_scaling' parameter (I changed it to be 10.0). you can find it ,like Patrick described, under the /move_group/trajectory_execution in rqt_reconfigure.
Another option is to uncheck the 'execution_duration_monitoring' and then you have no time limitation for your plan execution. you can find it under  /move_group/trajectory_execution too.

Hope it helps,

-sharon