[pr2eus_moveit] support tm :fast in :angle-vector-motion-plan

[knorth55]

When max_velocity_scaling_factor and max_acceleration_scaling_factor is 1.0 in MotionPlanRequest, it is same as :fast in :angle-vector-raw

Changes

• support tm :fast in :angle-vector-motion-plan
• add :scale key in :angle-vector-motion-plan

Minor Changes

• add trajectory_constraints comment out
• add max_velocity_scaling_factor and max_acceleration_scaling_factor key

[k-okada]

eps= ???

[knorth55]

thank you. Updated

[knorth55]

MotionPlanRequest in hydro does not have max_velocity_scaling_factor and max_acceleration_scaling_factor.

[k-okada]

it seems ok to me, but is it equal to (if (eq total-time :fast) (setq total-time nil)) ???

How about

 (setq orig-total-time (send (send (car (last (send traj :points))) :time_from_start) :to-sec)) ;; [sec]
(ros::ros-info ";; Moveit calculated Trajectory Total Time ~7,3f [sec]" orig-total-time)

...
(when (or (eq total-time :fast) (null total-time) (>= orig-total-time (/ total-time 1000))) ;; if orig-total-time is larger than total-time, use orig-total-time for safety reason.
  (cond ((eq total-time :fast)
         (setq total-time (* scale (* orig-total-time 1000))))
        (t
         (setq total-time (* orig-total-time 1000))))
  (ros::ros-info ";; Scaled Trajectory Total Time ~7,3f [sec]" (/ total-time 1000))
  )
(setq traj (send* self :trajectory-filter traj :start-offset-time start-offset-time :total-time total-time args))
(ros::ros-info ";; generated ~A points for ~A sec using [~A] group" (length (send traj :points)) (/ total-time 1000) (send ret :group_name)))

[knorth55]

Updated.
I change to show ros-info every time.

[knorth55]

@k-okada could you please merge this?