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@203:
rlm@203: rlm@203:
rlm@203:

The hand model directly loaded from blender. It has no physical rlm@203: presense in the simulation.

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@203:
rlm@203: rlm@203:
rlm@203:

The hand now has a physical presence, but there is nothing to hold rlm@203: it together.

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:
rlm@203: rlm@203:
rlm@203:

Now the hand is physical and has joints.

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: