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:
1)
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