gistfile1.txt

#!/usr/bin/env roseus
;; test.l
;; Author: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>

(require :pr2 "package://pr2eus/pr2.l")


(defun test-fail ()
  (setq *pr2* (pr2))
  (setq arm :larm av nil)
  (setq target-pose
        (make-coords :pos (float-vector 865.122 111.158 720.624)))

  (send *pr2* :angle-vector
        (float-vector 149.971 100.001 -20.1502 110.003 -110.003 -19.9939 -9.94175 -9.99002 -99.9969 69.9907 -104.986 -89.9965 70.0049 -5.72958 20.0052 -0.035299 50.514))
  
  (ros::ros-info "orig target pose: ~A" target-pose)
  (objects (list *pr2* target-pose))

  ;; prelim 1
  (send target-pose :translate (float-vector 0 0 200) *pr2*) ;; offset from plane
  (send target-pose :rotate pi/2 :y)
  (dotimes (i 3)
    (unless av
      (ros::ros-info "target-pose: ~A" target-pose)
      (setq av
            (send *pr2* arm :inverse-kinematics
                  target-pose
                  :debug-view :no-message
                  :rotation-axis :x))
      (ros::ros-info "orig av: ~A" (send *pr2* :angle-vector))
      (ros::ros-info "target-pose: ~A" target-pose)
      (send target-pose :translate (float-vector -40 0 -20) *pr2*)))
  (ros::ros-info "av: ~A" av)
  )

#|
19.irteusgl$ send *pr2* :angle-vector #f(149.971 100.001 -20.1502 110.003 -110.003 -19.9939 -9.94175 -9.99002 -99.9969 69.9907 -104.986 -89.9965 70.0049 -5.72958 20.0052 -0.035299 50.514)
#f(149.971 100.001 -20.1502 110.003 -110.003 -19.9939 -9.94175 -9.99002 -99.9969 69.9907 -104.986 -89.9965 70.0049 -5.72958 20.0052 -0.035299 50.514)
20.irteusgl$ setq cds #<coordinates #X87e43c0  785.122 111.158 880.624 / 0.0 1.571 0.0>
#<coordinates #X87e43c0  745.122 111.158 860.624 / 0.0 1.571 0.0>
21.irteusgl$ send *pr2* arm :inverse-kinematics cds :rotation-axis :x
#f(149.971 -4.28263 -18.6684 172.382 -23.8886 7.24314 -84.9711 -9.99002 -99.9969 69.9907 -104.986 -89.9965 70.0049 -5.72958 20.0052 -0.035299 50.514)
22.irteusgl$ 
|#

(defun test-fail2 ()
  (setq *pr2* (pr2))
  (setq arm :larm av nil)
  (send *pr2* :angle-vector
        (float-vector 50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0))
  (setq target-pose (make-coords :pos (float-vector 785.122 111.158 880.624)
                                 :rpy (list 0.0 1.571 0.0)))

  (setq av (send *pr2* arm :inverse-kinematics
                 target-pose
                 :debug-view :no-message
                 :rotation-axis :x))
  (ros::ros-info "av: ~A" av))