[ros-users] questions on trajectory_filter

Felix Meßmer Felix_Messmer at web.de
Mon Feb 28 11:14:03 UTC 2011


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 #######################

<launch> 
 <node pkg="trajectory_filter_server" name="trajectory_filter" type="trajectory_filter_server" output="screen">
 <!-- <rosparam command="load" file="$(find cob_arm_navigation)/config/joint_limits.yaml"/> -->
 <rosparam command="load" file="$(find cob_arm_navigation)/config/filters.yaml"/>

 <remap from="robot_description" to="robot_description" />
 <!--remap from="collision_map" to="collision_map_occ" /-->
 <!--remap from="collision_map_update" to="collision_map_occ_update" /-->    
 <!--param name="refresh_interval_collision_map" type="double" value="0.0" /-->
 <param name="refresh_interval_kinematic_state" type="double" value="1.0" />
 <param name="bounding_planes" type="string" value="0 0 1 -0.01" />
 <param name="compute_contacts" type="bool" value="false" />

 <!--param name="pointcloud_padd" type="double" value="0.00" /-->

 <rosparam command="load" file="$(find cob_arm_navigation)/config/cob3_robot_padding.yaml" />        

 </node>
</launch>

################################################################################



####################### cob_arm_navigation/filters.yaml #######################

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

########################################################################


In cob_arm_navigation/cob3_move_arm.launch:    <param name="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

___________________________________________________________
NEU: FreePhone - kostenlos mobil telefonieren und surfen!				
Jetzt informieren: http://produkte.web.de/go/webdefreephone



More information about the ros-users mailing list