Menu
▾
▴
[Euslisp-commit] SF.net SVN: euslisp:[670] trunk/EusLisp/models/darwin.l
|
From: <sn...@us...> - 2014-01-31 11:02:49
|
Revision: 670
http://sourceforge.net/p/euslisp/code/670
Author: snozawa
Date: 2014-01-31 11:02:39 +0000 (Fri, 31 Jan 2014)
Log Message:
-----------
add darwin euslisp model converted from ... darwin VRML -> darwin collada -> darwin euslisp
Added Paths:
-----------
trunk/EusLisp/models/darwin.l
Added: trunk/EusLisp/models/darwin.l
===================================================================
--- trunk/EusLisp/models/darwin.l (rev 0)
+++ trunk/EusLisp/models/darwin.l 2014-01-31 11:02:39 UTC (rev 670)
@@ -0,0 +1,3266 @@
+;;
+;; DO NOT EDIT THIS FILE
+;;
+;; this file is automatically generated from /home/nozawa/ros/groovy/jsk-ros-pkg-unreleased/jsk_robot_common/jsk_hrpsys_ros_bridge/models/DARWIN.dae on (Linux nozawa-ThinkPad-T430 3.2.0-58-generic x86_64) at Fri Jan 31 19:17:25 2014
+
+;;
+;; /home/nozawa/ros/groovy/jsk-ros-pkg-unreleased/jsk_robot_common/jsk_hrpsys_ros_bridge/build $ /home/nozawa/ros/groovy/jsk-ros-pkg/jsk_model_tools/euscollada/bin/collada2eus /home/nozawa/ros/groovy/jsk-ros-pkg-unreleased/jsk_robot_common/jsk_hrpsys_ros_bridge/models/DARWIN.dae /home/nozawa/ros/groovy/jsk-ros-pkg-unreleased/jsk_robot_common/jsk_hrpsys_ros_bridge/models/darwin.yaml /home/nozawa/ros/groovy/jsk-ros-pkg-unreleased/jsk_robot_common/jsk_hrpsys_ros_bridge/models/darwin.l
+;;
+
+;; copy euscollada-robot class definition from euscollada/src/euscollada-robot.l
+;;
+(defclass euscollada-robot
+;; This euscollada-robot class is for robots converted from collada files.
+;; This class provides :init-ending override.
+ :super robot-model
+ :slots ()
+ )
+(defmethod euscollada-robot
+ (:init-ending
+ ()
+ ;; fix all links' mass properties ;; root link relative (collada) -> link local (euslisp)
+ (dolist (l links)
+ (setq (l . acentroid) (send l :inverse-transform-vector
+ (send (send (car links) :worldcoords) :transform-vector
+ (l . acentroid))))
+ (setq (l . inertia-tensor) (m* (transpose (send l :worldrot))
+ (m* (send (car links) :worldrot)
+ (l . inertia-tensor))))
+ )
+ (send-super :init-ending)
+ (dolist (l (send self :links)) (send self :make-detail-collision-model-from-glvertices-for-one-link l))
+
+ ;;
+ (dolist (j (mapcan #'(lambda (x) (if (and (derivedp (cdr x) joint)
+ (not (memq (cdr x) (send self :joint-list))))
+ (list (cdr x)))) (send self :slots)))
+ (send (send j :child-link) :add-joint j)
+ (send (send j :child-link) :add-parent-link (send j :parent-link))
+ (send (send j :parent-link) :add-child-links (send j :child-link)))
+ ;; add sensor method ;; e.g., (send self :camera 0), (send self :force-sensor :rasensor), ... etc
+ (dolist (sensor-name '(:force-sensor :imu-sensor)) (send self :define-get-sensor-method sensor-name))
+ )
+ (:define-get-sensor-method
+ (sensor-name)
+ (eval `(defmethod ,(send (class self) :name)
+ (,sensor-name (&rest args)
+ (cond ((integerp (car args))
+ (forward-message-to (elt (send self ,(read-from-string (format nil "~As" sensor-name)) ) (car args)) (cdr args)))
+ ;; enable to access sensors by limb name
+ ((memq (car args) '(:larm :rarm :lleg :rleg :head :torso))
+ (find-if
+ #'(lambda (x) (member (send x :parent) (send self (car args) :links)))
+ (send self ,(read-from-string (format nil "~As" sensor-name)))))
+ ((and (keywordp (car args))
+ (derivedp (send self (car args)) cascaded-coords))
+ (send* self args))
+ ((keywordp (car args))
+ ;;(warn ";; no such sensor ~A~%" (car args))
+ nil)
+ (t
+ (forward-message-to (car (send self ,(read-from-string (format nil "~As" sensor-name)))) args)
+ )))
+ )))
+ ;; fullbody-inverse-kinematics overwrite
+ ;; reduce root-link's weight based on leg's joint limit
+ ;; increase stop and cog-gain
+ (:fullbody-inverse-kinematics
+ (target-coords &rest args &key (stop 250) (cog-gain 2.0) (additional-weight-list) &allow-other-keys)
+ (let* ((leg-ul (send self :calc-union-link-list (send self :legs :links))))
+ (send self :reset-joint-angle-limit-weight-old leg-ul)
+ (send-message* self robot-model :fullbody-inverse-kinematics
+ target-coords
+ :stop stop :cog-gain cog-gain
+ :additional-weight-list
+ (append
+ additional-weight-list
+ (list
+ (list (car (send self :links))
+ #'(lambda ()
+ ;; set root-link 6dof-joint's weight based on legs' joint limit
+ (let* ((min-weight
+ (reduce #'(lambda (x y) (min x y))
+ (coerce (send self :calc-inverse-kinematics-weight-from-link-list
+ leg-ul :union-link-list leg-ul) cons))))
+ (fill (instantiate float-vector 6) min-weight)))
+ )
+ ))
+ args)
+ ))
+ ;; make collision model from gl-vertices
+ (:make-detail-collision-model-from-glvertices-for-one-link
+ (ll &key (fat 0) (collision-func 'pqp-collision-check))
+ (unless (send ll :get (read-from-string (format nil ":~Amodel"
+ (string-right-trim "-COLLISION-CHECK" (string collision-func)))))
+ (send-message ll cascaded-coords
+ (read-from-string
+ (format nil ":make-~Amodel"
+ (string-right-trim "-COLLISION-CHECK" (string collision-func))))
+ :fat fat
+ :faces (flatten (mapcar #'(lambda (x)
+ (cond
+ ((derivedp x collada-body)
+ (send (x . glvertices) :convert-to-faces :wrt :world))
+ (t
+ (send x :faces))))
+ (send ll :bodies)))))
+ )
+ )
+
+;; copy euscollada-body class definition from euscollada/src/euscollada-robot.l
+(defclass collada-body
+ :super body
+ :slots (glvertices)
+ )
+(defmethod collada-body
+ (:draw (vwr)
+ (when glvertices
+ (send glvertices :draw vwr)))
+ (:set-color (&rest args)
+ (send-super* :set-color args)
+ (when glvertices (send* glvertices :set-color args)))
+ )
+;;
+(defun DARWIN () (setq *DARWIN* (instance DARWIN-robot :init)))
+
+(defclass DARWIN-robot
+ :super euscollada-robot
+ :slots (NECK_jt HEADL_jt PELVYL_jt PELVL_jt LEGUPPERL_jt LEGLOWERL_jt ANKLEL_jt FOOTL_jt PELVYR_jt PELVR_jt LEGUPPERR_jt LEGLOWERR_jt ANKLER_jt FOOTR_jt SHOULDERL_jt ARMUPPERL_jt ARMLOWERL_jt SHOULDERR_jt ARMUPPERR_jt ARMLOWERR_jt HEAD_LINK_lk NECK_LINK_lk FOOTL_LINK_lk ANKLEL_LINK_lk LEGLOWERL_LINK_lk LEGUPPERL_LINK_lk PELVL_LINK_lk PELVYL_LINK_lk FOOTR_LINK_lk ANKLER_LINK_lk LEGLOWERR_LINK_lk LEGUPPERR_LINK_lk PELVR_LINK_lk PELVYR_LINK_lk ARMLOWERL_LINK_lk ARMUPPERL_LINK_lk SHOULDERL_LINK_lk ARMLOWERR_LINK_lk ARMUPPERR_LINK_lk SHOULDERR_LINK_lk BODY_LINK_lk gsensor-sensor-coords gyrometer-sensor-coords Camera-sensor-coords ))
+(defmethod DARWIN-robot
+ (:init
+ (&rest args)
+ (let ()
+ (send-super* :init :name "DARWIN" args)
+
+ ;; node id=v1.node2, name=HEAD_LINK, sid=node2
+ (let ( b_g1_2_geom0 b_g1_2_geom1 b_g1_2_geom2 b_g1_2_geom3 b_g1_2_geom4)
+ ;; define bodyset-link for HEAD_LINK : v1.node2
+ (setq b_g1_2_geom0 (instance DARWIN_g1_2_geom0 :init))
+
+ ;; writeTransform(name=b_g1_2_geom0,domNode=HEAD_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_2_geom0 :transform localcds :world)
+ )
+ (setq b_g1_2_geom1 (instance DARWIN_g1_2_geom1 :init))
+
+ ;; writeTransform(name=b_g1_2_geom1,domNode=HEAD_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_2_geom1 :transform localcds :world)
+ )
+ (setq b_g1_2_geom2 (instance DARWIN_g1_2_geom2 :init))
+
+ ;; writeTransform(name=b_g1_2_geom2,domNode=HEAD_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_2_geom2 :transform localcds :world)
+ )
+ (setq b_g1_2_geom3 (instance DARWIN_g1_2_geom3 :init))
+
+ ;; writeTransform(name=b_g1_2_geom3,domNode=HEAD_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_2_geom3 :transform localcds :world)
+ )
+ (setq b_g1_2_geom4 (instance DARWIN_g1_2_geom4 :init))
+
+ ;; writeTransform(name=b_g1_2_geom4,domNode=HEAD_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_2_geom4 :transform localcds :world)
+ )
+ (send b_g1_2_geom0 :assoc b_g1_2_geom1)
+ (send b_g1_2_geom0 :assoc b_g1_2_geom2)
+ (send b_g1_2_geom0 :assoc b_g1_2_geom3)
+ (send b_g1_2_geom0 :assoc b_g1_2_geom4)
+ (setq HEAD_LINK_lk
+ (instance bodyset-link
+ :init (make-cascoords)
+ :bodies (list b_g1_2_geom0 b_g1_2_geom1 b_g1_2_geom2 b_g1_2_geom3 b_g1_2_geom4)
+ :name "HEAD_LINK"))
+ (send HEAD_LINK_lk :weight 158.042)
+ (let ((tmp-c-list (list
+ (make-coords :pos (float-vector 0.0000000000000000e+00 5.1000000000000000e+01 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector 6.3919799999999999e-02 1.8564500000000002e+01 7.6666200000000000e+00) :rot (matrix-exponent (scale 2.6417336902212489e+00 (float-vector 6.7167945353074188e-01 6.6302432332952410e-01 -3.3052300733545109e-01))))
+ ))
+ (tmp-c (make-coords)))
+ (dolist (cc tmp-c-list)
+ (setq tmp-c (send tmp-c :transform cc)))
+ (setq (HEAD_LINK_lk . inertia-tensor)
+ (m* (m* (send tmp-c :worldrot) (diagonal (float-vector 1.0474191171695740e+05 1.2396566143164740e+05 1.2929242685139530e+05))) (transpose (send tmp-c :worldrot))))
+ (setq (HEAD_LINK_lk . acentroid) (send tmp-c :worldpos))
+ )
+
+ ;; writeTransform(name=HEAD_LINK_lk,domNode=HEAD_LINK,targetCount=0,parent=:local), translateCount=1, rotateCount=2, matrixCount=0
+ (send HEAD_LINK_lk :transform
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)) :local)
+ ;;1;
+ (setq Camera-sensor-coords (make-cascoords :name "Camera" :coords (send HEAD_LINK_lk :copy-worldcoords)))
+ (send Camera-sensor-coords :put :sensor-type :base_pinhole_camera)
+ (send Camera-sensor-coords :put :sensor-id 3)
+ (send Camera-sensor-coords :transform (make-coords :pos #f( 0.0000000000000000e+00 3.2907400000000003e+01 3.5981600000000000e+01) :axis (let ((tmp-axis (float-vector 0.0000000000000000e+00 -1.0000000000000000e+00 0.0000000000000000e+00))) (if (eps= (norm tmp-axis) 0.0) (float-vector 1 0 0) tmp-axis)) :angle 3.1415899999999999e+00))
+ (send HEAD_LINK_lk :assoc Camera-sensor-coords)
+ )
+
+ ;; node id=v1.node1, name=NECK_LINK, sid=node1
+ (let ( b_g1_1_geom0)
+ ;; define bodyset-link for NECK_LINK : v1.node1
+ (setq b_g1_1_geom0 (instance DARWIN_g1_1_geom0 :init))
+
+ ;; writeTransform(name=b_g1_1_geom0,domNode=NECK_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_1_geom0 :transform localcds :world)
+ )
+ (setq NECK_LINK_lk
+ (instance bodyset-link
+ :init (make-cascoords)
+ :bodies (list b_g1_1_geom0)
+ :name "NECK_LINK"))
+ (send NECK_LINK_lk :weight 24.358)
+ (let ((tmp-c-list (list
+ (make-coords :pos (float-vector 0.0000000000000000e+00 5.1000000000000000e+01 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 1.4242800000000000e+00 -1.6567599999999999e+01 -7.1281100000000008e-01) :rot (matrix-exponent (scale 3.1377311432741881e+00 (float-vector -9.0408980651002557e-02 9.9545007233138982e-01 3.0089362124802201e-02))))
+ ))
+ (tmp-c (make-coords)))
+ (dolist (cc tmp-c-list)
+ (setq tmp-c (send tmp-c :transform cc)))
+ (setq (NECK_LINK_lk . inertia-tensor)
+ (m* (m* (send tmp-c :worldrot) (diagonal (float-vector 5.0356695238550246e+03 8.2854422988662136e+03 1.1391364177278760e+04))) (transpose (send tmp-c :worldrot))))
+ (setq (NECK_LINK_lk . acentroid) (send tmp-c :worldpos))
+ )
+
+ ;; writeTransform(name=NECK_LINK_lk,domNode=NECK_LINK,targetCount=0,parent=:local), translateCount=1, rotateCount=2, matrixCount=0
+ (send NECK_LINK_lk :transform
+ (make-coords :pos (float-vector 0.0000000000000000e+00 5.1000000000000000e+01 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00)) :local)
+ ;;1;
+
+ ;; writeTransform(name=HEAD_LINK_lk,domNode=NECK_LINK,targetCount=0,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send localcds :transform
+ (make-coords :pos (float-vector 0.0000000000000000e+00 5.1000000000000000e+01 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00)) :local)
+ (send HEAD_LINK_lk :transform localcds :world)
+ )
+ ;;2;
+ (send NECK_LINK_lk :assoc HEAD_LINK_lk)
+ )
+
+ ;; node id=v1.node8, name=FOOTL_LINK, sid=node8
+ (let ( b_g1_8_geom0 b_g1_8_geom1)
+ ;; define bodyset-link for FOOTL_LINK : v1.node8
+ (setq b_g1_8_geom0 (instance DARWIN_g1_8_geom0 :init))
+
+ ;; writeTransform(name=b_g1_8_geom0,domNode=FOOTL_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_8_geom0 :transform localcds :world)
+ )
+ (setq b_g1_8_geom1 (instance DARWIN_g1_8_geom1 :init))
+
+ ;; writeTransform(name=b_g1_8_geom1,domNode=FOOTL_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_8_geom1 :transform localcds :world)
+ )
+ (send b_g1_8_geom0 :assoc b_g1_8_geom1)
+ (setq FOOTL_LINK_lk
+ (instance bodyset-link
+ :init (make-cascoords)
+ :bodies (list b_g1_8_geom0 b_g1_8_geom1)
+ :name "FOOTL_LINK"))
+ (send FOOTL_LINK_lk :weight 79.446)
+ (let ((tmp-c-list (list
+ (make-coords :pos (float-vector 3.7000000000000000e+01 -1.2220000000000000e+02 -5.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 -1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector -1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector -0.0000000000000000e+00 -9.3000000000000000e+01 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector -1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 -9.3000000000000000e+01 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 9.5058799999999994e+00 -2.5995300000000000e+01 -5.0287700000000002e-01) :rot (matrix-exponent (scale 3.1374881783494764e+00 (float-vector 6.3355814941524424e-02 9.9794679653010854e-01 9.3931894684097673e-03))))
+ ))
+ (tmp-c (make-coords)))
+ (dolist (cc tmp-c-list)
+ (setq tmp-c (send tmp-c :transform cc)))
+ (setq (FOOTL_LINK_lk . inertia-tensor)
+ (m* (m* (send tmp-c :worldrot) (diagonal (float-vector 6.8003221353508503e+04 8.8232016045823475e+04 3.5821522600668002e+04))) (transpose (send tmp-c :worldrot))))
+ (setq (FOOTL_LINK_lk . acentroid) (send tmp-c :worldpos))
+ )
+
+ ;; writeTransform(name=FOOTL_LINK_lk,domNode=FOOTL_LINK,targetCount=0,parent=:local), translateCount=1, rotateCount=2, matrixCount=0
+ (send FOOTL_LINK_lk :transform
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00)) :local)
+ ;;1;
+ )
+
+ ;; node id=v1.node7, name=ANKLEL_LINK, sid=node7
+ (let ( b_g1_7_geom0)
+ ;; define bodyset-link for ANKLEL_LINK : v1.node7
+ (setq b_g1_7_geom0 (instance DARWIN_g1_7_geom0 :init))
+
+ ;; writeTransform(name=b_g1_7_geom0,domNode=ANKLEL_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_7_geom0 :transform localcds :world)
+ )
+ (setq ANKLEL_LINK_lk
+ (instance bodyset-link
+ :init (make-cascoords)
+ :bodies (list b_g1_7_geom0)
+ :name "ANKLEL_LINK"))
+ (send ANKLEL_LINK_lk :weight 167.108)
+ (let ((tmp-c-list (list
+ (make-coords :pos (float-vector 3.7000000000000000e+01 -1.2220000000000000e+02 -5.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 -1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector -1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector -0.0000000000000000e+00 -9.3000000000000000e+01 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector -1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 -9.3000000000000000e+01 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector -2.1373200000000001e-01 1.3873099999999999e+01 -1.8536100000000001e+01) :rot (matrix-exponent (scale 3.0379497570860514e+00 (float-vector -9.9932043867120068e-01 3.6659460910665437e-02 3.8398931936168461e-03))))
+ ))
+ (tmp-c (make-coords)))
+ (dolist (cc tmp-c-list)
+ (setq tmp-c (send tmp-c :transform cc)))
+ (setq (ANKLEL_LINK_lk . inertia-tensor)
+ (m* (m* (send tmp-c :worldrot) (diagonal (float-vector 1.2106265191898300e+05 1.0967651340802430e+05 4.1131904672992692e+04))) (transpose (send tmp-c :worldrot))))
+ (setq (ANKLEL_LINK_lk . acentroid) (send tmp-c :worldpos))
+ )
+
+ ;; writeTransform(name=ANKLEL_LINK_lk,domNode=ANKLEL_LINK,targetCount=0,parent=:local), translateCount=1, rotateCount=2, matrixCount=0
+ (send ANKLEL_LINK_lk :transform
+ (make-coords :pos (float-vector 0.0000000000000000e+00 -9.3000000000000000e+01 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)) :local)
+ ;;1;
+
+ ;; writeTransform(name=FOOTL_LINK_lk,domNode=ANKLEL_LINK,targetCount=0,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send localcds :transform
+ (make-coords :pos (float-vector 0.0000000000000000e+00 -9.3000000000000000e+01 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)) :local)
+ (send FOOTL_LINK_lk :transform localcds :world)
+ )
+ ;;2;
+ (send ANKLEL_LINK_lk :assoc FOOTL_LINK_lk)
+ )
+
+ ;; node id=v1.node6, name=LEGLOWERL_LINK, sid=node6
+ (let ( b_g1_6_geom0)
+ ;; define bodyset-link for LEGLOWERL_LINK : v1.node6
+ (setq b_g1_6_geom0 (instance DARWIN_g1_6_geom0 :init))
+
+ ;; writeTransform(name=b_g1_6_geom0,domNode=LEGLOWERL_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_6_geom0 :transform localcds :world)
+ )
+ (setq LEGLOWERL_LINK_lk
+ (instance bodyset-link
+ :init (make-cascoords)
+ :bodies (list b_g1_6_geom0)
+ :name "LEGLOWERL_LINK"))
+ (send LEGLOWERL_LINK_lk :weight 70.310)
+ (let ((tmp-c-list (list
+ (make-coords :pos (float-vector 3.7000000000000000e+01 -1.2220000000000000e+02 -5.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 -1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector -1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector -0.0000000000000000e+00 -9.3000000000000000e+01 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector -1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector -5.9246900000000002e-01 -3.9045499999999997e+01 6.5476299999999998e+00) :rot (matrix-exponent (scale 3.0125423596492524e+00 (float-vector -6.9866362538497062e-01 7.1259944941643427e-01 6.3805667893410523e-02))))
+ ))
+ (tmp-c (make-coords)))
+ (dolist (cc tmp-c-list)
+ (setq tmp-c (send tmp-c :transform cc)))
+ (setq (LEGLOWERL_LINK_lk . inertia-tensor)
+ (m* (m* (send tmp-c :worldrot) (diagonal (float-vector 4.3916891334151958e+04 9.3388120663165115e+04 1.1636003800268300e+05))) (transpose (send tmp-c :worldrot))))
+ (setq (LEGLOWERL_LINK_lk . acentroid) (send tmp-c :worldpos))
+ )
+
+ ;; writeTransform(name=LEGLOWERL_LINK_lk,domNode=LEGLOWERL_LINK,targetCount=0,parent=:local), translateCount=1, rotateCount=2, matrixCount=0
+ (send LEGLOWERL_LINK_lk :transform
+ (make-coords :pos (float-vector -0.0000000000000000e+00 -9.3000000000000000e+01 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector -1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)) :local)
+ ;;1;
+
+ ;; writeTransform(name=ANKLEL_LINK_lk,domNode=LEGLOWERL_LINK,targetCount=0,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send localcds :transform
+ (make-coords :pos (float-vector -0.0000000000000000e+00 -9.3000000000000000e+01 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector -1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)) :local)
+ (send ANKLEL_LINK_lk :transform localcds :world)
+ )
+ ;;2;
+ (send LEGLOWERL_LINK_lk :assoc ANKLEL_LINK_lk)
+ )
+
+ ;; node id=v1.node5, name=LEGUPPERL_LINK, sid=node5
+ (let ( b_g1_5_geom0 b_g1_5_geom1)
+ ;; define bodyset-link for LEGUPPERL_LINK : v1.node5
+ (setq b_g1_5_geom0 (instance DARWIN_g1_5_geom0 :init))
+
+ ;; writeTransform(name=b_g1_5_geom0,domNode=LEGUPPERL_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_5_geom0 :transform localcds :world)
+ )
+ (setq b_g1_5_geom1 (instance DARWIN_g1_5_geom1 :init))
+
+ ;; writeTransform(name=b_g1_5_geom1,domNode=LEGUPPERL_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_5_geom1 :transform localcds :world)
+ )
+ (send b_g1_5_geom0 :assoc b_g1_5_geom1)
+ (setq LEGUPPERL_LINK_lk
+ (instance bodyset-link
+ :init (make-cascoords)
+ :bodies (list b_g1_5_geom0 b_g1_5_geom1)
+ :name "LEGUPPERL_LINK"))
+ (send LEGUPPERL_LINK_lk :weight 119.043)
+ (let ((tmp-c-list (list
+ (make-coords :pos (float-vector 3.7000000000000000e+01 -1.2220000000000000e+02 -5.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 -1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector -1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector -3.2263500000000001e-01 -6.2965499999999992e+01 6.9190600000000002e-01) :rot (matrix-exponent (scale 3.1349165357532121e+00 (float-vector -7.0052749830315086e-01 -7.1349838760778816e-01 -1.3463840544786290e-02))))
+ ))
+ (tmp-c (make-coords)))
+ (dolist (cc tmp-c-list)
+ (setq tmp-c (send tmp-c :transform cc)))
+ (setq (LEGUPPERL_LINK_lk . inertia-tensor)
+ (m* (m* (send tmp-c :worldrot) (diagonal (float-vector 3.2779794154093579e+04 9.7969132072394365e+04 1.1504953377351201e+05))) (transpose (send tmp-c :worldrot))))
+ (setq (LEGUPPERL_LINK_lk . acentroid) (send tmp-c :worldpos))
+ )
+
+ ;; writeTransform(name=LEGUPPERL_LINK_lk,domNode=LEGUPPERL_LINK,targetCount=0,parent=:local), translateCount=1, rotateCount=2, matrixCount=0
+ (send LEGUPPERL_LINK_lk :transform
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector -1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)) :local)
+ ;;1;
+
+ ;; writeTransform(name=LEGLOWERL_LINK_lk,domNode=LEGUPPERL_LINK,targetCount=0,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send localcds :transform
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector -1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)) :local)
+ (send LEGLOWERL_LINK_lk :transform localcds :world)
+ )
+ ;;2;
+ (send LEGUPPERL_LINK_lk :assoc LEGLOWERL_LINK_lk)
+ )
+
+ ;; node id=v1.node4, name=PELVL_LINK, sid=node4
+ (let ( b_g1_4_geom0)
+ ;; define bodyset-link for PELVL_LINK : v1.node4
+ (setq b_g1_4_geom0 (instance DARWIN_g1_4_geom0 :init))
+
+ ;; writeTransform(name=b_g1_4_geom0,domNode=PELVL_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_4_geom0 :transform localcds :world)
+ )
+ (setq PELVL_LINK_lk
+ (instance bodyset-link
+ :init (make-cascoords)
+ :bodies (list b_g1_4_geom0)
+ :name "PELVL_LINK"))
+ (send PELVL_LINK_lk :weight 167.108)
+ (let ((tmp-c-list (list
+ (make-coords :pos (float-vector 3.7000000000000000e+01 -1.2220000000000000e+02 -5.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 -1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 7.9982799999999993e-02 -1.3873099999999999e+01 -1.8242400000000000e+01) :rot (matrix-exponent (scale 2.1278186543466693e+00 (float-vector -5.8206765894392221e-01 -6.1841948818868531e-01 -5.2797213661327491e-01))))
+ ))
+ (tmp-c (make-coords)))
+ (dolist (cc tmp-c-list)
+ (setq tmp-c (send tmp-c :transform cc)))
+ (setq (PELVL_LINK_lk . inertia-tensor)
+ (m* (m* (send tmp-c :worldrot) (diagonal (float-vector 4.1133583174242623e+04 1.2304396677250660e+05 1.1170008005325080e+05))) (transpose (send tmp-c :worldrot))))
+ (setq (PELVL_LINK_lk . acentroid) (send tmp-c :worldpos))
+ )
+
+ ;; writeTransform(name=PELVL_LINK_lk,domNode=PELVL_LINK,targetCount=0,parent=:local), translateCount=1, rotateCount=2, matrixCount=0
+ (send PELVL_LINK_lk :transform
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 -1.0000000000000000e+00)) :local)
+ ;;1;
+
+ ;; writeTransform(name=LEGUPPERL_LINK_lk,domNode=PELVL_LINK,targetCount=0,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send localcds :transform
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 -1.0000000000000000e+00)) :local)
+ (send LEGUPPERL_LINK_lk :transform localcds :world)
+ )
+ ;;2;
+ (send PELVL_LINK_lk :assoc LEGUPPERL_LINK_lk)
+ )
+
+ ;; node id=v1.node3, name=PELVYL_LINK, sid=node3
+ (let ( b_g1_3_geom0)
+ ;; define bodyset-link for PELVYL_LINK : v1.node3
+ (setq b_g1_3_geom0 (instance DARWIN_g1_3_geom0 :init))
+
+ ;; writeTransform(name=b_g1_3_geom0,domNode=PELVYL_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_3_geom0 :transform localcds :world)
+ )
+ (setq PELVYL_LINK_lk
+ (instance bodyset-link
+ :init (make-cascoords)
+ :bodies (list b_g1_3_geom0)
+ :name "PELVYL_LINK"))
+ (send PELVYL_LINK_lk :weight 27.069)
+ (let ((tmp-c-list (list
+ (make-coords :pos (float-vector 3.7000000000000000e+01 -1.2220000000000000e+02 -5.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 1.8437200000000001e+01 4.8013500000000003e-01) :rot (matrix-exponent (scale 3.0434509163188275e+00 (float-vector 7.0625355896011288e-01 -4.9110293344152837e-02 -7.0625355896011288e-01))))
+ ))
+ (tmp-c (make-coords)))
+ (dolist (cc tmp-c-list)
+ (setq tmp-c (send tmp-c :transform cc)))
+ (setq (PELVYL_LINK_lk . inertia-tensor)
+ (m* (m* (send tmp-c :worldrot) (diagonal (float-vector 6.0800445741485273e+03 1.1431265425851470e+04 1.5019139999999999e+04))) (transpose (send tmp-c :worldrot))))
+ (setq (PELVYL_LINK_lk . acentroid) (send tmp-c :worldpos))
+ )
+
+ ;; writeTransform(name=PELVYL_LINK_lk,domNode=PELVYL_LINK,targetCount=0,parent=:local), translateCount=1, rotateCount=2, matrixCount=0
+ (send PELVYL_LINK_lk :transform
+ (make-coords :pos (float-vector 3.7000000000000000e+01 -1.2220000000000000e+02 -5.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00)) :local)
+ ;;1;
+
+ ;; writeTransform(name=PELVL_LINK_lk,domNode=PELVYL_LINK,targetCount=0,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send localcds :transform
+ (make-coords :pos (float-vector 3.7000000000000000e+01 -1.2220000000000000e+02 -5.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00)) :local)
+ (send PELVL_LINK_lk :transform localcds :world)
+ )
+ ;;2;
+ (send PELVYL_LINK_lk :assoc PELVL_LINK_lk)
+ )
+
+ ;; node id=v1.node14, name=FOOTR_LINK, sid=node14
+ (let ( b_g1_14_geom0 b_g1_14_geom1)
+ ;; define bodyset-link for FOOTR_LINK : v1.node14
+ (setq b_g1_14_geom0 (instance DARWIN_g1_14_geom0 :init))
+
+ ;; writeTransform(name=b_g1_14_geom0,domNode=FOOTR_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_14_geom0 :transform localcds :world)
+ )
+ (setq b_g1_14_geom1 (instance DARWIN_g1_14_geom1 :init))
+
+ ;; writeTransform(name=b_g1_14_geom1,domNode=FOOTR_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_14_geom1 :transform localcds :world)
+ )
+ (send b_g1_14_geom0 :assoc b_g1_14_geom1)
+ (setq FOOTR_LINK_lk
+ (instance bodyset-link
+ :init (make-cascoords)
+ :bodies (list b_g1_14_geom0 b_g1_14_geom1)
+ :name "FOOTR_LINK"))
+ (send FOOTR_LINK_lk :weight 79.446)
+ (let ((tmp-c-list (list
+ (make-coords :pos (float-vector -3.7000000000000000e+01 -1.2220000000000000e+02 -5.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector -0.0000000000000000e+00 -9.3000000000000000e+01 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 -9.3000000000000000e+01 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector -1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector -9.5058799999999994e+00 -2.5995300000000000e+01 -5.0287700000000002e-01) :rot (matrix-exponent (scale 3.1374881783494764e+00 (float-vector 6.3355814941524424e-02 -9.9794679653010854e-01 -9.3931894684097673e-03))))
+ ))
+ (tmp-c (make-coords)))
+ (dolist (cc tmp-c-list)
+ (setq tmp-c (send tmp-c :transform cc)))
+ (setq (FOOTR_LINK_lk . inertia-tensor)
+ (m* (m* (send tmp-c :worldrot) (diagonal (float-vector 6.8003221353508503e+04 8.8232016045823475e+04 3.5821522600668002e+04))) (transpose (send tmp-c :worldrot))))
+ (setq (FOOTR_LINK_lk . acentroid) (send tmp-c :worldpos))
+ )
+
+ ;; writeTransform(name=FOOTR_LINK_lk,domNode=FOOTR_LINK,targetCount=0,parent=:local), translateCount=1, rotateCount=2, matrixCount=0
+ (send FOOTR_LINK_lk :transform
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00)) :local)
+ ;;1;
+ )
+
+ ;; node id=v1.node13, name=ANKLER_LINK, sid=node13
+ (let ( b_g1_13_geom0)
+ ;; define bodyset-link for ANKLER_LINK : v1.node13
+ (setq b_g1_13_geom0 (instance DARWIN_g1_13_geom0 :init))
+
+ ;; writeTransform(name=b_g1_13_geom0,domNode=ANKLER_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_13_geom0 :transform localcds :world)
+ )
+ (setq ANKLER_LINK_lk
+ (instance bodyset-link
+ :init (make-cascoords)
+ :bodies (list b_g1_13_geom0)
+ :name "ANKLER_LINK"))
+ (send ANKLER_LINK_lk :weight 167.108)
+ (let ((tmp-c-list (list
+ (make-coords :pos (float-vector -3.7000000000000000e+01 -1.2220000000000000e+02 -5.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector -0.0000000000000000e+00 -9.3000000000000000e+01 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 -9.3000000000000000e+01 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector -1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector 2.1373200000000001e-01 1.3873099999999999e+01 -1.8536100000000001e+01) :rot (matrix-exponent (scale 3.0379497570860514e+00 (float-vector -9.9932043867120068e-01 -3.6659460910665437e-02 -3.8398931936168461e-03))))
+ ))
+ (tmp-c (make-coords)))
+ (dolist (cc tmp-c-list)
+ (setq tmp-c (send tmp-c :transform cc)))
+ (setq (ANKLER_LINK_lk . inertia-tensor)
+ (m* (m* (send tmp-c :worldrot) (diagonal (float-vector 1.2106265191898300e+05 1.0967651340802430e+05 4.1131904672992692e+04))) (transpose (send tmp-c :worldrot))))
+ (setq (ANKLER_LINK_lk . acentroid) (send tmp-c :worldpos))
+ )
+
+ ;; writeTransform(name=ANKLER_LINK_lk,domNode=ANKLER_LINK,targetCount=0,parent=:local), translateCount=1, rotateCount=2, matrixCount=0
+ (send ANKLER_LINK_lk :transform
+ (make-coords :pos (float-vector 0.0000000000000000e+00 -9.3000000000000000e+01 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector -1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)) :local)
+ ;;1;
+
+ ;; writeTransform(name=FOOTR_LINK_lk,domNode=ANKLER_LINK,targetCount=0,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send localcds :transform
+ (make-coords :pos (float-vector 0.0000000000000000e+00 -9.3000000000000000e+01 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector -1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)) :local)
+ (send FOOTR_LINK_lk :transform localcds :world)
+ )
+ ;;2;
+ (send ANKLER_LINK_lk :assoc FOOTR_LINK_lk)
+ )
+
+ ;; node id=v1.node12, name=LEGLOWERR_LINK, sid=node12
+ (let ( b_g1_12_geom0)
+ ;; define bodyset-link for LEGLOWERR_LINK : v1.node12
+ (setq b_g1_12_geom0 (instance DARWIN_g1_12_geom0 :init))
+
+ ;; writeTransform(name=b_g1_12_geom0,domNode=LEGLOWERR_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_12_geom0 :transform localcds :world)
+ )
+ (setq LEGLOWERR_LINK_lk
+ (instance bodyset-link
+ :init (make-cascoords)
+ :bodies (list b_g1_12_geom0)
+ :name "LEGLOWERR_LINK"))
+ (send LEGLOWERR_LINK_lk :weight 70.310)
+ (let ((tmp-c-list (list
+ (make-coords :pos (float-vector -3.7000000000000000e+01 -1.2220000000000000e+02 -5.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector -0.0000000000000000e+00 -9.3000000000000000e+01 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector 5.9246900000000002e-01 -3.9045499999999997e+01 6.5476299999999998e+00) :rot (matrix-exponent (scale 3.0125423596492524e+00 (float-vector -6.9866362538497062e-01 -7.1259944941643427e-01 -6.3805667893410523e-02))))
+ ))
+ (tmp-c (make-coords)))
+ (dolist (cc tmp-c-list)
+ (setq tmp-c (send tmp-c :transform cc)))
+ (setq (LEGLOWERR_LINK_lk . inertia-tensor)
+ (m* (m* (send tmp-c :worldrot) (diagonal (float-vector 4.3916891334151958e+04 9.3388120663165115e+04 1.1636003800268300e+05))) (transpose (send tmp-c :worldrot))))
+ (setq (LEGLOWERR_LINK_lk . acentroid) (send tmp-c :worldpos))
+ )
+
+ ;; writeTransform(name=LEGLOWERR_LINK_lk,domNode=LEGLOWERR_LINK,targetCount=0,parent=:local), translateCount=1, rotateCount=2, matrixCount=0
+ (send LEGLOWERR_LINK_lk :transform
+ (make-coords :pos (float-vector -0.0000000000000000e+00 -9.3000000000000000e+01 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)) :local)
+ ;;1;
+
+ ;; writeTransform(name=ANKLER_LINK_lk,domNode=LEGLOWERR_LINK,targetCount=0,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send localcds :transform
+ (make-coords :pos (float-vector -0.0000000000000000e+00 -9.3000000000000000e+01 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)) :local)
+ (send ANKLER_LINK_lk :transform localcds :world)
+ )
+ ;;2;
+ (send LEGLOWERR_LINK_lk :assoc ANKLER_LINK_lk)
+ )
+
+ ;; node id=v1.node11, name=LEGUPPERR_LINK, sid=node11
+ (let ( b_g1_11_geom0 b_g1_11_geom1)
+ ;; define bodyset-link for LEGUPPERR_LINK : v1.node11
+ (setq b_g1_11_geom0 (instance DARWIN_g1_11_geom0 :init))
+
+ ;; writeTransform(name=b_g1_11_geom0,domNode=LEGUPPERR_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_11_geom0 :transform localcds :world)
+ )
+ (setq b_g1_11_geom1 (instance DARWIN_g1_11_geom1 :init))
+
+ ;; writeTransform(name=b_g1_11_geom1,domNode=LEGUPPERR_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_11_geom1 :transform localcds :world)
+ )
+ (send b_g1_11_geom0 :assoc b_g1_11_geom1)
+ (setq LEGUPPERR_LINK_lk
+ (instance bodyset-link
+ :init (make-cascoords)
+ :bodies (list b_g1_11_geom0 b_g1_11_geom1)
+ :name "LEGUPPERR_LINK"))
+ (send LEGUPPERR_LINK_lk :weight 119.043)
+ (let ((tmp-c-list (list
+ (make-coords :pos (float-vector -3.7000000000000000e+01 -1.2220000000000000e+02 -5.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00))))
+ (make-coords :pos (float-vector 3.2263500000000001e-01 -6.2965499999999992e+01 6.9190600000000002e-01) :rot (matrix-exponent (scale 3.1349165357532121e+00 (float-vector -7.0052749830315086e-01 7.1349838760778816e-01 1.3463840544786290e-02))))
+ ))
+ (tmp-c (make-coords)))
+ (dolist (cc tmp-c-list)
+ (setq tmp-c (send tmp-c :transform cc)))
+ (setq (LEGUPPERR_LINK_lk . inertia-tensor)
+ (m* (m* (send tmp-c :worldrot) (diagonal (float-vector 3.2779794154093579e+04 9.7969132072394365e+04 1.1504953377351201e+05))) (transpose (send tmp-c :worldrot))))
+ (setq (LEGUPPERR_LINK_lk . acentroid) (send tmp-c :worldpos))
+ )
+
+ ;; writeTransform(name=LEGUPPERR_LINK_lk,domNode=LEGUPPERR_LINK,targetCount=0,parent=:local), translateCount=1, rotateCount=2, matrixCount=0
+ (send LEGUPPERR_LINK_lk :transform
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)) :local)
+ ;;1;
+
+ ;; writeTransform(name=LEGLOWERR_LINK_lk,domNode=LEGUPPERR_LINK,targetCount=0,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send localcds :transform
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 1.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)) :local)
+ (send LEGLOWERR_LINK_lk :transform localcds :world)
+ )
+ ;;2;
+ (send LEGUPPERR_LINK_lk :assoc LEGLOWERR_LINK_lk)
+ )
+
+ ;; node id=v1.node10, name=PELVR_LINK, sid=node10
+ (let ( b_g1_10_geom0)
+ ;; define bodyset-link for PELVR_LINK : v1.node10
+ (setq b_g1_10_geom0 (instance DARWIN_g1_10_geom0 :init))
+
+ ;; writeTransform(name=b_g1_10_geom0,domNode=PELVR_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_10_geom0 :transform localcds :world)
+ )
+ (setq PELVR_LINK_lk
+ (instance bodyset-link
+ :init (make-cascoords)
+ :bodies (list b_g1_10_geom0)
+ :name "PELVR_LINK"))
+ (send PELVR_LINK_lk :weight 167.108)
+ (let ((tmp-c-list (list
+ (make-coords :pos (float-vector -3.7000000000000000e+01 -1.2220000000000000e+02 -5.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector -7.9982799999999993e-02 -1.3873099999999999e+01 -1.8242400000000000e+01) :rot (matrix-exponent (scale 2.1278186543466693e+00 (float-vector -5.8206765894392221e-01 6.1841948818868531e-01 5.2797213661327491e-01))))
+ ))
+ (tmp-c (make-coords)))
+ (dolist (cc tmp-c-list)
+ (setq tmp-c (send tmp-c :transform cc)))
+ (setq (PELVR_LINK_lk . inertia-tensor)
+ (m* (m* (send tmp-c :worldrot) (diagonal (float-vector 4.1133583174242623e+04 1.2304396677250660e+05 1.1170008005325080e+05))) (transpose (send tmp-c :worldrot))))
+ (setq (PELVR_LINK_lk . acentroid) (send tmp-c :worldpos))
+ )
+
+ ;; writeTransform(name=PELVR_LINK_lk,domNode=PELVR_LINK,targetCount=0,parent=:local), translateCount=1, rotateCount=2, matrixCount=0
+ (send PELVR_LINK_lk :transform
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00)) :local)
+ ;;1;
+
+ ;; writeTransform(name=LEGUPPERR_LINK_lk,domNode=PELVR_LINK,targetCount=0,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send localcds :transform
+ (make-coords :pos (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00)) :local)
+ (send LEGUPPERR_LINK_lk :transform localcds :world)
+ )
+ ;;2;
+ (send PELVR_LINK_lk :assoc LEGUPPERR_LINK_lk)
+ )
+
+ ;; node id=v1.node9, name=PELVYR_LINK, sid=node9
+ (let ( b_g1_9_geom0)
+ ;; define bodyset-link for PELVYR_LINK : v1.node9
+ (setq b_g1_9_geom0 (instance DARWIN_g1_9_geom0 :init))
+
+ ;; writeTransform(name=b_g1_9_geom0,domNode=PELVYR_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_9_geom0 :transform localcds :world)
+ )
+ (setq PELVYR_LINK_lk
+ (instance bodyset-link
+ :init (make-cascoords)
+ :bodies (list b_g1_9_geom0)
+ :name "PELVYR_LINK"))
+ (send PELVYR_LINK_lk :weight 27.069)
+ (let ((tmp-c-list (list
+ (make-coords :pos (float-vector -3.7000000000000000e+01 -1.2220000000000000e+02 -5.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 1.8437200000000001e+01 4.8013500000000003e-01) :rot (matrix-exponent (scale 3.0434509163188275e+00 (float-vector 7.0625355896011288e-01 -4.9110293344152837e-02 -7.0625355896011288e-01))))
+ ))
+ (tmp-c (make-coords)))
+ (dolist (cc tmp-c-list)
+ (setq tmp-c (send tmp-c :transform cc)))
+ (setq (PELVYR_LINK_lk . inertia-tensor)
+ (m* (m* (send tmp-c :worldrot) (diagonal (float-vector 6.0800445741485273e+03 1.1431265425851470e+04 1.5019139999999999e+04))) (transpose (send tmp-c :worldrot))))
+ (setq (PELVYR_LINK_lk . acentroid) (send tmp-c :worldpos))
+ )
+
+ ;; writeTransform(name=PELVYR_LINK_lk,domNode=PELVYR_LINK,targetCount=0,parent=:local), translateCount=1, rotateCount=2, matrixCount=0
+ (send PELVYR_LINK_lk :transform
+ (make-coords :pos (float-vector -3.7000000000000000e+01 -1.2220000000000000e+02 -5.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00)) :local)
+ ;;1;
+
+ ;; writeTransform(name=PELVR_LINK_lk,domNode=PELVYR_LINK,targetCount=0,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send localcds :transform
+ (make-coords :pos (float-vector -3.7000000000000000e+01 -1.2220000000000000e+02 -5.0000000000000000e+00)
+ :angle 0.0000000000000000e+00 :axis (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00)) :local)
+ (send PELVR_LINK_lk :transform localcds :world)
+ )
+ ;;2;
+ (send PELVYR_LINK_lk :assoc PELVR_LINK_lk)
+ )
+
+ ;; node id=v1.node17, name=ARMLOWERL_LINK, sid=node17
+ (let ( b_g1_17_geom0 b_g1_17_geom1)
+ ;; define bodyset-link for ARMLOWERL_LINK : v1.node17
+ (setq b_g1_17_geom0 (instance DARWIN_g1_17_geom0 :init))
+
+ ;; writeTransform(name=b_g1_17_geom0,domNode=ARMLOWERL_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_17_geom0 :transform localcds :world)
+ )
+ (setq b_g1_17_geom1 (instance DARWIN_g1_17_geom1 :init))
+
+ ;; writeTransform(name=b_g1_17_geom1,domNode=ARMLOWERL_LINK,targetCount=1,parent=:world), translateCount=1, rotateCount=2, matrixCount=0
+ (let ((localcds (make-coords)))
+ (send b_g1_17_geom1 :transform localcds :world)
+ )
+ (send b_g1_17_geom0 :assoc b_g1_17_geom1)
+ (setq ARMLOWERL_LINK_lk
+ (instance bodyset-link
+ :init (make-cascoords)
+ :bodies (list b_g1_17_geom0 b_g1_17_geom1)
+ :name "ARMLOWERL_LINK"))
+ (send ARMLOWERL_LINK_lk :weight 59.288)
+ (let ((tmp-c-list (list
+ (make-coords :pos (float-vector 8.2000000000000000e+01 0.0000000000000000e+00 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 -1.6000000000000000e+01 0.0000000000000000e+00) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector 0.0000000000000000e+00 -6.0000000000000000e+01 1.6000000000000000e+01) :rot (matrix-exponent (scale 0.0000000000000000e+00 (float-vector 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+00))))
+ (make-coords :pos (float-vector -6.6656399999999998e+00 -4.5838200000000001e+01 -1.3490100000000000e+01) :rot (matrix-exponent (scale 2.9760139441600675e+00 (float-vector 6.7568311286212879e-01 7.1948377229020299e-01 1.6060956510744590e-01))))
+ ))
+ (tmp-c (make-coords)))
+ (dolist (cc tmp-c-list)
+ ...
[truncated message content] |
