Merge pull request #526 from JuliaRobotics/tk/joint-wrenches

Stop transforming wrenches back to body frame in inverse_dynamics.

src/mechanism_algorithms.jl

@@ -451,29 +451,19 @@ function joint_wrenches_and_torques!(
         torquesout::SegmentedVector{JointID},
         net_wrenches_in_joint_wrenches_out::AbstractDict{BodyID, <:Wrench}, # TODO: consider having a separate Associative{Joint{M}, Wrench{T}} for joint wrenches
         state::MechanismState)
-    update_transforms!(state)
+    update_motion_subspaces!(state)
     # Note: pass in net wrenches as wrenches argument. wrenches argument is modified to be joint wrenches
     @boundscheck length(torquesout) == num_velocities(state) || error("length of torque vector is wrong")
-
     wrenches = net_wrenches_in_joint_wrenches_out
     for jointid in reverse(state.treejointids)
         parentbodyid, bodyid = predsucc(jointid, state)
         jointwrench = wrenches[bodyid]
-        if parentbodyid != BodyID(1) # TODO: ugly
-            # TODO: consider also doing this for the root:
-            wrenches[parentbodyid] += jointwrench # action = -reaction
+        wrenches[parentbodyid] += jointwrench # action = -reaction
+        for vindex in velocity_range(state, jointid)
+            Scol = state.motion_subspaces.data[vindex]
+            torquesout[vindex] = (transpose(Scol) * jointwrench)[1]
         end
     end
-
-    joints = state.treejoints
-    qs = values(segments(state.q))
-    τs = values(segments(torquesout))
-    foreach_with_extra_args(state, wrenches, joints, qs, τs) do state, wrenches, joint, qjoint, τjoint
-        # TODO: awkward to transform back to body frame; consider switching to body-frame implementation
-        bodyid = successorid(JointID(joint), state)
-        tf = inv(transform_to_root(state, bodyid, false))
-        joint_torque!(τjoint, joint, qjoint, transform(wrenches[bodyid], tf)) # TODO: consider using motion subspace
-    end
 end
 
 const dynamics_bias_doc = """Compute the 'dynamics bias term', i.e. the term

src/spatial/motion_force_interaction.jl

@@ -269,34 +269,42 @@ end
 end
 
 for (MatrixType, VectorType) in (:WrenchMatrix => :(Union{Twist, SpatialAcceleration}), :GeometricJacobian => :(Union{Momentum, Wrench}))
-    @eval @inline function LinearAlgebra.mul!(
-            dest::AbstractVector{T},
-            transposed_mat::LinearAlgebra.Transpose{<:Any, <:$MatrixType},
-            vec::$VectorType) where T
-        mat = parent(transposed_mat)
-        @boundscheck length(dest) == size(mat, 2) || throw(DimensionMismatch())
-        @framecheck mat.frame vec.frame
-
-        mat_angular = angular(mat)
-        mat_linear = linear(mat)
-        vec_angular = angular(vec)
-        vec_linear = linear(vec)
-
-        @inbounds begin
-            @simd for row in eachindex(dest)
-                dest[row] =
-                    mat_angular[1, row] * vec_angular[1] +
-                    mat_angular[2, row] * vec_angular[2] +
-                    mat_angular[3, row] * vec_angular[3]
-            end
-            @simd for row in eachindex(dest)
-                dest[row] +=
-                    mat_linear[1, row] * vec_linear[1] +
-                    mat_linear[2, row] * vec_linear[2] +
-                    mat_linear[3, row] * vec_linear[3]
+    @eval begin
+        @inline function LinearAlgebra.mul!(
+                dest::AbstractVector{T},
+                transposed_mat::LinearAlgebra.Transpose{<:Any, <:$MatrixType},
+                vec::$VectorType) where T
+            mat = parent(transposed_mat)
+            @boundscheck length(dest) == size(mat, 2) || throw(DimensionMismatch())
+            @framecheck mat.frame vec.frame
+
+            mat_angular = angular(mat)
+            mat_linear = linear(mat)
+            vec_angular = angular(vec)
+            vec_linear = linear(vec)
+
+            @inbounds begin
+                @simd for row in eachindex(dest)
+                    dest[row] =
+                        mat_angular[1, row] * vec_angular[1] +
+                        mat_angular[2, row] * vec_angular[2] +
+                        mat_angular[3, row] * vec_angular[3]
+                end
+                @simd for row in eachindex(dest)
+                    dest[row] +=
+                        mat_linear[1, row] * vec_linear[1] +
+                        mat_linear[2, row] * vec_linear[2] +
+                        mat_linear[3, row] * vec_linear[3]
+                end
             end
+            dest
+        end
+
+        function Base.:*(transposed_mat::LinearAlgebra.Transpose{<:Any, <:$MatrixType}, vec::$VectorType)
+            mat = parent(transposed_mat)
+            @framecheck mat.frame vec.frame
+            transpose(angular(mat)) * angular(vec) + transpose(linear(mat)) * linear(vec)
         end
-        dest
     end
 end
 

src/spatial/spatialmotion.jl

@@ -84,6 +84,12 @@ function LinearAlgebra.mul!(τ::AbstractVector, jac_transpose::Transpose{<:Any,
     mul!(τ, transpose(jac.J), force.v)
 end
 
+function Base.:*(jac_transpose::Transpose{<:Any, <:PointJacobian}, force::FreeVector3D)
+    jac = parent(jac_transpose)
+    @framecheck jac.frame force.frame
+    transpose(jac.J) * force.v
+end
+
 """
 $(TYPEDEF)
 

test/test_mechanism_algorithms.jl

@@ -363,6 +363,7 @@ end
                 f = FreeVector3D(J_point.frame, rand(), rand(), rand())
                 mul!(τ, transpose(J_point), f)
                 @test τ == transpose(J_point.J) * f.v
+                @test τ == transpose(J_point) * f
             end
         end
 

test/test_spatial.jl

@@ -190,6 +190,7 @@ end
         k = fill(NaN, size(mat, 2))
         mul!(k, transpose(mat), vec)
         @test isapprox(k, angular(mat)' * angular(vec) + linear(mat)' * linear(vec), atol = 1e-14)
+        @test k == transpose(mat) * vec
     end
 
     @testset "momentum matrix" begin