diff --git a/base_twerk/include/base_twerk/base_twerk_action_server.hpp b/base_twerk/include/base_twerk/base_twerk_action_server.hpp
index bfa8300..98c486c 100644
--- a/base_twerk/include/base_twerk/base_twerk_action_server.hpp
+++ b/base_twerk/include/base_twerk/base_twerk_action_server.hpp
@@ -9,6 +9,7 @@
 
 #include "base_twerk_msgs/action/base_twerk_action.hpp"
 #include "base_twerk_msgs/srv/base_pose_setpoint.hpp"
+#include "base_twerk_msgs/srv/get_current_base_pose.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
 namespace penta_pod::kin::base_twerk {
@@ -16,6 +17,7 @@ namespace penta_pod::kin::base_twerk {
 using BaseTwerkAction = base_twerk_msgs::action::BaseTwerkAction;
 using GoalHandle = rclcpp_action::ServerGoalHandle<BaseTwerkAction>;
 using BasePoseSetpointSrv = base_twerk_msgs::srv::BasePoseSetpoint;
+using GetCurrentBasePose = base_twerk_msgs::srv::GetCurrentBasePose;
 using geometry_msgs::msg::PoseStamped;
 using penta_pod::kin::commons::service_call_template;
 
@@ -27,7 +29,7 @@ class BaseTwerkActionServer {
 
   rclcpp::Client<BasePoseSetpointSrv>::SharedPtr setpoint_client_;
 
-  rclcpp::Subscription<PoseStamped>::SharedPtr base_pose_subscriber_;
+  rclcpp::Client<GetCurrentBasePose>::SharedPtr get_currnet_pose_client_;
 
   rclcpp::CallbackGroup::SharedPtr callback_group_;
 
@@ -46,9 +48,12 @@ class BaseTwerkActionServer {
   auto get_parameters() -> void;
 
   auto call_setpoint_client(const PoseStamped &base_to_basefootprint) -> bool;
+
   auto calculate_twerk_pose_from_goal(
       rclcpp::Time start_time, PoseStamped start_pose,
       const std::shared_ptr<GoalHandle> goal_handle) -> PoseStamped;
+      
+  auto quiry_current_base_pose() -> std::optional<GetCurrentBasePose::Response>;
 
   double update_interval_millis_double_;
   double max_permissible_displacement_meter_;
diff --git a/base_twerk/include/base_twerk/base_twerk_cmd_publisher.hpp b/base_twerk/include/base_twerk/base_twerk_cmd_publisher.hpp
index 2f4adb2..7b036c8 100644
--- a/base_twerk/include/base_twerk/base_twerk_cmd_publisher.hpp
+++ b/base_twerk/include/base_twerk/base_twerk_cmd_publisher.hpp
@@ -6,18 +6,23 @@
 
 // include messages
 #include "base_twerk_msgs/srv/base_pose_setpoint.hpp"
+#include "base_twerk_msgs/srv/get_current_base_pose.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
 namespace penta_pod::kin::base_twerk {
 
 using PoseStamped = geometry_msgs::msg::PoseStamped;
 using BasePoseSetpoint = base_twerk_msgs::srv::BasePoseSetpoint;
+using GetCurrentBasePose = base_twerk_msgs::srv::GetCurrentBasePose;
+
+const auto base_footprint = "base_footprint";
 
 class BaseTwerkCmdPuplisher {
 private:
   rclcpp::Node::SharedPtr node_;
   rclcpp::Publisher<PoseStamped>::SharedPtr base_pose_publisher_;
   rclcpp::Service<BasePoseSetpoint>::SharedPtr setpoint_service_;
+  rclcpp::Service<GetCurrentBasePose>::SharedPtr get_current_base_pose_;
   rclcpp::CallbackGroup::SharedPtr callback_group_;
   rclcpp::TimerBase::SharedPtr timer_;
 
@@ -31,6 +36,7 @@ class BaseTwerkCmdPuplisher {
   void load_parameters();
   void timer_callback();
   void create_setpoint_service();
+  void create_get_current_base_pose_service();
 
 public:
   explicit BaseTwerkCmdPuplisher();
diff --git a/base_twerk/launch/base_twerk_action_server.launch.py b/base_twerk/launch/base_twerk_action_server.launch.py
index 732e5c2..90081e3 100644
--- a/base_twerk/launch/base_twerk_action_server.launch.py
+++ b/base_twerk/launch/base_twerk_action_server.launch.py
@@ -3,6 +3,7 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
+
 def generate_launch_description():
     config = os.path.join(
         get_package_share_directory("penta_description"),
@@ -10,10 +11,12 @@ def generate_launch_description():
         "general_config.yaml",
     )
     ld = LaunchDescription()
-    ld.add_action(Node(
-        package="base_twerk",
-        executable="base_twerk_action_server_node",
-        output="screen",
-        parameters=[config],
-    ))
+    ld.add_action(
+        Node(
+            package="base_twerk",
+            executable="base_twerk_action_server_node",
+            output="screen",
+            parameters=[config],
+        )
+    )
     return ld
diff --git a/base_twerk/launch/null_pose_publisher.launch.py b/base_twerk/launch/null_pose_publisher.launch.py
index 457c8be..8de29ef 100644
--- a/base_twerk/launch/null_pose_publisher.launch.py
+++ b/base_twerk/launch/null_pose_publisher.launch.py
@@ -3,6 +3,7 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
+
 def generate_launch_description():
     config = os.path.join(
         get_package_share_directory("penta_description"),
@@ -10,10 +11,12 @@ def generate_launch_description():
         "general_config.yaml",
     )
     ld = LaunchDescription()
-    ld.add_action(Node(
-        package="base_twerk",
-        executable="base_twerk_publisher_node",
-        output="screen",
-        parameters=[config],
-    ))
+    ld.add_action(
+        Node(
+            package="base_twerk",
+            executable="base_twerk_publisher_node",
+            output="screen",
+            parameters=[config],
+        )
+    )
     return ld
diff --git a/base_twerk/src/base_twerk/base_twerk_action_server.cpp b/base_twerk/src/base_twerk/base_twerk_action_server.cpp
index 7576294..49a958d 100644
--- a/base_twerk/src/base_twerk/base_twerk_action_server.cpp
+++ b/base_twerk/src/base_twerk/base_twerk_action_server.cpp
@@ -1,4 +1,5 @@
 #include "base_twerk/base_twerk_action_server.hpp"
+#include "commons/quaternion_utils.hpp"
 
 namespace penta_pod::kin::base_twerk {
 
@@ -16,14 +17,9 @@ BaseTwerkActionServer::BaseTwerkActionServer()
   this->setpoint_client_ = node_->create_client<BasePoseSetpointSrv>(
       "cmd_null_setpoint", rmw_qos_profile_services_default, callback_group_);
 
-  this->base_pose_subscriber_ = node_->create_subscription<PoseStamped>(
-      "null_space_pose", 10, [this](const PoseStamped &msg) -> void {
-        {
-          std::lock_guard<std::mutex> lock(received_base_pose_mutex_);
-          received_base_pose_.timestamp = node_->now();
-          received_base_pose_.value = msg;
-        }
-      });
+  this->get_currnet_pose_client_ = node_->create_client<GetCurrentBasePose>(
+      "get_current_null_pose", rmw_qos_profile_services_default,
+      callback_group_);
 
   this->base_twerk_action_server_ =
       rclcpp_action::create_server<BaseTwerkAction>(
@@ -65,8 +61,9 @@ auto BaseTwerkActionServer::execute(
   const auto goal = goal_handle->get_goal();
   auto result = std::make_shared<BaseTwerkAction::Result>();
 
-  double mag_displacement = std::sqrt(
-      goal->rx * goal->rx + goal->ry * goal->ry + goal->rz * goal->rz);
+  double mag_displacement =
+      std::sqrt(goal->r[0] * goal->r[0] + goal->r[1] * goal->r[1] +
+                goal->r[2] * goal->r[2]);
   if (mag_displacement > max_permissible_displacement_meter_) {
     auto message = "Action aborted for the specified displacement is: " +
                    std::to_string(mag_displacement) +
@@ -96,26 +93,19 @@ auto BaseTwerkActionServer::execute(
     return false;
   };
 
-  // get current null pose
-  auto start_time = node_->now();
-  while (rclcpp::ok()) {
-    rate.sleep();
-
-    if (received_base_pose_.timestamp > start_time) {
-      break;
-    }
-
-    if (is_timed_out(start_time, 1s)) {
-      result->result_message = "Timed out, did not receive transform message";
-      goal_handle->abort(result);
-      RCLCPP_WARN(node_->get_logger(), "Time out, breaking out from action");
-      return;
-    }
+  // get current null space pose
+  auto get_currnet_pose_response = quiry_current_base_pose();
+  if (!get_currnet_pose_response.has_value()) {
+    result->result_message = "Failed to get current base pose";
+    goal_handle->abort(result);
+    return;
   }
 
   PoseStamped start_pose{};
   {
     std::lock_guard<std::mutex> lock(received_base_pose_mutex_);
+    received_base_pose_ = {node_->now(),
+                           get_currnet_pose_response.value().pose};
     start_pose = received_base_pose_.value;
     RCLCPP_INFO(node_->get_logger(), "Current base pose is: x:%f y:%f z:%f",
                 start_pose.pose.position.x, start_pose.pose.position.y,
@@ -126,13 +116,13 @@ auto BaseTwerkActionServer::execute(
       std::chrono::milliseconds(static_cast<int>(goal->dance_time_millis));
 
   // start control loop for base pose motion
-  start_time = node_->now();
+  auto start_time = node_->now();
   while (rclcpp::ok()) {
     rate.sleep();
 
-    auto current_pose =
+    auto updated_pose =
         calculate_twerk_pose_from_goal(start_time, start_pose, goal_handle);
-    if (!call_setpoint_client(current_pose)) {
+    if (!call_setpoint_client(updated_pose)) {
       return;
     }
 
@@ -148,6 +138,9 @@ auto BaseTwerkActionServer::execute(
     }
 
     if (goal_handle->is_canceling()) {
+      if (!call_setpoint_client(start_pose)) {
+        return;
+      }
       result->result_message = "action canceled";
       goal_handle->canceled(result);
       RCLCPP_INFO(node_->get_logger(), "Goal canceled");
@@ -156,6 +149,54 @@ auto BaseTwerkActionServer::execute(
   }
 }
 
+auto BaseTwerkActionServer::quiry_current_base_pose()
+    -> std::optional<GetCurrentBasePose::Response> {
+
+  // check if service is available
+  auto service_name = get_currnet_pose_client_->get_service_name();
+  if (!get_currnet_pose_client_->wait_for_service(1s)) {
+    RCLCPP_ERROR(node_->get_logger(), "Service %s not online!", service_name);
+    return std::nullopt;
+  }
+
+  // send request
+  auto get_current_pose_request =
+      std::make_shared<GetCurrentBasePose::Request>();
+
+  auto start_time = node_->now();
+
+  auto future =
+      get_currnet_pose_client_->async_send_request(get_current_pose_request);
+
+  using namespace std::chrono_literals;
+  const auto timeout_millis = std::chrono::milliseconds(500);
+
+  auto status = future.wait_for(timeout_millis);
+
+  if (status != std::future_status::ready) {
+    RCLCPP_ERROR(node_->get_logger(), "Service %s response timed out!",
+                 service_name);
+    return std::nullopt;
+  }
+
+  if (!future.valid()) {
+    RCLCPP_ERROR(node_->get_logger(), "Future from service %s is not valid!",
+                 service_name);
+    return std::nullopt;
+  }
+
+  auto result = future.get();
+  if (!result) {
+    RCLCPP_ERROR(node_->get_logger(), "Returned null Service %s response!",
+                 service_name);
+    return std::nullopt;
+  }
+
+  RCLCPP_INFO(node_->get_logger(), "Service %s response is ready!",
+              service_name);
+  return *result;
+}
+
 auto BaseTwerkActionServer::calculate_twerk_pose_from_goal(
     rclcpp::Time start_time, PoseStamped start_pose,
     const std::shared_ptr<GoalHandle> goal_handle) -> PoseStamped {
@@ -166,14 +207,23 @@ auto BaseTwerkActionServer::calculate_twerk_pose_from_goal(
 
   pose.pose.position.x =
       start_pose.pose.position.x +
-      goal->rx * sin(goal->w * delta_t_seconds + goal->phi_x);
+      goal->r[0] * sin(goal->w * delta_t_seconds + goal->phi[0]);
   pose.pose.position.y =
       start_pose.pose.position.y +
-      goal->ry * sin(goal->w * delta_t_seconds + goal->phi_y);
+      goal->r[1] * sin(goal->w * delta_t_seconds + goal->phi[1]);
   pose.pose.position.z =
       start_pose.pose.position.z +
-      goal->rz * sin(goal->w * delta_t_seconds + goal->phi_z);
+      goal->r[2] * sin(goal->w * delta_t_seconds + goal->phi[2]);
+
+  std::vector<double> rpy = {0., 0., 0.};
+  for (int i = 3; i < 6; i++) {
+    rpy[i - 3] = goal->r[i] * sin(goal->w * delta_t_seconds + goal->phi[i]);
+  }
 
+  auto roll = rpy[0];
+  auto pitch = rpy[1];
+  auto yaw = rpy[2];
+  pose.pose.orientation = rpy_to_quaternion(yaw, pitch, roll);
   return pose;
 }
 
diff --git a/base_twerk/src/base_twerk/base_twerk_cmd_publisher.cpp b/base_twerk/src/base_twerk/base_twerk_cmd_publisher.cpp
index 3c70302..f9b3833 100644
--- a/base_twerk/src/base_twerk/base_twerk_cmd_publisher.cpp
+++ b/base_twerk/src/base_twerk/base_twerk_cmd_publisher.cpp
@@ -20,17 +20,18 @@ BaseTwerkCmdPuplisher::BaseTwerkCmdPuplisher()
       std::chrono::milliseconds(static_cast<int>(update_interval_millis_)),
       [this]() { timer_callback(); });
 
+  callback_group_ = node_->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+
   create_setpoint_service();
+  create_get_current_base_pose_service();
 }
 
 void BaseTwerkCmdPuplisher::create_setpoint_service() {
-  callback_group_ = node_->create_callback_group(
-      rclcpp::CallbackGroupType::MutuallyExclusive);
   auto lambda =
       [this](const BasePoseSetpoint::Request::SharedPtr &request,
              const BasePoseSetpoint::Response::SharedPtr &response) -> bool {
     auto frame_id = request->pose.header.frame_id;
-    const auto base_footprint = "base_footprint";
     if (frame_id == "") {
       frame_id = base_footprint;
     }
@@ -54,6 +55,22 @@ void BaseTwerkCmdPuplisher::create_setpoint_service() {
       callback_group_);
 }
 
+void BaseTwerkCmdPuplisher::create_get_current_base_pose_service() {
+  auto lambda =
+      [this](const GetCurrentBasePose::Request::SharedPtr & /*request*/,
+             const GetCurrentBasePose::Response::SharedPtr &response) -> bool {
+    auto pose = base_pose_;
+    pose.header.stamp = node_->now();
+    pose.header.frame_id = base_footprint;
+    response->pose = pose;
+    return true;
+  };
+
+  get_current_base_pose_ = node_->create_service<GetCurrentBasePose>(
+      "get_current_null_pose", lambda, rmw_qos_profile_services_default,
+      callback_group_);
+}
+
 void BaseTwerkCmdPuplisher::timer_callback() {
   double deta_t_sec = update_interval_millis_ / 1000.0;
   using namespace penta_pod::kin::commons;
diff --git a/commons/include/commons/quaternion_utils.hpp b/commons/include/commons/quaternion_utils.hpp
new file mode 100644
index 0000000..e7b6cb6
--- /dev/null
+++ b/commons/include/commons/quaternion_utils.hpp
@@ -0,0 +1,38 @@
+#ifndef COMMONS_QUAT_UTILS_HPP_
+#define COMMONS_QUAT_UTILS_HPP_
+
+#include "geometry_msgs/msg/quaternion.hpp"
+
+using geometry_msgs::msg::Quaternion;
+
+Quaternion rpy_to_quaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
+{
+  double cy = cos(yaw * 0.5);
+  double sy = sin(yaw * 0.5);
+  double cp = cos(pitch * 0.5);
+  double sp = sin(pitch * 0.5);
+  double cr = cos(roll * 0.5);
+  double sr = sin(roll * 0.5);
+
+  Quaternion q;
+  q.w = cy * cp * cr + sy * sp * sr;
+  q.x = cy * cp * sr - sy * sp * cr;
+  q.y = sy * cp * sr + cy * sp * cr;
+  q.z = sy * cp * cr - cy * sp * sr;
+  return q;
+}
+
+Quaternion hamilton_product(Quaternion u, Quaternion v)
+{
+    Quaternion result;
+
+    result.w = u.w*v.w - u.x*v.x - u.y*v.y - u.z*v.z;
+    result.x = u.w*v.x + u.x*v.w + u.y*v.z - u.z*v.y;
+    result.y = u.w*v.y - u.x*v.z + u.y*v.w + u.z*v.x;
+    result.z = u.w*v.z + u.x*v.y - u.y*v.x + u.z*v.w;
+
+    return result;
+
+}
+
+#endif // COMMONS_QUAT_UTILS_HPP_
\ No newline at end of file
diff --git a/commons/include/commons/transform_utils.hpp b/commons/include/commons/transform_utils.hpp
index cc66e10..eb5b79f 100644
--- a/commons/include/commons/transform_utils.hpp
+++ b/commons/include/commons/transform_utils.hpp
@@ -52,10 +52,10 @@ auto get_direction_from_vec(const std::vector<double> &vec, double norm)
   return output_vec;
 }
 
-auto interpolate_transform(const rclcpp::Node::SharedPtr node, Transform target,
-                           Transform source, double linear_vel,
-                           double /*angular_vel*/, double dt_sec)
-    -> std::optional<Transform> {
+auto interpolate_transform(const rclcpp::Node::SharedPtr /*node*/,
+                           Transform target, Transform source,
+                           double linear_vel, double /*angular_vel*/,
+                           double dt_sec) -> std::optional<Transform> {
 
   Transform interpolated{};
 
@@ -65,9 +65,11 @@ auto interpolate_transform(const rclcpp::Node::SharedPtr node, Transform target,
 
   double linear_displacement = linear_vel * dt_sec;
   if (norm < linear_displacement) { // almost near eachothers
+    /*
     RCLCPP_INFO(node->get_logger(),
                 "norm %f is less than the discrete displacement %f ", norm,
                 linear_displacement);
+    */
     interpolated = target;
     return interpolated;
   }
@@ -123,9 +125,11 @@ auto interpolate_pose(const rclcpp::Node::SharedPtr node, PoseStamped target,
 
   double linear_displacement = linear_vel * dt_sec;
   if (norm < linear_displacement) {
+    /*
     RCLCPP_INFO(node->get_logger(),
                 "norm %f is less than the discrete displacement %f", norm,
                 linear_displacement);
+    */
     interpolated = target;
     return interpolated;
   }
@@ -143,7 +147,8 @@ auto interpolate_pose(const rclcpp::Node::SharedPtr node, PoseStamped target,
   interpolated.pose.position.z =
       source.pose.position.z + dir[2] * linear_displacement;
 
-  interpolated.pose.orientation = source.pose.orientation;
+  // ToDo interpolate orientation
+  interpolated.pose.orientation = target.pose.orientation;
 
   return interpolated;
 }
diff --git a/commons/src/commons/librarry.cpp b/commons/src/commons/librarry.cpp
index 4ca31e7..f6a7c4a 100644
--- a/commons/src/commons/librarry.cpp
+++ b/commons/src/commons/librarry.cpp
@@ -1,4 +1,5 @@
 #include "commons/ros2_utils.hpp"
 #include "commons/transform_utils.hpp"
+#include "commons/quaternion_utils.hpp"
 
 namespace penta_pod::kin::commons {}; // namespace penta_pod::kin::commons
diff --git a/docs/penta-pod calibration.odt b/docs/penta-pod calibration.odt
new file mode 100644
index 0000000..67ec8bb
Binary files /dev/null and b/docs/penta-pod calibration.odt differ
diff --git a/docs/penta-pod calibration.pdf b/docs/penta-pod calibration.pdf
new file mode 100644
index 0000000..5ee152b
Binary files /dev/null and b/docs/penta-pod calibration.pdf differ
diff --git a/gait_generator/CMakeLists.txt b/gait_generator/CMakeLists.txt
index f7e3d0d..c4c797b 100644
--- a/gait_generator/CMakeLists.txt
+++ b/gait_generator/CMakeLists.txt
@@ -16,12 +16,15 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(limb_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(gait_generator_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 
 # Declare library
 add_library(${PROJECT_NAME}
-        src/${PROJECT_NAME}/${PROJECT_NAME}.cpp
+        src/${PROJECT_NAME}/${PROJECT_NAME}_parameters.cpp
+        src/${PROJECT_NAME}/${PROJECT_NAME}_core.cpp
+        src/${PROJECT_NAME}/base_tf_broadcaster.cpp
         )
 
 target_include_directories(${PROJECT_NAME}
@@ -34,6 +37,7 @@ ament_target_dependencies(${PROJECT_NAME}
         std_msgs
         geometry_msgs
         limb_msgs
+        gait_generator_msgs
         tf2
         tf2_geometry_msgs
         )
@@ -43,6 +47,11 @@ target_link_libraries(${PROJECT_NAME}_node
         ${PROJECT_NAME}
         )
 
+
+add_executable(base_tf_broadcaster_node src/base_tf_broadcaster_node.cpp)
+target_link_libraries(base_tf_broadcaster_node
+        ${PROJECT_NAME}
+        )
 #############
 ## Install ##
 #############
@@ -54,6 +63,7 @@ install(
 
 install(TARGETS 
         ${PROJECT_NAME}_node
+        base_tf_broadcaster_node
         DESTINATION lib/${PROJECT_NAME}
         )
 
diff --git a/gait_generator/README.md b/gait_generator/README.md
new file mode 100644
index 0000000..373fc0a
--- /dev/null
+++ b/gait_generator/README.md
@@ -0,0 +1,11 @@
+# Gait generator
+
+
+Generates the gait after listining on /cmd_vel
+
+
+To change the gait patten, the following service is used
+
+```
+ros2 service call gait_generator/set_gait_pattern gait_generator_msgs/srv/SetGaitPattern "{"pattern": 1}"
+```
\ No newline at end of file
diff --git a/gait_generator/include/gait_generator/base_tf_broadcaster.hpp b/gait_generator/include/gait_generator/base_tf_broadcaster.hpp
new file mode 100644
index 0000000..7015fcd
--- /dev/null
+++ b/gait_generator/include/gait_generator/base_tf_broadcaster.hpp
@@ -0,0 +1,30 @@
+#ifndef BASE_TF_BROADCASTER_HPP_
+#define BASE_TF_BROADCASTER_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+#include <rclcpp/executors.hpp>
+
+// include messages
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "tf2_ros/transform_broadcaster.h"
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+namespace penta_pod::kin {
+
+using PoseStamped = geometry_msgs::msg::PoseStamped;
+const auto base_footprint = "base_footprint";
+
+class BaseTfBroadcaster {
+private:
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Subscription<PoseStamped>::SharedPtr base_pose_subscriber_;
+  auto base_pose_sub_callback(const PoseStamped::SharedPtr msg) -> void;
+
+public:
+  explicit BaseTfBroadcaster();
+  void spin() { rclcpp::spin(node_); };
+};
+
+} // namespace penta_pod::kin
+
+#endif // BASE_TF_BROADCASTER_HPP_
\ No newline at end of file
diff --git a/gait_generator/include/gait_generator/gait_generator.hpp b/gait_generator/include/gait_generator/gait_generator.hpp
index 70c74e8..a3638c0 100644
--- a/gait_generator/include/gait_generator/gait_generator.hpp
+++ b/gait_generator/include/gait_generator/gait_generator.hpp
@@ -5,6 +5,7 @@
 #include <rclcpp/executors.hpp>
 
 // include messages
+#include "gait_generator_msgs/srv/set_gait_pattern.hpp"
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/transform.hpp"
@@ -14,13 +15,14 @@
 #include <cmath>
 #include <tf2/LinearMath/Matrix3x3.h>
 #include <tf2/LinearMath/Quaternion.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include <iomanip>
 #include <sstream>
 
 namespace penta_pod::kin::gait_generator {
 
+using SetGaitPattern = gait_generator_msgs::srv::SetGaitPattern;
+
 inline auto double_to_string_formatted(double number, int digits_after_point)
     -> std::string {
   std::ostringstream oss;
@@ -35,8 +37,34 @@ class GaitGenerator {
   std::vector<double> q_state;
 
   double current_phase_, max_gait_linear_speed_, max_gait_turning_speed_;
+  bool is_walking_{false};
   int feet_num_;
 
+  struct GatiPatterns {
+    int active_gait_index_ = 0;
+    int gaits_num_ = 0;
+    std::vector<std::vector<long int>> patterns_;
+    std::vector<long int> get_active_pattern() {
+      return patterns_[active_gait_index_];
+    }
+    bool set_active_pattern(int i) {
+      if (i < 0)
+        return false;
+      if (i >= gaits_num_)
+        return false;
+      active_gait_index_ = i;
+      return true;
+    }
+    std::string active_pattern_to_string() {
+      auto active_pattern = get_active_pattern();
+      std::string log_message = "Feet pattern: ";
+      for (auto foot_index : active_pattern) {
+        log_message = log_message + std::to_string(foot_index) + " |";
+      }
+      return log_message;
+    }
+  } gait_patterns_;
+
   std::vector<geometry_msgs::msg::Transform> legs_body_transforms_;
   geometry_msgs::msg::Transform body_basefootprint_;
 
@@ -58,6 +86,10 @@ class GaitGenerator {
   rclcpp::TimerBase::SharedPtr timer_;
   geometry_msgs::msg::Twist cmd_vel_{};
 
+  rclcpp::Service<SetGaitPattern>::SharedPtr set_gait_pattern_server_;
+  rclcpp::CallbackGroup::SharedPtr service_callback_group_;
+  void create_set_gait_pattern_service();
+
   void declare_parameters();
   void load_parameters();
   void timer_callback(double delta_t_milli);
@@ -67,6 +99,7 @@ class GaitGenerator {
 
   void update_phase(double delta_t_milli);
   void update_feet_positions(double delta_t_milli);
+  void publish_base_to_basefootprint_transform();
 
   // move feet up (z up) calculation
   double foot_pos_z_generator(double b, double q, double phase_shift, int n) {
diff --git a/gait_generator/launch/gait_generator.launch.py b/gait_generator/launch/gait_generator.launch.py
index db2252a..dfece2e 100644
--- a/gait_generator/launch/gait_generator.launch.py
+++ b/gait_generator/launch/gait_generator.launch.py
@@ -3,6 +3,7 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
+
 def generate_launch_description():
     config = os.path.join(
         get_package_share_directory("penta_description"),
@@ -10,10 +11,20 @@ def generate_launch_description():
         "general_config.yaml",
     )
     ld = LaunchDescription()
-    ld.add_action(Node(
-        package="gait_generator",
-        executable="gait_generator_node",
-        output="screen",
-        parameters=[config],
-    ))
+    ld.add_action(
+        Node(
+            package="gait_generator",
+            executable="gait_generator_node",
+            output="screen",
+            parameters=[config],
+        )
+    )
+    ld.add_action(
+        Node(
+            package="gait_generator",
+            executable="base_tf_broadcaster_node",
+            output="screen",
+            parameters=[config],
+        )
+    )
     return ld
diff --git a/gait_generator/package.xml b/gait_generator/package.xml
index 733f927..a5609d4 100644
--- a/gait_generator/package.xml
+++ b/gait_generator/package.xml
@@ -13,6 +13,7 @@
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>limb_msgs</depend>
+  <depend>gait_generator_msgs</depend>
   <depend>launch_testing</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
diff --git a/gait_generator/scripts/generate_leg_body_transforms.py b/gait_generator/scripts/generate_leg_body_transforms.py
index 2f73b16..35f1bfc 100644
--- a/gait_generator/scripts/generate_leg_body_transforms.py
+++ b/gait_generator/scripts/generate_leg_body_transforms.py
@@ -7,7 +7,7 @@
      theta = 2*i*pi/5.
      x = r*cos(theta)
      y = r*sin(theta)
-     z = 0.05
+     z = 0.0
      qw = cos(theta/2.)
      qx = 0.
      qy = 0.
@@ -17,12 +17,12 @@
 
 print('***************')
 
-# generate initial feet position in body frame
-r = 0.25
+# generate initial feet position in base footprint
+r = 0.085+0.0455+0.096
 for i in range(5):
      theta = 2*i*pi/5.
      x = r*cos(theta)
      y = r*sin(theta)
-     z = -0.05
+     z = 0.0
      pos = [x, y, z]
      print(pos)
diff --git a/gait_generator/src/base_tf_broadcaster_node.cpp b/gait_generator/src/base_tf_broadcaster_node.cpp
new file mode 100644
index 0000000..00a535f
--- /dev/null
+++ b/gait_generator/src/base_tf_broadcaster_node.cpp
@@ -0,0 +1,8 @@
+#include "gait_generator/base_tf_broadcaster.hpp"
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+  penta_pod::kin::BaseTfBroadcaster().spin();
+  rclcpp::shutdown();
+  return 0;
+}
\ No newline at end of file
diff --git a/gait_generator/src/gait_generator/base_tf_broadcaster.cpp b/gait_generator/src/gait_generator/base_tf_broadcaster.cpp
new file mode 100644
index 0000000..819ee20
--- /dev/null
+++ b/gait_generator/src/gait_generator/base_tf_broadcaster.cpp
@@ -0,0 +1,37 @@
+#include "gait_generator/base_tf_broadcaster.hpp"
+
+namespace penta_pod::kin {
+
+BaseTfBroadcaster::BaseTfBroadcaster()
+    : node_{rclcpp::Node::make_shared("base_tf_broadcaster")} {
+  RCLCPP_INFO(node_->get_logger(),
+              "Starting base_tf_broadcaster, subscriping to null_space_pose "
+              "topic and broadcasting tf");
+  base_pose_subscriber_ = node_->create_subscription<PoseStamped>(
+      "null_space_pose", 10, [this](const PoseStamped::SharedPtr msg) {
+        this->base_pose_sub_callback(msg);
+      });
+}
+
+auto BaseTfBroadcaster::base_pose_sub_callback(const PoseStamped::SharedPtr msg)
+    -> void {
+  geometry_msgs::msg::TransformStamped transformStamped;
+  // header
+  transformStamped.header.stamp = rclcpp::Clock(RCL_ROS_TIME).now();
+  transformStamped.header.frame_id = "base_footprint";
+  transformStamped.child_frame_id = "base_link";
+  // translation
+  transformStamped.transform.translation.x = msg->pose.position.x;
+  transformStamped.transform.translation.y = msg->pose.position.y;
+  transformStamped.transform.translation.z = msg->pose.position.z;
+  // rotation
+  transformStamped.transform.rotation.x = msg->pose.orientation.x;
+  transformStamped.transform.rotation.y = msg->pose.orientation.y;
+  transformStamped.transform.rotation.z = msg->pose.orientation.z;
+  transformStamped.transform.rotation.w = msg->pose.orientation.w;
+  // broadcast
+  static auto br = std::make_shared<tf2_ros::TransformBroadcaster>(node_);
+  br->sendTransform(transformStamped);
+}
+
+} // namespace penta_pod::kin
\ No newline at end of file
diff --git a/gait_generator/src/gait_generator/gait_generator.cpp b/gait_generator/src/gait_generator/gait_generator.cpp
deleted file mode 100644
index b97ae0e..0000000
--- a/gait_generator/src/gait_generator/gait_generator.cpp
+++ /dev/null
@@ -1,304 +0,0 @@
-#include <rclcpp/executors.hpp>
-
-// include librarries
-#include "gait_generator/gait_generator.hpp"
-#include "rclcpp/rclcpp.hpp"
-
-// include messages
-#include "limb_msgs/msg/pxyz.hpp"
-#include <string>
-#include <vector>
-
-#include <tf2/LinearMath/Matrix3x3.h>
-#include <tf2/LinearMath/Quaternion.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-
-#include <stdexcept>
-
-#define pi 3.141592
-
-namespace penta_pod::kin::gait_generator {
-
-GaitGenerator::GaitGenerator()
-    : node_{rclcpp::Node::make_shared("gait_generator_node")} {
-  RCLCPP_INFO(node_->get_logger(), "Starting gait_generator_node");
-  this->declare_parameters();
-  this->load_parameters();
-  const double delta_t_milli = 10.;
-
-  current_phase_ = 0.;
-  for (int i = 0; i < feet_num_; i++) {
-    auto topic_name = "limb" + std::to_string(i) + "/xyz_msg";
-    xyz_publishers_.push_back(
-        node_->create_publisher<limb_msgs::msg::Pxyz>(topic_name, 10));
-    final_displacement_.push_back(geometry_msgs::msg::Point());
-  }
-  phase_shift_vec_ = init_phase_shift(feet_num_);
-
-  cmd_vel_subscription_ = node_->create_subscription<geometry_msgs::msg::Twist>(
-      "cmd_vel", 10, [this](const geometry_msgs::msg::Twist::SharedPtr msg) {
-        cmd_vel_sub_callback(msg);
-      });
-
-  cmd_null_pos_subscription_ =
-      node_->create_subscription<geometry_msgs::msg::PoseStamped>(
-          "null_space_pose", 10,
-          [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
-            cmd_null_pos_sub_callback(msg);
-          });
-
-  timer_ = node_->create_wall_timer(
-      std::chrono::milliseconds(static_cast<int>(delta_t_milli)),
-      [this, delta_t_milli]() { timer_callback(delta_t_milli); });
-}
-
-void GaitGenerator::cmd_vel_sub_callback(
-    const geometry_msgs::msg::Twist::SharedPtr msg) {
-
-  auto mag =
-      std::sqrt(msg->linear.x * msg->linear.x + msg->linear.y * msg->linear.y);
-  if (mag > max_gait_linear_speed_) {
-    msg->linear.x = msg->linear.x * max_gait_linear_speed_ / mag;
-    msg->linear.y = msg->linear.y * max_gait_linear_speed_ / mag;
-  }
-  mag = std::abs(msg->angular.z);
-  if (mag > max_gait_turning_speed_) {
-    msg->angular.z = msg->angular.z * max_gait_turning_speed_ / mag;
-  }
-  cmd_vel_ = *msg;
-  /*
-  RCLCPP_INFO(node_->get_logger(), "Received cmd_vel: linear.x=%.2f,
-  linear.y=%.2f, angular=%.2f", cmd_vel_.linear.x, cmd_vel_.linear.y,
-  cmd_vel_.angular.z);
-  */
-}
-
-void GaitGenerator::cmd_null_pos_sub_callback(
-    const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
-  // add to the initial displacement
-  body_basefootprint_.translation.x = msg->pose.position.x;
-  body_basefootprint_.translation.y = msg->pose.position.y;
-  body_basefootprint_.translation.z = msg->pose.position.z;
-  // consider the rotation absolute (initial rotation must be the identity)
-  // body_basefootprint_.rotation. = msg->rotation;
-  RCLCPP_INFO(node_->get_logger(),
-              "Received nullspace translation from equilibrium (default): "
-              "displacement.x=%.2f, displacement.y=%.2f, displacement.z=%.2f",
-              body_basefootprint_.translation.x,
-              body_basefootprint_.translation.y,
-              body_basefootprint_.translation.z);
-}
-
-void GaitGenerator::update_phase(double delta_t_milli) {
-
-  auto delta_t_sec = delta_t_milli / 1000.;
-  double w = 2.5;
-
-  // check if cmd_vel is zero and feet near the equilibrium
-  double vel_mag = std::sqrt(cmd_vel_.linear.x * cmd_vel_.linear.x +
-                             cmd_vel_.linear.y * cmd_vel_.linear.y);
-  auto collective_xy_distance_from_equilibrium = 0.0;
-  for (int i = 0; i < feet_num_; i++) {
-    collective_xy_distance_from_equilibrium +=
-        std::abs(feet_pos_in_footprint_[i].x -
-                 init_feet_pos_in_footprint_[i].x) +
-        std::abs(feet_pos_in_footprint_[i].y -
-                 init_feet_pos_in_footprint_[i].y);
-  }
-
-  if ((collective_xy_distance_from_equilibrium < 0.005) && (vel_mag < 0.001)) {
-    auto check_z_near_zero =
-        current_phase_ - std::floor(current_phase_ / (2 * pi)) * 2 * pi;
-    if (check_z_near_zero < w * delta_t_sec + 0.001) {
-      w = 0.0;
-      current_phase_ = std::floor(current_phase_ / (2 * pi)) * 2 * pi;
-    }
-  }
-  current_phase_ = current_phase_ + w * delta_t_sec;
-}
-
-void GaitGenerator::update_feet_positions(double delta_t_milli) {
-
-  auto delta_t_sec = delta_t_milli / 1000.;
-
-  double dx = cmd_vel_.linear.x * delta_t_sec;
-  double dy = cmd_vel_.linear.y * delta_t_sec;
-  double d_theta = cmd_vel_.angular.z * delta_t_sec;
-  double b = 0.05;
-
-  for (int i = 0; i < feet_num_; i++) {
-    auto temp =
-        foot_pos_z_generator(b, current_phase_, phase_shift_vec_[i], feet_num_);
-    if (temp == 0.) {
-      double x = feet_pos_in_footprint_[i].x;
-      double y = feet_pos_in_footprint_[i].y;
-      feet_pos_in_footprint_[i].x = x + dx - d_theta * y;
-      feet_pos_in_footprint_[i].y = y + dy + d_theta * x;
-      feet_pos_in_footprint_[i].z = 0.;
-      final_displacement_[i].x =
-          feet_pos_in_footprint_[i].x - init_feet_pos_in_footprint_[i].x;
-      final_displacement_[i].y =
-          feet_pos_in_footprint_[i].y - init_feet_pos_in_footprint_[i].y;
-    } else {
-      feet_pos_in_footprint_[i].x =
-          init_feet_pos_in_footprint_[i].x +
-          foot_pos_xy_generator(current_phase_, phase_shift_vec_[i],
-                                final_displacement_[i].x, feet_num_);
-      feet_pos_in_footprint_[i].y =
-          init_feet_pos_in_footprint_[i].y +
-          foot_pos_xy_generator(current_phase_, phase_shift_vec_[i],
-                                final_displacement_[i].y, feet_num_);
-      feet_pos_in_footprint_[i].z = temp;
-    }
-    if (i < static_cast<int>(legs_body_transforms_.size())) {
-      auto point =
-          applyInverseTransform(feet_pos_in_footprint_[i], body_basefootprint_);
-      point = applyInverseTransform(point, legs_body_transforms_[i]);
-      limb_msgs::msg::Pxyz xyz_msg;
-      xyz_msg.x = point.x;
-      xyz_msg.y = point.y;
-      xyz_msg.z = point.z;
-
-      xyz_publishers_[i]->publish(xyz_msg);
-    } else {
-      RCLCPP_ERROR_STREAM(node_->get_logger(),
-                          "No transform available for limb " << i);
-    }
-  }
-}
-
-void GaitGenerator::timer_callback(double delta_t_milli) {
-  update_phase(delta_t_milli);
-  update_feet_positions(delta_t_milli);
-}
-
-void GaitGenerator::declare_parameters() {
-  node_->declare_parameter<int>("limbs_num");
-  node_->declare_parameter<std::vector<double>>("legs_body_transforms",
-                                                std::vector<double>{});
-  node_->declare_parameter<std::vector<double>>(
-      "init_feet_pos_in_basefootprint", std::vector<double>{});
-  node_->declare_parameter<std::vector<double>>(
-      "init_body_basefootprint_transform", std::vector<double>{});
-  node_->declare_parameter<double>("gait_parameters.max_gait_linear_speed");
-  node_->declare_parameter<double>("gait_parameters.max_gait_turning_speed");
-}
-
-void GaitGenerator::load_parameters() {
-  // Helper function to load a parameter and throw error if not found
-  auto load_param = [this](const std::string &param_name, auto &param_value,
-                           const std::string &error_message) {
-    if (!node_->get_parameter(param_name, param_value)) {
-      RCLCPP_ERROR(node_->get_logger(), error_message.c_str());
-      throw std::runtime_error(error_message);
-    }
-  };
-
-  // Helper function to validate vector size
-  auto validate_vector_size = [this](const std::vector<double> &vec,
-                                     int expected_size,
-                                     const std::string &error_message) {
-    if (static_cast<int>(vec.size()) != expected_size) {
-      RCLCPP_ERROR(node_->get_logger(), error_message.c_str());
-      throw std::runtime_error(error_message);
-    }
-  };
-
-  // Helper function to create a transform from values
-  auto array_to_transform =
-      [this](const std::vector<double> &transforms_vec,
-             int frame_index) -> geometry_msgs::msg::Transform {
-    auto minimal_required_size = 7 * frame_index + 7;
-    if (transforms_vec.size() < static_cast<size_t>(minimal_required_size)) {
-      auto error_message =
-          "Error, can not create trasform from array at index " +
-          std::to_string(frame_index) +
-          " since minimal required size is less than " +
-          std::to_string(minimal_required_size);
-      RCLCPP_ERROR(node_->get_logger(), error_message.c_str());
-      throw std::runtime_error(error_message);
-    }
-    geometry_msgs::msg::Transform transform;
-    auto start_index = frame_index * 7;
-    transform.translation.x = transforms_vec[start_index + 0];
-    transform.translation.y = transforms_vec[start_index + 1];
-    transform.translation.z = transforms_vec[start_index + 2];
-    transform.rotation.x = transforms_vec[start_index + 3];
-    transform.rotation.y = transforms_vec[start_index + 4];
-    transform.rotation.z = transforms_vec[start_index + 5];
-    transform.rotation.w = transforms_vec[start_index + 6];
-    return transform;
-  };
-
-  // Load limbs number
-  load_param("limbs_num", feet_num_, "No limbs_num parameter found.");
-  RCLCPP_INFO(node_->get_logger(), "Loaded limbs_num value is: %d", feet_num_);
-
-  // Load and validate leg-body transforms
-  std::vector<double> transform_params;
-  load_param("legs_body_transforms", transform_params,
-             "No transform parameters found.");
-  validate_vector_size(transform_params, feet_num_ * 7,
-                       "Invalid transform parameter size, expected " +
-                           std::to_string(feet_num_) + "x7 elements.");
-
-  for (int frame_index = 0; frame_index < feet_num_; ++frame_index) {
-    auto transform = array_to_transform(transform_params, frame_index);
-    legs_body_transforms_.emplace_back(transform);
-
-    std::string message = "Shoulder [" + std::to_string(frame_index) +
-                          "] transform in body frame is: [ ";
-    for (int i = 0; i < 7; ++i) {
-      message +=
-          double_to_string_formatted(transform_params[i + 7 * frame_index], 4) +
-          " ";
-    }
-    message += "]";
-    RCLCPP_INFO(node_->get_logger(), message.c_str());
-  }
-
-  // Load and validate initial feet positions
-  std::vector<double> init_feet_pos_params;
-  load_param("init_feet_pos_in_basefootprint", init_feet_pos_params,
-             "No feet position parameters found.");
-  validate_vector_size(init_feet_pos_params, feet_num_ * 3,
-                       "Invalid feet position parameter size, expected " +
-                           std::to_string(feet_num_) + "x3 elements.");
-
-  for (int count = 0; count < feet_num_; ++count) {
-    geometry_msgs::msg::Point xyz;
-    xyz.x = init_feet_pos_params[0 + count * 3];
-    xyz.y = init_feet_pos_params[1 + count * 3];
-    xyz.z = init_feet_pos_params[2 + count * 3];
-    feet_pos_in_footprint_.emplace_back(xyz);
-    init_feet_pos_in_footprint_.emplace_back(xyz);
-  }
-
-  // Load and validate initial body-to-basefootprint transform
-  std::vector<double> body_basefootprint_params;
-  load_param("init_body_basefootprint_transform", body_basefootprint_params,
-             "No init transform body to basefootprint found.");
-  validate_vector_size(body_basefootprint_params, 7,
-                       "Size of init_body_basefootprint_transform must be 7 (3 "
-                       "for position followed by 4 quaternion).");
-
-  auto transform = array_to_transform(body_basefootprint_params, 0);
-  body_basefootprint_ = transform;
-
-  // Load gait parameters
-  load_param("gait_parameters.max_gait_linear_speed", max_gait_linear_speed_,
-             "Parameter gait_parameters.max_gait_linear_speed was not found.");
-  RCLCPP_INFO(
-      node_->get_logger(),
-      "Loaded gait_parameters.max_gait_linear_speed value is: %f [m/sec]",
-      max_gait_linear_speed_);
-
-  load_param("gait_parameters.max_gait_turning_speed", max_gait_turning_speed_,
-             "Parameter gait_parameters.max_gait_turning_speed was not found.");
-  RCLCPP_INFO(
-      node_->get_logger(),
-      "Loaded gait_parameters.max_gait_turning_speed value is: %f [rad/sec]",
-      max_gait_turning_speed_);
-}
-
-} // namespace penta_pod::kin::gait_generator
diff --git a/gait_generator/src/gait_generator/gait_generator_core.cpp b/gait_generator/src/gait_generator/gait_generator_core.cpp
new file mode 100644
index 0000000..1e0fd8c
--- /dev/null
+++ b/gait_generator/src/gait_generator/gait_generator_core.cpp
@@ -0,0 +1,226 @@
+#include <rclcpp/executors.hpp>
+
+// include librarries
+#include "gait_generator/gait_generator.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+// include messages
+#include "limb_msgs/msg/pxyz.hpp"
+#include <string>
+#include <vector>
+
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <stdexcept>
+
+#define pi 3.141592
+
+namespace penta_pod::kin::gait_generator {
+
+GaitGenerator::GaitGenerator()
+    : node_{rclcpp::Node::make_shared("gait_generator_node")} {
+  RCLCPP_INFO(node_->get_logger(), "Starting gait_generator_node");
+  this->declare_parameters();
+  this->load_parameters();
+  const double delta_t_milli = 10.;
+
+  current_phase_ = 0.;
+  for (int i = 0; i < feet_num_; i++) {
+    auto topic_name = "limb" + std::to_string(i) + "/xyz_msg";
+    xyz_publishers_.push_back(
+        node_->create_publisher<limb_msgs::msg::Pxyz>(topic_name, 10));
+    final_displacement_.push_back(geometry_msgs::msg::Point());
+  }
+  phase_shift_vec_ = init_phase_shift(feet_num_);
+
+  cmd_vel_subscription_ = node_->create_subscription<geometry_msgs::msg::Twist>(
+      "cmd_vel", 10, [this](const geometry_msgs::msg::Twist::SharedPtr msg) {
+        cmd_vel_sub_callback(msg);
+      });
+
+  cmd_null_pos_subscription_ =
+      node_->create_subscription<geometry_msgs::msg::PoseStamped>(
+          "null_space_pose", 10,
+          [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
+            cmd_null_pos_sub_callback(msg);
+          });
+
+  timer_ = node_->create_wall_timer(
+      std::chrono::milliseconds(static_cast<int>(delta_t_milli)),
+      [this, delta_t_milli]() { timer_callback(delta_t_milli); });
+
+  create_set_gait_pattern_service();
+}
+
+void GaitGenerator::create_set_gait_pattern_service() {
+  auto lambda =
+      [this](const SetGaitPattern::Request::SharedPtr &request,
+             const SetGaitPattern::Response::SharedPtr &response) -> bool {
+    int pattern = request->pattern;
+    if (is_walking_) {
+      std::string error_message =
+          "Can not change gait pattern while the robot is moving";
+      RCLCPP_WARN(node_->get_logger(), error_message.c_str());
+      response->message = error_message;
+      response->success = false;
+      return true;
+    }
+    if (!gait_patterns_.set_active_pattern(pattern)) {
+      std::string error_message = "Could not set gait pattern to " +
+                                  std::to_string(pattern) +
+                                  ", make sure u r using a valid value";
+      RCLCPP_WARN(node_->get_logger(), error_message.c_str());
+      response->message = error_message;
+      response->success = false;
+      return true;
+    }
+    std::string info_message =
+        "Changed successfully to gait pattern: " + std::to_string(pattern);
+    RCLCPP_INFO(node_->get_logger(), info_message.c_str());
+    response->message = info_message;
+    response->success = true;
+
+    auto log_message = gait_patterns_.active_pattern_to_string();
+    RCLCPP_INFO(node_->get_logger(), log_message.c_str());
+    return true;
+  };
+  service_callback_group_ = node_->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+  set_gait_pattern_server_ = node_->create_service<SetGaitPattern>(
+      "gait_generator/set_gait_pattern", lambda,
+      rmw_qos_profile_services_default, service_callback_group_);
+}
+
+void GaitGenerator::cmd_vel_sub_callback(
+    const geometry_msgs::msg::Twist::SharedPtr msg) {
+
+  auto mag =
+      std::sqrt(msg->linear.x * msg->linear.x + msg->linear.y * msg->linear.y);
+  if (mag > max_gait_linear_speed_) {
+    msg->linear.x = msg->linear.x * max_gait_linear_speed_ / mag;
+    msg->linear.y = msg->linear.y * max_gait_linear_speed_ / mag;
+  }
+  mag = std::abs(msg->angular.z);
+  if (mag > max_gait_turning_speed_) {
+    msg->angular.z = msg->angular.z * max_gait_turning_speed_ / mag;
+  }
+  cmd_vel_ = *msg;
+  /*
+  RCLCPP_INFO(node_->get_logger(), "Received cmd_vel: linear.x=%.2f,
+  linear.y=%.2f, angular=%.2f", cmd_vel_.linear.x, cmd_vel_.linear.y,
+  cmd_vel_.angular.z);
+  */
+}
+
+void GaitGenerator::cmd_null_pos_sub_callback(
+    const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
+  // add to the initial displacement
+  body_basefootprint_.translation.x = msg->pose.position.x;
+  body_basefootprint_.translation.y = msg->pose.position.y;
+  body_basefootprint_.translation.z = msg->pose.position.z;
+
+  body_basefootprint_.rotation.x = msg->pose.orientation.x;
+  body_basefootprint_.rotation.y = msg->pose.orientation.y;
+  body_basefootprint_.rotation.z = msg->pose.orientation.z;
+  body_basefootprint_.rotation.w = msg->pose.orientation.w;
+  
+  // consider the rotation absolute (initial rotation must be the identity)
+  // body_basefootprint_.rotation. = msg->rotation;
+  // RCLCPP_INFO(node_->get_logger(),
+  //             "Received nullspace translation from equilibrium (default): "
+  //             "displacement.x=%.2f, displacement.y=%.2f,
+  //             displacement.z=%.2f", body_basefootprint_.translation.x,
+  //             body_basefootprint_.translation.y,
+  //             body_basefootprint_.translation.z);
+}
+
+void GaitGenerator::update_phase(double delta_t_milli) {
+
+  auto delta_t_sec = delta_t_milli / 1000.;
+  double w = 2.5;
+
+  // check if cmd_vel is zero and feet near the equilibrium
+  double vel_mag = std::sqrt(cmd_vel_.linear.x * cmd_vel_.linear.x +
+                             cmd_vel_.linear.y * cmd_vel_.linear.y);
+  auto collective_xy_distance_from_equilibrium = 0.0;
+  for (int i = 0; i < feet_num_; i++) {
+    collective_xy_distance_from_equilibrium +=
+        std::abs(feet_pos_in_footprint_[i].x -
+                 init_feet_pos_in_footprint_[i].x) +
+        std::abs(feet_pos_in_footprint_[i].y -
+                 init_feet_pos_in_footprint_[i].y);
+  }
+
+  if ((collective_xy_distance_from_equilibrium < 0.005) && (vel_mag < 0.001)) {
+    auto check_z_near_zero =
+        current_phase_ - std::floor(current_phase_ / (2 * pi)) * 2 * pi;
+    if (check_z_near_zero < w * delta_t_sec + 0.001) {
+      w = 0.0;
+      current_phase_ = std::floor(current_phase_ / (2 * pi)) * 2 * pi;
+    }
+  }
+  is_walking_ = (w == 0.0) ? false : true;
+  current_phase_ = current_phase_ + w * delta_t_sec;
+}
+
+void GaitGenerator::update_feet_positions(double delta_t_milli) {
+
+  auto delta_t_sec = delta_t_milli / 1000.;
+
+  double dx = cmd_vel_.linear.x * delta_t_sec;
+  double dy = cmd_vel_.linear.y * delta_t_sec;
+  double d_theta = cmd_vel_.angular.z * delta_t_sec;
+  double b = 0.05;
+  auto gait_pattern = gait_patterns_.get_active_pattern();
+  for (int i = 0; i < feet_num_; i++) {
+    auto foot_index = gait_pattern[i];
+    auto temp = foot_pos_z_generator(b, current_phase_,
+                                     phase_shift_vec_[i], feet_num_);
+    if (temp == 0.) {
+      double x = feet_pos_in_footprint_[foot_index].x;
+      double y = feet_pos_in_footprint_[foot_index].y;
+      feet_pos_in_footprint_[foot_index].x = x + dx - d_theta * y;
+      feet_pos_in_footprint_[foot_index].y = y + dy + d_theta * x;
+      feet_pos_in_footprint_[foot_index].z = 0.;
+      final_displacement_[foot_index].x =
+          feet_pos_in_footprint_[foot_index].x -
+          init_feet_pos_in_footprint_[foot_index].x;
+      final_displacement_[foot_index].y =
+          feet_pos_in_footprint_[foot_index].y -
+          init_feet_pos_in_footprint_[foot_index].y;
+    } else {
+      feet_pos_in_footprint_[foot_index].x =
+          init_feet_pos_in_footprint_[foot_index].x +
+          foot_pos_xy_generator(current_phase_, phase_shift_vec_[i],
+                                final_displacement_[foot_index].x, feet_num_);
+      feet_pos_in_footprint_[foot_index].y =
+          init_feet_pos_in_footprint_[foot_index].y +
+          foot_pos_xy_generator(current_phase_, phase_shift_vec_[i],
+                                final_displacement_[foot_index].y, feet_num_);
+      feet_pos_in_footprint_[foot_index].z = temp;
+    }
+    if (foot_index < static_cast<int>(legs_body_transforms_.size())) {
+      auto point = applyInverseTransform(feet_pos_in_footprint_[foot_index],
+                                         body_basefootprint_);
+      point = applyInverseTransform(point, legs_body_transforms_[foot_index]);
+      limb_msgs::msg::Pxyz xyz_msg;
+      xyz_msg.x = point.x;
+      xyz_msg.y = point.y;
+      xyz_msg.z = point.z;
+
+      xyz_publishers_[foot_index]->publish(xyz_msg);
+    } else {
+      RCLCPP_ERROR_STREAM(node_->get_logger(),
+                          "No transform available for limb " << foot_index);
+    }
+  }
+}
+
+void GaitGenerator::timer_callback(double delta_t_milli) {
+  update_phase(delta_t_milli);
+  update_feet_positions(delta_t_milli);
+}
+
+} // namespace penta_pod::kin::gait_generator
diff --git a/gait_generator/src/gait_generator/gait_generator_parameters.cpp b/gait_generator/src/gait_generator/gait_generator_parameters.cpp
new file mode 100644
index 0000000..fbbf191
--- /dev/null
+++ b/gait_generator/src/gait_generator/gait_generator_parameters.cpp
@@ -0,0 +1,188 @@
+// include librarries
+#include "gait_generator/gait_generator.hpp"
+#include <stdexcept>
+
+namespace penta_pod::kin::gait_generator {
+
+void GaitGenerator::declare_parameters() {
+  node_->declare_parameter<int>("limbs_num");
+  node_->declare_parameter<std::vector<double>>("legs_body_transforms",
+                                                std::vector<double>{});
+  node_->declare_parameter<std::vector<double>>(
+      "init_feet_pos_in_basefootprint", std::vector<double>{});
+  node_->declare_parameter<std::vector<double>>(
+      "init_body_basefootprint_transform", std::vector<double>{});
+  node_->declare_parameter<double>("gait_parameters.max_gait_linear_speed");
+  node_->declare_parameter<double>("gait_parameters.max_gait_turning_speed");
+  node_->declare_parameter<int>("gait_parameters.gait_patterns.num");
+  node_->declare_parameter<std::vector<long int>>(
+      "gait_parameters.gait_patterns.feet_order", std::vector<long int>{});
+}
+
+void GaitGenerator::load_parameters() {
+  // Helper function to load a parameter and throw error if not found
+  auto load_param = [this](const std::string &param_name, auto &param_value,
+                           const std::string &error_message) {
+    if (!node_->get_parameter(param_name, param_value)) {
+      RCLCPP_ERROR(node_->get_logger(), error_message.c_str());
+      throw std::runtime_error(error_message);
+    }
+  };
+
+  // Helper function to validate vector size
+  auto validate_vector_size =
+      [this]<typename T>(const std::vector<T> &vec, int expected_size,
+                         const std::string &error_message) {
+        if (static_cast<int>(vec.size()) != expected_size) {
+          RCLCPP_ERROR(node_->get_logger(), error_message.c_str());
+          throw std::runtime_error(error_message);
+        }
+      };
+
+  // Helper function to create a transform from values
+  auto array_to_transform =
+      [this](const std::vector<double> &transforms_vec,
+             int frame_index) -> geometry_msgs::msg::Transform {
+    auto minimal_required_size = 7 * frame_index + 7;
+    if (transforms_vec.size() < static_cast<size_t>(minimal_required_size)) {
+      auto error_message =
+          "Error, can not create trasform from array at index " +
+          std::to_string(frame_index) +
+          " since minimal required size is less than " +
+          std::to_string(minimal_required_size);
+      RCLCPP_ERROR(node_->get_logger(), error_message.c_str());
+      throw std::runtime_error(error_message);
+    }
+    geometry_msgs::msg::Transform transform;
+    auto start_index = frame_index * 7;
+    transform.translation.x = transforms_vec[start_index + 0];
+    transform.translation.y = transforms_vec[start_index + 1];
+    transform.translation.z = transforms_vec[start_index + 2];
+    transform.rotation.x = transforms_vec[start_index + 3];
+    transform.rotation.y = transforms_vec[start_index + 4];
+    transform.rotation.z = transforms_vec[start_index + 5];
+    transform.rotation.w = transforms_vec[start_index + 6];
+    return transform;
+  };
+
+  // Load limbs number
+  load_param("limbs_num", feet_num_, "No limbs_num parameter found.");
+  RCLCPP_INFO(node_->get_logger(), "Loaded limbs_num value is: %d", feet_num_);
+
+  // Load and validate leg-body transforms
+  std::vector<double> transform_params;
+  load_param("legs_body_transforms", transform_params,
+             "No transform parameters found.");
+  validate_vector_size(transform_params, feet_num_ * 7,
+                       "Invalid transform parameter size, expected " +
+                           std::to_string(feet_num_) + "x7 elements.");
+
+  for (int frame_index = 0; frame_index < feet_num_; ++frame_index) {
+    auto transform = array_to_transform(transform_params, frame_index);
+    legs_body_transforms_.emplace_back(transform);
+
+    std::string message = "Shoulder [" + std::to_string(frame_index) +
+                          "] transform in body frame is: [ ";
+    for (int i = 0; i < 7; ++i) {
+      message +=
+          double_to_string_formatted(transform_params[i + 7 * frame_index], 4) +
+          " ";
+    }
+    message += "]";
+    RCLCPP_INFO(node_->get_logger(), message.c_str());
+  }
+
+  // Load and validate initial feet positions
+  std::vector<double> init_feet_pos_params;
+  load_param("init_feet_pos_in_basefootprint", init_feet_pos_params,
+             "No feet position parameters found.");
+  validate_vector_size(init_feet_pos_params, feet_num_ * 3,
+                       "Invalid feet position parameter size, expected " +
+                           std::to_string(feet_num_) + "x3 elements.");
+
+  for (int count = 0; count < feet_num_; ++count) {
+    geometry_msgs::msg::Point xyz;
+    xyz.x = init_feet_pos_params[0 + count * 3];
+    xyz.y = init_feet_pos_params[1 + count * 3];
+    xyz.z = init_feet_pos_params[2 + count * 3];
+    feet_pos_in_footprint_.emplace_back(xyz);
+    init_feet_pos_in_footprint_.emplace_back(xyz);
+  }
+
+  // Load and validate initial body-to-basefootprint transform
+  std::vector<double> body_basefootprint_params;
+  load_param("init_body_basefootprint_transform", body_basefootprint_params,
+             "No init transform body to basefootprint found.");
+  validate_vector_size(body_basefootprint_params, 7,
+                       "Size of init_body_basefootprint_transform must be 7 (3 "
+                       "for position followed by 4 quaternion).");
+
+  auto transform = array_to_transform(body_basefootprint_params, 0);
+  body_basefootprint_ = transform;
+
+  // Load gait parameters
+  load_param("gait_parameters.max_gait_linear_speed", max_gait_linear_speed_,
+             "Parameter gait_parameters.max_gait_linear_speed was not found.");
+  RCLCPP_INFO(
+      node_->get_logger(),
+      "Loaded gait_parameters.max_gait_linear_speed value is: %f [m/sec]",
+      max_gait_linear_speed_);
+
+  load_param("gait_parameters.max_gait_turning_speed", max_gait_turning_speed_,
+             "Parameter gait_parameters.max_gait_turning_speed was not found.");
+  RCLCPP_INFO(
+      node_->get_logger(),
+      "Loaded gait_parameters.max_gait_turning_speed value is: %f [rad/sec]",
+      max_gait_turning_speed_);
+
+  // load gait patterns
+  load_param("gait_parameters.gait_patterns.num", gait_patterns_.gaits_num_,
+             "Parameter gait_parameters.gait_patterns.num was not found.");
+  RCLCPP_INFO(node_->get_logger(),
+              "Loaded gait_parameters.gait_patterns value is: %d",
+              gait_patterns_.gaits_num_);
+
+  std::vector<long int> vector_gait_patterns;
+  load_param("gait_parameters.gait_patterns.feet_order", vector_gait_patterns,
+             "No init gait_parameters.gait_patterns.feet_order found.");
+  int assert_vector_size = feet_num_ * gait_patterns_.gaits_num_;
+  std::string if_error_message =
+      "Size of gait_parameters.gait_patterns.feet_order must be " +
+      std::to_string(assert_vector_size);
+  validate_vector_size(vector_gait_patterns, assert_vector_size,
+                       if_error_message);
+  // Populate the patterns.
+  int count = 0;
+  gait_patterns_.patterns_.resize(
+      gait_patterns_.gaits_num_); // resize to number of gaits
+  for (int i = 0; i < gait_patterns_.gaits_num_; i++) {
+    gait_patterns_.patterns_[i].resize(feet_num_); // resize to fit the feet
+    std::string log_message =
+        "Loaded gait_pattern [" + std::to_string(i) + "]: ";
+    for (int j = 0; j < feet_num_; j++) {
+      int temp = vector_gait_patterns[count];
+      if (temp < 0) {
+        std::string error_message =
+            "Foot index at gait_parameters.gait_patterns.feet_order[" +
+            std::to_string(count) + "] shall not be less than zero";
+        throw std::runtime_error(error_message);
+      }
+      if (temp >= feet_num_) {
+        std::string error_message =
+            "Foot index at gait_parameters.gait_patterns.feet_order[" +
+            std::to_string(count) +
+            "] shall not be more nor equal to the number of feet, specified "
+            "as " +
+            std::to_string(feet_num_);
+        throw std::runtime_error(error_message);
+      }
+      int foot_index = vector_gait_patterns[count];
+      log_message = log_message + std::to_string(foot_index) + " |";
+      gait_patterns_.patterns_[i][j] = foot_index;
+      count++;
+    }
+    RCLCPP_INFO(node_->get_logger(), log_message.c_str());
+  }
+}
+
+} // namespace penta_pod::kin::gait_generator
diff --git a/interfaces/base_twerk_msgs/CMakeLists.txt b/interfaces/base_twerk_msgs/CMakeLists.txt
index 95d799b..5f712e6 100644
--- a/interfaces/base_twerk_msgs/CMakeLists.txt
+++ b/interfaces/base_twerk_msgs/CMakeLists.txt
@@ -13,6 +13,7 @@ find_package(geometry_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
           "srv/BasePoseSetpoint.srv"
+          "srv/GetCurrentBasePose.srv"
           "action/BaseTwerkAction.action"
         DEPENDENCIES
           builtin_interfaces
diff --git a/interfaces/base_twerk_msgs/action/BaseTwerkAction.action b/interfaces/base_twerk_msgs/action/BaseTwerkAction.action
index df0d8ce..318a202 100644
--- a/interfaces/base_twerk_msgs/action/BaseTwerkAction.action
+++ b/interfaces/base_twerk_msgs/action/BaseTwerkAction.action
@@ -1,9 +1,8 @@
-float64 rx
-float64 ry
-float64 rz
-float64 phi_x
-float64 phi_y
-float64 phi_z
+# pose displacement
+float64[6] r # [rx, ry, rz, roll, pitch, yaw]
+# phase shift
+float64[6] phi  # [phi_x, phi_y, phi_z, phi_roll, phi_pitch, phi_yaw]
+# motion frequency and duration
 float64 w
 float64 dance_time_millis
 ---
diff --git a/interfaces/base_twerk_msgs/srv/GetCurrentBasePose.srv b/interfaces/base_twerk_msgs/srv/GetCurrentBasePose.srv
new file mode 100644
index 0000000..c589d39
--- /dev/null
+++ b/interfaces/base_twerk_msgs/srv/GetCurrentBasePose.srv
@@ -0,0 +1,3 @@
+# returns the current pose of the base
+---
+geometry_msgs/PoseStamped pose
diff --git a/interfaces/gait_generator_msgs/CMakeLists.txt b/interfaces/gait_generator_msgs/CMakeLists.txt
new file mode 100644
index 0000000..58edd4c
--- /dev/null
+++ b/interfaces/gait_generator_msgs/CMakeLists.txt
@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.12)
+project(gait_generator_msgs)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Werror -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+          "srv/SetGaitPattern.srv"
+        DEPENDENCIES
+          builtin_interfaces
+          std_msgs
+          geometry_msgs
+        ADD_LINTER_TESTS
+        )
+
+if(BUILD_TESTING)
+endif()
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()
diff --git a/interfaces/gait_generator_msgs/package.xml b/interfaces/gait_generator_msgs/package.xml
new file mode 100644
index 0000000..b6115d8
--- /dev/null
+++ b/interfaces/gait_generator_msgs/package.xml
@@ -0,0 +1,27 @@
+<?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>gait_generator_msgs</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="xx@xx.xx">mohammad.safeea</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/interfaces/gait_generator_msgs/srv/SetGaitPattern.srv b/interfaces/gait_generator_msgs/srv/SetGaitPattern.srv
new file mode 100644
index 0000000..34555fc
--- /dev/null
+++ b/interfaces/gait_generator_msgs/srv/SetGaitPattern.srv
@@ -0,0 +1,6 @@
+uint8 pattern
+---
+bool success
+string message
+
+
diff --git a/joints_aggregator/include/joints_aggregator/joints_aggregator.hpp b/joints_aggregator/include/joints_aggregator/joints_aggregator.hpp
index f094cab..6cba46c 100644
--- a/joints_aggregator/include/joints_aggregator/joints_aggregator.hpp
+++ b/joints_aggregator/include/joints_aggregator/joints_aggregator.hpp
@@ -30,9 +30,12 @@ class JointsAggregator {
 
   std::mutex q_mutex_;
   std::vector<double> q_;
+  std::vector<double> previous_q_;
   int limbs_num_;
   std::vector<long int> joints_per_limb_;
   int update_interval_millis_;
+  int publish_on_startup_counter_ {0};
+  bool publish_joints_only_on_value_change_{false};
 
   auto get_parameters() -> bool;
 
diff --git a/joints_aggregator/src/joints_aggregator/joints_aggregator.cpp b/joints_aggregator/src/joints_aggregator/joints_aggregator.cpp
index 03fc18d..415fc75 100644
--- a/joints_aggregator/src/joints_aggregator/joints_aggregator.cpp
+++ b/joints_aggregator/src/joints_aggregator/joints_aggregator.cpp
@@ -33,6 +33,7 @@ JointsAggregator::JointsAggregator()
   RCLCPP_INFO(node_->get_logger(), "total joints count is %d", joints_count);
 
   this->q_ = std::vector<double>(joints_count, 0.);
+  this->previous_q_ = std::vector<double>(joints_count, 0.);
 
   joint_state_publisher_ =
       node_->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
@@ -61,7 +62,38 @@ JointsAggregator::JointsAggregator()
           msg.name = joints_states_names;
           msg.position = q_;
         }
-        joint_state_publisher_->publish(msg);
+
+        // publish logic
+        auto force_publish_on_startup = [this]() -> bool {
+          constexpr long times_to_publish_on_startup = 1000;
+          if (publish_on_startup_counter_ < times_to_publish_on_startup) {
+            publish_on_startup_counter_++;
+            return true;
+          }
+          return false;
+        };
+
+        bool force_publish = !publish_joints_only_on_value_change_;
+
+        auto check_joints_value_change = [this]() -> bool {
+          constexpr auto epsilon = 1e-10;
+          auto total_angular_change_squared = 0.0;
+          for (size_t i = 0; i < q_.size(); i++) {
+            auto value = q_[i] - previous_q_[i];
+            total_angular_change_squared += (value * value);
+          }
+          return (total_angular_change_squared > epsilon);
+        };
+
+        if (force_publish_on_startup()) {
+          joint_state_publisher_->publish(msg);
+        } else if (force_publish || check_joints_value_change()) {
+          joint_state_publisher_->publish(msg);
+        }
+
+        for (size_t i = 0; i < q_.size(); i++) {
+          previous_q_[i] = q_[i];
+        }
       });
 }
 
@@ -73,9 +105,13 @@ void JointsAggregator::declare_parameters() {
   // joint_states publish rate (time interval)
   node_->declare_parameter<int>(
       "joints_aggregator.joints_update_interval_millis");
+  // publish only on value change
+  node_->declare_parameter<bool>(
+      "joints_aggregator.publish_joints_only_on_value_change");
 }
 
 auto JointsAggregator::get_parameters() -> bool {
+  // load limbs_num
   if (node_->get_parameter("limbs_num", limbs_num_)) {
     RCLCPP_INFO_STREAM(
         node_->get_logger(),
@@ -85,7 +121,7 @@ auto JointsAggregator::get_parameters() -> bool {
                  "ERROR, can not load (limbs_num) parameter");
     return false;
   }
-
+  // load joints_per_limb vector
   if (node_->get_parameter("joints_per_limb", joints_per_limb_)) {
     std::string formatted_values = "[";
     for (auto &value : joints_per_limb_)
@@ -99,6 +135,7 @@ auto JointsAggregator::get_parameters() -> bool {
                  "ERROR, can not load (joints_per_limb) parameter");
     return false;
   }
+  // sanity check
   if (joints_per_limb_.size() != static_cast<size_t>(limbs_num_)) {
     RCLCPP_ERROR(node_->get_logger(),
                  "ERROR, value of parameter (limbs_num) is not equal to the "
@@ -110,7 +147,7 @@ auto JointsAggregator::get_parameters() -> bool {
                 "comply with a value %d",
                 limbs_num_);
   }
-
+  // load joints_update_interval_millis
   if (!node_->get_parameter("joints_aggregator.joints_update_interval_millis",
                             update_interval_millis_)) {
     RCLCPP_ERROR(
@@ -132,6 +169,22 @@ auto JointsAggregator::get_parameters() -> bool {
   auto rate = 1000.0 / update_interval_millis_;
   RCLCPP_INFO_STREAM(node_->get_logger(),
                      "/joints_states: publish rate is: " << rate << " Hz");
+  // load publish_joints_only_on_value_change
+  if (node_->get_parameter(
+          "joints_aggregator.publish_joints_only_on_value_change",
+          publish_joints_only_on_value_change_)) {
+    RCLCPP_INFO_STREAM(
+        node_->get_logger(),
+        "loaded joints_aggregator.publish_joints_only_on_value_change) is: "
+            << publish_joints_only_on_value_change_);
+  } else {
+    RCLCPP_ERROR(
+        node_->get_logger(),
+        "ERROR, can not load "
+        "joints_aggregator.publish_joints_only_on_value_change parameter");
+    rclcpp::shutdown();
+    return false;
+  }
 
   return true;
 }
diff --git a/penta_description/config/general_config.yaml b/penta_description/config/general_config.yaml
index 81327b6..0194afe 100644
--- a/penta_description/config/general_config.yaml
+++ b/penta_description/config/general_config.yaml
@@ -5,27 +5,42 @@
     max_v: 0.005 # meter per sec
     max_w: 0.05 # rad per sec
     legs_body_transforms: [
-      0.085, 0.0, 0.05, 0.0, 0.0, 0.0, 1.0,
-      0.026266444521870536, 0.08083980388508806, 0.05, 0.0, 0.0, 0.5877852522924731, 0.8090169943749475,
-      -0.06876644452187053, 0.049961746444860226, 0.05, 0.0, 0.0, 0.9510565162951535, 0.30901699437494745,
-      -0.06876644452187054, -0.04996174644486021, 0.05, 0.0, 0.0, 0.9510565162951536, -0.30901699437494734,
-      0.026266444521870515, -0.08083980388508806, 0.05, 0.0, 0.0, 0.5877852522924732, -0.8090169943749473
+      0.085, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
+      0.026266444521870536, 0.08083980388508806, 0.0, 0.0, 0.0, 0.5877852522924731, 0.8090169943749475,
+      -0.06876644452187053, 0.049961746444860226, 0.0, 0.0, 0.0, 0.9510565162951535, 0.30901699437494745,
+      -0.06876644452187054, -0.04996174644486021, 0.0, 0.0, 0.0, 0.9510565162951536, -0.30901699437494734,
+      0.026266444521870515, -0.08083980388508806, 0.0, 0.0, 0.0, 0.5877852522924732, -0.8090169943749473
     ]
     init_feet_pos_in_basefootprint: [
-      0.25, 0.0, 0.0,
-      0.07725424859373686, 0.23776412907378838, 0.0,
-      -0.20225424859373684, 0.1469463130731183, 0.0,
-      -0.2022542485937369, -0.14694631307311826, 0.0,
-      0.07725424859373681, -0.2377641290737884, 0.0
+      0.2265, 0.0, 0.0,
+      0.06999234922592559, 0.2154143009408523, 0.0,
+      -0.18324234922592558, 0.1331333596442452, 0.0,
+      -0.18324234922592564, -0.13313335964424514, 0.0,
+      0.06999234922592555, -0.21541430094085232, 0.0
     ]
     init_body_basefootprint_transform: [
-      0.0, 0.0, 0.075, 0.0, 0.0, 0.0, 1.0
+      0.0, 0.0, 0.158, 0.0, 0.0, 0.0, 1.0
+    ]
+    joint_angles_at_initial_pose_degree: [
+      0.0, 0.0, 90.0,
+      0.0, 0.0, 90.0,
+      0.0, 0.0, 90.0,
+      0.0, 0.0, 90.0,
+      0.0, 0.0, 90.0,
     ]
     gait_parameters:
       max_gait_linear_speed: 0.035 # meter per sec
       max_gait_turning_speed: 0.25 # rad per sec
+      gait_patterns:
+        num: 3
+        feet_order: [
+          0, 2, 4, 1, 3,
+          0, 1, 2, 3, 4,
+          0, 4, 3, 2, 1
+        ]
     joints_aggregator:
-      joints_update_interval_millis: 50
+      joints_update_interval_millis: 10
+      publish_joints_only_on_value_change: true
     base_twerk:
       base_frame_transform_update_interval_millis: 20
       action_server_service_call_interval_millis: 100
@@ -33,19 +48,19 @@
       tracking_linear_velocity: 0.5 # [m/sec]
       tracking_angular_velocity: 1.0 # [rad/sec]
     i2c_actuators_params:
-      actuator_angle_bias_at_joint_zero_degree: [ # considering the motors are at their midrange at 135 degrees when robot joint is at zero"
-        135.0, 135.0, 135.0,
-        135.0, 135.0, 135.0,
-        135.0, 135.0, 135.0,
-        135.0, 135.0, 135.0,
-        135.0, 135.0, 135.0,
+      actuator_angles_at_initial_pose_degree: [
+        135.0, 146.0, 87.0,
+        135.0, 138.0, 87.0,
+        135.0, 138.0, 87.0,
+        135.0, 131.0, 91.0,
+        135.0, 131.0, 83.0,
       ]
       dir: [
-        1.0, 1.0, 1.0,
-        1.0, 1.0, 1.0,
-        1.0, 1.0, 1.0,
-        1.0, 1.0, 1.0,
-        1.0, 1.0, 1.0,
+        1.0, -1.0, -1.0,
+        1.0, -1.0, -1.0,
+        1.0, -1.0, -1.0,
+        1.0, -1.0, -1.0,
+        1.0, -1.0, -1.0,
       ]
     servo_parameters:
       servo_actuation_range_degree: [ # servo angle range
diff --git a/penta_description/config/penta.rviz b/penta_description/config/penta.rviz
index e02ee8c..d434855 100644
--- a/penta_description/config/penta.rviz
+++ b/penta_description/config/penta.rviz
@@ -3,7 +3,8 @@ Panels:
     Help Height: 70
     Name: Displays
     Property Tree Widget:
-      Expanded: ~
+      Expanded:
+        - /Global Options1
       Splitter Ratio: 0.5
     Tree Height: 296
   - Class: rviz_common/Views
@@ -59,6 +60,11 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        head_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         limb0_lower_arm_link:
           Alpha: 1
           Show Axes: false
@@ -139,6 +145,26 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        limb4_lower_arm_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        limb4_shoulder_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        limb4_shoulder_motor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        limb4_upper_arm_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
       Mass Properties:
         Inertia: false
         Mass: false
@@ -148,80 +174,23 @@ Visualization Manager:
       Value: true
       Visual Enabled: true
     - Class: rviz_default_plugins/TF
-      Enabled: true
+      Enabled: false
       Frame Timeout: 15
       Frames:
         All Enabled: true
-        base_footprint:
-          Value: true
-        base_link:
-          Value: true
-        limb0_lower_arm_link:
-          Value: true
-        limb0_shoulder_link:
-          Value: true
-        limb0_shoulder_motor:
-          Value: true
-        limb0_upper_arm_link:
-          Value: true
-        limb1_lower_arm_link:
-          Value: true
-        limb1_shoulder_link:
-          Value: true
-        limb1_shoulder_motor:
-          Value: true
-        limb1_upper_arm_link:
-          Value: true
-        limb2_lower_arm_link:
-          Value: true
-        limb2_shoulder_link:
-          Value: true
-        limb2_shoulder_motor:
-          Value: true
-        limb2_upper_arm_link:
-          Value: true
-        limb3_lower_arm_link:
-          Value: true
-        limb3_shoulder_link:
-          Value: true
-        limb3_shoulder_motor:
-          Value: true
-        limb3_upper_arm_link:
-          Value: true
       Marker Scale: 1
       Name: TF
       Show Arrows: true
       Show Axes: true
       Show Names: false
       Tree:
-        base_footprint:
-          base_link:
-            limb0_shoulder_motor:
-              limb0_shoulder_link:
-                limb0_upper_arm_link:
-                  limb0_lower_arm_link:
-                    {}
-            limb1_shoulder_motor:
-              limb1_shoulder_link:
-                limb1_upper_arm_link:
-                  limb1_lower_arm_link:
-                    {}
-            limb2_shoulder_motor:
-              limb2_shoulder_link:
-                limb2_upper_arm_link:
-                  limb2_lower_arm_link:
-                    {}
-            limb3_shoulder_motor:
-              limb3_shoulder_link:
-                limb3_upper_arm_link:
-                  limb3_lower_arm_link:
-                    {}
+        {}
       Update Interval: 0
-      Value: true
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: base_link
+    Fixed Frame: base_footprint
     Frame Rate: 30
   Name: root
   Tools:
@@ -233,35 +202,35 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 1.7000000476837158
+      Distance: 0.7511907815933228
       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
+        X: 0.01839996874332428
+        Y: -0.016720440238714218
+        Z: 0.056901950389146805
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.6899999380111694
+      Pitch: 0.49500012397766113
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 5.414999961853027
+      Yaw: 2.1449995040893555
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: false
+    collapsed: true
   Height: 800
-  Hide Left Dock: false
+  Hide Left Dock: true
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001cb000002c6fc0200000002fb000000100044006900730070006c006100790073010000003d000001ab000000c900fffffffb0000000a0056006900650077007301000001ee00000115000000a400ffffff000002df000002c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001cb000002c6fc0200000002fb000000100044006900730070006c006100790073000000003d000001ab000000c900fffffffb0000000a0056006900650077007300000001ee00000115000000a400ffffff000004b0000002c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
-    collapsed: false
+    collapsed: true
   Width: 1200
   X: 413
   Y: 99
diff --git a/penta_description/urdf/penta.urdf.xacro b/penta_description/urdf/penta.urdf.xacro
index 22ba5fe..ce2c466 100644
--- a/penta_description/urdf/penta.urdf.xacro
+++ b/penta_description/urdf/penta.urdf.xacro
@@ -13,7 +13,7 @@
     </visual>
   </link>
 
-  <joint name="joint_robot_body_center" type="fixed">
+  <joint name="joint_robot_body_center" type="floating">
     <parent link="base_footprint"/>
     <child link="base_link"/>
     <origin xyz="0.0 0.0 0.075" rpy="0 0 0"/>
diff --git a/penta_i2c_actuators/penta_i2c_actuators/penta_i2c_actuators_node.py b/penta_i2c_actuators/penta_i2c_actuators/penta_i2c_actuators_node.py
index 3e688f8..14714eb 100644
--- a/penta_i2c_actuators/penta_i2c_actuators/penta_i2c_actuators_node.py
+++ b/penta_i2c_actuators/penta_i2c_actuators/penta_i2c_actuators_node.py
@@ -60,7 +60,9 @@ def on_joint_states_callback(self, msg):
                 self.get_logger().error(f"Joint {name} not found in the current message.")
         # Calculate actuator setpoints in degrees
         for i in range(self.joints_count):
-            servo_setpoint = self.dir[i] * (self.q[i] * 180.0 / math.pi) + self.initial_joints_bias_degree[i]
+            q_i_degree = self.q[i] * 180.0 / math.pi
+            dq = q_i_degree - self.joint_angles_at_initial_pose_degree[i]
+            servo_setpoint = self.dir[i] * dq + self.actuator_angles_at_initial_pose_degree[i]
             max_val = self.servo_actuation_range_degree[i]
             self.actuator_setpoint_degree[i] = self.clamp(servo_setpoint, i, 0.0, max_val)
         # Publish actuators setpoint
@@ -94,7 +96,8 @@ def declare_params(self):
         # Declare robot geometry parameters
         self.declare_parameter('limbs_num', 5)  # Default value 5
         self.declare_parameter('joints_per_limb', [3]*5)  # Default value 3
-        self.declare_parameter('i2c_actuators_params.actuator_angle_bias_at_joint_zero_degree', [0.0] * 15)  # Default bias
+        self.declare_parameter('i2c_actuators_params.actuator_angles_at_initial_pose_degree', [0.0] * 15)    # Actuator angles at initial pose
+        self.declare_parameter('joint_angles_at_initial_pose_degree', [0.0] * 15) # Geometrical angles at initial pose 
         self.declare_parameter('i2c_actuators_params.dir', [1.0] * 15)  # Default direction (1.0 for no inversion)
         self.declare_parameter('servo_parameters.servo_actuation_range_degree', [180.0] * 15) # angular range degree
         self.declare_parameter('servo_parameters.servo_min_pulse_width_microsec', [500.0] * 15) # microseconds
@@ -112,16 +115,22 @@ def format_array_to_string(x_list):
         self.get_logger().info(f'Loaded limbs_num: {self.limbs_num}, joints_per_limb: {format_array_to_string(self.joints_per_limb)}')
         self.joints_count = sum(self.joints_per_limb)
         self.get_logger().info(f'Total limbs joints count is: {self.joints_count}') 
-        self.initial_joints_bias_degree = self.get_parameter('i2c_actuators_params.actuator_angle_bias_at_joint_zero_degree').get_parameter_value().double_array_value
+        self.actuator_angles_at_initial_pose_degree = self.get_parameter('i2c_actuators_params.actuator_angles_at_initial_pose_degree').get_parameter_value().double_array_value
+        self.joint_angles_at_initial_pose_degree = self.get_parameter('joint_angles_at_initial_pose_degree').get_parameter_value().double_array_value
         self.dir = self.get_parameter('i2c_actuators_params.dir').get_parameter_value().double_array_value
         self.servo_actuation_range_degree = self.get_parameter('servo_parameters.servo_actuation_range_degree').get_parameter_value().double_array_value
         self.servo_min_pulse_width_microsec = self.get_parameter('servo_parameters.servo_min_pulse_width_microsec').get_parameter_value().double_array_value
         self.servo_max_pulse_width_microsec = self.get_parameter('servo_parameters.servo_max_pulse_width_microsec').get_parameter_value().double_array_value
 
-        if len(self.initial_joints_bias_degree) != self.joints_count:
-            self.get_logger().error('ERROR: actuator_angle_bias_at_joint_zero_degree parameter size mismatch!')
+        if len(self.actuator_angles_at_initial_pose_degree) != self.joints_count:
+            self.get_logger().error('ERROR: actuator_angles_at_initial_pose_degree parameter size mismatch!')
         else:
-            self.get_logger().info(f'Initial joints bias in degree is loaded: {format_array_to_string(self.initial_joints_bias_degree)}')
+            self.get_logger().info(f'Actuators angles at initial configuration is loaded: {format_array_to_string(self.actuator_angles_at_initial_pose_degree)}')
+
+        if len(self.joint_angles_at_initial_pose_degree) != self.joints_count:
+            self.get_logger().error('ERROR: joint_angles_at_initial_pose_degree parameter size mismatch!')
+        else:
+            self.get_logger().info(f'Joints angles at initial configuration is loaded: {format_array_to_string(self.joint_angles_at_initial_pose_degree)}')
 
         if len(self.dir) != self.joints_count:
             self.get_logger().error('ERROR: Direction array size mismatch with joints count!')
diff --git a/penta_pod/launch/penta_sim_full_rviz.launch.py b/penta_pod/launch/penta_sim_full_rviz.launch.py
new file mode 100644
index 0000000..b5a6667
--- /dev/null
+++ b/penta_pod/launch/penta_sim_full_rviz.launch.py
@@ -0,0 +1,66 @@
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackageShare
+from launch.substitutions import LaunchConfiguration
+import os
+
+def generate_launch_description():
+    # Include scorption_cmd_vel_mux launcer
+    scorption_cmd_vel_mux_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(FindPackageShare('scorpion_cmd_vel_mux').find('scorpion_cmd_vel_mux'), 'launch', 'cmd_vel_mux.launch.py')
+        )
+    )
+
+    # Include RVIZ launch file
+    rviz_penta_pod_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(FindPackageShare('penta_pod').find('penta_pod'), 'launch', 'penta_rviz.launch.py')
+        )
+    )
+
+    # Include the penta_pod launch file
+    penta_pod_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(FindPackageShare('penta_pod').find('penta_pod'), 'launch', 'penta_pod.launch.py')
+        )
+    )
+    
+    # Include the gait_generator launch file
+    gait_generator_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(FindPackageShare('gait_generator').find('gait_generator'), 'launch', 'gait_generator.launch.py')
+        )
+    )
+
+    # Include the joints_aggregator launch file
+    joints_aggregator_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(FindPackageShare('joints_aggregator').find('joints_aggregator'), 'launch', 'joints_aggregator.launch.py')
+        )
+    )
+
+    # Include the null space cmd publisher launch file
+    null_space_publisher = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(FindPackageShare('base_twerk').find('base_twerk'), 'launch', 'null_pose_publisher.launch.py')
+        )
+    )
+
+    # Include the twerk action server launch file
+    twerk_action_server = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(FindPackageShare('base_twerk').find('base_twerk'), 'launch', 'base_twerk_action_server.launch.py')
+        )
+    )
+
+    return LaunchDescription([
+        scorption_cmd_vel_mux_launch,
+        rviz_penta_pod_launch,
+        penta_pod_launch,
+        gait_generator_launch,
+        joints_aggregator_launch,
+        null_space_publisher,
+        twerk_action_server
+    ])
diff --git a/penta_pod/launch/penta_sim_rviz.launch.py b/penta_pod/launch/penta_sim_rviz.launch.py
index 2373075..8335d96 100644
--- a/penta_pod/launch/penta_sim_rviz.launch.py
+++ b/penta_pod/launch/penta_sim_rviz.launch.py
@@ -35,24 +35,24 @@ def generate_launch_description():
     )
 
     # Include the null space cmd publisher launch file
-    null_space_publisher = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(FindPackageShare('base_twerk').find('base_twerk'), 'launch', 'null_pose_publisher.launch.py')
-        )
-    )
+    # null_space_publisher = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(FindPackageShare('base_twerk').find('base_twerk'), 'launch', 'null_pose_publisher.launch.py')
+    #     )
+    # )
 
     # Include the twerk action server launch file
-    twerk_action_server = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(FindPackageShare('base_twerk').find('base_twerk'), 'launch', 'base_twerk_action_server.launch.py')
-        )
-    )
+    # twerk_action_server = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(FindPackageShare('base_twerk').find('base_twerk'), 'launch', 'base_twerk_action_server.launch.py')
+    #     )
+    # )
 
     return LaunchDescription([
         rviz_penta_pod_launch,
         penta_pod_launch,
         gait_generator_launch,
         joints_aggregator_launch,
-        null_space_publisher,
-        twerk_action_server
+        # null_space_publisher,
+        # twerk_action_server
     ])
diff --git a/penta_pod/launch/realhardware_bringup.launch.py b/penta_pod/launch/realhardware_bringup.launch.py
index ccfd767..be9c69d 100644
--- a/penta_pod/launch/realhardware_bringup.launch.py
+++ b/penta_pod/launch/realhardware_bringup.launch.py
@@ -43,19 +43,19 @@ def generate_launch_description():
         launch_arguments={'mode': mode}.items()  # Pass the mode argument
     )
 
-        # Include the null space cmd publisher launch file
-    null_space_publisher = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(FindPackageShare('base_twerk').find('base_twerk'), 'launch', 'null_pose_publisher.launch.py')
-        )
-    )
+    # Include the null space cmd publisher launch file
+    # null_space_publisher = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(FindPackageShare('base_twerk').find('base_twerk'), 'launch', 'null_pose_publisher.launch.py')
+    #     )
+    # )
 
     # Include the twerk action server launch file
-    twerk_action_server = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(FindPackageShare('base_twerk').find('base_twerk'), 'launch', 'base_twerk_action_server.launch.py')
-        )
-    )
+    # twerk_action_server = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(FindPackageShare('base_twerk').find('base_twerk'), 'launch', 'base_twerk_action_server.launch.py')
+    #     )
+    # )
 
     return LaunchDescription([
         mode_arg,
@@ -63,6 +63,6 @@ def generate_launch_description():
         gait_generator_launch,
         joints_aggregator_launch,
         penta_i2c_actuators_launch,
-        null_space_publisher,
-        twerk_action_server
+        # null_space_publisher,
+        # twerk_action_server
     ])
diff --git a/penta_teleop/CMakeLists.txt b/penta_teleop/CMakeLists.txt
index c79f036..5c06980 100644
--- a/penta_teleop/CMakeLists.txt
+++ b/penta_teleop/CMakeLists.txt
@@ -8,9 +8,43 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(base_twerk_msgs REQUIRED)
+find_package(gait_generator_msgs REQUIRED)
+
+# Declare library
+add_library(${PROJECT_NAME}
+        src/${PROJECT_NAME}/joystick_extra_controls.cpp
+        src/${PROJECT_NAME}/declare_load_parameters.cpp
+        )
+
+target_include_directories(${PROJECT_NAME}
+        PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>)
+
+ament_target_dependencies(${PROJECT_NAME}
+        rclcpp
+        sensor_msgs
+        base_twerk_msgs
+        gait_generator_msgs
+        )
+
+add_executable(joystick_extra_controls_node src/joystick_extra_controls_node.cpp)
+target_link_libraries(joystick_extra_controls_node
+        ${PROJECT_NAME}
+        )
+
+# Install
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(TARGETS 
+  joystick_extra_controls_node
+  DESTINATION lib/${PROJECT_NAME}
+)
 
 install(DIRECTORY
   config
@@ -19,15 +53,6 @@ install(DIRECTORY
 )
 
 if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
 endif()
 
 ament_package()
diff --git a/penta_teleop/README.md b/penta_teleop/README.md
new file mode 100644
index 0000000..861429b
--- /dev/null
+++ b/penta_teleop/README.md
@@ -0,0 +1,25 @@
+# Pentapod control with Xbox joystick controller
+
+
+You can control the Pentapod from Xbox joystick controller
+
+Tested with wirless Xbox contorller provided with a dongle
+
+Connect the dongle to Raspberry PI, to control the robot as follows:
+
+
+1- Left joystick for linear motion in the plane (x and y)
+
+2- Right joystick for rotations
+
+3- D-Pad up and down buttons, to move the robot up and down
+
+4- X button to change walking pattern
+
+Note, make sure to enable the motion using:
+
+1- Use Right Trigger button (RT) to enable the turbo mode
+
+2- Use Left Trigger button (LT) to enable slow motion mode
+
+3- Walking pattern changes only when the robot is in stand-still
\ No newline at end of file
diff --git a/penta_teleop/config/joy_extra_controls.yaml b/penta_teleop/config/joy_extra_controls.yaml
new file mode 100644
index 0000000..5862a81
--- /dev/null
+++ b/penta_teleop/config/joy_extra_controls.yaml
@@ -0,0 +1,8 @@
+/**:
+  ros__parameters:
+    base_height_ctl: # parameters to change the ehight of the robot base using joystick DPad
+      d_pad_up_down_axis_index: 7
+      z_base_min_value: 0.01
+      z_base_max_value: 0.15
+    gait_patterns_ctl:
+      button_index: 2
diff --git a/penta_teleop/config/teleop_joystick.yaml b/penta_teleop/config/teleop_joystick.yaml
index bb92128..2cd4a80 100644
--- a/penta_teleop/config/teleop_joystick.yaml
+++ b/penta_teleop/config/teleop_joystick.yaml
@@ -7,10 +7,13 @@
 
     axis_linear:
       x: 1 # left stick up/down
+      y: 0 # left stick left/right
     scale_linear:
       x: 0.01
+      y: 0.01
     scale_linear_turbo:
       x: 0.035
+      y: 0.035
 
     axis_angular:
       yaw: 3 # right stick left/right
diff --git a/penta_teleop/include/penta_teleop/joystick_extra_controls.hpp b/penta_teleop/include/penta_teleop/joystick_extra_controls.hpp
new file mode 100644
index 0000000..34eaec5
--- /dev/null
+++ b/penta_teleop/include/penta_teleop/joystick_extra_controls.hpp
@@ -0,0 +1,60 @@
+#ifndef JOYSTICK_EXTRA_CONTROLS_HPP_
+#define JOYSTICK_EXTRA_CONTROLS_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+#include <rclcpp/executors.hpp>
+
+// include messages
+#include "base_twerk_msgs/srv/base_pose_setpoint.hpp"
+#include "base_twerk_msgs/srv/get_current_base_pose.hpp"
+#include "gait_generator_msgs/srv/set_gait_pattern.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "sensor_msgs/msg/joy.hpp"
+
+namespace penta_pod::teleop::joystick_extra_controls {
+
+using PoseStamped = geometry_msgs::msg::PoseStamped;
+using BasePoseSetpoint = base_twerk_msgs::srv::BasePoseSetpoint;
+using GetCurrentBasePose = base_twerk_msgs::srv::GetCurrentBasePose;
+using SetGaitPattern = gait_generator_msgs::srv::SetGaitPattern;
+
+class JoystickExtraControls {
+private:
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_subscriber_;
+
+  rclcpp::Client<GetCurrentBasePose>::SharedPtr get_current_base_pose_client_;
+  rclcpp::Client<BasePoseSetpoint>::SharedPtr set_base_pose_client_;
+  rclcpp::Client<SetGaitPattern>::SharedPtr set_gait_pattern_client_;
+
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  void joy_sub_callback(const sensor_msgs::msg::Joy::SharedPtr msg);
+  void dpad_up_down(const sensor_msgs::msg::Joy::SharedPtr msg);
+  void set_gait_pattern(const sensor_msgs::msg::Joy::SharedPtr msg);
+
+  void handle_get_base_pose_response(
+      rclcpp::Client<GetCurrentBasePose>::SharedFuture response,
+      double z_disp_base_command);
+  void handle_set_base_pose_response(
+      rclcpp::Client<BasePoseSetpoint>::SharedFuture response);
+
+  void declare_parameters();
+  bool get_parameters();
+
+  int d_pad_up_down_axis_index_{7};
+  int gait_patterns_ctl_button_index_{2};
+  int num_of_gait_patterns_{3};
+  double z_base_max_value_{0.01};
+  double z_base_min_value_{0.15};
+
+public:
+  explicit JoystickExtraControls();
+  auto get_node() { return node_; };
+};
+
+} // namespace penta_pod::teleop::joystick_extra_controls
+
+#endif // JOYSTICK_EXTRA_CONTROLS_HPP_
\ No newline at end of file
diff --git a/penta_teleop/launch/teleop_joystick.launch.py b/penta_teleop/launch/teleop_joystick.launch.py
index db77f29..24d6f2a 100644
--- a/penta_teleop/launch/teleop_joystick.launch.py
+++ b/penta_teleop/launch/teleop_joystick.launch.py
@@ -25,9 +25,21 @@ def generate_launch_description():
         # remappings=[("/cmd_vel", "/input/cmd_vel_teleop_joy")],
     )
 
+    joystick_extra_controls_configs = os.path.join(
+        get_package_share_directory("penta_teleop"), "config", "joy_extra_controls.yaml"
+    )
+
+    general_config_params = os.path.join(
+        get_package_share_directory("penta_description"), "config", "general_config.yaml"
+    )
+
+    joystick_extra_controls_node = Node(
+        package="penta_teleop",
+        executable="joystick_extra_controls_node",
+        name="joystick_extra_controls_node",
+        parameters=[joystick_extra_controls_configs, general_config_params],
+    )
+
     return LaunchDescription(
-        [
-            joy_node,
-            teleop_twist_joy_node,
-        ]
+        [joy_node, teleop_twist_joy_node, joystick_extra_controls_node]
     )
diff --git a/penta_teleop/package.xml b/penta_teleop/package.xml
index 71939f5..b4040a8 100644
--- a/penta_teleop/package.xml
+++ b/penta_teleop/package.xml
@@ -12,6 +12,8 @@
   <depend>sensor_msgs</depend>
   <depend>joy</depend>
   <depend>teleop_twist_joy</depend>
+  <depend>base_twerk_msgs</depend>
+  <depend>gait_generator_msgs</depend>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
diff --git a/penta_teleop/src/joystick_extra_controls_node.cpp b/penta_teleop/src/joystick_extra_controls_node.cpp
new file mode 100644
index 0000000..df94a30
--- /dev/null
+++ b/penta_teleop/src/joystick_extra_controls_node.cpp
@@ -0,0 +1,20 @@
+#include "penta_teleop/joystick_extra_controls.hpp"
+#include <rclcpp/executors/multi_threaded_executor.hpp>
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+
+  // Create a shared node instance
+  auto joystick_extra_controls = penta_pod::teleop::joystick_extra_controls::JoystickExtraControls();
+
+  // Multi-threaded executor
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(joystick_extra_controls.get_node());
+
+  // Spin the executor
+  executor.spin();
+
+  // Cleanup
+  rclcpp::shutdown();
+  return 0;
+}
diff --git a/penta_teleop/src/penta_teleop/declare_load_parameters.cpp b/penta_teleop/src/penta_teleop/declare_load_parameters.cpp
new file mode 100644
index 0000000..e83e78f
--- /dev/null
+++ b/penta_teleop/src/penta_teleop/declare_load_parameters.cpp
@@ -0,0 +1,65 @@
+#include "penta_teleop/joystick_extra_controls.hpp"
+
+namespace penta_pod::teleop::joystick_extra_controls {
+
+void JoystickExtraControls::declare_parameters() {
+  node_->declare_parameter<int>("base_height_ctl.d_pad_up_down_axis_index");
+  node_->declare_parameter<int>("gait_patterns_ctl.button_index");
+  node_->declare_parameter<int>("gait_parameters.gait_patterns.num");
+  node_->declare_parameter<double>("base_height_ctl.z_base_min_value");
+  node_->declare_parameter<double>("base_height_ctl.z_base_max_value");
+}
+
+bool JoystickExtraControls::get_parameters() {
+  std::string param_name;
+
+  param_name = "base_height_ctl.d_pad_up_down_axis_index";
+  if (!node_->get_parameter(param_name, this->d_pad_up_down_axis_index_)) {
+    RCLCPP_ERROR(node_->get_logger(), "Can not load parameters %s",
+                 param_name.c_str());
+    return false;
+  }
+  RCLCPP_INFO(node_->get_logger(), "Loaded parameter %s with value %d",
+              param_name.c_str(), this->d_pad_up_down_axis_index_);
+
+  param_name = "gait_patterns_ctl.button_index";
+  if (!node_->get_parameter(param_name,
+                            this->gait_patterns_ctl_button_index_)) {
+    RCLCPP_ERROR(node_->get_logger(), "Can not load parameters %s",
+                 param_name.c_str());
+    return false;
+  }
+  RCLCPP_INFO(node_->get_logger(), "Loaded parameter %s with value %d",
+              param_name.c_str(), this->gait_patterns_ctl_button_index_);
+
+  param_name = "gait_parameters.gait_patterns.num";
+  if (!node_->get_parameter(param_name, this->num_of_gait_patterns_)) {
+    RCLCPP_ERROR(node_->get_logger(), "Can not load parameters %s",
+                 param_name.c_str());
+    return false;
+  }
+  RCLCPP_INFO(node_->get_logger(), "Loaded parameter %s with value %d",
+              param_name.c_str(), this->num_of_gait_patterns_);
+
+  param_name = "base_height_ctl.z_base_max_value";
+  if (!node_->get_parameter(param_name, this->z_base_max_value_)) {
+    RCLCPP_ERROR(node_->get_logger(), "Can not load parameters %s",
+                 param_name.c_str());
+    return false;
+  }
+  RCLCPP_INFO(node_->get_logger(), "Loaded parameter %s with value %f",
+              param_name.c_str(), this->z_base_max_value_);
+
+  param_name = "base_height_ctl.z_base_min_value";
+  if (!node_->get_parameter(param_name, this->z_base_min_value_)) {
+    RCLCPP_ERROR(node_->get_logger(), "Can not load parameters %s",
+                 param_name.c_str());
+    return false;
+  }
+  RCLCPP_INFO(node_->get_logger(), "Loaded parameter %s with value %f",
+              param_name.c_str(), this->z_base_min_value_);
+
+  return true;
+}
+
+} // namespace penta_pod::teleop::joystick_extra_controls
diff --git a/penta_teleop/src/penta_teleop/joystick_extra_controls.cpp b/penta_teleop/src/penta_teleop/joystick_extra_controls.cpp
new file mode 100644
index 0000000..4dfdd5b
--- /dev/null
+++ b/penta_teleop/src/penta_teleop/joystick_extra_controls.cpp
@@ -0,0 +1,177 @@
+#include "penta_teleop/joystick_extra_controls.hpp"
+
+namespace penta_pod::teleop::joystick_extra_controls {
+
+JoystickExtraControls::JoystickExtraControls()
+    : node_{rclcpp::Node::make_shared("joystick_extra_controls")} {
+  RCLCPP_INFO(node_->get_logger(),
+              "Starting joystick_extra_controls, subscribing to joy topic and "
+              "calling base_pose_setpoint service");
+  // load parameters
+  this->declare_parameters();
+  this->get_parameters();
+
+  callback_group_ = node_->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  // Joystick subscriber
+  joy_subscriber_ = node_->create_subscription<sensor_msgs::msg::Joy>(
+      "joy", 10, [this](const sensor_msgs::msg::Joy::SharedPtr msg) {
+        this->joy_sub_callback(msg);
+      });
+
+  // Create service clients
+  get_current_base_pose_client_ = node_->create_client<GetCurrentBasePose>(
+      "get_current_null_pose", rmw_qos_profile_services_default,
+      callback_group_);
+  set_base_pose_client_ = node_->create_client<BasePoseSetpoint>(
+      "cmd_null_setpoint", rmw_qos_profile_services_default, callback_group_);
+  // Service cleint to change walking pattern
+  set_gait_pattern_client_ = node_->create_client<SetGaitPattern>(
+      "/gait_generator/set_gait_pattern", rmw_qos_profile_services_default,
+      callback_group_);
+}
+
+void JoystickExtraControls::joy_sub_callback(
+    const sensor_msgs::msg::Joy::SharedPtr msg) {
+
+  // move base up and down
+  dpad_up_down(msg);
+  // change walking pattern
+  set_gait_pattern(msg);
+}
+
+void JoystickExtraControls::set_gait_pattern(
+    const sensor_msgs::msg::Joy::SharedPtr msg) {
+
+  static int last_button_state = 0;
+  static int walking_pattern = 0;
+  int current_button_state =
+      msg->buttons[this->gait_patterns_ctl_button_index_];
+  if (current_button_state == 1 && last_button_state == 0) {
+    walking_pattern++;
+    if (walking_pattern > num_of_gait_patterns_) {
+      walking_pattern = 0;
+    }
+    // Button was pressed
+    RCLCPP_INFO(node_->get_logger(),
+                "Button index %d pressed, changing walking pattern",
+                this->gait_patterns_ctl_button_index_);
+    // Check if the service is available before calling
+    if (!set_gait_pattern_client_->wait_for_service(
+            std::chrono::seconds(100))) {
+      RCLCPP_ERROR(node_->get_logger(),
+                   "Service set_gait_pattern_client_ is unavailable.");
+      return;
+    }
+    // Add your logic to change the walking pattern here
+    auto request = std::make_shared<SetGaitPattern::Request>();
+    request->pattern = walking_pattern;
+    // Call the service asynchronously
+    auto future = set_gait_pattern_client_->async_send_request(
+        request, [this](rclcpp::Client<SetGaitPattern>::SharedFuture response) {
+          auto result = response.get();
+
+          if (!result) {
+            RCLCPP_ERROR(node_->get_logger(),
+                         "Failed change walking pattern (null return).");
+            return;
+          }
+          if (result->success) {
+            RCLCPP_INFO(node_->get_logger(),
+                        "Walking pattern changed successfully.");
+          } else {
+            RCLCPP_ERROR(node_->get_logger(),
+                         "Failed to change walking pattern.");
+          }
+        });
+  }
+  last_button_state = current_button_state;
+}
+
+void JoystickExtraControls::dpad_up_down(
+    const sensor_msgs::msg::Joy::SharedPtr msg) {
+  if (msg->axes[this->d_pad_up_down_axis_index_] != 0) {
+    double delta_z = 0.001;
+    double z_disp_base_command = msg->axes[7] * delta_z;
+
+    // Check if the service is available before calling
+    if (!get_current_base_pose_client_->wait_for_service(
+            std::chrono::seconds(100))) {
+      RCLCPP_ERROR(node_->get_logger(),
+                   "Service get_current_base_pose is unavailable.");
+      return;
+    }
+
+    // Prepare the request
+    auto request = std::make_shared<GetCurrentBasePose::Request>();
+
+    // Call the service asynchronously with a callback
+    auto future = get_current_base_pose_client_->async_send_request(
+        request,
+        [this, z_disp_base_command](
+            rclcpp::Client<GetCurrentBasePose>::SharedFuture response) {
+          this->handle_get_base_pose_response(response, z_disp_base_command);
+        });
+  }
+}
+
+void JoystickExtraControls::handle_get_base_pose_response(
+    rclcpp::Client<GetCurrentBasePose>::SharedFuture response,
+    double z_disp_base_command) {
+
+  auto result = response.get();
+
+  if (!result) {
+    RCLCPP_ERROR(node_->get_logger(), "Failed to get base pose.");
+    return;
+  }
+
+  // Sanity checks
+  if ((result->pose.pose.position.z > this->z_base_max_value_) &&
+      z_disp_base_command > 0.) {
+    RCLCPP_ERROR(node_->get_logger(),
+                 "Already above the max Z limit, can not move up any further.");
+    return;
+  }
+
+  if ((result->pose.pose.position.z < this->z_base_min_value_) &&
+      z_disp_base_command < 0.) {
+    RCLCPP_ERROR(
+        node_->get_logger(),
+        "Already below the min Z limit, can not move down any further.");
+    return;
+  }
+
+  RCLCPP_INFO(node_->get_logger(), "Current base pose: %f %f %f %f %f %f %f",
+              result->pose.pose.position.x, result->pose.pose.position.y,
+              result->pose.pose.position.z, result->pose.pose.orientation.x,
+              result->pose.pose.orientation.y, result->pose.pose.orientation.z,
+              result->pose.pose.orientation.w);
+
+  // Prepare new base pose setpoint request
+  auto setpoint_request = std::make_shared<BasePoseSetpoint::Request>();
+  setpoint_request->pose = result->pose;
+  setpoint_request->pose.pose.position.z += z_disp_base_command;
+
+  // Call the set_base_pose service asynchronously
+  auto future = set_base_pose_client_->async_send_request(
+      setpoint_request,
+      [this](rclcpp::Client<BasePoseSetpoint>::SharedFuture setpoint_response) {
+        this->handle_set_base_pose_response(setpoint_response);
+      });
+}
+
+void JoystickExtraControls::handle_set_base_pose_response(
+    rclcpp::Client<BasePoseSetpoint>::SharedFuture response) {
+
+  auto result = response.get();
+
+  if (!result) {
+    RCLCPP_ERROR(node_->get_logger(), "Failed to set base pose.");
+  } else {
+    RCLCPP_INFO(node_->get_logger(), "Base pose set successfully.");
+  }
+}
+
+} // namespace penta_pod::teleop::joystick_extra_controls
diff --git a/scorpion_cmd_vel_mux/CMakeLists.txt b/scorpion_cmd_vel_mux/CMakeLists.txt
new file mode 100644
index 0000000..d36babf
--- /dev/null
+++ b/scorpion_cmd_vel_mux/CMakeLists.txt
@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.12)
+project(scorpion_cmd_vel_mux)
+
+# Default to C++20
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 20)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Werror -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+
+#############
+## Install ##
+#############
+
+install(DIRECTORY
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+##############.
+## Testing  ##
+##############
+
+if(BUILD_TESTING)
+endif()
+
+ament_package()
diff --git a/scorpion_cmd_vel_mux/README.md b/scorpion_cmd_vel_mux/README.md
new file mode 100644
index 0000000..6b3b745
--- /dev/null
+++ b/scorpion_cmd_vel_mux/README.md
@@ -0,0 +1,18 @@
+# scorpion_cmd_vel_mux
+
+requires cmd_vel_mux
+
+
+```
+git clone https://github.com/kobuki-base/cmd_vel_mux.git
+```
+
+Then, colcon build,
+
+Finally, launch the `scorpion_cmd_vel_mux`
+
+```
+ros2 launch scorpion_cmd_vel_mux cmd_vel_mux.launch.py
+```
+
+
diff --git a/scorpion_cmd_vel_mux/config/cmd_vel_mux_params.yaml b/scorpion_cmd_vel_mux/config/cmd_vel_mux_params.yaml
new file mode 100644
index 0000000..a41986b
--- /dev/null
+++ b/scorpion_cmd_vel_mux/config/cmd_vel_mux_params.yaml
@@ -0,0 +1,28 @@
+scorpion_cmd_vel_mux:
+  ros__parameters:
+    subscribers:
+        joystick_teleop:
+          topic:       "input/cmd_vel_teleop_joy"
+          timeout:     0.1
+          priority:    9
+          short_desc:  "Remote control joystick"
+        keyboard_teleop:
+          topic:       "input/cmd_vel_teleop_keyboard"
+          timeout:     0.1
+          priority:    8
+          short_desc:  "Keyboard operation"
+        navigation_stack:
+          topic:       "input/cmd_vel_nav2"
+          timeout:     0.5
+          priority:    1
+          short_desc:  "Navigation stack controller"
+        web_application:
+          topic:       "input/cmd_vel_teleop_web"
+          timeout:     0.3
+          priority:    7
+          short_desc:  "Web application"
+        handheld_device:
+          topic:       "cmd_vel_mux_input/cmd_vel_teleop_handheld_joy"
+          timeout:     0.3
+          priority:    6
+          short_desc:  "handheld device teleoperation"
diff --git a/scorpion_cmd_vel_mux/launch/cmd_vel_mux.launch.py b/scorpion_cmd_vel_mux/launch/cmd_vel_mux.launch.py
new file mode 100644
index 0000000..03805c5
--- /dev/null
+++ b/scorpion_cmd_vel_mux/launch/cmd_vel_mux.launch.py
@@ -0,0 +1,25 @@
+import os
+
+import ament_index_python.packages
+import launch
+import launch_ros.actions
+
+import yaml
+
+
+def generate_launch_description():
+    share_dir = ament_index_python.packages.get_package_share_directory('scorpion_cmd_vel_mux')
+    params_file = os.path.join(share_dir, 'config', 'cmd_vel_mux_params.yaml')
+    with open(params_file, 'r') as f:
+        params = yaml.safe_load(f)['scorpion_cmd_vel_mux']['ros__parameters']
+
+    cmd_vel_mux_node = launch_ros.actions.Node(
+        package='cmd_vel_mux',
+        executable='cmd_vel_mux_node',
+        output='both',
+        parameters=[params],
+        # remappings={("/cmd_vel", "output/cmd_vel_ref"), # this is for the output topic
+        #             ("input/cmd_vel_nav2", "/cmd_vel")} # this is for the input from nav2
+    )
+
+    return launch.LaunchDescription([cmd_vel_mux_node])
diff --git a/scorpion_cmd_vel_mux/package.xml b/scorpion_cmd_vel_mux/package.xml
new file mode 100644
index 0000000..390bb23
--- /dev/null
+++ b/scorpion_cmd_vel_mux/package.xml
@@ -0,0 +1,30 @@
+<?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>scorpion_cmd_vel_mux</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="xx@xx.xx">mohammad.safeea</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+  <depend>launch_testing</depend>
+
+  <depend>cmd_vel_mux</depend>
+  <depend>slam_toolbox</depend>
+  <depend>nav2_bringup</depend>
+  <depend>nav2_simple_commander</depend>
+  
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>