search

jsk-ros-pkg / jsk_control

jsk control ros packages
http://github.com/jsk-ros-pkg/jsk_control
14 stars 51 forks source link

footstep plannerの最後の２歩（finalize）で足が干渉する #649

Closed snozawa closed 7 years ago

snozawa commented 7 years ago

footstep plannerの最後の２歩（finalize）で足が干渉します． 詳細は後で報告します．

snozawa commented 7 years ago

再現コードです。 config/HRP2JSKNTS_footstep_planner_params.yamlを以下でおきかえます。

heuristic: straight
default_lfoot_to_rfoot_offset: [0.0, -0.21, 0.0]
footstep_size_x: 0.254
footstep_size_y: 0.150368
rleg_footstep_offset: [0.016411, 0.0, 0.0]
lleg_footstep_offset: [0.016411, 0.0, 0.0]
collision_bbox_size: [0.499967, 0.39561, 1.33184]
collision_bbox_offset: [-0.055675, 0.0, 0.0]
successors:
  - x: 0.15
    y: 0.0
    theta: 0.349066
  - x: 0.15
    y: 0.0
    theta: 0.174533
  - x: 0.15
    y: 0.0
    theta: 0.0
  - x: 0.15
    y: 0.0
    theta: -0.174533
  - x: 0.15
    y: 0.0
    theta: -0.349066
  - x: 0.1
    y: 0.0
    theta: 0.349066
  - x: 0.1
    y: 0.0
    theta: 0.174533
  - x: 0.1
    y: 0.0
    theta: 0.0
  - x: 0.1
    y: 0.0
    theta: -0.174533
  - x: 0.1
    y: 0.0
    theta: -0.349066
  - x: 0.1
    y: -0.05
    theta: 0.349066
  - x: 0.1
    y: -0.05
    theta: 0.174533
  - x: 0.1
    y: -0.05
    theta: 0.0
  - x: 0.1
    y: -0.05
    theta: -0.174533
  - x: 0.1
    y: -0.05
    theta: -0.349066
  - x: 0.05
    y: 0.0
    theta: 0.349066
  - x: 0.05
    y: 0.0
    theta: 0.174533
  - x: 0.05
    y: 0.0
    theta: 0.0
  - x: 0.05
    y: 0.0
    theta: -0.174533
  - x: 0.05
    y: 0.0
    theta: -0.349066
  - x: 0.05
    y: -0.05
    theta: 0.349066
  - x: 0.05
    y: -0.05
    theta: 0.174533
  - x: 0.05
    y: -0.05
    theta: 0.0
  - x: 0.05
    y: -0.05
    theta: -0.174533
  - x: 0.05
    y: -0.05
    theta: -0.349066
  - x: 0.0
    y: 0.0
    theta: 0.349066
  - x: 0.0
    y: 0.0
    theta: 0.174533
  - x: 0.0
    y: 0.0
    theta: 0.0
  - x: 0.0
    y: 0.0
    theta: -0.174533
  - x: 0.0
    y: 0.0
    theta: -0.349066
  - x: 0.0
    y: -0.05
    theta: 0.349066
  - x: 0.0
    y: -0.05
    theta: 0.174533
  - x: 0.0
    y: -0.05
    theta: 0.0
  - x: 0.0
    y: -0.05
    theta: -0.174533
  - x: 0.0
    y: -0.05
    theta: -0.349066
  - x: -0.05
    y: 0.0
    theta: 0.349066
  - x: -0.05
    y: 0.0
    theta: 0.174533
  - x: -0.05
    y: 0.0
    theta: 0.0
  - x: -0.05
    y: 0.0
    theta: -0.174533
  - x: -0.05
    y: 0.0
    theta: -0.349066
  - x: -0.05
    y: -0.05
    theta: 0.349066
  - x: -0.05
    y: -0.05
    theta: 0.174533
  - x: -0.05
    y: -0.05
    theta: 0.0
  - x: -0.05
    y: -0.05
    theta: -0.174533
  - x: -0.05
    y: -0.05
    theta: -0.349066

次に、

  1. roslaunch jsk_footstep_planner optimistic_footstep_planner.launch USE_SIMPLE_FOOTSTEP_CONTROLLER:=true GLOBAL_FRAME:=odom USE_PERCEPTION:=false ROBOT:=HRP2JSKNTS USE_CONTROLLER:=false USE_MARKER:=false USE_OBSTACLE_MODEL:=true
  2. 
;; Information about obstacle collision model
(setq *all* '(#s(coordinates plist nil rot #2f((1.0 2.277658e-16 0.0) (-2.277658e-16 1.0 0.0) (0.0 0.0 1.0)) pos #f(1950.0 -1300.0 0.0)) #s(coordinates plist nil rot #2f((-0.173648 0.984808 0.0) (-0.984808 -0.173648 0.0) (0.0 0.0 1.0)) pos #f(2600.0 -150.0 0.0)) #1=#s(body plist ((:volume . 8.807672e+08) (:centroid . #f(450.0 -5.098346e-15 450.0))) rot #2f((7.216450e-16 1.0 0.0) (-1.0 7.216450e-16 0.0) (0.0 0.0 1.0)) pos #f(2700.0 -600.0 0.0) parent nil descendants nil worldcoords #s(coordinates plist nil rot #2f((7.216450e-16 1.0 0.0) (-1.0 7.216450e-16 0.0) (0.0 0.0 1.0)) pos #f(2700.0 -600.0 0.0)) manager #1# changed nil box #s(bounding-box geometry::minpoint #f(2328.39 -1607.42 -107.418) geometry::maxpoint #f(3071.61 -492.582 1007.42)) faces (#16=#s(face plist ((:tmp-normal . #f(1.095788e+06 9.313226e-10 -2.561315e+06))) normal #f(0.0 0.0 1.0) distance -998.64 convexp t edges (#24=#s(edge plist nil pvert #3=#f(2334.24 -1598.64 998.64) nvert #2=#f(2334.24 -501.36 998.64) pface #5=#s(face plist ((:tmp-normal . #f(5.501323e+05 2.561315e+06 0.0))) normal #f(-1.0 -7.735087e-16 0.0) distance 2334.24 convexp t edges (#21=#s(edge plist nil pvert #8=#f(2334.24 -501.36 -98.64) nvert #2# pface #11=#s(face plist ((:tmp-normal . #f(5.501323e+05 3.363997e+06 0.0))) normal #f(-5.801315e-16 1.0 0.0) distance 501.36 convexp t edges (#18=#s(edge plist nil pvert #10=#f(3065.76 -501.36 -98.64) nvert #19=#f(3065.76 -501.36 998.64) pface #12=#s(face plist ((:tmp-normal . #f(1.754156e+06 3.363997e+06 0.0))) normal #f(1.0 7.735087e-16 0.0) distance -3065.76 convexp t edges (#14=#s(edge plist nil pvert #6=#f(3065.76 -1598.64 -98.64) nvert #15=#f(3065.76 -1598.64 998.64) pface #4=#s(face plist ((:tmp-normal . #f(1.754156e+06 2.561315e+06 0.0))) normal #f(4.350986e-16 -1.0 0.0) distance -1598.64 convexp t edges (#23=#s(edge plist nil pvert #3# nvert #7=#f(2334.24 -1598.64 -98.64) pface #4# nface #5# angle 1.5708 flags 0) #13=#s(edge plist nil pvert #6# nvert #7# pface #9=#s(face plist ((:tmp-normal . #f(1.082357e+05 8.731149e-11 2.561315e+06))) normal #f(0.0 0.0 -1.0) distance -98.64 convexp t edges (#22=#s(edge plist nil pvert #7# nvert #8# pface #9# nface #5# angle 1.5708 flags 0) #20=#s(edge plist nil pvert #8# nvert #10# pface #9# nface #11# angle 1.5708 flags 0) #17=#s(edge plist nil pvert #10# nvert #6# pface #9# nface #12# angle 1.5708 flags 0) #13#) vertices (#7# #8# #10# #6# #7#) model-normal #f(0.0 0.0 -1.0) model-distance -98.64 holes nil mbody #1# primitive-face #9# id (:bottom)) nface #4# angle 1.5708 flags 0) #14# #25=#s(edge plist nil pvert #15# nvert #3# pface #4# nface #16# angle 1.5708 flags 0)) vertices (#3# #7# #6# #15# #3#) model-normal #f(1.0 0.0 0.0) model-distance -998.64 holes nil mbody #1# primitive-face #4# id (:side 0 0)) nface #12# angle 1.5708 flags 0) #17# #18# #26=#s(edge plist nil pvert #19# nvert #15# pface #12# nface #16# angle 1.5708 flags 0)) vertices (#15# #6# #10# #19# #15#) model-normal #f(0.0 1.0 0.0) model-distance -365.76 holes nil mbody #1# primitive-face #12# id (:side 0 1)) nface #11# angle 1.5708 flags 0) #20# #21# #27=#s(edge plist nil pvert #2# nvert #19# pface #11# nface #16# angle 1.5708 flags 0)) vertices (#19# #10# #8# #2# #19#) model-normal #f(-1.0 0.0 0.0) model-distance -98.64 holes nil mbody #1# primitive-face #11# id (:side 0 2)) nface #5# angle 1.5708 flags 0) #22# #23# #24#) vertices (#2# #8# #7# #3# #2#) model-normal #f(0.0 -1.0 0.0) model-distance -365.76 holes nil mbody #1# primitive-face #5# id (:side 0 3)) nface #16# angle 1.5708 flags 0) #25# #26# #27#) vertices (#2# #3# #15# #19# #2#) model-normal #f(0.0 0.0 1.0) model-distance -998.64 holes nil mbody #1# primitive-face #16# id (:top)) #9# #5# #11# #12# #4#) edges (#22# #24# #20# #21# #27# #17# #18# #26# #23# #13# #14# #25#) vertices (#8# #2# #10# #19# #7# #6# #15# #3#) model-vertices (#f(-98.64 -365.76 -98.64) #f(-98.64 -365.76 998.64) #f(-98.64 365.76 -98.64) #f(-98.64 365.76 998.64) #f(998.64 -365.76 -98.64) #f(998.64 365.76 -98.64) #f(998.64 365.76 998.64) #f(998.64 -365.76 998.64)) convexp t geometry::evertedp nil csg ((:prism (#8# #10# #6# #7#) 1097.28 1)))))

(defun test-fs-plan () ;; Initialize (ros::load-ros-manifest "jsk_footstep_planner") (load "package://jsk_footstep_controller/euslisp/util.l") (unless (boundp 'robot) (initialize-eus-footstep-planning-client) (load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l") (hrp2jsknts) (setq robot hrp2jsknts) (objects (list robot))) ;; Set collision (publish-footstep-planning-obstacle-model-from-eus-pointcloud (get-pointcloud-within-bodies-2D (list (caddr all)))) ;; Plan (setq fs (footstep-array->coords (plan-footstep-from-goal-coords (cadr all) :start-coords (car all) :robot robot ))) (pprint (mapcar #'(lambda (fs0 fs1) (Send (send fs0 :transformation fs1) :worldpos)) (butlast fs) (cdr fs))) ) (warn ";; (test-fs-plan)~%")

をtest-fs-plan.lとして保存して、

roseus "(jsk)" "(rbrain)" # なんでか現状jskrbeusglがいります irteusgl$ (test-fs-plan)

を実行すると、

...

f(-17.3284 -113.188 0.0)

f(3.742439e-05 210.0 0.0))


となって、最後のfinalizeのが-113[mm]とかになり、これはメリこみます。
snozawa commented 7 years ago

上記テストケースで治ってるのを確認しました。 https://github.com/jsk-ros-pkg/jsk_control/pull/653