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