diff --git a/euslisp/robot-assembler-viewer.l b/euslisp/robot-assembler-viewer.l index bafd72b..9851de0 100644 --- a/euslisp/robot-assembler-viewer.l +++ b/euslisp/robot-assembler-viewer.l @@ -416,6 +416,7 @@ (push (list (list :initial-parts t) (list :parts-type (iparts . type)) (list :parts-name (send iparts :name)) + (list :servo-id (send iparts :servo-id)) ;; TODO: generalize this (servo-id -> e.g., parts-attributes), make gui for this ) *attach-history*) (if *robot-name* (setq *current-robot* (make-robot-assembler-robot iparts :name *robot-name*)) @@ -567,6 +568,7 @@ (list :parts-name (send *current-parts* :name)) (list :parts-type (*current-parts* . type)) (list :parts-point (send *current-parts-point* :name)) + (list :servo-id (send *current-parts* :servo-id)) ;; TODO: generalize this (servo-id -> e.g., parts-attributes), make gui for this (list :configuration (if use-config (if (derivedp use-config coordinates) (write-coords-as-string use-config) diff --git a/euslisp/robot-assembler.l b/euslisp/robot-assembler.l index 35f4840..c78baeb 100644 --- a/euslisp/robot-assembler.l +++ b/euslisp/robot-assembler.l @@ -96,6 +96,7 @@ (:active-fixed-points () nil) (:horns (&rest args) nil) (:active-horns () nil) + (:servo-id (&rest args) nil) (:find (name search-method &key ((:class cls))) (let ((res (send self search-method))) ;; type?? (find-if #'(lambda (x) (eq name (send x :name))) res) @@ -168,6 +169,7 @@ (:horns (&rest args) nil) (:active-horns () nil) (:not-active-horns () nil) + (:servo-id (&rest args) nil) (:sensor-parts? () (if extra-data t)) (:sensor-type? (tp) (if extra-data (some #'(lambda (exdata) (eq tp (send exdata :get :type))) extra-data))) @@ -234,7 +236,8 @@ (defclass roboasm-actuator :super roboasm-parts :slots (horns ;; list of roboasm-fixed-point // TODO: horns -> actuated-points - ) + servo-id + ) ) (defmethod roboasm-actuator (:remove-point (pt) @@ -257,6 +260,7 @@ (remove-if #'(lambda (x) (send x :descendants)) horns)) (:not-active-horns () (remove-if-not #'(lambda (x) (send x :descendants)) horns)) + (:servo-id (&optional (arg nil)) (if arg (setq servo-id arg) servo-id)) ) (defclass roboasm-robot @@ -657,6 +661,7 @@ (limit (send start-coords :limit)) (vlimit (send start-coords :vlimit)) (tqlimit (send start-coords :tqlimit)) + (servo-id (send (send start-coords :parent) :servo-id)) type-name ) (if new-name (setq name new-name)) @@ -689,6 +694,7 @@ (format strm "~A~%" offset name type-name);; uniq name (format strm "~A ~%" offset parent-name) (format strm "~A ~%" offset child-name) + (format strm "~A ~%" offset (if servo-id servo-id -1)) (when initial-angle ;; type check (case type (:rotational @@ -1525,6 +1531,7 @@ (p-point (cadr (assoc :parts-point plist))) (attach-point (cadr (assoc :robot-parts-point plist))) (config (cadr (assoc :configuration plist))) + (servo-id (cadr (assoc :servo-id plist))) (p (make-parts-from-key p-type p-name)) r-p p-p ) @@ -1532,6 +1539,7 @@ (progn (setq r-p (send arobot :find attach-point :active-fixed-points)) (setq p-p (send p :find p-point :active-fixed-points)) + (send p :servo-id servo-id) ;; set servo-id here // TODO: support more general attributes ;;(setq r-p (find-if #'(lambda (x) (eq attach-point (send x :name))) (send arobot :active-fixed-points))) ;;(setq p-p (find-if #'(lambda (x) (eq p-point (send x :name))) (send p :active-fixed-points))) (cond