Mercurial > cortex

comparison org/body.org @ 136:47a4d74761f0

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
fixed some more problems with proprioception
author Robert McIntyre <rlm@mit.edu>
date Wed, 01 Feb 2012 17:39:23 -0700
parents 421cc43441ae
children 39c89ae5c7d0
comparison
equal deleted inserted replaced
135:421cc43441ae 136:47a4d74761f0
72 ;; this function will report proprioceptive information for the 72 ;; this function will report proprioceptive information for the
73 ;; joint. 73 ;; joint.
74 (fn [] 74 (fn []
75 ;; x is the "twist" axis, y and z are the "bend" axes 75 ;; x is the "twist" axis, y and z are the "bend" axes
76 (let [rot-a (.getWorldRotation obj-a) 76 (let [rot-a (.getWorldRotation obj-a)
77 ;;inv-a (.inverse rot-a)
77 rot-b (.getWorldRotation obj-b) 78 rot-b (.getWorldRotation obj-b)
78 relative (.mult rot-b (.inverse rot-a)) 79 ;;relative (.mult rot-b inv-a)
79 basis (doto (Matrix3f.) 80 basis (doto (Matrix3f.)
80 (.setColumn 0 x) 81 (.setColumn 0 (.mult rot-a x))
81 (.setColumn 1 y) 82 (.setColumn 1 (.mult rot-a y))
82 (.setColumn 2 z)) 83 (.setColumn 2 (.mult rot-a z)))
83 rotation-about-joint 84 rotation-about-joint
84 (doto (Quaternion.) 85 (doto (Quaternion.)
85 (.fromRotationMatrix 86 (.fromRotationMatrix
86 (.mult (.invert basis) 87 (.mult (.invert basis)
87 (.toRotationMatrix relative)))) 88 (.toRotationMatrix rot-b))))
88 [yaw roll pitch] 89 [yaw roll pitch]
89 (seq (.toAngles rotation-about-joint nil))] 90 (seq (.toAngles rotation-about-joint nil))]
90 ;;return euler angles of the quaternion around the new basis 91 ;;return euler angles of the quaternion around the new basis
91 [yaw roll pitch] 92 [yaw roll pitch]
92 )))) 93 ))))
356 :position (Vector3f. 1.2 2 0) 357 :position (Vector3f. 1.2 2 0)
357 :physical? false) 358 :physical? false)
358 joint (join-at-point hand finger (Vector3f. 1.2 2 0 )) 359 joint (join-at-point hand finger (Vector3f. 1.2 2 0 ))
359 creature (nodify [hand finger joint-node]) 360 creature (nodify [hand finger joint-node])
360 ;; ******************************************* 361 ;; *******************************************
362 rot (doto (Quaternion.)
363 (.fromAngleAxis (/ Math/PI 4)
364 (Vector3f. 0 0 -1)))
361 hand2 (box 1 0.2 0.2 :position (Vector3f. 0 1.5 -3) 365 hand2 (box 1 0.2 0.2 :position (Vector3f. 0 1.5 -3)
362 :mass 0 :color ColorRGBA/Blue) 366 :mass 0 :color ColorRGBA/Blue :rotation rot)
363 finger2 (box 1 0.2 0.2 :position (Vector3f. 2.4 1.5 -3) 367 finger2 (box 1 0.2 0.2 :position (Vector3f. 2.4 1.5 -3)
364 :mass 1 :color ColorRGBA/Magenta) 368 :mass 1 :color ColorRGBA/Magenta :rotation rot)
365 joint-node2 (box 0.1 0.05 0.05 :color ColorRGBA/Gray 369 joint-node2 (box 0.1 0.05 0.05 :color ColorRGBA/Gray
366 :position (Vector3f. 1.2 1.5 -3) 370 :position (Vector3f. 1.2 1.5 -3)
367 :physical? false) 371 :physical? false :rotation rot)
368 joint2 (join-at-point hand2 finger2 (Vector3f. 1.2 1.5 -3)) 372 joint2 (join-at-point hand2 finger2 (Vector3f. 1.2 1.5 -3))
369 creature2 (nodify [hand2 finger2 joint-node2]) 373 creature2 (nodify [hand2 finger2 joint-node2])
370 ;; ******************************************* 374 ;; *******************************************
371 375
372 floor (box 10 10 10 :position (Vector3f. 0 -15 0) 376 floor (box 10 10 10 :position (Vector3f. 0 -15 0)
381 385
382 386
383 root (nodify [creature creature2 floor]) 387 root (nodify [creature creature2 floor])
384 prop (joint-proprioception creature joint-node) 388 prop (joint-proprioception creature joint-node)
385 prop-view (proprioception-debug-window)] 389 prop-view (proprioception-debug-window)]
390
386 391
387 392
393 (.setCollisionGroup
394 (.getControl hand RigidBodyControl)
395 PhysicsCollisionObject/COLLISION_GROUP_NONE)
396 (.setCollisionGroup
397 (.getControl hand2 RigidBodyControl)
398 PhysicsCollisionObject/COLLISION_GROUP_NONE)
399
388 400
389 (comment
390 (.setCollisionGroup
391 (.getControl hand RigidBodyControl)
392 PhysicsCollisionObject/COLLISION_GROUP_NONE)
393 )
394 401
395 402
396 (world 403 (world
397 root 404 root
398 (merge standard-debug-controls 405 (merge standard-debug-controls
412 right (Vector3f. 0 -1 0) 419 right (Vector3f. 0 -1 0)
413 up (Vector3f. 0 0 1) 420 up (Vector3f. 0 0 1)
414 down (Vector3f. 0 0 -1) 421 down (Vector3f. 0 0 -1)
415 roll-left (Vector3f. -1 0 0) 422 roll-left (Vector3f. -1 0 0)
416 roll-right (Vector3f. 1 0 0)] 423 roll-right (Vector3f. 1 0 0)]
417 (if @move-up? (tap finger up force)) 424 (if @move-up? (do (tap finger up force)
418 (if @move-down? (tap finger down force)) 425 (tap finger2 up force)))
419 (if @move-left? (tap finger left force)) 426 (if @move-down? (do (tap finger down force)
420 (if @move-right? (tap finger right force)) 427 (tap finger2 down force)))
421 (if @roll-left? (tap finger roll-left (/ force 10))) 428 (if @move-left? (do (tap finger left force)
422 (if @roll-right? (tap finger roll-right (/ force 10)))) 429 (tap finger2 left force)))
430 (if @move-right? (do (tap finger right force)
431 (tap finger2 right force)))
432 (if @roll-left? (do (tap finger roll-left (/ force 10))
433 (tap finger2 roll-left (/ force 10))))
434 (if @roll-right? (do (tap finger roll-right (/ force 10))
435 (tap finger2 roll-right (/ force 10)))))
423 (prop-view (list (prop))))))) 436 (prop-view (list (prop)))))))
424 437
425 #+end_src 438 #+end_src
426 439
427 #+results: test-body 440 #+results: test-body