Rosanswers logo

Dear community,

I have some questions about the trajectory_filter...

First of all, here is how we currently set it all up:

  • cob_arm_navigation/cob3_trajectory_filter.launch (sorry, can't post or attach the file on answers.ros)

  • cob_arm_navigation/filters.yaml (dito)

  • in cob_arm_navigation/cob3_move_arm.launch:
    "trajectory_filter_allowed_time" type="double" value="2.0"

This is actually the configuration that I took from the pr2_arm_navigation tutorials a while ago...adopted to the care-o-bot...but I never looked into it more closely.

When I now do planned motion using the OMPL planner or our own PRMCE planner (which I am currently implementing and testing; for the idea behind it see Leven&Hutchinson: "A Framework for Real-time Path Planning in Changing Environments"), we come across some problems:


Moving from a collision-free position to our home position using OMPL planner, I sometimes (not always) get the following EXCEPTION from the trajectory_filter

[ INFO] [1298888734.279572962, 27.239000000]: Received new goal [ INFO] [1298888734.376962222, 27.284000000]: Displaying move arm joint goal. [ INFO] [1298888734.432411426, 27.296000000]: Constraint violated:: Joint name:arm_1_joint, value: 0.508538, Constraint: 0.000000, tolerance_above: 0.100000, tolerance_below: 0.100000 [ WARN] [1298888734.432526798, 27.297000000]: State violates goal constraints.

[ INFO] [1298888734.459090616, 27.305000000]: Received request for planning [ INFO] [1298888734.459197428, 27.305000000]: Selected motion planner: 'kinematic::LBKPIECE[LBKPIECEkConfig2cob]', with priority 11 [ INFO] [1298888734.461874732, 27.305000000]: ompl planning for group arm Info: LBKPIECE1: Starting with 2 states Info: LBKPIECE1: Created 36 (21 start + 15 goal) states in 32 cells (18 start + 14 goal) [ INFO] [1298888734.589164524, 27.353000000]: Ompl says ok [ INFO] [1298888734.590284186, 27.353000000]: Motion planning succeeded [ INFO] [1298888734.656534973, 27.396000000]: Done planning. Transitioning to control [ INFO] [1298888734.900913391, 27.523000000]: Got trajectory with 27 points [ INFO] [1298888734.931195555, 27.538000000]: Trajectory filter took 0.015000 seconds [ERROR] [1298888734.931611377, 27.538000000]: Exception thrown while processing service call: Duration is out of dual 32-bit range [ERROR] [1298888734.931930840, 27.538000000]: Service call to filter trajectory failed. [ INFO] [1298888734.932046852, 27.538000000]: Sending trajectory with 27 points and timestamp: 27.738000 [ INFO] [1298888734.932089759, 27.538000000]: Joint: 0 name: arm_1_joint [ INFO] [1298888734.932120095, 27.538000000]: Joint: 1 name: arm_2_joint [ INFO] [1298888734.932147412, 27.538000000]: Joint: 2 name: arm_3_joint [ INFO] [1298888734.932173614, 27.538000000]: Joint: 3 name: arm_4_joint [ INFO] [1298888734.932200234, 27.538000000]: Joint: 4 name: arm_5_joint [ INFO] [1298888734.932226304, 27.538000000]: Joint: 5 name: arm_6_joint [ INFO] [1298888734.932252208, 27.538000000]: Joint: 6 name: arm_7_joint [ INFO] [1298888736.978149558, 28.600000000]: Trajectory controller status came back as failed [ INFO] [1298888737.067294931, 28.642000000]: Constraint violated:: Joint name:arm_2_joint, value: -0.465754, Constraint: 0.000000, tolerance_above: 0.100000, tolerance_below: 0.100000 [ WARN] [1298888737.084768419, 28.659000000]: State violates goal constraints.

[ WARN] [1298888737.085246261, 28.659000000]: Though trajectory is done current state does not seem to be at goal

But the filtered trajectory does have filled time_from_start and also the movement in visualization does reach the goal. Also, the planned trajectory does already have a duration time_from_start, which starts with 0.0 for the first trajectory point and than adds 10 ms per point (equidistant).

2) BTW, why does the planned trajectory (PATH?) already include time_from_start?

After the trajectory filter they seem to be adjusted and not equidistant anymore. How are these new time_from_start s calculated and where? I guess it's with respect to the distance of the neighboring trajectory points and the max_vel of the arm.

As described in http://code.ros.org/lurker/message/20110211.181836.d7d7eafa.en.html

we also have problems with moving on that filtered trajectory! We first assumed that it has something to do with the dynamic model of the arm, but maybe it's just because the arm can not hold the velocities that derives from these time_from_start s.

Since, when setting the time_from_start to about 2 seconds per trajectory point, it works fine.

I'd be pleased to hear some of your opinions on this! Also, if anybody has suggestions on other/better trajectory_filter configurations, please let me know!!!

As always, if you have questions or need more details, let me know as well!

Thanks! Felix

Originally posted by Felix Messmer on ROS Answers with karma: 198 on 2011-02-28

Post score: 0


1 Answer 1


Rosanswers logo

Hi Felix,

Here's the answer to the 2nd part of your question - how does the trajectory filtering work right now?

The trajectory filter takes paths from the planner and "shortcuts" them with cubic splines to try and find a smoother path to the goal. It does this by trying to connect random waypoints on the plan using acceleration and velocity bounded cubic splines. This is why you see the time_from_start change for the waypoints - the trajectory_filter is changing them so that the resultant trajectory obeys velocity and acceleration constraints and is also collision-free.

The reason your arm cannot track these trajectories probably has more to do with your controller. You can decrease the acceleration and velocity limits so you get slower trajectories, here's where those parameters are specified - http://www.ros.org/wiki/trajectory_filters/Tutorials/Tutorial%201#The_joint_limits_specification

The planners are free to give back just paths and leave the time_from_start parameter at 0. OMPL just chooses to specify a nominal time there with the assumption that a post-processing step will take care of the parameterization into a trajectory.

Originally posted by Sachin Chitta with karma: 1304 on 2011-02-28

This answer was ACCEPTED on the original site

Post score: 1


Your Answer

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