Merge pull request #144 from tkoolen/remove-joint-to-joint-transform

Remove Mechanism.jointToJointTransforms
JuliaRobotics · Jan 11, 2017 · a4860d1 · a4860d1
2 parents c2a806b + 01e65ad
commit a4860d1
Show file tree

Hide file tree

Showing 2 changed files with 39 additions and 41 deletions.
diff --git a/src/mechanism.jl b/src/mechanism.jl
@@ -1,13 +1,11 @@
 type Mechanism{T<:Real}
     toposortedTree::Vector{TreeVertex{RigidBody{T}, Joint{T}}} # TODO: consider replacing with just the root vertex after creating iterator
-    jointToJointTransforms::Dict{Joint{T}, Transform3D{T}}
     gravitationalAcceleration::FreeVector3D{SVector{3, T}} # TODO: consider removing
 
     function Mechanism(rootBody::RigidBody{T}; gravity::SVector{3, T} = SVector(zero(T), zero(T), T(-9.81)))
         tree = Tree{RigidBody{T}, Joint{T}}(rootBody)
-        jointToJointTransforms = Dict{Joint{T}, Transform3D{T}}()
         gravitationalAcceleration = FreeVector3D(default_frame(rootBody), gravity)
-        new(toposort(tree), jointToJointTransforms, gravitationalAcceleration)
+        new(toposort(tree), gravitationalAcceleration)
     end
 end
 
@@ -60,8 +58,6 @@ function canonicalize_frame_definitions!{T}(m::Mechanism{T}, vertex::TreeVertex{
         joint = edge_to_parent_data(vertex)
         parentBody = vertex_data(parent(vertex))
         newDefaultFrame = joint.frameAfter
-        parentDefaultFrame = default_frame(parentBody)
-        m.jointToJointTransforms[joint] = fixed_transform(parentBody, joint.frameBefore, parentDefaultFrame)
         change_default_frame!(body, newDefaultFrame)
     end
 end
@@ -102,9 +98,6 @@ function attach!{T}(m::Mechanism{T}, parentBody::RigidBody{T}, childMechanism::M
     end
     canonicalize_frame_definitions!(m, parentVertex)
 
-    # merge joint-to-joint transforms
-    merge!(m.jointToJointTransforms, childMechanism.jointToJointTransforms)
-
     # merge trees
     for child in children(childRootVertex)
         vertex = insert_subtree!(parentVertex, child)
@@ -123,7 +116,6 @@ function submechanism{T}(m::Mechanism{T}, submechanismRootBody::RigidBody{T})
         insert_subtree!(root_vertex(ret), child)
     end
     ret.toposortedTree = toposort(tree(ret))
-    merge!(ret.jointToJointTransforms, Dict(k => v for (k, v) in m.jointToJointTransforms if k ∈ joints(ret)))
     canonicalize_frame_definitions!(ret)
     ret
 end
@@ -143,9 +135,7 @@ function reattach!{T}(mechanism::Mechanism{T}, oldSubtreeRootBody::RigidBody{T},
     @assert newSubtreeRoot ∈ toposort(oldSubtreeRoot)
     @assert parentVertex ∉ toposort(oldSubtreeRoot)
 
-    # detach oldSubtreeRoot (but keep body-fixed frame information)
-    oldRootJoint = edge_to_parent_data(oldSubtreeRoot)
-    delete!(mechanism.jointToJointTransforms, oldRootJoint)
+    # detach oldSubtreeRoot
     detach!(oldSubtreeRoot)
 
     # reroot
@@ -187,7 +177,6 @@ function remove_fixed_joints!(m::Mechanism)
                 # add identity joint transform as a body-fixed frame definition
                 jointTransform = Transform3D{T}(joint.frameAfter, joint.frameBefore)
                 add_frame!(parentBody, jointTransform)
-                delete!(m.jointToJointTransforms, joint)
 
                 # migrate body fixed frames to parent body
                 for tf in frame_definitions(body)

diff --git a/src/mechanism_state.jl b/src/mechanism_state.jl
@@ -3,41 +3,34 @@ immutable JointState{X<:Real, M<:Real, C<:Real}
     joint::Joint{M}
     q::VectorSegment{X}
     v::VectorSegment{X}
-    beforeJointToParent::Transform3D{C}
-    afterJointToParent::CacheElement{Transform3D{C}}
+    jointTransform::CacheElement{Transform3D{C}}
     twist::CacheElement{Twist{C}}
     biasAcceleration::CacheElement{SpatialAcceleration{C}}
     motionSubspace::CacheElement{MotionSubspace{C}}
 
-    function JointState(joint::Joint{M}, beforeJointToParent::Transform3D{C}, q::VectorSegment{X}, v::VectorSegment{X})
-        afterJointToParent = CacheElement{Transform3D{C}}()
+    function JointState(joint::Joint{M}, q::VectorSegment{X}, v::VectorSegment{X})
+        jointTransform = CacheElement{Transform3D{C}}()
         twist = CacheElement{Twist{C}}()
         biasAcceleration = CacheElement{SpatialAcceleration{C}}()
         motionSubspace = CacheElement{MotionSubspace{C}}()
-        new(joint, q, v, beforeJointToParent, afterJointToParent, twist, biasAcceleration, motionSubspace)
+        new(joint, q, v, jointTransform, twist, biasAcceleration, motionSubspace)
     end
 end
-
-function JointState{X, M}(joint::Joint{M}, beforeJointToParent::Transform3D{M}, q::VectorSegment{X}, v::VectorSegment{X})
-    C = promote_type(M, X)
-    JointState{X, M, C}(joint, convert(Transform3D{C}, beforeJointToParent), q, v)
-end
+JointState{X, M}(joint::Joint{M}, q::VectorSegment{X}, v::VectorSegment{X}) = JointState{X, M, promote_type(M, X)}(joint, q, v)
 
 configuration(state::JointState) = state.q
 velocity(state::JointState) = state.v
 configuration_range(state::JointState) = first(parentindexes(state.q))
 velocity_range(state::JointState) = first(parentindexes(state.v))
-parent_frame(state::JointState) = state.beforeJointToParent.to
-transform(state::JointState) = @cache_element_get!(state.afterJointToParent, state.beforeJointToParent * joint_transform(state.joint, state.q))
-twist(state::JointState) = @cache_element_get!(state.twist, change_base(joint_twist(state.joint, state.q, state.v), parent_frame(state)))
-bias_acceleration(state::JointState) = @cache_element_get!(state.biasAcceleration, change_base(bias_acceleration(state.joint, state.q, state.v), parent_frame(state)))
-motion_subspace(state::JointState) = @cache_element_get!(state.motionSubspace, change_base(motion_subspace(state.joint, state.q), parent_frame(state)))
-
+transform(state::JointState) = @cache_element_get!(state.jointTransform, joint_transform(state.joint, state.q))
+twist(state::JointState) = @cache_element_get!(state.twist, joint_twist(state.joint, state.q, state.v))
+bias_acceleration(state::JointState) = @cache_element_get!(state.biasAcceleration, bias_acceleration(state.joint, state.q, state.v))
+motion_subspace(state::JointState) = @cache_element_get!(state.motionSubspace, motion_subspace(state.joint, state.q))
 zero_configuration!(state::JointState) = (zero_configuration!(state.joint, state.q))
 rand_configuration!(state::JointState) = (rand_configuration!(state.joint, state.q))
 
 function setdirty!(state::JointState)
-    setdirty!(state.afterJointToParent)
+    setdirty!(state.jointTransform)
     setdirty!(state.twist)
     setdirty!(state.biasAcceleration)
     setdirty!(state.motionSubspace)
@@ -113,8 +106,7 @@ immutable MechanismState{X<:Real, M<:Real, C<:Real}
             vEnd = vStart + num_velocities(joint) - 1
             qJoint = view(q, qStart : qEnd)
             vJoint = view(v, vStart : vEnd)
-            beforeJointToParent = mechanism.jointToJointTransforms[joint]
-            jointState = JointState(joint, beforeJointToParent, qJoint, vJoint)
+            jointState = JointState(joint, qJoint, vJoint)
             insert!(parentStateVertex, bodyState, jointState)
             zero_configuration!(joint, qJoint)
             qStart = qEnd + 1
@@ -212,31 +204,48 @@ end
 
 # the following functions return quantities expressed in world frame and w.r.t. world frame (where applicable)
 function transform_to_root{X, M, C}(vertex::TreeVertex{RigidBodyState{M, C}, JointState{X, M, C}})
-    @cache_element_get!(vertex_data(vertex).transformToWorld,
-        transform_to_root(parent(vertex)) * transform(edge_to_parent_data(vertex)))
+    @cache_element_get!(vertex_data(vertex).transformToWorld, begin
+        parentVertex = parent(vertex)
+        parentBody = vertex_data(parentVertex).body
+        jointState = edge_to_parent_data(vertex)
+        parentToWorld = transform_to_root(parentVertex)
+        beforeJointToParent = frame_definition(parentBody, jointState.joint.frameBefore)
+        afterJointToBeforeJoint = transform(jointState)
+        parentToWorld * beforeJointToParent * afterJointToBeforeJoint
+    end)
 end
 
 function twist_wrt_world{X, M, C}(vertex::TreeVertex{RigidBodyState{M, C}, JointState{X, M, C}})
-    @cache_element_get!(vertex_data(vertex).twist,
-        twist_wrt_world(parent(vertex)) + transform(twist(edge_to_parent_data(vertex)), transform_to_root(vertex)))
+    @cache_element_get!(vertex_data(vertex).twist, begin
+        parentVertex = parent(vertex)
+        parentFrame = default_frame(vertex_data(parentVertex).body)
+        parentTwist = twist_wrt_world(parentVertex)
+        jointTwist = change_base(twist(edge_to_parent_data(vertex)), parentFrame) # to make frames line up
+        parentTwist + transform(jointTwist, transform_to_root(vertex))
+    end)
 end
 
 function bias_acceleration{X, M, C}(vertex::TreeVertex{RigidBodyState{M, C}, JointState{X, M, C}})
     @cache_element_get!(vertex_data(vertex).biasAcceleration, begin
         parentVertex = parent(vertex)
+        parentFrame = default_frame(vertex_data(parentVertex).body)
         parentBias = bias_acceleration(parentVertex)
         toRoot = transform_to_root(vertex)
-        jointBias = bias_acceleration(edge_to_parent_data(vertex))
-        twistWrtWorld = transform(twist_wrt_world(vertex), inv(toRoot)) # TODO
-        jointTwist = twist(edge_to_parent_data(vertex))
+        jointBias = change_base(bias_acceleration(edge_to_parent_data(vertex)), parentFrame) # to make frames line up
+        twistWrtWorld = transform(twist_wrt_world(vertex), inv(toRoot)) # TODO: awkward way of doing this
+        jointTwist = change_base(twist(edge_to_parent_data(vertex)), parentFrame) # to make frames line up
         jointBias = transform(jointBias, toRoot, twistWrtWorld, jointTwist)
         parentBias + jointBias
     end)
 end
 
 function motion_subspace{X, M, C}(vertex::TreeVertex{RigidBodyState{M, C}, JointState{X, M, C}})
-    @cache_element_get!(vertex_data(vertex).motionSubspace,
-        transform(motion_subspace(edge_to_parent_data(vertex)), transform_to_root(vertex)))
+    @cache_element_get!(vertex_data(vertex).motionSubspace, begin
+        parentVertex = parent(vertex)
+        parentFrame = default_frame(vertex_data(parentVertex).body)
+        motionSubspace = change_base(motion_subspace(edge_to_parent_data(vertex)), parentFrame)
+        transform(motionSubspace, transform_to_root(vertex))
+    end)
 end
 
 function spatial_inertia{X, M, C}(vertex::TreeVertex{RigidBodyState{M, C}, JointState{X, M, C}})