tweak rendering

JuliaComputing · Sep 19, 2024 · 222255c · 222255c
1 parent 4ac8bc4
commit 222255c
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 7 deletions.
diff --git 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
@@ -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
@@ -49,15 +49,15 @@
  <joint name="shoulder" type="continuous">
  <parent link="base_link" />
  <child link="upper_link" />
- <origin xyz="0 .15 0" />
+ <origin xyz=".15 0 0" />
  <axis xyz="1 0 0" />
  <dynamics damping="0.1" />
  </joint>
 
  <joint name="elbow" type="continuous">
  <parent link="upper_link" />
  <child link="lower_link" />
- <origin xyz="0 .1 -1" />
+ <origin xyz=".1 0 -1" />
  <axis xyz="1 0 0" />
  <dynamics damping="0.1" />
  </joint>