Can load, but not yet save the example physics URDF file.
This commit is contained in:
James Harton
parent
a5b859edd3
commit
91b52f931e
14 changed files with 1074 additions and 77 deletions
|
|
@ -13,17 +13,37 @@ defmodule Kinemat.Robot do
|
|||
materials: [Material.t()]
|
||||
}
|
||||
|
||||
@doc """
|
||||
Initialise a new empty Robot description.
|
||||
"""
|
||||
@spec init(String.t()) :: Kinemat.Robot.t()
|
||||
def init(name), do: %Robot{name: name}
|
||||
|
||||
@doc """
|
||||
Add a new link to the robot.
|
||||
"""
|
||||
@spec add_link(Robot.t(), Link.t()) :: Robot.t()
|
||||
def add_link(%Robot{links: links} = robot, %Link{} = link),
|
||||
do: %Robot{robot | links: [link | links]}
|
||||
|
||||
@doc """
|
||||
Add a new joint to the robot.
|
||||
"""
|
||||
@spec add_joint(Robot.t(), Joint.t()) :: Robot.t()
|
||||
def add_joint(%Robot{joints: joints} = robot, %Joint{} = joint),
|
||||
do: %Robot{robot | joints: [joint | joints]}
|
||||
|
||||
@doc """
|
||||
Add a new material to the robot.
|
||||
"""
|
||||
@spec add_material(Robot.t(), Material.t()) :: Robot.t()
|
||||
def add_material(%Robot{materials: materials} = robot, %Material{} = material),
|
||||
do: %Robot{robot | materials: [material | materials]}
|
||||
|
||||
@doc """
|
||||
Retrieve a named link from the Robot.
|
||||
"""
|
||||
@spec get_link(Robot.t(), String.t()) :: {:ok, Link.t()} | {:error, any}
|
||||
def get_link(%Robot{links: links}, name) do
|
||||
case Enum.find(links, fn
|
||||
%Link{name: ^name} -> true
|
||||
|
|
@ -33,4 +53,18 @@ defmodule Kinemat.Robot do
|
|||
_ -> {:error, "No such link"}
|
||||
end
|
||||
end
|
||||
|
||||
@doc """
|
||||
Retrieve a named joint from the Robot.
|
||||
"""
|
||||
@spec get_joint(Robot.t(), String.t()) :: {:ok, Joint.t()} | {:error, any}
|
||||
def get_joint(%Robot{joints: joints}, name) do
|
||||
case Enum.find(joints, fn
|
||||
%Joint{name: ^name} -> true
|
||||
_ -> false
|
||||
end) do
|
||||
%Joint{} = joint -> {:ok, joint}
|
||||
_ -> {:error, "No such joint"}
|
||||
end
|
||||
end
|
||||
end
|
||||
|
|
|
|||
48
lib/kinemat/robot/collision.ex
Normal file
48
lib/kinemat/robot/collision.ex
Normal file
|
|
@ -0,0 +1,48 @@
|
|||
defmodule Kinemat.Robot.Collision do
|
||||
defstruct geometry: nil, origin: nil
|
||||
alias Kinemat.{Robot.Collision, Frame, Robot.Geometric, Geometry, Robot.Orientable}
|
||||
|
||||
@moduledoc """
|
||||
A container for the collision properties of a link.
|
||||
"""
|
||||
|
||||
@type t :: %Collision{
|
||||
geometry: Geometry.t() | nil,
|
||||
origin: Frame.t() | nil
|
||||
}
|
||||
|
||||
@doc """
|
||||
Initialise an empty collision container.
|
||||
"""
|
||||
@spec init :: Collision.t()
|
||||
def init, do: %Collision{}
|
||||
|
||||
@doc """
|
||||
Set the geometry to be stored in the collision container.
|
||||
"""
|
||||
@spec geometry(Collision.t(), Geometry.t()) :: Collision.t()
|
||||
def geometry(%Collision{} = collision, geometry),
|
||||
do: %Collision{collision | geometry: geometry}
|
||||
|
||||
@doc """
|
||||
Set the origin of the collision geometry.
|
||||
"""
|
||||
@spec origin(Collision.t(), Point.t()) :: Collision.t()
|
||||
def origin(%Collision{} = collision, origin),
|
||||
do: %Collision{collision | origin: origin}
|
||||
|
||||
defimpl Geometric do
|
||||
def set(%Collision{} = collision, geometry),
|
||||
do: %Collision{collision | geometry: geometry}
|
||||
|
||||
def get(%Collision{geometry: geometry}),
|
||||
do: geometry
|
||||
end
|
||||
|
||||
defimpl Orientable do
|
||||
def set(%Collision{} = collision, %Frame{} = origin),
|
||||
do: %Collision{collision | origin: origin}
|
||||
|
||||
def get(%Collision{origin: origin}), do: origin
|
||||
end
|
||||
end
|
||||
19
lib/kinemat/robot/geometric.ex
Normal file
19
lib/kinemat/robot/geometric.ex
Normal file
|
|
@ -0,0 +1,19 @@
|
|||
defprotocol Kinemat.Robot.Geometric do
|
||||
@moduledoc """
|
||||
Defines a protocol for setting a geometry on links, visuals, etc.
|
||||
"""
|
||||
alias Kinemat.Robot.Geometric
|
||||
alias Kinemat.Geometry
|
||||
|
||||
@doc """
|
||||
Set the geometry on a type capable of having a geometry.
|
||||
"""
|
||||
@spec set(Geometric.t(), Geometry.t()) :: Proto.t()
|
||||
def set(geometric, geometry)
|
||||
|
||||
@doc """
|
||||
Get the geometry from a type capable of having a geometry.
|
||||
"""
|
||||
@spec get(Geometric.t()) :: Geometry.t()
|
||||
def get(geometric)
|
||||
end
|
||||
41
lib/kinemat/robot/inertia.ex
Normal file
41
lib/kinemat/robot/inertia.ex
Normal file
|
|
@ -0,0 +1,41 @@
|
|||
defmodule Kinemat.Robot.Inertia do
|
||||
defstruct origin: nil, mass: nil, matrix: nil
|
||||
alias Kinemat.{Frame, Robot.Inertia, Robot.Orientable, Orientations.RotationMatrix}
|
||||
|
||||
@moduledoc """
|
||||
A container for the inertial properties of a link.
|
||||
"""
|
||||
|
||||
@type t :: %Inertia{
|
||||
origin: Frame.t() | nil,
|
||||
mass: number | nil,
|
||||
matrix: RotationMatrix | nil
|
||||
}
|
||||
|
||||
@doc """
|
||||
Initialise empty inertia container..
|
||||
"""
|
||||
@spec init() :: Inertia.t()
|
||||
def init(), do: %Inertia{}
|
||||
|
||||
@doc """
|
||||
Set the mass component of the inertial container.
|
||||
"""
|
||||
@spec mass(Inertia.t(), number) :: Inertia.t()
|
||||
def mass(%Inertia{} = inertia, mass) when is_number(mass),
|
||||
do: %Inertia{inertia | mass: mass}
|
||||
|
||||
@doc """
|
||||
Set the matrix of the inertial container.
|
||||
"""
|
||||
@spec matrix(Inertia.t(), RotationMatrix.t()) :: Inertia.t()
|
||||
def matrix(%Inertia{} = inertia, %RotationMatrix{} = matrix),
|
||||
do: %Inertia{inertia | matrix: matrix}
|
||||
|
||||
defimpl Orientable do
|
||||
def set(%Inertia{} = inertia, %Frame{} = frame),
|
||||
do: %Inertia{inertia | origin: frame}
|
||||
|
||||
def get(%Inertia{origin: origin}), do: origin
|
||||
end
|
||||
end
|
||||
|
|
@ -1,20 +1,62 @@
|
|||
defmodule Kinemat.Robot.Joint do
|
||||
defstruct name: nil, type: nil, parent_name: nil, child_name: nil, origin: nil
|
||||
alias Kinemat.{Frame, Robot.Joint}
|
||||
defstruct name: nil,
|
||||
type: nil,
|
||||
parent_name: nil,
|
||||
child_name: nil,
|
||||
origin: nil,
|
||||
axis: nil,
|
||||
limit: nil
|
||||
|
||||
alias Kinemat.{Frame, Robot.Joint, Robot.Limit, Robot.Orientable}
|
||||
|
||||
@moduledoc """
|
||||
Represents a join in a Robot body.
|
||||
Represents a joint in a Robot body.
|
||||
"""
|
||||
|
||||
@type t :: %Joint{
|
||||
name: String.t(),
|
||||
type: String.t(),
|
||||
parent_name: String.t(),
|
||||
child_name: String.t(),
|
||||
origin: Frame
|
||||
parent_name: String.t() | nil,
|
||||
child_name: String.t() | nil,
|
||||
origin: Frame | nil,
|
||||
axis: Frame | nil,
|
||||
limit: Limit | nil
|
||||
}
|
||||
|
||||
@doc """
|
||||
Initialise a new joint.
|
||||
"""
|
||||
@spec init(String.t(), String.t()) :: Joint.t()
|
||||
def init(name, type), do: %Joint{name: name, type: type}
|
||||
|
||||
def origin(joint, frame), do: %Joint{joint | origin: frame}
|
||||
@doc """
|
||||
Set the name of the parent link.
|
||||
"""
|
||||
@spec parent_name(Joint.t(), String.t()) :: Joint.t()
|
||||
def parent_name(%Joint{} = joint, name), do: %Joint{joint | parent_name: name}
|
||||
|
||||
@doc """
|
||||
Set the name of the child link.
|
||||
"""
|
||||
@spec child_name(Joint.t(), String.t()) :: Joint.t()
|
||||
def child_name(%Joint{} = joint, name), do: %Joint{joint | child_name: name}
|
||||
|
||||
@doc """
|
||||
Set the reference frame of the joint axis.
|
||||
"""
|
||||
@spec axis(Joint.t(), Frame.t()) :: Joint.t()
|
||||
def axis(%Joint{} = joint, frame), do: %Joint{joint | axis: frame}
|
||||
|
||||
@doc """
|
||||
Set the safety limits of the joint.
|
||||
"""
|
||||
@spec limit(Joint.t(), Limit.t()) :: Joint.t()
|
||||
def limit(%Joint{} = joint, %Limit{} = limit), do: %Joint{joint | limit: limit}
|
||||
|
||||
defimpl Orientable do
|
||||
def set(%Joint{} = joint, %Frame{} = frame),
|
||||
do: %Joint{joint | origin: frame}
|
||||
|
||||
def get(%Joint{origin: origin}), do: origin
|
||||
end
|
||||
end
|
||||
|
|
|
|||
31
lib/kinemat/robot/limit.ex
Normal file
31
lib/kinemat/robot/limit.ex
Normal file
|
|
@ -0,0 +1,31 @@
|
|||
defmodule Kinemat.Robot.Limit do
|
||||
defstruct effort: nil, lower: nil, upper: nil, velocity: nil
|
||||
alias Kinemat.Robot.Limit
|
||||
|
||||
@moduledoc """
|
||||
Encapsulates the limits of a joint.
|
||||
"""
|
||||
|
||||
@type t :: %Limit{
|
||||
effort: number | nil,
|
||||
lower: number | nil,
|
||||
upper: number | nil,
|
||||
velocity: number | nil
|
||||
}
|
||||
|
||||
@type options :: [option]
|
||||
@type option :: {:effort | :lower | :upper | :velocity, number | nil}
|
||||
|
||||
@doc """
|
||||
Initialise a new effort container with the provided values.
|
||||
"""
|
||||
@spec init(options) :: Limit.t()
|
||||
def init(options) when is_list(options) do
|
||||
fields =
|
||||
options
|
||||
|> Keyword.take(~w[effort lower upper velocity]a)
|
||||
|> Enum.into(%{})
|
||||
|
||||
struct(Limit, fields)
|
||||
end
|
||||
end
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
defmodule Kinemat.Robot.Link do
|
||||
defstruct name: nil, visual: nil, origin: nil
|
||||
alias Kinemat.{Robot.Link, Visual}
|
||||
defstruct name: nil, visual: nil, origin: nil, collision: nil, inertia: nil
|
||||
alias Kinemat.{Robot.Collision, Robot.Inertia, Robot.Link, Robot.Visual}
|
||||
|
||||
@moduledoc """
|
||||
Defines a solid body in the kinematic chain of a robot.
|
||||
|
|
@ -8,12 +8,23 @@ defmodule Kinemat.Robot.Link do
|
|||
|
||||
@type t :: %Link{
|
||||
name: String.t(),
|
||||
visual: Visual.t()
|
||||
visual: Visual.t() | nil,
|
||||
collision: Collision.t() | nil,
|
||||
inertia: Inertia.t() | nil
|
||||
}
|
||||
|
||||
@spec init(String.t()) :: Link.t()
|
||||
def init(name), do: %Link{name: name}
|
||||
|
||||
@spec visual(Link.t(), Visual.t()) :: Link.t()
|
||||
def visual(link, visual), do: %{link | visual: visual}
|
||||
def visual(%Link{} = link, %Visual{} = visual),
|
||||
do: %Link{link | visual: visual}
|
||||
|
||||
@spec collision(Link.t(), Collision.t()) :: Link.t()
|
||||
def collision(%Link{} = link, %Collision{} = collision),
|
||||
do: %Link{link | collision: collision}
|
||||
|
||||
@spec inertia(Link.t(), Inertia.t()) :: Link.t()
|
||||
def inertia(%Link{} = link, %Inertia{} = inertia),
|
||||
do: %Link{link | inertia: inertia}
|
||||
end
|
||||
|
|
|
|||
19
lib/kinemat/robot/orientable.ex
Normal file
19
lib/kinemat/robot/orientable.ex
Normal file
|
|
@ -0,0 +1,19 @@
|
|||
defprotocol Kinemat.Robot.Orientable do
|
||||
@moduledoc """
|
||||
Defines a protocol for setting the orientation of various parts of the robot.
|
||||
"""
|
||||
alias Kinemat.Robot.Orientable
|
||||
alias Kinemat.Frame
|
||||
|
||||
@doc """
|
||||
Set the orientation frame on the orientable.
|
||||
"""
|
||||
@spec set(Orientable.t(), Frame.t()) :: Orientable.t()
|
||||
def set(orienable, orientation)
|
||||
|
||||
@doc """
|
||||
Retrieve the orientation from the orientable.
|
||||
"""
|
||||
@spec get(Orientable.t()) :: Frame.t()
|
||||
def get(orientable)
|
||||
end
|
||||
|
|
@ -1,13 +1,13 @@
|
|||
defmodule Kinemat.Robot.Visual do
|
||||
defstruct geometries: [], material_name: nil, origin: nil
|
||||
alias Kinemat.Robot.{Geometry, Point, Visual}
|
||||
defstruct geometry: nil, material_name: nil, origin: nil
|
||||
alias Kinemat.{Frame, Geometry, Robot.Geometric, Robot.Orientable, Robot.Visual}
|
||||
|
||||
@moduledoc """
|
||||
A container for the visual properties of a Robot description.
|
||||
"""
|
||||
|
||||
@type t :: %Visual{
|
||||
geometries: [Geometry.t()],
|
||||
geometry: Geometry.t(),
|
||||
origin: Point.t() | nil,
|
||||
material_name: String.t() | nil
|
||||
}
|
||||
|
|
@ -19,21 +19,31 @@ defmodule Kinemat.Robot.Visual do
|
|||
def init, do: %Visual{}
|
||||
|
||||
@doc """
|
||||
Set the geometries to be stored in the container.
|
||||
"""
|
||||
@spec geometries(Visual.t(), [Geometry.t()]) :: Visual.t()
|
||||
def geometries(%Visual{} = visual, geometries), do: %Visual{visual | geometries: geometries}
|
||||
|
||||
@doc """
|
||||
Set the origin of the visual geometries.
|
||||
Set the origin of the visual geometry.
|
||||
"""
|
||||
@spec origin(Visual.t(), Point.t()) :: Visual.t()
|
||||
def origin(%Visual{} = visual, origin), do: %Visual{visual | origin: origin}
|
||||
def origin(%Visual{} = visual, origin),
|
||||
do: %Visual{visual | origin: origin}
|
||||
|
||||
@doc """
|
||||
Set the material name of this visual geometries.
|
||||
Set the material name of this visual geometry.
|
||||
"""
|
||||
@spec material_name(Visual.t(), String.t()) :: Visual.t()
|
||||
def material_name(%Visual{} = visual, material_name),
|
||||
do: %Visual{visual | material_name: material_name}
|
||||
|
||||
defimpl Geometric do
|
||||
def set(%Visual{} = visual, geometry),
|
||||
do: %Visual{visual | geometry: geometry}
|
||||
|
||||
def get(%Visual{geometry: geometry}),
|
||||
do: geometry
|
||||
end
|
||||
|
||||
defimpl Orientable do
|
||||
def set(%Visual{} = visual, %Frame{} = origin),
|
||||
do: %Visual{visual | origin: origin}
|
||||
|
||||
def get(%Visual{origin: origin}), do: origin
|
||||
end
|
||||
end
|
||||
|
|
|
|||
|
|
@ -38,15 +38,8 @@ defmodule Kinemat.URDF.Load do
|
|||
with {:ok, name} <- get_attribute_value(node, :name),
|
||||
link <- Robot.Link.init(name),
|
||||
{:ok, children} <- get_children(node),
|
||||
{:ok, link} <-
|
||||
Enum.reduce_while(children, link, fn node, link ->
|
||||
case parse(node, link) do
|
||||
{:ok, link} -> {:cont, {:ok, link}}
|
||||
{:error, reason} -> {:halt, {:error, reason}}
|
||||
end
|
||||
end) do
|
||||
{:ok, Robot.add_link(robot, link)}
|
||||
end
|
||||
{:ok, link} <- reduce_oks(children, link, &parse(&1, &2)),
|
||||
do: {:ok, Robot.add_link(robot, link)}
|
||||
end
|
||||
|
||||
defp parse(:joint, node, %Robot{} = robot) do
|
||||
|
|
@ -54,9 +47,8 @@ defmodule Kinemat.URDF.Load do
|
|||
{:ok, type} <- get_attribute_value(node, :type),
|
||||
joint <- Robot.Joint.init(name, type),
|
||||
{:ok, children} <- get_children(node),
|
||||
{:ok, joint} <- reduce_oks(children, joint, &parse(&1, &2)) do
|
||||
{:ok, Robot.add_joint(robot, joint)}
|
||||
end
|
||||
{:ok, joint} <- reduce_oks(children, joint, &parse(&1, &2)),
|
||||
do: {:ok, Robot.add_joint(robot, joint)}
|
||||
end
|
||||
|
||||
defp parse(:material, node, %Robot{} = robot) do
|
||||
|
|
@ -73,52 +65,45 @@ defmodule Kinemat.URDF.Load do
|
|||
end
|
||||
|
||||
defp parse(:parent, node, %Robot.Joint{} = joint) do
|
||||
with {:ok, name} <- get_attribute_value(node, :link) do
|
||||
{:ok, %{joint | parent_name: name}}
|
||||
end
|
||||
with {:ok, name} <- get_attribute_value(node, :link),
|
||||
do: {:ok, Robot.Joint.parent_name(joint, name)}
|
||||
end
|
||||
|
||||
defp parse(:child, node, %Robot.Joint{} = joint) do
|
||||
with {:ok, name} <- get_attribute_value(node, :link) do
|
||||
{:ok, %{joint | child_name: name}}
|
||||
end
|
||||
with {:ok, name} <- get_attribute_value(node, :link),
|
||||
do: {:ok, Robot.Joint.child_name(joint, name)}
|
||||
end
|
||||
|
||||
defp parse(:visual, node, %Robot.Link{} = link) do
|
||||
with {:ok, children} <- get_children(node),
|
||||
visual <- Robot.Visual.init(),
|
||||
{:ok, visual} <- reduce_oks(children, visual, &parse(&1, &2)) do
|
||||
{:ok, Robot.Link.visual(link, visual)}
|
||||
end
|
||||
{:ok, visual} <- reduce_oks(children, visual, &parse(&1, &2)),
|
||||
do: {:ok, Robot.Link.visual(link, visual)}
|
||||
end
|
||||
|
||||
defp parse(:geometry, node, %Robot.Visual{} = visual) do
|
||||
defp parse(:geometry, node, geometric) do
|
||||
with {:ok, children} <- get_children(node),
|
||||
{:ok, geometries} <- reduce_oks(children, [], &parse(&1, &2)),
|
||||
do: {:ok, Robot.Visual.geometries(visual, geometries)}
|
||||
{:ok, geometric} <- reduce_oks(children, geometric, &parse(&1, &2)),
|
||||
do: {:ok, geometric}
|
||||
end
|
||||
|
||||
defp parse(:cylinder, node, geometries) when is_list(geometries) do
|
||||
defp parse(:cylinder, node, geometric) do
|
||||
with [length] <- extract_floats(node, :length, [0]),
|
||||
[radius] <- extract_floats(node, :radius, [0]),
|
||||
cylinder <- Geometry.Cylinder.init(length, radius),
|
||||
{:ok, children} <- get_children(node),
|
||||
{:ok, cylinder} <- reduce_oks(children, cylinder, &parse(&1, &2)),
|
||||
do: {:ok, [cylinder | geometries]}
|
||||
do: {:ok, Robot.Geometric.set(geometric, cylinder)}
|
||||
end
|
||||
|
||||
defp parse(:box, node, geometries) when is_list(geometries) do
|
||||
defp parse(:box, node, geometric) do
|
||||
with [x, y, z] <- extract_floats(node, :size, [0, 0, 0]),
|
||||
box <- Geometry.Box.init(x, y, z),
|
||||
{:ok, children} <- get_children(node),
|
||||
{:ok, box} <- reduce_oks(children, box, &parse(&1, &2)),
|
||||
do: {:ok, [box | geometries]}
|
||||
do: {:ok, Robot.Geometric.set(geometric, box)}
|
||||
end
|
||||
|
||||
defp parse(:sphere, node, geometries) when is_list(geometries) do
|
||||
defp parse(:sphere, node, geometric) do
|
||||
with [radius] <- extract_floats(node, :radius, [0]),
|
||||
sphere <- Geometry.Sphere.init(radius),
|
||||
do: {:ok, [sphere | geometries]}
|
||||
do: {:ok, Robot.Geometric.set(geometric, sphere)}
|
||||
end
|
||||
|
||||
defp parse(:material, node, %Robot.Visual{} = visual) do
|
||||
|
|
@ -126,7 +111,7 @@ defmodule Kinemat.URDF.Load do
|
|||
do: {:ok, Robot.Visual.material_name(visual, name)}
|
||||
end
|
||||
|
||||
defp parse(:origin, node, parent) do
|
||||
defp parse(:origin, node, orientable) do
|
||||
[roll, pitch, yaw] =
|
||||
node
|
||||
|> extract_floats(:rpy, [0, 0, 0])
|
||||
|
|
@ -138,15 +123,68 @@ defmodule Kinemat.URDF.Load do
|
|||
translation = Coordinates.Cartesian.init(x, y, z)
|
||||
frame = Frame.init(translation, orientation)
|
||||
|
||||
case parent do
|
||||
%Robot.Joint{} -> {:ok, Robot.Joint.origin(parent, frame)}
|
||||
%Robot.Visual{} -> {:ok, Robot.Visual.origin(parent, frame)}
|
||||
end
|
||||
{:ok, Robot.Orientable.set(orientable, frame)}
|
||||
end
|
||||
|
||||
defp parse(:mesh, node, parent) do
|
||||
defp parse(:mesh, node, geometric) do
|
||||
with {:ok, filename} <- get_attribute_value(node, :filename),
|
||||
do: {:ok, [Geometry.Mesh.init(filename) | parent]}
|
||||
mesh <- Geometry.Mesh.init(filename),
|
||||
do: {:ok, Robot.Geometric.set(geometric, mesh)}
|
||||
end
|
||||
|
||||
defp parse(:axis, node, %Robot.Joint{} = joint) do
|
||||
[roll, pitch, yaw] =
|
||||
node
|
||||
|> extract_floats(:rpy, [0, 0, 0])
|
||||
|> Enum.map(&Angle.Radian.init(&1))
|
||||
|
||||
[x, y, z] = extract_floats(node, :xyz, [0, 0, 0])
|
||||
|
||||
orientation = Orientations.Euler.init(:xyz, roll, pitch, yaw)
|
||||
translation = Coordinates.Cartesian.init(x, y, z)
|
||||
frame = Frame.init(translation, orientation)
|
||||
{:ok, Robot.Joint.axis(joint, frame)}
|
||||
end
|
||||
|
||||
defp parse(:limit, node, %Robot.Joint{} = joint) do
|
||||
[effort] = extract_floats(node, :effort, [0])
|
||||
[lower] = extract_floats(node, :lower, [0])
|
||||
[upper] = extract_floats(node, :upper, [0])
|
||||
[velocity] = extract_floats(node, :velocity, [0])
|
||||
|
||||
limit = Robot.Limit.init(effort: effort, lower: lower, upper: upper, velocity: velocity)
|
||||
{:ok, Robot.Joint.limit(joint, limit)}
|
||||
end
|
||||
|
||||
defp parse(:collision, node, %Robot.Link{} = link) do
|
||||
with {:ok, children} <- get_children(node),
|
||||
collision <- Robot.Collision.init(),
|
||||
{:ok, collision} <- reduce_oks(children, collision, &parse(&1, &2)),
|
||||
do: {:ok, Robot.Link.collision(link, collision)}
|
||||
end
|
||||
|
||||
defp parse(:inertial, node, %Robot.Link{} = link) do
|
||||
with inertia <- Robot.Inertia.init(),
|
||||
{:ok, children} <- get_children(node),
|
||||
{:ok, inertia} <- reduce_oks(children, inertia, &parse(&1, &2)),
|
||||
do: {:ok, Robot.Link.inertia(link, inertia)}
|
||||
end
|
||||
|
||||
defp parse(:mass, node, %Robot.Inertia{} = inertia) do
|
||||
[mass] = extract_floats(node, :value, [0])
|
||||
{:ok, Robot.Inertia.mass(inertia, mass)}
|
||||
end
|
||||
|
||||
defp parse(:inertia, node, %Robot.Inertia{} = inertia) do
|
||||
[ixx] = extract_floats(node, :ixx, [0])
|
||||
[ixy] = extract_floats(node, :ixy, [0])
|
||||
[ixz] = extract_floats(node, :ixz, [0])
|
||||
[iyy] = extract_floats(node, :iyy, [0])
|
||||
[iyz] = extract_floats(node, :iyz, [0])
|
||||
[izz] = extract_floats(node, :izz, [0])
|
||||
|
||||
matrix = Orientations.RotationMatrix.init({ixx, ixy, ixz, ixy, iyy, iyz, ixz, iyz, izz})
|
||||
{:ok, Robot.Inertia.matrix(inertia, matrix)}
|
||||
end
|
||||
|
||||
defp extract_floats(node, attribute_name, default_value) do
|
||||
|
|
|
|||
|
|
@ -63,14 +63,14 @@ defmodule Kinemat.URDF.Save do
|
|||
defp to_tag(%Robot.Link{name: name, visual: visual}),
|
||||
do: {:link, [name: name], [to_tag(visual)]}
|
||||
|
||||
defp to_tag(%Robot.Visual{geometries: geometries, origin: origin, material_name: material_name}) do
|
||||
geometries =
|
||||
geometries
|
||||
|> Enum.map(&to_tag(&1))
|
||||
defp to_tag(%Robot.Visual{geometry: geometry, origin: origin, material_name: material_name}) do
|
||||
geometry =
|
||||
geometry
|
||||
|> to_tag()
|
||||
|
||||
contents =
|
||||
if Enum.any?(geometries),
|
||||
do: [{:geometry, [], geometries}],
|
||||
if geometry,
|
||||
do: [{:geometry, [], [geometry]}],
|
||||
else: []
|
||||
|
||||
contents =
|
||||
|
|
|
|||
256
priv/fixtures/urdf/flexible.urdf
Normal file
256
priv/fixtures/urdf/flexible.urdf
Normal file
|
|
@ -0,0 +1,256 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot name="flexible">
|
||||
|
||||
<material name="blue">
|
||||
<color rgba="0 0 0.8 1"/>
|
||||
</material>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="right_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 0.1 0.2"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_right_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="right_leg"/>
|
||||
<origin xyz="0 -0.22 0.25"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.4 0.1 0.1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="right_base_joint" type="fixed">
|
||||
<parent link="right_leg"/>
|
||||
<child link="right_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_front_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name="right_front_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_front_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_back_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name="right_back_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_back_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 0.1 0.2"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_left_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="left_leg"/>
|
||||
<origin xyz="0 0.22 0.25"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.4 0.1 0.1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_base_joint" type="fixed">
|
||||
<parent link="left_leg"/>
|
||||
<child link="left_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_front_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name="left_front_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_front_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_back_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name="left_back_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_back_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<joint name="gripper_extension" type="prismatic">
|
||||
<parent link="base_link"/>
|
||||
<child link="gripper_pole"/>
|
||||
<limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.19 0 0.2"/>
|
||||
</joint>
|
||||
|
||||
<link name="gripper_pole">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius="0.01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="left_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_gripper">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="left_tip_joint" type="fixed">
|
||||
<parent link="left_gripper"/>
|
||||
<child link="left_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_tip">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name="right_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 -1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="right_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_gripper">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="right_tip_joint" type="fixed">
|
||||
<parent link="right_gripper"/>
|
||||
<child link="right_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_tip">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="head">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name="head_swivel" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="head"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin xyz="0 0 0.3"/>
|
||||
</joint>
|
||||
|
||||
<link name="box">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.08 0.08 0.08"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="tobox" type="fixed">
|
||||
<parent link="head"/>
|
||||
<child link="box"/>
|
||||
<origin xyz="0.1814 0 0.1414"/>
|
||||
</joint>
|
||||
</robot>
|
||||
417
priv/fixtures/urdf/physics.urdf
Normal file
417
priv/fixtures/urdf/physics.urdf
Normal file
|
|
@ -0,0 +1,417 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot name="physics">
|
||||
|
||||
<material name="blue">
|
||||
<color rgba="0 0 0.8 1"/>
|
||||
</material>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<link name="right_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 0.1 0.2"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.6 0.1 0.2"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_to_right_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="right_leg"/>
|
||||
<origin xyz="0 -0.22 0.25"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.4 0.1 0.1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.4 0.1 0.1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_base_joint" type="fixed">
|
||||
<parent link="right_leg"/>
|
||||
<child link="right_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_front_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_front_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_front_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_back_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_back_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_back_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 0.1 0.2"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.6 0.1 0.2"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_left_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="left_leg"/>
|
||||
<origin xyz="0 0.22 0.25"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.4 0.1 0.1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.4 0.1 0.1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_base_joint" type="fixed">
|
||||
<parent link="left_leg"/>
|
||||
<child link="left_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_front_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_front_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_front_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_back_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_back_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_back_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<joint name="gripper_extension" type="prismatic">
|
||||
<parent link="base_link"/>
|
||||
<child link="gripper_pole"/>
|
||||
<limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.19 0 0.2"/>
|
||||
</joint>
|
||||
|
||||
<link name="gripper_pole">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius="0.01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius="0.01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="left_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_gripper">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_tip_joint" type="fixed">
|
||||
<parent link="left_gripper"/>
|
||||
<child link="left_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_tip">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 -1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="right_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_gripper">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_tip_joint" type="fixed">
|
||||
<parent link="right_gripper"/>
|
||||
<child link="right_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_tip">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<link name="head">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="2"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="head_swivel" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="head"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin xyz="0 0 0.3"/>
|
||||
</joint>
|
||||
|
||||
<link name="box">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.08 0.08 0.08"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
<origin xyz="-0.04 0 0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.08 0.08 0.08"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="tobox" type="fixed">
|
||||
<parent link="head"/>
|
||||
<child link="box"/>
|
||||
<origin xyz="0.1814 0 0.1414"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
|
@ -16,7 +16,7 @@ defmodule Kinemat.URDF.LoadTest do
|
|||
assert robot.name == "myfirst"
|
||||
assert [link] = robot.links
|
||||
assert link.name == "base_link"
|
||||
assert [shape] = link.visual.geometries
|
||||
assert shape = link.visual.geometry
|
||||
assert shape.__struct__ == Geometry.Cylinder
|
||||
assert shape.length == 0.6
|
||||
assert shape.radius == 0.2
|
||||
|
|
@ -28,7 +28,7 @@ defmodule Kinemat.URDF.LoadTest do
|
|||
assert robot.name == "multipleshapes"
|
||||
assert [link0, link1] = robot.links
|
||||
assert link0.name == "right_leg"
|
||||
assert [shape] = link0.visual.geometries
|
||||
assert shape = link0.visual.geometry
|
||||
assert shape.__struct__ == Geometry.Box
|
||||
assert shape.x == 0.6
|
||||
assert shape.y == 0.1
|
||||
|
|
@ -90,23 +90,54 @@ defmodule Kinemat.URDF.LoadTest do
|
|||
{:ok, robot} = load(fixture_path("urdf/visual.urdf"))
|
||||
|
||||
assert {:ok, link} = Robot.get_link(robot, "left_gripper")
|
||||
assert [mesh] = link.visual.geometries
|
||||
assert mesh = link.visual.geometry
|
||||
assert mesh.filename == "package://urdf_tutorial/meshes/l_finger.dae"
|
||||
|
||||
assert {:ok, link} = Robot.get_link(robot, "left_tip")
|
||||
assert [mesh] = link.visual.geometries
|
||||
assert mesh = link.visual.geometry
|
||||
assert mesh.filename == "package://urdf_tutorial/meshes/l_finger_tip.dae"
|
||||
|
||||
assert {:ok, link} = Robot.get_link(robot, "right_gripper")
|
||||
assert [mesh] = link.visual.geometries
|
||||
assert mesh = link.visual.geometry
|
||||
assert mesh.filename == "package://urdf_tutorial/meshes/l_finger.dae"
|
||||
|
||||
assert {:ok, link} = Robot.get_link(robot, "right_tip")
|
||||
assert [mesh] = link.visual.geometries
|
||||
assert mesh = link.visual.geometry
|
||||
assert mesh.filename == "package://urdf_tutorial/meshes/l_finger_tip.dae"
|
||||
|
||||
assert {:ok, link} = Robot.get_link(robot, "head")
|
||||
assert [sphere] = link.visual.geometries
|
||||
assert sphere = link.visual.geometry
|
||||
assert sphere.radius == 0.2
|
||||
end
|
||||
|
||||
test "it parses `flexible.urdf`" do
|
||||
{:ok, robot} = load(fixture_path("urdf/flexible.urdf"))
|
||||
|
||||
assert {:ok, joint} = Robot.get_joint(robot, "right_front_wheel_joint")
|
||||
assert orientation = joint.axis.orientation
|
||||
assert Euler.x(orientation) == ~a(0)
|
||||
assert Euler.y(orientation) == ~a(0)
|
||||
assert Euler.z(orientation) == ~a(0)
|
||||
|
||||
assert translation = joint.axis.point
|
||||
assert Cartesian.x(translation) == 0
|
||||
assert Cartesian.y(translation) == 1.0
|
||||
assert Cartesian.z(translation) == 0
|
||||
|
||||
assert {:ok, joint} = Robot.get_joint(robot, "gripper_extension")
|
||||
assert joint.limit.effort == 1000.0
|
||||
assert joint.limit.lower == -0.38
|
||||
assert joint.limit.upper == 0
|
||||
assert joint.limit.velocity == 0.5
|
||||
end
|
||||
|
||||
test "it parses `physics.urdf`" do
|
||||
{:ok, robot} = load(fixture_path("urdf/physics.urdf"))
|
||||
|
||||
assert {:ok, link} = Robot.get_link(robot, "base_link")
|
||||
assert %Geometry.Cylinder{length: 0.6, radius: 0.2} = link.collision.geometry
|
||||
|
||||
assert link.inertia.mass == 10
|
||||
assert {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0} = link.inertia.matrix.matrix
|
||||
end
|
||||
end
|
||||
|
|
|
|||
Loading…
Reference in a new issue