diff --git a/README.md b/README.md
index 1f83fad..ffb2b5b 100644
--- a/README.md
+++ b/README.md
@@ -1,2 +1,34 @@
 # AstroROS
-Spacecraft Orbit/Attitude Simulation
+Spacecraft Orbit/Attitude Simulation for ROS2
+# Introduction
+This repository is for AstroROS : ADR simulator for On-Orbit Servicing.
+
+This repo is a fork repo from https://github.com/uscl/AstroROS .
+
+Every changes (even they are trivial) will be applied in this repo and revised version will be uploaded in original repository.
+
+# How to use AstroROS
+
+AstroROS for ROS2 is on-progress.
+
+First, you have to install ROS2 Humble and ignition Fortress.
+
+And then, git clone this package.
+
+```
+mkdir -p astroros_ws && cd astroros_ws
+```
+```
+git clone https://github.com/MichaelBentleyOh/AstroROS
+```
+Finally build the package with colcon.
+```
+colcon build --symlink-install
+```
+# Update Log & Plan
+
+2025.03.03 : Upload "satellite_description" folder
+
+2025.03.31 : update for orbit propagation
+
+2025.09.?? : add impedance controller for tentacle grasp
diff --git a/orbit_simulator_matlab/orbit_simulator.slx b/orbit_simulator_matlab/orbit_simulator.slx
deleted file mode 100644
index ea56ea6..0000000
Binary files a/orbit_simulator_matlab/orbit_simulator.slx and /dev/null differ
diff --git a/orbit_simulator_matlab/readme.md b/orbit_simulator_matlab/readme.md
deleted file mode 100644
index 75b5a26..0000000
--- a/orbit_simulator_matlab/readme.md
+++ /dev/null
@@ -1,3 +0,0 @@
-This matlab/simulink files are implementation of orbit_simulator for AstroROS.
-
-These will be replaced matlab/simulink with ROS(C++) code for convenience in next release.
diff --git a/orbit_simulator_matlab/sim_data_3.mat b/orbit_simulator_matlab/sim_data_3.mat
deleted file mode 100644
index 7d5aabc..0000000
Binary files a/orbit_simulator_matlab/sim_data_3.mat and /dev/null differ
diff --git a/orbit_simulator_matlab/viewer.m b/orbit_simulator_matlab/viewer.m
deleted file mode 100644
index 08601e1..0000000
--- a/orbit_simulator_matlab/viewer.m
+++ /dev/null
@@ -1,110 +0,0 @@
-clear all;
-close all;
-clc;
-
-Re = 6371.2*1000;
-
-u = load('sim_data_3.mat');
-
-LVLH_Quat = u.out.SS_LVLH_Quat;
-R_Quat = u.out.SS_R_Quat;
-State = u.out.T_statevec;
-Pos_I = State(:,1:3);
-Vel_I = State(:,4:6);
-ECI2LVLH = u.out.SS_ECI2LVLH.signals.values;
-
-figure;
-plot3(Pos_I(:,1), Pos_I(:,2), Pos_I(:,3), 'k');
-hold on;    grid on;
-sat_pos = plot3(Pos_I(1,1), Pos_I(1,2), Pos_I(1,3), 'k.');
-axis equal;
-axis([-Re*1.2, Re*1.2, -Re*1.2, Re*1.2, -Re*1.2, Re*1.2]);
-
-C_ECI2LVLH = ECI2LVLH(:,:,1)*1000000;
-LVLH_X = quiver3(Pos_I(1,1), Pos_I(1,2), Pos_I(1,3), C_ECI2LVLH(1,1), C_ECI2LVLH(2,1), C_ECI2LVLH(3,1), 1.5,'Color',[0 0 0]);
-LVLH_Y = quiver3(Pos_I(1,1), Pos_I(1,2), Pos_I(1,3), C_ECI2LVLH(1,2), C_ECI2LVLH(2,2), C_ECI2LVLH(3,2), 1.5,'Color',[0 0 0]);
-LVLH_Z = quiver3(Pos_I(1,1), Pos_I(1,2), Pos_I(1,3), C_ECI2LVLH(1,3), C_ECI2LVLH(2,3), C_ECI2LVLH(3,3), 1.5,'Color',[0 0 0]);
-
-C_ECI2BODY = C_ECI2LVLH*quat2dcm(LVLH_Quat(1,:));
-BODY_X = quiver3(Pos_I(1,1), Pos_I(1,2), Pos_I(1,3), C_ECI2BODY(1,1), C_ECI2BODY(2,1), C_ECI2BODY(3,1), 1.5,'Color',[1 0 0]);
-BODY_Y = quiver3(Pos_I(1,1), Pos_I(1,2), Pos_I(1,3), C_ECI2BODY(1,2), C_ECI2BODY(2,2), C_ECI2BODY(3,2), 1.5,'Color',[0 1 0]);
-BODY_Z = quiver3(Pos_I(1,1), Pos_I(1,2), Pos_I(1,3), C_ECI2BODY(1,3), C_ECI2BODY(2,3), C_ECI2BODY(3,3), 1.5,'Color',[0 0 1]);
-
-str = sprintf("LVLH Attitude Control");
-xlabel('X [m]');
-ylabel('Y [m]');
-zlabel('Z [m]');
-
-for i=1:100:length(State)
-    title(str,"FontSize",20);
-
-    set(sat_pos, 'XData', Pos_I(i,1), 'YData', Pos_I(i,2), 'ZData', Pos_I(i,3),'MarkerSize',1.5);
-
-    C_ECI2LVLH = ECI2LVLH(:,:,i)*1000000;
-    set(LVLH_X, 'XData', Pos_I(i,1), 'YData', Pos_I(i,2), 'ZData', Pos_I(i,3), ...
-                       'UData', C_ECI2LVLH(1,1),'VData', C_ECI2LVLH(2,1),'WData', C_ECI2LVLH(3,1),...
-                       'LineWidth',2,'LineStyle','--');
-    set(LVLH_Y, 'XData', Pos_I(i,1), 'YData', Pos_I(i,2), 'ZData', Pos_I(i,3), ...
-                       'UData', C_ECI2LVLH(1,2),'VData', C_ECI2LVLH(2,2),'WData', C_ECI2LVLH(3,2),...
-                       'LineWidth',2,'LineStyle','--');
-    set(LVLH_Z, 'XData', Pos_I(i,1), 'YData', Pos_I(i,2), 'ZData', Pos_I(i,3), ...
-                       'UData', C_ECI2LVLH(1,3),'VData', C_ECI2LVLH(2,3),'WData', C_ECI2LVLH(3,3),...
-                       'LineWidth',2,'LineStyle','--');
-
-    C_ECI2BODY = C_ECI2LVLH*quat2dcm(LVLH_Quat(i,:));
-    set(BODY_X, 'XData', Pos_I(i,1), 'YData', Pos_I(i,2), 'ZData', Pos_I(i,3), ...
-                       'UData', C_ECI2BODY(1,1),'VData', C_ECI2BODY(2,1),'WData', C_ECI2BODY(3,1));
-    set(BODY_Y, 'XData', Pos_I(i,1), 'YData', Pos_I(i,2), 'ZData', Pos_I(i,3), ...
-                       'UData', C_ECI2BODY(1,2),'VData', C_ECI2BODY(2,2),'WData', C_ECI2BODY(3,2));
-    set(BODY_Z, 'XData', Pos_I(i,1), 'YData', Pos_I(i,2), 'ZData', Pos_I(i,3), ...
-                       'UData', C_ECI2BODY(1,3),'VData', C_ECI2BODY(2,3),'WData', C_ECI2BODY(3,3));
-
-    drawnow;
-
-end
-
-ECI2LVLH_euler = zeros(length(LVLH_Quat),3);
-ECI2BODY_euler = zeros(length(LVLH_Quat),3);
-for i=1:length(LVLH_Quat)
-    ECI2LVLH_euler_elem = ECI2LVLH(:,:,i);
-    ECI2LVLH_euler_elem_rpy = rotm2eul(ECI2LVLH_euler_elem);
-    ECI2LVLH_euler(i,1) = ECI2LVLH_euler_elem_rpy(1);
-    ECI2LVLH_euler(i,2) = ECI2LVLH_euler_elem_rpy(2);
-    ECI2LVLH_euler(i,3) = ECI2LVLH_euler_elem_rpy(3);
-
-    ECI2BODY_euler_elem = ECI2LVLH_euler_elem*quat2dcm(LVLH_Quat(i,:));
-    ECI2BODY_euler_elem_rpy = rotm2eul(ECI2BODY_euler_elem);
-    ECI2BODY_euler(i,1) = ECI2BODY_euler_elem_rpy(1);
-    ECI2BODY_euler(i,2) = ECI2BODY_euler_elem_rpy(2);
-    ECI2BODY_euler(i,3) = ECI2BODY_euler_elem_rpy(3);
-end
-
-tout = u.out.tout;
-figure;
-subplot(3,1,1);
-plot(tout,ECI2BODY_euler(:,1),'Color','red','LineStyle','--','LineWidth',1);
-hold on; grid on;
-plot(tout,ECI2LVLH_euler(:,1),'Color','black','LineStyle','-','LineWidth',1);
-legend(["Body","LVLH"]);
-xlabel("t [sec]","FontSize",15);
-ylabel("angle [rad]","FontSize",15);
-title("Roll \phi","FontSize",20);
-fontsize(20,"points");
-subplot(3,1,2);
-plot(tout,ECI2BODY_euler(:,2),'Color','green','LineStyle','--','LineWidth',1);
-hold on; grid on;
-plot(tout,ECI2LVLH_euler(:,2),'Color','black','LineStyle','-','LineWidth',1);
-legend(["Body","LVLH"]);
-xlabel("t [sec]","FontSize",15);
-ylabel("angle [rad]","FontSize",15);
-title("Pitch \theta","FontSize",20);
-fontsize(20,"points");
-subplot(3,1,3);
-plot(tout,ECI2BODY_euler(:,3),'Color','blue','LineStyle','--','LineWidth',1);
-hold on; grid on;
-plot(tout,ECI2LVLH_euler(:,3),'Color','black','LineStyle','-','LineWidth',1);
-legend(["Body","LVLH"]);
-xlabel("t [sec]","FontSize",15);
-ylabel("angle [rad]","FontSize",15);
-title("Yaw \psi","FontSize",20);
-fontsize(20,"points");
\ No newline at end of file
diff --git a/reaction_wheel_tentacle_ws/readme.md b/reaction_wheel_tentacle_ws/readme.md
deleted file mode 100644
index cfdf269..0000000
--- a/reaction_wheel_tentacle_ws/readme.md
+++ /dev/null
@@ -1 +0,0 @@
-Clear-space like ADR satellite was designed for demo operation.
diff --git a/reaction_wheel_tentacle_ws/src/CMakeLists.txt b/reaction_wheel_tentacle_ws/src/CMakeLists.txt
deleted file mode 100644
index 2016816..0000000
--- a/reaction_wheel_tentacle_ws/src/CMakeLists.txt
+++ /dev/null
@@ -1 +0,0 @@
-/opt/ros/noetic/share/catkin/cmake/toplevel.cmake
\ No newline at end of file
diff --git a/reaction_wheel_tentacle_ws/src/cubesat_description/CMakeLists.txt b/reaction_wheel_tentacle_ws/src/cubesat_description/CMakeLists.txt
deleted file mode 100644
index a4d4c96..0000000
--- a/reaction_wheel_tentacle_ws/src/cubesat_description/CMakeLists.txt
+++ /dev/null
@@ -1,197 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(cubesat_description)
-
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
-
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-  rospy
-)
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES fusion2urdf
-#  CATKIN_DEPENDS rospy
-#  DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-# include
-  ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/fusion2urdf.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/fusion2urdf_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
-# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_fusion2urdf.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
diff --git a/reaction_wheel_tentacle_ws/src/cubesat_description/LICENSE b/reaction_wheel_tentacle_ws/src/cubesat_description/LICENSE
deleted file mode 100644
index 314c3af..0000000
--- a/reaction_wheel_tentacle_ws/src/cubesat_description/LICENSE
+++ /dev/null
@@ -1,21 +0,0 @@
-MIT License
-
-Copyright (c) 2018 Toshinori Kitamura
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
diff --git a/reaction_wheel_tentacle_ws/src/cubesat_description/launch/controller.launch b/reaction_wheel_tentacle_ws/src/cubesat_description/launch/controller.launch
deleted file mode 100644
index 9d85686..0000000
--- a/reaction_wheel_tentacle_ws/src/cubesat_description/launch/controller.launch
+++ /dev/null
@@ -1,9 +0,0 @@
-<launch>
-
-<rosparam file="$(find cubesat_description)/launch/controller.yaml" command="load"/>
-<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="cubesat" args="joint_state_controller "/>
-<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen">
-  <remap from="/joint_states" to="/cubesat/joint_states"/>
-</node>
-
-</launch>
\ No newline at end of file
diff --git a/reaction_wheel_tentacle_ws/src/cubesat_description/launch/controller.yaml b/reaction_wheel_tentacle_ws/src/cubesat_description/launch/controller.yaml
deleted file mode 100644
index 3ca8cff..0000000
--- a/reaction_wheel_tentacle_ws/src/cubesat_description/launch/controller.yaml
+++ /dev/null
@@ -1,7 +0,0 @@
-cubesat_controller:
-  # Publish all joint states -----------------------------------
-  joint_state_controller:
-    type: joint_state_controller/JointStateController
-    publish_rate: 50
-
-  # Position Controllers --------------------------------------
diff --git a/reaction_wheel_tentacle_ws/src/cubesat_description/launch/display.launch b/reaction_wheel_tentacle_ws/src/cubesat_description/launch/display.launch
deleted file mode 100644
index a297edd..0000000
--- a/reaction_wheel_tentacle_ws/src/cubesat_description/launch/display.launch
+++ /dev/null
@@ -1,10 +0,0 @@
-<launch>
-  <arg name="model" default="$(find cubesat_description)/urdf/cubesat.xacro"/>
-  <arg name="gui" default="true"/>
-  <arg name="rvizconfig" default="$(find cubesat_description)/launch/urdf.rviz"/>
-  <param name="robot_description" command="$(find xacro)/xacro $(arg model)"/>
-  <param name="use_gui" value="$(arg gui)"/>
-  <node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"/>
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
-  <node name="rviz" pkg="rviz" args="-d $(arg rvizconfig)" type="rviz" required="true"/>
-</launch>
diff --git a/reaction_wheel_tentacle_ws/src/cubesat_description/launch/gazebo.launch b/reaction_wheel_tentacle_ws/src/cubesat_description/launch/gazebo.launch
deleted file mode 100644
index 4f985d2..0000000
--- a/reaction_wheel_tentacle_ws/src/cubesat_description/launch/gazebo.launch
+++ /dev/null
@@ -1,11 +0,0 @@
-<launch>
-  <param name="cubesat_description" command="$(find xacro)/xacro $(find cubesat_description)/urdf/cubesat.xacro"/>
-  <node name="spawn_cubesat_urdf" pkg="gazebo_ros" type="spawn_model" args="-param cubesat_description -urdf -model cubesat -x 2.65 -y 0 -z 1 -R 0.5 -P 0.0 -Y 0.0"/>
-  <!-- <include file="$(find gazebo_ros)/launch/empty_world.launch">
-    <arg name="paused" value="true"/>
-    <arg name="use_sim_time" value="true"/>
-    <arg name="gui" value="true"/>
-    <arg name="headless" value="false"/>
-    <arg name="debug" value="false"/>
-  </include> -->
-</launch>
diff --git a/reaction_wheel_tentacle_ws/src/cubesat_description/launch/urdf.rviz b/reaction_wheel_tentacle_ws/src/cubesat_description/launch/urdf.rviz
deleted file mode 100644
index b80bcfa..0000000
--- a/reaction_wheel_tentacle_ws/src/cubesat_description/launch/urdf.rviz
+++ /dev/null
@@ -1,422 +0,0 @@
-Panels:
-  - Class: rviz/Displays
-    Help Height: 78
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /Status1
-        - /RobotModel1
-        - /TF1
-      Splitter Ratio: 0.5
-    Tree Height: 591
-  - Class: rviz/Selection
-    Name: Selection
-  - Class: rviz/Tool Properties
-    Expanded:
-      - /2D Pose Estimate1
-      - /2D Nav Goal1
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
-  - Class: rviz/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-  - Class: rviz/Time
-    Experimental: false
-    Name: Time
-    SyncMode: 0
-    SyncSource: ""
-Preferences:
-  PromptSaveOnExit: true
-Toolbars:
-  toolButtonStyle: 2
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 10
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Alpha: 1
-      Class: rviz/RobotModel
-      Collision Enabled: false
-      Enabled: true
-      Links:
-        2dofknuckle_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        2dofknuckle_link_10:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        2dofknuckle_link_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        2dofknuckle_link_3:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        2dofknuckle_link_4:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        2dofknuckle_link_5:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        2dofknuckle_link_6:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        2dofknuckle_link_7:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        2dofknuckle_link_8:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        2dofknuckle_link_9:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        All Links Enabled: true
-        Expand Joint Details: false
-        Expand Link Details: false
-        Expand Tree: false
-        Link Tree Style: Links in Alphabetic Order
-        base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        bucket_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        face_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        facebracket_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        hcsr04_cans_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        hcsr04_chips_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        hcsr04_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        leg_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        leg_link_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        leg_link_3:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        leg_link_4:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        lipo_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        mg90_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        mg90_link_10:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        mg90_link_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        mg90_link_3:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        mg90_link_4:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        mg90_link_5:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        mg90_link_6:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        mg90_link_7:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        mg90_link_8:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        mg90_link_9:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        mpu9250_cap_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        mpu9250_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        mpu9250_mpu_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        pizerow_cam_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        pizerow_con_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        pizerow_cpu_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        pizerow_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        raspicam_cam_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        raspicam_chips_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        raspicam_con_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        raspicam_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        servobottom_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        servobottom_link_10:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        servobottom_link_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        servobottom_link_3:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        servobottom_link_4:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        servobottom_link_5:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        servobottom_link_6:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        servobottom_link_7:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        servobottom_link_8:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        servobottom_link_9:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        shell_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-      Name: RobotModel
-      Robot Description: robot_description
-      TF Prefix: ""
-      Update Interval: 0
-      Value: true
-      Visual Enabled: true
-    - Class: rviz/TF
-      Enabled: false
-      Frame Timeout: 15
-      Frames:
-        All Enabled: true
-      Marker Scale: 0.5
-      Name: TF
-      Show Arrows: true
-      Show Axes: true
-      Show Names: true
-      Tree:
-        {}
-      Update Interval: 0
-      Value: false
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Default Light: true
-    Fixed Frame: base_link
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz/Interact
-      Hide Inactive Objects: true
-    - Class: rviz/MoveCamera
-    - Class: rviz/Select
-    - Class: rviz/FocusCamera
-    - Class: rviz/Measure
-    - Class: rviz/SetInitialPose
-      Theta std deviation: 0.2617993950843811
-      Topic: /initialpose
-      X std deviation: 0.5
-      Y std deviation: 0.5
-    - Class: rviz/SetGoal
-      Topic: /move_base_simple/goal
-    - Class: rviz/PublishPoint
-      Single click: true
-      Topic: /clicked_point
-  Value: true
-  Views:
-    Current:
-      Class: rviz/Orbit
-      Distance: 0.34426525235176086
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: -0.0016349668148905039
-        Y: -0.014781012199819088
-        Z: 0.017814036458730698
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.46039697527885437
-      Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 0.593582272529602
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 882
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000142000002d9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000037000002d9000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002d9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000037000002d9000000a100fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004c000000044fc0100000002fb0000000800540069006d00650100000000000004c00000026b00fffffffb0000000800540069006d0065010000000000000450000000000000000000000267000002d900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Time:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: false
-  Width: 1216
-  X: 700
-  Y: 385
diff --git a/reaction_wheel_tentacle_ws/src/cubesat_description/meshes/base_link.stl b/reaction_wheel_tentacle_ws/src/cubesat_description/meshes/base_link.stl
deleted file mode 100644
index c80f753..0000000
Binary files a/reaction_wheel_tentacle_ws/src/cubesat_description/meshes/base_link.stl and /dev/null differ
diff --git a/reaction_wheel_tentacle_ws/src/cubesat_description/package.xml b/reaction_wheel_tentacle_ws/src/cubesat_description/package.xml
deleted file mode 100644
index c963d83..0000000
--- a/reaction_wheel_tentacle_ws/src/cubesat_description/package.xml
+++ /dev/null
@@ -1,62 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>cubesat_description</name>
-  <version>0.0.0</version>
-<description>The cubesat_description package</description>
-
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="syuntoku14@todo.todo">syuntoku14</maintainer>
-
-
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
-
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/fusion2urdf</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
-  <!--   Note that this is equivalent to the following: -->
-  <!--   <build_depend>roscpp</build_depend> -->
-  <!--   <exec_depend>roscpp</exec_depend> -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use build_export_depend for packages you need in order to build against this package: -->
-  <!--   <build_export_depend>message_generation</build_export_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <!-- Use doc_depend for packages you need only for building documentation: -->
-  <!--   <doc_depend>doxygen</doc_depend> -->
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>rospy</build_depend>
-  <build_export_depend>rospy</build_export_depend>
-  <exec_depend>rospy</exec_depend>
-
-
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <!-- Other tools can request additional information be placed here -->
-
-  </export>
-</package>
diff --git a/reaction_wheel_tentacle_ws/src/cubesat_description/urdf/cubesat.gazebo b/reaction_wheel_tentacle_ws/src/cubesat_description/urdf/cubesat.gazebo
deleted file mode 100644
index 5c96c4f..0000000
--- a/reaction_wheel_tentacle_ws/src/cubesat_description/urdf/cubesat.gazebo
+++ /dev/null
@@ -1,17 +0,0 @@
-<?xml version="1.0" ?>
-<robot name="cubesat" xmlns:xacro="http://www.ros.org/wiki/xacro" >
-
-<xacro:property name="body_color" value="Gazebo/Silver" />
-
-<!-- <gazebo>
-  <plugin name="control" filename="libgazebo_ros_control.so"/>
-</gazebo> -->
-<gazebo reference="base_link">
-  <material>${body_color}</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-</robot>
diff --git a/reaction_wheel_tentacle_ws/src/cubesat_description/urdf/cubesat.trans b/reaction_wheel_tentacle_ws/src/cubesat_description/urdf/cubesat.trans
deleted file mode 100644
index 92e7967..0000000
--- a/reaction_wheel_tentacle_ws/src/cubesat_description/urdf/cubesat.trans
+++ /dev/null
@@ -1,4 +0,0 @@
-<?xml version="1.0" ?>
-<robot name="cubesat" xmlns:xacro="http://www.ros.org/wiki/xacro" >
-
-</robot>
diff --git a/reaction_wheel_tentacle_ws/src/cubesat_description/urdf/cubesat.xacro b/reaction_wheel_tentacle_ws/src/cubesat_description/urdf/cubesat.xacro
deleted file mode 100644
index e2db32c..0000000
--- a/reaction_wheel_tentacle_ws/src/cubesat_description/urdf/cubesat.xacro
+++ /dev/null
@@ -1,28 +0,0 @@
-<?xml version="1.0" ?>
-<robot name="cubesat" xmlns:xacro="http://www.ros.org/wiki/xacro">
-
-<xacro:include filename="$(find cubesat_description)/urdf/materials.xacro" />
-<xacro:include filename="$(find cubesat_description)/urdf/cubesat.trans" />
-<xacro:include filename="$(find cubesat_description)/urdf/cubesat.gazebo" />
-<link name="base_link">
-  <inertial>
-    <origin xyz="0.000500648953679641 -0.00018140463958650211 -1.898172859337299e-05" rpy="0 0 0"/>
-    <mass value="30"/>
-    <inertia ixx="1.66667" iyy="1.66667" izz="1.66667" ixy="-8e-06" iyz="0.0" ixz="1e-06"/>
-  </inertial>
-  <visual>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://cubesat_description/meshes/base_link.stl" scale="0.009 0.009 0.009"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>
-  <collision>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://cubesat_description/meshes/base_link.stl" scale="0.009 0.009 0.009"/>
-    </geometry>
-  </collision>
-</link>
-
-</robot>
diff --git a/reaction_wheel_tentacle_ws/src/cubesat_description/urdf/materials.xacro b/reaction_wheel_tentacle_ws/src/cubesat_description/urdf/materials.xacro
deleted file mode 100644
index b4be976..0000000
--- a/reaction_wheel_tentacle_ws/src/cubesat_description/urdf/materials.xacro
+++ /dev/null
@@ -1,8 +0,0 @@
-<?xml version="1.0" ?>
-<robot name="cubesat" xmlns:xacro="http://www.ros.org/wiki/xacro" >
-
-<material name="silver">
-  <color rgba="0.700 0.700 0.700 1.000"/>
-</material>
-
-</robot>
diff --git a/reaction_wheel_tentacle_ws/src/ft_sensor_plugin/CMakeLists.txt b/reaction_wheel_tentacle_ws/src/ft_sensor_plugin/CMakeLists.txt
deleted file mode 100644
index 6157973..0000000
--- a/reaction_wheel_tentacle_ws/src/ft_sensor_plugin/CMakeLists.txt
+++ /dev/null
@@ -1,60 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(ft_sensor_plugin)
-
-# Set policy for CMake 3.1+. Use OLD policy to let FindBoost.cmake, dependency
-# of gazebo, use quoted variables in if()
-
-find_package(catkin REQUIRED COMPONENTS 
-  roscpp
-  gazebo_dev 
-  gazebo_plugins
-  geometry_msgs 
-  std_msgs
-  xacro)
-
-find_package(Eigen3 REQUIRED)
-
-catkin_package(
-  INCLUDE_DIRS include
-  CATKIN_DEPENDS 
-    roscpp
-    gazebo_dev 
-    gazebo_plugins 
-    geometry_msgs
-    std_msgs
-)
-
-# Plugins require c++11
-set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
-
-include_directories( include
-  ${catkin_INCLUDE_DIRS}
-  ${EIGEN3_INCLUDE_DIRS}
-  ${GAZEBO_INCLUDE_DIRS}
-  ${GAZEBO_MSG_INCLUDE_DIRS}
-  ${CMAKE_CURRENT_BINARY_DIR}
-)
-link_directories(
-  ${catkin_LIBRARY_DIRS}
-)
-
-add_library(ft_sensor_plugin
-  src/ft_sensor_plugin.cpp
-)
-target_link_libraries(ft_sensor_plugin
-  ${catkin_LIBRARIES}
-  ${GAZEBO_LIBRARIES}
-)
-install(TARGETS ft_sensor_plugin
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-install(FILES
-  ft_sensor_plugin.xml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-install(DIRECTORY include/${PROJECT_NAME}/
-        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
\ No newline at end of file
diff --git a/reaction_wheel_tentacle_ws/src/ft_sensor_plugin/include/ft_sensor_plugin/ft_sensor_plugin.h b/reaction_wheel_tentacle_ws/src/ft_sensor_plugin/include/ft_sensor_plugin/ft_sensor_plugin.h
deleted file mode 100644
index a9a6ef0..0000000
--- a/reaction_wheel_tentacle_ws/src/ft_sensor_plugin/include/ft_sensor_plugin/ft_sensor_plugin.h
+++ /dev/null
@@ -1,71 +0,0 @@
-#ifndef GAZEBO_FT_SENSOR_PLUGIN_H
-#define GAZEBO_FT_SENSOR_PLUGIN_H
-
-#include <ros/callback_queue.h>
-#include <ros/advertise_options.h>
-
-#include <gazebo/physics/physics.hh>
-#include <gazebo/transport/TransportTypes.hh>
-#include <gazebo/common/Plugin.hh>
-#include <gazebo/common/Event.hh>
-
-#include <gazebo/sensors/sensors.hh>
-
-#include <ros/ros.h>
-#include <boost/thread.hpp>
-#include <boost/thread/mutex.hpp>
-#include <geometry_msgs/Wrench.h>
-
-namespace gazebo{
-    class ftSensor : public ModelPlugin{
-        public : ftSensor();
-        public : virtual ~ftSensor();
-
-        public : void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
-
-        protected : virtual void UpdateChild();
-
-        protected : double gaussian_noise_;
-
-        private : double GaussianKernel(double mu, double sigma);
-
-        private : physics::JointPtr joint_;
-
-        private : physics::LinkPtr parent_link_;
-
-        private : physics::LinkPtr child_link_;
-
-        private : physics::ModelPtr model_;
-
-        private : physics::WorldPtr world_;
-
-        private : ros::NodeHandle* rosnode_;
-        private : ros::Publisher pub_;
-
-        private : geometry_msgs::Wrench wrench_msg_;
-
-        private : std::string joint_name_;
-        private : std::string topic_name_;
-        private : std::string robot_namespace_;
-        private : std::string frame_name_;
-
-        private : boost::mutex lock_;
-        private : boost::thread callback_queue_thread_;
-
-        private : common::Time last_time_;
-
-        private : double update_rate_;
-
-        private : int ft_connect_count_;
-
-        private : void ftConnect();
-        private : void ftDisConnect();
-
-        private : ros::CallbackQueue queue_;
-        private : void QueueThread();
-
-        private : event::ConnectionPtr update_connection_;
-    };
-}
-
-#endif
\ No newline at end of file
diff --git a/reaction_wheel_tentacle_ws/src/ft_sensor_plugin/package.xml b/reaction_wheel_tentacle_ws/src/ft_sensor_plugin/package.xml
deleted file mode 100644
index acfc5e2..0000000
--- a/reaction_wheel_tentacle_ws/src/ft_sensor_plugin/package.xml
+++ /dev/null
@@ -1,24 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>ft_sensor_plugin</name>
-  <version>0.0.1</version>
-  <description>ft_sensor_plugin package</description>
-  <maintainer email="michael1015999@gmail.com">Minsik Oh</maintainer>
-
-  <license>BSD</license>
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <depend>roscpp</depend>
-  <depend>eigen</depend>
-  <depend>xacro</depend>
-  <depend>std_msgs</depend>
-  <depend>geometry_msgs</depend>
-  <depend>gazebo_ros</depend>
-  <depend>gazebo_plugins</depend>
-  <depend>gazebo_dev</depend>
-
-  <export>
-    <gazebo_ros plugin_path="${prefix}/lib" gazebo_media_path="${prefix}" gazebo_model_path="${prefix}" />
-  </export>
-
-</package>
\ No newline at end of file
diff --git a/reaction_wheel_tentacle_ws/src/ft_sensor_plugin/src/ft_sensor_plugin.cpp b/reaction_wheel_tentacle_ws/src/ft_sensor_plugin/src/ft_sensor_plugin.cpp
deleted file mode 100644
index 3986a25..0000000
--- a/reaction_wheel_tentacle_ws/src/ft_sensor_plugin/src/ft_sensor_plugin.cpp
+++ /dev/null
@@ -1,260 +0,0 @@
-/*
- * Copyright (C) 2012-2014 Open Source Robotics Foundation
- *
- * 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.
- *
-*/
-/*
- * Desc: Force Torque Sensor Plugin
- * Author: Francisco Suarez-Ruiz
- * Date: 5 June 2014
- */
-
-#include <ft_sensor_plugin/ft_sensor_plugin.h>
-#include <tf/tf.h>
-#ifdef ENABLE_PROFILER
-#include <ignition/common/Profiler.hh>
-#endif
-#include <ignition/math/Rand.hh>
-
-namespace gazebo
-{
-GZ_REGISTER_MODEL_PLUGIN(ftSensor);
-
-////////////////////////////////////////////////////////////////////////////////
-// Constructor
-ftSensor::ftSensor()
-{
-  this->ft_connect_count_ = 0;
-}
-
-////////////////////////////////////////////////////////////////////////////////
-// Destructor
-ftSensor::~ftSensor()
-{
-  this->update_connection_.reset();
-  // Custom Callback Queue
-  this->queue_.clear();
-  this->queue_.disable();
-  this->rosnode_->shutdown();
-  this->callback_queue_thread_.join();
-  delete this->rosnode_;
-}
-
-////////////////////////////////////////////////////////////////////////////////
-// Load the controller
-void ftSensor::Load( physics::ModelPtr _model, sdf::ElementPtr _sdf )
-{
-  // Save pointers
-  this->model_ = _model;
-  this->world_ = this->model_->GetWorld();
-
-  // load parameters
-  this->robot_namespace_ = "";
-  if (_sdf->HasElement("robotNamespace"))
-    this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
-
-  if (!_sdf->HasElement("jointName"))
-  {
-    ROS_FATAL_NAMED("ft_sensor", "ft_sensor plugin missing <jointName>, cannot proceed");
-    return;
-  }
-  else
-    this->joint_name_ = _sdf->GetElement("jointName")->Get<std::string>();
-
-  this->joint_ = this->model_->GetJoint(this->joint_name_);
-  if (!this->joint_)
-  {
-    ROS_FATAL_NAMED("ft_sensor", "gazebo_ros_ft_sensor plugin error: jointName: %s does not exist\n",this->joint_name_.c_str());
-    return;
-  }
-
-  this->parent_link_ = this->joint_->GetParent();
-  this->child_link_ = this->joint_->GetChild();
-  this->frame_name_ = this->child_link_->GetName();
-
-  ROS_INFO_NAMED("ft_sensor", "ft_sensor plugin reporting wrench values to the frame [%s]", this->frame_name_.c_str());
-
-  if (!_sdf->HasElement("topicName"))
-  {
-    ROS_FATAL_NAMED("ft_sensor", "ft_sensor plugin missing <topicName>, cannot proceed");
-    return;
-  }
-  else
-    this->topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();
-
-  if (!_sdf->HasElement("gaussianNoise"))
-  {
-    ROS_INFO_NAMED("ft_sensor", "ft_sensor plugin missing <gaussianNoise>, defaults to 0.0");
-    this->gaussian_noise_ = 0.0;
-  }
-  else
-    this->gaussian_noise_ = _sdf->Get<double>("gaussianNoise");
-
-  if (!_sdf->HasElement("updateRate"))
-  {
-    ROS_DEBUG_NAMED("ft_sensor", "ft_sensor plugin missing <updateRate>, defaults to 0.0"
-             " (as fast as possible)");
-    this->update_rate_ = 0;
-  }
-  else
-    this->update_rate_ = _sdf->GetElement("updateRate")->Get<double>();
-
-  // Make sure the ROS node for Gazebo has already been initialized
-  if (!ros::isInitialized())
-  {
-    ROS_FATAL_STREAM_NAMED("ft_sensor", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
-      << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
-    return;
-  }
-
-  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
-
-  // resolve tf prefix
-  std::string prefix;
-  this->rosnode_->getParam(std::string("tf_prefix"), prefix);
-  this->frame_name_ = tf::resolve(prefix, this->frame_name_);
-
-  // Custom Callback Queue
-  ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<geometry_msgs::Wrench>(
-    this->topic_name_,1,
-    boost::bind( &ftSensor::ftConnect,this),
-    boost::bind( &ftSensor::ftDisConnect,this), ros::VoidPtr(), &this->queue_);
-  this->pub_ = this->rosnode_->advertise(ao);
-
-  // Custom Callback Queue
-  this->callback_queue_thread_ = boost::thread( boost::bind( &ftSensor::QueueThread,this ) );
-
-  // New Mechanism for Updating every World Cycle
-  // Listen to the update event. This event is broadcast every
-  // simulation iteration.
-  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
-      boost::bind(&ftSensor::UpdateChild, this));
-}
-
-////////////////////////////////////////////////////////////////////////////////
-// Someone subscribes to me
-void ftSensor::ftConnect()
-{
-  this->ft_connect_count_++;
-}
-
-////////////////////////////////////////////////////////////////////////////////
-// Someone subscribes to me
-void ftSensor::ftDisConnect()
-{
-  this->ft_connect_count_--;
-}
-
-////////////////////////////////////////////////////////////////////////////////
-// Update the controller
-void ftSensor::UpdateChild()
-{
-#ifdef ENABLE_PROFILER
-  IGN_PROFILE("ftSensor::UpdateChild");
-  IGN_PROFILE_BEGIN("fill ROS message");
-#endif
-#if GAZEBO_MAJOR_VERSION >= 8
-  common::Time cur_time = this->world_->SimTime();
-#else
-  common::Time cur_time = this->world_->GetSimTime();
-#endif
-
-  // rate control
-  if (this->update_rate_ > 0 &&
-      (cur_time-this->last_time_).Double() < (1.0/this->update_rate_))
-    return;
-
-  if (this->ft_connect_count_ == 0)
-    return;
-
-  physics::JointWrench wrench;
-  ignition::math::Vector3d torque;
-  ignition::math::Vector3d force;
-
-  // FIXME: Should include options for diferent frames and measure directions
-  // E.g: https://bitbucket.org/osrf/gazebo/raw/default/gazebo/sensors/ForceTorqueSensor.hh
-  // Get force torque at the joint
-  // The wrench is reported in the CHILD <frame>
-  // The <measure_direction> is child_to_parent
-  wrench = this->joint_->GetForceTorque(0);
-// #if GAZEBO_MAJOR_VERSION >= 8
-//   force = wrench.body2Force;
-//   torque = wrench.body2Torque;
-// #else
-//   force = wrench.body2Force.Ign();
-//   torque = wrench.body2Torque.Ign();
-// #endif
-
-  force = wrench.body2Force;
-  torque = wrench.body2Torque;
-
-  this->lock_.lock();
-  // copy data into wrench message
-
-  this->wrench_msg_.force.x = force.X() + this->GaussianKernel(0, this->gaussian_noise_);
-  this->wrench_msg_.force.y = force.Y() + this->GaussianKernel(0, this->gaussian_noise_);
-  this->wrench_msg_.force.z = force.Z() + this->GaussianKernel(0, this->gaussian_noise_);
-  this->wrench_msg_.torque.x = torque.X() + this->GaussianKernel(0, this->gaussian_noise_);
-  this->wrench_msg_.torque.y = torque.Y() + this->GaussianKernel(0, this->gaussian_noise_);
-  this->wrench_msg_.torque.z = torque.Z() + this->GaussianKernel(0, this->gaussian_noise_);
-#ifdef ENABLE_PROFILER
-  IGN_PROFILE_END();
-  IGN_PROFILE_BEGIN("publish");
-#endif
-  this->pub_.publish(this->wrench_msg_);
-#ifdef ENABLE_PROFILER
-  IGN_PROFILE_END();
-#endif
-  this->lock_.unlock();
-
-  // save last time stamp
-  this->last_time_ = cur_time;
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Utility for adding noise
-double ftSensor::GaussianKernel(double mu, double sigma)
-{
-  // using Box-Muller transform to generate two independent standard
-  // normally disbributed normal variables see wikipedia
-
-  // normalized uniform random variable
-  double U = ignition::math::Rand::DblUniform();
-
-  // normalized uniform random variable
-  double V = ignition::math::Rand::DblUniform();
-
-  double X = sqrt(-2.0 * ::log(U)) * cos(2.0*M_PI * V);
-  // double Y = sqrt(-2.0 * ::log(U)) * sin(2.0*M_PI * V);
-
-  // there are 2 indep. vars, we'll just use X
-  // scale to our mu and sigma
-  X = sigma * X + mu;
-  return X;
-}
-
-// Custom Callback Queue
-////////////////////////////////////////////////////////////////////////////////
-// custom callback queue thread
-void ftSensor::QueueThread()
-{
-  static const double timeout = 0.01;
-
-  while (this->rosnode_->ok())
-  {
-    this->queue_.callAvailable(ros::WallDuration(timeout));
-  }
-}
-
-}
\ No newline at end of file
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_control/CMakeLists.txt b/reaction_wheel_tentacle_ws/src/reaction_wheel_control/CMakeLists.txt
deleted file mode 100644
index bdf2eca..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_control/CMakeLists.txt
+++ /dev/null
@@ -1,12 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(reaction_wheel_control)
-
-find_package(catkin REQUIRED)
-
-catkin_package()
-
-install(DIRECTORY config
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
-
-install(DIRECTORY launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_control/config/reaction_wheel_control.yaml b/reaction_wheel_tentacle_ws/src/reaction_wheel_control/config/reaction_wheel_control.yaml
deleted file mode 100644
index bb02d81..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_control/config/reaction_wheel_control.yaml
+++ /dev/null
@@ -1,98 +0,0 @@
-reaction_wheel:
-  # Publish all joint states -----------------------------------
-  joint_state_controller:
-    type: joint_state_controller/JointStateController
-    publish_rate: 100
-
-  # Velocity Controllers ---------------------------------------
-  # previous setting
-  # reaction_wheel_front_velocity_controller:
-  #   type: effort_controllers/JointVelocityController
-  #   joint: reaction_wheel_front_joint
-  #   pid: {p: 0.01, i: 0, d: 0}
-  reaction_wheel_front_velocity_controller:
-    type: velocity_controllers/JointVelocityController
-    joint: reaction_wheel_front_joint
-    pid: {p: 0.01, i: 0, d: 0}
-  reaction_wheel_back_velocity_controller:
-    type: velocity_controllers/JointVelocityController
-    joint: reaction_wheel_back_joint
-    pid: {p: 0.01, i: 0, d: 0}
-  reaction_wheel_right_velocity_controller:
-    type: velocity_controllers/JointVelocityController
-    joint: reaction_wheel_right_joint
-    pid: {p: 0.01, i: 0, d: 0}
-  reaction_wheel_left_velocity_controller:
-    type: velocity_controllers/JointVelocityController
-    joint: reaction_wheel_left_joint
-    pid: {p: 0.01, i: 0, d: 0}
-  #tentacle position controller----------------------------------------------
-  # previous setting
-  # tentacle_right_link_1_joint_position_controller:
-  #   type: effort_controllers/JointPositionController
-  #   joint: tentacle_right_link_1_joint
-  #   pid: {p: 0.1, i: 0, d: 0}
-  tentacle_right_link_1_joint_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: tentacle_right_link_1_joint
-    pid: {p: 0.05, i: 0, d: 0}
-  tentacle_left_link_1_joint_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: tentacle_left_link_1_joint
-    pid: {p: 0.05, i: 0, d: 0}
-  tentacle_left_link_2_joint_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: tentacle_left_link_2_joint
-    pid: {p: 0.05, i: 0, d: 0}
-  tentacle_right_link_2_joint_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: tentacle_right_link_2_joint
-    pid: {p: 0.05, i: 0, d: 0}
-  tentacle_right_link_3_joint_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: tentacle_right_link_3_joint
-    pid: {p: 0.05, i: 0, d: 0}
-  tentacle_left_link_3_joint_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: tentacle_left_link_3_joint
-    pid: {p: 0.05, i: 0, d: 0}
-  tentacle_right_link_4_joint_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: tentacle_right_link_4_joint
-    pid: {p: 0.05, i: 0, d: 0}
-  tentacle_left_link_4_joint_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: tentacle_left_link_4_joint
-    pid: {p: 0.05, i: 0, d: 0}
-  tentacle_up_link_1_joint_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: tentacle_up_link_1_joint
-    pid: {p: 0.05, i: 0, d: 0}
-  tentacle_up_link_2_joint_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: tentacle_up_link_2_joint
-    pid: {p: 0.05, i: 0, d: 0}
-  tentacle_up_link_3_joint_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: tentacle_up_link_3_joint
-    pid: {p: 0.05, i: 0, d: 0}
-  tentacle_up_link_4_joint_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: tentacle_up_link_4_joint
-    pid: {p: 0.05, i: 0, d: 0}
-  tentacle_down_link_1_joint_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: tentacle_down_link_1_joint
-    pid: {p: 0.05, i: 0, d: 0}
-  tentacle_down_link_2_joint_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: tentacle_down_link_2_joint
-    pid: {p: 0.05, i: 0, d: 0}
-  tentacle_down_link_3_joint_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: tentacle_down_link_3_joint
-    pid: {p: 0.05, i: 0, d: 0}
-  tentacle_down_link_4_joint_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: tentacle_down_link_4_joint
-    pid: {p: 0.05, i: 0, d: 0}
\ No newline at end of file
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_control/launch/reaction_wheel_velocity_control.launch b/reaction_wheel_tentacle_ws/src/reaction_wheel_control/launch/reaction_wheel_velocity_control.launch
deleted file mode 100644
index e9c6ff7..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_control/launch/reaction_wheel_velocity_control.launch
+++ /dev/null
@@ -1,42 +0,0 @@
-<launch>
-
-  <!-- Load joint controller configurations from YAML file to parameter server -->
-  <rosparam file="$(find reaction_wheel_control)/config/reaction_wheel_control.yaml" command="load"/>
-
-  <!-- load the controllers -->
-  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
-	output="screen" ns="/reaction_wheel" args="joint_state_controller
-					  reaction_wheel_front_velocity_controller
-					  reaction_wheel_back_velocity_controller
-            reaction_wheel_right_velocity_controller
-            reaction_wheel_left_velocity_controller
-            
-            tentacle_right_link_1_joint_position_controller
-            tentacle_right_link_2_joint_position_controller
-            tentacle_right_link_3_joint_position_controller
-            tentacle_right_link_4_joint_position_controller
-
-            tentacle_left_link_1_joint_position_controller
-            tentacle_left_link_2_joint_position_controller
-            tentacle_left_link_3_joint_position_controller
-            tentacle_left_link_4_joint_position_controller
-            
-            tentacle_up_link_1_joint_position_controller
-            tentacle_up_link_2_joint_position_controller
-            tentacle_up_link_3_joint_position_controller
-            tentacle_up_link_4_joint_position_controller
-
-            tentacle_down_link_1_joint_position_controller
-            tentacle_down_link_2_joint_position_controller
-            tentacle_down_link_3_joint_position_controller
-            tentacle_down_link_4_joint_position_controller
-
-            "/>
-
-  <!-- convert joint states to TF transforms for rviz, etc -->
-  <node name="reaction_wheel_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
-	respawn="false" output="screen">
-    <remap from="/joint_states" to="/reaction_wheel_state/joint_states" />
-  </node>
-
-</launch>
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_control/launch/tentacle_position_control.launch b/reaction_wheel_tentacle_ws/src/reaction_wheel_control/launch/tentacle_position_control.launch
deleted file mode 100644
index fd65591..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_control/launch/tentacle_position_control.launch
+++ /dev/null
@@ -1,39 +0,0 @@
-<launch>
-
-  <!-- Load joint controller configurations from YAML file to parameter server -->
-  <rosparam file="$(find reaction_wheel_control)/config/tentacle_control.yaml" command="load"/>
-
-  <!-- load the controllers -->
-  <node name="tentacle_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
-	output="screen" ns="/tentacle" args="joint_state_controller
-					  tentacle_right_mount_joint_position_controller
-            tentacle_left_mount_joint_position_controller
-            
-            tentacle_right_link_1_joint_position_controller
-            tentacle_right_link_2_joint_position_controller
-            tentacle_right_link_3_joint_position_controller
-            tentacle_right_link_4_joint_position_controller
-
-            tentacle_left_link_1_joint_position_controller
-            tentacle_left_link_2_joint_position_controller
-            tentacle_left_link_3_joint_position_controller
-            tentacle_left_link_4_joint_position_controller
-
-            tentacle_up_link_1_joint_position_controller
-            tentacle_up_link_2_joint_position_controller
-            tentacle_up_link_3_joint_position_controller
-            tentacle_up_link_4_joint_position_controller
-
-            tentacle_down_link_1_joint_position_controller
-            tentacle_down_link_2_joint_position_controller
-            tentacle_down_link_3_joint_position_controller
-            tentacle_down_link_4_joint_position_controller
-					  "/>
-
-  <!-- convert joint states to TF transforms for rviz, etc -->
-  <node name="tentacle_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
-	respawn="false" output="screen">
-    <remap from="/joint_states" to="/tentacle_state/joint_states" />
-  </node>
-
-</launch>
\ No newline at end of file
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_control/package.xml b/reaction_wheel_tentacle_ws/src/reaction_wheel_control/package.xml
deleted file mode 100644
index 4af7e22..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_control/package.xml
+++ /dev/null
@@ -1,30 +0,0 @@
-<?xml version="1.0"?>
-<package>
-  <name>reaction_wheel_control</name>
-  <version>0.0.0</version>
-  <description>The reaction_wheel_control package</description>
-
-  <maintainer email="davetcoleman@gmail.com">Dave Coleman</maintainer>
-
-  <license>BSD</license>
-
-  <url type="website">http://http://gazebosim.org/w/index.php?title=Tutorials/1.9/Using_A_URDF_In_Gazebo</url>
-  <url type="bugtracker">https://github.com/osrf/gazebo_ros_demos/issues</url>
-  <url type="repository">https://github.com/osrf/gazebo_ros_demos</url>
-
-  <author email="davetcoleman@gmail.com">Dave Coleman</author>
-
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <run_depend>controller_manager</run_depend>
-  <run_depend>joint_state_controller</run_depend>
-  <run_depend>robot_state_publisher</run_depend>
-
-  <run_depend>rqt_gui</run_depend>
-  <run_depend>effort_controllers</run_depend>
-
-
-  <export>
-  </export>
-
-</package>
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/CMakeLists.txt b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/CMakeLists.txt
deleted file mode 100644
index eb404bd..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/CMakeLists.txt
+++ /dev/null
@@ -1,212 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(reaction_wheel_main)
-
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
-
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-  gazebo_ros
-  roscpp
-  rospy
-  gazebo_plugins
-  tf2
-  tf2_ros
-  std_msgs
-  geometry_msgs
-)
-
-
-find_package(Boost REQUIRED COMPONENTS system)
-find_package (Eigen3 3.3 REQUIRED)
-find_package(gazebo REQUIRED)
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-link_directories(${GAZEBO_LIBRARY_DIRS})
-include_directories(${GAZEBO_INCLUDE_DIRS})
-include_directories(${catkin_INCLUDE_DIRS})
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-  DEPENDS
-  roscpp
-  gazebo_ros
-  std_msgs
-  geometry_msgs
-  gazebo_plugins
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-# include
-  ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/fusion2urdf.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/fusion2urdf_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
-# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_fusion2urdf.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/LICENSE b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/LICENSE
deleted file mode 100644
index 314c3af..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/LICENSE
+++ /dev/null
@@ -1,21 +0,0 @@
-MIT License
-
-Copyright (c) 2018 Toshinori Kitamura
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/launch/display.launch b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/launch/display.launch
deleted file mode 100644
index 8a2a4b1..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/launch/display.launch
+++ /dev/null
@@ -1,10 +0,0 @@
-<launch>
-  <arg name="model" default="$(find reaction_wheel_main)/urdf/reaction_wheel.xacro"/>
-  <arg name="gui" default="true"/>
-  <arg name="rvizconfig" default="$(find reaction_wheel_main)/launch/urdf.rviz"/>
-  <param name="robot_description" command="$(find xacro)/xacro $(arg model)"/>
-  <param name="use_gui" value="$(arg gui)"/>
-  <node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"/>
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
-  <node name="rviz" pkg="rviz" args="-d $(arg rvizconfig)" type="rviz" required="true"/>
-</launch>
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/launch/gazebo.launch b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/launch/gazebo.launch
deleted file mode 100644
index ce19678..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/launch/gazebo.launch
+++ /dev/null
@@ -1,22 +0,0 @@
-<launch>
-  <param name="robot_description" command="$(find xacro)/xacro $(find reaction_wheel_main)/urdf/reaction_wheel.xacro"/>
-  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model reaction_wheel -x 0 -y 0 -z 1"/>
-  <include file="$(find gazebo_ros)/launch/empty_world.launch">
-    <arg name="world_name" value="$(find reaction_wheel_main)/worlds/no_gravity.world"/>
-    <arg name="paused" value="false"/>
-    <arg name="use_sim_time" value="true"/>
-    <arg name="gui" value="true"/>
-    <arg name="headless" value="false"/>
-    <arg name="debug" value="false"/>
-  </include>
-
-  <include file="$(find reaction_wheel_control)/launch/reaction_wheel_velocity_control.launch"/>
-  <include file="$(find cubesat_description)/launch/gazebo.launch"/>
-
-  <node name="chaser_transition_position_throttle" pkg="topic_tools" type="throttle" args="messages /reaction_wheel_plugin/transition_position 50.0" />
-  <node name="chaser_transition_velocity_throttle" pkg="topic_tools" type="throttle" args="messages /reaction_wheel_plugin/transition_velocity 50.0" />
-  <node name="chaser_transition_acceleration_throttle" pkg="topic_tools" type="throttle" args="messages /reaction_wheel_plugin/transition_acceleration 50.0" />
-  <node name="chaser_quaternion_throttle" pkg="topic_tools" type="throttle" args="messages /reaction_wheel_plugin/quaternion 50.0" />
-  <node name="chaser_rotation_velocity_throttle" pkg="topic_tools" type="throttle" args="messages /reaction_wheel_plugin/rotation_velocity 50.0" />
-
-</launch>
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/launch/urdf.rviz b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/launch/urdf.rviz
deleted file mode 100644
index 53035f0..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/launch/urdf.rviz
+++ /dev/null
@@ -1,264 +0,0 @@
-Panels:
-  - Class: rviz/Displays
-    Help Height: 78
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /Status1
-        - /RobotModel1
-        - /TF1
-      Splitter Ratio: 0.5
-    Tree Height: 579
-  - Class: rviz/Selection
-    Name: Selection
-  - Class: rviz/Tool Properties
-    Expanded:
-      - /2D Pose Estimate1
-      - /2D Nav Goal1
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
-  - Class: rviz/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-  - Class: rviz/Time
-    Name: Time
-    SyncMode: 0
-    SyncSource: ""
-Preferences:
-  PromptSaveOnExit: true
-Toolbars:
-  toolButtonStyle: 2
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 10
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Alpha: 1
-      Class: rviz/RobotModel
-      Collision Enabled: false
-      Enabled: true
-      Links:
-        All Links Enabled: true
-        Expand Joint Details: false
-        Expand Link Details: false
-        Expand Tree: false
-        Link Tree Style: Links in Alphabetic Order
-        back_window:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        front_window:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        left_window:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        reaction_wheel_back:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        reaction_wheel_front:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        reaction_wheel_left:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        reaction_wheel_mount:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        reaction_wheel_right:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        right_window:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        thruster_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        thruster_10:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        thruster_11:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        thruster_12:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        thruster_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        thruster_3:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        thruster_4:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        thruster_5:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        thruster_6:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        thruster_7:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        thruster_8:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        thruster_9:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-      Name: RobotModel
-      Robot Description: robot_description
-      TF Prefix: ""
-      Update Interval: 0
-      Value: true
-      Visual Enabled: true
-    - Class: rviz/TF
-      Enabled: false
-      Filter (blacklist): ""
-      Filter (whitelist): ""
-      Frame Timeout: 15
-      Frames:
-        All Enabled: true
-      Marker Alpha: 1
-      Marker Scale: 0.5
-      Name: TF
-      Show Arrows: true
-      Show Axes: true
-      Show Names: true
-      Tree:
-        {}
-      Update Interval: 0
-      Value: false
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Default Light: true
-    Fixed Frame: base_link
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz/Interact
-      Hide Inactive Objects: true
-    - Class: rviz/MoveCamera
-    - Class: rviz/Select
-    - Class: rviz/FocusCamera
-    - Class: rviz/Measure
-    - Class: rviz/SetInitialPose
-      Theta std deviation: 0.2617993950843811
-      Topic: /initialpose
-      X std deviation: 0.5
-      Y std deviation: 0.5
-    - Class: rviz/SetGoal
-      Topic: /move_base_simple/goal
-    - Class: rviz/PublishPoint
-      Single click: true
-      Topic: /clicked_point
-  Value: true
-  Views:
-    Current:
-      Class: rviz/Orbit
-      Distance: 3.7193899154663086
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Field of View: 0.7853981852531433
-      Focal Point:
-        X: -0.0016349668148905039
-        Y: -0.014781012199819088
-        Z: 0.017814036458730698
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.8153961300849915
-      Target Frame: <Fixed Frame>
-      Yaw: 5.211766242980957
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 882
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002cefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ce000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002cefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002ce000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004c000000044fc0100000002fb0000000800540069006d00650100000000000004c0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000024f000002ce00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Time:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: false
-  Width: 1216
-  X: 700
-  Y: 161
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/base_camera.stl b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/base_camera.stl
deleted file mode 100644
index bf2d4c6..0000000
Binary files a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/base_camera.stl and /dev/null differ
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/base_joint_mount.stl b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/base_joint_mount.stl
deleted file mode 100644
index aebdfe1..0000000
Binary files a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/base_joint_mount.stl and /dev/null differ
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/base_link.stl b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/base_link.stl
deleted file mode 100644
index ff679e4..0000000
Binary files a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/base_link.stl and /dev/null differ
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/dummy.stl b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/dummy.stl
deleted file mode 100644
index cc84789..0000000
Binary files a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/dummy.stl and /dev/null differ
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/link_no_1.stl b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/link_no_1.stl
deleted file mode 100644
index 7026e95..0000000
Binary files a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/link_no_1.stl and /dev/null differ
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/link_no_2.stl b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/link_no_2.stl
deleted file mode 100644
index 181778e..0000000
Binary files a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/link_no_2.stl and /dev/null differ
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/link_no_3.stl b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/link_no_3.stl
deleted file mode 100644
index dc58664..0000000
Binary files a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/link_no_3.stl and /dev/null differ
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/link_no_4.stl b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/link_no_4.stl
deleted file mode 100644
index e4889da..0000000
Binary files a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/link_no_4.stl and /dev/null differ
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/reaction_wheel.stl b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/reaction_wheel.stl
deleted file mode 100644
index 7e07539..0000000
Binary files a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/reaction_wheel.stl and /dev/null differ
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/reaction_wheel_mount.stl b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/reaction_wheel_mount.stl
deleted file mode 100644
index dea9f05..0000000
Binary files a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/reaction_wheel_mount.stl and /dev/null differ
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/thruster.stl b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/thruster.stl
deleted file mode 100644
index 57e2946..0000000
Binary files a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/thruster.stl and /dev/null differ
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/window.stl b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/window.stl
deleted file mode 100644
index d5a8d13..0000000
Binary files a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/meshes/window.stl and /dev/null differ
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/package.xml b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/package.xml
deleted file mode 100644
index 2904684..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/package.xml
+++ /dev/null
@@ -1,38 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>reaction_wheel_main</name>
-  <version>0.0.0</version>
-<description>The sot_reaction_wheel_main package</description>
-
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="syuntoku14@todo.todo">syuntoku14</maintainer>
-
-
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
-
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <build_depend>rospy</build_depend>
-  <build_export_depend>rospy</build_export_depend>
-  <exec_depend>rospy</exec_depend>
-
-  <depend>std_msgs</depend>
-  <depend>roscpp</depend>
-  <depend>geometry_msgs</depend>
-  <depend>gazebo_ros</depend>
-  <depend>gazebo_plugins</depend>
-  <depend>gazebo_dev</depend>
-  <depend>rviz</depend>
-  <depend>robot_state_publisher</depend>
-  <depend>joint_state_publisher</depend>
-
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <gazebo_ros plugin_path="${prefix}/lib" gazebo_media_path="${prefix}" gazebo_model_path="${prefix}" />
-  </export>
-</package>
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/urdf/ft_sensor_plugin.xacro b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/urdf/ft_sensor_plugin.xacro
deleted file mode 100644
index 2e4436d..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/urdf/ft_sensor_plugin.xacro
+++ /dev/null
@@ -1,212 +0,0 @@
-<?xml version="1.0" ?>
-<robot name="reaction_wheel" xmlns:xacro="http://www.ros.org/wiki/xacro" >
-
-<gazebo reference="tentacle_right_link_1_joint">
-  <provideFeedback>true</provideFeedback>
-</gazebo>
-<gazebo>
-  <plugin name="tentacle_right_link_1_joint_ft_sensor_plugin" filename="libft_sensor_plugin.so">
-    <robotNamespace>/reaction_wheel</robotNamespace>
-    <jointName>tentacle_right_link_1_joint</jointName>
-    <topicName>/tentacle_right_link_1_joint_ft_sensor</topicName>
-    <gaussianNoise>0.0</gaussianNoise>
-    <updateRate>1.0</updateRate>
-  </plugin>
-</gazebo>
-
-<gazebo reference="tentacle_right_link_2_joint">
-  <provideFeedback>true</provideFeedback>
-</gazebo>
-<gazebo>
-  <plugin name="tentacle_right_link_2_joint_ft_sensor_plugin" filename="libft_sensor_plugin.so">
-    <robotNamespace>/reaction_wheel</robotNamespace>
-    <jointName>tentacle_right_link_2_joint</jointName>
-    <topicName>/tentacle_right_link_2_joint_ft_sensor</topicName>
-    <gaussianNoise>0.0</gaussianNoise>
-    <updateRate>1.0</updateRate>
-  </plugin>
-</gazebo>
- 
-<gazebo reference="tentacle_right_link_3_joint">
-  <provideFeedback>true</provideFeedback>
-</gazebo>
-<gazebo>
-  <plugin name="tentacle_right_link_3_joint_ft_sensor_plugin" filename="libft_sensor_plugin.so">
-    <robotNamespace>/reaction_wheel</robotNamespace>
-    <jointName>tentacle_right_link_3_joint</jointName>
-    <topicName>/tentacle_right_link_3_joint_ft_sensor</topicName>
-    <gaussianNoise>0.0</gaussianNoise>
-    <updateRate>1.0</updateRate>
-  </plugin>
-</gazebo>
- 
-<gazebo reference="tentacle_right_link_4_joint">
-  <provideFeedback>true</provideFeedback>
-</gazebo>
-<gazebo>
-  <plugin name="tentacle_right_link_4_joint_ft_sensor_plugin" filename="libft_sensor_plugin.so">
-    <robotNamespace>/reaction_wheel</robotNamespace>
-    <jointName>tentacle_right_link_4_joint</jointName>
-    <topicName>/tentacle_right_link_4_joint_ft_sensor</topicName>
-    <gaussianNoise>0.0</gaussianNoise>
-    <updateRate>1.0</updateRate>
-  </plugin>
-</gazebo>
-
-<gazebo reference="tentacle_left_link_1_joint">
-  <provideFeedback>true</provideFeedback>
-</gazebo>
-<gazebo>
-  <plugin name="tentacle_left_link_1_joint_ft_sensor_plugin" filename="libft_sensor_plugin.so">
-    <robotNamespace>/reaction_wheel</robotNamespace>
-    <jointName>tentacle_left_link_1_joint</jointName>
-    <topicName>/tentacle_left_link_1_joint_ft_sensor</topicName>
-    <gaussianNoise>0.0</gaussianNoise>
-    <updateRate>1.0</updateRate>
-  </plugin>
-</gazebo>
-
-<gazebo reference="tentacle_left_link_2_joint">
-  <provideFeedback>true</provideFeedback>
-</gazebo>
-<gazebo>
-  <plugin name="tentacle_left_link_2_joint_ft_sensor_plugin" filename="libft_sensor_plugin.so">
-    <robotNamespace>/reaction_wheel</robotNamespace>
-    <jointName>tentacle_left_link_2_joint</jointName>
-    <topicName>/tentacle_left_link_2_joint_ft_sensor</topicName>
-    <gaussianNoise>0.0</gaussianNoise>
-    <updateRate>1.0</updateRate>
-  </plugin>
-</gazebo>
- 
-<gazebo reference="tentacle_left_link_3_joint">
-  <provideFeedback>true</provideFeedback>
-</gazebo>
-<gazebo>
-  <plugin name="tentacle_left_link_3_joint_ft_sensor_plugin" filename="libft_sensor_plugin.so">
-    <robotNamespace>/reaction_wheel</robotNamespace>
-    <jointName>tentacle_left_link_3_joint</jointName>
-    <topicName>/tentacle_left_link_3_joint_ft_sensor</topicName>
-    <gaussianNoise>0.0</gaussianNoise>
-    <updateRate>1.0</updateRate>
-  </plugin>
-</gazebo>
- 
-<gazebo reference="tentacle_left_link_4_joint">
-  <provideFeedback>true</provideFeedback>
-</gazebo>
-<gazebo>
-  <plugin name="tentacle_left_link_4_joint_ft_sensor_plugin" filename="libft_sensor_plugin.so">
-    <robotNamespace>/reaction_wheel</robotNamespace>
-    <jointName>tentacle_left_link_4_joint</jointName>
-    <topicName>/tentacle_left_link_4_joint_ft_sensor</topicName>
-    <gaussianNoise>0.0</gaussianNoise>
-    <updateRate>1.0</updateRate>
-  </plugin>
-</gazebo>
-
-<gazebo reference="tentacle_up_link_1_joint">
-  <provideFeedback>true</provideFeedback>
-</gazebo>
-<gazebo>
-  <plugin name="tentacle_up_link_1_joint_ft_sensor_plugin" filename="libft_sensor_plugin.so">
-    <robotNamespace>/reaction_wheel</robotNamespace>
-    <jointName>tentacle_up_link_1_joint</jointName>
-    <topicName>/tentacle_up_link_1_joint_ft_sensor</topicName>
-    <gaussianNoise>0.0</gaussianNoise>
-    <updateRate>1.0</updateRate>
-  </plugin>
-</gazebo>
-
-<gazebo reference="tentacle_up_link_2_joint">
-  <provideFeedback>true</provideFeedback>
-</gazebo>
-<gazebo>
-  <plugin name="tentacle_up_link_2_joint_ft_sensor_plugin" filename="libft_sensor_plugin.so">
-    <robotNamespace>/reaction_wheel</robotNamespace>
-    <jointName>tentacle_up_link_2_joint</jointName>
-    <topicName>/tentacle_up_link_2_joint_ft_sensor</topicName>
-    <gaussianNoise>0.0</gaussianNoise>
-    <updateRate>1.0</updateRate>
-  </plugin>
-</gazebo>
-
-<gazebo reference="tentacle_up_link_3_joint">
-  <provideFeedback>true</provideFeedback>
-</gazebo>
-<gazebo>
-  <plugin name="tentacle_up_link_3_joint_ft_sensor_plugin" filename="libft_sensor_plugin.so">
-    <robotNamespace>/reaction_wheel</robotNamespace>
-    <jointName>tentacle_up_link_3_joint</jointName>
-    <topicName>/tentacle_up_link_3_joint_ft_sensor</topicName>
-    <gaussianNoise>0.0</gaussianNoise>
-    <updateRate>1.0</updateRate>
-  </plugin>
-</gazebo>
-
-<gazebo reference="tentacle_up_link_4_joint">
-  <provideFeedback>true</provideFeedback>
-</gazebo>
-<gazebo>
-  <plugin name="tentacle_up_link_4_joint_ft_sensor_plugin" filename="libft_sensor_plugin.so">
-    <robotNamespace>/reaction_wheel</robotNamespace>
-    <jointName>tentacle_up_link_4_joint</jointName>
-    <topicName>/tentacle_up_link_4_joint_ft_sensor</topicName>
-    <gaussianNoise>0.0</gaussianNoise>
-    <updateRate>1.0</updateRate>
-  </plugin>
-</gazebo>
-
-<gazebo reference="tentacle_down_link_1_joint">
-  <provideFeedback>true</provideFeedback>
-</gazebo>
-<gazebo>
-  <plugin name="tentacle_down_link_1_joint_ft_sensor_plugin" filename="libft_sensor_plugin.so">
-    <robotNamespace>/reaction_wheel</robotNamespace>
-    <jointName>tentacle_down_link_1_joint</jointName>
-    <topicName>/tentacle_down_link_1_joint_ft_sensor</topicName>
-    <gaussianNoise>0.0</gaussianNoise>
-    <updateRate>1.0</updateRate>
-  </plugin>
-</gazebo>
-
-<gazebo reference="tentacle_down_link_2_joint">
-  <provideFeedback>true</provideFeedback>
-</gazebo>
-<gazebo>
-  <plugin name="tentacle_down_link_2_joint_ft_sensor_plugin" filename="libft_sensor_plugin.so">
-    <robotNamespace>/reaction_wheel</robotNamespace>
-    <jointName>tentacle_down_link_2_joint</jointName>
-    <topicName>/tentacle_down_link_2_joint_ft_sensor</topicName>
-    <gaussianNoise>0.0</gaussianNoise>
-    <updateRate>1.0</updateRate>
-  </plugin>
-</gazebo>
-
-<gazebo reference="tentacle_down_link_3_joint">
-  <provideFeedback>true</provideFeedback>
-</gazebo>
-<gazebo>
-  <plugin name="tentacle_down_link_3_joint_ft_sensor_plugin" filename="libft_sensor_plugin.so">
-    <robotNamespace>/reaction_wheel</robotNamespace>
-    <jointName>tentacle_down_link_3_joint</jointName>
-    <topicName>/tentacle_down_link_3_joint_ft_sensor</topicName>
-    <gaussianNoise>0.0</gaussianNoise>
-    <updateRate>1.0</updateRate>
-  </plugin>
-</gazebo>
-
-<gazebo reference="tentacle_down_link_4_joint">
-  <provideFeedback>true</provideFeedback>
-</gazebo>
-<gazebo>
-  <plugin name="tentacle_down_link_4_joint_ft_sensor_plugin" filename="libft_sensor_plugin.so">
-    <robotNamespace>/reaction_wheel</robotNamespace>
-    <jointName>tentacle_down_link_4_joint</jointName>
-    <topicName>/tentacle_down_link_4_joint_ft_sensor</topicName>
-    <gaussianNoise>0.0</gaussianNoise>
-    <updateRate>1.0</updateRate>
-  </plugin>
-</gazebo>
-
-</robot>
\ No newline at end of file
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/urdf/materials.xacro b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/urdf/materials.xacro
deleted file mode 100644
index e889df4..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/urdf/materials.xacro
+++ /dev/null
@@ -1,36 +0,0 @@
-<?xml version="1.0" ?>
-<robot name="reaction_wheel" xmlns:xacro="http://www.ros.org/wiki/xacro" >
-
-<material name="silver">
-  <color rgba="0.500 0.500 0.500 1.000"/>
-</material>
-
-<material name="white">
-  <color rgba="1.0 1.0 1.0 1.000"/>
-</material>
-
-<material name="black">
-  <color rgba="0.0 0.0 0.0 1.000"/>
-</material>
-
-<material name="red">
-  <color rgba="1.0 0.0 0.0 1.000"/>
-</material>
-
-<material name="green">
-  <color rgba="0.0 1.0 0.0 1.000"/>
-</material>
-
-<material name="blue">
-  <color rgba="0.0 0.0 1.0 1.000"/>
-</material>
-
-<material name="yellow">
-  <color rgba="1.0 0.831 0.0 1.000"/>
-</material>
-
-<material name="brown">
-  <color rgba="0.588 0.294 0.0 1.000"/>
-</material>
-
-</robot>
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/urdf/reaction_wheel.gazebo b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/urdf/reaction_wheel.gazebo
deleted file mode 100644
index ce0c0a1..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/urdf/reaction_wheel.gazebo
+++ /dev/null
@@ -1,406 +0,0 @@
-<?xml version="1.0" ?>
-<robot name="reaction_wheel" xmlns:xacro="http://www.ros.org/wiki/xacro" >
-
-<xacro:include filename="$(find reaction_wheel_main)/urdf/ft_sensor_plugin.xacro" />
-
-<xacro:property name="body_color" value="Gazebo/Silver" />
-
-<gazebo>
-  <plugin name="control" filename="libgazebo_ros_control.so">
-    <robotNamespace>/reaction_wheel</robotNamespace>
-    <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
-  </plugin>
-</gazebo>
-
-<gazebo>
-  <plugin name="reaction_wheel_plugin" filename="libreaction_wheel_plugin.so">
-  </plugin>
-</gazebo>
-
-<gazebo reference="base_link">
-  <material>Gazebo/White</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-
-</gazebo>
-
-<gazebo reference="thruster_1">
-  <material>Gazebo/Yellow</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="thruster_2">
-  <material>Gazebo/Yellow</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="thruster_3">
-  <material>Gazebo/Yellow</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="thruster_4">
-  <material>Gazebo/Yellow</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="thruster_5">
-  <material>Gazebo/Yellow</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="thruster_6">
-  <material>Gazebo/Yellow</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="thruster_7">
-  <material>Gazebo/Yellow</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="thruster_8">
-  <material>Gazebo/Yellow</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="thruster_9">
-  <material>Gazebo/Yellow</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="thruster_10">
-  <material>Gazebo/Yellow</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="thruster_11">
-  <material>Gazebo/Yellow</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="thruster_12">
-  <material>Gazebo/Yellow</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="front_window">
-  <!--<material>${body_color}</material>--> <!-- intended invisible -->
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-<gazebo reference="back_window">
-  <!--<material>${body_color}</material>--> <!-- intended invisible -->
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-<gazebo reference="right_window">
-  <!--<material>${body_color}</material>--> <!-- intended invisible -->
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-<gazebo reference="left_window">
-  <!--<material>${body_color}</material>--> <!-- intended invisible -->
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="base_camera">
-  <material>Gazebo/DarkGrey</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-    <sensor name="camera" type="depth">
-        <update_rate>10</update_rate>
-        <always_on>1</always_on>
-        <visualize>true</visualize>
-        <camera>
-          <horizontal_fov>1.047198</horizontal_fov>
-          <image>
-            <width>640</width>
-            <height>480</height>
-            <format>R8G8B8</format>
-          </image>
-          <clip>
-            <near>0.05</near>
-            <far>3</far>
-          </clip>
-        </camera>
-        <plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
-          <baseline>0.2</baseline>
-          <alwaysOn>true</alwaysOn>
-          <!-- Keep this zero, update_rate in the parent <sensor> tag
-            will control the frame rate. -->
-          <updateRate>0</updateRate>
-          <cameraName>tof_camera</cameraName>
-          <imageTopicName>/base_camera/color/image_raw</imageTopicName>
-          <cameraInfoTopicName>/base_camera/color/camera_info</cameraInfoTopicName>
-          <depthImageTopicName>/base_camera/depth/image_raw</depthImageTopicName>
-          <depthImageInfoTopicName>/base_camera/depth/camera_info</depthImageInfoTopicName>
-          <pointCloudTopicName>/base_camera/depth/points</pointCloudTopicName>
-          <frameName>base_camera</frameName>
-          <pointCloudCutoff>0.05</pointCloudCutoff>
-          <distortionK1>0</distortionK1>
-          <distortionK2>0</distortionK2>
-          <distortionK3>0</distortionK3>
-          <distortionT1>0</distortionT1>
-          <distortionT2>0</distortionT2>
-          <CxPrime>0</CxPrime>
-          <Cx>0</Cx>
-          <Cy>0</Cy>
-          <focalLength>0</focalLength>
-          <hackBaseline>0</hackBaseline>
-  	</plugin>
-  </sensor>
-</gazebo>
-
-<gazebo reference="dummy">
-  <material>Gazebo/DarkGrey</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="reaction_wheel_mount">
-  <material>Gazebo/DarkGrey</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="reaction_wheel_front">
-  <material>Gazebo/Yellow</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="reaction_wheel_right">
-  <material>Gazebo/Red</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="reaction_wheel_left">
-  <material>Gazebo/Green</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="reaction_wheel_back">
-  <material>Gazebo/Blue</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="tentacle_right_mount">
-  <material>Gazebo/Black</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="tentacle_left_mount">
-  <material>Gazebo/Black</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="tentacle_up_mount">
-  <material>Gazebo/Black</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="tentacle_down_mount">
-  <material>Gazebo/Black</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="tentacle_right_link_1">
-  <material>Gazebo/Gray</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-<gazebo reference="tentacle_left_link_1">
-  <material>Gazebo/Gray</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-<gazebo reference="tentacle_right_link_2">
-  <material>Gazebo/Gray</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-<gazebo reference="tentacle_left_link_2">
-  <material>Gazebo/Gray</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-<gazebo reference="tentacle_right_link_3">
-  <material>Gazebo/Gray</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-<gazebo reference="tentacle_left_link_3">
-  <material>Gazebo/Gray</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-<gazebo reference="tentacle_right_link_4">
-  <material>Gazebo/Gray</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-<gazebo reference="tentacle_left_link_4">
-  <material>Gazebo/Gray</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="tentacle_up_link_1">
-  <material>Gazebo/Gray</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="tentacle_up_link_2">
-  <material>Gazebo/Gray</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="tentacle_up_link_3">
-  <material>Gazebo/Gray</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="tentacle_up_link_4">
-  <material>Gazebo/Gray</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-
-<gazebo reference="tentacle_down_link_1">
-  <material>Gazebo/Gray</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="tentacle_down_link_2">
-  <material>Gazebo/Gray</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="tentacle_down_link_3">
-  <material>Gazebo/Gray</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-<gazebo reference="tentacle_down_link_4">
-  <material>Gazebo/Gray</material>
-  <mu1>0.2</mu1>
-  <mu2>0.2</mu2>
-  <selfCollide>true</selfCollide>
-  <dynamics damping="0.0" friction="100.0"/>
-</gazebo>
-
-</robot>
\ No newline at end of file
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/urdf/reaction_wheel.trans b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/urdf/reaction_wheel.trans
deleted file mode 100644
index cdfd632..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/urdf/reaction_wheel.trans
+++ /dev/null
@@ -1,217 +0,0 @@
-<?xml version="1.0" ?>
-<robot name="reaction_wheel" xmlns:xacro="http://www.ros.org/wiki/xacro" >
-<!--
-<transmission name="{joint}_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="{joint}">
-    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="{joint}_actr">
-    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
--->
-<transmission name="reaction_wheel_front_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="reaction_wheel_front_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="reaction_wheel_front_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-<transmission name="reaction_wheel_right_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="reaction_wheel_right_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="reaction_wheel_right_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-<transmission name="reaction_wheel_left_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="reaction_wheel_left_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="reaction_wheel_left_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-<transmission name="reaction_wheel_back_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="reaction_wheel_back_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="reaction_wheel_back_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-
-<transmission name="tentacle_right_link_1_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="tentacle_right_link_1_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="tentacle_right_link_1_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-<transmission name="tentacle_left_link_1_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="tentacle_left_link_1_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="tentacle_left_link_1_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-
-<transmission name="tentacle_right_link_2_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="tentacle_right_link_2_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="tentacle_right_link_2_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-<transmission name="tentacle_left_link_2_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="tentacle_left_link_2_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="tentacle_left_link_2_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-<transmission name="tentacle_right_link_3_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="tentacle_right_link_3_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="tentacle_right_link_3_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-<transmission name="tentacle_left_link_3_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="tentacle_left_link_3_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="tentacle_left_link_3_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-<transmission name="tentacle_right_link_4_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="tentacle_right_link_4_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="tentacle_right_link_4_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-<transmission name="tentacle_left_link_4_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="tentacle_left_link_4_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="tentacle_left_link_4_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-<transmission name="tentacle_up_link_1_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="tentacle_up_link_1_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="tentacle_up_link_1_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-<transmission name="tentacle_up_link_2_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="tentacle_up_link_2_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="tentacle_up_link_2_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-<transmission name="tentacle_up_link_3_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="tentacle_up_link_3_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="tentacle_up_link_3_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-<transmission name="tentacle_up_link_4_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="tentacle_up_link_4_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="tentacle_up_link_4_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-<transmission name="tentacle_down_link_1_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="tentacle_down_link_1_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="tentacle_down_link_1_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-<transmission name="tentacle_down_link_2_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="tentacle_down_link_2_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="tentacle_down_link_2_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-<transmission name="tentacle_down_link_3_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="tentacle_down_link_3_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="tentacle_down_link_3_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-<transmission name="tentacle_down_link_4_joint_tran">
-  <type>transmission_interface/SimpleTransmission</type>
-  <joint name="tentacle_down_link_4_joint">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-  </joint>
-  <actuator name="tentacle_down_link_4_joint_actr">
-    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
-    <mechanicalReduction>1</mechanicalReduction>
-  </actuator>
-</transmission>
-</robot>
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/urdf/reaction_wheel.xacro b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/urdf/reaction_wheel.xacro
deleted file mode 100644
index c2a99a9..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/urdf/reaction_wheel.xacro
+++ /dev/null
@@ -1,1452 +0,0 @@
-<?xml version="1.0" ?>
-<robot name="reaction_wheel" xmlns:xacro="http://www.ros.org/wiki/xacro">
-
-<xacro:include filename="$(find reaction_wheel_main)/urdf/materials.xacro" />
-<xacro:include filename="$(find reaction_wheel_main)/urdf/reaction_wheel.trans" />
-<xacro:include filename="$(find reaction_wheel_main)/urdf/reaction_wheel.gazebo" />
-
-<xacro:property name="PI"                value="3.1415926535897931"/>
-<xacro:property name="pi"                value="3.1415926535897931"/>
-
-<!-- total base mass = 30 kg -->
-<xacro:property name="base_mass"         value="18.4"    />
-<xacro:property name="wheel_mass"        value="1"       /> <!-- X 4 -->
-<xacro:property name="mount_mass"        value="1"       />
-<xacro:property name="window_mass"       value="0.25"    /> <!-- X 4 -->
-<xacro:property name="thruster_mass"     value="0.05"    /> <!-- X 12 -->
-<xacro:property name="dummy_mass"        value="5"       />
-<xacro:property name="base_camera_mass"        value="0.001"       /> <!--unit : [kg]-->
-
-<!-- arm mass = 1kg -->
-<xacro:property name="link_1_mass"       value="3"     />
-<xacro:property name="link_2_mass"       value="2.5"     />
-<xacro:property name="link_3_mass"       value="2"     />
-<xacro:property name="link_4_mass"       value="1"    />
-<xacro:property name="link_mount_mass"   value="1"    />
-
-<xacro:property name="base_length"       value="1"       />
-<xacro:property name="base_thickness"    value="0.1"     />
-
-<xacro:property name="link_mount_length" value="0.225"   />
-<xacro:property name="link_mount_height" value="0.3"     />
-<xacro:property name="link_mount_width"  value="0.15"    />
-<xacro:property name="link_mount_circle" value="0.075"   />
-<xacro:property name="link_mount_offset" value="0.0385"  />
-
-<xacro:property name="link_1_height"     value="0.1"     />
-<xacro:property name="link_1_length"     value="1.35"    />
-<xacro:property name="link_1_width"      value="0.15"    />
-<xacro:property name="link_1_circle"     value="0.075"   />
-
-<xacro:property name="link_2_height"     value="0.1"     />
-<xacro:property name="link_2_length"     value="0.927654"/>
-<xacro:property name="link_2_width"      value="0.15"    />
-<xacro:property name="link_2_circle"     value="0.052654"/>
-
-<xacro:property name="link_3_height"     value="0.1"     />
-<xacro:property name="link_3_length"     value="0.688548"/>
-<xacro:property name="link_3_width"      value="0.105308"/>
-<xacro:property name="link_3_circle"     value="0.035894" />
-
-<xacro:property name="link_4_height"     value="0.1"     />
-<xacro:property name="link_4_length"     value="0.460616"/>
-<xacro:property name="link_4_width"      value="0.071789"/>
-<xacro:property name="link_4_circle"     value="0.024721" />
-
-<xacro:property name="mount_height"      value="0.35"    />
-<xacro:property name="mount_width"       value="0.7"     />
-<xacro:property name="mount_slide"       value="0.606218"/>
-
-<xacro:property name="dummy_length"      value="0.5"     />
-<xacro:property name="dummy_depth"       value="0.2"     />
-
-<xacro:property name="wheel_radius"      value="0.2"     />
-<xacro:property name="wheel_length"      value="0.1"     />
-
-<xacro:property name="thruster_radius"   value="0.05"    />
-<xacro:property name="thruster_length"   value="0.06"    />
-
-<xacro:property name="window_height"     value="0.8"     />
-<xacro:property name="window_width"      value="0.8"     />
-<xacro:property name="window_depth"      value="0.1"     />
-
-<xacro:property name="base_camera_length"      value="0.005"     /> <!--unit : [m]-->
-<!--==========================-->
-<link name="dummy_base_link">
-</link>
-<joint name="dummy_to_base_fixed_joint" type="fixed">
-  <parent link="dummy_base_link"/>
-  <child link="base_link"/>
-  <origin xyz="0 0 0" rpy="0 0 0"/>
-</joint>
-<!--==========================-->
-<link name="base_link">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/> <!--axis does not move. Visual and Collision part move.-->
-    <mass value="${base_mass}"/>
-    <inertia ixx="${1/6*base_mass*base_length*base_length}" 
-             iyy="${1/6*base_mass*base_length*base_length}" 
-             izz="${1/6*base_mass*base_length*base_length}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/base_link.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="white"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/base_link.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="thruster_1">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${thruster_mass}"/>
-    <inertia ixx="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             iyy="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             izz="${1/2*thruster_mass*thruster_radius*thruster_radius}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="yellow"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="thruster_2">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${thruster_mass}"/>
-    <inertia ixx="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             iyy="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             izz="${1/2*thruster_mass*thruster_radius*thruster_radius}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="yellow"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-<link name="thruster_3">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${thruster_mass}"/>
-    <inertia ixx="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             iyy="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             izz="${1/2*thruster_mass*thruster_radius*thruster_radius}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="yellow"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-<link name="thruster_7">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${thruster_mass}"/>
-    <inertia ixx="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             iyy="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             izz="${1/2*thruster_mass*thruster_radius*thruster_radius}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="yellow"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-<link name="thruster_8">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${thruster_mass}"/>
-    <inertia ixx="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             iyy="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             izz="${1/2*thruster_mass*thruster_radius*thruster_radius}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="yellow"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-<link name="thruster_9">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${thruster_mass}"/>
-    <inertia ixx="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             iyy="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             izz="${1/2*thruster_mass*thruster_radius*thruster_radius}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="yellow"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="thruster_4">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${thruster_mass}"/>
-    <inertia ixx="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             iyy="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             izz="${1/2*thruster_mass*thruster_radius*thruster_radius}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="yellow"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-<link name="thruster_5">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${thruster_mass}"/>
-    <inertia ixx="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             iyy="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             izz="${1/2*thruster_mass*thruster_radius*thruster_radius}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="yellow"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-<link name="thruster_6">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${thruster_mass}"/>
-    <inertia ixx="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             iyy="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             izz="${1/2*thruster_mass*thruster_radius*thruster_radius}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="yellow"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="thruster_10">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${thruster_mass}"/>
-    <inertia ixx="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             iyy="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             izz="${1/2*thruster_mass*thruster_radius*thruster_radius}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="yellow"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-<link name="thruster_11">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${thruster_mass}"/>
-    <inertia ixx="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             iyy="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             izz="${1/2*thruster_mass*thruster_radius*thruster_radius}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="yellow"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-<link name="thruster_12">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${thruster_mass}"/>
-    <inertia ixx="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             iyy="${1/12*thruster_mass*(3*thruster_radius*thruster_radius+thruster_length*thruster_length)}" 
-             izz="${1/2*thruster_mass*thruster_radius*thruster_radius}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="yellow"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/thruster.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="front_window">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${window_mass}"/>
-    <inertia ixx="${1/3*window_mass*window_height*window_width}" 
-             iyy="${1/3*window_mass*window_height*window_depth}" 
-             izz="${1/3*window_mass*window_width*window_depth}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <!--<visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/window.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>-->
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/window.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-<link name="right_window">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${window_mass}"/>
-    <inertia ixx="${1/3*window_mass*window_height*window_width}" 
-             iyy="${1/3*window_mass*window_height*window_depth}" 
-             izz="${1/3*window_mass*window_width*window_depth}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <!--<visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/window.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>-->
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/window.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-<link name="left_window">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${window_mass}"/>
-    <inertia ixx="${1/3*window_mass*window_height*window_width}" 
-             iyy="${1/3*window_mass*window_height*window_depth}" 
-             izz="${1/3*window_mass*window_width*window_depth}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <!--<visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/window.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>-->
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/window.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-<link name="back_window">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${window_mass}"/>
-    <inertia ixx="${1/3*window_mass*window_height*window_width}" 
-             iyy="${1/3*window_mass*window_height*window_depth}" 
-             izz="${1/3*window_mass*window_width*window_depth}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <!--<visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/window.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>-->
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/window.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<!-- ======================================== -->
-<link name="base_camera">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/> <!--axis does not move. Visual and Collision part move.-->
-    <mass value="${base_camera_mass}"/>
-    <inertia ixx="${1/6*base_camera_mass*base_camera_length*base_camera_length}" 
-             iyy="${1/6*base_camera_mass*base_camera_length*base_camera_length}" 
-             izz="${1/6*base_camera_mass*base_camera_length*base_camera_length}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/base_camera.stl" scale="0.01 0.01 0.01"/>
-    </geometry>
-    <material name="black"/>
-  </visual>
-  <!-- <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/base_camera.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision> -->
-</link>
-<!-- ======================================== -->
-<link name="dummy">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${dummy_mass}"/>
-    <inertia ixx="${1/3*dummy_mass*dummy_length*dummy_length}" 
-             iyy="${1/3*dummy_mass*dummy_length*dummy_depth}" 
-             izz="${1/3*dummy_mass*dummy_length*dummy_depth}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/dummy.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="brown"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/dummy.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="reaction_wheel_mount">
-  <inertial>
-    <origin xyz="0 0 -0.0875" rpy="0 0 0"/>
-    <mass value="${mount_mass}"/>
-    <inertia ixx="${1/20*mount_mass*(2*mount_width*mount_width)}" 
-             iyy="${1/20*mount_mass*(2*mount_width*mount_width)}" 
-             izz="${1/20*mount_mass*(2*mount_width*mount_width)}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/reaction_wheel_mount.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="brown"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/reaction_wheel_mount.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="reaction_wheel_front">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${wheel_mass}"/>
-    <inertia ixx="${1/12*wheel_mass*(3*wheel_radius*wheel_radius+wheel_length*wheel_length)}"
-             iyy="${1/12*wheel_mass*(3*wheel_radius*wheel_radius+wheel_length*wheel_length)}" 
-             izz="${1/2*wheel_mass*wheel_radius*wheel_radius}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-    
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/reaction_wheel.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="red"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/reaction_wheel.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="reaction_wheel_right">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${wheel_mass}"/>
-    <inertia ixx="${1/12*wheel_mass*(3*wheel_radius*wheel_radius+wheel_length*wheel_length)}"
-             iyy="${1/12*wheel_mass*(3*wheel_radius*wheel_radius+wheel_length*wheel_length)}"
-             izz="${1/2*wheel_mass*wheel_radius*wheel_radius}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-    
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/reaction_wheel.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="green"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/reaction_wheel.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="reaction_wheel_left">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${wheel_mass}"/>
-    <inertia ixx="${1/12*wheel_mass*(3*wheel_radius*wheel_radius+wheel_length*wheel_length)}" 
-             iyy="${1/12*wheel_mass*(3*wheel_radius*wheel_radius+wheel_length*wheel_length)}"
-             izz="${1/2*wheel_mass*wheel_radius*wheel_radius}" 
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-    
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/reaction_wheel.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="blue"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/reaction_wheel.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="reaction_wheel_back">
-  <inertial>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <mass value="${wheel_mass}"/>
-    <inertia ixx="${1/12*wheel_mass*(3*wheel_radius*wheel_radius+wheel_length*wheel_length)}"
-             iyy="${1/12*wheel_mass*(3*wheel_radius*wheel_radius+wheel_length*wheel_length)}" 
-             izz="${1/2*wheel_mass*wheel_radius*wheel_radius}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-    
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/reaction_wheel.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="yellow"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/reaction_wheel.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-
-</link>
-
-<!-- ================================================================================================ -->
-<!-- ================================================================================================ -->
-<!-- ================================================================================================ -->
-
-<link name="tentacle_right_mount">
-  <inertial>
-    <origin xyz="0.102783 0 0" rpy="0 0 0"/>
-    <mass value="${link_mount_mass}"/>
-    <inertia ixx="${1/6*link_mount_mass*link_mount_height*link_mount_width}" 
-             iyy="${1/6*link_mount_mass*link_mount_height*link_mount_length}"
-             izz="${1/6*link_mount_mass*link_mount_length*link_mount_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/base_joint_mount.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="black"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/base_joint_mount.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="tentacle_left_mount">
-  <inertial>
-    <origin xyz="0.102783 0 0" rpy="0 0 0"/>
-    <mass value="${link_mount_mass}"/>
-    <inertia ixx="${1/6*link_mount_mass*link_mount_height*link_mount_width}" 
-             iyy="${1/6*link_mount_mass*link_mount_height*link_mount_length}"
-             izz="${1/6*link_mount_mass*link_mount_length*link_mount_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/base_joint_mount.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="black"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/base_joint_mount.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="tentacle_up_mount">
-  <inertial>
-    <origin xyz="0.102783 0 0" rpy="0 0 0"/>
-    <mass value="${link_mount_mass}"/>
-    <inertia ixx="${1/6*link_mount_mass*link_mount_height*link_mount_width}" 
-             iyy="${1/6*link_mount_mass*link_mount_height*link_mount_length}"
-             izz="${1/6*link_mount_mass*link_mount_length*link_mount_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/base_joint_mount.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="black"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/base_joint_mount.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="tentacle_down_mount">
-  <inertial>
-    <origin xyz="0.102783 0 0" rpy="0 0 0"/>
-    <mass value="${link_mount_mass}"/>
-    <inertia ixx="${1/6*link_mount_mass*link_mount_height*link_mount_width}" 
-             iyy="${1/6*link_mount_mass*link_mount_height*link_mount_length}"
-             izz="${1/6*link_mount_mass*link_mount_length*link_mount_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/base_joint_mount.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="black"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/base_joint_mount.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="tentacle_right_link_1">
-  <inertial>
-    <origin xyz="0.6 0 0" rpy="0 0 0"/>
-    <mass value="${link_1_mass}"/>
-    <inertia ixx="${1/6*link_1_mass*link_1_height*link_1_width}"
-             iyy="${1/6*link_1_mass*link_1_height*link_1_length}" 
-             izz="${1/6*link_1_mass*link_1_length*link_1_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_1.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_1.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="tentacle_left_link_1">
-  <inertial>
-    <origin xyz="0.6 0 0" rpy="0 0 0"/>
-    <mass value="${link_1_mass}"/>
-    <inertia ixx="${1/6*link_1_mass*link_1_height*link_1_width}"
-             iyy="${1/6*link_1_mass*link_1_height*link_1_length}" 
-             izz="${1/6*link_1_mass*link_1_length*link_1_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_1.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_1.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="tentacle_right_link_2">
-  <inertial>
-    <origin xyz="0.262191 0 0" rpy="0 0 0"/>
-    <mass value="${link_2_mass}"/>
-    <inertia ixx="${1/6*link_2_mass*link_2_height*link_2_width}"
-             iyy="${1/6*link_2_mass*link_2_height*link_2_length}" 
-             izz="${1/6*link_2_mass*link_2_length*link_2_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_2.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_2.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="tentacle_left_link_2">
-  <inertial>
-    <origin xyz="0.262191 0 0" rpy="0 0 0"/>
-    <mass value="${link_2_mass}"/>
-    <inertia ixx="${1/6*link_2_mass*link_2_height*link_2_width}"
-             iyy="${1/6*link_2_mass*link_2_height*link_2_length}" 
-             izz="${1/6*link_2_mass*link_2_length*link_2_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_2.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_2.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="tentacle_left_link_3">
-  <inertial>
-    <origin xyz="0.270232 0 0" rpy="0 0 0"/>
-    <mass value="${link_3_mass}"/>
-    <inertia ixx="${1/6*link_3_mass*link_3_height*link_3_width}"
-             iyy="${1/6*link_3_mass*link_3_height*link_3_length}" 
-             izz="${1/6*link_3_mass*link_3_length*link_3_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_3.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_3.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="tentacle_right_link_3">
-  <inertial>
-    <origin xyz="0.270232 0 0" rpy="0 0 0"/>
-    <mass value="${link_3_mass}"/>
-    <inertia ixx="${1/6*link_3_mass*link_3_height*link_3_width}"
-             iyy="${1/6*link_3_mass*link_3_height*link_3_length}" 
-             izz="${1/6*link_3_mass*link_3_length*link_3_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_3.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_3.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="tentacle_right_link_4">
-  <inertial>
-    <origin xyz="0.180482 0 0" rpy="0 0 0"/>
-    <mass value="${link_4_mass}"/>
-    <inertia ixx="${1/6*link_4_mass*link_4_height*link_4_width}"
-             iyy="${1/6*link_4_mass*link_4_height*link_4_length}" 
-             izz="${1/6*link_4_mass*link_4_length*link_4_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-    
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_4.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_4.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-<link name="tentacle_left_link_4">
-  <inertial>
-    <origin xyz="0.180482 0 0" rpy="0 0 0"/>
-    <mass value="${link_4_mass}"/>
-    <inertia ixx="${1/6*link_4_mass*link_4_height*link_4_width}"
-             iyy="${1/6*link_4_mass*link_4_height*link_4_length}" 
-             izz="${1/6*link_4_mass*link_4_length*link_4_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_4.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_4.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="tentacle_up_link_1">
-  <inertial>
-    <origin xyz="0.6 0 0" rpy="0 0 0"/>
-    <mass value="${link_1_mass}"/>
-    <inertia ixx="${1/6*link_1_mass*link_1_height*link_1_width}"
-             iyy="${1/6*link_1_mass*link_1_height*link_1_length}" 
-             izz="${1/6*link_1_mass*link_1_length*link_1_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-    
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_1.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_1.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="tentacle_up_link_2">
-  <inertial>
-    <origin xyz="0.262191 0 0" rpy="0 0 0"/>
-    <mass value="${link_2_mass}"/>
-    <inertia ixx="${1/6*link_2_mass*link_2_height*link_2_width}"
-             iyy="${1/6*link_2_mass*link_2_height*link_2_length}" 
-             izz="${1/6*link_2_mass*link_2_length*link_2_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-    
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_2.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_2.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="tentacle_up_link_3">
-  <inertial>
-    <origin xyz="0.270232 0 0" rpy="0 0 0"/>
-    <mass value="${link_3_mass}"/>
-    <inertia ixx="${1/6*link_3_mass*link_3_height*link_3_width}"
-             iyy="${1/6*link_3_mass*link_3_height*link_3_length}" 
-             izz="${1/6*link_3_mass*link_3_length*link_3_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-    
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_3.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_3.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="tentacle_up_link_4">
-  <inertial>
-    <origin xyz="0.180482 0 0" rpy="0 0 0"/>
-    <mass value="${link_4_mass}"/>
-    <inertia ixx="${1/6*link_4_mass*link_4_height*link_4_width}"
-             iyy="${1/6*link_4_mass*link_4_height*link_4_length}" 
-             izz="${1/6*link_4_mass*link_4_length*link_4_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-    
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_4.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_4.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="tentacle_down_link_1">
-  <inertial>
-    <origin xyz="0.6 0 0" rpy="0 0 0"/>
-    <mass value="${link_1_mass}"/>
-    <inertia ixx="${1/6*link_1_mass*link_1_height*link_1_width}"
-             iyy="${1/6*link_1_mass*link_1_height*link_1_length}" 
-             izz="${1/6*link_1_mass*link_1_length*link_1_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-    
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_1.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_1.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="tentacle_down_link_2">
-  <inertial>
-    <origin xyz="0.262191 0 0" rpy="0 0 0"/>
-    <mass value="${link_2_mass}"/>
-    <inertia ixx="${1/6*link_2_mass*link_2_height*link_2_width}"
-             iyy="${1/6*link_2_mass*link_2_height*link_2_length}" 
-             izz="${1/6*link_2_mass*link_2_length*link_2_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-    
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_2.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_2.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="tentacle_down_link_3">
-  <inertial>
-    <origin xyz="0.270232 0 0" rpy="0 0 0"/>
-    <mass value="${link_3_mass}"/>
-    <inertia ixx="${1/6*link_3_mass*link_3_height*link_3_width}"
-             iyy="${1/6*link_3_mass*link_3_height*link_3_length}" 
-             izz="${1/6*link_3_mass*link_3_length*link_3_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-    
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_3.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_3.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-<link name="tentacle_down_link_4">
-  <inertial>
-    <origin xyz="0.180482 0 0" rpy="0 0 0"/>
-    <mass value="${link_4_mass}"/>
-    <inertia ixx="${1/6*link_4_mass*link_4_height*link_4_width}"
-             iyy="${1/6*link_4_mass*link_4_height*link_4_length}" 
-             izz="${1/6*link_4_mass*link_4_length*link_4_width}"
-             ixy="0.0" iyz="0.0" ixz="0.0"/>
-    
-  </inertial>
-  <visual>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_4.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-    <material name="silver"/>
-  </visual>
-  <collision>
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-    <geometry>
-      <mesh filename="package://reaction_wheel_main/meshes/link_no_4.stl" scale="0.001 0.001 0.001"/>
-    </geometry>
-  </collision>
-</link>
-
-
-<!-- ================================================================================================ -->
-
-<!--
-<joint name="joint_z" type="continuous">
-  <parent link="cube"/>
-  <child link="wheel_z"/>
-  <origin xyz="0 0 ${cube_length/2-wheel_length/2+offset}" rpy="0 0 0"/>
-  <axis xyz="0 0 1"/>
-  <dynamics damping="0.7" friction = "0.01"/>
-  <limit lower="-3.0" upper="3.0" effort="1000.0" velocity="0.5"/>
-
-</joint>
--->
-
-<joint name="mount_joint" type="fixed">
-  <parent link="base_link"/>
-  <child link="reaction_wheel_mount"/>
-  <origin xyz="0 0 -${base_length/2 - base_thickness - mount_height/2}" rpy="0 0 0"/>
-</joint>
-
-<joint name="dummy_joint" type="fixed">
-  <parent link="base_link"/>
-  <child link="dummy"/>
-  <origin xyz="0 0 ${base_length/2 - base_thickness - dummy_depth/2}" rpy="0 0 0"/>
-</joint>
-<!-- ================================================================================================ -->
-<joint name="tentacle_right_mount_joint" type="fixed">
-  <parent link="base_link"/>
-  <child link="tentacle_right_mount"/>
-  <origin xyz="${base_length/2 - link_mount_length/2 + link_mount_offset} -${base_length/2} 0" rpy="0 0 -${pi/2}"/>
-</joint>
-
-<joint name="tentacle_left_mount_joint" type="fixed">
-  <parent link="base_link"/>
-  <child link="tentacle_left_mount"/>
-  <origin xyz="${base_length/2 - link_mount_length/2 + link_mount_offset} ${base_length/2} 0" rpy="0 0 ${pi/2}"  />
-</joint>
-
-<joint name="tentacle_up_mount_joint" type="fixed">
-  <parent link="base_link"/>
-  <child link="tentacle_up_mount"/>
-  <origin xyz="${base_length/2 - link_mount_length/2 + link_mount_offset} 0 ${base_length/2}" rpy="${pi/2} -${pi/2} 0"  />
-</joint>
-
-<joint name="tentacle_down_mount_joint" type="fixed">
-  <parent link="base_link"/>
-  <child link="tentacle_down_mount"/>
-  <origin xyz="${base_length/2 - link_mount_length/2 + link_mount_offset} 0 -${base_length/2}" rpy="${pi/2} ${pi/2} 0"  />
-</joint>
-
-<!-- ================================================================================================ -->
-<joint name="front_window_joint" type="fixed">
-  <parent link="base_link"/>
-  <child link="front_window"/>
-  <origin xyz="${base_length/2 -base_thickness + window_depth/2} 0 0" rpy="0 ${pi/2} 0"/>
-</joint>
-<joint name="right_window_joint" type="fixed">
-  <parent link="base_link"/>
-  <child link="right_window"/>
-  <origin xyz="0 -${base_length/2 -base_thickness + window_depth/2} 0" rpy="${pi/2} ${pi/2} 0"/>
-</joint>
-<joint name="left_window_joint" type="fixed">
-  <parent link="base_link"/>
-  <child link="left_window"/>
-  <origin xyz="0 ${base_length/2 -base_thickness + window_depth/2} 0" rpy="-${pi/2} ${pi/2} 0"/>
-</joint>
-<joint name="back_window_joint" type="fixed">
-  <parent link="base_link"/>
-  <child link="back_window"/>
-  <origin xyz="-${base_length/2 -base_thickness + window_depth/2} 0 0" rpy="0 -${pi/2} 0"/>
-</joint>
-
-<joint name="base_camera_joint" type="fixed">
-  <parent link="front_window"/>
-  <child link="base_camera"/>
-  <origin xyz="0 0 0" rpy="0 -${pi/2} 0"/>
-</joint>
-
-<joint name="reaction_wheel_front_joint" type="continuous">
-  <parent link="reaction_wheel_mount"/>
-  <child link="reaction_wheel_front"/>
-  <origin xyz="${mount_width/4} 0.0 0.0 " rpy="0 ${pi/4} 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-
-<joint name="reaction_wheel_right_joint" type="continuous">
-  <parent link="reaction_wheel_mount"/>
-  <child link="reaction_wheel_right"/>
-  <origin xyz="0.0 -${mount_width/4} 0.0 " rpy="${pi/4} 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-
-<joint name="reaction_wheel_left_joint" type="continuous">
-  <parent link="reaction_wheel_mount"/>
-  <child link="reaction_wheel_left"/>
-  <origin xyz="0.0 ${mount_width/4} 0.0 " rpy="-${pi/4} 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-
-<joint name="reaction_wheel_back_joint" type="continuous">
-  <parent link="reaction_wheel_mount"/>
-  <child link="reaction_wheel_back"/>
-  <origin xyz="-${mount_width/4} 0.0 0.0 " rpy="0 -${pi/4} 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-
-<joint name="thruster_1_joint" type="continuous">
-  <parent link="base_link"/>
-  <child link="thruster_1"/>
-  <origin xyz="${base_length/2 - base_thickness/2 + thruster_length} ${base_length/2 - base_thickness/2} ${base_length/2 - base_thickness/2}" rpy="0 ${pi/2} 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-
-<joint name="thruster_2_joint" type="continuous">
-  <parent link="base_link"/>
-  <child link="thruster_2"/>
-  <origin xyz="${base_length/2 - base_thickness/2} ${base_length/2 - base_thickness/2 + thruster_length} ${base_length/2 - base_thickness/2}" rpy="-${pi/2} 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-
-<joint name="thruster_3_joint" type="continuous">
-  <parent link="base_link"/>
-  <child link="thruster_3"/>
-  <origin xyz="${base_length/2 - base_thickness/2} ${base_length/2 - base_thickness/2} ${base_length/2 - base_thickness/2 + thruster_length}" rpy="0 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-
-<joint name="thruster_7_joint" type="continuous">
-  <parent link="base_link"/>
-  <child link="thruster_7"/>
-  <origin xyz="-${base_length/2 - base_thickness/2 + thruster_length} -${base_length/2 - base_thickness/2} ${base_length/2 - base_thickness/2}" rpy="0 -${pi/2} 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-
-<joint name="thruster_8_joint" type="continuous">
-  <parent link="base_link"/>
-  <child link="thruster_8"/>
-  <origin xyz="-${base_length/2 - base_thickness/2} -${base_length/2 - base_thickness/2 + thruster_length} ${base_length/2 - base_thickness/2}" rpy="${pi/2} 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-
-<joint name="thruster_9_joint" type="continuous">
-  <parent link="base_link"/>
-  <child link="thruster_9"/>
-  <origin xyz="-${base_length/2 - base_thickness/2} -${base_length/2 - base_thickness/2} ${base_length/2 - base_thickness/2 + thruster_length}" rpy="0 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-
-<joint name="thruster_4_joint" type="continuous">
-  <parent link="base_link"/>
-  <child link="thruster_4"/>
-  <origin xyz="-${base_length/2 - base_thickness/2 + thruster_length} ${base_length/2 - base_thickness/2} -${base_length/2 - base_thickness/2}" rpy="0 -${pi/2} 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-
-<joint name="thruster_5_joint" type="continuous">
-  <parent link="base_link"/>
-  <child link="thruster_5"/>
-  <origin xyz="-${base_length/2 - base_thickness/2} ${base_length/2 - base_thickness/2 + thruster_length} -${base_length/2 - base_thickness/2}" rpy="-${pi/2} 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-
-<joint name="thruster_6_joint" type="continuous">
-  <parent link="base_link"/>
-  <child link="thruster_6"/>
-  <origin xyz="-${base_length/2 - base_thickness/2} ${base_length/2 - base_thickness/2} -${base_length/2 - base_thickness/2 + thruster_length}" rpy="${pi} 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-
-<joint name="thruster_10_joint" type="continuous">
-  <parent link="base_link"/>
-  <child link="thruster_10"/>
-  <origin xyz="${base_length/2 - base_thickness/2 + thruster_length} -${base_length/2 - base_thickness/2} -${base_length/2 - base_thickness/2}" rpy="0 ${pi/2} 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-
-<joint name="thruster_11_joint" type="continuous">
-  <parent link="base_link"/>
-  <child link="thruster_11"/>
-  <origin xyz="${base_length/2 - base_thickness/2} -${base_length/2 - base_thickness/2 + thruster_length} -${base_length/2 - base_thickness/2}" rpy="${pi/2} 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-
-<joint name="thruster_12_joint" type="continuous">
-  <parent link="base_link"/>
-  <child link="thruster_12"/>
-  <origin xyz="${base_length/2 - base_thickness/2} -${base_length/2 - base_thickness/2} -${base_length/2 - base_thickness/2 + thruster_length}" rpy="-${pi} 0 0"/>
-  <axis xyz="0 0 1"/>
-
-</joint>
-<!-- ================================================================================================ -->
-<joint name="tentacle_right_link_1_joint" type="continuous">
-  <parent link="tentacle_right_mount"/>
-  <child link="tentacle_right_link_1"/>
-  <origin xyz="${link_mount_length-link_mount_circle} 0 0" rpy="0 0 ${pi/2}"/>
-  <axis xyz="0 0 1"/>
-  <!-- <physics>
-    <provide_feedback>true</provide_feedback>
-  </physics>
-  <sensor name="force_torque" type="force_torque">
-    <update_rate>1.0</update_rate>
-  </sensor> -->
-</joint>
-
-<joint name="tentacle_left_link_1_joint" type="continuous">
-  <parent link="tentacle_left_mount"/>
-  <child link="tentacle_left_link_1"/>
-  <origin xyz="${link_mount_length-link_mount_circle} 0 0" rpy="0 0 -${pi/2}"/>
-  <axis xyz="0 0 1"/>
-</joint>
-<!-- ================================================================================================ -->
-<joint name="tentacle_right_link_2_joint" type="continuous">
-  <parent link="tentacle_right_link_1"/>
-  <child link="tentacle_right_link_2"/>
-  <origin xyz="${link_1_length-link_1_circle*2} 0 0" rpy="0 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-<joint name="tentacle_left_link_2_joint" type="continuous">
-  <parent link="tentacle_left_link_1"/>
-  <child link="tentacle_left_link_2"/>
-  <origin xyz="${link_1_length-link_1_circle*2} 0 0" rpy="0 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-<!-- ================================================================================================ -->
-<joint name="tentacle_right_link_3_joint" type="continuous">
-  <parent link="tentacle_right_link_2"/>
-  <child link="tentacle_right_link_3"/>
-  <origin xyz="${link_2_length-link_2_circle*2} 0 0" rpy="0 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-<joint name="tentacle_left_link_3_joint" type="continuous">
-  <parent link="tentacle_left_link_2"/>
-  <child link="tentacle_left_link_3"/>
-  <origin xyz="${link_2_length-link_2_circle*2} 0 0" rpy="0 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-<!-- ================================================================================================ -->
-<joint name="tentacle_right_link_4_joint" type="continuous">
-  <parent link="tentacle_right_link_3"/>
-  <child link="tentacle_right_link_4"/>
-  <origin xyz="${link_3_length-link_3_circle*2} 0 0" rpy="0 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-<joint name="tentacle_left_link_4_joint" type="continuous">
-  <parent link="tentacle_left_link_3"/>
-  <child link="tentacle_left_link_4"/>
-  <origin xyz="${link_3_length-link_3_circle*2} 0 0" rpy="0 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-<!-- ================================================================================================ -->
-<joint name="tentacle_up_link_1_joint" type="continuous">
-  <parent link="tentacle_up_mount"/>
-  <child link="tentacle_up_link_1"/>
-  <origin xyz="${link_mount_length-link_mount_circle} 0 0" rpy="${pi} 0 -${pi/2}"/>
-  <axis xyz="0 0 1"/>
-</joint>
-<joint name="tentacle_down_link_1_joint" type="continuous">
-  <parent link="tentacle_down_mount"/>
-  <child link="tentacle_down_link_1"/>
-  <origin xyz="${link_mount_length-link_mount_circle} 0 0" rpy="-${pi} 0 ${pi/2}"/>
-  <axis xyz="0 0 1"/>
-</joint>
-<!-- ================================================================================================ -->
-<joint name="tentacle_up_link_2_joint" type="continuous">
-  <parent link="tentacle_up_link_1"/>
-  <child link="tentacle_up_link_2"/>
-  <origin xyz="${link_1_length-link_1_circle*2} 0 0" rpy="0 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-<joint name="tentacle_down_link_2_joint" type="continuous">
-  <parent link="tentacle_down_link_1"/>
-  <child link="tentacle_down_link_2"/>
-  <origin xyz="${link_1_length-link_1_circle*2} 0 0" rpy="0 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-<!-- ================================================================================================ -->
-<joint name="tentacle_up_link_3_joint" type="continuous">
-  <parent link="tentacle_up_link_2"/>
-  <child link="tentacle_up_link_3"/>
-  <origin xyz="${link_2_length-link_2_circle*2} 0 0" rpy="0 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-<joint name="tentacle_down_link_3_joint" type="continuous">
-  <parent link="tentacle_down_link_2"/>
-  <child link="tentacle_down_link_3"/>
-  <origin xyz="${link_2_length-link_2_circle*2} 0 0" rpy="0 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-<!-- ================================================================================================ -->
-<joint name="tentacle_up_link_4_joint" type="continuous">
-  <parent link="tentacle_up_link_3"/>
-  <child link="tentacle_up_link_4"/>
-  <origin xyz="${link_3_length-link_3_circle*2} 0 0" rpy="0 0 0"/>
-  <axis xyz="0 0 1"/>
-</joint>
-<joint name="tentacle_down_link_4_joint" type="continuous">
-  <parent link="tentacle_down_link_3"/>
-  <child link="tentacle_down_link_4"/>
-  <origin xyz="${link_3_length-link_3_circle*2} 0 0" rpy="0 0 0"/>
-  <axis xyz="0 0 1"/>
-  <!-- <sensor name="force_torque" type="force_torque">
-    <update_rate>100.0<update_rate>
-  </sensor> -->
-</joint>
-<!-- ================================================================================================ -->
-</robot>
\ No newline at end of file
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/worlds/no_gravity.world b/reaction_wheel_tentacle_ws/src/reaction_wheel_main/worlds/no_gravity.world
deleted file mode 100644
index 1f7422b..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_main/worlds/no_gravity.world
+++ /dev/null
@@ -1,32 +0,0 @@
-<?xml version='1.0'?>
-<sdf version='1.7'>
-
-  <world name="default">
-    <gravity>0 0 0</gravity>
-
-    <!-- Global light source -->
-    <include>
-      <uri>model://sun</uri>
-    </include>
-    <scene>
-      <ambient>0.5 0.5 0.5 1</ambient>
-      <background>0.0 0.0 0.0 1</background>
-      <shadows>false</shadows>
-    </scene>
-    
-    <physics type="ode">
-    	<max_step_size>0.001</max_step_size>
-    	<real_time_update_rate>1000</real_time_update_rate>
-    </physics>
-
-    <gui fullscreen='0'>
-      <camera name='user_camera'>
-        <pose>5.927360 -4.376610 2.0 0.000000 0.275643 2.356190</pose>
-        <view_controller>orbit</view_controller>
-      </camera>
-    </gui>
-
-  </world>
-
-
-</sdf>
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_plugin/CMakeLists.txt b/reaction_wheel_tentacle_ws/src/reaction_wheel_plugin/CMakeLists.txt
deleted file mode 100644
index 6395430..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_plugin/CMakeLists.txt
+++ /dev/null
@@ -1,55 +0,0 @@
-cmake_minimum_required(VERSION 3.0.2)
-project(reaction_wheel_plugin)
-
-add_compile_options(-std=c++11)
-
-find_package(catkin REQUIRED COMPONENTS
-  gazebo_ros
-  roscpp
-  rospy
-  std_msgs
-  geometry_msgs
-  gazebo_plugins
-)
-
-find_package(Boost REQUIRED COMPONENTS system)
-find_package (Eigen3 3.3 REQUIRED)
-find_package(gazebo REQUIRED)
-
-link_directories(${GAZEBO_LIBRARY_DIRS})
-include_directories(${GAZEBO_INCLUDE_DIRS})
-include_directories(${catkin_INCLUDE_DIRS})
-##++++++++++++++++++++++++++++++##
-include_directories(
-  include
-  ${Boost_INCLUDE_DIRS}
-  ${catkin_INCLUDE_DIRS}
-  )
-##++++++++++++++++++++++++++++++##
-list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
-
-catkin_package(
-  DEPENDS
-  roscpp
-  gazebo_ros
-  std_msgs
-  geometry_msgs
-)
-##+++++++++++++++++++++++++++++##
-add_library(reaction_wheel_plugin SHARED src/reaction_wheel_plugin.cpp)
-##+++++++++++++++++++++++++++++##
-
-add_dependencies(reaction_wheel_plugin ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
-
-install(TARGETS reaction_wheel_plugin
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-)
-
-install(FILES
-reaction_wheel_plugin.xml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-##+++++++++++++++++++++++++++++##
-target_link_libraries(reaction_wheel_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
\ No newline at end of file
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_plugin/package.xml b/reaction_wheel_tentacle_ws/src/reaction_wheel_plugin/package.xml
deleted file mode 100644
index 1b9b871..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_plugin/package.xml
+++ /dev/null
@@ -1,35 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>reaction_wheel_plugin</name>
-  <version>0.0.1</version>
-  <description>virtual satellite plugin package</description>
-
-  <maintainer email="michael1015999@gmail.com">Minsik Oh</maintainer>
-
-  <license>MIT</license>
-
-  <buildtool_depend>catkin</buildtool_depend>
-  
-  <depend>std_msgs</depend>
-  <depend>roscpp</depend>
-  <depend>geometry_msgs</depend>
-  
-  <build_depend>gazebo_ros</build_depend>
-  <build_depend>aruco_ros</build_depend>
-  <build_depend>gazebo_plugins</build_depend>
-  <!--++++++++++++++++++++++++++++++++++++++-->
-  <build_depend>gazebo_dev</build_depend>
-  <!--++++++++++++++++++++++++++++++++++++++-->
-  <build_export_depend>gazebo_ros</build_export_depend>
-  <build_export_depend>gazebo_plugins</build_export_depend>
-  
-  <!--++++++++++++++++++++++++++++++++++++++-->
-  <exec_depend>gazebo_dev</exec_depend>
-  <!--++++++++++++++++++++++++++++++++++++++-->
-  <exec_depend>gazebo_ros</exec_depend>
-  <exec_depend>aruco_ros</exec_depend>
-  <exec_depend>gazebo_plugins</exec_depend>
-  <export>
-    <gazebo_ros plugin_path="${prefix}/lib" gazebo_media_path="${prefix}" gazebo_model_path="${prefix}" />
-  </export>
-</package>
diff --git a/reaction_wheel_tentacle_ws/src/reaction_wheel_plugin/src/reaction_wheel_plugin.cpp b/reaction_wheel_tentacle_ws/src/reaction_wheel_plugin/src/reaction_wheel_plugin.cpp
deleted file mode 100644
index 47dc3b8..0000000
--- a/reaction_wheel_tentacle_ws/src/reaction_wheel_plugin/src/reaction_wheel_plugin.cpp
+++ /dev/null
@@ -1,342 +0,0 @@
-// C++ standard header
-#include <functional>
-#include <iostream>
-#include <sstream>
-#include <thread>
-
-// ros header with message header
-#include <ros/ros.h>
-#include <ros/callback_queue.h>
-#include <ros/subscribe_options.h>
-#include <geometry_msgs/Pose.h>
-#include <geometry_msgs/Vector3.h>
-#include <geometry_msgs/Vector3Stamped.h>
-#include <geometry_msgs/Quaternion.h>
-#include <geometry_msgs/QuaternionStamped.h>
-
-// gazebo header 
-#include <gazebo/gazebo.hh>
-#include <gazebo/physics/physics.hh>
-#include <gazebo/physics/Model.hh>
-#include <gazebo/physics/Link.hh>
-#include <gazebo/physics/World.hh>
-#include <gazebo/common/common.hh>
-#include <gazebo/msgs/msgs.hh>
-
-// ignition header
-#include <ignition/math/Vector3.hh>
-#include <ignition/math/Quaternion.hh>
-#include <ignition/math/Matrix3.hh>
-#include <ignition/math/Pose3.hh>
-
-// tf2 header
-#include <tf2/LinearMath/Quaternion.h>
-#include <tf2/LinearMath/Vector3.h>
-#include <tf2/LinearMath/Matrix3x3.h>
-
-// there are un-used headers that maybe will be utilized later.
-// After development, Un-used headers should be removed for simplicity. 
-
-// Example of custom message publish code
-// m_publisher = nh_for_group.advertise<my_robotics_pkg::MyCustomMessageType>("great_custom_topic", queue_size, should_latch);
-
-namespace gazebo
-{
-  class reaction_wheel_plugin : public ModelPlugin
-  // Define plugin name for get plugined model data => Make sure that plugin description should be written in .xacro or .sdf etc files.
-  {
-  public:
-    void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) // Get model's object in gazebo world
-    {
-      this->model = _model; // Get model
-
-      // Initialize ROS
-      if (!ros::isInitialized())
-      {
-        int argc = 0;
-        char **argv = nullptr;
-        // Make handler if ros doesn't start
-        ros::init(argc, argv, "reaction_wheel_plugin", ros::init_options::NoSigintHandler);
-      }
-      // Create a ROS node handler
-      this->rosNode.reset(new ros::NodeHandle("reaction_wheel_plugin")); // make node handler and reset node
-      
-      ros::SubscribeOptions torqueSO = ros::SubscribeOptions::create<geometry_msgs::Vector3>("/torque",1,boost::bind(&reaction_wheel_plugin::ApplyTorque, this, _1),
-      ros::VoidPtr(), &this->rosQueue);
-      ros::SubscribeOptions forceSO = ros::SubscribeOptions::create<geometry_msgs::Vector3>("/force",1,boost::bind(&reaction_wheel_plugin::ApplyForce, this, _1),
-      ros::VoidPtr(), &this->rosQueue);
-
-      ros::SubscribeOptions thrusterSO_1 = ros::SubscribeOptions::create<geometry_msgs::Vector3>("/thruster_1",1,boost::bind(&reaction_wheel_plugin::ApplyThrust1, this, _1),
-      ros::VoidPtr(), &this->rosQueue);
-      ros::SubscribeOptions thrusterSO_2 = ros::SubscribeOptions::create<geometry_msgs::Vector3>("/thruster_2",1,boost::bind(&reaction_wheel_plugin::ApplyThrust2, this, _1),
-      ros::VoidPtr(), &this->rosQueue);
-      ros::SubscribeOptions thrusterSO_3 = ros::SubscribeOptions::create<geometry_msgs::Vector3>("/thruster_3",1,boost::bind(&reaction_wheel_plugin::ApplyThrust3, this, _1),
-      ros::VoidPtr(), &this->rosQueue);
-      ros::SubscribeOptions thrusterSO_4 = ros::SubscribeOptions::create<geometry_msgs::Vector3>("/thruster_4",1,boost::bind(&reaction_wheel_plugin::ApplyThrust4, this, _1),
-      ros::VoidPtr(), &this->rosQueue);
-      ros::SubscribeOptions thrusterSO_5 = ros::SubscribeOptions::create<geometry_msgs::Vector3>("/thruster_5",1,boost::bind(&reaction_wheel_plugin::ApplyThrust5, this, _1),
-      ros::VoidPtr(), &this->rosQueue);
-      ros::SubscribeOptions thrusterSO_6 = ros::SubscribeOptions::create<geometry_msgs::Vector3>("/thruster_6",1,boost::bind(&reaction_wheel_plugin::ApplyThrust6, this, _1),
-      ros::VoidPtr(), &this->rosQueue);
-      ros::SubscribeOptions thrusterSO_7 = ros::SubscribeOptions::create<geometry_msgs::Vector3>("/thruster_7",1,boost::bind(&reaction_wheel_plugin::ApplyThrust7, this, _1),
-      ros::VoidPtr(), &this->rosQueue);
-      ros::SubscribeOptions thrusterSO_8 = ros::SubscribeOptions::create<geometry_msgs::Vector3>("/thruster_8",1,boost::bind(&reaction_wheel_plugin::ApplyThrust8, this, _1),
-      ros::VoidPtr(), &this->rosQueue);
-      ros::SubscribeOptions thrusterSO_9 = ros::SubscribeOptions::create<geometry_msgs::Vector3>("/thruster_9",1,boost::bind(&reaction_wheel_plugin::ApplyThrust9, this, _1),
-      ros::VoidPtr(), &this->rosQueue);
-      ros::SubscribeOptions thrusterSO_10 = ros::SubscribeOptions::create<geometry_msgs::Vector3>("/thruster_10",1,boost::bind(&reaction_wheel_plugin::ApplyThrust10, this, _1),
-      ros::VoidPtr(), &this->rosQueue);
-      ros::SubscribeOptions thrusterSO_11 = ros::SubscribeOptions::create<geometry_msgs::Vector3>("/thruster_11",1,boost::bind(&reaction_wheel_plugin::ApplyThrust11, this, _1),
-      ros::VoidPtr(), &this->rosQueue);
-      ros::SubscribeOptions thrusterSO_12 = ros::SubscribeOptions::create<geometry_msgs::Vector3>("/thruster_12",1,boost::bind(&reaction_wheel_plugin::ApplyThrust12, this, _1),
-      ros::VoidPtr(), &this->rosQueue);
-
-
-      this->TorqueSub = this->rosNode->subscribe(torqueSO);
-      this->ForceSub = this->rosNode->subscribe(forceSO);
-
-      this->ThrustSub1 = this->rosNode->subscribe(thrusterSO_1);
-      this->ThrustSub2 = this->rosNode->subscribe(thrusterSO_2);
-      this->ThrustSub3 = this->rosNode->subscribe(thrusterSO_3);
-      this->ThrustSub4 = this->rosNode->subscribe(thrusterSO_4);
-      this->ThrustSub5 = this->rosNode->subscribe(thrusterSO_5);
-      this->ThrustSub6 = this->rosNode->subscribe(thrusterSO_6);
-      this->ThrustSub7 = this->rosNode->subscribe(thrusterSO_7);
-      this->ThrustSub8 = this->rosNode->subscribe(thrusterSO_8);
-      this->ThrustSub9 = this->rosNode->subscribe(thrusterSO_9);
-      this->ThrustSub10 = this->rosNode->subscribe(thrusterSO_10);
-      this->ThrustSub11 = this->rosNode->subscribe(thrusterSO_11);
-      this->ThrustSub12 = this->rosNode->subscribe(thrusterSO_12);
-
-      this->rosQueueThread = std::thread(std::bind(&reaction_wheel_plugin::QueueThread, this));
-      
-      // Publish "translation" topics
-      this->translationPositionPub = this->rosNode->advertise<geometry_msgs::Vector3>("transition_position", 1);
-      this->translationVelocityPub = this->rosNode->advertise<geometry_msgs::Vector3>("transition_velocity", 1);
-      this->translationAccelerationPub = this->rosNode->advertise<geometry_msgs::Vector3>("transition_acceleration", 1);
-      
-      // Publish "angular" topics
-      this->angularPositionPub = this->rosNode->advertise<geometry_msgs::Vector3>("rotation_position", 1);
-      this->angularVelocityPub = this->rosNode->advertise<geometry_msgs::Vector3>("rotation_velocity", 1); //++++
-      this->angularAccelerationPub = this->rosNode->advertise<geometry_msgs::Vector3>("rotation_acceleration", 1);
-      
-      // Publish "quaternion" topics
-      this->quaternionPub = this->rosNode->advertise<geometry_msgs::Quaternion>("quaternion",1);
-
-      // Update event to ROS and Gazeboc
-      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
-          std::bind(&reaction_wheel_plugin::OnUpdate, this)); 
-    }
-    
-    void ApplyTorque(const geometry_msgs::Vector3::ConstPtr& _msg)
-    {
-      ignition::math::Vector3d t(_msg->x, _msg->y, _msg->z);
-      this->model->GetLink("dummy_base_link")->AddRelativeTorque(t);
-    }
-
-    void ApplyForce(const geometry_msgs::Vector3::ConstPtr& _msg)
-    {
-      ignition::math::Vector3d t(_msg->x, _msg->y, _msg->z);
-      this->model->GetLink("dummy_base_link")->AddRelativeForce(t);
-    }
-
-    void ApplyThrust1(const geometry_msgs::Vector3::ConstPtr& _msg)
-    {
-      ignition::math::Vector3d t(_msg->x, _msg->y, -_msg->z);
-      this->model->GetLink("thruster_1")->AddRelativeForce(t);
-    }
-    void ApplyThrust2(const geometry_msgs::Vector3::ConstPtr& _msg)
-    {
-      ignition::math::Vector3d t(_msg->x, _msg->y, -_msg->z);
-      this->model->GetLink("thruster_2")->AddRelativeForce(t);
-    }
-    void ApplyThrust3(const geometry_msgs::Vector3::ConstPtr& _msg)
-    {
-      ignition::math::Vector3d t(_msg->x, _msg->y, -_msg->z);
-      this->model->GetLink("thruster_3")->AddRelativeForce(t);
-    }
-    void ApplyThrust4(const geometry_msgs::Vector3::ConstPtr& _msg)
-    {
-      ignition::math::Vector3d t(_msg->x, _msg->y, -_msg->z);
-      this->model->GetLink("thruster_4")->AddRelativeForce(t);
-    }
-    void ApplyThrust5(const geometry_msgs::Vector3::ConstPtr& _msg)
-    {
-      ignition::math::Vector3d t(_msg->x, _msg->y, -_msg->z);
-      this->model->GetLink("thruster_5")->AddRelativeForce(t);
-    }
-    void ApplyThrust6(const geometry_msgs::Vector3::ConstPtr& _msg)
-    {
-      ignition::math::Vector3d t(_msg->x, _msg->y, -_msg->z);
-      this->model->GetLink("thruster_6")->AddRelativeForce(t);
-    }
-    void ApplyThrust7(const geometry_msgs::Vector3::ConstPtr& _msg)
-    {
-      ignition::math::Vector3d t(_msg->x, _msg->y, -_msg->z);
-      this->model->GetLink("thruster_7")->AddRelativeForce(t);
-    }
-    void ApplyThrust8(const geometry_msgs::Vector3::ConstPtr& _msg)
-    {
-      ignition::math::Vector3d t(_msg->x, _msg->y, -_msg->z);
-      this->model->GetLink("thruster_8")->AddRelativeForce(t);
-    }
-    void ApplyThrust9(const geometry_msgs::Vector3::ConstPtr& _msg)
-    {
-      ignition::math::Vector3d t(_msg->x, _msg->y, -_msg->z);
-      this->model->GetLink("thruster_9")->AddRelativeForce(t);
-    }
-    void ApplyThrust10(const geometry_msgs::Vector3::ConstPtr& _msg)
-    {
-      ignition::math::Vector3d t(_msg->x, _msg->y, -_msg->z);
-      this->model->GetLink("thruster_10")->AddRelativeForce(t);
-    }
-    void ApplyThrust11(const geometry_msgs::Vector3::ConstPtr& _msg)
-    {
-      ignition::math::Vector3d t(_msg->x, _msg->y, -_msg->z);
-      this->model->GetLink("thruster_11")->AddRelativeForce(t);
-    }
-    void ApplyThrust12(const geometry_msgs::Vector3::ConstPtr& _msg)
-    {
-      ignition::math::Vector3d t(_msg->x, _msg->y, -_msg->z);
-      this->model->GetLink("thruster_12")->AddRelativeForce(t);
-    }
-
-    void OnUpdate()
-    {
-      // if (flag == 0){
-      // this->model->SetAngularVel(ignition::math::Vector3d(0, 0, 0.01)); //++++++
-        // flag = 1;
-      // }
-      
-      // Get current angular informations
-      ignition::math::Vector3d euler = this->model->WorldPose().Rot().Euler();            // unit : [rad]
-      ignition::math::Vector3d angularVelocity = this->model->WorldAngularVel();          // unit : [rad/s]
-      ignition::math::Vector3d angularAcceleration = this->model->WorldAngularAccel();    // unit : [rad/s^2]
-      
-      // Get current translation informations
-      ignition::math::Vector3d translationPosition = this->model->WorldPose().Pos();      // unit : [m]
-      ignition::math::Vector3d translationVelocity = this->model->WorldLinearVel();       // unit : [m/s]
-      ignition::math::Vector3d translationAcceleration = this->model->WorldLinearAccel(); // unit : [m/s^2]
-      
-      // Get current quaternion information
-      ignition::math::Quaterniond quaternion = this->model->WorldPose().Rot();             //unit : [1/z]
-      quaternion.Normalize(); //++++++
-      // // Convert from radians to degrees
-      // euler *= 180.0 / M_PI; 
-      // // Reset rotation if it reaches 360 degrees
-      // for (int i = 0; i < 3; i++)
-      // {
-      //   while (euler[i] >= 360.0)
-      //     euler[i] -= 360.0;
-      //   while (euler[i] < 0.0)
-      //     euler[i] += 360.0;
-      // }
-
-      // Publish translation informations as ROS topics
-      geometry_msgs::Vector3 transPosMsg;
-      // transPosMsg.header.stamp = ros::Time::now();
-      transPosMsg.x = translationVelocity.X();
-      transPosMsg.y = translationVelocity.Y();
-      transPosMsg.z = translationVelocity.Z();
-      this->translationVelocityPub.publish(transPosMsg);
-
-      geometry_msgs::Vector3 transVelMsg;
-      // transVelMsg.header.stamp = ros::Time::now();
-      transVelMsg.x = translationPosition.X();
-      transVelMsg.y = translationPosition.Y();
-      transVelMsg.z = translationPosition.Z();
-      this->translationPositionPub.publish(transVelMsg);
-
-      geometry_msgs::Vector3 transAccelMsg;
-      // transAccelMsg.header.stamp = ros::Time::now();
-      transAccelMsg.x = translationAcceleration.X();
-      transAccelMsg.y = translationAcceleration.Y();
-      transAccelMsg.z = translationAcceleration.Z();
-      this->translationAccelerationPub.publish(transAccelMsg);
-
-      // Publish angular infomations as ROS topics
-      geometry_msgs::Vector3 anglePosMsg;
-      // anglePosMsg.header.stamp = ros::Time::now();
-      anglePosMsg.x = euler.X();
-      anglePosMsg.y = euler.Y();
-      anglePosMsg.z = euler.Z();
-      this->angularPositionPub.publish(anglePosMsg);
-
-      // geometry_msgs::Vector3Stamped angularVelMsg;
-      geometry_msgs::Vector3 angularVelMsg; //+++
-      // angularVelMsg.header.stamp = ros::Time::now();
-      // angularVelMsg.vector.x = angularVelocity.X();
-      // angularVelMsg.vector.y = angularVelocity.Y();
-      // angularVelMsg.vector.z = angularVelocity.Z();
-      angularVelMsg.x = angularVelocity.X();//+++
-      angularVelMsg.y = angularVelocity.Y();//+++
-      angularVelMsg.z = angularVelocity.Z();//+++
-      this->angularVelocityPub.publish(angularVelMsg);
-
-      geometry_msgs::Vector3 angularAccelMsg;
-      // angularAccelMsg.header.stamp = ros::Time::now();
-      angularAccelMsg.x = angularAcceleration.X();
-      angularAccelMsg.y = angularAcceleration.Y();
-      angularAccelMsg.z = angularAcceleration.Z();
-      this->angularAccelerationPub.publish(angularAccelMsg);
-
-      // Publish quaternion informations as ROS topics
-      geometry_msgs::Quaternion quaternionMsg;
-      // quaternionMsg.header.stamp = ros::Time::now();
-      quaternionMsg.x = quaternion.X();
-      quaternionMsg.y = quaternion.Y();
-      quaternionMsg.z = quaternion.Z();
-      quaternionMsg.w = quaternion.W();
-      this->quaternionPub.publish(quaternionMsg);
-    }
-
-  private:
-    // gazebo object members
-    physics::ModelPtr model;                    // target selection => "one script one model" for simplicity
-    event::ConnectionPtr updateConnection;      // engine event updater
-
-    // ROS-related members
-    std::unique_ptr<ros::NodeHandle> rosNode;   // node generator
-
-    ros::Publisher angularPositionPub;          // angular position
-    ros::Publisher angularVelocityPub;          // angular velocity
-    ros::Publisher angularAccelerationPub;      // angular acceleration
-    ros::Publisher translationPositionPub;      // translation position
-    ros::Publisher translationVelocityPub;      // translation velocity
-    ros::Publisher translationAccelerationPub;  // translation acceleration
-    ros::Publisher quaternionPub;               // quaternion
-
-    ros::Subscriber TorqueSub;                  // Torque
-    ros::Subscriber ForceSub;                   // Force
-
-    ros::Subscriber ThrustSub1;
-    ros::Subscriber ThrustSub2;
-    ros::Subscriber ThrustSub3;              
-    ros::Subscriber ThrustSub4;
-    ros::Subscriber ThrustSub5;
-    ros::Subscriber ThrustSub6;
-    ros::Subscriber ThrustSub7;
-    ros::Subscriber ThrustSub8;
-    ros::Subscriber ThrustSub9;
-    ros::Subscriber ThrustSub10;
-    ros::Subscriber ThrustSub11;
-    ros::Subscriber ThrustSub12;                  // Thruster Force
-
-    // A thread the keeps running the rosQueue
-    ros::CallbackQueue rosQueue;                // subscriber queue
-    // A thread the keeps running the rosQueue
-    std::thread rosQueueThread;                 // thread control
-    // int flag = 0; //++++++
-    
-    void QueueThread()
-    {
-      static const double timeout = 0.001;
-      while (this->rosNode->ok()){
-        this->rosQueue.callAvailable(ros::WallDuration(timeout));
-      }
-    }
-  };
-
-  // Upload plugin register into gazebo engine so that this script makes {lib(register name).so} file (share file)
-  GZ_REGISTER_MODEL_PLUGIN(reaction_wheel_plugin)
-}
\ No newline at end of file
diff --git a/src/clear_space/CMakeLists.txt b/src/clear_space/CMakeLists.txt
new file mode 100644
index 0000000..c59e28e
--- /dev/null
+++ b/src/clear_space/CMakeLists.txt
@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 3.5)
+project(clear_space)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+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(ament_cmake_python REQUIRED) #for python scripts
+find_package(rclcpp REQUIRED)
+#find_package(rclpy REQUIRED) #for python scripts
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+set(dependencies "rclcpp" "geometry_msgs" "nav_msgs" "tf2" "tf2_ros")
+
+# add_executable(tf_broadcaster src/tf_broadcaster.cpp)
+# ament_target_dependencies(tf_broadcaster ${dependencies})
+
+
+# install(TARGETS
+#   tf_broadcaster
+#   DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY launch urdf meshes rviz worlds config # src scripts
+  DESTINATION share/${PROJECT_NAME})
+
+
+# Install Python modules
+# ament_python_install_package(${PROJECT_NAME})
+
+# Install Python executables #They must start with the shebag tag
+# install(PROGRAMS
+#   scripts/omni_teleop_keyboard.py
+#   DESTINATION lib/${PROJECT_NAME} )
+
+ament_package()
+
+
diff --git a/src/clear_space/config/ros2_control.yaml b/src/clear_space/config/ros2_control.yaml
new file mode 100644
index 0000000..db26995
--- /dev/null
+++ b/src/clear_space/config/ros2_control.yaml
@@ -0,0 +1,56 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 50
+    use_sim_time: true
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+      joints:
+        - arm_link_1_top_joint
+        - arm_link_2_top_joint
+        - arm_link_3_top_joint
+        - arm_link_1_left_joint
+        - arm_link_2_left_joint
+        - arm_link_3_left_joint
+        - arm_link_1_bottom_joint
+        - arm_link_2_bottom_joint
+        - arm_link_3_bottom_joint
+        - arm_link_1_right_joint
+        - arm_link_2_right_joint
+        - arm_link_3_right_joint
+
+  # This controller will be substituted by custom impedance controller after.
+    arm_controller:
+      type: position_controllers/JointGroupPositionController
+
+    rw_controller:
+      type: velocity_controllers/JointGroupVelocityController
+
+    imu_sensor_broadcaster:
+      type: imu_sensor_broadcaster/IMUSensorBroadcaster
+
+arm_controller:
+  ros__parameters:
+    joints:
+      - arm_link_1_top_joint
+      - arm_link_2_top_joint
+      - arm_link_3_top_joint
+      - arm_link_1_left_joint
+      - arm_link_2_left_joint
+      - arm_link_3_left_joint
+      - arm_link_1_bottom_joint
+      - arm_link_2_bottom_joint
+      - arm_link_3_bottom_joint
+      - arm_link_1_right_joint
+      - arm_link_2_right_joint
+      - arm_link_3_right_joint
+    interface_name: position
+
+rw_controller:
+  ros__parameters:
+    joints:
+      - rw_front_joint
+      - rw_left_joint
+      - rw_back_joint
+      - rw_right_joint
+    interface_name: velocity
diff --git a/src/clear_space/launch/description.launch.py b/src/clear_space/launch/description.launch.py
new file mode 100644
index 0000000..4f1b68f
--- /dev/null
+++ b/src/clear_space/launch/description.launch.py
@@ -0,0 +1,55 @@
+import os,xacro
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler, LogInfo
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.substitutions import Command, LaunchConfiguration, FindExecutable
+from launch.event_handlers import OnProcessStart
+
+def generate_launch_description():
+    this_pkg_path = os.path.join(get_package_share_directory('clear_space'))
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+
+    robot_model = 'satellite'
+
+    xacro_file = os.path.join(this_pkg_path, 'urdf', robot_model+'.xacro') #.urdf
+    doc = xacro.process_file(xacro_file)
+    robot_desc = doc.toprettyxml(indent='  ')
+
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="screen",
+        parameters=[{'robot_description': robot_desc}]
+    )
+
+    joint_state_publisher_exec = ExecuteProcess(
+        cmd=[[
+            FindExecutable(name='ros2'),
+            ' run ',
+            ' joint_state_publisher_gui ',
+            ' joint_state_publisher_gui'
+        ]],
+        shell=True
+    )
+
+    open_rviz = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', str(this_pkg_path+"/rviz/viewer.rviz")],
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='true',
+            description='Use simulation (Gazebo) clock if true'
+        ),
+        robot_state_publisher,
+        joint_state_publisher_exec,
+        open_rviz  # Ensure RViz is included in the launch process
+    ])
diff --git a/src/clear_space/launch/test.launch.py b/src/clear_space/launch/test.launch.py
new file mode 100644
index 0000000..a3a9e04
--- /dev/null
+++ b/src/clear_space/launch/test.launch.py
@@ -0,0 +1,119 @@
+import os, xacro
+from pathlib import Path
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.actions import ExecuteProcess
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    this_pkg_path = os.path.join(
+        get_package_share_directory('clear_space'))
+    # Launch arguments
+    simu_time = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='True',
+        description='Use simulation (Gazebo) clock if true')
+    robot_model = 'satellite'
+    world_file = "no_gravity.sdf"
+    # Set ign sim resource path
+    ign_resource_path = SetEnvironmentVariable(
+        name='GZ_SIM_RESOURCE_PATH',
+        value=[
+            os.path.join(this_pkg_path, 'worlds'), ':' + str(Path(this_pkg_path).parent.resolve())
+        ]
+    )
+
+    open_rviz = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', str(this_pkg_path+"/rviz/ign_viewer.rviz")],
+    )
+    open_ign = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('ros_gz_sim'), 'launch'), '/gz_sim.launch.py']),
+            launch_arguments=[
+                ('gz_args', [this_pkg_path+"/worlds/"+world_file, ' -v 4', ' -r'])
+
+        ]
+    )
+    xacro_file = os.path.join(this_pkg_path, 'urdf', robot_model+'.xacro') #.urdf
+    doc = xacro.process_file(xacro_file)
+    robot_desc = doc.toprettyxml(indent='  ')
+
+    ignition_spawn_entity = Node(
+        package='ros_gz_sim',
+        executable='create',
+        output='screen',
+        arguments=['-string', robot_desc,
+                   '-name', 'satellite',
+                   '-allow_renaming', 'false',
+                   '-x', '0', '-y', '0', '-z', '0'],
+    )
+
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="screen",
+        parameters=[{'robot_description': robot_desc}]
+    )
+    bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=[
+            '/bus_camera/points@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked',
+            '/bus_camera/camera_info@sensor_msgs/msg/CameraInfo@ignition.msgs.CameraInfo',
+            '/bus_camera/image@sensor_msgs/msg/Image@ignition.msgs.Image',
+            '/world/world_model/model/'+robot_model+'/joint_state@sensor_msgs/msg/JointState',
+        ],
+        output='screen',
+        remappings=[
+            ('/world/world_model/model/'+robot_model+'/joint_state', '/'+robot_model+'/joint_states')
+        ]
+    )
+
+    load_joint_state_broadcaster = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster'],
+        output='screen'
+    )
+
+    load_arm_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'arm_controller'],
+        output='screen'
+    )
+    load_rw_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'rw_controller'],
+        output='screen'
+    )
+    # load_imu_sensor_broadcaster = ExecuteProcess(
+    #     cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+    #          'imu_sensor_broadcaster'],
+    #     output='screen'
+    # )
+    plot_node = Node(
+                package="orbit_viewer",
+                executable="orbit_viewer",
+                name="orbit_plot_node",
+    )
+    return LaunchDescription(
+        [
+            simu_time,
+            ign_resource_path,
+            # open_rviz,
+            open_ign,
+            ignition_spawn_entity,
+            robot_state_publisher,
+            load_joint_state_broadcaster,
+            load_arm_controller,
+            load_rw_controller,
+            bridge,
+            plot_node
+        ]
+    )
diff --git a/src/clear_space/meshes/arm_link_1.stl b/src/clear_space/meshes/arm_link_1.stl
new file mode 100644
index 0000000..e9cadba
Binary files /dev/null and b/src/clear_space/meshes/arm_link_1.stl differ
diff --git a/src/clear_space/meshes/arm_link_2.stl b/src/clear_space/meshes/arm_link_2.stl
new file mode 100644
index 0000000..960a620
Binary files /dev/null and b/src/clear_space/meshes/arm_link_2.stl differ
diff --git a/src/clear_space/meshes/arm_link_3.stl b/src/clear_space/meshes/arm_link_3.stl
new file mode 100644
index 0000000..18c1445
Binary files /dev/null and b/src/clear_space/meshes/arm_link_3.stl differ
diff --git a/src/clear_space/meshes/arm_mount.stl b/src/clear_space/meshes/arm_mount.stl
new file mode 100644
index 0000000..e8f8e7d
Binary files /dev/null and b/src/clear_space/meshes/arm_mount.stl differ
diff --git a/src/clear_space/meshes/bus_camera.stl b/src/clear_space/meshes/bus_camera.stl
new file mode 100644
index 0000000..fb7cf21
Binary files /dev/null and b/src/clear_space/meshes/bus_camera.stl differ
diff --git a/src/clear_space/meshes/bus_frame.stl b/src/clear_space/meshes/bus_frame.stl
new file mode 100644
index 0000000..8659d7d
Binary files /dev/null and b/src/clear_space/meshes/bus_frame.stl differ
diff --git a/src/clear_space/meshes/dummy.stl b/src/clear_space/meshes/dummy.stl
new file mode 100644
index 0000000..cd81473
Binary files /dev/null and b/src/clear_space/meshes/dummy.stl differ
diff --git a/src/clear_space/meshes/reaction_wheel.stl b/src/clear_space/meshes/reaction_wheel.stl
new file mode 100644
index 0000000..5bfe11b
Binary files /dev/null and b/src/clear_space/meshes/reaction_wheel.stl differ
diff --git a/src/clear_space/meshes/reaction_wheel_mount.stl b/src/clear_space/meshes/reaction_wheel_mount.stl
new file mode 100644
index 0000000..7847d78
Binary files /dev/null and b/src/clear_space/meshes/reaction_wheel_mount.stl differ
diff --git a/src/clear_space/meshes/thruster.stl b/src/clear_space/meshes/thruster.stl
new file mode 100644
index 0000000..90c8629
Binary files /dev/null and b/src/clear_space/meshes/thruster.stl differ
diff --git a/src/clear_space/meshes/window.stl b/src/clear_space/meshes/window.stl
new file mode 100644
index 0000000..f9e57cc
Binary files /dev/null and b/src/clear_space/meshes/window.stl differ
diff --git a/src/clear_space/package.xml b/src/clear_space/package.xml
new file mode 100644
index 0000000..6da151c
--- /dev/null
+++ b/src/clear_space/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>clear_space</name>
+  <version>0.1.0</version>
+  <description>Package to simulate Intel Real Sense D435 in Ignition Gazebo Fortress with ROS 2 Humble</description>
+  <maintainer email="aaqibmahamood@gmail.com">Aaqib Mahamood</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_ros</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>xacro</exec_depend>
+  <!-- <exec_depend>ament_index_python</exec_depend> -->
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/clear_space/rviz/ign_viewer.rviz b/src/clear_space/rviz/ign_viewer.rviz
new file mode 100644
index 0000000..a773c4f
--- /dev/null
+++ b/src/clear_space/rviz/ign_viewer.rviz
@@ -0,0 +1,149 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 355
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /bus_camera/image
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: bus_frame
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 18.625627517700195
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.7253976464271545
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.7353980541229248
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: true
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d000001ee000000c900fffffffa000000010100000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb000000100044006900730070006c0061007900730100000000000001560000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000231000000bc0000000000000000fb0000000a0049006d0061006700650100000231000000bc0000002800ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1200
+  X: 2439
+  Y: 93
diff --git a/src/clear_space/rviz/viewer.rviz b/src/clear_space/rviz/viewer.rviz
new file mode 100644
index 0000000..8775cae
--- /dev/null
+++ b/src/clear_space/rviz/viewer.rviz
@@ -0,0 +1,556 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+        - /RobotModel1/Description Topic1
+        - /TF1
+        - /PointCloud21
+        - /PointCloud21/Topic1
+      Splitter Ratio: 0.5
+    Tree Height: 746
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.5
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        arm_link_1_bottom:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_link_1_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_link_1_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_link_1_top:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_link_2_bottom:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_link_2_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_link_2_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_link_2_top:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_link_3_bottom:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_link_3_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_link_3_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_link_3_top:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_mount_bottom:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_mount_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_mount_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_mount_top:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        bus_camera:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        bus_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        dummy:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rw_back:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rw_front:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rw_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rw_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rw_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        thru_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        thru_10:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        thru_11:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        thru_12:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        thru_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        thru_3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        thru_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        thru_5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        thru_6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        thru_7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        thru_8:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        thru_9:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        window_back:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        window_front:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        window_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        window_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        arm_link_1_bottom:
+          Value: true
+        arm_link_1_left:
+          Value: true
+        arm_link_1_right:
+          Value: true
+        arm_link_1_top:
+          Value: true
+        arm_link_2_bottom:
+          Value: true
+        arm_link_2_left:
+          Value: true
+        arm_link_2_right:
+          Value: true
+        arm_link_2_top:
+          Value: true
+        arm_link_3_bottom:
+          Value: true
+        arm_link_3_left:
+          Value: true
+        arm_link_3_right:
+          Value: true
+        arm_link_3_top:
+          Value: true
+        arm_mount_bottom:
+          Value: true
+        arm_mount_left:
+          Value: true
+        arm_mount_right:
+          Value: true
+        arm_mount_top:
+          Value: true
+        bus_camera:
+          Value: true
+        bus_frame:
+          Value: true
+        dummy:
+          Value: true
+        rw_back:
+          Value: true
+        rw_front:
+          Value: true
+        rw_left:
+          Value: true
+        rw_mount:
+          Value: true
+        rw_right:
+          Value: true
+        thru_1:
+          Value: true
+        thru_10:
+          Value: true
+        thru_11:
+          Value: true
+        thru_12:
+          Value: true
+        thru_2:
+          Value: true
+        thru_3:
+          Value: true
+        thru_4:
+          Value: true
+        thru_5:
+          Value: true
+        thru_6:
+          Value: true
+        thru_7:
+          Value: true
+        thru_8:
+          Value: true
+        thru_9:
+          Value: true
+        window_back:
+          Value: true
+        window_front:
+          Value: true
+        window_left:
+          Value: true
+        window_right:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        bus_frame:
+          arm_mount_bottom:
+            arm_link_1_bottom:
+              arm_link_2_bottom:
+                arm_link_3_bottom:
+                  {}
+          arm_mount_left:
+            arm_link_1_left:
+              arm_link_2_left:
+                arm_link_3_left:
+                  {}
+          arm_mount_right:
+            arm_link_1_right:
+              arm_link_2_right:
+                arm_link_3_right:
+                  {}
+          arm_mount_top:
+            arm_link_1_top:
+              arm_link_2_top:
+                arm_link_3_top:
+                  {}
+          dummy:
+            {}
+          rw_mount:
+            rw_back:
+              {}
+            rw_front:
+              {}
+            rw_left:
+              {}
+            rw_right:
+              {}
+          thru_1:
+            {}
+          thru_10:
+            {}
+          thru_11:
+            {}
+          thru_12:
+            {}
+          thru_2:
+            {}
+          thru_3:
+            {}
+          thru_4:
+            {}
+          thru_5:
+            {}
+          thru_6:
+            {}
+          thru_7:
+            {}
+          thru_8:
+            {}
+          thru_9:
+            {}
+          window_back:
+            {}
+          window_front:
+            bus_camera:
+              {}
+          window_left:
+            {}
+          window_right:
+            {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: ""
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: ""
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 1000
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /bus_camera/points
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: bus_frame
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 33.25444412231445
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 1.5585654973983765
+        Y: 0.9942892789840698
+        Z: -0.14615142345428467
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.4547969102859497
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 5.383584499359131
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1043
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000375fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000231000000bc0000000000000000000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 1920
+  Y: 0
diff --git a/src/clear_space/urdf/common_properties.xacro b/src/clear_space/urdf/common_properties.xacro
new file mode 100644
index 0000000..ccebfe0
--- /dev/null
+++ b/src/clear_space/urdf/common_properties.xacro
@@ -0,0 +1,96 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:property name="bus_frame_mass"             value="400.0" />
+    <xacro:property name="bus_frame_length"           value="3.0"   />
+    <xacro:property name="bus_frame_thickness"        value="0.3" />
+
+    <xacro:property name="rw_mount_mass"              value="4.0"   />
+    <xacro:property name="rw_mount_width"             value="2.0"   />
+    <xacro:property name="rw_mount_height"            value="1.0"   />
+
+    <xacro:property name="rw_mass"                    value="1.0"   />
+    <xacro:property name="rw_height"                  value="0.1" />
+    <xacro:property name="rw_radius"                  value="0.25"/>
+
+    <xacro:property name="dummy_mass"                 value="44.0"  />
+    <xacro:property name="dummy_height"               value="0.3" />
+    <xacro:property name="dummy_width"                value="2.0"   />
+
+    <xacro:property name="thruster_mass"              value="1.0"   />
+    <xacro:property name="thruster_height"            value="0.05"/>
+    <xacro:property name="thruster_radius"            value="0.05"/>
+    <xacro:property name="thruster_bias"              value="0.1" />
+
+    <xacro:property name="window_mass"                value="1.0"   />
+    <xacro:property name="window_length"              value="2.4" />
+    <xacro:property name="window_thickness"           value="0.3" />
+
+    <xacro:property name="bus_camera_mass"            value="1.0"   />
+    <xacro:property name="bus_camera_height"          value="0.05"/>
+    <xacro:property name="bus_camera_length"          value="0.5" />
+
+    <xacro:property name="arm_mount_mass"             value="1.0"   />
+    <xacro:property name="arm_mount_height"           value="0.4" />
+    <xacro:property name="arm_mount_radius"           value="0.15"/>
+    <xacro:property name="arm_mount_ixx"              value="0.35"/>
+    <xacro:property name="arm_mount_iyy"              value="0.04"/>
+    <xacro:property name="arm_mount_izz"              value="0.02"/>
+    <xacro:property name="arm_mount_bias"             value="0.1" />
+
+    <xacro:property name="arm_link_radius"            value="0.15"/>
+    <xacro:property name="arm_link_height"            value="0.3" />
+    <xacro:property name="arm_link_1_mass"            value="20.0"  />
+    <xacro:property name="arm_link_2_mass"            value="15.0"  />
+    <xacro:property name="arm_link_3_mass"            value="10.0"  />
+    <xacro:property name="arm_link_1_length"          value="4.0"   />
+    <xacro:property name="arm_link_2_length"          value="3.0"   />
+    <xacro:property name="arm_link_3_length"          value="1.85"/>
+    <xacro:property name="arm_link_1_ixx"             value="0.312"/>
+    <xacro:property name="arm_link_2_ixx"             value="0.229"/>
+    <xacro:property name="arm_link_3_ixx"             value="0.15"/>
+    <xacro:property name="arm_link_1_iyy"             value="31.526"/>
+    <xacro:property name="arm_link_2_iyy"             value="13.598"/>
+    <xacro:property name="arm_link_3_iyy"             value="3.761"/>
+    <xacro:property name="arm_link_1_izz"             value="31.524"/>
+    <xacro:property name="arm_link_2_izz"             value="13.596"/>
+    <xacro:property name="arm_link_3_izz"             value="3.759"/>
+
+    <!-- Material Definition -->
+    <material name="white">
+        <color rgba="1 1 1 0.8"/>
+    </material>
+    <material name="red">
+        <color rgba="1 0 0 1"/>
+    </material>
+    <material name="orange">
+        <color rgba="1 0.3 0.1 1"/>
+    </material>
+    <material name="blue">
+        <color rgba="0.2 0.2 1 1"/>
+    </material>
+    <material name="green">
+        <color rgba="0.0 1.0 0.0 1"/>
+    </material>
+    <material name="black">
+        <color rgba="0 0 0 1"/>
+    </material>
+    <material name="grey">
+        <color rgba="0.7 0.7 0.7 1"/>
+    </material>
+    <material name="pink">
+        <color rgba="1 0.41 0.71 1"/>
+    </material>
+    <material name="cyan">
+        <color rgba="0 1 1 1"/>
+    </material>
+    <material name="brown">
+        <color rgba="0.35 0.22 0.15 1"/>
+    </material>
+    <material name="purple">
+        <color rgba="0.5 0 0.5 1"/>
+    </material>
+    <material name="yellow">
+        <color rgba="1 1 0 1"/>
+    </material>
+</robot>
diff --git a/src/clear_space/urdf/satellite.xacro b/src/clear_space/urdf/satellite.xacro
new file mode 100644
index 0000000..24574fd
--- /dev/null
+++ b/src/clear_space/urdf/satellite.xacro
@@ -0,0 +1,1684 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="satellite">
+
+  <xacro:property name="bus_frame_mass"             value="400.0" />
+  <xacro:property name="bus_frame_length"           value="3.0"   />
+  <xacro:property name="bus_frame_thickness"        value="0.3" />
+
+  <xacro:property name="rw_mount_mass"              value="4.0"   />
+  <xacro:property name="rw_mount_width"             value="2.0"   />
+  <xacro:property name="rw_mount_height"            value="1.0"   />
+
+  <xacro:property name="rw_mass"                    value="1.0"   />
+  <xacro:property name="rw_height"                  value="0.1" />
+  <xacro:property name="rw_radius"                  value="0.25"/>
+
+  <xacro:property name="dummy_mass"                 value="44.0"  />
+  <xacro:property name="dummy_height"               value="0.3" />
+  <xacro:property name="dummy_width"                value="2.0"   />
+
+  <xacro:property name="thruster_mass"              value="1.0"   />
+  <xacro:property name="thruster_height"            value="0.05"/>
+  <xacro:property name="thruster_radius"            value="0.05"/>
+  <xacro:property name="thruster_bias"              value="0.1" />
+
+  <xacro:property name="window_mass"                value="1.0"   />
+  <xacro:property name="window_length"              value="2.4" />
+  <xacro:property name="window_thickness"           value="0.3" />
+
+  <xacro:property name="bus_camera_mass"            value="1.0"   />
+  <xacro:property name="bus_camera_height"          value="0.05"/>
+  <xacro:property name="bus_camera_length"          value="0.5" />
+
+  <xacro:property name="arm_mount_mass"             value="1.0"   />
+  <xacro:property name="arm_mount_height"           value="0.4" />
+  <xacro:property name="arm_mount_radius"           value="0.15"/>
+  <xacro:property name="arm_mount_ixx"              value="0.35"/>
+  <xacro:property name="arm_mount_iyy"              value="0.04"/>
+  <xacro:property name="arm_mount_izz"              value="0.02"/>
+  <xacro:property name="arm_mount_bias"             value="0.1" />
+
+  <xacro:property name="arm_link_radius"            value="0.15"/>
+  <xacro:property name="arm_link_height"            value="0.3" />
+  <xacro:property name="arm_link_1_mass"            value="20.0"  />
+  <xacro:property name="arm_link_2_mass"            value="15.0"  />
+  <xacro:property name="arm_link_3_mass"            value="10.0"  />
+  <xacro:property name="arm_link_1_length"          value="4.0"   />
+  <xacro:property name="arm_link_2_length"          value="3.0"   />
+  <xacro:property name="arm_link_3_length"          value="1.85"/>
+  <xacro:property name="arm_link_1_ixx"             value="0.312"/>
+  <xacro:property name="arm_link_2_ixx"             value="0.229"/>
+  <xacro:property name="arm_link_3_ixx"             value="0.15"/>
+  <xacro:property name="arm_link_1_iyy"             value="31.526"/>
+  <xacro:property name="arm_link_2_iyy"             value="13.598"/>
+  <xacro:property name="arm_link_3_iyy"             value="3.761"/>
+  <xacro:property name="arm_link_1_izz"             value="31.524"/>
+  <xacro:property name="arm_link_2_izz"             value="13.596"/>
+  <xacro:property name="arm_link_3_izz"             value="3.759"/>
+
+  <material name="white">
+      <color rgba="1 1 1 0.8"/>
+  </material>
+  <material name="red">
+      <color rgba="1 0 0 1"/>
+  </material>
+  <material name="orange">
+      <color rgba="1 0.3 0.1 1"/>
+  </material>
+  <material name="blue">
+      <color rgba="0.2 0.2 1 1"/>
+  </material>
+  <material name="green">
+      <color rgba="0.0 1.0 0.0 1"/>
+  </material>
+  <material name="black">
+      <color rgba="0 0 0 1"/>
+  </material>
+  <material name="grey">
+      <color rgba="0.7 0.7 0.7 1"/>
+  </material>
+  <material name="pink">
+      <color rgba="1 0.41 0.71 1"/>
+  </material>
+  <material name="cyan">
+      <color rgba="0 1 1 1"/>
+  </material>
+  <material name="brown">
+      <color rgba="0.35 0.22 0.15 1"/>
+  </material>
+  <material name="purple">
+      <color rgba="0.5 0 0.5 1"/>
+  </material>
+  <material name="yellow">
+      <color rgba="1 1 0 1"/>
+  </material>
+  <xacro:property name="PI"                         value="3.1415926535897931"/>
+
+  <link name="bus_frame">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${bus_frame_mass}"/>
+      <inertia
+              ixx="${(bus_frame_mass/6)*bus_frame_length*bus_frame_length}"
+              iyy="${(bus_frame_mass/6)*bus_frame_length*bus_frame_length}"
+              izz="${(bus_frame_mass/6)*bus_frame_length*bus_frame_length}"
+              ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/bus_frame.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/bus_frame.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="rw_mount">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${rw_mount_mass}"/>
+      <inertia ixx="${(rw_mount_mass/20)*(4*rw_mount_height*rw_mount_height+rw_mount_width*rw_mount_width)}"
+              iyy="${(rw_mount_mass/20)*(4*rw_mount_height*rw_mount_height+rw_mount_width*rw_mount_width)}"
+              izz="${(rw_mount_mass/10)*(rw_mount_width*rw_mount_width)}"
+              ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/reaction_wheel_mount.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="brown"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/reaction_wheel_mount.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rw_mount_joint" type="fixed">
+    <origin xyz="0 0 -${rw_mount_height-bus_frame_thickness}" rpy="0.0 0.0 0.0"/>
+    <parent link="bus_frame"/>
+    <child link="rw_mount"/>
+  </joint>
+  <link name="rw_front">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${rw_mass}"/>
+      <inertia ixx="${(rw_mass/12)*(3*rw_radius*rw_radius+rw_height*rw_height)}"
+              iyy="${(rw_mass/12)*(3*rw_radius*rw_radius+rw_height*rw_height)}"
+              izz="${(rw_mass/2)*rw_radius*rw_radius}"
+              ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/reaction_wheel.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="red"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/reaction_wheel.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rw_front_joint" type="continuous">
+    <origin xyz="${rw_mount_width/4} 0 0" rpy="0.0 ${pi/4} 0.0"/>
+    <parent link="rw_mount"/>
+    <child link="rw_front"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+  <link name="rw_left">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${rw_mass}"/>
+      <inertia ixx="${(rw_mass/12)*(3*rw_radius*rw_radius+rw_height*rw_height)}"
+              iyy="${(rw_mass/12)*(3*rw_radius*rw_radius+rw_height*rw_height)}"
+              izz="${(rw_mass/2)*rw_radius*rw_radius}"
+              ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/reaction_wheel.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/reaction_wheel.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rw_left_joint" type="continuous">
+    <origin xyz="0 ${rw_mount_width/4} 0" rpy="-${pi/4} 0.0 0.0"/>
+    <parent link="rw_mount"/>
+    <child link="rw_left"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+  <link name="rw_back">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${rw_mass}"/>
+      <inertia ixx="${(rw_mass/12)*(3*rw_radius*rw_radius+rw_height*rw_height)}"
+              iyy="${(rw_mass/12)*(3*rw_radius*rw_radius+rw_height*rw_height)}"
+              izz="${(rw_mass/2)*rw_radius*rw_radius}"
+              ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/reaction_wheel.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/reaction_wheel.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rw_back_joint" type="continuous">
+    <origin xyz="-${rw_mount_width/4} 0 0" rpy="0.0 -${pi/4} 0.0"/>
+    <parent link="rw_mount"/>
+    <child link="rw_back"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+  <link name="rw_right">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${rw_mass}"/>
+      <inertia ixx="${(rw_mass/12)*(3*rw_radius*rw_radius+rw_height*rw_height)}"
+              iyy="${(rw_mass/12)*(3*rw_radius*rw_radius+rw_height*rw_height)}"
+              izz="${(rw_mass/2)*rw_radius*rw_radius}"
+              ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/reaction_wheel.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/reaction_wheel.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rw_right_joint" type="continuous">
+    <origin xyz="0 -${rw_mount_width/4} 0" rpy="${pi/4} 0.0 0.0"/>
+    <parent link="rw_mount"/>
+    <child link="rw_right"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+  <link name="dummy">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${dummy_mass}"/>
+      <inertia
+              ixx="${(dummy_mass/12)*(dummy_width*dummy_width+dummy_height*dummy_height)}"
+              iyy="${(dummy_mass/12)*(dummy_width*dummy_width+dummy_height*dummy_height)}"
+              izz="${(dummy_mass/12)*(dummy_width*dummy_width+dummy_width*dummy_width)}"
+              ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/dummy.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="brown"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/dummy.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="dummy_joint" type="fixed">
+    <origin xyz="0 0 ${bus_frame_length/2-bus_frame_thickness-dummy_height/2}" rpy="0 0.0 0.0"/>
+    <parent link="bus_frame"/>
+    <child link="dummy"/>
+  </joint>
+  <link name="thru_1">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${thruster_mass}"/>
+      <inertia
+        ixx="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        iyy="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        izz="${(thruster_mass/2)*thruster_radius*thruster_radius}"
+        ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="thru_1_joint" type="revolute">
+    <origin xyz=" ${bus_frame_length/2+thruster_height/2}
+                  ${bus_frame_length/2-thruster_bias}
+                  ${bus_frame_length/2-thruster_bias}"
+            rpy="0.0 ${pi/2} 0.0"/>
+    <parent link="bus_frame"/>
+    <child link="thru_1"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+  <link name="thru_2">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${thruster_mass}"/>
+      <inertia
+        ixx="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        iyy="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        izz="${(thruster_mass/2)*thruster_radius*thruster_radius}"
+        ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="thru_2_joint" type="revolute">
+    <origin xyz=" ${bus_frame_length/2-thruster_bias}
+                  ${bus_frame_length/2+thruster_height/2}
+                  ${bus_frame_length/2-thruster_bias}"
+            rpy="0.0 ${pi/2} ${pi/2}"/>
+    <parent link="bus_frame"/>
+    <child link="thru_2"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+  <link name="thru_3">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${thruster_mass}"/>
+      <inertia
+        ixx="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        iyy="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        izz="${(thruster_mass/2)*thruster_radius*thruster_radius}"
+        ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="thru_3_joint" type="revolute">
+    <origin xyz=" ${bus_frame_length/2-thruster_bias}
+                  ${bus_frame_length/2-thruster_bias}
+                  ${bus_frame_length/2+thruster_height/2}"
+            rpy="0.0 0.0 0.0"/>
+    <parent link="bus_frame"/>
+    <child link="thru_3"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+  <link name="thru_4">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${thruster_mass}"/>
+      <inertia
+        ixx="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        iyy="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        izz="${(thruster_mass/2)*thruster_radius*thruster_radius}"
+        ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="thru_4_joint" type="revolute">
+    <origin xyz=" -${bus_frame_length/2+thruster_height/2}
+                  ${bus_frame_length/2-thruster_bias}
+                  -${bus_frame_length/2-thruster_bias}"
+            rpy="0.0 -${pi/2} 0.0"/>
+    <parent link="bus_frame"/>
+    <child link="thru_4"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+  <link name="thru_5">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${thruster_mass}"/>
+      <inertia
+        ixx="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        iyy="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        izz="${(thruster_mass/2)*thruster_radius*thruster_radius}"
+        ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="thru_5_joint" type="revolute">
+    <origin xyz=" -${bus_frame_length/2-thruster_bias}
+                  ${bus_frame_length/2+thruster_height/2}
+                  -${bus_frame_length/2-thruster_bias}"
+            rpy="-${pi/2} 0.0 0.0"/>
+    <parent link="bus_frame"/>
+    <child link="thru_5"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+  <link name="thru_6">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${thruster_mass}"/>
+      <inertia
+        ixx="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        iyy="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        izz="${(thruster_mass/2)*thruster_radius*thruster_radius}"
+        ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="thru_6_joint" type="revolute">
+    <origin xyz=" -${bus_frame_length/2-thruster_bias}
+                  ${bus_frame_length/2-thruster_bias}
+                  -${bus_frame_length/2+thruster_height/2}"
+            rpy="0.0 -${pi} 0.0"/>
+    <parent link="bus_frame"/>
+    <child link="thru_6"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+  <link name="thru_7">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${thruster_mass}"/>
+      <inertia
+        ixx="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        iyy="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        izz="${(thruster_mass/2)*thruster_radius*thruster_radius}"
+        ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="thru_7_joint" type="revolute">
+    <origin xyz=" -${bus_frame_length/2+thruster_height/2}
+                  -${bus_frame_length/2-thruster_bias}
+                  ${bus_frame_length/2-thruster_bias}"
+            rpy="0.0 -${pi/2} 0.0"/>
+    <parent link="bus_frame"/>
+    <child link="thru_7"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+  <link name="thru_8">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${thruster_mass}"/>
+      <inertia
+        ixx="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        iyy="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        izz="${(thruster_mass/2)*thruster_radius*thruster_radius}"
+        ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="thru_8_joint" type="revolute">
+    <origin xyz=" -${bus_frame_length/2-thruster_bias}
+                  -${bus_frame_length/2+thruster_height/2}
+                  ${bus_frame_length/2-thruster_bias}"
+            rpy="${pi/2} 0.0 0.0"/>
+    <parent link="bus_frame"/>
+    <child link="thru_8"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+  <link name="thru_9">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${thruster_mass}"/>
+      <inertia
+        ixx="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        iyy="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        izz="${(thruster_mass/2)*thruster_radius*thruster_radius}"
+        ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="thru_9_joint" type="revolute">
+    <origin xyz=" -${bus_frame_length/2-thruster_bias}
+                  -${bus_frame_length/2-thruster_bias}
+                  ${bus_frame_length/2+thruster_height/2}"
+            rpy="0 0.0 0.0"/>
+    <parent link="bus_frame"/>
+    <child link="thru_9"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+  <link name="thru_10">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${thruster_mass}"/>
+      <inertia
+        ixx="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        iyy="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        izz="${(thruster_mass/2)*thruster_radius*thruster_radius}"
+        ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="thru_10_joint" type="revolute">
+    <origin xyz=" ${bus_frame_length/2+thruster_height/2}
+                  -${bus_frame_length/2-thruster_bias}
+                  -${bus_frame_length/2-thruster_bias}"
+            rpy="0.0 ${pi/2} 0.0"/>
+    <parent link="bus_frame"/>
+    <child link="thru_10"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+  <link name="thru_11">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${thruster_mass}"/>
+      <inertia
+        ixx="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        iyy="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        izz="${(thruster_mass/2)*thruster_radius*thruster_radius}"
+        ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="thru_11_joint" type="revolute">
+    <origin xyz=" ${bus_frame_length/2-thruster_bias}
+                  -${bus_frame_length/2+thruster_height/2}
+                  -${bus_frame_length/2-thruster_bias}"
+            rpy="${pi/2} 0.0 0.0"/>
+    <parent link="bus_frame"/>
+    <child link="thru_11"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+  <link name="thru_12">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${thruster_mass}"/>
+      <inertia
+        ixx="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        iyy="${(thruster_mass/12)*(3*thruster_radius*thruster_radius+thruster_height*thruster_height)}"
+        izz="${(thruster_mass/2)*thruster_radius*thruster_radius}"
+        ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/thruster.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="thru_12_joint" type="revolute">
+    <origin xyz=" ${bus_frame_length/2-thruster_bias}
+                  -${bus_frame_length/2-thruster_bias}
+                  -${bus_frame_length/2+thruster_height/2}"
+            rpy="0.0 ${pi} 0.0"/>
+    <parent link="bus_frame"/>
+    <child link="thru_12"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+  <link name="window_front">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${window_mass}"/>
+      <inertia
+          ixx="${(window_mass/12)*(window_length*window_length+window_length*window_length)}"
+          iyy="${(window_mass/12)*(window_length*window_length+window_thickness*window_thickness)}"
+          izz="${(window_mass/12)*(window_length*window_length+window_thickness*window_thickness)}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <!-- <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/window.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual> -->
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/window.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="window_front_joint" type="fixed">
+    <origin xyz="${bus_frame_length/2-window_thickness/2}
+                  0.0
+                  0.0"
+            rpy="0.0 0.0 0.0"/>
+    <parent link="bus_frame"/>
+    <child link="window_front"/>
+  </joint>
+  <link name="window_left">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${window_mass}"/>
+      <inertia
+          ixx="${(window_mass/12)*(window_length*window_length+window_length*window_length)}"
+          iyy="${(window_mass/12)*(window_length*window_length+window_thickness*window_thickness)}"
+          izz="${(window_mass/12)*(window_length*window_length+window_thickness*window_thickness)}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <!-- <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/window.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual> -->
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/window.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="window_left_joint" type="fixed">
+    <origin xyz=" 0.0
+                  ${bus_frame_length/2-window_thickness/2}
+                  0.0"
+            rpy="0.0 0.0 ${pi/2}"/>
+    <parent link="bus_frame"/>
+    <child link="window_left"/>
+  </joint>
+  <link name="window_back">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${window_mass}"/>
+      <inertia
+          ixx="${(window_mass/12)*(window_length*window_length+window_length*window_length)}"
+          iyy="${(window_mass/12)*(window_length*window_length+window_thickness*window_thickness)}"
+          izz="${(window_mass/12)*(window_length*window_length+window_thickness*window_thickness)}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <!-- <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/window.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual> -->
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/window.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="window_back_joint" type="fixed">
+    <origin xyz=" -${bus_frame_length/2-window_thickness/2}
+                  0.0
+                  0.0"
+            rpy="0.0 0.0 ${pi}"/>
+    <parent link="bus_frame" />
+    <child link="window_back"/>
+  </joint>
+  <link name="window_right">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${window_mass}"/>
+      <inertia
+          ixx="${(window_mass/12)*(window_length*window_length+window_length*window_length)}"
+          iyy="${(window_mass/12)*(window_length*window_length+window_thickness*window_thickness)}"
+          izz="${(window_mass/12)*(window_length*window_length+window_thickness*window_thickness)}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <!-- <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/window.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual> -->
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/window.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="window_right_joint" type="fixed">
+    <origin xyz=" 0.0
+                  -${bus_frame_length/2-window_thickness/2}
+                  0.0"
+            rpy="0.0 0.0 -${pi/2}"/>
+    <parent link="bus_frame" />
+    <child link="window_right"/>
+  </joint>
+  <link name="bus_camera">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${bus_camera_mass}"/>
+      <inertia
+          ixx="${(bus_camera_mass/12)*(bus_camera_length*bus_camera_length+bus_camera_height*bus_camera_height)}"
+          iyy="${(bus_camera_mass/12)*(bus_camera_length*bus_camera_length+bus_camera_height*bus_camera_height)}"
+          izz="${(bus_camera_mass/12)*(bus_camera_length*bus_camera_length+bus_camera_length*bus_camera_length)}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/bus_camera.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/bus_camera.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="bus_camera_joint" type="revolute">
+    <origin xyz=" ${window_thickness/2-bus_camera_height/2}
+                  0.0
+                  0.0"
+            rpy="0.0 0.0 0.0"/>
+    <limit effort="25" lower="-${pi/200}" upper="${pi/200}" velocity="1.5" />
+    <axis xyz="0.0 0.0 -1.0"/>
+    <parent link="window_front" />
+    <child link="bus_camera"/>
+  </joint>
+  <link name="arm_mount_top">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${arm_mount_mass}"/>
+      <inertia
+          ixx="${arm_mount_ixx}"
+          iyy="${arm_mount_iyy}"
+          izz="${arm_mount_izz}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_mount.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_mount.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_mount_top_joint" type="fixed">
+    <origin xyz=" ${bus_frame_length/2+arm_mount_bias}
+                  0.0
+                  ${bus_frame_length/2-bus_frame_thickness/2}"
+            rpy="${pi/2} 0.0 0.0"/>
+    <parent link="bus_frame" />
+    <child link="arm_mount_top"/>
+  </joint>
+  <link name="arm_link_1_top">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${arm_link_1_mass}"/>
+      <inertia
+          ixx="${arm_link_1_ixx}"
+          iyy="${arm_link_1_iyy}"
+          izz="${arm_link_1_izz}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_1.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_1.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_link_1_top_joint" type="revolute">
+    <origin xyz=" ${arm_mount_bias}
+                  0.0
+                  0.0"
+            rpy="0.0 0.0 0.0"/>
+    <parent link="arm_mount_top" />
+    <child link="arm_link_1_top"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 -1.0"/>
+  </joint>
+  <link name="arm_link_2_top">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${arm_link_2_mass}"/>
+      <inertia
+          ixx="${arm_link_2_ixx}"
+          iyy="${arm_link_2_iyy}"
+          izz="${arm_link_2_izz}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_2.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_2.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_link_2_top_joint" type="revolute">
+    <origin xyz=" ${arm_link_1_length}
+                  0.0
+                  0.0"
+            rpy="0.0 0.0 0.0"/>
+    <parent link="arm_link_1_top" />
+    <child link="arm_link_2_top"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 -1.0"/>
+  </joint>
+  <link name="arm_link_3_top">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${arm_link_3_mass}"/>
+      <inertia
+          ixx="${arm_link_3_ixx}"
+          iyy="${arm_link_3_iyy}"
+          izz="${arm_link_3_izz}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_3.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_3.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_link_3_top_joint" type="revolute">
+    <origin xyz=" ${arm_link_2_length}
+                  0.0
+                  0.0"
+            rpy="0.0 0.0 0.0"/>
+    <parent link="arm_link_2_top" />
+    <child link="arm_link_3_top"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 -1.0"/>
+  </joint>
+
+  <link name="arm_mount_left">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${arm_mount_mass}"/>
+      <inertia
+          ixx="${arm_mount_ixx}"
+          iyy="${arm_mount_iyy}"
+          izz="${arm_mount_izz}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_mount.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_mount.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_mount_left_joint" type="fixed">
+    <origin xyz=" ${bus_frame_length/2+arm_mount_bias}
+                  ${bus_frame_length/2-bus_frame_thickness/2}
+                  0"
+            rpy="0.0 0.0 0.0"/>
+    <parent link="bus_frame" />
+    <child link="arm_mount_left"/>
+  </joint>
+  <link name="arm_link_1_left">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${arm_link_1_mass}"/>
+      <inertia
+          ixx="${arm_link_1_ixx}"
+          iyy="${arm_link_1_iyy}"
+          izz="${arm_link_1_izz}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_1.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_1.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_link_1_left_joint" type="revolute">
+    <origin xyz=" ${arm_mount_bias}
+                  0.0
+                  0.0"
+            rpy="0.0 0.0 0.0"/>
+    <parent link="arm_mount_left" />
+    <child link="arm_link_1_left"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 -1.0"/>
+  </joint>
+  <link name="arm_link_2_left">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${arm_link_2_mass}"/>
+      <inertia
+          ixx="${arm_link_2_ixx}"
+          iyy="${arm_link_2_iyy}"
+          izz="${arm_link_2_izz}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_2.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_2.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_link_2_left_joint" type="revolute">
+    <origin xyz=" ${arm_link_1_length}
+                  0.0
+                  0.0"
+            rpy="0.0 0.0 0.0"/>
+    <parent link="arm_link_1_left" />
+    <child link="arm_link_2_left"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 -1.0"/>
+  </joint>
+  <link name="arm_link_3_left">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${arm_link_3_mass}"/>
+      <inertia
+          ixx="${arm_link_3_ixx}"
+          iyy="${arm_link_3_iyy}"
+          izz="${arm_link_3_izz}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_3.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_3.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_link_3_left_joint" type="revolute">
+    <origin xyz=" ${arm_link_2_length}
+                  0.0
+                  0.0"
+            rpy="0.0 0.0 0.0"/>
+    <parent link="arm_link_2_left" />
+    <child link="arm_link_3_left"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 -1.0"/>
+  </joint>
+  <link name="arm_mount_right">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${arm_mount_mass}"/>
+      <inertia
+          ixx="${arm_mount_ixx}"
+          iyy="${arm_mount_iyy}"
+          izz="${arm_mount_izz}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_mount.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_mount.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_mount_right_joint" type="fixed">
+    <origin xyz=" ${bus_frame_length/2+arm_mount_bias}
+                  -${bus_frame_length/2-bus_frame_thickness/2}
+                  0"
+            rpy="0.0 0.0 0.0"/>
+    <parent link="bus_frame" />
+    <child link="arm_mount_right"/>
+  </joint>
+  <link name="arm_link_1_right">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${arm_link_1_mass}"/>
+      <inertia
+          ixx="${arm_link_1_ixx}"
+          iyy="${arm_link_1_iyy}"
+          izz="${arm_link_1_izz}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_1.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_1.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_link_1_right_joint" type="revolute">
+    <origin xyz=" ${arm_mount_bias}
+                  0.0
+                  0.0"
+            rpy="0.0 0.0 0.0"/>
+    <parent link="arm_mount_right" />
+    <child link="arm_link_1_right"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+  <link name="arm_link_2_right">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${arm_link_2_mass}"/>
+      <inertia
+          ixx="${arm_link_2_ixx}"
+          iyy="${arm_link_2_iyy}"
+          izz="${arm_link_2_izz}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_2.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_2.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_link_2_right_joint" type="revolute">
+    <origin xyz=" ${arm_link_1_length}
+                  0.0
+                  0.0"
+            rpy="0.0 0.0 0.0"/>
+    <parent link="arm_link_1_right" />
+    <child link="arm_link_2_right"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+  <link name="arm_link_3_right">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${arm_link_3_mass}"/>
+      <inertia
+          ixx="${arm_link_3_ixx}"
+          iyy="${arm_link_3_iyy}"
+          izz="${arm_link_3_izz}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_3.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_3.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_link_3_right_joint" type="revolute">
+    <origin xyz=" ${arm_link_2_length}
+                  0.0
+                  0.0"
+            rpy="0.0 0.0 0.0"/>
+    <parent link="arm_link_2_right" />
+    <child link="arm_link_3_right"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+  <link name="arm_mount_bottom">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${arm_mount_mass}"/>
+      <inertia
+          ixx="${arm_mount_ixx}"
+          iyy="${arm_mount_iyy}"
+          izz="${arm_mount_izz}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_mount.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_mount.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_mount_bottom_joint" type="fixed">
+    <origin xyz=" ${bus_frame_length/2+arm_mount_bias}
+                  0.0
+                  -${bus_frame_length/2-bus_frame_thickness/2}"
+            rpy="${pi/2} 0.0 0.0"/>
+    <parent link="bus_frame" />
+    <child link="arm_mount_bottom"/>
+  </joint>
+  <link name="arm_link_1_bottom">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${arm_link_1_mass}"/>
+      <inertia
+          ixx="${arm_link_1_ixx}"
+          iyy="${arm_link_1_iyy}"
+          izz="${arm_link_1_izz}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_1.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_1.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_link_1_bottom_joint" type="revolute">
+    <origin xyz=" ${arm_mount_bias}
+                  0.0
+                  0.0"
+            rpy="0.0 0.0 0.0"/>
+    <parent link="arm_mount_bottom" />
+    <child link="arm_link_1_bottom"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+  <link name="arm_link_2_bottom">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${arm_link_2_mass}"/>
+      <inertia
+          ixx="${arm_link_2_ixx}"
+          iyy="${arm_link_2_iyy}"
+          izz="${arm_link_2_izz}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_2.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_2.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_link_2_bottom_joint" type="revolute">
+    <origin xyz=" ${arm_link_1_length}
+                  0.0
+                  0.0"
+            rpy="0.0 0.0 0.0"/>
+    <parent link="arm_link_1_bottom" />
+    <child link="arm_link_2_bottom"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+  <link name="arm_link_3_bottom">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${arm_link_3_mass}"/>
+      <inertia
+          ixx="${arm_link_3_ixx}"
+          iyy="${arm_link_3_iyy}"
+          izz="${arm_link_3_izz}"
+          ixy="0.0" iyz="0.0" ixz="0.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_3.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find clear_space)/meshes/arm_link_3.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm_link_3_bottom_joint" type="revolute">
+    <origin xyz=" ${arm_link_2_length}
+                  0.0
+                  0.0"
+            rpy="0.0 0.0 0.0"/>
+    <parent link="arm_link_2_bottom" />
+    <child link="arm_link_3_bottom"/>
+    <limit effort="25" lower="-${pi/2}" upper="${pi/2}" velocity="1.5" />
+    <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+
+  <ros2_control name="arm_controller" type="system">
+    <hardware>
+      <plugin>ign_ros2_control/IgnitionSystem</plugin>
+    </hardware>
+    <joint name="arm_link_1_top_joint">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">-0.7853981633974483</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="arm_link_2_top_joint">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="arm_link_3_top_joint">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="arm_link_1_bottom_joint">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">-0.7853981633974483</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="arm_link_2_bottom_joint">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="arm_link_3_bottom_joint">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="arm_link_1_right_joint">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">-0.7853981633974483</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="arm_link_2_right_joint">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="arm_link_3_right_joint">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="arm_link_1_left_joint">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">-0.7853981633974483</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="arm_link_2_left_joint">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="arm_link_3_left_joint">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <sensor name="satellite_imu_sensor">
+      <state_interface name="orientation.x"/>
+      <state_interface name="orientation.y"/>
+      <state_interface name="orientation.z"/>
+      <state_interface name="orientation.w"/>
+      <state_interface name="linear_acceleration.x"/>
+      <state_interface name="linear_acceleration.y"/>
+      <state_interface name="linear_acceleration.z"/>
+      <state_interface name="angular_velocity.x"/>
+      <state_interface name="angular_velocity.y"/>
+      <state_interface name="angular_velocity.z"/>
+    </sensor>
+  </ros2_control>
+
+  <ros2_control name="rw_controller" type="system">
+    <hardware>
+      <plugin>ign_ros2_control/IgnitionSystem</plugin>
+    </hardware>
+    <joint name="rw_front_joint">
+      <command_interface name="velocity"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="rw_back_joint">
+      <command_interface name="velocity"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="rw_right_joint">
+      <command_interface name="velocity"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="rw_left_joint">
+      <command_interface name="velocity"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+  </ros2_control>
+
+  <gazebo>
+    <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+      <parameters>$(find clear_space)/config/ros2_control.yaml</parameters>
+    </plugin>
+  </gazebo>
+
+  <gazebo reference="bus_frame">
+    <sensor name="satellite_imu_sensor" type="imu">
+      <always_on>1</always_on>
+      <update_rate>10.0</update_rate>
+      <visualize>true</visualize>
+      <topic>satellite_imu</topic>
+      <imu>
+        <topic>/imu/data</topic>
+      </imu>
+    </sensor>
+  </gazebo>
+
+  <gazebo reference="bus_camera">
+    <sensor name="rgbd_camera" type="rgbd_camera">
+      <ignition_frame_id>/satellite/bus_camera</ignition_frame_id>
+      <camera name="bus_camera_frame">
+        <horizontal_fov>1.3962634</horizontal_fov>
+        <lens>
+          <intrinsics>
+            <fx>277.1</fx>
+            <fy>277.1</fy>
+            <cx>160.5</cx>
+            <cy>120.5</cy>
+            <s>0</s>
+          </intrinsics>
+        </lens>
+        <distortion>
+          <k1>0.104207</k1>
+          <k2>-0.255558</k2>
+          <k3>0.128694</k3>
+          <p1>0.000512</p1>
+          <p2>0.000319</p2>
+          <center>0.5 0.5</center>
+        </distortion>
+        <image>
+          <width>640</width>
+          <height>480</height>
+          <format>L8</format>
+        </image>
+        <clip>
+          <near>0.01</near>
+          <far>300</far>
+        </clip>
+        <depth_camera>
+          <clip>
+            <near>0.1</near>
+            <far>10</far>
+          </clip>
+        </depth_camera>
+        <noise>
+          <type>gaussian</type>
+          <mean>0</mean>
+          <stddev>0.003</stddev>
+        </noise>
+      </camera>
+      <always_on>1</always_on>
+      <update_rate>10</update_rate>
+      <visualize>0</visualize>
+      <topic>bus_camera</topic>
+    </sensor>
+  </gazebo>
+
+  <!-- <gazebo>
+    <plugin filename="ignition-gazebo-imu-system" name="ignition::gazebo::systems::Imu">
+      <enable_orientation>true</enable_orientation>
+      <enable_angular_velocity>true</enable_angular_velocity>
+      <enable_linear_acceleration>true</enable_linear_acceleration>
+    </plugin>
+  </gazebo> -->
+
+  <gazebo>
+    <plugin filename="libign_plugin.so" name="ign_plugin::IgnitionPlugin">
+      <model_name>satellite</model_name>
+    </plugin>
+  </gazebo>
+
+  <gazebo>
+    <plugin filename="libthruster_plugin.so" name="thruster_plugin::ThrusterPlugin">
+      <link_name>thru_1</link_name>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libthruster_plugin.so" name="thruster_plugin::ThrusterPlugin">
+      <link_name>thru_2</link_name>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libthruster_plugin.so" name="thruster_plugin::ThrusterPlugin">
+      <link_name>thru_3</link_name>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libthruster_plugin.so" name="thruster_plugin::ThrusterPlugin">
+      <link_name>thru_4</link_name>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libthruster_plugin.so" name="thruster_plugin::ThrusterPlugin">
+      <link_name>thru_5</link_name>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libthruster_plugin.so" name="thruster_plugin::ThrusterPlugin">
+      <link_name>thru_6</link_name>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libthruster_plugin.so" name="thruster_plugin::ThrusterPlugin">
+      <link_name>thru_7</link_name>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libthruster_plugin.so" name="thruster_plugin::ThrusterPlugin">
+      <link_name>thru_8</link_name>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libthruster_plugin.so" name="thruster_plugin::ThrusterPlugin">
+      <link_name>thru_9</link_name>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libthruster_plugin.so" name="thruster_plugin::ThrusterPlugin">
+      <link_name>thru_10</link_name>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libthruster_plugin.so" name="thruster_plugin::ThrusterPlugin">
+      <link_name>thru_11</link_name>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libthruster_plugin.so" name="thruster_plugin::ThrusterPlugin">
+      <link_name>thru_12</link_name>
+    </plugin>
+  </gazebo>
+</robot>
diff --git a/src/clear_space/worlds/empty.sdf b/src/clear_space/worlds/empty.sdf
new file mode 100644
index 0000000..0c96a24
--- /dev/null
+++ b/src/clear_space/worlds/empty.sdf
@@ -0,0 +1,75 @@
+<?xml version="1.0" ?>
+<sdf version="1.9">
+  <world name="world_model">
+    <scene>
+      <shadows>0</shadows>
+    </scene>
+    <physics name="1ms" type="ignored">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="libignition-gazebo-sensors-system.so"
+      name="ignition::gazebo::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+
+    <plugin filename="gz-sim-imu-system"
+      name="gz::sim::systems::Imu">
+    </plugin>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>1 1 1 1</diffuse>
+      <specular>0.5 0.5 0.5 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <!-- <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model> -->
+
+  </world>
+</sdf>
diff --git a/src/clear_space/worlds/no_gravity.sdf b/src/clear_space/worlds/no_gravity.sdf
new file mode 100644
index 0000000..f966156
--- /dev/null
+++ b/src/clear_space/worlds/no_gravity.sdf
@@ -0,0 +1,68 @@
+<?xml version="1.0" ?>
+<sdf version="1.7">
+  <world name="no_gravity_world">
+    <scene>
+      <grid>false</grid>
+    </scene>
+    <!-- Define physics properties -->
+    <physics name="1ms" type="ode">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <gravity>0 0 0</gravity>
+    <magnetic_field>0 0 0</magnetic_field>
+
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="ignition::gazebo::systems::Physics">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-user-commands-system"
+      name="ignition::gazebo::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-scene-broadcaster-system"
+      name="ignition::gazebo::systems::SceneBroadcaster">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-imu-system"
+      name="ignition::gazebo::systems::Imu">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-sensors-system"
+      name="ignition::gazebo::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+
+    <scene>
+      <ambient>1.0 1.0 1.0 1</ambient>
+      <background>0 0 0 1</background>
+      <shadows>false</shadows>
+    </scene>
+
+    <!-- Include the sun -->
+    <!-- <light name='sun' type='directional'>
+      <pose>0 0 10 0 -0 0</pose>
+      <cast_shadows>true</cast_shadows>
+      <intensity>1</intensity>
+      <direction>-0.5 0.1 -0.9</direction>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <linear>0.01</linear>
+        <constant>0.90000000000000002</constant>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <spot>
+        <inner_angle>0</inner_angle>
+        <outer_angle>0</outer_angle>
+        <falloff>0</falloff>
+      </spot>
+    </light> -->
+
+    <!-- Add other models or configurations here -->
+  </world>
+</sdf>
+
+
diff --git a/src/ign_plugin/CHANGELOG.rst b/src/ign_plugin/CHANGELOG.rst
new file mode 100644
index 0000000..efad10f
--- /dev/null
+++ b/src/ign_plugin/CHANGELOG.rst
@@ -0,0 +1,43 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package ign_ros2_control
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.4.0 (2022-03-18)
+------------------
+* Fix default ign gazebo version Galactic (`#44 <https://github.com/ignitionrobotics/ign_ros2_control/issues/44>`_)
+* Contributors: Alejandro Hernández Cordero
+
+0.3.0 (2022-03-16)
+------------------
+* Fix ignition version in package.xml (`#40 <https://github.com/ignitionrobotics/ign_ros2_control/issues/40>`_)
+* Contributors: Alejandro Hernández Cordero
+
+0.2.0 (2022-02-17)
+------------------
+* Merge pull request `#36 <https://github.com/ignitionrobotics/ign_ros2_control/issues/36>`_ from ignitionrobotics/ahcorde/foxy_to_galactic
+  Foxy -> Galactic
+* Merge remote-tracking branch 'origin/foxy' into ahcorde/foxy_to_galactic
+* Fixed position control (`#29 <https://github.com/ignitionrobotics/ign_ros2_control/issues/29>`_)
+* typo fix. (`#25 <https://github.com/ignitionrobotics/ign_ros2_control/issues/25>`_)
+* Contributors: Alejandro Hernández Cordero, Tomoya Fujita
+
+0.1.2 (2022-02-14)
+------------------
+* Fixed position control (`#29 <https://github.com/ignitionrobotics/ign_ros2_control/issues/29>`_) (`#34 <https://github.com/ignitionrobotics/ign_ros2_control/issues/34>`_)
+* typo fix. (`#25 <https://github.com/ignitionrobotics/ign_ros2_control/issues/25>`_) (`#26 <https://github.com/ignitionrobotics/ign_ros2_control/issues/26>`_)
+  Co-authored-by: Tomoya Fujita <Tomoya.Fujita@sony.com>
+* Contributors: Alejandro Hernández Cordero
+
+0.1.1 (2022-01-07)
+------------------
+* Change package names from ignition\_ to ign\_ (`#19 <https://github.com/ignitionrobotics/ign_ros2_control/pull/22>`_)
+  * Change package names from ignition\_ to ign\_
+* Contributors: Alejandro Hernández Cordero
+
+0.1.0 (2022-01-07)
+------------------
+* Ignition ros2 control (`#1 <https://github.com/ignitionrobotics/ign_ros2_control/issues/1>`_)
+  Co-authored-by: ahcorde <ahcorde@gmail.com>
+  Co-authored-by: Louise Poubel <louise@openrobotics.org>
+  Co-authored-by: Vatan Aksoy Tezer <vatan@picknik.ai>
+* Contributors: Alejandro Hernández Cordero, Louise Poubel, Vatan Aksoy Tezer
diff --git a/src/ign_plugin/CMakeLists.txt b/src/ign_plugin/CMakeLists.txt
new file mode 100644
index 0000000..55dbab9
--- /dev/null
+++ b/src/ign_plugin/CMakeLists.txt
@@ -0,0 +1,129 @@
+cmake_minimum_required(VERSION 3.5)
+project(ign_plugin)
+
+# Default to C11
+if(NOT CMAKE_C_STANDARD)
+    set(CMAKE_C_STANDARD 11)
+endif()
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 17)
+endif()
+
+# Compiler options
+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(ament_index_cpp REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(yaml_cpp_vendor REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
+
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+if("$ENV{IGNITION_VERSION}" STREQUAL "citadel")
+  find_package(ignition-gazebo3 REQUIRED)
+  set(IGN_GAZEBO_VER ${ignition-gazebo3_VERSION_MAJOR})
+  message(STATUS "Compiling against Ignition Citadel")
+
+elseif("$ENV{IGNITION_VERSION}" STREQUAL "edifice")
+  find_package(ignition-gazebo5 REQUIRED)
+  set(IGN_GAZEBO_VER ${ignition-gazebo5_VERSION_MAJOR})
+  message(STATUS "Compiling against Ignition Edifice")
+
+elseif("$ENV{IGNITION_VERSION}" STREQUAL "fortress")
+  find_package(ignition-gazebo6 REQUIRED)
+  set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR})
+  message(STATUS "Compiling against Ignition Fortress")
+
+else()
+  find_package(ignition-gazebo6 REQUIRED)
+  set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR})
+  message(STATUS "Compiling against Ignition Fortress")
+endif()
+
+find_package(ignition-plugin1 REQUIRED)
+set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
+
+include_directories(include)
+include_directories(include/${PROJECT_NAME})
+
+include_directories(${nav2_msgs_INCLUDE_DIRS})
+include_directories(${std_msgs_INCLUDE_DIRS})
+include_directories(${geometry_msgs_INCLUDE_DIRS})
+
+add_library(${PROJECT_NAME} SHARED
+  src/ignition_plugin.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ignition-gazebo${IGN_GAZEBO_VER}::core
+  ignition-plugin${IGN_PLUGIN_VER}::register
+)
+ament_target_dependencies(${PROJECT_NAME}
+  ament_index_cpp
+  pluginlib
+  rclcpp
+  yaml_cpp_vendor
+  std_msgs
+  nav2_msgs
+  geometry_msgs
+  Eigen3
+)
+
+#########
+
+# add_library(ign_hardware_plugins SHARED
+#   src/ign_system.cpp
+# )
+# ament_target_dependencies(ign_hardware_plugins
+#   rclcpp_lifecycle
+#   hardware_interface
+#   rclcpp
+# )
+# target_link_libraries(ign_hardware_plugins
+#   ignition-gazebo${IGN_GAZEBO_VER}::core
+# )
+
+## Install
+# install(TARGETS
+#   ign_hardware_plugins
+#   ARCHIVE DESTINATION lib
+#   LIBRARY DESTINATION lib
+#   RUNTIME DESTINATION bin
+# )
+
+# Testing and linting
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+# ament_export_include_directories(include)
+# ament_export_include_directories(include/${PROJECT_NAME})
+# ament_export_libraries(${PROJECT_NAME} ign_hardware_plugins)
+
+# Install directories
+install(TARGETS ${PROJECT_NAME}
+  DESTINATION lib
+)
+# Install parameters file
+install(DIRECTORY config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+# Install header and source files
+install(DIRECTORY include
+  DESTINATION include/${PROJECT_NAME}
+)
+# pluginlib_export_plugin_description_file(ign_plugin ign_hardware_plugins.xml)
+
+# Setup the project
+ament_package()
diff --git a/src/ign_plugin/LICENSE b/src/ign_plugin/LICENSE
new file mode 100644
index 0000000..6d8d58f
--- /dev/null
+++ b/src/ign_plugin/LICENSE
@@ -0,0 +1,191 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        https://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
+
+   Copyright 2013-2018 Docker, Inc.
+
+   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/src/ign_plugin/config/satellite.yaml b/src/ign_plugin/config/satellite.yaml
new file mode 100644
index 0000000..1af8ecc
--- /dev/null
+++ b/src/ign_plugin/config/satellite.yaml
@@ -0,0 +1,6 @@
+semi_major:   7000000.0 # [m]
+eccentricity: 0.0       # X
+inclination:  45.0      # [deg]
+ascending:    0.0       # [deg]
+perigee:      0.0       # [deg]
+mean_anomaly: 0.0       # [deg]
diff --git a/src/ign_plugin/include/ign_plugin/Kinematics.hpp b/src/ign_plugin/include/ign_plugin/Kinematics.hpp
new file mode 100644
index 0000000..1c40cca
--- /dev/null
+++ b/src/ign_plugin/include/ign_plugin/Kinematics.hpp
@@ -0,0 +1,601 @@
+/// @brief Simple Kinematic library
+#ifndef KINEMATICS_HPP_
+#define KINEMATICS_HPP_
+
+#define _USE_MATH_DEFINES
+
+#include <iostream>
+#include <cmath>
+#include <Eigen/Dense>
+#include <vector>
+
+using namespace std;
+using namespace Eigen;
+using Vector3d = Eigen::Vector3d;
+using Vector4d = Eigen::Vector4d;
+using Matrix3d = Eigen::Matrix3d;
+
+/*
+
+  Make sure that DCM(Direction Cosine Matrix) and rotation matrix are different rotation method
+  which have transpose relation to each other.
+  DCM => vector value rotation
+  rotation matrix => axis rotation
+
+  Default Unit
+  Angle : [radian]
+  quaternion : [1/z]
+*/
+
+class Kinematics {
+  public:
+    // Skew-symmetric cross product matrix
+    Matrix3d skew(const Vector3d& w) {
+    /**
+     * @brief Generates a skew-symmetric matrix from a 3-element vector.
+     *
+     * This function constructs a 3×3 skew-symmetric matrix from the given
+     * 3D vector. The skew-symmetric matrix is useful for cross-product
+     * computations in matrix form.
+     *
+     * @param w A 3-element Eigen::Vector3d representing the input vector.
+     * @return A 3×3 Eigen::Matrix3d representing the skew-symmetric matrix.
+     */
+        Matrix3d cross_mat;
+        cross_mat << 0.0, -w.z(), w.y(),
+                     w.z(), 0.0, -w.x(),
+                    -w.y(), w.x(), 0.0;
+        return cross_mat;
+    }
+
+    // Rotation matrix around x-axis
+    Matrix3d rotm_x(double angle) {
+    /**
+     * @brief Generates a x-axis rotation matrix from given angle.
+     *
+     * This function constructs a 3×3 rotation matrix from the given
+     * angle. The matrix is for x-axis rotation.
+     *
+     * @param angle A scalar double value with radian unit.
+     * @return A 3×3 Eigen::Matrix3d representing x-axis rotation matrix.
+     */
+        Matrix3d rot_mat;
+        rot_mat << 1.0,    0.0    ,  0.0,
+                   0.0, cos(angle), -sin(angle),
+                   0.0, sin(angle),  cos(angle);
+        return rot_mat;
+    }
+
+    // Rotation matrix around y-axis
+    Matrix3d rotm_y(double angle) {
+    /**
+     * @brief Generates a y-axis rotation matrix from given angle.
+     *
+     * This function constructs a 3×3 rotation matrix from the given
+     * angle. The matrix is for y-axis rotation.
+     *
+     * @param angle A scalar double value with radian unit.
+     * @return A 3×3 Eigen::Matrix3d representing y-axis rotation matrix.
+     */
+        Matrix3d rot_mat;
+        rot_mat << cos(angle), 0.0, sin(angle),
+                    0.0      , 1.0,        0.0,
+                  -sin(angle), 0.0, cos(angle);
+        return rot_mat;
+    }
+
+    // Rotation matrix around z-axis
+    Matrix3d rotm_z(double angle) {
+    /**
+     * @brief Generates a z-axis rotation matrix from given angle.
+     *
+     * This function constructs a 3×3 rotation matrix from the given
+     * angle. The matrix is for z-axis rotation.
+     *
+     * @param angle A scalar double value with radian unit.
+     * @return A 3×3 Eigen::Matrix3d representing z-axis rotation matrix.
+     */
+        Matrix3d rot_mat;
+        rot_mat << cos(angle), -sin(angle),  0.0,
+                   sin(angle), cos(angle) ,  0.0,
+                   0.0       ,    0.0     ,  1.0;
+        return rot_mat;
+    }
+
+    // DCM matrix around x-axis
+    Matrix3d dcm_x(double angle) {
+    /**
+     * @brief Generates a x-axis DCM matrix from given angle.
+     *
+     * This function constructs a 3×3 rotation matrix from the given
+     * angle. The matrix is for x-axis rotation.
+     *
+     * @param angle A scalar double value with radian unit.
+     * @return A 3×3 Eigen::Matrix3d representing x-axis DCM matrix.
+     */
+        Matrix3d dcm_mat;
+        dcm_mat << 1.0,    0.0,        0.0,
+                   0.0,   cos(angle), sin(angle),
+                   0.0,  -sin(angle), cos(angle);
+        return dcm_mat;
+    }
+
+    // DCM matrix around y-axis
+    Matrix3d dcm_y(double angle) {
+    /**
+     * @brief Generates a y-axis DCM matrix from given angle.
+     *
+     * This function constructs a 3×3 rotation matrix from the given
+     * angle. The matrix is for y-axis rotation.
+     *
+     * @param angle A scalar double value with radian unit.
+     * @return A 3×3 Eigen::Matrix3d representing y-axis DCM matrix.
+     */
+        Matrix3d dcm_mat;
+        dcm_mat << cos(angle), 0.0, -sin(angle),
+                   0.0       , 1.0,  0.0,
+                   sin(angle), 0.0,  cos(angle);
+        return dcm_mat;
+    }
+
+    // DCM matrix around z-axis
+    Matrix3d dcm_z(double angle) {
+    /**
+     * @brief Generates a z-axis DCM matrix from given angle.
+     *
+     * This function constructs a 3×3 rotation matrix from the given
+     * angle. The matrix is for z-axis rotation.
+     *
+     * @param angle A scalar double value with radian unit.
+     * @return A 3×3 Eigen::Matrix3d representing z-axis DCM matrix.
+     */
+        Matrix3d dcm_mat;
+        dcm_mat <<  cos(angle), sin(angle), 0.0,
+                   -sin(angle), cos(angle), 0.0,
+                        0.0   ,    0.0    , 1.0;
+        return dcm_mat;
+    }
+
+    // Rodrigues rotation formula
+    Matrix3d euler2rotm(double euler_angle, Vector3d euler_vector) {
+    /**
+     * @brief Generates a rotation matrix from euler angle & vector.
+     *
+     * This function constructs a 3×3 rotation matrix from the given
+     * angle & vector.
+     *
+     * @param angle        A scalar double value with radian unit.
+     * @param euler_vector A Eigen::Vector3d vector
+     * @return A 3×3 Eigen::Matrix3d representing rotation matrix.
+     */
+    euler_vector.normalize(); // Ensure the vector is a unit vector
+
+    double cosine_0 = cos(euler_angle); // cos(theta)
+    double sine = sin(euler_angle);     // sin(theta)
+    double cosine_1 = 1 - cosine_0;     // (1 - cos(theta))
+    Matrix3d skew = Kinematics::skew(euler_vector); // Skew-symmetric matrix
+
+    return cosine_0 * Matrix3d::Identity()
+           + cosine_1 * (euler_vector * euler_vector.transpose())
+           + sine * skew;
+    }
+
+    // Extract Euler angles from a rotation matrix (x-y-z)
+    Vector3d rotm2xyz(const Matrix3d& rot_mat) {
+    /**
+     * @brief Extracts euler angles from rotation matrix in sequence of x-y-z.
+     *
+     * This function constructs an euler angle vector from the given
+     * rotation matrix in sequence of x-y-z.
+     *
+     * @param rot_mat      A 3×3 Eigen::Matrix3d double value with radian unit.
+     * @return             A 1×3 Eigen::Vector3d representing euler angle sequence.
+     */
+    // Clamp value to valid range for asin
+    double eul2 = asin(std::clamp(rot_mat(0, 2), -1.0, 1.0));
+    double eul1 = atan2(-rot_mat(1, 2), rot_mat(2, 2));
+    double eul3 = atan2(-rot_mat(0, 1), rot_mat(0, 0));
+
+    return Vector3d(eul1, eul2, eul3);
+    }
+
+    // Extract Euler angles from a rotation matrix (z-y-x)
+    Vector3d rotm2zyx(const Matrix3d& rot_mat) {
+    /**
+     * @brief Extracts euler angles from rotation matrix in sequence of z-y-x.
+     *
+     * This function constructs an euler angle vector from the given
+     * rotation matrix in sequence of z-y-x.
+     *
+     * @param rot_mat      A 3×3 Eigen::Matrix3d double value with radian unit.
+     * @return             A 1×3 Eigen::Vector3d representing euler angle sequence.
+     */
+    double eul2 = asin(std::clamp(-rot_mat(0, 2), -1.0, 1.0));
+    double eul1 = atan2(rot_mat(1, 2), rot_mat(2, 2));
+    double eul3 = atan2(rot_mat(0, 1), rot_mat(0, 0));
+
+    return Vector3d(eul1, eul2, eul3);
+    }
+
+    // Extract Euler angles from a rotation matrix (z-x-z)
+    Vector3d rotm2zxz(const Matrix3d& rot_mat) {
+    /**
+     * @brief Extracts euler angles from rotation matrix in sequence of z-x-z.
+     *
+     * This function constructs an euler angle vector from the given
+     * rotation matrix in sequence of z-x-z.
+     *
+     * @param rot_mat      A 3×3 Eigen::Matrix3d double value with radian unit.
+     * @return             A 1×3 Eigen::Vector3d representing euler angle sequence.
+     */
+    double eul2 = acos(std::clamp(rot_mat(2, 2), -1.0, 1.0));
+    double eul1 = atan2(rot_mat(1, 2), rot_mat(0, 2));
+    double eul3 = atan2(rot_mat(2, 1), -rot_mat(2, 0));
+
+    return Vector3d(eul1, eul2, eul3);
+    }
+
+    // Extract Euler angles from a rotation matrix (z-y-z)
+    Vector3d rotm2zyz(const Matrix3d& rot_mat) {
+    /**
+     * @brief Extracts euler angles from rotation matrix in sequence of z-y-z.
+     *
+     * This function constructs an euler angle vector from the given
+     * rotation matrix in sequence of z-y-z.
+     *
+     * @param rot_mat      A 3×3 Eigen::Matrix3d double value with radian unit.
+     * @return             A 1×3 Eigen::Vector3d representing euler angle sequence.
+     */
+    double eul2 = acos(std::clamp(rot_mat(2, 2), -1.0, 1.0));
+    double eul1 = atan2(rot_mat(1, 2), rot_mat(0, 2));
+    double eul3 = atan2(rot_mat(2, 1), -rot_mat(2, 0));
+
+    return Vector3d(eul1, eul2, eul3);
+    }
+
+    // Extract Euler angles from a rotation matrix (x-y-x)
+    Vector3d rotm2xyx(const Matrix3d& rot_mat) {
+    /**
+     * @brief Extracts euler angles from rotation matrix in sequence of x-y-x.
+     *
+     * This function constructs an euler angle vector from the given
+     * rotation matrix in sequence of x-y-x.
+     *
+     * @param rot_mat      A 3×3 Eigen::Matrix3d double value with radian unit.
+     * @return             A 1×3 Eigen::Vector3d representing euler angle sequence.
+     */
+    double eul2 = acos(std::clamp(rot_mat(0, 0), -1.0, 1.0));
+    double eul1 = atan2(rot_mat(0, 1), -rot_mat(0, 2));
+    double eul3 = atan2(rot_mat(1, 0), rot_mat(2, 0));
+    return Vector3d(eul1, eul2, eul3);
+    }
+
+    // Extract Euler angles from a rotation matrix (x-z-x)
+    Vector3d rotm2xzx(const Matrix3d& rot_mat) {
+    /**
+     * @brief Extracts euler angles from rotation matrix in sequence of x-z-x.
+     *
+     * This function constructs an euler angle vector from the given
+     * rotation matrix in sequence of x-z-x.
+     *
+     * @param rot_mat      A 3×3 Eigen::Matrix3d double value with radian unit.
+     * @return             A 1×3 Eigen::Vector3d representing euler angle sequence.
+     */
+    double eul2 = acos(std::clamp(rot_mat(0, 0), -1.0, 1.0));
+    double eul1 = atan2(rot_mat(0, 2), rot_mat(0, 1));
+    double eul3 = atan2(rot_mat(2, 0), -rot_mat(1, 0));
+    return Vector3d(eul1, eul2, eul3);
+    }
+
+    // Extract Euler angles from a rotation matrix (y-x-y)
+    Vector3d rotm2yxy(const Matrix3d& rot_mat) {
+    /**
+     * @brief Extracts euler angles from rotation matrix in sequence of y-x-y.
+     *
+     * This function constructs an euler angle vector from the given
+     * rotation matrix in sequence of y-x-y.
+     *
+     * @param rot_mat      A 3×3 Eigen::Matrix3d double value with radian unit.
+     * @return             A 1×3 Eigen::Vector3d representing euler angle sequence.
+     */
+    double eul2 = acos(std::clamp(rot_mat(1, 1), -1.0, 1.0));
+    double eul1 = atan2(rot_mat(1, 0), rot_mat(1, 2));
+    double eul3 = atan2(rot_mat(0, 1), -rot_mat(2, 1));
+    return Vector3d(eul1, eul2, eul3);
+    }
+
+    // Extract Euler angles from a rotation matrix (y-z-y)
+    Vector3d rotm2yzy(const Matrix3d& rot_mat) {
+    /**
+     * @brief Extracts euler angles from rotation matrix in sequence of y-z-y.
+     *
+     * This function constructs an euler angle vector from the given
+     * rotation matrix in sequence of y-z-y.
+     *
+     * @param rot_mat      A 3×3 Eigen::Matrix3d double value with radian unit.
+     * @return             A 1×3 Eigen::Vector3d representing euler angle sequence.
+     */
+    double eul2 = acos(std::clamp(rot_mat(1, 1), -1.0, 1.0));
+    double eul1 = atan2(rot_mat(1, 2), rot_mat(1, 0));
+    double eul3 = atan2(rot_mat(2, 1), -rot_mat(0, 1));
+    return Vector3d(eul1, eul2, eul3);
+    }
+
+    // Extract Euler angles from a rotation matrix (x-z-y)
+    Vector3d rotm2xzy(const Matrix3d& rot_mat) {
+    /**
+     * @brief Extracts euler angles from rotation matrix in sequence of x-z-y.
+     *
+     * This function constructs an euler angle vector from the given
+     * rotation matrix in sequence of x-z-y.
+     *
+     * @param rot_mat      A 3×3 Eigen::Matrix3d double value with radian unit.
+     * @return             A 1×3 Eigen::Vector3d representing euler angle sequence.
+     */
+    double eul2 = asin(std::clamp(-rot_mat(0, 1), -1.0, 1.0));
+    double eul1 = atan2(rot_mat(0, 2), rot_mat(0, 0));
+    double eul3 = atan2(rot_mat(2, 1), rot_mat(1, 1));
+    return Vector3d(eul1, eul2, eul3);
+    }
+
+    // Extract Euler angles from a rotation matrix (y-x-z)
+    Vector3d rotm2yxz(const Matrix3d& rot_mat) {
+    /**
+     * @brief Extracts euler angles from rotation matrix in sequence of y-x-z.
+     *
+     * This function constructs an euler angle vector from the given
+     * rotation matrix in sequence of y-x-z.
+     *
+     * @param rot_mat      A 3×3 Eigen::Matrix3d double value with radian unit.
+     * @return             A 1×3 Eigen::Vector3d representing euler angle sequence.
+     */
+    double eul2 = asin(std::clamp(-rot_mat(1, 0), -1.0, 1.0));
+    double eul1 = atan2(rot_mat(1, 2), rot_mat(1, 1));
+    double eul3 = atan2(rot_mat(2, 0), rot_mat(0, 0));
+    return Vector3d(eul1, eul2, eul3);
+    }
+
+    // Extract Euler angles from a rotation matrix (y-z-x)
+    Vector3d rotm2yzx(const Matrix3d& rot_mat) {
+    /**
+     * @brief Extracts euler angles from rotation matrix in sequence of y-z-x.
+     *
+     * This function constructs an euler angle vector from the given
+     * rotation matrix in sequence of y-z-x.
+     *
+     * @param rot_mat      A 3×3 Eigen::Matrix3d double value with radian unit.
+     * @return             A 1×3 Eigen::Vector3d representing euler angle sequence.
+     */
+    double eul2 = asin(std::clamp(rot_mat(1, 2), -1.0, 1.0));
+    double eul1 = atan2(-rot_mat(1, 0), rot_mat(1, 1));
+    double eul3 = atan2(-rot_mat(0, 2), rot_mat(2, 2));
+    return Vector3d(eul1, eul2, eul3);
+    }
+
+    // Extract Euler angles from a rotation matrix (z-x-y)
+    Vector3d rotm2zxy(const Matrix3d& rot_mat) {
+    /**
+     * @brief Extracts euler angles from rotation matrix in sequence of z-x-y.
+     *
+     * This function constructs an euler angle vector from the given
+     * rotation matrix in sequence of z-x-y.
+     *
+     * @param rot_mat      A 3×3 Eigen::Matrix3d double value with radian unit.
+     * @return             A 1×3 Eigen::Vector3d representing euler angle sequence.
+     */
+    double eul2 = asin(std::clamp(-rot_mat(2, 0), -1.0, 1.0));
+    double eul1 = atan2(rot_mat(1, 0), rot_mat(0, 0));
+    double eul3 = atan2(rot_mat(2, 1), rot_mat(2, 2));
+
+    return Vector3d(eul1, eul2, eul3);
+    }
+
+    // Normalize a quaternion (Scalar part first: [w, x, y, z])
+    Vector4d quat_norm(const Vector4d& q) {
+    /**
+     * @brief Normalizes a quaternion represented as a 4D vector.
+     *
+     * If the quaternion has zero norm, it returns
+     * the identity quaternion (1, 0, 0, 0).
+     *
+     * @param q A 4D Eigen::Vector4d representing a quaternion.
+     * @return A normalized Eigen::Vector4d quaternion.
+     */
+        double norm = q.norm();
+        if (norm < std::numeric_limits<double>::epsilon()) {
+            return Vector4d(1, 0, 0, 0); // Return identity quaternion
+        }
+
+        return q / norm;
+    }
+
+    // Quaternion conjugate
+    Vector4d quat_conj(const Vector4d& q) {
+    /**
+     * @brief Normalizes a quaternion represented as a 4D vector.
+     *
+     * If the quaternion has zero norm, it returns
+     * the identity quaternion (1, 0, 0, 0).
+     *
+     * @param q A 4D Eigen::Vector4d representing a quaternion.
+     * @return A conjugated Eigen::Vector4d quaternion.
+     */
+        return Vector4d(q.w(), -q.x(), -q.y(), -q.z());
+    }
+
+    // Quaternion product
+    Vector4d quat_prod(const Vector4d& q1, const Vector4d& q2) {
+    /**
+     * @brief Computes the Hamilton product of two quaternions.
+     *
+     * The function multiplies two quaternions represented as 4D vectors.
+     * The quaternion multiplication follows the Hamilton product rule.
+     *
+     * @param q1 First quaternion (Eigen::Vector4d).
+     * @param q2 Second quaternion (Eigen::Vector4d).
+     * @return The resulting quaternion (Eigen::Vector4d).
+     */
+        double w1 = q1(0), w2 = q2(0);   // Scalar parts
+        Vector3d v1 = q1.tail<3>(), v2 = q2.tail<3>(); // Vector parts
+
+        double w = w1 * w2 - v1.dot(v2); // Scalar part of the result
+        Vector3d v = w1 * v2 + w2 * v1 + v1.cross(v2); // Vector part
+
+        return Vector4d(w, v.x(), v.y(), v.z());
+    }
+
+    // Quaternion to Rotation Matrix
+    Matrix3d quat2rotm(const Vector4d& q) {
+    /**
+     * @brief Contructs rotation matrix from quaternion.
+     *
+     * The function constructs rotation matrix represented as 4D vectors.
+     * The quaternion multiplication follows the Hamilton product rule.
+     *
+     * @param q  quaternion (Eigen::Vector4d).
+     * @return   A 3x3 rotaion matrix from quaternion
+     */
+        double w = q.w();
+        double x = q.x();
+        double y = q.y();
+        double z = q.z();
+
+        Matrix3d rot_mat;
+        rot_mat << 1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w),
+                   2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w),
+                   2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y);
+        return rot_mat;
+    }
+
+    // Rotation Matrix to Quaternion
+    Vector4d rotm2quat(const Matrix3d& rot_mat) {
+    /**
+     * @brief Extracts quaternion from rotation matrix.
+     *
+     * The function extracts a quaternion from rotation matrix.
+     *
+     * @param rot_mat  A 3x3 Eigen::Matrix3d rotation matrix
+     * @return         A 1x4 Eigen::Vector4d quaternion
+     */
+        double trace = rot_mat.trace();
+        Vector4d q;
+        if (trace > 0.0) {
+            double s = 0.5 / sqrt(trace + 1.0);
+            q.w() = 0.25 / s;
+            q.x() = (rot_mat(2, 1) - rot_mat(1, 2)) * s;
+            q.y() = (rot_mat(0, 2) - rot_mat(2, 0)) * s;
+            q.z() = (rot_mat(1, 0) - rot_mat(0, 1)) * s;
+        } else {
+            if (rot_mat(0, 0) > rot_mat(1, 1) && rot_mat(0, 0) > rot_mat(2, 2)) {
+                double s = 2.0 * sqrt(1.0 + rot_mat(0, 0) - rot_mat(1, 1) - rot_mat(2, 2));
+                q.w() = (rot_mat(2, 1) - rot_mat(1, 2)) / s;
+                q.x() = 0.25 * s;
+                q.y() = (rot_mat(0, 1) + rot_mat(1, 0)) / s;
+                q.z() = (rot_mat(0, 2) + rot_mat(2, 0)) / s;
+            } else if (rot_mat(1, 1) > rot_mat(2, 2)) {
+                double s = 2.0 * sqrt(1.0 + rot_mat(1, 1) - rot_mat(0, 0) - rot_mat(2, 2));
+                q.w() = (rot_mat(0, 2) - rot_mat(2, 0)) / s;
+                q.x() = (rot_mat(0, 1) + rot_mat(1, 0)) / s;
+                q.y() = 0.25 * s;
+                q.z() = (rot_mat(1, 2) + rot_mat(2, 1)) / s;
+            } else {
+                double s = 2.0 * sqrt(1.0 + rot_mat(2, 2) - rot_mat(0, 0) - rot_mat(1, 1));
+                q.w() = (rot_mat(1, 0) - rot_mat(0, 1)) / s;
+                q.x() = (rot_mat(0, 2) + rot_mat(2, 0)) / s;
+                q.y() = (rot_mat(1, 2) + rot_mat(2, 1)) / s;
+                q.z() = 0.25 * s;
+            }
+        }
+        return q;
+    }
+
+    // Euler Angles from Quaternion
+    Vector3d quat2euler(const Vector4d& q) {
+    /**
+     * @brief Extracts RPY from quaternion.
+     *
+     * The function extracts roll/pitch/yaw from quaternion.
+     *
+     * @param q  A 1x4 Eigen::Vector4d quaternion
+     * @return   A 1x3 Eigen::Vector3d RPY vector
+     */
+        double w = q.w();
+        double x = q.x();
+        double y = q.y();
+        double z = q.z();
+
+        double roll = atan2(2.0 * (w * x + y * z), 1 - 2 * (x * x + y * y));
+        double pitch = asin(2.0 * (w * y - z * x));
+        double yaw = atan2(2.0 * (w * z + x * y), 1 - 2 * (y * y + z * z));
+
+        return Vector3d(roll, pitch, yaw);
+    }
+
+    // Quaternion difference Calculation
+    Vector4d quat_diff(const Vector4d& q1, const Vector4d& q2) {
+    /**
+     * @brief Computes quaternion difference between given two quaternions.
+     *
+     * This function calculates quaternion difference.
+     *
+     * @param q1 The quaternion (Eigen::Vector4d).
+     * @param q2 The quaternion (Eigen::Vector4d).
+     * @return   A 1x4 Eigen::Vector4d quaternion difference.
+     */
+        Vector4d q_err = quat_prod(quat_conj(q1), q2);
+        return q_err;
+    }
+
+    Vector4d quat_dot(const Vector4d& quat, const Vector3d& w) {
+    /**
+     * @brief Computes the time derivative of a quaternion given angular velocity.
+     *
+     * This function calculates `q_dot = 0.5 * (q ⊗ w)`, where `⊗` represents quaternion-vector multiplication.
+     *
+     * @param quat The quaternion (Eigen::Vector4d).
+     * @param w The angular velocity vector (Eigen::Vector3d).
+     * @return The quaternion derivative (Eigen::Vector4d).
+     */
+        Vector3d qv = quat.tail<3>(); // Extract vector part
+        double qs = -0.5 * qv.dot(w); // Scalar part derivative
+        Vector3d qv_dot = 0.5 * (quat(0) * w + qv.cross(w)); // Vector part derivative
+
+        return Vector4d(qs, qv_dot.x(), qv_dot.y(), qv_dot.z());
+    }
+
+    // Runge-Kutta 4th Order Integration for Quaternion Update
+    Vector4d quat_update(const Vector4d& qd, const Vector3d& w, double dt, double tol = 1e-6) {
+    /**
+     * @brief Computes the integration of a quaternion derivative.
+     *
+     * This function calculates integration of quaternion derivation.
+     *
+     * @param qd The quaternion derivative   (Eigen::Vector4d).
+     * @param w The angular velocity vector  (Eigen::Vector3d).
+     * @param dt a time step for integration (double).
+     * @return  integrated quaternion        (Eigen::Vector4d).
+     */
+        auto quatDeriv = [&](const Vector4d& q, const Vector3d& w) {
+            return quat_dot(qd,w);
+        };
+
+        Vector4d k1 = quatDeriv(qd, w);
+        Vector4d k2 = quatDeriv(qd + 0.5 * dt * k1, w);
+        Vector4d k3 = quatDeriv(qd + 0.5 * dt * k2, w);
+        Vector4d k4 = quatDeriv(qd + dt * k3, w);
+
+        Vector4d q_next = qd + (dt / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4);
+
+        // Error estimation for adaptive step size
+        double error = (k1 - k4).norm();
+        if (error > tol) {
+            dt *= 0.5;
+            return quat_update(qd, w, dt, tol);
+        } else if (error < tol / 10) {
+            dt *= 2.0;
+        }
+
+        return quat_norm(q_next);
+    }
+};
+#endif // KINEMATICS_HPP_
diff --git a/src/ign_plugin/include/ign_plugin/OrbitMechanics.hpp b/src/ign_plugin/include/ign_plugin/OrbitMechanics.hpp
new file mode 100644
index 0000000..47f9fe5
--- /dev/null
+++ b/src/ign_plugin/include/ign_plugin/OrbitMechanics.hpp
@@ -0,0 +1,311 @@
+#ifndef ORBIT_MECHANICS_HPP_
+#define ORBIT_MECHANICS_HPP_
+
+#define _USE_MATH_DEFINES
+
+#include <iostream>
+#include <cmath>
+#include <vector>
+#include <array>
+#include <Eigen/Dense>
+#include <filesystem>
+
+#include "ign_plugin/Kinematics.hpp"
+
+
+// const auto package_share_path = ament_index_cpp::get_package_share_directory("pinocchio_ros_example");
+// const auto urdf_path = std::filesystem::path(package_share_path) / "ur_robot_model" / "ur5_gripper.urdf";
+// const auto srdf_path = std::filesystem::path(package_share_path) / "ur_robot_model" / "ur5_gripper.srdf";
+
+using namespace std;
+using namespace Eigen;
+using Vector3d = Eigen::Vector3d;
+using Vector4d = Eigen::Vector4d;
+using Matrix3d = Eigen::Matrix3d;
+
+typedef Matrix<double, 6, 1> Vector6d;
+
+class OrbitMechanics {
+public:
+    // Constructor & Deconstructor
+    OrbitMechanics(double semi,   double eccen,   double incli,
+                   double ascend, double perigee, double anomaly,
+                   int year, int month,  int day,
+                   int hour, int minute, int second)
+    {
+        semi_   = semi; eccen_  = eccen;
+        perigee_ = perigee * deg2rad_; ascend_  = ascend * deg2rad_;
+        incli_  = incli * deg2rad_;    anomaly_ = anomaly * deg2rad_;
+        keplerian_ << semi_, eccen_, incli_, ascend_, perigee_, anomaly_;
+
+        year_ = year; month_ = month;   day_ = day;
+        hour_ = hour; minute_ = minute; second_ = second;
+        time_ << year, month, day, hour, minute, second;
+
+        cartestian_state_ = kepel2state(keplerian_);
+        // cout << "cartesian_state : " << cartestian_state_ << endl;
+        x_ = cartestian_state_(0);  y_ = cartestian_state_(1);  z_ = cartestian_state_(2);
+        vx_ = cartestian_state_(3); vy_ = cartestian_state_(4); vz_ = cartestian_state_(5);
+    }
+
+    ~OrbitMechanics() {}
+
+    // Time update
+    // Time update code will be added. TBD
+
+    // Orbit propagation
+    pair<Vector6d, Matrix3d> calcOrbitPropagation (const Vector6d& initial_condition,
+                                                   Vector3d& sat_accel)
+    {
+        Vector6d dfdt;
+        Matrix3d eci2lvlh;
+
+        double x  = initial_condition(0);
+        double y  = initial_condition(1);
+        double z  = initial_condition(2);
+        double x1 = initial_condition(3);
+        double y1 = initial_condition(4);
+        double z1 = initial_condition(5);
+
+        double r = sqrt(x*x + y*y + z*z);
+
+        // J2 Perturbation
+        double term1 = 3 * J2_ * u_ * re_ * re_ * x;
+        double term2 = 2 * pow(r, 5);
+        double term3 = (1 - ((5 * z * z) / (r * r)));
+        double J2_a_I = -(term1 / term2) * term3;
+        term1 = 3 * J2_ * u_ * re_ * re_ * y;
+        double J2_a_J = -(term1 / term2) * term3;
+        term1 = 3 * J2_ * u_ * re_ * re_ * z;
+        term3 = (3 - ((5 * z * z) / (r * r)));
+        double J2_a_K = -(term1 / term2) * term3;
+        Vector3d J2_vec(J2_a_I, J2_a_J, J2_a_K);
+
+        // J4 Perturbation
+        term1 = (15 * J4_ * u_ * re_ * re_ * re_ * re_ * x) / (8 * pow(r, 7));
+        term2 = (14 * z * z) / (r * r);
+        term3 = (21 * pow(z, 4)) / (pow(r, 4));
+        double J4_a_I = term1 * (1 - term2 + term3);
+        term1 = (15 * J4_ * u_ * re_ * re_ * re_ * re_ * y) / (8 * pow(r, 7));
+        double J4_a_J = term1 * (1 - term2 + term3);
+        term1 = (15 * J4_ * u_ * re_ * re_ * re_ * re_ * z) / (8 * pow(r, 7));
+        term2 = (70 * z * z) / (3 * r * r);
+        term3 = (21 * pow(z, 4)) / (pow(r, 4));
+        double J4_a_K = term1 * (5 - term2 + term3);
+        Vector3d J4_vec(J4_a_I, J4_a_J, J4_a_K);
+
+        // Kepler Motion Equations
+        dfdt(0) = x1;
+        dfdt(1) = y1;
+        dfdt(2) = z1;
+        dfdt(3) = -u_ * x / pow(r, 3) + sat_accel(0);
+        dfdt(4) = -u_ * y / pow(r, 3) + sat_accel(1);
+        dfdt(5) = -u_ * z / pow(r, 3) + sat_accel(2);
+        // dfdt(3) = -u_ * x / pow(r, 3) + sat_accel(0)
+        //           +J2_vec(0) + J2_vec(0);
+        // dfdt(4) = -u_ * y / pow(r, 3) + sat_accel(1)
+        //           + J2_vec(1) + J2_vec(1);
+        // dfdt(5) = -u_ * z / pow(r, 3) + sat_accel(2)
+        //           + J2_vec(2) + J2_vec(2);
+
+        // LVLH Frame Calculation
+        Vector3d lvlh_z = -Vector3d(x, y, z).normalized();
+        Vector3d rcv(x1, y1, z1);
+        Vector3d lvlh_x = rcv.normalized();
+        Vector3d lvlh_y = lvlh_z.cross(lvlh_x);
+
+        eci2lvlh.col(0) = lvlh_x;
+        eci2lvlh.col(1) = lvlh_y;
+        eci2lvlh.col(2) = lvlh_z;
+
+        return make_pair(dfdt, eci2lvlh);
+    }
+
+    Vector6d derivativeFunction(const Vector6d& state, Vector3d& sat_accel)
+    {
+        Vector6d tmp_state = state;
+        auto result = calcOrbitPropagation(tmp_state, sat_accel);
+        // result.first is the Vector6d derivative
+        return result.first;
+    }
+
+    Vector6d integrateStateRK4(const Vector6d& current_state,
+                               double dt,
+                               Vector3d& sat_accel)
+    {
+        Vector6d temp_state = current_state;
+        // k1
+        Vector6d k1 = derivativeFunction(temp_state, sat_accel);
+        // k2
+        Vector6d state_k2 = temp_state + 0.5 * dt * k1;
+        Vector6d k2       = derivativeFunction(state_k2, sat_accel);
+        // k3
+        Vector6d state_k3 = temp_state + 0.5 * dt * k2;
+        Vector6d k3       = derivativeFunction(state_k3, sat_accel);
+        // k4
+        Vector6d state_k4 = temp_state + dt * k3;
+        Vector6d k4       = derivativeFunction(state_k4, sat_accel);
+        // next_state
+        Vector6d next_state = temp_state
+                              + (dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+        x_ = next_state(0); y_ = next_state(1); z_ = next_state(2);
+        vx_ = next_state(3); vy_ = next_state(4); vz_ = next_state(5);
+        return next_state;
+    }
+
+    // Orbit state descriptor
+    double findKepler(double mean_anomaly, double eccentricity)
+    {
+        double exc2 = eccentricity * eccentricity;
+        double am1 = fmod(mean_anomaly, 2*M_PI);
+        double am2 = am1 + am1;
+        double am3 = am2 + am1;
+        double shoot = am1 + eccentricity*(1.0 - 0.125*exc2)*sin(am1) +
+                       0.5*exc2*(sin(am2) + 0.75*eccentricity*sin(am3));
+        shoot = fmod(shoot, 2*M_PI);
+
+        double e1 = 1.0;
+        double ic = 0.0;
+
+        while ((abs(e1) > 1.e-12) && (ic <= 10)){
+            e1 = (shoot - am1 - eccentricity*sin(shoot)) /
+                 (1.0 -eccentricity*cos(shoot));
+            shoot = shoot - e1;
+            ic = ic + 1;
+        }
+
+        if (ic >= 10){
+            cout << "[WARN] subroutine kepler did not converge in 10 iteration" << endl;
+        }
+
+        return shoot; // eccentric
+    }
+
+    Vector6d kepel2state(Vector6d& kepel)
+    {
+        double a = kepel(0);
+        double exc = kepel(1);
+
+        double c1 = sqrt(1.0 - exc*exc);
+
+        Matrix3d orb2iner = kin_.rotm_z(-kepel(3)) *
+                            kin_.rotm_x(-kepel(2)) *
+                            kin_.rotm_z(-kepel(4));
+
+        double E = findKepler(kepel(5), exc);
+
+        double sE = sin(E);
+        double cE = cos(E);
+        double c3 = sqrt(u_/a) / (1.0 -exc*cE);
+
+        Vector3d x(a * (cE - exc), a*c1*sE, 0.0);
+        x = orb2iner.transpose() * x;
+        Vector3d v(-c3*sE, c1*c3*cE, 0.0);
+        v = orb2iner.transpose() * v;
+
+        Vector6d statevec(x(0),x(1),x(2),v(0),v(1),v(2));
+
+        return statevec;
+    }
+
+    Vector6d state2kepel(Vector6d& statevec){
+        Vector3d xp;
+        xp << statevec(0), statevec(1), statevec(2);
+        Vector3d xv;
+        xv << statevec(3), statevec(4), statevec(5);
+
+        double r = xp.norm();
+        double vq = xv.squaredNorm();
+        double ainv = 2.0 / r - vq / u_;
+
+        Vector3d h = kin_.skew(xp) * xv;
+        double hm = h.norm();
+        Vector3d an(0.0, 0.0, 0.0);
+
+        if (hm < 1.e-10){
+            clog << "[WARN] Subroutine kepler did not converge in 10 iterations." << endl;
+            return Vector6d::Zero();
+        }
+        h = h / hm;
+        double incl = acos(h(2));
+        double raan = atan2(h(0), -h(1));
+        double d = xp.dot(xv) / u_;
+        double esene = d * sqrt(u_ * ainv);
+        double ecose = 1 - r * ainv;
+        double exc = sqrt(esene*esene + ecose*ecose);
+        double E = atan2(esene, ecose);
+        double mean = fmod(E - esene, 2 * M_PI);
+
+        if (mean < 0) mean = mean + 2 * M_PI;
+
+        double arpe = 0.0;
+        if (exc < 1.e-10) {
+            arpe = 0.0;
+        }
+        else{
+            double dp = 1.0/r - ainv;
+            Vector3d ev = dp*xp - d*xv;
+            double abev = ev.norm();
+            ev = ev / abev;
+            an(0) = cos(raan);
+            an(1) = sin(raan);
+            an(2) = 0;
+            Vector3d tmp = kin_.skew(h) * an;
+            double fi = ev.dot(tmp);
+            double arpe = acos(ev.dot(an));
+            if (fi < 0){
+                arpe = -arpe + 2 * M_PI;
+            }
+        }
+        Vector6d kepel;
+        kepel << 1.0 / ainv, exc, incl, raan, arpe, mean;
+
+        return kepel;
+    }
+
+    Vector6d getKeplerian(){
+        return Vector6d(semi_, eccen_, incli_, ascend_, perigee_, anomaly_);
+    }
+
+    Vector6d getCartesian(){
+        return Vector6d(x_, y_, z_,vx_, vy_, vz_);
+    }
+
+private:
+    Kinematics kin_;
+    // Constant parameter
+    double rad2deg_ = 180.0 / M_PI;
+    double deg2rad_ = M_PI / 180.0;
+    double u_ = 3.9865e14;
+    double re_ = 6378137.0;
+    double J2_ = 0.0010826267;
+    double J4_ = -0.0000016196;
+
+    // day time parameter
+    Vector6d time_;
+    int year_;
+    int month_;
+    int day_;
+    int hour_;
+    int minute_;
+    int second_;
+
+    // keplerian element
+    Vector6d keplerian_;
+    double semi_;    // semi-major axis
+    double eccen_;   // eccentricity
+    double incli_;   // inclination
+    double ascend_;  // ascending node
+    double perigee_; // perigee
+    double anomaly_; // mean anomaly
+
+    // cartesian element
+    Vector6d cartestian_state_;
+    double x_;
+    double y_;
+    double z_;
+    double vx_;
+    double vy_;
+    double vz_;
+};
+#endif // ORBIT_MECHANICS_HPP_
diff --git a/src/ign_plugin/package.xml b/src/ign_plugin/package.xml
new file mode 100644
index 0000000..99d8400
--- /dev/null
+++ b/src/ign_plugin/package.xml
@@ -0,0 +1,39 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>ign_plugin</name>
+  <version>0.4.0</version>
+  <description>Ignition ros2_control package allows to control simulated robots using ros2_control framework.</description>
+  <maintainer email="alejandro@openrobotics.com">Alejandro Hernández</maintainer>
+  <author>Alejandro Hernández</author>
+  <license>Apache 2</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>ament_index_cpp</depend>
+  <!-- default version to use in official ROS2 packages is Ignition Fortress for ROS2 Rolling -->
+  <depend condition="$IGNITION_VERSION == ''">ignition-gazebo6</depend>
+  <depend condition="$IGNITION_VERSION == citadel">ignition-gazebo3</depend>
+  <depend condition="$IGNITION_VERSION == edifice">ignition-gazebo5</depend>
+  <depend condition="$IGNITION_VERSION == fortress">ignition-gazebo6</depend>
+  <depend>ignition-plugin</depend>
+  <depend>pluginlib</depend>
+
+  <depend>yaml_cpp_vendor</depend>
+  <depend>rclcpp_lifecycle</depend>
+
+  <depend>hardware_interface</depend>
+  <depend>controller_manager</depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav2_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+      <build_type>ament_cmake</build_type>
+      <!-- <ign_plugin plugin="${prefix}/ign_hardware_plugins.xml"/> -->
+  </export>
+</package>
diff --git a/src/ign_plugin/src/ignition_plugin.cpp b/src/ign_plugin/src/ignition_plugin.cpp
new file mode 100644
index 0000000..248f85a
--- /dev/null
+++ b/src/ign_plugin/src/ignition_plugin.cpp
@@ -0,0 +1,428 @@
+// Source file template to make custom plugin for gazebo ignition
+
+// Include Ignition Gazebo headers
+// You could erase already includeed files in .hpp file.
+
+// C++
+#include <map>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+#include <thread>
+#include <chrono>
+#include <mutex>
+#include <filesystem>
+
+#include <ignition/gazebo/System.hh>
+#include <ignition/gazebo/Model.hh>
+#include <ignition/gazebo/Link.hh>
+
+#include <ignition/gazebo/components/Link.hh>
+#include <ignition/gazebo/components/Model.hh>
+#include <ignition/gazebo/components/Name.hh>
+#include <ignition/gazebo/components/Pose.hh>
+#include <ignition/gazebo/components/LinearVelocity.hh>
+#include <ignition/gazebo/components/AngularVelocity.hh>
+#include <ignition/gazebo/components/LinearAcceleration.hh>
+#include <ignition/gazebo/components/AngularAcceleration.hh>
+
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/RollingMean.hh>
+#include <ignition/math/Helpers.hh>
+
+#include <ignition/plugin/Register.hh>
+
+// Include ROS 2 headers
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/float32.hpp>
+#include <std_msgs/msg/float32_multi_array.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/wrench.hpp>
+
+#include <yaml-cpp/yaml.h>
+#include "ament_index_cpp/get_package_share_directory.hpp"
+
+#include "ign_plugin/OrbitMechanics.hpp"
+
+#define PI 3.1415926535897931
+
+
+/// @brief Plugin Template for AstroROS in source file
+// Notice namepace and class have to be same as header file.
+
+namespace ign_plugin // Name your plugin namespace
+{
+
+class IgnitionPlugin // Name your plugin class
+  : public ignition::gazebo::System,
+    public ignition::gazebo::ISystemConfigure,
+    public ignition::gazebo::ISystemPreUpdate,
+    public ignition::gazebo::ISystemPostUpdate // these inheritances are fixed.
+{
+public:
+
+  /// \brief Constructor
+  IgnitionPlugin() : orbit_(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0){
+
+    // We assume ROS has already been initialized externally, but if not:
+    if (!rclcpp::ok()) {
+      rclcpp::init(0, nullptr);  // or handle arguments properly
+    }
+
+    this->lin_vel_x_.SetWindowSize(10); this->lin_acc_x_.SetWindowSize(10);
+    this->lin_vel_y_.SetWindowSize(10); this->lin_acc_y_.SetWindowSize(10);
+    this->lin_vel_z_.SetWindowSize(10); this->lin_acc_z_.SetWindowSize(10);
+
+    this->ang_vel_x_.SetWindowSize(10); this->ang_acc_x_.SetWindowSize(10);
+    this->ang_vel_y_.SetWindowSize(10); this->ang_acc_y_.SetWindowSize(10);
+    this->ang_vel_z_.SetWindowSize(10); this->ang_acc_z_.SetWindowSize(10);
+
+  };
+
+  /// \brief Destructor
+  ~IgnitionPlugin() override
+  {
+    if (this->ros_spin_thread_.joinable())
+    {
+      this->ros_spin_thread_.join();
+    }
+    rclcpp::shutdown();
+  }
+
+  /// \brief Configure: Called once at startup
+  /*
+  _entity : objects in ignition.(world,gui,robot etc.) You will add this plugin into xacro file.
+            So you could think this entity is robot "model" you want.
+  _sdf    : sdf/xacro/urdf file. It means an object of sdf you wrote; the file that this plugin
+            added.
+  _ecm    : Entity manager for ignition entities. every entities are monitored by it.
+  &       : If there is a factor in original function but do not use that factor, then "&" could be
+            used without warning. (TBR)
+  */
+  void Configure(
+    const ignition::gazebo::Entity &_entity,
+    const std::shared_ptr<const sdf::Element> &_sdf,
+    ignition::gazebo::EntityComponentManager &_ecm,
+    ignition::gazebo::EventManager &) override
+  {
+    // For demonstration, get link name from SDF parameter (or you could hard-code)
+    if (_sdf->HasElement("model_name")) {
+      this->model_name_ = _sdf->Get<std::string>("model_name");
+    } else {
+      std::cout<<"[ERROR] Missing <model_name> parameter in SDF. Check your SDF/Xacro/URDF file."<<std::endl;
+      return;
+    }
+
+    try {
+      std::filesystem::path yaml_path = this->yaml_file_ / (this->model_name_ + ".yaml");
+      this->yaml_config_ = YAML::LoadFile(yaml_path.string());
+      std::cout << this->yaml_config_["semi_major"].as<double>() << std::endl;
+      std::cout << this->yaml_config_["eccentricity"].as<double>() << std::endl;
+      std::cout << this->yaml_config_["inclination"].as<double>() << std::endl;
+      std::cout << this->yaml_config_["ascending"].as<double>() << std::endl;
+      std::cout << this->yaml_config_["perigee"].as<double>() << std::endl;
+      std::cout << this->yaml_config_["mean_anomaly"].as<double>() << std::endl;
+
+    } catch (const std::exception &e) {
+      std::cout << "[ign_plugin] Failed to load YAML file" << std::endl;
+      return;
+    }
+    this->orbit_ = OrbitMechanics(this->yaml_config_["semi_major"].as<double>(),
+                                  this->yaml_config_["eccentricity"].as<double>(),
+                                  this->yaml_config_["inclination"].as<double>(),
+                                  this->yaml_config_["ascending"].as<double>(),
+                                  this->yaml_config_["perigee"].as<double>(),
+                                  this->yaml_config_["mean_anomaly"].as<double>(),
+                                  0,0,0,0,0,0);
+
+    this->node_ = std::make_shared<rclcpp::Node>(this->model_name_+"_plugin_node");
+    this->orbit_node_ = std::make_shared<rclcpp::Node>(this->model_name_+"_orbit_plugin_node");
+    RCLCPP_INFO(this->node_->get_logger(),"[ign_plugin] Find model name : [%s]", this->model_name_.c_str());
+
+    this->model_info_pub_ = this->node_->create_publisher<std_msgs::msg::Float32MultiArray>(
+                            this->model_name_+"_info", 10);
+    this->model_info_timer_ = this->node_->create_wall_timer(
+      std::chrono::milliseconds(100),  // 10 Hz timer
+      std::bind(&IgnitionPlugin::ModelPoseCallback, this));
+
+    this->model_orbit_pub_ = this->orbit_node_->create_publisher<std_msgs::msg::Float32MultiArray>(
+      this->model_name_+"_orbit", 10);
+    this->model_orbit_timer_ = this->node_->create_wall_timer(
+      std::chrono::milliseconds(100),  // 10 Hz timer
+      std::bind(&IgnitionPlugin::ModelOrbitCallback, this));
+
+    // Make ros thread so that plugin node can operate indepenetly
+    this->ros_spin_thread_ = std::thread([this]() {
+      rclcpp::executors::MultiThreadedExecutor executor;
+      executor.add_node(this->node_);
+      executor.add_node(this->orbit_node_);
+      RCLCPP_INFO(this->node_->get_logger(), "[ign_plugin] ROS 2 MultiThreadedExecutor started.");
+      executor.spin();  // Use MultiThreadedExecutor for safer threading
+    });
+
+    // Attempt to locate the model entity in ECM
+    auto model = ignition::gazebo::Model(_entity);
+    if (!model.Valid(_ecm)) {
+      RCLCPP_ERROR(this->node_->get_logger(), "[ign_plugin] This plugin must be attached to a valid model.");
+      return;
+    }
+    this->model_entity_ = model.Entity();
+    auto model_entity_name = _ecm.Component<ignition::gazebo::components::Name>(this->model_entity_);
+
+    if (model_entity_name->Data() != this->model_name_){
+      RCLCPP_ERROR(this->node_->get_logger(), "[ign_plugin] Unmatched entity name: [%s] [%s].",
+                    model_entity_name->Data().c_str(), this->model_name_.c_str());
+      return;
+    }
+    RCLCPP_INFO(this->node_->get_logger(),"[ign_plugin] Model name matched : [%s] [%s]",
+                model_entity_name->Data().c_str(), this->model_name_.c_str());
+  }
+
+  /// \brief PreUpdate: Called every simulation iteration before physics update
+  /*
+  _info : update data in ignition simulation; puasing, dt, simulation time etc.
+  _ecm : entity component manager
+  */
+  void PreUpdate(const ignition::gazebo::UpdateInfo &_info,
+    ignition::gazebo::EntityComponentManager &_ecm) override
+  {
+    if (_info.paused) {
+    return;  // Do nothing if simulation is paused
+    }
+    if (!this->is_init_)
+    {
+      return;
+    }
+
+    // this->model_orbit_data_.clear();
+    // Vector3d model_accel{
+    //   static_cast<double>(this->model_data_[10]),
+    //   static_cast<double>(this->model_data_[11]),
+    //   static_cast<double>(this->model_data_[12])
+    // };
+    // auto cart_state = this->orbit_.getCartesian();
+    // auto state = this->orbit_.integrateStateRK4(cart_state, _info.dt.count(), model_accel);
+
+    // this->model_orbit_data_.push_back(state(0));
+    // this->model_orbit_data_.push_back(state(1));
+    // this->model_orbit_data_.push_back(state(2));
+    // this->model_orbit_data_.push_back(state(3));
+    // this->model_orbit_data_.push_back(state(4));
+    // this->model_orbit_data_.push_back(state(5));
+  }
+  //===============================================
+  /// \brief PostUpdate: Called after physics update, so we can read final states
+  /*
+  _info : update data in ignition simulation; puasing, dt, simulation time etc.
+  _ecm : entity component manager
+  */
+  void PostUpdate(const ignition::gazebo::UpdateInfo &_info,
+                  const ignition::gazebo::EntityComponentManager &_ecm) override
+  {
+    if (_info.paused) {
+      return;
+    }
+    // Pose Component 얻기
+    auto model_pose = _ecm.Component<ignition::gazebo::components::Pose>(this->model_entity_);
+    if (!model_pose) {
+      RCLCPP_WARN(this->node_->get_logger(), "[ign_plugin] Model pose component not found.");
+      return;
+    }
+    std::lock_guard<std::mutex> lock(this->data_mutex_);
+
+    auto &model_pose_data = model_pose->Data(); // gz::math::Pose3d
+
+    if (!this->is_init_)
+    {
+      this->last_pose_ = model_pose_data;
+      this->last_update_time_ = std::chrono::steady_clock::time_point(_info.simTime);
+      this->last_vel_ = {0, 0, 0, 0, 0, 0};
+      this->is_init_ = true;
+      return;
+    }
+
+    std::chrono::duration<double> dt = _info.dt;
+    if (dt.count() <= 0)
+      return;
+    // this->last_update_time_ = std::chrono::steady_clock::time_point(_info.simTime);
+
+    // RCLCPP_INFO(this->node_->get_logger(), "[ign_plugin] dt : [%lf]", dt.count());
+    this->model_data_.clear();
+
+    double dx = model_pose_data.Pos().X() - this->last_pose_.Pos().X();
+    double dy = model_pose_data.Pos().Y() - this->last_pose_.Pos().Y();
+    double dz = model_pose_data.Pos().Z() - this->last_pose_.Pos().Z();
+
+    double lin_vel_x = dx / dt.count();
+    double lin_vel_y = dy / dt.count();
+    double lin_vel_z = dz / dt.count();
+// =========================================================================================
+
+    // Compute angular velocity
+    double curr_roll = model_pose_data.Rot().Roll();
+    double curr_pitch = model_pose_data.Rot().Pitch();
+    double curr_yaw = model_pose_data.Rot().Yaw();
+
+    double last_roll = this->last_pose_.Rot().Roll();
+    double last_pitch = this->last_pose_.Rot().Pitch();
+    double last_yaw = this->last_pose_.Rot().Yaw();
+
+    double roll_rate = (curr_roll - last_roll) / dt.count();
+    double pitch_rate = (curr_pitch - last_pitch) / dt.count();
+    double yaw_rate = (curr_yaw - last_yaw) / dt.count();
+
+    this->lin_vel_x_.Push(lin_vel_x);
+    this->lin_vel_y_.Push(lin_vel_y);
+    this->lin_vel_z_.Push(lin_vel_z);
+
+    this->ang_vel_x_.Push(roll_rate);
+    this->ang_vel_y_.Push(pitch_rate);
+    this->ang_vel_z_.Push(yaw_rate);
+
+    // Compute acceleration
+    std::vector<double> new_vel = {this->lin_vel_x_.Mean(), this->lin_vel_y_.Mean(),
+                                   this->lin_vel_z_.Mean(), this->ang_vel_x_.Mean(),
+                                   this->ang_vel_y_.Mean(), this->ang_vel_z_.Mean()};
+
+    double dvx = new_vel[0] - this->last_vel_[0];
+    double dvy = new_vel[1] - this->last_vel_[1];
+    double dvz = new_vel[2] - this->last_vel_[2];
+
+    double lin_acc_x = dvx / dt.count();
+    double lin_acc_y = dvy / dt.count();
+    double lin_acc_z = dvz / dt.count();
+
+    double dwx = new_vel[3] - this->last_vel_[3];
+    double dwy = new_vel[4] - this->last_vel_[4];
+    double dwz = new_vel[5] - this->last_vel_[5];
+
+    double ang_acc_x = dwx / dt.count();
+    double ang_acc_y = dwy / dt.count();
+    double ang_acc_z = dwz / dt.count();
+
+    this->lin_acc_x_.Push(lin_acc_x);
+    this->lin_acc_y_.Push(lin_acc_y);
+    this->lin_acc_z_.Push(lin_acc_z);
+
+    this->ang_acc_x_.Push(ang_acc_x);
+    this->ang_acc_y_.Push(ang_acc_y);
+    this->ang_acc_z_.Push(ang_acc_z);
+
+    // Store updated velocity and pose
+    this->last_vel_ = new_vel;
+    this->last_pose_ = model_pose_data;
+
+    // model world data (ECI frame)
+    this->model_data_.push_back(model_pose_data.Pos().X());
+    this->model_data_.push_back(model_pose_data.Pos().Y());
+    this->model_data_.push_back(model_pose_data.Pos().Z());
+
+    this->model_data_.push_back(model_pose_data.Rot().W());
+    this->model_data_.push_back(model_pose_data.Rot().X());
+    this->model_data_.push_back(model_pose_data.Rot().Y());
+    this->model_data_.push_back(model_pose_data.Rot().Z());
+
+    this->model_data_.push_back(this->lin_vel_x_.Mean());
+    this->model_data_.push_back(this->lin_vel_y_.Mean());
+    this->model_data_.push_back(this->lin_vel_z_.Mean());
+
+    this->model_data_.push_back(this->lin_acc_x_.Mean());
+    this->model_data_.push_back(this->lin_acc_y_.Mean());
+    this->model_data_.push_back(this->lin_acc_z_.Mean());
+
+    this->model_data_.push_back(this->ang_vel_x_.Mean());
+    this->model_data_.push_back(this->ang_vel_y_.Mean());
+    this->model_data_.push_back(this->ang_vel_z_.Mean());
+
+    this->model_data_.push_back(this->ang_acc_x_.Mean());
+    this->model_data_.push_back(this->ang_acc_y_.Mean());
+    this->model_data_.push_back(this->ang_acc_z_.Mean());
+
+    this->model_orbit_data_.clear();
+    Vector3d model_accel{
+      static_cast<double>(this->model_data_[10]),
+      static_cast<double>(this->model_data_[11]),
+      static_cast<double>(this->model_data_[12])
+    };
+    auto cart_state = this->orbit_.getCartesian();
+    auto state = this->orbit_.integrateStateRK4(cart_state, dt.count(), model_accel);
+    // RCLCPP_INFO(this->node_->get_logger(), "[ign_plugin] dt : [%lf]", dt.count());
+    this->model_orbit_data_.push_back(state(0));
+    this->model_orbit_data_.push_back(state(1));
+    this->model_orbit_data_.push_back(state(2));
+    this->model_orbit_data_.push_back(state(3));
+    this->model_orbit_data_.push_back(state(4));
+    this->model_orbit_data_.push_back(state(5));
+  }
+
+private:
+
+  std::mutex data_mutex_;  // mutex 멤버 추가
+  //===============================================
+  // ROS 2 Node, Executor, Thread
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Node::SharedPtr orbit_node_;
+  rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr model_info_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr model_orbit_pub_;
+  rclcpp::TimerBase::SharedPtr model_info_timer_;
+  rclcpp::TimerBase::SharedPtr model_orbit_timer_;
+  std::thread ros_spin_thread_;
+  bool is_init_ = false;
+
+  void ModelPoseCallback(){
+    std::lock_guard<std::mutex> lock(this->data_mutex_);
+    std_msgs::msg::Float32MultiArray msg;
+    msg.layout.dim.resize(1);
+    msg.layout.dim[0].label = "model pose information";
+    msg.layout.dim[0].size = this->model_data_.size();
+    msg.layout.dim[0].stride = 1;
+    msg.layout.data_offset = 0;
+    msg.data = this->model_data_;
+
+    this->model_info_pub_->publish(msg);
+    // RCLCPP_INFO(this->node_->get_logger(), "Publishing message to /model_pose at 10 Hz");
+  }
+  void ModelOrbitCallback(){
+    std::lock_guard<std::mutex> lock(this->data_mutex_);
+    std_msgs::msg::Float32MultiArray msg;
+    msg.layout.dim.resize(1);
+    msg.layout.dim[0].label = "model orbit information";
+    msg.layout.dim[0].size = this->model_orbit_data_.size();
+    msg.layout.dim[0].stride = 1;
+    msg.layout.data_offset = 0;
+    msg.data = this->model_orbit_data_;
+
+    this->model_orbit_pub_->publish(msg);
+    // RCLCPP_INFO(this->node_->get_logger(), "Publishing message to /model_pose at 10 Hz");
+  }
+  OrbitMechanics orbit_;
+  // If you want the model as well
+  std::string model_name_;
+  ignition::gazebo::Entity model_entity_{ignition::gazebo::kNullEntity};
+  std::vector<float> model_data_;
+  std::vector<float> model_orbit_data_;
+
+  ignition::math::Pose3d last_pose_;
+  std::vector<double> last_vel_;
+  std::chrono::steady_clock::time_point last_update_time_;
+  ignition::math::RollingMean lin_vel_x_, lin_vel_y_, lin_vel_z_;
+  ignition::math::RollingMean lin_acc_x_, lin_acc_y_, lin_acc_z_;
+  ignition::math::RollingMean ang_vel_x_, ang_vel_y_, ang_vel_z_;
+  ignition::math::RollingMean ang_acc_x_, ang_acc_y_, ang_acc_z_;
+
+  YAML::Node yaml_config_;
+  std::string pkg_share_path_ = ament_index_cpp::get_package_share_directory("ign_plugin");
+  std::filesystem::path yaml_file_ = std::filesystem::path(pkg_share_path_) / "config";
+}; // class IgnitionPlugin
+
+}  // namespace ign_plugin
+
+// Register the plugin with Ignition
+IGNITION_ADD_PLUGIN(
+  ign_plugin::IgnitionPlugin, // this is the registration name for this plugin
+  ignition::gazebo::System,
+  ign_plugin::IgnitionPlugin::ISystemConfigure,
+  ign_plugin::IgnitionPlugin::ISystemPreUpdate,
+  ign_plugin::IgnitionPlugin::ISystemPostUpdate
+)
diff --git a/src/orbit_viewer/orbit_viewer/__init__.py b/src/orbit_viewer/orbit_viewer/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/orbit_viewer/orbit_viewer/orbit_viewer.py b/src/orbit_viewer/orbit_viewer/orbit_viewer.py
new file mode 100644
index 0000000..4f43344
--- /dev/null
+++ b/src/orbit_viewer/orbit_viewer/orbit_viewer.py
@@ -0,0 +1,91 @@
+import rclpy, csv, threading, numpy as np
+from rclpy.node import Node
+from rclpy.executors import MultiThreadedExecutor
+from std_msgs.msg import Float32MultiArray
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+
+class OrbitPlotNode(Node):
+    def __init__(self):
+        super().__init__('orbit_plot_node')
+
+        self.create_subscription(Float32MultiArray,
+                                 '/satellite_orbit',
+                                 self.listener_callback, 10)
+
+        # trajectory data (protected by a lock – we use threads)
+        self._lock   = threading.Lock()
+        self.x_list, self.y_list, self.z_list = [], [], []
+
+        self.csv_file   = open('data.csv', 'a', newline='')
+        self.csv_writer = csv.writer(self.csv_file)
+
+    # ───────────────────────────────── ROS callback ────────────────────────────
+    def listener_callback(self, msg):
+        if len(msg.data) < 3:
+            return
+
+        with self._lock:                         # thread‑safe append
+            self.x_list.append(float(msg.data[0]))
+            self.y_list.append(float(msg.data[1]))
+            self.z_list.append(float(msg.data[2]))
+
+        # raw logging (no lock needed, csv handles its own buffering)
+        self.csv_writer.writerow(msg.data[:3])
+
+    # snapshot gives the GUI thread a *consistent copy*
+    def snapshot(self):
+        with self._lock:
+            return (np.asarray(self.x_list, dtype=float),
+                    np.asarray(self.y_list, dtype=float),
+                    np.asarray(self.z_list, dtype=float))
+
+    def destroy_node(self):
+        self.csv_file.close()
+        super().destroy_node()
+
+# ──────────────────────────────────── main ────────────────────────────────────
+def main():
+    rclpy.init()
+    node      = OrbitPlotNode()
+    executor  = MultiThreadedExecutor()
+    executor.add_node(node)
+
+    # Matplotlib: one Line3D for the trail, one scatter for the satellite
+    fig = plt.figure()
+    ax  = fig.add_subplot(111, projection='3d')
+    limit = (-1.0e7, 1.0e7)
+    ax.set(xlim=limit, ylim=limit, zlim=limit,
+           xlabel='x [m]', ylabel='y [m]', zlabel='z [m]')
+
+    trail_line, = ax.plot([], [], [], lw=1.0)              # whole trajectory
+    head_scatter = ax.scatter([], [], [], s=30)            # current position
+
+    plt.ion(); plt.show()
+
+    try:
+        while rclpy.ok():
+            executor.spin_once(0.01)                       # 100 Hz GUI update
+
+            xs, ys, zs = node.snapshot()
+            if xs.size == 0:                               # nothing yet
+                continue
+
+            # update the trail
+            trail_line.set_data(xs, ys)
+            trail_line.set_3d_properties(zs)
+
+            # update the head (single latest point)
+            head_scatter._offsets3d = (xs[-1:], ys[-1:], zs[-1:])
+
+            plt.draw(); plt.pause(0.001)
+
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        executor.shutdown()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
diff --git a/src/orbit_viewer/package.xml b/src/orbit_viewer/package.xml
new file mode 100644
index 0000000..4e313e0
--- /dev/null
+++ b/src/orbit_viewer/package.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+  <name>orbit_viewer</name>
+  <version>0.15.2</version>
+  <description>Examples of minimal subscribers using rclpy.</description>
+  <maintainer email="sloretz@openrobotics.org">Shane Loretz</maintainer>
+  <maintainer email="aditya.pande@openrobotics.org">Aditya Pande</maintainer>
+  <license>Apache License 2.0</license>
+
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <!-- These test dependencies are optional
+  Their purpose is to make sure that the code passes the linters -->
+  <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/src/orbit_viewer/resource/orbit_viewer b/src/orbit_viewer/resource/orbit_viewer
new file mode 100644
index 0000000..e69de29
diff --git a/src/orbit_viewer/setup.cfg b/src/orbit_viewer/setup.cfg
new file mode 100644
index 0000000..7504ad3
--- /dev/null
+++ b/src/orbit_viewer/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/orbit_viewer
+[install]
+install_scripts=$base/lib/orbit_viewer
diff --git a/src/orbit_viewer/setup.py b/src/orbit_viewer/setup.py
new file mode 100644
index 0000000..19f49fc
--- /dev/null
+++ b/src/orbit_viewer/setup.py
@@ -0,0 +1,35 @@
+from setuptools import setup
+
+package_name = 'orbit_viewer'
+
+setup(
+    name=package_name,
+    version='0.15.2',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    author='Mikael Arguedas',
+    author_email='mikael@osrfoundation.org',
+    maintainer='Aditya Pande, Shane Loretz',
+    maintainer_email='aditya.pande@openrobotics.org, shane@openrobotics.org',
+    keywords=['ROS'],
+    classifiers=[
+        'Intended Audience :: Developers',
+        'License :: OSI Approved :: Apache Software License',
+        'Programming Language :: Python',
+        'Topic :: Software Development',
+    ],
+    description='Examples of minimal subscribers using rclpy.',
+    license='Apache License, Version 2.0',
+    entry_points={
+        'console_scripts': [
+            'orbit_viewer ='
+            ' orbit_viewer.orbit_viewer:main',
+        ],
+    },
+)
diff --git a/src/thruster_plugin/CHANGELOG.rst b/src/thruster_plugin/CHANGELOG.rst
new file mode 100644
index 0000000..efad10f
--- /dev/null
+++ b/src/thruster_plugin/CHANGELOG.rst
@@ -0,0 +1,43 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package ign_ros2_control
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.4.0 (2022-03-18)
+------------------
+* Fix default ign gazebo version Galactic (`#44 <https://github.com/ignitionrobotics/ign_ros2_control/issues/44>`_)
+* Contributors: Alejandro Hernández Cordero
+
+0.3.0 (2022-03-16)
+------------------
+* Fix ignition version in package.xml (`#40 <https://github.com/ignitionrobotics/ign_ros2_control/issues/40>`_)
+* Contributors: Alejandro Hernández Cordero
+
+0.2.0 (2022-02-17)
+------------------
+* Merge pull request `#36 <https://github.com/ignitionrobotics/ign_ros2_control/issues/36>`_ from ignitionrobotics/ahcorde/foxy_to_galactic
+  Foxy -> Galactic
+* Merge remote-tracking branch 'origin/foxy' into ahcorde/foxy_to_galactic
+* Fixed position control (`#29 <https://github.com/ignitionrobotics/ign_ros2_control/issues/29>`_)
+* typo fix. (`#25 <https://github.com/ignitionrobotics/ign_ros2_control/issues/25>`_)
+* Contributors: Alejandro Hernández Cordero, Tomoya Fujita
+
+0.1.2 (2022-02-14)
+------------------
+* Fixed position control (`#29 <https://github.com/ignitionrobotics/ign_ros2_control/issues/29>`_) (`#34 <https://github.com/ignitionrobotics/ign_ros2_control/issues/34>`_)
+* typo fix. (`#25 <https://github.com/ignitionrobotics/ign_ros2_control/issues/25>`_) (`#26 <https://github.com/ignitionrobotics/ign_ros2_control/issues/26>`_)
+  Co-authored-by: Tomoya Fujita <Tomoya.Fujita@sony.com>
+* Contributors: Alejandro Hernández Cordero
+
+0.1.1 (2022-01-07)
+------------------
+* Change package names from ignition\_ to ign\_ (`#19 <https://github.com/ignitionrobotics/ign_ros2_control/pull/22>`_)
+  * Change package names from ignition\_ to ign\_
+* Contributors: Alejandro Hernández Cordero
+
+0.1.0 (2022-01-07)
+------------------
+* Ignition ros2 control (`#1 <https://github.com/ignitionrobotics/ign_ros2_control/issues/1>`_)
+  Co-authored-by: ahcorde <ahcorde@gmail.com>
+  Co-authored-by: Louise Poubel <louise@openrobotics.org>
+  Co-authored-by: Vatan Aksoy Tezer <vatan@picknik.ai>
+* Contributors: Alejandro Hernández Cordero, Louise Poubel, Vatan Aksoy Tezer
diff --git a/src/thruster_plugin/CMakeLists.txt b/src/thruster_plugin/CMakeLists.txt
new file mode 100644
index 0000000..ed14eb3
--- /dev/null
+++ b/src/thruster_plugin/CMakeLists.txt
@@ -0,0 +1,121 @@
+cmake_minimum_required(VERSION 3.5)
+project(thruster_plugin)
+
+# Default to C11
+if(NOT CMAKE_C_STANDARD)
+    set(CMAKE_C_STANDARD 11)
+endif()
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 17)
+endif()
+
+# Compiler options
+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(ament_index_cpp REQUIRED)
+find_package(controller_manager REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(yaml_cpp_vendor REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
+
+if("$ENV{IGNITION_VERSION}" STREQUAL "citadel")
+  find_package(ignition-gazebo3 REQUIRED)
+  set(IGN_GAZEBO_VER ${ignition-gazebo3_VERSION_MAJOR})
+  message(STATUS "Compiling against Ignition Citadel")
+
+elseif("$ENV{IGNITION_VERSION}" STREQUAL "edifice")
+  find_package(ignition-gazebo5 REQUIRED)
+  set(IGN_GAZEBO_VER ${ignition-gazebo5_VERSION_MAJOR})
+  message(STATUS "Compiling against Ignition Edifice")
+
+elseif("$ENV{IGNITION_VERSION}" STREQUAL "fortress")
+  find_package(ignition-gazebo6 REQUIRED)
+  set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR})
+  message(STATUS "Compiling against Ignition Fortress")
+
+else()
+  find_package(ignition-gazebo6 REQUIRED)
+  set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR})
+  message(STATUS "Compiling against Ignition Fortress")
+endif()
+
+find_package(ignition-plugin1 REQUIRED)
+set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
+
+# include_directories(include)
+# include_directories(include/${PROJECT_NAME})
+include_directories(${nav2_msgs_INCLUDE_DIRS})
+include_directories(${std_msgs_INCLUDE_DIRS})
+include_directories(${geometry_msgs_INCLUDE_DIRS})
+
+add_library(${PROJECT_NAME} SHARED
+  src/thruster_plugin.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ignition-gazebo${IGN_GAZEBO_VER}::core
+  ignition-plugin${IGN_PLUGIN_VER}::register
+)
+ament_target_dependencies(${PROJECT_NAME}
+  ament_index_cpp
+  controller_manager
+  hardware_interface
+  pluginlib
+  rclcpp
+  yaml_cpp_vendor
+  rclcpp_lifecycle
+  std_msgs
+  nav2_msgs
+  geometry_msgs
+)
+
+#########
+
+# add_library(ign_hardware_plugins SHARED
+#   src/ign_system.cpp
+# )
+# ament_target_dependencies(ign_hardware_plugins
+#   rclcpp_lifecycle
+#   hardware_interface
+#   rclcpp
+# )
+# target_link_libraries(ign_hardware_plugins
+#   ignition-gazebo${IGN_GAZEBO_VER}::core
+# )
+
+## Install
+# install(TARGETS
+#   ign_hardware_plugins
+#   ARCHIVE DESTINATION lib
+#   LIBRARY DESTINATION lib
+#   RUNTIME DESTINATION bin
+# )
+
+# Testing and linting
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+# ament_export_include_directories(include)
+# ament_export_include_directories(include/${PROJECT_NAME})
+# ament_export_libraries(${PROJECT_NAME} ign_hardware_plugins)
+
+# Install directories
+install(TARGETS ${PROJECT_NAME}
+  DESTINATION lib
+)
+
+# pluginlib_export_plugin_description_file(ign_plugin ign_hardware_plugins.xml)
+
+# Setup the project
+ament_package()
diff --git a/src/thruster_plugin/LICENSE b/src/thruster_plugin/LICENSE
new file mode 100644
index 0000000..6d8d58f
--- /dev/null
+++ b/src/thruster_plugin/LICENSE
@@ -0,0 +1,191 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        https://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
+
+   Copyright 2013-2018 Docker, Inc.
+
+   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/src/thruster_plugin/package.xml b/src/thruster_plugin/package.xml
new file mode 100644
index 0000000..f3dde9f
--- /dev/null
+++ b/src/thruster_plugin/package.xml
@@ -0,0 +1,39 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>thruster_plugin</name>
+  <version>0.4.0</version>
+  <description>Ignition ros2_control package allows to control simulated robots using ros2_control framework.</description>
+  <maintainer email="alejandro@openrobotics.com">Alejandro Hernández</maintainer>
+  <author>Alejandro Hernández</author>
+  <license>Apache 2</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>ament_index_cpp</depend>
+  <!-- default version to use in official ROS2 packages is Ignition Fortress for ROS2 Rolling -->
+  <depend condition="$IGNITION_VERSION == ''">ignition-gazebo6</depend>
+  <depend condition="$IGNITION_VERSION == citadel">ignition-gazebo3</depend>
+  <depend condition="$IGNITION_VERSION == edifice">ignition-gazebo5</depend>
+  <depend condition="$IGNITION_VERSION == fortress">ignition-gazebo6</depend>
+  <depend>ignition-plugin</depend>
+  <depend>pluginlib</depend>
+
+  <depend>yaml_cpp_vendor</depend>
+  <depend>rclcpp_lifecycle</depend>
+
+  <depend>hardware_interface</depend>
+  <depend>controller_manager</depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav2_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+      <build_type>ament_cmake</build_type>
+      <!-- <ign_plugin plugin="${prefix}/ign_hardware_plugins.xml"/> -->
+  </export>
+</package>
diff --git a/src/thruster_plugin/src/thruster_plugin.cpp b/src/thruster_plugin/src/thruster_plugin.cpp
new file mode 100644
index 0000000..844b1d0
--- /dev/null
+++ b/src/thruster_plugin/src/thruster_plugin.cpp
@@ -0,0 +1,241 @@
+// Source file template to make custom plugin for gazebo ignition
+
+// Include Ignition Gazebo headers
+// You could erase already includeed files in .hpp file.
+
+// C++
+#include <map>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+#include <thread>
+#include <chrono>
+#include <mutex>
+
+#include <ignition/gazebo/System.hh>
+#include <ignition/gazebo/Model.hh>
+#include <ignition/gazebo/Link.hh>
+
+#include <ignition/gazebo/components/Link.hh>
+#include <ignition/gazebo/components/Model.hh>
+#include <ignition/gazebo/components/Name.hh>
+#include <ignition/gazebo/components/Pose.hh>
+
+#include <ignition/math/Vector3.hh>
+
+#include <ignition/plugin/Register.hh>
+
+// Include ROS 2 headers
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/float32.hpp>
+
+#define PI 3.1415926535897931
+
+/// @brief Plugin Template for AstroROS in source file
+// Notice namepace and class have to be same as header file.
+
+namespace thruster_plugin // Name your plugin namespace
+{
+
+class ThrusterPlugin // Name your plugin class
+  : public ignition::gazebo::System,
+    public ignition::gazebo::ISystemConfigure,
+    public ignition::gazebo::ISystemPreUpdate,
+    public ignition::gazebo::ISystemPostUpdate // these inheritances are fixed.
+{
+public:
+
+  /// \brief Constructor
+  ThrusterPlugin(){
+
+    // We assume ROS has already been initialized externally, but if not:
+    if (!rclcpp::ok()) {
+      rclcpp::init(0, nullptr);  // or handle arguments properly
+    }
+  };
+
+  /// \brief Destructor
+  ~ThrusterPlugin() override
+  {
+    if (this->ros_spin_thread_.joinable())
+    {
+      this->ros_spin_thread_.join();
+    }
+    rclcpp::shutdown();
+  }
+
+  /// \brief Configure: Called once at startup
+  /*
+  _entity : objects in ignition.(world,gui,robot etc.) You will add this plugin into xacro file.
+            So you could think this entity is robot "model" you want.
+  _sdf    : sdf/xacro/urdf file. It means an object of sdf you wrote; the file that this plugin
+            added.
+  _ecm    : Entity manager for ignition entities. every entities are monitored by it.
+  &       : If there is a factor in original function but do not use that factor, then "&" could be
+            used without warning. (TBR)
+  */
+  void Configure(
+    const ignition::gazebo::Entity &_entity,
+    const std::shared_ptr<const sdf::Element> &_sdf,
+    ignition::gazebo::EntityComponentManager &_ecm,
+    ignition::gazebo::EventManager &) override
+  {
+    // For demonstration, get link name from SDF parameter (or you could hard-code)
+    if (_sdf->HasElement("link_name")) {
+      this->link_name_ = _sdf->Get<std::string>("link_name");
+    } else {
+      std::cout<<"[ERROR] Missing <link_name> parameter in SDF. Check your SDF/Xacro/URDF file."<<std::endl;
+      return;
+    }
+    // Contain ECM for this plugin
+    // this->ecm_ = &_ecm;
+
+    this->node_ = std::make_shared<rclcpp::Node>(this->link_name_+"_plugin_node");
+    RCLCPP_INFO(this->node_->get_logger(),
+            "[%s_plugin] Find model name : [%s]",
+            this->link_name_.c_str(), this->link_name_.c_str());
+
+    // ex) topic name should be thru_1_cmd; link_name => thru_1
+    this->thruster_cmd_sub_ = this->node_->create_subscription<std_msgs::msg::Float32>(
+                            this->link_name_+"_cmd", 10,
+                            std::bind(&ThrusterPlugin::CommandCallback, this, std::placeholders::_1)
+                          );
+
+    // Make ros thread so that plugin node can operate indepenetly
+    this->ros_spin_thread_ = std::thread([this]() {
+      rclcpp::executors::MultiThreadedExecutor executor;
+      executor.add_node(this->node_);
+      RCLCPP_INFO(this->node_->get_logger(),
+                "[%s_plugin] ROS 2 MultiThreadedExecution",
+                 this->link_name_.c_str());
+      executor.spin();  // Use MultiThreadedExecutor for safer threading
+    });
+
+    // Attempt to locate the model entity in ECM
+    auto model = ignition::gazebo::Model(_entity);
+    if (!model.Valid(_ecm)) {
+      RCLCPP_ERROR(this->node_->get_logger(),
+                 "[%s_plugin] This plugin must be attached to a valid model.",
+                 this->link_name_.c_str());
+      return;
+    }
+
+    auto link_entities = _ecm.ChildrenByComponents(model.Entity(), ignition::gazebo::components::Link());
+    auto link_exist  = false;
+    for (auto &link_entity : link_entities) {
+      auto link_comp_name = _ecm.Component<ignition::gazebo::components::Name>(link_entity);
+      if (link_comp_name && link_comp_name->Data() == this->link_name_) {
+        this->link_entity_ = link_entity;
+        this->link_ = ignition::gazebo::Link(link_entity);
+        RCLCPP_INFO(this->node_->get_logger(), "[%s_plugin] Found link [%s] (Entity=%lu).",
+        this->link_name_.c_str(),link_comp_name->Data().c_str(),
+        static_cast<unsigned long>(link_entity));
+        link_exist = true;
+      }
+    }
+    if(!link_exist){
+      RCLCPP_ERROR(this->node_->get_logger(),
+      "[%s_plugin] Link doesn't exist in this model. Check your SDF/Xacro/URDF file.",
+      this->link_name_.c_str());
+      return;
+    }
+  }
+
+  /// \brief PreUpdate: Called every simulation iteration before physics update
+  /*
+  _info : update data in ignition simulation; puasing, dt, simulation time etc.
+  _ecm : entity component manager
+  */
+  void PreUpdate(const ignition::gazebo::UpdateInfo &_info,
+    ignition::gazebo::EntityComponentManager &_ecm) override
+  {
+    if (_info.paused) {
+    return;  // Do nothing if simulation is paused
+    }
+    auto link_pose = _ecm.Component<ignition::gazebo::components::Pose>(this->link_entity_);
+    if (!this->is_init_){
+      if (link_pose) {
+        this->link_pose_ = link_pose->Data();
+        this->is_init_ = true;
+        return;
+      }
+    }
+    std::lock_guard<std::mutex> lock(this->data_mutex_);
+    this->link_pose_ = link_pose->Data();
+
+    if (this->shot_){
+      ignition::math::Vector3d force(0, 0, -this->force_);
+      ignition::math::Vector3d torque(0, 0, 0);
+      this->link_.AddWorldWrench(_ecm,
+      this->link_pose_.Rot().RotateVector(force),
+      torque);
+      this->force_ = 0;
+      this->shot_ = false;
+    }
+  }
+  //===============================================
+  /// \brief PostUpdate: Called after physics update, so we can read final states
+  /*
+  _info : update data in ignition simulation; puasing, dt, simulation time etc.
+  _ecm : entity component manager
+  */
+  void PostUpdate(const ignition::gazebo::UpdateInfo &_info,
+                  const ignition::gazebo::EntityComponentManager &_ecm) override
+  {
+    if (_info.paused) {
+      return;
+    }
+    auto link_pose = _ecm.Component<ignition::gazebo::components::Pose>(this->link_entity_);
+    if (!link_pose) {
+      RCLCPP_WARN(this->node_->get_logger(),
+      "[%s_plugin] Model pose component not found.",
+      this->link_name_.c_str());
+      return;
+    }
+    std::lock_guard<std::mutex> lock(this->data_mutex_);
+    this->link_pose_ = link_pose->Data();
+  }
+
+private:
+
+  std::mutex data_mutex_;  // mutex for avoiding race condition
+  //===============================================
+  // ROS 2 Node, Executor, Thread
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr thruster_cmd_sub_;
+  std::thread ros_spin_thread_;
+  bool is_init_ = false;
+
+  //===============================================
+  // ignition::gazebo::EntityComponentManager* ecm_;
+
+  //===============================================
+  std::string link_name_;
+  ignition::gazebo::Entity link_entity_{ignition::gazebo::kNullEntity};
+  ignition::gazebo::Link link_;
+  ignition::math::Pose3d link_pose_;
+
+  //===============================================
+  float force_ = 0;
+  bool shot_ = false;
+  void CommandCallback(const std_msgs::msg::Float32::SharedPtr _msg)
+  {
+    std::lock_guard<std::mutex> lock(this->data_mutex_);
+    // this->link_.AddWorldForce(*this->ecm_,
+    // this->link_pose_.Rot().RotateVector(ignition::math::Vector3d(0, 0, -_msg->data)));
+    this->force_ = _msg->data;
+    this->shot_=true;
+  }
+}; // class ThrusterPlugin
+
+}  // namespace thruster_plugin
+
+// Register the plugin with Ignition
+IGNITION_ADD_PLUGIN(
+  thruster_plugin::ThrusterPlugin, // this is the registration name for this plugin
+  ignition::gazebo::System,
+  thruster_plugin::ThrusterPlugin::ISystemConfigure,
+  thruster_plugin::ThrusterPlugin::ISystemPreUpdate,
+  thruster_plugin::ThrusterPlugin::ISystemPostUpdate
+)