ik-bench.l

#!/usr/bin/env roseus

;; take benchmark of inverse kinematics

(require "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l")
(require "package://hrpsys_ros_bridge_tutorials/euslisp/jaxon-interface.l")
(require "package://hrpsys_ros_bridge_tutorials/euslisp/jaxon_red-interface.l")
;;(objects (list *hrp2*))

(defun benchmark-normal-setup (r &key (test-times 100) (iteration-times 100))
  (format t "benchrmark of ~A~%" (send r :name))
  (format t "  benchmarking of simple inverse kinematics~%")
  (format t "  benchmark parameters:~%")
  (format t "    :test-times ~A~%" test-times)
  (format t "    :iteration-times ~A~%" iteration-times)
  (let ((timer (instance mtimer :init)))
    (send timer :start)
    (dotimes (i test-times)
      (send r :reset-manip-pose)
      (send r :larm :inverse-kinematics (send (send r :larm :end-coords :copy-worldcoords)
                                              :translate (float-vector 0 0 0))
            :stop iteration-times :min-loop (1- iteration-times)))
    (format t "  took ~A sec for ~A iteration~%" (/ (send timer :stop) test-times)
            iteration-times)
    (format t "  one-iteration: ~A sec~%"
            (/ (/ (send timer :stop) test-times) iteration-times))
    )
  )

(defun benchmark-collision-links (r &key (test-times 10) (iteration-times 100))
  (format t "benchrmark of ~A~%" (send r :name))
  (format t "  benchmarking of simple inverse kinematics~%")
  (format t "  benchmark parameters:~%")
  (format t "    :test-times ~A~%" test-times)
  (format t "    :iteration-times ~A~%" iteration-times)
  ;; no collision
  (format t "no collision~%")
  (let ((timer (instance mtimer :init)))
    (send timer :start)
    (dotimes (i test-times)
      (send r :reset-manip-pose)
      (send r :larm :inverse-kinematics (send (send r :larm :end-coords :copy-worldcoords)
                                              :translate (float-vector 0 0 0))
            :collision-avoidance-link-pair nil
            :stop iteration-times :min-loop (1- iteration-times)))
    (let ((duration (send timer :stop)))
      (format t "  took ~A sec for ~A iteration~%" (/ duration test-times)
              iteration-times)
      (format t "  one-iteration: ~A sec~%"
              (/ (/ duration test-times) iteration-times)))
    )
  (let ((collision-pairs (arm-collision-links r)))
    (dotimes (i (length collision-pairs))
      ;;(format t "~A collision pairs~%" i)
      (let ((target-collision-paris (subseq collision-pairs 0 i)))
        (let ((timer (instance mtimer :init)))
          (send timer :start)
          (dotimes (i test-times)
            ;;(format t "run ~A/~A~%" i test-times)
            (send r :reset-manip-pose)
            (send r :larm :inverse-kinematics (send (send r :larm :end-coords :copy-worldcoords)
                                                    :translate (float-vector 0 0 0))
                  :collision-avoidance-link-pair target-collision-paris
                  :avoid-collision-distance 100
                  :avoid-collision-null-gain 5.0
                  :avoid-collision-joint-gain 0.8
                  :warnp nil
                  :stop iteration-times :min-loop (1- iteration-times)))
          (let ((duration (send timer :stop)))
            (format t "~A,~A,~A~%" i (/ duration test-times) (/ (/ duration test-times) iteration-times))
            ;; (format t "  took ~A sec for ~A iteration~%" (/ duration test-times)
            ;;         iteration-times)
            ;; (format t "  one-iteration: ~A sec~%"
            ;;         (/ (/ duration test-times) iteration-times))
            )
          )))
  ))


(defun arm-collision-links (r)
  (let* ((all-links (send r :links))
         ;;(arm-links (send r :link-list (send r :larm :end-coords :parent)))
         (arm-links (butlast (send r :larm :links)))
         (non-arm-links (remove-if #'(lambda (l) (member l arm-links)) all-links)))
    (apply #'append (mapcar #'(lambda (non-arm-link)
                                (mapcar #'(lambda (arm-link)
                                            (list arm-link non-arm-link))
                                        arm-links))
                            non-arm-links)
           )))

(unless (boundp '*hrp2*)
  (setq *hrp2* (instance hrp2jsknt-robot :init)))
(unless (boundp '*jaxon*)
  (setq *jaxon* (instance jaxon-robot :init)))
(unless (boundp '*jaxon-red*)
  (setq *jaxon-red* (instance jaxon_red-robot :init)))
(benchmark-normal-setup *hrp2*)
(benchmark-normal-setup *jaxon*)
(benchmark-normal-setup *jaxon-red*)
(benchmark-collision-links *hrp2*)
(benchmark-collision-links *jaxon*)
(benchmark-collision-links *jaxon-red*)

;; (load "package://jsk_ik_server/euslisp/ik-bench.l")
;;(objects (list *hrp2*))