Scaling for Meshes (#52)

Co-authored-by: jfkunz

Author: jfkunz

src/optional_components/shapes.jl

@@ -9,23 +9,23 @@ mutable struct Mesh{T} <: Shape{T}
     qoffset::UnitQuaternion{T}
 
     path::String
+    scale::SVector{3,T}
     color::RGBA
 
-
     function Mesh(path::String;
-            color = RGBA(0.75, 0.75, 0.75), xoffset::AbstractVector = zeros(3), qoffset::UnitQuaternion = one(UnitQuaternion)
+            scale::AbstractVector = ones(3), color = RGBA(0.75, 0.75, 0.75), xoffset::AbstractVector = zeros(3), qoffset::UnitQuaternion = one(UnitQuaternion)
         )
         T = promote_type(eltype.((xoffset, qoffset))...)
 
-        new{T}(xoffset, qoffset, path, color)
+        new{T}(xoffset, qoffset, path, scale, color)
     end
 
     function Mesh(path::String, m::Real, J::AbstractMatrix;
-            name::String="", color = RGBA(0.75, 0.75, 0.75), xoffset::AbstractVector = zeros(3), qoffset::UnitQuaternion = one(UnitQuaternion)
+            scale::AbstractVector = ones(3), name::String="", color = RGBA(0.75, 0.75, 0.75), xoffset::AbstractVector = zeros(3), qoffset::UnitQuaternion = one(UnitQuaternion)
         )
         T = promote_type(eltype.((m, J, xoffset, qoffset))...)
 
-        return Body(m, J; name=name, shape=new{T}(xoffset, qoffset, path, color))
+        return Body(m, J; name=name, shape=new{T}(xoffset, qoffset, path, scale, color))
     end
 end
 

src/ui/urdf.jl

@@ -143,7 +143,8 @@ function parse_shape(xvisual, materialdict, T)
                 shape = Sphere(r, zero(T), color = color, xoffset = x, qoffset = q)
             elseif name(shapenode) == "mesh"
                 path = attribute(shapenode, "filename")
-                shape = Mesh(path, zero(T), zeros(T, 3, 3), color = color, xoffset = x, qoffset = q)
+                scale = parse_vector(shapenode, "scale", T, default = "1 1 1")
+                shape = Mesh(path, zero(T), zeros(T, 3, 3), scale=scale, color = color, xoffset = x, qoffset = q)
             else
                 @info "Unkown geometry."
                 shape = nothing