From 5f6a36a8a85fe213bf7389bed75b204d0b76609c Mon Sep 17 00:00:00 2001 From: Twan Koolen Date: Mon, 3 Sep 2018 17:08:48 -0400 Subject: [PATCH 1/2] Fixes related to loop joint functionality. * fix failure to propagate stabilization_gains kwarg in a constraint_bias! method * fix linearized pd control method: position error needs to be rotated to body frame (same as in nonlinear method) * fix default for flipped_joint_map kwarg in remove_joint! --- src/mechanism_algorithms.jl | 2 +- src/mechanism_modification.jl | 2 +- src/pdcontrol.jl | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/mechanism_algorithms.jl b/src/mechanism_algorithms.jl index 706ea5dc..07c6fa96 100644 --- 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 index f8857b6f..f332b033 100644 --- 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 index 53edcba0..63bc419d 100644 --- a/src/pdcontrol.jl +++ b/src/pdcontrol.jl @@ -117,7 +117,7 @@ function pd(gains::SE3PDGains, e::Transform3D, ė::Twist, ::SE3PDMethod{:Linear ψ = linearized_rodrigues_vec(R) ang = pd(angular(gains), FreeVector3D(bodyframe, ψ.sx, ψ.sy, ψ.sz), FreeVector3D(bodyframe, angular(ė))) - lin = pd(linear(gains), FreeVector3D(bodyframe, p), FreeVector3D(bodyframe, linear(ė))) + lin = pd(linear(gains), FreeVector3D(bodyframe, R' * p), FreeVector3D(bodyframe, linear(ė))) SpatialAcceleration(ė.body, ė.base, ang, lin) end From b6c387e9261cc96fb49d6f3f4dca090973519589 Mon Sep 17 00:00:00 2001 From: Twan Koolen Date: Mon, 3 Sep 2018 22:26:31 -0400 Subject: [PATCH 2/2] Test that linearized PD control now converges from a wider range of initial conditions. --- test/test_pd_control.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/test_pd_control.jl b/test/test_pd_control.jl index 893ec1e0..4897838c 100644 --- 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)