From e63769f50d5beb8893c98fcb960a548b84735263 Mon Sep 17 00:00:00 2001 From: Kazuhiro Sasabuchi Date: Wed, 22 Apr 2015 15:02:15 +0900 Subject: [PATCH 1/3] added angle-vector min-max-table test --- irteus/test/angle-vector-min-max.l | 109 +++++++++++++++++++++++++++++ 1 file changed, 109 insertions(+) create mode 100644 irteus/test/angle-vector-min-max.l diff --git a/irteus/test/angle-vector-min-max.l b/irteus/test/angle-vector-min-max.l new file mode 100644 index 000000000..4e972daa8 --- /dev/null +++ b/irteus/test/angle-vector-min-max.l @@ -0,0 +1,109 @@ +;; test code for angle-vector with min-max + +(require :unittest "lib/llib/unittest.l") +(init-unit-test) + +;; setup sample robot +(defclass 2dof-robot + :super robot-model + :slots (end-coords l1 l2 l3 j1 j2)) +(defmethod 2dof-robot + (:init () + (send-super :init) + (setq l3 (send self :make-link (make-cube 4 3 10) #f(0 0 5) :white 'l3)) + (setq end-coords (make-cascoords :pos #f(0 0 80))) + (send l3 :assoc end-coords) + ;; + (setq l2 (send self :make-link (make-cube 0 0 0) #f(0 0 0) :blue 'l2)) + (send l2 :assoc l3) + (send l2 :locate #f(0 0 1.1)) + ;; + ;; (setq l1 (send self :make-link (body+ (make-cube 6 11 2) + ;; (make-cube 6 3 4 :pos #f(0 4 0)) + ;; (make-cube 6 3 4 :pos #f(0 -4 0)) + ;; ) #f(0 0 -1) :blue 'l1)) + ;; + (setq l1 (send self :make-link (make-cube 6 11 2) #f(0 0 -1) :blue 'l1)) + ;; + (send l1 :assoc l2) + (setq j1 (instance rotational-joint :init :parent-link l1 :child-link l2 :axis :z + :min-angle -90 :max-angle 90) + j2 (instance rotational-joint :init :parent-link l2 :child-link l3 :axis :y + :min-angle -45 :max-angle 45)) + ;; + (setq links (list l1 l2 l3) joint-list (list j1 j2)) + (send self :init-ending) + self) + (:make-link (b off color name) + (send b :locate off) (send b :set-color color) + (instance bodyset-link :init (make-cascoords) :bodies (list b) :name name)) + ;; + (:j1 (&rest args) (forward-message-to j1 args)) + (:j2 (&rest args) (forward-message-to j2 args)) + (:end-coords (&rest args) (forward-message-to end-coords args)) + ) + +(setq *robot* (instance 2dof-robot :init)) +(objects (list *robot*)) + + +;; test angle-vector without min-max +(defun test-joint-common + (&key (margin 0)) + (let* ((j1-min-orig (send *robot* :j1 :min-angle)) (j2-min-orig (send *robot* :j2 :min-angle)) + (j1-max-orig (send *robot* :j1 :max-angle)) (j2-max-orig (send *robot* :j2 :max-angle)) + tmpj2-j1 + tmpj1-j2) + (do ((x j1-min-orig (+ x 5))) + ((> x j1-max-orig)) + (do ((y j2-min-orig (+ y 5))) + ((> y j2-max-orig)) + (send *robot* :j2 :joint-angle y) + (send *robot* :j1 :joint-angle x) + (setq tmpj2-j1 (send *robot* :angle-vector)) + (send *robot* :angle-vector (float-vector x y)) + (assert (v= (send *robot* :angle-vector) tmpj2-j1) (format nil "check angle-vector match (v= ~A ~A)" (send *robot* :angle-vector) tmpj2-j1)) + (send *robot* :j1 :joint-angle x) + (send *robot* :j2 :joint-angle y) + (setq tmpj1-j2 (send *robot* :angle-vector)) + (send *robot* :angle-vector (float-vector x y)) + (assert (v= (send *robot* :angle-vector) tmpj1-j2) (format nil "check angle-vector match (v= ~A ~A)" (send *robot* :angle-vector) tmpj1-j2)) + (send *irtviewer* :draw-objects) + )))) + +;; test angle-vector with min-max +(defun test-make-joint-min-max-table-common + (&key (margin 0)) ;; [deg] + (let* ((j1-min-orig (send *robot* :j1 :min-angle)) (j2-min-orig (send *robot* :j2 :min-angle)) + (j1-max-orig (send *robot* :j1 :max-angle)) (j2-max-orig (send *robot* :j2 :max-angle)) + tmpj2-j1 + tmpj1-j2) + (send *robot* :self-collision-check) + (send *robot* :make-joint-min-max-table + (send (send *robot* :j1) :parent-link) (send (send *robot* :j2) :child-link) (send *robot* :j1) (send *robot* :j2) + :margin margin) + (do ((x j1-min-orig (+ x 5))) + ((> x j1-max-orig)) + (do ((y j2-min-orig (+ y 5))) + ((> y j2-max-orig)) + (send *robot* :j2 :joint-angle y) + (send *robot* :j1 :joint-angle x) + (setq tmpj2-j1 (send *robot* :angle-vector)) + (send *robot* :angle-vector (float-vector x y)) + (assert (v= (send *robot* :angle-vector) tmpj2-j1) (format nil "check angle-vector match (v= ~A ~A)" (send *robot* :angle-vector) tmpj2-j1)) + (send *robot* :j1 :joint-angle x) + (send *robot* :j2 :joint-angle y) + (setq tmpj1-j2 (send *robot* :angle-vector)) + (send *robot* :angle-vector (float-vector x y)) + (assert (v= (send *robot* :angle-vector) tmpj1-j2) (format nil "check angle-vector match (v= ~A ~A)" (send *robot* :angle-vector) tmpj1-j2)) + (send *irtviewer* :draw-objects) + )))) + +(deftest test-joint-common-margin-0deg + (test-joint-common :margin 0)) + +(deftest test-make-joint-min-max-table-margin-0deg + (test-make-joint-min-max-table-common :margin 0)) + +(run-all-tests) +(exit) From 47165008c6e92d82f9b346dab15c7e181566542e Mon Sep 17 00:00:00 2001 From: Kazuhiro Sasabuchi Date: Wed, 22 Apr 2015 16:02:24 +0900 Subject: [PATCH 2/3] fixed v= to eps-v= --- irteus/test/angle-vector-min-max.l | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/irteus/test/angle-vector-min-max.l b/irteus/test/angle-vector-min-max.l index 4e972daa8..669a59e0a 100644 --- a/irteus/test/angle-vector-min-max.l +++ b/irteus/test/angle-vector-min-max.l @@ -62,12 +62,12 @@ (send *robot* :j1 :joint-angle x) (setq tmpj2-j1 (send *robot* :angle-vector)) (send *robot* :angle-vector (float-vector x y)) - (assert (v= (send *robot* :angle-vector) tmpj2-j1) (format nil "check angle-vector match (v= ~A ~A)" (send *robot* :angle-vector) tmpj2-j1)) + (assert (eps-v= (send *robot* :angle-vector) tmpj2-j1 0.01) (format nil "check angle-vector match (v= ~A ~A)" (send *robot* :angle-vector) tmpj2-j1)) (send *robot* :j1 :joint-angle x) (send *robot* :j2 :joint-angle y) (setq tmpj1-j2 (send *robot* :angle-vector)) (send *robot* :angle-vector (float-vector x y)) - (assert (v= (send *robot* :angle-vector) tmpj1-j2) (format nil "check angle-vector match (v= ~A ~A)" (send *robot* :angle-vector) tmpj1-j2)) + (assert (eps-v= (send *robot* :angle-vector) tmpj1-j2 0.01) (format nil "check angle-vector match (v= ~A ~A)" (send *robot* :angle-vector) tmpj1-j2)) (send *irtviewer* :draw-objects) )))) @@ -90,12 +90,12 @@ (send *robot* :j1 :joint-angle x) (setq tmpj2-j1 (send *robot* :angle-vector)) (send *robot* :angle-vector (float-vector x y)) - (assert (v= (send *robot* :angle-vector) tmpj2-j1) (format nil "check angle-vector match (v= ~A ~A)" (send *robot* :angle-vector) tmpj2-j1)) + (assert (eps-v= (send *robot* :angle-vector) tmpj2-j1 0.01) (format nil "check angle-vector match (v= ~A ~A)" (send *robot* :angle-vector) tmpj2-j1)) (send *robot* :j1 :joint-angle x) (send *robot* :j2 :joint-angle y) (setq tmpj1-j2 (send *robot* :angle-vector)) (send *robot* :angle-vector (float-vector x y)) - (assert (v= (send *robot* :angle-vector) tmpj1-j2) (format nil "check angle-vector match (v= ~A ~A)" (send *robot* :angle-vector) tmpj1-j2)) + (assert (eps-v= (send *robot* :angle-vector) tmpj1-j2 0.01) (format nil "check angle-vector match (v= ~A ~A)" (send *robot* :angle-vector) tmpj1-j2)) (send *irtviewer* :draw-objects) )))) From af6dcc38110ad9942fc1c0030f8ded824958be3b Mon Sep 17 00:00:00 2001 From: Kazuhiro Sasabuchi Date: Thu, 23 Apr 2015 14:23:14 +0900 Subject: [PATCH 3/3] changed robot same as joint.l --- irteus/test/angle-vector-min-max.l | 26 +++++++++++--------------- 1 file changed, 11 insertions(+), 15 deletions(-) diff --git a/irteus/test/angle-vector-min-max.l b/irteus/test/angle-vector-min-max.l index 669a59e0a..fcf2b3a53 100644 --- a/irteus/test/angle-vector-min-max.l +++ b/irteus/test/angle-vector-min-max.l @@ -10,34 +10,30 @@ (defmethod 2dof-robot (:init () (send-super :init) - (setq l3 (send self :make-link (make-cube 4 3 10) #f(0 0 5) :white 'l3)) + (setq l3 (send self :make-link (make-cube 40 40 80) #f(0 0 40) :read 'l3)) (setq end-coords (make-cascoords :pos #f(0 0 80))) (send l3 :assoc end-coords) - ;; - (setq l2 (send self :make-link (make-cube 0 0 0) #f(0 0 0) :blue 'l2)) + (send l3 :locate #f(0 0 10)) + ;; + (setq l2 (send self :make-link (make-cube 60 60 10) #f(0 0 5) :blue 'l2)) (send l2 :assoc l3) - (send l2 :locate #f(0 0 1.1)) - ;; - ;; (setq l1 (send self :make-link (body+ (make-cube 6 11 2) - ;; (make-cube 6 3 4 :pos #f(0 4 0)) - ;; (make-cube 6 3 4 :pos #f(0 -4 0)) - ;; ) #f(0 0 -1) :blue 'l1)) - ;; - (setq l1 (send self :make-link (make-cube 6 11 2) #f(0 0 -1) :blue 'l1)) - ;; + (send l2 :locate #f(0 0 80)) + ;; + (setq l1 (send self :make-link (body+ (make-cube 30 20 80 :pos #f(0 0 40)) + (make-cube 300 300 2)) #f(0 0 0) :white 'l1)) (send l1 :assoc l2) (setq j1 (instance rotational-joint :init :parent-link l1 :child-link l2 :axis :z :min-angle -90 :max-angle 90) j2 (instance rotational-joint :init :parent-link l2 :child-link l3 :axis :y - :min-angle -45 :max-angle 45)) - ;; + :min-angle -90 :max-angle 90)) + ;; (setq links (list l1 l2 l3) joint-list (list j1 j2)) (send self :init-ending) self) (:make-link (b off color name) (send b :locate off) (send b :set-color color) (instance bodyset-link :init (make-cascoords) :bodies (list b) :name name)) - ;; + ;; (:j1 (&rest args) (forward-message-to j1 args)) (:j2 (&rest args) (forward-message-to j2 args)) (:end-coords (&rest args) (forward-message-to end-coords args))