Merge pull request #148 from tkoolen/constraint-bias

Add constraint_bias
JuliaRobotics · Jan 31, 2017 · aa5c651 · aa5c651
2 parents fe47724 + b89abd6
commit aa5c651
Show file tree

Hide file tree

Showing 4 changed files with 40 additions and 1 deletion.
diff --git a/src/RigidBodyDynamics.jl b/src/RigidBodyDynamics.jl
@@ -51,6 +51,7 @@ export
     Twist,
     GeometricJacobian,
     Wrench,
+    WrenchMatrix,
     Momentum,
     MomentumMatrix,
     SpatialAcceleration,
@@ -94,6 +95,7 @@ export
     motion_subspace,
     constraint_wrench_subspace,
     bias_acceleration,
+    constraint_bias!,
     spatial_inertia,
     crb_inertia,
     setdirty!,

diff --git a/src/joint.jl b/src/joint.jl
@@ -53,6 +53,14 @@ function bias_acceleration{M, X}(joint::Joint{M}, q::AbstractVector{X}, v::Abstr
     @rtti_dispatch (QuaternionFloating{M}, Revolute{M}, Prismatic{M}, Fixed{M}) _bias_acceleration(joint.jointType, joint.frameAfter, joint.frameBefore, q, v)
 end
 
+function constraint_bias!{M}(joint::Joint{M}, bias::AbstractVector, jointTwist::Twist)
+    @framecheck jointTwist.body joint.frameAfter
+    @framecheck jointTwist.base joint.frameBefore
+    @framecheck jointTwist.frame joint.frameAfter
+    @boundscheck length(bias) == 6 - num_velocities(joint) || error("wrong size")
+    @rtti_dispatch (QuaternionFloating{M}, Revolute{M}, Prismatic{M}, Fixed{M}) _constraint_bias!(joint.jointType, bias, jointTwist)
+end
+
 function configuration_derivative_to_velocity!{M}(joint::Joint{M}, v::AbstractVector, q::AbstractVector, q̇::AbstractVector)::Void
     @boundscheck check_num_velocities(joint, v)
     @boundscheck check_num_positions(joint, q)

diff --git a/src/joint_types.jl b/src/joint_types.jl
@@ -83,6 +83,11 @@ function _bias_acceleration{T<:Number, X<:Number}(
     zero(SpatialAcceleration{S}, frameAfter, frameBefore, frameAfter)
 end
 
+function _constraint_bias!(jt::QuaternionFloating, bias::AbstractVector, jointTwist::Twist)
+    bias[:] = 0
+    nothing
+end
+
 function _configuration_derivative_to_velocity!(jt::QuaternionFloating, v::AbstractVector, q::AbstractVector, q̇::AbstractVector)
     quat = rotation(jt, q)
     @inbounds quatdot = SVector(q̇[1], q̇[2], q̇[3], q̇[4])
@@ -202,6 +207,11 @@ function _bias_acceleration{T<:Number, X<:Number}(
     zero(SpatialAcceleration{promote_type(T, X)}, frameAfter, frameBefore, frameAfter)
 end
 
+function _constraint_bias!(jt::OneDegreeOfFreedomFixedAxis, bias::AbstractVector, jointTwist::Twist)
+    bias[:] = 0
+    nothing
+end
+
 function _configuration_derivative_to_velocity!(::OneDegreeOfFreedomFixedAxis, v::AbstractVector, q::AbstractVector, q̇::AbstractVector)
     copy!(v, q̇)
     nothing
@@ -351,6 +361,7 @@ function _bias_acceleration{T<:Number, X<:Number}(
     zero(SpatialAcceleration{promote_type(T, X)}, frameAfter, frameBefore, frameAfter)
 end
 
+_constraint_bias!(jt::Fixed, bias::AbstractVector, jointTwist::Twist) = (bias[:] = 0; nothing)
 _configuration_derivative_to_velocity!(::Fixed, v::AbstractVector, q::AbstractVector, q̇::AbstractVector) = nothing
 _velocity_to_configuration_derivative!(::Fixed, q̇::AbstractVector, q::AbstractVector, v::AbstractVector) = nothing
 _joint_torque!(jt::Fixed, τ::AbstractVector, q::AbstractVector, joint_wrench::Wrench) = nothing
diff --git a/test/test_mechanism_algorithms.jl b/test/test_mechanism_algorithms.jl
@@ -80,11 +80,29 @@
         for joint in joints(mechanism)
             qjoint = configuration(x, joint)
             S = motion_subspace(joint, qjoint)
-            T = constraint_wrench_subspace(joint, qjoint)::RigidBodyDynamics.WrenchSubspace{Float64}
+            T = constraint_wrench_subspace(joint, qjoint)::RigidBodyDynamics.WrenchSubspace{Float64} # TODO
             @test isapprox(T.angular' * S.angular + T.linear' * S.linear, zeros(6 - num_velocities(joint), num_velocities(joint)); atol = 1e-14)
         end
     end
 
+    @testset "constraint_bias" begin
+        for joint in joints(mechanism)
+            qjoint = configuration(x, joint)
+            vjoint = velocity(x, joint)
+            q̇joint = similar(qjoint)
+            velocity_to_configuration_derivative!(joint, q̇joint, qjoint, vjoint)
+            qjointAutodiff = create_autodiff(qjoint, q̇joint)
+            TAutodiff = constraint_wrench_subspace(joint, qjointAutodiff)#::RigidBodyDynamics.WrenchSubspace{eltype(qjointAutodiff)} # TODO
+            angular = map(x -> ForwardDiff.partials(x, 1), TAutodiff.angular)
+            linear = map(x -> ForwardDiff.partials(x, 1), TAutodiff.linear)
+            Ṫ = WrenchMatrix(joint.frameAfter, angular, linear)
+            jointTwist = transform(x, relative_twist(x, joint.frameAfter, joint.frameBefore), joint.frameAfter)
+            bias = fill(NaN, 6 - num_velocities(joint))
+            constraint_bias!(joint, bias, jointTwist)
+            @test isapprox(Ṫ.angular' * jointTwist.angular + Ṫ.linear' * jointTwist.linear, bias; atol = 1e-14)
+        end
+    end
+
     @testset "relative_acceleration" begin
         for body in bodies(mechanism)
             for base in bodies(mechanism)