From 91b52f931e9e97bab18b2309a494bdf13933a7ce Mon Sep 17 00:00:00 2001 From: James Harton Date: Mon, 6 Jul 2020 21:35:34 +1200 Subject: [PATCH] Can load, but not yet save the example physics URDF file. --- lib/kinemat/robot.ex | 34 +++ lib/kinemat/robot/collision.ex | 48 ++++ lib/kinemat/robot/geometric.ex | 19 ++ lib/kinemat/robot/inertia.ex | 41 +++ lib/kinemat/robot/joint.ex | 56 ++++- lib/kinemat/robot/limit.ex | 31 +++ lib/kinemat/robot/link.ex | 19 +- lib/kinemat/robot/orientable.ex | 19 ++ lib/kinemat/robot/visual.ex | 34 ++- lib/kinemat/urdf/load.ex | 120 ++++++--- lib/kinemat/urdf/save.ex | 12 +- priv/fixtures/urdf/flexible.urdf | 256 +++++++++++++++++++ priv/fixtures/urdf/physics.urdf | 417 +++++++++++++++++++++++++++++++ test/kinemat/urdf/load_test.exs | 45 +++- 14 files changed, 1074 insertions(+), 77 deletions(-) create mode 100644 lib/kinemat/robot/collision.ex create mode 100644 lib/kinemat/robot/geometric.ex create mode 100644 lib/kinemat/robot/inertia.ex create mode 100644 lib/kinemat/robot/limit.ex create mode 100644 lib/kinemat/robot/orientable.ex create mode 100644 priv/fixtures/urdf/flexible.urdf create mode 100644 priv/fixtures/urdf/physics.urdf diff --git a/lib/kinemat/robot.ex b/lib/kinemat/robot.ex index f6a98aa..7212501 100644 --- a/lib/kinemat/robot.ex +++ b/lib/kinemat/robot.ex @@ -13,17 +13,37 @@ defmodule Kinemat.Robot do materials: [Material.t()] } + @doc """ + Initialise a new empty Robot description. + """ + @spec init(String.t()) :: Kinemat.Robot.t() def init(name), do: %Robot{name: name} + @doc """ + Add a new link to the robot. + """ + @spec add_link(Robot.t(), Link.t()) :: Robot.t() def add_link(%Robot{links: links} = robot, %Link{} = link), do: %Robot{robot | links: [link | links]} + @doc """ + Add a new joint to the robot. + """ + @spec add_joint(Robot.t(), Joint.t()) :: Robot.t() def add_joint(%Robot{joints: joints} = robot, %Joint{} = joint), do: %Robot{robot | joints: [joint | joints]} + @doc """ + Add a new material to the robot. + """ + @spec add_material(Robot.t(), Material.t()) :: Robot.t() def add_material(%Robot{materials: materials} = robot, %Material{} = material), do: %Robot{robot | materials: [material | materials]} + @doc """ + Retrieve a named link from the Robot. + """ + @spec get_link(Robot.t(), String.t()) :: {:ok, Link.t()} | {:error, any} def get_link(%Robot{links: links}, name) do case Enum.find(links, fn %Link{name: ^name} -> true @@ -33,4 +53,18 @@ defmodule Kinemat.Robot do _ -> {:error, "No such link"} end end + + @doc """ + Retrieve a named joint from the Robot. + """ + @spec get_joint(Robot.t(), String.t()) :: {:ok, Joint.t()} | {:error, any} + def get_joint(%Robot{joints: joints}, name) do + case Enum.find(joints, fn + %Joint{name: ^name} -> true + _ -> false + end) do + %Joint{} = joint -> {:ok, joint} + _ -> {:error, "No such joint"} + end + end end diff --git a/lib/kinemat/robot/collision.ex b/lib/kinemat/robot/collision.ex new file mode 100644 index 0000000..ecb12e5 --- /dev/null +++ b/lib/kinemat/robot/collision.ex @@ -0,0 +1,48 @@ +defmodule Kinemat.Robot.Collision do + defstruct geometry: nil, origin: nil + alias Kinemat.{Robot.Collision, Frame, Robot.Geometric, Geometry, Robot.Orientable} + + @moduledoc """ + A container for the collision properties of a link. + """ + + @type t :: %Collision{ + geometry: Geometry.t() | nil, + origin: Frame.t() | nil + } + + @doc """ + Initialise an empty collision container. + """ + @spec init :: Collision.t() + def init, do: %Collision{} + + @doc """ + Set the geometry to be stored in the collision container. + """ + @spec geometry(Collision.t(), Geometry.t()) :: Collision.t() + def geometry(%Collision{} = collision, geometry), + do: %Collision{collision | geometry: geometry} + + @doc """ + Set the origin of the collision geometry. + """ + @spec origin(Collision.t(), Point.t()) :: Collision.t() + def origin(%Collision{} = collision, origin), + do: %Collision{collision | origin: origin} + + defimpl Geometric do + def set(%Collision{} = collision, geometry), + do: %Collision{collision | geometry: geometry} + + def get(%Collision{geometry: geometry}), + do: geometry + end + + defimpl Orientable do + def set(%Collision{} = collision, %Frame{} = origin), + do: %Collision{collision | origin: origin} + + def get(%Collision{origin: origin}), do: origin + end +end diff --git a/lib/kinemat/robot/geometric.ex b/lib/kinemat/robot/geometric.ex new file mode 100644 index 0000000..ff24241 --- /dev/null +++ b/lib/kinemat/robot/geometric.ex @@ -0,0 +1,19 @@ +defprotocol Kinemat.Robot.Geometric do + @moduledoc """ + Defines a protocol for setting a geometry on links, visuals, etc. + """ + alias Kinemat.Robot.Geometric + alias Kinemat.Geometry + + @doc """ + Set the geometry on a type capable of having a geometry. + """ + @spec set(Geometric.t(), Geometry.t()) :: Proto.t() + def set(geometric, geometry) + + @doc """ + Get the geometry from a type capable of having a geometry. + """ + @spec get(Geometric.t()) :: Geometry.t() + def get(geometric) +end diff --git a/lib/kinemat/robot/inertia.ex b/lib/kinemat/robot/inertia.ex new file mode 100644 index 0000000..cb5a457 --- /dev/null +++ b/lib/kinemat/robot/inertia.ex @@ -0,0 +1,41 @@ +defmodule Kinemat.Robot.Inertia do + defstruct origin: nil, mass: nil, matrix: nil + alias Kinemat.{Frame, Robot.Inertia, Robot.Orientable, Orientations.RotationMatrix} + + @moduledoc """ + A container for the inertial properties of a link. + """ + + @type t :: %Inertia{ + origin: Frame.t() | nil, + mass: number | nil, + matrix: RotationMatrix | nil + } + + @doc """ + Initialise empty inertia container.. + """ + @spec init() :: Inertia.t() + def init(), do: %Inertia{} + + @doc """ + Set the mass component of the inertial container. + """ + @spec mass(Inertia.t(), number) :: Inertia.t() + def mass(%Inertia{} = inertia, mass) when is_number(mass), + do: %Inertia{inertia | mass: mass} + + @doc """ + Set the matrix of the inertial container. + """ + @spec matrix(Inertia.t(), RotationMatrix.t()) :: Inertia.t() + def matrix(%Inertia{} = inertia, %RotationMatrix{} = matrix), + do: %Inertia{inertia | matrix: matrix} + + defimpl Orientable do + def set(%Inertia{} = inertia, %Frame{} = frame), + do: %Inertia{inertia | origin: frame} + + def get(%Inertia{origin: origin}), do: origin + end +end diff --git a/lib/kinemat/robot/joint.ex b/lib/kinemat/robot/joint.ex index 1810033..d859758 100644 --- a/lib/kinemat/robot/joint.ex +++ b/lib/kinemat/robot/joint.ex @@ -1,20 +1,62 @@ defmodule Kinemat.Robot.Joint do - defstruct name: nil, type: nil, parent_name: nil, child_name: nil, origin: nil - alias Kinemat.{Frame, Robot.Joint} + defstruct name: nil, + type: nil, + parent_name: nil, + child_name: nil, + origin: nil, + axis: nil, + limit: nil + + alias Kinemat.{Frame, Robot.Joint, Robot.Limit, Robot.Orientable} @moduledoc """ - Represents a join in a Robot body. + Represents a joint in a Robot body. """ @type t :: %Joint{ name: String.t(), type: String.t(), - parent_name: String.t(), - child_name: String.t(), - origin: Frame + parent_name: String.t() | nil, + child_name: String.t() | nil, + origin: Frame | nil, + axis: Frame | nil, + limit: Limit | nil } + @doc """ + Initialise a new joint. + """ + @spec init(String.t(), String.t()) :: Joint.t() def init(name, type), do: %Joint{name: name, type: type} - def origin(joint, frame), do: %Joint{joint | origin: frame} + @doc """ + Set the name of the parent link. + """ + @spec parent_name(Joint.t(), String.t()) :: Joint.t() + def parent_name(%Joint{} = joint, name), do: %Joint{joint | parent_name: name} + + @doc """ + Set the name of the child link. + """ + @spec child_name(Joint.t(), String.t()) :: Joint.t() + def child_name(%Joint{} = joint, name), do: %Joint{joint | child_name: name} + + @doc """ + Set the reference frame of the joint axis. + """ + @spec axis(Joint.t(), Frame.t()) :: Joint.t() + def axis(%Joint{} = joint, frame), do: %Joint{joint | axis: frame} + + @doc """ + Set the safety limits of the joint. + """ + @spec limit(Joint.t(), Limit.t()) :: Joint.t() + def limit(%Joint{} = joint, %Limit{} = limit), do: %Joint{joint | limit: limit} + + defimpl Orientable do + def set(%Joint{} = joint, %Frame{} = frame), + do: %Joint{joint | origin: frame} + + def get(%Joint{origin: origin}), do: origin + end end diff --git a/lib/kinemat/robot/limit.ex b/lib/kinemat/robot/limit.ex new file mode 100644 index 0000000..1f83892 --- /dev/null +++ b/lib/kinemat/robot/limit.ex @@ -0,0 +1,31 @@ +defmodule Kinemat.Robot.Limit do + defstruct effort: nil, lower: nil, upper: nil, velocity: nil + alias Kinemat.Robot.Limit + + @moduledoc """ + Encapsulates the limits of a joint. + """ + + @type t :: %Limit{ + effort: number | nil, + lower: number | nil, + upper: number | nil, + velocity: number | nil + } + + @type options :: [option] + @type option :: {:effort | :lower | :upper | :velocity, number | nil} + + @doc """ + Initialise a new effort container with the provided values. + """ + @spec init(options) :: Limit.t() + def init(options) when is_list(options) do + fields = + options + |> Keyword.take(~w[effort lower upper velocity]a) + |> Enum.into(%{}) + + struct(Limit, fields) + end +end diff --git a/lib/kinemat/robot/link.ex b/lib/kinemat/robot/link.ex index 4d06864..d217946 100644 --- a/lib/kinemat/robot/link.ex +++ b/lib/kinemat/robot/link.ex @@ -1,6 +1,6 @@ defmodule Kinemat.Robot.Link do - defstruct name: nil, visual: nil, origin: nil - alias Kinemat.{Robot.Link, Visual} + defstruct name: nil, visual: nil, origin: nil, collision: nil, inertia: nil + alias Kinemat.{Robot.Collision, Robot.Inertia, Robot.Link, Robot.Visual} @moduledoc """ Defines a solid body in the kinematic chain of a robot. @@ -8,12 +8,23 @@ defmodule Kinemat.Robot.Link do @type t :: %Link{ name: String.t(), - visual: Visual.t() + visual: Visual.t() | nil, + collision: Collision.t() | nil, + inertia: Inertia.t() | nil } @spec init(String.t()) :: Link.t() def init(name), do: %Link{name: name} @spec visual(Link.t(), Visual.t()) :: Link.t() - def visual(link, visual), do: %{link | visual: visual} + def visual(%Link{} = link, %Visual{} = visual), + do: %Link{link | visual: visual} + + @spec collision(Link.t(), Collision.t()) :: Link.t() + def collision(%Link{} = link, %Collision{} = collision), + do: %Link{link | collision: collision} + + @spec inertia(Link.t(), Inertia.t()) :: Link.t() + def inertia(%Link{} = link, %Inertia{} = inertia), + do: %Link{link | inertia: inertia} end diff --git a/lib/kinemat/robot/orientable.ex b/lib/kinemat/robot/orientable.ex new file mode 100644 index 0000000..c31f6ae --- /dev/null +++ b/lib/kinemat/robot/orientable.ex @@ -0,0 +1,19 @@ +defprotocol Kinemat.Robot.Orientable do + @moduledoc """ + Defines a protocol for setting the orientation of various parts of the robot. + """ + alias Kinemat.Robot.Orientable + alias Kinemat.Frame + + @doc """ + Set the orientation frame on the orientable. + """ + @spec set(Orientable.t(), Frame.t()) :: Orientable.t() + def set(orienable, orientation) + + @doc """ + Retrieve the orientation from the orientable. + """ + @spec get(Orientable.t()) :: Frame.t() + def get(orientable) +end diff --git a/lib/kinemat/robot/visual.ex b/lib/kinemat/robot/visual.ex index 28852d7..3a0c427 100644 --- a/lib/kinemat/robot/visual.ex +++ b/lib/kinemat/robot/visual.ex @@ -1,13 +1,13 @@ defmodule Kinemat.Robot.Visual do - defstruct geometries: [], material_name: nil, origin: nil - alias Kinemat.Robot.{Geometry, Point, Visual} + defstruct geometry: nil, material_name: nil, origin: nil + alias Kinemat.{Frame, Geometry, Robot.Geometric, Robot.Orientable, Robot.Visual} @moduledoc """ A container for the visual properties of a Robot description. """ @type t :: %Visual{ - geometries: [Geometry.t()], + geometry: Geometry.t(), origin: Point.t() | nil, material_name: String.t() | nil } @@ -19,21 +19,31 @@ defmodule Kinemat.Robot.Visual do def init, do: %Visual{} @doc """ - Set the geometries to be stored in the container. - """ - @spec geometries(Visual.t(), [Geometry.t()]) :: Visual.t() - def geometries(%Visual{} = visual, geometries), do: %Visual{visual | geometries: geometries} - - @doc """ - Set the origin of the visual geometries. + Set the origin of the visual geometry. """ @spec origin(Visual.t(), Point.t()) :: Visual.t() - def origin(%Visual{} = visual, origin), do: %Visual{visual | origin: origin} + def origin(%Visual{} = visual, origin), + do: %Visual{visual | origin: origin} @doc """ - Set the material name of this visual geometries. + Set the material name of this visual geometry. """ @spec material_name(Visual.t(), String.t()) :: Visual.t() def material_name(%Visual{} = visual, material_name), do: %Visual{visual | material_name: material_name} + + defimpl Geometric do + def set(%Visual{} = visual, geometry), + do: %Visual{visual | geometry: geometry} + + def get(%Visual{geometry: geometry}), + do: geometry + end + + defimpl Orientable do + def set(%Visual{} = visual, %Frame{} = origin), + do: %Visual{visual | origin: origin} + + def get(%Visual{origin: origin}), do: origin + end end diff --git a/lib/kinemat/urdf/load.ex b/lib/kinemat/urdf/load.ex index ff450c9..42ded93 100644 --- a/lib/kinemat/urdf/load.ex +++ b/lib/kinemat/urdf/load.ex @@ -38,15 +38,8 @@ defmodule Kinemat.URDF.Load do with {:ok, name} <- get_attribute_value(node, :name), link <- Robot.Link.init(name), {:ok, children} <- get_children(node), - {:ok, link} <- - Enum.reduce_while(children, link, fn node, link -> - case parse(node, link) do - {:ok, link} -> {:cont, {:ok, link}} - {:error, reason} -> {:halt, {:error, reason}} - end - end) do - {:ok, Robot.add_link(robot, link)} - end + {:ok, link} <- reduce_oks(children, link, &parse(&1, &2)), + do: {:ok, Robot.add_link(robot, link)} end defp parse(:joint, node, %Robot{} = robot) do @@ -54,9 +47,8 @@ defmodule Kinemat.URDF.Load do {:ok, type} <- get_attribute_value(node, :type), joint <- Robot.Joint.init(name, type), {:ok, children} <- get_children(node), - {:ok, joint} <- reduce_oks(children, joint, &parse(&1, &2)) do - {:ok, Robot.add_joint(robot, joint)} - end + {:ok, joint} <- reduce_oks(children, joint, &parse(&1, &2)), + do: {:ok, Robot.add_joint(robot, joint)} end defp parse(:material, node, %Robot{} = robot) do @@ -73,52 +65,45 @@ defmodule Kinemat.URDF.Load do end defp parse(:parent, node, %Robot.Joint{} = joint) do - with {:ok, name} <- get_attribute_value(node, :link) do - {:ok, %{joint | parent_name: name}} - end + with {:ok, name} <- get_attribute_value(node, :link), + do: {:ok, Robot.Joint.parent_name(joint, name)} end defp parse(:child, node, %Robot.Joint{} = joint) do - with {:ok, name} <- get_attribute_value(node, :link) do - {:ok, %{joint | child_name: name}} - end + with {:ok, name} <- get_attribute_value(node, :link), + do: {:ok, Robot.Joint.child_name(joint, name)} end defp parse(:visual, node, %Robot.Link{} = link) do with {:ok, children} <- get_children(node), visual <- Robot.Visual.init(), - {:ok, visual} <- reduce_oks(children, visual, &parse(&1, &2)) do - {:ok, Robot.Link.visual(link, visual)} - end + {:ok, visual} <- reduce_oks(children, visual, &parse(&1, &2)), + do: {:ok, Robot.Link.visual(link, visual)} end - defp parse(:geometry, node, %Robot.Visual{} = visual) do + defp parse(:geometry, node, geometric) do with {:ok, children} <- get_children(node), - {:ok, geometries} <- reduce_oks(children, [], &parse(&1, &2)), - do: {:ok, Robot.Visual.geometries(visual, geometries)} + {:ok, geometric} <- reduce_oks(children, geometric, &parse(&1, &2)), + do: {:ok, geometric} end - defp parse(:cylinder, node, geometries) when is_list(geometries) do + defp parse(:cylinder, node, geometric) do with [length] <- extract_floats(node, :length, [0]), [radius] <- extract_floats(node, :radius, [0]), cylinder <- Geometry.Cylinder.init(length, radius), - {:ok, children} <- get_children(node), - {:ok, cylinder} <- reduce_oks(children, cylinder, &parse(&1, &2)), - do: {:ok, [cylinder | geometries]} + do: {:ok, Robot.Geometric.set(geometric, cylinder)} end - defp parse(:box, node, geometries) when is_list(geometries) do + defp parse(:box, node, geometric) do with [x, y, z] <- extract_floats(node, :size, [0, 0, 0]), box <- Geometry.Box.init(x, y, z), - {:ok, children} <- get_children(node), - {:ok, box} <- reduce_oks(children, box, &parse(&1, &2)), - do: {:ok, [box | geometries]} + do: {:ok, Robot.Geometric.set(geometric, box)} end - defp parse(:sphere, node, geometries) when is_list(geometries) do + defp parse(:sphere, node, geometric) do with [radius] <- extract_floats(node, :radius, [0]), sphere <- Geometry.Sphere.init(radius), - do: {:ok, [sphere | geometries]} + do: {:ok, Robot.Geometric.set(geometric, sphere)} end defp parse(:material, node, %Robot.Visual{} = visual) do @@ -126,7 +111,7 @@ defmodule Kinemat.URDF.Load do do: {:ok, Robot.Visual.material_name(visual, name)} end - defp parse(:origin, node, parent) do + defp parse(:origin, node, orientable) do [roll, pitch, yaw] = node |> extract_floats(:rpy, [0, 0, 0]) @@ -138,15 +123,68 @@ defmodule Kinemat.URDF.Load do translation = Coordinates.Cartesian.init(x, y, z) frame = Frame.init(translation, orientation) - case parent do - %Robot.Joint{} -> {:ok, Robot.Joint.origin(parent, frame)} - %Robot.Visual{} -> {:ok, Robot.Visual.origin(parent, frame)} - end + {:ok, Robot.Orientable.set(orientable, frame)} end - defp parse(:mesh, node, parent) do + defp parse(:mesh, node, geometric) do with {:ok, filename} <- get_attribute_value(node, :filename), - do: {:ok, [Geometry.Mesh.init(filename) | parent]} + mesh <- Geometry.Mesh.init(filename), + do: {:ok, Robot.Geometric.set(geometric, mesh)} + end + + defp parse(:axis, node, %Robot.Joint{} = joint) do + [roll, pitch, yaw] = + node + |> extract_floats(:rpy, [0, 0, 0]) + |> Enum.map(&Angle.Radian.init(&1)) + + [x, y, z] = extract_floats(node, :xyz, [0, 0, 0]) + + orientation = Orientations.Euler.init(:xyz, roll, pitch, yaw) + translation = Coordinates.Cartesian.init(x, y, z) + frame = Frame.init(translation, orientation) + {:ok, Robot.Joint.axis(joint, frame)} + end + + defp parse(:limit, node, %Robot.Joint{} = joint) do + [effort] = extract_floats(node, :effort, [0]) + [lower] = extract_floats(node, :lower, [0]) + [upper] = extract_floats(node, :upper, [0]) + [velocity] = extract_floats(node, :velocity, [0]) + + limit = Robot.Limit.init(effort: effort, lower: lower, upper: upper, velocity: velocity) + {:ok, Robot.Joint.limit(joint, limit)} + end + + defp parse(:collision, node, %Robot.Link{} = link) do + with {:ok, children} <- get_children(node), + collision <- Robot.Collision.init(), + {:ok, collision} <- reduce_oks(children, collision, &parse(&1, &2)), + do: {:ok, Robot.Link.collision(link, collision)} + end + + defp parse(:inertial, node, %Robot.Link{} = link) do + with inertia <- Robot.Inertia.init(), + {:ok, children} <- get_children(node), + {:ok, inertia} <- reduce_oks(children, inertia, &parse(&1, &2)), + do: {:ok, Robot.Link.inertia(link, inertia)} + end + + defp parse(:mass, node, %Robot.Inertia{} = inertia) do + [mass] = extract_floats(node, :value, [0]) + {:ok, Robot.Inertia.mass(inertia, mass)} + end + + defp parse(:inertia, node, %Robot.Inertia{} = inertia) do + [ixx] = extract_floats(node, :ixx, [0]) + [ixy] = extract_floats(node, :ixy, [0]) + [ixz] = extract_floats(node, :ixz, [0]) + [iyy] = extract_floats(node, :iyy, [0]) + [iyz] = extract_floats(node, :iyz, [0]) + [izz] = extract_floats(node, :izz, [0]) + + matrix = Orientations.RotationMatrix.init({ixx, ixy, ixz, ixy, iyy, iyz, ixz, iyz, izz}) + {:ok, Robot.Inertia.matrix(inertia, matrix)} end defp extract_floats(node, attribute_name, default_value) do diff --git a/lib/kinemat/urdf/save.ex b/lib/kinemat/urdf/save.ex index 4f305c4..d5dcfec 100644 --- a/lib/kinemat/urdf/save.ex +++ b/lib/kinemat/urdf/save.ex @@ -63,14 +63,14 @@ defmodule Kinemat.URDF.Save do defp to_tag(%Robot.Link{name: name, visual: visual}), do: {:link, [name: name], [to_tag(visual)]} - defp to_tag(%Robot.Visual{geometries: geometries, origin: origin, material_name: material_name}) do - geometries = - geometries - |> Enum.map(&to_tag(&1)) + defp to_tag(%Robot.Visual{geometry: geometry, origin: origin, material_name: material_name}) do + geometry = + geometry + |> to_tag() contents = - if Enum.any?(geometries), - do: [{:geometry, [], geometries}], + if geometry, + do: [{:geometry, [], [geometry]}], else: [] contents = diff --git a/priv/fixtures/urdf/flexible.urdf b/priv/fixtures/urdf/flexible.urdf new file mode 100644 index 0000000..1767348 --- /dev/null +++ b/priv/fixtures/urdf/flexible.urdf @@ -0,0 +1,256 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/priv/fixtures/urdf/physics.urdf b/priv/fixtures/urdf/physics.urdf new file mode 100644 index 0000000..f7b239c --- /dev/null +++ b/priv/fixtures/urdf/physics.urdf @@ -0,0 +1,417 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/kinemat/urdf/load_test.exs b/test/kinemat/urdf/load_test.exs index 8bed4de..64a1d27 100644 --- a/test/kinemat/urdf/load_test.exs +++ b/test/kinemat/urdf/load_test.exs @@ -16,7 +16,7 @@ defmodule Kinemat.URDF.LoadTest do assert robot.name == "myfirst" assert [link] = robot.links assert link.name == "base_link" - assert [shape] = link.visual.geometries + assert shape = link.visual.geometry assert shape.__struct__ == Geometry.Cylinder assert shape.length == 0.6 assert shape.radius == 0.2 @@ -28,7 +28,7 @@ defmodule Kinemat.URDF.LoadTest do assert robot.name == "multipleshapes" assert [link0, link1] = robot.links assert link0.name == "right_leg" - assert [shape] = link0.visual.geometries + assert shape = link0.visual.geometry assert shape.__struct__ == Geometry.Box assert shape.x == 0.6 assert shape.y == 0.1 @@ -90,23 +90,54 @@ defmodule Kinemat.URDF.LoadTest do {:ok, robot} = load(fixture_path("urdf/visual.urdf")) assert {:ok, link} = Robot.get_link(robot, "left_gripper") - assert [mesh] = link.visual.geometries + assert mesh = link.visual.geometry assert mesh.filename == "package://urdf_tutorial/meshes/l_finger.dae" assert {:ok, link} = Robot.get_link(robot, "left_tip") - assert [mesh] = link.visual.geometries + assert mesh = link.visual.geometry assert mesh.filename == "package://urdf_tutorial/meshes/l_finger_tip.dae" assert {:ok, link} = Robot.get_link(robot, "right_gripper") - assert [mesh] = link.visual.geometries + assert mesh = link.visual.geometry assert mesh.filename == "package://urdf_tutorial/meshes/l_finger.dae" assert {:ok, link} = Robot.get_link(robot, "right_tip") - assert [mesh] = link.visual.geometries + assert mesh = link.visual.geometry assert mesh.filename == "package://urdf_tutorial/meshes/l_finger_tip.dae" assert {:ok, link} = Robot.get_link(robot, "head") - assert [sphere] = link.visual.geometries + assert sphere = link.visual.geometry assert sphere.radius == 0.2 end + + test "it parses `flexible.urdf`" do + {:ok, robot} = load(fixture_path("urdf/flexible.urdf")) + + assert {:ok, joint} = Robot.get_joint(robot, "right_front_wheel_joint") + assert orientation = joint.axis.orientation + assert Euler.x(orientation) == ~a(0) + assert Euler.y(orientation) == ~a(0) + assert Euler.z(orientation) == ~a(0) + + assert translation = joint.axis.point + assert Cartesian.x(translation) == 0 + assert Cartesian.y(translation) == 1.0 + assert Cartesian.z(translation) == 0 + + assert {:ok, joint} = Robot.get_joint(robot, "gripper_extension") + assert joint.limit.effort == 1000.0 + assert joint.limit.lower == -0.38 + assert joint.limit.upper == 0 + assert joint.limit.velocity == 0.5 + end + + test "it parses `physics.urdf`" do + {:ok, robot} = load(fixture_path("urdf/physics.urdf")) + + assert {:ok, link} = Robot.get_link(robot, "base_link") + assert %Geometry.Cylinder{length: 0.6, radius: 0.2} = link.collision.geometry + + assert link.inertia.mass == 10 + assert {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0} = link.inertia.matrix.matrix + end end