From 9db3350737c40177f65dac073483ceab0bb71f39 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Tue, 20 Apr 2021 20:35:08 +0900 Subject: [PATCH 01/17] add :rhand :lhand to robot-model --- irteus/irtrobot.l | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/irteus/irtrobot.l b/irteus/irtrobot.l index 99db73876..023b8cc10 100644 --- a/irteus/irtrobot.l +++ b/irteus/irtrobot.l @@ -90,12 +90,14 @@ :slots (larm-end-coords rarm-end-coords lleg-end-coords rleg-end-coords head-end-coords torso-end-coords + rhand-end-coords lhand-end-coords larm-root-link rarm-root-link lleg-root-link rleg-root-link head-root-link torso-root-link + rhand-root-link lhand-root-link larm-collision-avoidance-links rarm-collision-avoidance-links - larm rarm lleg rleg torso head + larm rarm lleg rleg torso head rhand lhand ;; sensor slots force-sensors imu-sensors cameras ;; @@ -113,7 +115,8 @@ (setq end-coords-list (remove nil (append end-coords-list (list larm-end-coords rarm-end-coords lleg-end-coords rleg-end-coords - head-end-coords torso-end-coords)))) + head-end-coords torso-end-coords + rhand-end-coords lhand-end-coords)))) ) ) ;; End-coords accessor @@ -123,6 +126,8 @@ (:lleg-end-coords () lleg-end-coords) (:head-end-coords () head-end-coords) (:torso-end-coords () torso-end-coords) + (:rhand-end-coords () rhand-end-coords) + (:lhand-end-coords () lhand-end-coords) ;; Root-link accessor (:rarm-root-link () rarm-root-link) (:larm-root-link () larm-root-link) @@ -130,6 +135,8 @@ (:lleg-root-link () lleg-root-link) (:head-root-link () head-root-link) (:torso-root-link () torso-root-link) + (:rhand-root-link () rhand-root-link) + (:lhand-root-link () lhand-root-link) (:limb (limb method &rest args) (let (ret) @@ -326,7 +333,11 @@ (:head (&rest args) (unless args (setq args (list nil))) (send* self :limb :head args)) (:torso (&rest args) - (unless args (setq args (list nil))) (send* self :limb :torso args)) + (unless args (setq args (list nil))) (send* self :limb :torso args)) + (:lhand (&rest args) + (unless args (setq args (list nil))) (send* self :limb :lhand args)) + (:rhand (&rest args) + (unless args (setq args (list nil))) (send* self :limb :rhand args)) (:arms (&rest args) (list (send* self :larm args) (send* self :rarm args))) (:legs (&rest args) (list (send* self :lleg args) (send* self :rleg args))) (:look-at-hand @@ -573,7 +584,7 @@ (let ((tmp-limbs (remove-duplicates (mapcar #'(lambda (l) - (find-if #'(lambda (tmp-limb) (member l (send self tmp-limb :links))) '(:rarm :larm :rleg :lleg :torso :head))) + (find-if #'(lambda (tmp-limb) (member l (send self tmp-limb :links))) '(:rarm :larm :rleg :lleg :torso :head :rhand :lhand))) (send-all (send self :joint-list) :child-link)))) (ordered-joint-list (send self :joint-list))) (format t " #f(~% ") From b1c5abc07d56f3e9b23e6bf135e7e0763aa0a624 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Wed, 5 May 2021 16:15:06 +0900 Subject: [PATCH 02/17] [irtrobot.l] fix indent --- irteus/irtrobot.l | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/irteus/irtrobot.l b/irteus/irtrobot.l index 023b8cc10..179259b52 100644 --- a/irteus/irtrobot.l +++ b/irteus/irtrobot.l @@ -90,11 +90,11 @@ :slots (larm-end-coords rarm-end-coords lleg-end-coords rleg-end-coords head-end-coords torso-end-coords - rhand-end-coords lhand-end-coords + rhand-end-coords lhand-end-coords larm-root-link rarm-root-link lleg-root-link rleg-root-link head-root-link torso-root-link - rhand-root-link lhand-root-link + rhand-root-link lhand-root-link larm-collision-avoidance-links rarm-collision-avoidance-links larm rarm lleg rleg torso head rhand lhand From 25c8b23e5acf21809cffc734035c42a2148cb0ac Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Wed, 5 May 2021 16:15:54 +0900 Subject: [PATCH 03/17] [demo/sample-arm-model.l] add limbs (rarm, rhand) --- irteus/demo/sample-arm-model.l | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/irteus/demo/sample-arm-model.l b/irteus/demo/sample-arm-model.l index 74b80efc2..ea0b5f038 100644 --- a/irteus/demo/sample-arm-model.l +++ b/irteus/demo/sample-arm-model.l @@ -441,6 +441,14 @@ joint-fl joint-fr)) (setq collision-avoidance-links (list sarm-b2 sarm-b3 sarm-b4 sarm-b5 sarm-b6)) + ;; limbs + (setq rarm (list sarm-b1 sarm-b2 sarm-b3 sarm-b4 sarm-b5 sarm-b6)) + (setq rarm-root-link sarm-b0) + (setq rarm-end-coords end-coords) + (setq rhand (list sarm-fr sarm-fl)) + (setq rhand-root-link sarm-b6) + (setq rhand-end-coords end-coords) + (send self :init-ending) self)) (:joint0 (&rest args) (forward-message-to joint0 args)) @@ -458,8 +466,8 @@ ) (:move-fingers (l) - (send joint-fl :joint-angle l) - (send joint-fr :joint-angle l)) + (send self :limb :rhand :angle-vector (float-vector l l)) + l) (:init-ending () (setq bodies (flatten (send-all links :bodies))) @@ -488,8 +496,7 @@ (setq a (send self :open-hand)) (while (> a 0) (if (collision-check-objects - (list (send self :joint-fr :child-link) - (send self :joint-fl :child-link)) + (send self :limb :rhand :links) (list obj)) (return)) (send self :move-fingers a) From 44148c43cfc2278b02b71424e2a7141ccecd449c Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Thu, 6 May 2021 13:08:58 +0900 Subject: [PATCH 04/17] [irtrobot.l] add :hands --- irteus/irtrobot.l | 1 + 1 file changed, 1 insertion(+) diff --git a/irteus/irtrobot.l b/irteus/irtrobot.l index 179259b52..694834b9e 100644 --- a/irteus/irtrobot.l +++ b/irteus/irtrobot.l @@ -340,6 +340,7 @@ (unless args (setq args (list nil))) (send* self :limb :rhand args)) (:arms (&rest args) (list (send* self :larm args) (send* self :rarm args))) (:legs (&rest args) (list (send* self :lleg args) (send* self :rleg args))) + (:hands (&rest args) (list (send* self :lhand args) (send* self :rhand args))) (:look-at-hand (l/r) "look at hand position, l/r supports :rarm, :larm, :arms, and '(:rarm :larm)" From 9196a0f754d801a7e6adc61b8427d890a6a8994f Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 9 May 2021 03:26:16 +0900 Subject: [PATCH 05/17] [irtrobot.l] limb-list --- irteus/demo/sample-arm-model.l | 14 +- irteus/irtbvh.l | 12 +- irteus/irtdyna.l | 10 +- irteus/irtrobot.l | 275 ++++++++++++++++++++------------- 4 files changed, 185 insertions(+), 126 deletions(-) diff --git a/irteus/demo/sample-arm-model.l b/irteus/demo/sample-arm-model.l index ea0b5f038..171b38884 100644 --- a/irteus/demo/sample-arm-model.l +++ b/irteus/demo/sample-arm-model.l @@ -442,12 +442,14 @@ (setq collision-avoidance-links (list sarm-b2 sarm-b3 sarm-b4 sarm-b5 sarm-b6)) ;; limbs - (setq rarm (list sarm-b1 sarm-b2 sarm-b3 sarm-b4 sarm-b5 sarm-b6)) - (setq rarm-root-link sarm-b0) - (setq rarm-end-coords end-coords) - (setq rhand (list sarm-fr sarm-fl)) - (setq rhand-root-link sarm-b6) - (setq rhand-end-coords end-coords) + (send self :add-limb :rarm + :end-coords end-coords + :links (list sarm-b1 sarm-b2 sarm-b3 sarm-b4 sarm-b5 sarm-b6) + :root-link sarm-b0) + (send self :add-limb :rhand + :end-coords end-coords + :links (list sarm-fr sarm-fl) + :root-link sarm-b6) (send self :init-ending) self)) diff --git a/irteus/irtbvh.l b/irteus/irtbvh.l index f463778c4..9a7ac057e 100644 --- a/irteus/irtbvh.l +++ b/irteus/irtbvh.l @@ -507,14 +507,14 @@ Other Sites are: (:copy-joint-to (robot limb joint &optional (sign 1)) (if (find-method robot (intern (format nil "~A-~A-R" (symbol-name limb) (symbol-name joint)) "KEYWORD")) - (send robot limb (intern (format nil "~A-R" (symbol-name joint)) "KEYWORD") :joint-angle - (* sign (elt (send self limb joint :joint :joint-angle) 2)))) + (send robot :limb limb (intern (format nil "~A-R" (symbol-name joint)) "KEYWORD") :joint-angle + (* sign (elt (send self :limb limb joint :joint :joint-angle) 2)))) (if (find-method robot (intern (format nil "~A-~A-P" (symbol-name limb) (symbol-name joint)) "KEYWORD")) - (send robot limb (intern (format nil "~A-P" (symbol-name joint)) "KEYWORD") :joint-angle - (elt (send self limb joint :joint :joint-angle) 0))) + (send robot :limb limb (intern (format nil "~A-P" (symbol-name joint)) "KEYWORD") :joint-angle + (elt (send self :limb limb joint :joint :joint-angle) 0))) (if (find-method robot (intern (format nil "~A-~A-Y" (symbol-name limb) (symbol-name joint)) "KEYWORD")) - (send robot limb (intern (format nil "~A-Y" (symbol-name joint)) "KEYWORD") :joint-angle - (* sign (elt (send self limb joint :joint :joint-angle) 1))))) + (send robot :limb limb (intern (format nil "~A-Y" (symbol-name joint)) "KEYWORD") :joint-angle + (* sign (elt (send self :limb limb joint :joint :joint-angle) 1))))) (:copy-state-to (robot) ;;(warning-message 2 ";; irteus copy-state-to in irtbvh.l robot:~A~%" robot) diff --git a/irteus/irtdyna.l b/irteus/irtdyna.l index 4c6cab57c..df8008a56 100644 --- a/irteus/irtdyna.l +++ b/irteus/irtdyna.l @@ -1146,10 +1146,10 @@ :centroid-thre 0.1 :target-centroid-pos (v- (send self :centroid) (send df :refcog)))) (:move-base-pos - (let ((tc (mapcar #'(lambda (l) (send self l :end-coords :copy-worldcoords)) '(:rleg :lleg)))) + (let ((tc (mapcar #'(lambda (l) (send self :limb l :end-coords :copy-worldcoords)) '(:rleg :lleg)))) (send self :translate (float-vector (- (elt (send df :refcog) 0)) (- (elt (send df :refcog) 1)) 0) :world) (mapcar #'(lambda (l ttc) - (send self l :inverse-kinematics ttc :thre 0.1 :rthre (deg2rad 0.1))) + (send self :limb l :inverse-kinematics ttc :thre 0.1 :rthre (deg2rad 0.1))) '(:rleg :lleg) tc))) ) (push (list :angle-vector (send self :angle-vector) @@ -1732,9 +1732,9 @@ (send self :get-counter-footstep-limbs (car footstep-node-list))) (let ((current-support-limbs (car support-leg-list))) (send self :append-support-leg-coords-list - (mapcar #'(lambda (l) (send robot l :end-coords :copy-worldcoords)) current-support-limbs)) + (mapcar #'(lambda (l) (send robot :limb l :end-coords :copy-worldcoords)) current-support-limbs)) (send self :append-swing-leg-dst-coords-list - (mapcar #'(lambda (l) (send robot l :end-coords :copy-worldcoords)) current-swing-limbs)) + (mapcar #'(lambda (l) (send robot :limb l :end-coords :copy-worldcoords)) current-swing-limbs)) (let ((current-support-leg-coords (car support-leg-coords-list)) (current-swing-leg-dst-coords (car swing-leg-dst-coords-list))) (send self :append-refzmp-cur-list @@ -1758,7 +1758,7 @@ ) swing-leg-src-coords current-swing-leg-dst-coords refzmp-next (send self :get-limbs-zmp - (mapcar #'(lambda (l) (send robot l :end-coords :copy-worldcoords)) (send self :get-counter-footstep-limbs (send-all (car footstep-node-list) :name))) + (mapcar #'(lambda (l) (send robot :limb l :end-coords :copy-worldcoords)) (send self :get-counter-footstep-limbs (send-all (car footstep-node-list) :name))) (send self :get-counter-footstep-limbs (send-all (car footstep-node-list) :name))) refzmp-prev (car refzmp-cur-list)) (send self :make-gait-parameter) diff --git a/irteus/irtrobot.l b/irteus/irtrobot.l index 694834b9e..8f58f6aa3 100644 --- a/irteus/irtrobot.l +++ b/irteus/irtrobot.l @@ -87,17 +87,16 @@ (defclass robot-model :super cascaded-link - :slots (larm-end-coords rarm-end-coords - lleg-end-coords rleg-end-coords - head-end-coords torso-end-coords - rhand-end-coords lhand-end-coords - larm-root-link rarm-root-link - lleg-root-link rleg-root-link - head-root-link torso-root-link - rhand-root-link lhand-root-link + :slots (larm-end-coords rarm-end-coords ;; obsoleted. for backward compatibility + lleg-end-coords rleg-end-coords ;; obsoleted. for backward compatibility + head-end-coords torso-end-coords ;; obsoleted. for backward compatibility + larm-root-link rarm-root-link ;; obsoleted. for backward compatibility + lleg-root-link rleg-root-link ;; obsoleted. for backward compatibility + head-root-link torso-root-link ;; obsoleted. for backward compatibility larm-collision-avoidance-links rarm-collision-avoidance-links - larm rarm lleg rleg torso head rhand lhand + larm rarm lleg rleg torso head ;; obsoleted. for backward compatibility + limb-list ;; sensor slots force-sensors imu-sensors cameras ;; @@ -113,89 +112,142 @@ (not (every #'null (send self :legs :end-coords)))) ;; for robot with legs end-coords (send self :make-support-polygons)) (setq end-coords-list (remove nil (append end-coords-list - (list larm-end-coords rarm-end-coords - lleg-end-coords rleg-end-coords - head-end-coords torso-end-coords - rhand-end-coords lhand-end-coords)))) + (mapcar #'(lambda (l) (cdr (assoc :end-coords (cdr (assoc l limb-list))))) + '(:larm :rarm :lleg :rleg :head :torso))))) ) ) ;; End-coords accessor - (:rarm-end-coords () rarm-end-coords) - (:larm-end-coords () larm-end-coords) - (:rleg-end-coords () rleg-end-coords) - (:lleg-end-coords () lleg-end-coords) - (:head-end-coords () head-end-coords) - (:torso-end-coords () torso-end-coords) - (:rhand-end-coords () rhand-end-coords) - (:lhand-end-coords () lhand-end-coords) + (:rarm-end-coords () (if (cdr (assoc :rarm limb-list)) + (cdr (assoc :end-coords (cdr (assoc :rarm limb-list)))) + rarm-end-coords ;; obsoleted. for backward compatibility + )) + (:larm-end-coords () (if (cdr (assoc :larm limb-list)) + (cdr (assoc :end-coords (cdr (assoc :larm limb-list)))) + larm-end-coords ;; obsoleted. for backward compatibility + )) + (:rleg-end-coords () (if (cdr (assoc :rleg limb-list)) + (cdr (assoc :end-coords (cdr (assoc :rleg limb-list)))) + rleg-end-coords ;; obsoleted. for backward compatibility + )) + (:lleg-end-coords () (if (cdr (assoc :lleg limb-list)) + (cdr (assoc :end-coords (cdr (assoc :lleg limb-list)))) + lleg-end-coords ;; obsoleted. for backward compatibility + )) + (:head-end-coords () (if (cdr (assoc :head limb-list)) + (cdr (assoc :end-coords (cdr (assoc :head limb-list)))) + head-end-coords ;; obsoleted. for backward compatibility + )) + (:torso-end-coords () (if (cdr (assoc :torso limb-list)) + (cdr (assoc :end-coords (cdr (assoc :torso limb-list)))) + torso-end-coords ;; obsoleted. for backward compatibility + )) ;; Root-link accessor - (:rarm-root-link () rarm-root-link) - (:larm-root-link () larm-root-link) - (:rleg-root-link () rleg-root-link) - (:lleg-root-link () lleg-root-link) - (:head-root-link () head-root-link) - (:torso-root-link () torso-root-link) - (:rhand-root-link () rhand-root-link) - (:lhand-root-link () lhand-root-link) + (:rarm-root-link () (if (cdr (assoc :rarm limb-list)) + (cdr (assoc :root-link (cdr (assoc :rarm limb-list)))) + rarm-root-link ;; obsoleted. for backward compatibility + )) + (:larm-root-link () (if (cdr (assoc :larm limb-list)) + (cdr (assoc :root-link (cdr (assoc :larm limb-list)))) + larm-root-link ;; obsoleted. for backward compatibility + )) + (:rleg-root-link () (if (cdr (assoc :rleg limb-list)) + (cdr (assoc :root-link (cdr (assoc :rleg limb-list)))) + rleg-root-link ;; obsoleted. for backward compatibility + )) + (:lleg-root-link () (if (cdr (assoc :lleg limb-list)) + (cdr (assoc :root-link (cdr (assoc :lleg limb-list)))) + lleg-root-link ;; obsoleted. for backward compatibility + )) + (:head-root-link () (if (cdr (assoc :head limb-list)) + (cdr (assoc :root-link (cdr (assoc :head limb-list)))) + head-root-link ;; obsoleted. for backward compatibility + )) + (:torso-root-link () (if (cdr (assoc :torso limb-list)) + (cdr (assoc :root-link (cdr (assoc :torso limb-list)))) + torso-root-link ;; obsoleted. for backward compatibility + )) + (:add-limb + (limb &key links end-coords root-link collision-avoidance-links) + (when (assoc limb limb-list) + (send self :remove-limb limb)) + (let ((l (cons limb + (list (cons :links links) + (cons :end-coords end-coords) + (cons :root-link root-link) + (cons :collision-avoidance-links collision-avoidance-links))))) + (setq limb-list (append limb-list (list l))) + l)) + (:remove-limb + (limb) + (setq limb-list (remove-if #'(lambda (l) (equal (car l) limb)) limb-list))) (:limb - (limb method &rest args) - (let (ret) + (limb &optional method &rest args) + (let ((l (cdr (assoc limb limb-list))) + ret) (case method - (:end-coords - (user::forward-message-to - (send self (read-from-string (format nil "~A-END-COORDS" limb))) - args)) + (:end-coords + (let ((end-coords (cond (l (cdr (assoc :end-coords l))) + ((member limb '(:rarm :larm :rleg :lleg :head :torso)) ;; obsoleted. for backward compatibility + (send self (read-from-string (format nil "~A-END-COORDS" limb)))) + (t (error ";; error: cannot find limb ~A~%" limb))))) + (user::forward-message-to end-coords args))) (:root-link - (user::forward-message-to - (send self (read-from-string (format nil "~A-ROOT-LINK" limb))) - args)) + (let ((root-link (cond (l (cdr (assoc :root-link l))) + ((member limb '(:rarm :larm :rleg :lleg :head :torso)) ;; obsoleted. for backward compatibility + (send self (read-from-string (format nil "~A-ROOT-LINK" limb)))) + (t (error ";; error: cannot find limb ~A~%" limb))))) + (user::forward-message-to root-link args))) (:angle-vector (if args (progn - (mapcar #'(lambda (l a) - (send l :joint :joint-angle a)) - (send self limb) (coerce (car args) cons)) - (send self limb :angle-vector)) - (coerce (mapcar #'(lambda (l) (send l :joint :joint-angle)) - (send self limb)) float-vector))) + (mapcar #'(lambda (link a) + (send link :joint :joint-angle a)) + (send self :limb limb) (coerce (car args) cons)) + (send self :limb limb :angle-vector)) + (coerce (mapcar #'(lambda (link) (send link :joint :joint-angle)) + (send self :limb limb)) float-vector))) (:inverse-kinematics (let* ((link-list (if (memq :link-list args) (cadr (memq :link-list args)) (send self :link-list - (send self limb :end-coords :parent) - (send self limb :root-link)))) + (send self :limb limb :end-coords :parent) + (send self :limb limb :root-link)))) (collision-avoidance-link-pair (if (memq :collision-avoidance-link-pair args) (cadr (memq :collision-avoidance-link-pair args)) (send self :collision-avoidance-link-pair-from-link-list link-list - :collision-avoidance-links (send self limb :collision-avoidance-links))))) + :collision-avoidance-links (send self :linb limb :collision-avoidance-links))))) (send* self :inverse-kinematics (car args) :move-target (if (memq :move-target args) (cadr (memq :move-target args)) - (send self limb :end-coords)) + (send self :limb limb :end-coords)) :collision-avoidance-link-pair collision-avoidance-link-pair :link-list link-list (cdr args)))) (:move-end - (send* self limb :inverse-kinematics args)) + (send* self :limb limb :inverse-kinematics args)) (:move-end-rot - (let ((coords (send self limb :end-coords :copy-worldcoords)) + (let ((coords (send self :limb limb :end-coords :copy-worldcoords)) (angle (pop args)) (axis (pop args)) (wrt (pop args))) (unless wrt (setq wrt :local)) - (send* self limb :move-end + (send* self :limb limb :move-end (send coords :rotate (deg2rad angle) axis wrt) args))) (:move-end-pos - (let ((coords (send self limb :end-coords :copy-worldcoords)) + (let ((coords (send self :limb limb :end-coords :copy-worldcoords)) (pos (pop args)) (wrt (pop args))) (unless wrt (setq wrt :local)) - (send* self limb :move-end (send coords :translate pos wrt) args))) + (send* self :limb limb :move-end (send coords :translate pos wrt) args))) (:look-at - (if (send self :head :links) (send* self :inverse-kinematics-loop-for-look-at limb args))) + (if (send self :limb :head :links) (send* self :inverse-kinematics-loop-for-look-at limb args))) (:collision-avoidance-links + (let ((collision-avoidance-links (cond (l (cdr (assoc :collision-avoidance-links l))) + ((member limb '(:rarm :larm :rleg :lleg :head :torso)) ;; obsoleted. for backward compatibility + (cdr (assoc (intern (format nil "~A-COLLISION-AVOIDANCE-LINKS" + (string-upcase limb))) (send self :slots)))) + (t (error ";; error: cannot find limb ~A~%" limb))))) (user::forward-message-to - (cdr (assoc (intern (format nil "~A-COLLISION-AVOIDANCE-LINKS" - (string-upcase limb))) (send self :slots))) - args)) + collision-avoidance-links + args))) (:links (send self :limb limb nil)) (:joint-list (send-all (send self :limb limb :links) :joint)) (:gripper (send* self :gripper limb args)) @@ -207,8 +259,11 @@ (cond ((or (null method) (send bodyset-link :method method)) (if method - (send-all (cdr (assoc (intern (string-upcase limb)) (send self :slots))) method) - (cdr (assoc (intern (string-upcase limb)) (send self :slots))))) + (send-all (send self :limb limb) method) + (cond (l (cdr (assoc :links l))) + ((member limb '(:rarm :larm :rleg :lleg :head :torso)) ;; obsoleted. for backward compatibility + (cdr (assoc (intern (string-upcase limb)) (send self :slots)))) + (t (error ";; error: cannot find limb ~A~%" limb))))) (t (let ((limb-joint-name (intern (format nil "~A-~A" (string-upcase limb) (string-upcase method)) *keyword-package*))) @@ -218,17 +273,24 @@ )))) ) ;; case method )) ;; defmethod + (:limbs + () + (cond (limb-list (mapcar #'car limb-list)) + ((or rarm larm rleg lleg head torso) ;; obsoleted. for backward compatibility + (remove-if-not #'(lambda (l) (send self :limbs l)) + (list :rarm :larm :rleg :lleg :head :torso)) + (t nil)))) (:inverse-kinematics-loop-for-look-at (limb &rest args) (let* ((move-target (if (memq :move-target args) (cadr (memq :move-target args)) - (send self limb :end-coords))) + (send self :limb limb :end-coords))) (link-list (if (memq :link-list args) (cadr (memq :link-list args)) (send self :link-list (send move-target :parent) - (send self limb :root-link)))) + (send self :limb limb :root-link)))) (stop (if (memq :stop args) (cadr (memq :stop args)) 100)) (warnp (if (memq :warnp args) (cadr (memq :warnp args)) t)) dif-pos dif-rot p-dif-rot (count 0)) @@ -295,7 +357,7 @@ (limb &rest args) (cond ((memq :links args) - (sort (all-child-links (send self limb :end-coords :parent)) #'(lambda (a b) (string< (string (send a :name)) (string (send b :name)))))) + (sort (all-child-links (send self :limb limb :end-coords :parent)) #'(lambda (a b) (string< (string (send a :name)) (string (send b :name)))))) ((memq :joint-list args) (send-all (send self :gripper limb :links) :joint)) ((memq :angle-vector args) @@ -315,7 +377,7 @@ (find sensor-name sens :test #'equal :key #'(lambda (x) (send x :name))))) (:get-sensors-method-by-limb (sensors-type limb) - (remove-if-not #'(lambda (x) (member (send x :parent) (all-child-links (send self limb :root-link)))) + (remove-if-not #'(lambda (x) (member (send x :parent) (all-child-links (send self :limb limb :root-link)))) (send self sensors-type))) ;; sensor accessors (:force-sensors () "Returns force sensors." force-sensors) @@ -334,18 +396,13 @@ (unless args (setq args (list nil))) (send* self :limb :head args)) (:torso (&rest args) (unless args (setq args (list nil))) (send* self :limb :torso args)) - (:lhand (&rest args) - (unless args (setq args (list nil))) (send* self :limb :lhand args)) - (:rhand (&rest args) - (unless args (setq args (list nil))) (send* self :limb :rhand args)) - (:arms (&rest args) (list (send* self :larm args) (send* self :rarm args))) - (:legs (&rest args) (list (send* self :lleg args) (send* self :rleg args))) - (:hands (&rest args) (list (send* self :lhand args) (send* self :rhand args))) + (:arms (&rest args) (list (send* self :limb :larm args) (send* self :limb :rarm args))) + (:legs (&rest args) (list (send* self :limb :lleg args) (send* self :limb :rleg args))) (:look-at-hand (l/r) "look at hand position, l/r supports :rarm, :larm, :arms, and '(:rarm :larm)" (send self :look-at-target - (mapcar #'(lambda (x) (send self x :end-coords)) + (mapcar #'(lambda (x) (send self :limb x :end-coords)) (cond ((consp l/r) l/r) ((eq l/r :arms) '(:rarm :larm)) @@ -404,19 +461,19 @@ "move robot head to look at targets, look-at-target support t/nil float-vector coordinates, center of list of float-vector or list of coordinates" (cond ((float-vector-p look-at-target) - (send self :head :look-at look-at-target)) + (send self :limb :head :look-at look-at-target)) ((coordinates-p look-at-target) - (send self :head :look-at (send look-at-target :worldpos))) + (send self :limb :head :look-at (send look-at-target :worldpos))) ((and (consp look-at-target) (every #'identity (mapcar #'float-vector-p look-at-target))) - (send self :head :look-at + (send self :limb :head :look-at (scale (/ 1.0 (length look-at-target)) (reduce #'v+ look-at-target :initial-value #f(0 0 0))))) ((and (consp look-at-target) (every #'identity (mapcar #'coordinates-p look-at-target))) - (send self :head :look-at + (send self :limb :head :look-at (scale (/ 1.0 (length look-at-target)) (reduce #'v+ (send-all look-at-target :worldpos) :initial-value #f(0 0 0))))) ((null look-at-target)) - (t (send self :head :look-at (send (car target-coords) :worldpos))) + (t (send self :limb :head :look-at (send (car target-coords) :worldpos))) )) ;; init-pose (:init-pose () "Set robot to initial posture." (send self :angle-vector (instantiate float-vector (send self :calc-target-joint-dimension (cdr (send self :links)))))) @@ -451,19 +508,19 @@ ;; set default force & moment by solving mimimum internal forces (let ((target-coords)) (dolist (limb '(:rleg :lleg)) - (if (send self limb) - (push (send self limb :end-coords) target-coords))) + (if (send self :limb limb) + (push (send self :limb limb :end-coords) target-coords))) (let* ((ret-fm (send self :calc-contact-wrenches-from-total-wrench (send-all target-coords :worldpos) :total-wrench (concatenate float-vector (send (car (send self :links)) :force) (send (car (send self :links)) :moment))))) (send self :calc-torque-from-ext-wrenches :force-list (car ret-fm) :moment-list (cadr ret-fm) :target-coords target-coords) ))) (:calc-force-from-joint-torque - (limb all-torque &key (move-target (send self limb :end-coords)) (use-torso)) + (limb all-torque &key (move-target (send self :limb limb :end-coords)) (use-torso)) "Calculates end-effector force and moment from joint torques." (let* ((link-list (send self :link-list (send move-target :parent) - (unless use-torso (car (send self limb :links))))) + (unless use-torso (car (send self :limb limb :links))))) (jacobian (send self :calc-jacobian-from-link-list link-list @@ -528,10 +585,10 @@ )) (:joint-angle-limit-nspace-for-6dof (&key (avoid-nspace-gain 0.01) (limbs '(:rleg :lleg))) - (let* ((ll (mapcar #'(lambda (l) (send self l :links)) limbs)) + (let* ((ll (mapcar #'(lambda (l) (send self :limb l :links)) limbs)) (J (send self :calc-jacobian-from-link-list (mapcar #'(lambda (l) (append (list (car (send self :links))) l)) ll) - :move-target (mapcar #'(lambda (l) (send self l :end-coords)) limbs) + :move-target (mapcar #'(lambda (l) (send self :limb l :end-coords)) limbs) :translation-axis (make-list (length limbs) :initial-element t) :rotation-axis (make-list (length limbs) :initial-element t))) (Jb (make-matrix (array-dimension J 0) 6)) @@ -553,7 +610,7 @@ ))) (:joint-order (limb &optional jname-list) - (let ((joint-list (mapcar #'(lambda (x) (cons x (find-if #'(lambda (y) (eq y (send self x))) (send self limb :joint-list)))) (mapcan #'(lambda (x) (if (substringp (format nil "~A-" (symbol-name limb)) (string x)) (list x)))(send self :methods)))) + (let ((joint-list (mapcar #'(lambda (x) (cons x (find-if #'(lambda (y) (eq y (send self x))) (send self :limb limb :joint-list)))) (mapcan #'(lambda (x) (if (substringp (format nil "~A-" (symbol-name limb)) (string x)) (list x)))(send self :methods)))) j (result) name) (unless jname-list (setq jname-list @@ -585,12 +642,12 @@ (let ((tmp-limbs (remove-duplicates (mapcar #'(lambda (l) - (find-if #'(lambda (tmp-limb) (member l (send self tmp-limb :links))) '(:rarm :larm :rleg :lleg :torso :head :rhand :lhand))) + (find-if #'(lambda (tmp-limb) (member l (send self :limb tmp-limb :links))) (send self :limbs))) (send-all (send self :joint-list) :child-link)))) (ordered-joint-list (send self :joint-list))) (format t " #f(~% ") (dolist (tmp-limb tmp-limbs) - (dolist (j (send self tmp-limb :joint-list)) + (dolist (j (send self :limb tmp-limb :joint-list)) (format t "~6,2f " (elt vec (position j ordered-joint-list)))) (format t "~% ")) (format t ")~%") @@ -603,11 +660,11 @@ (limbs (if (send self :force-sensors) (remove nil (mapcar #'(lambda (fs) - (find-if #'(lambda (l) (equal fs (car (send self l :force-sensors)))) '(:rleg :lleg))) + (find-if #'(lambda (l) (equal fs (car (send self :limb l :force-sensors)))) '(:rleg :lleg))) (send self :force-sensors))) '(:rleg :lleg))) (force-sensors (mapcar #'(lambda (l) (send self :force-sensor l)) limbs)) - (cop-coords (mapcar #'(lambda (l) (send self l :end-coords)) limbs)) + (cop-coords (mapcar #'(lambda (l) (send self :limb l :end-coords)) limbs)) (fz-thre 1e-3) ;; [N] (limb-cop-fz-list (mapcar #'(lambda (fs f m cc) @@ -651,15 +708,15 @@ (cond ((or (eq l/r :left) (eq l/r :lleg)) (setq support-coords - (send self :lleg :end-coords :copy-worldcoords))) + (send self :limb :lleg :end-coords :copy-worldcoords))) ((or (eq l/r :right) (eq l/r :rleg)) (setq support-coords - (send self :rleg :end-coords :copy-worldcoords))) + (send self :limb :rleg :end-coords :copy-worldcoords))) (t (setq support-coords (midcoords mid - (send self :lleg :end-coords :copy-worldcoords) - (send self :rleg :end-coords :copy-worldcoords))))) + (send self :limb :lleg :end-coords :copy-worldcoords) + (send self :limb :rleg :end-coords :copy-worldcoords))))) (setq tmp-coords (send fix-coords :copy-worldcoords)) (setq move-coords (send support-coords :transformation self)) (send tmp-coords :transform move-coords :local) @@ -678,11 +735,11 @@ #'midpoint mid (mapcar #'(lambda (tmp-leg) - (send self tmp-leg :end-coords :worldpos)) + (send self :limb tmp-leg :end-coords :worldpos)) (remove-if-not #'(lambda (x) (memq x '(:rleg :lleg))) fix-limbs))) - (send self leg :end-coords :worldpos))) + (send self :limb leg :end-coords :worldpos))) (fix-limbs-target-coords - (mapcar #'(lambda (x) (send self x :end-coords :copy-worldcoords)) fix-limbs)) + (mapcar #'(lambda (x) (send self :limb x :end-coords :copy-worldcoords)) fix-limbs)) (root-link-virtual-joint-weight #f(0.1 0.1 0.0 0.0 0.0 0.5)) ;; use only translation x, y and rotation z &allow-other-keys) "Move robot COG to change centroid-on-foot location, @@ -707,7 +764,7 @@ ((:all-limbs al) (if (send self :force-sensors) (remove nil (mapcar #'(lambda (fs) - (find-if #'(lambda (l) (equal fs (car (send self l :force-sensors)))) '(:rleg :lleg))) + (find-if #'(lambda (l) (equal fs (car (send self :limb l :force-sensors)))) '(:rleg :lleg))) (send self :force-sensors))) '(:rleg :lleg))) ((:default-zmp-offsets dzo) @@ -784,7 +841,7 @@ (send self :angle-vector) (send (car (send self :links)) :copy-worldcoords) :dt dt :pZMPz (elt (elt ret 5) 2)))) - (end-coords-list (mapcar #'(lambda (x) (send self x :end-coords :copy-worldcoords)) (gg . all-limbs))) + (end-coords-list (mapcar #'(lambda (x) (send self :limb x :end-coords :copy-worldcoords)) (gg . all-limbs))) (contact-state (elt ret 8)) ) (when debug-view @@ -855,13 +912,13 @@ (target-coords-func diff-func &optional (limit-func)) (send self :reset-pose) (send self :fix-leg-to-coords (make-coords) '(:rleg :lleg)) - (let* ((tc (send self :rleg :end-coords :copy-worldcoords)) + (let* ((tc (send self :limb :rleg :end-coords :copy-worldcoords)) (init-tc (send tc :copy-worldcoords))) (while (and (send self :inverse-kinematics (funcall target-coords-func tc) - :link-list (send self :link-list (send self :rleg :end-coords :parent)) - :move-target (send self :rleg :end-coords) :warnp nil) + :link-list (send self :link-list (send self :limb :rleg :end-coords :parent)) + :move-target (send self :limb :rleg :end-coords) :warnp nil) (if limit-func (funcall limit-func tc init-tc) t) )) (funcall diff-func tc init-tc)))) @@ -963,26 +1020,26 @@ ;; calculate foot step list for quad walk (dolist (fs biped-fsl) (let* ((fs-leg-name (send fs :name)) - (offset (send (send self fs-leg-name :end-coords :copy-worldcoords) :transformation fs :world)) + (offset (send (send self :limb fs-leg-name :end-coords :copy-worldcoords) :transformation fs :world)) (target-arm-name)) (case type (:crawl (dolist (l (list fs-leg-name (case fs-leg-name (:lleg :larm) (:rleg :rarm)))) - (push (list (make-coords :coords (send (send self l :end-coords :copy-worldcoords) :transform offset :world) :name l)) + (push (list (make-coords :coords (send (send self :limb l :end-coords :copy-worldcoords) :transform offset :world) :name l)) quadruped-fsl))) (:trot (push (mapcar #'(lambda (l) - (make-coords :coords (send (send self l :end-coords :copy-worldcoords) :transform offset :world) :name l)) + (make-coords :coords (send (send self :limb l :end-coords :copy-worldcoords) :transform offset :world) :name l)) (list fs-leg-name (setq target-arm-name (case fs-leg-name (:lleg :rarm) (:rleg :larm))))) quadruped-fsl)) (:pace (push (mapcar #'(lambda (l) - (make-coords :coords (send (send self l :end-coords :copy-worldcoords) :transform offset :world) :name l)) + (make-coords :coords (send (send self :limb l :end-coords :copy-worldcoords) :transform offset :world) :name l)) (list fs-leg-name (case fs-leg-name (:lleg :larm) (:rleg :rarm)))) quadruped-fsl)) (:gallop (push (mapcar #'(lambda (l) - (make-coords :coords (send (send self l :end-coords :copy-worldcoords) :transform offset :world) :name l)) + (make-coords :coords (send (send self :limb l :end-coords :copy-worldcoords) :transform offset :world) :name l)) (list (case fs-leg-name (:rleg :rleg) (:lleg :rarm)) (case fs-leg-name (:rleg :lleg) (:lleg :larm)))) quadruped-fsl)) @@ -1025,11 +1082,11 @@ (if (and l (send l :descendants)) (append (list l) (flatten (mapcar #'all-descendant-links (remove-if-not #'(lambda (x) (derivedp x bodyset-link)) (send l :descendants))))) ))) - (unless (send self name :links) (return-from :make-sole-polygon nil)) - (send-all (all-descendant-links (car (send self name :links))) :worldcoords) + (unless (send self :limb name :links) (return-from :make-sole-polygon nil)) + (send-all (all-descendant-links (car (send self :limb name :links))) :worldcoords) (let* ((target-nm (read-from-string (format nil "~A-sole-body" name))) (vs (remove-duplicates - (flatten (send-all (flatten (send-all (all-descendant-links (car (send self name :links))) :bodies)) :vertices)) + (flatten (send-all (flatten (send-all (all-descendant-links (car (send self :limb name :links))) :bodies)) :vertices)) :test #'(lambda (x y) (eps-v= x y *epsilon*)))) (min-vs (find-extream vs #'(lambda (x) (elt x 2)) #'<)) (b (send @@ -1038,7 +1095,7 @@ #'(lambda (p) (< 5.0 (- (elt p 2) (elt min-vs 2)))) vs)) ;; 5.0 is margin :body))) - (send (send self name :end-coords :parent) :assoc b) + (send (send self :limb name :end-coords :parent) :assoc b) (send self :put target-nm b) (send b :worldcoords) (let ((f (find-if #'(lambda (x) (memq :bottom (send x :id))) (send b :faces)))) @@ -1107,7 +1164,7 @@ (:calc-static-balance-point (&key (target-points (mapcar #'(lambda (tmp-arm) - (send (send self tmp-arm :end-coords) :worldpos)) + (send (send self :limb tmp-arm :end-coords) :worldpos)) '(:rarm :larm))) (force-list (make-list (length target-points) :initial-element (float-vector 0 0 0))) (moment-list (make-list (length target-points) :initial-element (float-vector 0 0 0))) From 14b514e9b7a99c165358eb83e9d915513ed6262a Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 9 May 2021 03:59:43 +0900 Subject: [PATCH 06/17] fix --- irteus/irtrobot.l | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/irteus/irtrobot.l b/irteus/irtrobot.l index 8f58f6aa3..1f3673f9f 100644 --- a/irteus/irtrobot.l +++ b/irteus/irtrobot.l @@ -112,8 +112,10 @@ (not (every #'null (send self :legs :end-coords)))) ;; for robot with legs end-coords (send self :make-support-polygons)) (setq end-coords-list (remove nil (append end-coords-list - (mapcar #'(lambda (l) (cdr (assoc :end-coords (cdr (assoc l limb-list))))) - '(:larm :rarm :lleg :rleg :head :torso))))) + (mapcar #'(lambda (l) (cdr (assoc :end-coords l))) limb-list) + (list larm-end-coords rarm-end-coords ;; obsoleted. for backward compatibility + lleg-end-coords rleg-end-coords + head-end-coords torso-end-coords)))) ) ) ;; End-coords accessor From da88ca1f86ad92238530f974fa6b58de7882ec64 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 9 May 2021 11:26:26 +0900 Subject: [PATCH 07/17] fix --- irteus/irtrobot.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/irteus/irtrobot.l b/irteus/irtrobot.l index 1f3673f9f..1984500c9 100644 --- a/irteus/irtrobot.l +++ b/irteus/irtrobot.l @@ -218,7 +218,7 @@ (if (memq :collision-avoidance-link-pair args) (cadr (memq :collision-avoidance-link-pair args)) (send self :collision-avoidance-link-pair-from-link-list link-list - :collision-avoidance-links (send self :linb limb :collision-avoidance-links))))) + :collision-avoidance-links (send self :limb limb :collision-avoidance-links))))) (send* self :inverse-kinematics (car args) :move-target (if (memq :move-target args) (cadr (memq :move-target args)) From dd7d83fc93c194009a65d73663d8f680876e218a Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 9 May 2021 12:03:33 +0900 Subject: [PATCH 08/17] [sample-robot-model.l] use :add-limb --- irteus/demo/sample-robot-model.l | 130 +++++++++++++++++++------------ 1 file changed, 82 insertions(+), 48 deletions(-) diff --git a/irteus/demo/sample-robot-model.l b/irteus/demo/sample-robot-model.l index 9d212a186..c64d04306 100644 --- a/irteus/demo/sample-robot-model.l +++ b/irteus/demo/sample-robot-model.l @@ -66,22 +66,40 @@ (upper-leg-weight 2152.7) (lower-leg-weight 898.7) (foot-weight 313.5) ) (send-super* :init :name name args) - ;; 1. make links links and assoc all links - (let ((aroot-link (send self :make-root-link waist-length waist-weight))) - (setq torso (send self :make-torso-links torso-length torso-weight) - head (send self :make-head-links head-weight) - rarm (send self :make-arm-links :rarm - arm-radius upper-arm-length lower-arm-length shoulder-width hand-length - upper-arm-weight lower-arm-weight hand-weight) - larm (send self :make-arm-links :larm - arm-radius upper-arm-length lower-arm-length shoulder-width hand-length - upper-arm-weight lower-arm-weight hand-weight) - rleg (send self :make-leg-links :rleg - leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset - upper-leg-weight lower-leg-weight foot-weight) - lleg (send self :make-leg-links :lleg - leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset - upper-leg-weight lower-leg-weight foot-weight)) + ;; 1. make links and assoc all links + (let* ((aroot-link (send self :make-root-link waist-length waist-weight)) + (torso-info (send self :make-torso-links torso-length torso-weight)) + (torso (cdr (assoc :links torso-info))) + (torso-end-coords (cdr (assoc :end-coords torso-info))) + (torso-root-link (cdr (assoc :root-link torso-info))) + (head-info (send self :make-head-links head-weight)) + (head (cdr (assoc :links head-info))) + (head-end-coords (cdr (assoc :end-coords head-info))) + (head-root-link (cdr (assoc :root-link head-info))) + (rarm-info (send self :make-arm-links :rarm + arm-radius upper-arm-length lower-arm-length shoulder-width hand-length + upper-arm-weight lower-arm-weight hand-weight)) + (rarm (cdr (assoc :links rarm-info))) + (rarm-end-coords (cdr (assoc :end-coords rarm-info))) + (rarm-root-link (cdr (assoc :root-link rarm-info))) + (larm-info (send self :make-arm-links :larm + arm-radius upper-arm-length lower-arm-length shoulder-width hand-length + upper-arm-weight lower-arm-weight hand-weight)) + (larm (cdr (assoc :links larm-info))) + (larm-end-coords (cdr (assoc :end-coords larm-info))) + (larm-root-link (cdr (assoc :root-link larm-info))) + (rleg-info (send self :make-leg-links :rleg + leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset + upper-leg-weight lower-leg-weight foot-weight)) + (rleg (cdr (assoc :links rleg-info))) + (rleg-end-coords (cdr (assoc :end-coords rleg-info))) + (rleg-root-link (cdr (assoc :root-link rleg-info))) + (lleg-info (send self :make-leg-links :lleg + leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset + upper-leg-weight lower-leg-weight foot-weight)) + (lleg (cdr (assoc :links lleg-info))) + (lleg-end-coords (cdr (assoc :end-coords lleg-info))) + (lleg-root-link (cdr (assoc :root-link lleg-info)))) ;; arrange limbs (send (car rarm) :translate (float-vector 0 (- shoulder-width) (- torso-length 25)) :world) (send (car larm) :translate (float-vector 0 shoulder-width (- torso-length 25)) :world) @@ -145,10 +163,16 @@ jll0 jll1 jll2 jll3 jll4 jll5 jlr0 jlr1 jlr2 jlr3 jlr4 jlr5 )) + + ;; add limbs + (send self :add-limb :torso :links torso :end-coords torso-end-coords :root-link torso-root-link) + (send self :add-limb :head :links head :end-coords head-end-coords :root-link head-root-link) + (send self :add-limb :rarm :links rarm :end-coords rarm-end-coords :root-link rarm-root-link) + (send self :add-limb :larm :links larm :end-coords larm-end-coords :root-link larm-root-link) + (send self :add-limb :rleg :links rleg :end-coords rleg-end-coords :root-link rleg-root-link) + (send self :add-limb :lleg :links lleg :end-coords lleg-end-coords :root-link lleg-root-link) + ;; These are for robot-model. - (setq larm-root-link (car larm) rarm-root-link (car rarm) - lleg-root-link (car lleg) rleg-root-link (car rleg) - torso-root-link (car torso) head-root-link (car head)) (setq collision-avoidance-links (list aroot-link (elt torso 1) (elt larm 3) (elt rarm 3))) ;; set max torques @@ -166,23 +190,23 @@ ;; set sensors (setq force-sensors (mapcar #'(lambda (l) - (send (send self l :end-coords :parent) :assoc - (make-cascoords :coords (send self l :end-coords :copy-worldcoords) + (send (send self :limb l :end-coords :parent) :assoc + (make-cascoords :coords (send self :limb l :end-coords :copy-worldcoords) :name (format nil "~A-fsensor" (string-downcase l))))) (list :rarm :larm :rleg :lleg))) (setq imu-sensors (list (let ((sen (make-cascoords - :worldpos (send (car (send self :torso :waist-p :child-link :bodies)) :centroid) + :worldpos (send (car (send self :limb :torso :waist-p :child-link :bodies)) :centroid) :name "torso-imusensor"))) - (send (send self :torso :waist-p :child-link) :assoc sen) + (send (send self :limb :torso :waist-p :child-link) :assoc sen) sen))) (setq cameras (mapcar #'(lambda (pos name) (let ((sen (instance camera-model :init (make-cylinder 10 20) :name name))) (send sen :rotate pi/2 :y) (send sen :rotate -pi/2 :z) - (send sen :locate (v+ pos (send (car (send (send self :head :neck-p :child-link) :bodies)) :centroid)) :world) - (send (send self :head :neck-p :child-link) :assoc sen) + (send sen :locate (v+ pos (send (car (send (send self :limb :head :neck-p :child-link) :bodies)) :centroid)) :world) + (send (send self :limb :head :neck-p :child-link) :assoc sen) sen)) (list (float-vector 0 30 0) (float-vector 0 -30 0)) (list "left-camera" "right-camera"))) @@ -205,13 +229,15 @@ (send bc2 :set-color :green) (setq bc2 (instance bodyset-link :init (make-cascoords :pos #f(0 0 -12.5)) :bodies (list bc2) :name :torso-link1 :weight torso-weight)) (send bc1 :assoc bc2) - (list bc1 bc2))) + (list (cons :links (list bc1 bc2)) + (cons :root-link bc1)))) (:make-head-links (head-weight) (let ((bh0 (make-default-robot-link 0 50 :y :head-link0)) (bh2 (make-cube 120 100 150)) (bh2e (make-cylinder 10 30)) - (bh1)) + (bh1) + (end-coords)) (send bh2 :locate #f(0 0 80)) (send bh2 :set-color :green) (send bh2e :rotate pi/2 :y) @@ -219,10 +245,12 @@ (send bh2e :set-color :green) (send bh2 :assoc bh2e) (setq bh1 (instance bodyset-link :init (make-cascoords) :bodies (list bh2 bh2e) :name :head-link1 :weight head-weight)) - (setq head-end-coords (make-cascoords :pos #f(60 0 100) :rpy (float-vector 0 pi/2 0))) - (send bh1 :assoc head-end-coords) + (setq end-coords (make-cascoords :pos #f(60 0 100) :rpy (float-vector 0 pi/2 0))) + (send bh1 :assoc end-coords) (send bh0 :assoc bh1) - (list bh0 bh1))) + (list (cons :links (list bh0 bh1)) + (cons :root-link bh0) + (cons :end-coords end-coords)))) (:make-arm-links (l/r arm-radius upper-arm-length lower-arm-length shoulder-width hand-length upper-arm-weight lower-arm-weight hand-weight) @@ -232,21 +260,22 @@ (ba4 (make-default-robot-link lower-arm-length arm-radius :y (read-from-string (format nil "~A-link3" l/r)))) (ba5 (make-default-robot-link 0 arm-radius :z (read-from-string (format nil "~A-link4" l/r)))) (ba6 (make-default-robot-link 0 arm-radius :x (read-from-string (format nil "~A-link5" l/r)))) - (ba7 (make-default-robot-link hand-length arm-radius :y (read-from-string (format nil "~A-link6" l/r))))) + (ba7 (make-default-robot-link hand-length arm-radius :y (read-from-string (format nil "~A-link6" l/r)))) + (end-coords)) (send ba3 :weight upper-arm-weight) (send ba4 :weight lower-arm-weight) (send ba7 :weight hand-weight) (case l/r (:rarm - (setq rarm-end-coords (make-cascoords)) - (send rarm-end-coords :locate (float-vector 0 0 (- hand-length))) - (send rarm-end-coords :rotate pi/2 :y) - (send ba7 :assoc rarm-end-coords)) + (setq end-coords (make-cascoords)) + (send end-coords :locate (float-vector 0 0 (- hand-length))) + (send end-coords :rotate pi/2 :y) + (send ba7 :assoc end-coords)) (:larm - (setq larm-end-coords (make-cascoords)) - (send larm-end-coords :locate (float-vector 0 0 (- hand-length))) - (send larm-end-coords :rotate pi/2 :y) - (send ba7 :assoc larm-end-coords))) + (setq end-coords (make-cascoords)) + (send end-coords :locate (float-vector 0 0 (- hand-length))) + (send end-coords :rotate pi/2 :y) + (send ba7 :assoc end-coords))) (send ba6 :assoc ba7) (send ba5 :assoc ba6) (send ba5 :translate (float-vector 0 0 (- lower-arm-length)) :world) @@ -255,7 +284,9 @@ (send ba3 :assoc ba4) (send ba2 :assoc ba3) (send ba1 :assoc ba2) - (list ba1 ba2 ba3 ba4 ba5 ba6 ba7))) + (list (cons :links (list ba1 ba2 ba3 ba4 ba5 ba6 ba7)) + (cons :root-link ba1) + (cons :end-coords end-coords)))) (:make-leg-links (l/r leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset upper-leg-weight lower-leg-weight foot-weight) @@ -265,7 +296,8 @@ (bl4 (make-default-robot-link (- lower-leg-length (/ leg-radius 2.0)) leg-radius :y (read-from-string (format nil "~A-link3" l/r)))) (bl5 (make-default-robot-link 0 leg-radius :x (read-from-string (format nil "~A-link4" l/r)))) (bl6b (make-cube foot-depth foot-width foot-thickness)) - (bl6)) + (bl6) + (end-coords)) (send bl3 :weight upper-leg-weight) (send bl4 :weight lower-leg-weight) (send bl6b :locate (float-vector foot-offset 0 (- ankle-length))) @@ -274,13 +306,13 @@ (send bl6 :weight foot-weight) (case l/r (:rleg - (setq rleg-end-coords (make-cascoords)) - (send rleg-end-coords :locate (float-vector 0 0 (- (+ ankle-length (/ foot-thickness 2.0))))) - (send bl6 :assoc rleg-end-coords)) + (setq end-coords (make-cascoords)) + (send end-coords :locate (float-vector 0 0 (- (+ ankle-length (/ foot-thickness 2.0))))) + (send bl6 :assoc end-coords)) (:lleg - (setq lleg-end-coords (make-cascoords)) - (send lleg-end-coords :locate (float-vector 0 0 (- (+ ankle-length (/ foot-thickness 2.0))))) - (send bl6 :assoc lleg-end-coords))) + (setq end-coords (make-cascoords)) + (send end-coords :locate (float-vector 0 0 (- (+ ankle-length (/ foot-thickness 2.0))))) + (send bl6 :assoc end-coords))) (send bl5 :assoc bl6) (send bl5 :translate (float-vector 0 0 (- lower-leg-length)) :world) (send bl4 :assoc bl5) @@ -288,7 +320,9 @@ (send bl3 :assoc bl4) (send bl2 :assoc bl3) (send bl1 :assoc bl2) - (list bl1 bl2 bl3 bl4 bl5 bl6))) + (list (cons :links (list bl1 bl2 bl3 bl4 bl5 bl6)) + (cons :root-link bl1) + (cons :end-coords end-coords)))) (:reset-pose () (send self :angle-vector #f(0.0 0.0 0.0 0.0 10.0 20.0 0.0 -20.0 10.0 0.0 0.0 10.0 20.0 0.0 -20.0 10.0 0.0 0.0 0.0 0.0 -15.0 30.0 -15.0 0.0 0.0 0.0 -15.0 30.0 -15.0 0.0))) ) From d4beae2e82bcd0576410b93e1a79b411d4f53700 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 9 May 2021 13:10:11 +0900 Subject: [PATCH 09/17] [sample-centaur-robot.l] --- irteus/demo/sample-centaur-robot.l | 410 +++++++++++++++++++++++++++++ irteus/irtmodel.l | 2 +- irteus/irtrobot.l | 27 +- 3 files changed, 428 insertions(+), 11 deletions(-) create mode 100644 irteus/demo/sample-centaur-robot.l diff --git a/irteus/demo/sample-centaur-robot.l b/irteus/demo/sample-centaur-robot.l new file mode 100644 index 000000000..78dc6b677 --- /dev/null +++ b/irteus/demo/sample-centaur-robot.l @@ -0,0 +1,410 @@ +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;;; +;;; Copyright (c) 1987- JSK, The University of Tokyo. All Rights Reserved. +;;; +;;; This software is a collection of EusLisp code for robot applications, +;;; which has been developed by the JSK Laboratory for the IRT project. +;;; For more information on EusLisp and its application to the robotics, +;;; please refer to the following papers. +;;; +;;; Toshihiro Matsui +;;; Multithread object-oriented language euslisp for parallel and +;;; asynchronous programming in robotics +;;; Workshop on Concurrent Object-based Systems, +;;; IEEE 6th Symposium on Parallel and Distributed Processing, 1994 +;;; +;;; Redistribution and use in source and binary forms, with or without +;;; modification, are permitted provided that the following conditions are met: +;;; +;;; * Redistributions of source code must retain the above copyright notice, +;;; this list of conditions and the following disclaimer. +;;; * Redistributions in binary form must reproduce the above copyright notice, +;;; this list of conditions and the following disclaimer in the documentation +;;; and/or other materials provided with the distribution. +;;; * Neither the name of JSK Robotics Laboratory, The University of Tokyo +;;; (JSK) nor the names of its contributors may be used to endorse or promote +;;; products derived from this software without specific prior written +;;; permission. +;;; +;;; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +;;; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +;;; THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +;;; PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR +;;; CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +;;; EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +;;; PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; +;;; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +;;; WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR +;;; OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +;;; ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +;;; + +(defclass sample-centaur-robot + :super robot-model + :slots (jc0 jc1 jh0 jh1 + jal0 jal1 jal2 jal3 jal4 jal5 jal6 + jar0 jar1 jar2 jar3 jar4 jar5 jar6 + jllf0 jllf1 jllf2 jllf3 jllf4 jllf5 + jlrf0 jlrf1 jlrf2 jlrf3 jlrf4 jlrf5 + jllb0 jllb1 jllb2 jllb3 jllb4 jllb5 + jlrb0 jlrb1 jlrb2 jlrb3 jlrb4 jlrb5)) +(defmethod sample-centaur-robot + (:init + (&rest args + &key (name "sample-centaur-robot") + ;; key word arguments for configure model size + (leg-radius 50) (upper-leg-length 250) (lower-leg-length 250) (ankle-length 50) + (crotch-width 75) (foot-depth 200) (foot-width 100) (foot-thickness 25) (foot-offset 50) + (arm-radius 50) (upper-arm-length 275) (lower-arm-length 195) (shoulder-width 150) (hand-length 50) + (waist-height 100) (waist-length 500) (torso-height 200) (torso-length 100) + ;; mass distribution + ;; refering to "WEIGHT, VOLUME, AND CENTER OF MASS OF SEGMENTS OF THE HUMAN BODY", C.E.Clauser et al., AMRL technical report TR-69-70(1969), http://www.dtic.mil/dtic/tr/fulltext/u2/710622.pdf + ;; ratio is '((:head 7.3) (:trunk 50.7) (:upper-arm 2.6) (:forearm 1.6) (:hand 0.7) (:tight 10.3) (:calf 4.3) (:foot 1.5)) + ;; default total mass is 20.9[kg], which is calculated from total-height and avarage 6-years-old-Japanese Male weight http://www.mhlw.go.jp/toukei/youran/data25k/2-06.xls + (head-weight 1525.7) + (torso-weight (/ (* torso-height 10596.3) (+ torso-height waist-height))) + (waist-weight (/ (* waist-length (/ (* waist-height 10596.3) (+ torso-height waist-height))) torso-length)) + (upper-arm-weight 543.4) (lower-arm-weight 334.4) (hand-weight 146.3) + (upper-leg-weight 2152.7) (lower-leg-weight 898.7) (foot-weight 313.5) + ) + (send-super* :init :name name args) + ;; 1. make links and assoc all links + (let* ((aroot-link (send self :make-root-link waist-height waist-length waist-weight)) + (torso-info (send self :make-torso-links torso-height torso-length torso-weight)) + (torso (cdr (assoc :links torso-info))) + (torso-end-coords (cdr (assoc :end-coords torso-info))) + (torso-root-link (cdr (assoc :root-link torso-info))) + (head-info (send self :make-head-links head-weight)) + (head (cdr (assoc :links head-info))) + (head-end-coords (cdr (assoc :end-coords head-info))) + (head-root-link (cdr (assoc :root-link head-info))) + (rarm-info (send self :make-arm-links :rarm + arm-radius upper-arm-length lower-arm-length shoulder-width hand-length + upper-arm-weight lower-arm-weight hand-weight)) + (rarm (cdr (assoc :links rarm-info))) + (rarm-end-coords (cdr (assoc :end-coords rarm-info))) + (rarm-root-link (cdr (assoc :root-link rarm-info))) + (larm-info (send self :make-arm-links :larm + arm-radius upper-arm-length lower-arm-length shoulder-width hand-length + upper-arm-weight lower-arm-weight hand-weight)) + (larm (cdr (assoc :links larm-info))) + (larm-end-coords (cdr (assoc :end-coords larm-info))) + (larm-root-link (cdr (assoc :root-link larm-info))) + (rfleg-info (send self :make-leg-links :rleg + leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset + upper-leg-weight lower-leg-weight foot-weight)) + (rfleg (cdr (assoc :links rfleg-info))) + (rfleg-end-coords (cdr (assoc :end-coords rfleg-info))) + (rfleg-root-link (cdr (assoc :root-link rfleg-info))) + (lfleg-info (send self :make-leg-links :lleg + leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset + upper-leg-weight lower-leg-weight foot-weight)) + (lfleg (cdr (assoc :links lfleg-info))) + (lfleg-end-coords (cdr (assoc :end-coords lfleg-info))) + (lfleg-root-link (cdr (assoc :root-link lfleg-info))) + (rbleg-info (send self :make-leg-links :rleg + leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset + upper-leg-weight lower-leg-weight foot-weight)) + (rbleg (cdr (assoc :links rbleg-info))) + (rbleg-end-coords (cdr (assoc :end-coords rbleg-info))) + (rbleg-root-link (cdr (assoc :root-link rbleg-info))) + (lbleg-info (send self :make-leg-links :lleg + leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset + upper-leg-weight lower-leg-weight foot-weight)) + (lbleg (cdr (assoc :links lbleg-info))) + (lbleg-end-coords (cdr (assoc :end-coords lbleg-info))) + (lbleg-root-link (cdr (assoc :root-link lbleg-info)))) + ;; arrange limbs + (send aroot-link :translate (float-vector (/ (- torso-length waist-length) 2) 0 0) :world) + (send (car rarm) :translate (float-vector 0 (- shoulder-width) (- torso-height 25)) :world) + (send (car larm) :translate (float-vector 0 shoulder-width (- torso-height 25)) :world) + (send (car rfleg) :translate (float-vector 0 (- crotch-width) (+ (- waist-height) -50)) :world) + (send (car lfleg) :translate (float-vector 0 crotch-width (+ (- waist-height) -50)) :world) + (send (car rbleg) :translate (float-vector (- torso-length waist-length) (- crotch-width) (+ (- waist-height) -50)) :world) + (send (car lbleg) :translate (float-vector (- torso-length waist-length) crotch-width (+ (- waist-height) -50)) :world) + (send (car head) :locate (float-vector 0 0 (+ 25 torso-height))) + + ;; 2. assoc links + ;; Root link should be associated with "self". + (send self :assoc aroot-link) + (send aroot-link :assoc (car torso)) + (send (cadr torso) :assoc (car head)) + (send (cadr torso) :assoc (car rarm)) + (send (cadr torso) :assoc (car larm)) + (send aroot-link :assoc (car rfleg)) + (send aroot-link :assoc (car lfleg)) + (send aroot-link :assoc (car rbleg)) + (send aroot-link :assoc (car lbleg)) + + ;; 3. make all joints + ;; Before making joints, you should :assoc all links. + (setq jc0 (instance rotational-joint :init :parent-link aroot-link :child-link (car torso) :name :torso-waist-y :axis :z :min -45 :max 45)) + (setq jc1 (instance rotational-joint :init :parent-link (car torso) :child-link (cadr torso) :name :torso-waist-p :axis :y)) + + (setq jh0 (instance rotational-joint :init :parent-link (cadr torso) :child-link (car head) :name :head-neck-y :axis :z :min -150 :max 150)) + (setq jh1 (instance rotational-joint :init :parent-link (car head) :child-link (cadr head) :name :head-neck-p :axis :y)) + (setq jal0 (instance rotational-joint :init :parent-link (cadr torso) :child-link (elt larm 0) :name :larm-shoulder-p :axis :y :min -150 :max 150)) + (setq jal1 (instance rotational-joint :init :parent-link (elt larm 0) :child-link (elt larm 1) :name :larm-shoulder-r :axis :x :min -30 :max 180)) + (setq jal2 (instance rotational-joint :init :parent-link (elt larm 1) :child-link (elt larm 2) :name :larm-shoulder-y :axis :z)) + (setq jal3 (instance rotational-joint :init :parent-link (elt larm 2) :child-link (elt larm 3) :name :larm-elbow-p :axis :y :min -180 :max 0)) + (setq jal4 (instance rotational-joint :init :parent-link (elt larm 3) :child-link (elt larm 4) :name :larm-wrist-y :axis :z)) + (setq jal5 (instance rotational-joint :init :parent-link (elt larm 4) :child-link (elt larm 5) :name :larm-wrist-r :axis :x)) + (setq jal6 (instance rotational-joint :init :parent-link (elt larm 5) :child-link (elt larm 6) :name :larm-wrist-p :axis :y)) + + (setq jar0 (instance rotational-joint :init :parent-link (cadr torso) :child-link (elt rarm 0) :name :rarm-shoulder-p :axis :y :min -150 :max 150)) + (setq jar1 (instance rotational-joint :init :parent-link (elt rarm 0) :child-link (elt rarm 1) :name :rarm-shoulder-r :axis :-x :min -30 :max 180)) + (setq jar2 (instance rotational-joint :init :parent-link (elt rarm 1) :child-link (elt rarm 2) :name :rarm-shoulder-y :axis :-z)) + (setq jar3 (instance rotational-joint :init :parent-link (elt rarm 2) :child-link (elt rarm 3) :name :rarm-elbow-p :axis :y :min -180 :max 0)) + (setq jar4 (instance rotational-joint :init :parent-link (elt rarm 3) :child-link (elt rarm 4) :name :rarm-wrist-y :axis :-z)) + (setq jar5 (instance rotational-joint :init :parent-link (elt rarm 4) :child-link (elt rarm 5) :name :rarm-wrist-r :axis :-x)) + (setq jar6 (instance rotational-joint :init :parent-link (elt rarm 5) :child-link (elt rarm 6) :name :rarm-wrist-p :axis :y)) + + (setq jllf0 (instance rotational-joint :init :parent-link aroot-link :child-link (elt lfleg 0) :name :lfleg-crotch-y :axis :z)) + (setq jllf1 (instance rotational-joint :init :parent-link (elt lfleg 0) :child-link (elt lfleg 1) :name :lfleg-crotch-r :axis :x)) + (setq jllf2 (instance rotational-joint :init :parent-link (elt lfleg 1) :child-link (elt lfleg 2) :name :lfleg-crotch-p :axis :y :min -120 :max 120)) + (setq jllf3 (instance rotational-joint :init :parent-link (elt lfleg 2) :child-link (elt lfleg 3) :name :lfleg-knee-p :axis :y :min 0)) + (setq jllf4 (instance rotational-joint :init :parent-link (elt lfleg 3) :child-link (elt lfleg 4) :name :lfleg-ankle-p :axis :y)) + (setq jllf5 (instance rotational-joint :init :parent-link (elt lfleg 4) :child-link (elt lfleg 5) :name :lfleg-ankle-r :axis :x)) + + (setq jlrf0 (instance rotational-joint :init :parent-link aroot-link :child-link (elt rfleg 0) :name :rfleg-crotch-y :axis :-z)) + (setq jlrf1 (instance rotational-joint :init :parent-link (elt rfleg 0) :child-link (elt rfleg 1) :name :rfleg-crotch-r :axis :-x)) + (setq jlrf2 (instance rotational-joint :init :parent-link (elt rfleg 1) :child-link (elt rfleg 2) :name :rfleg-crotch-p :axis :y :min -120 :max 120)) + (setq jlrf3 (instance rotational-joint :init :parent-link (elt rfleg 2) :child-link (elt rfleg 3) :name :rfleg-knee-p :axis :y :min 0)) + (setq jlrf4 (instance rotational-joint :init :parent-link (elt rfleg 3) :child-link (elt rfleg 4) :name :rfleg-ankle-p :axis :y)) + (setq jlrf5 (instance rotational-joint :init :parent-link (elt rfleg 4) :child-link (elt rfleg 5) :name :rfleg-ankle-r :axis :-x)) + + (setq jllb0 (instance rotational-joint :init :parent-link aroot-link :child-link (elt lbleg 0) :name :lbleg-crotch-y :axis :z)) + (setq jllb1 (instance rotational-joint :init :parent-link (elt lbleg 0) :child-link (elt lbleg 1) :name :lbleg-crotch-r :axis :x)) + (setq jllb2 (instance rotational-joint :init :parent-link (elt lbleg 1) :child-link (elt lbleg 2) :name :lbleg-crotch-p :axis :y :min -120 :max 120)) + (setq jllb3 (instance rotational-joint :init :parent-link (elt lbleg 2) :child-link (elt lbleg 3) :name :lbleg-knee-p :axis :y :min 0)) + (setq jllb4 (instance rotational-joint :init :parent-link (elt lbleg 3) :child-link (elt lbleg 4) :name :lbleg-ankle-p :axis :y)) + (setq jllb5 (instance rotational-joint :init :parent-link (elt lbleg 4) :child-link (elt lbleg 5) :name :lbleg-ankle-r :axis :x)) + + (setq jlrb0 (instance rotational-joint :init :parent-link aroot-link :child-link (elt rbleg 0) :name :rbleg-crotch-y :axis :-z)) + (setq jlrb1 (instance rotational-joint :init :parent-link (elt rbleg 0) :child-link (elt rbleg 1) :name :rbleg-crotch-r :axis :-x)) + (setq jlrb2 (instance rotational-joint :init :parent-link (elt rbleg 1) :child-link (elt rbleg 2) :name :rbleg-crotch-p :axis :y :min -120 :max 120)) + (setq jlrb3 (instance rotational-joint :init :parent-link (elt rbleg 2) :child-link (elt rbleg 3) :name :rbleg-knee-p :axis :y :min 0)) + (setq jlrb4 (instance rotational-joint :init :parent-link (elt rbleg 3) :child-link (elt rbleg 4) :name :rbleg-ankle-p :axis :y)) + (setq jlrb5 (instance rotational-joint :init :parent-link (elt rbleg 4) :child-link (elt rbleg 5) :name :rbleg-ankle-r :axis :-x)) + + ;; 4. define slots for robot class + ;; links and joint-list for cascaded-link. + (setq links (append (list aroot-link) torso head larm rarm lfleg rfleg lbleg rbleg)) + (setq joint-list (list jc0 jc1 jh0 jh1 + jal0 jal1 jal2 jal3 jal4 jal5 jal6 + jar0 jar1 jar2 jar3 jar4 jar5 jar6 + jllf0 jllf1 jllf2 jllf3 jllf4 jllf5 + jlrf0 jlrf1 jlrf2 jlrf3 jlrf4 jlrf5 + jllb0 jllb1 jllb2 jllb3 jllb4 jllb5 + jlrb0 jlrb1 jlrb2 jlrb3 jlrb4 jlrb5)) + + ;; add limbs + (send self :add-limb :torso :links torso :end-coords torso-end-coords :root-link torso-root-link) + (send self :add-limb :head :links head :end-coords head-end-coords :root-link head-root-link) + (send self :add-limb :rarm :links rarm :end-coords rarm-end-coords :root-link rarm-root-link) + (send self :add-limb :larm :links larm :end-coords larm-end-coords :root-link larm-root-link) + (send self :add-limb :rfleg :links rfleg :end-coords rfleg-end-coords :root-link rfleg-root-link) + (send self :add-limb :lfleg :links lfleg :end-coords lfleg-end-coords :root-link lfleg-root-link) + (send self :add-limb :rbleg :links rbleg :end-coords rbleg-end-coords :root-link rbleg-root-link) + (send self :add-limb :lbleg :links lbleg :end-coords lbleg-end-coords :root-link lbleg-root-link) + + ;; These are for robot-model. + (setq collision-avoidance-links (list aroot-link (elt torso 1) (elt larm 3) (elt rarm 3))) + + ;; set max torques + (dolist (l (list aroot-link (elt torso 1) (elt head 1))) + (let* ((valid-bodies (remove-if #'(lambda (x) + (and (> (send x :volume) 0) (< (send x :volume) 0))) ;; nan check + (send l :bodies)))) + (send l :centroid + (if (= (length valid-bodies) 1) + (send (car valid-bodies) :centroid) + (scale (/ 1.0 (reduce #'+ (mapcar #'(lambda (x) (send x :volume)) valid-bodies))) + (reduce #'v+ (mapcar #'(lambda (x) (scale (send x :volume) (send x :centroid))) valid-bodies))))) + )) + (send-all (send self :joint-list) :max-joint-torque 7.5) ;; [Nm] + ;; set sensors + (setq force-sensors + (mapcar #'(lambda (l) + (send (send self :limb l :end-coords :parent) :assoc + (make-cascoords :coords (send self :limb l :end-coords :copy-worldcoords) + :name (format nil "~A-fsensor" (string-downcase l))))) + (list :rarm :larm :rfleg :lfleg :rbleg :lbleg))) + (setq imu-sensors + (list (let ((sen (make-cascoords + :worldpos (send (car (send self :limb :torso :waist-p :child-link :bodies)) :centroid) + :name "torso-imusensor"))) + (send (send self :limb :torso :waist-p :child-link) :assoc sen) + sen))) + (setq cameras + (mapcar #'(lambda (pos name) + (let ((sen (instance camera-model :init (make-cylinder 10 20) :name name))) + (send sen :rotate pi/2 :y) + (send sen :rotate -pi/2 :z) + (send sen :locate (v+ pos (send (car (send (send self :limb :head :neck-p :child-link) :bodies)) :centroid)) :world) + (send (send self :limb :head :neck-p :child-link) :assoc sen) + sen)) + (list (float-vector 0 30 0) (float-vector 0 -30 0)) + (list "left-camera" "right-camera"))) + ;; 5. call :init-ending after defining links and joint-list and return "self" + (send self :init-ending) + self)) + ;; Methods to define robot links + (:make-root-link + (waist-height waist-length waist-weight) + (let ((bc0 (make-cube waist-length 200 waist-height))) + (send bc0 :locate (float-vector 0 0 (+ (* -0.5 waist-height) -25))) + (send bc0 :set-color :green) + (instance bodyset-link :init (make-cascoords) :bodies (list bc0) :name :waist :weight waist-weight))) + (:make-torso-links + (torso-height torso-length torso-weight) + (let ((bc1 (make-default-robot-link 0 50 :y :torso-link0)) + (bc2 (make-cube torso-length 200 torso-height))) + (send bc1 :locate #f(0 0 -12.5)) + (send bc2 :locate (float-vector 0 0 (* 0.5 torso-height))) + (send bc2 :set-color :green) + (setq bc2 (instance bodyset-link :init (make-cascoords :pos #f(0 0 -12.5)) :bodies (list bc2) :name :torso-link1 :weight torso-weight)) + (send bc1 :assoc bc2) + (list (cons :links (list bc1 bc2)) + (cons :root-link bc1)))) + (:make-head-links + (head-weight) + (let ((bh0 (make-default-robot-link 0 50 :y :head-link0)) + (bh2 (make-cube 120 100 150)) + (bh2e (make-cylinder 10 30)) + (bh1) + (end-coords)) + (send bh2 :locate #f(0 0 80)) + (send bh2 :set-color :green) + (send bh2e :rotate pi/2 :y) + (send bh2e :locate #f(60 0 70) :world) + (send bh2e :set-color :green) + (send bh2 :assoc bh2e) + (setq bh1 (instance bodyset-link :init (make-cascoords) :bodies (list bh2 bh2e) :name :head-link1 :weight head-weight)) + (setq end-coords (make-cascoords :pos #f(60 0 100) :rpy (float-vector 0 pi/2 0))) + (send bh1 :assoc end-coords) + (send bh0 :assoc bh1) + (list (cons :links (list bh0 bh1)) + (cons :root-link bh0) + (cons :end-coords end-coords)))) + (:make-arm-links + (l/r arm-radius upper-arm-length lower-arm-length shoulder-width hand-length + upper-arm-weight lower-arm-weight hand-weight) + (let ((ba1 (make-default-robot-link 0 arm-radius :y (read-from-string (format nil "~A-link0" l/r)))) + (ba2 (make-default-robot-link 0 arm-radius :x (read-from-string (format nil "~A-link1" l/r)))) + (ba3 (make-default-robot-link upper-arm-length arm-radius :z (read-from-string (format nil "~A-link2" l/r)))) + (ba4 (make-default-robot-link lower-arm-length arm-radius :y (read-from-string (format nil "~A-link3" l/r)))) + (ba5 (make-default-robot-link 0 arm-radius :z (read-from-string (format nil "~A-link4" l/r)))) + (ba6 (make-default-robot-link 0 arm-radius :x (read-from-string (format nil "~A-link5" l/r)))) + (ba7 (make-default-robot-link hand-length arm-radius :y (read-from-string (format nil "~A-link6" l/r)))) + (end-coords)) + (send ba3 :weight upper-arm-weight) + (send ba4 :weight lower-arm-weight) + (send ba7 :weight hand-weight) + (case l/r + (:rarm + (setq end-coords (make-cascoords)) + (send end-coords :locate (float-vector 0 0 (- hand-length))) + (send end-coords :rotate pi/2 :y) + (send ba7 :assoc end-coords)) + (:larm + (setq end-coords (make-cascoords)) + (send end-coords :locate (float-vector 0 0 (- hand-length))) + (send end-coords :rotate pi/2 :y) + (send ba7 :assoc end-coords))) + (send ba6 :assoc ba7) + (send ba5 :assoc ba6) + (send ba5 :translate (float-vector 0 0 (- lower-arm-length)) :world) + (send ba4 :assoc ba5) + (send ba4 :translate (float-vector 0 0 (- upper-arm-length)) :world) + (send ba3 :assoc ba4) + (send ba2 :assoc ba3) + (send ba1 :assoc ba2) + (list (cons :links (list ba1 ba2 ba3 ba4 ba5 ba6 ba7)) + (cons :root-link ba1) + (cons :end-coords end-coords)))) + (:make-leg-links + (l/r leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset + upper-leg-weight lower-leg-weight foot-weight) + (let* ((bl1 (make-default-robot-link 0 leg-radius :y (read-from-string (format nil "~A-link0" l/r)))) + (bl2 (make-default-robot-link 0 leg-radius :x (read-from-string (format nil "~A-link1" l/r)))) + (bl3 (make-default-robot-link (- upper-leg-length (/ leg-radius 2.0)) leg-radius :z (read-from-string (format nil "~A-link2" l/r)))) + (bl4 (make-default-robot-link (- lower-leg-length (/ leg-radius 2.0)) leg-radius :y (read-from-string (format nil "~A-link3" l/r)))) + (bl5 (make-default-robot-link 0 leg-radius :x (read-from-string (format nil "~A-link4" l/r)))) + (bl6b (make-cube foot-depth foot-width foot-thickness)) + (bl6) + (end-coords)) + (send bl3 :weight upper-leg-weight) + (send bl4 :weight lower-leg-weight) + (send bl6b :locate (float-vector foot-offset 0 (- ankle-length))) + (send bl6b :set-color :green) + (setq bl6 (make-default-robot-link ankle-length leg-radius :y (read-from-string (format nil "~A-link5" l/r)) (list bl6b))) + (send bl6 :weight foot-weight) + (case l/r + (:rleg + (setq end-coords (make-cascoords)) + (send end-coords :locate (float-vector 0 0 (- (+ ankle-length (/ foot-thickness 2.0))))) + (send bl6 :assoc end-coords)) + (:lleg + (setq end-coords (make-cascoords)) + (send end-coords :locate (float-vector 0 0 (- (+ ankle-length (/ foot-thickness 2.0))))) + (send bl6 :assoc end-coords))) + (send bl5 :assoc bl6) + (send bl5 :translate (float-vector 0 0 (- lower-leg-length)) :world) + (send bl4 :assoc bl5) + (send bl4 :translate (float-vector 0 0 (- upper-leg-length)) :world) + (send bl3 :assoc bl4) + (send bl2 :assoc bl3) + (send bl1 :assoc bl2) + (list (cons :links (list bl1 bl2 bl3 bl4 bl5 bl6)) + (cons :root-link bl1) + (cons :end-coords end-coords)))) + (:reset-pose () + (send self :angle-vector #f(0.0 0.0 0.0 0.0 10.0 20.0 0.0 -20.0 10.0 0.0 0.0 10.0 20.0 0.0 -20.0 10.0 0.0 0.0 0.0 0.0 -15.0 30.0 -15.0 0.0 0.0 0.0 -15.0 30.0 -15.0 0.0 0.0 0.0 -15.0 30.0 -15.0 0.0 0.0 0.0 -15.0 30.0 -15.0 0.0))) + ) + +(defmethod sample-centaur-robot + ;; user-defined joint + (:torso-waist-y (&rest args) (forward-message-to jc0 args)) + (:torso-waist-p (&rest args) (forward-message-to jc1 args)) + (:head-neck-y (&rest args) (forward-message-to jh0 args)) + (:head-neck-p (&rest args) (forward-message-to jh1 args)) + (:larm-shoulder-p (&rest args) (forward-message-to jal0 args)) + (:larm-shoulder-r (&rest args) (forward-message-to jal1 args)) + (:larm-shoulder-y (&rest args) (forward-message-to jal2 args)) + (:larm-elbow-p (&rest args) (forward-message-to jal3 args)) + (:larm-wrist-y (&rest args) (forward-message-to jal4 args)) + (:larm-wrist-r (&rest args) (forward-message-to jal5 args)) + (:larm-wrist-p (&rest args) (forward-message-to jal6 args)) + (:rarm-shoulder-p (&rest args) (forward-message-to jar0 args)) + (:rarm-shoulder-r (&rest args) (forward-message-to jar1 args)) + (:rarm-shoulder-y (&rest args) (forward-message-to jar2 args)) + (:rarm-elbow-p (&rest args) (forward-message-to jar3 args)) + (:rarm-wrist-y (&rest args) (forward-message-to jar4 args)) + (:rarm-wrist-r (&rest args) (forward-message-to jar5 args)) + (:rarm-wrist-p (&rest args) (forward-message-to jar6 args)) + (:lfleg-crotch-y (&rest args) (forward-message-to jllf0 args)) + (:lfleg-crotch-r (&rest args) (forward-message-to jllf1 args)) + (:lfleg-crotch-p (&rest args) (forward-message-to jllf2 args)) + (:lfleg-knee-p (&rest args) (forward-message-to jllf3 args)) + (:lfleg-ankle-p (&rest args) (forward-message-to jllf4 args)) + (:lfleg-ankle-r (&rest args) (forward-message-to jllf5 args)) + (:rfleg-crotch-y (&rest args) (forward-message-to jlrf0 args)) + (:rfleg-crotch-r (&rest args) (forward-message-to jlrf1 args)) + (:rfleg-crotch-p (&rest args) (forward-message-to jlrf2 args)) + (:rfleg-knee-p (&rest args) (forward-message-to jlrf3 args)) + (:rfleg-ankle-p (&rest args) (forward-message-to jlrf4 args)) + (:rfleg-ankle-r (&rest args) (forward-message-to jlrf5 args)) + (:lbleg-crotch-y (&rest args) (forward-message-to jllb0 args)) + (:lbleg-crotch-r (&rest args) (forward-message-to jllb1 args)) + (:lbleg-crotch-p (&rest args) (forward-message-to jllb2 args)) + (:lbleg-knee-p (&rest args) (forward-message-to jllb3 args)) + (:lbleg-ankle-p (&rest args) (forward-message-to jllb4 args)) + (:lbleg-ankle-r (&rest args) (forward-message-to jllb5 args)) + (:rbleg-crotch-y (&rest args) (forward-message-to jlrb0 args)) + (:rbleg-crotch-r (&rest args) (forward-message-to jlrb1 args)) + (:rbleg-crotch-p (&rest args) (forward-message-to jlrb2 args)) + (:rbleg-knee-p (&rest args) (forward-message-to jlrb3 args)) + (:rbleg-ankle-p (&rest args) (forward-message-to jlrb4 args)) + (:rbleg-ankle-r (&rest args) (forward-message-to jlrb5 args)) + ) diff --git a/irteus/irtmodel.l b/irteus/irtmodel.l index 51023e6b1..ee860e6ca 100644 --- a/irteus/irtmodel.l +++ b/irteus/irtmodel.l @@ -3127,7 +3127,7 @@ (list ,(cadr (member :move-target params))) ,(cadr (member :move-target params)))) (t - (mapcar #'(lambda (_limb) (send ,robot _limb :end-coords)) ,limbs)))) + (mapcar #'(lambda (_limb) (send ,robot :limb _limb :end-coords)) ,limbs)))) (,link-list (mapcar #'(lambda (_move-target) (send ,robot :link-list diff --git a/irteus/irtrobot.l b/irteus/irtrobot.l index 1984500c9..9bd193a3a 100644 --- a/irteus/irtrobot.l +++ b/irteus/irtrobot.l @@ -732,20 +732,27 @@ (rthre (mapcar #'(lambda (x) (deg2rad (if (memq x '(:rleg :lleg)) 1 5))) fix-limbs)) (mid 0.5) (target-centroid-pos - (if (eq leg :both) - (apply - #'midpoint mid - (mapcar - #'(lambda (tmp-leg) - (send self :limb tmp-leg :end-coords :worldpos)) - (remove-if-not #'(lambda (x) (memq x '(:rleg :lleg))) fix-limbs))) - (send self :limb leg :end-coords :worldpos))) + (cond ((eq leg :both) + (apply + #'midpoint mid + (mapcar + #'(lambda (tmp-leg) + (send self :limb tmp-leg :end-coords :worldpos)) + (remove-if-not #'(lambda (x) (memq x '(:rleg :lleg))) fix-limbs)))) + ((listp leg) + (scale + (/ 1.0 (length leg)) + (reduce #'v+ + (mapcar + #'(lambda (tmp-leg) + (send self :limb tmp-leg :end-coords :worldpos)) leg)))) + (t (send self :limb leg :end-coords :worldpos)))) (fix-limbs-target-coords (mapcar #'(lambda (x) (send self :limb x :end-coords :copy-worldcoords)) fix-limbs)) (root-link-virtual-joint-weight #f(0.1 0.1 0.0 0.0 0.0 0.5)) ;; use only translation x, y and rotation z &allow-other-keys) "Move robot COG to change centroid-on-foot location, - leg : legs for target of robot's centroid, which should be :both, :rleg, and :lleg. + leg : legs for target of robot's centroid, which should be limb (ex. :rleg), list of limbs, and :both (which means '(:rleg :lleg)). fix-limbs : limb names which are fixed in this IK." (with-move-target-link-list (mt ll self fix-limbs) @@ -753,7 +760,7 @@ (send* self :fullbody-inverse-kinematics fix-limbs-target-coords :move-target mt :link-list ll - :fix-limbs (remove-if-not #'(lambda (x) (memq x '(:rleg :lleg))) fix-limbs) + :fix-limbs fix-limbs :root-link-virtual-joint-weight root-link-virtual-joint-weight :thre thre :rthre rthre :target-centroid-pos target-centroid-pos From 7a0ccdaad997c529217d7df8e8074394b6e581be Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 9 May 2021 13:50:56 +0900 Subject: [PATCH 10/17] [sample-multidof-arm-model] use add-limb --- irteus/demo/sample-multidof-arm-model.l | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/irteus/demo/sample-multidof-arm-model.l b/irteus/demo/sample-multidof-arm-model.l index d9c5c14b0..06ceaa558 100644 --- a/irteus/demo/sample-multidof-arm-model.l +++ b/irteus/demo/sample-multidof-arm-model.l @@ -70,13 +70,11 @@ :bodies (list b)))) bb))) arm-length-list)) - (let ((tpos 0)) + (let ((tpos 0) + rarm-end-coords) (dotimes (i (length links)) (send (elt links i) :locate (float-vector 0 0 tpos) :world) - (setq tpos (+ tpos (elt arm-length-list i)))) - (setq rarm-end-coords (make-cascoords :pos (float-vector 0 0 tpos)))) - (setq rarm links) - (send (car (last links)) :assoc rarm-end-coords) + (setq tpos (+ tpos (elt arm-length-list i))))) (send self :assoc (car links)) (setq joint-list (mapcar @@ -95,6 +93,9 @@ (eval `(defmethod ,(send (class self) :name) (,(read-from-string (format nil ":~A" (send j :name))) () (elt joint-list ,(position j joint-list)))))) + (let ((rarm-end-coords (make-cascoords :pos (float-vector 0 0 (reduce #'+ arm-length-list))))) + (send (car (last links)) :assoc rarm-end-coords) + (send self :add-limb :rarm :links (cdr links) :end-coords rarm-end-coords :root-link (cadr links))) (send-super :init-ending) self) ) From 3c33f3f9286141ded5c56b9ce95e0d92182c41cb Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 9 May 2021 17:52:32 +0900 Subject: [PATCH 11/17] sample-centaur-robot walk demo --- ...r-robot.l => sample-centaur-robot-model.l} | 0 irteus/demo/walk-motion.l | 34 +++++ irteus/irtdyna.l | 14 +- irteus/irtmodel.l | 4 +- irteus/irtrobot.l | 138 ++++++++++-------- 5 files changed, 127 insertions(+), 63 deletions(-) rename irteus/demo/{sample-centaur-robot.l => sample-centaur-robot-model.l} (100%) diff --git a/irteus/demo/sample-centaur-robot.l b/irteus/demo/sample-centaur-robot-model.l similarity index 100% rename from irteus/demo/sample-centaur-robot.l rename to irteus/demo/sample-centaur-robot-model.l diff --git a/irteus/demo/walk-motion.l b/irteus/demo/walk-motion.l index c809be8d4..7053b79a8 100644 --- a/irteus/demo/walk-motion.l +++ b/irteus/demo/walk-motion.l @@ -40,6 +40,7 @@ ;;; (load "sample-robot-model.l") +(load "sample-centaur-robot-model.l") (defun walk-motion (robot) @@ -246,6 +247,39 @@ :go-backward-over go-backward-over)) (warn "(crawl-walk-motion-for-sample-robot) ;; for walking motion~%") +(defun quad-walk-motion-for-sample-centaur-robot + (gen-footstep-func) + (unless (boundp '*sample-centaur-robot*) + (setq *sample-centaur-robot* (instance sample-centaur-robot :init))) + ;; initial quad pose + (send *sample-centaur-robot* :reset-pose) + (send *sample-centaur-robot* :fix-leg-to-coords (make-coords) '(:rfleg :lfleg :rbleg :lbleg)) + + ;; prepare footsteps + (let ((footstep-list (funcall gen-footstep-func))) + (objects (list *sample-centaur-robot*)) + ;; solve walk motion + (send *sample-centaur-robot* :calc-walk-pattern-from-footstep-list + footstep-list :debug-view :no-message + :all-limbs '(:rfleg :lfleg :rbleg :lbleg) + :init-pose-function + #'(lambda () + (send *sample-centaur-robot* :move-centroid-on-foot '(:rfleg :lfleg :rbleg :lbleg) '(:rfleg :lfleg :rbleg :lbleg))) + :default-step-height 70) + )) + +(defun trot-walk-motion-for-sample-centaur-robot + () + (quad-walk-motion-for-sample-centaur-robot + #'(lambda () (send *sample-centaur-robot* :go-pos-quadruped-params->footstep-list 200 100 0 :legs '(:rfleg :lfleg :rbleg :lbleg) :type :trot)))) +(warn "(trot-walk-motion-for-sample-centaur-robot) ;; for walking motion~%") + +(defun crawl-walk-motion-for-sample-centaur-robot + () + (quad-walk-motion-for-sample-centaur-robot + #'(lambda () (send *sample-centaur-robot* :go-pos-quadruped-params->footstep-list 50 0 0 :legs '(:rfleg :lfleg :rbleg :lbleg) :type :crawl)))) +(warn "(crawl-walk-motion-for-sample-centaur-robot) ;; for walking motion~%") + (defun walk-motion-for-robots () (unless (boundp '*robots*) (setq *robots* diff --git a/irteus/irtdyna.l b/irteus/irtdyna.l index df8008a56..1f08eb872 100644 --- a/irteus/irtdyna.l +++ b/irteus/irtdyna.l @@ -1994,7 +1994,7 @@ ;; solve-angle-vector methods (:solve-av-by-move-centroid-on-foot (support-leg support-leg-coords swing-leg-coords cog robot - &rest args &key (cog-gain 3.5) (stop 100) (additional-nspace-list) &allow-other-keys) + &rest args &key (cog-gain 3.5) (stop 100) (additional-nspace-list) (robot-xv) &allow-other-keys) (let* ((legs (append (send self :get-counter-footstep-limbs support-leg) support-leg)) (leg-order (mapcar #'(lambda (l) (position l legs)) all-limbs)) @@ -2003,6 +2003,8 @@ (setq args (append args (list :thre (mapcar #'(lambda (x) (if (memq x '(:rleg :lleg)) ik-thre (* 5 ik-thre))) all-limbs))))) (unless (memq :rthre args) (setq args (append args (list :rthre (mapcar #'(lambda (x) (deg2rad (if (memq x '(:rleg :lleg)) ik-rthre (* 5 ik-rthre)))) all-limbs))))) + (unless robot-xv + (setq robot-xv (case (length legs) (2 #f(1 0 0)) (4 #f(0 0 1))))) (send* robot :move-centroid-on-foot :both all-limbs :target-centroid-pos cog :fix-limbs-target-coords (mapcar #'(lambda (idx) (elt fix-coords idx)) leg-order) @@ -2012,9 +2014,13 @@ additional-nspace-list (list (list (car (send robot :links)) #'(lambda () - (let* ((fcoords (apply #'midcoords 0.5 - (mapcar #'(lambda (x) (elt fix-coords (position x legs))) '(:rleg :lleg)))) - (xvr (send robot :rotate-vector (case (length legs) (2 #f(1 0 0)) (4 #f(0 0 1))))) + (let* ((fcoords (let ((coords (make-coords))) + (dotimes (i (length legs)) + (setq coords (midcoords (/ 1.0 (+ i 1.0)) + coords + (send robot :limb (elt legs i) :end-coords :worldcoords)))) + coords)) + (xvr (send robot :rotate-vector robot-xv)) (xvf (send fcoords :rotate-vector #f(1 0 0)))) (if (> (elt (send fcoords :inverse-rotate-vector xvr) 0) 0) (setq xvr (scale -1 xvr))) (dolist (xv (list xvr xvf)) (setf (elt xv 2) 0.0)) diff --git a/irteus/irtmodel.l b/irteus/irtmodel.l index ee860e6ca..8868bc40f 100644 --- a/irteus/irtmodel.l +++ b/irteus/irtmodel.l @@ -3127,7 +3127,9 @@ (list ,(cadr (member :move-target params))) ,(cadr (member :move-target params)))) (t - (mapcar #'(lambda (_limb) (send ,robot :limb _limb :end-coords)) ,limbs)))) + (mapcar #'(lambda (_limb) (if (symbolp _limb) + (send ,robot :limb _limb :end-coords) + _limb)) ,limbs)))) (,link-list (mapcar #'(lambda (_move-target) (send ,robot :link-list diff --git a/irteus/irtrobot.l b/irteus/irtrobot.l index 9bd193a3a..325338b05 100644 --- a/irteus/irtrobot.l +++ b/irteus/irtrobot.l @@ -191,13 +191,13 @@ (let ((end-coords (cond (l (cdr (assoc :end-coords l))) ((member limb '(:rarm :larm :rleg :lleg :head :torso)) ;; obsoleted. for backward compatibility (send self (read-from-string (format nil "~A-END-COORDS" limb)))) - (t (error ";; error: cannot find limb ~A~%" limb))))) + (t nil)))) (user::forward-message-to end-coords args))) (:root-link (let ((root-link (cond (l (cdr (assoc :root-link l))) ((member limb '(:rarm :larm :rleg :lleg :head :torso)) ;; obsoleted. for backward compatibility (send self (read-from-string (format nil "~A-ROOT-LINK" limb)))) - (t (error ";; error: cannot find limb ~A~%" limb))))) + (t nil)))) (user::forward-message-to root-link args))) (:angle-vector (if args @@ -246,7 +246,7 @@ ((member limb '(:rarm :larm :rleg :lleg :head :torso)) ;; obsoleted. for backward compatibility (cdr (assoc (intern (format nil "~A-COLLISION-AVOIDANCE-LINKS" (string-upcase limb))) (send self :slots)))) - (t (error ";; error: cannot find limb ~A~%" limb))))) + (t nil)))) (user::forward-message-to collision-avoidance-links args))) @@ -265,7 +265,7 @@ (cond (l (cdr (assoc :links l))) ((member limb '(:rarm :larm :rleg :lleg :head :torso)) ;; obsoleted. for backward compatibility (cdr (assoc (intern (string-upcase limb)) (send self :slots)))) - (t (error ";; error: cannot find limb ~A~%" limb))))) + (t nil)))) (t (let ((limb-joint-name (intern (format nil "~A-~A" (string-upcase limb) (string-upcase method)) *keyword-package*))) @@ -487,7 +487,8 @@ (av (send self :angle-vector)) (root-coords (send (car (send self :links)) :copy-worldcoords)) (calc-torque-buffer-args (send self :calc-torque-buffer-args)) - (distribute-total-wrench-to-torque-method (if (and (not (every #'null (send self :legs))) ;; For legged robots + (support-leg-coords-list (remove nil (send self :legs :end-coords))) + (distribute-total-wrench-to-torque-method (if (and support-leg-coords-list ;; For legged robots (not (and force-list moment-list target-coords))) ;; If no force-list, moment-list, target-coords are specified :distribute-total-wrench-to-torque-method-default))) "Returns torque vector" @@ -499,23 +500,19 @@ :force-list force-list :moment-list moment-list :target-coords target-coords :calc-torque-buffer-args calc-torque-buffer-args))) (when (find-method self distribute-total-wrench-to-torque-method) - (let ((ext-wrench-tq (send self distribute-total-wrench-to-torque-method))) + (let ((ext-wrench-tq (send self distribute-total-wrench-to-torque-method support-leg-coords-list))) (dotimes (i (length ret-tq)) (setf (elt ret-tq i) (- (elt ret-tq i) (elt ext-wrench-tq i))) (send (elt joint-list i) :joint-torque (elt ret-tq i))))) ret-tq )) (:distribute-total-wrench-to-torque-method-default - () + (&optional (target-coords (remove nil (send self :legs :end-coords)))) ;; set default force & moment by solving mimimum internal forces - (let ((target-coords)) - (dolist (limb '(:rleg :lleg)) - (if (send self :limb limb) - (push (send self :limb limb :end-coords) target-coords))) - (let* ((ret-fm (send self :calc-contact-wrenches-from-total-wrench (send-all target-coords :worldpos) - :total-wrench (concatenate float-vector (send (car (send self :links)) :force) (send (car (send self :links)) :moment))))) - (send self :calc-torque-from-ext-wrenches :force-list (car ret-fm) :moment-list (cadr ret-fm) :target-coords target-coords) - ))) + (let* ((ret-fm (send self :calc-contact-wrenches-from-total-wrench (send-all target-coords :worldpos) + :total-wrench (concatenate float-vector (send (car (send self :links)) :force) (send (car (send self :links)) :moment))))) + (send self :calc-torque-from-ext-wrenches :force-list (car ret-fm) :moment-list (cadr ret-fm) :target-coords target-coords) + )) (:calc-force-from-joint-torque (limb all-torque &key (move-target (send self :limb limb :end-coords)) (use-torso)) "Calculates end-effector force and moment from joint torques." @@ -701,20 +698,33 @@ (apply #'midcoords mid (send self :legs :end-coords)) ) (:fix-leg-to-coords - (fix-coords &optional (l/r :both) &key (mid 0.5) &allow-other-keys) ;; support-leg + (fix-coords &optional (legs :both) &key (mid 0.5) &allow-other-keys) ;; support-leg "Fix robot's legs to a coords - In the following codes, leged robot is assumed." - (unless (not (some #'null (send self :legs :links))) - (return-from :fix-leg-to-coords nil)) + In the following codes, leged robot is assumed. + legs is :right, :left, :both, limb names (ex. :rleg or coordinates) and list of limb names" (let (support-coords tmp-coords move-coords pos rot ra) (cond - ((or (eq l/r :left) (eq l/r :lleg)) + ((eq legs :left) (setq support-coords (send self :limb :lleg :end-coords :copy-worldcoords))) - ((or (eq l/r :right) (eq l/r :rleg)) + ((eq legs :right) (setq support-coords (send self :limb :rleg :end-coords :copy-worldcoords))) - (t + + ((symbolp legs) + (setq support-coords + (send self :limb legs :end-coords :copy-worldcoords))) + ((derivedp legs coordinates) + (setq support-coords (send legs :copy-worldcoords))) + ((listp legs) + (setq support-coords (make-coords)) + (dotimes (i (length legs)) + (setq support-coords (midcoords (/ 1.0 (+ i 1.0)) + support-coords + (cond ((symbolp (elt legs i)) + (send self :limb (elt legs i) :end-coords :copy-worldcoords)) + (t (send (elt legs i) :copy-worldcoords))))))) + (t ;;(eq legs :both) (setq support-coords (midcoords mid (send self :limb :lleg :end-coords :copy-worldcoords) @@ -745,15 +755,19 @@ (reduce #'v+ (mapcar #'(lambda (tmp-leg) - (send self :limb tmp-leg :end-coords :worldpos)) leg)))) - (t (send self :limb leg :end-coords :worldpos)))) + (if (symbolp tmp-leg) + (send self :limb tmp-leg :end-coords :worldpos) + (send tmp-leg :worldpos))) leg)))) + (t (if (symbolp leg) + (send self :limb leg :end-coords :worldpos) + (send leg :worldpos))))) (fix-limbs-target-coords (mapcar #'(lambda (x) (send self :limb x :end-coords :copy-worldcoords)) fix-limbs)) (root-link-virtual-joint-weight #f(0.1 0.1 0.0 0.0 0.0 0.5)) ;; use only translation x, y and rotation z &allow-other-keys) "Move robot COG to change centroid-on-foot location, - leg : legs for target of robot's centroid, which should be limb (ex. :rleg), list of limbs, and :both (which means '(:rleg :lleg)). - fix-limbs : limb names which are fixed in this IK." + leg : legs for target of robot's centroid, which should be limb (ex. :rleg or coordinates), list of limbs, and :both (which means '(:rleg :lleg)). + fix-limbs : limb names (ex. :rleg or coordinates) which are fixed in this IK." (with-move-target-link-list (mt ll self fix-limbs) (let* () @@ -830,6 +844,7 @@ (let* ((res) (ret) (tm 0) (gg (instance gait-generator :init self dt))) (funcall init-pose-function) + (setq solve-angle-vector-args (append solve-angle-vector-args (list :robot-xv (send self :inverse-rotate-vector #F(1 0 0))))) ;; initial move centroid on foot (send gg :initialize-gait-parameter footstep-list default-step-time (send self :centroid) @@ -909,25 +924,27 @@ ;; generate footstep parameter ;; currently only default, forward and outside (:gen-footstep-parameter - (&key (ratio 1.0)) + (&key (ratio 1.0) + (legs '(:rleg :lleg))) "Generate footstep parameter" (warn ";; generating footstep-parameter...~%") (let ((pav (send self :angle-vector)) - (pc (send self :copy-worldcoords))) + (pc (send self :copy-worldcoords)) + (rleg_ (car legs)) (lleg_ (cadr legs))) (send self :reset-pose) - (send self :fix-leg-to-coords (make-coords) '(:rleg :lleg)) - (let ((dol (abs (elt (apply #'v- (send self :legs :end-coords :worldpos)) 1)))) + (send self :fix-leg-to-coords (make-coords) legs) + (let ((dol (abs (elt (apply #'v- (mapcar #'(lambda (l) (send self :limb l :end-coords :worldpos)) legs)) 1)))) (labels ((ik-test (target-coords-func diff-func &optional (limit-func)) (send self :reset-pose) - (send self :fix-leg-to-coords (make-coords) '(:rleg :lleg)) - (let* ((tc (send self :limb :rleg :end-coords :copy-worldcoords)) + (send self :fix-leg-to-coords (make-coords) legs) + (let* ((tc (send self :limb rleg_ :end-coords :copy-worldcoords)) (init-tc (send tc :copy-worldcoords))) (while (and (send self :inverse-kinematics (funcall target-coords-func tc) - :link-list (send self :link-list (send self :limb :rleg :end-coords :parent)) - :move-target (send self :limb :rleg :end-coords) :warnp nil) + :link-list (send self :link-list (send self :limb rleg_ :end-coords :parent)) + :move-target (send self :limb rleg_ :end-coords) :warnp nil) (if limit-func (funcall limit-func tc init-tc) t) )) (funcall diff-func tc init-tc)))) @@ -950,15 +967,16 @@ ))) )) (:footstep-parameter - () + (&key (legs '(:rleg :lleg))) (unless (send self :get :footstep-parameter) - (send self :gen-footstep-parameter)) + (send self :gen-footstep-parameter :legs legs)) (send self :get :footstep-parameter) ) ;; simple footstep gen (:go-pos-params->footstep-list (xx yy th ;; [mm] [mm] [deg] - &key ((:footstep-parameter prm) (send self :footstep-parameter)) + &key (legs '(:rleg :lleg)) + ((:footstep-parameter prm) (send self :footstep-parameter :legs legs)) ((:default-half-offset defp) (cadr (memq :default-half-offset prm))) ((:forward-offset-length xx-max) (cadr (memq :forward-offset-length prm))) ((:outside-offset-length yy-max) (cadr (memq :outside-offset-length prm))) @@ -971,24 +989,25 @@ (send cc :name leg) cc)))) "Calculate foot step list from goal x position [mm], goal y position [mm], and goal yaw orientation [deg]." - (let* ((dx xx) (dy yy) (dth th) + (unless (= (length legs) 2) (error ";; error: length of limbs != 2")) + (let* ((dx xx) (dy yy) (dth th) (rleg_ (car legs)) (lleg_ (cadr legs)) (leg (if (eps= (float yy) 0.0) - (if (> th 0.0) :lleg :rleg) - (if (> yy 0.0) :lleg :rleg))) - (mc (apply #'midcoords 0.5 (send self :legs :end-coords :copy-worldcoords))) + (if (> th 0.0) lleg_ rleg_) + (if (> yy 0.0) lleg_ rleg_))) + (mc (apply #'midcoords 0.5 (mapcar #'(lambda (l) (send self :limb l :end-coords :copy-worldcoords)) legs))) (gc (send (make-coords :pos (float-vector xx yy 0) :rpy (float-vector (deg2rad th) 0 0)) :transform mc :world)) (leg-translate-pos (mapcar #'(lambda (l) - (list l (scale (case l (:rleg -1) (:lleg 1)) defp))) - '(:rleg :lleg))) - (ret (list (funcall gen-go-pos-step-node-func mc (case leg (:lleg :rleg) (:rleg :lleg)) leg-translate-pos)))) + (list l (scale (cond ((eq l rleg_) -1) ((eq l lleg_) 1)) defp))) + (list rleg_ lleg_))) + (ret (list (funcall gen-go-pos-step-node-func mc (cond ((eq leg lleg_) rleg_) ((eq leg rleg_) lleg_)) leg-translate-pos)))) (labels ((do-push-steps (finish-check func) (until (funcall finish-check) (funcall func) (push (funcall gen-go-pos-step-node-func mc leg leg-translate-pos) ret) - (setq leg (case leg (:lleg :rleg) (:rleg :lleg))) - ))) + (setq leg (cond ((eq leg lleg_) rleg_) ((eq leg rleg_) lleg_))) + ))) (do-push-steps #'(lambda () (and (eps= (float dx) 0.0) (eps= (float dy) 0.0) @@ -998,18 +1017,18 @@ ((< xx-max dx) xx-max) ((> (- xx-max) dx) (- xx-max)) (t dx))) - (ddy (case leg - (:rleg + (ddy (cond + ((eq leg rleg_) (cond ((< 0.0 dy) 0.0) ((> (- yy-max) dy) (- yy-max)) (t dy))) - (:lleg + ((eq leg lleg_) (cond ((< yy-max dy) yy-max) ((> 0.0 dy) 0.0) (t dy))) - )) + )) (ddth (cond ((< th-max dth) th-max) ((> (- th-max) dth) (- th-max)) @@ -1022,10 +1041,13 @@ (reverse ret)))) (:go-pos-quadruped-params->footstep-list (xx yy th ;; [mm] [mm] [deg] - &key (type :crawl)) + &key (type :crawl) + (legs '(:rleg :lleg :rarm :larm))) "Calculate foot step list for quadruped walking from goal x position [mm], goal y position [mm], and goal yaw orientation [deg]." - (let ((biped-fsl (send self :go-pos-params->footstep-list xx yy th :forward-offset-length 100 :rotate-rad 3)) - quadruped-fsl) + (unless (= (length legs) 4) (error ";; error: length of limbs != 4")) + (let* ((rleg_ (elt legs 0)) (lleg_ (elt legs 1)) (rarm_ (elt legs 2)) (larm_ (elt legs 3)) + (biped-fsl (send self :go-pos-params->footstep-list xx yy th :forward-offset-length 100 :rotate-rad 3 :legs (list rleg_ lleg_))) + quadruped-fsl) ;; calculate foot step list for quad walk (dolist (fs biped-fsl) (let* ((fs-leg-name (send fs :name)) @@ -1033,24 +1055,24 @@ (target-arm-name)) (case type (:crawl - (dolist (l (list fs-leg-name (case fs-leg-name (:lleg :larm) (:rleg :rarm)))) + (dolist (l (list fs-leg-name (cond ((eq fs-leg-name lleg_) larm_) ((eq fs-leg-name rleg_) rarm_)))) (push (list (make-coords :coords (send (send self :limb l :end-coords :copy-worldcoords) :transform offset :world) :name l)) quadruped-fsl))) (:trot (push (mapcar #'(lambda (l) (make-coords :coords (send (send self :limb l :end-coords :copy-worldcoords) :transform offset :world) :name l)) - (list fs-leg-name (setq target-arm-name (case fs-leg-name (:lleg :rarm) (:rleg :larm))))) + (list fs-leg-name (setq target-arm-name (cond ((eq fs-leg-name lleg_) rarm_) ((eq fs-leg-name rleg_) larm_))))) quadruped-fsl)) (:pace (push (mapcar #'(lambda (l) (make-coords :coords (send (send self :limb l :end-coords :copy-worldcoords) :transform offset :world) :name l)) - (list fs-leg-name (case fs-leg-name (:lleg :larm) (:rleg :rarm)))) + (list fs-leg-name (cond ((eq fs-leg-name lleg_) larm_) ((eq fs-leg-name rleg_) rarm_)))) quadruped-fsl)) (:gallop (push (mapcar #'(lambda (l) (make-coords :coords (send (send self :limb l :end-coords :copy-worldcoords) :transform offset :world) :name l)) - (list (case fs-leg-name (:rleg :rleg) (:lleg :rarm)) - (case fs-leg-name (:rleg :lleg) (:lleg :larm)))) + (list (cond ((eq fs-leg-name rleg_) rleg_) ((eq fs-leg-name lleg_) rarm_)) + (cond ((eq fs-leg-name rleg_) lleg_) ((eq fs-leg-name lleg_) larm_)))) quadruped-fsl)) ))) (reverse quadruped-fsl))) From 66734445eee97fae0fa1a8e02cd1e51423cdf9e4 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 9 May 2021 20:21:12 +0900 Subject: [PATCH 12/17] fix --- irteus/demo/sample-multidof-arm-model.l | 3 +-- irteus/irtrobot.l | 9 ++++++++- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/irteus/demo/sample-multidof-arm-model.l b/irteus/demo/sample-multidof-arm-model.l index 06ceaa558..92564d05b 100644 --- a/irteus/demo/sample-multidof-arm-model.l +++ b/irteus/demo/sample-multidof-arm-model.l @@ -70,8 +70,7 @@ :bodies (list b)))) bb))) arm-length-list)) - (let ((tpos 0) - rarm-end-coords) + (let ((tpos 0)) (dotimes (i (length links)) (send (elt links i) :locate (float-vector 0 0 tpos) :world) (setq tpos (+ tpos (elt arm-length-list i))))) diff --git a/irteus/irtrobot.l b/irteus/irtrobot.l index 325338b05..88cb982ad 100644 --- a/irteus/irtrobot.l +++ b/irteus/irtrobot.l @@ -844,7 +844,14 @@ (let* ((res) (ret) (tm 0) (gg (instance gait-generator :init self dt))) (funcall init-pose-function) - (setq solve-angle-vector-args (append solve-angle-vector-args (list :robot-xv (send self :inverse-rotate-vector #F(1 0 0))))) + (setq solve-angle-vector-args + (append solve-angle-vector-args + (list :robot-xv (let ((fcoords (make-coords))) + (dotimes (i (length al)) + (setq fcoords (midcoords (/ 1.0 (+ i 1.0)) + fcoords + (send self :limb (elt al i) :end-coords :worldcoords)))) + (send self :inverse-rotate-vector (send fcoords :rotate-vector #F(1 0 0))))))) ;; initial move centroid on foot (send gg :initialize-gait-parameter footstep-list default-step-time (send self :centroid) From ab4adb085f6bf2bae04a7a672d1b8ac7a84cbcb3 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Mon, 10 May 2021 13:15:08 +0900 Subject: [PATCH 13/17] fix --- irteus/irtrobot.l | 87 ++++++++++++++++++++++------------------------- 1 file changed, 40 insertions(+), 47 deletions(-) diff --git a/irteus/irtrobot.l b/irteus/irtrobot.l index 88cb982ad..fe0b05e6b 100644 --- a/irteus/irtrobot.l +++ b/irteus/irtrobot.l @@ -702,33 +702,28 @@ "Fix robot's legs to a coords In the following codes, leged robot is assumed. legs is :right, :left, :both, limb names (ex. :rleg or coordinates) and list of limb names" - (let (support-coords tmp-coords move-coords pos rot ra) - (cond - ((eq legs :left) - (setq support-coords - (send self :limb :lleg :end-coords :copy-worldcoords))) - ((eq legs :right) - (setq support-coords - (send self :limb :rleg :end-coords :copy-worldcoords))) - - ((symbolp legs) - (setq support-coords - (send self :limb legs :end-coords :copy-worldcoords))) - ((derivedp legs coordinates) - (setq support-coords (send legs :copy-worldcoords))) - ((listp legs) - (setq support-coords (make-coords)) - (dotimes (i (length legs)) - (setq support-coords (midcoords (/ 1.0 (+ i 1.0)) - support-coords - (cond ((symbolp (elt legs i)) - (send self :limb (elt legs i) :end-coords :copy-worldcoords)) - (t (send (elt legs i) :copy-worldcoords))))))) - (t ;;(eq legs :both) - (setq support-coords - (midcoords mid - (send self :limb :lleg :end-coords :copy-worldcoords) - (send self :limb :rleg :end-coords :copy-worldcoords))))) + (let (leg-coords support-coords tmp-coords move-coords pos rot ra) + ;; Handle legs argument + (when (eq legs :both) + (setq legs '(:rleg :lleg))) + (unless (listp legs) + (setq legs (list legs))) + (setq leg-coords (remove nil (mapcar #'(lambda (leg) + (cond + ((eq leg :left) (send self :limb :lleg :end-coords)) + ((eq leg :right) (send self :limb :rleg :end-coords)) + ((symbolp leg) (send self :limb leg :end-coords)) + ((derivedp leg coordinates) leg))) + legs))) + (when (null leg-coords) + (return-from :fix-leg-to-coords nil)) ;; leged robot is assumed. + ;; calculate mid-coords of legs + (setq support-coords (make-coords)) + (dotimes (i (length leg-coords)) + (setq support-coords (midcoords (/ 1.0 (+ i 1.0)) + support-coords + (send (elt leg-coords i) :worldcoords)))) + ;; fix self to target coords (setq tmp-coords (send fix-coords :copy-worldcoords)) (setq move-coords (send support-coords :transformation self)) (send tmp-coords :transform move-coords :local) @@ -742,27 +737,25 @@ (rthre (mapcar #'(lambda (x) (deg2rad (if (memq x '(:rleg :lleg)) 1 5))) fix-limbs)) (mid 0.5) (target-centroid-pos - (cond ((eq leg :both) - (apply - #'midpoint mid - (mapcar - #'(lambda (tmp-leg) - (send self :limb tmp-leg :end-coords :worldpos)) - (remove-if-not #'(lambda (x) (memq x '(:rleg :lleg))) fix-limbs)))) - ((listp leg) - (scale - (/ 1.0 (length leg)) - (reduce #'v+ - (mapcar - #'(lambda (tmp-leg) - (if (symbolp tmp-leg) - (send self :limb tmp-leg :end-coords :worldpos) - (send tmp-leg :worldpos))) leg)))) - (t (if (symbolp leg) - (send self :limb leg :end-coords :worldpos) - (send leg :worldpos))))) + (let* ((legs (cond ((listp leg) leg) ((eq leg :both) '(:rleg :lleg)) (t (list leg)))) + (leg-coords (remove nil (mapcar #'(lambda (leg) + (cond + ((eq leg :left) (send self :limb :lleg :end-coords)) + ((eq leg :right) (send self :limb :rleg :end-coords)) + ((symbolp leg) (send self :limb leg :end-coords)) + ((derivedp leg coordinates) leg))) + legs)))) + (scale (/ 1.0 (length leg-coords)) + (reduce #'v+ (mapcar #'(lambda (leg) (send leg :worldpos)) leg-coords))))) (fix-limbs-target-coords - (mapcar #'(lambda (x) (send self :limb x :end-coords :copy-worldcoords)) fix-limbs)) + (let* ((limbs (if (listp fix-limbs) fix-limbs (list fix-limbs)))) + (send-all (remove nil (mapcar #'(lambda (limb) + (cond + ((eq limb :left) (send self :limb :lleg :end-coords)) + ((eq limb :right) (send self :limb :rleg :end-coords)) + ((symbolp limb) (send self :limb limb :end-coords)) + ((derivedp limb coordinates) limb))) + limbs)) :copy-worldcoords))) (root-link-virtual-joint-weight #f(0.1 0.1 0.0 0.0 0.0 0.5)) ;; use only translation x, y and rotation z &allow-other-keys) "Move robot COG to change centroid-on-foot location, From 4891d487a3fba348ddc0c1c7b38f2bdb43d840d1 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Tue, 25 May 2021 18:52:33 +0900 Subject: [PATCH 14/17] [irteus] remove unnecessary diff --- irteus/irtmodel.l | 4 +--- irteus/irtrobot.l | 35 ++++++++++++++++++----------------- 2 files changed, 19 insertions(+), 20 deletions(-) diff --git a/irteus/irtmodel.l b/irteus/irtmodel.l index 8868bc40f..ee860e6ca 100644 --- a/irteus/irtmodel.l +++ b/irteus/irtmodel.l @@ -3127,9 +3127,7 @@ (list ,(cadr (member :move-target params))) ,(cadr (member :move-target params)))) (t - (mapcar #'(lambda (_limb) (if (symbolp _limb) - (send ,robot :limb _limb :end-coords) - _limb)) ,limbs)))) + (mapcar #'(lambda (_limb) (send ,robot :limb _limb :end-coords)) ,limbs)))) (,link-list (mapcar #'(lambda (_move-target) (send ,robot :link-list diff --git a/irteus/irtrobot.l b/irteus/irtrobot.l index fe0b05e6b..29751d214 100644 --- a/irteus/irtrobot.l +++ b/irteus/irtrobot.l @@ -96,7 +96,7 @@ larm-collision-avoidance-links rarm-collision-avoidance-links larm rarm lleg rleg torso head ;; obsoleted. for backward compatibility - limb-list + limb-list ;; sensor slots force-sensors imu-sensors cameras ;; @@ -397,7 +397,7 @@ (:head (&rest args) (unless args (setq args (list nil))) (send* self :limb :head args)) (:torso (&rest args) - (unless args (setq args (list nil))) (send* self :limb :torso args)) + (unless args (setq args (list nil))) (send* self :limb :torso args)) (:arms (&rest args) (list (send* self :limb :larm args) (send* self :limb :rarm args))) (:legs (&rest args) (list (send* self :limb :lleg args) (send* self :limb :rleg args))) (:look-at-hand @@ -487,8 +487,8 @@ (av (send self :angle-vector)) (root-coords (send (car (send self :links)) :copy-worldcoords)) (calc-torque-buffer-args (send self :calc-torque-buffer-args)) - (support-leg-coords-list (remove nil (send self :legs :end-coords))) - (distribute-total-wrench-to-torque-method (if (and support-leg-coords-list ;; For legged robots + (support-legs '(:rleg :lleg)) + (distribute-total-wrench-to-torque-method (if (and (some #'(lambda (l) (send self :limb l)) support-legs) ;; For legged robots (not (and force-list moment-list target-coords))) ;; If no force-list, moment-list, target-coords are specified :distribute-total-wrench-to-torque-method-default))) "Returns torque vector" @@ -500,19 +500,23 @@ :force-list force-list :moment-list moment-list :target-coords target-coords :calc-torque-buffer-args calc-torque-buffer-args))) (when (find-method self distribute-total-wrench-to-torque-method) - (let ((ext-wrench-tq (send self distribute-total-wrench-to-torque-method support-leg-coords-list))) + (let ((ext-wrench-tq (send self distribute-total-wrench-to-torque-method support-legs))) (dotimes (i (length ret-tq)) (setf (elt ret-tq i) (- (elt ret-tq i) (elt ext-wrench-tq i))) (send (elt joint-list i) :joint-torque (elt ret-tq i))))) ret-tq )) (:distribute-total-wrench-to-torque-method-default - (&optional (target-coords (remove nil (send self :legs :end-coords)))) + (&optional (limbs '(:rleg :lleg))) ;; set default force & moment by solving mimimum internal forces - (let* ((ret-fm (send self :calc-contact-wrenches-from-total-wrench (send-all target-coords :worldpos) - :total-wrench (concatenate float-vector (send (car (send self :links)) :force) (send (car (send self :links)) :moment))))) - (send self :calc-torque-from-ext-wrenches :force-list (car ret-fm) :moment-list (cadr ret-fm) :target-coords target-coords) - )) + (let ((target-coords)) + (dolist (limb limbs) + (if (send self :limb limb) + (push (send self :limb limb :end-coords) target-coords))) + (let* ((ret-fm (send self :calc-contact-wrenches-from-total-wrench (send-all target-coords :worldpos) + :total-wrench (concatenate float-vector (send (car (send self :links)) :force) (send (car (send self :links)) :moment))))) + (send self :calc-torque-from-ext-wrenches :force-list (car ret-fm) :moment-list (cadr ret-fm) :target-coords target-coords) + ))) (:calc-force-from-joint-torque (limb all-torque &key (move-target (send self :limb limb :end-coords)) (use-torso)) "Calculates end-effector force and moment from joint torques." @@ -712,8 +716,7 @@ (cond ((eq leg :left) (send self :limb :lleg :end-coords)) ((eq leg :right) (send self :limb :rleg :end-coords)) - ((symbolp leg) (send self :limb leg :end-coords)) - ((derivedp leg coordinates) leg))) + (t (send self :limb leg :end-coords)))) legs))) (when (null leg-coords) (return-from :fix-leg-to-coords nil)) ;; leged robot is assumed. @@ -742,8 +745,7 @@ (cond ((eq leg :left) (send self :limb :lleg :end-coords)) ((eq leg :right) (send self :limb :rleg :end-coords)) - ((symbolp leg) (send self :limb leg :end-coords)) - ((derivedp leg coordinates) leg))) + (t (send self :limb leg :end-coords)))) legs)))) (scale (/ 1.0 (length leg-coords)) (reduce #'v+ (mapcar #'(lambda (leg) (send leg :worldpos)) leg-coords))))) @@ -753,14 +755,13 @@ (cond ((eq limb :left) (send self :limb :lleg :end-coords)) ((eq limb :right) (send self :limb :rleg :end-coords)) - ((symbolp limb) (send self :limb limb :end-coords)) - ((derivedp limb coordinates) limb))) + (t (send self :limb limb :end-coords)))) limbs)) :copy-worldcoords))) (root-link-virtual-joint-weight #f(0.1 0.1 0.0 0.0 0.0 0.5)) ;; use only translation x, y and rotation z &allow-other-keys) "Move robot COG to change centroid-on-foot location, leg : legs for target of robot's centroid, which should be limb (ex. :rleg or coordinates), list of limbs, and :both (which means '(:rleg :lleg)). - fix-limbs : limb names (ex. :rleg or coordinates) which are fixed in this IK." + fix-limbs : list of limb names which are fixed in this IK." (with-move-target-link-list (mt ll self fix-limbs) (let* () From 25739d4995d3ead0a258a1a5b614c7e30c18698e Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Tue, 25 May 2021 18:53:01 +0900 Subject: [PATCH 15/17] [test/irteus-demo.l] add centaur robot test --- irteus/test/irteus-demo.l | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/irteus/test/irteus-demo.l b/irteus/test/irteus-demo.l index dfb097c26..3cfa9567d 100644 --- a/irteus/test/irteus-demo.l +++ b/irteus/test/irteus-demo.l @@ -95,6 +95,16 @@ (not (some #'null (mapcar #'(lambda (x) (cadr (memq :angle-vector x))) (crawl-walk-motion-for-sample-robot :go-backward-over nil)))))) +(deftest test-trot-walk-motion-for-sample-centaur-robot + (assert + (not (some #'null (mapcar #'(lambda (x) (cadr (memq :angle-vector x))) + (trot-walk-motion-for-sample-centaur-robot)))))) + +(deftest test-crawl-walk-motion-for-sample-centaur-robot + (assert + (not (some #'null (mapcar #'(lambda (x) (cadr (memq :angle-vector x))) + (crawl-walk-motion-for-sample-centaur-robot)))))) + (deftest test-walk-motion-for-robots (assert (every #'identity From 9eaf78a956889e6a7513e455339f60776fe897cb Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Tue, 25 May 2021 19:06:41 +0900 Subject: [PATCH 16/17] [irtdyna.l] add document --- irteus/irtdyna.l | 1 + 1 file changed, 1 insertion(+) diff --git a/irteus/irtdyna.l b/irteus/irtdyna.l index 1f08eb872..2cb953dff 100644 --- a/irteus/irtdyna.l +++ b/irteus/irtdyna.l @@ -1995,6 +1995,7 @@ (:solve-av-by-move-centroid-on-foot (support-leg support-leg-coords swing-leg-coords cog robot &rest args &key (cog-gain 3.5) (stop 100) (additional-nspace-list) (robot-xv) &allow-other-keys) + "robot-xv: X diraction of footcoords represented in robot local coordinate" (let* ((legs (append (send self :get-counter-footstep-limbs support-leg) support-leg)) (leg-order (mapcar #'(lambda (l) (position l legs)) all-limbs)) From 74c2bba7a3e94abc57f34c9cc773ce67a547999e Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Fri, 15 Apr 2022 19:29:37 +0900 Subject: [PATCH 17/17] remove unnecessary diff --- irteus/demo/sample-arm-model.l | 17 +-- irteus/demo/sample-centaur-robot-model.l | 12 +- irteus/demo/sample-multidof-arm-model.l | 8 +- irteus/demo/sample-robot-model.l | 130 ++++++++------------ irteus/irtbvh.l | 12 +- irteus/irtdyna.l | 12 +- irteus/irtmodel.l | 2 +- irteus/irtrobot.l | 146 +++++++++++------------ 8 files changed, 146 insertions(+), 193 deletions(-) diff --git a/irteus/demo/sample-arm-model.l b/irteus/demo/sample-arm-model.l index 171b38884..74b80efc2 100644 --- a/irteus/demo/sample-arm-model.l +++ b/irteus/demo/sample-arm-model.l @@ -441,16 +441,6 @@ joint-fl joint-fr)) (setq collision-avoidance-links (list sarm-b2 sarm-b3 sarm-b4 sarm-b5 sarm-b6)) - ;; limbs - (send self :add-limb :rarm - :end-coords end-coords - :links (list sarm-b1 sarm-b2 sarm-b3 sarm-b4 sarm-b5 sarm-b6) - :root-link sarm-b0) - (send self :add-limb :rhand - :end-coords end-coords - :links (list sarm-fr sarm-fl) - :root-link sarm-b6) - (send self :init-ending) self)) (:joint0 (&rest args) (forward-message-to joint0 args)) @@ -468,8 +458,8 @@ ) (:move-fingers (l) - (send self :limb :rhand :angle-vector (float-vector l l)) - l) + (send joint-fl :joint-angle l) + (send joint-fr :joint-angle l)) (:init-ending () (setq bodies (flatten (send-all links :bodies))) @@ -498,7 +488,8 @@ (setq a (send self :open-hand)) (while (> a 0) (if (collision-check-objects - (send self :limb :rhand :links) + (list (send self :joint-fr :child-link) + (send self :joint-fl :child-link)) (list obj)) (return)) (send self :move-fingers a) diff --git a/irteus/demo/sample-centaur-robot-model.l b/irteus/demo/sample-centaur-robot-model.l index 78dc6b677..d9fde75bd 100644 --- a/irteus/demo/sample-centaur-robot-model.l +++ b/irteus/demo/sample-centaur-robot-model.l @@ -226,23 +226,23 @@ ;; set sensors (setq force-sensors (mapcar #'(lambda (l) - (send (send self :limb l :end-coords :parent) :assoc - (make-cascoords :coords (send self :limb l :end-coords :copy-worldcoords) + (send (send self l :end-coords :parent) :assoc + (make-cascoords :coords (send self l :end-coords :copy-worldcoords) :name (format nil "~A-fsensor" (string-downcase l))))) (list :rarm :larm :rfleg :lfleg :rbleg :lbleg))) (setq imu-sensors (list (let ((sen (make-cascoords - :worldpos (send (car (send self :limb :torso :waist-p :child-link :bodies)) :centroid) + :worldpos (send (car (send self :torso :waist-p :child-link :bodies)) :centroid) :name "torso-imusensor"))) - (send (send self :limb :torso :waist-p :child-link) :assoc sen) + (send (send self :torso :waist-p :child-link) :assoc sen) sen))) (setq cameras (mapcar #'(lambda (pos name) (let ((sen (instance camera-model :init (make-cylinder 10 20) :name name))) (send sen :rotate pi/2 :y) (send sen :rotate -pi/2 :z) - (send sen :locate (v+ pos (send (car (send (send self :limb :head :neck-p :child-link) :bodies)) :centroid)) :world) - (send (send self :limb :head :neck-p :child-link) :assoc sen) + (send sen :locate (v+ pos (send (car (send (send self :head :neck-p :child-link) :bodies)) :centroid)) :world) + (send (send self :head :neck-p :child-link) :assoc sen) sen)) (list (float-vector 0 30 0) (float-vector 0 -30 0)) (list "left-camera" "right-camera"))) diff --git a/irteus/demo/sample-multidof-arm-model.l b/irteus/demo/sample-multidof-arm-model.l index 92564d05b..d9c5c14b0 100644 --- a/irteus/demo/sample-multidof-arm-model.l +++ b/irteus/demo/sample-multidof-arm-model.l @@ -73,7 +73,10 @@ (let ((tpos 0)) (dotimes (i (length links)) (send (elt links i) :locate (float-vector 0 0 tpos) :world) - (setq tpos (+ tpos (elt arm-length-list i))))) + (setq tpos (+ tpos (elt arm-length-list i)))) + (setq rarm-end-coords (make-cascoords :pos (float-vector 0 0 tpos)))) + (setq rarm links) + (send (car (last links)) :assoc rarm-end-coords) (send self :assoc (car links)) (setq joint-list (mapcar @@ -92,9 +95,6 @@ (eval `(defmethod ,(send (class self) :name) (,(read-from-string (format nil ":~A" (send j :name))) () (elt joint-list ,(position j joint-list)))))) - (let ((rarm-end-coords (make-cascoords :pos (float-vector 0 0 (reduce #'+ arm-length-list))))) - (send (car (last links)) :assoc rarm-end-coords) - (send self :add-limb :rarm :links (cdr links) :end-coords rarm-end-coords :root-link (cadr links))) (send-super :init-ending) self) ) diff --git a/irteus/demo/sample-robot-model.l b/irteus/demo/sample-robot-model.l index c64d04306..9d212a186 100644 --- a/irteus/demo/sample-robot-model.l +++ b/irteus/demo/sample-robot-model.l @@ -66,40 +66,22 @@ (upper-leg-weight 2152.7) (lower-leg-weight 898.7) (foot-weight 313.5) ) (send-super* :init :name name args) - ;; 1. make links and assoc all links - (let* ((aroot-link (send self :make-root-link waist-length waist-weight)) - (torso-info (send self :make-torso-links torso-length torso-weight)) - (torso (cdr (assoc :links torso-info))) - (torso-end-coords (cdr (assoc :end-coords torso-info))) - (torso-root-link (cdr (assoc :root-link torso-info))) - (head-info (send self :make-head-links head-weight)) - (head (cdr (assoc :links head-info))) - (head-end-coords (cdr (assoc :end-coords head-info))) - (head-root-link (cdr (assoc :root-link head-info))) - (rarm-info (send self :make-arm-links :rarm - arm-radius upper-arm-length lower-arm-length shoulder-width hand-length - upper-arm-weight lower-arm-weight hand-weight)) - (rarm (cdr (assoc :links rarm-info))) - (rarm-end-coords (cdr (assoc :end-coords rarm-info))) - (rarm-root-link (cdr (assoc :root-link rarm-info))) - (larm-info (send self :make-arm-links :larm - arm-radius upper-arm-length lower-arm-length shoulder-width hand-length - upper-arm-weight lower-arm-weight hand-weight)) - (larm (cdr (assoc :links larm-info))) - (larm-end-coords (cdr (assoc :end-coords larm-info))) - (larm-root-link (cdr (assoc :root-link larm-info))) - (rleg-info (send self :make-leg-links :rleg - leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset - upper-leg-weight lower-leg-weight foot-weight)) - (rleg (cdr (assoc :links rleg-info))) - (rleg-end-coords (cdr (assoc :end-coords rleg-info))) - (rleg-root-link (cdr (assoc :root-link rleg-info))) - (lleg-info (send self :make-leg-links :lleg - leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset - upper-leg-weight lower-leg-weight foot-weight)) - (lleg (cdr (assoc :links lleg-info))) - (lleg-end-coords (cdr (assoc :end-coords lleg-info))) - (lleg-root-link (cdr (assoc :root-link lleg-info)))) + ;; 1. make links links and assoc all links + (let ((aroot-link (send self :make-root-link waist-length waist-weight))) + (setq torso (send self :make-torso-links torso-length torso-weight) + head (send self :make-head-links head-weight) + rarm (send self :make-arm-links :rarm + arm-radius upper-arm-length lower-arm-length shoulder-width hand-length + upper-arm-weight lower-arm-weight hand-weight) + larm (send self :make-arm-links :larm + arm-radius upper-arm-length lower-arm-length shoulder-width hand-length + upper-arm-weight lower-arm-weight hand-weight) + rleg (send self :make-leg-links :rleg + leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset + upper-leg-weight lower-leg-weight foot-weight) + lleg (send self :make-leg-links :lleg + leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset + upper-leg-weight lower-leg-weight foot-weight)) ;; arrange limbs (send (car rarm) :translate (float-vector 0 (- shoulder-width) (- torso-length 25)) :world) (send (car larm) :translate (float-vector 0 shoulder-width (- torso-length 25)) :world) @@ -163,16 +145,10 @@ jll0 jll1 jll2 jll3 jll4 jll5 jlr0 jlr1 jlr2 jlr3 jlr4 jlr5 )) - - ;; add limbs - (send self :add-limb :torso :links torso :end-coords torso-end-coords :root-link torso-root-link) - (send self :add-limb :head :links head :end-coords head-end-coords :root-link head-root-link) - (send self :add-limb :rarm :links rarm :end-coords rarm-end-coords :root-link rarm-root-link) - (send self :add-limb :larm :links larm :end-coords larm-end-coords :root-link larm-root-link) - (send self :add-limb :rleg :links rleg :end-coords rleg-end-coords :root-link rleg-root-link) - (send self :add-limb :lleg :links lleg :end-coords lleg-end-coords :root-link lleg-root-link) - ;; These are for robot-model. + (setq larm-root-link (car larm) rarm-root-link (car rarm) + lleg-root-link (car lleg) rleg-root-link (car rleg) + torso-root-link (car torso) head-root-link (car head)) (setq collision-avoidance-links (list aroot-link (elt torso 1) (elt larm 3) (elt rarm 3))) ;; set max torques @@ -190,23 +166,23 @@ ;; set sensors (setq force-sensors (mapcar #'(lambda (l) - (send (send self :limb l :end-coords :parent) :assoc - (make-cascoords :coords (send self :limb l :end-coords :copy-worldcoords) + (send (send self l :end-coords :parent) :assoc + (make-cascoords :coords (send self l :end-coords :copy-worldcoords) :name (format nil "~A-fsensor" (string-downcase l))))) (list :rarm :larm :rleg :lleg))) (setq imu-sensors (list (let ((sen (make-cascoords - :worldpos (send (car (send self :limb :torso :waist-p :child-link :bodies)) :centroid) + :worldpos (send (car (send self :torso :waist-p :child-link :bodies)) :centroid) :name "torso-imusensor"))) - (send (send self :limb :torso :waist-p :child-link) :assoc sen) + (send (send self :torso :waist-p :child-link) :assoc sen) sen))) (setq cameras (mapcar #'(lambda (pos name) (let ((sen (instance camera-model :init (make-cylinder 10 20) :name name))) (send sen :rotate pi/2 :y) (send sen :rotate -pi/2 :z) - (send sen :locate (v+ pos (send (car (send (send self :limb :head :neck-p :child-link) :bodies)) :centroid)) :world) - (send (send self :limb :head :neck-p :child-link) :assoc sen) + (send sen :locate (v+ pos (send (car (send (send self :head :neck-p :child-link) :bodies)) :centroid)) :world) + (send (send self :head :neck-p :child-link) :assoc sen) sen)) (list (float-vector 0 30 0) (float-vector 0 -30 0)) (list "left-camera" "right-camera"))) @@ -229,15 +205,13 @@ (send bc2 :set-color :green) (setq bc2 (instance bodyset-link :init (make-cascoords :pos #f(0 0 -12.5)) :bodies (list bc2) :name :torso-link1 :weight torso-weight)) (send bc1 :assoc bc2) - (list (cons :links (list bc1 bc2)) - (cons :root-link bc1)))) + (list bc1 bc2))) (:make-head-links (head-weight) (let ((bh0 (make-default-robot-link 0 50 :y :head-link0)) (bh2 (make-cube 120 100 150)) (bh2e (make-cylinder 10 30)) - (bh1) - (end-coords)) + (bh1)) (send bh2 :locate #f(0 0 80)) (send bh2 :set-color :green) (send bh2e :rotate pi/2 :y) @@ -245,12 +219,10 @@ (send bh2e :set-color :green) (send bh2 :assoc bh2e) (setq bh1 (instance bodyset-link :init (make-cascoords) :bodies (list bh2 bh2e) :name :head-link1 :weight head-weight)) - (setq end-coords (make-cascoords :pos #f(60 0 100) :rpy (float-vector 0 pi/2 0))) - (send bh1 :assoc end-coords) + (setq head-end-coords (make-cascoords :pos #f(60 0 100) :rpy (float-vector 0 pi/2 0))) + (send bh1 :assoc head-end-coords) (send bh0 :assoc bh1) - (list (cons :links (list bh0 bh1)) - (cons :root-link bh0) - (cons :end-coords end-coords)))) + (list bh0 bh1))) (:make-arm-links (l/r arm-radius upper-arm-length lower-arm-length shoulder-width hand-length upper-arm-weight lower-arm-weight hand-weight) @@ -260,22 +232,21 @@ (ba4 (make-default-robot-link lower-arm-length arm-radius :y (read-from-string (format nil "~A-link3" l/r)))) (ba5 (make-default-robot-link 0 arm-radius :z (read-from-string (format nil "~A-link4" l/r)))) (ba6 (make-default-robot-link 0 arm-radius :x (read-from-string (format nil "~A-link5" l/r)))) - (ba7 (make-default-robot-link hand-length arm-radius :y (read-from-string (format nil "~A-link6" l/r)))) - (end-coords)) + (ba7 (make-default-robot-link hand-length arm-radius :y (read-from-string (format nil "~A-link6" l/r))))) (send ba3 :weight upper-arm-weight) (send ba4 :weight lower-arm-weight) (send ba7 :weight hand-weight) (case l/r (:rarm - (setq end-coords (make-cascoords)) - (send end-coords :locate (float-vector 0 0 (- hand-length))) - (send end-coords :rotate pi/2 :y) - (send ba7 :assoc end-coords)) + (setq rarm-end-coords (make-cascoords)) + (send rarm-end-coords :locate (float-vector 0 0 (- hand-length))) + (send rarm-end-coords :rotate pi/2 :y) + (send ba7 :assoc rarm-end-coords)) (:larm - (setq end-coords (make-cascoords)) - (send end-coords :locate (float-vector 0 0 (- hand-length))) - (send end-coords :rotate pi/2 :y) - (send ba7 :assoc end-coords))) + (setq larm-end-coords (make-cascoords)) + (send larm-end-coords :locate (float-vector 0 0 (- hand-length))) + (send larm-end-coords :rotate pi/2 :y) + (send ba7 :assoc larm-end-coords))) (send ba6 :assoc ba7) (send ba5 :assoc ba6) (send ba5 :translate (float-vector 0 0 (- lower-arm-length)) :world) @@ -284,9 +255,7 @@ (send ba3 :assoc ba4) (send ba2 :assoc ba3) (send ba1 :assoc ba2) - (list (cons :links (list ba1 ba2 ba3 ba4 ba5 ba6 ba7)) - (cons :root-link ba1) - (cons :end-coords end-coords)))) + (list ba1 ba2 ba3 ba4 ba5 ba6 ba7))) (:make-leg-links (l/r leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset upper-leg-weight lower-leg-weight foot-weight) @@ -296,8 +265,7 @@ (bl4 (make-default-robot-link (- lower-leg-length (/ leg-radius 2.0)) leg-radius :y (read-from-string (format nil "~A-link3" l/r)))) (bl5 (make-default-robot-link 0 leg-radius :x (read-from-string (format nil "~A-link4" l/r)))) (bl6b (make-cube foot-depth foot-width foot-thickness)) - (bl6) - (end-coords)) + (bl6)) (send bl3 :weight upper-leg-weight) (send bl4 :weight lower-leg-weight) (send bl6b :locate (float-vector foot-offset 0 (- ankle-length))) @@ -306,13 +274,13 @@ (send bl6 :weight foot-weight) (case l/r (:rleg - (setq end-coords (make-cascoords)) - (send end-coords :locate (float-vector 0 0 (- (+ ankle-length (/ foot-thickness 2.0))))) - (send bl6 :assoc end-coords)) + (setq rleg-end-coords (make-cascoords)) + (send rleg-end-coords :locate (float-vector 0 0 (- (+ ankle-length (/ foot-thickness 2.0))))) + (send bl6 :assoc rleg-end-coords)) (:lleg - (setq end-coords (make-cascoords)) - (send end-coords :locate (float-vector 0 0 (- (+ ankle-length (/ foot-thickness 2.0))))) - (send bl6 :assoc end-coords))) + (setq lleg-end-coords (make-cascoords)) + (send lleg-end-coords :locate (float-vector 0 0 (- (+ ankle-length (/ foot-thickness 2.0))))) + (send bl6 :assoc lleg-end-coords))) (send bl5 :assoc bl6) (send bl5 :translate (float-vector 0 0 (- lower-leg-length)) :world) (send bl4 :assoc bl5) @@ -320,9 +288,7 @@ (send bl3 :assoc bl4) (send bl2 :assoc bl3) (send bl1 :assoc bl2) - (list (cons :links (list bl1 bl2 bl3 bl4 bl5 bl6)) - (cons :root-link bl1) - (cons :end-coords end-coords)))) + (list bl1 bl2 bl3 bl4 bl5 bl6))) (:reset-pose () (send self :angle-vector #f(0.0 0.0 0.0 0.0 10.0 20.0 0.0 -20.0 10.0 0.0 0.0 10.0 20.0 0.0 -20.0 10.0 0.0 0.0 0.0 0.0 -15.0 30.0 -15.0 0.0 0.0 0.0 -15.0 30.0 -15.0 0.0))) ) diff --git a/irteus/irtbvh.l b/irteus/irtbvh.l index 9a7ac057e..f463778c4 100644 --- a/irteus/irtbvh.l +++ b/irteus/irtbvh.l @@ -507,14 +507,14 @@ Other Sites are: (:copy-joint-to (robot limb joint &optional (sign 1)) (if (find-method robot (intern (format nil "~A-~A-R" (symbol-name limb) (symbol-name joint)) "KEYWORD")) - (send robot :limb limb (intern (format nil "~A-R" (symbol-name joint)) "KEYWORD") :joint-angle - (* sign (elt (send self :limb limb joint :joint :joint-angle) 2)))) + (send robot limb (intern (format nil "~A-R" (symbol-name joint)) "KEYWORD") :joint-angle + (* sign (elt (send self limb joint :joint :joint-angle) 2)))) (if (find-method robot (intern (format nil "~A-~A-P" (symbol-name limb) (symbol-name joint)) "KEYWORD")) - (send robot :limb limb (intern (format nil "~A-P" (symbol-name joint)) "KEYWORD") :joint-angle - (elt (send self :limb limb joint :joint :joint-angle) 0))) + (send robot limb (intern (format nil "~A-P" (symbol-name joint)) "KEYWORD") :joint-angle + (elt (send self limb joint :joint :joint-angle) 0))) (if (find-method robot (intern (format nil "~A-~A-Y" (symbol-name limb) (symbol-name joint)) "KEYWORD")) - (send robot :limb limb (intern (format nil "~A-Y" (symbol-name joint)) "KEYWORD") :joint-angle - (* sign (elt (send self :limb limb joint :joint :joint-angle) 1))))) + (send robot limb (intern (format nil "~A-Y" (symbol-name joint)) "KEYWORD") :joint-angle + (* sign (elt (send self limb joint :joint :joint-angle) 1))))) (:copy-state-to (robot) ;;(warning-message 2 ";; irteus copy-state-to in irtbvh.l robot:~A~%" robot) diff --git a/irteus/irtdyna.l b/irteus/irtdyna.l index 2cb953dff..cc9dee60d 100644 --- a/irteus/irtdyna.l +++ b/irteus/irtdyna.l @@ -1146,10 +1146,10 @@ :centroid-thre 0.1 :target-centroid-pos (v- (send self :centroid) (send df :refcog)))) (:move-base-pos - (let ((tc (mapcar #'(lambda (l) (send self :limb l :end-coords :copy-worldcoords)) '(:rleg :lleg)))) + (let ((tc (mapcar #'(lambda (l) (send self l :end-coords :copy-worldcoords)) '(:rleg :lleg)))) (send self :translate (float-vector (- (elt (send df :refcog) 0)) (- (elt (send df :refcog) 1)) 0) :world) (mapcar #'(lambda (l ttc) - (send self :limb l :inverse-kinematics ttc :thre 0.1 :rthre (deg2rad 0.1))) + (send self l :inverse-kinematics ttc :thre 0.1 :rthre (deg2rad 0.1))) '(:rleg :lleg) tc))) ) (push (list :angle-vector (send self :angle-vector) @@ -1732,9 +1732,9 @@ (send self :get-counter-footstep-limbs (car footstep-node-list))) (let ((current-support-limbs (car support-leg-list))) (send self :append-support-leg-coords-list - (mapcar #'(lambda (l) (send robot :limb l :end-coords :copy-worldcoords)) current-support-limbs)) + (mapcar #'(lambda (l) (send robot l :end-coords :copy-worldcoords)) current-support-limbs)) (send self :append-swing-leg-dst-coords-list - (mapcar #'(lambda (l) (send robot :limb l :end-coords :copy-worldcoords)) current-swing-limbs)) + (mapcar #'(lambda (l) (send robot l :end-coords :copy-worldcoords)) current-swing-limbs)) (let ((current-support-leg-coords (car support-leg-coords-list)) (current-swing-leg-dst-coords (car swing-leg-dst-coords-list))) (send self :append-refzmp-cur-list @@ -1758,7 +1758,7 @@ ) swing-leg-src-coords current-swing-leg-dst-coords refzmp-next (send self :get-limbs-zmp - (mapcar #'(lambda (l) (send robot :limb l :end-coords :copy-worldcoords)) (send self :get-counter-footstep-limbs (send-all (car footstep-node-list) :name))) + (mapcar #'(lambda (l) (send robot l :end-coords :copy-worldcoords)) (send self :get-counter-footstep-limbs (send-all (car footstep-node-list) :name))) (send self :get-counter-footstep-limbs (send-all (car footstep-node-list) :name))) refzmp-prev (car refzmp-cur-list)) (send self :make-gait-parameter) @@ -2019,7 +2019,7 @@ (dotimes (i (length legs)) (setq coords (midcoords (/ 1.0 (+ i 1.0)) coords - (send robot :limb (elt legs i) :end-coords :worldcoords)))) + (send robot (elt legs i) :end-coords :worldcoords)))) coords)) (xvr (send robot :rotate-vector robot-xv)) (xvf (send fcoords :rotate-vector #f(1 0 0)))) diff --git a/irteus/irtmodel.l b/irteus/irtmodel.l index ee860e6ca..51023e6b1 100644 --- a/irteus/irtmodel.l +++ b/irteus/irtmodel.l @@ -3127,7 +3127,7 @@ (list ,(cadr (member :move-target params))) ,(cadr (member :move-target params)))) (t - (mapcar #'(lambda (_limb) (send ,robot :limb _limb :end-coords)) ,limbs)))) + (mapcar #'(lambda (_limb) (send ,robot _limb :end-coords)) ,limbs)))) (,link-list (mapcar #'(lambda (_move-target) (send ,robot :link-list diff --git a/irteus/irtrobot.l b/irteus/irtrobot.l index 29751d214..f9f7559b7 100644 --- a/irteus/irtrobot.l +++ b/irteus/irtrobot.l @@ -178,6 +178,9 @@ (cons :root-link root-link) (cons :collision-avoidance-links collision-avoidance-links))))) (setq limb-list (append limb-list (list l))) + (unless (find-method self limb) + (eval `(defmethod ,(send (class self) :name) + (,limb (&rest args) (unless args (setq args (list nil))) (send* self :limb ,limb args))))) l)) (:remove-limb (limb) @@ -202,45 +205,45 @@ (:angle-vector (if args (progn - (mapcar #'(lambda (link a) - (send link :joint :joint-angle a)) - (send self :limb limb) (coerce (car args) cons)) - (send self :limb limb :angle-vector)) - (coerce (mapcar #'(lambda (link) (send link :joint :joint-angle)) - (send self :limb limb)) float-vector))) + (mapcar #'(lambda (l a) + (send l :joint :joint-angle a)) + (send self limb) (coerce (car args) cons)) + (send self limb :angle-vector)) + (coerce (mapcar #'(lambda (l) (send l :joint :joint-angle)) + (send self limb)) float-vector))) (:inverse-kinematics (let* ((link-list (if (memq :link-list args) (cadr (memq :link-list args)) (send self :link-list - (send self :limb limb :end-coords :parent) - (send self :limb limb :root-link)))) + (send self limb :end-coords :parent) + (send self limb :root-link)))) (collision-avoidance-link-pair (if (memq :collision-avoidance-link-pair args) (cadr (memq :collision-avoidance-link-pair args)) (send self :collision-avoidance-link-pair-from-link-list link-list - :collision-avoidance-links (send self :limb limb :collision-avoidance-links))))) + :collision-avoidance-links (send self limb :collision-avoidance-links))))) (send* self :inverse-kinematics (car args) :move-target (if (memq :move-target args) (cadr (memq :move-target args)) - (send self :limb limb :end-coords)) + (send self limb :end-coords)) :collision-avoidance-link-pair collision-avoidance-link-pair :link-list link-list (cdr args)))) (:move-end - (send* self :limb limb :inverse-kinematics args)) + (send* self limb :inverse-kinematics args)) (:move-end-rot - (let ((coords (send self :limb limb :end-coords :copy-worldcoords)) + (let ((coords (send self limb :end-coords :copy-worldcoords)) (angle (pop args)) (axis (pop args)) (wrt (pop args))) (unless wrt (setq wrt :local)) - (send* self :limb limb :move-end + (send* self limb :move-end (send coords :rotate (deg2rad angle) axis wrt) args))) (:move-end-pos - (let ((coords (send self :limb limb :end-coords :copy-worldcoords)) + (let ((coords (send self limb :end-coords :copy-worldcoords)) (pos (pop args)) (wrt (pop args))) (unless wrt (setq wrt :local)) - (send* self :limb limb :move-end (send coords :translate pos wrt) args))) + (send* self limb :move-end (send coords :translate pos wrt) args))) (:look-at - (if (send self :limb :head :links) (send* self :inverse-kinematics-loop-for-look-at limb args))) + (if (send self :head :links) (send* self :inverse-kinematics-loop-for-look-at limb args))) (:collision-avoidance-links (let ((collision-avoidance-links (cond (l (cdr (assoc :collision-avoidance-links l))) ((member limb '(:rarm :larm :rleg :lleg :head :torso)) ;; obsoleted. for backward compatibility @@ -261,7 +264,7 @@ (cond ((or (null method) (send bodyset-link :method method)) (if method - (send-all (send self :limb limb) method) + (send-all (send self limb) method) (cond (l (cdr (assoc :links l))) ((member limb '(:rarm :larm :rleg :lleg :head :torso)) ;; obsoleted. for backward compatibility (cdr (assoc (intern (string-upcase limb)) (send self :slots)))) @@ -275,24 +278,17 @@ )))) ) ;; case method )) ;; defmethod - (:limbs - () - (cond (limb-list (mapcar #'car limb-list)) - ((or rarm larm rleg lleg head torso) ;; obsoleted. for backward compatibility - (remove-if-not #'(lambda (l) (send self :limbs l)) - (list :rarm :larm :rleg :lleg :head :torso)) - (t nil)))) (:inverse-kinematics-loop-for-look-at (limb &rest args) (let* ((move-target (if (memq :move-target args) (cadr (memq :move-target args)) - (send self :limb limb :end-coords))) + (send self limb :end-coords))) (link-list (if (memq :link-list args) (cadr (memq :link-list args)) (send self :link-list (send move-target :parent) - (send self :limb limb :root-link)))) + (send self limb :root-link)))) (stop (if (memq :stop args) (cadr (memq :stop args)) 100)) (warnp (if (memq :warnp args) (cadr (memq :warnp args)) t)) dif-pos dif-rot p-dif-rot (count 0)) @@ -359,7 +355,7 @@ (limb &rest args) (cond ((memq :links args) - (sort (all-child-links (send self :limb limb :end-coords :parent)) #'(lambda (a b) (string< (string (send a :name)) (string (send b :name)))))) + (sort (all-child-links (send self limb :end-coords :parent)) #'(lambda (a b) (string< (string (send a :name)) (string (send b :name)))))) ((memq :joint-list args) (send-all (send self :gripper limb :links) :joint)) ((memq :angle-vector args) @@ -379,7 +375,7 @@ (find sensor-name sens :test #'equal :key #'(lambda (x) (send x :name))))) (:get-sensors-method-by-limb (sensors-type limb) - (remove-if-not #'(lambda (x) (member (send x :parent) (all-child-links (send self :limb limb :root-link)))) + (remove-if-not #'(lambda (x) (member (send x :parent) (all-child-links (send self limb :root-link)))) (send self sensors-type))) ;; sensor accessors (:force-sensors () "Returns force sensors." force-sensors) @@ -398,13 +394,13 @@ (unless args (setq args (list nil))) (send* self :limb :head args)) (:torso (&rest args) (unless args (setq args (list nil))) (send* self :limb :torso args)) - (:arms (&rest args) (list (send* self :limb :larm args) (send* self :limb :rarm args))) - (:legs (&rest args) (list (send* self :limb :lleg args) (send* self :limb :rleg args))) + (:arms (&rest args) (list (send* self :larm args) (send* self :rarm args))) + (:legs (&rest args) (list (send* self :lleg args) (send* self :rleg args))) (:look-at-hand (l/r) "look at hand position, l/r supports :rarm, :larm, :arms, and '(:rarm :larm)" (send self :look-at-target - (mapcar #'(lambda (x) (send self :limb x :end-coords)) + (mapcar #'(lambda (x) (send self x :end-coords)) (cond ((consp l/r) l/r) ((eq l/r :arms) '(:rarm :larm)) @@ -463,19 +459,19 @@ "move robot head to look at targets, look-at-target support t/nil float-vector coordinates, center of list of float-vector or list of coordinates" (cond ((float-vector-p look-at-target) - (send self :limb :head :look-at look-at-target)) + (send self :head :look-at look-at-target)) ((coordinates-p look-at-target) - (send self :limb :head :look-at (send look-at-target :worldpos))) + (send self :head :look-at (send look-at-target :worldpos))) ((and (consp look-at-target) (every #'identity (mapcar #'float-vector-p look-at-target))) - (send self :limb :head :look-at + (send self :head :look-at (scale (/ 1.0 (length look-at-target)) (reduce #'v+ look-at-target :initial-value #f(0 0 0))))) ((and (consp look-at-target) (every #'identity (mapcar #'coordinates-p look-at-target))) - (send self :limb :head :look-at + (send self :head :look-at (scale (/ 1.0 (length look-at-target)) (reduce #'v+ (send-all look-at-target :worldpos) :initial-value #f(0 0 0))))) ((null look-at-target)) - (t (send self :limb :head :look-at (send (car target-coords) :worldpos))) + (t (send self :head :look-at (send (car target-coords) :worldpos))) )) ;; init-pose (:init-pose () "Set robot to initial posture." (send self :angle-vector (instantiate float-vector (send self :calc-target-joint-dimension (cdr (send self :links)))))) @@ -488,7 +484,7 @@ (root-coords (send (car (send self :links)) :copy-worldcoords)) (calc-torque-buffer-args (send self :calc-torque-buffer-args)) (support-legs '(:rleg :lleg)) - (distribute-total-wrench-to-torque-method (if (and (some #'(lambda (l) (send self :limb l)) support-legs) ;; For legged robots + (distribute-total-wrench-to-torque-method (if (and (some #'(lambda (l) (send self l)) support-legs) ;; For legged robots (not (and force-list moment-list target-coords))) ;; If no force-list, moment-list, target-coords are specified :distribute-total-wrench-to-torque-method-default))) "Returns torque vector" @@ -511,19 +507,19 @@ ;; set default force & moment by solving mimimum internal forces (let ((target-coords)) (dolist (limb limbs) - (if (send self :limb limb) - (push (send self :limb limb :end-coords) target-coords))) + (if (send self limb) + (push (send self limb :end-coords) target-coords))) (let* ((ret-fm (send self :calc-contact-wrenches-from-total-wrench (send-all target-coords :worldpos) :total-wrench (concatenate float-vector (send (car (send self :links)) :force) (send (car (send self :links)) :moment))))) (send self :calc-torque-from-ext-wrenches :force-list (car ret-fm) :moment-list (cadr ret-fm) :target-coords target-coords) ))) (:calc-force-from-joint-torque - (limb all-torque &key (move-target (send self :limb limb :end-coords)) (use-torso)) + (limb all-torque &key (move-target (send self limb :end-coords)) (use-torso)) "Calculates end-effector force and moment from joint torques." (let* ((link-list (send self :link-list (send move-target :parent) - (unless use-torso (car (send self :limb limb :links))))) + (unless use-torso (car (send self limb :links))))) (jacobian (send self :calc-jacobian-from-link-list link-list @@ -588,10 +584,10 @@ )) (:joint-angle-limit-nspace-for-6dof (&key (avoid-nspace-gain 0.01) (limbs '(:rleg :lleg))) - (let* ((ll (mapcar #'(lambda (l) (send self :limb l :links)) limbs)) + (let* ((ll (mapcar #'(lambda (l) (send self l :links)) limbs)) (J (send self :calc-jacobian-from-link-list (mapcar #'(lambda (l) (append (list (car (send self :links))) l)) ll) - :move-target (mapcar #'(lambda (l) (send self :limb l :end-coords)) limbs) + :move-target (mapcar #'(lambda (l) (send self l :end-coords)) limbs) :translation-axis (make-list (length limbs) :initial-element t) :rotation-axis (make-list (length limbs) :initial-element t))) (Jb (make-matrix (array-dimension J 0) 6)) @@ -613,7 +609,7 @@ ))) (:joint-order (limb &optional jname-list) - (let ((joint-list (mapcar #'(lambda (x) (cons x (find-if #'(lambda (y) (eq y (send self x))) (send self :limb limb :joint-list)))) (mapcan #'(lambda (x) (if (substringp (format nil "~A-" (symbol-name limb)) (string x)) (list x)))(send self :methods)))) + (let ((joint-list (mapcar #'(lambda (x) (cons x (find-if #'(lambda (y) (eq y (send self x))) (send self limb :joint-list)))) (mapcan #'(lambda (x) (if (substringp (format nil "~A-" (symbol-name limb)) (string x)) (list x)))(send self :methods)))) j (result) name) (unless jname-list (setq jname-list @@ -645,12 +641,12 @@ (let ((tmp-limbs (remove-duplicates (mapcar #'(lambda (l) - (find-if #'(lambda (tmp-limb) (member l (send self :limb tmp-limb :links))) (send self :limbs))) + (find-if #'(lambda (tmp-limb) (member l (send self tmp-limb :links))) (append (list :rarm :larm :rleg :lleg :torso :head) (mapcar #'car limb-list)))) (send-all (send self :joint-list) :child-link)))) (ordered-joint-list (send self :joint-list))) (format t " #f(~% ") (dolist (tmp-limb tmp-limbs) - (dolist (j (send self :limb tmp-limb :joint-list)) + (dolist (j (send self tmp-limb :joint-list)) (format t "~6,2f " (elt vec (position j ordered-joint-list)))) (format t "~% ")) (format t ")~%") @@ -663,11 +659,11 @@ (limbs (if (send self :force-sensors) (remove nil (mapcar #'(lambda (fs) - (find-if #'(lambda (l) (equal fs (car (send self :limb l :force-sensors)))) '(:rleg :lleg))) + (find-if #'(lambda (l) (equal fs (car (send self l :force-sensors)))) '(:rleg :lleg))) (send self :force-sensors))) '(:rleg :lleg))) (force-sensors (mapcar #'(lambda (l) (send self :force-sensor l)) limbs)) - (cop-coords (mapcar #'(lambda (l) (send self :limb l :end-coords)) limbs)) + (cop-coords (mapcar #'(lambda (l) (send self l :end-coords)) limbs)) (fz-thre 1e-3) ;; [N] (limb-cop-fz-list (mapcar #'(lambda (fs f m cc) @@ -714,9 +710,9 @@ (setq legs (list legs))) (setq leg-coords (remove nil (mapcar #'(lambda (leg) (cond - ((eq leg :left) (send self :limb :lleg :end-coords)) - ((eq leg :right) (send self :limb :rleg :end-coords)) - (t (send self :limb leg :end-coords)))) + ((eq leg :left) (send self :lleg :end-coords)) + ((eq leg :right) (send self :rleg :end-coords)) + (t (send self leg :end-coords)))) legs))) (when (null leg-coords) (return-from :fix-leg-to-coords nil)) ;; leged robot is assumed. @@ -743,9 +739,9 @@ (let* ((legs (cond ((listp leg) leg) ((eq leg :both) '(:rleg :lleg)) (t (list leg)))) (leg-coords (remove nil (mapcar #'(lambda (leg) (cond - ((eq leg :left) (send self :limb :lleg :end-coords)) - ((eq leg :right) (send self :limb :rleg :end-coords)) - (t (send self :limb leg :end-coords)))) + ((eq leg :left) (send self :lleg :end-coords)) + ((eq leg :right) (send self :rleg :end-coords)) + (t (send self leg :end-coords)))) legs)))) (scale (/ 1.0 (length leg-coords)) (reduce #'v+ (mapcar #'(lambda (leg) (send leg :worldpos)) leg-coords))))) @@ -753,9 +749,9 @@ (let* ((limbs (if (listp fix-limbs) fix-limbs (list fix-limbs)))) (send-all (remove nil (mapcar #'(lambda (limb) (cond - ((eq limb :left) (send self :limb :lleg :end-coords)) - ((eq limb :right) (send self :limb :rleg :end-coords)) - (t (send self :limb limb :end-coords)))) + ((eq limb :left) (send self :lleg :end-coords)) + ((eq limb :right) (send self :rleg :end-coords)) + (t (send self limb :end-coords)))) limbs)) :copy-worldcoords))) (root-link-virtual-joint-weight #f(0.1 0.1 0.0 0.0 0.0 0.5)) ;; use only translation x, y and rotation z &allow-other-keys) @@ -781,7 +777,7 @@ ((:all-limbs al) (if (send self :force-sensors) (remove nil (mapcar #'(lambda (fs) - (find-if #'(lambda (l) (equal fs (car (send self :limb l :force-sensors)))) '(:rleg :lleg))) + (find-if #'(lambda (l) (equal fs (car (send self l :force-sensors)))) '(:rleg :lleg))) (send self :force-sensors))) '(:rleg :lleg))) ((:default-zmp-offsets dzo) @@ -844,7 +840,7 @@ (dotimes (i (length al)) (setq fcoords (midcoords (/ 1.0 (+ i 1.0)) fcoords - (send self :limb (elt al i) :end-coords :worldcoords)))) + (send self (elt al i) :end-coords :worldcoords)))) (send self :inverse-rotate-vector (send fcoords :rotate-vector #F(1 0 0))))))) ;; initial move centroid on foot (send gg :initialize-gait-parameter @@ -866,7 +862,7 @@ (send self :angle-vector) (send (car (send self :links)) :copy-worldcoords) :dt dt :pZMPz (elt (elt ret 5) 2)))) - (end-coords-list (mapcar #'(lambda (x) (send self :limb x :end-coords :copy-worldcoords)) (gg . all-limbs))) + (end-coords-list (mapcar #'(lambda (x) (send self x :end-coords :copy-worldcoords)) (gg . all-limbs))) (contact-state (elt ret 8)) ) (when debug-view @@ -934,18 +930,18 @@ (rleg_ (car legs)) (lleg_ (cadr legs))) (send self :reset-pose) (send self :fix-leg-to-coords (make-coords) legs) - (let ((dol (abs (elt (apply #'v- (mapcar #'(lambda (l) (send self :limb l :end-coords :worldpos)) legs)) 1)))) + (let ((dol (abs (elt (apply #'v- (mapcar #'(lambda (l) (send self l :end-coords :worldpos)) legs)) 1)))) (labels ((ik-test (target-coords-func diff-func &optional (limit-func)) (send self :reset-pose) (send self :fix-leg-to-coords (make-coords) legs) - (let* ((tc (send self :limb rleg_ :end-coords :copy-worldcoords)) + (let* ((tc (send self rleg_ :end-coords :copy-worldcoords)) (init-tc (send tc :copy-worldcoords))) (while (and (send self :inverse-kinematics (funcall target-coords-func tc) - :link-list (send self :link-list (send self :limb rleg_ :end-coords :parent)) - :move-target (send self :limb rleg_ :end-coords) :warnp nil) + :link-list (send self :link-list (send self rleg_ :end-coords :parent)) + :move-target (send self rleg_ :end-coords) :warnp nil) (if limit-func (funcall limit-func tc init-tc) t) )) (funcall diff-func tc init-tc)))) @@ -995,7 +991,7 @@ (leg (if (eps= (float yy) 0.0) (if (> th 0.0) lleg_ rleg_) (if (> yy 0.0) lleg_ rleg_))) - (mc (apply #'midcoords 0.5 (mapcar #'(lambda (l) (send self :limb l :end-coords :copy-worldcoords)) legs))) + (mc (apply #'midcoords 0.5 (mapcar #'(lambda (l) (send self l :end-coords :copy-worldcoords)) legs))) (gc (send (make-coords :pos (float-vector xx yy 0) :rpy (float-vector (deg2rad th) 0 0)) :transform mc :world)) (leg-translate-pos (mapcar #'(lambda (l) @@ -1052,26 +1048,26 @@ ;; calculate foot step list for quad walk (dolist (fs biped-fsl) (let* ((fs-leg-name (send fs :name)) - (offset (send (send self :limb fs-leg-name :end-coords :copy-worldcoords) :transformation fs :world)) + (offset (send (send self fs-leg-name :end-coords :copy-worldcoords) :transformation fs :world)) (target-arm-name)) (case type (:crawl (dolist (l (list fs-leg-name (cond ((eq fs-leg-name lleg_) larm_) ((eq fs-leg-name rleg_) rarm_)))) - (push (list (make-coords :coords (send (send self :limb l :end-coords :copy-worldcoords) :transform offset :world) :name l)) + (push (list (make-coords :coords (send (send self l :end-coords :copy-worldcoords) :transform offset :world) :name l)) quadruped-fsl))) (:trot (push (mapcar #'(lambda (l) - (make-coords :coords (send (send self :limb l :end-coords :copy-worldcoords) :transform offset :world) :name l)) + (make-coords :coords (send (send self l :end-coords :copy-worldcoords) :transform offset :world) :name l)) (list fs-leg-name (setq target-arm-name (cond ((eq fs-leg-name lleg_) rarm_) ((eq fs-leg-name rleg_) larm_))))) quadruped-fsl)) (:pace (push (mapcar #'(lambda (l) - (make-coords :coords (send (send self :limb l :end-coords :copy-worldcoords) :transform offset :world) :name l)) + (make-coords :coords (send (send self l :end-coords :copy-worldcoords) :transform offset :world) :name l)) (list fs-leg-name (cond ((eq fs-leg-name lleg_) larm_) ((eq fs-leg-name rleg_) rarm_)))) quadruped-fsl)) (:gallop (push (mapcar #'(lambda (l) - (make-coords :coords (send (send self :limb l :end-coords :copy-worldcoords) :transform offset :world) :name l)) + (make-coords :coords (send (send self l :end-coords :copy-worldcoords) :transform offset :world) :name l)) (list (cond ((eq fs-leg-name rleg_) rleg_) ((eq fs-leg-name lleg_) rarm_)) (cond ((eq fs-leg-name rleg_) lleg_) ((eq fs-leg-name lleg_) larm_)))) quadruped-fsl)) @@ -1114,11 +1110,11 @@ (if (and l (send l :descendants)) (append (list l) (flatten (mapcar #'all-descendant-links (remove-if-not #'(lambda (x) (derivedp x bodyset-link)) (send l :descendants))))) ))) - (unless (send self :limb name :links) (return-from :make-sole-polygon nil)) - (send-all (all-descendant-links (car (send self :limb name :links))) :worldcoords) + (unless (send self name :links) (return-from :make-sole-polygon nil)) + (send-all (all-descendant-links (car (send self name :links))) :worldcoords) (let* ((target-nm (read-from-string (format nil "~A-sole-body" name))) (vs (remove-duplicates - (flatten (send-all (flatten (send-all (all-descendant-links (car (send self :limb name :links))) :bodies)) :vertices)) + (flatten (send-all (flatten (send-all (all-descendant-links (car (send self name :links))) :bodies)) :vertices)) :test #'(lambda (x y) (eps-v= x y *epsilon*)))) (min-vs (find-extream vs #'(lambda (x) (elt x 2)) #'<)) (b (send @@ -1127,7 +1123,7 @@ #'(lambda (p) (< 5.0 (- (elt p 2) (elt min-vs 2)))) vs)) ;; 5.0 is margin :body))) - (send (send self :limb name :end-coords :parent) :assoc b) + (send (send self name :end-coords :parent) :assoc b) (send self :put target-nm b) (send b :worldcoords) (let ((f (find-if #'(lambda (x) (memq :bottom (send x :id))) (send b :faces)))) @@ -1196,7 +1192,7 @@ (:calc-static-balance-point (&key (target-points (mapcar #'(lambda (tmp-arm) - (send (send self :limb tmp-arm :end-coords) :worldpos)) + (send (send self tmp-arm :end-coords) :worldpos)) '(:rarm :larm))) (force-list (make-list (length target-points) :initial-element (float-vector 0 0 0))) (moment-list (make-list (length target-points) :initial-element (float-vector 0 0 0)))