diff --git a/lib/kinemat/geometry/mesh.ex b/lib/kinemat/geometry/mesh.ex
index 52d73a9..ed067f7 100644
--- a/lib/kinemat/geometry/mesh.ex
+++ b/lib/kinemat/geometry/mesh.ex
@@ -1,5 +1,5 @@
defmodule Kinemat.Geometry.Mesh do
- defstruct filename: nil
+ defstruct filename: nil, scale: 1
alias Kinemat.Geometry.Mesh
@moduledoc """
@@ -16,4 +16,11 @@ defmodule Kinemat.Geometry.Mesh do
"""
@spec init(Path.t()) :: Mesh.t()
def init(filename), do: %Mesh{filename: filename}
+
+ @doc """
+ Set the scaling factor for the mesh.
+ """
+ @spec scale(Mesh.t(), number) :: Mesh.t()
+ def scale(%Mesh{} = mesh, scale) when is_number(scale),
+ do: %Mesh{mesh | scale: scale}
end
diff --git a/lib/kinemat/robot.ex b/lib/kinemat/robot.ex
index 7212501..83f4bd4 100644
--- a/lib/kinemat/robot.ex
+++ b/lib/kinemat/robot.ex
@@ -1,6 +1,6 @@
defmodule Kinemat.Robot do
- defstruct name: nil, links: [], joints: [], materials: []
- alias Kinemat.{Robot, Robot.Joint, Robot.Link, Robot.Material}
+ defstruct name: nil, links: [], joints: [], materials: [], transmissions: []
+ alias Kinemat.{Robot, Robot.Joint, Robot.Link, Robot.Material, Robot.Transmission}
@moduledoc """
A data-structure representing a robot.
@@ -10,7 +10,8 @@ defmodule Kinemat.Robot do
name: String.t(),
links: [Link.t()],
joints: [Joint.t()],
- materials: [Material.t()]
+ materials: [Material.t()],
+ transmissions: [Transmission.t()]
}
@doc """
@@ -33,6 +34,13 @@ defmodule Kinemat.Robot do
def add_joint(%Robot{joints: joints} = robot, %Joint{} = joint),
do: %Robot{robot | joints: [joint | joints]}
+ @doc """
+ Add a new transmission to the robot.
+ """
+ @spec add_transmission(Robot.t(), Transmission.t()) :: Robot.t()
+ def add_transmission(%Robot{transmissions: transmissions} = robot, transmission),
+ do: %Robot{robot | transmissions: [transmission | transmissions]}
+
@doc """
Add a new material to the robot.
"""
diff --git a/lib/kinemat/robot/actuator.ex b/lib/kinemat/robot/actuator.ex
new file mode 100644
index 0000000..e082af2
--- /dev/null
+++ b/lib/kinemat/robot/actuator.ex
@@ -0,0 +1,39 @@
+defmodule Kinemat.Robot.Actuator do
+ defstruct name: nil, mechanical_reduction: nil, hardware_interfaces: []
+ alias Kinemat.Robot.{Actuator, HardwareInterface}
+
+ @moduledoc """
+ A transmission actuator
+
+ An actuator that a transmission is connected to with optional mechanical
+ reduction and hardware interfaces.
+ """
+
+ @type t :: %Actuator{
+ name: String.t(),
+ mechanical_reduction: number | nil,
+ hardware_interfaces: [HardwareInterface.t()]
+ }
+
+ @doc """
+ Initialise a new actuator by `name`.
+ """
+ @spec init(String.t()) :: Actuator.t()
+ def init(name), do: %Actuator{name: name}
+
+ @doc """
+ Set the mechanical reduction of the actuator.
+ """
+ @spec mechanical_reduction(Actuator.t(), number) :: Actuator.t()
+ def mechanical_reduction(%Actuator{} = actuator, mechanical_reduction)
+ when is_number(mechanical_reduction),
+ do: %Actuator{actuator | mechanical_reduction: mechanical_reduction}
+
+ @doc """
+ Set the hardware interfaces of the actuator.
+ """
+ @spec hardware_interfaces(Actuator.t(), [HardwareInterface.t()]) :: Actuator.t()
+ def hardware_interfaces(%Actuator{} = actuator, hardware_interfaces)
+ when is_list(hardware_interfaces),
+ do: %Actuator{actuator | hardware_interfaces: hardware_interfaces}
+end
diff --git a/lib/kinemat/robot/calibration.ex b/lib/kinemat/robot/calibration.ex
new file mode 100644
index 0000000..1202116
--- /dev/null
+++ b/lib/kinemat/robot/calibration.ex
@@ -0,0 +1,21 @@
+defmodule Kinemat.Robot.Calibration do
+ defstruct rising: nil, falling: nil
+ alias Kinemat.Robot.Calibration
+
+ @moduledoc """
+ Encapsulates the calibration of a joint.
+ """
+
+ @type optional_number :: number | nil
+
+ @type t :: %Calibration{
+ rising: optional_number,
+ falling: optional_number
+ }
+
+ @doc """
+ Initialise a new calibration container with the provided values.
+ """
+ @spec init(optional_number, optional_number) :: Calibration.t()
+ def init(rising, falling), do: %Calibration{rising: rising, falling: falling}
+end
diff --git a/lib/kinemat/robot/collision.ex b/lib/kinemat/robot/collision.ex
index 3421a59..cbed573 100644
--- a/lib/kinemat/robot/collision.ex
+++ b/lib/kinemat/robot/collision.ex
@@ -1,5 +1,5 @@
defmodule Kinemat.Robot.Collision do
- defstruct geometry: nil, origin: nil
+ defstruct geometry: nil, origin: nil, name: nil
alias Kinemat.{Frame, Geometry, Robot.Collision, Robot.Geometric, Robot.Orientable}
@moduledoc """
@@ -8,7 +8,8 @@ defmodule Kinemat.Robot.Collision do
@type t :: %Collision{
geometry: Geometry.t() | nil,
- origin: Frame.t() | nil
+ origin: Frame.t() | nil,
+ name: String.t() | nil
}
@doc """
@@ -17,6 +18,12 @@ defmodule Kinemat.Robot.Collision do
@spec init :: Collision.t()
def init, do: %Collision{}
+ @doc """
+ Initialise an empty collision container by name.
+ """
+ @spec init(String.t()) :: Collision.t()
+ def init(name), do: %Collision{name: name}
+
@doc """
Set the geometry to be stored in the collision container.
"""
diff --git a/lib/kinemat/robot/dynamics.ex b/lib/kinemat/robot/dynamics.ex
new file mode 100644
index 0000000..55eff20
--- /dev/null
+++ b/lib/kinemat/robot/dynamics.ex
@@ -0,0 +1,21 @@
+defmodule Kinemat.Robot.Dynamics do
+ defstruct damping: nil, friction: nil
+ alias Kinemat.Robot.Dynamics
+
+ @moduledoc """
+ Encapsulates the dynamics of a joint.
+ """
+
+ @type optional_number :: number | nil
+
+ @type t :: %Dynamics{
+ damping: optional_number,
+ friction: optional_number
+ }
+
+ @doc """
+ Initialise a new dynamics container with the provided values.
+ """
+ @spec init(optional_number, optional_number) :: Dynamics.t()
+ def init(damping, friction), do: %Dynamics{damping: damping, friction: friction}
+end
diff --git a/lib/kinemat/robot/hardware_interface.ex b/lib/kinemat/robot/hardware_interface.ex
new file mode 100644
index 0000000..37245db
--- /dev/null
+++ b/lib/kinemat/robot/hardware_interface.ex
@@ -0,0 +1,22 @@
+defmodule Kinemat.Robot.HardwareInterface do
+ defstruct name: nil
+ alias __MODULE__
+
+ @moduledoc """
+ Joint transmission hardware interface
+
+ See [Hardware Interfaces on the ROS Wiki][1] for more information.
+
+ 1: http://wiki.ros.org/ros_control#Hardware_Interfaces
+ """
+
+ @type t :: %HardwareInterface{
+ name: String.t()
+ }
+
+ @doc """
+ Initialise a new hardware interface.
+ """
+ @spec init(String.t()) :: HardwareInterface.t()
+ def init(name), do: %HardwareInterface{name: name}
+end
diff --git a/lib/kinemat/robot/joint.ex b/lib/kinemat/robot/joint.ex
index d859758..c27f044 100644
--- a/lib/kinemat/robot/joint.ex
+++ b/lib/kinemat/robot/joint.ex
@@ -5,29 +5,54 @@ defmodule Kinemat.Robot.Joint do
child_name: nil,
origin: nil,
axis: nil,
- limit: nil
+ limit: nil,
+ calibration: nil,
+ dynamics: nil,
+ mimic: nil,
+ safety_controller: nil
- alias Kinemat.{Frame, Robot.Joint, Robot.Limit, Robot.Orientable}
+ alias Kinemat.{
+ Frame,
+ Orientation,
+ Robot.Calibration,
+ Robot.Dynamics,
+ Robot.Joint,
+ Robot.Limit,
+ Robot.Mimic,
+ Robot.Orientable,
+ Robot.SafetyController
+ }
@moduledoc """
Represents a joint in a Robot body.
"""
+ @type joint_type :: :revolute | :continuous | :prismatic | :fixed | :floating | :planar
+
@type t :: %Joint{
name: String.t(),
- type: String.t(),
+ type: joint_type | nil,
parent_name: String.t() | nil,
child_name: String.t() | nil,
- origin: Frame | nil,
- axis: Frame | nil,
- limit: Limit | nil
+ origin: Frame.t() | nil,
+ axis: Orientation.t() | nil,
+ limit: Limit.t() | nil,
+ calibration: Calibration.t() | nil,
+ dynamics: Dynamics.t() | nil,
+ mimic: Mimic.t() | nil,
+ safety_controller: SafetyController.t() | 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 init(name, "revolute"), do: %Joint{name: name, type: :revolute}
+ def init(name, "continuous"), do: %Joint{name: name, type: :continuous}
+ def init(name, "prismatic"), do: %Joint{name: name, type: :prismatic}
+ def init(name, "fixed"), do: %Joint{name: name, type: :fixed}
+ def init(name, "floating"), do: %Joint{name: name, type: :floating}
+ def init(name, "planar"), do: %Joint{name: name, type: :planar}
@doc """
Set the name of the parent link.
@@ -44,7 +69,7 @@ defmodule Kinemat.Robot.Joint do
@doc """
Set the reference frame of the joint axis.
"""
- @spec axis(Joint.t(), Frame.t()) :: Joint.t()
+ @spec axis(Joint.t(), Orientation.t()) :: Joint.t()
def axis(%Joint{} = joint, frame), do: %Joint{joint | axis: frame}
@doc """
@@ -53,10 +78,44 @@ defmodule Kinemat.Robot.Joint do
@spec limit(Joint.t(), Limit.t()) :: Joint.t()
def limit(%Joint{} = joint, %Limit{} = limit), do: %Joint{joint | limit: limit}
+ @doc """
+ Set the calibration of the joint.
+ """
+ @spec calibration(Joint.t(), Calibration.t()) :: Joint.t()
+ def calibration(%Joint{} = joint, %Calibration{} = calibration),
+ do: %Joint{joint | calibration: calibration}
+
+ @doc """
+ Set the dynamics of the joint.
+ """
+ @spec dynamics(Joint.t(), Dynamics.t()) :: Joint.t()
+ def dynamics(%Joint{} = joint, %Dynamics{} = dynamics),
+ do: %Joint{joint | dynamics: dynamics}
+
+ @doc """
+ Make the joint mimic another joint.
+ """
+ @spec mimic(Joint.t(), Mimic.t()) :: Joint.t()
+ def mimic(%Joint{} = joint, %Mimic{} = mimic),
+ do: %Joint{joint | mimic: mimic}
+
+ @doc """
+ Add a safety controller to the joint.
+ """
+ @spec safety_controller(Joint.t(), SafetyController.t()) :: Joint.t()
+ def safety_controller(%Joint{} = joint, %SafetyController{} = safety_controller),
+ do: %Joint{joint | safety_controller: safety_controller}
+
defimpl Orientable do
+ import Angle.Sigil
+ alias Kinemat.{Coordinates.Cartesian, Frame, Orientations.Euler}
+
def set(%Joint{} = joint, %Frame{} = frame),
do: %Joint{joint | origin: frame}
+ def get(%Joint{origin: nil}),
+ do: Frame.init(Cartesian.init(0, 0, 0), Euler.init(:xyz, ~a(0), ~a(0), ~a(0)))
+
def get(%Joint{origin: origin}), do: origin
end
end
diff --git a/lib/kinemat/robot/limit.ex b/lib/kinemat/robot/limit.ex
index 1f83892..9ed704d 100644
--- a/lib/kinemat/robot/limit.ex
+++ b/lib/kinemat/robot/limit.ex
@@ -17,7 +17,7 @@ defmodule Kinemat.Robot.Limit do
@type option :: {:effort | :lower | :upper | :velocity, number | nil}
@doc """
- Initialise a new effort container with the provided values.
+ Initialise a new limit container with the provided values.
"""
@spec init(options) :: Limit.t()
def init(options) when is_list(options) do
diff --git a/lib/kinemat/robot/material.ex b/lib/kinemat/robot/material.ex
index 6b43fbb..f7aea12 100644
--- a/lib/kinemat/robot/material.ex
+++ b/lib/kinemat/robot/material.ex
@@ -1,12 +1,16 @@
defmodule Kinemat.Robot.Material do
- defstruct name: nil, colour: nil
- alias Kinemat.Robot.{Colour, Material}
+ defstruct name: nil, colour: nil, texture: nil
+ alias Kinemat.Robot.{Colour, Material, Texture}
@moduledoc """
A material used in the Robot description.
"""
- @type t :: %Material{name: String.t(), colour: Colour.t()}
+ @type t :: %Material{
+ name: String.t(),
+ colour: Colour.t() | nil,
+ texture: Texture.t() | nil
+ }
@doc """
Create a new material with the provided name.
@@ -19,4 +23,11 @@ defmodule Kinemat.Robot.Material do
"""
@spec colour(Material.t(), Colour.t()) :: Material.t()
def colour(%Material{} = material, %Colour{} = colour), do: %Material{material | colour: colour}
+
+ @doc """
+ Set the texture of the material.
+ """
+ @spec texture(Material.t(), Texture.t()) :: Material.t()
+ def texture(%Material{} = material, %Texture{} = texture),
+ do: %Material{material | texture: texture}
end
diff --git a/lib/kinemat/robot/mimic.ex b/lib/kinemat/robot/mimic.ex
new file mode 100644
index 0000000..2b2f9b0
--- /dev/null
+++ b/lib/kinemat/robot/mimic.ex
@@ -0,0 +1,23 @@
+defmodule Kinemat.Robot.Mimic do
+ defstruct joint_name: nil, multiplier: nil, offset: nil
+ alias Kinemat.Robot.Mimic
+
+ @moduledoc """
+ Encapsulates a joint mimicry.
+ """
+
+ @type optional_number :: number | nil
+
+ @type t :: %Mimic{
+ joint_name: String.t() | nil,
+ multiplier: optional_number,
+ offset: optional_number
+ }
+
+ @doc """
+ Initialise a new mimic container with the provided values.
+ """
+ @spec init(String.t(), optional_number, optional_number) :: Mimic.t()
+ def init(joint_name, multiplier, offset),
+ do: %Mimic{joint_name: joint_name, multiplier: multiplier, offset: offset}
+end
diff --git a/lib/kinemat/robot/safety_controller.ex b/lib/kinemat/robot/safety_controller.ex
new file mode 100644
index 0000000..90c7983
--- /dev/null
+++ b/lib/kinemat/robot/safety_controller.ex
@@ -0,0 +1,29 @@
+defmodule Kinemat.Robot.SafetyController do
+ defstruct k_velocity: nil, k_position: nil, soft_upper_limit: nil, soft_lower_limit: nil
+ alias Kinemat.Robot.SafetyController
+
+ @moduledoc """
+ Encapsulates the safety controller of a joint.
+ """
+
+ @type optional_number :: number | nil
+
+ @type t :: %SafetyController{
+ k_velocity: optional_number,
+ k_position: optional_number,
+ soft_upper_limit: optional_number,
+ soft_lower_limit: optional_number
+ }
+
+ @doc """
+ Initialise a new safety controller container with the provided values.
+ """
+ @spec init(number, optional_number, optional_number, optional_number) :: SafetyController.t()
+ def init(k_velocity, k_position, soft_lower_limit, soft_upper_limit),
+ do: %SafetyController{
+ k_velocity: k_velocity,
+ k_position: k_position,
+ soft_lower_limit: soft_lower_limit,
+ soft_upper_limit: soft_upper_limit
+ }
+end
diff --git a/lib/kinemat/robot/texture.ex b/lib/kinemat/robot/texture.ex
new file mode 100644
index 0000000..59fda3c
--- /dev/null
+++ b/lib/kinemat/robot/texture.ex
@@ -0,0 +1,16 @@
+defmodule Kinemat.Robot.Texture do
+ defstruct filename: nil
+ alias Kinemat.Robot.Texture
+
+ @moduledoc """
+ A texture to apply to a material.
+ """
+
+ @type t :: %Texture{filename: Path.t() | nil}
+
+ @doc """
+ Create a new texture with the provided filename.
+ """
+ @spec init(Path.t()) :: Material.t()
+ def init(filename), do: %Texture{filename: filename}
+end
diff --git a/lib/kinemat/robot/transmission.ex b/lib/kinemat/robot/transmission.ex
new file mode 100644
index 0000000..19c6831
--- /dev/null
+++ b/lib/kinemat/robot/transmission.ex
@@ -0,0 +1,52 @@
+defmodule Kinemat.Robot.Transmission do
+ defstruct name: nil, type: nil, joints: [], actuators: []
+ alias Kinemat.Robot.Transmission
+
+ @moduledoc """
+ URDF Transmissions
+
+ As per the spec:
+
+ > The transmission element is an extension to the URDF robot description model
+ > that is used to describe the relationship between an actuator and a joint.
+ > This allows one to model concepts such as gear ratios and parallel linkages.
+ > A transmission transforms efforts/flow variables such that their product -
+ > power - remains constant. Multiple actuators may be linked to multiple
+ > joints through complex transmission.
+ """
+
+ @type t :: %Transmission{
+ name: String.t(),
+ type: String.t() | nil,
+ joints: [Transmission.Joint.t()],
+ actuators: [Transmission.Actuator.t()]
+ }
+
+ @doc """
+ Initialises a new transmission.
+
+ Every transmission must have at least a `name`.
+ """
+ @spec init(String.t()) :: Transmission.t()
+ def init(name), do: %Transmission{name: name}
+
+ @doc """
+ Set the transmission type
+ """
+ @spec type(Transmission.t(), String.t()) :: Transmission.t()
+ def type(%Transmission{} = transmission, type), do: %Transmission{transmission | type: type}
+
+ @doc """
+ Set the transmission's joints
+ """
+ @spec joints(Transmission.t(), [Transmission.Joint.t()]) :: Transmission.t()
+ def joints(%Transmission{} = transmission, joints) when is_list(joints),
+ do: %Transmission{transmission | joints: joints}
+
+ @doc """
+ Set the transmission's actuators
+ """
+ @spec actuators(Transmission.t(), [Transmission.Actuator.t()]) :: Transmission.t()
+ def actuators(%Transmission{} = transmission, actuators) when is_list(actuators),
+ do: %Transmission{transmission | actuators: actuators}
+end
diff --git a/lib/kinemat/robot/transmission_joint.ex b/lib/kinemat/robot/transmission_joint.ex
new file mode 100644
index 0000000..004334d
--- /dev/null
+++ b/lib/kinemat/robot/transmission_joint.ex
@@ -0,0 +1,31 @@
+defmodule Kinemat.Robot.TransmissionJoint do
+ defstruct name: nil, hardware_interfaces: []
+ alias Kinemat.Robot.{HardwareInterface, TransmissionJoint}
+
+ @moduledoc """
+ Transmission Joint
+
+ A joint that the transmission is connected to, specified by name and a number
+ of hardware interfaces.
+ """
+
+ @type t :: %TransmissionJoint{
+ name: String.t(),
+ hardware_interfaces: [HardwareInterface.t()]
+ }
+
+ @doc """
+ Initialise a transmission joint.
+ """
+ @spec init(String.t()) :: TransmissionJoint.t()
+ def init(name), do: %TransmissionJoint{name: name}
+
+ @doc """
+ Set the hardware interfaces associated with the transmission joint.
+ """
+ @spec hardware_interfaces(TransmissionJoint.t(), [HardwareInterface.t()]) ::
+ TransmissionJoint.t()
+ def hardware_interfaces(%TransmissionJoint{} = joint, hardware_interfaces)
+ when is_list(hardware_interfaces),
+ do: %TransmissionJoint{joint | hardware_interfaces: hardware_interfaces}
+end
diff --git a/lib/kinemat/robot/visual.ex b/lib/kinemat/robot/visual.ex
index 3a0c427..9acdd99 100644
--- a/lib/kinemat/robot/visual.ex
+++ b/lib/kinemat/robot/visual.ex
@@ -1,6 +1,6 @@
defmodule Kinemat.Robot.Visual do
- defstruct geometry: nil, material_name: nil, origin: nil
- alias Kinemat.{Frame, Geometry, Robot.Geometric, Robot.Orientable, Robot.Visual}
+ defstruct geometry: nil, material: nil, origin: nil
+ alias Kinemat.{Frame, Geometry, Robot.Geometric, Robot.Material, Robot.Orientable, Robot.Visual}
@moduledoc """
A container for the visual properties of a Robot description.
@@ -9,7 +9,7 @@ defmodule Kinemat.Robot.Visual do
@type t :: %Visual{
geometry: Geometry.t(),
origin: Point.t() | nil,
- material_name: String.t() | nil
+ material: Material.t() | nil
}
@doc """
@@ -26,11 +26,11 @@ defmodule Kinemat.Robot.Visual do
do: %Visual{visual | origin: origin}
@doc """
- Set the material name of this visual geometry.
+ Set the material 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}
+ @spec material(Visual.t(), Material.t()) :: Visual.t()
+ def material(%Visual{} = visual, %Material{} = material),
+ do: %Visual{visual | material: material}
defimpl Geometric do
def set(%Visual{} = visual, geometry),
diff --git a/lib/kinemat/urdf/load.ex b/lib/kinemat/urdf/load.ex
index 42ded93..3f09720 100644
--- a/lib/kinemat/urdf/load.ex
+++ b/lib/kinemat/urdf/load.ex
@@ -1,10 +1,8 @@
defmodule Kinemat.URDF.Load do
- alias Kinemat.{Coordinates, Frame, Geometry, Orientations, Robot}
- import Kinemat.URDF.XmlHelpers
+ alias Kinemat.URDF.Parser
@moduledoc """
- This module implements the ability to parse a URDF file using the `:xmerl` OTP
- library.
+ This module parses URDF XML files and strings.
"""
@doc """
@@ -12,191 +10,15 @@ defmodule Kinemat.URDF.Load do
"""
@spec load(Path.t()) :: {:ok, Robot.t()} | {:error, any}
def load(path) do
- with {node, []} <- :xmerl_scan.file(path), do: parse(node, nil)
+ with {node, []} <- :xmerl_scan.file(path), do: Parser.parse(node)
end
- defp parse(node, state) when is_element(node) do
- with {:ok, name} <- get_name(node), do: parse(name, node, state)
- end
-
- defp parse(node, state) when is_text(node), do: {:ok, state}
-
- defp parse(:robot, node, nil) when is_element(node) do
- with {:ok, name} <- get_attribute_value(node, :name),
- robot <- Robot.init(name),
- {:ok, children} <- get_children(node) do
- Enum.reduce_while(children, {:ok, robot}, fn node, {:ok, robot} ->
- case parse(node, robot) do
- {:ok, robot} -> {:cont, {:ok, robot}}
- {:error, reason} -> {:halt, {:error, reason}}
- end
- end)
- end
- end
-
- defp parse(:link, node, %Robot{} = robot) do
- with {:ok, name} <- get_attribute_value(node, :name),
- link <- Robot.Link.init(name),
- {:ok, children} <- get_children(node),
- {:ok, link} <- reduce_oks(children, link, &parse(&1, &2)),
- do: {:ok, Robot.add_link(robot, link)}
- end
-
- defp parse(:joint, node, %Robot{} = robot) do
- with {:ok, name} <- get_attribute_value(node, :name),
- {: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
-
- defp parse(:material, node, %Robot{} = robot) do
- with {:ok, name} <- get_attribute_value(node, :name),
- material <- Robot.Material.init(name),
- {:ok, children} <- get_children(node),
- {:ok, material} <- reduce_oks(children, material, &parse(&1, &2)),
- do: {:ok, Robot.add_material(robot, material)}
- end
-
- defp parse(:color, node, %Robot.Material{} = material) do
- [r, g, b, a] = extract_floats(node, :rgba, [0, 0, 0, 0])
- {:ok, Robot.Material.colour(material, Robot.Colour.init(r, g, b, a))}
- end
-
- defp parse(:parent, node, %Robot.Joint{} = joint) do
- 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, 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
-
- defp parse(:geometry, node, geometric) do
- with {:ok, children} <- get_children(node),
- {:ok, geometric} <- reduce_oks(children, geometric, &parse(&1, &2)),
- do: {:ok, geometric}
- end
-
- 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),
- do: {:ok, Robot.Geometric.set(geometric, cylinder)}
- end
-
- defp parse(:box, node, geometric) do
- with [x, y, z] <- extract_floats(node, :size, [0, 0, 0]),
- box <- Geometry.Box.init(x, y, z),
- do: {:ok, Robot.Geometric.set(geometric, box)}
- end
-
- defp parse(:sphere, node, geometric) do
- with [radius] <- extract_floats(node, :radius, [0]),
- sphere <- Geometry.Sphere.init(radius),
- do: {:ok, Robot.Geometric.set(geometric, sphere)}
- end
-
- defp parse(:material, node, %Robot.Visual{} = visual) do
- with {:ok, name} <- get_attribute_value(node, :name),
- do: {:ok, Robot.Visual.material_name(visual, name)}
- end
-
- defp parse(:origin, node, orientable) 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.Orientable.set(orientable, frame)}
- end
-
- defp parse(:mesh, node, geometric) do
- with {:ok, filename} <- get_attribute_value(node, :filename),
- 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
- with {:ok, string} <- get_attribute_value(node, attribute_name),
- string_values <- String.split(string, ~r/\s+/),
- float_values <-
- string_values
- |> Enum.filter(&(byte_size(&1) > 0))
- |> Enum.map(&elem(Float.parse(&1), 0)) do
- float_values
- else
- _ -> default_value
- end
+ @doc """
+ Attempt to parse a URDF XML string.
+ """
+ def parse(xml) do
+ with xml <- String.to_charlist(xml),
+ {node, []} <- :xmerl_scan.string(xml),
+ do: Parser.parse(node)
end
end
diff --git a/lib/kinemat/urdf/parser.ex b/lib/kinemat/urdf/parser.ex
new file mode 100644
index 0000000..00dfd8d
--- /dev/null
+++ b/lib/kinemat/urdf/parser.ex
@@ -0,0 +1,322 @@
+defmodule Kinemat.URDF.Parser do
+ alias Kinemat.{Coordinates, Frame, Geometry, Orientations, Robot}
+ import Kinemat.URDF.XmlHelpers
+ require Logger
+
+ @moduledoc """
+ This module implements URDF parsing using the XML nodes generated by Erlang's
+ `:xmerl` application.
+ """
+
+ @doc """
+ Attempts to parse an `:xmerl` `xmlElement` record as a URDF document.
+ """
+ @spec parse(Kinemat.URDF.XmlHelpers.xmlElement()) :: {:ok, Robot.t()} | {:error, any}
+ def parse(node) when is_element(node), do: parse(node, nil)
+
+ defp parse(node, state) when is_element(node) do
+ with {:ok, name} <- get_name(node),
+ do: parse(name, node, state)
+ end
+
+ defp parse(node, state) when is_text(node), do: {:ok, state}
+
+ defp parse(:robot, node, nil) when is_element(node) do
+ with {:ok, name} <- get_attribute_value(node, :name),
+ robot <- Robot.init(name),
+ {:ok, children} <- get_children(node) do
+ Enum.reduce_while(children, {:ok, robot}, fn node, {:ok, robot} ->
+ case parse(node, robot) do
+ {:ok, robot} -> {:cont, {:ok, robot}}
+ {:error, reason} -> {:halt, {:error, reason}}
+ end
+ end)
+ end
+ end
+
+ defp parse(:link, node, %Robot{} = robot) do
+ with {:ok, name} <- get_attribute_value(node, :name),
+ link <- Robot.Link.init(name),
+ {:ok, children} <- get_children(node),
+ {:ok, link} <- reduce_oks(children, link, &parse(&1, &2)),
+ do: {:ok, Robot.add_link(robot, link)}
+ end
+
+ defp parse(:joint, node, %Robot{} = robot) do
+ with {:ok, name} <- get_attribute_value(node, :name),
+ {: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
+
+ defp parse(:material, node, %Robot{} = robot) do
+ with {:ok, name} <- get_attribute_value(node, :name),
+ material <- Robot.Material.init(name),
+ {:ok, children} <- get_children(node),
+ {:ok, material} <- reduce_oks(children, material, &parse(&1, &2)),
+ do: {:ok, Robot.add_material(robot, material)}
+ end
+
+ defp parse(:color, node, %Robot.Material{} = material) do
+ [r, g, b, a] = extract_floats(node, :rgba, [0, 0, 0, 0])
+ {:ok, Robot.Material.colour(material, Robot.Colour.init(r, g, b, a))}
+ end
+
+ defp parse(:parent, node, %Robot.Joint{} = joint) do
+ 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, 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
+
+ defp parse(:geometry, node, geometric) do
+ with {:ok, children} <- get_children(node),
+ {:ok, geometric} <- reduce_oks(children, geometric, &parse(&1, &2)),
+ do: {:ok, geometric}
+ end
+
+ 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),
+ do: {:ok, Robot.Geometric.set(geometric, cylinder)}
+ end
+
+ defp parse(:box, node, geometric) do
+ with [x, y, z] <- extract_floats(node, :size, [0, 0, 0]),
+ box <- Geometry.Box.init(x, y, z),
+ do: {:ok, Robot.Geometric.set(geometric, box)}
+ end
+
+ defp parse(:sphere, node, geometric) do
+ with [radius] <- extract_floats(node, :radius, [0]),
+ sphere <- Geometry.Sphere.init(radius),
+ do: {:ok, Robot.Geometric.set(geometric, sphere)}
+ end
+
+ defp parse(:material, node, %Robot.Visual{} = visual) do
+ with {:ok, name} <- get_attribute_value(node, :name),
+ material <- Robot.Material.init(name),
+ {:ok, children} <- get_children(node),
+ {:ok, material} <- reduce_oks(children, material, &parse(&1, &2)),
+ visual <- Robot.Visual.material(visual, material),
+ do: {:ok, visual}
+ end
+
+ defp parse(:origin, node, orientable) 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.Orientable.set(orientable, frame)}
+ end
+
+ defp parse(:mesh, node, geometric) do
+ with {:ok, filename} <- get_attribute_value(node, :filename),
+ [scale] <- extract_floats(node, :scale, [1.0]),
+ mesh <- Geometry.Mesh.init(filename),
+ mesh <- Geometry.Mesh.scale(mesh, scale),
+ do: {:ok, Robot.Geometric.set(geometric, mesh)}
+ end
+
+ defp parse(:axis, node, %Robot.Joint{} = joint) do
+ [x, y, z] = extract_floats(node, :xyz, [0, 0, 0])
+
+ translation = Coordinates.Cartesian.init(x, y, z)
+ {:ok, Robot.Joint.axis(joint, translation)}
+ 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(:calibration, node, %Robot.Joint{} = joint) do
+ [rising] = extract_floats(node, :rising, [nil])
+ [falling] = extract_floats(node, :falling, [nil])
+
+ calibration = Robot.Calibration.init(rising, falling)
+ {:ok, Robot.Joint.calibration(joint, calibration)}
+ end
+
+ defp parse(:dynamics, node, %Robot.Joint{} = joint) do
+ [damping] = extract_floats(node, :damping, [0])
+ [friction] = extract_floats(node, :friction, [0])
+
+ dynamics = Robot.Dynamics.init(damping, friction)
+ {:ok, Robot.Joint.dynamics(joint, dynamics)}
+ end
+
+ defp parse(:mimic, node, %Robot.Joint{} = joint) do
+ [multiplier] = extract_floats(node, :multiplier, [1])
+ [offset] = extract_floats(node, :offset, [0])
+
+ with {:ok, name} <- get_attribute_value(node, :joint),
+ mimic <- Robot.Mimic.init(name, multiplier, offset),
+ do: {:ok, Robot.Joint.mimic(joint, mimic)}
+ end
+
+ defp parse(:safety_controller, node, %Robot.Joint{} = joint) do
+ [soft_lower_limit] = extract_floats(node, :soft_lower_limit, [0])
+ [soft_upper_limit] = extract_floats(node, :soft_upper_limit, [0])
+ [k_position] = extract_floats(node, :k_position, [0])
+ [k_velocity] = extract_floats(node, :k_velocity, [0])
+
+ safety_controller =
+ Robot.SafetyController.init(k_velocity, k_position, soft_lower_limit, soft_upper_limit)
+
+ {:ok, Robot.Joint.safety_controller(joint, safety_controller)}
+ end
+
+ defp parse(:collision, node, %Robot.Link{} = link) do
+ collision =
+ case get_attribute_value(node, :name) do
+ {:ok, name} -> Robot.Collision.init(name)
+ _ -> Robot.Collision.init()
+ end
+
+ with {:ok, children} <- get_children(node),
+ {: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 parse(:transmission, node, %Robot{} = robot) do
+ with {:ok, name} <- get_attribute_value(node, :name),
+ transmission <- Robot.Transmission.init(name),
+ {:ok, children} <- get_children(node),
+ {:ok, transmission} <- reduce_oks(children, transmission, &parse(&1, &2)),
+ robot <- Robot.add_transmission(robot, transmission),
+ do: {:ok, robot}
+ end
+
+ defp parse(:type, node, %Robot.Transmission{} = transmission) do
+ with {:ok, contents} <- get_text(node),
+ do: {:ok, Robot.Transmission.type(transmission, String.trim(contents))}
+ end
+
+ defp parse(:joint, node, %Robot.Transmission{joints: joints} = transmission) do
+ with {:ok, name} <- get_attribute_value(node, :name),
+ joint <- Robot.TransmissionJoint.init(name),
+ {:ok, children} <- get_children(node),
+ {:ok, joint} <- reduce_oks(children, joint, &parse(&1, &2)),
+ transmission <- Robot.Transmission.joints(transmission, [joint | joints]),
+ do: {:ok, transmission}
+ end
+
+ defp parse(
+ :hardwareInterface,
+ node,
+ %Robot.TransmissionJoint{hardware_interfaces: hardware_interfaces} = joint
+ ) do
+ with {:ok, contents} <- get_text(node),
+ hardware_interface <- Robot.HardwareInterface.init(contents),
+ joint <-
+ Robot.TransmissionJoint.hardware_interfaces(joint, [
+ hardware_interface | hardware_interfaces
+ ]),
+ do: {:ok, joint}
+ end
+
+ defp parse(
+ :hardwareInterface,
+ node,
+ %Robot.Actuator{hardware_interfaces: hardware_interfaces} = actuator
+ ) do
+ with {:ok, contents} <- get_text(node),
+ hardware_interface <- Robot.HardwareInterface.init(contents),
+ actuator <-
+ Robot.Actuator.hardware_interfaces(actuator, [
+ hardware_interface | hardware_interfaces
+ ]),
+ do: {:ok, actuator}
+ end
+
+ defp parse(:actuator, node, %Robot.Transmission{actuators: actuators} = transmission) do
+ with {:ok, name} <- get_attribute_value(node, :name),
+ actuator <- Robot.Actuator.init(name),
+ {:ok, children} <- get_children(node),
+ {:ok, actuator} <- reduce_oks(children, actuator, &parse(&1, &2)),
+ transmission <- Robot.Transmission.actuators(transmission, [actuator | actuators]),
+ do: {:ok, transmission}
+ end
+
+ defp parse(:mechanicalReduction, node, %Robot.Actuator{} = actuator) do
+ with {:ok, contents} <- get_text(node),
+ {value, _} <- Float.parse(String.trim(contents)),
+ actuator <- Robot.Actuator.mechanical_reduction(actuator, value),
+ do: {:ok, actuator}
+ end
+
+ defp parse(:texture, node, %Robot.Material{} = material) do
+ with {:ok, filename} <- get_attribute_value(node, :filename),
+ texture <- Robot.Texture.init(filename),
+ material <- Robot.Material.texture(material, texture),
+ do: {:ok, material}
+ end
+
+ defp parse(:gazebo, _node, %Robot{name: name} = robot) do
+ Logger.warn("Ignoring gazebo element in robot #{inspect(name)}")
+ {:ok, robot}
+ end
+
+ defp extract_floats(node, attribute_name, default_value) do
+ with {:ok, string} <- get_attribute_value(node, attribute_name),
+ string_values <- String.split(string, ~r/\s+/),
+ float_values <-
+ string_values
+ |> Enum.filter(&(byte_size(&1) > 0))
+ |> Enum.map(&elem(Float.parse(&1), 0)) do
+ float_values
+ else
+ _ -> default_value
+ end
+ end
+end
diff --git a/lib/kinemat/urdf/save.ex b/lib/kinemat/urdf/save.ex
index 0cbd96f..c1672e1 100644
--- a/lib/kinemat/urdf/save.ex
+++ b/lib/kinemat/urdf/save.ex
@@ -46,6 +46,9 @@ defmodule Kinemat.URDF.Save do
{:robot, [name: name], Enum.reverse(contents)}
end
+ defp to_tag(%Robot.Material{name: name, colour: nil}),
+ do: {:material, [name: name], []}
+
defp to_tag(%Robot.Material{name: name, colour: colour}),
do: {:material, [name: name], [to_tag(colour)]}
@@ -72,7 +75,7 @@ defmodule Kinemat.URDF.Save do
{:link, [name: name], contents}
end
- defp to_tag(%Robot.Visual{geometry: geometry, origin: origin, material_name: material_name}) do
+ defp to_tag(%Robot.Visual{geometry: geometry, origin: origin, material: material}) do
geometry =
geometry
|> to_tag()
@@ -88,8 +91,8 @@ defmodule Kinemat.URDF.Save do
else: contents
contents =
- if material_name,
- do: [{:material, [name: material_name], []} | contents],
+ if material,
+ do: [to_tag(material) | contents],
else: contents
{:visual, [], Enum.reverse(contents)}
diff --git a/lib/kinemat/urdf/xml_helpers.ex b/lib/kinemat/urdf/xml_helpers.ex
index 8f81fde..d0acb0b 100644
--- a/lib/kinemat/urdf/xml_helpers.ex
+++ b/lib/kinemat/urdf/xml_helpers.ex
@@ -56,6 +56,18 @@ defmodule Kinemat.URDF.XmlHelpers do
def get_content(node) when is_element(node), do: {:ok, xmlElement(node, :content)}
def get_content(_node), do: {:error, "Node is not an element"}
+ @doc """
+ Return all the text contents of the element, if any.
+ """
+ def get_text(node) when is_element(node) do
+ with contents <- xmlElement(node, :content),
+ text_nodes <- Enum.map(contents, &xmlText(&1, :value)),
+ result <- Enum.join(text_nodes, ""),
+ do: {:ok, result}
+ end
+
+ def get_text(_node), do: {:error, "Node is not an element"}
+
@doc """
Returns all the child elements of an XML element.
"""
diff --git a/test/kinemat/urdf/load_test.exs b/test/kinemat/urdf/load_test.exs
index 64a1d27..76df1c0 100644
--- a/test/kinemat/urdf/load_test.exs
+++ b/test/kinemat/urdf/load_test.exs
@@ -3,14 +3,26 @@ defmodule Kinemat.URDF.LoadTest do
import Kinemat.URDF.Load
import Angle.Sigil
import KinematTest.FileHelpers
- alias Kinemat.{Coordinates.Cartesian, Geometry, Orientations.Euler, Robot}
+
+ alias Kinemat.{
+ Coordinates.Cartesian,
+ Frame,
+ Geometry,
+ Orientations.Euler,
+ Orientations.RotationMatrix,
+ Robot
+ }
+
+ @angle_04 ~a(0.4)r
+ @angle_05 ~a(0.5)r
+ @angle_06 ~a(0.6)r
describe "load/1" do
test "when the file doesn't exist it returns an error" do
assert {:error, _} = load(fixture_path("urdf/fake.file"))
end
- test "it parses `one_shape.urdf`" do
+ test "it loads and parses `one_shape.urdf`" do
assert {:ok, robot} = load(fixture_path("urdf/one_shape.urdf"))
assert robot.name == "myfirst"
@@ -22,7 +34,7 @@ defmodule Kinemat.URDF.LoadTest do
assert shape.radius == 0.2
end
- test "it parses `multiple_shapes.urdf`" do
+ test "it loads and parses `multiple_shapes.urdf`" do
assert {:ok, robot} = load(fixture_path("urdf/multiple_shapes.urdf"))
assert robot.name == "multipleshapes"
@@ -36,108 +48,853 @@ defmodule Kinemat.URDF.LoadTest do
assert link1.name == "base_link"
assert [joint] = robot.joints
assert joint.name == "base_to_right_leg"
- assert joint.type == "fixed"
+ assert joint.type == :fixed
assert joint.parent_name == "base_link"
assert joint.child_name == "right_leg"
end
+
+ test "it loads and parses `origins.urdf`" do
+ {:ok, robot} = load(fixture_path("urdf/origins.urdf"))
+
+ assert [joint] = robot.joints
+ assert joint.origin.point.x == 0
+ assert joint.origin.point.y == -0.22
+ assert joint.origin.point.z == 0.25
+
+ assert {:ok, link} = Robot.get_link(robot, "right_leg")
+ assert link.visual.origin.orientation.__struct__ == Euler
+ assert link.visual.origin.orientation.representation == :xyz
+ assert link.visual.origin.orientation.x == ~a(0)
+ assert link.visual.origin.orientation.y == ~a(1.57075)r
+ assert link.visual.origin.orientation.z == ~a(0)
+ assert link.visual.origin.point.__struct__ == Cartesian
+ assert link.visual.origin.point.x == 0
+ assert link.visual.origin.point.y == 0
+ assert link.visual.origin.point.z == -0.3
+ end
+
+ test "it loads and parses `materials.urdf`" do
+ {:ok, robot} = load(fixture_path("urdf/materials.urdf"))
+
+ assert [materials0, materials1] = robot.materials
+
+ assert materials0.name == "white"
+ assert materials0.colour.r == 1
+ assert materials0.colour.g == 1
+ assert materials0.colour.b == 1
+ assert materials0.colour.a == 1
+
+ assert materials1.name == "blue"
+ assert materials1.colour.r == 0
+ assert materials1.colour.g == 0
+ assert materials1.colour.b == 0.8
+ assert materials1.colour.a == 1
+
+ assert {:ok, link} = Robot.get_link(robot, "base_link")
+ assert link.visual.material.name == "blue"
+
+ assert {:ok, link} = Robot.get_link(robot, "right_leg")
+ assert link.visual.material.name == "white"
+ end
+
+ test "it loads and parses `visual.urdf`" do
+ {:ok, robot} = load(fixture_path("urdf/visual.urdf"))
+
+ assert {:ok, link} = Robot.get_link(robot, "left_gripper")
+ 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.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.geometry
+ assert mesh.filename == "package://urdf_tutorial/meshes/l_finger.dae"
+
+ assert {:ok, link} = Robot.get_link(robot, "right_tip")
+ 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.geometry
+ assert sphere.radius == 0.2
+ end
+
+ test "it loads and parses `flexible.urdf`" do
+ {:ok, robot} = load(fixture_path("urdf/flexible.urdf"))
+
+ assert {:ok, joint} = Robot.get_joint(robot, "right_front_wheel_joint")
+ assert Cartesian.x(joint.axis) == 0
+ assert Cartesian.y(joint.axis) == 1.0
+ assert Cartesian.z(joint.axis) == 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 loads and 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
- test "it parses `origins.urdf`" do
- {:ok, robot} = load(fixture_path("urdf/origins.urdf"))
+ describe "parse/1" do
+ test "it parses the robot element" do
+ {:ok, robot} = parse(~S[])
+ assert robot.name == "Marty McFly"
+ end
- assert [joint] = robot.joints
- assert joint.origin.point.x == 0
- assert joint.origin.point.y == -0.22
- assert joint.origin.point.z == 0.25
+ test "it parses a transmission element" do
+ xml = ~S"""
+
+
+
+
+ """
- assert {:ok, link} = Robot.get_link(robot, "right_leg")
- assert link.visual.origin.orientation.__struct__ == Euler
- assert link.visual.origin.orientation.representation == :xyz
- assert link.visual.origin.orientation.x == ~a(0)
- assert link.visual.origin.orientation.y == ~a(1.57075)r
- assert link.visual.origin.orientation.z == ~a(0)
- assert link.visual.origin.point.__struct__ == Cartesian
- assert link.visual.origin.point.x == 0
- assert link.visual.origin.point.y == 0
- assert link.visual.origin.point.z == -0.3
- end
+ assert {:ok, robot} = parse(xml)
+ assert [%Robot.Transmission{name: "simple_trans"}] = robot.transmissions
+ end
- test "it parses `materials.urdf`" do
- {:ok, robot} = load(fixture_path("urdf/materials.urdf"))
+ test "it parses a transmission type element" do
+ xml = ~S"""
+
+
+ transmission_interface/SimpleTransmission
+
+
+ """
- assert [materials0, materials1] = robot.materials
+ assert {:ok, robot} = parse(xml)
+ assert [transmission] = robot.transmissions
+ assert transmission.type == "transmission_interface/SimpleTransmission"
+ end
- assert materials0.name == "white"
- assert materials0.colour.r == 1
- assert materials0.colour.g == 1
- assert materials0.colour.b == 1
- assert materials0.colour.a == 1
+ test "it parses a transmission joint element" do
+ xml = ~S"""
+
+
+
+
+
+ """
- assert materials1.name == "blue"
- assert materials1.colour.r == 0
- assert materials1.colour.g == 0
- assert materials1.colour.b == 0.8
- assert materials1.colour.a == 1
+ assert {:ok, robot} = parse(xml)
+ assert [transmission] = robot.transmissions
+ assert [%Robot.TransmissionJoint{name: "foo_joint"}] = transmission.joints
+ end
- assert {:ok, link} = Robot.get_link(robot, "base_link")
- assert link.visual.material_name == "blue"
+ test "it parses a transmission joint containing a hardware interface element" do
+ xml = ~S"""
+
+
+
+ EffortJointInterface
+
+
+
+ """
- assert {:ok, link} = Robot.get_link(robot, "right_leg")
- assert link.visual.material_name == "white"
- end
+ assert {:ok, robot} = parse(xml)
+ assert [transmission] = robot.transmissions
+ assert [joint] = transmission.joints
- test "it parses `visual.urdf`" do
- {:ok, robot} = load(fixture_path("urdf/visual.urdf"))
+ assert [%Robot.HardwareInterface{name: "EffortJointInterface"}] = joint.hardware_interfaces
+ end
- assert {:ok, link} = Robot.get_link(robot, "left_gripper")
- assert mesh = link.visual.geometry
- assert mesh.filename == "package://urdf_tutorial/meshes/l_finger.dae"
+ test "it parses a transmission actuator element" do
+ xml = ~S"""
+
+
+
+
+
+ """
- assert {:ok, link} = Robot.get_link(robot, "left_tip")
- assert mesh = link.visual.geometry
- assert mesh.filename == "package://urdf_tutorial/meshes/l_finger_tip.dae"
+ assert {:ok, robot} = parse(xml)
+ assert [transmission] = robot.transmissions
+ assert [%Robot.Actuator{name: "foo_motor"}] = transmission.actuators
+ end
- assert {:ok, link} = Robot.get_link(robot, "right_gripper")
- assert mesh = link.visual.geometry
- assert mesh.filename == "package://urdf_tutorial/meshes/l_finger.dae"
+ test "it parses a transmission actuator containing a mechanical reduction element" do
+ xml = ~S"""
+
+
+
+ 50
+
+
+
+ """
- assert {:ok, link} = Robot.get_link(robot, "right_tip")
- assert mesh = link.visual.geometry
- assert mesh.filename == "package://urdf_tutorial/meshes/l_finger_tip.dae"
+ assert {:ok, robot} = parse(xml)
+ assert [transmission] = robot.transmissions
+ assert [actuator] = transmission.actuators
+ assert actuator.mechanical_reduction == 50
+ end
- assert {:ok, link} = Robot.get_link(robot, "head")
- assert sphere = link.visual.geometry
- assert sphere.radius == 0.2
- end
+ test "it parses a transmission actuator containing a hardware interface element" do
+ xml = ~S"""
+
+
+
+ EffortJointInterface
+
+
+
+ """
- test "it parses `flexible.urdf`" do
- {:ok, robot} = load(fixture_path("urdf/flexible.urdf"))
+ assert {:ok, robot} = parse(xml)
+ assert [transmission] = robot.transmissions
+ assert [actuator] = transmission.actuators
- 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 [%Robot.HardwareInterface{name: "EffortJointInterface"}] =
+ actuator.hardware_interfaces
+ end
- assert translation = joint.axis.point
- assert Cartesian.x(translation) == 0
- assert Cartesian.y(translation) == 1.0
- assert Cartesian.z(translation) == 0
+ test "it parses a link element" do
+ xml = ~S"""
+
+
+
+ """
- 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
+ assert {:ok, robot} = parse(xml)
+ assert [%Robot.Link{name: "my_link"}] = robot.links
+ end
- test "it parses `physics.urdf`" do
- {:ok, robot} = load(fixture_path("urdf/physics.urdf"))
+ test "it parses a link containing an inertial element" do
+ xml = ~S"""
+
+
+
+
+
+ """
- assert {:ok, link} = Robot.get_link(robot, "base_link")
- assert %Geometry.Cylinder{length: 0.6, radius: 0.2} = link.collision.geometry
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+ assert %Robot.Inertia{} = link.inertia
+ end
- 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
+ test "it parses a link containing an inertial origin element" do
+ xml = ~S"""
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+
+ assert %Frame{
+ point: %Cartesian{x: 0.1, y: 0.2, z: 0.3},
+ orientation: %Euler{x: @angle_04, y: @angle_05, z: @angle_06, representation: :xyz}
+ } = link.inertia.origin
+ end
+
+ test "it parses a link containing an inertial mass element" do
+ xml = ~S"""
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+ assert link.inertia.mass == 1
+ end
+
+ test "it parses a link containing an inertial inertia element" do
+ xml = ~S"""
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+ assert %RotationMatrix{matrix: {1, 2, 3, 2, 4, 5, 3, 5, 6}}
+ end
+
+ test "it parses a link containing an visual element" do
+ xml = ~S"""
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+ assert %Robot.Visual{} = link.visual
+ end
+
+ test "it parses a link containing an visual origin element" do
+ xml = ~S"""
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+
+ assert %Frame{
+ point: %Cartesian{x: 0.1, y: 0.2, z: 0.3},
+ orientation: %Euler{x: @angle_04, y: @angle_05, z: @angle_06, representation: :xyz}
+ } = link.visual.origin
+ end
+
+ test "it parses a link containing a visual geometry element" do
+ xml = ~S"""
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+ refute link.visual.geometry
+ end
+
+ test "it parses a link containing a visual geometry box" do
+ xml = ~S"""
+
+
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+ assert %Geometry.Box{x: 1.0, y: 2.0, z: 3.0} = link.visual.geometry
+ end
+
+ test "it parses a link containing a visual geometry cylinder" do
+ xml = ~S"""
+
+
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+ assert %Geometry.Cylinder{length: 1.0, radius: 2.0} = link.visual.geometry
+ end
+
+ test "it parses a link containing a visual geometry sphere" do
+ xml = ~S"""
+
+
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+ assert %Geometry.Sphere{radius: 2.0} = link.visual.geometry
+ end
+
+ test "it parses a link containing a visual geometry mesh" do
+ xml = ~S"""
+
+
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+ assert %Geometry.Mesh{filename: "myfile", scale: 2.0} = link.visual.geometry
+ end
+
+ test "it parses a link containing a visual material element" do
+ xml = ~S"""
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+ assert %Robot.Material{name: "my material"} = link.visual.material
+ end
+
+ test "it parses a link containing a visual material color element" do
+ xml = ~S"""
+
+
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+ assert %Robot.Colour{r: 0.25, g: 0.5, b: 0.75, a: 1.0} = link.visual.material.colour
+ end
+
+ test "it parses a link containing a visual material texture element" do
+ xml = ~S"""
+
+
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+ assert %Robot.Texture{filename: "mytexture"} = link.visual.material.texture
+ end
+
+ test "it parses a link containing a visual origin element" do
+ xml = ~S"""
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+
+ assert %Frame{
+ point: %Cartesian{x: 0.1, y: 0.2, z: 0.3},
+ orientation: %Euler{x: @angle_04, y: @angle_05, z: @angle_06, representation: :xyz}
+ } = link.visual.origin
+ end
+
+ test "it parses a link containing a collision element" do
+ xml = ~S"""
+
+
+
+
+
+ """
+
+ assert {:ok, _robot} = parse(xml)
+ end
+
+ test "it parses a link containing a collision element with a name attribute" do
+ xml = ~S"""
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+ assert %Robot.Collision{name: "crash"} = link.collision
+ end
+
+ test "it parses a link containing a collision origin element" do
+ xml = ~S"""
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+
+ assert %Frame{
+ point: %Cartesian{x: 0.1, y: 0.2, z: 0.3},
+ orientation: %Euler{x: @angle_04, y: @angle_05, z: @angle_06, representation: :xyz}
+ } = link.collision.origin
+ end
+
+ test "it parses a link containing a collision geometry element" do
+ xml = ~S"""
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, _robot} = parse(xml)
+ end
+
+ test "it parses a link containing a collision geometry box element" do
+ xml = ~S"""
+
+
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+ assert %Geometry.Box{x: 1.0, y: 2.0, z: 3.0} = link.collision.geometry
+ end
+
+ test "it parses a link containing a collision geometry cylinder" do
+ xml = ~S"""
+
+
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+ assert %Geometry.Cylinder{length: 1.0, radius: 2.0} = link.collision.geometry
+ end
+
+ test "it parses a link containing a collision geometry sphere" do
+ xml = ~S"""
+
+
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+ assert %Geometry.Sphere{radius: 2.0} = link.collision.geometry
+ end
+
+ test "it parses a link containing a collision geometry mesh" do
+ xml = ~S"""
+
+
+
+
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [link] = robot.links
+ assert %Geometry.Mesh{filename: "myfile", scale: 2.0} = link.collision.geometry
+ end
+
+ test "it parses a joint" do
+ xml = ~S"""
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [_joint] = robot.joints
+ end
+
+ test "it parses a joint origin element" do
+ xml = ~S"""
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [joint] = robot.joints
+
+ assert %Frame{
+ point: %Cartesian{x: 0.1, y: 0.2, z: 0.3},
+ orientation: %Euler{x: @angle_04, y: @angle_05, z: @angle_06, representation: :xyz}
+ } = joint.origin
+ end
+
+ test "it parses a joint parent element" do
+ xml = ~S"""
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [joint] = robot.joints
+ assert %Robot.Joint{parent_name: "my_parent"} = joint
+ end
+
+ test "it parses a joint child element" do
+ xml = ~S"""
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [joint] = robot.joints
+ assert %Robot.Joint{child_name: "my_child"} = joint
+ end
+
+ test "it parses a joint axis element" do
+ xml = ~S"""
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [joint] = robot.joints
+ assert %Cartesian{x: 0.1, y: 0.2, z: 0.3} = joint.axis
+ end
+
+ test "it parses a joint calibration element" do
+ xml = ~S"""
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [joint] = robot.joints
+ assert %Robot.Calibration{rising: nil, falling: nil} = joint.calibration
+ end
+
+ test "it parses a joint calibration element with rising and falling attributes" do
+ xml = ~S"""
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [joint] = robot.joints
+ assert %Robot.Calibration{rising: 1.0, falling: 2.0} = joint.calibration
+ end
+
+ test "it parses a joint dynamics element" do
+ xml = ~S"""
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [joint] = robot.joints
+ assert %Robot.Dynamics{damping: 0, friction: 0} = joint.dynamics
+ end
+
+ test "it parses a joint dynamics element with damping and friction attributes" do
+ xml = ~S"""
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [joint] = robot.joints
+ assert %Robot.Dynamics{damping: 1.0, friction: 2.0} = joint.dynamics
+ end
+
+ test "it parses a joint limit element" do
+ xml = ~S"""
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [joint] = robot.joints
+ assert %Robot.Limit{lower: 0, upper: 0} = joint.limit
+ end
+
+ test "it parses a joint limit element with an effort and velocity attribute" do
+ xml = ~S"""
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [joint] = robot.joints
+ assert %Robot.Limit{effort: 13.0, velocity: 14.0} = joint.limit
+ end
+
+ test "it parses a joint mimic element" do
+ xml = ~S"""
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [joint] = robot.joints
+ assert %Robot.Mimic{joint_name: "other_joint", multiplier: 1, offset: 0} = joint.mimic
+ end
+
+ test "it parses a joint mimic element with multiplier and offset attributes" do
+ xml = ~S"""
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [joint] = robot.joints
+ assert %Robot.Mimic{joint_name: "other_joint", multiplier: 2.0, offset: 3.0} = joint.mimic
+ end
+
+ test "it parses a joint safety controller element" do
+ xml = ~S"""
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [joint] = robot.joints
+
+ assert %Robot.SafetyController{
+ k_velocity: 13.0,
+ k_position: 0,
+ soft_lower_limit: 0,
+ soft_upper_limit: 0
+ } = joint.safety_controller
+ end
+
+ test "it parses a joint safety controller element with k position, soft lower limit and soft upper limit attributes" do
+ xml = ~S"""
+
+
+
+
+
+ """
+
+ assert {:ok, robot} = parse(xml)
+ assert [joint] = robot.joints
+
+ assert %Robot.SafetyController{
+ k_velocity: 13.0,
+ k_position: 14.0,
+ soft_lower_limit: 1.0,
+ soft_upper_limit: 2.0
+ } = joint.safety_controller
+ end
+
+ test "it ignores the gazebo element" do
+ xml = ~S"""
+
+
+
+ """
+
+ {:ok, _robot} = parse(xml)
+ end
end
end