Merge pull request #55 from janbruedigam/small_updates2

Small updates 2

Author: janbruedigam

src/optional_components/shapes.jl

@@ -29,15 +29,17 @@ mutable struct Mesh{T} <: Shape{T}
     color::RGBA
 
     function Mesh(path::String;
-            scale::AbstractVector = ones(3), color = RGBA(0.75, 0.75, 0.75), xoffset::AbstractVector = zeros(3), qoffset::UnitQuaternion = one(UnitQuaternion)
+            xoffset::AbstractVector = szeros(3), qoffset::UnitQuaternion = one(UnitQuaternion),
+            scale::AbstractVector = sones(3), color = RGBA(0.75, 0.75, 0.75)
         )
         T = promote_type(eltype.((xoffset, qoffset))...)
 
         new{T}(xoffset, qoffset, path, scale, color)
     end
 
     function Mesh(path::String, m::Real, J::AbstractMatrix;
-            scale::AbstractVector = ones(3), name::String="", color = RGBA(0.75, 0.75, 0.75), xoffset::AbstractVector = zeros(3), qoffset::UnitQuaternion = one(UnitQuaternion)
+            xoffset::AbstractVector = szeros(3), qoffset::UnitQuaternion = one(UnitQuaternion),
+            scale::AbstractVector = sones(3), name::String="", color = RGBA(0.75, 0.75, 0.75)
         )
         T = promote_type(eltype.((m, J, xoffset, qoffset))...)
 
@@ -57,24 +59,27 @@ mutable struct Box{T} <: Shape{T}
     qoffset::UnitQuaternion{T}
 
     xyz::SVector{3,T}
+    scale::SVector{3,T}
     color::RGBA
 
 
     function Box(x::Real, y::Real, z::Real;
-            color = RGBA(0.75, 0.75, 0.75), xoffset::AbstractVector = zeros(3), qoffset::UnitQuaternion = one(UnitQuaternion)
+            xoffset::AbstractVector = szeros(3), qoffset::UnitQuaternion = one(UnitQuaternion),
+            scale::AbstractVector = sones(3), color = RGBA(0.75, 0.75, 0.75)
         )
         T = promote_type(eltype.((x, y, z, xoffset, qoffset))...)
 
-        new{T}(xoffset, qoffset, [x;y;z], color)
+        new{T}(xoffset, qoffset, [x;y;z], scale, color)
     end
 
     function Box(x::Real, y::Real, z::Real, m::Real;
-            name::String="", color = RGBA(0.75, 0.75, 0.75), xoffset::AbstractVector = zeros(3), qoffset::UnitQuaternion = one(UnitQuaternion)
+            xoffset::AbstractVector = szeros(3), qoffset::UnitQuaternion = one(UnitQuaternion),
+            scale::AbstractVector = sones(3), name::String="", color = RGBA(0.75, 0.75, 0.75)
         )
         T = promote_type(eltype.((x, y, z, m, xoffset, qoffset))...)
         J = 1 / 12 * m * diagm([y^2 + z^2;x^2 + z^2;x^2 + y^2])
 
-        return Body(m, J; name=name, shape=new{T}(xoffset, qoffset, [x;y;z], color))
+        return Body(m, J; name=name, shape=new{T}(xoffset, qoffset, [x;y;z], scale, color))
     end
 end
 
@@ -90,24 +95,27 @@ mutable struct Cylinder{T} <: Shape{T}
     qoffset::UnitQuaternion{T}
 
     rh::SVector{2,T}
+    scale::SVector{3,T}
     color::RGBA
 
     # Cylinder points in the z direction
     function Cylinder(r::Real, h::Real;
-            color = RGBA(0.75, 0.75, 0.75), xoffset::AbstractVector = zeros(3), qoffset::UnitQuaternion = one(UnitQuaternion)
+            xoffset::AbstractVector = szeros(3), qoffset::UnitQuaternion = one(UnitQuaternion),
+            scale::AbstractVector = sones(3), color = RGBA(0.75, 0.75, 0.75)
         )
         T = promote_type(eltype.((r, h, xoffset, qoffset))...)
 
-        new{T}(xoffset, qoffset, [r;h], color)
+        new{T}(xoffset, qoffset, [r;h], scale, color)
     end
 
     function Cylinder(r::Real, h::Real, m::Real;
-            name::String="", color = RGBA(0.75, 0.75, 0.75), xoffset::AbstractVector = zeros(3), qoffset::UnitQuaternion = one(UnitQuaternion)
+            xoffset::AbstractVector = szeros(3), qoffset::UnitQuaternion = one(UnitQuaternion),
+            scale::AbstractVector = sones(3), name::String="", color = RGBA(0.75, 0.75, 0.75)
         )
         T = promote_type(eltype.((r, h, m, xoffset, qoffset))...)
         J = 1 / 2 * m * diagm([r^2 + 1 / 6 * h^2;r^2 + 1 / 6 * h^2;r^2])
 
-        return Body(m, J; name=name, shape=new{T}(xoffset, qoffset, [r;h], color))
+        return Body(m, J; name=name, shape=new{T}(xoffset, qoffset, [r;h], scale, color))
     end
 end
 
@@ -123,23 +131,26 @@ mutable struct Sphere{T} <: Shape{T}
     qoffset::UnitQuaternion{T}
 
     r::T
+    scale::SVector{3,T}
     color::RGBA
 
     function Sphere(r::Real;
-            color = RGBA(0.75, 0.75, 0.75), xoffset::AbstractVector = zeros(3), qoffset::UnitQuaternion = one(UnitQuaternion)
+            xoffset::AbstractVector = szeros(3), qoffset::UnitQuaternion = one(UnitQuaternion),
+            scale::AbstractVector = sones(3), color = RGBA(0.75, 0.75, 0.75)
         )
         T = promote_type(eltype.((r, xoffset, qoffset))...)
 
-        new{T}(xoffset, qoffset, r, color)
+        new{T}(xoffset, qoffset, r, scale, color)
     end
 
     function Sphere(r::Real, m::Real;
-            name::String="", color = RGBA(0.75, 0.75, 0.75), xoffset::AbstractVector = zeros(3), qoffset::UnitQuaternion = one(UnitQuaternion)
+            xoffset::AbstractVector = szeros(3), qoffset::UnitQuaternion = one(UnitQuaternion),
+            scale::AbstractVector = sones(3), name::String="", color = RGBA(0.75, 0.75, 0.75)
         )
         T = promote_type(eltype.((r, m, xoffset, qoffset))...)
         J = 2 / 5 * m * diagm([r^2 for i = 1:3])
 
-        return Body(m, J; name=name, shape=new{T}(xoffset, qoffset, r, color))
+        return Body(m, J; name=name, shape=new{T}(xoffset, qoffset, r, scale, color))
     end
 end
 
@@ -154,24 +165,27 @@ mutable struct Pyramid{T} <: Shape{T}
     qoffset::UnitQuaternion{T}
 
     wh::SVector{2,T}
+    scale::SVector{3,T}
     color::RGBA
 
     # Pyramid points in the z direction, Center of mass at 1/4 h
     function Pyramid(w::Real, h::Real;
-            color = RGBA(0.75, 0.75, 0.75), xoffset::AbstractVector = zeros(3), qoffset::UnitQuaternion = one(UnitQuaternion)
+            xoffset::AbstractVector = szeros(3), qoffset::UnitQuaternion = one(UnitQuaternion),
+            scale::AbstractVector = sones(3), color = RGBA(0.75, 0.75, 0.75)
         )
         T = promote_type(eltype.((w, h, xoffset, qoffset))...)
 
-        new{T}(xoffset, qoffset, [w;h], color)
+        new{T}(xoffset, qoffset, [w;h], scale, color)
     end
 
     function Pyramid(w::Real, h::Real, m::Real;
-            name::String="", color = RGBA(0.75, 0.75, 0.75), xoffset::AbstractVector = zeros(3), qoffset::UnitQuaternion = one(UnitQuaternion)
+            xoffset::AbstractVector = szeros(3), qoffset::UnitQuaternion = one(UnitQuaternion),
+            scale::AbstractVector = sones(3), name::String="", color = RGBA(0.75, 0.75, 0.75)
         )
         T = promote_type(eltype.((w, h, m, xoffset, qoffset))...)
         J = 1/80 * m * diagm([4*w^2+3*h^2;4*w^2+3*h^2;8*w^2])
 
-        return Body(m, J; name=name, shape=new{T}(xoffset, qoffset, [w;h], color))
+        return Body(m, J; name=name, shape=new{T}(xoffset, qoffset, [w;h], scale, color))
     end
 end
 
@@ -181,6 +195,7 @@ function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, shape::Shape{T}) wh
     println(io," xoffset: "*string(shape.xoffset))
     println(io," qoffset: "*string(shape.qoffset))
     println(io," color:   "*string(shape.color))
+    println(io," scale:   "*string(shape.scale))
 end
 
 function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, shape::EmptyShape{T}) where {T}

src/ui/urdf.jl

@@ -138,6 +138,10 @@ function parse_shape(xvisual, materialdict, T)
                 r = parse_scalar(shapenode, "radius", T, default = "0.5")
                 l = parse_scalar(shapenode, "length", T, default = "1")
                 shape = Cylinder(r, l, zero(T), color = color, xoffset = x, qoffset = q)
+            elseif name(shapenode) == "pyramid"
+                w = parse_scalar(shapenode, "width", T, default = "1")
+                h = parse_scalar(shapenode, "height", T, default = "1")
+                shape = Cylinder(w, h, zero(T), color = color, xoffset = x, qoffset = q)
             elseif name(shapenode) == "sphere"
                 r = parse_scalar(shapenode, "radius", T, default = "0.5")
                 shape = Sphere(r, zero(T), color = color, xoffset = x, qoffset = q)
@@ -191,33 +195,50 @@ function parse_links(xlinks, materialdict, T)
     return ldict
 end
 
-function parse_joint(xjoint, plink, clink, T)
-    jointtype = attribute(xjoint, "type")
-    x, q = parse_pose(find_element(xjoint, "origin"), T)
-    axis = parse_vector(find_element(xjoint, "axis"), "xyz", T, default = "1 0 0")
-    p1 = x
-    name = attribute(xjoint, "name")
-    
+function joint_selector(jointtype, link1, link2, T; 
+        axis = SA{T}[1;0;0], p1 = szeros(T,3), p2 = szeros(T,3), qoffset = one(UnitQuaternion{T}), name = ""
+    )
+
     # TODO limits for revolute joint?
     if jointtype == "revolute" || jointtype == "continuous"
-        joint = EqualityConstraint(Revolute(plink, clink, axis; p1=p1, qoffset = q), name=name)
+        joint = EqualityConstraint(Revolute(link1, link2, axis; p1=p1, p2=p2, qoffset = qoffset), name=name)
     elseif jointtype == "prismatic"
-        joint = EqualityConstraint(Prismatic(plink, clink, axis; p1=p1, qoffset = q), name=name)
+        joint = EqualityConstraint(Prismatic(link1, link2, axis; p1=p1, p2=p2, qoffset = qoffset), name=name)
     elseif jointtype == "planar"
-        joint = EqualityConstraint(Planar(plink, clink, axis; p1=p1, qoffset = q), name=name)
+        joint = EqualityConstraint(Planar(link1, link2, axis; p1=p1, p2=p2, qoffset = qoffset), name=name)
+    elseif jointtype == "planarfree"
+        joint = EqualityConstraint(PlanarFree(link1, link2, axis; p1=p1, p2=p2), name=name)
     elseif jointtype == "fixed"
-        joint = EqualityConstraint(Fixed(plink, clink; p1=p1, qoffset = q), name=name)
+        joint = EqualityConstraint(Fixed(link1, link2; p1=p1, p2=p2, qoffset = qoffset), name=name)
     elseif jointtype == "floating"
-        joint = EqualityConstraint(Floating(plink, clink), name=name)
+        joint = EqualityConstraint(Floating(link1, link2), name=name)
     elseif jointtype == "ball"
-        joint = EqualityConstraint(Spherical(plink, clink; p1=p1, qoffset = q), name=name)
+        joint = EqualityConstraint(Spherical(link1, link2; p1=p1, p2=p2, qoffset = qoffset), name=name)
+    elseif jointtype == "fixedorientation"
+        joint = EqualityConstraint(FixedOrientation(link1, link2; qoffset = qoffset), name=name)
+    elseif jointtype == "cylindrical"
+        joint = EqualityConstraint(Cylindrical(link1, link2, axis; p1=p1, p2=p2, qoffset = qoffset), name=name)
+    elseif jointtype == "cylindricalfree"
+        joint = EqualityConstraint(CylindricalFree(link1, link2, axis; p1=p1, p2=p2), name=name)
+    elseif jointtype == "planaraxis"
+        joint = EqualityConstraint(PlanarAxis(link1, link2, axis; p1=p1, p2=p2, qoffset = qoffset), name=name)
     else
         @error "Unknown joint type"
     end
 
     return joint
 end
 
+function parse_joint(xjoint, plink, clink, T)
+    jointtype = attribute(xjoint, "type")
+    x, q = parse_pose(find_element(xjoint, "origin"), T)
+    axis = parse_vector(find_element(xjoint, "axis"), "xyz", T, default = "1 0 0")
+    p1 = x
+    name = attribute(xjoint, "name")
+
+    return joint_selector(jointtype, plink, clink, T, axis = axis, p1 = p1, qoffset = q, name = name)
+end
+
 function parse_loop_joint(xjoint, link1, link2, T)
     find_element(xjoint, "link1")
     find_element(xjoint, "link2")
@@ -230,24 +251,7 @@ function parse_loop_joint(xjoint, link1, link2, T)
     p2 = x2
     name = attribute(xjoint, "name")
 
-    # TODO limits for revolute joint?
-    if jointtype == "revolute" || jointtype == "continuous"
-        joint = EqualityConstraint(Revolute(link1, link2, axis; p1=p1, p2=p2, qoffset = q1), name=name)
-    elseif jointtype == "prismatic"
-        joint = EqualityConstraint(Prismatic(link1, link2, axis; p1=p1, p2=p2, qoffset = q1), name=name)
-    elseif jointtype == "planar"
-        joint = EqualityConstraint(Planar(link1, link2, axis; p1=p1, p2=p2, qoffset = q1), name=name)
-    elseif jointtype == "fixed"
-        joint = EqualityConstraint(Fixed(link1, link2; p1=p1, p2=p2, qoffset = q1), name=name)
-    elseif jointtype == "floating"
-        joint = EqualityConstraint(Floating(link1, link2), name=name)
-    elseif jointtype == "ball"
-        joint = EqualityConstraint(Spherical(plink, clink; p1=p1, p2=p2, qoffset = q), name=name)
-    else
-        @error "Unknown joint type"
-    end
-
-    return joint
+    return joint_selector(jointtype, link1, link2, T, axis = axis, p1 = p1, p2 = p2, qoffset = q1, name = name)
 end
 
 function parse_joints(xjoints, ldict, floating, T)

test/runtests.jl

@@ -18,6 +18,10 @@ include("initialization/initialization_test.jl")
     include("ui_test.jl")
 end
 
+@safetestset "URDF Tests" begin
+    include("urdf_test.jl")
+end
+
 @safetestset "Optionals Tests" begin
     include("optionals_test.jl")
 end

test/urdf_test.jl

@@ -0,0 +1,7 @@
+using ConstrainedDynamics
+
+Mechanism("urdf_test.urdf", floating=false)
+@test true
+
+Mechanism("urdf_test.urdf", floating=true)
+@test true