Can now parse the entire URDF spec.
This commit is contained in:
James Harton
parent
ce7a32c588
commit
ed941134a3
21 changed files with 1558 additions and 296 deletions
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
"""
|
||||
|
|
|
|||
39
lib/kinemat/robot/actuator.ex
Normal file
39
lib/kinemat/robot/actuator.ex
Normal file
|
|
@ -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
|
||||
21
lib/kinemat/robot/calibration.ex
Normal file
21
lib/kinemat/robot/calibration.ex
Normal file
|
|
@ -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
|
||||
|
|
@ -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.
|
||||
"""
|
||||
|
|
|
|||
21
lib/kinemat/robot/dynamics.ex
Normal file
21
lib/kinemat/robot/dynamics.ex
Normal file
|
|
@ -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
|
||||
22
lib/kinemat/robot/hardware_interface.ex
Normal file
22
lib/kinemat/robot/hardware_interface.ex
Normal file
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
23
lib/kinemat/robot/mimic.ex
Normal file
23
lib/kinemat/robot/mimic.ex
Normal file
|
|
@ -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
|
||||
29
lib/kinemat/robot/safety_controller.ex
Normal file
29
lib/kinemat/robot/safety_controller.ex
Normal file
|
|
@ -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
|
||||
16
lib/kinemat/robot/texture.ex
Normal file
16
lib/kinemat/robot/texture.ex
Normal file
|
|
@ -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
|
||||
52
lib/kinemat/robot/transmission.ex
Normal file
52
lib/kinemat/robot/transmission.ex
Normal file
|
|
@ -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
|
||||
31
lib/kinemat/robot/transmission_joint.ex
Normal file
31
lib/kinemat/robot/transmission_joint.ex
Normal file
|
|
@ -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
|
||||
|
|
@ -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),
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
322
lib/kinemat/urdf/parser.ex
Normal file
322
lib/kinemat/urdf/parser.ex
Normal file
|
|
@ -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
|
||||
|
|
@ -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)}
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
"""
|
||||
|
|
|
|||
|
|
@ -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[<robot name="Marty McFly"/>])
|
||||
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"""
|
||||
<robot name="">
|
||||
<transmission name="simple_trans">
|
||||
</transmission>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<transmission name="simple_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
</transmission>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<transmission name="simple_trans">
|
||||
<joint name="foo_joint" />
|
||||
</transmission>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<transmission name="simple_trans">
|
||||
<joint name="foo_joint">
|
||||
<hardwareInterface>EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
</transmission>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<transmission name="simple_trans">
|
||||
<actuator name="foo_motor" />
|
||||
</transmission>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<transmission name="simple_trans">
|
||||
<actuator name="foo_motor">
|
||||
<mechanicalReduction>50</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<transmission name="simple_trans">
|
||||
<actuator name="foo_motor">
|
||||
<hardwareInterface>EffortJointInterface</hardwareInterface>
|
||||
</actuator>
|
||||
</transmission>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link" />
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<inertial />
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<inertial>
|
||||
<origin xyz="0.1 0.2 0.3" rpy="0.4 0.5 0.6"/>
|
||||
</inertial>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<inertial>
|
||||
<mass value="1" />
|
||||
</inertial>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<inertial>
|
||||
<inertia ixx="1" ixy="2" ixz="3" iyy="4" iyz="5" izz="6" />
|
||||
</inertial>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<visual />
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<visual>
|
||||
<origin xyz="0.1 0.2 0.3" rpy="0.4 0.5 0.6"/>
|
||||
</visual>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<visual>
|
||||
<geometry/>
|
||||
</visual>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="1 2 3" />
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="1" radius="2" />
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="2" />
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="myfile" scale="2" />
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<visual>
|
||||
<material name="my material" />
|
||||
</visual>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<visual>
|
||||
<material name="my material">
|
||||
<color rgba="0.25 0.5 0.75 1" />
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<visual>
|
||||
<material name="my material">
|
||||
<texture filename="mytexture" />
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<visual>
|
||||
<origin xyz="0.1 0.2 0.3" rpy="0.4 0.5 0.6"/>
|
||||
</visual>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<collision />
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
assert {:ok, _robot} = parse(xml)
|
||||
end
|
||||
|
||||
test "it parses a link containing a collision element with a name attribute" do
|
||||
xml = ~S"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<collision name="crash" />
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<collision>
|
||||
<origin xyz="0.1 0.2 0.3" rpy="0.4 0.5 0.6"/>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<collision>
|
||||
<geometry/>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
assert {:ok, _robot} = parse(xml)
|
||||
end
|
||||
|
||||
test "it parses a link containing a collision geometry box element" do
|
||||
xml = ~S"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="1 2 3" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="1" radius="2" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<collision>
|
||||
<geometry>
|
||||
<sphere radius="2" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<link name="my_link">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="myfile" scale="2" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<joint name="my_joint" type="fixed" />
|
||||
</robot>
|
||||
"""
|
||||
|
||||
assert {:ok, robot} = parse(xml)
|
||||
assert [_joint] = robot.joints
|
||||
end
|
||||
|
||||
test "it parses a joint origin element" do
|
||||
xml = ~S"""
|
||||
<robot name="">
|
||||
<joint name="my_joint" type="fixed">
|
||||
<origin xyz="0.1 0.2 0.3" rpy="0.4 0.5 0.6"/>
|
||||
</joint>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<joint name="my_joint" type="fixed">
|
||||
<parent link="my_parent" />
|
||||
</joint>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<joint name="my_joint" type="fixed">
|
||||
<child link="my_child" />
|
||||
</joint>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<joint name="my_joint" type="fixed">
|
||||
<axis xyz="0.1 0.2 0.3" />
|
||||
</joint>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<joint name="my_joint" type="fixed">
|
||||
<calibration />
|
||||
</joint>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<joint name="my_joint" type="fixed">
|
||||
<calibration rising="1" falling="2" />
|
||||
</joint>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<joint name="my_joint" type="fixed">
|
||||
<dynamics />
|
||||
</joint>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<joint name="my_joint" type="fixed">
|
||||
<dynamics damping="1" friction="2"/>
|
||||
</joint>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<joint name="my_joint" type="fixed">
|
||||
<limit />
|
||||
</joint>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<joint name="my_joint" type="fixed">
|
||||
<limit effort="13" velocity="14" />
|
||||
</joint>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<joint name="my_joint" type="fixed">
|
||||
<mimic joint="other_joint" />
|
||||
</joint>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<joint name="my_joint" type="fixed">
|
||||
<mimic joint="other_joint" multiplier="2" offset="3" />
|
||||
</joint>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<joint name="my_joint" type="fixed">
|
||||
<safety_controller k_velocity="13" />
|
||||
</joint>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<joint name="my_joint" type="fixed">
|
||||
<safety_controller k_velocity="13" k_position="14" soft_lower_limit="1" soft_upper_limit="2" />
|
||||
</joint>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
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"""
|
||||
<robot name="">
|
||||
<gazebo />
|
||||
</robot>
|
||||
"""
|
||||
|
||||
{:ok, _robot} = parse(xml)
|
||||
end
|
||||
end
|
||||
end
|
||||
|
|
|
|||
Loading…
Reference in a new issue