Merge pull request #20 from JuliaRobotics/rd/fix-joint-resolution

Fix error for joints with limits near 0
JuliaRobotics · Sep 21, 2018 · 07ca12e · 07ca12e
2 parents 8302f73 + 305a229
commit 07ca12e
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 5 deletions.
diff --git a/REQUIRE b/REQUIRE
@@ -1,7 +1,7 @@
 julia 0.7
 ColorTypes 0.2.0
 CoordinateTransformations 0.4.1
-DataStructures 0.7
+OrderedCollections 1.0
 GeometryTypes 0.4
 Interpolations 0.3.6
 InteractBase 0.8

diff --git a/src/manipulate.jl b/src/manipulate.jl
@@ -6,7 +6,7 @@ using MeshCatMechanisms
 using RigidBodyDynamics
 using RigidBodyDynamics: Bounds, position_bounds, lower, upper
 using InteractBase: slider, Widget, observe, vbox
-using DataStructures: OrderedDict
+using OrderedCollections: OrderedDict
 
 function remove_infs(b::Bounds, default=Float64(π))
     Bounds(isfinite(lower(b)) ? lower(b) : -default,
@@ -22,12 +22,13 @@ end
 slider_labels(joint::Joint) = [string("q", i) for i in 1:num_positions(joint)]
 slider_labels(joint::Joint{T, <:QuaternionFloating}) where {T} = ["rw", "rx", "ry", "rz", "x", "y", "z"]
 
-function sliders(joint::Joint, values=zeros(num_positions(joint));
+function sliders(joint::Joint, values=clamp.(zeros(num_positions(joint)), position_bounds(joint));
                  bounds=slider_range(joint),
                  labels=slider_labels(joint),
                  resolution=0.01, prefix="")
     map(bounds, labels, values) do b, label, value
-        slider(lower(b):resolution:upper(b),
+        num_steps = ceil(Int, (upper(b) - lower(b)) / resolution)
+        slider(range(lower(b), stop=upper(b), length=num_steps),
                value=value,
                label=string(prefix, label))
     end
@@ -45,7 +46,7 @@ function widget(joint::Joint{T, <:Fixed}, args...) where T
     Widget{:rbd_joint}()
 end
 
-function widget(joint::Joint, initial_value=zeros(num_positions(joint)); prefix=string(joint, '.'))
+function widget(joint::Joint, initial_value=clamp.(zeros(num_positions(joint)), position_bounds(joint)); prefix=string(joint, '.'))
     s = sliders(joint, initial_value, prefix=prefix)
     keys = Symbol.(slider_labels(joint))
     w = Widget{:rbd_joint}(OrderedDict(zip(keys, s)))

diff --git a/test/REQUIRE b/test/REQUIRE
@@ -1,3 +1,4 @@
 ValkyrieRobot 0.0.1
 NBInclude 1.1
 Blink
+StaticArrays
diff --git a/test/runtests.jl b/test/runtests.jl
@@ -6,6 +6,7 @@ using RigidBodyDynamics.OdeIntegrators
 using CoordinateTransformations: Translation
 using ValkyrieRobot
 using NBInclude
+using StaticArrays
 
 vis = Visualizer()
 
@@ -89,6 +90,13 @@ vis = Visualizer()
         integrate(integrator, 10., 1e-3, max_realtime_rate = 1.)
     end
 
+    @testset "position bounds resolution" begin
+        j = Joint("foo", Revolute(SVector(1., 0, 0)))
+        RigidBodyDynamics.position_bounds(j) .= RigidBodyDynamics.Bounds(-1.234, 0.0)
+        MeshCatMechanisms.Manipulate.sliders(j)
+        MeshCatMechanisms.Manipulate.widget(j)
+    end
+
     @testset "manipulation" begin
         delete!(vis)
         mechanism = rand_chain_mechanism(Float64, [QuaternionFloating{Float64}; [Revolute{Float64} for i = 1:5]]...)