Euslisp code for object-pr2 collision check

pr2-collision-check.l

#!/usr/bin/env roseus

(ros::load-ros-manifest "roseus")
(require "package://pr2eus/pr2-interface.l")

(if (not
     (boundp '*pr2*))
    (pr2-init))
(if (not
     (boundp '*irtviewer*))
    (make-irtviewer))
(send *irtviewer* :objects *pr2*)

(setq *manip-pose* #f(131.783 54.9158 -1.3071 0.272882 -88.7191 -357.653 -5.85497 360.042 -45.3354 1.53671 2.39065 -95.778 -1.10987 -5.93392 359.975 1.67125 48.912))

(setq *init-pose* (float-vector 12.0 45.0 60.0 0.0 -120.0 0.0 -45.0 0.0 -45.0 60.0 0.0 -120.0 0.0 -45.0 0.0 0.0 35.0))

(setq *goal-pos* (make-coords))
(send *goal-pos* :replace-pos #f(600 0 700))

(setq *table* nil)

(defun generate-scene-model()
  (setq *table* (make-cube 500 1000 700))
  (send *table* :translate #f(700 0 350))
  (send *table* :set-color #f(0.87 0.72 0.53))
  (objects *table*)
  )

(defun collision-check(box &optional (arm :larm))
  (let (res)
    (dolist (link (append (send *pr2* arm :links)
			  (send (send (send *pr2* arm :end-coords :parent) :parent) :child-links)))
      ;; (dolist (box *boxes*)
      (setq res (pqp-collision-check link box))
      (ros::ros-info (format nil "Checking :~A~%" link))
        (when (eq res 1)
          (ros::ros-fatal (format nil "check arm collision failed:~A~%" res))
          (return-from collision-check nil))
	;; )
      ) t)
  )


(defun main-run1()
  (generate-scene-model)
  (send *pr2* :larm :inverse-kinematics *goal-pos*)
  (collision-check *table* :larm)
  )

;; collision without error reported
(defun main-run2()
  (generate-scene-model)
  (setq test-pos *goal-pos*)
  (send test-pos :replace-pos #f(700 -10 500))
  (send *pr2* :larm :inverse-kinematics test-pos :rotation-axis :z)
  (collision-check *table* :larm)
  )

(send *pr2* :angle-vector *manip-pose*)