feat(les6): Add parol 6 robot + objects

src/parol6/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.8)
+project(parol6)
+
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+install(
+  DIRECTORY launch meshes urdf rviz
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

src/parol6/launch/display.launch.xml

@@ -0,0 +1,14 @@
+<launch>
+  <let name="urdf_path" value="$(find-pkg-share parol6)/urdf/PAROL6.urdf.xacro"/>
+  <let name="rviz_config_path" value="$(find-pkg-share parol6)/rviz/urdf_config.rviz"/>
+  <node pkg="robot_state_publisher" exec="robot_state_publisher">
+    <param name="robot_description" value="$(command 'xacro $(var urdf_path)')"/>
+  </node>
+  <node pkg="joint_state_publisher_gui" exec="joint_state_publisher_gui"/>
+  <node pkg="rviz2" exec="rviz2" output="screen" args="-d $(var rviz_config_path)"/>
+  <!-- <arg name="model" />
+  <param name="robot_description" textfile="$(find parol6)/urdf/parol6.urdf" />
+  <node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find parol6)/urdf.rviz" /> -->
+</launch>

src/parol6/launch/display_with_objects.launch.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+  <let name="urdf_path" value="$(find-pkg-share parol6)/urdf/PAROL6_with_objects.urdf.xacro"/>
+  <let name="rviz_config_path" value="$(find-pkg-share parol6)/rviz/display.rviz"/>
+
+  <node pkg="robot_state_publisher" exec="robot_state_publisher">
+    <param name="robot_description" value="$(command 'xacro $(var urdf_path)')"/>
+  </node>
+
+  <node pkg="joint_state_publisher_gui" exec="joint_state_publisher_gui">
+    <param name="robot_description" value="$(command 'xacro $(var urdf_path)')"/>
+  </node>
+
+  <node pkg="rviz2" exec="rviz2" output="screen" args="-d $(var rviz_config_path)"/>
+</launch>

src/parol6/launch/gazebo.launch

@@ -0,0 +1,20 @@
+<launch>
+  <include
+    file="$(find gazebo_ros)/launch/empty_world.launch" />
+  <node
+    name="tf_footprint_base"
+    pkg="tf"
+    type="static_transform_publisher"
+    args="0 0 0 0 0 0 base_link base_footprint 40" />
+  <node
+    name="spawn_model"
+    pkg="gazebo_ros"
+    type="spawn_model"
+    args="-file $(find parol6)/urdf/parol6.urdf -urdf -model parol6"
+    output="screen" />
+  <node
+    name="fake_joint_calibration"
+    pkg="rostopic"
+    type="rostopic"
+    args="pub /calibrated std_msgs/Bool true" />
+</launch>

src/parol6/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>parol6</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="v@w.com">sirdep</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/parol6/rviz/display.rviz

@@ -0,0 +1,164 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 549
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Viewsc
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Scale: 0.3
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 2.0
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 100
+  Y: 100

src/parol6/rviz/urdf_config.rviz

@@ -0,0 +1,239 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 85
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1/Description Topic1
+      Splitter Ratio: 0.5
+    Tree Height: 517
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        arm_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        elbow_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        forearm_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        hand_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        shoulder_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tool_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        wrist_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        arm_link:
+          Value: true
+        base_link:
+          Value: true
+        elbow_link:
+          Value: true
+        forearm_link:
+          Value: true
+        hand_link:
+          Value: true
+        shoulder_link:
+          Value: true
+        tool_link:
+          Value: true
+        wrist_link:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        base_link:
+          shoulder_link:
+            arm_link:
+              elbow_link:
+                forearm_link:
+                  wrist_link:
+                    hand_link:
+                      tool_link:
+                        {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 3.4432098865509033
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.299166202545166
+        Y: -0.3069069981575012
+        Z: 0.8311280012130737
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.3503985106945038
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.5403982400894165
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 854
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000160000002a3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000048000002a3000000f300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000048000002a3000000c500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004bd00000042fc0100000002fb0000000800540069006d00650100000000000004bd000002dc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000356000002a300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1213
+  X: 400
+  Y: 84

src/parol6/urdf/PAROL6.csv

@@ -0,0 +1,8 @@
+Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
+base_link,-0.0274562034602567,-0.00137265400595656,0.031237920982338,0,0,0,0.812661291810144,0.00110740279540382,4.67463481205112E-05,8.47873933528831E-06,0.00106582362830227,-2.6194130573118E-06,0.00147883396699374,0,0,0,0,0,0,package://PAROL6/meshes/base_link.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://PAROL6/meshes/base_link.STL,,Assem9^PACKAssem1-1,Coordinate System1,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
+L1,0.00524529891406311,0.0289722820784871,0.0992791429452917,0,0,0,0.644800830530233,0.00109429409029172,-6.27156874097953E-05,-5.70466862844753E-05,0.00056996197700816,-0.000121686402940313,0.00129487722474286,0,0,0,0,0,0,package://PAROL6/meshes/L1.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://PAROL6/meshes/L1.STL,,Assem5^PACKAssem1-1,Coordinate System1,Axis1,L1,revolute,0,0,0,0,0,0,base_link,0,0,1,0,0,0,0,,,,,,,,
+L2,-0.0094181617193695,-0.0783879603613605,0.0369429741417731,0,0,0,0.512334779399788,0.00123357482646866,3.63236238096924E-06,-3.30834557934783E-07,0.000205462733796053,4.86441772103282E-07,0.00132225033266162,0,0,0,0,0,0,package://PAROL6/meshes/L2.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://PAROL6/meshes/L2.STL,,Assem8^PACKAssem1-1,Coordinate System2,Axis2,L2,revolute,0.0234207210610375,0,0.1105,-1.5707963267949,0,0,L1,0,0,1,0,0,0,0,,,,,,,,
+L3,0.0155509885239593,-0.0191127686426613,-0.00153322577917236,0,0,0,0.550970997806044,0.00057827573271994,0.000116505232322314,-1.19955293256513E-05,0.000567011676944701,1.55814255036021E-05,0.000815862001506843,0,0,0,0,0,0,package://PAROL6/meshes/L3.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://PAROL6/meshes/L3.STL,,Assem1^PACKAssem1-1,Coordinate System3,Axis3,L3,revolute,0,-0.18,0,3.1416,0,-1.5708,L2,0,0,-1,0,0,0,0,,,,,,,,
+L4,0.000947028430634433,-0.008978947112462,-0.0924798659827059,0,0,0,0.39346362633915,0.000371672516830215,-1.05560000666389E-05,-1.56899445418326E-07,0.000332364226344361,-6.10397110934672E-05,0.000250606789571909,0,0,0,0,0,0,package://PAROL6/meshes/L4.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://PAROL6/meshes/L4.STL,,Assem2^PACKAssem1-1,Coordinate System4,Axis4,L4,revolute,0.0435,0,0,1.5707963267949,0,3.14159265358979,L3,0,0,-1,0,0,0,0,,,,,,,,
+L5,6.7162171684676E-06,-0.00621420980825221,-0.000960871025916146,0,0,0,0.185171777001718,0.000109176748460021,-1.27195013439061E-07,8.4436049148633E-08,8.41307588934268E-05,-4.41596193640049E-07,0.000110629136236229,0,0,0,0,0,0,package://PAROL6/meshes/L5.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://PAROL6/meshes/L5.STL,,Assem3^PACKAssem1-1,Coordinate System5,Axis5,L5,revolute,0,0,-0.17635,-1.5708,0,0,L4,0,0,-1,0,0,0,0,,,,,,,,
+L6,0.00632955087228015,-0.000453856352095022,-0.0554441567386594,0,0,0,0.0703261410665101,1.40276237518287E-05,3.15503246005279E-07,-9.28469598788221E-07,1.81406721166117E-05,-9.43630009030796E-07,2.34608103291027E-05,0,0,0,0,0,0,package://PAROL6/meshes/L6.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://PAROL6/meshes/L6.STL,,Assem7^PACKAssem1-1,Coordinate System6,Axis6,L6,continuous,0,0,0,1.5708,0,0,L5,0,0,-1,0,0,0,0,,,,,,,,

src/parol6/urdf/PAROL6.urdf.xacro

@@ -0,0 +1,317 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot name="parol6">
+
+  <link name="world"/>
+  <joint name="base_joint" type="fixed">
+    <parent link="world"/>
+    <child link="base_link"/>
+    <origin rpy="0 0 0" xyz="0.0 0.0 0.00"/>
+  </joint>
+
+
+  <link name="base_link">
+    <inertial>
+      <origin xyz="-0.0274562034602567 -0.00137265400595656 0.031237920982338" rpy="0 0 0" />
+      <mass value="0.812661291810144" />
+      <inertia ixx="0.00110740279540382" ixy="4.67463481205112E-05" ixz="8.47873933528831E-06" iyy="0.00106582362830227" iyz="-2.6194130573118E-06" izz="0.00147883396699374" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://parol6/meshes/base_link.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://parol6/meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="L1">
+    <inertial>
+      <origin xyz="0.00524529891406311 0.0289722820784871 0.0992791429452917" rpy="0 0 0" />
+      <mass value="0.644800830530233" />
+      <inertia ixx="0.00109429409029172" ixy="-6.27156874097953E-05" ixz="-5.70466862844753E-05" iyy="0.00056996197700816" iyz="-0.000121686402940313" izz="0.00129487722474286" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://parol6/meshes/L1.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 0.5 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://parol6/meshes/L1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L1" type="revolute">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <parent link="base_link" />
+    <child link="L1" />
+    <axis xyz="0 0 1" />
+    <limit lower="-1.7" upper="1.7" effort="300" velocity="3" />
+  </joint>
+  <link name="L2">
+    <inertial>
+      <origin xyz="-0.0094181617193695 -0.0783879603613605 0.0369429741417731" rpy="0 0 0" />
+      <mass value="0.512334779399788" />
+      <inertia ixx="0.00123357482646866" ixy="3.63236238096924E-06" ixz="-3.30834557934783E-07" iyy="0.000205462733796053" iyz="4.86441772103282E-07" izz="0.00132225033266162" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://parol6/meshes/L2.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://parol6/meshes/L2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L2" type="revolute">
+    <origin xyz="0.0234207210610375 0 0.1105" rpy="-1.5707963267949 0 0" />
+    <parent link="L1" />
+    <child link="L2" />
+    <axis xyz="0 0 1" />
+    <limit lower="-0.98" upper="1" effort="300" velocity="3" />
+  </joint>
+  <link name="L3">
+    <inertial>
+      <origin xyz="0.0155509885239593 -0.0191127686426613 -0.00153322577917236" rpy="0 0 0" />
+      <mass value="0.550970997806044" />
+      <inertia ixx="0.00057827573271994" ixy="0.000116505232322314" ixz="-1.19955293256513E-05" iyy="0.000567011676944701" iyz="1.55814255036021E-05" izz="0.000815862001506843" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://parol6/meshes/L3.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 0.5 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://parol6/meshes/L3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L3" type="revolute">
+    <origin xyz="0 -0.18 0" rpy="3.1416 0 -1.5708" />
+    <parent link="L2" />
+    <child link="L3" />
+    <axis xyz="0 0 -1" />
+    <limit lower="-2" upper="1.3" effort="300" velocity="3" />
+  </joint>
+  <link name="L4">
+    <inertial>
+      <origin xyz="0.000947028430634433 -0.008978947112462 -0.0924798659827059" rpy="0 0 0" />
+      <mass value="0.39346362633915" />
+      <inertia ixx="0.000371672516830215" ixy="-1.05560000666389E-05" ixz="-1.56899445418326E-07" iyy="0.000332364226344361" iyz="-6.10397110934672E-05" izz="0.000250606789571909" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://parol6/meshes/L4.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://parol6/meshes/L4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L4" type="revolute">
+    <origin xyz="0.0435 0 0" rpy="1.5707963267949 0 3.14159265358979" />
+    <parent link="L3" />
+    <child link="L4" />
+    <axis xyz="0 0 -1" />
+    <limit lower="-2" upper="2" effort="300" velocity="3" />
+  </joint>
+  <link name="L5">
+    <inertial>
+      <origin xyz="6.7162171684676E-06 -0.00621420980825221 -0.000960871025916146" rpy="0 0 0" />
+      <mass value="0.185171777001718" />
+      <inertia ixx="0.000109176748460021" ixy="-1.27195013439061E-07" ixz="8.4436049148633E-08" iyy="8.41307588934268E-05" iyz="-4.41596193640049E-07" izz="0.000110629136236229" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://parol6/meshes/L5.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 0.5 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://parol6/meshes/L5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L5" type="revolute">
+    <origin xyz="0 0 -0.17635" rpy="-1.5708 0 0" />
+    <parent link="L4" />
+    <child link="L5" />
+    <axis xyz="0 0 -1" />
+    <limit lower="-2.1" upper="2.1" effort="300" velocity="3" />
+  </joint>
+  <link name="L6">
+    <inertial>
+      <origin xyz="0.00632955087228015 -0.000453856352095022 -0.0554441567386594" rpy="0 0 0" />
+      <mass value="0.0703261410665101" />
+      <inertia ixx="1.40276237518287E-05" ixy="3.15503246005279E-07" ixz="-9.28469598788221E-07" iyy="1.81406721166117E-05" iyz="-9.43630009030796E-07" izz="2.34608103291027E-05" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://parol6/meshes/L6.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://parol6/meshes/L6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L6" type="continuous">
+    <origin xyz="0 0 0" rpy="1.5708 0 0" />
+    <parent link="L5" />
+    <child link="L6" />
+    <axis xyz="0 0 -1" />
+    <limit lower="-3.1" upper="3.1" effort="300" velocity="3" />
+  </joint>
+
+  <transmission name="TRANS_L1">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="L1">
+
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="ACT_L1">
+
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <transmission name="TRANS_L2">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="L2">
+
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="ACT_L2">
+
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <transmission name="TRANS_L3">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="L3">
+
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="ACT_L3">
+
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <transmission name="TRANS_L4">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="L4">
+
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="ACT_L4">
+
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <transmission name="TRANS_L5">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="L5">
+
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="ACT_L5">
+
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <transmission name="TRANS_L6">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="L6">
+
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="ACT_L6">
+
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <gazebo>
+    <plugin name="control" filename="libgazebo_ros_control.so">
+      <robotNamespace>/</robotNamespace>
+    </plugin>
+  </gazebo>
+
+  <gazebo reference="L1">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+
+  <gazebo reference="L2">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+
+  <gazebo reference="L3">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+
+  <gazebo reference="L4">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+
+  <gazebo reference="L5">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+
+  <gazebo reference="L6">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+
+</robot>

src/parol6/urdf/PAROL6_ros2_control.xacro

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <ros2_control name="Arm" type="system">
+        <hardware>
+            <plugin>mock_components/GenericSystem</plugin>
+        </hardware>
+        <joint name="L1">
+            <command_interface name="position"/>
+            <state_interface name="position">
+                <param name="initial_value">0.0</param>
+            </state_interface>
+        </joint>
+    </ros2_control>
+    <!-- <ros2_control name="Gripper" type="system">
+        <hardware>
+            <plugin>mock_components/GenericSystem</plugin>
+        </hardware>
+        <joint name="joint_6">
+            <command_interface name="position"/>
+            <state_interface name="position">
+                <param name="initial_value">0.0</param>
+            </state_interface>
+        </joint>
+    </ros2_control> -->
+</robot>

src/parol6/urdf/PAROL6_with_objects.urdf.xacro

@@ -0,0 +1,121 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="parol6_with_objects">
+  <xacro:include filename="$(find parol6)/urdf/PAROL6.urdf.xacro"/>
+
+  <joint name="base_conn_4_joint" type="fixed">
+    <parent link="world"/>
+    <child link="base_conn_4"/>
+    <origin xyz="0.0 0 0" rpy="0 0 0"/>
+  </joint>
+
+  <link name="base_conn_4">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.1"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://parol6/meshes/base_conn_4.STL" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.5 0.5 0.5 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://parol6/meshes/base_conn_4.STL" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="board_conn_4_joint" type="fixed">
+    <parent link="world"/>
+    <child link="board_conn_4"/>
+    <origin xyz="0.281 0.031 0.006" rpy="0 0 -0.226"/>
+  </joint>
+
+  <link name="board_conn_4">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.05"/>
+      <inertia ixx="0.0005" ixy="0.0" ixz="0.0" iyy="0.0005" iyz="0.0" izz="0.0005"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://parol6/meshes/objects/board_conn_4.STL" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="blue">
+        <color rgba="0.2 0.2 0.8 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://parol6/meshes/objects/board_conn_4.STL" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="muntje_joint" type="fixed">
+    <parent link="world"/>
+    <child link="muntje"/>
+    <origin xyz="0.2 0.0 0.0" rpy="0 0 0"/>
+  </joint>
+
+  <link name="muntje">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.01"/>
+      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://parol6/meshes/objects/muntje.STL" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="gold">
+        <color rgba="0.9 0.7 0.2 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://parol6/meshes/objects/muntje.STL" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="muntjes_houder_joint" type="fixed">
+    <parent link="world"/>
+    <child link="muntjes_houder"/>
+    <origin xyz="0.1 0.0 0.0" rpy="1.5708 0 0"/>
+  </joint>
+
+  <link name="muntjes_houder">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.15"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://parol6/meshes/objects/muntjes houder.STL" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="green">
+        <color rgba="0.2 0.8 0.2 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://parol6/meshes/objects/muntjes houder.STL" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+
+</robot>

src/parol6/urdf/base_conn_4.urdf

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<robot name="base_conn_4">
+
+  <!-- Dummy world link to avoid KDL warning -->
+  <link name="base_conn_4_world"/>
+
+  <joint name="base_conn_4_fixed" type="fixed">
+    <parent link="base_conn_4_world"/>
+    <child link="base_conn_4"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+
+  <link name="base_conn_4">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.1"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+    </inertial>
+
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://parol6/meshes/objects/base_conn_4.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.5 0.5 0.5 1.0"/>
+      </material>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://parol6/meshes/objects/base_conn_4.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+</robot>

src/parol6/urdf/board_conn_4.urdf

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<robot name="board_conn_4">
+
+  <!-- Dummy world link to avoid KDL warning -->
+  <link name="board_conn_4_world"/>
+
+  <joint name="board_conn_4_fixed" type="fixed">
+    <parent link="board_conn_4_world"/>
+    <child link="board_conn_4"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+
+  <link name="board_conn_4">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.05"/>
+      <inertia ixx="0.0005" ixy="0.0" ixz="0.0" iyy="0.0005" iyz="0.0" izz="0.0005"/>
+    </inertial>
+
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://parol6/meshes/objects/board_conn_4.STL"/>
+      </geometry>
+      <material name="blue">
+        <color rgba="0.2 0.2 0.8 1.0"/>
+      </material>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://parol6/meshes/objects/board_conn_4.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+</robot>

src/parol6/urdf/muntje.urdf

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<robot name="muntje">
+
+  <!-- Dummy world link to avoid KDL warning -->
+  <link name="muntje_world"/>
+
+  <joint name="muntje_fixed" type="fixed">
+    <parent link="muntje_world"/>
+    <child link="muntje"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+
+  <link name="muntje">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.01"/>
+      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+    </inertial>
+
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://parol6/meshes/objects/muntje.STL"/>
+      </geometry>
+      <material name="gold">
+        <color rgba="0.9 0.7 0.2 1.0"/>
+      </material>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://parol6/meshes/objects/muntje.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+</robot>

src/parol6/urdf/muntjes_houder.urdf

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<robot name="muntjes_houder">
+
+  <!-- Dummy world link to avoid KDL warning -->
+  <link name="muntjes_houder_world"/>
+
+  <joint name="muntjes_houder_fixed" type="fixed">
+    <parent link="muntjes_houder_world"/>
+    <child link="muntjes_houder"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+
+  <link name="muntjes_houder">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.15"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+    </inertial>
+
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://parol6/meshes/objects/muntjes houder.STL"/>
+      </geometry>
+      <material name="green">
+        <color rgba="0.2 0.8 0.2 1.0"/>
+      </material>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://parol6/meshes/objects/muntjes houder.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+</robot>