feat: Add final parol6 assignment

.fish/activate.fish

@@ -0,0 +1,75 @@
+# Self-contained environment: ros2-lectures
+# Exported on: Thu Nov 13 06:53:17 PM CET 2025
+# Original environment from: /home/wessel/.config/fish/environments/configs/ros2-lectures
+
+# ROS2 development environment (requires distrobox)
+# Environment: ros2-lectures
+
+# First check if running inside distrobox
+if not test -f /run/.containerenv; and test -z "$CONTAINER_ID"
+    echo (set_color red)"This ROS2 environment should only be run inside a distrobox container"(set_color normal)
+    return 1
+end
+
+# Check if a previous initialization has occurred
+if test -n "$__ENV_INITIALIZED"
+    echo (set_color yellow)"Environment already initialized"(set_color normal)
+    return 0
+end
+
+# Mark as initialized (only after distrobox check passes)
+set -gx __ENV_INITIALIZED "1"
+set -gx CURRENT_ENV "ros2-lectures"
+# Source ROS2 setup files using bass
+if type -q bass
+    bass source /opt/ros/jazzy/setup.bash
+    if test -f ./install/setup.bash
+        bass source ./install/setup.bash
+    end
+else
+    echo (set_color red)"Error: bass is required for ROS2 environment. Install with: fisher install edc/bass"(set_color normal)
+    return 1
+end
+
+# Set environment variable for the prompt prefix
+set -gx ROS2_ACTIVE 1
+
+# Save the original prompt function if it exists
+# Only save if we don't already have a backup or if current prompt is not from an environment
+if not functions -q __env_orig_prompt
+    if functions -q fish_prompt
+        functions -c fish_prompt __env_orig_prompt
+    else
+        function __env_orig_prompt
+            echo -n (whoami)'@'(prompt_hostname)' '(set_color $fish_color_cwd)(prompt_pwd)(set_color normal)'> '
+        end
+    end
+else
+    # If we already have a backup, we're switching environments
+    # No need to create a new backup
+end
+
+# Define new prompt with ROS2 prefix
+function fish_prompt
+    echo -n (set_color magenta)'(ros2-lectures)'(set_color normal)' '
+    __env_orig_prompt
+end
+
+# ROS2 aliases and functions
+alias cb="colcon build"
+alias cbe="colcon build && env deactivate && cd ."
+alias cbs="colcon build --symlink-install"
+alias cbt="colcon build --packages-select"
+alias ct="colcon test"
+alias ctr="colcon test-result"
+
+echo (set_color green)"Activated ROS2 environment: ros2-lectures"(set_color normal)
+
+# Custom deactivation function
+function __env_custom_deactivate
+    # Remove ROS2-specific variables and aliases
+    set -e ROS2_ACTIVE
+    functions -e cb cbs cbt ct ctr 2>/dev/null
+
+    echo (set_color blue)"ROS2 environment cleanup completed"(set_color normal)
+end

.fish/readme.norg

@@ -0,0 +1,60 @@
+* Exported Fish Environment: ros2-lectures
+This directory contains a self-contained fish environment.
+
+** Files Structure
+   @code
+   .fish/
+   |-- activate.fish
+   |-- readme.norg
+   |-- bin/
+   @end
+
+** Usage
+
+*** Automatic Activation (Recommended)
+    The environment will automatically activate when you `cd` into this directory
+    if your Fish shell is configured with the auto-activation script.
+
+    @code fish
+    function check_and_source_activate
+      if test -f (pwd)/.fish/activate.fish
+        source (pwd)/.fish/activate.fish
+      elif test -f (pwd)/activate.fish
+        source (pwd)/activate.fish
+      end
+    end
+
+    function cd
+      builtin cd $argv && check_and_source_activate
+    end
+    @end
+
+*** Manual Activation
+    To manually activate the environment, run from the project root:
+    @code bash
+    source ./.fish/activate.fish
+    @end
+
+*** Deactivation
+    To deactivate the environment, run:
+    @code bash
+    env deactivate
+    @end
+
+    Or simply `cd` to a different directory if using auto-activation.
+
+** What This Environment Provides
+   - Prompt showing the environment name
+   - Environment-specific aliases and functions
+   - Custom environment variables
+   - Automatic cleanup when deactivated
+
+** Requirements
+   - Fish shell
+   - `bass` plugin (`fisher install edc/bass`) for compatibility with bash scripts
+
+** Sharing
+   This environment is completely self-contained. You can:
+   - Copy this directory to another machine
+   - Share it with others
+   - Version control it with your project (add .fish/ to your repo)

src/wt-assign1-2025-1-parol6-base-pkg/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.8)
+project(wt-assign1-2025-1-parol6-base-pkg)
+
+
+# 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/wt-assign1-2025-1-parol6-base-pkg/launch/display.launch.xml

@@ -0,0 +1,9 @@
+<launch>
+  <let name="urdf_path" value="$(find-pkg-share wt-assign1-2025-1-parol6-base-pkg)/urdf/PAROL6.urdf.xacro"/>
+  <let name="rviz_config_path" value="$(find-pkg-share wt-assign1-2025-1-parol6-base-pkg)/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)"/>
+</launch>

src/wt-assign1-2025-1-parol6-base-pkg/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/wt-assign1-2025-1-parol6-base-pkg/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>wt-assign1-2025-1-parol6-base-pkg</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/wt-assign1-2025-1-parol6-base-pkg/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/wt-assign1-2025-1-parol6-base-pkg/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/wt-assign1-2025-1-parol6-base-pkg/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://wt-assign1-2025-1-parol6-base-pkg/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://wt-assign1-2025-1-parol6-base-pkg/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://wt-assign1-2025-1-parol6-base-pkg/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://wt-assign1-2025-1-parol6-base-pkg/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://wt-assign1-2025-1-parol6-base-pkg/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://wt-assign1-2025-1-parol6-base-pkg/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://wt-assign1-2025-1-parol6-base-pkg/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://wt-assign1-2025-1-parol6-base-pkg/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://wt-assign1-2025-1-parol6-base-pkg/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://wt-assign1-2025-1-parol6-base-pkg/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://wt-assign1-2025-1-parol6-base-pkg/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://wt-assign1-2025-1-parol6-base-pkg/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://wt-assign1-2025-1-parol6-base-pkg/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://wt-assign1-2025-1-parol6-base-pkg/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/wt-assign1-2025-1-parol6-base-pkg/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/wt-assign1-2025-1-parol6-base-pkg/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/wt-assign1-2025-1-parol6-base-pkg/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/wt-assign1-2025-1-parol6-base-pkg/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/wt-assign1-2025-1-parol6-base-pkg/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>

src/wt-assign1-2025-2-parol6-fixed-pkg/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.8)
+project(wt-assign1-2025-2-parol6-fixed-pkg)
+
+
+# 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/wt-assign1-2025-2-parol6-fixed-pkg/launch/display.launch.xml

@@ -0,0 +1,11 @@
+<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)"/>
+</launch>

src/wt-assign1-2025-2-parol6-fixed-pkg/launch/display_with_objects.launch.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+  <let name="urdf_path" value="$(find-pkg-share wt-assign1-2025-2-parol6-fixed-pkg)/urdf/PAROL6_with_objects.urdf.xacro"/>
+  <let name="rviz_config_path" value="$(find-pkg-share wt-assign1-2025-2-parol6-fixed-pkg)/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/wt-assign1-2025-2-parol6-fixed-pkg/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/wt-assign1-2025-2-parol6-fixed-pkg/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>wt-assign1-2025-2-parol6-fixed-pkg</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/wt-assign1-2025-2-parol6-fixed-pkg/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/wt-assign1-2025-2-parol6-fixed-pkg/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/wt-assign1-2025-2-parol6-fixed-pkg/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/wt-assign1-2025-2-parol6-fixed-pkg/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/wt-assign1-2025-2-parol6-fixed-pkg/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/wt-assign1-2025-2-parol6-fixed-pkg/urdf/PAROL6_with_objects.urdf.xacro

@@ -0,0 +1,9 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="parol6_with_objects">
+  <xacro:include filename="$(find wt-assign1-2025-2-parol6-fixed-pkg)/urdf/PAROL6.urdf.xacro"/>
+
+  <xacro:include filename="$(find wt-assign1-2025-2-parol6-fixed-pkg)/urdf/muntjes_houder.urdf"/>
+  <xacro:include filename="$(find wt-assign1-2025-2-parol6-fixed-pkg)/urdf/board_conn_4.urdf"/>
+  <xacro:include filename="$(find wt-assign1-2025-2-parol6-fixed-pkg)/urdf/muntje.urdf"/>
+  <xacro:include filename="$(find wt-assign1-2025-2-parol6-fixed-pkg)/urdf/base_conn_4.urdf"/>
+</robot>

src/wt-assign1-2025-2-parol6-fixed-pkg/urdf/base_conn_4.urdf

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<robot name="base_conn_4">
+  <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://wt-assign1-2025-2-parol6-fixed-pkg/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://wt-assign1-2025-2-parol6-fixed-pkg/meshes/base_conn_4.STL" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

src/wt-assign1-2025-2-parol6-fixed-pkg/urdf/board_conn_4.urdf

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<robot name="board_conn_4">
+  <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://wt-assign1-2025-2-parol6-fixed-pkg/meshes/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://wt-assign1-2025-2-parol6-fixed-pkg/meshes/board_conn_4.STL" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

src/wt-assign1-2025-2-parol6-fixed-pkg/urdf/muntje.urdf

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<robot name="muntje">
+  <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://wt-assign1-2025-2-parol6-fixed-pkg/meshes/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://wt-assign1-2025-2-parol6-fixed-pkg/meshes/muntje.STL" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

src/wt-assign1-2025-2-parol6-fixed-pkg/urdf/muntjes_houder.urdf

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<robot name="muntjes_houder">
+  <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://wt-assign1-2025-2-parol6-fixed-pkg/meshes/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://wt-assign1-2025-2-parol6-fixed-pkg/meshes/muntjes houder.STL" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

src/wt-assign1-2025-3-parol6-mesh-pkg/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.8)
+project(wt-assign1-2025-3-parol6-mesh-pkg)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(visualization_msgs REQUIRED)
+
+add_executable(mesh_maker
+    src/Main.cpp
+    src/nodes/MeshMaker.cpp
+)
+
+ament_target_dependencies(mesh_maker rclcpp visualization_msgs)
+
+target_include_directories(mesh_maker PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/src
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/mesh_maker
+)
+
+install (
+    TARGETS
+        mesh_maker
+    DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+    DIRECTORY meshes
+    DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

src/wt-assign1-2025-3-parol6-mesh-pkg/package.xml

@@ -0,0 +1,18 @@
+<?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>wt-assign1-2025-3-parol6-mesh-pkg</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="wessel@go2it.eu">wessel</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/wt-assign1-2025-3-parol6-mesh-pkg/rviz/display.rviz

@@ -0,0 +1,254 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 85
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1/Description Topic1
+        - /Marker1
+        - /Marker1/Topic1
+      Splitter Ratio: 0.5
+    Tree Height: 1149
+  - 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
+        L1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        L2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        L3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        L4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        L5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        L6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      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
+        L1:
+          Value: true
+        L2:
+          Value: true
+        L3:
+          Value: true
+        L4:
+          Value: true
+        L5:
+          Value: true
+        L6:
+          Value: true
+        base_link:
+          Value: true
+        world:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        world:
+          base_link:
+            L1:
+              L2:
+                L3:
+                  L4:
+                    L5:
+                      L6:
+                        {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz_default_plugins/Marker
+      Enabled: true
+      Name: Marker
+      Namespaces:
+        mesh: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /board_markers
+      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: 4.246264934539795
+      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.46539902687072754
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.8953932523727417
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1436
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001600000050cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a0000050c000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000048000002a30000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004ae00000036fc0100000002fb0000000800540069006d00650100000000000004ae000002e800fffffffb0000000800540069006d00650100000000000004500000000000000000000003480000050c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1198
+  X: 30
+  Y: 54

src/wt-assign1-2025-3-parol6-mesh-pkg/src/Main.cpp

@@ -0,0 +1,15 @@
+#include <cstdlib>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nodes/MeshMaker.hpp"
+
+int main(int argc,char *argv[]) {
+    rclcpp::init(argc,argv);
+
+    auto node = std::make_shared<assignments::one::mesh_maker::MeshMakerNode>();
+
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+
+    return 0;
+}

src/wt-assign1-2025-3-parol6-mesh-pkg/src/nodes/MeshMaker.cpp

@@ -0,0 +1,147 @@
+#include "MeshMaker.hpp"
+
+namespace assignments::one::mesh_maker {
+
+MeshMakerNode::MeshMakerNode():
+Node("mesh_maker_node")
+{
+    this->declare_parameter("loc_board_x", 0.1);
+    this->declare_parameter("loc_board_y", 0.0);
+    this->declare_parameter("loc_board_z", 0.0);
+
+    this->declare_parameter("loc_coin_holder_x", 0.30);
+    this->declare_parameter("loc_coin_holder_y", 0.08);
+    this->declare_parameter("loc_coin_holder_z", 0.0);
+
+    this->declare_parameter("loc_coin_x", 0.34);
+    this->declare_parameter("loc_coin_y", 0.08);
+    this->declare_parameter("loc_coin_z", 0.0);
+
+    board_location_ = {
+        this->get_parameter("loc_board_x").as_double(),
+        this->get_parameter("loc_board_y").as_double(),
+        this->get_parameter("loc_board_z").as_double()
+    };
+
+    coin_holder_location_ = {
+        this->get_parameter("loc_coin_holder_x").as_double(),
+        this->get_parameter("loc_coin_holder_y").as_double(),
+        this->get_parameter("loc_coin_holder_z").as_double()
+    };
+
+    coin_location_ = {
+        this->get_parameter("loc_coin_x").as_double(),
+        this->get_parameter("loc_coin_y").as_double(),
+        this->get_parameter("loc_coin_z").as_double()
+    };
+
+    marker_publisher_ = this->create_publisher<visualization_msgs::msg::Marker>("board_markers", 10);
+    wall_timer_ = this->create_wall_timer(
+        std::chrono::seconds(1),
+        std::bind(&MeshMakerNode::publish_all_markers, this));
+}
+
+
+void MeshMakerNode::publish_all_markers() {
+    publish_marker_coin();
+    publish_marker_coin_holder();
+    publish_marker_board();
+    publish_marker_board_base();
+}
+
+visualization_msgs::msg::Marker MeshMakerNode::create_marker(
+    std::string mesh_resource,
+    MeshPosition pos,
+    MeshOrientation orientation
+) {
+    auto marker = visualization_msgs::msg::Marker();
+    marker.header.frame_id = "world";
+    marker.mesh_resource = mesh_resource;
+    marker.mesh_use_embedded_materials = true;
+
+    marker.header.stamp = this->now();
+    marker.ns = "mesh";
+    marker.id = next_mesh_id_++;
+
+    marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE;
+    marker.action = visualization_msgs::msg::Marker::ADD;
+
+    marker.pose.position.x = pos.x;
+    marker.pose.position.y = pos.y;
+    marker.pose.position.z = pos.z;
+
+    // In quaternions
+    marker.pose.orientation.x = orientation.x;
+    marker.pose.orientation.y = orientation.y;
+    marker.pose.orientation.z = orientation.z;
+    marker.pose.orientation.w = orientation.w;
+
+    marker.scale.x = mesh_scale_;
+    marker.scale.y = mesh_scale_;
+    marker.scale.z = mesh_scale_;
+
+    marker.color.r = 0.0;
+    marker.color.g = 1.0;
+    marker.color.b = 0.0;
+    marker.color.a = 1.0;
+
+    return marker;
+}
+
+void MeshMakerNode::publish_marker_coin() {
+    auto coin = create_marker(
+        "package://wt-assign1-2025-3-parol6-mesh-pkg/meshes/muntje.STL",
+        coin_location_,
+        default_orientation_
+    );
+
+    marker_publisher_->publish(coin);
+}
+
+void MeshMakerNode::publish_marker_coin_holder() {
+    MeshOrientation coin_holder_orientation = {
+        0.7071068,
+        0.0,
+        0.0,
+        0.7071068
+    };
+
+    auto coin_holder = create_marker(
+        "package://wt-assign1-2025-3-parol6-mesh-pkg/meshes/muntjes houder.STL",
+        coin_holder_location_,
+        coin_holder_orientation
+    );
+
+    marker_publisher_->publish(coin_holder);
+}
+
+void MeshMakerNode::publish_marker_board() {
+    MeshPosition board_pos = board_location_;
+    board_pos.x += 0.281;
+    board_pos.y += 0.031;
+    board_pos.z += 0.006;
+
+    MeshOrientation board_orientation = {
+        0.0, 0.0, -0.1127597, 0.9936223
+    };
+
+    auto board = create_marker(
+        "package://wt-assign1-2025-3-parol6-mesh-pkg/meshes/board_conn_4.STL",
+        board_pos,
+        board_orientation
+    );
+
+    marker_publisher_->publish(board);
+}
+
+void MeshMakerNode::publish_marker_board_base() {
+    auto board_base = create_marker(
+        "package://wt-assign1-2025-3-parol6-mesh-pkg/meshes/base_conn_4.STL",
+        board_location_,
+        default_orientation_
+    );
+
+    marker_publisher_->publish(board_base);
+}
+
+} // namespace assignments::one::mesh_maker

src/wt-assign1-2025-3-parol6-mesh-pkg/src/nodes/MeshMaker.hpp

@@ -0,0 +1,58 @@
+#pragma once
+
+#include "rclcpp/rclcpp.hpp"
+#include "visualization_msgs/msg/marker.hpp"
+
+namespace assignments::one::mesh_maker {
+
+struct MeshPosition {
+    double x;
+    double y;
+    double z;
+};
+
+struct MeshOrientation {
+    double x;
+    double y;
+    double z;
+    double w;
+};
+
+class MeshMakerNode : public rclcpp::Node {
+public:
+    MeshMakerNode();
+
+private:
+    rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr marker_publisher_;
+    rclcpp::TimerBase::SharedPtr wall_timer_;
+
+    int next_mesh_id_ = 0;
+    double mesh_scale_ = 0.001;
+
+    MeshPosition coin_location_;
+    MeshPosition board_location_;
+    MeshPosition coin_holder_location_;
+
+    MeshOrientation default_orientation_ = {
+        0.0,
+        0.0,
+        0.0,
+        1.0
+    };
+
+    void publish_all_markers();
+
+    visualization_msgs::msg::Marker create_marker(
+        std::string mesh_resource,
+        MeshPosition pos,
+        MeshOrientation orientation
+    );
+
+
+    void publish_marker_coin();
+    void publish_marker_coin_holder();
+    void publish_marker_board();
+    void publish_marker_board_base();
+};
+
+} // namespace assignments::one::mesh_maker

src/wt-assign1-2025-4-parol6-static-pkg/CMakeLists.txt

@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION 3.8)
+project(wt-assign1-2025-4-parol6-static-pkg)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+add_executable(board_location_publisher
+    src/Main.cpp
+    src/nodes/BoardLocationPublisher.cpp
+)
+
+ament_target_dependencies(board_location_publisher rclcpp geometry_msgs tf2 tf2_ros)
+
+target_include_directories(board_location_publisher PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/src
+)
+
+install (
+    TARGETS
+        board_location_publisher
+    DESTINATION lib/${PROJECT_NAME}
+)
+
+# RViz simulation
+install(
+    DIRECTORY launch meshes urdf rviz
+    DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

src/wt-assign1-2025-4-parol6-static-pkg/launch/display.launch.xml

@@ -0,0 +1,36 @@
+<launch>
+  <let name="urdf_path" value="$(find-pkg-share wt-assign1-2025-4-parol6-static-pkg)/urdf/PAROL6.urdf.xacro"/>
+  <let name="rviz_config_path" value="$(find-pkg-share wt-assign1-2025-4-parol6-static-pkg)/rviz/urdf_config.rviz"/>
+
+  <let name="board_urdf_path" value="$(find-pkg-share wt-assign1-2025-4-parol6-static-pkg)/urdf/playboard.urdf.xacro"/>
+
+  <arg name="x" default="0.60"/>
+  <arg name="y" default="0.00"/>
+  <arg name="z" default="0.00"/>
+  <arg name="roll" default="0.0"/>
+  <arg name="pitch" default="0.0"/>
+  <arg name="yaw" default="0.0"/>
+
+  <node pkg="robot_state_publisher" exec="robot_state_publisher" name="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"/>
+
+  <!-- Board Publisher with remap-->
+  <node pkg="robot_state_publisher" exec="robot_state_publisher" name="board_state_publisher">
+    <param name="robot_description" value="$(command 'xacro $(var board_urdf_path)')"/>
+    <remap from="/robot_description" to="/board_description"/>
+  </node>
+
+  <!-- Static TF publisher for all Board parts -->
+  <node pkg="wt-assign1-2025-4-parol6-static-pkg" exec="board_location_publisher" name="board_static_tf">
+    <param name="x" value="$(var x)"/>
+    <param name="y" value="$(var y)"/>
+    <param name="z" value="$(var z)"/>
+    <param name="roll" value="$(var roll)"/>
+    <param name="pitch" value="$(var pitch)"/>
+    <param name="yaw" value="$(var yaw)"/>
+  </node>
+
+  <node pkg="rviz2" exec="rviz2" output="screen" args="-d $(var rviz_config_path)"/>
+</launch>

src/wt-assign1-2025-4-parol6-static-pkg/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/wt-assign1-2025-4-parol6-static-pkg/package.xml

@@ -0,0 +1,18 @@
+<?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>wt-assign1-2025-4-parol6-static-pkg</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="wessel@go2it.eu">wessel</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/wt-assign1-2025-4-parol6-static-pkg/rviz/urdf_config.rviz

@@ -0,0 +1,356 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 85
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1/Description Topic1
+        - /TF1
+        - /Marker1
+        - /Marker1/Topic1
+        - /RobotModel2
+      Splitter Ratio: 0.5
+    Tree Height: 1217
+  - 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
+        L1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        L2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        L3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        L4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        L5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        L6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      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
+        L1:
+          Value: true
+        L2:
+          Value: true
+        L3:
+          Value: true
+        L4:
+          Value: true
+        L5:
+          Value: true
+        L6:
+          Value: true
+        base_conn_4:
+          Value: true
+        base_link:
+          Value: true
+        board_conn_4:
+          Value: true
+        muntje:
+          Value: true
+        muntjes_houder:
+          Value: true
+        world:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        world:
+          base_conn_4:
+            board_conn_4:
+              {}
+            muntje:
+              {}
+            muntjes_houder:
+              {}
+          base_link:
+            L1:
+              L2:
+                L3:
+                  L4:
+                    L5:
+                      L6:
+                        {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz_default_plugins/Marker
+      Enabled: true
+      Name: Marker
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /visualization_marker
+      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: /board_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        L1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        L2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        L3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        L4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        L5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        L6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        Link Tree Style: Links in Alphabetic Order
+        base_conn_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        board_conn_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        muntje:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        muntjes_houder:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: 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.869234561920166
+      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.0703982338309288
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 1.1203973293304443
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1504
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016000000550fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a00000550000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000048000002a30000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000008d000000036fc0100000002fb0000000800540069006d00650100000000000008d0000002e800fffffffb0000000800540069006d006501000000000000045000000000000000000000076a0000055000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 2256
+  X: 0
+  Y: 0

src/wt-assign1-2025-4-parol6-static-pkg/src/Main.cpp

@@ -0,0 +1,15 @@
+#include <cstdlib>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nodes/BoardLocationPublisher.hpp"
+
+int main(int argc,char *argv[]) {
+    rclcpp::init(argc,argv);
+
+    auto node = std::make_shared<assignments::one::board_location_publisher::BoardLocationPublisher>();
+
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+
+    return 0;
+}

src/wt-assign1-2025-4-parol6-static-pkg/src/nodes/BoardLocationPublisher.cpp

@@ -0,0 +1,49 @@
+#include "BoardLocationPublisher.hpp"
+
+namespace assignments::one::board_location_publisher {
+
+BoardLocationPublisher::BoardLocationPublisher():
+Node("mesh_maker_node")
+{
+    this->declare_parameter("x", 0.60);
+    this->declare_parameter("y", 0.00);
+    this->declare_parameter("z", 0.00);
+    this->declare_parameter("roll", 0.0);
+    this->declare_parameter("pitch", 0.0);
+    this->declare_parameter("yaw", 0.0);
+
+    board_location_x_ = this->get_parameter("x").as_double();
+    board_location_y_ = this->get_parameter("y").as_double();
+    board_location_z_ = this->get_parameter("z").as_double();
+
+    board_location_roll_ = this->get_parameter("roll").as_double();
+    board_location_pitch_ = this->get_parameter("pitch").as_double();
+    board_location_yaw_ = this->get_parameter("yaw").as_double();
+
+    position_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
+
+    create_transform();
+}
+
+geometry_msgs::msg::TransformStamped BoardLocationPublisher::create_transform() {
+    geometry_msgs::msg::TransformStamped transformStamped;
+
+    transformStamped.header.stamp = this->now();
+    transformStamped.header.frame_id = "world";
+    transformStamped.child_frame_id = "base_conn_4";
+
+    transformStamped.transform.translation.x = board_location_x_;
+    transformStamped.transform.translation.y = board_location_y_;
+    transformStamped.transform.translation.z = board_location_z_;
+
+    tf2::Quaternion q;
+    q.setRPY(board_location_roll_, board_location_pitch_, board_location_yaw_);
+    transformStamped.transform.rotation.x = q.x();
+    transformStamped.transform.rotation.y = q.y();
+    transformStamped.transform.rotation.z = q.z();
+    transformStamped.transform.rotation.w = q.w();
+
+    return transformStamped;
+}
+
+} // namespace assignments::one::board_location_publisher

src/wt-assign1-2025-4-parol6-static-pkg/src/nodes/BoardLocationPublisher.hpp

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+
+#include "tf2_ros/static_transform_broadcaster.h"
+#include "tf2/LinearMath/Quaternion.h"
+
+namespace assignments::one::board_location_publisher {
+
+class BoardLocationPublisher : public rclcpp::Node {
+public:
+    BoardLocationPublisher();
+
+private:
+    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> position_broadcaster_;
+
+    double board_location_x_;
+    double board_location_y_;
+    double board_location_z_;
+
+    double board_location_roll_;
+    double board_location_pitch_;
+    double board_location_yaw_;
+
+    geometry_msgs::msg::TransformStamped create_transform();
+};
+
+} // namespace assignments::one::board_location_publisher

src/wt-assign1-2025-4-parol6-static-pkg/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/wt-assign1-2025-4-parol6-static-pkg/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://wt-assign1-2025-3-parol6-mesh-pkg/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://wt-assign1-2025-3-parol6-mesh-pkg/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://wt-assign1-2025-3-parol6-mesh-pkg/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://wt-assign1-2025-3-parol6-mesh-pkg/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://wt-assign1-2025-3-parol6-mesh-pkg/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://wt-assign1-2025-3-parol6-mesh-pkg/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://wt-assign1-2025-3-parol6-mesh-pkg/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://wt-assign1-2025-3-parol6-mesh-pkg/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://wt-assign1-2025-3-parol6-mesh-pkg/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://wt-assign1-2025-3-parol6-mesh-pkg/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://wt-assign1-2025-3-parol6-mesh-pkg/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://wt-assign1-2025-3-parol6-mesh-pkg/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://wt-assign1-2025-3-parol6-mesh-pkg/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://wt-assign1-2025-3-parol6-mesh-pkg/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/wt-assign1-2025-4-parol6-static-pkg/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/wt-assign1-2025-4-parol6-static-pkg/urdf/base_conn_4.urdf

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<robot name="base_conn_4">
+  <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">
+    <visual>
+      <geometry>
+        <mesh filename="package://wt-assign1-2025-2-parol6-fixed-pkg/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>
+      <geometry>
+        <mesh filename="package://wt-assign1-2025-2-parol6-fixed-pkg/meshes/base_conn_4.STL" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

src/wt-assign1-2025-4-parol6-static-pkg/urdf/board_conn_4.urdf

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<robot name="board_conn_4">
+  <joint name="board_conn_4_joint" type="fixed">
+    <parent link="base_conn_4"/>
+    <child link="board_conn_4"/>
+    <origin xyz="0.281 0.031 0.006" rpy="0 0 -0.226"/>
+  </joint>
+
+  <link name="board_conn_4">
+    <visual>
+      <geometry>
+        <mesh filename="package://wt-assign1-2025-2-parol6-fixed-pkg/meshes/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>
+      <geometry>
+        <mesh filename="package://wt-assign1-2025-2-parol6-fixed-pkg/meshes/board_conn_4.STL" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

src/wt-assign1-2025-4-parol6-static-pkg/urdf/muntje.urdf

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<robot name="muntje">
+  <joint name="muntje_joint" type="fixed">
+    <parent link="base_conn_4"/>
+    <child link="muntje"/>
+    <origin xyz="0.2 0.0 0.0" rpy="0 0 0"/>
+  </joint>
+
+  <link name="muntje">
+    <visual>
+      <geometry>
+        <mesh filename="package://wt-assign1-2025-2-parol6-fixed-pkg/meshes/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>
+      <geometry>
+        <mesh filename="package://wt-assign1-2025-2-parol6-fixed-pkg/meshes/muntje.STL" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

src/wt-assign1-2025-4-parol6-static-pkg/urdf/muntjes_houder.urdf

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<robot name="muntjes_houder">
+  <joint name="muntjes_houder_joint" type="fixed">
+    <parent link="base_conn_4"/>
+    <child link="muntjes_houder"/>
+    <origin xyz="0.1 0.0 0.0" rpy="1.5708 0 0"/>
+  </joint>
+
+  <link name="muntjes_houder">
+    <visual>
+      <geometry>
+        <mesh filename="package://wt-assign1-2025-2-parol6-fixed-pkg/meshes/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>
+      <geometry>
+        <mesh filename="package://wt-assign1-2025-2-parol6-fixed-pkg/meshes/muntjes houder.STL" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

src/wt-assign1-2025-4-parol6-static-pkg/urdf/playboard.urdf.xacro

@@ -0,0 +1,9 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="parol6_with_objects">
+  <xacro:include filename="$(find wt-assign1-2025-2-parol6-fixed-pkg)/urdf/PAROL6.urdf.xacro"/>
+
+  <xacro:include filename="$(find wt-assign1-2025-2-parol6-fixed-pkg)/urdf/base_conn_4.urdf"/>
+  <xacro:include filename="$(find wt-assign1-2025-2-parol6-fixed-pkg)/urdf/board_conn_4.urdf"/>
+  <xacro:include filename="$(find wt-assign1-2025-2-parol6-fixed-pkg)/urdf/muntje.urdf"/>
+  <xacro:include filename="$(find wt-assign1-2025-2-parol6-fixed-pkg)/urdf/muntjes_houder.urdf"/>
+</robot>