diff --git a/ext/Render.jl b/ext/Render.jl
index b454e463..b7162081 100644
--- a/ext/Render.jl
+++ b/ext/Render.jl
@@ -509,7 +509,7 @@ function render!(scene, ::typeof(BodyBox), sys, sol, t)
@assert isapprox(det(R0), 1.0, atol=1e-6)
# NOTE: The rotation by this R and the translation with r_shape needs to be double checked
- origin = Vec3f(0, -width/2, -height/2) + r_shape
+ origin = Vec3f(-length/2, -width/2, -height/2) + r_shape
extent = Vec3f(length, width, height)
thing = Makie.Rect3f(origin, extent)
m = mesh!(scene, thing; color, specular = Vec3f(1.5))
diff --git a/ext/URDF.jl b/ext/URDF.jl
index e5769cfa..ba89ad28 100644
--- a/ext/URDF.jl
+++ b/ext/URDF.jl
@@ -172,7 +172,7 @@ function parse_body(graph, xml_link::XMLElement)
if R != I
@warn "Ignoring rotation of link $linkname"
end
- if norm(r) == 0 || type === :sphere
+ if type === :sphere
radius = geometry.radius
"$(Symbol(linkname)) = Body(; m=$(mass), r_cm=$(r_cm), I_11 = $(inertia[1,1]), I_22 = $(inertia[2,2]), I_33 = $(inertia[3,3]), I_21 = $(inertia[2,1]), I_31 = $(inertia[3,1]), I_32 = $(inertia[3,2]), color=$(color), radius=$(radius), sparse_I=true)"
elseif type === :cylinder
@@ -184,10 +184,11 @@ function parse_body(graph, xml_link::XMLElement)
length = size[1]
width = size[2]
height = size[3]
- "$(Symbol(linkname)) = BodyBox(; r=$(r), m=$(mass), r_cm=$(r_cm), body.I_11 = $(inertia[1,1]), body.I_22 = $(inertia[2,2]), body.I_33 = $(inertia[3,3]), body.I_21 = $(inertia[2,1]), body.I_31 = $(inertia[3,1]), body.I_32 = $(inertia[3,2]), color=$(color), length=$(length), width=$(width), height=$(height), sparse_I=true)"
-
+ if iszero(r)
+ r = [1, 0, 0]
+ end
+ "$(Symbol(linkname)) = BodyBox(; r=$(r), body.m=$(mass), body.r_cm=$(r_cm), body.I_11 = $(inertia[1,1]), body.I_22 = $(inertia[2,2]), body.I_33 = $(inertia[3,3]), body.I_21 = $(inertia[2,1]), body.I_31 = $(inertia[3,1]), body.I_32 = $(inertia[3,2]), color=$(color), length=$(length), width=$(width), height=$(height))"
end
-
end
default_modelname(filename) = uppercasefirst(splitext(basename(filename))[1])
diff --git a/test/urdf/doublependulum.urdf b/test/urdf/doublependulum.urdf
index 72ab4b3e..49b64d6a 100644
--- a/test/urdf/doublependulum.urdf
+++ b/test/urdf/doublependulum.urdf
@@ -49,7 +49,7 @@
-
+
@@ -57,7 +57,7 @@
-
+