diff --git a/ext/Render.jl b/ext/Render.jl index 028a6877..c39dc27f 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -949,4 +949,97 @@ function perp(r) end end + +# ============================================================================== +## Visualizers +# ============================================================================== + +function render!(scene, ::typeof(Multibody.BoxVisualizer), sys, sol, t) + + # NOTE: This draws a solid box without the hole in the middle. Cannot figure out how to render a hollow box + color = get_color(sys, sol, [1, 0.2, 1, 0.9]) + width = Float32(sol(sol.t[1], idxs=sys.width)) + height = Float32(sol(sol.t[1], idxs=sys.height)) + length = Float32(sol(sol.t[1], idxs=sys.length)) + + length_dir = sol(sol.t[1], idxs=collect(sys.render_length_dir)) + width_dir = sol(sol.t[1], idxs=collect(sys.render_width_dir)) + height_dir = normalize(cross(normalize(length_dir), normalize(width_dir))) + width_dir = normalize(cross(height_dir, length_dir)) + + Rfun = get_rot_fun(sol, sys.frame_a) + r_0a = get_fun(sol, collect(sys.frame_a.r_0)) # Origin is translated by r_shape + r_shape = sol(sol.t[1], idxs=collect(sys.r_shape)) + # r = sol(sol.t[1], idxs=collect(sys.r)) + + R0 = [length_dir width_dir height_dir] + # R0 = Multibody.from_nxy(r, width_dir).R' + @assert isapprox(det(R0), 1.0, atol=1e-5) "Rotation matrix R0 is not a valid rotation matrix, got `R0 = $R0` with determinant `det(R0) = $(det(R0))`" + # NOTE: The rotation by this R and the translation with r_shape needs to be double checked + + 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)) + on(t) do t + r1 = Point3f(r_0a(t)) + R = Rfun(t) + q = Rotations.QuatRotation(R*R0).q + Q = Makie.Quaternionf(q.v1, q.v2, q.v3, q.s) + Makie.transform!(m, translation=r1, rotation=Q) + end + + true +end + +function render!(scene, ::typeof(Multibody.SphereVisualizer), sys, sol, t) + sol(sol.t[1], idxs=sys.render)==true || return true # yes, == true + color = get_color(sys, sol, :purple) + framefun = get_frame_fun(sol, sys.frame_a) + radius = sol(sol.t[1], idxs=sys.radius) |> Float32 + thing = @lift begin # Sphere + Ta = framefun($t) + coords = Ta[1:3, 4] + point = Point3f(coords) + Sphere(point, Float32(radius)) + end + mesh!(scene, thing; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1)) +end + +function render!(scene, ::typeof(Multibody.CylinderVisualizer), sys, sol, t) + color = get_color(sys, sol, :purple) + radius = Float32(sol(sol.t[1], idxs=sys.radius)) + r_0a = get_fun(sol, collect(sys.frame_a.r_0)) + + + + + length_dir = sol(sol.t[1], idxs=collect(sys.render_length_dir)) + width_dir = randn(3,3) + height_dir = normalize(cross(normalize(length_dir), normalize(width_dir))) + width_dir = normalize(cross(height_dir, length_dir)) + + Rfun = get_rot_fun(sol, sys.frame_a) + + R0 = [length_dir width_dir height_dir] + + r1 = Point3f(0,0,0) + r2 = Point3f((length*length_direction)...) + origin = r1 + extremity = r2 + thing = Makie.GeometryBasics.Cylinder(origin, extremity, radius) + m = mesh!(scene, thing; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1), transparency=true) + + + on(t) do t + r1 = Point3f(r_0a(t)) + R = Rfun(t) + q = Rotations.QuatRotation(R*R0).q + Q = Makie.Quaternionf(q.v1, q.v2, q.v3, q.s) + Makie.transform!(m, translation=r1, rotation=Q) + end + + true +end + end \ No newline at end of file diff --git a/ext/URDF.jl b/ext/URDF.jl index af38ff9f..a59d59fb 100644 --- a/ext/URDF.jl +++ b/ext/URDF.jl @@ -55,7 +55,7 @@ function parse_pose(xml_pose::XMLElement) rot, trans end -function parse_joint(xml_joint::XMLElement) +function parse_joint(xml_joint::XMLElement, render_fixed, render_joints) urdf_joint_type = attribute(xml_joint, "type") name = getname(xml_joint) @@ -69,7 +69,7 @@ function parse_joint(xml_joint::XMLElement) components = "$name = URDFRevolute(; r=$r, R=$R, n=$axis)" else components = """ - $name = URDFRevolute(; r=$r, R=$R, n=$axis, axisflange=true) + $name = URDFRevolute(; r=$r, R=$R, n=$axis, axisflange=true, render=$render_joints) $(name)_damper = Rotational.Damper(; d=$damping) """ connections = """ @@ -81,7 +81,7 @@ function parse_joint(xml_joint::XMLElement) axis = parse_vector(T, find_element(xml_joint, "axis"), "xyz", "1 0 0") damping = parse_scalar(Float64, find_element(xml_joint, "dynamics"), "damping", "0") if iszero(damping) - components = "$name = URDFPrismatic(; r=$r, R=$R, n=$axis)" + components = "$name = URDFPrismatic(; r=$r, R=$R, n=$axis, render=$render_joints)" else components = """ $name = URDFPrismatic(; r=$r, R=$R, n=$axis, axisflange=true) @@ -99,9 +99,9 @@ function parse_joint(xml_joint::XMLElement) if norm(r) == 0 && R == I components = "$name = NullJoint()" # Null joint elseif R == I(3) - components = "$name = FixedTranslation(; r=$r)" + components = "$name = FixedTranslation(; r=$r, render=$render_fixed)" else - components = "$name = FixedTranslation(; r=$r)" + components = "$name = FixedTranslation(; r=$r, render=$render_fixed)" @warn "Ignoring rotation of joint $name" # R = RotMatrix3(R) # components = "$name = FixedRotation(; r=$r, n = $(rotation_axis(R)), angle = $(rotation_angle(R)))" @@ -130,6 +130,7 @@ function parse_inertia(xml_inertial::XMLElement) moment = R * moment * R' # TODO: Double-check and test inertia transform, rotation convention RotXYZ? Transformation RIR' or R'IR? end + mass, moment, r_cm end @@ -138,7 +139,8 @@ getname(xml_link::XMLElement) = attribute(xml_link, "name") function parse_geometry(xml_link::XMLElement) xml_geometry = find_element(xml_link, "visual", "geometry") if xml_geometry === nothing - return 0.1, 1, :sphere + @error "No geometry found for link $(getname(xml_link)), using sphere" + return (; radius=0.1), :sphere elseif (cylinder = find_element(xml_geometry, "cylinder")) !== nothing radius = parse_scalar(Float64, cylinder, "radius", "0.1") length = parse_scalar(Float64, cylinder, "length", "1") @@ -166,22 +168,47 @@ function parse_geometry(xml_link::XMLElement) return geometry, type end -function parse_body(graph, xml_link::XMLElement) +function parse_body(graph, xml_link::XMLElement; min_mass) xml_inertial = find_element(xml_link, "inertial") - mass,inertia,r_cm = xml_inertial == nothing ? (0,0*I(3),zeros(3)) : parse_inertia(xml_inertial) + mass,inertia,r_cm = xml_inertial == nothing ? (0.0,0.0*I(3),zeros(3)) : parse_inertia(xml_inertial) + if min_mass > 0 + mass = max(mass, min_mass) + for i = 1:3 + inertia[i,i] = max(inertia[i,i], min_mass*0.01) + end + end linkname = getname(xml_link) R, r = parse_pose(find_element(xml_link, "visual", "origin")) color = parse_color(xml_link, "1 0 0 1") geometry, type = parse_geometry(xml_link) + if geometry === nothing + error("No geometry found for link $linkname") + end if R != I @warn "Ignoring rotation of link $linkname" end - if type === :sphere + if mass == 0 + # We special case this since the dynamics becomes strange with zero mass + if type === :sphere + return "$(Symbol(linkname)) = SphereVisualizer(; radius=$(geometry.radius))" # color=$(color), + elseif type === :cylinder + if iszero(r) + r = [1, 0, 0] + end + return "$(Symbol(linkname)) = CylinderVisualizer(; length_direction=$(r), radius=$(geometry.radius), length=$(geometry.length))" # color=$(color), + elseif type === :box + return "$(Symbol(linkname)) = BoxVisualizer(; length_direction=[1,0,0], width_direction=[0,1,0], length=$(geometry.size[1]), width=$(geometry.size[2]), height=$(geometry.size[3]))" # color=$(color), + end + + elseif 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 (; radius, length) = geometry + if iszero(r) + r = [1, 0, 0] + end r = normalize(r)*length "$(Symbol(linkname)) = BodyShape(; r=$(r), 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 === :box @@ -193,6 +220,8 @@ function parse_body(graph, xml_link::XMLElement) 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))" + else + error("Geometry type $type not recognized") end end @@ -209,11 +238,11 @@ urdf2multibody(joinpath(dirname(pathof(Multibody)), "..", "test", "urdf", "doubl ## Keyword arguments - `extras=false`: If `true`, the generated code will include package imports, a simulation of the model and a rendering of the model. -- `out=nothing`: If provided, the generated code will be written to this file, otherwise the string will only be returned. +- `out=nothing`: If provided, the generated code will be written to this file, otherwise the translated model is returned as a string. - `worldconnection=:rigid`: If `:rigid`, the world frame will be connected to the root link with a rigid connection. If a joint constructor is provided, this component will be instantiated and the root link is connected to the world through this, e.g., `worldconnection = FreeMotion`, `()->Prismatic(n=[0, 1, 0])` etc. - `modelname`: The name of the model, default is the name of the URDF file with the extension removed and the first letter upper case. """ -function Multibody.urdf2multibody(filename::AbstractString; extras=false, out=nothing, worldconnection = :rigid, modelname = default_modelname(filename)) +function Multibody.urdf2multibody(filename::AbstractString; extras=false, out=nothing, worldconnection = :rigid, modelname = default_modelname(filename), solver = "FBDF", render_fixed=false, render_joints = true, min_mass=0) gravity = 9.81 floating::Bool = false @@ -246,14 +275,14 @@ function Multibody.urdf2multibody(filename::AbstractString; extras=false, out=no # Parse all joints and possible extra connections due to axisflanges joints_extraconnections = map(xml_joints) do l - parse_joint(l) + parse_joint(l, render_fixed, render_joints) end joints = first.(joints_extraconnections) extra_connections = filter(!isempty, last.(joints_extraconnections)) # Parse all bodies bodies = map(xml_links) do l - parse_body(graph, l) + parse_body(graph, l; min_mass) end roots = [v for v in vertices(graph) if indegree(graph, v) == 0] @@ -305,7 +334,7 @@ function Multibody.urdf2multibody(filename::AbstractString; extras=false, out=no model = complete(model) ssys = structural_simplify(IRSystem(model)) prob = ODEProblem(ssys, [], (0.0, 10.0)) - sol = solve(prob, FBDF()) + sol = solve(prob, $(solver)()) plot(sol) |> display import GLMakie diff --git a/src/Multibody.jl b/src/Multibody.jl index 5afef606..4470e20d 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -251,4 +251,7 @@ include("robot/FullRobot.jl") export PlanarMechanics include("PlanarMechanics/PlanarMechanics.jl") +export SphereVisualizer, CylinderVisualizer, BoxVisualizer +include("visualizers.jl") + end