diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l index 46895fcf..cd5113ed 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l @@ -3,6 +3,8 @@ (unless (assoc :init-org (send hrp2jsknt-robot :methods)) (rplaca (assoc :init (send hrp2jsknt-robot :methods)) :init-org)) +(unless (assoc :init-ending-org (send hrp2jsknt-robot :methods)) + (rplaca (assoc :init-ending (send hrp2jsknt-robot :methods)) :init-ending-org)) (eval `(defmethod hrp2jsknt-robot ,@(get-hrp2-with-hand-class-methods))) @@ -11,9 +13,8 @@ (:init-ending (&rest args) (prog1 - (send-super* :init-ending args) + (send* self :init-ending-org args) (send self :set-additional-end-coords) - (send self :define-min-max-table) )) (:inverse-kinematics (target-coords diff --git a/hrpsys_ros_bridge_tutorials/euslisp/jaxon-utils.l b/hrpsys_ros_bridge_tutorials/euslisp/jaxon-utils.l index 56a045da..25b6c61f 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/jaxon-utils.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/jaxon-utils.l @@ -1,11 +1,14 @@ (require :jaxon "package://hrpsys_ros_bridge_tutorials/models/jaxon.l") (require :thkhand-utils "package://hrpsys_ros_bridge_tutorials/euslisp/thkhand-utils.l") +(unless (assoc :init-ending-org (send JAXON-robot :methods)) + (rplaca (assoc :init-ending (send JAXON-robot :methods)) :init-ending-org)) + (defmethod JAXON-robot (:init-ending (&rest args) (prog1 - (send-super* :init-ending args) + (send* self :init-ending-org args) (send self :add-hoist-point-coords) (send self :add-shin-cushion-parts) (send self :add-shin-contact-coords) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/jaxon_red-utils.l b/hrpsys_ros_bridge_tutorials/euslisp/jaxon_red-utils.l index 483718aa..35f8f36f 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/jaxon_red-utils.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/jaxon_red-utils.l @@ -1,11 +1,14 @@ (require :jaxon "package://hrpsys_ros_bridge_tutorials/models/jaxon_red.l") (require :thkhand-utils "package://hrpsys_ros_bridge_tutorials/euslisp/thkhand-utils.l") +(unless (assoc :init-ending-org (send JAXON_RED-robot :methods)) + (rplaca (assoc :init-ending (send JAXON_RED-robot :methods)) :init-ending-org)) + (defmethod JAXON_RED-robot (:init-ending (&rest args) (prog1 - (send-super* :init-ending args) + (send* self :init-ending-org args) (send self :add-hoist-point-coords) (send self :add-shin-cushion-parts) (send self :add-shin-contact-coords) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/staro-utils.l b/hrpsys_ros_bridge_tutorials/euslisp/staro-utils.l index 7a084d07..ee3b2312 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/staro-utils.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/staro-utils.l @@ -61,11 +61,14 @@ ) ) +(unless (assoc :init-ending-org (send STARO-robot :methods)) + (rplaca (assoc :init-ending (send STARO-robot :methods)) :init-ending-org)) + (defmethod STARO-robot (:init-ending (&rest args) (prog1 - (send-super* :init-ending args) + (send* self :init-ending-org args) (send self :add-additional-body-parts) ;; add contact-end-coords to arms diff --git a/hrpsys_ros_bridge_tutorials/test/test-all-robots-check.l b/hrpsys_ros_bridge_tutorials/test/test-all-robots-check.l new file mode 100644 index 00000000..849bcbbf --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/test/test-all-robots-check.l @@ -0,0 +1,25 @@ +#!/usr/bin/env roseus + +(require :unittest "lib/llib/unittest.l") +(init-unit-test) + +(setq *all-robots* '(hrp2jsk hrp2jsknt hrp2jsknts hrp2w hrp2g urataleg jaxon jaxon_red chidori ystleg hrp4r staro)) + +(deftest test-model-gen + (dolist (rb *all-robots*) + (load (format nil "package://hrpsys_ros_bridge_tutorials/euslisp/~A-interface.l" rb)) + (format t ";; ~A done~%" (send (funcall rb) :name)) + ) + (assert t)) + +(deftest test-joint-min-max-check + (dolist (rb *all-robots*) + (let ((check0 (not (not (find-method (eval (read-from-string (format nil "*~A*" rb))) :define-min-max-table)))) + (check1 (not (not (find-if #'(lambda (j) (send j :joint-min-max-table)) (send (eval (read-from-string (format nil "*~A*" rb))) :joint-list)))))) + (format t ";; ~10,A (has :define-min-max-table = ~3,A) (has min/max table = ~3,A) (total = ~3,A)~%" + rb check0 check1 (eq check0 check1)) + (assert (eq check0 check1)) + ))) + +(run-all-tests) +(exit)