defmodule Robot.Links do
@moduledoc """
Provide functions that import robot links
"""
import SweetXml
import Robot.Utils
import Robot.Mesh
alias ELA.Matrix, as: Matrix
alias ELA.Vector, as: Vector
@doc """
Get base frame information in a Map
TODO: develop information model for robot.
## Parameters
- xmldoc: xml file readed
## Examples
result = get_base_frame(xmldoc)
"""
@spec get_base_frame(binary()) :: Robot.Frame
def get_base_frame(xmldoc) do
result = xmldoc |> xpath(
~x"//library_visual_scenes/visual_scene",
translates: ~x"./node/node/node[@name=\"base\"]/translate/text()"l,
rotate_axis_sid: ~x"./node/node/node[@name=\"base\"]/rotate/@sid",
rotates: ~x"./node/node/node[@name=\"base\"]/rotate/text()"l,
)
frame = convert_to_frame(result, "base")
end
@doc """
Get base link information in a Map
TODO: develop information model for robot.
## Parameters
- xmldoc: xml file readed
## Examples
result = get_base_link(xmldoc)
"""
@spec get_base_link(binary()) :: Robot.Link
def get_base_link(xmldoc) do
#result = xmldoc |> xpath(
# ~x"///library_visual_scenes/visual_scene",
# link_name: ~x"./node/node/@name",
## instance_geometry: ~x"./node/node/instance_geometry/@url",
# translate: ~x"./node/node/translate/text()",
# rotate: ~x"./node/node/rotate/text()"
link_name = 'base_link'
link_base_path = get_link_path(link_name)
result = get_link(xmldoc, link_base_path, link_name)
frame = get_base_frame(xmldoc)
link_base = %Robot.Link{link_name: result[:link_name], instance_geometry: result[:instance_geometry], frame: frame, linked_links: result[:linked_links]}
end
@doc """
Get link_1 information and put it in a Map
TODO: develop information model for robot.
## Parameters
- xmldoc: xml file readed
## Examples
iex> get_link_1(xmldoc)
%{frame:
%{frames:
[%{rotate: [1.0, 0.0, 0.0, 0.0], translate: [0.0, 0.0, 0.0]},
%{rotate: [1.0, 0.0, 0.0, 0.0], translate: [0.0, 0.0, 0.227]},
%{rotate: [1.0, 0.0, 0.0, 0.0], translate: [0.0, 0.0, 0.0]}],
name: 'link_1',
rotate_axis: [0.0, 0.0, 1.0, 0.0],
rotate_axis_sid: 'node_joint_1_axis0'},
instance_geometry: ['#gkmodel0_link_1_geom0'],
link_name: 'link_1',
linked_links: ['link_2', 'link_cylinder']}
"""
@spec get_link_1(binary()) :: Robot.Link
def get_link_1(xmldoc) do
link_name = 'link_1'
link_1_path = get_link_path(link_name)
result = get_link(xmldoc, link_1_path, link_name)
frame = get_link_frame(xmldoc, link_1_path, link_name)
link_1 = %Robot.Link{link_name: result[:link_name], instance_geometry: result[:instance_geometry], frame: frame, linked_links: result[:linked_links]}
end
@doc """
TODO: merge to get_link_n, after development of information model
"""
def get_link_2(xmldoc) do
link_name = 'link_2'
link_2_path=get_link_path(link_name)
result = get_link(xmldoc, link_2_path, link_name)
frame = get_link_frame(xmldoc, link_2_path, link_name)
link_2 = %Robot.Link{link_name: result[:link_name], instance_geometry: result[:instance_geometry], frame: frame, linked_links: result[:linked_links]}
end
@doc """
TODO: merge to get_link_n, after development of information model
"""
def get_link_3(xmldoc) do
link_name = 'link_3'
link_3_path = get_link_path(link_name)
result = get_link(xmldoc, link_3_path, link_name)
frame = get_link_frame(xmldoc, link_3_path, link_name)
link_3 = %Robot.Link{link_name: result[:link_name], instance_geometry: result[:instance_geometry], frame: frame, linked_links: result[:linked_links]}
end
@doc """
TODO: merge to get_link_n, after development of information model
"""
def get_link_4(xmldoc) do
link_name = 'link_4'
link_4_path = get_link_path(link_name)
result = get_link(xmldoc, link_4_path, link_name)
frame = get_link_frame(xmldoc, link_4_path, link_name)
link_4 = %Robot.Link{link_name: result[:link_name], instance_geometry: result[:instance_geometry], frame: frame, linked_links: result[:linked_links]}
end
@doc """
TODO: merge to get_link_n, after development of information model
"""
def get_link_5(xmldoc) do
link_name = 'link_5'
link_5_path = get_link_path(link_name)
result = get_link(xmldoc, link_5_path, link_name)
frame = get_link_frame(xmldoc, link_5_path, link_name)
link_5 = %Robot.Link{link_name: result[:link_name], instance_geometry: result[:instance_geometry], frame: frame, linked_links: result[:linked_links]}
end
@doc """
TODO: merge to get_link_n, after development of information model
"""
def get_link_6(xmldoc) do
link_name = 'link_6'
link_6_path = get_link_path(link_name)
result = get_link(xmldoc, link_6_path, link_name)
frame = get_link_frame(xmldoc, link_6_path, link_name)
link_6 = %Robot.Link{link_name: result[:link_name], instance_geometry: result[:instance_geometry], frame: frame, linked_links: result[:linked_links]}
end
@doc """
Get link path based on link name
TODO: develop information model for robot.
## Parameters
- link: name of link
## Examples
link_name = 'link_6'
link_6_path = get_link_path(link_name)
"""
@spec get_link_path(Charlist) :: %SweetXpath{}
def get_link_path(link) do
link_base_name = 'base_link'
link_1_name = 'link_1'
link_2_name = 'link_2'
link_3_name = 'link_3'
link_4_name = 'link_4'
link_5_name = 'link_5'
link_6_name = 'link_6'
link_base_path = %SweetXpath{path: '//library_visual_scenes/visual_scene/node'}
link_1_path = %SweetXpath{path: '//library_visual_scenes/visual_scene/node/node'}
link_2_path = %SweetXpath{path: '//library_visual_scenes/visual_scene/node/node/node[@name=\"'++link_1_name++'\"]'}
link_3_path = %SweetXpath{path: '//library_visual_scenes/visual_scene/node/node/node[@name=\"'++link_1_name++'\"]/node[@name=\"'++link_2_name++'\"]'}
link_4_path = %SweetXpath{path: '//library_visual_scenes/visual_scene/node/node/node[@name=\"'++link_1_name++'\"]/node[@name=\"'++link_2_name++'\"]/node[@name=\"'++link_3_name++'\"]'}
link_5_path = %SweetXpath{path: '//library_visual_scenes/visual_scene/node/node/node[@name=\"'++link_1_name++'\"]/node[@name=\"'++link_2_name++'\"]/node[@name=\"'++link_3_name++'\"]/node[@name=\"'++link_4_name++'\"]'}
link_6_path = %SweetXpath{path: '//library_visual_scenes/visual_scene/node/node/node[@name=\"'++link_1_name++'\"]/node[@name=\"'++link_2_name++'\"]/node[@name=\"'++link_3_name++'\"]/node[@name=\"'++link_4_name++'\"]/node[@name=\"'++link_5_name++'\"]'}
link_paths = [
%{name: link_base_name, path: link_base_path},
%{name: link_1_name, path: link_1_path},
%{name: link_2_name, path: link_2_path},
%{name: link_3_name, path: link_3_path},
%{name: link_4_name, path: link_4_path},
%{name: link_5_name, path: link_5_path},
%{name: link_6_name, path: link_6_path}
]
link_paths
|> Enum.find(fn map -> map[:name] == link end)
|> Map.fetch!(:path)
end
@doc """
Get link information and return a Map based on name input
TODO: develop information model for robot.
## Parameters
- xmldoc: xml file readed
- link_path: link node path in xml
- link_name: link name
## Examples
link_name = 'link_6'
link_6_path = get_link_path(link_name)
result = get_link(xmldoc, link_6_path, link_name)
"""
@spec get_link(binary(), %SweetXpath{}, Charlist) :: %{link_name: Charlist,
instance_geometry: List,
linked_links: List}
def get_link(xmldoc, link_path, link_name) do
link_name_path = %SweetXpath{path: './node[@name=\"'++link_name++'\"]/@name'}
instance_geometry_path = %SweetXpath{path: './node[@name=\"'++link_name++'\"]/instance_geometry/@url', is_list: true}
linked_links_path = %SweetXpath{path: './node[@name=\"'++link_name++'\"]/node/@name', is_list: true}
result = xmldoc |> xpath(
link_path,
link_name: link_name_path,
instance_geometry: instance_geometry_path,
linked_links: linked_links_path
)
end
@doc """
Get link frames information and return a Map based on name input
TODO: develop information model for robot.
## Parameters
- xmldoc: xml file readed
- link_path: link node path in xml
- link_name: link name
## Examples
link_name = 'link_6'
link_6_path = get_link_path(link_name)
result = get_link(xmldoc, link_6_path, link_name)
"""
@spec get_link_frame(binary(), %SweetXpath{}, Charlist) :: Robot.Frame
def get_link_frame(xmldoc, link_path, link_name) do
translates_path = %SweetXpath{path: './node[@name=\"'++link_name++'\"]/translate/text()', is_list: true}
rotate_axis_sid_path = %SweetXpath{path: './node[@name=\"'++link_name++'\"]/rotate/@sid'}
rotates_path = %SweetXpath{path: './node[@name=\"'++link_name++'\"]/rotate/text()', is_list: true}
result = xmldoc |> xpath(
link_path,
translates: instance_geometry_path = translates_path,
rotate_axis_sid: rotate_axis_sid_path,
rotates: rotates_path
)
frame = convert_to_frame(result, link_name)
end
@doc """
TODO
"""
def get_tool_frame do
end
@doc """
Convert string list to float list in frame, and add name to frame
## Parameters
- xmlresult: frame
- name: link node path in xml
## Examples
translates_path = %SweetXpath{path: './node[@name=\"'++link_name++'\"]/translate/text()', is_list: true}
rotate_axis_sid_path = %SweetXpath{path: './node[@name=\"'++link_name++'\"]/rotate/@sid'}
rotates_path = %SweetXpath{path: './node[@name=\"'++link_name++'\"]/rotate/text()', is_list: true}
result = xmldoc |> xpath(
link_path,
translates: instance_geometry_path = translates_path,
rotate_axis_sid: rotate_axis_sid_path,
rotates: rotates_path
)
frame = convert_to_frame(result, link_name)
"""
@spec convert_to_frame(binary(), Charlist) :: Robot.Frame
def convert_to_frame(xmlresult,name) do
[t1 | t] = xmlresult[:translates]
[t2 | [t3]] = t
[r1 | r] = xmlresult[:rotates]
[r2 | r_] = r
[r3 | [r4]] = r_
frame = %Robot.Frame{name: name,
rotate_axis_sid: xmlresult[:rotate_axis_sid],
frames: [%{translate: charlist_to_flist(t1), rotate: charlist_to_flist(r1)},
%{translate: charlist_to_flist(t2), rotate: charlist_to_flist(r2)},
%{translate: charlist_to_flist(t3), rotate: charlist_to_flist(r4)}],
rotate_axis: charlist_to_flist(r3)
}
end
@doc """
Convert List of Char to a List of FLoat.
## Parameters
- charlist: Charlist Example: '1 2 3'
## Examples
iex(6)> charlist_to_flist('1 2 3')
[1.0, 2.0, 3.0]
"""
@spec charlist_to_flist(Charlist) :: List
def charlist_to_flist(charlist) do
pos_list_f = String.split(to_string(charlist))
|> Enum.map(fn s -> Float.parse(s) |> elem(0) end)
end
@doc """
Add meshes and transform info to robot model by using the name of the link
## Parameters
- robot_model: robot model used to store mesh and tranform information
- geometries: List of geometries to store geometry data
- names: names of the gerometries
- link: robto link
- name: name of the geometry
- joint_value: angle of the joint in rad
## Examples
geometries = File.read!("geometries.txt") |> :erlang.binary_to_term
link_base = File.read!("link_base.txt") |> :erlang.binary_to_term
link_1 = File.read!("link_1.txt") |> :erlang.binary_to_term
link_2 = File.read!("link_2.txt") |> :erlang.binary_to_term
link_3 = File.read!("link_3.txt") |> :erlang.binary_to_term
link_4 = File.read!("link_4.txt") |> :erlang.binary_to_term
link_5 = File.read!("link_5.txt") |> :erlang.binary_to_term
link_6 = File.read!("link_6.txt") |> :erlang.binary_to_term
robot_model = %{points: [], indices: [], translates: [], rotates: []}
robot_model = add_link_to_robot_model_by_name(robot_model, geometries, names, link_base, 'gkmodel0_base_link_geom0', 0.0)
robot_model = add_link_to_robot_model_by_name(robot_model, geometries, names, link_1, 'gkmodel0_link_1_geom0', :math.pi/6)
robot_model = add_link_to_robot_model_by_name(robot_model, geometries, names, link_2, 'gkmodel0_link_2_geom0', :math.pi/6)
robot_model = add_link_to_robot_model_by_name(robot_model, geometries, names, link_3, 'gkmodel0_link_3_geom0', :math.pi/6)
robot_model = add_link_to_robot_model_by_name(robot_model, geometries, names, link_4, 'gkmodel0_link_4_geom0', :math.pi/6)
robot_model = add_link_to_robot_model_by_name(robot_model, geometries, names, link_5, 'gkmodel0_link_5_geom0', :math.pi/6)
robot_model = add_link_to_robot_model_by_name(robot_model, geometries, names, link_6, 'gkmodel0_link_6_geom0', :math.pi/6)
"""
@spec add_link_to_robot_model_by_name(%{points: List, indices: List, translates: List, rotates: List}, List, List, %{link_name: Charlist, instance_geometry: List, frame: %{name: Charlist, rotate_axis_sid: Charlist, frames: List, rotate_axis: List}, linked_links: List}, Charlist, Float) :: Rbot.Mesh
def add_link_to_robot_model_by_name(robot_model, geometries, names, link, name, joint_value) do
index = Enum.find_index(names,fn x-> x==name end)
points = robot_model.points
indices = robot_model.indices
translates = robot_model.translates
rotates = robot_model.rotates
new_points = List.insert_at(points, length(points),Enum.at(geometries,index)|>Map.fetch!(:positions))
robot_model = %{robot_model | points: new_points}
new_indices = List.insert_at(indices, length(indices),Enum.at(geometries,index)|>Map.fetch!(:triangles))
robot_model = %{robot_model | indices: new_indices}
tranf_temp = apply_joint(List.last(robot_model.translates),List.last(robot_model.rotates), get_trans_or_rot_from_link(link, :translate),get_trans_or_rot_from_link(link, :rotate), get_joint_from_link(link, joint_value))
new_translates = List.insert_at(translates, length(translates),tranf_temp[:trans])
robot_model = %{robot_model | translates: new_translates}
new_rotates = List.insert_at(rotates, length(rotates),tranf_temp[:rot])
robot_model = %{robot_model | rotates: new_rotates}
end
defp get_trans_or_rot_from_link(link, key) do
link|>Map.fetch!(:frame)|>Map.fetch!(:frames)|>Enum.at(1)|>Map.fetch!(key)
end
@doc """
Get rotaion vector based on rotation axis and input joint angle.
## Parameters
- link: link
- joint_value: angle of the joint in rad
## Examples
get_joint_from_link(link, joint_value)
"""
@spec get_joint_from_link(%{link_name: Charlist, instance_geometry: List, frame: %{name: Charlist, rotate_axis_sid: Charlist, frames: List, rotate_axis: List}, linked_links: List}, Float) :: List
defp get_joint_from_link(link, joint_value) do
rot = link|>Map.fetch!(:frame)|>Map.fetch!(:rotate_axis)
rot = List.replace_at(rot, 3,joint_value)
rot
end
#def add_two_list(old_trans, new_trans) do
# if old_trans do
# trans = new_trans|> Enum.with_index |> Enum.map(fn({x, i}) -> x + Enum.at(old_trans,i) end)
# else
# trans = new_trans
# end
#end
@doc """
Perform frame transformation.
## Parameters
- old_trans: the first translation vector
- old_rots: the first rotation axis+angle
- new_trans: the secord tranlation vector
- new_rots: the secord rotation axis+angle
- joint_vector: the joint rotaion axis+angle
"""
@spec apply_joint(List, List, List, List, List) :: List
def apply_joint(old_trans, old_rots, new_trans, new_rots, joint_vector) when old_trans==nil do
apply_joint([0.0,0.0,0.0], [1.0,0.0,0.0,0.0], new_trans, new_rots, joint_vector)
end
def apply_joint(old_trans, old_rots, new_trans, new_rots, joint_vector) do
old_mat = matrixFromAxisAngle(old_rots, old_trans)
new_mat = matrixFromAxisAngle(new_rots, new_trans)
joint_vector = matrixFromAxisAngle(joint_vector, [0.0,0.0,0.0])
mat_temp = Matrix.mult(old_mat, new_mat)
axisAngleFromMatrix(Matrix.mult(mat_temp, joint_vector))
end
@doc """
Generate Robot mesh based on joints
## Parameters
- joints: Joints input
## Examples
joints = %Robot.Joints{joint1: :math.pi/6, joint2: :math.pi/6, joint3: :math.pi/6, joint4: :math.pi/6, joint5: :math.pi/6, joint6: :math.pi/6}
robot_model = get_robot_model_based_on_joint_value(joints)
points = robot_model.points
indices = robot_model.indices
translates = robot_model.translates
rotates = robot_model.rotates
draw(points,indices,translates,rotates)
"""
@spec get_robot_model_based_on_joint_value(Robot.Joints) :: Robot.Mesh
def get_robot_model_based_on_joint_value(joints) do
geometries = File.read!("geometries.txt") |> :erlang.binary_to_term
link_base = File.read!("link_base.txt") |> :erlang.binary_to_term
link_1 = File.read!("link_1.txt") |> :erlang.binary_to_term
link_2 = File.read!("link_2.txt") |> :erlang.binary_to_term
link_3 = File.read!("link_3.txt") |> :erlang.binary_to_term
link_4 = File.read!("link_4.txt") |> :erlang.binary_to_term
link_5 = File.read!("link_5.txt") |> :erlang.binary_to_term
link_6 = File.read!("link_6.txt") |> :erlang.binary_to_term
robot_model = %Robot.Mesh{points: [], indices: [], translates: [], rotates: []}
#points = geometries |> Enum.map(fn map -> map[:positions] end)
#indices = geometries |> Enum.map(fn map -> map[:triangles] end)
#translates = geometries |> Enum.map(fn map -> [:rand.uniform() * 5, :rand.uniform() * 5,:rand.uniform() * 5] end)
#rotates = geometries |> Enum.map(fn map -> [:rand.uniform() * 5, :rand.uniform() * 5,:rand.uniform() * 5] end)
names = geometries |> Enum.map(fn map -> map[:name] end)
#draw([Enum.at(points,1)],[Enum.at(indices,1)],[[0.0,0.0,0.0]],[[0.0,0.0,0.0]])
robot_model = add_link_to_robot_model_by_name(robot_model, geometries, names, link_base, 'gkmodel0_base_link_geom0', 0.0)
robot_model = add_link_to_robot_model_by_name(robot_model, geometries, names, link_1, 'gkmodel0_link_1_geom0', joints.joint1)
robot_model = add_link_to_robot_model_by_name(robot_model, geometries, names, link_2, 'gkmodel0_link_2_geom0', joints.joint2)
robot_model = add_link_to_robot_model_by_name(robot_model, geometries, names, link_3, 'gkmodel0_link_3_geom0', joints.joint3)
robot_model = add_link_to_robot_model_by_name(robot_model, geometries, names, link_4, 'gkmodel0_link_4_geom0', joints.joint4)
robot_model = add_link_to_robot_model_by_name(robot_model, geometries, names, link_5, 'gkmodel0_link_5_geom0', joints.joint5)
robot_model = add_link_to_robot_model_by_name(robot_model, geometries, names, link_6, 'gkmodel0_link_6_geom0', joints.joint6)
end
end
