0
$\begingroup$

Rosanswers logo

Hi,

I'm using move_arm for collision free arm navigation on ROS fuerte and KUKA robot.

Usually things go just fine, but sometimes the filter chain fails and move_arm crashes. I enabled debug output, and noticed that there are NaNs in the trajectory before failing. Here's the last part of the output.

4.510401 1.350002 -0.500625 1.096641 2.923427 0.000000 -0.000000, 0.000052 0.024558 0.038131 -0.061985 -0.000026 0.000000 -0.000767, -0.000767 -0.359263 -0.557837 0.906790 0.000378 0.000000 -0.000000 [DEBUG] [1359107633.997296605]: 1.395015: -nan -nan -nan -nan -nan 0.000000 -nan, -nan -nan -nan -nan -nan 0.000000 -0.000000, -nan -nan -nan -nan -nan 0.000000 17393607366705369643880719888468171476259579135312614891693586456362663844874551296.000000 [1359107633.997331749]: [DEBUG] [1359107633.997369972]: Final trajectory has 31 points and 1.395015 total time [ INFO] [1359107633.997802352]: Final trajectory invalid with error code 1 [ INFO] [1359107633.997844887]: Trajectory error codes size is 0 [ WARN] [1359107633.997901998]: Filter chain failed to process trajectory [move_arm-9] process has died [pid 13489, exit code -11, cmd /opt/ros/fuerte/stacks/arm_navigation/move_arm/bin/move_arm_simple_action arm_ik:=youbot_arm_kinematics/get_constraint_aware_ik arm_fk:=youbot_arm_kinematics/get_fk /trajectory_filter_server/filter_trajectory_with_constraints:=trajectory_filter/filter_trajectory_with_constraints __name:=move_arm __log:=/home/youbot/.ros/log/ac7a1c36-66c8-11e2-9973-742f688386f9/move_arm-9.log]. log file: /home/youbot/.ros/log/ac7a1c36-66c8-11e2-9973-742f688386f9/move_arm-9*.log

My question is: how should I solve this? Why NaNs happens? Maybe I can configure something in the filter chain in order to avoid NaNs? Why move_arm is crashing, anyway? Should I try to avoid this and maybe use the unfiltered trajectory when the filter fails?

Every suggestion is welcome

Thank you

EDIT: additional information

gdb backtrace

0 0xb7e8bdaa in ros::DurationBaseros::Duration::operator-(ros::Duration const&) const () from /opt/ros/fuerte/lib/librostime.so 1 0x080dd063 in move_arm::MoveArm::filterTrajectory (this=0xbfffe544, trajectory_in=..., trajectory_out=...) at /tmp/buildd/ros-fuerte-arm-navigation-1.1.11/debian/ros-fuerte-arm-navigation/opt/ros/fuerte/stacks/arm_navigation/move_arm/src/move_arm_simple_action.cpp:357 2 0x080e6df5 in move_arm::MoveArm::executeCycle (this=0xbfffe544, req=...) at /tmp/buildd/ros-fuerte-arm-navigation-1.1.11/debian/ros-fuerte-arm-navigation/opt/ros/fuerte/stacks/arm_navigation/move_arm/src/move_arm_simple_action.cpp:945 3 0x080ee78c in move_arm::MoveArm::execute (this=0xbfffe544, goal=...) at /tmp/buildd/ros-fuerte-arm-navigation-1.1.11/debian/ros-fuerte-arm-navigation/opt/ros/fuerte/stacks/arm_navigation/move_arm/src/move_arm_simple_action.cpp:1154 4 0x080c3dd9 in operator() (a0=..., this=) at /usr/include/boost/function/function_template.hpp:1013 5 actionlib::SimpleActionServer<arm_navigation_msgs::MoveArmAction_std::allocator<void

::executeLoop (this=0x81852a8) at /opt/ros/fuerte/include/actionlib/server/simple_action_server_imp.h:344 6 0xb7c4348c in thread_proxy () from /usr/lib/libboost_thread.so.1.46.1 7 0xb7c24d4c in start_thread () from /lib/i386-linux-gnu/libpthread.so.0 8 0xb7a32d3e in clone () from /lib/i386-linux-gnu/libc.so.6

EDIT:

ok the crashing problem was due to a bug in trajectory_filter_server, that always returned true even when failing. Now move_arm does not crash anymore.

However, what should I do when trajectory filtering fails? Retry filtering? Would it help? Using the unfiltered trajectory seems not a good solution...

UPDATE: try to filter again the planned trajectory did not help. Replanning neither, the filtering chain keep failing.

I'm trying to keep the unfiltered trajectory when the filtering chain fails. If anyone have better suggestions please let me know.


Originally posted by Kilin on ROS Answers with karma: 56 on 2013-01-24

Post score: 0


Original comments

Comment by sedwards on 2013-01-25:
What does your filters yaml file look like? We had a similar problem and solved it by removing the offending filter. I'd have to dig through my emails to find out which one. I'm out of the office so it will take me some time.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Hello sedwards, thank you for replying.

The filters.yaml file should be like this one (in this moment I have not the laptop on which we run ROS, I took this from the stack repository, but I don't remember to have modified it)

service_type: FilterJointTrajectoryWithConstraints filter_chain: - name: unnormalize_trajectory type: UnNormalizeFilterJointTrajectoryWithConstraints - name: cubic_spline_short_cutter_smoother type: CubicSplineShortCutterFilterJointTrajectoryWithConstraints params: {discretization: 0.01}

Also the joints_limits.yaml could play a role in this issue? It is also left as default, however.

Anyway: by now I've wrote a simple workaround (not so efficient, though), I just replan everything if the trajectory filter chain fails. (I tried to do this inside move_arm, before, without success - the filter chain kept failing again and again, so I just do this at an higher level in my code, checking the final result). After a fail, the subsequent trial always go well, so it is acceptable for now. I have a failure every 8-10 requests, I think.

Anyway it would be great to avoid the filter chain failure at all... Removing one of the two filters could solve the issue, but it could also cause worse trajectories, am I right?

I've not investigated the reason which filter fails, and why, maybe there's a solution at that level...


Originally posted by Kilin with karma: 56 on 2013-01-25

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by sedwards on 2013-01-25:
The CubicSplineShortCutterFilterJointTrajectoryWithConstraints is the one that caused our problems. We just removed it since our controller was performing smoothing at a lower level. Sorry that's only a half answer

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.