Can now parse the entire URDF spec.
This commit is contained in:
parent
ce7a32c588
commit
ed941134a3
21 changed files with 1558 additions and 296 deletions
|
@ -1,5 +1,5 @@
|
||||||
defmodule Kinemat.Geometry.Mesh do
|
defmodule Kinemat.Geometry.Mesh do
|
||||||
defstruct filename: nil
|
defstruct filename: nil, scale: 1
|
||||||
alias Kinemat.Geometry.Mesh
|
alias Kinemat.Geometry.Mesh
|
||||||
|
|
||||||
@moduledoc """
|
@moduledoc """
|
||||||
|
@ -16,4 +16,11 @@ defmodule Kinemat.Geometry.Mesh do
|
||||||
"""
|
"""
|
||||||
@spec init(Path.t()) :: Mesh.t()
|
@spec init(Path.t()) :: Mesh.t()
|
||||||
def init(filename), do: %Mesh{filename: filename}
|
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
|
end
|
||||||
|
|
|
@ -1,6 +1,6 @@
|
||||||
defmodule Kinemat.Robot do
|
defmodule Kinemat.Robot do
|
||||||
defstruct name: nil, links: [], joints: [], materials: []
|
defstruct name: nil, links: [], joints: [], materials: [], transmissions: []
|
||||||
alias Kinemat.{Robot, Robot.Joint, Robot.Link, Robot.Material}
|
alias Kinemat.{Robot, Robot.Joint, Robot.Link, Robot.Material, Robot.Transmission}
|
||||||
|
|
||||||
@moduledoc """
|
@moduledoc """
|
||||||
A data-structure representing a robot.
|
A data-structure representing a robot.
|
||||||
|
@ -10,7 +10,8 @@ defmodule Kinemat.Robot do
|
||||||
name: String.t(),
|
name: String.t(),
|
||||||
links: [Link.t()],
|
links: [Link.t()],
|
||||||
joints: [Joint.t()],
|
joints: [Joint.t()],
|
||||||
materials: [Material.t()]
|
materials: [Material.t()],
|
||||||
|
transmissions: [Transmission.t()]
|
||||||
}
|
}
|
||||||
|
|
||||||
@doc """
|
@doc """
|
||||||
|
@ -33,6 +34,13 @@ defmodule Kinemat.Robot do
|
||||||
def add_joint(%Robot{joints: joints} = robot, %Joint{} = joint),
|
def add_joint(%Robot{joints: joints} = robot, %Joint{} = joint),
|
||||||
do: %Robot{robot | joints: [joint | joints]}
|
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 """
|
@doc """
|
||||||
Add a new material to the robot.
|
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
|
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}
|
alias Kinemat.{Frame, Geometry, Robot.Collision, Robot.Geometric, Robot.Orientable}
|
||||||
|
|
||||||
@moduledoc """
|
@moduledoc """
|
||||||
|
@ -8,7 +8,8 @@ defmodule Kinemat.Robot.Collision do
|
||||||
|
|
||||||
@type t :: %Collision{
|
@type t :: %Collision{
|
||||||
geometry: Geometry.t() | nil,
|
geometry: Geometry.t() | nil,
|
||||||
origin: Frame.t() | nil
|
origin: Frame.t() | nil,
|
||||||
|
name: String.t() | nil
|
||||||
}
|
}
|
||||||
|
|
||||||
@doc """
|
@doc """
|
||||||
|
@ -17,6 +18,12 @@ defmodule Kinemat.Robot.Collision do
|
||||||
@spec init :: Collision.t()
|
@spec init :: Collision.t()
|
||||||
def init, do: %Collision{}
|
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 """
|
@doc """
|
||||||
Set the geometry to be stored in the collision container.
|
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,
|
child_name: nil,
|
||||||
origin: nil,
|
origin: nil,
|
||||||
axis: 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 """
|
@moduledoc """
|
||||||
Represents a joint in a Robot body.
|
Represents a joint in a Robot body.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
|
@type joint_type :: :revolute | :continuous | :prismatic | :fixed | :floating | :planar
|
||||||
|
|
||||||
@type t :: %Joint{
|
@type t :: %Joint{
|
||||||
name: String.t(),
|
name: String.t(),
|
||||||
type: String.t(),
|
type: joint_type | nil,
|
||||||
parent_name: String.t() | nil,
|
parent_name: String.t() | nil,
|
||||||
child_name: String.t() | nil,
|
child_name: String.t() | nil,
|
||||||
origin: Frame | nil,
|
origin: Frame.t() | nil,
|
||||||
axis: Frame | nil,
|
axis: Orientation.t() | nil,
|
||||||
limit: Limit | nil
|
limit: Limit.t() | nil,
|
||||||
|
calibration: Calibration.t() | nil,
|
||||||
|
dynamics: Dynamics.t() | nil,
|
||||||
|
mimic: Mimic.t() | nil,
|
||||||
|
safety_controller: SafetyController.t() | nil
|
||||||
}
|
}
|
||||||
|
|
||||||
@doc """
|
@doc """
|
||||||
Initialise a new joint.
|
Initialise a new joint.
|
||||||
"""
|
"""
|
||||||
@spec init(String.t(), String.t()) :: Joint.t()
|
@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 """
|
@doc """
|
||||||
Set the name of the parent link.
|
Set the name of the parent link.
|
||||||
|
@ -44,7 +69,7 @@ defmodule Kinemat.Robot.Joint do
|
||||||
@doc """
|
@doc """
|
||||||
Set the reference frame of the joint axis.
|
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}
|
def axis(%Joint{} = joint, frame), do: %Joint{joint | axis: frame}
|
||||||
|
|
||||||
@doc """
|
@doc """
|
||||||
|
@ -53,10 +78,44 @@ defmodule Kinemat.Robot.Joint do
|
||||||
@spec limit(Joint.t(), Limit.t()) :: Joint.t()
|
@spec limit(Joint.t(), Limit.t()) :: Joint.t()
|
||||||
def limit(%Joint{} = joint, %Limit{} = limit), do: %Joint{joint | limit: limit}
|
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
|
defimpl Orientable do
|
||||||
|
import Angle.Sigil
|
||||||
|
alias Kinemat.{Coordinates.Cartesian, Frame, Orientations.Euler}
|
||||||
|
|
||||||
def set(%Joint{} = joint, %Frame{} = frame),
|
def set(%Joint{} = joint, %Frame{} = frame),
|
||||||
do: %Joint{joint | origin: 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
|
def get(%Joint{origin: origin}), do: origin
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
|
|
|
@ -17,7 +17,7 @@ defmodule Kinemat.Robot.Limit do
|
||||||
@type option :: {:effort | :lower | :upper | :velocity, number | nil}
|
@type option :: {:effort | :lower | :upper | :velocity, number | nil}
|
||||||
|
|
||||||
@doc """
|
@doc """
|
||||||
Initialise a new effort container with the provided values.
|
Initialise a new limit container with the provided values.
|
||||||
"""
|
"""
|
||||||
@spec init(options) :: Limit.t()
|
@spec init(options) :: Limit.t()
|
||||||
def init(options) when is_list(options) do
|
def init(options) when is_list(options) do
|
||||||
|
|
|
@ -1,12 +1,16 @@
|
||||||
defmodule Kinemat.Robot.Material do
|
defmodule Kinemat.Robot.Material do
|
||||||
defstruct name: nil, colour: nil
|
defstruct name: nil, colour: nil, texture: nil
|
||||||
alias Kinemat.Robot.{Colour, Material}
|
alias Kinemat.Robot.{Colour, Material, Texture}
|
||||||
|
|
||||||
@moduledoc """
|
@moduledoc """
|
||||||
A material used in the Robot description.
|
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 """
|
@doc """
|
||||||
Create a new material with the provided name.
|
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()
|
@spec colour(Material.t(), Colour.t()) :: Material.t()
|
||||||
def colour(%Material{} = material, %Colour{} = colour), do: %Material{material | colour: colour}
|
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
|
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
|
defmodule Kinemat.Robot.Visual do
|
||||||
defstruct geometry: nil, material_name: nil, origin: nil
|
defstruct geometry: nil, material: nil, origin: nil
|
||||||
alias Kinemat.{Frame, Geometry, Robot.Geometric, Robot.Orientable, Robot.Visual}
|
alias Kinemat.{Frame, Geometry, Robot.Geometric, Robot.Material, Robot.Orientable, Robot.Visual}
|
||||||
|
|
||||||
@moduledoc """
|
@moduledoc """
|
||||||
A container for the visual properties of a Robot description.
|
A container for the visual properties of a Robot description.
|
||||||
|
@ -9,7 +9,7 @@ defmodule Kinemat.Robot.Visual do
|
||||||
@type t :: %Visual{
|
@type t :: %Visual{
|
||||||
geometry: Geometry.t(),
|
geometry: Geometry.t(),
|
||||||
origin: Point.t() | nil,
|
origin: Point.t() | nil,
|
||||||
material_name: String.t() | nil
|
material: Material.t() | nil
|
||||||
}
|
}
|
||||||
|
|
||||||
@doc """
|
@doc """
|
||||||
|
@ -26,11 +26,11 @@ defmodule Kinemat.Robot.Visual do
|
||||||
do: %Visual{visual | origin: origin}
|
do: %Visual{visual | origin: origin}
|
||||||
|
|
||||||
@doc """
|
@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()
|
@spec material(Visual.t(), Material.t()) :: Visual.t()
|
||||||
def material_name(%Visual{} = visual, material_name),
|
def material(%Visual{} = visual, %Material{} = material),
|
||||||
do: %Visual{visual | material_name: material_name}
|
do: %Visual{visual | material: material}
|
||||||
|
|
||||||
defimpl Geometric do
|
defimpl Geometric do
|
||||||
def set(%Visual{} = visual, geometry),
|
def set(%Visual{} = visual, geometry),
|
||||||
|
|
|
@ -1,10 +1,8 @@
|
||||||
defmodule Kinemat.URDF.Load do
|
defmodule Kinemat.URDF.Load do
|
||||||
alias Kinemat.{Coordinates, Frame, Geometry, Orientations, Robot}
|
alias Kinemat.URDF.Parser
|
||||||
import Kinemat.URDF.XmlHelpers
|
|
||||||
|
|
||||||
@moduledoc """
|
@moduledoc """
|
||||||
This module implements the ability to parse a URDF file using the `:xmerl` OTP
|
This module parses URDF XML files and strings.
|
||||||
library.
|
|
||||||
"""
|
"""
|
||||||
|
|
||||||
@doc """
|
@doc """
|
||||||
|
@ -12,191 +10,15 @@ defmodule Kinemat.URDF.Load do
|
||||||
"""
|
"""
|
||||||
@spec load(Path.t()) :: {:ok, Robot.t()} | {:error, any}
|
@spec load(Path.t()) :: {:ok, Robot.t()} | {:error, any}
|
||||||
def load(path) do
|
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
|
end
|
||||||
|
|
||||||
defp parse(node, state) when is_element(node) do
|
@doc """
|
||||||
with {:ok, name} <- get_name(node), do: parse(name, node, state)
|
Attempt to parse a URDF XML string.
|
||||||
end
|
"""
|
||||||
|
def parse(xml) do
|
||||||
defp parse(node, state) when is_text(node), do: {:ok, state}
|
with xml <- String.to_charlist(xml),
|
||||||
|
{node, []} <- :xmerl_scan.string(xml),
|
||||||
defp parse(:robot, node, nil) when is_element(node) do
|
do: Parser.parse(node)
|
||||||
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
|
|
||||||
end
|
end
|
||||||
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)}
|
{:robot, [name: name], Enum.reverse(contents)}
|
||||||
end
|
end
|
||||||
|
|
||||||
|
defp to_tag(%Robot.Material{name: name, colour: nil}),
|
||||||
|
do: {:material, [name: name], []}
|
||||||
|
|
||||||
defp to_tag(%Robot.Material{name: name, colour: colour}),
|
defp to_tag(%Robot.Material{name: name, colour: colour}),
|
||||||
do: {:material, [name: name], [to_tag(colour)]}
|
do: {:material, [name: name], [to_tag(colour)]}
|
||||||
|
|
||||||
|
@ -72,7 +75,7 @@ defmodule Kinemat.URDF.Save do
|
||||||
{:link, [name: name], contents}
|
{:link, [name: name], contents}
|
||||||
end
|
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 =
|
||||||
geometry
|
geometry
|
||||||
|> to_tag()
|
|> to_tag()
|
||||||
|
@ -88,8 +91,8 @@ defmodule Kinemat.URDF.Save do
|
||||||
else: contents
|
else: contents
|
||||||
|
|
||||||
contents =
|
contents =
|
||||||
if material_name,
|
if material,
|
||||||
do: [{:material, [name: material_name], []} | contents],
|
do: [to_tag(material) | contents],
|
||||||
else: contents
|
else: contents
|
||||||
|
|
||||||
{:visual, [], Enum.reverse(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) when is_element(node), do: {:ok, xmlElement(node, :content)}
|
||||||
def get_content(_node), do: {:error, "Node is not an element"}
|
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 """
|
@doc """
|
||||||
Returns all the child elements of an XML element.
|
Returns all the child elements of an XML element.
|
||||||
"""
|
"""
|
||||||
|
|
|
@ -3,14 +3,26 @@ defmodule Kinemat.URDF.LoadTest do
|
||||||
import Kinemat.URDF.Load
|
import Kinemat.URDF.Load
|
||||||
import Angle.Sigil
|
import Angle.Sigil
|
||||||
import KinematTest.FileHelpers
|
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
|
describe "load/1" do
|
||||||
test "when the file doesn't exist it returns an error" do
|
test "when the file doesn't exist it returns an error" do
|
||||||
assert {:error, _} = load(fixture_path("urdf/fake.file"))
|
assert {:error, _} = load(fixture_path("urdf/fake.file"))
|
||||||
end
|
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 {:ok, robot} = load(fixture_path("urdf/one_shape.urdf"))
|
||||||
|
|
||||||
assert robot.name == "myfirst"
|
assert robot.name == "myfirst"
|
||||||
|
@ -22,7 +34,7 @@ defmodule Kinemat.URDF.LoadTest do
|
||||||
assert shape.radius == 0.2
|
assert shape.radius == 0.2
|
||||||
end
|
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 {:ok, robot} = load(fixture_path("urdf/multiple_shapes.urdf"))
|
||||||
|
|
||||||
assert robot.name == "multipleshapes"
|
assert robot.name == "multipleshapes"
|
||||||
|
@ -36,108 +48,853 @@ defmodule Kinemat.URDF.LoadTest do
|
||||||
assert link1.name == "base_link"
|
assert link1.name == "base_link"
|
||||||
assert [joint] = robot.joints
|
assert [joint] = robot.joints
|
||||||
assert joint.name == "base_to_right_leg"
|
assert joint.name == "base_to_right_leg"
|
||||||
assert joint.type == "fixed"
|
assert joint.type == :fixed
|
||||||
assert joint.parent_name == "base_link"
|
assert joint.parent_name == "base_link"
|
||||||
assert joint.child_name == "right_leg"
|
assert joint.child_name == "right_leg"
|
||||||
end
|
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
|
end
|
||||||
|
|
||||||
test "it parses `origins.urdf`" do
|
describe "parse/1" do
|
||||||
{:ok, robot} = load(fixture_path("urdf/origins.urdf"))
|
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
|
test "it parses a transmission element" do
|
||||||
assert joint.origin.point.x == 0
|
xml = ~S"""
|
||||||
assert joint.origin.point.y == -0.22
|
<robot name="">
|
||||||
assert joint.origin.point.z == 0.25
|
<transmission name="simple_trans">
|
||||||
|
</transmission>
|
||||||
|
</robot>
|
||||||
|
"""
|
||||||
|
|
||||||
assert {:ok, link} = Robot.get_link(robot, "right_leg")
|
assert {:ok, robot} = parse(xml)
|
||||||
assert link.visual.origin.orientation.__struct__ == Euler
|
assert [%Robot.Transmission{name: "simple_trans"}] = robot.transmissions
|
||||||
assert link.visual.origin.orientation.representation == :xyz
|
end
|
||||||
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 parses `materials.urdf`" do
|
test "it parses a transmission type element" do
|
||||||
{:ok, robot} = load(fixture_path("urdf/materials.urdf"))
|
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"
|
test "it parses a transmission joint element" do
|
||||||
assert materials0.colour.r == 1
|
xml = ~S"""
|
||||||
assert materials0.colour.g == 1
|
<robot name="">
|
||||||
assert materials0.colour.b == 1
|
<transmission name="simple_trans">
|
||||||
assert materials0.colour.a == 1
|
<joint name="foo_joint" />
|
||||||
|
</transmission>
|
||||||
|
</robot>
|
||||||
|
"""
|
||||||
|
|
||||||
assert materials1.name == "blue"
|
assert {:ok, robot} = parse(xml)
|
||||||
assert materials1.colour.r == 0
|
assert [transmission] = robot.transmissions
|
||||||
assert materials1.colour.g == 0
|
assert [%Robot.TransmissionJoint{name: "foo_joint"}] = transmission.joints
|
||||||
assert materials1.colour.b == 0.8
|
end
|
||||||
assert materials1.colour.a == 1
|
|
||||||
|
|
||||||
assert {:ok, link} = Robot.get_link(robot, "base_link")
|
test "it parses a transmission joint containing a hardware interface element" do
|
||||||
assert link.visual.material_name == "blue"
|
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 {:ok, robot} = parse(xml)
|
||||||
assert link.visual.material_name == "white"
|
assert [transmission] = robot.transmissions
|
||||||
end
|
assert [joint] = transmission.joints
|
||||||
|
|
||||||
test "it parses `visual.urdf`" do
|
assert [%Robot.HardwareInterface{name: "EffortJointInterface"}] = joint.hardware_interfaces
|
||||||
{:ok, robot} = load(fixture_path("urdf/visual.urdf"))
|
end
|
||||||
|
|
||||||
assert {:ok, link} = Robot.get_link(robot, "left_gripper")
|
test "it parses a transmission actuator element" do
|
||||||
assert mesh = link.visual.geometry
|
xml = ~S"""
|
||||||
assert mesh.filename == "package://urdf_tutorial/meshes/l_finger.dae"
|
<robot name="">
|
||||||
|
<transmission name="simple_trans">
|
||||||
|
<actuator name="foo_motor" />
|
||||||
|
</transmission>
|
||||||
|
</robot>
|
||||||
|
"""
|
||||||
|
|
||||||
assert {:ok, link} = Robot.get_link(robot, "left_tip")
|
assert {:ok, robot} = parse(xml)
|
||||||
assert mesh = link.visual.geometry
|
assert [transmission] = robot.transmissions
|
||||||
assert mesh.filename == "package://urdf_tutorial/meshes/l_finger_tip.dae"
|
assert [%Robot.Actuator{name: "foo_motor"}] = transmission.actuators
|
||||||
|
end
|
||||||
|
|
||||||
assert {:ok, link} = Robot.get_link(robot, "right_gripper")
|
test "it parses a transmission actuator containing a mechanical reduction element" do
|
||||||
assert mesh = link.visual.geometry
|
xml = ~S"""
|
||||||
assert mesh.filename == "package://urdf_tutorial/meshes/l_finger.dae"
|
<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 {:ok, robot} = parse(xml)
|
||||||
assert mesh = link.visual.geometry
|
assert [transmission] = robot.transmissions
|
||||||
assert mesh.filename == "package://urdf_tutorial/meshes/l_finger_tip.dae"
|
assert [actuator] = transmission.actuators
|
||||||
|
assert actuator.mechanical_reduction == 50
|
||||||
|
end
|
||||||
|
|
||||||
assert {:ok, link} = Robot.get_link(robot, "head")
|
test "it parses a transmission actuator containing a hardware interface element" do
|
||||||
assert sphere = link.visual.geometry
|
xml = ~S"""
|
||||||
assert sphere.radius == 0.2
|
<robot name="">
|
||||||
end
|
<transmission name="simple_trans">
|
||||||
|
<actuator name="foo_motor">
|
||||||
|
<hardwareInterface>EffortJointInterface</hardwareInterface>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
</robot>
|
||||||
|
"""
|
||||||
|
|
||||||
test "it parses `flexible.urdf`" do
|
assert {:ok, robot} = parse(xml)
|
||||||
{:ok, robot} = load(fixture_path("urdf/flexible.urdf"))
|
assert [transmission] = robot.transmissions
|
||||||
|
assert [actuator] = transmission.actuators
|
||||||
|
|
||||||
assert {:ok, joint} = Robot.get_joint(robot, "right_front_wheel_joint")
|
assert [%Robot.HardwareInterface{name: "EffortJointInterface"}] =
|
||||||
assert orientation = joint.axis.orientation
|
actuator.hardware_interfaces
|
||||||
assert Euler.x(orientation) == ~a(0)
|
end
|
||||||
assert Euler.y(orientation) == ~a(0)
|
|
||||||
assert Euler.z(orientation) == ~a(0)
|
|
||||||
|
|
||||||
assert translation = joint.axis.point
|
test "it parses a link element" do
|
||||||
assert Cartesian.x(translation) == 0
|
xml = ~S"""
|
||||||
assert Cartesian.y(translation) == 1.0
|
<robot name="">
|
||||||
assert Cartesian.z(translation) == 0
|
<link name="my_link" />
|
||||||
|
</robot>
|
||||||
|
"""
|
||||||
|
|
||||||
assert {:ok, joint} = Robot.get_joint(robot, "gripper_extension")
|
assert {:ok, robot} = parse(xml)
|
||||||
assert joint.limit.effort == 1000.0
|
assert [%Robot.Link{name: "my_link"}] = robot.links
|
||||||
assert joint.limit.lower == -0.38
|
end
|
||||||
assert joint.limit.upper == 0
|
|
||||||
assert joint.limit.velocity == 0.5
|
|
||||||
end
|
|
||||||
|
|
||||||
test "it parses `physics.urdf`" do
|
test "it parses a link containing an inertial element" do
|
||||||
{:ok, robot} = load(fixture_path("urdf/physics.urdf"))
|
xml = ~S"""
|
||||||
|
<robot name="">
|
||||||
|
<link name="my_link">
|
||||||
|
<inertial />
|
||||||
|
</link>
|
||||||
|
</robot>
|
||||||
|
"""
|
||||||
|
|
||||||
assert {:ok, link} = Robot.get_link(robot, "base_link")
|
assert {:ok, robot} = parse(xml)
|
||||||
assert %Geometry.Cylinder{length: 0.6, radius: 0.2} = link.collision.geometry
|
assert [link] = robot.links
|
||||||
|
assert %Robot.Inertia{} = link.inertia
|
||||||
|
end
|
||||||
|
|
||||||
assert link.inertia.mass == 10
|
test "it parses a link containing an inertial origin element" do
|
||||||
assert {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0} = link.inertia.matrix.matrix
|
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
|
||||||
end
|
end
|
||||||
|
|
Loading…
Reference in a new issue