diff --git a/ros_trick/.gitignore b/ros_trick/.gitignore
new file mode 100644
index 00000000..57d0c1c2
--- /dev/null
+++ b/ros_trick/.gitignore
@@ -0,0 +1,2 @@
+.vscode
+docker_repo/
diff --git a/ros_trick/LICENSE b/ros_trick/LICENSE
new file mode 100644
index 00000000..7a4a3ea2
--- /dev/null
+++ b/ros_trick/LICENSE
@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
\ No newline at end of file
diff --git a/ros_trick/README.md b/ros_trick/README.md
new file mode 100644
index 00000000..ed6b34df
--- /dev/null
+++ b/ros_trick/README.md
@@ -0,0 +1,152 @@
+# NASA Space ROS Sim Summer Sprint Challenge
+
+   Team lead freelance username: xfiderek  
+   Submission title: Improving fidelity of SSRMS demo with Trick
+
+# Improving fidelity of SSRMS demo with Trick (Moveit2 - canadarm2 - Trick demo)
+
+This folder features an end-to-end demo of canadarm2 (SSRMS) modelled using [Trick](https://nasa.github.io/trick/) and connected to moveit2 via authored ROS2<>Trick bridge.
+
+The demo includes the following:
+
+1. [ros_trick_bridge](./ros_src/ros_trick_bridge/) - A ROS2 package that allowing spawning & controlling trick simulations from ROS2 launch. It also sends /clock messages from trick to ROS2.
+1. [trick_ros2_control](./ros_src/trick_ros2_control) - A ROS2 Control plugin that enables communication between ros2_control-enabled robots and trick simulations.
+1. [SIM_trick_canadarm](./trick_src/SIM_trick_canadarm) - A trick simulation of canadarm demo running at 100Hz. The simulation calculates forward dynamics with Articulated Body Algorithm, with friction model that takes into account Stribeck effect.
+1. Demo-related packages with moveit configurations, visualization, launch files and additional utils.
+1. Dockerfiles and compose file launching an end to end canadarm demo
+
+Both `ros_trick_bridge` and `trick_ros2_control` can be easily imported into any project that wants to leverage connection between trick and ROS2. Any type of data can be exchanged between two systems in the provided framework. Further sections in the readme describe how to adjust the provided code to other types of robots and simulations.
+
+## Starting the demo
+
+The demo environment runs Trick simulation of canadarm2 (SSRMS), connected to moveit2 via authored `ros_trick_bridge` node and `trick_ros2_control` plugin. Trick is used for modelling dynamics of SSRMS using Articulated Body Algorithm, taking Stribeck effect into account for friction forces. Joint Trajectory Controller from ROS2 control package is used to convert trajectory points into torque commands using PID controller. Additionally, the demo features Wrench visualizer, that subscribes to trick data to visualize joint torques in RVIZ.
+
+Prerequisites - docker, docker compose
+
+1. Build docker images in this folder:
+
+    ```bash
+    ./build.sh
+    ```
+
+    If building for the first time, this will clone the [space-ros/docker](https://github.com/space-ros/docker.git) repo from the `humble-2024.07.0` branch, due to dependency on canadarm assets and rviz2
+
+1. Start containers:
+
+    ```bash
+    xhost +local:docker
+    ./run.sh
+    ```
+
+    This should start two docker containers - one for Trick and another for ROS part.  
+    You should now be able to see the Trick GUI:  
+    <img src="./docs/trick_main.png" alt="trick_main" width="500"/>
+
+1. Login to the container with ROS part to open RVIZ
+
+    ```bash
+    docker exec -it canadarm_ros_trick_demo bash
+    ```
+
+    In the container:
+
+    ```bash
+    source install/setup.bash && ros2 launch trick_canadarm_moveit_config moveit_rviz.launch.py
+    ```
+
+    (Optional) If rviz does not render with the previous command, try overriding mesa loader driver:
+
+    ```bash
+    export MESA_LOADER_DRIVER_OVERRIDE=softpipe
+    source install/setup.bash && ros2 launch trick_canadarm_moveit_config moveit_rviz.launch.py
+    ```
+
+    <img src="./docs/moveit_rviz.png" alt="moveit_rviz" width="500"/>
+1. From Rviz, Set target pose for canadarm with `Motion Planning` GUI. You can set target in joint space in `Motion Planning->Joints` tab. Set similar values to those below for reproducibility:
+
+    <img src="./docs/rviz_joints.png" alt="rviz_joints" width="500"/>
+1. In `MotionPlanning` RVIZ Moveit GUI, go to `Planning` and hit `Plan & Execute`
+1. You should be able to see magnitudes of torques applied to joints (pink arrows)  
+    <img src="./docs/rviz_motion.png" alt="rviz_motion" width="500"/>
+
+*Note that the displayed torque values are `total_torque = input_torque - friction_torque`, thus they may dissapear shortly after starting execution, because arm operates in no-gravity environment and input and friction torques are close to each other after start. Friction torques can be visualized by changing topics in RVIZ to `/wrench_topic/{joint_name}/friction_torque` (e.g. `/wrench_topic/B1/friction_torque`) and bumping torque arrow scale to 1.0.*
+
+*PID coefficients were manually tuned to ensure smooth movement, however as robot dynamics is quite sensitive, additional tuning may be required to achieve optimal / energy efficient trajectory execution*
+
+### Plotting sim data with Trick
+
+You can plot simulation data using `Trick View` application. Trick View is automatically started together with docker containers. To plot the data, simply open the GUI and navigate to `CanadarmManip.manip` in Trick Variables tree.
+
+<img src="./docs/trick_view_variables.png" alt="trick_view_variables" width="500"/>  
+
+<img src="./docs/trick_velocity_plot.png" alt="trick_velocity_plot" width="500"/>
+
+## Architecture of ROS2 Trick bridge
+
+The main design goal was to make it easy to integrate existing trick simulations with ROS2 with no code changes on Trick side. That is the communication happens over [Trick Variable Server](https://nasa.github.io/trick/tutorial/TutVariableServer).
+
+<img src="./docs/ros_trick_bridge_architecture.png" alt="ros_trick_bridge_architecture" width="500"/>
+
+There are two ROS2 packages provided in the solution that interact directly with Trick:
+
+1. `ros_trick_bridge` - this package contains a lifecycle (managed) node that spawns the trick simulation with proper parameters as a subprocess. Managed node transitions (active/inactive) can be used to start and pause the simulation. Additionally, the node subscribes to clock data from trick using Trick Variable Server and publishes it on ROS2 /clock topic.
+1. `trick_ros2_control` - this is an optional package that can be used to quickly integrate ros2_control-enabled robots with trick. It uses a separate Trick Variable Server Client to exchange command and state data with the simulation.
+
+Additionally, any other ROS2 node can be easily adjusted in provided framework to exchange any data with trick, using the provided primitives.
+
+## Bridging ROS2 and Trick in your own project
+
+The bridge and related components can be easily reused either natively or in docker-based environment. First, the native part will be described as it highlights how to configure the provided components to run with other simulations.
+
+### Integrating Trick simulation with ROS2 using ros_trick_bridge
+
+To run and control your simulation as ros node and publish /clock messages:
+
+1. install trick on your target machine
+1. Compile your simulation with [trick-CP](https://nasa.github.io/trick/tutorial/ATutAnalyticSim#trick-cp)
+1. copy/include [ros_src/ros_trick_bridge](./ros_src/ros_trick_bridge/) into your ROS2 workspace.
+1. Create simulation-specific param file from [ros_trick_bridge_example_params.yaml](ros_src/ros_trick_bridge/params/ros_trick_bridge_example_params.yaml) template.
+1. Include the provided [ros_trick_bridge.launch.py](ros_src/ros_trick_bridge/launch/ros_trick_bridge.launch.py) in your project's bringup launch file.
+1. Now the trick simulation should start together with your launch file and `/clock` messages should be published
+
+The first 3 steps are handled in the provided docker images
+
+### Using ros2_control with trick_ros2_control
+
+Additionally, to use trick_ros2_control to control your robot with ROS2 control:
+
+1. copy/include [ros_src/trick_ros2_control](ros_src/trick_ros2_control) into your ROS2 workspace
+1. Define trick-related variables in your robot's URDF file as in canadarm [SSRMS_Canadarm2.ros2_control.xacro](ros_src/trick_canadarm_moveit_config/config/SSRMS_Canadarm2.ros2_control.xacro). For every joint interface, define corresponding trick variable. For example, assuming that your Base_Joint's position in trick is defined as `CanadarmManip.manip.q[0]` then add the following:
+
+```xml
+<joint name="Base_Joint">
+    <state_interface name="position"/>
+    <param name="state_interface/position/trick_variable_name">CanadarmManip.manip.q[0]</param>
+</joint>
+```
+
+See [SSRMS_Canadarm2.ros2_control.xacro](./ros_src/trick_canadarm_moveit_config/config/SSRMS_Canadarm2.ros2_control.xacro) for reference.
+Note that currently, initial positions are set via trick simulation. Initial commands are set to 0
+
+### Exchange any other type of data (pub/sub) between trick and ros2
+
+* (Python) There is a TrickVariableServerClient class provided as part of ros_trick_bridge. Check [canadarm_wrench_publisher_node.py](ros_src/canadarm_wrench_publisher/canadarm_wrench_publisher/canadarm_wrench_publisher_node.py) for example usage.
+* (C++) - Take a look at Socket class and utils.hpp in ros2_trick to create your own node to exchange data
+
+### Using provided docker images as starting point
+
+The project was split into two docker images, so that the standalone ros trick bridge could be easily carved out and used in different projects.
+
+1. [Dockerfile.ros_trick_bridge](./Dockerfile.ros_trick_bridge) featuring:
+
+* Trick simulation enviroment
+* `ros_trick_bridge` ros package with default parameters
+* Example trick simulation code (canadarm simulation) from `trick_src` folder
+
+1. [Dockerfile.canadarm_trick_demo](./Dockerfile.canadarm_ros_trick_demo) featuring:
+
+* `trick_ros2_control`, containing ROS2 control plugin.
+* `canadarm_moveit_config` package, starting moveit2 and ROS2 control.
+* `canadarm_wrench_publisher` package, containing sample node that publishes Wrench data from trick
+
+The files can be adjusted easily to facilitate docker-based development.
diff --git a/ros_trick/build.sh b/ros_trick/build.sh
new file mode 100755
index 00000000..47416b2c
--- /dev/null
+++ b/ros_trick/build.sh
@@ -0,0 +1,15 @@
+#!/bin/bash
+
+set -e
+
+CURRENT_PATH=$(pwd)
+SPACEROS_DOCKER_REPO_PATH="${CURRENT_PATH}/docker_repo"
+
+if [ ! -d ${SPACEROS_DOCKER_REPO_PATH} ]; then
+    mkdir -p ${SPACEROS_DOCKER_REPO_PATH}
+    git clone -b humble-2024.07.0 https://github.com/space-ros/docker.git ${SPACEROS_DOCKER_REPO_PATH}
+    cd ${CURRENT_PATH}
+fi
+cd ${SPACEROS_DOCKER_REPO_PATH}/moveit2 && ./build.sh
+cd ${SPACEROS_DOCKER_REPO_PATH}/space_robots && ./build.sh
+cd ${CURRENT_PATH} && docker compose build
diff --git a/ros_trick/canadarm_ros_trick_demo.Dockerfile b/ros_trick/canadarm_ros_trick_demo.Dockerfile
new file mode 100644
index 00000000..a3a982d7
--- /dev/null
+++ b/ros_trick/canadarm_ros_trick_demo.Dockerfile
@@ -0,0 +1,50 @@
+# Copyright 2024 Blazej Fiderek (xfiderek)
+# 
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+# 
+#     https://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# The base image is built from https://github.com/space-ros/docker/blob/humble-2024.07.0/space_robots/
+# Git tag: humble-2024.07.0
+FROM openrobotics/space_robots_demo:latest
+
+
+# Rviz does not work inside the newest space_robots_demo, so we have to upgrade the following packages.
+# These packages are already present in the base image and here we are upgrading them to latest versions
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
+    --mount=type=cache,target=/var/lib/apt,sharing=locked \ 
+    sudo apt-get update && sudo apt-get install -y \
+    mesa-libgallium \
+    libdrm-amdgpu1 \
+    libdrm-common \
+    libdrm-intel1 \
+    libdrm-radeon1 \
+    libdrm2 \
+    libegl-mesa0 \
+    libgbm1 \
+    libgl1-mesa-dev \
+    libglapi-mesa \
+    libglx-mesa0 \
+    liborc-0.4-0 \
+    libpq5 \
+    linux-libc-dev \
+    mesa-va-drivers \
+    mesa-vdpau-drivers \
+    mesa-vulkan-drivers
+
+ENV TRICK_DEMO_WS=/opt/ros_trick_demo_ws
+
+WORKDIR ${TRICK_DEMO_WS}
+COPY --chown=spaceros-user:space-ros-user ros_src ${TRICK_DEMO_WS}/src
+
+RUN /bin/bash -c 'source ${DEMO_DIR}/install/setup.bash \
+   && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON --event-handlers desktop_notification- status-'
+RUN rm -rf build log src
diff --git a/ros_trick/docker-compose.yml b/ros_trick/docker-compose.yml
new file mode 100644
index 00000000..64261a2a
--- /dev/null
+++ b/ros_trick/docker-compose.yml
@@ -0,0 +1,44 @@
+# Copyright 2024 Blazej Fiderek (xfiderek)
+# 
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+# 
+#     https://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+services:
+  ros_trick_bridge:
+    container_name: ros_trick_bridge
+    command: [ "bash", "-c", ". ~/.bashrc && . install/setup.bash && ros2 launch ros_trick_bridge ros_trick_bridge.launch.py" ]
+    build:
+      context: .
+      dockerfile: ros_trick_bridge.Dockerfile
+    image: ros_trick_bridge:latest
+    environment:
+      - DISPLAY=${DISPLAY}
+      - TERM=${TERM}
+      - QT_X11_NO_MITSHM=1
+    network_mode: host
+    volumes:
+      - /tmp/.X11-unix:/tmp/.X11-unix
+
+  canadarm_ros_trick_demo:
+    container_name: canadarm_ros_trick_demo
+    build:
+      context: .
+      dockerfile: canadarm_ros_trick_demo.Dockerfile
+    image: canadarm_ros_trick_demo:latest
+    network_mode: host
+    environment:
+      - DISPLAY=${DISPLAY}
+      - TERM=${TERM}
+      - QT_X11_NO_MITSHM=1
+    volumes:
+      - /tmp/.X11-unix:/tmp/.X11-unix
+    command: [ "bash", "-c", ". ~/.bashrc && . install/setup.bash && ros2 launch trick_canadarm_moveit_config demo.launch.py" ]
diff --git a/ros_trick/docs/moveit_rviz.png b/ros_trick/docs/moveit_rviz.png
new file mode 100644
index 00000000..bf26d2bd
Binary files /dev/null and b/ros_trick/docs/moveit_rviz.png differ
diff --git a/ros_trick/docs/ros_trick_bridge_architecture.png b/ros_trick/docs/ros_trick_bridge_architecture.png
new file mode 100644
index 00000000..4823dda9
Binary files /dev/null and b/ros_trick/docs/ros_trick_bridge_architecture.png differ
diff --git a/ros_trick/docs/rviz_joints.png b/ros_trick/docs/rviz_joints.png
new file mode 100644
index 00000000..316e1c05
Binary files /dev/null and b/ros_trick/docs/rviz_joints.png differ
diff --git a/ros_trick/docs/rviz_motion.png b/ros_trick/docs/rviz_motion.png
new file mode 100644
index 00000000..860a856d
Binary files /dev/null and b/ros_trick/docs/rviz_motion.png differ
diff --git a/ros_trick/docs/trick_main.png b/ros_trick/docs/trick_main.png
new file mode 100644
index 00000000..ab586779
Binary files /dev/null and b/ros_trick/docs/trick_main.png differ
diff --git a/ros_trick/docs/trick_velocity_plot.png b/ros_trick/docs/trick_velocity_plot.png
new file mode 100644
index 00000000..db5fbbf5
Binary files /dev/null and b/ros_trick/docs/trick_velocity_plot.png differ
diff --git a/ros_trick/docs/trick_view_variables.png b/ros_trick/docs/trick_view_variables.png
new file mode 100644
index 00000000..a99ad390
Binary files /dev/null and b/ros_trick/docs/trick_view_variables.png differ
diff --git a/ros_trick/ros_src/canadarm_wrench_publisher/canadarm_wrench_publisher/__init__.py b/ros_trick/ros_src/canadarm_wrench_publisher/canadarm_wrench_publisher/__init__.py
new file mode 100644
index 00000000..be353d1a
--- /dev/null
+++ b/ros_trick/ros_src/canadarm_wrench_publisher/canadarm_wrench_publisher/__init__.py
@@ -0,0 +1,13 @@
+# Copyright 2024 Blazej Fiderek (xfiderek)
+# 
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+# 
+#     https://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
diff --git a/ros_trick/ros_src/canadarm_wrench_publisher/canadarm_wrench_publisher/canadarm_wrench_publisher_node.py b/ros_trick/ros_src/canadarm_wrench_publisher/canadarm_wrench_publisher/canadarm_wrench_publisher_node.py
new file mode 100644
index 00000000..ba38587d
--- /dev/null
+++ b/ros_trick/ros_src/canadarm_wrench_publisher/canadarm_wrench_publisher/canadarm_wrench_publisher_node.py
@@ -0,0 +1,117 @@
+# Copyright 2024 Blazej Fiderek (xfiderek)
+# 
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+# 
+#     https://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+import time
+
+import rclpy
+from geometry_msgs.msg import WrenchStamped
+from rclpy.executors import SingleThreadedExecutor
+from rclpy.node import Node
+from ros_trick_bridge.trick_variable_server_client import TrickVariableServerClient
+from std_msgs.msg import Header
+
+
+class WrenchPublisher(Node):
+    def __init__(self):
+        super().__init__("wrench_publisher")
+        self._trick_host = "localhost"
+        self._trick_port = 49765
+        self._ndof = 7
+        self._trick_variable_names = [
+            item
+            for i in range(self._ndof)
+            for item in [
+                f"CanadarmManip.manip.applied_torque[{i}]",
+                f"CanadarmManip.manip.friction_torque[{i}]",
+            ]
+        ]
+
+        # let everything start
+        time.sleep(5.0)
+
+        self.get_logger().info("starting trick variable server")
+        self._trick_variable_server_client = TrickVariableServerClient(
+            host=self._trick_host,
+            port=self._trick_port,
+            client_tag="wrench_pub",
+            trick_variable_names=self._trick_variable_names,
+            trick_var_cycle_time=0.1,
+            on_receive_callback=self.trick_data_callback,
+        )
+        self.link_frames = ["B1", "B2", "B3", "B4", "B5", "B6", "EE_SSRMS"]
+        self.rot_axes = [
+            [0.0, 0.0, 1.0],
+            [1.0, 0.0, 0.0],
+            [0.0, 0.0, 1.0],
+            [0.0, 0.0, 1.0],
+            [0.0, 0.0, -1.0],
+            [1.0, 0.0, 0.0],
+            [0.0, 0.0, 1.0],
+        ]
+        self.get_logger().info("spawning wrench publishers")
+        self.wrench_publishers = [
+            publisher
+            for link_name in self.link_frames
+            for publisher in [
+                self.create_publisher(
+                    WrenchStamped, f"wrench_topic/{link_name}/total_torque", 10
+                ),
+                self.create_publisher(
+                    WrenchStamped, f"wrench_topic/{link_name}/friction_torque", 10
+                ),
+            ]
+        ]
+
+        self._trick_variable_server_client.start_listening()
+
+    def trick_data_callback(self, trick_data):
+        # self.get_logger().info("received trick cb")
+
+        for i in range(self._ndof):
+            total_torque_var_name = self._trick_variable_names[2 * i]
+            friction_torque_var_name = self._trick_variable_names[2 * i + 1]
+            total_torque_val = float(trick_data[total_torque_var_name])
+            friction_torque_val = float(trick_data[friction_torque_var_name])
+            total_torque_pub = self.wrench_publishers[2 * i]
+            friction_torque_pub = self.wrench_publishers[2 * i + 1]
+            now = self.get_clock().now().to_msg()
+
+            msg = WrenchStamped()
+            msg.header = Header()
+            msg.header.frame_id = self.link_frames[i]
+            msg.header.stamp = now
+
+            # send total torque
+            msg.wrench.torque.x = self.rot_axes[i][0] * total_torque_val
+            msg.wrench.torque.y = self.rot_axes[i][1] * total_torque_val
+            msg.wrench.torque.z = self.rot_axes[i][2] * total_torque_val
+            total_torque_pub.publish(msg)
+
+            # send friction torque
+            msg.wrench.torque.x = self.rot_axes[i][0] * friction_torque_val
+            msg.wrench.torque.y = self.rot_axes[i][1] * friction_torque_val
+            msg.wrench.torque.z = self.rot_axes[i][2] * friction_torque_val
+            friction_torque_pub.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = WrenchPublisher()
+    rclpy.spin(node)
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/ros_trick/ros_src/canadarm_wrench_publisher/package.xml b/ros_trick/ros_src/canadarm_wrench_publisher/package.xml
new file mode 100644
index 00000000..889b15db
--- /dev/null
+++ b/ros_trick/ros_src/canadarm_wrench_publisher/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>canadarm_wrench_publisher</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="fiderekblazej@gmail.com">blazej</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/ros_trick/ros_src/canadarm_wrench_publisher/resource/canadarm_wrench_publisher b/ros_trick/ros_src/canadarm_wrench_publisher/resource/canadarm_wrench_publisher
new file mode 100644
index 00000000..c21c341a
--- /dev/null
+++ b/ros_trick/ros_src/canadarm_wrench_publisher/resource/canadarm_wrench_publisher
@@ -0,0 +1,14 @@
+// Copyright 2024 Blazej Fiderek (xfiderek)
+// 
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+// 
+//     https://www.apache.org/licenses/LICENSE-2.0
+// 
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
diff --git a/ros_trick/ros_src/canadarm_wrench_publisher/setup.cfg b/ros_trick/ros_src/canadarm_wrench_publisher/setup.cfg
new file mode 100644
index 00000000..b76e8f59
--- /dev/null
+++ b/ros_trick/ros_src/canadarm_wrench_publisher/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/canadarm_wrench_publisher
+[install]
+install_scripts=$base/lib/canadarm_wrench_publisher
diff --git a/ros_trick/ros_src/canadarm_wrench_publisher/setup.py b/ros_trick/ros_src/canadarm_wrench_publisher/setup.py
new file mode 100644
index 00000000..1917642a
--- /dev/null
+++ b/ros_trick/ros_src/canadarm_wrench_publisher/setup.py
@@ -0,0 +1,25 @@
+from setuptools import find_packages, setup
+
+package_name = "canadarm_wrench_publisher"
+
+setup(
+    name=package_name,
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="Blazej Fiderek (xfiderek)",
+    maintainer_email="fiderekblazej@gmail.com",
+    description="Get joint torques from trick and publish them as GeometryWrench",
+    license="Apache-2.0",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "canadarm_wrench_publisher_node = canadarm_wrench_publisher.canadarm_wrench_publisher_node:main"
+        ],
+    },
+)
diff --git a/ros_trick/ros_src/ros_trick_bridge/launch/ros_trick_bridge.launch.py b/ros_trick/ros_src/ros_trick_bridge/launch/ros_trick_bridge.launch.py
new file mode 100644
index 00000000..1a616cc5
--- /dev/null
+++ b/ros_trick/ros_src/ros_trick_bridge/launch/ros_trick_bridge.launch.py
@@ -0,0 +1,150 @@
+# Copyright 2024 Blazej Fiderek (xfiderek)
+# 
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+# 
+#     https://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    EmitEvent,
+    LogInfo,
+    RegisterEventHandler,
+)
+from launch.conditions import IfCondition
+from launch.events.matchers import matches_action
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import LifecycleNode
+from launch_ros.event_handlers import OnStateTransition
+from launch_ros.events.lifecycle import ChangeState
+from launch_ros.substitutions import FindPackageShare
+from lifecycle_msgs.msg import Transition
+
+
+def generate_launch_description():
+    ld = LaunchDescription()
+
+    namespace_arg = DeclareLaunchArgument(
+        "namespace", default_value="", description="ROS2 namespace"
+    )
+    node_name_arg = DeclareLaunchArgument(
+        "node_name", default_value="ros_trick_bridge", description="Node name"
+    )
+    autostart_arg = DeclareLaunchArgument(
+        "autostart",
+        default_value="True",
+        description="The bridge is a lifecycle node. Set this argument to true to transit to active on startup (start the simulation)",
+    )
+    param_file_arg = DeclareLaunchArgument(
+        "param_file",
+        default_value=[
+            FindPackageShare("ros_trick_bridge"),
+            "/params/ros_trick_bridge_example_params.yaml",
+        ],
+        description="ROS Trick Bridge config",
+    )
+
+    ros_trick_bridge_lifecycle_node = LifecycleNode(
+        package="ros_trick_bridge",
+        executable="ros_trick_bridge_node",
+        name=LaunchConfiguration("node_name"),
+        namespace=LaunchConfiguration("namespace"),
+        parameters=[LaunchConfiguration("param_file")],
+        output="both",
+        emulate_tty=True,
+    )
+
+    ros_trick_emit_configure_event = EmitEvent(
+        event=ChangeState(
+            lifecycle_node_matcher=matches_action(ros_trick_bridge_lifecycle_node),
+            transition_id=Transition.TRANSITION_CONFIGURE,
+        ),
+        condition=IfCondition(PythonExpression([LaunchConfiguration("autostart")])),
+    )
+    ros_trick_emit_activate_event = EmitEvent(
+        event=ChangeState(
+            lifecycle_node_matcher=matches_action(ros_trick_bridge_lifecycle_node),
+            transition_id=Transition.TRANSITION_ACTIVATE,
+        ),
+        condition=IfCondition(PythonExpression([LaunchConfiguration("autostart")])),
+    )
+    ros_trick_inactive_state_handler = RegisterEventHandler(
+        OnStateTransition(
+            target_lifecycle_node=ros_trick_bridge_lifecycle_node,
+            goal_state="inactive",
+            entities=[
+                ros_trick_emit_activate_event,
+            ],
+        ),
+        condition=IfCondition(PythonExpression([LaunchConfiguration("autostart")])),
+    )
+
+    # Add logging for debugging
+    ld.add_action(LogInfo(msg="Launching ROS Trick Bridge Lifecycle Node"))
+    ld.add_action(namespace_arg)
+    ld.add_action(node_name_arg)
+    ld.add_action(autostart_arg)
+    ld.add_action(param_file_arg)
+    ld.add_action(ros_trick_bridge_lifecycle_node)
+    ld.add_action(ros_trick_inactive_state_handler)
+    ld.add_action(ros_trick_emit_configure_event)
+    ld.add_action(LogInfo(msg="Emitting configure event"))
+
+    # Add event handlers for logging state transitions
+    ld.add_action(
+        RegisterEventHandler(
+            OnStateTransition(
+                target_lifecycle_node=ros_trick_bridge_lifecycle_node,
+                start_state="unconfigured",
+                goal_state="configuring",
+                entities=[
+                    LogInfo(msg="Transitioning from 'unconfigured' to 'configuring'")
+                ],
+            )
+        )
+    )
+
+    ld.add_action(
+        RegisterEventHandler(
+            OnStateTransition(
+                target_lifecycle_node=ros_trick_bridge_lifecycle_node,
+                start_state="configuring",
+                goal_state="inactive",
+                entities=[
+                    LogInfo(msg="Transitioning from 'configuring' to 'inactive'")
+                ],
+            )
+        )
+    )
+
+    ld.add_action(
+        RegisterEventHandler(
+            OnStateTransition(
+                target_lifecycle_node=ros_trick_bridge_lifecycle_node,
+                start_state="inactive",
+                goal_state="activating",
+                entities=[LogInfo(msg="Transitioning from 'inactive' to 'activating'")],
+            )
+        )
+    )
+
+    ld.add_action(
+        RegisterEventHandler(
+            OnStateTransition(
+                target_lifecycle_node=ros_trick_bridge_lifecycle_node,
+                start_state="activating",
+                goal_state="active",
+                entities=[LogInfo(msg="Transitioning from 'activating' to 'active'")],
+            )
+        )
+    )
+
+    return ld
diff --git a/ros_trick/ros_src/ros_trick_bridge/package.xml b/ros_trick/ros_src/ros_trick_bridge/package.xml
new file mode 100644
index 00000000..389ce4f2
--- /dev/null
+++ b/ros_trick/ros_src/ros_trick_bridge/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>ros_trick_bridge</name>
+  <version>0.0.0</version>
+  <description>Bridge between ROS and NASA Trick simulator</description>
+  <maintainer email="fiderekblazej@gmail.com">Blazej Fiderek (xfiderek)</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/ros_trick/ros_src/ros_trick_bridge/params/ros_trick_bridge_example_params.yaml b/ros_trick/ros_src/ros_trick_bridge/params/ros_trick_bridge_example_params.yaml
new file mode 100644
index 00000000..b7cfcbaa
--- /dev/null
+++ b/ros_trick/ros_src/ros_trick_bridge/params/ros_trick_bridge_example_params.yaml
@@ -0,0 +1,8 @@
+ros_trick_bridge:
+  ros__parameters:
+    sim_directory: /opt/trick_sims/SIM_trick_canadarm
+    sim_inputfile: RUN_2DPlanar/input.py
+    sim_executable: S_main_Linux_11.4_x86_64.exe
+    sim_args: ''
+    publish_clock: true
+    clock_publish_period: 0.02
diff --git a/ros_trick/ros_src/ros_trick_bridge/resource/ros_trick_bridge b/ros_trick/ros_src/ros_trick_bridge/resource/ros_trick_bridge
new file mode 100644
index 00000000..c21c341a
--- /dev/null
+++ b/ros_trick/ros_src/ros_trick_bridge/resource/ros_trick_bridge
@@ -0,0 +1,14 @@
+// Copyright 2024 Blazej Fiderek (xfiderek)
+// 
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+// 
+//     https://www.apache.org/licenses/LICENSE-2.0
+// 
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
diff --git a/ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/__init__.py b/ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/__init__.py
new file mode 100644
index 00000000..be353d1a
--- /dev/null
+++ b/ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/__init__.py
@@ -0,0 +1,13 @@
+# Copyright 2024 Blazej Fiderek (xfiderek)
+# 
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+# 
+#     https://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
diff --git a/ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/ros_trick_bridge_node.py b/ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/ros_trick_bridge_node.py
new file mode 100644
index 00000000..7c6b921c
--- /dev/null
+++ b/ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/ros_trick_bridge_node.py
@@ -0,0 +1,198 @@
+# Copyright 2024 Blazej Fiderek (xfiderek)
+# 
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+# 
+#     https://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+import os
+import subprocess
+import threading
+import time
+
+import rclpy
+from builtin_interfaces.msg import Time
+from rclpy.executors import SingleThreadedExecutor
+from rclpy.lifecycle import Node, State, TransitionCallbackReturn
+from ros_trick_bridge.trick_variable_server_client import TrickVariableServerClient
+from rosgraph_msgs.msg import Clock
+
+
+class RosTrickBridgeNode(Node):
+    TRICK_TICK_VALUE_VAR_NAME = "trick_sys.sched.time_tic_value"
+    TRICK_TIME_TICS_VAR_NAME = "trick_sys.sched.time_tics"
+
+    def __init__(self, node_name: str, **kwargs):
+        self._trick_subprocess = None
+        self._std_print_period = 0.1
+        self._stdout_print_thread = None
+
+        self._trick_variable_server_client = None
+        self._clock_publisher = None
+
+        super().__init__(node_name, **kwargs)
+
+    # def on_init(self, state: State)  -> TransitionCallbackReturn:
+    #     self.get_logger().info('nddd')
+
+    def on_configure(self, state: State) -> TransitionCallbackReturn:
+        # Declare ROS 2 parameters
+        self.declare_parameter("sim_directory", "/path/to/sim")
+        self.declare_parameter("sim_executable", "S_main_Linux_11.4_x86_64.exe")
+        self.declare_parameter("sim_inputfile", "RUN_2DPlanar/input.py")
+        self.declare_parameter("sim_args", "")
+        self.declare_parameter("trick_host", "localhost")
+        self.declare_parameter("trick_port", 49765)
+        self.declare_parameter("publish_clock", True)
+        self.declare_parameter("clock_publish_period", 0.1)
+
+        self.get_logger().info(f"Configuring from state: {state.label}")
+        if not "TRICK_HOME" in os.environ:
+            error = "TRICK_HOME env variable is not set, terminating"
+            self.get_logger().error(error)
+            raise Exception(error)
+
+        # Get the parameters
+        sim_directory = (
+            self.get_parameter("sim_directory").get_parameter_value().string_value + "/"
+        )
+        sim_executable = (
+            self.get_parameter("sim_executable").get_parameter_value().string_value
+        )
+        sim_inputfile = (
+            self.get_parameter("sim_inputfile").get_parameter_value().string_value
+        )
+        sim_args = self.get_parameter("sim_args").get_parameter_value().string_value
+        try:
+            cmd = [f"./{sim_executable}", sim_inputfile, sim_args]
+            self.get_logger().info(f"Starting trick with command: {cmd}")
+            self._trick_subprocess = subprocess.Popen(
+                cmd,
+                stdout=subprocess.PIPE,
+                stderr=subprocess.PIPE,
+                text=True,
+                cwd=sim_directory,
+            )
+            self.get_logger().info("######################################")
+            self.get_logger().info("Trick subprocess started successfully.")
+            self.get_logger().info("######################################")
+            self.get_logger().info(f"Trick pid is {self._trick_subprocess.pid}")
+        except Exception as e:
+            self.get_logger().error("######################################")
+            self.get_logger().error(f"Failed to start trick subprocess: {e}")
+            self.get_logger().error("######################################")
+            return TransitionCallbackReturn.FAILURE
+
+        # it must run on a separate thread to avoid IO blocking
+        self._stdout_print_thread = threading.Thread(
+            target=self._print_subprocess_stdout
+        )
+        self._stdout_print_thread.start()
+
+        # give a process some time to spin up
+        time.sleep(1.0)
+        return TransitionCallbackReturn.SUCCESS
+
+    def on_activate(self, state: State) -> TransitionCallbackReturn:
+        if self.get_parameter("publish_clock").get_parameter_value().bool_value:
+            trick_data_callback = self._publish_clock_from_trick
+            self._clock_publisher = self.create_publisher(
+                msg_type=Clock, topic="/clock", qos_profile=10
+            )
+        else:
+            # we don't need any data from trick otherwise
+            trick_data_callback = None
+
+        try:
+            self._trick_variable_server_client = TrickVariableServerClient(
+                self.get_parameter("trick_host").get_parameter_value().string_value,
+                self.get_parameter("trick_port").get_parameter_value().integer_value,
+                "ros_trick_bridge",
+                [self.TRICK_TICK_VALUE_VAR_NAME, self.TRICK_TIME_TICS_VAR_NAME],
+                self.get_parameter("clock_publish_period")
+                .get_parameter_value()
+                .double_value,
+                trick_data_callback,
+            )
+        except Exception as e:
+            self.get_logger().error(
+                f"Failed to start trick variable server client: {e}"
+            )
+            return TransitionCallbackReturn.FAILURE
+
+        self.get_logger().info(f"Activating from state: {state.label}")
+        self._trick_variable_server_client.unfreeze_sim()
+        return super().on_activate(state)
+
+    def on_deactivate(self, state: State) -> TransitionCallbackReturn:
+        self.get_logger().info(f"Deactivating from state: {state.label}")
+        self._trick_variable_server_client.freeze_sim()
+        return super().on_deactivate(state)
+
+    def on_cleanup(self, state: State) -> TransitionCallbackReturn:
+        self.get_logger().info(f"Cleaning up from state: {state.label}")
+        self._teardown_sim()
+
+        return TransitionCallbackReturn.SUCCESS
+
+    def on_shutdown(self, state: State) -> TransitionCallbackReturn:
+        self.get_logger().info(f"Shutting down from state: {state.label}")
+        self._teardown_sim()
+
+        return TransitionCallbackReturn.SUCCESS
+
+    def on_error(self, state: State) -> TransitionCallbackReturn:
+        self.get_logger().info(f"Error from state: {state.label}")
+        return TransitionCallbackReturn.SUCCESS
+
+    def _teardown_sim(self):
+        self._trick_variable_server_client = None
+
+        # Terminate the subprocess if it's still running
+        if self._trick_subprocess and self._trick_subprocess.poll() is None:
+            self._trick_subprocess.terminate()
+
+    def _publish_clock_from_trick(self, trick_data):
+        ticks_per_second = int(trick_data[self.TRICK_TICK_VALUE_VAR_NAME])
+        curr_time_tics = int(trick_data[self.TRICK_TIME_TICS_VAR_NAME])
+        seconds = curr_time_tics // ticks_per_second
+        nanoseconds = (curr_time_tics % ticks_per_second) * (
+            1000000000 // ticks_per_second
+        )
+        clock_msg = Clock()
+        clock_msg.clock = Time()
+        clock_msg.clock.sec = seconds
+        clock_msg.clock.nanosec = nanoseconds
+        self._clock_publisher.publish(clock_msg)
+
+    def _print_subprocess_stdout(self):
+        while True:
+            if self._trick_subprocess:
+                # Read from stdout
+                stdout_line = self._trick_subprocess.stdout.readline()
+                if stdout_line:
+                    self.get_logger().info(stdout_line)
+            time.sleep(self._std_print_period)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    executor = SingleThreadedExecutor()
+    lifecycle_node = RosTrickBridgeNode("ros_trick_bridge")
+    executor.add_node(lifecycle_node)
+    try:
+        executor.spin()
+    except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
+        lifecycle_node._teardown_sim()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/trick_variable_server_client.py b/ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/trick_variable_server_client.py
new file mode 100644
index 00000000..b5ad418a
--- /dev/null
+++ b/ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/trick_variable_server_client.py
@@ -0,0 +1,85 @@
+# Copyright 2024 Blazej Fiderek (xfiderek)
+# 
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+# 
+#     https://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+import socket
+import threading
+import time
+
+
+class TrickVariableServerClient:
+    def __init__(
+        self,
+        host,
+        port,
+        client_tag,
+        trick_variable_names,
+        trick_var_cycle_time=0.1,
+        on_receive_callback=None,
+    ):
+        self._client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self._client_socket.connect((host, port))
+
+        self._insock = self._client_socket.makefile("r")
+
+        self._variable_names = trick_variable_names
+        self._latest_data = {}
+        self._on_receive_callback = on_receive_callback
+        self._trick_var_cycle_time = trick_var_cycle_time
+
+        # send configuration info to trick and ask for variables
+        self._client_socket.send(
+            f'trick.var_set_client_tag("{client_tag}") \n'.encode()
+        )
+        self._client_socket.send(
+            f"trick.var_cycle({str(trick_var_cycle_time)}) \n".encode()
+        )
+
+        self._receive_thread = threading.Thread(target=self._rcv_trick_data_from_socket)
+        self._receive_thread.start()
+
+    def freeze_sim(self):
+        self._client_socket.send("trick.exec_freeze()\n".encode())
+        self.stop_listening()
+
+    def unfreeze_sim(self):
+        self.start_listening()
+        self._client_socket.send("trick.exec_run()\n".encode())
+
+    def start_listening(self):
+        command = b""
+        for trick_var_name in self._variable_names:
+            command += f'trick.var_add("{trick_var_name}") \n'.encode()
+        self._client_socket.send("trick.var_ascii()\n".encode())
+        self._client_socket.send(command)
+
+    def stop_listening(self):
+        self._client_socket.send("trick.var_clear()\n".encode())
+
+    def send_data_to_sim(self, var_name: str, value: str) -> None:
+        self._client_socket.send(f"{var_name} = {value} \n".encode())
+
+    def _rcv_trick_data_from_socket(self):
+        while True:
+            if self._insock:
+                line = self._insock.readline()
+                if line:
+                    variables = line.split("\t")
+                    if variables[0] == "0":
+                        for i, var_name in enumerate(self._variable_names):
+                            self._latest_data[var_name] = variables[i + 1].strip()
+                        if self._on_receive_callback:
+                            self._on_receive_callback(self._latest_data.copy())
+            else:
+                time.sleep(self._trick_var_cycle_time)
diff --git a/ros_trick/ros_src/ros_trick_bridge/setup.cfg b/ros_trick/ros_src/ros_trick_bridge/setup.cfg
new file mode 100644
index 00000000..19ca6234
--- /dev/null
+++ b/ros_trick/ros_src/ros_trick_bridge/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/ros_trick_bridge
+[install]
+install_scripts=$base/lib/ros_trick_bridge
diff --git a/ros_trick/ros_src/ros_trick_bridge/setup.py b/ros_trick/ros_src/ros_trick_bridge/setup.py
new file mode 100644
index 00000000..f65525fa
--- /dev/null
+++ b/ros_trick/ros_src/ros_trick_bridge/setup.py
@@ -0,0 +1,30 @@
+import os
+from glob import glob
+
+from setuptools import find_packages, setup
+
+package_name = "ros_trick_bridge"
+
+setup(
+    name=package_name,
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        (os.path.join("share", package_name, "launch"), glob("launch/*.py")),
+        (os.path.join("share", package_name, "params"), glob("params/*")),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="Blazej Fiderek (xfiderek)",
+    maintainer_email="fiderekblazej@gmail.com",
+    description="",
+    license="Apache-2.0",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "ros_trick_bridge_node = ros_trick_bridge.ros_trick_bridge_node:main"
+        ],
+    },
+)
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/.setup_assistant b/ros_trick/ros_src/trick_canadarm_moveit_config/.setup_assistant
new file mode 100644
index 00000000..23da2f9a
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/.setup_assistant
@@ -0,0 +1,27 @@
+moveit_setup_assistant_config:
+  urdf:
+    package: simulation
+    relative_path: models/canadarm/urdf/SSRMS_Canadarm2.urdf
+  srdf:
+    relative_path: config/SSRMS_Canadarm2.srdf
+  package_settings:
+    author_name: Dharini Dutia
+    author_email: dharini@openrobotics.org
+    generated_timestamp: 1672757808
+  control_xacro:
+    command:
+      - position
+      - velocity
+    state:
+      - position
+      - velocity
+  modified_urdf:
+    xacros:
+      - control_xacro
+  control_xacro:
+    command:
+      - position
+      - velocity
+    state:
+      - position
+      - velocity
\ No newline at end of file
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/CMakeLists.txt b/ros_trick/ros_src/trick_canadarm_moveit_config/CMakeLists.txt
new file mode 100644
index 00000000..29f23015
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/CMakeLists.txt
@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.22)
+project(trick_canadarm_moveit_config)
+
+find_package(ament_cmake REQUIRED)
+
+ament_package()
+
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}
+  PATTERN "setup_assistant.launch" EXCLUDE)
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
+install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME})
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/config/SSRMS_Canadarm2.ros2_control.xacro b/ros_trick/ros_src/trick_canadarm_moveit_config/config/SSRMS_Canadarm2.ros2_control.xacro
new file mode 100644
index 00000000..8b5dacac
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/config/SSRMS_Canadarm2.ros2_control.xacro
@@ -0,0 +1,90 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="SSRMS_Canadarm2_ros2_control" params="name">
+        <ros2_control name="${name}" type="system">
+            <hardware>
+                <plugin>trick_ros2_control/TrickSystem</plugin>
+                <param name="trick_variable_server_host">localhost</param>
+                <param name="trick_variable_server_port">49765</param>
+                <param name="trick_variable_cycle_rate">100.0</param>
+            </hardware>
+
+            <joint name="Base_Joint">
+                <state_interface name="position"/>
+                <state_interface name="velocity"/>
+                <state_interface name="acceleration"/>
+                <command_interface name="effort"/>
+                <param name="state_interface/position/trick_variable_name">CanadarmManip.manip.q[0]</param>
+                <param name="state_interface/velocity/trick_variable_name">CanadarmManip.manip.q_dot[0]</param>
+                <param name="state_interface/acceleration/trick_variable_name">CanadarmManip.manip.q_dotdot[0]</param>
+                <param name="command_interface/effort/trick_variable_name">CanadarmManip.manip.input_torque[0]</param>
+            </joint>
+
+            <joint name="Shoulder_Roll">
+                <state_interface name="position"/>
+                <state_interface name="velocity"/>
+                <state_interface name="acceleration"/>
+                <command_interface name="effort"/>
+                <param name="state_interface/position/trick_variable_name">CanadarmManip.manip.q[1]</param>
+                <param name="state_interface/velocity/trick_variable_name">CanadarmManip.manip.q_dot[1]</param>
+                <param name="state_interface/acceleration/trick_variable_name">CanadarmManip.manip.q_dotdot[1]</param>
+                <param name="command_interface/effort/trick_variable_name">CanadarmManip.manip.input_torque[1]</param>
+            </joint>
+
+            <joint name="Shoulder_Yaw">
+                <state_interface name="position"/>
+                <state_interface name="velocity"/>
+                <state_interface name="acceleration"/>
+                <command_interface name="effort"/>
+                <param name="state_interface/position/trick_variable_name">CanadarmManip.manip.q[2]</param>
+                <param name="state_interface/velocity/trick_variable_name">CanadarmManip.manip.q_dot[2]</param>
+                <param name="state_interface/acceleration/trick_variable_name">CanadarmManip.manip.q_dotdot[2]</param>
+                <param name="command_interface/effort/trick_variable_name">CanadarmManip.manip.input_torque[2]</param>
+            </joint>
+
+            <joint name="Elbow_Pitch">
+                <state_interface name="position"/>
+                <state_interface name="velocity"/>
+                <state_interface name="acceleration"/>
+                <command_interface name="effort"/>
+                <param name="state_interface/position/trick_variable_name">CanadarmManip.manip.q[3]</param>
+                <param name="state_interface/velocity/trick_variable_name">CanadarmManip.manip.q_dot[3]</param>
+                <param name="state_interface/acceleration/trick_variable_name">CanadarmManip.manip.q_dotdot[3]</param>
+                <param name="command_interface/effort/trick_variable_name">CanadarmManip.manip.input_torque[3]</param>
+            </joint>
+
+            <joint name="Wrist_Pitch">
+              <state_interface name="position"/>
+              <state_interface name="velocity"/>
+              <state_interface name="acceleration"/>
+              <command_interface name="effort"/>
+              <param name="state_interface/position/trick_variable_name">CanadarmManip.manip.q[4]</param>
+              <param name="state_interface/velocity/trick_variable_name">CanadarmManip.manip.q_dot[4]</param>
+              <param name="state_interface/acceleration/trick_variable_name">CanadarmManip.manip.q_dotdot[4]</param>
+              <param name="command_interface/effort/trick_variable_name">CanadarmManip.manip.input_torque[4]</param>
+            </joint>
+
+            <joint name="Wrist_Yaw">
+                <state_interface name="position"/>
+                <state_interface name="velocity"/>
+                <state_interface name="acceleration"/>
+                <command_interface name="effort"/>
+                <param name="state_interface/position/trick_variable_name">CanadarmManip.manip.q[5]</param>
+                <param name="state_interface/velocity/trick_variable_name">CanadarmManip.manip.q_dot[5]</param>
+                <param name="state_interface/acceleration/trick_variable_name">CanadarmManip.manip.q_dotdot[5]</param>
+                <param name="command_interface/effort/trick_variable_name">CanadarmManip.manip.input_torque[5]</param>
+            </joint>
+
+            <joint name="Wrist_Roll">
+                <state_interface name="position"/>
+                <state_interface name="velocity"/>
+                <state_interface name="acceleration"/>
+                <command_interface name="effort"/>
+                <param name="state_interface/position/trick_variable_name">CanadarmManip.manip.q[6]</param>
+                <param name="state_interface/velocity/trick_variable_name">CanadarmManip.manip.q_dot[6]</param>
+                <param name="state_interface/acceleration/trick_variable_name">CanadarmManip.manip.q_dotdot[6]</param>
+                <param name="command_interface/effort/trick_variable_name">CanadarmManip.manip.input_torque[6]</param>
+            </joint>
+        </ros2_control>
+    </xacro:macro>
+</robot>
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/config/SSRMS_Canadarm2.srdf b/ros_trick/ros_src/trick_canadarm_moveit_config/config/SSRMS_Canadarm2.srdf
new file mode 100644
index 00000000..3b6cf266
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/config/SSRMS_Canadarm2.srdf
@@ -0,0 +1,56 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot name="SSRMS_Canadarm2">
+    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+    <group name="canadarm">
+        <link name="world"/>
+        <link name="Base_SSRMS"/>
+        <link name="B1"/>
+        <link name="B2"/>
+        <link name="B3"/>
+        <link name="B4"/>
+        <link name="B5"/>
+        <link name="B6"/>
+        <link name="EE_SSRMS"/>
+        <joint name="world_joint"/>
+        <joint name="world_joint"/>
+        <joint name="Base_Joint"/>
+        <joint name="Shoulder_Roll"/>
+        <joint name="Shoulder_Yaw"/>
+        <joint name="Elbow_Pitch"/>
+        <joint name="Wrist_Pitch"/>
+        <joint name="Wrist_Yaw"/>
+        <joint name="Wrist_Roll"/>
+    </group>
+    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+    <group_state name="home" group="canadarm">
+        <joint name="Base_Joint" value="0"/>
+        <joint name="Elbow_Pitch" value="0"/>
+        <joint name="Shoulder_Roll" value="0"/>
+        <joint name="Shoulder_Yaw" value="0"/>
+        <joint name="Wrist_Pitch" value="0"/>
+        <joint name="Wrist_Roll" value="0"/>
+        <joint name="Wrist_Yaw" value="0"/>
+    </group_state>
+    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
+    <end_effector name="end_effector" parent_link="EE_SSRMS" group="canadarm"/>
+    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
+    <virtual_joint name="world_joint" type="fixed" parent_frame="world" child_link="Base_SSRMS"/>
+    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+    <disable_collisions link1="B1" link2="B2" reason="User"/>
+    <disable_collisions link1="B1" link2="Base_SSRMS" reason="User"/>
+    <disable_collisions link1="B1" link2="EE_SSRMS" reason="User"/>
+    <disable_collisions link1="B2" link2="B3" reason="User"/>
+    <disable_collisions link1="B3" link2="B4" reason="User"/>
+    <disable_collisions link1="B4" link2="B5" reason="User"/>
+    <disable_collisions link1="B5" link2="B6" reason="User"/>
+    <disable_collisions link1="B6" link2="EE_SSRMS" reason="User"/>
+    <disable_collisions link1="Base_SSRMS" link2="EE_SSRMS" reason="User"/>
+</robot>
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/config/SSRMS_Canadarm2.urdf.xacro b/ros_trick/ros_src/trick_canadarm_moveit_config/config/SSRMS_Canadarm2.urdf.xacro
new file mode 100644
index 00000000..a1ffd28c
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/config/SSRMS_Canadarm2.urdf.xacro
@@ -0,0 +1,295 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="SSRMS_Canadarm2">
+  <xacro:include filename="$(find trick_canadarm_moveit_config)/config/SSRMS_Canadarm2.ros2_control.xacro" />
+  <xacro:SSRMS_Canadarm2_ros2_control name="TrickSystem" />
+
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="Base_SSRMS"/>
+    <!-- Rotate Frame to Match the frames on Fig. 13 in Paper -->
+    <origin rpy="3.1416 0.0 0.0" xyz="0.5 0.0 1.5"/>
+  </joint>
+  <!-- SSRMS_Canadarm2 Dummy Base Body-->
+  <link name="Base_SSRMS">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.5"/>
+      <mass value="243.66"/>
+      <inertia ixx="44.413" ixy="0" ixz="0" iyy="44.413" iyz="0" izz="9.336"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/ee.dae" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/ee.stl" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Dummy Joint Joining Base to Body B1 -->
+  <joint name="Base_Joint" type="revolute">
+    <parent link="Base_SSRMS"/>
+    <child link="B1"/>
+    <!-- Rotate Frame to Match the frames on Fig. 13 in Paper -->
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2332" lower="-4.71239" upper="4.71239" velocity="0.0698132"/>
+  </joint>
+  <!--Manipulator-->
+  <!-- Link 1 -->
+  <link name="B1">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0 -0.175"/>
+      <mass value="105.98"/>
+      <inertia ixx="12.19" ixy="0" ixz="0" iyy="12.19" iyz="0" izz="3.061"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -1.5708 0" xyz="0.25082 0 -0.175"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/joint_v3_0.dae" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 -1.5708 0" xyz="0.25082 0 -0.175"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/joint_v3_0.stl" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Joint 1: Shoulder Roll Joint -->
+  <joint name="Shoulder_Roll" type="revolute">
+    <parent link="B1"/>
+    <child link="B2"/>
+    <origin rpy="0 0 0" xyz="0.25082 0 -0.175"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="2332" lower="-4.71239" upper="4.71239" velocity="0.0698132"/>
+  </joint>
+  <!-- Link 2 -->
+  <link name="B2">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.175 0 0"/>
+      <mass value="105.98"/>
+      <inertia ixx="3.061" ixy="0" ixz="0" iyy="12.13" iyz="0" izz="12.13"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.175 0 -0.25082"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/joint_v3_0.dae" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.175 0 -0.25082"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/joint_v3_0.stl" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Joint 2: Shoulder Yaw Joint -->
+  <joint name="Shoulder_Yaw" type="revolute">
+    <parent link="B2"/>
+    <child link="B3"/>
+    <origin rpy="0 0 0" xyz="0.175 0 -0.25082"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2332" lower="-4.71239" upper="4.71239" velocity="0.0698132"/>
+  </joint>
+  <!-- Link 3 -->
+  <link name="B3">
+    <inertial>
+      <origin rpy="0 0 0" xyz="4.0 0 -0.175"/>
+      <mass value="314.88"/>
+      <inertia ixx="15.41" ixy="0" ixz="0" iyy="2094.71" iyz="0" izz="2103.19"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -1.5708 0" xyz="0.25082 0 -0.175"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/joint_v3_0.dae" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 -1.5708 0" xyz="0.25082 0 -0.175"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/joint_v3_0.stl" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin rpy="-1.5708 0 -1.5708" xyz="0.25082 0 -0.175"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/long_link_1_v3_0.dae" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="-1.5708 0 -1.5708" xyz="0.25082 0 -0.175"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/long_link_1_v3_0.stl" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin rpy="0 1.5708 0" xyz="7.36082 0 -0.175"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/joint_v3_0.dae" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5708 0" xyz="7.36082 0 -0.175"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/joint_v3_0.stl" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin rpy="0 0 1.5708" xyz="7.61164 0 -0.35"/>
+      <geometry>
+        <cylinder length="0.5" radius="0.165"/>
+      </geometry>
+    </visual>
+  </link>
+  <!-- Joint 4: Elbow Pitch Joint -->
+  <joint name="Elbow_Pitch" type="revolute">
+    <parent link="B3"/>
+    <child link="B4"/>
+    <origin rpy="0 0 0" xyz="7.61164 0 -0.6"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2332" lower="-4.71239" upper="4.71239" velocity="0.0698132"/>
+  </joint>
+  <!-- Link 5 -->
+  <link name="B4">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-3.6 0 -0.175"/>
+      <mass value="279.2"/>
+      <inertia ixx="9.522" ixy="0" ixz="0" iyy="1966.28" iyz="0" izz="1966.28"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.5708 0" xyz="-0.25082 0 -0.175"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/link_joint_v2_1.dae" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5708 0" xyz="-0.25082 0 -0.175"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/link_joint_v2_1.stl" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin rpy="1.57 -3.14 1.57" xyz="-7.36082 0 -0.175"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/long_link_2_v3_0.dae" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.57 -3.14 1.57" xyz="-7.36082 0 -0.175"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/long_link_2_v3_0.stl" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin rpy="0 -1.57 0" xyz="-7.36082 0 -0.175"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/joint_v3_0.dae" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 -1.57 0" xyz="-7.36082 0 -0.175"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/joint_v3_0.stl" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Joint 5: Wrist Pitch Joint -->
+  <joint name="Wrist_Pitch" type="revolute">
+    <parent link="B4"/>
+    <child link="B5"/>
+    <origin rpy="0 0 0" xyz="-7.61164 0 -0.35"/>
+    <axis xyz="0 0 -1"/>
+    <limit effort="2332" lower="-4.71239" upper="4.71239" velocity="0.0698132"/>
+  </joint>
+  <!-- Link 6 -->
+  <link name="B5">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="105.98"/>
+      <inertia ixx="8.0386" ixy="0" ixz="0" iyy="8.305" iyz="0" izz="3.061"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -3.14 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/joint_v3_0.dae" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 -3.14 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/joint_v3_0.stl" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Joint 6: Wrist Yaw Joint -->
+  <joint name="Wrist_Yaw" type="revolute">
+    <parent link="B5"/>
+    <child link="B6"/>
+    <origin rpy="0 0 0" xyz="-0.175 0 -0.25082"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="2332" lower="-4.71239" upper="4.71239" velocity="0.0698132"/>
+  </joint>
+  <!-- Link 7 -->
+  <link name="B6">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="105.98"/>
+      <inertia ixx="3.061" ixy="0" ixz="0" iyy="12.13" iyz="0" izz="12.13"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -1.5708 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/joint_v3_0.dae" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 -1.5708 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/joint_v3_0.stl" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Joint 7: Wrist Roll Joint -->
+  <joint name="Wrist_Roll" type="revolute">
+    <parent link="B6"/>
+    <child link="EE_SSRMS"/>
+    <origin rpy="0 0 0" xyz="-0.25082 0 -0.175"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2332" lower="-4.71239" upper="4.71239" velocity="0.0698132"/>
+  </joint>
+  <!-- Link 8 -->
+  <link name="EE_SSRMS">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 -0.5"/>
+      <mass value="243.66"/>
+      <inertia ixx="44.41" ixy="0" ixz="0" iyy="44.41" iyz="0" izz="9.336"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -3.1415 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/ee.dae" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 -3.1415 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://simulation/models/canadarm/meshes/ee.stl" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Import SSRMS_Canadarm2 urdf file -->
+
+  
+</robot>
+<!-- URDF Created from information in the following Paper:
+
+Nguyen, Phung Khac, and Peter C. Hughes. "Teleoperation: from the space shuttle to the space station." Progress in astronautics and aeronautics 161 (1994): 353-353.
+
+The link CoM locations (with respect to link frame) are set to zero as the inertia tensor is defined with respect to the link frame in the paper.
+
+ -->
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/config/initial_positions.yaml b/ros_trick/ros_src/trick_canadarm_moveit_config/config/initial_positions.yaml
new file mode 100644
index 00000000..a60f036d
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/config/initial_positions.yaml
@@ -0,0 +1,8 @@
+initial_positions:
+  Base_Joint: 0
+  Elbow_Pitch: 0
+  Shoulder_Roll: 0
+  Shoulder_Yaw: 0
+  Wrist_Pitch: 0
+  Wrist_Roll: 0
+  Wrist_Yaw: 0
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/config/joint_limits.yaml b/ros_trick/ros_src/trick_canadarm_moveit_config/config/joint_limits.yaml
new file mode 100644
index 00000000..276f5eb3
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/config/joint_limits.yaml
@@ -0,0 +1,45 @@
+# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+
+# For beginners, we downscale velocity and acceleration limits.
+# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
+default_velocity_scaling_factor: 0.1
+default_acceleration_scaling_factor: 0.1
+
+# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+joint_limits:
+  Base_Joint:
+    has_velocity_limits: true
+    max_velocity: 0.1
+    has_acceleration_limits: true
+    max_acceleration: 0.025
+  Elbow_Pitch:
+    has_velocity_limits: true
+    max_velocity: 0.1
+    has_acceleration_limits: true
+    max_acceleration: 0.025
+  Shoulder_Roll:
+    has_velocity_limits: true
+    max_velocity: 0.1
+    has_acceleration_limits: true
+    max_acceleration: 0.025
+  Shoulder_Yaw:
+    has_velocity_limits: true
+    max_velocity: 0.1
+    has_acceleration_limits: true
+    max_acceleration: 0.025
+  Wrist_Pitch:
+    has_velocity_limits: true
+    max_velocity: 0.1
+    has_acceleration_limits: true
+    max_acceleration: 0.025
+  Wrist_Roll:
+    has_velocity_limits: true
+    max_velocity: 0.1
+    has_acceleration_limits: true
+    max_acceleration: 0.025
+  Wrist_Yaw:
+    has_velocity_limits: true
+    max_velocity: 0.1
+    has_acceleration_limits: true
+    max_acceleration: 0.025
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/config/kinematics.yaml b/ros_trick/ros_src/trick_canadarm_moveit_config/config/kinematics.yaml
new file mode 100644
index 00000000..f54b70cd
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/config/kinematics.yaml
@@ -0,0 +1,8 @@
+canadarm:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.05
+canadarm_planning_group:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.005
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/config/moveit.rviz b/ros_trick/ros_src/trick_canadarm_moveit_config/config/moveit.rviz
new file mode 100644
index 00000000..c4d16195
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/config/moveit.rviz
@@ -0,0 +1,448 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Wrench1
+        - /Wrench2
+        - /Wrench3
+        - /Wrench4
+        - /Wrench5
+        - /Wrench6
+        - /Wrench7
+        - /TF1
+        - /MotionPlanning1
+        - /MotionPlanning1/Scene Robot1
+        - /MotionPlanning1/Planning Request1
+        - /MotionPlanning1/Planned Path1
+      Splitter Ratio: 0.5
+    Tree Height: 165
+  - Class: rviz_common/Help
+    Name: Help
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    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
+    - Acceleration_Scaling_Factor: 0.1
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: true
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: false
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
+      Name: MotionPlanning
+      Planned Path:
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          B1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          B2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          B3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          B4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          B5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          B6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          Base_SSRMS:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          EE_SSRMS:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Loop Animation: false
+        Robot Alpha: 0.30000001192092896
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
+        State Display Time: 0.05 s
+        Trail Step Size: 1
+        Trajectory Topic: display_planned_path
+        Use Sim Time: false
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.07999999821186066
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 0.25
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 1.5
+        Joint Violation Color: 255; 0; 255
+        Planning Group: canadarm
+        Query Goal State: true
+        Query Start State: false
+        Show Workspace: false
+        Start State Alpha: 0
+        Start State Color: 0; 255; 0
+      Planning Scene Topic: monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 1
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          B1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          B2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          B3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          B4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          B5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          B6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          Base_SSRMS:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          EE_SSRMS:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+      Velocity_Scaling_Factor: 0.1
+    - Accept NaN Values: false
+      Alpha: 1
+      Arrow Width: 0.4000000059604645
+      Class: rviz_default_plugins/Wrench
+      Enabled: true
+      Force Arrow Scale: 2
+      Force Color: 204; 51; 51
+      History Length: 1
+      Name: Wrench
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /wrench_topic/B1/total_torque
+      Torque Arrow Scale: 0.029999999329447746
+      Torque Color: 255; 0; 255
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        B1:
+          Value: true
+        B2:
+          Value: true
+        B3:
+          Value: true
+        B4:
+          Value: true
+        B5:
+          Value: true
+        B6:
+          Value: true
+        Base_SSRMS:
+          Value: true
+        EE_SSRMS:
+          Value: true
+        world:
+          Value: true
+      Marker Scale: 4
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        world:
+          Base_SSRMS:
+            B1:
+              B2:
+                B3:
+                  B4:
+                    B5:
+                      B6:
+                        EE_SSRMS:
+                          {}
+      Update Interval: 0
+      Value: true
+    - Accept NaN Values: false
+      Alpha: 1
+      Arrow Width: 0.4000000059604645
+      Class: rviz_default_plugins/Wrench
+      Enabled: true
+      Force Arrow Scale: 2
+      Force Color: 204; 51; 51
+      History Length: 1
+      Name: Wrench
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /wrench_topic/B2/total_torque
+      Torque Arrow Scale: 0.029999999329447746
+      Torque Color: 255; 0; 255
+      Value: true
+    - Accept NaN Values: false
+      Alpha: 1
+      Arrow Width: 0.4000000059604645
+      Class: rviz_default_plugins/Wrench
+      Enabled: true
+      Force Arrow Scale: 2
+      Force Color: 204; 51; 51
+      History Length: 1
+      Name: Wrench
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /wrench_topic/B3/total_torque
+      Torque Arrow Scale: 0.029999999329447746
+      Torque Color: 255; 0; 255
+      Value: true
+    - Accept NaN Values: false
+      Alpha: 1
+      Arrow Width: 0.4000000059604645
+      Class: rviz_default_plugins/Wrench
+      Enabled: true
+      Force Arrow Scale: 2
+      Force Color: 204; 51; 51
+      History Length: 1
+      Name: Wrench
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /wrench_topic/B4/total_torque
+      Torque Arrow Scale: 0.029999999329447746
+      Torque Color: 255; 0; 255
+      Value: true
+    - Accept NaN Values: false
+      Alpha: 1
+      Arrow Width: 0.4000000059604645
+      Class: rviz_default_plugins/Wrench
+      Enabled: true
+      Force Arrow Scale: 2
+      Force Color: 204; 51; 51
+      History Length: 1
+      Name: Wrench
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /wrench_topic/B5/total_torque
+      Torque Arrow Scale: 0.029999999329447746
+      Torque Color: 255; 0; 255
+      Value: true
+    - Accept NaN Values: false
+      Alpha: 1
+      Arrow Width: 0.4000000059604645
+      Class: rviz_default_plugins/Wrench
+      Enabled: true
+      Force Arrow Scale: 2
+      Force Color: 204; 51; 51
+      History Length: 1
+      Name: Wrench
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /wrench_topic/B6/total_torque
+      Torque Arrow Scale: 0.029999999329447746
+      Torque Color: 255; 0; 255
+      Value: true
+    - Accept NaN Values: false
+      Alpha: 1
+      Arrow Width: 0.4000000059604645
+      Class: rviz_default_plugins/Wrench
+      Enabled: true
+      Force Arrow Scale: 2
+      Force Color: 204; 51; 51
+      History Length: 1
+      Name: Wrench
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /wrench_topic/EE_SSRMS/total_torque
+      Torque Arrow Scale: 0.029999999329447746
+      Torque Color: 255; 0; 255
+      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
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 13.27796459197998
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 2.8083510398864746
+        Y: -0.03512916713953018
+        Z: 1.6076182126998901
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.40500012040138245
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz_default_plugins)
+      Yaw: 5.786983013153076
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1080
+  Help:
+    collapsed: false
+  Hide Left Dock: false
+  Hide Right Dock: false
+  MotionPlanning:
+    collapsed: false
+  MotionPlanning - Trajectory Slider:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000100000000000001e600000397fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004600fffffffb000000100044006900730070006c0061007900730100000025000000e7000000ce00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000112000001e3000001a300fffffffb0000000800480065006c0070000000029a0000006e0000007000fffffffb0000000a0056006900650077007301000002fb000000c1000000ac00ffffff000005940000039700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 0
+  Y: 27
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/config/moveit_controllers.yaml b/ros_trick/ros_src/trick_canadarm_moveit_config/config/moveit_controllers.yaml
new file mode 100644
index 00000000..e38b88b3
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/config/moveit_controllers.yaml
@@ -0,0 +1,18 @@
+moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+
+moveit_simple_controller_manager:
+  controller_names:
+    - canadarm_controller
+
+  canadarm_controller:
+    type: FollowJointTrajectory
+    action_ns: follow_joint_trajectory
+    default: true
+    joints:
+      - Base_Joint
+      - Shoulder_Roll
+      - Shoulder_Yaw
+      - Elbow_Pitch
+      - Wrist_Pitch
+      - Wrist_Yaw
+      - Wrist_Roll
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/config/pilz_cartesian_limits.yaml b/ros_trick/ros_src/trick_canadarm_moveit_config/config/pilz_cartesian_limits.yaml
new file mode 100644
index 00000000..b2997caf
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/config/pilz_cartesian_limits.yaml
@@ -0,0 +1,6 @@
+# Limits for the Pilz planner
+cartesian_limits:
+  max_trans_vel: 1.0
+  max_trans_acc: 2.25
+  max_trans_dec: -5.0
+  max_rot_vel: 1.57
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/config/ros2_controllers.yaml b/ros_trick/ros_src/trick_canadarm_moveit_config/config/ros2_controllers.yaml
new file mode 100644
index 00000000..244d22bd
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/config/ros2_controllers.yaml
@@ -0,0 +1,77 @@
+# This config file is used by ros2_control
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    canadarm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+canadarm_controller:
+  ros__parameters:
+    open_loop_control: false
+    allow_nonzero_velocity_at_trajectory_end: false
+    constraints:
+      stopped_velocity_tolerance: 0.001
+      goal_time: 0.0
+    joints:
+      - Base_Joint
+      - Shoulder_Roll
+      - Shoulder_Yaw
+      - Elbow_Pitch
+      - Wrist_Pitch
+      - Wrist_Yaw
+      - Wrist_Roll
+    command_interfaces:
+      - effort
+    state_interfaces:
+      - position
+      - velocity
+    state_publish_rate: 50.0
+    action_monitor_rate: 20.0
+    gains:
+      Base_Joint:
+        p: 92000.0
+        i: 0.0
+        d: 2050.0
+        ff_velocity_scale: 0.0
+        i_clamp: 0.0
+      Shoulder_Roll:
+        p: 92000.0
+        i: 0.0
+        d: 2050.0
+        ff_velocity_scale: 0.0
+        i_clamp: 0.0
+      Shoulder_Yaw:
+        p: 92000.0
+        i: 0.0
+        d: 2050.0
+        ff_velocity_scale: 0.0
+        i_clamp: 0.0
+      Elbow_Pitch:
+        p: 92000.0
+        i: 0.0
+        d: 2050.0
+        ff_velocity_scale: 0.0
+        i_clamp: 0.0
+      Wrist_Pitch:
+        p: 50000.0
+        i: 0.0
+        d: 1100.0
+        ff_velocity_scale: 0.0
+        i_clamp: 0.0
+      Wrist_Yaw:
+        p: 500.0
+        i: 0.0
+        d: 50.0
+        ff_velocity_scale: 0.0
+        i_clamp: 0.0
+      Wrist_Roll:
+        p: 500.0
+        i: 0.0
+        d: 50.0
+        ff_velocity_scale: 0.0
+        i_clamp: 0.0
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/launch/__pycache__/moveit_rviz.launch.cpython-310.pyc b/ros_trick/ros_src/trick_canadarm_moveit_config/launch/__pycache__/moveit_rviz.launch.cpython-310.pyc
new file mode 100644
index 00000000..6f04eb38
Binary files /dev/null and b/ros_trick/ros_src/trick_canadarm_moveit_config/launch/__pycache__/moveit_rviz.launch.cpython-310.pyc differ
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/launch/demo.launch.py b/ros_trick/ros_src/trick_canadarm_moveit_config/launch/demo.launch.py
new file mode 100644
index 00000000..61fe9c9e
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/launch/demo.launch.py
@@ -0,0 +1,67 @@
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node, SetParameter
+from moveit_configs_utils import MoveItConfigsBuilder
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder(
+        "SSRMS_Canadarm2", package_name="trick_canadarm_moveit_config"
+    ).to_moveit_configs()
+    moveit_config.move_group_capabilities["capabilities"] = ""
+    ld = LaunchDescription()
+    virtual_joints_launch = (
+        moveit_config.package_path / "launch/static_virtual_joint_tfs.launch.py"
+    )
+    if virtual_joints_launch.exists():
+        ld.add_action(
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(str(virtual_joints_launch)),
+            )
+        )
+
+    # Given the published joint states, publish tf for the robot links
+    ld.add_action(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                str(moveit_config.package_path / "launch/rsp.launch.py")
+            ),
+        )
+    )
+
+    ld.add_action(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                str(moveit_config.package_path / "launch/move_group.launch.py")
+            ),
+        )
+    )
+
+    ld.add_action(
+        Node(
+            package="canadarm_wrench_publisher",
+            executable="canadarm_wrench_publisher_node",
+        )
+    )
+
+    ld.add_action(
+        Node(
+            package="controller_manager",
+            executable="ros2_control_node",
+            parameters=[
+                moveit_config.robot_description,
+                str(moveit_config.package_path / "config/ros2_controllers.yaml"),
+            ],
+        )
+    )
+
+    ld.add_action(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                str(moveit_config.package_path / "launch/spawn_controllers.launch.py")
+            ),
+        )
+    )
+
+    return ld
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/launch/move_group.launch.py b/ros_trick/ros_src/trick_canadarm_moveit_config/launch/move_group.launch.py
new file mode 100644
index 00000000..31727c2f
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/launch/move_group.launch.py
@@ -0,0 +1,73 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.parameter_descriptions import ParameterValue
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launch_utils import (
+    DeclareBooleanLaunchArg,
+    add_debuggable_node,
+)
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder(
+        "SSRMS_Canadarm2", package_name="trick_canadarm_moveit_config"
+    ).to_moveit_configs()
+    # print("prinint capab")
+    # print(moveit_config.move_group_capabilities)
+    moveit_config.move_group_capabilities["capabilities"] = ""
+    ld = LaunchDescription()
+
+    ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
+    ld.add_action(
+        DeclareBooleanLaunchArg("allow_trajectory_execution", default_value=True)
+    )
+    ld.add_action(
+        DeclareBooleanLaunchArg("publish_monitored_planning_scene", default_value=True)
+    )
+    # load non-default MoveGroup capabilities (space separated)
+    ld.add_action(DeclareLaunchArgument("capabilities", default_value=""))
+    # inhibit these default MoveGroup capabilities (space separated)
+    ld.add_action(DeclareLaunchArgument("disable_capabilities", default_value=""))
+
+    # do not copy dynamics information from /joint_states to internal robot monitoring
+    # default to false, because almost nothing in move_group relies on this information
+    ld.add_action(DeclareBooleanLaunchArg("monitor_dynamics", default_value=False))
+
+    should_publish = LaunchConfiguration("publish_monitored_planning_scene")
+
+    move_group_configuration = {
+        "publish_robot_description_semantic": True,
+        "allow_trajectory_execution": LaunchConfiguration("allow_trajectory_execution"),
+        # Note: Wrapping the following values is necessary so that the parameter value can be the empty string
+        "capabilities": ParameterValue(
+            LaunchConfiguration("capabilities"), value_type=str
+        ),
+        "disable_capabilities": ParameterValue(
+            LaunchConfiguration("disable_capabilities"), value_type=str
+        ),
+        # Publish the planning scene of the physical robot so that rviz plugin can know actual robot
+        "publish_planning_scene": should_publish,
+        "publish_geometry_updates": should_publish,
+        "publish_state_updates": should_publish,
+        "publish_transforms_updates": should_publish,
+        "monitor_dynamics": False,
+    }
+
+    move_group_params = [
+        moveit_config.to_dict(),
+        move_group_configuration,
+    ]
+
+    add_debuggable_node(
+        ld,
+        package="moveit_ros_move_group",
+        executable="move_group",
+        commands_file=str(moveit_config.package_path / "launch" / "gdb_settings.gdb"),
+        output="screen",
+        parameters=move_group_params,
+        extra_debug_args=["--debug"],
+        # Set the display variable, in case OpenGL code is used internally
+        additional_env={"DISPLAY": ""},
+    )
+    return ld
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/launch/moveit_rviz.launch.py b/ros_trick/ros_src/trick_canadarm_moveit_config/launch/moveit_rviz.launch.py
new file mode 100644
index 00000000..91b265fe
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/launch/moveit_rviz.launch.py
@@ -0,0 +1,14 @@
+from launch import LaunchDescription
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_moveit_rviz_launch
+
+
+def generate_launch_description():
+    ld = LaunchDescription()
+
+    moveit_config = MoveItConfigsBuilder(
+        "SSRMS_Canadarm2", package_name="trick_canadarm_moveit_config"
+    ).to_moveit_configs()
+
+    ld.add_action(generate_moveit_rviz_launch(moveit_config))
+    return ld
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/launch/rsp.launch.py b/ros_trick/ros_src/trick_canadarm_moveit_config/launch/rsp.launch.py
new file mode 100644
index 00000000..540aedc4
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/launch/rsp.launch.py
@@ -0,0 +1,32 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from moveit_configs_utils import MoveItConfigsBuilder
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder(
+        "SSRMS_Canadarm2", package_name="trick_canadarm_moveit_config"
+    ).to_moveit_configs()
+    moveit_config.move_group_capabilities["capabilities"] = ""
+
+    ld = LaunchDescription()
+    ld.add_action(DeclareLaunchArgument("publish_frequency", default_value="15.0"))
+
+    # Given the published joint states, publish tf for the robot links and the robot description
+    rsp_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        respawn=True,
+        output="screen",
+        parameters=[
+            moveit_config.robot_description,
+            {
+                "publish_frequency": LaunchConfiguration("publish_frequency"),
+            },
+        ],
+    )
+    ld.add_action(rsp_node)
+
+    return ld
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/launch/setup_assistant.launch.py b/ros_trick/ros_src/trick_canadarm_moveit_config/launch/setup_assistant.launch.py
new file mode 100644
index 00000000..2b308057
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/launch/setup_assistant.launch.py
@@ -0,0 +1,9 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_setup_assistant_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder(
+        "SSRMS_Canadarm2", package_name="trick_canadarm_moveit_config"
+    ).to_moveit_configs()
+    return generate_setup_assistant_launch(moveit_config)
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/launch/spawn_controllers.launch.py b/ros_trick/ros_src/trick_canadarm_moveit_config/launch/spawn_controllers.launch.py
new file mode 100644
index 00000000..449daaba
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/launch/spawn_controllers.launch.py
@@ -0,0 +1,9 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_spawn_controllers_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder(
+        "SSRMS_Canadarm2", package_name="trick_canadarm_moveit_config"
+    ).to_moveit_configs()
+    return generate_spawn_controllers_launch(moveit_config)
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/launch/static_virtual_joint_tfs.launch.py b/ros_trick/ros_src/trick_canadarm_moveit_config/launch/static_virtual_joint_tfs.launch.py
new file mode 100644
index 00000000..0ee48dc6
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/launch/static_virtual_joint_tfs.launch.py
@@ -0,0 +1,9 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder(
+        "SSRMS_Canadarm2", package_name="trick_canadarm_moveit_config"
+    ).to_moveit_configs()
+    return generate_static_virtual_joint_tfs_launch(moveit_config)
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/launch/warehouse_db.launch.py b/ros_trick/ros_src/trick_canadarm_moveit_config/launch/warehouse_db.launch.py
new file mode 100644
index 00000000..7673e3c4
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/launch/warehouse_db.launch.py
@@ -0,0 +1,9 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_warehouse_db_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder(
+        "SSRMS_Canadarm2", package_name="trick_canadarm_moveit_config"
+    ).to_moveit_configs()
+    return generate_warehouse_db_launch(moveit_config)
diff --git a/ros_trick/ros_src/trick_canadarm_moveit_config/package.xml b/ros_trick/ros_src/trick_canadarm_moveit_config/package.xml
new file mode 100644
index 00000000..224b2372
--- /dev/null
+++ b/ros_trick/ros_src/trick_canadarm_moveit_config/package.xml
@@ -0,0 +1,46 @@
+<?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>trick_canadarm_moveit_config</name>
+  <version>0.3.0</version>
+  <description>
+     An automatically generated package with all the configuration and launch files for using the SSRMS_Canadarm2 with the MoveIt Motion Planning Framework
+     Derived from spaceros/demos/canadarm_moveit_config demo
+  </description>
+  <maintainer email="dharini@openrobotics.org">Dharini Dutia</maintainer>
+
+  <license>BSD</license>
+
+  <author email="fiderekblazej@gmail.com">Blazej Fiderek(xfiderek)</author>
+  <author email="dharini@openrobotics.org">Dharini Dutia</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>moveit_ros_move_group</exec_depend>
+  <exec_depend>moveit_kinematics</exec_depend>
+  <exec_depend>moveit_planners</exec_depend>
+  <exec_depend>moveit_simple_controller_manager</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>xacro</exec_depend>
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>moveit_configs_utils</exec_depend>
+  <exec_depend>moveit_ros_move_group</exec_depend>
+  <exec_depend>moveit_ros_visualization</exec_depend>
+  <exec_depend>moveit_ros_warehouse</exec_depend>
+  <exec_depend>moveit_setup_assistant</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>rviz_common</exec_depend>
+  <exec_depend>rviz_default_plugins</exec_depend>
+  <exec_depend>simulation</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>warehouse_ros_mongo</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+
+  <export>
+      <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/ros_trick/ros_src/trick_ros2_control/CMakeLists.txt b/ros_trick/ros_src/trick_ros2_control/CMakeLists.txt
new file mode 100644
index 00000000..dde3b3c5
--- /dev/null
+++ b/ros_trick/ros_src/trick_ros2_control/CMakeLists.txt
@@ -0,0 +1,81 @@
+cmake_minimum_required(VERSION 3.8)
+project(trick_ros2_control)
+
+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(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(realtime_tools REQUIRED)
+
+
+add_library(
+  trick_ros2_control
+  SHARED
+  src/trick_system.cpp
+  src/socket.cpp
+)
+target_include_directories(
+  trick_ros2_control
+  PUBLIC
+  include
+)
+ament_target_dependencies(
+  trick_ros2_control
+  hardware_interface
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+)
+# prevent pluginlib from using boost
+target_compile_definitions(trick_ros2_control PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+pluginlib_export_plugin_description_file(
+  hardware_interface trick_ros2_control.xml)
+
+install(
+  TARGETS
+  trick_ros2_control
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+endif()
+
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  trick_ros2_control
+)
+ament_export_dependencies(
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+)
+
+ament_package()
diff --git a/ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/socket.hpp b/ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/socket.hpp
new file mode 100644
index 00000000..bbfd90fc
--- /dev/null
+++ b/ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/socket.hpp
@@ -0,0 +1,51 @@
+// Copyright 2024 Blazej Fiderek (xfiderek)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     https://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// Largely inspired by
+// https://github.com/nasa/trick/blob/master/trick_sims/SIM_billiards/models/graphics/cpp/Socket.cpp
+#ifndef TRICK_ROS2_CONTROL__SOCKET_HPP_
+#define TRICK_ROS2_CONTROL__SOCKET_HPP_
+
+#include <arpa/inet.h>
+#include <cstring>
+#include <iostream>
+#include <netinet/in.h>
+#include <string>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <unistd.h>
+#define SOCKET_BUF_SIZE 20480
+
+class Socket
+{
+public:
+  Socket();
+  int init(std::string hostname, int port);
+
+  int send(std::string message);
+  int operator<<(std::string message);
+
+  std::string receive();
+  void operator>>(std::string& ret);
+
+private:
+  int _port;
+  std::string _hostname;
+  int _socket_fd;
+  bool _initialized;
+  char carry_on_buffer_[SOCKET_BUF_SIZE] = { '\0' };
+  char trick_delimiter_ = '\n';
+};
+
+#endif
diff --git a/ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/trick_system.hpp b/ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/trick_system.hpp
new file mode 100644
index 00000000..a9630c1b
--- /dev/null
+++ b/ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/trick_system.hpp
@@ -0,0 +1,142 @@
+// Copyright 2024 Blazej Fiderek (xfiderek)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     https://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TRICK_ROS2_CONTROL__TRICK_SYSTEM_HPP_
+#define TRICK_ROS2_CONTROL__TRICK_SYSTEM_HPP_
+
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/realtime_buffer.h"
+#include "trick_ros2_control/socket.hpp"
+#include "trick_ros2_control/utils.hpp"
+#include "trick_ros2_control/visibility_control.h"
+#include <limits>
+#include <string>
+#include <thread>
+#include <vector>
+
+namespace trick_ros2_control
+{
+
+/**
+ * @brief Data structure representing robot data on ROS side
+ *
+ */
+struct RosJointData
+{
+  std::string name;
+  double joint_position = std::numeric_limits<double>::quiet_NaN();
+  double joint_velocity = std::numeric_limits<double>::quiet_NaN();
+  double joint_acceleration = std::numeric_limits<double>::quiet_NaN();
+  double joint_effort = std::numeric_limits<double>::quiet_NaN();
+  double joint_position_cmd;
+  double joint_velocity_cmd;
+  double joint_acceleration_cmd;
+  double joint_effort_cmd;
+};
+
+/**
+ * @brief Data structure representing data on Trick side
+ * Used in the file to map trick variable to pointer to the data
+ * Used for both sending and receiving data
+ */
+struct TrickData
+{
+  const std::string trick_var_name;
+  double* const data_ptr;
+  TrickData(const std::string& trick_var_name, double* const data_ptr)
+    : trick_var_name(trick_var_name), data_ptr(data_ptr){};
+};
+
+/**
+ * @brief Mapping from the pointer where command is stored to the corresponding trick variable name
+ */
+using CommandDataToTrickVarName = std::pair<double* const, const std::string>;
+/**
+ * @brief Mapping from trick variable name to pointer where its value is stored
+ */
+using TrickVarNameToStateData = std::pair<const std::string, double* const>;
+
+class TrickSystem : public hardware_interface::SystemInterface
+{
+public:
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+
+private:
+  /**
+   * @brief Represents data received from trick
+   */
+  std::vector<TrickData> trick_state_variables_;
+  /**
+   * @brief Represents data to be sent to trick
+   */
+  std::vector<TrickData> trick_command_variables_;
+  /**
+   * @brief Thread used for receiving data from trick asynchronously
+   */
+  std::thread incoming_data_thread_;
+  /**
+   * @brief Socket encapsulating the connection with trick simulation
+   */
+  Socket trick_conn_socket_;
+  /**
+   * @brief Buffer for accessing incoming data in realtime-safe manner
+   */
+  realtime_tools::RealtimeBuffer<std::vector<double>> trick_data_incoming_buffer_;
+
+  int trick_server_port_;
+  std::string trick_hostname_;
+  double trick_variable_cycle_rate_;
+
+  const int MAX_NO_OF_RECONNECTS = 5;
+  const int RECONNECT_WAIT_TIME_S = 2;
+  std::vector<struct RosJointData> joints_data_;
+
+  // parameter names used in URDF file for trick-related params
+  const std::string JOINT_PARAM_STATE_INTERFACE_KEY = "state_interface";
+  const std::string JOINT_PARAM_COMMAND_INTERFACE_KEY = "command_interface";
+  const std::string JOINT_PARAM_TRICK_VARIABLE_NAME_KEY = "trick_variable_name";
+};
+
+}  // namespace trick_ros2_control
+
+#endif  // TRICK_ROS2_CONTROL__TRICK_SYSTEM_HPP_
diff --git a/ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/utils.hpp b/ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/utils.hpp
new file mode 100644
index 00000000..c6a73e26
--- /dev/null
+++ b/ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/utils.hpp
@@ -0,0 +1,52 @@
+// Copyright 2024 Blazej Fiderek (xfiderek)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     https://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <sstream>
+#include <string>
+#include <vector>
+
+double trick_string_convert(const std::string& str)
+{
+  std::istringstream ss(str);
+  double num;
+  ss >> num;
+  return num;
+}
+
+std::vector<std::string> trick_split_response(std::string& str, const char delim)
+{
+  std::stringstream ss(str);
+  std::string s;
+  std::vector<std::string> ret;
+  while (ss >> s)
+  {
+    ret.push_back(s);
+  }
+  return ret;
+}
+
+std::vector<double> trick_response_convert(std::string& response)
+{
+  auto responseSplit = trick_split_response(response, '\t');
+  std::vector<double> result;
+  if (responseSplit[0] != "0")
+  {
+    return result;
+  }
+  for (int i = 1; i < responseSplit.size(); i++)
+  {
+    result.push_back(trick_string_convert(responseSplit[i]));
+  }
+  return result;
+}
diff --git a/ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/visibility_control.h b/ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/visibility_control.h
new file mode 100644
index 00000000..1996b0e1
--- /dev/null
+++ b/ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/visibility_control.h
@@ -0,0 +1,56 @@
+// Copyright 2024 Blazej Fiderek (xfiderek)
+// 
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+// 
+//     https://www.apache.org/licenses/LICENSE-2.0
+// 
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#ifndef TRICK_ROS2_CONTROL__VISIBILITY_CONTROL_H_
+#define TRICK_ROS2_CONTROL__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport))
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport))
+#else
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport)
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport)
+#endif
+#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC                             \
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT
+#else
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC                             \
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT
+#endif
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE                        \
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL
+#else
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT                             \
+  __attribute__((visibility("default")))
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT
+#if __GNUC__ >= 4
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC                             \
+  __attribute__((visibility("default")))
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL                              \
+  __attribute__((visibility("hidden")))
+#else
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL
+#endif
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE
+#endif
+
+#endif // TRICK_ROS2_CONTROL__VISIBILITY_CONTROL_H_
diff --git a/ros_trick/ros_src/trick_ros2_control/package.xml b/ros_trick/ros_src/trick_ros2_control/package.xml
new file mode 100644
index 00000000..ca76e06d
--- /dev/null
+++ b/ros_trick/ros_src/trick_ros2_control/package.xml
@@ -0,0 +1,29 @@
+<?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>trick_ros2_control</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="fiderekblazej@gmail.com">blazej</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>hardware_interface</depend>
+
+  <depend>pluginlib</depend>
+
+  <depend>rclcpp</depend>
+
+  <depend>rclcpp_lifecycle</depend>
+  <depend>realtime_tools</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/ros_trick/ros_src/trick_ros2_control/src/socket.cpp b/ros_trick/ros_src/trick_ros2_control/src/socket.cpp
new file mode 100644
index 00000000..09ecde6c
--- /dev/null
+++ b/ros_trick/ros_src/trick_ros2_control/src/socket.cpp
@@ -0,0 +1,109 @@
+// Copyright 2024 Blazej Fiderek (xfiderek)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     https://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// Largely inspired by
+// https://github.com/nasa/trick/blob/master/trick_sims/SIM_billiards/models/graphics/cpp/Socket.cpp
+
+#include "trick_ros2_control/socket.hpp"
+
+Socket::Socket() : _initialized(false)
+{
+}
+
+int Socket::init(std::string hostname, int port)
+{
+  _hostname = hostname;
+  _port = port;
+  _socket_fd = socket(AF_INET, SOCK_STREAM, 0);
+  if (_socket_fd < 0)
+  {
+    std::cout << "Socket connection failed" << std::endl;
+    return -1;
+  }
+
+  struct sockaddr_in serv_addr;
+  serv_addr.sin_family = AF_INET;
+  serv_addr.sin_port = htons(port);  // convert to weird network byte format
+
+  if (inet_pton(AF_INET, "127.0.0.1", &serv_addr.sin_addr) <= 0)
+  {
+    std::cout << "Invalid address/ Address not supported" << std::endl;
+    return -1;
+  }
+
+  if (connect(_socket_fd, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0)
+  {
+    std::cout << "Connection failed" << std::endl;
+    return -1;
+  }
+
+  _initialized = true;
+  return 0;
+}
+
+int Socket::send(std::string message)
+{
+  // weird syntax I've never used before - since the send syscall that i'm
+  // trying to use is overloaded in this class, I have to append :: to the front
+  // of it so that the compiler knows to look in the global namespace
+  int success = ::send(_socket_fd, message.c_str(), message.size(), 0);
+  if (success < static_cast<int>(message.size()))
+  {
+    std::cout << "Failed to send message" << std::endl;
+  }
+  return success;
+}
+
+int Socket::operator<<(std::string message)
+{
+  return send(message);
+}
+
+std::string Socket::receive()
+{
+  char buffer[SOCKET_BUF_SIZE];
+  int numBytes = read(_socket_fd, buffer, SOCKET_BUF_SIZE);
+  if (numBytes < 0)
+  {
+    throw std::runtime_error("Failed to read from socket\n");
+  }
+  else if (numBytes < SOCKET_BUF_SIZE)
+  {
+    buffer[numBytes] = '\0';
+  }
+
+  std::string string_buffer = std::string(carry_on_buffer_) + std::string(buffer);
+  int last_newline_idx = string_buffer.rfind(trick_delimiter_, string_buffer.size() - 1);
+  int second_last_newline_idx = string_buffer.rfind(trick_delimiter_, string_buffer.size() - 2);
+
+  // it means that we can clear carry on buffer
+  if (last_newline_idx == string_buffer.size() - 1)
+  {
+    carry_on_buffer_[0] = '\0';
+  }
+  // in this case we need to store the remaining characters in the carry on
+  // buffer
+  else
+  {
+    std::strcpy(carry_on_buffer_, string_buffer.substr(last_newline_idx + 1).c_str());
+  }
+
+  // Always return the latest message
+  return string_buffer.substr(second_last_newline_idx + 1, last_newline_idx - second_last_newline_idx - 1);
+}
+
+void Socket::operator>>(std::string& ret)
+{
+  ret = receive();
+}
diff --git a/ros_trick/ros_src/trick_ros2_control/src/trick_system.cpp b/ros_trick/ros_src/trick_ros2_control/src/trick_system.cpp
new file mode 100644
index 00000000..2c603e9d
--- /dev/null
+++ b/ros_trick/ros_src/trick_ros2_control/src/trick_system.cpp
@@ -0,0 +1,268 @@
+// Copyright (c) 2024, Blazej Fiderek (xfiderek), Stogl Robotics Consulting UG
+// (haftungsbeschränkt) (template)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "trick_ros2_control/trick_system.hpp"
+
+namespace trick_ros2_control
+{
+
+hardware_interface::CallbackReturn TrickSystem::on_init(const hardware_interface::HardwareInfo& info)
+{
+  if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
+  {
+    return CallbackReturn::ERROR;
+  }
+
+  if (info_.hardware_parameters.find("trick_variable_server_host") == info_.hardware_parameters.end())
+  {
+    RCLCPP_FATAL(rclcpp::get_logger("TrickSystem"),
+                 "trick_variable_server_host param is not defined. Define in "
+                 "it 'ros2_control/hardware' block in your "
+                 "robot's URDF");
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+  if (info_.hardware_parameters.find("trick_variable_server_port") == info_.hardware_parameters.end())
+  {
+    RCLCPP_FATAL(rclcpp::get_logger("TrickSystem"),
+                 "trick_variable_server_port param is not defined.\n Define in "
+                 "it 'ros2_control/hardware' block in "
+                 "your robot's URDF");
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+  if (info_.hardware_parameters.find("trick_variable_cycle_rate") == info_.hardware_parameters.end())
+  {
+    RCLCPP_FATAL(rclcpp::get_logger("TrickSystem"),
+                 "trick_variable_cycle_rate param is not defined.\n Define in "
+                 "it 'ros2_control/hardware' block in your "
+                 "robot's URDF");
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+  trick_hostname_ = info_.hardware_parameters["trick_variable_server_host"];
+  trick_server_port_ = std::stoi(info_.hardware_parameters["trick_variable_server_port"]);
+  trick_variable_cycle_rate_ = std::stod(info_.hardware_parameters["trick_variable_cycle_rate"]);
+  size_t n_joints = info_.joints.size();
+  joints_data_.resize(n_joints);
+  if (n_joints > 0)
+  {
+    trick_data_incoming_buffer_.writeFromNonRT(std::vector<double>(n_joints * info.joints[0].state_interfaces.size(),
+                                                                   std::numeric_limits<double>::quiet_NaN()));
+  }
+
+  return CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn TrickSystem::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+  bool trick_conn_successful = false;
+  for (int i = 0; i < MAX_NO_OF_RECONNECTS && rclcpp::ok(); ++i)
+  {
+    trick_conn_successful = trick_conn_socket_.init(trick_hostname_, trick_server_port_) >= 0;
+    if (trick_conn_successful)
+    {
+      RCLCPP_INFO(rclcpp::get_logger("TrickSystem"), "Connected to trick server successfully");
+      break;
+    }
+    RCLCPP_INFO(rclcpp::get_logger("TrickSystem"), "Could not connect to trick, retrying %d/%d", i + 1,
+                MAX_NO_OF_RECONNECTS);
+    std::this_thread::sleep_for(std::chrono::seconds(RECONNECT_WAIT_TIME_S));
+  }
+  if (!trick_conn_successful)
+  {
+    std::string err = "could not connect to Trick server on host: " + trick_hostname_ +
+                      " and port: " + std::to_string(trick_server_port_);
+    RCLCPP_FATAL(rclcpp::get_logger("TrickSystem"), err.c_str());
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+
+  trick_conn_socket_ << "trick.var_pause()\n";
+  trick_conn_socket_ << "trick.var_ascii()\n";
+  trick_conn_socket_ << "trick.var_set_client_tag(\"ros2_control_" + info_.name + "\") \n";
+  trick_conn_socket_ << "trick.var_cycle(\"" + std::to_string(1.0 / trick_variable_cycle_rate_) + "\")\n";
+
+  for (const TrickData& trick_state_variable : trick_state_variables_)
+  {
+    const std::string& trick_var_name = trick_state_variable.trick_var_name;
+    std::string request_ = "trick.var_add(\"" + trick_var_name + "\")\n";
+    RCLCPP_INFO(rclcpp::get_logger("TrickSystem"), "adding trick variable with command: %s", request_.c_str());
+    trick_conn_socket_ << request_;
+  }
+
+  incoming_data_thread_ = std::thread([this]() {
+    while (rclcpp::ok())
+    {
+      if (this->get_state().label() == "active")
+      {
+        std::string incoming_msg;
+        this->trick_conn_socket_ >> incoming_msg;
+        std::vector<double> trick_data = trick_response_convert(incoming_msg);
+        this->trick_data_incoming_buffer_.writeFromNonRT(trick_data);
+      }
+      else
+      {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+      }
+    }
+  });
+
+  return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+std::vector<hardware_interface::StateInterface> TrickSystem::export_state_interfaces()
+{
+  std::vector<hardware_interface::StateInterface> state_interfaces;
+  for (unsigned int i = 0; i < info_.joints.size(); i++)
+  {
+    auto& joint_info = info_.joints[i];
+    for (unsigned int j = 0; j < joint_info.state_interfaces.size(); ++j)
+    {
+      double* state_variable_ptr_;
+      std::string joint_interface_name = joint_info.state_interfaces[j].name;
+      if (joint_interface_name == hardware_interface::HW_IF_POSITION)
+      {
+        state_variable_ptr_ = &joints_data_[i].joint_position;
+      }
+      if (joint_interface_name == hardware_interface::HW_IF_VELOCITY)
+      {
+        state_variable_ptr_ = &joints_data_[i].joint_velocity;
+      }
+      if (joint_interface_name == hardware_interface::HW_IF_ACCELERATION)
+      {
+        state_variable_ptr_ = &joints_data_[i].joint_acceleration;
+      }
+      if (joint_interface_name == hardware_interface::HW_IF_EFFORT)
+      {
+        state_variable_ptr_ = &joints_data_[i].joint_effort;
+      }
+      state_interfaces.emplace_back(
+          hardware_interface::StateInterface(joint_info.name, joint_interface_name, state_variable_ptr_));
+      std::string trick_variable_name_param_key =
+          JOINT_PARAM_STATE_INTERFACE_KEY + "/" + joint_interface_name + "/" + JOINT_PARAM_TRICK_VARIABLE_NAME_KEY;
+
+      if (joint_info.parameters.find(trick_variable_name_param_key) == joint_info.parameters.end())
+      {
+        throw std::runtime_error("Missing parameter: " + trick_variable_name_param_key +
+                                 ". Add it to joint parameters in ros2_control block, so that trick "
+                                 "variable can be "
+                                 "binded to "
+                                 "ros2_control variable");
+      }
+      std::string trick_variable_name = joint_info.parameters[trick_variable_name_param_key];
+      trick_state_variables_.emplace_back(TrickData(trick_variable_name, state_variable_ptr_));
+    }
+  }
+  return state_interfaces;
+}
+
+std::vector<hardware_interface::CommandInterface> TrickSystem::export_command_interfaces()
+{
+  std::vector<hardware_interface::CommandInterface> command_interfaces;
+  for (unsigned int i = 0; i < info_.joints.size(); i++)
+  {
+    auto& joint_info = info_.joints[i];
+    for (unsigned int j = 0; j < joint_info.command_interfaces.size(); ++j)
+    {
+      double* command_variable_ptr_;
+      std::string joint_interface_name = joint_info.command_interfaces[j].name;
+      if (joint_interface_name == hardware_interface::HW_IF_POSITION)
+      {
+        command_variable_ptr_ = &joints_data_[i].joint_position;
+      }
+      if (joint_interface_name == hardware_interface::HW_IF_VELOCITY)
+      {
+        command_variable_ptr_ = &joints_data_[i].joint_velocity;
+      }
+      if (joint_interface_name == hardware_interface::HW_IF_ACCELERATION)
+      {
+        command_variable_ptr_ = &joints_data_[i].joint_acceleration;
+      }
+      if (joint_interface_name == hardware_interface::HW_IF_EFFORT)
+      {
+        command_variable_ptr_ = &joints_data_[i].joint_effort;
+      }
+      // TODO(later) - parametrize it with joint params
+      *command_variable_ptr_ = 0.0;
+      command_interfaces.emplace_back(
+          hardware_interface::CommandInterface(joint_info.name, joint_interface_name, command_variable_ptr_));
+      std::string trick_variable_name_param_key =
+          JOINT_PARAM_COMMAND_INTERFACE_KEY + "/" + joint_interface_name + "/" + JOINT_PARAM_TRICK_VARIABLE_NAME_KEY;
+
+      if (joint_info.parameters.find(trick_variable_name_param_key) == joint_info.parameters.end())
+      {
+        throw std::runtime_error("Missing parameter: " + trick_variable_name_param_key +
+                                 ". Add it to joint parameters in ros2_control block, so that trick "
+                                 "variable can be "
+                                 "binded to "
+                                 "ros2_control variable");
+      }
+      std::string trick_variable_name = joint_info.parameters[trick_variable_name_param_key];
+      trick_command_variables_.emplace_back(TrickData(trick_variable_name, command_variable_ptr_));
+    }
+  }
+
+  return command_interfaces;
+}
+
+hardware_interface::CallbackReturn TrickSystem::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+  trick_conn_socket_ << "trick.var_unpause()\n";
+  // receive first message to  ensure connection works
+  trick_conn_socket_.receive();
+  return CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn TrickSystem::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+  trick_conn_socket_ << "trick.var_pause()\n";
+  return CallbackReturn::SUCCESS;
+}
+
+hardware_interface::return_type TrickSystem::read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
+{
+  const std::vector<double>& raw_doubles_from_trick = *trick_data_incoming_buffer_.readFromRT();
+  if (raw_doubles_from_trick.size() != trick_state_variables_.size())
+  {
+    RCLCPP_FATAL(rclcpp::get_logger("TrickSystem"),
+                 "size of data received from trick does not match number of "
+                 "declared state variables. Expected: %d. "
+                 "Got: %d",
+                 trick_state_variables_.size(), raw_doubles_from_trick.size());
+    return hardware_interface::return_type::OK;
+  }
+  for (int i = 0; i < raw_doubles_from_trick.size(); ++i)
+  {
+    double value_from_trick = raw_doubles_from_trick[i];
+    double* const target_state_variable_ptr_ = trick_state_variables_[i].data_ptr;
+    *target_state_variable_ptr_ = value_from_trick;
+  }
+
+  return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type TrickSystem::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
+{
+  std::string command = "";
+  for (const TrickData& trick_command_variable : trick_command_variables_)
+  {
+    command += trick_command_variable.trick_var_name + " = " + std::to_string(*trick_command_variable.data_ptr) + "\n";
+  }
+  trick_conn_socket_ << command;
+  return hardware_interface::return_type::OK;
+}
+
+}  // namespace trick_ros2_control
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(trick_ros2_control::TrickSystem, hardware_interface::SystemInterface)
diff --git a/ros_trick/ros_src/trick_ros2_control/trick_ros2_control.xml b/ros_trick/ros_src/trick_ros2_control/trick_ros2_control.xml
new file mode 100644
index 00000000..b87eb63d
--- /dev/null
+++ b/ros_trick/ros_src/trick_ros2_control/trick_ros2_control.xml
@@ -0,0 +1,9 @@
+<library path="trick_ros2_control">
+  <class name="trick_ros2_control/TrickSystem"
+         type="trick_ros2_control::TrickSystem"
+         base_class_type="hardware_interface::SystemInterface">
+    <description>
+      ros2_control hardware interface.
+    </description>
+  </class>
+</library>
diff --git a/ros_trick/ros_trick_bridge.Dockerfile b/ros_trick/ros_trick_bridge.Dockerfile
new file mode 100644
index 00000000..a8ce66ec
--- /dev/null
+++ b/ros_trick/ros_trick_bridge.Dockerfile
@@ -0,0 +1,112 @@
+# Copyright 2024 Blazej Fiderek (xfiderek)
+# 
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+# 
+#     https://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# Tested with humble-2024.07.0 release
+FROM osrf/space-ros:latest
+
+# ------------------------------------------------------------------------------
+# Get Install trick dependencies
+# ------------------------------------------------------------------------------
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
+  --mount=type=cache,target=/var/lib/apt,sharing=locked \ 
+  sudo apt-get update && sudo apt-get install -y \
+  bison \
+  clang \
+  flex \
+  git \
+  llvm \
+  make \
+  maven \
+  swig \
+  cmake \
+  curl \
+  g++ \
+  libx11-dev \
+  libxml2-dev \
+  libxt-dev \
+  libmotif-common \
+  libmotif-dev \
+  python3-dev \
+  zlib1g-dev \
+  llvm-dev \
+  libclang-dev \
+  libudunits2-dev \
+  libgtest-dev \
+  openjdk-11-jdk \
+  zip
+
+ENV PYTHON_VERSION=3
+
+# ------------------------------------------------------------------------------
+# Get Trick version 19.7.2 from GitHub, configure and build it.
+# ------------------------------------------------------------------------------
+WORKDIR /opt/trick
+RUN git clone --branch 19.7.2 --depth 1 https://github.com/nasa/trick.git .
+# cd into the directory we just created and ..
+# configure and make Trick.
+RUN ./configure && make
+
+# ------------------------------------------------------------------------------
+# Add ${TRICK_HOME}/bin to the PATH variable.
+# ------------------------------------------------------------------------------
+ENV TRICK_HOME="/opt/trick"
+RUN echo "export PATH=${PATH}:${TRICK_HOME}/bin" >> ~/.bashrc
+
+
+# ------------------------------------------------------------------------------
+# Build SPACEROS workspace with ros trick bridge
+# ------------------------------------------------------------------------------
+WORKDIR /opt/ros_trick_bridge_ws
+ENV ROS_TRICK_BRIDGE_WS="/opt/ros_trick_bridge_ws/"
+COPY --chown=spaceros-user:spaceros-user ros_src/ros_trick_bridge src/
+RUN /bin/bash -c 'source ${SPACEROS_DIR}/install/setup.bash \
+  && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON --event-handlers desktop_notification- status-'
+RUN rm -rf build log src
+
+# ------------------------------------------------------------------------------
+# ------------------------------------------------------------------------------
+# Parts below are only required for the canadarm demo. You can replace them as you see fit.
+# ------------------------------------------------------------------------------
+# ------------------------------------------------------------------------------
+
+# ------------------------------------------------------------------------------
+# Install RBDL, which is used for calculating forward dynamics in trick
+# ------------------------------------------------------------------------------
+WORKDIR /opt/rbdl
+RUN git clone --branch v3.3.0 --depth 1 https://github.com/rbdl/rbdl.git . \
+  && git submodule update --init --remote --depth 1 addons/urdfreader/
+RUN   mkdir ./rbdl-build \ 
+  && cd rbdl-build/ \
+  && cmake \
+  -DCMAKE_BUILD_TYPE=Release \ 
+  -DRBDL_BUILD_ADDON_URDFREADER="ON" \ 
+  .. \
+  && make \
+  && sudo make install \
+  && cd .. \
+  && rm -r ./rbdl-build
+# make it easy to link the rbdl library.
+# LD_LIBRARY_PATH is empty at this stage of dockerbuild
+ENV LD_LIBRARY_PATH="/usr/local/lib"
+
+# ------------------------------------------------------------------------------
+# copy the created canadarm trick simulation and compile it
+# ------------------------------------------------------------------------------
+WORKDIR /opt/trick_sims/SIM_trick_canadarm
+COPY --chown=spaceros-user:spaceros-user trick_src/SIM_trick_canadarm/ .
+RUN ${TRICK_HOME}/bin/trick-CP
+# include canadarm URDF for RBDL
+COPY --chown=spaceros-user:spaceros-user ros_src/trick_canadarm_moveit_config/config/SSRMS_Canadarm2.urdf.xacro .
+
+WORKDIR ${ROS_TRICK_BRIDGE_WS}
diff --git a/ros_trick/run.sh b/ros_trick/run.sh
new file mode 100755
index 00000000..cec12614
--- /dev/null
+++ b/ros_trick/run.sh
@@ -0,0 +1,18 @@
+#!/bin/bash
+# Copyright 2024 Blazej Fiderek (xfiderek)
+# 
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+# 
+#     https://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+set -e
+docker compose up --build
diff --git a/ros_trick/trick_src/SIM_trick_canadarm/RUN_2DPlanar/input.py b/ros_trick/trick_src/SIM_trick_canadarm/RUN_2DPlanar/input.py
new file mode 100644
index 00000000..9af246aa
--- /dev/null
+++ b/ros_trick/trick_src/SIM_trick_canadarm/RUN_2DPlanar/input.py
@@ -0,0 +1,31 @@
+# Copyright 2024 Blazej Fiderek (xfiderek)
+# 
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+# 
+#     https://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+trick.real_time_enable()
+trick.exec_set_software_frame(0.02)
+
+trick.exec_set_enable_freeze(True)
+trick.exec_set_freeze_command(True)
+
+
+trick.var_set_copy_mode(2)
+trick.var_server_set_port(49765)
+trick_message.mtcout.init()
+trick.message_subscribe(trick_message.mtcout)
+trick_message.separate_thread_set_enabled(True)
+simControlPanel = trick.SimControlPanel()
+trick.add_external_application(simControlPanel)
+trick_view = trick.TrickView()
+trick.add_external_application(trick_view)
diff --git a/ros_trick/trick_src/SIM_trick_canadarm/S_define b/ros_trick/trick_src/SIM_trick_canadarm/S_define
new file mode 100644
index 00000000..3095ff9f
--- /dev/null
+++ b/ros_trick/trick_src/SIM_trick_canadarm/S_define
@@ -0,0 +1,49 @@
+// Copyright 2024 Blazej Fiderek (xfiderek)
+// 
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+// 
+//     https://www.apache.org/licenses/LICENSE-2.0
+// 
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/************************TRICK HEADER*************************
+PURPOSE:
+    ( Simulate kinematic, planar 2 degree-of-freedom manipulator )
+LIBRARY DEPENDENCIES:
+    (
+        (manipulator/manipulator.cc)
+    )
+*************************************************************/
+#include "sim_objects/default_trick_sys.sm"
+
+##include "include/trick/exec_proto.h"
+##include "manipulator/manipulator.hh"
+
+class ManipulatorSimObject : public Trick::SimObject
+{
+
+    public:
+        Manip manip;
+
+        ManipulatorSimObject(): manip()
+        {
+            ("default_data")      manip.defaultData();
+            ("derivative")        manip.stateDeriv();
+            ("integration")       trick_ret = manip.stateInteg();
+        }
+
+};
+
+ManipulatorSimObject CanadarmManip;
+
+IntegLoop armIntegLoop(0.01) CanadarmManip;
+
+void create_connections() {
+    armIntegLoop.getIntegrator(Runge_Kutta_4, 2*CanadarmManip.manip.NDOF);
+}
diff --git a/ros_trick/trick_src/SIM_trick_canadarm/S_overrides.mk b/ros_trick/trick_src/SIM_trick_canadarm/S_overrides.mk
new file mode 100644
index 00000000..7d855259
--- /dev/null
+++ b/ros_trick/trick_src/SIM_trick_canadarm/S_overrides.mk
@@ -0,0 +1,18 @@
+# Copyright 2024 Blazej Fiderek (xfiderek)
+# 
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+# 
+#     https://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+TRICK_CFLAGS += -I./models -I${TRICK_HOME} -I/usr/include/eigen3 -g -O0 -Wall -Wextra -Wshadow
+TRICK_CXXFLAGS += -I./models -I${TRICK_HOME} -I/usr/include/eigen3 -g -O0 -Wall -Wextra -Wshadow
+TRICK_USER_LINK_LIBS += -lrbdl -lrbdl_urdfreader
+
diff --git a/ros_trick/trick_src/SIM_trick_canadarm/models/manipulator/manipulator.cc b/ros_trick/trick_src/SIM_trick_canadarm/models/manipulator/manipulator.cc
new file mode 100644
index 00000000..3adbb88d
--- /dev/null
+++ b/ros_trick/trick_src/SIM_trick_canadarm/models/manipulator/manipulator.cc
@@ -0,0 +1,112 @@
+// Copyright 2024 Blazej Fiderek (xfiderek)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     https://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "manipulator/manipulator.hh"
+#include "trick/integrator_c_intf.h"
+
+Manip::Manip() : rbdl_model()
+{
+  RigidBodyDynamics::Addons::URDFReadFromFile("./SSRMS_Canadarm2.urdf.xacro", &rbdl_model, false);
+  std::cout << "Loaded URDF model into trick";
+  std::cout << RigidBodyDynamics::Utils::GetModelDOFOverview(rbdl_model);
+
+  rbdl_model.gravity = RigidBodyDynamics::Math::Vector3d::Zero();
+
+  q_vec_ = RigidBodyDynamics::Math::VectorNd::Zero(rbdl_model.q_size);
+  q_dotvec_ = RigidBodyDynamics::Math::VectorNd::Zero(rbdl_model.q_size);
+  q_ddotvec_ = RigidBodyDynamics::Math::VectorNd::Zero(rbdl_model.q_size);
+  torque_vec_ = RigidBodyDynamics::Math::VectorNd::Zero(rbdl_model.q_size);
+
+  std::cout << "starting Trick Canadarm simulation" << std::endl;
+}
+
+int Manip::defaultData()
+{
+  for (size_t i = 0; i < NDOF; ++i)
+  {
+    q[i] = 0.0;
+    q_dot[i] = 0.0;
+    q_dotdot[i] = 0.0;
+    input_torque[i] = 0.0;
+    applied_torque[i] = 0.0;
+    friction_torque[i] = 0.0;
+    q_vec_[i] = 0.0;
+    q_dotvec_[i] = 0.0;
+    q_ddotvec_[i] = 0.0;
+    torque_vec_[i] = 0.0;
+    breakaway_friction_torque[i] = 2.5;
+    coloumb_friction_torque[i] = 2.0;
+    breakaway_friction_velocity[i] = 0.005;
+    coulomb_velocity_threshold[i] = breakaway_friction_velocity[i] / 10;
+    stribeck_velocity_threshold[i] = M_SQRT2 * breakaway_friction_torque[i];
+    viscous_friction_coefficient[i] = 0.001;
+  }
+
+  return (0);
+}
+
+int Manip::forwardDynamics()
+{
+  // calculate torques and store as RBDL vector as well
+  for (size_t i = 0; i < NDOF; ++i)
+  {
+    // https://www.mathworks.com/help/simscape/ref/rotationalfriction.html
+    double curr_to_stribeck_vel = q_dot[i] / stribeck_velocity_threshold[i];
+    double curr_to_coulomb_vel = q_dot[i] / coulomb_velocity_threshold[i];
+    friction_torque[i] = M_SQRT2 * M_E * (breakaway_friction_torque[i] - coloumb_friction_torque[i]) *
+                         std::exp(-std::pow(curr_to_stribeck_vel, 2)) * curr_to_stribeck_vel;
+    friction_torque[i] += coloumb_friction_torque[i] * std::tanh(curr_to_coulomb_vel);
+    friction_torque[i] += viscous_friction_coefficient[i] * q_dot[i];
+
+    applied_torque[i] = input_torque[i] - friction_torque[i];
+
+    //  copy positions and velocities from C-arrays to RBDL vectors
+    torque_vec_[i] = applied_torque[i];
+    q_vec_[i] = q[i];
+    q_dotvec_[i] = q_dot[i];
+    q_ddotvec_[i] = 0.0;
+  }
+  // calculate dynamics (q_dotdot) with RBDL
+  RigidBodyDynamics::ForwardDynamics(rbdl_model, q_vec_, q_dotvec_, torque_vec_, q_ddotvec_);
+
+  // copy Q_dotdot from RBDL vector type to simple C-arrays
+  for (size_t i = 0; i < NDOF; ++i)
+  {
+    q_dotdot[i] = q_ddotvec_[i];
+  }
+
+  return (0);
+}
+
+int Manip::stateDeriv()
+{
+  int status_code = forwardDynamics();
+  return (status_code);
+}
+
+int Manip::stateInteg()
+{
+  int integration_step;
+  load_state(&q[0], &q[1], &q[2], &q[3], &q[4], &q[5], &q[6], &q_dot[0], &q_dot[1], &q_dot[2], &q_dot[3], &q_dot[4],
+             &q_dot[5], &q_dot[6], NULL);
+  load_deriv(&q_dot[0], &q_dot[1], &q_dot[2], &q_dot[3], &q_dot[4], &q_dot[5], &q_dot[6], &q_dotdot[0], &q_dotdot[1],
+             &q_dotdot[2], &q_dotdot[3], &q_dotdot[4], &q_dotdot[5], &q_dotdot[6], NULL);
+  // integrate
+  integration_step = integrate();
+
+  unload_state(&q[0], &q[1], &q[2], &q[3], &q[4], &q[5], &q[6], &q_dot[0], &q_dot[1], &q_dot[2], &q_dot[3], &q_dot[4],
+               &q_dot[5], &q_dot[6], NULL);
+
+  return (integration_step);
+}
\ No newline at end of file
diff --git a/ros_trick/trick_src/SIM_trick_canadarm/models/manipulator/manipulator.hh b/ros_trick/trick_src/SIM_trick_canadarm/models/manipulator/manipulator.hh
new file mode 100644
index 00000000..8f283636
--- /dev/null
+++ b/ros_trick/trick_src/SIM_trick_canadarm/models/manipulator/manipulator.hh
@@ -0,0 +1,97 @@
+// Copyright 2024 Blazej Fiderek (xfiderek)
+// 
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+// 
+//     https://www.apache.org/licenses/LICENSE-2.0
+// 
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#ifndef __MANIPULATOR_HH_
+#define __MANIPULATOR_HH_
+/**************************************************************************
+PURPOSE: (2D Manipulator class definitions including kinematics and control)
+***************************************************************************/
+#define TRICK_NO_MONTE_CARLO
+#define TRICK_NO_MASTERSLAVE
+#define TRICK_NO_INSTRUMENTATION
+
+#include <rbdl/rbdl.h>
+#include <rbdl/rbdl_utils.h>
+#include <rbdl/addons/urdfreader/urdfreader.h>
+#include <cmath>
+#include <iostream>
+#include <new>
+
+class Manip
+{
+public:
+  Manip();
+
+  /**
+   * @brief Calculate Q_dotdot (joint accelerations) given current state and input torques
+   * The dynamics is calculated using Articulated Body Algorithm (ABA) with RBDL library
+   * Friction is modelled with Stribeck model taken from Mathworks docs
+   * https://www.mathworks.com/help/simscape/ref/rotationalfriction.html
+   * @return int status code
+   */
+  int forwardDynamics();
+
+  /**
+   * @brief Calculate state derivative. Calls forwardDynamics().
+   *
+   * @return int status code
+   */
+  int stateDeriv();
+  /**
+   * @brief Propagate state for the current timestamp by integration. Called after stateDeriv()
+   *
+   * @return int status code
+   */
+  int stateInteg();
+
+  /**
+   * @brief Initializes all data structures. Called on startup.
+   *
+   * @return int status code
+   */
+  int defaultData();
+
+  /**
+   * @brief Model encapsulating kinematics and dynamics of Canadarm with RBDL library
+   *
+   */
+  RigidBodyDynamics::Model rbdl_model; /* ** -- class from RBDL that calculates forward dynamics */
+  static const size_t NDOF = 7;        /* ** -- ndof */
+
+  double input_torque[NDOF] = { 0.0 }; /* *i (N.m) input (commanded) torque for each joint */
+
+  double q[NDOF] = { 0.0 };               /* *o rad angle of joints */
+  double q_dot[NDOF] = { 0.0 };           /* *o (rad/s) velocity of joints */
+  double q_dotdot[NDOF] = { 0.0 };        /* *o (rad/s^2) accelerations of joints */
+  double friction_torque[NDOF] = { 0.0 }; /* *o (N.m) Torque comming from friction */
+  double applied_torque[NDOF] = { 0.0 };  /* *o (N.m) final torque applied for each joint */
+
+  // Friction-related parameters. For now we assume same params for every joint
+  // The friction is modelled using Stribeck function
+  // https://www.mathworks.com/help/simscape/ref/rotationalfriction.html
+  double breakaway_friction_torque[NDOF];    /* *i (N.m)  */
+  double coloumb_friction_torque[NDOF];      /* *i (N.m)  */
+  double breakaway_friction_velocity[NDOF];  /* *i (rad/s)  */
+  double coulomb_velocity_threshold[NDOF];   /* *i (rad.s)  */
+  double stribeck_velocity_threshold[NDOF];  /* *i (N.m/rad.s)  */
+  double viscous_friction_coefficient[NDOF]; /* *i (N.m/rad.s)  */
+
+private:
+  RigidBodyDynamics::Math::VectorNd q_vec_;
+  RigidBodyDynamics::Math::VectorNd q_dotvec_;
+  RigidBodyDynamics::Math::VectorNd q_ddotvec_;
+  RigidBodyDynamics::Math::VectorNd torque_vec_;
+};
+#endif