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