Merge pull request #769 from knorth55/ik-reachability-fetch

[merge after #772] [jsk_ik_server] add fetch ik reachability map script

jsk_ik_server/README.md

@@ -46,6 +46,23 @@ For more detailed infomation. please read [docs/jaxon_ik_evaluation](docs/jaxon_
 ![](./data/jaxon-zup-iterate009-concatenated.png)
 ![](./data/jaxon-zup-iterate009.csv.png)
 
+### Fetch IK Evaluation
+
+#### X-up IK reachability with `:rotation-axis :z`
+
+![](./data/fetch-xup-iterate009-concatenated.png)
+![](./data/fetch-xup-iterate009.csv.png)
+
+#### Y-up IK reachability with `:rotation-axis :z`
+
+![](./data/fetch-yup-iterate009-concatenated.png)
+![](./data/fetch-yup-iterate009.csv.png)
+
+#### Z-up IK reachability with `:rotation-axis :z`
+
+![](./data/fetch-zup-iterate009-concatenated.png)
+![](./data/fetch-zup-iterate009.csv.png)
+
 ### Baxter IK Evaluation
 
 #### X-up IK reachability with `:rotation-axis :z`

jsk_ik_server/euslisp/ik-evaluation.l

@@ -517,12 +517,12 @@
                                          ik-arg
                                          rot-matrix
                                          output-directory
-                                         &key (min-distance 0))
+                                         &key (min-distance 0) (arm :larm))
   (format t "Generate reachability map for ~A with rotation ~A~%" (send robot :name) rot-matrix)
   (setq *grid* (instance ik-grid :init
                          min-max-x min-max-y min-max-z
                          :grid-step grid-step))
-  (ik-evaluation *robot* *grid* :larm ik-arg :update-view nil
+  (ik-evaluation *robot* *grid* arm ik-arg :update-view nil
                  :rotation rot-matrix
                  :min-distance min-distance)
   (send *grid* :dump-to-csv (format nil "~A/~A-input.csv" output-directory file-prefix))

jsk_ik_server/scripts/ik-grid/fetch.l

@@ -0,0 +1,36 @@
+#!/usr/bin/env roseus
+
+(load "package://jsk_ik_server/euslisp/ik-evaluation.l")
+
+(warning-message 2 "It takes much time! Be patient!~%")
+
+(require "package://fetcheus/fetch-interface.l")
+(setq *robot* (make-robot-model-from-name "fetch"))
+(setq *xrange* '(0.0 . 1000.0))
+(setq *yrange* '(-2000.0 . 2000.0))
+(setq *zrange* '(0.0 . 2000.0))
+(setq *output-directory* (ros::resolve-ros-path "package://jsk_ik_server/data/"))
+
+(generate-ik-grid-for-robot *robot* "fetch-zup"
+                            *xrange* *yrange* *zrange*
+                            100
+                            '(:rotation-axis :z :use-torso t)
+                            (unit-matrix)
+                            *output-directory*
+                            :arm :rarm)
+(generate-ik-grid-for-robot *robot* "fetch-yup"
+                            *xrange* *yrange* *zrange*
+                            100
+                            '(:rotation-axis :z :use-torso t)
+                            (send (make-coords :rpy (list 0 0 pi/2)) :worldrot)
+                            *output-directory*
+                            :arm :rarm)
+(generate-ik-grid-for-robot *robot* "fetch-xup"
+                            *xrange* *yrange* *zrange*
+                            100
+                            '(:rotation-axis :z :use-torso t)
+                            (send (make-coords :rpy (list 0 -pi/2 0)) :worldrot)
+                            *output-directory*
+                            :arm :rarm)
+
+(sys::exit 0)

jsk_ik_server/scripts/ik-grid/fetch.sh

@@ -0,0 +1,15 @@
+ #!/bin/sh
+
+rosrun jsk_ik_server fetch.l
+find $(rospack find jsk_ik_server) -name fetch-*.csv -exec rosrun jsk_ik_server plot_ik_grid.py {} {}.png \;
+
+# Concatenate pickview images
+for prefix in xup yup zup
+do
+    convert +append ../../data/fetch-${prefix}-input-slant.png ../../data/fetch-${prefix}-input-up.png ../../data/fetch-${prefix}-input-side.png ../../data/fetch-${prefix}-input-front.png ../../data/fetch-${prefix}-input-concatenated.png
+
+    for i in $(seq 0 9)
+    do
+        convert +append ../../data/fetch-${prefix}-iterate$(printf "%03d" ${i})-slant.png ../../data/fetch-${prefix}-iterate$(printf "%03d" ${i})-up.png ../../data/fetch-${prefix}-iterate$(printf "%03d" ${i})-side.png ../../data/fetch-${prefix}-iterate$(printf "%03d" ${i})-front.png ../../data/fetch-${prefix}-iterate$(printf "%03d" ${i})-concatenated.png
+    done
+done