Mercurial > cortex

annotate org/body.org @ 202:d5c597a7aed4

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
writing prose for body.org
author Robert McIntyre <rlm@mit.edu>
date Wed, 08 Feb 2012 05:50:15 -0700
parents 026f69582022
children 0e5d5ee5a914
rev   line source
rlm@202 1 #+title: Building a Body
rlm@0 2 #+author: Robert McIntyre
rlm@0 3 #+email: rlm@mit.edu
rlm@4 4 #+description: Simulating a body (movement, touch, propioception) in jMonkeyEngine3.
rlm@4 5 #+SETUPFILE: ../../aurellem/org/setup.org
rlm@4 6 #+INCLUDE: ../../aurellem/org/level-0.org
rlm@4 7
rlm@202 8
rlm@202 9 * Design Constraints
rlm@202 10
rlm@202 11 I use [[www.blender.org/][blender]] to design bodies. The design of the bodies is
rlm@202 12 determined by the requirements of the AI that will use them. The
rlm@202 13 bodies must be easy for an AI to sense and control, and they must be
rlm@202 14 relatively simple for jMonkeyEngine to compute.
rlm@202 15
rlm@202 16 ** Bag of Bones
rlm@202 17
rlm@202 18 How to create such a body? One option I ultimately rejected is to use
rlm@202 19 blender's [[http://wiki.blender.org/index.php/Doc:2.6/Manual/Rigging/Armatures][armature]] system. The idea would have been to define a mesh
rlm@202 20 which describes the creature's entire body. To this you add an
rlm@202 21 (skeleton) which deforms this mesh. This technique is used extensively
rlm@202 22 to model humans and create realistic animations. It is hard to use for
rlm@202 23 my purposes because it is difficult to update the creature's Physics
rlm@202 24 Collision Mesh in tandem with its Geometric Mesh under the influence
rlm@202 25 of the armature. Withouth this the creature will not be able to grab
rlm@202 26 things in its environment, and it won't be able to tell where its
rlm@202 27 physical body is by using its eyes. Also, armatures do not specify
rlm@202 28 any rotational limits for a joint, making it hard to model elbows,
rlm@202 29 shoulders, etc.
rlm@202 30
rlm@202 31 ** EVE
rlm@202 32
rlm@202 33 Instead of using the human-like "deformable bag of bones" approach, I
rlm@202 34 decided to base my body plans on the robot EVE from the movie wall-E.
rlm@202 35
rlm@202 36 #+caption: EVE from the movie WALL-E. This body plan turns out to be much better suited to my purposes than a more human-like one.
rlm@202 37 [[../images/Eve.jpg]]
rlm@202 38
rlm@202 39 The main reason that I use eve-style bodies is so that there will be
rlm@202 40 correspondence between the AI's vision and the physical presence of
rlm@202 41 its body.
rlm@202 42
rlm@202 43 * Solidifying the Body
rlm@202 44
rlm@202 45 Here is a hand designed eve-style in blender.
rlm@202 46
rlm@202 47
rlm@202 48 #+attr_html: width="500"
rlm@202 49 [[../images/hand-screenshot0.png]]
rlm@202 50
rlm@202 51 If we load it directly into jMonkeyEngine, we get this:
rlm@202 52
rlm@202 53 #+name: test-0
rlm@202 54 #+begin_src clojure
rlm@202 55 (ns cortex.test.body
rlm@202 56 (:use (cortex world util body))
rlm@202 57 (:import (com.aurellem.capture Capture RatchetTimer)
rlm@202 58 (com.jme3.math Quaternion Vector3f)
rlm@202 59 java.io.File))
rlm@202 60
rlm@202 61 (def hand-path "Models/test-creature/hand.blend")
rlm@202 62
rlm@202 63 (defn hand [] (load-blender-model hand-path))
rlm@202 64
rlm@202 65 (defn setup [world]
rlm@202 66 (let [cam (.getCamera world)]
rlm@202 67 (println-repl cam)
rlm@202 68 (.setLocation
rlm@202 69 cam (Vector3f.
rlm@202 70 -6.9015837, 8.644911, 5.6043186))
rlm@202 71 (.setRotation
rlm@202 72 cam
rlm@202 73 (Quaternion.
rlm@202 74 0.14046453, 0.85894054, -0.34301838, 0.3533118)))
rlm@202 75 (light-up-everything world)
rlm@202 76 (.setTimer world (RatchetTimer. 60))
rlm@202 77 world)
rlm@202 78
rlm@202 79 (defn test-one []
rlm@202 80 (world (hand)
rlm@202 81 standard-debug-controls
rlm@202 82 (comp
rlm@202 83 #(Capture/captureVideo
rlm@202 84 % (File. "/home/r/proj/cortex/render/body/1"))
rlm@202 85 setup)
rlm@202 86 no-op))
rlm@202 87 #+end_src
rlm@202 88
rlm@202 89
rlm@202 90 #+begin_src clojure :results silent
rlm@202 91 (.start (cortex.test.body/test-one))
rlm@202 92 #+end_src
rlm@202 93
rlm@202 94 #+begin_html
rlm@202 95 <video controls="controls" width="755">
rlm@202 96 <source src="../video/ghost-hand.ogg" type="video/ogg"
rlm@202 97 preload="none" poster="../images/aurellem-1280x480.png" />
rlm@202 98 </video>
rlm@202 99 #+end_html
rlm@202 100
rlm@202 101 You will notice that the hand has no physical presence -- it's a
rlm@202 102 hologram through witch everything passes. Therefore, the first thing
rlm@202 103 to do is to make it solid. Blender has physics simulation on par with
rlm@202 104 jMonkeyEngine (they both use bullet as their physics backend), but it
rlm@202 105 can be difficult to translate between the two systems, so for now I
rlm@202 106 specify the mass of each object in blender and construct the physics
rlm@202 107 shape based on the mesh in jMonkeyEngine.
rlm@202 108
rlm@202 109 #+name: joints-1
rlm@202 110 #+begin_src clojure
rlm@202 111 (defn physical!
rlm@202 112 "Iterate through the nodes in creature and make them real physical
rlm@202 113 objects in the simulation."
rlm@202 114 [#^Node creature]
rlm@202 115 (dorun
rlm@202 116 (map
rlm@202 117 (fn [geom]
rlm@202 118 (let [physics-control
rlm@202 119 (RigidBodyControl.
rlm@202 120 (HullCollisionShape.
rlm@202 121 (.getMesh geom))
rlm@202 122 (if-let [mass (meta-data geom "mass")]
rlm@202 123 (do
rlm@202 124 (println-repl
rlm@202 125 "setting" (.getName geom) "mass to" (float mass))
rlm@202 126 (float mass))
rlm@202 127 (float 1)))]
rlm@202 128 (.addControl geom physics-control)))
rlm@202 129 (filter #(isa? (class %) Geometry )
rlm@202 130 (node-seq creature)))))
rlm@202 131 #+end_src
rlm@202 132
rlm@202 133 =(physical!)= iterates through a creature's node structure, creating
rlm@202 134 CollisionShapes for each geometry with the mass specified in that
rlm@202 135 geometry's meta-data.
rlm@202 136
rlm@0 137 #+begin_src clojure
rlm@202 138 (in-ns 'cortex.test.body)
rlm@160 139
rlm@202 140 (def normal-gravity
rlm@202 141 {"key-g" (fn [world _]
rlm@202 142 (set-gravity world (Vector3f. 0 -9.81 0)))})
rlm@202 143
rlm@202 144 (defn floor []
rlm@202 145 (box 10 3 10 :position (Vector3f. 0 -10 0)
rlm@202 146 :color ColorRGBA/Gray :mass 0))
rlm@202 147
rlm@202 148 (defn test-two []
rlm@202 149 (world (nodify
rlm@202 150 [(doto (hand)
rlm@202 151 (physical!))
rlm@202 152 (floor)])
rlm@202 153 (merge standard-debug-controls normal-gravity)
rlm@202 154 (comp
rlm@202 155 #(Capture/captureVideo
rlm@202 156 % (File. "/home/r/proj/cortex/render/body/2"))
rlm@202 157 #(do (set-gravity % Vector3f/ZERO) %)
rlm@202 158 setup)
rlm@202 159 no-op))
rlm@202 160 #+end_src
rlm@202 161
rlm@202 162 #+begin_html
rlm@202 163 <video controls="controls" width="755">
rlm@202 164 <source src="../video/crumbly-hand.ogg" type="video/ogg"
rlm@202 165 preload="none" poster="../images/aurellem-1280x480.png" />
rlm@202 166 </video>
rlm@202 167 #+end_html
rlm@202 168
rlm@202 169 Now that's some progress.
rlm@202 170
rlm@202 171
rlm@202 172 * Joints
rlm@202 173
rlm@202 174 Obviously, an AI is not going to be doing much just lying in pieces on
rlm@202 175 the floor. So, the next step to making a proper body is to connect
rlm@202 176 those pieces together with joints. jMonkeyEngine has a large array of
rlm@202 177 joints available via bullet, such as Point2Point, Cone, Hinge, and a
rlm@202 178 generic Six Degree of Freedom joint, with or without spring
rlm@202 179 restitution.
rlm@202 180
rlm@202 181 Although it should be possible to specify the joints using blender's
rlm@202 182 physics system, and then automatically import them with jMonkeyEngine,
rlm@202 183 the support isn't there yet, and there are a few problems with bullet
rlm@202 184 itself that need to be solved before it can happen.
rlm@202 185
rlm@202 186 So, I will use the same system for specifying joints as I will do for
rlm@202 187 some senses. Each joint is specified by an empty node whose parent
rlm@202 188 has the name "joints". Their orientation and meta-data determine what
rlm@202 189 joint is created.
rlm@202 190
rlm@202 191 [[../images/hand-screenshot1.png]]
rlm@202 192
rlm@202 193
rlm@202 194
rlm@202 195
rlm@202 196 #+name: joints-2
rlm@202 197 #+begin_src clojure
rlm@135 198 (defn joint-targets
rlm@135 199 "Return the two closest two objects to the joint object, ordered
rlm@135 200 from bottom to top according to the joint's rotation."
rlm@135 201 [#^Node parts #^Node joint]
rlm@135 202 (loop [radius (float 0.01)]
rlm@135 203 (let [results (CollisionResults.)]
rlm@135 204 (.collideWith
rlm@135 205 parts
rlm@135 206 (BoundingBox. (.getWorldTranslation joint)
rlm@135 207 radius radius radius)
rlm@135 208 results)
rlm@135 209 (let [targets
rlm@135 210 (distinct
rlm@135 211 (map #(.getGeometry %) results))]
rlm@135 212 (if (>= (count targets) 2)
rlm@135 213 (sort-by
rlm@135 214 #(let [v
rlm@135 215 (jme-to-blender
rlm@135 216 (.mult
rlm@135 217 (.inverse (.getWorldRotation joint))
rlm@135 218 (.subtract (.getWorldTranslation %)
rlm@135 219 (.getWorldTranslation joint))))]
rlm@135 220 (println-repl (.getName %) ":" v)
rlm@135 221 (.dot (Vector3f. 1 1 1)
rlm@135 222 v))
rlm@135 223 (take 2 targets))
rlm@135 224 (recur (float (* radius 2))))))))
rlm@135 225
rlm@160 226 (defmulti joint-dispatch
rlm@160 227 "Translate blender pseudo-joints into real JME joints."
rlm@160 228 (fn [constraints & _]
rlm@160 229 (:type constraints)))
rlm@141 230
rlm@160 231 (defmethod joint-dispatch :point
rlm@160 232 [constraints control-a control-b pivot-a pivot-b rotation]
rlm@160 233 (println-repl "creating POINT2POINT joint")
rlm@160 234 ;; bullet's point2point joints are BROKEN, so we must use the
rlm@160 235 ;; generic 6DOF joint instead of an actual Point2Point joint!
rlm@141 236
rlm@160 237 ;; should be able to do this:
rlm@160 238 (comment
rlm@160 239 (Point2PointJoint.
rlm@160 240 control-a
rlm@160 241 control-b
rlm@160 242 pivot-a
rlm@160 243 pivot-b))
rlm@141 244
rlm@160 245 ;; but instead we must do this:
rlm@160 246 (println-repl "substuting 6DOF joint for POINT2POINT joint!")
rlm@160 247 (doto
rlm@160 248 (SixDofJoint.
rlm@160 249 control-a
rlm@160 250 control-b
rlm@160 251 pivot-a
rlm@160 252 pivot-b
rlm@160 253 false)
rlm@160 254 (.setLinearLowerLimit Vector3f/ZERO)
rlm@160 255 (.setLinearUpperLimit Vector3f/ZERO)
rlm@160 256 ;;(.setAngularLowerLimit (Vector3f. 1 1 1))
rlm@160 257 ;;(.setAngularUpperLimit (Vector3f. 0 0 0))
rlm@141 258
rlm@160 259 ))
rlm@160 260
rlm@160 261
rlm@160 262 (defmethod joint-dispatch :hinge
rlm@160 263 [constraints control-a control-b pivot-a pivot-b rotation]
rlm@160 264 (println-repl "creating HINGE joint")
rlm@160 265 (let [axis
rlm@160 266 (if-let
rlm@160 267 [axis (:axis constraints)]
rlm@160 268 axis
rlm@160 269 Vector3f/UNIT_X)
rlm@160 270 [limit-1 limit-2] (:limit constraints)
rlm@160 271 hinge-axis
rlm@160 272 (.mult
rlm@160 273 rotation
rlm@160 274 (blender-to-jme axis))]
rlm@160 275 (doto
rlm@160 276 (HingeJoint.
rlm@160 277 control-a
rlm@160 278 control-b
rlm@160 279 pivot-a
rlm@160 280 pivot-b
rlm@160 281 hinge-axis
rlm@160 282 hinge-axis)
rlm@160 283 (.setLimit limit-1 limit-2))))
rlm@160 284
rlm@160 285 (defmethod joint-dispatch :cone
rlm@160 286 [constraints control-a control-b pivot-a pivot-b rotation]
rlm@160 287 (let [limit-xz (:limit-xz constraints)
rlm@160 288 limit-xy (:limit-xy constraints)
rlm@160 289 twist (:twist constraints)]
rlm@160 290
rlm@160 291 (println-repl "creating CONE joint")
rlm@160 292 (println-repl rotation)
rlm@160 293 (println-repl
rlm@160 294 "UNIT_X --> " (.mult rotation (Vector3f. 1 0 0)))
rlm@160 295 (println-repl
rlm@160 296 "UNIT_Y --> " (.mult rotation (Vector3f. 0 1 0)))
rlm@160 297 (println-repl
rlm@160 298 "UNIT_Z --> " (.mult rotation (Vector3f. 0 0 1)))
rlm@160 299 (doto
rlm@160 300 (ConeJoint.
rlm@160 301 control-a
rlm@160 302 control-b
rlm@160 303 pivot-a
rlm@160 304 pivot-b
rlm@160 305 rotation
rlm@160 306 rotation)
rlm@160 307 (.setLimit (float limit-xz)
rlm@160 308 (float limit-xy)
rlm@160 309 (float twist)))))
rlm@160 310
rlm@160 311 (defn connect
rlm@175 312 "Create a joint between 'obj-a and 'obj-b at the location of
rlm@175 313 'joint. The type of joint is determined by the metadata on 'joint.
rlm@175 314
rlm@175 315 Here are some examples:
rlm@160 316 {:type :point}
rlm@160 317 {:type :hinge :limit [0 (/ Math/PI 2)] :axis (Vector3f. 0 1 0)}
rlm@160 318 (:axis defaults to (Vector3f. 1 0 0) if not provided for hinge joints)
rlm@160 319
rlm@160 320 {:type :cone :limit-xz 0]
rlm@160 321 :limit-xy 0]
rlm@160 322 :twist 0]} (use XZY rotation mode in blender!)"
rlm@160 323 [#^Node obj-a #^Node obj-b #^Node joint]
rlm@160 324 (let [control-a (.getControl obj-a RigidBodyControl)
rlm@160 325 control-b (.getControl obj-b RigidBodyControl)
rlm@160 326 joint-center (.getWorldTranslation joint)
rlm@160 327 joint-rotation (.toRotationMatrix (.getWorldRotation joint))
rlm@160 328 pivot-a (world-to-local obj-a joint-center)
rlm@160 329 pivot-b (world-to-local obj-b joint-center)]
rlm@160 330
rlm@160 331 (if-let [constraints
rlm@160 332 (map-vals
rlm@160 333 eval
rlm@160 334 (read-string
rlm@160 335 (meta-data joint "joint")))]
rlm@160 336 ;; A side-effect of creating a joint registers
rlm@160 337 ;; it with both physics objects which in turn
rlm@160 338 ;; will register the joint with the physics system
rlm@160 339 ;; when the simulation is started.
rlm@160 340 (do
rlm@160 341 (println-repl "creating joint between"
rlm@160 342 (.getName obj-a) "and" (.getName obj-b))
rlm@160 343 (joint-dispatch constraints
rlm@160 344 control-a control-b
rlm@160 345 pivot-a pivot-b
rlm@160 346 joint-rotation))
rlm@160 347 (println-repl "could not find joint meta-data!"))))
rlm@160 348
rlm@175 349 (defvar
rlm@175 350 ^{:arglists '([creature])}
rlm@175 351 joints
rlm@175 352 (sense-nodes "joints")
rlm@175 353 "Return the children of the creature's \"joints\" node.")
rlm@160 354
rlm@175 355 (defn joints!
rlm@175 356 "Connect the solid parts of the creature with physical joints. The
rlm@175 357 joints are taken from the \"joints\" node in the creature."
rlm@175 358 [#^Node creature]
rlm@160 359 (dorun
rlm@160 360 (map
rlm@160 361 (fn [joint]
rlm@175 362 (let [[obj-a obj-b] (joint-targets creature joint)]
rlm@160 363 (connect obj-a obj-b joint)))
rlm@175 364 (joints creature))))
rlm@160 365
rlm@175 366 (defn body!
rlm@175 367 "Endow the creature with a physical body connected with joints. The
rlm@175 368 particulars of the joints and the masses of each pody part are
rlm@175 369 determined in blender."
rlm@175 370 [#^Node creature]
rlm@175 371 (physical! creature)
rlm@175 372 (joints! creature))
rlm@64 373 #+end_src
rlm@63 374
rlm@202 375 * Bookkeeping
rlm@175 376
rlm@202 377 #+name: body-0
rlm@202 378 #+begin_src clojure
rlm@202 379 (ns cortex.body
rlm@202 380 "Assemble a physical creature using the definitions found in a
rlm@202 381 specially prepared blender file. Creates rigid bodies and joints so
rlm@202 382 that a creature can have a physical presense in the simulation."
rlm@202 383 {:author "Robert McIntyre"}
rlm@202 384 (:use (cortex world util sense))
rlm@202 385 (:use clojure.contrib.def)
rlm@202 386 (:import
rlm@202 387 (com.jme3.math Vector3f Quaternion Vector2f Matrix3f)
rlm@202 388 (com.jme3.bullet.joints
rlm@202 389 SixDofJoint Point2PointJoint HingeJoint ConeJoint)
rlm@202 390 com.jme3.bullet.control.RigidBodyControl
rlm@202 391 com.jme3.collision.CollisionResults
rlm@202 392 com.jme3.bounding.BoundingBox
rlm@202 393 com.jme3.scene.Node
rlm@202 394 com.jme3.scene.Geometry
rlm@202 395 com.jme3.bullet.collision.shapes.HullCollisionShape))
rlm@202 396 #+end_src
rlm@133 397
rlm@202 398 * Source
rlm@202 399
rlm@202 400 * COMMENT Examples
rlm@63 401
rlm@69 402 #+name: test-body
rlm@64 403 #+begin_src clojure
rlm@69 404 (ns cortex.test.body
rlm@64 405 (:use (cortex world util body))
rlm@135 406 (:require cortex.silly)
rlm@64 407 (:import
rlm@64 408 com.jme3.math.Vector3f
rlm@64 409 com.jme3.math.ColorRGBA
rlm@64 410 com.jme3.bullet.joints.Point2PointJoint
rlm@64 411 com.jme3.bullet.control.RigidBodyControl
rlm@145 412 com.jme3.system.NanoTimer
rlm@145 413 com.jme3.math.Quaternion))
rlm@63 414
rlm@64 415 (defn worm-segments
rlm@64 416 "Create multiple evenly spaced box segments. They're fabulous!"
rlm@64 417 [segment-length num-segments interstitial-space radius]
rlm@64 418 (letfn [(nth-segment
rlm@64 419 [n]
rlm@64 420 (box segment-length radius radius :mass 0.1
rlm@64 421 :position
rlm@64 422 (Vector3f.
rlm@64 423 (* 2 n (+ interstitial-space segment-length)) 0 0)
rlm@64 424 :name (str "worm-segment" n)
rlm@64 425 :color (ColorRGBA/randomColor)))]
rlm@64 426 (map nth-segment (range num-segments))))
rlm@63 427
rlm@64 428 (defn connect-at-midpoint
rlm@64 429 "Connect two physics objects with a Point2Point joint constraint at
rlm@64 430 the point equidistant from both objects' centers."
rlm@64 431 [segmentA segmentB]
rlm@64 432 (let [centerA (.getWorldTranslation segmentA)
rlm@64 433 centerB (.getWorldTranslation segmentB)
rlm@64 434 midpoint (.mult (.add centerA centerB) (float 0.5))
rlm@64 435 pivotA (.subtract midpoint centerA)
rlm@64 436 pivotB (.subtract midpoint centerB)
rlm@64 437
rlm@64 438 ;; A side-effect of creating a joint registers
rlm@64 439 ;; it with both physics objects which in turn
rlm@64 440 ;; will register the joint with the physics system
rlm@64 441 ;; when the simulation is started.
rlm@64 442 joint (Point2PointJoint.
rlm@64 443 (.getControl segmentA RigidBodyControl)
rlm@64 444 (.getControl segmentB RigidBodyControl)
rlm@64 445 pivotA
rlm@64 446 pivotB)]
rlm@64 447 segmentB))
rlm@63 448
rlm@64 449 (defn eve-worm
rlm@72 450 "Create a worm-like body bound by invisible joint constraints."
rlm@64 451 []
rlm@64 452 (let [segments (worm-segments 0.2 5 0.1 0.1)]
rlm@64 453 (dorun (map (partial apply connect-at-midpoint)
rlm@64 454 (partition 2 1 segments)))
rlm@64 455 (nodify "worm" segments)))
rlm@63 456
rlm@64 457 (defn worm-pattern
rlm@64 458 "This is a simple, mindless motor control pattern that drives the
rlm@64 459 second segment of the worm's body at an offset angle with
rlm@64 460 sinusoidally varying strength."
rlm@64 461 [time]
rlm@64 462 (let [angle (* Math/PI (/ 9 20))
rlm@63 463 direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))]
rlm@63 464 [Vector3f/ZERO
rlm@63 465 (.mult
rlm@63 466 direction
rlm@63 467 (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))))
rlm@63 468 Vector3f/ZERO
rlm@63 469 Vector3f/ZERO
rlm@63 470 Vector3f/ZERO]))
rlm@60 471
rlm@64 472 (defn test-motor-control
rlm@69 473 "Testing motor-control:
rlm@69 474 You should see a multi-segmented worm-like object fall onto the
rlm@64 475 table and begin writhing and moving."
rlm@60 476 []
rlm@64 477 (let [worm (eve-worm)
rlm@60 478 time (atom 0)
rlm@63 479 worm-motor-map (vector-motor-control worm)]
rlm@60 480 (world
rlm@60 481 (nodify [worm
rlm@60 482 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
rlm@60 483 :color ColorRGBA/Gray)])
rlm@60 484 standard-debug-controls
rlm@60 485 (fn [world]
rlm@60 486 (enable-debug world)
rlm@60 487 (light-up-everything world)
rlm@63 488 (comment
rlm@63 489 (com.aurellem.capture.Capture/captureVideo
rlm@63 490 world
rlm@63 491 (file-str "/home/r/proj/cortex/tmp/moving-worm")))
rlm@63 492 )
rlm@60 493
rlm@60 494 (fn [_ _]
rlm@60 495 (swap! time inc)
rlm@64 496 (Thread/sleep 20)
rlm@60 497 (dorun (worm-motor-map
rlm@60 498 (worm-pattern @time)))))))
rlm@60 499
rlm@130 500
rlm@135 501
rlm@130 502 (defn join-at-point [obj-a obj-b world-pivot]
rlm@130 503 (cortex.silly/joint-dispatch
rlm@130 504 {:type :point}
rlm@130 505 (.getControl obj-a RigidBodyControl)
rlm@130 506 (.getControl obj-b RigidBodyControl)
rlm@130 507 (cortex.silly/world-to-local obj-a world-pivot)
rlm@130 508 (cortex.silly/world-to-local obj-b world-pivot)
rlm@130 509 nil
rlm@130 510 ))
rlm@130 511
rlm@133 512 (import com.jme3.bullet.collision.PhysicsCollisionObject)
rlm@130 513
rlm@130 514 (defn blab-* []
rlm@130 515 (let [hand (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0)
rlm@130 516 :mass 0 :color ColorRGBA/Green)
rlm@130 517 finger (box 0.5 0.2 0.2 :position (Vector3f. 2.4 0 0)
rlm@130 518 :mass 1 :color ColorRGBA/Red)
rlm@130 519 connection-point (Vector3f. 1.2 0 0)
rlm@130 520 root (nodify [hand finger])]
rlm@130 521
rlm@130 522 (join-at-point hand finger (Vector3f. 1.2 0 0))
rlm@130 523
rlm@130 524 (.setCollisionGroup
rlm@130 525 (.getControl hand RigidBodyControl)
rlm@130 526 PhysicsCollisionObject/COLLISION_GROUP_NONE)
rlm@130 527 (world
rlm@130 528 root
rlm@130 529 standard-debug-controls
rlm@130 530 (fn [world]
rlm@130 531 (enable-debug world)
rlm@130 532 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
rlm@130 533 (set-gravity world Vector3f/ZERO)
rlm@130 534 )
rlm@130 535 no-op)))
rlm@133 536 (comment
rlm@133 537
rlm@133 538 (defn proprioception-debug-window
rlm@133 539 []
rlm@133 540 (let [time (atom 0)]
rlm@133 541 (fn [prop-data]
rlm@133 542 (if (= 0 (rem (swap! time inc) 40))
rlm@133 543 (println-repl prop-data)))))
rlm@133 544 )
rlm@133 545
rlm@131 546 (comment
rlm@131 547 (dorun
rlm@131 548 (map
rlm@131 549 (comp
rlm@131 550 println-repl
rlm@131 551 (fn [[p y r]]
rlm@131 552 (format
rlm@131 553 "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
rlm@131 554 p y r)))
rlm@131 555 prop-data)))
rlm@131 556
rlm@130 557
rlm@130 558
rlm@137 559
rlm@64 560 (defn test-proprioception
rlm@69 561 "Testing proprioception:
rlm@69 562 You should see two foating bars, and a printout of pitch, yaw, and
rlm@64 563 roll. Pressing key-r/key-t should move the blue bar up and down and
rlm@64 564 change only the value of pitch. key-f/key-g moves it side to side
rlm@64 565 and changes yaw. key-v/key-b will spin the blue segment clockwise
rlm@64 566 and counterclockwise, and only affect roll."
rlm@60 567 []
rlm@145 568 (let [hand (box 0.2 1 0.2 :position (Vector3f. 0 0 0)
rlm@142 569 :mass 0 :color ColorRGBA/Green :name "hand")
rlm@145 570 finger (box 0.2 1 0.2 :position (Vector3f. 0 2.4 0)
rlm@132 571 :mass 1 :color ColorRGBA/Red :name "finger")
rlm@133 572 joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
rlm@145 573 :position (Vector3f. 0 1.2 0)
rlm@145 574 :rotation (doto (Quaternion.)
rlm@145 575 (.fromAngleAxis
rlm@145 576 (/ Math/PI 2)
rlm@145 577 (Vector3f. 0 0 1)))
rlm@133 578 :physical? false)
rlm@145 579 joint (join-at-point hand finger (Vector3f. 0 1.2 0 ))
rlm@135 580 creature (nodify [hand finger joint-node])
rlm@145 581 finger-control (.getControl finger RigidBodyControl)
rlm@145 582 hand-control (.getControl hand RigidBodyControl)]
rlm@145 583
rlm@145 584
rlm@145 585 (let
rlm@135 586 ;; *******************************************
rlm@137 587
rlm@145 588 [floor (box 10 10 10 :position (Vector3f. 0 -15 0)
rlm@135 589 :mass 0 :color ColorRGBA/Gray)
rlm@137 590
rlm@137 591 root (nodify [creature floor])
rlm@133 592 prop (joint-proprioception creature joint-node)
rlm@139 593 prop-view (proprioception-debug-window)
rlm@139 594
rlm@139 595 controls
rlm@139 596 (merge standard-debug-controls
rlm@140 597 {"key-o"
rlm@139 598 (fn [_ _] (.setEnabled finger-control true))
rlm@140 599 "key-p"
rlm@139 600 (fn [_ _] (.setEnabled finger-control false))
rlm@140 601 "key-k"
rlm@140 602 (fn [_ _] (.setEnabled hand-control true))
rlm@140 603 "key-l"
rlm@140 604 (fn [_ _] (.setEnabled hand-control false))
rlm@139 605 "key-i"
rlm@139 606 (fn [world _] (set-gravity world (Vector3f. 0 0 0)))
rlm@142 607 "key-period"
rlm@142 608 (fn [world _]
rlm@142 609 (.setEnabled finger-control false)
rlm@142 610 (.setEnabled hand-control false)
rlm@142 611 (.rotate creature (doto (Quaternion.)
rlm@142 612 (.fromAngleAxis
rlm@142 613 (float (/ Math/PI 15))
rlm@142 614 (Vector3f. 0 0 -1))))
rlm@142 615
rlm@142 616 (.setEnabled finger-control true)
rlm@142 617 (.setEnabled hand-control true)
rlm@142 618 (set-gravity world (Vector3f. 0 0 0))
rlm@142 619 )
rlm@142 620
rlm@142 621
rlm@139 622 }
rlm@139 623 )
rlm@130 624
rlm@139 625 ]
rlm@139 626 (comment
rlm@139 627 (.setCollisionGroup
rlm@139 628 (.getControl hand RigidBodyControl)
rlm@139 629 PhysicsCollisionObject/COLLISION_GROUP_NONE)
rlm@139 630 )
rlm@140 631 (apply
rlm@140 632 world
rlm@140 633 (with-movement
rlm@140 634 hand
rlm@140 635 ["key-y" "key-u" "key-h" "key-j" "key-n" "key-m"]
rlm@140 636 [10 10 10 10 1 1]
rlm@140 637 (with-movement
rlm@140 638 finger
rlm@140 639 ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]
rlm@145 640 [1 1 10 10 10 10]
rlm@140 641 [root
rlm@140 642 controls
rlm@140 643 (fn [world]
rlm@140 644 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
rlm@140 645 (set-gravity world (Vector3f. 0 0 0))
rlm@140 646 (light-up-everything world))
rlm@145 647 (fn [_ _] (prop-view (list (prop))))]))))))
rlm@138 648
rlm@64 649 #+end_src
rlm@56 650
rlm@130 651 #+results: test-body
rlm@130 652 : #'cortex.test.body/test-proprioception
rlm@130 653
rlm@60 654
rlm@63 655 * COMMENT code-limbo
rlm@61 656 #+begin_src clojure
rlm@61 657 ;;(.loadModel
rlm@61 658 ;; (doto (asset-manager)
rlm@61 659 ;; (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@61 660 ;; "Models/person/person.blend")
rlm@61 661
rlm@64 662
rlm@64 663 (defn load-blender-model
rlm@64 664 "Load a .blend file using an asset folder relative path."
rlm@64 665 [^String model]
rlm@64 666 (.loadModel
rlm@64 667 (doto (asset-manager)
rlm@64 668 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@64 669 model))
rlm@64 670
rlm@64 671
rlm@61 672 (defn view-model [^String model]
rlm@61 673 (view
rlm@61 674 (.loadModel
rlm@61 675 (doto (asset-manager)
rlm@61 676 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@61 677 model)))
rlm@61 678
rlm@61 679 (defn load-blender-scene [^String model]
rlm@61 680 (.loadModel
rlm@61 681 (doto (asset-manager)
rlm@61 682 (.registerLoader BlenderLoader (into-array String ["blend"])))
rlm@61 683 model))
rlm@61 684
rlm@61 685 (defn worm
rlm@61 686 []
rlm@61 687 (.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml"))
rlm@61 688
rlm@61 689 (defn oto
rlm@61 690 []
rlm@61 691 (.loadModel (asset-manager) "Models/Oto/Oto.mesh.xml"))
rlm@61 692
rlm@61 693 (defn sinbad
rlm@61 694 []
rlm@61 695 (.loadModel (asset-manager) "Models/Sinbad/Sinbad.mesh.xml"))
rlm@61 696
rlm@61 697 (defn worm-blender
rlm@61 698 []
rlm@61 699 (first (seq (.getChildren (load-blender-model
rlm@61 700 "Models/anim2/simple-worm.blend")))))
rlm@61 701
rlm@61 702 (defn body
rlm@61 703 "given a node with a SkeletonControl, will produce a body sutiable
rlm@61 704 for AI control with movement and proprioception."
rlm@61 705 [node]
rlm@61 706 (let [skeleton-control (.getControl node SkeletonControl)
rlm@61 707 krc (KinematicRagdollControl.)]
rlm@61 708 (comment
rlm@61 709 (dorun
rlm@61 710 (map #(.addBoneName krc %)
rlm@61 711 ["mid2" "tail" "head" "mid1" "mid3" "mid4" "Dummy-Root" ""]
rlm@61 712 ;;"mid2" "mid3" "tail" "head"]
rlm@61 713 )))
rlm@61 714 (.addControl node krc)
rlm@61 715 (.setRagdollMode krc)
rlm@61 716 )
rlm@61 717 node
rlm@61 718 )
rlm@61 719 (defn show-skeleton [node]
rlm@61 720 (let [sd
rlm@61 721
rlm@61 722 (doto
rlm@61 723 (SkeletonDebugger. "aurellem-skel-debug"
rlm@61 724 (skel node))
rlm@61 725 (.setMaterial (green-x-ray)))]
rlm@61 726 (.attachChild node sd)
rlm@61 727 node))
rlm@61 728
rlm@61 729
rlm@61 730
rlm@61 731 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61 732
rlm@61 733 ;; this could be a good way to give objects special properties like
rlm@61 734 ;; being eyes and the like
rlm@61 735
rlm@61 736 (.getUserData
rlm@61 737 (.getChild
rlm@61 738 (load-blender-model "Models/property/test.blend") 0)
rlm@61 739 "properties")
rlm@61 740
rlm@61 741 ;; the properties are saved along with the blender file.
rlm@61 742 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61 743
rlm@61 744
rlm@61 745
rlm@61 746
rlm@61 747 (defn init-debug-skel-node
rlm@61 748 [f debug-node skeleton]
rlm@61 749 (let [bones
rlm@61 750 (map #(.getBone skeleton %)
rlm@61 751 (range (.getBoneCount skeleton)))]
rlm@61 752 (dorun (map #(.setUserControl % true) bones))
rlm@61 753 (dorun (map (fn [b]
rlm@61 754 (println (.getName b)
rlm@61 755 " -- " (f b)))
rlm@61 756 bones))
rlm@61 757 (dorun
rlm@61 758 (map #(.attachChild
rlm@61 759 debug-node
rlm@61 760 (doto
rlm@61 761 (sphere 0.1
rlm@61 762 :position (f %)
rlm@61 763 :physical? false)
rlm@61 764 (.setMaterial (green-x-ray))))
rlm@61 765 bones)))
rlm@61 766 debug-node)
rlm@61 767
rlm@61 768 (import jme3test.bullet.PhysicsTestHelper)
rlm@61 769
rlm@61 770
rlm@61 771 (defn test-zzz [the-worm world value]
rlm@61 772 (if (not value)
rlm@61 773 (let [skeleton (skel the-worm)]
rlm@61 774 (println-repl "enabling bones")
rlm@61 775 (dorun
rlm@61 776 (map
rlm@61 777 #(.setUserControl (.getBone skeleton %) true)
rlm@61 778 (range (.getBoneCount skeleton))))
rlm@61 779
rlm@61 780
rlm@61 781 (let [b (.getBone skeleton 2)]
rlm@61 782 (println-repl "moving " (.getName b))
rlm@61 783 (println-repl (.getLocalPosition b))
rlm@61 784 (.setUserTransforms b
rlm@61 785 Vector3f/UNIT_X
rlm@61 786 Quaternion/IDENTITY
rlm@61 787 ;;(doto (Quaternion.)
rlm@61 788 ;; (.fromAngles (/ Math/PI 2)
rlm@61 789 ;; 0
rlm@61 790 ;; 0
rlm@61 791
rlm@61 792 (Vector3f. 1 1 1))
rlm@61 793 )
rlm@61 794
rlm@61 795 (println-repl "hi! <3"))))
rlm@61 796
rlm@61 797
rlm@61 798 (defn test-ragdoll []
rlm@61 799
rlm@61 800 (let [the-worm
rlm@61 801
rlm@61 802 ;;(.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml")
rlm@61 803 (doto (show-skeleton (worm-blender))
rlm@61 804 (.setLocalTranslation (Vector3f. 0 10 0))
rlm@61 805 ;;(worm)
rlm@61 806 ;;(oto)
rlm@61 807 ;;(sinbad)
rlm@61 808 )
rlm@61 809 ]
rlm@61 810
rlm@61 811
rlm@61 812 (.start
rlm@61 813 (world
rlm@61 814 (doto (Node.)
rlm@61 815 (.attachChild the-worm))
rlm@61 816 {"key-return" (fire-cannon-ball)
rlm@61 817 "key-space" (partial test-zzz the-worm)
rlm@61 818 }
rlm@61 819 (fn [world]
rlm@61 820 (light-up-everything world)
rlm@61 821 (PhysicsTestHelper/createPhysicsTestWorld
rlm@61 822 (.getRootNode world)
rlm@61 823 (asset-manager)
rlm@61 824 (.getPhysicsSpace
rlm@61 825 (.getState (.getStateManager world) BulletAppState)))
rlm@61 826 (set-gravity world Vector3f/ZERO)
rlm@61 827 ;;(.setTimer world (NanoTimer.))
rlm@61 828 ;;(org.lwjgl.input.Mouse/setGrabbed false)
rlm@61 829 )
rlm@61 830 no-op
rlm@61 831 )
rlm@61 832
rlm@61 833
rlm@61 834 )))
rlm@61 835
rlm@61 836
rlm@61 837 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61 838 ;;; here is the ragdoll stuff
rlm@61 839
rlm@61 840 (def worm-mesh (.getMesh (.getChild (worm-blender) 0)))
rlm@61 841 (def mesh worm-mesh)
rlm@61 842
rlm@61 843 (.getFloatBuffer mesh VertexBuffer$Type/Position)
rlm@61 844 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
rlm@61 845 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
rlm@61 846
rlm@61 847
rlm@61 848 (defn position [index]
rlm@61 849 (.get
rlm@61 850 (.getFloatBuffer worm-mesh VertexBuffer$Type/Position)
rlm@61 851 index))
rlm@61 852
rlm@61 853 (defn bones [index]
rlm@61 854 (.get
rlm@61 855 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
rlm@61 856 index))
rlm@61 857
rlm@61 858 (defn bone-weights [index]
rlm@61 859 (.get
rlm@61 860 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
rlm@61 861 index))
rlm@61 862
rlm@61 863
rlm@61 864
rlm@61 865 (defn vertex-bones [vertex]
rlm@61 866 (vec (map (comp int bones) (range (* vertex 4) (+ (* vertex 4) 4)))))
rlm@61 867
rlm@61 868 (defn vertex-weights [vertex]
rlm@61 869 (vec (map (comp float bone-weights) (range (* vertex 4) (+ (* vertex 4) 4)))))
rlm@61 870
rlm@61 871 (defn vertex-position [index]
rlm@61 872 (let [offset (* index 3)]
rlm@61 873 (Vector3f. (position offset)
rlm@61 874 (position (inc offset))
rlm@61 875 (position (inc(inc offset))))))
rlm@61 876
rlm@61 877 (def vertex-info (juxt vertex-position vertex-bones vertex-weights))
rlm@61 878
rlm@61 879 (defn bone-control-color [index]
rlm@61 880 (get {[1 0 0 0] ColorRGBA/Red
rlm@61 881 [1 2 0 0] ColorRGBA/Magenta
rlm@61 882 [2 0 0 0] ColorRGBA/Blue}
rlm@61 883 (vertex-bones index)
rlm@61 884 ColorRGBA/White))
rlm@61 885
rlm@61 886 (defn influence-color [index bone-num]
rlm@61 887 (get
rlm@61 888 {(float 0) ColorRGBA/Blue
rlm@61 889 (float 0.5) ColorRGBA/Green
rlm@61 890 (float 1) ColorRGBA/Red}
rlm@61 891 ;; find the weight of the desired bone
rlm@61 892 ((zipmap (vertex-bones index)(vertex-weights index))
rlm@61 893 bone-num)
rlm@61 894 ColorRGBA/Blue))
rlm@61 895
rlm@61 896 (def worm-vertices (set (map vertex-info (range 60))))
rlm@61 897
rlm@61 898
rlm@61 899 (defn test-info []
rlm@61 900 (let [points (Node.)]
rlm@61 901 (dorun
rlm@61 902 (map #(.attachChild points %)
rlm@61 903 (map #(sphere 0.01
rlm@61 904 :position (vertex-position %)
rlm@61 905 :color (influence-color % 1)
rlm@61 906 :physical? false)
rlm@61 907 (range 60))))
rlm@61 908 (view points)))
rlm@61 909
rlm@61 910
rlm@61 911 (defrecord JointControl [joint physics-space]
rlm@61 912 PhysicsControl
rlm@61 913 (setPhysicsSpace [this space]
rlm@61 914 (dosync
rlm@61 915 (ref-set (:physics-space this) space))
rlm@61 916 (.addJoint space (:joint this)))
rlm@61 917 (update [this tpf])
rlm@61 918 (setSpatial [this spatial])
rlm@61 919 (render [this rm vp])
rlm@61 920 (getPhysicsSpace [this] (deref (:physics-space this)))
rlm@61 921 (isEnabled [this] true)
rlm@61 922 (setEnabled [this state]))
rlm@61 923
rlm@61 924 (defn add-joint
rlm@61 925 "Add a joint to a particular object. When the object is added to the
rlm@61 926 PhysicsSpace of a simulation, the joint will also be added"
rlm@61 927 [object joint]
rlm@61 928 (let [control (JointControl. joint (ref nil))]
rlm@61 929 (.addControl object control))
rlm@61 930 object)
rlm@61 931
rlm@61 932
rlm@61 933 (defn hinge-world
rlm@61 934 []
rlm@61 935 (let [sphere1 (sphere)
rlm@61 936 sphere2 (sphere 1 :position (Vector3f. 3 3 3))
rlm@61 937 joint (Point2PointJoint.
rlm@61 938 (.getControl sphere1 RigidBodyControl)
rlm@61 939 (.getControl sphere2 RigidBodyControl)
rlm@61 940 Vector3f/ZERO (Vector3f. 3 3 3))]
rlm@61 941 (add-joint sphere1 joint)
rlm@61 942 (doto (Node. "hinge-world")
rlm@61 943 (.attachChild sphere1)
rlm@61 944 (.attachChild sphere2))))
rlm@61 945
rlm@61 946
rlm@61 947 (defn test-joint []
rlm@61 948 (view (hinge-world)))
rlm@61 949
rlm@61 950 ;; (defn copier-gen []
rlm@61 951 ;; (let [count (atom 0)]
rlm@61 952 ;; (fn [in]
rlm@61 953 ;; (swap! count inc)
rlm@61 954 ;; (clojure.contrib.duck-streams/copy
rlm@61 955 ;; in (File. (str "/home/r/tmp/mao-test/clojure-images/"
rlm@61 956 ;; ;;/home/r/tmp/mao-test/clojure-images
rlm@61 957 ;; (format "%08d.png" @count)))))))
rlm@61 958 ;; (defn decrease-framerate []
rlm@61 959 ;; (map
rlm@61 960 ;; (copier-gen)
rlm@61 961 ;; (sort
rlm@61 962 ;; (map first
rlm@61 963 ;; (partition
rlm@61 964 ;; 4
rlm@61 965 ;; (filter #(re-matches #".*.png$" (.getCanonicalPath %))
rlm@61 966 ;; (file-seq
rlm@61 967 ;; (file-str
rlm@61 968 ;; "/home/r/media/anime/mao-temp/images"))))))))
rlm@61 969
rlm@61 970
rlm@61 971
rlm@61 972 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61 973
rlm@61 974 (defn proprioception
rlm@61 975 "Create a proprioception map that reports the rotations of the
rlm@61 976 various limbs of the creature's body"
rlm@61 977 [creature]
rlm@61 978 [#^Node creature]
rlm@61 979 (let [
rlm@61 980 nodes (node-seq creature)
rlm@61 981 joints
rlm@61 982 (map
rlm@61 983 :joint
rlm@61 984 (filter
rlm@61 985 #(isa? (class %) JointControl)
rlm@61 986 (reduce
rlm@61 987 concat
rlm@61 988 (map (fn [node]
rlm@61 989 (map (fn [num] (.getControl node num))
rlm@61 990 (range (.getNumControls node))))
rlm@61 991 nodes))))]
rlm@61 992 (fn []
rlm@61 993 (reduce concat (map relative-positions (list (first joints)))))))
rlm@61 994
rlm@61 995
rlm@63 996 (defn skel [node]
rlm@63 997 (doto
rlm@63 998 (.getSkeleton
rlm@63 999 (.getControl node SkeletonControl))
rlm@63 1000 ;; this is necessary to force the skeleton to have accurate world
rlm@63 1001 ;; transforms before it is rendered to the screen.
rlm@63 1002 (.resetAndUpdate)))
rlm@63 1003
rlm@63 1004 (defn green-x-ray []
rlm@63 1005 (doto (Material. (asset-manager)
rlm@63 1006 "Common/MatDefs/Misc/Unshaded.j3md")
rlm@63 1007 (.setColor "Color" ColorRGBA/Green)
rlm@63 1008 (-> (.getAdditionalRenderState)
rlm@63 1009 (.setDepthTest false))))
rlm@63 1010
rlm@63 1011 (defn test-worm []
rlm@63 1012 (.start
rlm@63 1013 (world
rlm@63 1014 (doto (Node.)
rlm@63 1015 ;;(.attachChild (point-worm))
rlm@63 1016 (.attachChild (load-blender-model
rlm@63 1017 "Models/anim2/joint-worm.blend"))
rlm@63 1018
rlm@63 1019 (.attachChild (box 10 1 10
rlm@63 1020 :position (Vector3f. 0 -2 0) :mass 0
rlm@63 1021 :color (ColorRGBA/Gray))))
rlm@63 1022 {
rlm@63 1023 "key-space" (fire-cannon-ball)
rlm@63 1024 }
rlm@63 1025 (fn [world]
rlm@63 1026 (enable-debug world)
rlm@63 1027 (light-up-everything world)
rlm@63 1028 ;;(.setTimer world (NanoTimer.))
rlm@63 1029 )
rlm@63 1030 no-op)))
rlm@63 1031
rlm@63 1032
rlm@63 1033
rlm@63 1034 ;; defunct movement stuff
rlm@63 1035 (defn torque-controls [control]
rlm@63 1036 (let [torques
rlm@63 1037 (concat
rlm@63 1038 (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
rlm@63 1039 (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
rlm@63 1040 [Vector3f/UNIT_X])]
rlm@63 1041 (map (fn [torque-axis]
rlm@63 1042 (fn [torque]
rlm@63 1043 (.applyTorque
rlm@63 1044 control
rlm@63 1045 (.mult (.mult (.getPhysicsRotation control)
rlm@63 1046 torque-axis)
rlm@63 1047 (float
rlm@63 1048 (* (.getMass control) torque))))))
rlm@63 1049 torques)))
rlm@63 1050
rlm@63 1051 (defn motor-map
rlm@63 1052 "Take a creature and generate a function that will enable fine
rlm@63 1053 grained control over all the creature's limbs."
rlm@63 1054 [#^Node creature]
rlm@63 1055 (let [controls (keep #(.getControl % RigidBodyControl)
rlm@63 1056 (node-seq creature))
rlm@63 1057 limb-controls (reduce concat (map torque-controls controls))
rlm@63 1058 body-control (partial map #(%1 %2) limb-controls)]
rlm@63 1059 body-control))
rlm@63 1060
rlm@63 1061 (defn test-motor-map
rlm@63 1062 "see how torque works."
rlm@63 1063 []
rlm@63 1064 (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
rlm@63 1065 :mass 1 :color ColorRGBA/Green)
rlm@63 1066 motor-map (motor-map finger)]
rlm@63 1067 (world
rlm@63 1068 (nodify [finger
rlm@63 1069 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
rlm@63 1070 :color ColorRGBA/Gray)])
rlm@63 1071 standard-debug-controls
rlm@63 1072 (fn [world]
rlm@63 1073 (set-gravity world Vector3f/ZERO)
rlm@63 1074 (light-up-everything world)
rlm@63 1075 (.setTimer world (NanoTimer.)))
rlm@63 1076 (fn [_ _]
rlm@145 1077 (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
rlm@145 1078 0]))))))
rlm@145 1079
rlm@145 1080 (defn joint-proprioception [#^Node parts #^Node joint]
rlm@145 1081 (let [[obj-a obj-b] (joint-targets parts joint)
rlm@145 1082 joint-rot (.getWorldRotation joint)
rlm@145 1083 pre-inv-a (.inverse (.getWorldRotation obj-a))
rlm@145 1084 x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X))
rlm@145 1085 y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y))
rlm@145 1086 z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z))
rlm@145 1087
rlm@145 1088 x Vector3f/UNIT_Y
rlm@145 1089 y Vector3f/UNIT_Z
rlm@145 1090 z Vector3f/UNIT_X
rlm@145 1091
rlm@145 1092
rlm@145 1093 tmp-rot-a (.getWorldRotation obj-a)]
rlm@145 1094 (println-repl "x:" (.mult tmp-rot-a x))
rlm@145 1095 (println-repl "y:" (.mult tmp-rot-a y))
rlm@145 1096 (println-repl "z:" (.mult tmp-rot-a z))
rlm@145 1097 (println-repl "rot-a" (.getWorldRotation obj-a))
rlm@145 1098 (println-repl "rot-b" (.getWorldRotation obj-b))
rlm@145 1099 (println-repl "joint-rot" joint-rot)
rlm@145 1100 ;; this function will report proprioceptive information for the
rlm@145 1101 ;; joint.
rlm@145 1102 (fn []
rlm@145 1103 ;; x is the "twist" axis, y and z are the "bend" axes
rlm@145 1104 (let [rot-a (.getWorldRotation obj-a)
rlm@145 1105 ;;inv-a (.inverse rot-a)
rlm@145 1106 rot-b (.getWorldRotation obj-b)
rlm@145 1107 ;;relative (.mult rot-b inv-a)
rlm@145 1108 basis (doto (Matrix3f.)
rlm@145 1109 (.setColumn 0 (.mult rot-a x))
rlm@145 1110 (.setColumn 1 (.mult rot-a y))
rlm@145 1111 (.setColumn 2 (.mult rot-a z)))
rlm@145 1112 rotation-about-joint
rlm@145 1113 (doto (Quaternion.)
rlm@145 1114 (.fromRotationMatrix
rlm@145 1115 (.mult (.invert basis)
rlm@145 1116 (.toRotationMatrix rot-b))))
rlm@145 1117 [yaw roll pitch]
rlm@145 1118 (seq (.toAngles rotation-about-joint nil))]
rlm@145 1119 ;;return euler angles of the quaternion around the new basis
rlm@145 1120 [yaw roll pitch]))))
rlm@145 1121
rlm@61 1122 #+end_src
rlm@0 1123
rlm@0 1124
rlm@0 1125
rlm@0 1126
rlm@0 1127
rlm@0 1128
rlm@0 1129
rlm@73 1130 * COMMENT generate Source
rlm@44 1131 #+begin_src clojure :tangle ../src/cortex/body.clj
rlm@175 1132 <<joints>>
rlm@0 1133 #+end_src
rlm@64 1134
rlm@69 1135 #+begin_src clojure :tangle ../src/cortex/test/body.clj
rlm@202 1136 <<test-0>>
rlm@64 1137 #+end_src
rlm@64 1138
rlm@64 1139
rlm@0 1140