rlm@202: #+title: Building a Body
rlm@0: #+author: Robert McIntyre
rlm@0: #+email: rlm@mit.edu
rlm@4: #+description: Simulating a body (movement, touch, propioception) in jMonkeyEngine3.
rlm@4: #+SETUPFILE: ../../aurellem/org/setup.org
rlm@4: #+INCLUDE: ../../aurellem/org/level-0.org
rlm@4:
rlm@202:
rlm@202: * Design Constraints
rlm@202:
rlm@202: I use [[www.blender.org/][blender]] to design bodies. The design of the bodies is
rlm@202: determined by the requirements of the AI that will use them. The
rlm@202: bodies must be easy for an AI to sense and control, and they must be
rlm@202: relatively simple for jMonkeyEngine to compute.
rlm@202:
rlm@202: ** Bag of Bones
rlm@202:
rlm@202: How to create such a body? One option I ultimately rejected is to use
rlm@202: 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: which describes the creature's entire body. To this you add an
rlm@202: (skeleton) which deforms this mesh. This technique is used extensively
rlm@202: to model humans and create realistic animations. It is hard to use for
rlm@202: my purposes because it is difficult to update the creature's Physics
rlm@202: Collision Mesh in tandem with its Geometric Mesh under the influence
rlm@202: of the armature. Withouth this the creature will not be able to grab
rlm@202: things in its environment, and it won't be able to tell where its
rlm@202: physical body is by using its eyes. Also, armatures do not specify
rlm@202: any rotational limits for a joint, making it hard to model elbows,
rlm@202: shoulders, etc.
rlm@202:
rlm@202: ** EVE
rlm@202:
rlm@202: Instead of using the human-like "deformable bag of bones" approach, I
rlm@202: decided to base my body plans on the robot EVE from the movie wall-E.
rlm@202:
rlm@202: #+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: [[../images/Eve.jpg]]
rlm@202:
rlm@204: EVE's body is composed of several rigid components that are held
rlm@204: together by invisible joint constraints. This is what I mean by
rlm@204: "eve-like". The main reason that I use eve-style bodies is so that
rlm@204: there will be correspondence between the AI's vision and the physical
rlm@204: presence of its body. Each individual section is simulated by a
rlm@204: separate rigid body that corresponds exactly with its visual
rlm@204: representation and does not change. Sections are connected by
rlm@204: invisible joints that are well supported in jMonkyeEngine. Bullet, the
rlm@204: physics backend for jMonkeyEngine, can efficiently simulate hundreds
rlm@204: of rigid bodies connected by joints. Sections do not have to stay as
rlm@204: one piece forever; they can be dynamically replaced with multiple
rlm@204: sections to simulate splitting in two. This could be used to simulate
rlm@204: retractable claws or EVE's hands, which could coalece into one object
rlm@204: in the movie.
rlm@202:
rlm@202: * Solidifying the Body
rlm@202:
rlm@202: Here is a hand designed eve-style in blender.
rlm@202:
rlm@203: #+attr_html: width="755"
rlm@202: [[../images/hand-screenshot0.png]]
rlm@202:
rlm@202: If we load it directly into jMonkeyEngine, we get this:
rlm@202:
rlm@202: #+name: test-0
rlm@202: #+begin_src clojure
rlm@202: (ns cortex.test.body
rlm@202: (:use (cortex world util body))
rlm@202: (:import (com.aurellem.capture Capture RatchetTimer)
rlm@202: (com.jme3.math Quaternion Vector3f)
rlm@202: java.io.File))
rlm@202:
rlm@202: (def hand-path "Models/test-creature/hand.blend")
rlm@202:
rlm@202: (defn hand [] (load-blender-model hand-path))
rlm@202:
rlm@202: (defn setup [world]
rlm@202: (let [cam (.getCamera world)]
rlm@202: (println-repl cam)
rlm@202: (.setLocation
rlm@202: cam (Vector3f.
rlm@202: -6.9015837, 8.644911, 5.6043186))
rlm@202: (.setRotation
rlm@202: cam
rlm@202: (Quaternion.
rlm@202: 0.14046453, 0.85894054, -0.34301838, 0.3533118)))
rlm@202: (light-up-everything world)
rlm@202: (.setTimer world (RatchetTimer. 60))
rlm@202: world)
rlm@202:
rlm@202: (defn test-one []
rlm@202: (world (hand)
rlm@202: standard-debug-controls
rlm@202: (comp
rlm@202: #(Capture/captureVideo
rlm@202: % (File. "/home/r/proj/cortex/render/body/1"))
rlm@202: setup)
rlm@202: no-op))
rlm@202: #+end_src
rlm@202:
rlm@202:
rlm@202: #+begin_src clojure :results silent
rlm@202: (.start (cortex.test.body/test-one))
rlm@202: #+end_src
rlm@202:
rlm@202: #+begin_html
rlm@203:
rlm@202: #+end_html
rlm@202:
rlm@202: You will notice that the hand has no physical presence -- it's a
rlm@204: hologram through which everything passes. Therefore, the first thing
rlm@202: to do is to make it solid. Blender has physics simulation on par with
rlm@202: jMonkeyEngine (they both use bullet as their physics backend), but it
rlm@202: can be difficult to translate between the two systems, so for now I
rlm@202: specify the mass of each object in blender and construct the physics
rlm@202: shape based on the mesh in jMonkeyEngine.
rlm@202:
rlm@203: #+name: body-1
rlm@202: #+begin_src clojure
rlm@202: (defn physical!
rlm@202: "Iterate through the nodes in creature and make them real physical
rlm@202: objects in the simulation."
rlm@202: [#^Node creature]
rlm@202: (dorun
rlm@202: (map
rlm@202: (fn [geom]
rlm@202: (let [physics-control
rlm@202: (RigidBodyControl.
rlm@202: (HullCollisionShape.
rlm@202: (.getMesh geom))
rlm@202: (if-let [mass (meta-data geom "mass")]
rlm@202: (do
rlm@202: (println-repl
rlm@202: "setting" (.getName geom) "mass to" (float mass))
rlm@202: (float mass))
rlm@202: (float 1)))]
rlm@202: (.addControl geom physics-control)))
rlm@202: (filter #(isa? (class %) Geometry )
rlm@202: (node-seq creature)))))
rlm@202: #+end_src
rlm@202:
rlm@202: =(physical!)= iterates through a creature's node structure, creating
rlm@202: CollisionShapes for each geometry with the mass specified in that
rlm@202: geometry's meta-data.
rlm@202:
rlm@203: #+name: test-1
rlm@0: #+begin_src clojure
rlm@202: (in-ns 'cortex.test.body)
rlm@160:
rlm@202: (def normal-gravity
rlm@202: {"key-g" (fn [world _]
rlm@202: (set-gravity world (Vector3f. 0 -9.81 0)))})
rlm@202:
rlm@202: (defn floor []
rlm@202: (box 10 3 10 :position (Vector3f. 0 -10 0)
rlm@202: :color ColorRGBA/Gray :mass 0))
rlm@202:
rlm@202: (defn test-two []
rlm@202: (world (nodify
rlm@202: [(doto (hand)
rlm@202: (physical!))
rlm@202: (floor)])
rlm@202: (merge standard-debug-controls normal-gravity)
rlm@202: (comp
rlm@202: #(Capture/captureVideo
rlm@202: % (File. "/home/r/proj/cortex/render/body/2"))
rlm@202: #(do (set-gravity % Vector3f/ZERO) %)
rlm@202: setup)
rlm@202: no-op))
rlm@202: #+end_src
rlm@202:
rlm@202: #+begin_html
rlm@203:
rlm@202: #+end_html
rlm@202:
rlm@202: Now that's some progress.
rlm@202:
rlm@202:
rlm@202: * Joints
rlm@202:
rlm@202: Obviously, an AI is not going to be doing much just lying in pieces on
rlm@202: the floor. So, the next step to making a proper body is to connect
rlm@202: those pieces together with joints. jMonkeyEngine has a large array of
rlm@202: joints available via bullet, such as Point2Point, Cone, Hinge, and a
rlm@202: generic Six Degree of Freedom joint, with or without spring
rlm@202: restitution.
rlm@202:
rlm@202: Although it should be possible to specify the joints using blender's
rlm@202: physics system, and then automatically import them with jMonkeyEngine,
rlm@202: the support isn't there yet, and there are a few problems with bullet
rlm@202: itself that need to be solved before it can happen.
rlm@202:
rlm@202: So, I will use the same system for specifying joints as I will do for
rlm@202: some senses. Each joint is specified by an empty node whose parent
rlm@202: has the name "joints". Their orientation and meta-data determine what
rlm@202: joint is created.
rlm@202:
rlm@203: #+attr_html: width="755"
rlm@203: #+caption: joints hack in blender. Each empty node here will be transformed into a joint in jMonkeyEngine
rlm@202: [[../images/hand-screenshot1.png]]
rlm@202:
rlm@203: The empty node in the upper right, highlighted in yellow, is the
rlm@203: parent node of all the emptys which represent joints. The following
rlm@203: functions must do three things to translate these into real joints:
rlm@202:
rlm@203: - Find the children of the "joints" node.
rlm@203: - Determine the two spatials the joint it meant to connect.
rlm@203: - Create the joint based on the meta-data of the empty node.
rlm@202:
rlm@203: ** Finding the Joints
rlm@203: #+name: joints-2
rlm@203: #+begin_src clojure
rlm@203: (defvar
rlm@203: ^{:arglists '([creature])}
rlm@203: joints
rlm@203: (sense-nodes "joints")
rlm@203: "Return the children of the creature's \"joints\" node.")
rlm@203: #+end_src
rlm@202:
rlm@203: The higher order function =(sense-nodes)= from cortex.sense makes our
rlm@203: first task very easy.
rlm@203:
rlm@203: ** Joint Targets and Orientation
rlm@203:
rlm@203: This technique for finding a joint's targets is very similiar to
rlm@203: =(cortex.sense/closest-node)=. A small cube, centered around the
rlm@203: empty-node, grows exponentially until it intersects two /physical/
rlm@203: objects. The objects are ordered according to the joint's rotation,
rlm@203: with the first one being the object that has more negative coordinates
rlm@203: in the joint's reference frame. Since the objects must be physical,
rlm@203: the empty-node itself escapes detection. Because the objects must be
rlm@203: physical, =(joint-targets)= must be called /after/ =(physical!)= is
rlm@203: called.
rlm@203:
rlm@203: #+name: joints-3
rlm@202: #+begin_src clojure
rlm@135: (defn joint-targets
rlm@135: "Return the two closest two objects to the joint object, ordered
rlm@135: from bottom to top according to the joint's rotation."
rlm@135: [#^Node parts #^Node joint]
rlm@135: (loop [radius (float 0.01)]
rlm@135: (let [results (CollisionResults.)]
rlm@135: (.collideWith
rlm@135: parts
rlm@135: (BoundingBox. (.getWorldTranslation joint)
rlm@135: radius radius radius)
rlm@135: results)
rlm@135: (let [targets
rlm@135: (distinct
rlm@135: (map #(.getGeometry %) results))]
rlm@135: (if (>= (count targets) 2)
rlm@135: (sort-by
rlm@135: #(let [v
rlm@135: (jme-to-blender
rlm@135: (.mult
rlm@135: (.inverse (.getWorldRotation joint))
rlm@135: (.subtract (.getWorldTranslation %)
rlm@135: (.getWorldTranslation joint))))]
rlm@135: (println-repl (.getName %) ":" v)
rlm@135: (.dot (Vector3f. 1 1 1)
rlm@135: v))
rlm@135: (take 2 targets))
rlm@135: (recur (float (* radius 2))))))))
rlm@203: #+end_src
rlm@135:
rlm@203: ** Generating Joints
rlm@203:
rlm@203: This long chunk of code iterates through all the different ways of
rlm@203: specifying joints using blender meta-data and converts each one to the
rlm@203: appropriate jMonkyeEngine joint.
rlm@203:
rlm@203: #+name: joints-4
rlm@203: #+begin_src clojure
rlm@160: (defmulti joint-dispatch
rlm@160: "Translate blender pseudo-joints into real JME joints."
rlm@160: (fn [constraints & _]
rlm@160: (:type constraints)))
rlm@141:
rlm@160: (defmethod joint-dispatch :point
rlm@160: [constraints control-a control-b pivot-a pivot-b rotation]
rlm@160: (println-repl "creating POINT2POINT joint")
rlm@160: ;; bullet's point2point joints are BROKEN, so we must use the
rlm@160: ;; generic 6DOF joint instead of an actual Point2Point joint!
rlm@141:
rlm@160: ;; should be able to do this:
rlm@160: (comment
rlm@160: (Point2PointJoint.
rlm@160: control-a
rlm@160: control-b
rlm@160: pivot-a
rlm@160: pivot-b))
rlm@141:
rlm@160: ;; but instead we must do this:
rlm@160: (println-repl "substuting 6DOF joint for POINT2POINT joint!")
rlm@160: (doto
rlm@160: (SixDofJoint.
rlm@160: control-a
rlm@160: control-b
rlm@160: pivot-a
rlm@160: pivot-b
rlm@160: false)
rlm@160: (.setLinearLowerLimit Vector3f/ZERO)
rlm@203: (.setLinearUpperLimit Vector3f/ZERO)))
rlm@160:
rlm@160: (defmethod joint-dispatch :hinge
rlm@160: [constraints control-a control-b pivot-a pivot-b rotation]
rlm@160: (println-repl "creating HINGE joint")
rlm@160: (let [axis
rlm@160: (if-let
rlm@160: [axis (:axis constraints)]
rlm@160: axis
rlm@160: Vector3f/UNIT_X)
rlm@160: [limit-1 limit-2] (:limit constraints)
rlm@160: hinge-axis
rlm@160: (.mult
rlm@160: rotation
rlm@160: (blender-to-jme axis))]
rlm@160: (doto
rlm@160: (HingeJoint.
rlm@160: control-a
rlm@160: control-b
rlm@160: pivot-a
rlm@160: pivot-b
rlm@160: hinge-axis
rlm@160: hinge-axis)
rlm@160: (.setLimit limit-1 limit-2))))
rlm@160:
rlm@160: (defmethod joint-dispatch :cone
rlm@160: [constraints control-a control-b pivot-a pivot-b rotation]
rlm@160: (let [limit-xz (:limit-xz constraints)
rlm@160: limit-xy (:limit-xy constraints)
rlm@160: twist (:twist constraints)]
rlm@160:
rlm@160: (println-repl "creating CONE joint")
rlm@160: (println-repl rotation)
rlm@160: (println-repl
rlm@160: "UNIT_X --> " (.mult rotation (Vector3f. 1 0 0)))
rlm@160: (println-repl
rlm@160: "UNIT_Y --> " (.mult rotation (Vector3f. 0 1 0)))
rlm@160: (println-repl
rlm@160: "UNIT_Z --> " (.mult rotation (Vector3f. 0 0 1)))
rlm@160: (doto
rlm@160: (ConeJoint.
rlm@160: control-a
rlm@160: control-b
rlm@160: pivot-a
rlm@160: pivot-b
rlm@160: rotation
rlm@160: rotation)
rlm@160: (.setLimit (float limit-xz)
rlm@160: (float limit-xy)
rlm@160: (float twist)))))
rlm@160:
rlm@160: (defn connect
rlm@175: "Create a joint between 'obj-a and 'obj-b at the location of
rlm@175: 'joint. The type of joint is determined by the metadata on 'joint.
rlm@175:
rlm@175: Here are some examples:
rlm@160: {:type :point}
rlm@160: {:type :hinge :limit [0 (/ Math/PI 2)] :axis (Vector3f. 0 1 0)}
rlm@160: (:axis defaults to (Vector3f. 1 0 0) if not provided for hinge joints)
rlm@160:
rlm@160: {:type :cone :limit-xz 0]
rlm@160: :limit-xy 0]
rlm@160: :twist 0]} (use XZY rotation mode in blender!)"
rlm@160: [#^Node obj-a #^Node obj-b #^Node joint]
rlm@160: (let [control-a (.getControl obj-a RigidBodyControl)
rlm@160: control-b (.getControl obj-b RigidBodyControl)
rlm@160: joint-center (.getWorldTranslation joint)
rlm@160: joint-rotation (.toRotationMatrix (.getWorldRotation joint))
rlm@160: pivot-a (world-to-local obj-a joint-center)
rlm@160: pivot-b (world-to-local obj-b joint-center)]
rlm@160:
rlm@160: (if-let [constraints
rlm@160: (map-vals
rlm@160: eval
rlm@160: (read-string
rlm@160: (meta-data joint "joint")))]
rlm@160: ;; A side-effect of creating a joint registers
rlm@160: ;; it with both physics objects which in turn
rlm@160: ;; will register the joint with the physics system
rlm@160: ;; when the simulation is started.
rlm@160: (do
rlm@160: (println-repl "creating joint between"
rlm@160: (.getName obj-a) "and" (.getName obj-b))
rlm@160: (joint-dispatch constraints
rlm@160: control-a control-b
rlm@160: pivot-a pivot-b
rlm@160: joint-rotation))
rlm@160: (println-repl "could not find joint meta-data!"))))
rlm@203: #+end_src
rlm@160:
rlm@203: Creating joints is now a matter applying =(connect)= to each joint
rlm@203: node.
rlm@160:
rlm@203: #+begin_src clojure
rlm@175: (defn joints!
rlm@175: "Connect the solid parts of the creature with physical joints. The
rlm@175: joints are taken from the \"joints\" node in the creature."
rlm@175: [#^Node creature]
rlm@160: (dorun
rlm@160: (map
rlm@160: (fn [joint]
rlm@175: (let [[obj-a obj-b] (joint-targets creature joint)]
rlm@160: (connect obj-a obj-b joint)))
rlm@175: (joints creature))))
rlm@203: #+end_src
rlm@160:
rlm@203:
rlm@203: ** Round 3
rlm@203:
rlm@203: Now we can test the hand in all its glory.
rlm@203:
rlm@203: #+begin_src clojure
rlm@203: (in-ns 'cortex.test.body)
rlm@203:
rlm@203: (def debug-control
rlm@203: {"key-h" (fn [world val]
rlm@203: (if val (enable-debug world)))
rlm@203:
rlm@203: "key-u" (fn [world _] (set-gravity world Vector3f/ZERO))
rlm@203: })
rlm@203:
rlm@203: (defn test-three []
rlm@203: (world (nodify
rlm@203: [(doto (hand)
rlm@203: (physical!)
rlm@203: (joints!) )
rlm@203: (floor)])
rlm@203: (merge standard-debug-controls debug-control
rlm@203: normal-gravity)
rlm@203: (comp
rlm@203: #(Capture/captureVideo
rlm@203: % (File. "/home/r/proj/cortex/render/body/3"))
rlm@203: #(do (set-gravity % Vector3f/ZERO) %)
rlm@203: setup)
rlm@203: no-op))
rlm@203: #+end_src
rlm@203:
rlm@203: =(physical!)= makes the hand solid, then =(joints!)= connects each
rlm@203: piece together.
rlm@203:
rlm@203:
rlm@203: #+begin_html
rlm@203:
rlm@203: #+end_html
rlm@203:
rlm@203: The joints are visualized as green connections between each segment
rlm@203: for debug purposes. You can see that they correspond to the empty
rlm@203: nodes in the blender file.
rlm@203:
rlm@203: * Wrap-Up!
rlm@203:
rlm@203: It is convienent to combine =(physical!)= and =(joints!)= into one
rlm@203: function that completely creates the creature's physical body.
rlm@203:
rlm@203: #+name: joints-4
rlm@203: #+begin_src clojure
rlm@175: (defn body!
rlm@175: "Endow the creature with a physical body connected with joints. The
rlm@175: particulars of the joints and the masses of each pody part are
rlm@175: determined in blender."
rlm@175: [#^Node creature]
rlm@175: (physical! creature)
rlm@175: (joints! creature))
rlm@64: #+end_src
rlm@63:
rlm@202: * Bookkeeping
rlm@175:
rlm@203: Header; here for completeness.
rlm@203:
rlm@202: #+name: body-0
rlm@202: #+begin_src clojure
rlm@202: (ns cortex.body
rlm@202: "Assemble a physical creature using the definitions found in a
rlm@202: specially prepared blender file. Creates rigid bodies and joints so
rlm@202: that a creature can have a physical presense in the simulation."
rlm@202: {:author "Robert McIntyre"}
rlm@202: (:use (cortex world util sense))
rlm@202: (:use clojure.contrib.def)
rlm@202: (:import
rlm@202: (com.jme3.math Vector3f Quaternion Vector2f Matrix3f)
rlm@202: (com.jme3.bullet.joints
rlm@202: SixDofJoint Point2PointJoint HingeJoint ConeJoint)
rlm@202: com.jme3.bullet.control.RigidBodyControl
rlm@202: com.jme3.collision.CollisionResults
rlm@202: com.jme3.bounding.BoundingBox
rlm@202: com.jme3.scene.Node
rlm@202: com.jme3.scene.Geometry
rlm@202: com.jme3.bullet.collision.shapes.HullCollisionShape))
rlm@202: #+end_src
rlm@133:
rlm@202: * Source
rlm@202:
rlm@203: Dylan -- I'll fill these in later
rlm@203: - cortex.body
rlm@203: - cortex.test.body
rlm@203: - blender files
rlm@203:
rlm@202: * COMMENT Examples
rlm@63:
rlm@69: #+name: test-body
rlm@64: #+begin_src clojure
rlm@69: (ns cortex.test.body
rlm@64: (:use (cortex world util body))
rlm@135: (:require cortex.silly)
rlm@64: (:import
rlm@64: com.jme3.math.Vector3f
rlm@64: com.jme3.math.ColorRGBA
rlm@64: com.jme3.bullet.joints.Point2PointJoint
rlm@64: com.jme3.bullet.control.RigidBodyControl
rlm@145: com.jme3.system.NanoTimer
rlm@145: com.jme3.math.Quaternion))
rlm@63:
rlm@64: (defn worm-segments
rlm@64: "Create multiple evenly spaced box segments. They're fabulous!"
rlm@64: [segment-length num-segments interstitial-space radius]
rlm@64: (letfn [(nth-segment
rlm@64: [n]
rlm@64: (box segment-length radius radius :mass 0.1
rlm@64: :position
rlm@64: (Vector3f.
rlm@64: (* 2 n (+ interstitial-space segment-length)) 0 0)
rlm@64: :name (str "worm-segment" n)
rlm@64: :color (ColorRGBA/randomColor)))]
rlm@64: (map nth-segment (range num-segments))))
rlm@63:
rlm@64: (defn connect-at-midpoint
rlm@64: "Connect two physics objects with a Point2Point joint constraint at
rlm@64: the point equidistant from both objects' centers."
rlm@64: [segmentA segmentB]
rlm@64: (let [centerA (.getWorldTranslation segmentA)
rlm@64: centerB (.getWorldTranslation segmentB)
rlm@64: midpoint (.mult (.add centerA centerB) (float 0.5))
rlm@64: pivotA (.subtract midpoint centerA)
rlm@64: pivotB (.subtract midpoint centerB)
rlm@64:
rlm@64: ;; A side-effect of creating a joint registers
rlm@64: ;; it with both physics objects which in turn
rlm@64: ;; will register the joint with the physics system
rlm@64: ;; when the simulation is started.
rlm@64: joint (Point2PointJoint.
rlm@64: (.getControl segmentA RigidBodyControl)
rlm@64: (.getControl segmentB RigidBodyControl)
rlm@64: pivotA
rlm@64: pivotB)]
rlm@64: segmentB))
rlm@63:
rlm@64: (defn eve-worm
rlm@72: "Create a worm-like body bound by invisible joint constraints."
rlm@64: []
rlm@64: (let [segments (worm-segments 0.2 5 0.1 0.1)]
rlm@64: (dorun (map (partial apply connect-at-midpoint)
rlm@64: (partition 2 1 segments)))
rlm@64: (nodify "worm" segments)))
rlm@63:
rlm@64: (defn worm-pattern
rlm@64: "This is a simple, mindless motor control pattern that drives the
rlm@64: second segment of the worm's body at an offset angle with
rlm@64: sinusoidally varying strength."
rlm@64: [time]
rlm@64: (let [angle (* Math/PI (/ 9 20))
rlm@63: direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))]
rlm@63: [Vector3f/ZERO
rlm@63: (.mult
rlm@63: direction
rlm@63: (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))))
rlm@63: Vector3f/ZERO
rlm@63: Vector3f/ZERO
rlm@63: Vector3f/ZERO]))
rlm@60:
rlm@64: (defn test-motor-control
rlm@69: "Testing motor-control:
rlm@69: You should see a multi-segmented worm-like object fall onto the
rlm@64: table and begin writhing and moving."
rlm@60: []
rlm@64: (let [worm (eve-worm)
rlm@60: time (atom 0)
rlm@63: worm-motor-map (vector-motor-control worm)]
rlm@60: (world
rlm@60: (nodify [worm
rlm@60: (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
rlm@60: :color ColorRGBA/Gray)])
rlm@60: standard-debug-controls
rlm@60: (fn [world]
rlm@60: (enable-debug world)
rlm@60: (light-up-everything world)
rlm@63: (comment
rlm@63: (com.aurellem.capture.Capture/captureVideo
rlm@63: world
rlm@63: (file-str "/home/r/proj/cortex/tmp/moving-worm")))
rlm@63: )
rlm@60:
rlm@60: (fn [_ _]
rlm@60: (swap! time inc)
rlm@64: (Thread/sleep 20)
rlm@60: (dorun (worm-motor-map
rlm@60: (worm-pattern @time)))))))
rlm@60:
rlm@130:
rlm@135:
rlm@130: (defn join-at-point [obj-a obj-b world-pivot]
rlm@130: (cortex.silly/joint-dispatch
rlm@130: {:type :point}
rlm@130: (.getControl obj-a RigidBodyControl)
rlm@130: (.getControl obj-b RigidBodyControl)
rlm@130: (cortex.silly/world-to-local obj-a world-pivot)
rlm@130: (cortex.silly/world-to-local obj-b world-pivot)
rlm@130: nil
rlm@130: ))
rlm@130:
rlm@133: (import com.jme3.bullet.collision.PhysicsCollisionObject)
rlm@130:
rlm@130: (defn blab-* []
rlm@130: (let [hand (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0)
rlm@130: :mass 0 :color ColorRGBA/Green)
rlm@130: finger (box 0.5 0.2 0.2 :position (Vector3f. 2.4 0 0)
rlm@130: :mass 1 :color ColorRGBA/Red)
rlm@130: connection-point (Vector3f. 1.2 0 0)
rlm@130: root (nodify [hand finger])]
rlm@130:
rlm@130: (join-at-point hand finger (Vector3f. 1.2 0 0))
rlm@130:
rlm@130: (.setCollisionGroup
rlm@130: (.getControl hand RigidBodyControl)
rlm@130: PhysicsCollisionObject/COLLISION_GROUP_NONE)
rlm@130: (world
rlm@130: root
rlm@130: standard-debug-controls
rlm@130: (fn [world]
rlm@130: (enable-debug world)
rlm@130: (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
rlm@130: (set-gravity world Vector3f/ZERO)
rlm@130: )
rlm@130: no-op)))
rlm@133: (comment
rlm@133:
rlm@133: (defn proprioception-debug-window
rlm@133: []
rlm@133: (let [time (atom 0)]
rlm@133: (fn [prop-data]
rlm@133: (if (= 0 (rem (swap! time inc) 40))
rlm@133: (println-repl prop-data)))))
rlm@133: )
rlm@133:
rlm@131: (comment
rlm@131: (dorun
rlm@131: (map
rlm@131: (comp
rlm@131: println-repl
rlm@131: (fn [[p y r]]
rlm@131: (format
rlm@131: "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
rlm@131: p y r)))
rlm@131: prop-data)))
rlm@131:
rlm@130:
rlm@130:
rlm@137:
rlm@64: (defn test-proprioception
rlm@69: "Testing proprioception:
rlm@69: You should see two foating bars, and a printout of pitch, yaw, and
rlm@64: roll. Pressing key-r/key-t should move the blue bar up and down and
rlm@64: change only the value of pitch. key-f/key-g moves it side to side
rlm@64: and changes yaw. key-v/key-b will spin the blue segment clockwise
rlm@64: and counterclockwise, and only affect roll."
rlm@60: []
rlm@145: (let [hand (box 0.2 1 0.2 :position (Vector3f. 0 0 0)
rlm@142: :mass 0 :color ColorRGBA/Green :name "hand")
rlm@145: finger (box 0.2 1 0.2 :position (Vector3f. 0 2.4 0)
rlm@132: :mass 1 :color ColorRGBA/Red :name "finger")
rlm@133: joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
rlm@145: :position (Vector3f. 0 1.2 0)
rlm@145: :rotation (doto (Quaternion.)
rlm@145: (.fromAngleAxis
rlm@145: (/ Math/PI 2)
rlm@145: (Vector3f. 0 0 1)))
rlm@133: :physical? false)
rlm@145: joint (join-at-point hand finger (Vector3f. 0 1.2 0 ))
rlm@135: creature (nodify [hand finger joint-node])
rlm@145: finger-control (.getControl finger RigidBodyControl)
rlm@145: hand-control (.getControl hand RigidBodyControl)]
rlm@145:
rlm@145:
rlm@145: (let
rlm@135: ;; *******************************************
rlm@137:
rlm@145: [floor (box 10 10 10 :position (Vector3f. 0 -15 0)
rlm@135: :mass 0 :color ColorRGBA/Gray)
rlm@137:
rlm@137: root (nodify [creature floor])
rlm@133: prop (joint-proprioception creature joint-node)
rlm@139: prop-view (proprioception-debug-window)
rlm@139:
rlm@139: controls
rlm@139: (merge standard-debug-controls
rlm@140: {"key-o"
rlm@139: (fn [_ _] (.setEnabled finger-control true))
rlm@140: "key-p"
rlm@139: (fn [_ _] (.setEnabled finger-control false))
rlm@140: "key-k"
rlm@140: (fn [_ _] (.setEnabled hand-control true))
rlm@140: "key-l"
rlm@140: (fn [_ _] (.setEnabled hand-control false))
rlm@139: "key-i"
rlm@139: (fn [world _] (set-gravity world (Vector3f. 0 0 0)))
rlm@142: "key-period"
rlm@142: (fn [world _]
rlm@142: (.setEnabled finger-control false)
rlm@142: (.setEnabled hand-control false)
rlm@142: (.rotate creature (doto (Quaternion.)
rlm@142: (.fromAngleAxis
rlm@142: (float (/ Math/PI 15))
rlm@142: (Vector3f. 0 0 -1))))
rlm@142:
rlm@142: (.setEnabled finger-control true)
rlm@142: (.setEnabled hand-control true)
rlm@142: (set-gravity world (Vector3f. 0 0 0))
rlm@142: )
rlm@142:
rlm@142:
rlm@139: }
rlm@139: )
rlm@130:
rlm@139: ]
rlm@139: (comment
rlm@139: (.setCollisionGroup
rlm@139: (.getControl hand RigidBodyControl)
rlm@139: PhysicsCollisionObject/COLLISION_GROUP_NONE)
rlm@139: )
rlm@140: (apply
rlm@140: world
rlm@140: (with-movement
rlm@140: hand
rlm@140: ["key-y" "key-u" "key-h" "key-j" "key-n" "key-m"]
rlm@140: [10 10 10 10 1 1]
rlm@140: (with-movement
rlm@140: finger
rlm@140: ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]
rlm@145: [1 1 10 10 10 10]
rlm@140: [root
rlm@140: controls
rlm@140: (fn [world]
rlm@140: (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
rlm@140: (set-gravity world (Vector3f. 0 0 0))
rlm@140: (light-up-everything world))
rlm@145: (fn [_ _] (prop-view (list (prop))))]))))))
rlm@138:
rlm@64: #+end_src
rlm@56:
rlm@130: #+results: test-body
rlm@130: : #'cortex.test.body/test-proprioception
rlm@130:
rlm@60:
rlm@63: * COMMENT code-limbo
rlm@61: #+begin_src clojure
rlm@61: ;;(.loadModel
rlm@61: ;; (doto (asset-manager)
rlm@61: ;; (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@61: ;; "Models/person/person.blend")
rlm@61:
rlm@64:
rlm@64: (defn load-blender-model
rlm@64: "Load a .blend file using an asset folder relative path."
rlm@64: [^String model]
rlm@64: (.loadModel
rlm@64: (doto (asset-manager)
rlm@64: (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@64: model))
rlm@64:
rlm@64:
rlm@61: (defn view-model [^String model]
rlm@61: (view
rlm@61: (.loadModel
rlm@61: (doto (asset-manager)
rlm@61: (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@61: model)))
rlm@61:
rlm@61: (defn load-blender-scene [^String model]
rlm@61: (.loadModel
rlm@61: (doto (asset-manager)
rlm@61: (.registerLoader BlenderLoader (into-array String ["blend"])))
rlm@61: model))
rlm@61:
rlm@61: (defn worm
rlm@61: []
rlm@61: (.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml"))
rlm@61:
rlm@61: (defn oto
rlm@61: []
rlm@61: (.loadModel (asset-manager) "Models/Oto/Oto.mesh.xml"))
rlm@61:
rlm@61: (defn sinbad
rlm@61: []
rlm@61: (.loadModel (asset-manager) "Models/Sinbad/Sinbad.mesh.xml"))
rlm@61:
rlm@61: (defn worm-blender
rlm@61: []
rlm@61: (first (seq (.getChildren (load-blender-model
rlm@61: "Models/anim2/simple-worm.blend")))))
rlm@61:
rlm@61: (defn body
rlm@61: "given a node with a SkeletonControl, will produce a body sutiable
rlm@61: for AI control with movement and proprioception."
rlm@61: [node]
rlm@61: (let [skeleton-control (.getControl node SkeletonControl)
rlm@61: krc (KinematicRagdollControl.)]
rlm@61: (comment
rlm@61: (dorun
rlm@61: (map #(.addBoneName krc %)
rlm@61: ["mid2" "tail" "head" "mid1" "mid3" "mid4" "Dummy-Root" ""]
rlm@61: ;;"mid2" "mid3" "tail" "head"]
rlm@61: )))
rlm@61: (.addControl node krc)
rlm@61: (.setRagdollMode krc)
rlm@61: )
rlm@61: node
rlm@61: )
rlm@61: (defn show-skeleton [node]
rlm@61: (let [sd
rlm@61:
rlm@61: (doto
rlm@61: (SkeletonDebugger. "aurellem-skel-debug"
rlm@61: (skel node))
rlm@61: (.setMaterial (green-x-ray)))]
rlm@61: (.attachChild node sd)
rlm@61: node))
rlm@61:
rlm@61:
rlm@61:
rlm@61: ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61:
rlm@61: ;; this could be a good way to give objects special properties like
rlm@61: ;; being eyes and the like
rlm@61:
rlm@61: (.getUserData
rlm@61: (.getChild
rlm@61: (load-blender-model "Models/property/test.blend") 0)
rlm@61: "properties")
rlm@61:
rlm@61: ;; the properties are saved along with the blender file.
rlm@61: ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61:
rlm@61:
rlm@61:
rlm@61:
rlm@61: (defn init-debug-skel-node
rlm@61: [f debug-node skeleton]
rlm@61: (let [bones
rlm@61: (map #(.getBone skeleton %)
rlm@61: (range (.getBoneCount skeleton)))]
rlm@61: (dorun (map #(.setUserControl % true) bones))
rlm@61: (dorun (map (fn [b]
rlm@61: (println (.getName b)
rlm@61: " -- " (f b)))
rlm@61: bones))
rlm@61: (dorun
rlm@61: (map #(.attachChild
rlm@61: debug-node
rlm@61: (doto
rlm@61: (sphere 0.1
rlm@61: :position (f %)
rlm@61: :physical? false)
rlm@61: (.setMaterial (green-x-ray))))
rlm@61: bones)))
rlm@61: debug-node)
rlm@61:
rlm@61: (import jme3test.bullet.PhysicsTestHelper)
rlm@61:
rlm@61:
rlm@61: (defn test-zzz [the-worm world value]
rlm@61: (if (not value)
rlm@61: (let [skeleton (skel the-worm)]
rlm@61: (println-repl "enabling bones")
rlm@61: (dorun
rlm@61: (map
rlm@61: #(.setUserControl (.getBone skeleton %) true)
rlm@61: (range (.getBoneCount skeleton))))
rlm@61:
rlm@61:
rlm@61: (let [b (.getBone skeleton 2)]
rlm@61: (println-repl "moving " (.getName b))
rlm@61: (println-repl (.getLocalPosition b))
rlm@61: (.setUserTransforms b
rlm@61: Vector3f/UNIT_X
rlm@61: Quaternion/IDENTITY
rlm@61: ;;(doto (Quaternion.)
rlm@61: ;; (.fromAngles (/ Math/PI 2)
rlm@61: ;; 0
rlm@61: ;; 0
rlm@61:
rlm@61: (Vector3f. 1 1 1))
rlm@61: )
rlm@61:
rlm@61: (println-repl "hi! <3"))))
rlm@61:
rlm@61:
rlm@61: (defn test-ragdoll []
rlm@61:
rlm@61: (let [the-worm
rlm@61:
rlm@61: ;;(.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml")
rlm@61: (doto (show-skeleton (worm-blender))
rlm@61: (.setLocalTranslation (Vector3f. 0 10 0))
rlm@61: ;;(worm)
rlm@61: ;;(oto)
rlm@61: ;;(sinbad)
rlm@61: )
rlm@61: ]
rlm@61:
rlm@61:
rlm@61: (.start
rlm@61: (world
rlm@61: (doto (Node.)
rlm@61: (.attachChild the-worm))
rlm@61: {"key-return" (fire-cannon-ball)
rlm@61: "key-space" (partial test-zzz the-worm)
rlm@61: }
rlm@61: (fn [world]
rlm@61: (light-up-everything world)
rlm@61: (PhysicsTestHelper/createPhysicsTestWorld
rlm@61: (.getRootNode world)
rlm@61: (asset-manager)
rlm@61: (.getPhysicsSpace
rlm@61: (.getState (.getStateManager world) BulletAppState)))
rlm@61: (set-gravity world Vector3f/ZERO)
rlm@61: ;;(.setTimer world (NanoTimer.))
rlm@61: ;;(org.lwjgl.input.Mouse/setGrabbed false)
rlm@61: )
rlm@61: no-op
rlm@61: )
rlm@61:
rlm@61:
rlm@61: )))
rlm@61:
rlm@61:
rlm@61: ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61: ;;; here is the ragdoll stuff
rlm@61:
rlm@61: (def worm-mesh (.getMesh (.getChild (worm-blender) 0)))
rlm@61: (def mesh worm-mesh)
rlm@61:
rlm@61: (.getFloatBuffer mesh VertexBuffer$Type/Position)
rlm@61: (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
rlm@61: (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
rlm@61:
rlm@61:
rlm@61: (defn position [index]
rlm@61: (.get
rlm@61: (.getFloatBuffer worm-mesh VertexBuffer$Type/Position)
rlm@61: index))
rlm@61:
rlm@61: (defn bones [index]
rlm@61: (.get
rlm@61: (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
rlm@61: index))
rlm@61:
rlm@61: (defn bone-weights [index]
rlm@61: (.get
rlm@61: (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
rlm@61: index))
rlm@61:
rlm@61:
rlm@61:
rlm@61: (defn vertex-bones [vertex]
rlm@61: (vec (map (comp int bones) (range (* vertex 4) (+ (* vertex 4) 4)))))
rlm@61:
rlm@61: (defn vertex-weights [vertex]
rlm@61: (vec (map (comp float bone-weights) (range (* vertex 4) (+ (* vertex 4) 4)))))
rlm@61:
rlm@61: (defn vertex-position [index]
rlm@61: (let [offset (* index 3)]
rlm@61: (Vector3f. (position offset)
rlm@61: (position (inc offset))
rlm@61: (position (inc(inc offset))))))
rlm@61:
rlm@61: (def vertex-info (juxt vertex-position vertex-bones vertex-weights))
rlm@61:
rlm@61: (defn bone-control-color [index]
rlm@61: (get {[1 0 0 0] ColorRGBA/Red
rlm@61: [1 2 0 0] ColorRGBA/Magenta
rlm@61: [2 0 0 0] ColorRGBA/Blue}
rlm@61: (vertex-bones index)
rlm@61: ColorRGBA/White))
rlm@61:
rlm@61: (defn influence-color [index bone-num]
rlm@61: (get
rlm@61: {(float 0) ColorRGBA/Blue
rlm@61: (float 0.5) ColorRGBA/Green
rlm@61: (float 1) ColorRGBA/Red}
rlm@61: ;; find the weight of the desired bone
rlm@61: ((zipmap (vertex-bones index)(vertex-weights index))
rlm@61: bone-num)
rlm@61: ColorRGBA/Blue))
rlm@61:
rlm@61: (def worm-vertices (set (map vertex-info (range 60))))
rlm@61:
rlm@61:
rlm@61: (defn test-info []
rlm@61: (let [points (Node.)]
rlm@61: (dorun
rlm@61: (map #(.attachChild points %)
rlm@61: (map #(sphere 0.01
rlm@61: :position (vertex-position %)
rlm@61: :color (influence-color % 1)
rlm@61: :physical? false)
rlm@61: (range 60))))
rlm@61: (view points)))
rlm@61:
rlm@61:
rlm@61: (defrecord JointControl [joint physics-space]
rlm@61: PhysicsControl
rlm@61: (setPhysicsSpace [this space]
rlm@61: (dosync
rlm@61: (ref-set (:physics-space this) space))
rlm@61: (.addJoint space (:joint this)))
rlm@61: (update [this tpf])
rlm@61: (setSpatial [this spatial])
rlm@61: (render [this rm vp])
rlm@61: (getPhysicsSpace [this] (deref (:physics-space this)))
rlm@61: (isEnabled [this] true)
rlm@61: (setEnabled [this state]))
rlm@61:
rlm@61: (defn add-joint
rlm@61: "Add a joint to a particular object. When the object is added to the
rlm@61: PhysicsSpace of a simulation, the joint will also be added"
rlm@61: [object joint]
rlm@61: (let [control (JointControl. joint (ref nil))]
rlm@61: (.addControl object control))
rlm@61: object)
rlm@61:
rlm@61:
rlm@61: (defn hinge-world
rlm@61: []
rlm@61: (let [sphere1 (sphere)
rlm@61: sphere2 (sphere 1 :position (Vector3f. 3 3 3))
rlm@61: joint (Point2PointJoint.
rlm@61: (.getControl sphere1 RigidBodyControl)
rlm@61: (.getControl sphere2 RigidBodyControl)
rlm@61: Vector3f/ZERO (Vector3f. 3 3 3))]
rlm@61: (add-joint sphere1 joint)
rlm@61: (doto (Node. "hinge-world")
rlm@61: (.attachChild sphere1)
rlm@61: (.attachChild sphere2))))
rlm@61:
rlm@61:
rlm@61: (defn test-joint []
rlm@61: (view (hinge-world)))
rlm@61:
rlm@61: ;; (defn copier-gen []
rlm@61: ;; (let [count (atom 0)]
rlm@61: ;; (fn [in]
rlm@61: ;; (swap! count inc)
rlm@61: ;; (clojure.contrib.duck-streams/copy
rlm@61: ;; in (File. (str "/home/r/tmp/mao-test/clojure-images/"
rlm@61: ;; ;;/home/r/tmp/mao-test/clojure-images
rlm@61: ;; (format "%08d.png" @count)))))))
rlm@61: ;; (defn decrease-framerate []
rlm@61: ;; (map
rlm@61: ;; (copier-gen)
rlm@61: ;; (sort
rlm@61: ;; (map first
rlm@61: ;; (partition
rlm@61: ;; 4
rlm@61: ;; (filter #(re-matches #".*.png$" (.getCanonicalPath %))
rlm@61: ;; (file-seq
rlm@61: ;; (file-str
rlm@61: ;; "/home/r/media/anime/mao-temp/images"))))))))
rlm@61:
rlm@61:
rlm@61:
rlm@61: ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61:
rlm@61: (defn proprioception
rlm@61: "Create a proprioception map that reports the rotations of the
rlm@61: various limbs of the creature's body"
rlm@61: [creature]
rlm@61: [#^Node creature]
rlm@61: (let [
rlm@61: nodes (node-seq creature)
rlm@61: joints
rlm@61: (map
rlm@61: :joint
rlm@61: (filter
rlm@61: #(isa? (class %) JointControl)
rlm@61: (reduce
rlm@61: concat
rlm@61: (map (fn [node]
rlm@61: (map (fn [num] (.getControl node num))
rlm@61: (range (.getNumControls node))))
rlm@61: nodes))))]
rlm@61: (fn []
rlm@61: (reduce concat (map relative-positions (list (first joints)))))))
rlm@61:
rlm@61:
rlm@63: (defn skel [node]
rlm@63: (doto
rlm@63: (.getSkeleton
rlm@63: (.getControl node SkeletonControl))
rlm@63: ;; this is necessary to force the skeleton to have accurate world
rlm@63: ;; transforms before it is rendered to the screen.
rlm@63: (.resetAndUpdate)))
rlm@63:
rlm@63: (defn green-x-ray []
rlm@63: (doto (Material. (asset-manager)
rlm@63: "Common/MatDefs/Misc/Unshaded.j3md")
rlm@63: (.setColor "Color" ColorRGBA/Green)
rlm@63: (-> (.getAdditionalRenderState)
rlm@63: (.setDepthTest false))))
rlm@63:
rlm@63: (defn test-worm []
rlm@63: (.start
rlm@63: (world
rlm@63: (doto (Node.)
rlm@63: ;;(.attachChild (point-worm))
rlm@63: (.attachChild (load-blender-model
rlm@63: "Models/anim2/joint-worm.blend"))
rlm@63:
rlm@63: (.attachChild (box 10 1 10
rlm@63: :position (Vector3f. 0 -2 0) :mass 0
rlm@63: :color (ColorRGBA/Gray))))
rlm@63: {
rlm@63: "key-space" (fire-cannon-ball)
rlm@63: }
rlm@63: (fn [world]
rlm@63: (enable-debug world)
rlm@63: (light-up-everything world)
rlm@63: ;;(.setTimer world (NanoTimer.))
rlm@63: )
rlm@63: no-op)))
rlm@63:
rlm@63:
rlm@63:
rlm@63: ;; defunct movement stuff
rlm@63: (defn torque-controls [control]
rlm@63: (let [torques
rlm@63: (concat
rlm@63: (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
rlm@63: (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
rlm@63: [Vector3f/UNIT_X])]
rlm@63: (map (fn [torque-axis]
rlm@63: (fn [torque]
rlm@63: (.applyTorque
rlm@63: control
rlm@63: (.mult (.mult (.getPhysicsRotation control)
rlm@63: torque-axis)
rlm@63: (float
rlm@63: (* (.getMass control) torque))))))
rlm@63: torques)))
rlm@63:
rlm@63: (defn motor-map
rlm@63: "Take a creature and generate a function that will enable fine
rlm@63: grained control over all the creature's limbs."
rlm@63: [#^Node creature]
rlm@63: (let [controls (keep #(.getControl % RigidBodyControl)
rlm@63: (node-seq creature))
rlm@63: limb-controls (reduce concat (map torque-controls controls))
rlm@63: body-control (partial map #(%1 %2) limb-controls)]
rlm@63: body-control))
rlm@63:
rlm@63: (defn test-motor-map
rlm@63: "see how torque works."
rlm@63: []
rlm@63: (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
rlm@63: :mass 1 :color ColorRGBA/Green)
rlm@63: motor-map (motor-map finger)]
rlm@63: (world
rlm@63: (nodify [finger
rlm@63: (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
rlm@63: :color ColorRGBA/Gray)])
rlm@63: standard-debug-controls
rlm@63: (fn [world]
rlm@63: (set-gravity world Vector3f/ZERO)
rlm@63: (light-up-everything world)
rlm@63: (.setTimer world (NanoTimer.)))
rlm@63: (fn [_ _]
rlm@145: (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: 0]))))))
rlm@145:
rlm@145: (defn joint-proprioception [#^Node parts #^Node joint]
rlm@145: (let [[obj-a obj-b] (joint-targets parts joint)
rlm@145: joint-rot (.getWorldRotation joint)
rlm@145: pre-inv-a (.inverse (.getWorldRotation obj-a))
rlm@145: x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X))
rlm@145: y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y))
rlm@145: z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z))
rlm@145:
rlm@145: x Vector3f/UNIT_Y
rlm@145: y Vector3f/UNIT_Z
rlm@145: z Vector3f/UNIT_X
rlm@145:
rlm@145:
rlm@145: tmp-rot-a (.getWorldRotation obj-a)]
rlm@145: (println-repl "x:" (.mult tmp-rot-a x))
rlm@145: (println-repl "y:" (.mult tmp-rot-a y))
rlm@145: (println-repl "z:" (.mult tmp-rot-a z))
rlm@145: (println-repl "rot-a" (.getWorldRotation obj-a))
rlm@145: (println-repl "rot-b" (.getWorldRotation obj-b))
rlm@145: (println-repl "joint-rot" joint-rot)
rlm@145: ;; this function will report proprioceptive information for the
rlm@145: ;; joint.
rlm@145: (fn []
rlm@145: ;; x is the "twist" axis, y and z are the "bend" axes
rlm@145: (let [rot-a (.getWorldRotation obj-a)
rlm@145: ;;inv-a (.inverse rot-a)
rlm@145: rot-b (.getWorldRotation obj-b)
rlm@145: ;;relative (.mult rot-b inv-a)
rlm@145: basis (doto (Matrix3f.)
rlm@145: (.setColumn 0 (.mult rot-a x))
rlm@145: (.setColumn 1 (.mult rot-a y))
rlm@145: (.setColumn 2 (.mult rot-a z)))
rlm@145: rotation-about-joint
rlm@145: (doto (Quaternion.)
rlm@145: (.fromRotationMatrix
rlm@145: (.mult (.invert basis)
rlm@145: (.toRotationMatrix rot-b))))
rlm@145: [yaw roll pitch]
rlm@145: (seq (.toAngles rotation-about-joint nil))]
rlm@145: ;;return euler angles of the quaternion around the new basis
rlm@145: [yaw roll pitch]))))
rlm@145:
rlm@61: #+end_src
rlm@0:
rlm@0:
rlm@0:
rlm@0:
rlm@0:
rlm@0:
rlm@0:
rlm@73: * COMMENT generate Source
rlm@44: #+begin_src clojure :tangle ../src/cortex/body.clj
rlm@175: <>
rlm@0: #+end_src
rlm@64:
rlm@69: #+begin_src clojure :tangle ../src/cortex/test/body.clj
rlm@202: <>
rlm@64: #+end_src
rlm@64:
rlm@64:
rlm@0: