[Fetch1075] Moveit! path orientation constraints を追加すると自己干渉するようになる (Fetch collides with himself when using Moveit! path orientation constraints)

[sktometometo]

詳細は後ほど記載

[knorth55]

SRDFが少しおかしいので修正．
ZebraDevs/fetch_ros#162

これでFetchが若干肘をベースとこすっていたのが治る？

cc. @708yamaguchi

[knorth55]

特異点周りで関節角度が大きく変更していて，MoveIt的には2つの姿勢を遷移したことになっているが，Euslispの補間で当たっている気がする．
Moveitをもっと細かく解く必要がある？
これはlongest_valid_segment_fractionの変更で可能．
arm_with_torsoグループについても変更した．
ZebraDevs/fetch_ros#164

以下の例では，肩関節が大きく突然変化している．特異点周りで大きく変わっている．
これはMoveIt的には次の瞬間，肩関節が大きく遷移していることになり，補間は実行（Euslisp）に任されている．
その際にこの軌道だとベースにあたる．
コレが原因？

Screencast.2022-10-13.20.38.42.mp4

[knorth55]

にたようなはなし
moveit/moveit#3213

computeCartesianにはjump_thresholdというのがあるが，これがあったらやばい挙動は止められる？

[knorth55]

これも関係しそう？
Orientation ConstraintsをあたえるとOMPLはCartesian Pathで計算するため，joint jumpがおきやすい？と書いてある．
moveit/moveit#2250

[knorth55]

moveit/moveit#2273

これはすごくそれっぽい

[knorth55]

moveit/moveit#562

[knorth55]

検証の仕方

Depends: #1666

roslaunch jsk_fetch_startup fetch_gazebo_bringup.launch

#!/usr/bin/env roseus

(require "package://fetcheus/fetch-interface.l")
(ros::roseus "hoge")

(setq *pre-grasp-pose* (make-coords :pos #f(-64.85 360.9 823.818) :rpy #f(-1.926 0.181 0.85)))
(setq *grasp-pose* (make-coords :pos #f(-50.682 263.943 778.426) :rpy #f(-1.632 0.097 0.772)))
(setq *pro-grasp-pose* (make-coords :pos #f(-46.346 261.287 950) :rpy #f(-1.571 0 0)))
(setq *ready-pose* (make-coords :pos #f(363.334 -139.026 508.384) :rpy #f(0 0 0)))

(fetch-init)

(defun pre-grasp ()
  (send *fetch* :angle-vector (send *ri* :state :potentio-vector))
  (send *fetch* :inverse-kinematics *pre-grasp-pose* :use-torso nil)
  (send *ri* :angle-vector (send *fetch* :angle-vector) 5000)
  (send *ri* :wait-interpolation)
  )

(defun grasp ()
  (send *fetch* :inverse-kinematics *grasp-pose* :use-torso nil)
  (send *ri* :angle-vector (send *fetch* :angle-vector) 1000)
  (send *ri* :wait-interpolation)
  (send *fetch* :inverse-kinematics *pro-grasp-pose* :use-torso nil)
  (send *ri* :angle-vector (send *fetch* :angle-vector) 2000)
  (send *ri* :wait-interpolation)
  )

(defun ready ()
  (setq path-constraints
        (instance moveit_msgs::constraints :init
                  :name ""
                  :orientation_constraints
                  (list (instance moveit_msgs::OrientationConstraint :init
                                  :header
                                  (instance std_msgs::Header :init :frame_id "base_link")
                                  :link_name "gripper_link"
                                  :orientation
                                  (instance geometry_msgs::Quaternion :init
                                            :x 0 :y 0 :z 0 :w 1.0)
                                  :absolute_x_axis_tolerance 3.14
                                  :absolute_y_axis_tolerance 3.14
                                  :absolute_z_axis_tolerance 3.14 
                                  ))
                  ))
  (send *fetch* :inverse-kinematics *ready-pose* :use-torso nil)
  (send *ri* :angle-vector (send *fetch* :angle-vector) 5000 :default-controller 0
        :use-torso t
        :path-constraints path-constraints
        )
  (send *ri* :wait-interpolation)
  )

(defun main ()
  (pre-grasp)
  (grasp)
  (ready)
  )

Good case

Screencast.2022-10-13.23.06.20.mp4

Bad case

Screencast.2022-10-13.23.09.34.mp4

[knorth55]

diff --git a/fetch_moveit_config/config/ompl_planning.yaml b/fetch_moveit_config/config/ompl_planning
index 6910e9f..9af2709 100644
--- a/fetch_moveit_config/config/ompl_planning.yaml
+++ b/fetch_moveit_config/config/ompl_planning.yaml
@@ -36,6 +36,7 @@ arm:
     - PRMstarkConfigDefault
   projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint)
   longest_valid_segment_fraction: 0.005
+  enforce_joint_model_state_space: true
 arm_with_torso:
   planner_configs:
     - SBLkConfigDefault
@@ -51,6 +52,7 @@ arm_with_torso:
     - PRMstarkConfigDefault
   projection_evaluator: joints(torso_lift_joint,shoulder_pan_joint)
   longest_valid_segment_fraction: 0.05
+  enforce_joint_model_state_space: true
 gripper:
   planner_configs:
     - SBLkConfigDefault

@sktometometo
can you try enforce_joint_model_state_space in ompl_planning.yaml fetch_moveit_config?
you can use develop/fetch branch of knorth55/fetch_ros.
https://github.com/knorth55/fetch_ros/tree/develop/fetch

moveit/moveit#541

[knorth55]

enforce_joint_model_state_spaceで自己干渉しなくなった．
これとlongest_valid_segment_fractionの値を小さくすることで，見ている限りは拘束をそれなりに守りながら動いているようである．

このIssueの解決のために必要なPRは以下の4つである．
#1664
ZebraDevs/fetch_ros#162
ZebraDevs/fetch_ros#163
ZebraDevs/fetch_ros#164