Merge pull request #482 from JuliaRobotics/tk/loop-fixes

Fixes related to loop joint functionality.
JuliaRobotics · Sep 4, 2018 · 185a11a · 185a11a
2 parents 55a57b8 + b6c387e
commit 185a11a
Show file tree

Hide file tree

Showing 4 changed files with 4 additions and 4 deletions.
diff --git a/src/mechanism_algorithms.jl b/src/mechanism_algorithms.jl
@@ -694,7 +694,7 @@ end
 
 function constraint_bias!(result::DynamicsResult, state::MechanismState{X};
         stabilization_gains::Union{AbstractDict{JointID, <:SE3PDGains}, Nothing}=default_constraint_stabilization_gains(X)) where X
-    constraint_bias!(result.constraintbias, state)
+    constraint_bias!(result.constraintbias, state; stabilization_gains=stabilization_gains)
 end
 
 function contact_dynamics!(result::DynamicsResult{T, M}, state::MechanismState{X, M, C}) where {X, M, C, T}

diff --git a/src/mechanism_modification.jl b/src/mechanism_modification.jl
@@ -168,7 +168,7 @@ Also optionally, `spanning_tree_next_edge` can be used to select which joints sh
 new spanning tree, if rebuilding the spanning tree is required.
 """
 function remove_joint!(mechanism::Mechanism{M}, joint::Joint{M};
-        flipped_joint_map::AbstractDict = Dict{<:Joint{M}, <:Joint{M}}(),
+        flipped_joint_map::AbstractDict = Dict{Joint{M}, Joint{M}}(),
         spanning_tree_next_edge = first #= breadth first =#) where {M}
     istreejoint = joint ∈ tree_joints(mechanism)
     remove_edge!(mechanism.graph, joint)

diff --git a/src/pdcontrol.jl b/src/pdcontrol.jl
@@ -117,7 +117,7 @@ function pd(gains::SE3PDGains, e::Transform3D, ė::Twist, ::SE3PDMethod{:Linear
     ψ = linearized_rodrigues_vec(R)
 
     ang = pd(angular(gains), FreeVector3D(bodyframe, ψ.sx, ψ.sy, ψ.sz), FreeVector3D(bodyframe, angular(ė)))
-    lin = pd(linear(gains), FreeVector3D(bodyframe, p), FreeVector3D(bodyframe, linear(ė)))
+    lin = pd(linear(gains), FreeVector3D(bodyframe, R' * p), FreeVector3D(bodyframe, linear(ė)))
     SpatialAcceleration(ė.body, ė.base, ang, lin)
 end
 

diff --git a/test/test_pd_control.jl b/test/test_pd_control.jl
@@ -117,7 +117,7 @@
             xdes = rand(Transform3D{Float64}, bodyframe, baseframe)
             Tdes = rand(Twist{Float64}, bodyframe, baseframe, bodyframe)
 
-            ϵ = 1e-4
+            ϵ = 1e-2
             x = xdes * Transform3D(bodyframe, bodyframe, AngleAxis(ϵ, randn(), randn(), randn()), ϵ * randn(SVector{3}))
             T = rand(Twist{Float64}, bodyframe, baseframe, bodyframe)