Skip to content

Commit

Permalink
Merge pull request #55 from janbruedigam/small_updates2
Browse files Browse the repository at this point in the history 
Small updates 2
  • Loading branch information
@janbruedigam
janbruedigam authored Mar 11, 2021
2 parents 0788e22 + a4b9578 commit a5a472c
Show file tree
Hide file tree
Showing 5 changed files with 374 additions and 49 deletions.
51 changes: 33 additions & 18 deletions src/optional_components/shapes.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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))...)

Expand All @@ -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

Expand All @@ -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

Expand All @@ -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

Expand All @@ -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

Expand All @@ -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}
Expand Down
66 changes: 35 additions & 31 deletions src/ui/urdf.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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")
Expand All @@ -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)
Expand Down
4 changes: 4 additions & 0 deletions test/runtests.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
7 changes: 7 additions & 0 deletions test/urdf_test.jl
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
using ConstrainedDynamics

Mechanism("urdf_test.urdf", floating=false)
@test true

Mechanism("urdf_test.urdf", floating=true)
@test true
Loading

0 comments on commit a5a472c

Please sign in to comment.