From eebc4ec6ed2968195a0b752de031d218b1c2474f Mon Sep 17 00:00:00 2001 From: Kyle-Eldridge Date: Fri, 16 Jan 2026 16:00:03 -0800 Subject: [PATCH 1/5] Change drivetrain calculations and add empty class --- slugbot_package/CMakeLists.txt | 10 ++ .../include/slugbot_package/Drivetrain.hpp | 34 +++++++ .../include/slugbot_package/Simulation.hpp | 34 +++++-- .../include/slugbot_package/SlugbotDriver.hpp | 19 ++-- slugbot_package/launch/robot_launch.py | 29 ++++++ slugbot_package/launch/simulation_launch.py | 2 - slugbot_package/src/Drivetrain.cpp | 69 ++++++++++++++ slugbot_package/src/Simulation.cpp | 75 ++++++++------- slugbot_package/src/SlugbotDriver.cpp | 93 ++++++++++++++----- 9 files changed, 292 insertions(+), 73 deletions(-) create mode 100644 slugbot_package/include/slugbot_package/Drivetrain.hpp create mode 100644 slugbot_package/launch/robot_launch.py create mode 100644 slugbot_package/src/Drivetrain.cpp diff --git a/slugbot_package/CMakeLists.txt b/slugbot_package/CMakeLists.txt index 7235290..be94a96 100644 --- a/slugbot_package/CMakeLists.txt +++ b/slugbot_package/CMakeLists.txt @@ -77,6 +77,16 @@ install(TARGETS slugbot_driver_node DESTINATION lib/${PROJECT_NAME} ) + +add_executable(drivetrain_node src/Drivetrain.cpp) +ament_target_dependencies(drivetrain_node + rclcpp + std_msgs +) +install(TARGETS + drivetrain_node + DESTINATION lib/${PROJECT_NAME} +) # Install additional directories. install(DIRECTORY launch diff --git a/slugbot_package/include/slugbot_package/Drivetrain.hpp b/slugbot_package/include/slugbot_package/Drivetrain.hpp new file mode 100644 index 0000000..b107c6c --- /dev/null +++ b/slugbot_package/include/slugbot_package/Drivetrain.hpp @@ -0,0 +1,34 @@ +#pragma once + +#include +#include + +#include "rclcpp/macros.hpp" + +#include "std_msgs/msg/float64.hpp" +#include "std_msgs/msg/string.hpp" +#include "rclcpp/rclcpp.hpp" + +class Drivetrain : public rclcpp::Node { +public: + Drivetrain(); + void update(); + +private: + rclcpp::Subscription::SharedPtr front_left_wheel_subscription; + rclcpp::Subscription::SharedPtr front_right_wheel_subscription; + rclcpp::Subscription::SharedPtr back_left_wheel_subscription; + rclcpp::Subscription::SharedPtr back_right_wheel_subscription; + + rclcpp::Subscription::SharedPtr left_turn_angle_subscription; + rclcpp::Subscription::SharedPtr right_turn_angle_subscription; + + double front_left_wheel_speed_msg = 0.0; + double front_right_wheel_speed_msg = 0.0; + double back_left_wheel_speed_msg = 0.0; + double back_right_wheel_speed_msg = 0.0; + + double left_turn_angle_msg = 0.0; + double right_turn_angle_msg = 0.0; + rclcpp::TimerBase::SharedPtr timer; +}; \ No newline at end of file diff --git a/slugbot_package/include/slugbot_package/Simulation.hpp b/slugbot_package/include/slugbot_package/Simulation.hpp index 7786774..a06b792 100644 --- a/slugbot_package/include/slugbot_package/Simulation.hpp +++ b/slugbot_package/include/slugbot_package/Simulation.hpp @@ -1,6 +1,9 @@ #ifndef SIMULATION_HPP #define SIMULATION_HPP +#include +#include + #include "rclcpp/macros.hpp" #include "webots_ros2_driver/PluginInterface.hpp" #include "webots_ros2_driver/WebotsNode.hpp" @@ -8,6 +11,7 @@ #include "std_msgs/msg/float64.hpp" #include "std_msgs/msg/string.hpp" #include "rclcpp/rclcpp.hpp" +#include namespace slugbot_driver { class Simulation : public webots_ros2_driver::PluginInterface { @@ -17,28 +21,40 @@ class Simulation : public webots_ros2_driver::PluginInterface { std::unordered_map ¶meters) override; private: + rclcpp::Subscription::SharedPtr front_left_wheel_subscription; + rclcpp::Subscription::SharedPtr front_right_wheel_subscription; + rclcpp::Subscription::SharedPtr back_left_wheel_subscription; + rclcpp::Subscription::SharedPtr back_right_wheel_subscription; + + rclcpp::Subscription::SharedPtr left_turn_angle_subscription; + rclcpp::Subscription::SharedPtr right_turn_angle_subscription; - rclcpp::Subscription::SharedPtr left_wheel_subscription; - rclcpp::Subscription::SharedPtr right_wheel_subscription; - rclcpp::Subscription::SharedPtr turn_angle_subscription; rclcpp::Publisher::SharedPtr keyboard_publisher; - double left_wheel_speed_msg; - double right_wheel_speed_msg; - double turn_angle_msg; + + double front_left_wheel_speed_msg = 0.0; + double front_right_wheel_speed_msg = 0.0; + double back_left_wheel_speed_msg = 0.0; + double back_right_wheel_speed_msg = 0.0; + + double left_turn_angle_msg = 0.0; + double right_turn_angle_msg = 0.0; + std::string keys_pressed; WbDeviceTag right_motors[3]; - WbDeviceTag *right_side; + WbDeviceTag *right_side = nullptr; WbDeviceTag left_motors[3]; - WbDeviceTag *left_side; + WbDeviceTag *left_side = nullptr; WbDeviceTag turn_motors[4]; - // distance sensors for debugging + // distance sensors for debugging (optional) WbDeviceTag ds_left; WbDeviceTag ds_right; + int time_step_ms = 0; }; } // namespace slugbot_driver + #endif \ No newline at end of file diff --git a/slugbot_package/include/slugbot_package/SlugbotDriver.hpp b/slugbot_package/include/slugbot_package/SlugbotDriver.hpp index 43415db..1741758 100644 --- a/slugbot_package/include/slugbot_package/SlugbotDriver.hpp +++ b/slugbot_package/include/slugbot_package/SlugbotDriver.hpp @@ -1,14 +1,12 @@ #ifndef SLUGBOT_DRIVER_HPP #define SLUGBOT_DRIVER_HPP +#include +#include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/float64.hpp" #include "std_msgs/msg/string.hpp" #include "geometry_msgs/msg/twist.hpp" #include "messages/msg/controller_input.hpp" -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/range.hpp" - -#include "rclcpp/rclcpp.hpp" class SlugbotDriver : public rclcpp::Node { public: @@ -16,16 +14,23 @@ class SlugbotDriver : public rclcpp::Node { void update(); private: - rclcpp::Publisher::SharedPtr left_wheel_publisher; - rclcpp::Publisher::SharedPtr right_wheel_publisher; - rclcpp::Publisher::SharedPtr turn_angle_publisher; + rclcpp::Publisher::SharedPtr back_left_wheel_publisher; + rclcpp::Publisher::SharedPtr back_right_wheel_publisher; + rclcpp::Publisher::SharedPtr front_left_wheel_publisher; + rclcpp::Publisher::SharedPtr front_right_wheel_publisher; + rclcpp::Publisher::SharedPtr left_turn_angle_publisher; + rclcpp::Publisher::SharedPtr right_turn_angle_publisher; + rclcpp::Subscription::SharedPtr keyboard_subscription; rclcpp::Subscription::SharedPtr cmd_vel_subscription_avoid; rclcpp::Subscription::SharedPtr controller_subscription; + geometry_msgs::msg::Twist cmd_vel_msg_avoid; std::string keys_pressed; messages::msg::ControllerInput controller_input; + rclcpp::TimerBase::SharedPtr timer; bool recieved_input = false; }; + #endif \ No newline at end of file diff --git a/slugbot_package/launch/robot_launch.py b/slugbot_package/launch/robot_launch.py new file mode 100644 index 0000000..d3a9262 --- /dev/null +++ b/slugbot_package/launch/robot_launch.py @@ -0,0 +1,29 @@ +import os +import shutil +import launch +from launch_ros.actions import Node +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from ament_index_python.packages import get_package_share_directory, get_package_prefix + +def generate_launch_description(): + obstacle_avoider = Node( + package='slugbot_package', + executable='obstacle_avoider', + ) + + slugbot_node = Node( + package='slugbot_package', + executable='slugbot_driver_node', + ) + + drivetrain = Node( + package='slugbot_package', + executable='drivetrain_node', + ) + + return LaunchDescription([ + slugbot_node, + obstacle_avoider, + drivetrain + ]) \ No newline at end of file diff --git a/slugbot_package/launch/simulation_launch.py b/slugbot_package/launch/simulation_launch.py index 42d4aac..bd2f239 100644 --- a/slugbot_package/launch/simulation_launch.py +++ b/slugbot_package/launch/simulation_launch.py @@ -1,5 +1,3 @@ -# TODO: Add a separate launch file for the real robot - import os import shutil import launch diff --git a/slugbot_package/src/Drivetrain.cpp b/slugbot_package/src/Drivetrain.cpp new file mode 100644 index 0000000..bc52832 --- /dev/null +++ b/slugbot_package/src/Drivetrain.cpp @@ -0,0 +1,69 @@ +#include "slugbot_package/Drivetrain.hpp" + +#include "rclcpp/rclcpp.hpp" +#include +#include +#include "std_msgs/msg/float64.hpp" +#include "std_msgs/msg/string.hpp" + +Drivetrain::Drivetrain() + : Node("drivetrain") { + + // TODO: Create motors and sensors + + // subscribe to wheel/turn commands published by the logic node + front_left_wheel_subscription = this->create_subscription( + "/front_left_wheel", rclcpp::SensorDataQoS().reliable(), + [this](const std_msgs::msg::Float64::SharedPtr msg){ + this->front_left_wheel_speed_msg = msg->data; + } + ); + front_right_wheel_subscription = this->create_subscription( + "/front_right_wheel", rclcpp::SensorDataQoS().reliable(), + [this](const std_msgs::msg::Float64::SharedPtr msg){ + this->front_right_wheel_speed_msg = msg->data; + } + ); + + back_left_wheel_subscription = this->create_subscription( + "/back_left_wheel", rclcpp::SensorDataQoS().reliable(), + [this](const std_msgs::msg::Float64::SharedPtr msg){ + this->back_left_wheel_speed_msg = msg->data; + } + ); + back_right_wheel_subscription = this->create_subscription( + "/back_right_wheel", rclcpp::SensorDataQoS().reliable(), + [this](const std_msgs::msg::Float64::SharedPtr msg){ + this->back_right_wheel_speed_msg = msg->data; + } + ); + + left_turn_angle_subscription = this->create_subscription( + "/left_turn_angle", rclcpp::SensorDataQoS().reliable(), + [this](const std_msgs::msg::Float64::SharedPtr msg){ + this->left_turn_angle_msg = msg->data; + } + ); + right_turn_angle_subscription = this->create_subscription( + "/right_turn_angle", rclcpp::SensorDataQoS().reliable(), + [this](const std_msgs::msg::Float64::SharedPtr msg){ + this->right_turn_angle_msg = msg->data; + } + ); + + timer = this->create_wall_timer(std::chrono::milliseconds(20), + std::bind(&Drivetrain::update, this)); +} + +void Drivetrain::update() { + // Set motor speeds + // TODO: update real motors +} + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + auto drivetrain = std::make_shared(); + rclcpp::spin(drivetrain); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/slugbot_package/src/Simulation.cpp b/slugbot_package/src/Simulation.cpp index 5d25782..be6bd92 100644 --- a/slugbot_package/src/Simulation.cpp +++ b/slugbot_package/src/Simulation.cpp @@ -1,11 +1,8 @@ #include "slugbot_package/Simulation.hpp" -#include "slugbot_package/Simulation.hpp" - #include "rclcpp/rclcpp.hpp" -#include -#include -#include +#include +#include #include #include #include @@ -16,7 +13,6 @@ #define WHEEL_COUNT 6 void set_position(WbDeviceTag *side, float value); -void set_velocity(WbDeviceTag *side, float value); namespace slugbot_driver { void Simulation::init( @@ -41,28 +37,48 @@ void Simulation::init( set_position(left_side, INFINITY); set_position(right_side, INFINITY); - set_velocity(left_side, 0); - set_velocity(right_side, 0); + for (int i = 0; i < WHEEL_COUNT>>1; i++) { + wb_motor_set_velocity(left_motors[i], 0); + wb_motor_set_velocity(right_motors[i], 0); + } // subscribe to wheel/turn commands published by the logic node - left_wheel_subscription = node->create_subscription( - "/left_wheel", rclcpp::SensorDataQoS().reliable(), + front_left_wheel_subscription = node->create_subscription( + "/front_left_wheel", rclcpp::SensorDataQoS().reliable(), + [this](const std_msgs::msg::Float64::SharedPtr msg){ + this->front_left_wheel_speed_msg = msg->data; + } + ); + front_right_wheel_subscription = node->create_subscription( + "/front_right_wheel", rclcpp::SensorDataQoS().reliable(), [this](const std_msgs::msg::Float64::SharedPtr msg){ - this->left_wheel_speed_msg = msg->data; + this->front_right_wheel_speed_msg = msg->data; } ); - right_wheel_subscription = node->create_subscription( - "/right_wheel", rclcpp::SensorDataQoS().reliable(), + back_left_wheel_subscription = node->create_subscription( + "/back_left_wheel", rclcpp::SensorDataQoS().reliable(), + [this](const std_msgs::msg::Float64::SharedPtr msg){ + this->back_left_wheel_speed_msg = msg->data; + } + ); + back_right_wheel_subscription = node->create_subscription( + "/back_right_wheel", rclcpp::SensorDataQoS().reliable(), [this](const std_msgs::msg::Float64::SharedPtr msg){ - this->right_wheel_speed_msg = msg->data; + this->back_right_wheel_speed_msg = msg->data; } ); - turn_angle_subscription = node->create_subscription( - "/turn_angle", rclcpp::SensorDataQoS().reliable(), + left_turn_angle_subscription = node->create_subscription( + "/left_turn_angle", rclcpp::SensorDataQoS().reliable(), [this](const std_msgs::msg::Float64::SharedPtr msg){ - this->turn_angle_msg = msg->data; + this->left_turn_angle_msg = msg->data; + } + ); + right_turn_angle_subscription = node->create_subscription( + "/right_turn_angle", rclcpp::SensorDataQoS().reliable(), + [this](const std_msgs::msg::Float64::SharedPtr msg){ + this->right_turn_angle_msg = msg->data; } ); @@ -101,18 +117,18 @@ void Simulation::step() { msg->data = keys; keyboard_publisher->publish(std::move(msg)); - // Set speeds and angles - set_velocity(left_side, left_wheel_speed_msg); - set_velocity(right_side, right_wheel_speed_msg); // Manually set all 6 speeds - for (int i = 0; i < 3; i++) { - wb_motor_set_velocity(left_motors[i], left_wheel_speed_msg); - wb_motor_set_velocity(right_motors[i], right_wheel_speed_msg); + for (int i = 0; i < 3; i += 2) { + wb_motor_set_velocity(left_motors[i], front_left_wheel_speed_msg); + wb_motor_set_velocity(right_motors[i], front_right_wheel_speed_msg); } + wb_motor_set_velocity(left_motors[1], back_left_wheel_speed_msg); + wb_motor_set_velocity(right_motors[1], back_right_wheel_speed_msg); - for(int i = 0; i < 4; i++) { - wb_motor_set_position(turn_motors[i], i < 2 ? turn_angle_msg : -turn_angle_msg); - } + wb_motor_set_position(turn_motors[0], left_turn_angle_msg); + wb_motor_set_position(turn_motors[1], right_turn_angle_msg); + wb_motor_set_position(turn_motors[2], -left_turn_angle_msg); + wb_motor_set_position(turn_motors[3], -right_turn_angle_msg); } } // namespace slugbot_driver @@ -123,13 +139,6 @@ void set_position(WbDeviceTag *side, float value) { } } -void set_velocity(WbDeviceTag *side, float value) { - for (int i = 0; i < WHEEL_COUNT>>1; i++) { - wb_motor_set_velocity(*side, value); - side++; - } -} - #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(slugbot_driver::Simulation, webots_ros2_driver::PluginInterface) \ No newline at end of file diff --git a/slugbot_package/src/SlugbotDriver.cpp b/slugbot_package/src/SlugbotDriver.cpp index 8f1debb..afe2301 100644 --- a/slugbot_package/src/SlugbotDriver.cpp +++ b/slugbot_package/src/SlugbotDriver.cpp @@ -8,18 +8,23 @@ #include "geometry_msgs/msg/twist.hpp" #include "messages/msg/controller_input.hpp" -#define HALF_DISTANCE_BETWEEN_WHEELS 0.24 -#define WHEEL_RADIUS 0.06 -#define MAX_WHEEL_SPEED 25.0 -#define MAX_LINEAR_SPEED (MAX_WHEEL_SPEED * WHEEL_RADIUS) -#define MAX_TURN_ANGLE (M_PI / 6) -#define IGNORE_AVOID_MESSAGE true +const double HALF_DISTANCE_BETWEEN_WHEELS = 0.24; +const double FORWARD_DISTANCE_BETWEEN_WHEELS = 0.30; +const double WHEEL_RADIUS = 0.06; +const double MAX_WHEEL_SPEED = 25.0; +const double MAX_LINEAR_SPEED = (MAX_WHEEL_SPEED * WHEEL_RADIUS); +const double MAX_TURN_ANGLE = (M_PI / 6.0); +const bool IGNORE_AVOID_MESSAGE = true; SlugbotDriver::SlugbotDriver() : Node("slugbot_driver") { - left_wheel_publisher = this->create_publisher("/left_wheel", 10); - right_wheel_publisher = this->create_publisher("/right_wheel", 10); - turn_angle_publisher = this->create_publisher("/turn_angle", 10); + // This assumes a 4 wheel drive with the front wheels turning, but it will also work with 6 wheels + back_left_wheel_publisher = this->create_publisher("/back_left_wheel", 10); + back_right_wheel_publisher = this->create_publisher("/back_right_wheel", 10); + front_left_wheel_publisher = this->create_publisher("/front_left_wheel", 10); + front_right_wheel_publisher = this->create_publisher("/front_right_wheel", 10); + left_turn_angle_publisher = this->create_publisher("/left_turn_angle", 10); + right_turn_angle_publisher = this->create_publisher("/right_turn_angle", 10); cmd_vel_subscription_avoid = this->create_subscription( "/cmd_vel_avoid", 10, @@ -56,7 +61,7 @@ void SlugbotDriver::update() { double ly = controller_input.left_y; double rx = controller_input.right_x; speed = -MAX_LINEAR_SPEED * ly * std::abs(ly); - angle = MAX_TURN_ANGLE * rx * std::abs(rx); + angle = rx * std::abs(rx); } else { if (keys_pressed.find('w') != std::string::npos){ speed = MAX_LINEAR_SPEED; @@ -65,10 +70,10 @@ void SlugbotDriver::update() { speed -= MAX_LINEAR_SPEED; } if (keys_pressed.find('a') != std::string::npos){ - angle = -MAX_TURN_ANGLE; + angle = -1; } if (keys_pressed.find('d') != std::string::npos){ - angle += MAX_TURN_ANGLE; + angle += 1; } } @@ -78,21 +83,65 @@ void SlugbotDriver::update() { } speed += cmd_vel_msg_avoid.linear.x; + speed = std::clamp(speed, -MAX_LINEAR_SPEED, MAX_LINEAR_SPEED); + double max_angular_speed = speed / (HALF_DISTANCE_BETWEEN_WHEELS + FORWARD_DISTANCE_BETWEEN_WHEELS/std::tan(MAX_TURN_ANGLE)); + angle *= max_angular_speed; angle += cmd_vel_msg_avoid.angular.z; + angle = std::clamp(angle, -max_angular_speed, max_angular_speed); + + double back_left, back_right, front_left, front_right, left_angle, right_angle; + if(std::abs(angle) < 1e-5) { + back_left = speed; + back_right = speed; + front_left = speed; + front_right = speed; + left_angle = 0.0; + right_angle = 0.0; + }else{ + double radius = speed / angle; + double left_r = radius - HALF_DISTANCE_BETWEEN_WHEELS; + double right_r = radius + HALF_DISTANCE_BETWEEN_WHEELS; + back_left = angle * left_r; + back_right = angle * right_r; + double front_left_r = std::hypot(left_r, FORWARD_DISTANCE_BETWEEN_WHEELS); + double front_right_r = std::hypot(right_r, FORWARD_DISTANCE_BETWEEN_WHEELS); + front_left = angle * front_left_r * std::abs(left_r) / left_r; + front_right = angle * front_right_r * std::abs(right_r) / right_r; + left_angle = std::atan(FORWARD_DISTANCE_BETWEEN_WHEELS / left_r); + right_angle = std::atan(FORWARD_DISTANCE_BETWEEN_WHEELS / right_r); + double highest_speed = std::max({std::abs(back_left), std::abs(back_right), std::abs(front_left), std::abs(front_right)}); + if (highest_speed > MAX_LINEAR_SPEED) { + double scale = MAX_LINEAR_SPEED / highest_speed; + back_left *= scale; + back_right *= scale; + front_left *= scale; + front_right *= scale; + } + } + + auto back_left_msg = std_msgs::msg::Float64(); + back_left_msg.data = back_left / WHEEL_RADIUS; + back_left_wheel_publisher->publish(back_left_msg); + + auto back_right_msg = std_msgs::msg::Float64(); + back_right_msg.data = back_right / WHEEL_RADIUS; + back_right_wheel_publisher->publish(back_right_msg); - double wheelSpeed = std::clamp(speed / WHEEL_RADIUS, -MAX_WHEEL_SPEED, MAX_WHEEL_SPEED); + auto front_left_msg = std_msgs::msg::Float64(); + front_left_msg.data = front_left / WHEEL_RADIUS; + front_left_wheel_publisher->publish(front_left_msg); - auto left_msg = std::make_unique(); - left_msg->data = wheelSpeed; - left_wheel_publisher->publish(std::move(left_msg)); + auto front_right_msg = std_msgs::msg::Float64(); + front_right_msg.data = front_right / WHEEL_RADIUS; + front_right_wheel_publisher->publish(front_right_msg); - auto right_msg = std::make_unique(); - right_msg->data = wheelSpeed; - right_wheel_publisher->publish(std::move(right_msg)); + auto left_angle_msg = std_msgs::msg::Float64(); + left_angle_msg.data = left_angle; + left_turn_angle_publisher->publish(left_angle_msg); - auto turn_msg = std::make_unique(); - turn_msg->data = angle; - turn_angle_publisher->publish(std::move(turn_msg)); + auto right_angle_msg = std_msgs::msg::Float64(); + right_angle_msg.data = right_angle; + right_turn_angle_publisher->publish(right_angle_msg); } int main(int argc, char **argv) { From 217eac61925bb079cbe9cbccebd7e21ff13a4e71 Mon Sep 17 00:00:00 2001 From: Kyle-Eldridge Date: Fri, 16 Jan 2026 17:54:29 -0800 Subject: [PATCH 2/5] Fix but with negative speed --- slugbot_package/src/SlugbotDriver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/slugbot_package/src/SlugbotDriver.cpp b/slugbot_package/src/SlugbotDriver.cpp index afe2301..400f42d 100644 --- a/slugbot_package/src/SlugbotDriver.cpp +++ b/slugbot_package/src/SlugbotDriver.cpp @@ -84,7 +84,7 @@ void SlugbotDriver::update() { speed += cmd_vel_msg_avoid.linear.x; speed = std::clamp(speed, -MAX_LINEAR_SPEED, MAX_LINEAR_SPEED); - double max_angular_speed = speed / (HALF_DISTANCE_BETWEEN_WHEELS + FORWARD_DISTANCE_BETWEEN_WHEELS/std::tan(MAX_TURN_ANGLE)); + double max_angular_speed = std::abs(speed) / (HALF_DISTANCE_BETWEEN_WHEELS + FORWARD_DISTANCE_BETWEEN_WHEELS/std::tan(MAX_TURN_ANGLE)); angle *= max_angular_speed; angle += cmd_vel_msg_avoid.angular.z; angle = std::clamp(angle, -max_angular_speed, max_angular_speed); From 95e9b003fe226b020ca2b5729682eb7926344530 Mon Sep 17 00:00:00 2001 From: Kyle-Eldridge Date: Thu, 22 Jan 2026 12:22:41 -0800 Subject: [PATCH 3/5] Switch to swerve --- messages/CMakeLists.txt | 1 + messages/msg/WheelStates.msg | 2 + slugbot_package/CMakeLists.txt | 27 +++- .../include/slugbot_package/Simulation.hpp | 25 +-- .../include/slugbot_package/SlugbotDriver.hpp | 17 +- slugbot_package/package.xml | 2 + slugbot_package/protos/SlugRobot.proto | 118 -------------- slugbot_package/src/Simulation.cpp | 94 ++++------- slugbot_package/src/SlugbotDriver.cpp | 150 +++++++----------- utils/math/Pose2d.cpp | 72 +++++++++ utils/math/Pose2d.hpp | 30 ++++ utils/math/Rotation2d.cpp | 60 +++++++ utils/math/Rotation2d.hpp | 30 ++++ utils/math/Translation2d.cpp | 57 +++++++ utils/math/Translation2d.hpp | 30 ++++ 15 files changed, 408 insertions(+), 307 deletions(-) create mode 100644 messages/msg/WheelStates.msg create mode 100644 utils/math/Pose2d.cpp create mode 100644 utils/math/Pose2d.hpp create mode 100644 utils/math/Rotation2d.cpp create mode 100644 utils/math/Rotation2d.hpp create mode 100644 utils/math/Translation2d.cpp create mode 100644 utils/math/Translation2d.hpp diff --git a/messages/CMakeLists.txt b/messages/CMakeLists.txt index 4052d91..28421f5 100644 --- a/messages/CMakeLists.txt +++ b/messages/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(rosidl_default_generators REQUIRED) # Generate messages rosidl_generate_interfaces(${PROJECT_NAME} "msg/ControllerInput.msg" + "msg/WheelStates.msg" ) ament_export_dependencies(rosidl_default_runtime) diff --git a/messages/msg/WheelStates.msg b/messages/msg/WheelStates.msg new file mode 100644 index 0000000..f611855 --- /dev/null +++ b/messages/msg/WheelStates.msg @@ -0,0 +1,2 @@ +float64[4] speeds +float64[4] angles diff --git a/slugbot_package/CMakeLists.txt b/slugbot_package/CMakeLists.txt index be94a96..9f54339 100644 --- a/slugbot_package/CMakeLists.txt +++ b/slugbot_package/CMakeLists.txt @@ -56,6 +56,7 @@ ament_target_dependencies( rclcpp webots_ros2_driver std_msgs + messages ) install(TARGETS ${PROJECT_NAME} @@ -95,10 +96,34 @@ install(DIRECTORY protos DESTINATION share/${PROJECT_NAME}/ ) +# Geometry utilities library from repository utils +add_library(slugbot_geometry STATIC + ${CMAKE_SOURCE_DIR}/../utils/math/Translation2d.cpp + ${CMAKE_SOURCE_DIR}/../utils/math/Rotation2d.cpp + ${CMAKE_SOURCE_DIR}/../utils/math/Pose2d.cpp +) +target_include_directories(slugbot_geometry PUBLIC + ${CMAKE_SOURCE_DIR}/../utils +) +target_compile_features(slugbot_geometry PUBLIC cxx_std_14) +install(TARGETS + slugbot_geometry + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +ament_export_libraries(slugbot_geometry) +ament_export_include_directories(${CMAKE_SOURCE_DIR}/../utils) ament_export_include_directories( include ) ament_export_libraries( ${PROJECT_NAME} ) -ament_package() \ No newline at end of file +ament_package() + +# Link geometry library into executables that may use it +target_link_libraries(slugbot_driver_node slugbot_geometry) +target_link_libraries(${PROJECT_NAME} slugbot_geometry) +target_link_libraries(obstacle_avoider slugbot_geometry) +target_link_libraries(drivetrain_node slugbot_geometry) \ No newline at end of file diff --git a/slugbot_package/include/slugbot_package/Simulation.hpp b/slugbot_package/include/slugbot_package/Simulation.hpp index a06b792..2abbfae 100644 --- a/slugbot_package/include/slugbot_package/Simulation.hpp +++ b/slugbot_package/include/slugbot_package/Simulation.hpp @@ -8,8 +8,8 @@ #include "webots_ros2_driver/PluginInterface.hpp" #include "webots_ros2_driver/WebotsNode.hpp" -#include "std_msgs/msg/float64.hpp" #include "std_msgs/msg/string.hpp" +#include "messages/msg/wheel_states.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -21,32 +21,15 @@ class Simulation : public webots_ros2_driver::PluginInterface { std::unordered_map ¶meters) override; private: - rclcpp::Subscription::SharedPtr front_left_wheel_subscription; - rclcpp::Subscription::SharedPtr front_right_wheel_subscription; - rclcpp::Subscription::SharedPtr back_left_wheel_subscription; - rclcpp::Subscription::SharedPtr back_right_wheel_subscription; - - rclcpp::Subscription::SharedPtr left_turn_angle_subscription; - rclcpp::Subscription::SharedPtr right_turn_angle_subscription; + rclcpp::Subscription::SharedPtr wheel_states_subscription; rclcpp::Publisher::SharedPtr keyboard_publisher; - double front_left_wheel_speed_msg = 0.0; - double front_right_wheel_speed_msg = 0.0; - double back_left_wheel_speed_msg = 0.0; - double back_right_wheel_speed_msg = 0.0; - - double left_turn_angle_msg = 0.0; - double right_turn_angle_msg = 0.0; + messages::msg::WheelStates wheel_states_msg; std::string keys_pressed; - WbDeviceTag right_motors[3]; - WbDeviceTag *right_side = nullptr; - - WbDeviceTag left_motors[3]; - WbDeviceTag *left_side = nullptr; - + WbDeviceTag motors[4]; WbDeviceTag turn_motors[4]; // distance sensors for debugging (optional) diff --git a/slugbot_package/include/slugbot_package/SlugbotDriver.hpp b/slugbot_package/include/slugbot_package/SlugbotDriver.hpp index 1741758..c47e293 100644 --- a/slugbot_package/include/slugbot_package/SlugbotDriver.hpp +++ b/slugbot_package/include/slugbot_package/SlugbotDriver.hpp @@ -3,10 +3,9 @@ #include #include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/float64.hpp" #include "std_msgs/msg/string.hpp" -#include "geometry_msgs/msg/twist.hpp" #include "messages/msg/controller_input.hpp" +#include "messages/msg/wheel_states.hpp" class SlugbotDriver : public rclcpp::Node { public: @@ -14,18 +13,14 @@ class SlugbotDriver : public rclcpp::Node { void update(); private: - rclcpp::Publisher::SharedPtr back_left_wheel_publisher; - rclcpp::Publisher::SharedPtr back_right_wheel_publisher; - rclcpp::Publisher::SharedPtr front_left_wheel_publisher; - rclcpp::Publisher::SharedPtr front_right_wheel_publisher; - rclcpp::Publisher::SharedPtr left_turn_angle_publisher; - rclcpp::Publisher::SharedPtr right_turn_angle_publisher; + void optimize_wheel_states(messages::msg::WheelStates& wheels); + + messages::msg::WheelStates current_wheel_states; + + rclcpp::Publisher::SharedPtr wheel_states_publisher; rclcpp::Subscription::SharedPtr keyboard_subscription; - rclcpp::Subscription::SharedPtr cmd_vel_subscription_avoid; rclcpp::Subscription::SharedPtr controller_subscription; - - geometry_msgs::msg::Twist cmd_vel_msg_avoid; std::string keys_pressed; messages::msg::ControllerInput controller_input; diff --git a/slugbot_package/package.xml b/slugbot_package/package.xml index 62df8e8..244800d 100644 --- a/slugbot_package/package.xml +++ b/slugbot_package/package.xml @@ -16,6 +16,8 @@ webots_ros2_driver pluginlib sensor_msgs + ament_index_cpp + ament_index_cpp ament_lint_auto ament_lint_common diff --git a/slugbot_package/protos/SlugRobot.proto b/slugbot_package/protos/SlugRobot.proto index 7885ed9..fdac779 100644 --- a/slugbot_package/protos/SlugRobot.proto +++ b/slugbot_package/protos/SlugRobot.proto @@ -422,65 +422,6 @@ PROTO SlugRobot [ } ] } - HingeJoint { - jointParameters HingeJointParameters { - axis 0 0 -1 - anchor -0.087 -0.078 0.02 - } - device [ - RotationalMotor { - name "MiddleLeftWheel" - maxVelocity 25 - maxTorque 1.0 - } - ] - endPoint Solid { - translation -0.087 -0.078 0.02 - children [ - DEF WHEEL_MIDDLELEFT_TRANSFORM Transform { - scale 0.001 0.001 0.001 - children [ - Shape { - appearance USE METAL_APPEARANCE - geometry IndexedFaceSet { - coord Coordinate { - point [ - 29.89 -0.000122 14.72, 28.09 -10.22 14.72, -0.1191 -0.000122 14.83, -0.1119 0.04053 14.83, -0.1191 -0.000122 39.96, -0.1119 0.04053 39.96, 15.24 -0.000122 39.96, 14.32 -5.213 39.96, 22.9 -19.21 14.72, -0.09125 0.07642 14.83, -0.09125 0.07642 39.96, 11.68 -9.797 39.96, 14.94 -25.88 14.72, -0.05957 0.103 14.83, -0.05957 0.103 39.96, 7.621 -13.2 39.96, 5.191 -29.43 14.72, -0.02072 0.1172 14.83, -0.02072 0.1172 39.96, 2.647 -15.01 39.96, -5.189 -29.44 14.72, 0.02063 0.1172 14.83, 0.02063 0.1172 39.96, -2.646 -15.01 39.96, -14.94 -25.89 14.72, 0.05951 0.103 14.83, 0.05951 0.103 39.96, -7.621 -13.2 39.96, -22.9 -19.21 14.72, 0.09119 0.07642 14.83, 0.09119 0.07642 39.96, -11.68 -9.798 39.96, -28.09 -10.22 14.72, 0.1119 0.04065 14.83, 0.1119 0.04065 39.96, -14.32 -5.214 39.96, -29.89 -0.001709 14.72, 0.1191 -0.000122 14.83, 0.1191 -0.000122 39.96, -15.24 -0.000854 39.96, -28.09 10.22 14.72, 0.1119 -0.04077 14.83, 0.1119 -0.04077 39.96, -14.32 5.212 39.96, -22.9 19.21 14.72, 0.09125 -0.07666 14.83, 0.09125 -0.07666 39.96, -11.68 9.797 39.96, -14.95 25.88 14.72, 0.05957 -0.1033 14.83, 0.05957 -0.1033 39.96, -7.622 13.2 39.96, -5.192 29.43 14.72, 0.02072 -0.1174 14.83, 0.02072 -0.1174 39.96, -2.648 15.01 39.96, 5.188 29.44 14.72, -0.02063 -0.1174 14.83, -0.02063 -0.1174 39.96, 2.646 15.01 39.96, 14.94 25.89 14.72, -0.05951 -0.1033 14.83, -0.05951 -0.1033 39.96, 7.62 13.2 39.96, 22.89 19.21 14.72, -0.09119 -0.07666 14.83, -0.09119 -0.07666 39.96, 11.68 9.798 39.96, 28.09 10.23 14.72, -0.1119 -0.04089 14.83, -0.1119 -0.04089 39.96, 14.32 5.214 39.96, 0.003052 7.764 -0.4676, -6.721 3.882 -0.4676, -6.721 -3.882 -0.4676, 0.002319 -7.764 -0.4676, 6.726 -3.883 -0.4676, 6.727 3.881 -0.4676, 6.727 3.881 -40.91, 6.726 -3.883 -40.91, 0.002319 -7.764 -40.91, -6.721 -3.882 -40.91, -6.721 3.882 -40.91, 0.003052 7.764 -40.91, 6.618 13.73 40.91, 11.92 9.5 40.91, 14.86 3.389 40.91, 14.86 -3.393 40.91, 11.92 -9.504 40.91, 6.614 -13.73 40.91, 0.001648 -15.24 40.91, -6.61 -13.73 40.91, -11.91 -9.501 40.91, -14.85 -3.391 40.91, -14.85 3.392 40.91, -11.91 9.502 40.91, -6.609 13.73 40.91, 0.003052 15.24 40.91, 0.003052 15.24 39.81, -6.609 13.73 39.81, -11.91 9.502 39.81, -14.85 3.392 39.81, -14.85 -3.391 39.81, -11.91 -9.501 39.81, -6.61 -13.73 39.81, 0.001648 -15.24 39.81, 6.614 -13.73 39.81, 11.92 -9.504 39.81, 14.86 -3.393 39.81, 14.86 3.389 39.81, 11.92 9.5 39.81, 6.618 13.73 39.81 - ] - } - coordIndex [ - 0, 1, 2, -1, 1, 3, 2, -1, 2, 3, 4, -1, 3, 5, 4, -1, 4, 5, 6, -1, 5, 7, 6, -1, 6, 7, 0, -1, 7, 1, 0, -1, 1, 8, 3, -1, 8, 9, 3, -1, 3, 9, 5, -1, 9, 10, 5, -1, 5, 10, 7, -1, 10, 11, 7, -1, 7, 11, 1, -1, 11, 8, 1, -1, 8, 12, 9, -1, 12, 13, 9, -1, 9, 13, 10, -1, 13, 14, 10, -1, 10, 14, 11, -1, 14, 15, 11, -1, 11, 15, 8, -1, 15, 12, 8, -1, 12, 16, 13, -1, 16, 17, 13, -1, 13, 17, 14, -1, 17, 18, 14, -1, 14, 18, 15, -1, 18, 19, 15, -1, 15, 19, 12, -1, 19, 16, 12, -1, 16, 20, 17, -1, 20, 21, 17, -1, 17, 21, 18, -1, 21, 22, 18, -1, 18, 22, 19, -1, 22, 23, 19, -1, 19, 23, 16, -1, 23, 20, 16, -1, 20, 24, 21, -1, 24, 25, 21, -1, 21, 25, 22, -1, 25, 26, 22, -1, 22, 26, 23, -1, 26, 27, 23, -1, 23, 27, 20, -1, 27, 24, 20, -1, 24, 28, 25, -1, 28, 29, 25, -1, 25, 29, 26, -1, 29, 30, 26, -1, 26, 30, 27, -1, 30, 31, 27, -1, 27, 31, 24, -1, 31, 28, 24, -1, 28, 32, 29, -1, 32, 33, 29, -1, 29, 33, 30, -1, 33, 34, 30, -1, 30, 34, 31, -1, 34, 35, 31, -1, 31, 35, 28, -1, 35, 32, 28, -1, 32, 36, 33, -1, 36, 37, 33, -1, 33, 37, 34, -1, 37, 38, 34, -1, 34, 38, 35, -1, 38, 39, 35, -1, 35, 39, 32, -1, 39, 36, 32, -1, 36, 40, 37, -1, 40, 41, 37, -1, 37, 41, 38, -1, 41, 42, 38, -1, 38, 42, 39, -1, 42, 43, 39, -1, 39, 43, 36, -1, 43, 40, 36, -1, 40, 44, 41, -1, 44, 45, 41, -1, 41, 45, 42, -1, 45, 46, 42, -1, 42, 46, 43, -1, 46, 47, 43, -1, 43, 47, 40, -1, 47, 44, 40, -1, 44, 48, 45, -1, 48, 49, 45, -1, 45, 49, 46, -1, 49, 50, 46, -1, 46, 50, 47, -1, 50, 51, 47, -1, 47, 51, 44, -1, 51, 48, 44, -1, 48, 52, 49, -1, 52, 53, 49, -1, 49, 53, 50, -1, 53, 54, 50, -1, 50, 54, 51, -1, 54, 55, 51, -1, 51, 55, 48, -1, 55, 52, 48, -1, 52, 56, 53, -1, 56, 57, 53, -1, 53, 57, 54, -1, 57, 58, 54, -1, 54, 58, 55, -1, 58, 59, 55, -1, 55, 59, 52, -1, 59, 56, 52, -1, 56, 60, 57, -1, 60, 61, 57, -1, 57, 61, 58, -1, 61, 62, 58, -1, 58, 62, 59, -1, 62, 63, 59, -1, 59, 63, 56, -1, 63, 60, 56, -1, 60, 64, 61, -1, 64, 65, 61, -1, 61, 65, 62, -1, 65, 66, 62, -1, 62, 66, 63, -1, 66, 67, 63, -1, 63, 67, 60, -1, 67, 64, 60, -1, 64, 68, 65, -1, 68, 69, 65, -1, 65, 69, 66, -1, 69, 70, 66, -1, 66, 70, 67, -1, 70, 71, 67, -1, 67, 71, 64, -1, 71, 68, 64, -1, 68, 0, 69, -1, 0, 2, 69, -1, 69, 2, 70, -1, 2, 4, 70, -1, 70, 4, 71, -1, 4, 6, 71, -1, 71, 6, 68, -1, 6, 0, 68, -1, 72, 73, 74, -1, 74, 75, 72, -1, 72, 75, 76, -1, 76, 77, 72, -1, 78, 79, 80, -1, 80, 81, 78, -1, 78, 81, 82, -1, 82, 83, 78, -1, 72, 83, 82, -1, 82, 73, 72, -1, 73, 82, 81, -1, 81, 74, 73, -1, 74, 81, 80, -1, 80, 75, 74, -1, 75, 80, 79, -1, 79, 76, 75, -1, 76, 79, 78, -1, 78, 77, 76, -1, 77, 78, 83, -1, 83, 72, 77, -1, 84, 85, 86, -1, 86, 87, 84, -1, 84, 87, 88, -1, 88, 89, 84, -1, 84, 89, 90, -1, 90, 91, 84, -1, 84, 91, 92, -1, 92, 93, 84, -1, 84, 93, 94, -1, 94, 95, 84, -1, 84, 95, 96, -1, 96, 97, 84, -1, 98, 99, 100, -1, 100, 101, 98, -1, 98, 101, 102, -1, 102, 103, 98, -1, 98, 103, 104, -1, 104, 105, 98, -1, 98, 105, 106, -1, 106, 107, 98, -1, 98, 107, 108, -1, 108, 109, 98, -1, 98, 109, 110, -1, 110, 111, 98, -1, 96, 99, 98, -1, 98, 97, 96, -1, 95, 100, 99, -1, 99, 96, 95, -1, 94, 101, 100, -1, 100, 95, 94, -1, 93, 102, 101, -1, 101, 94, 93, -1, 92, 103, 102, -1, 102, 93, 92, -1, 91, 104, 103, -1, 103, 92, 91, -1, 90, 105, 104, -1, 104, 91, 90, -1, 89, 106, 105, -1, 105, 90, 89, -1, 88, 107, 106, -1, 106, 89, 88, -1, 87, 108, 107, -1, 107, 88, 87, -1, 86, 109, 108, -1, 108, 87, 86, -1, 85, 110, 109, -1, 109, 86, 85, -1, 84, 111, 110, -1, 110, 85, 84, -1, 97, 98, 111, -1, 111, 84, 97, -1 - ] - creaseAngle 1 - } - } - ] - } - DEF WHEEL_MIDDLE_LEFT_TRANSFORM Transform { - scale 0.001 0.001 0.001 - children [ - Shape { - appearance USE METAL_APPEARANCE - geometry IndexedFaceSet { - coord Coordinate { - point [ - -57.06 0 -40.26, -54.27 17.63 -40.26, -52.66 17.11 -40.26, -55.37 0 -40.26, -59 0 -39.29, -56.11 18.23 -39.29, -60.21 0 -36.39, -57.26 18.6 -36.39, -60.45 0 36.87, -57.49 18.68 36.87, -57.79 0 40.26, -54.96 17.86 40.26, -55.13 0 40.26, -52.43 17.04 40.26, -57.31 0 36.63, -54.5 17.71 36.63, -57.31 0 29.14, -54.5 17.71 29.14, -37.96 0 20.43, -36.1 11.73 20.43, -30.22 0 20.43, -28.75 9.34 20.43, -30.22 0 16.8, -28.75 9.34 16.8, -37.72 0 16.8, -35.87 11.66 16.8, -57.55 0 -28.65, -54.73 17.78 -28.65, -57.31 0 -35.91, -54.5 17.71 -35.91, -57.31 0 -39.05, -54.5 17.71 -39.05, -46.17 33.54 -40.26, -44.8 32.55 -40.26, -47.73 34.68 -39.29, -48.71 35.39 -36.39, -48.9 35.53 36.87, -46.75 33.97 40.26, -44.6 32.4 40.26, -46.36 33.68 36.63, -46.36 33.68 29.14, -30.71 22.31 20.43, -24.45 17.77 20.43, -24.45 17.77 16.8, -30.52 22.17 16.8, -46.56 33.83 -28.65, -46.36 33.68 -35.91, -46.36 33.68 -39.05, -33.54 46.17 -40.26, -32.55 44.8 -40.26, -34.68 47.73 -39.29, -35.39 48.71 -36.39, -35.53 48.9 36.87, -33.97 46.75 40.26, -32.41 44.6 40.26, -33.68 46.36 36.63, -33.68 46.36 29.14, -22.31 30.71 20.43, -17.77 24.45 20.43, -17.77 24.45 16.8, -22.17 30.52 16.8, -33.83 46.56 -28.65, -33.68 46.36 -35.91, -33.68 46.36 -39.05, -17.63 54.27 -40.26, -17.11 52.66 -40.26, -18.23 56.11 -39.29, -18.61 57.26 -36.39, -18.68 57.49 36.87, -17.86 54.96 40.26, -17.04 52.43 40.26, -17.71 54.5 36.63, -17.71 54.5 29.14, -11.73 36.1 20.43, -9.341 28.75 20.43, -9.341 28.75 16.8, -11.66 35.87 16.8, -17.78 54.73 -28.65, -17.71 54.5 -35.91, -17.71 54.5 -39.05, -0.001524 57.06 -40.26, -0.001493 55.37 -40.26, -0.001585 59 -39.29, -0.001616 60.21 -36.39, -0.001616 60.45 36.87, -0.001554 57.79 40.26, -0.001493 55.13 40.26, -0.001524 57.31 36.63, -0.001524 57.31 29.14, -0.001036 37.96 20.43, -0.000822 30.22 20.43, -0.000822 30.22 16.8, -0.001036 37.72 16.8, -0.001554 57.55 -28.65, -0.001524 57.31 -35.91, -0.001524 57.31 -39.05, 17.63 54.27 -40.26, 17.11 52.66 -40.26, 18.23 56.11 -39.29, 18.6 57.26 -36.39, 18.68 57.49 36.87, 17.86 54.96 40.26, 17.03 52.43 40.26, 17.71 54.5 36.63, 17.71 54.5 29.14, 11.73 36.1 20.43, 9.339 28.75 20.43, 9.339 28.75 16.8, 11.66 35.87 16.8, 17.78 54.73 -28.65, 17.71 54.5 -35.91, 17.71 54.5 -39.05, 33.54 46.17 -40.26, 32.54 44.8 -40.26, 34.68 47.73 -39.29, 35.39 48.71 -36.39, 35.53 48.91 36.87, 33.97 46.75 40.26, 32.4 44.6 40.26, 33.68 46.36 36.63, 33.68 46.36 29.14, 22.31 30.71 20.43, 17.76 24.45 20.43, 17.76 24.45 16.8, 22.17 30.52 16.8, 33.82 46.56 -28.65, 33.68 46.36 -35.91, 33.68 46.36 -39.05, 46.16 33.54 -40.26, 44.8 32.55 -40.26, 47.73 34.68 -39.29, 48.71 35.39 -36.39, 48.9 35.53 36.87, 46.75 33.97 40.26, 44.6 32.41 40.26, 46.36 33.69 36.63, 46.36 33.69 29.14, 30.71 22.31 20.43, 24.45 17.77 20.43, 24.45 17.77 16.8, 30.52 22.17 16.8, 46.56 33.83 -28.65, 46.36 33.69 -35.91, 46.36 33.69 -39.05, 54.27 17.64 -40.26, 52.66 17.11 -40.26, 56.11 18.23 -39.29, 57.26 18.61 -36.39, 57.49 18.68 36.87, 54.96 17.86 40.26, 52.43 17.04 40.26, 54.5 17.71 36.63, 54.5 17.71 29.14, 36.1 11.73 20.43, 28.74 9.341 20.43, 28.74 9.341 16.8, 35.87 11.66 16.8, 54.73 17.79 -28.65, 54.5 17.71 -35.91, 54.5 17.71 -39.05, 57.06 0.003052 -40.26, 55.37 0.00293 -40.26, 59 0.003174 -39.29, 60.21 0.003174 -36.39, 60.45 0.003174 36.87, 57.79 0.003052 40.26, 55.13 0.00293 40.26, 57.31 0.003052 36.63, 57.31 0.003052 29.14, 37.96 0.002075 20.43, 30.22 0.001587 20.43, 30.22 0.001587 16.8, 37.72 0.002075 16.8, 57.55 0.003052 -28.65, 57.31 0.003052 -35.91, 57.31 0.003052 -39.05, 54.27 -17.63 -40.26, 52.66 -17.11 -40.26, 56.11 -18.23 -39.29, 57.26 -18.6 -36.39, 57.49 -18.68 36.87, 54.96 -17.85 40.26, 52.43 -17.03 40.26, 54.5 -17.71 36.63, 54.5 -17.71 29.14, 36.1 -11.73 20.43, 28.75 -9.338 20.43, 28.75 -9.338 16.8, 35.87 -11.65 16.8, 54.73 -17.78 -28.65, 54.5 -17.71 -35.91, 54.5 -17.71 -39.05, 46.17 -33.54 -40.26, 44.8 -32.54 -40.26, 47.73 -34.68 -39.29, 48.71 -35.39 -36.39, 48.91 -35.53 36.87, 46.75 -33.96 40.26, 44.6 -32.4 40.26, 46.36 -33.68 36.63, 46.36 -33.68 29.14, 30.71 -22.31 20.43, 24.45 -17.76 20.43, 24.45 -17.76 16.8, 30.52 -22.17 16.8, 46.56 -33.82 -28.65, 46.36 -33.68 -35.91, 46.36 -33.68 -39.05, 33.54 -46.16 -40.26, 32.55 -44.79 -40.26, 34.68 -47.73 -39.29, 35.39 -48.71 -36.39, 35.53 -48.9 36.87, 33.97 -46.75 40.26, 32.41 -44.6 40.26, 33.69 -46.36 36.63, 33.69 -46.36 29.14, 22.32 -30.71 20.43, 17.77 -24.45 20.43, 17.77 -24.45 16.8, 22.17 -30.51 16.8, 33.83 -46.55 -28.65, 33.69 -46.36 -35.91, 33.69 -46.36 -39.05, 17.64 -54.27 -40.26, 17.11 -52.66 -40.26, 18.24 -56.11 -39.29, 18.61 -57.26 -36.39, 18.68 -57.49 36.87, 17.86 -54.96 40.26, 17.04 -52.43 40.26, 17.71 -54.5 36.63, 17.71 -54.5 29.14, 11.73 -36.1 20.43, 9.342 -28.74 20.43, 9.342 -28.74 16.8, 11.66 -35.87 16.8, 17.79 -54.73 -28.65, 17.71 -54.5 -35.91, 17.71 -54.5 -39.05, 0.004549 -57.06 -40.26, 0.004427 -55.37 -40.26, 0.004702 -59 -39.29, 0.004793 -60.21 -36.39, 0.004824 -60.45 36.87, 0.00461 -57.79 40.26, 0.004396 -55.13 40.26, 0.00458 -57.31 36.63, 0.00458 -57.31 29.14, 0.003023 -37.96 20.43, 0.002413 -30.22 20.43, 0.002413 -30.22 16.8, 0.003023 -37.72 16.8, 0.00458 -57.55 -28.65, 0.00458 -57.31 -35.91, 0.00458 -57.31 -39.05, -17.63 -54.27 -40.26, -17.11 -52.66 -40.26, -18.23 -56.11 -39.29, -18.6 -57.26 -36.39, -18.67 -57.49 36.87, -17.85 -54.96 40.26, -17.03 -52.43 40.26, -17.7 -54.5 36.63, -17.7 -54.5 29.14, -11.73 -36.11 20.43, -9.337 -28.75 20.43, -9.337 -28.75 16.8, -11.65 -35.88 16.8, -17.78 -54.73 -28.65, -17.7 -54.5 -35.91, -17.7 -54.5 -39.05, -33.54 -46.17 -40.26, -32.54 -44.8 -40.26, -34.67 -47.73 -39.29, -35.38 -48.71 -36.39, -35.53 -48.91 36.87, -33.96 -46.76 40.26, -32.4 -44.6 40.26, -33.68 -46.36 36.63, -33.68 -46.36 29.14, -22.31 -30.71 20.43, -17.76 -24.45 20.43, -17.76 -24.45 16.8, -22.17 -30.52 16.8, -33.82 -46.56 -28.65, -33.68 -46.36 -35.91, -33.68 -46.36 -39.05, -46.16 -33.55 -40.26, -44.79 -32.55 -40.26, -47.73 -34.68 -39.29, -48.71 -35.39 -36.39, -48.9 -35.54 36.87, -46.75 -33.97 40.26, -44.6 -32.41 40.26, -46.36 -33.69 36.63, -46.36 -33.69 29.14, -30.71 -22.32 20.43, -24.45 -17.77 20.43, -24.45 -17.77 16.8, -30.51 -22.17 16.8, -46.55 -33.83 -28.65, -46.36 -33.69 -35.91, -46.36 -33.69 -39.05, -54.27 -17.64 -40.26, -52.66 -17.12 -40.26, -56.11 -18.24 -39.29, -57.26 -18.61 -36.39, -57.49 -18.69 36.87, -54.96 -17.86 40.26, -52.43 -17.04 40.26, -54.5 -17.71 36.63, -54.5 -17.71 29.14, -36.1 -11.73 20.43, -28.74 -9.343 20.43, -28.74 -9.343 16.8, -35.87 -11.66 16.8, -54.73 -17.79 -28.65, -54.5 -17.71 -35.91, -54.5 -17.71 -39.05 - ] - } - coordIndex [ - 0, 1, 2, -1, 2, 3, 0, -1, 4, 5, 1, -1, 1, 0, 4, -1, 6, 7, 5, -1, 5, 4, 6, -1, 8, 9, 7, -1, 7, 6, 8, -1, 10, 11, 9, -1, 9, 8, 10, -1, 12, 13, 11, -1, 11, 10, 12, -1, 14, 15, 13, -1, 13, 12, 14, -1, 16, 17, 15, -1, 15, 14, 16, -1, 18, 19, 17, -1, 17, 16, 18, -1, 20, 21, 19, -1, 19, 18, 20, -1, 22, 23, 21, -1, 21, 20, 22, -1, 24, 25, 23, -1, 23, 22, 24, -1, 26, 27, 25, -1, 25, 24, 26, -1, 28, 29, 27, -1, 27, 26, 28, -1, 30, 31, 29, -1, 29, 28, 30, -1, 3, 2, 31, -1, 31, 30, 3, -1, 1, 32, 33, -1, 33, 2, 1, -1, 5, 34, 32, -1, 32, 1, 5, -1, 7, 35, 34, -1, 34, 5, 7, -1, 9, 36, 35, -1, 35, 7, 9, -1, 11, 37, 36, -1, 36, 9, 11, -1, 13, 38, 37, -1, 37, 11, 13, -1, 15, 39, 38, -1, 38, 13, 15, -1, 17, 40, 39, -1, 39, 15, 17, -1, 19, 41, 40, -1, 40, 17, 19, -1, 21, 42, 41, -1, 41, 19, 21, -1, 23, 43, 42, -1, 42, 21, 23, -1, 25, 44, 43, -1, 43, 23, 25, -1, 27, 45, 44, -1, 44, 25, 27, -1, 29, 46, 45, -1, 45, 27, 29, -1, 31, 47, 46, -1, 46, 29, 31, -1, 2, 33, 47, -1, 47, 31, 2, -1, 32, 48, 49, -1, 49, 33, 32, -1, 34, 50, 48, -1, 48, 32, 34, -1, 35, 51, 50, -1, 50, 34, 35, -1, 36, 52, 51, -1, 51, 35, 36, -1, 37, 53, 52, -1, 52, 36, 37, -1, 38, 54, 53, -1, 53, 37, 38, -1, 39, 55, 54, -1, 54, 38, 39, -1, 40, 56, 55, -1, 55, 39, 40, -1, 41, 57, 56, -1, 56, 40, 41, -1, 42, 58, 57, -1, 57, 41, 42, -1, 43, 59, 58, -1, 58, 42, 43, -1, 44, 60, 59, -1, 59, 43, 44, -1, 45, 61, 60, -1, 60, 44, 45, -1, 46, 62, 61, -1, 61, 45, 46, -1, 47, 63, 62, -1, 62, 46, 47, -1, 33, 49, 63, -1, 63, 47, 33, -1, 48, 64, 65, -1, 65, 49, 48, -1, 50, 66, 64, -1, 64, 48, 50, -1, 51, 67, 66, -1, 66, 50, 51, -1, 52, 68, 67, -1, 67, 51, 52, -1, 53, 69, 68, -1, 68, 52, 53, -1, 54, 70, 69, -1, 69, 53, 54, -1, 55, 71, 70, -1, 70, 54, 55, -1, 56, 72, 71, -1, 71, 55, 56, -1, 57, 73, 72, -1, 72, 56, 57, -1, 58, 74, 73, -1, 73, 57, 58, -1, 59, 75, 74, -1, 74, 58, 59, -1, 60, 76, 75, -1, 75, 59, 60, -1, 61, 77, 76, -1, 76, 60, 61, -1, 62, 78, 77, -1, 77, 61, 62, -1, 63, 79, 78, -1, 78, 62, 63, -1, 49, 65, 79, -1, 79, 63, 49, -1, 64, 80, 81, -1, 81, 65, 64, -1, 66, 82, 80, -1, 80, 64, 66, -1, 67, 83, 82, -1, 82, 66, 67, -1, 68, 84, 83, -1, 83, 67, 68, -1, 69, 85, 84, -1, 84, 68, 69, -1, 70, 86, 85, -1, 85, 69, 70, -1, 71, 87, 86, -1, 86, 70, 71, -1, 72, 88, 87, -1, 87, 71, 72, -1, 73, 89, 88, -1, 88, 72, 73, -1, 74, 90, 89, -1, 89, 73, 74, -1, 75, 91, 90, -1, 90, 74, 75, -1, 76, 92, 91, -1, 91, 75, 76, -1, 77, 93, 92, -1, 92, 76, 77, -1, 78, 94, 93, -1, 93, 77, 78, -1, 79, 95, 94, -1, 94, 78, 79, -1, 65, 81, 95, -1, 95, 79, 65, -1, 80, 96, 97, -1, 97, 81, 80, -1, 82, 98, 96, -1, 96, 80, 82, -1, 83, 99, 98, -1, 98, 82, 83, -1, 84, 100, 99, -1, 99, 83, 84, -1, 85, 101, 100, -1, 100, 84, 85, -1, 86, 102, 101, -1, 101, 85, 86, -1, 87, 103, 102, -1, 102, 86, 87, -1, 88, 104, 103, -1, 103, 87, 88, -1, 89, 105, 104, -1, 104, 88, 89, -1, 90, 106, 105, -1, 105, 89, 90, -1, 91, 107, 106, -1, 106, 90, 91, -1, 92, 108, 107, -1, 107, 91, 92, -1, 93, 109, 108, -1, 108, 92, 93, -1, 94, 110, 109, -1, 109, 93, 94, -1, 95, 111, 110, -1, 110, 94, 95, -1, 81, 97, 111, -1, 111, 95, 81, -1, 96, 112, 113, -1, 113, 97, 96, -1, 98, 114, 112, -1, 112, 96, 98, -1, 99, 115, 114, -1, 114, 98, 99, -1, 100, 116, 115, -1, 115, 99, 100, -1, 101, 117, 116, -1, 116, 100, 101, -1, 102, 118, 117, -1, 117, 101, 102, -1, 103, 119, 118, -1, 118, 102, 103, -1, 104, 120, 119, -1, 119, 103, 104, -1, 105, 121, 120, -1, 120, 104, 105, -1, 106, 122, 121, -1, 121, 105, 106, -1, 107, 123, 122, -1, 122, 106, 107, -1, 108, 124, 123, -1, 123, 107, 108, -1, 109, 125, 124, -1, 124, 108, 109, -1, 110, 126, 125, -1, 125, 109, 110, -1, 111, 127, 126, -1, 126, 110, 111, -1, 97, 113, 127, -1, 127, 111, 97, -1, 112, 128, 129, -1, 129, 113, 112, -1, 114, 130, 128, -1, 128, 112, 114, -1, 115, 131, 130, -1, 130, 114, 115, -1, 116, 132, 131, -1, 131, 115, 116, -1, 117, 133, 132, -1, 132, 116, 117, -1, 118, 134, 133, -1, 133, 117, 118, -1, 119, 135, 134, -1, 134, 118, 119, -1, 120, 136, 135, -1, 135, 119, 120, -1, 121, 137, 136, -1, 136, 120, 121, -1, 122, 138, 137, -1, 137, 121, 122, -1, 123, 139, 138, -1, 138, 122, 123, -1, 124, 140, 139, -1, 139, 123, 124, -1, 125, 141, 140, -1, 140, 124, 125, -1, 126, 142, 141, -1, 141, 125, 126, -1, 127, 143, 142, -1, 142, 126, 127, -1, 113, 129, 143, -1, 143, 127, 113, -1, 128, 144, 145, -1, 145, 129, 128, -1, 130, 146, 144, -1, 144, 128, 130, -1, 131, 147, 146, -1, 146, 130, 131, -1, 132, 148, 147, -1, 147, 131, 132, -1, 133, 149, 148, -1, 148, 132, 133, -1, 134, 150, 149, -1, 149, 133, 134, -1, 135, 151, 150, -1, 150, 134, 135, -1, 136, 152, 151, -1, 151, 135, 136, -1, 137, 153, 152, -1, 152, 136, 137, -1, 138, 154, 153, -1, 153, 137, 138, -1, 139, 155, 154, -1, 154, 138, 139, -1, 140, 156, 155, -1, 155, 139, 140, -1, 141, 157, 156, -1, 156, 140, 141, -1, 142, 158, 157, -1, 157, 141, 142, -1, 143, 159, 158, -1, 158, 142, 143, -1, 129, 145, 159, -1, 159, 143, 129, -1, 144, 160, 161, -1, 161, 145, 144, -1, 146, 162, 160, -1, 160, 144, 146, -1, 147, 163, 162, -1, 162, 146, 147, -1, 148, 164, 163, -1, 163, 147, 148, -1, 149, 165, 164, -1, 164, 148, 149, -1, 150, 166, 165, -1, 165, 149, 150, -1, 151, 167, 166, -1, 166, 150, 151, -1, 152, 168, 167, -1, 167, 151, 152, -1, 153, 169, 168, -1, 168, 152, 153, -1, 154, 170, 169, -1, 169, 153, 154, -1, 155, 171, 170, -1, 170, 154, 155, -1, 156, 172, 171, -1, 171, 155, 156, -1, 157, 173, 172, -1, 172, 156, 157, -1, 158, 174, 173, -1, 173, 157, 158, -1, 159, 175, 174, -1, 174, 158, 159, -1, 145, 161, 175, -1, 175, 159, 145, -1, 160, 176, 177, -1, 177, 161, 160, -1, 162, 178, 176, -1, 176, 160, 162, -1, 163, 179, 178, -1, 178, 162, 163, -1, 164, 180, 179, -1, 179, 163, 164, -1, 165, 181, 180, -1, 180, 164, 165, -1, 166, 182, 181, -1, 181, 165, 166, -1, 167, 183, 182, -1, 182, 166, 167, -1, 168, 184, 183, -1, 183, 167, 168, -1, 169, 185, 184, -1, 184, 168, 169, -1, 170, 186, 185, -1, 185, 169, 170, -1, 171, 187, 186, -1, 186, 170, 171, -1, 172, 188, 187, -1, 187, 171, 172, -1, 173, 189, 188, -1, 188, 172, 173, -1, 174, 190, 189, -1, 189, 173, 174, -1, 175, 191, 190, -1, 190, 174, 175, -1, 161, 177, 191, -1, 191, 175, 161, -1, 176, 192, 193, -1, 193, 177, 176, -1, 178, 194, 192, -1, 192, 176, 178, -1, 179, 195, 194, -1, 194, 178, 179, -1, 180, 196, 195, -1, 195, 179, 180, -1, 181, 197, 196, -1, 196, 180, 181, -1, 182, 198, 197, -1, 197, 181, 182, -1, 183, 199, 198, -1, 198, 182, 183, -1, 184, 200, 199, -1, 199, 183, 184, -1, 185, 201, 200, -1, 200, 184, 185, -1, 186, 202, 201, -1, 201, 185, 186, -1, 187, 203, 202, -1, 202, 186, 187, -1, 188, 204, 203, -1, 203, 187, 188, -1, 189, 205, 204, -1, 204, 188, 189, -1, 190, 206, 205, -1, 205, 189, 190, -1, 191, 207, 206, -1, 206, 190, 191, -1, 177, 193, 207, -1, 207, 191, 177, -1, 192, 208, 209, -1, 209, 193, 192, -1, 194, 210, 208, -1, 208, 192, 194, -1, 195, 211, 210, -1, 210, 194, 195, -1, 196, 212, 211, -1, 211, 195, 196, -1, 197, 213, 212, -1, 212, 196, 197, -1, 198, 214, 213, -1, 213, 197, 198, -1, 199, 215, 214, -1, 214, 198, 199, -1, 200, 216, 215, -1, 215, 199, 200, -1, 201, 217, 216, -1, 216, 200, 201, -1, 202, 218, 217, -1, 217, 201, 202, -1, 203, 219, 218, -1, 218, 202, 203, -1, 204, 220, 219, -1, 219, 203, 204, -1, 205, 221, 220, -1, 220, 204, 205, -1, 206, 222, 221, -1, 221, 205, 206, -1, 207, 223, 222, -1, 222, 206, 207, -1, 193, 209, 223, -1, 223, 207, 193, -1, 208, 224, 225, -1, 225, 209, 208, -1, 210, 226, 224, -1, 224, 208, 210, -1, 211, 227, 226, -1, 226, 210, 211, -1, 212, 228, 227, -1, 227, 211, 212, -1, 213, 229, 228, -1, 228, 212, 213, -1, 214, 230, 229, -1, 229, 213, 214, -1, 215, 231, 230, -1, 230, 214, 215, -1, 216, 232, 231, -1, 231, 215, 216, -1, 217, 233, 232, -1, 232, 216, 217, -1, 218, 234, 233, -1, 233, 217, 218, -1, 219, 235, 234, -1, 234, 218, 219, -1, 220, 236, 235, -1, 235, 219, 220, -1, 221, 237, 236, -1, 236, 220, 221, -1, 222, 238, 237, -1, 237, 221, 222, -1, 223, 239, 238, -1, 238, 222, 223, -1, 209, 225, 239, -1, 239, 223, 209, -1, 224, 240, 241, -1, 241, 225, 224, -1, 226, 242, 240, -1, 240, 224, 226, -1, 227, 243, 242, -1, 242, 226, 227, -1, 228, 244, 243, -1, 243, 227, 228, -1, 229, 245, 244, -1, 244, 228, 229, -1, 230, 246, 245, -1, 245, 229, 230, -1, 231, 247, 246, -1, 246, 230, 231, -1, 232, 248, 247, -1, 247, 231, 232, -1, 233, 249, 248, -1, 248, 232, 233, -1, 234, 250, 249, -1, 249, 233, 234, -1, 235, 251, 250, -1, 250, 234, 235, -1, 236, 252, 251, -1, 251, 235, 236, -1, 237, 253, 252, -1, 252, 236, 237, -1, 238, 254, 253, -1, 253, 237, 238, -1, 239, 255, 254, -1, 254, 238, 239, -1, 225, 241, 255, -1, 255, 239, 225, -1, 240, 256, 257, -1, 257, 241, 240, -1, 242, 258, 256, -1, 256, 240, 242, -1, 243, 259, 258, -1, 258, 242, 243, -1, 244, 260, 259, -1, 259, 243, 244, -1, 245, 261, 260, -1, 260, 244, 245, -1, 246, 262, 261, -1, 261, 245, 246, -1, 247, 263, 262, -1, 262, 246, 247, -1, 248, 264, 263, -1, 263, 247, 248, -1, 249, 265, 264, -1, 264, 248, 249, -1, 250, 266, 265, -1, 265, 249, 250, -1, 251, 267, 266, -1, 266, 250, 251, -1, 252, 268, 267, -1, 267, 251, 252, -1, 253, 269, 268, -1, 268, 252, 253, -1, 254, 270, 269, -1, 269, 253, 254, -1, 255, 271, 270, -1, 270, 254, 255, -1, 241, 257, 271, -1, 271, 255, 241, -1, 256, 272, 273, -1, 273, 257, 256, -1, 258, 274, 272, -1, 272, 256, 258, -1, 259, 275, 274, -1, 274, 258, 259, -1, 260, 276, 275, -1, 275, 259, 260, -1, 261, 277, 276, -1, 276, 260, 261, -1, 262, 278, 277, -1, 277, 261, 262, -1, 263, 279, 278, -1, 278, 262, 263, -1, 264, 280, 279, -1, 279, 263, 264, -1, 265, 281, 280, -1, 280, 264, 265, -1, 266, 282, 281, -1, 281, 265, 266, -1, 267, 283, 282, -1, 282, 266, 267, -1, 268, 284, 283, -1, 283, 267, 268, -1, 269, 285, 284, -1, 284, 268, 269, -1, 270, 286, 285, -1, 285, 269, 270, -1, 271, 287, 286, -1, 286, 270, 271, -1, 257, 273, 287, -1, 287, 271, 257, -1, 272, 288, 289, -1, 289, 273, 272, -1, 274, 290, 288, -1, 288, 272, 274, -1, 275, 291, 290, -1, 290, 274, 275, -1, 276, 292, 291, -1, 291, 275, 276, -1, 277, 293, 292, -1, 292, 276, 277, -1, 278, 294, 293, -1, 293, 277, 278, -1, 279, 295, 294, -1, 294, 278, 279, -1, 280, 296, 295, -1, 295, 279, 280, -1, 281, 297, 296, -1, 296, 280, 281, -1, 282, 298, 297, -1, 297, 281, 282, -1, 283, 299, 298, -1, 298, 282, 283, -1, 284, 300, 299, -1, 299, 283, 284, -1, 285, 301, 300, -1, 300, 284, 285, -1, 286, 302, 301, -1, 301, 285, 286, -1, 287, 303, 302, -1, 302, 286, 287, -1, 273, 289, 303, -1, 303, 287, 273, -1, 288, 304, 305, -1, 305, 289, 288, -1, 290, 306, 304, -1, 304, 288, 290, -1, 291, 307, 306, -1, 306, 290, 291, -1, 292, 308, 307, -1, 307, 291, 292, -1, 293, 309, 308, -1, 308, 292, 293, -1, 294, 310, 309, -1, 309, 293, 294, -1, 295, 311, 310, -1, 310, 294, 295, -1, 296, 312, 311, -1, 311, 295, 296, -1, 297, 313, 312, -1, 312, 296, 297, -1, 298, 314, 313, -1, 313, 297, 298, -1, 299, 315, 314, -1, 314, 298, 299, -1, 300, 316, 315, -1, 315, 299, 300, -1, 301, 317, 316, -1, 316, 300, 301, -1, 302, 318, 317, -1, 317, 301, 302, -1, 303, 319, 318, -1, 318, 302, 303, -1, 289, 305, 319, -1, 319, 303, 289, -1, 304, 0, 3, -1, 3, 305, 304, -1, 306, 4, 0, -1, 0, 304, 306, -1, 307, 6, 4, -1, 4, 306, 307, -1, 308, 8, 6, -1, 6, 307, 308, -1, 309, 10, 8, -1, 8, 308, 309, -1, 310, 12, 10, -1, 10, 309, 310, -1, 311, 14, 12, -1, 12, 310, 311, -1, 312, 16, 14, -1, 14, 311, 312, -1, 313, 18, 16, -1, 16, 312, 313, -1, 314, 20, 18, -1, 18, 313, 314, -1, 315, 22, 20, -1, 20, 314, 315, -1, 316, 24, 22, -1, 22, 315, 316, -1, 317, 26, 24, -1, 24, 316, 317, -1, 318, 28, 26, -1, 26, 317, 318, -1, 319, 30, 28, -1, 28, 318, 319, -1, 305, 3, 30, -1, 30, 319, 305, -1 - ] - creaseAngle 1 - } - } - ] - } - ] - name "MiddleLeftWheel" - boundingObject USE BOUND_WHEEL - physics USE WHEEL_PHYSICS - } - } ] boundingObject Pose { translation 0.08 0 0 @@ -880,65 +821,6 @@ PROTO SlugRobot [ } } } - HingeJoint { - jointParameters HingeJointParameters { - axis 0 0 -1 - anchor -0.087 -0.078 -0.017 - } - device [ - RotationalMotor { - name "MiddleRightWheel" - maxVelocity 25 - maxTorque 1.0 - } - ] - endPoint Solid { - translation -0.087 -0.078 -0.017 - children [ - DEF WHEEL_MIDDLERIGHT Transform { - scale 0.001 0.001 0.001 - children [ - Shape { - appearance USE METAL_APPEARANCE - geometry IndexedFaceSet { - coord Coordinate { - point [ - -57.06 0.004028 40.26, -54.27 -17.63 40.26, -52.66 -17.11 40.26, -55.37 0.004028 40.26, -59 0.004028 39.29, -56.11 -18.23 39.29, -60.21 0.003906 36.39, -57.26 -18.6 36.39, -60.45 0 -36.87, -57.49 -18.68 -36.87, -57.79 -0.000122 -40.26, -54.96 -17.86 -40.26, -55.13 -0.000122 -40.26, -52.43 -17.04 -40.26, -57.31 0 -36.63, -54.5 -17.71 -36.63, -57.31 0.000366 -29.14, -54.5 -17.71 -29.14, -37.96 0.000854 -20.43, -36.1 -11.73 -20.43, -30.22 0.000854 -20.43, -28.75 -9.339 -20.43, -30.22 0.001099 -16.8, -28.75 -9.339 -16.8, -37.72 0.001099 -16.8, -35.87 -11.65 -16.8, -57.55 0.003418 28.65, -54.73 -17.78 28.65, -57.31 0.003784 35.91, -54.5 -17.7 35.91, -57.31 0.004028 39.05, -54.5 -17.7 39.05, -46.17 -33.54 40.26, -44.8 -32.54 40.26, -47.73 -34.67 39.29, -48.71 -35.38 36.39, -48.9 -35.53 -36.87, -46.75 -33.97 -40.26, -44.6 -32.4 -40.26, -46.36 -33.68 -36.63, -46.36 -33.68 -29.13, -30.71 -22.31 -20.43, -24.45 -17.76 -20.43, -24.45 -17.76 -16.8, -30.52 -22.17 -16.8, -46.56 -33.82 28.65, -46.36 -33.68 35.91, -46.36 -33.68 39.05, -33.54 -46.16 40.26, -32.55 -44.79 40.26, -34.68 -47.73 39.29, -35.39 -48.7 36.39, -35.53 -48.9 -36.87, -33.97 -46.75 -40.26, -32.41 -44.6 -40.26, -33.68 -46.36 -36.63, -33.68 -46.36 -29.13, -22.31 -30.71 -20.43, -17.77 -24.45 -20.43, -17.77 -24.45 -16.8, -22.17 -30.51 -16.8, -33.83 -46.55 28.66, -33.68 -46.36 35.91, -33.68 -46.36 39.05, -17.63 -54.27 40.26, -17.11 -52.66 40.26, -18.23 -56.11 39.29, -18.61 -57.26 36.39, -18.68 -57.49 -36.87, -17.86 -54.96 -40.26, -17.04 -52.43 -40.26, -17.71 -54.5 -36.63, -17.71 -54.5 -29.13, -11.73 -36.1 -20.43, -9.341 -28.74 -20.43, -9.341 -28.74 -16.8, -11.66 -35.87 -16.8, -17.78 -54.73 28.66, -17.71 -54.5 35.91, -17.71 -54.5 39.05, -0.001528 -57.06 40.26, -0.001497 -55.37 40.26, -0.001589 -58.99 39.29, -0.001619 -60.2 36.39, -0.001619 -60.45 -36.87, -0.001558 -57.79 -40.26, -0.001497 -55.13 -40.26, -0.001528 -57.31 -36.63, -0.001528 -57.31 -29.13, -0.00104 -37.96 -20.43, -0.000826 -30.22 -20.43, -0.000826 -30.22 -16.8, -0.00104 -37.72 -16.8, -0.001558 -57.54 28.66, -0.001528 -57.3 35.91, -0.001528 -57.3 39.05, 17.63 -54.27 40.26, 17.11 -52.66 40.26, 18.23 -56.11 39.29, 18.6 -57.26 36.39, 18.68 -57.49 -36.87, 17.86 -54.96 -40.26, 17.03 -52.43 -40.26, 17.71 -54.5 -36.63, 17.71 -54.5 -29.13, 11.73 -36.1 -20.43, 9.339 -28.74 -20.43, 9.339 -28.74 -16.8, 11.66 -35.87 -16.8, 17.78 -54.73 28.66, 17.71 -54.5 35.91, 17.71 -54.5 39.05, 33.54 -46.16 40.26, 32.54 -44.79 40.26, 34.68 -47.73 39.29, 35.39 -48.71 36.39, 35.53 -48.91 -36.87, 33.97 -46.75 -40.26, 32.4 -44.6 -40.26, 33.68 -46.36 -36.63, 33.68 -46.36 -29.13, 22.31 -30.71 -20.43, 17.76 -24.45 -20.43, 17.76 -24.45 -16.8, 22.17 -30.52 -16.8, 33.82 -46.55 28.66, 33.68 -46.36 35.91, 33.68 -46.36 39.05, 46.16 -33.54 40.26, 44.8 -32.54 40.26, 47.73 -34.68 39.29, 48.71 -35.39 36.39, 48.9 -35.53 -36.87, 46.75 -33.97 -40.26, 44.6 -32.41 -40.26, 46.36 -33.69 -36.63, 46.36 -33.69 -29.13, 30.71 -22.31 -20.43, 24.45 -17.77 -20.43, 24.45 -17.77 -16.8, 30.52 -22.17 -16.8, 46.56 -33.82 28.65, 46.36 -33.68 35.91, 46.36 -33.68 39.05, 54.27 -17.63 40.26, 52.66 -17.11 40.26, 56.11 -18.23 39.29, 57.26 -18.6 36.39, 57.49 -18.68 -36.87, 54.96 -17.86 -40.26, 52.43 -17.04 -40.26, 54.5 -17.71 -36.63, 54.5 -17.71 -29.14, 36.1 -11.73 -20.43, 28.74 -9.34 -20.43, 28.74 -9.34 -16.8, 35.87 -11.66 -16.8, 54.73 -17.78 28.65, 54.5 -17.71 35.91, 54.5 -17.71 39.05, 57.06 0.000977 40.26, 55.37 0.001099 40.26, 59 0.000854 39.29, 60.21 0.000732 36.39, 60.45 -0.003174 -36.87, 57.79 -0.003174 -40.26, 55.13 -0.003052 -40.26, 57.31 -0.003052 -36.63, 57.31 -0.002686 -29.14, 37.96 -0.001221 -20.43, 30.22 -0.000732 -20.43, 30.22 -0.000488 -16.8, 37.72 -0.000977 -16.8, 57.55 0.000366 28.65, 57.31 0.000732 35.91, 57.31 0.000977 39.05, 54.27 17.63 40.26, 52.66 17.11 40.26, 56.11 18.23 39.29, 57.26 18.61 36.39, 57.49 18.68 -36.88, 54.96 17.85 -40.26, 52.43 17.03 -40.26, 54.5 17.71 -36.63, 54.5 17.71 -29.14, 36.1 11.73 -20.43, 28.75 9.339 -20.43, 28.75 9.339 -16.81, 35.87 11.66 -16.81, 54.73 17.78 28.65, 54.5 17.71 35.91, 54.5 17.71 39.05, 46.17 33.54 40.26, 44.8 32.55 40.26, 47.73 34.68 39.29, 48.71 35.39 36.39, 48.91 35.53 -36.88, 46.75 33.96 -40.26, 44.6 32.4 -40.26, 46.36 33.68 -36.63, 46.36 33.68 -29.14, 30.71 22.31 -20.43, 24.45 17.76 -20.43, 24.45 17.77 -16.81, 30.52 22.17 -16.81, 46.56 33.83 28.65, 46.36 33.68 35.9, 46.36 33.68 39.05, 33.54 46.17 40.26, 32.55 44.8 40.26, 34.68 47.73 39.29, 35.39 48.71 36.39, 35.53 48.9 -36.88, 33.97 46.75 -40.26, 32.41 44.6 -40.26, 33.69 46.36 -36.63, 33.69 46.36 -29.14, 22.32 30.71 -20.43, 17.77 24.45 -20.43, 17.77 24.45 -16.81, 22.17 30.52 -16.81, 33.83 46.56 28.65, 33.69 46.36 35.9, 33.69 46.36 39.05, 17.64 54.27 40.26, 17.11 52.66 40.26, 18.24 56.11 39.29, 18.61 57.26 36.39, 18.68 57.49 -36.88, 17.86 54.96 -40.26, 17.04 52.43 -40.26, 17.71 54.5 -36.63, 17.71 54.5 -29.14, 11.73 36.1 -20.43, 9.342 28.75 -20.43, 9.342 28.75 -16.81, 11.66 35.87 -16.81, 17.79 54.73 28.65, 17.71 54.5 35.9, 17.71 54.5 39.05, 0.004545 57.07 40.26, 0.004423 55.38 40.26, 0.004698 59 39.29, 0.004789 60.21 36.39, 0.00482 60.45 -36.88, 0.004606 57.79 -40.26, 0.004393 55.13 -40.26, 0.004576 57.31 -36.64, 0.004576 57.31 -29.14, 0.003019 37.96 -20.43, 0.002409 30.23 -20.43, 0.002409 30.23 -16.81, 0.003019 37.72 -16.81, 0.004576 57.55 28.65, 0.004576 57.31 35.9, 0.004576 57.31 39.05, -17.63 54.28 40.26, -17.11 52.67 40.26, -18.23 56.12 39.29, -18.6 57.27 36.39, -18.67 57.49 -36.88, -17.85 54.96 -40.26, -17.03 52.43 -40.26, -17.7 54.5 -36.63, -17.7 54.5 -29.14, -11.73 36.11 -20.43, -9.337 28.75 -20.43, -9.337 28.75 -16.81, -11.65 35.88 -16.81, -17.78 54.74 28.65, -17.7 54.51 35.9, -17.7 54.51 39.05, -33.54 46.17 40.26, -32.54 44.8 40.26, -34.67 47.74 39.29, -35.38 48.72 36.39, -35.53 48.91 -36.88, -33.96 46.76 -40.26, -32.4 44.6 -40.26, -33.68 46.36 -36.63, -33.68 46.36 -29.14, -22.31 30.71 -20.43, -17.76 24.45 -20.43, -17.76 24.45 -16.81, -22.17 30.52 -16.81, -33.82 46.56 28.65, -33.68 46.37 35.9, -33.68 46.37 39.05, -46.16 33.55 40.26, -44.79 32.55 40.26, -47.73 34.69 39.29, -48.71 35.4 36.39, -48.9 35.54 -36.88, -46.75 33.97 -40.26, -44.6 32.41 -40.26, -46.36 33.69 -36.63, -46.36 33.69 -29.14, -30.71 22.32 -20.43, -24.45 17.77 -20.43, -24.45 17.77 -16.81, -30.51 22.18 -16.81, -46.55 33.83 28.65, -46.36 33.69 35.9, -46.36 33.69 39.05, -54.27 17.64 40.26, -52.66 17.12 40.26, -56.11 18.24 39.29, -57.26 18.61 36.39, -57.49 18.69 -36.88, -54.96 17.86 -40.26, -52.43 17.04 -40.26, -54.5 17.71 -36.63, -54.5 17.71 -29.14, -36.1 11.74 -20.43, -28.74 9.344 -20.43, -28.74 9.344 -16.81, -35.87 11.66 -16.81, -54.73 17.79 28.65, -54.5 17.72 35.91, -54.5 17.72 39.05 - ] - } - coordIndex [ - 0, 1, 2, -1, 2, 3, 0, -1, 4, 5, 1, -1, 1, 0, 4, -1, 6, 7, 5, -1, 5, 4, 6, -1, 8, 9, 7, -1, 7, 6, 8, -1, 10, 11, 9, -1, 9, 8, 10, -1, 12, 13, 11, -1, 11, 10, 12, -1, 14, 15, 13, -1, 13, 12, 14, -1, 16, 17, 15, -1, 15, 14, 16, -1, 18, 19, 17, -1, 17, 16, 18, -1, 20, 21, 19, -1, 19, 18, 20, -1, 22, 23, 21, -1, 21, 20, 22, -1, 24, 25, 23, -1, 23, 22, 24, -1, 26, 27, 25, -1, 25, 24, 26, -1, 28, 29, 27, -1, 27, 26, 28, -1, 30, 31, 29, -1, 29, 28, 30, -1, 3, 2, 31, -1, 31, 30, 3, -1, 1, 32, 33, -1, 33, 2, 1, -1, 5, 34, 32, -1, 32, 1, 5, -1, 7, 35, 34, -1, 34, 5, 7, -1, 9, 36, 35, -1, 35, 7, 9, -1, 11, 37, 36, -1, 36, 9, 11, -1, 13, 38, 37, -1, 37, 11, 13, -1, 15, 39, 38, -1, 38, 13, 15, -1, 17, 40, 39, -1, 39, 15, 17, -1, 19, 41, 40, -1, 40, 17, 19, -1, 21, 42, 41, -1, 41, 19, 21, -1, 23, 43, 42, -1, 42, 21, 23, -1, 25, 44, 43, -1, 43, 23, 25, -1, 27, 45, 44, -1, 44, 25, 27, -1, 29, 46, 45, -1, 45, 27, 29, -1, 31, 47, 46, -1, 46, 29, 31, -1, 2, 33, 47, -1, 47, 31, 2, -1, 32, 48, 49, -1, 49, 33, 32, -1, 34, 50, 48, -1, 48, 32, 34, -1, 35, 51, 50, -1, 50, 34, 35, -1, 36, 52, 51, -1, 51, 35, 36, -1, 37, 53, 52, -1, 52, 36, 37, -1, 38, 54, 53, -1, 53, 37, 38, -1, 39, 55, 54, -1, 54, 38, 39, -1, 40, 56, 55, -1, 55, 39, 40, -1, 41, 57, 56, -1, 56, 40, 41, -1, 42, 58, 57, -1, 57, 41, 42, -1, 43, 59, 58, -1, 58, 42, 43, -1, 44, 60, 59, -1, 59, 43, 44, -1, 45, 61, 60, -1, 60, 44, 45, -1, 46, 62, 61, -1, 61, 45, 46, -1, 47, 63, 62, -1, 62, 46, 47, -1, 33, 49, 63, -1, 63, 47, 33, -1, 48, 64, 65, -1, 65, 49, 48, -1, 50, 66, 64, -1, 64, 48, 50, -1, 51, 67, 66, -1, 66, 50, 51, -1, 52, 68, 67, -1, 67, 51, 52, -1, 53, 69, 68, -1, 68, 52, 53, -1, 54, 70, 69, -1, 69, 53, 54, -1, 55, 71, 70, -1, 70, 54, 55, -1, 56, 72, 71, -1, 71, 55, 56, -1, 57, 73, 72, -1, 72, 56, 57, -1, 58, 74, 73, -1, 73, 57, 58, -1, 59, 75, 74, -1, 74, 58, 59, -1, 60, 76, 75, -1, 75, 59, 60, -1, 61, 77, 76, -1, 76, 60, 61, -1, 62, 78, 77, -1, 77, 61, 62, -1, 63, 79, 78, -1, 78, 62, 63, -1, 49, 65, 79, -1, 79, 63, 49, -1, 64, 80, 81, -1, 81, 65, 64, -1, 66, 82, 80, -1, 80, 64, 66, -1, 67, 83, 82, -1, 82, 66, 67, -1, 68, 84, 83, -1, 83, 67, 68, -1, 69, 85, 84, -1, 84, 68, 69, -1, 70, 86, 85, -1, 85, 69, 70, -1, 71, 87, 86, -1, 86, 70, 71, -1, 72, 88, 87, -1, 87, 71, 72, -1, 73, 89, 88, -1, 88, 72, 73, -1, 74, 90, 89, -1, 89, 73, 74, -1, 75, 91, 90, -1, 90, 74, 75, -1, 76, 92, 91, -1, 91, 75, 76, -1, 77, 93, 92, -1, 92, 76, 77, -1, 78, 94, 93, -1, 93, 77, 78, -1, 79, 95, 94, -1, 94, 78, 79, -1, 65, 81, 95, -1, 95, 79, 65, -1, 80, 96, 97, -1, 97, 81, 80, -1, 82, 98, 96, -1, 96, 80, 82, -1, 83, 99, 98, -1, 98, 82, 83, -1, 84, 100, 99, -1, 99, 83, 84, -1, 85, 101, 100, -1, 100, 84, 85, -1, 86, 102, 101, -1, 101, 85, 86, -1, 87, 103, 102, -1, 102, 86, 87, -1, 88, 104, 103, -1, 103, 87, 88, -1, 89, 105, 104, -1, 104, 88, 89, -1, 90, 106, 105, -1, 105, 89, 90, -1, 91, 107, 106, -1, 106, 90, 91, -1, 92, 108, 107, -1, 107, 91, 92, -1, 93, 109, 108, -1, 108, 92, 93, -1, 94, 110, 109, -1, 109, 93, 94, -1, 95, 111, 110, -1, 110, 94, 95, -1, 81, 97, 111, -1, 111, 95, 81, -1, 96, 112, 113, -1, 113, 97, 96, -1, 98, 114, 112, -1, 112, 96, 98, -1, 99, 115, 114, -1, 114, 98, 99, -1, 100, 116, 115, -1, 115, 99, 100, -1, 101, 117, 116, -1, 116, 100, 101, -1, 102, 118, 117, -1, 117, 101, 102, -1, 103, 119, 118, -1, 118, 102, 103, -1, 104, 120, 119, -1, 119, 103, 104, -1, 105, 121, 120, -1, 120, 104, 105, -1, 106, 122, 121, -1, 121, 105, 106, -1, 107, 123, 122, -1, 122, 106, 107, -1, 108, 124, 123, -1, 123, 107, 108, -1, 109, 125, 124, -1, 124, 108, 109, -1, 110, 126, 125, -1, 125, 109, 110, -1, 111, 127, 126, -1, 126, 110, 111, -1, 97, 113, 127, -1, 127, 111, 97, -1, 112, 128, 129, -1, 129, 113, 112, -1, 114, 130, 128, -1, 128, 112, 114, -1, 115, 131, 130, -1, 130, 114, 115, -1, 116, 132, 131, -1, 131, 115, 116, -1, 117, 133, 132, -1, 132, 116, 117, -1, 118, 134, 133, -1, 133, 117, 118, -1, 119, 135, 134, -1, 134, 118, 119, -1, 120, 136, 135, -1, 135, 119, 120, -1, 121, 137, 136, -1, 136, 120, 121, -1, 122, 138, 137, -1, 137, 121, 122, -1, 123, 139, 138, -1, 138, 122, 123, -1, 124, 140, 139, -1, 139, 123, 124, -1, 125, 141, 140, -1, 140, 124, 125, -1, 126, 142, 141, -1, 141, 125, 126, -1, 127, 143, 142, -1, 142, 126, 127, -1, 113, 129, 143, -1, 143, 127, 113, -1, 128, 144, 145, -1, 145, 129, 128, -1, 130, 146, 144, -1, 144, 128, 130, -1, 131, 147, 146, -1, 146, 130, 131, -1, 132, 148, 147, -1, 147, 131, 132, -1, 133, 149, 148, -1, 148, 132, 133, -1, 134, 150, 149, -1, 149, 133, 134, -1, 135, 151, 150, -1, 150, 134, 135, -1, 136, 152, 151, -1, 151, 135, 136, -1, 137, 153, 152, -1, 152, 136, 137, -1, 138, 154, 153, -1, 153, 137, 138, -1, 139, 155, 154, -1, 154, 138, 139, -1, 140, 156, 155, -1, 155, 139, 140, -1, 141, 157, 156, -1, 156, 140, 141, -1, 142, 158, 157, -1, 157, 141, 142, -1, 143, 159, 158, -1, 158, 142, 143, -1, 129, 145, 159, -1, 159, 143, 129, -1, 144, 160, 161, -1, 161, 145, 144, -1, 146, 162, 160, -1, 160, 144, 146, -1, 147, 163, 162, -1, 162, 146, 147, -1, 148, 164, 163, -1, 163, 147, 148, -1, 149, 165, 164, -1, 164, 148, 149, -1, 150, 166, 165, -1, 165, 149, 150, -1, 151, 167, 166, -1, 166, 150, 151, -1, 152, 168, 167, -1, 167, 151, 152, -1, 153, 169, 168, -1, 168, 152, 153, -1, 154, 170, 169, -1, 169, 153, 154, -1, 155, 171, 170, -1, 170, 154, 155, -1, 156, 172, 171, -1, 171, 155, 156, -1, 157, 173, 172, -1, 172, 156, 157, -1, 158, 174, 173, -1, 173, 157, 158, -1, 159, 175, 174, -1, 174, 158, 159, -1, 145, 161, 175, -1, 175, 159, 145, -1, 160, 176, 177, -1, 177, 161, 160, -1, 162, 178, 176, -1, 176, 160, 162, -1, 163, 179, 178, -1, 178, 162, 163, -1, 164, 180, 179, -1, 179, 163, 164, -1, 165, 181, 180, -1, 180, 164, 165, -1, 166, 182, 181, -1, 181, 165, 166, -1, 167, 183, 182, -1, 182, 166, 167, -1, 168, 184, 183, -1, 183, 167, 168, -1, 169, 185, 184, -1, 184, 168, 169, -1, 170, 186, 185, -1, 185, 169, 170, -1, 171, 187, 186, -1, 186, 170, 171, -1, 172, 188, 187, -1, 187, 171, 172, -1, 173, 189, 188, -1, 188, 172, 173, -1, 174, 190, 189, -1, 189, 173, 174, -1, 175, 191, 190, -1, 190, 174, 175, -1, 161, 177, 191, -1, 191, 175, 161, -1, 176, 192, 193, -1, 193, 177, 176, -1, 178, 194, 192, -1, 192, 176, 178, -1, 179, 195, 194, -1, 194, 178, 179, -1, 180, 196, 195, -1, 195, 179, 180, -1, 181, 197, 196, -1, 196, 180, 181, -1, 182, 198, 197, -1, 197, 181, 182, -1, 183, 199, 198, -1, 198, 182, 183, -1, 184, 200, 199, -1, 199, 183, 184, -1, 185, 201, 200, -1, 200, 184, 185, -1, 186, 202, 201, -1, 201, 185, 186, -1, 187, 203, 202, -1, 202, 186, 187, -1, 188, 204, 203, -1, 203, 187, 188, -1, 189, 205, 204, -1, 204, 188, 189, -1, 190, 206, 205, -1, 205, 189, 190, -1, 191, 207, 206, -1, 206, 190, 191, -1, 177, 193, 207, -1, 207, 191, 177, -1, 192, 208, 209, -1, 209, 193, 192, -1, 194, 210, 208, -1, 208, 192, 194, -1, 195, 211, 210, -1, 210, 194, 195, -1, 196, 212, 211, -1, 211, 195, 196, -1, 197, 213, 212, -1, 212, 196, 197, -1, 198, 214, 213, -1, 213, 197, 198, -1, 199, 215, 214, -1, 214, 198, 199, -1, 200, 216, 215, -1, 215, 199, 200, -1, 201, 217, 216, -1, 216, 200, 201, -1, 202, 218, 217, -1, 217, 201, 202, -1, 203, 219, 218, -1, 218, 202, 203, -1, 204, 220, 219, -1, 219, 203, 204, -1, 205, 221, 220, -1, 220, 204, 205, -1, 206, 222, 221, -1, 221, 205, 206, -1, 207, 223, 222, -1, 222, 206, 207, -1, 193, 209, 223, -1, 223, 207, 193, -1, 208, 224, 225, -1, 225, 209, 208, -1, 210, 226, 224, -1, 224, 208, 210, -1, 211, 227, 226, -1, 226, 210, 211, -1, 212, 228, 227, -1, 227, 211, 212, -1, 213, 229, 228, -1, 228, 212, 213, -1, 214, 230, 229, -1, 229, 213, 214, -1, 215, 231, 230, -1, 230, 214, 215, -1, 216, 232, 231, -1, 231, 215, 216, -1, 217, 233, 232, -1, 232, 216, 217, -1, 218, 234, 233, -1, 233, 217, 218, -1, 219, 235, 234, -1, 234, 218, 219, -1, 220, 236, 235, -1, 235, 219, 220, -1, 221, 237, 236, -1, 236, 220, 221, -1, 222, 238, 237, -1, 237, 221, 222, -1, 223, 239, 238, -1, 238, 222, 223, -1, 209, 225, 239, -1, 239, 223, 209, -1, 224, 240, 241, -1, 241, 225, 224, -1, 226, 242, 240, -1, 240, 224, 226, -1, 227, 243, 242, -1, 242, 226, 227, -1, 228, 244, 243, -1, 243, 227, 228, -1, 229, 245, 244, -1, 244, 228, 229, -1, 230, 246, 245, -1, 245, 229, 230, -1, 231, 247, 246, -1, 246, 230, 231, -1, 232, 248, 247, -1, 247, 231, 232, -1, 233, 249, 248, -1, 248, 232, 233, -1, 234, 250, 249, -1, 249, 233, 234, -1, 235, 251, 250, -1, 250, 234, 235, -1, 236, 252, 251, -1, 251, 235, 236, -1, 237, 253, 252, -1, 252, 236, 237, -1, 238, 254, 253, -1, 253, 237, 238, -1, 239, 255, 254, -1, 254, 238, 239, -1, 225, 241, 255, -1, 255, 239, 225, -1, 240, 256, 257, -1, 257, 241, 240, -1, 242, 258, 256, -1, 256, 240, 242, -1, 243, 259, 258, -1, 258, 242, 243, -1, 244, 260, 259, -1, 259, 243, 244, -1, 245, 261, 260, -1, 260, 244, 245, -1, 246, 262, 261, -1, 261, 245, 246, -1, 247, 263, 262, -1, 262, 246, 247, -1, 248, 264, 263, -1, 263, 247, 248, -1, 249, 265, 264, -1, 264, 248, 249, -1, 250, 266, 265, -1, 265, 249, 250, -1, 251, 267, 266, -1, 266, 250, 251, -1, 252, 268, 267, -1, 267, 251, 252, -1, 253, 269, 268, -1, 268, 252, 253, -1, 254, 270, 269, -1, 269, 253, 254, -1, 255, 271, 270, -1, 270, 254, 255, -1, 241, 257, 271, -1, 271, 255, 241, -1, 256, 272, 273, -1, 273, 257, 256, -1, 258, 274, 272, -1, 272, 256, 258, -1, 259, 275, 274, -1, 274, 258, 259, -1, 260, 276, 275, -1, 275, 259, 260, -1, 261, 277, 276, -1, 276, 260, 261, -1, 262, 278, 277, -1, 277, 261, 262, -1, 263, 279, 278, -1, 278, 262, 263, -1, 264, 280, 279, -1, 279, 263, 264, -1, 265, 281, 280, -1, 280, 264, 265, -1, 266, 282, 281, -1, 281, 265, 266, -1, 267, 283, 282, -1, 282, 266, 267, -1, 268, 284, 283, -1, 283, 267, 268, -1, 269, 285, 284, -1, 284, 268, 269, -1, 270, 286, 285, -1, 285, 269, 270, -1, 271, 287, 286, -1, 286, 270, 271, -1, 257, 273, 287, -1, 287, 271, 257, -1, 272, 288, 289, -1, 289, 273, 272, -1, 274, 290, 288, -1, 288, 272, 274, -1, 275, 291, 290, -1, 290, 274, 275, -1, 276, 292, 291, -1, 291, 275, 276, -1, 277, 293, 292, -1, 292, 276, 277, -1, 278, 294, 293, -1, 293, 277, 278, -1, 279, 295, 294, -1, 294, 278, 279, -1, 280, 296, 295, -1, 295, 279, 280, -1, 281, 297, 296, -1, 296, 280, 281, -1, 282, 298, 297, -1, 297, 281, 282, -1, 283, 299, 298, -1, 298, 282, 283, -1, 284, 300, 299, -1, 299, 283, 284, -1, 285, 301, 300, -1, 300, 284, 285, -1, 286, 302, 301, -1, 301, 285, 286, -1, 287, 303, 302, -1, 302, 286, 287, -1, 273, 289, 303, -1, 303, 287, 273, -1, 288, 304, 305, -1, 305, 289, 288, -1, 290, 306, 304, -1, 304, 288, 290, -1, 291, 307, 306, -1, 306, 290, 291, -1, 292, 308, 307, -1, 307, 291, 292, -1, 293, 309, 308, -1, 308, 292, 293, -1, 294, 310, 309, -1, 309, 293, 294, -1, 295, 311, 310, -1, 310, 294, 295, -1, 296, 312, 311, -1, 311, 295, 296, -1, 297, 313, 312, -1, 312, 296, 297, -1, 298, 314, 313, -1, 313, 297, 298, -1, 299, 315, 314, -1, 314, 298, 299, -1, 300, 316, 315, -1, 315, 299, 300, -1, 301, 317, 316, -1, 316, 300, 301, -1, 302, 318, 317, -1, 317, 301, 302, -1, 303, 319, 318, -1, 318, 302, 303, -1, 289, 305, 319, -1, 319, 303, 289, -1, 304, 0, 3, -1, 3, 305, 304, -1, 306, 4, 0, -1, 0, 304, 306, -1, 307, 6, 4, -1, 4, 306, 307, -1, 308, 8, 6, -1, 6, 307, 308, -1, 309, 10, 8, -1, 8, 308, 309, -1, 310, 12, 10, -1, 10, 309, 310, -1, 311, 14, 12, -1, 12, 310, 311, -1, 312, 16, 14, -1, 14, 311, 312, -1, 313, 18, 16, -1, 16, 312, 313, -1, 314, 20, 18, -1, 18, 313, 314, -1, 315, 22, 20, -1, 20, 314, 315, -1, 316, 24, 22, -1, 22, 315, 316, -1, 317, 26, 24, -1, 24, 316, 317, -1, 318, 28, 26, -1, 26, 317, 318, -1, 319, 30, 28, -1, 28, 318, 319, -1, 305, 3, 30, -1, 30, 319, 305, -1 - ] - creaseAngle 1 - } - } - ] - } - DEF MIDDLE_WHEEL_MIDDLERIGHT Transform { - scale 0.001 0.001 0.001 - children [ - Shape { - appearance USE METAL_APPEARANCE - geometry IndexedFaceSet { - coord Coordinate { - point [ - 29.89 0.000153 -14.71, 28.09 10.22 -14.72, -0.1191 0.000153 -14.83, -0.1119 -0.0405 -14.83, -0.1191 -0.00119 -39.96, -0.1119 -0.04184 -39.96, 15.24 -0.00119 -39.96, 14.32 5.212 -39.96, 22.9 19.21 -14.72, -0.09125 -0.07638 -14.83, -0.09125 -0.07773 -39.96, 11.68 9.796 -39.96, 14.94 25.88 -14.72, -0.05957 -0.103 -14.83, -0.05957 -0.1043 -39.96, 7.621 13.2 -39.96, 5.191 29.43 -14.72, -0.02072 -0.1172 -14.83, -0.02072 -0.1185 -39.96, 2.647 15.01 -39.96, -5.189 29.44 -14.72, 0.02063 -0.1172 -14.83, 0.02063 -0.1185 -39.96, -2.646 15.01 -39.96, -14.94 25.89 -14.72, 0.05951 -0.103 -14.83, 0.05951 -0.1043 -39.96, -7.621 13.2 -39.96, -22.9 19.21 -14.72, 0.09119 -0.07638 -14.83, 0.09119 -0.07773 -39.96, -11.68 9.797 -39.96, -28.09 10.22 -14.72, 0.1119 -0.04062 -14.83, 0.1119 -0.04196 -39.96, -14.32 5.213 -39.96, -29.89 0.00174 -14.71, 0.1191 0.000153 -14.83, 0.1191 -0.00119 -39.96, -15.24 -0.000458 -39.96, -28.09 -10.22 -14.71, 0.1119 0.0408 -14.83, 0.1119 0.03946 -39.96, -14.32 -5.214 -39.96, -22.9 -19.21 -14.71, 0.09125 0.07669 -14.83, 0.09125 0.07535 -39.96, -11.68 -9.798 -39.96, -14.95 -25.88 -14.71, 0.05957 0.1033 -14.83, 0.05957 0.102 -39.96, -7.622 -13.2 -39.96, -5.192 -29.43 -14.71, 0.02072 0.1175 -14.83, 0.02072 0.1161 -39.96, -2.648 -15.01 -39.96, 5.188 -29.44 -14.71, -0.02063 0.1175 -14.83, -0.02063 0.1161 -39.96, 2.646 -15.01 -39.96, 14.94 -25.89 -14.71, -0.05951 0.1033 -14.83, -0.05951 0.102 -39.96, 7.62 -13.2 -39.96, 22.89 -19.21 -14.71, -0.09119 0.07669 -14.83, -0.09119 0.07535 -39.96, 11.68 -9.8 -39.96, 28.09 -10.23 -14.71, -0.1119 0.04092 -14.83, -0.1119 0.03958 -39.96, 14.32 -5.216 -39.96, 0.003054 -7.763 0.4682, -6.721 -3.881 0.468, -6.721 3.882 0.4676, 0.002321 7.765 0.4673, 6.726 3.884 0.4676, 6.727 -3.88 0.468, 6.727 -3.878 40.91, 6.726 3.886 40.91, 0.002321 7.767 40.91, -6.721 3.885 40.91, -6.721 -3.879 40.91, 0.003054 -7.761 40.91, 6.618 -13.73 -40.91, 11.92 -9.501 -40.91, 14.86 -3.39 -40.91, 14.86 3.392 -40.91, 11.92 9.502 -40.91, 6.614 13.73 -40.91, 0.00165 15.24 -40.91, -6.61 13.73 -40.91, -11.91 9.5 -40.91, -14.85 3.389 -40.91, -14.85 -3.393 -40.91, -11.91 -9.504 -40.91, -6.609 -13.73 -40.91, 0.003054 -15.24 -40.91, 0.003054 -15.24 -39.81, -6.609 -13.73 -39.81, -11.91 -9.504 -39.81, -14.85 -3.393 -39.81, -14.85 3.389 -39.81, -11.91 9.5 -39.81, -6.61 13.73 -39.81, 0.00165 15.24 -39.81, 6.614 13.73 -39.81, 11.92 9.502 -39.81, 14.86 3.392 -39.81, 14.86 -3.39 -39.81, 11.92 -9.501 -39.81, 6.618 -13.73 -39.81 - ] - } - coordIndex [ - 0, 1, 2, -1, 1, 3, 2, -1, 2, 3, 4, -1, 3, 5, 4, -1, 4, 5, 6, -1, 5, 7, 6, -1, 6, 7, 0, -1, 7, 1, 0, -1, 1, 8, 3, -1, 8, 9, 3, -1, 3, 9, 5, -1, 9, 10, 5, -1, 5, 10, 7, -1, 10, 11, 7, -1, 7, 11, 1, -1, 11, 8, 1, -1, 8, 12, 9, -1, 12, 13, 9, -1, 9, 13, 10, -1, 13, 14, 10, -1, 10, 14, 11, -1, 14, 15, 11, -1, 11, 15, 8, -1, 15, 12, 8, -1, 12, 16, 13, -1, 16, 17, 13, -1, 13, 17, 14, -1, 17, 18, 14, -1, 14, 18, 15, -1, 18, 19, 15, -1, 15, 19, 12, -1, 19, 16, 12, -1, 16, 20, 17, -1, 20, 21, 17, -1, 17, 21, 18, -1, 21, 22, 18, -1, 18, 22, 19, -1, 22, 23, 19, -1, 19, 23, 16, -1, 23, 20, 16, -1, 20, 24, 21, -1, 24, 25, 21, -1, 21, 25, 22, -1, 25, 26, 22, -1, 22, 26, 23, -1, 26, 27, 23, -1, 23, 27, 20, -1, 27, 24, 20, -1, 24, 28, 25, -1, 28, 29, 25, -1, 25, 29, 26, -1, 29, 30, 26, -1, 26, 30, 27, -1, 30, 31, 27, -1, 27, 31, 24, -1, 31, 28, 24, -1, 28, 32, 29, -1, 32, 33, 29, -1, 29, 33, 30, -1, 33, 34, 30, -1, 30, 34, 31, -1, 34, 35, 31, -1, 31, 35, 28, -1, 35, 32, 28, -1, 32, 36, 33, -1, 36, 37, 33, -1, 33, 37, 34, -1, 37, 38, 34, -1, 34, 38, 35, -1, 38, 39, 35, -1, 35, 39, 32, -1, 39, 36, 32, -1, 36, 40, 37, -1, 40, 41, 37, -1, 37, 41, 38, -1, 41, 42, 38, -1, 38, 42, 39, -1, 42, 43, 39, -1, 39, 43, 36, -1, 43, 40, 36, -1, 40, 44, 41, -1, 44, 45, 41, -1, 41, 45, 42, -1, 45, 46, 42, -1, 42, 46, 43, -1, 46, 47, 43, -1, 43, 47, 40, -1, 47, 44, 40, -1, 44, 48, 45, -1, 48, 49, 45, -1, 45, 49, 46, -1, 49, 50, 46, -1, 46, 50, 47, -1, 50, 51, 47, -1, 47, 51, 44, -1, 51, 48, 44, -1, 48, 52, 49, -1, 52, 53, 49, -1, 49, 53, 50, -1, 53, 54, 50, -1, 50, 54, 51, -1, 54, 55, 51, -1, 51, 55, 48, -1, 55, 52, 48, -1, 52, 56, 53, -1, 56, 57, 53, -1, 53, 57, 54, -1, 57, 58, 54, -1, 54, 58, 55, -1, 58, 59, 55, -1, 55, 59, 52, -1, 59, 56, 52, -1, 56, 60, 57, -1, 60, 61, 57, -1, 57, 61, 58, -1, 61, 62, 58, -1, 58, 62, 59, -1, 62, 63, 59, -1, 59, 63, 56, -1, 63, 60, 56, -1, 60, 64, 61, -1, 64, 65, 61, -1, 61, 65, 62, -1, 65, 66, 62, -1, 62, 66, 63, -1, 66, 67, 63, -1, 63, 67, 60, -1, 67, 64, 60, -1, 64, 68, 65, -1, 68, 69, 65, -1, 65, 69, 66, -1, 69, 70, 66, -1, 66, 70, 67, -1, 70, 71, 67, -1, 67, 71, 64, -1, 71, 68, 64, -1, 68, 0, 69, -1, 0, 2, 69, -1, 69, 2, 70, -1, 2, 4, 70, -1, 70, 4, 71, -1, 4, 6, 71, -1, 71, 6, 68, -1, 6, 0, 68, -1, 72, 73, 74, -1, 74, 75, 72, -1, 72, 75, 76, -1, 76, 77, 72, -1, 78, 79, 80, -1, 80, 81, 78, -1, 78, 81, 82, -1, 82, 83, 78, -1, 72, 83, 82, -1, 82, 73, 72, -1, 73, 82, 81, -1, 81, 74, 73, -1, 74, 81, 80, -1, 80, 75, 74, -1, 75, 80, 79, -1, 79, 76, 75, -1, 76, 79, 78, -1, 78, 77, 76, -1, 77, 78, 83, -1, 83, 72, 77, -1, 84, 85, 86, -1, 86, 87, 84, -1, 84, 87, 88, -1, 88, 89, 84, -1, 84, 89, 90, -1, 90, 91, 84, -1, 84, 91, 92, -1, 92, 93, 84, -1, 84, 93, 94, -1, 94, 95, 84, -1, 84, 95, 96, -1, 96, 97, 84, -1, 98, 99, 100, -1, 100, 101, 98, -1, 98, 101, 102, -1, 102, 103, 98, -1, 98, 103, 104, -1, 104, 105, 98, -1, 98, 105, 106, -1, 106, 107, 98, -1, 98, 107, 108, -1, 108, 109, 98, -1, 98, 109, 110, -1, 110, 111, 98, -1, 96, 99, 98, -1, 98, 97, 96, -1, 95, 100, 99, -1, 99, 96, 95, -1, 94, 101, 100, -1, 100, 95, 94, -1, 93, 102, 101, -1, 101, 94, 93, -1, 92, 103, 102, -1, 102, 93, 92, -1, 91, 104, 103, -1, 103, 92, 91, -1, 90, 105, 104, -1, 104, 91, 90, -1, 89, 106, 105, -1, 105, 90, 89, -1, 88, 107, 106, -1, 106, 89, 88, -1, 87, 108, 107, -1, 107, 88, 87, -1, 86, 109, 108, -1, 108, 87, 86, -1, 85, 110, 109, -1, 109, 86, 85, -1, 84, 111, 110, -1, 110, 85, 84, -1, 97, 98, 111, -1, 111, 84, 97, -1 - ] - creaseAngle 1 - } - } - ] - } - ] - name "MiddleRightWheel" - boundingObject USE BOUND_WHEEL - physics USE WHEEL_PHYSICS - } - } DEF armMD Transform { translation 0.035 -0.04 0.003 rotation 0 0 1 -0.02 diff --git a/slugbot_package/src/Simulation.cpp b/slugbot_package/src/Simulation.cpp index be6bd92..0d00564 100644 --- a/slugbot_package/src/Simulation.cpp +++ b/slugbot_package/src/Simulation.cpp @@ -6,11 +6,12 @@ #include #include #include -#include "std_msgs/msg/float64.hpp" #include "std_msgs/msg/string.hpp" +#include "messages/msg/wheel_states.hpp" +#include "../../utils/math/Rotation2d.hpp" #define WHEEL_RADIUS 0.06 -#define WHEEL_COUNT 6 +#define WHEEL_COUNT 4 void set_position(WbDeviceTag *side, float value); @@ -19,66 +20,27 @@ void Simulation::init( webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) { - left_motors[0] = wb_robot_get_device("FrontLeftWheel"); - left_motors[1] = wb_robot_get_device("MiddleLeftWheel"); - left_motors[2] = wb_robot_get_device("BackLeftWheel"); - left_side = left_motors; - - right_motors[0] = wb_robot_get_device("FrontRightWheel"); - right_motors[1] = wb_robot_get_device("MiddleRightWheel"); - right_motors[2] = wb_robot_get_device("BackRightWheel"); - right_side = right_motors; + motors[0] = wb_robot_get_device("FrontLeftWheel"); + motors[1] = wb_robot_get_device("FrontRightWheel"); + motors[2] = wb_robot_get_device("BackLeftWheel"); + motors[3] = wb_robot_get_device("BackRightWheel"); turn_motors[0] = wb_robot_get_device("FrontLeftArm"); turn_motors[1] = wb_robot_get_device("FrontRightArm"); turn_motors[2] = wb_robot_get_device("BackLeftArm"); turn_motors[3] = wb_robot_get_device("BackRightArm"); - set_position(left_side, INFINITY); - set_position(right_side, INFINITY); - - for (int i = 0; i < WHEEL_COUNT>>1; i++) { - wb_motor_set_velocity(left_motors[i], 0); - wb_motor_set_velocity(right_motors[i], 0); + for (int i = 0; i < WHEEL_COUNT; i++) { + wb_motor_set_position(turn_motors[i], 0); + wb_motor_set_position(motors[i], INFINITY); + wb_motor_set_velocity(motors[i], 0); } - // subscribe to wheel/turn commands published by the logic node - front_left_wheel_subscription = node->create_subscription( - "/front_left_wheel", rclcpp::SensorDataQoS().reliable(), - [this](const std_msgs::msg::Float64::SharedPtr msg){ - this->front_left_wheel_speed_msg = msg->data; - } - ); - front_right_wheel_subscription = node->create_subscription( - "/front_right_wheel", rclcpp::SensorDataQoS().reliable(), - [this](const std_msgs::msg::Float64::SharedPtr msg){ - this->front_right_wheel_speed_msg = msg->data; - } - ); - - back_left_wheel_subscription = node->create_subscription( - "/back_left_wheel", rclcpp::SensorDataQoS().reliable(), - [this](const std_msgs::msg::Float64::SharedPtr msg){ - this->back_left_wheel_speed_msg = msg->data; - } - ); - back_right_wheel_subscription = node->create_subscription( - "/back_right_wheel", rclcpp::SensorDataQoS().reliable(), - [this](const std_msgs::msg::Float64::SharedPtr msg){ - this->back_right_wheel_speed_msg = msg->data; - } - ); - - left_turn_angle_subscription = node->create_subscription( - "/left_turn_angle", rclcpp::SensorDataQoS().reliable(), - [this](const std_msgs::msg::Float64::SharedPtr msg){ - this->left_turn_angle_msg = msg->data; - } - ); - right_turn_angle_subscription = node->create_subscription( - "/right_turn_angle", rclcpp::SensorDataQoS().reliable(), - [this](const std_msgs::msg::Float64::SharedPtr msg){ - this->right_turn_angle_msg = msg->data; + // subscribe to wheel states message + wheel_states_subscription = node->create_subscription( + "/wheel_states", rclcpp::SensorDataQoS().reliable(), + [this](const messages::msg::WheelStates::SharedPtr msg){ + this->wheel_states_msg = *msg; } ); @@ -117,25 +79,23 @@ void Simulation::step() { msg->data = keys; keyboard_publisher->publish(std::move(msg)); - // Manually set all 6 speeds - for (int i = 0; i < 3; i += 2) { - wb_motor_set_velocity(left_motors[i], front_left_wheel_speed_msg); - wb_motor_set_velocity(right_motors[i], front_right_wheel_speed_msg); + // This simulation is currently only an approximation of swerve + for(int i = 0; i < 4; i++) { + double speed = wheel_states_msg.speeds[i]; + double angle = wheel_states_msg.angles[i]; + if(std::abs(angle) > M_PI/2){ + angle = Rotation2d(angle).invert().getRadians(); + speed *= -1; + } + wb_motor_set_velocity(motors[i], -speed); + wb_motor_set_position(turn_motors[i], angle); } - wb_motor_set_velocity(left_motors[1], back_left_wheel_speed_msg); - wb_motor_set_velocity(right_motors[1], back_right_wheel_speed_msg); - - wb_motor_set_position(turn_motors[0], left_turn_angle_msg); - wb_motor_set_position(turn_motors[1], right_turn_angle_msg); - wb_motor_set_position(turn_motors[2], -left_turn_angle_msg); - wb_motor_set_position(turn_motors[3], -right_turn_angle_msg); } } // namespace slugbot_driver void set_position(WbDeviceTag *side, float value) { - for (int i = 0; i < WHEEL_COUNT>>1; i++) { + for (int i = 0; i < 4; i++) { wb_motor_set_position(*side, value); - side++; } } diff --git a/slugbot_package/src/SlugbotDriver.cpp b/slugbot_package/src/SlugbotDriver.cpp index 400f42d..6beb8cc 100644 --- a/slugbot_package/src/SlugbotDriver.cpp +++ b/slugbot_package/src/SlugbotDriver.cpp @@ -1,38 +1,27 @@ #include "slugbot_package/SlugbotDriver.hpp" #include "rclcpp/rclcpp.hpp" -#include #include -#include "std_msgs/msg/float64.hpp" #include "std_msgs/msg/string.hpp" -#include "geometry_msgs/msg/twist.hpp" #include "messages/msg/controller_input.hpp" +#include "messages/msg/wheel_states.hpp" +#include "../../utils/math/Translation2d.hpp" +#include "../../utils/math/Rotation2d.hpp" -const double HALF_DISTANCE_BETWEEN_WHEELS = 0.24; -const double FORWARD_DISTANCE_BETWEEN_WHEELS = 0.30; +const double TRACK_WIDTH = 0.5; +const double TRACK_LENGTH = 0.6; const double WHEEL_RADIUS = 0.06; const double MAX_WHEEL_SPEED = 25.0; const double MAX_LINEAR_SPEED = (MAX_WHEEL_SPEED * WHEEL_RADIUS); -const double MAX_TURN_ANGLE = (M_PI / 6.0); -const bool IGNORE_AVOID_MESSAGE = true; +const double MAX_ROTATIONAL_SPEED = 0.2 * (MAX_LINEAR_SPEED / ((std::hypot(TRACK_WIDTH, TRACK_LENGTH) / 2))); SlugbotDriver::SlugbotDriver() : Node("slugbot_driver") { - // This assumes a 4 wheel drive with the front wheels turning, but it will also work with 6 wheels - back_left_wheel_publisher = this->create_publisher("/back_left_wheel", 10); - back_right_wheel_publisher = this->create_publisher("/back_right_wheel", 10); - front_left_wheel_publisher = this->create_publisher("/front_left_wheel", 10); - front_right_wheel_publisher = this->create_publisher("/front_right_wheel", 10); - left_turn_angle_publisher = this->create_publisher("/left_turn_angle", 10); - right_turn_angle_publisher = this->create_publisher("/right_turn_angle", 10); - - cmd_vel_subscription_avoid = this->create_subscription( - "/cmd_vel_avoid", 10, - [this](const geometry_msgs::msg::Twist::SharedPtr msg){ - this->cmd_vel_msg_avoid = *msg; - } - ); + // Swerve drive + wheel_states_publisher = this->create_publisher("/wheel_states", 10); + current_wheel_states = messages::msg::WheelStates(); + controller_subscription = this->create_subscription( "/controller_input", 10, [this](const messages::msg::ControllerInput::SharedPtr msg){ @@ -54,94 +43,77 @@ SlugbotDriver::SlugbotDriver() } void SlugbotDriver::update() { - double speed = 0; - double angle = 0; + // Forward, left, and CCW are positive + Translation2d speeds = Translation2d(); + double rotation = 0.0; if (recieved_input) { + double lx = controller_input.left_x; double ly = controller_input.left_y; double rx = controller_input.right_x; - speed = -MAX_LINEAR_SPEED * ly * std::abs(ly); - angle = rx * std::abs(rx); + speeds = Translation2d(-ly * std::hypot(lx, ly) * MAX_LINEAR_SPEED, + -lx * std::hypot(lx, ly) * MAX_LINEAR_SPEED); + rotation = -rx * std::abs(rx) * MAX_ROTATIONAL_SPEED; } else { + double x = 0, y = 0; if (keys_pressed.find('w') != std::string::npos){ - speed = MAX_LINEAR_SPEED; + x = 1; } if (keys_pressed.find('s') != std::string::npos){ - speed -= MAX_LINEAR_SPEED; + x -= 1; } if (keys_pressed.find('a') != std::string::npos){ - angle = -1; + y = 1; } if (keys_pressed.find('d') != std::string::npos){ - angle += 1; + y -= 1; + } + if (keys_pressed.find('q') != std::string::npos){ + rotation = MAX_ROTATIONAL_SPEED; } + if (keys_pressed.find('e') != std::string::npos){ + rotation -= MAX_ROTATIONAL_SPEED; + } + speeds = Translation2d(x, y).normalized() * MAX_LINEAR_SPEED; } - if (IGNORE_AVOID_MESSAGE) { - cmd_vel_msg_avoid.linear.x = 0.0; - cmd_vel_msg_avoid.angular.z = 0.0; + Translation2d wheels[4] = { + Translation2d(speeds.getX() - rotation * TRACK_WIDTH/2, speeds.getY() + rotation * TRACK_LENGTH/2) / WHEEL_RADIUS, // Front Left + Translation2d(speeds.getX() + rotation * TRACK_WIDTH/2, speeds.getY() + rotation * TRACK_LENGTH/2) / WHEEL_RADIUS, // Front Right + Translation2d(speeds.getX() - rotation * TRACK_WIDTH/2, speeds.getY() - rotation * TRACK_LENGTH/2) / WHEEL_RADIUS, // Back Left + Translation2d(speeds.getX() + rotation * TRACK_WIDTH/2, speeds.getY() - rotation * TRACK_LENGTH/2) / WHEEL_RADIUS // Back Right + }; + double max_wheel_speed = 0.0; + for (int i = 0; i < 4; i++) { + double wheel_speed = wheels[i].norm(); + if (wheel_speed > max_wheel_speed) { + max_wheel_speed = wheel_speed; + } } - - speed += cmd_vel_msg_avoid.linear.x; - speed = std::clamp(speed, -MAX_LINEAR_SPEED, MAX_LINEAR_SPEED); - double max_angular_speed = std::abs(speed) / (HALF_DISTANCE_BETWEEN_WHEELS + FORWARD_DISTANCE_BETWEEN_WHEELS/std::tan(MAX_TURN_ANGLE)); - angle *= max_angular_speed; - angle += cmd_vel_msg_avoid.angular.z; - angle = std::clamp(angle, -max_angular_speed, max_angular_speed); - - double back_left, back_right, front_left, front_right, left_angle, right_angle; - if(std::abs(angle) < 1e-5) { - back_left = speed; - back_right = speed; - front_left = speed; - front_right = speed; - left_angle = 0.0; - right_angle = 0.0; - }else{ - double radius = speed / angle; - double left_r = radius - HALF_DISTANCE_BETWEEN_WHEELS; - double right_r = radius + HALF_DISTANCE_BETWEEN_WHEELS; - back_left = angle * left_r; - back_right = angle * right_r; - double front_left_r = std::hypot(left_r, FORWARD_DISTANCE_BETWEEN_WHEELS); - double front_right_r = std::hypot(right_r, FORWARD_DISTANCE_BETWEEN_WHEELS); - front_left = angle * front_left_r * std::abs(left_r) / left_r; - front_right = angle * front_right_r * std::abs(right_r) / right_r; - left_angle = std::atan(FORWARD_DISTANCE_BETWEEN_WHEELS / left_r); - right_angle = std::atan(FORWARD_DISTANCE_BETWEEN_WHEELS / right_r); - double highest_speed = std::max({std::abs(back_left), std::abs(back_right), std::abs(front_left), std::abs(front_right)}); - if (highest_speed > MAX_LINEAR_SPEED) { - double scale = MAX_LINEAR_SPEED / highest_speed; - back_left *= scale; - back_right *= scale; - front_left *= scale; - front_right *= scale; + if (max_wheel_speed > MAX_WHEEL_SPEED) { + for (int i = 0; i < 4; i++) { + wheels[i] = wheels[i] * (MAX_WHEEL_SPEED / max_wheel_speed); } } - auto back_left_msg = std_msgs::msg::Float64(); - back_left_msg.data = back_left / WHEEL_RADIUS; - back_left_wheel_publisher->publish(back_left_msg); - - auto back_right_msg = std_msgs::msg::Float64(); - back_right_msg.data = back_right / WHEEL_RADIUS; - back_right_wheel_publisher->publish(back_right_msg); - - auto front_left_msg = std_msgs::msg::Float64(); - front_left_msg.data = front_left / WHEEL_RADIUS; - front_left_wheel_publisher->publish(front_left_msg); - - auto front_right_msg = std_msgs::msg::Float64(); - front_right_msg.data = front_right / WHEEL_RADIUS; - front_right_wheel_publisher->publish(front_right_msg); - - auto left_angle_msg = std_msgs::msg::Float64(); - left_angle_msg.data = left_angle; - left_turn_angle_publisher->publish(left_angle_msg); + auto wheel_msg = messages::msg::WheelStates(); + for(int i = 0; i < 4; i++) { + wheel_msg.speeds[i] = wheels[i].norm(); + wheel_msg.angles[i] = wheels[i].getAngle(); + } + optimize_wheel_states(wheel_msg); + wheel_states_publisher->publish(wheel_msg); +} - auto right_angle_msg = std_msgs::msg::Float64(); - right_angle_msg.data = right_angle; - right_turn_angle_publisher->publish(right_angle_msg); +void SlugbotDriver::optimize_wheel_states(messages::msg::WheelStates& wheels) { + for(int i = 0; i < 4; i++){ + double a = Rotation2d(wheels.angles[i] - current_wheel_states.angles[i]).modulus().getRadians(); + if (std::abs(a) > M_PI / 2){ + wheels.angles[i] = Rotation2d(wheels.angles[i] + M_PI).modulus().getRadians(); + wheels.speeds[i] = -wheels.speeds[i]; + } + } + current_wheel_states = wheels; } int main(int argc, char **argv) { diff --git a/utils/math/Pose2d.cpp b/utils/math/Pose2d.cpp new file mode 100644 index 0000000..4250b00 --- /dev/null +++ b/utils/math/Pose2d.cpp @@ -0,0 +1,72 @@ +#include "Pose2d.hpp" + +Pose2d::Pose2d(Translation2d translation, Rotation2d rotation) { + this->x = translation.getX(); + this->y = translation.getY(); + this->angle = rotation.getRadians(); +} +Pose2d::Pose2d(double x, double y, double radians) { + this->x = x; + this->y = y; + this->angle = radians; +} +Pose2d::Pose2d() { + this->x = 0.0; + this->y = 0.0; + this->angle = 0.0; +} +Pose2d::Pose2d(double x, double y, Rotation2d rotation) { + this->x = x; + this->y = y; + this->angle = rotation.getRadians(); +} +Translation2d Pose2d::getTranslation() const { + return Translation2d(this->x, this->y); +} +Rotation2d Pose2d::getRotation() const { + return Rotation2d(this->angle); +} +double Pose2d::getX() const { + return this->x; +} +double Pose2d::getY() const { + return this->y; +} +double Pose2d::getAngle() const { + return this->angle; +} +Pose2d Pose2d::operator+(const Pose2d& other) const { + Translation2d new_translation = this->getTranslation() + other.getTranslation().rotateBy(this->angle); + Rotation2d new_rotation = this->getRotation() + other.getRotation(); + return Pose2d(new_translation, new_rotation.modulus()); +} +Pose2d Pose2d::operator-(const Pose2d& other) const { + Translation2d delta_translation = other.getTranslation() - this->getTranslation(); + Translation2d rotated_translation = delta_translation.rotateBy(-this->angle); + Rotation2d delta_rotation = other.getRotation() - this->getRotation(); + return Pose2d(rotated_translation, delta_rotation.modulus()); +} +Pose2d Pose2d::transformBy(const Pose2d& other) const { + return (*this) + other; +} +Pose2d Pose2d::translate(const Translation2d& translation) const { + Translation2d new_translation = this->getTranslation() + translation; + return Pose2d(new_translation, this->getRotation()); +} +Pose2d Pose2d::rotateInPlace(const Rotation2d& rotation) const { + Rotation2d new_rotation = this->getRotation() + rotation; + return Pose2d(this->x, this->y, new_rotation.modulus()); +} +Pose2d Pose2d::rotateAround(const Translation2d& center, const Rotation2d& rotation) const { + Translation2d translated_point = this->getTranslation() - center; + Translation2d rotated_point = translated_point.rotateBy(rotation.getRadians()); + Translation2d final_point = rotated_point + center; + Rotation2d new_rotation = this->getRotation() + rotation; + return Pose2d(final_point, new_rotation.modulus()); +} +Pose2d Pose2d::rotateOrigin(const Rotation2d& rotation) const { + return this->rotateAround(Translation2d(0.0, 0.0), rotation); +} +double Pose2d::getDistance(const Pose2d& other) const { + return this->getTranslation().getDistance(other.getTranslation()); +} diff --git a/utils/math/Pose2d.hpp b/utils/math/Pose2d.hpp new file mode 100644 index 0000000..07eada2 --- /dev/null +++ b/utils/math/Pose2d.hpp @@ -0,0 +1,30 @@ +#pragma once +#include "Translation2d.hpp" +#include "Rotation2d.hpp" + +class Pose2d { +private: + double x; + double y; + double angle; +public: + Pose2d(Translation2d translation, Rotation2d rotation); + Pose2d(double x, double y, double radians); + Pose2d(); + Pose2d(double x, double y, Rotation2d rotation); + + Translation2d getTranslation() const; + Rotation2d getRotation() const; + double getX() const; + double getY() const; + double getAngle() const; + + Pose2d operator+(const Pose2d& other) const; + Pose2d operator-(const Pose2d& other) const; + Pose2d transformBy(const Pose2d& other) const; + Pose2d translate(const Translation2d& translation) const; + Pose2d rotateInPlace(const Rotation2d& rotation) const; + Pose2d rotateAround(const Translation2d& center, const Rotation2d& rotation) const; + Pose2d rotateOrigin(const Rotation2d& rotation) const; + double getDistance(const Pose2d& other) const; +}; diff --git a/utils/math/Rotation2d.cpp b/utils/math/Rotation2d.cpp new file mode 100644 index 0000000..d0fc078 --- /dev/null +++ b/utils/math/Rotation2d.cpp @@ -0,0 +1,60 @@ +#include "Rotation2d.hpp" +#include "Translation2d.hpp" + +Rotation2d::Rotation2d(double radians) { + this->radians = radians; +} +Rotation2d::Rotation2d(double x, double y) { + this->radians = std::atan2(y, x); +} +Rotation2d::Rotation2d() { + this->radians = 0.0; +} +Rotation2d::Rotation2d(Translation2d translation){ + this->radians = translation.getAngle(); +} +double Rotation2d::getRadians() const { + return this->radians; +} +double Rotation2d::getDegrees() const { + return this->radians * (180.0 / M_PI); +} +double Rotation2d::getRotations() const { + return this->radians / (2.0 * M_PI); +} +double Rotation2d::sin() const { + return std::sin(this->radians); +} +double Rotation2d::cos() const { + return std::cos(this->radians); +} +double Rotation2d::tan() const { + return std::tan(this->radians); +} +Rotation2d Rotation2d::operator+(const Rotation2d& other) const { + return Rotation2d(this->radians + other.radians); +} +Rotation2d Rotation2d::operator-(const Rotation2d& other) const { + return Rotation2d(this->radians - other.radians); +} +Rotation2d Rotation2d::operator*(double scalar) const { + return Rotation2d(this->radians * scalar); +} +Rotation2d Rotation2d::operator/(double scalar) const { + return Rotation2d(this->radians / scalar); +} +Rotation2d Rotation2d::rotateBy(const Rotation2d& other) const { + return Rotation2d(this->radians + other.radians); +} +Rotation2d Rotation2d::modulus() const { + double mod_radians = std::fmod(this->radians, 2.0 * M_PI); + if (mod_radians > M_PI) { + mod_radians -= 2.0 * M_PI; + }else if(mod_radians < -M_PI){ + mod_radians += 2.0 * M_PI; + } + return Rotation2d(mod_radians); +} +Rotation2d Rotation2d::invert() const { + return Rotation2d(this->radians + M_PI).modulus(); +} diff --git a/utils/math/Rotation2d.hpp b/utils/math/Rotation2d.hpp new file mode 100644 index 0000000..eeefe2b --- /dev/null +++ b/utils/math/Rotation2d.hpp @@ -0,0 +1,30 @@ +#pragma once +#include + +class Translation2d; // forward declaration + +class Rotation2d { +private: + double radians; +public: + Rotation2d(); + Rotation2d(double radians); + Rotation2d(double x, double y); + Rotation2d(Translation2d translation); + + double getRadians() const; + double getDegrees() const; + double getRotations() const; + double sin() const; + double cos() const; + double tan() const; + + Rotation2d operator+(const Rotation2d& other) const; + Rotation2d operator-(const Rotation2d& other) const; + Rotation2d operator*(double scalar) const; + Rotation2d operator/(double scalar) const; + + Rotation2d rotateBy(const Rotation2d& other) const; + Rotation2d modulus() const; + Rotation2d invert() const; +}; diff --git a/utils/math/Translation2d.cpp b/utils/math/Translation2d.cpp new file mode 100644 index 0000000..2c7e3fd --- /dev/null +++ b/utils/math/Translation2d.cpp @@ -0,0 +1,57 @@ +#include "Translation2d.hpp" +#include "Rotation2d.hpp" + +Translation2d::Translation2d(double x, double y){ + this->x = x; + this->y = y; +} +Translation2d::Translation2d(){ + this->x = 0.0; + this->y = 0.0; +} +Translation2d::Translation2d(double magnitude, Rotation2d direction){ + this->x = magnitude * direction.cos(); + this->y = magnitude * direction.sin(); +} +Translation2d Translation2d::operator+(const Translation2d& other) const { + return Translation2d(this->x + other.x, this->y + other.y); +} +Translation2d Translation2d::operator-(const Translation2d& other) const { + return Translation2d(this->x - other.x, this->y - other.y); +} +Translation2d Translation2d::operator*(double scalar) const { + return Translation2d(this->x * scalar, this->y * scalar); +} +Translation2d Translation2d::operator/(double scalar) const { + if (scalar == 0.0) { + return Translation2d(0.0, 0.0); + } + return Translation2d(this->x / scalar, this->y / scalar); +} +double Translation2d::norm() const { + return std::hypot(this->x, this->y); +} +Translation2d Translation2d::normalized() const { + double n = this->norm(); + if (n == 0) { + return Translation2d(0, 0); + } + return Translation2d(this->x / n, this->y / n); +} +double Translation2d::getX() const { + return this->x; +} +double Translation2d::getY() const { + return this->y; +} +double Translation2d::getAngle() const { + return std::atan2(this->y, this->x); +} +Translation2d Translation2d::rotateBy(double angle) const { + double cosA = std::cos(angle); + double sinA = std::sin(angle); + return Translation2d(this->x * cosA - this->y * sinA, this->x * sinA + this->y * cosA); +} +double Translation2d::getDistance(const Translation2d& other) const { + return (*this - other).norm(); +} \ No newline at end of file diff --git a/utils/math/Translation2d.hpp b/utils/math/Translation2d.hpp new file mode 100644 index 0000000..c4282d9 --- /dev/null +++ b/utils/math/Translation2d.hpp @@ -0,0 +1,30 @@ +#pragma once +#include + +class Rotation2d; // forward declaration + +class Translation2d { +private: + double x; + double y; + +public: + Translation2d(); + Translation2d(double x, double y); + Translation2d(double magnitude, Rotation2d direction); + + Translation2d operator+(const Translation2d& other) const; + Translation2d operator-(const Translation2d& other) const; + Translation2d operator*(double scalar) const; + Translation2d operator/(double scalar) const; + + double norm() const; + Translation2d normalized() const; + + double getX() const; + double getY() const; + double getAngle() const; + + Translation2d rotateBy(double angle) const; + double getDistance(const Translation2d& other) const; +}; From 9964e65e29bb8fe60fd83a9288aebb5dc54aabf3 Mon Sep 17 00:00:00 2001 From: Kyle-Eldridge Date: Thu, 22 Jan 2026 14:24:21 -0800 Subject: [PATCH 4/5] Increase max rotation --- slugbot_package/src/SlugbotDriver.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/slugbot_package/src/SlugbotDriver.cpp b/slugbot_package/src/SlugbotDriver.cpp index 6beb8cc..69dfb98 100644 --- a/slugbot_package/src/SlugbotDriver.cpp +++ b/slugbot_package/src/SlugbotDriver.cpp @@ -13,7 +13,7 @@ const double TRACK_LENGTH = 0.6; const double WHEEL_RADIUS = 0.06; const double MAX_WHEEL_SPEED = 25.0; const double MAX_LINEAR_SPEED = (MAX_WHEEL_SPEED * WHEEL_RADIUS); -const double MAX_ROTATIONAL_SPEED = 0.2 * (MAX_LINEAR_SPEED / ((std::hypot(TRACK_WIDTH, TRACK_LENGTH) / 2))); +const double MAX_ROTATIONAL_SPEED = (MAX_LINEAR_SPEED / ((std::hypot(TRACK_WIDTH, TRACK_LENGTH) / 2))); SlugbotDriver::SlugbotDriver() : Node("slugbot_driver") { @@ -63,10 +63,10 @@ void SlugbotDriver::update() { x -= 1; } if (keys_pressed.find('a') != std::string::npos){ - y = 1; + y = 0.5; } if (keys_pressed.find('d') != std::string::npos){ - y -= 1; + y -= 0.5; } if (keys_pressed.find('q') != std::string::npos){ rotation = MAX_ROTATIONAL_SPEED; From 2fe8bd7cc46a62967860ccc7556da9141fddf8b7 Mon Sep 17 00:00:00 2001 From: Kyle-Eldridge Date: Wed, 28 Jan 2026 20:43:56 -0800 Subject: [PATCH 5/5] PR changes --- slugbot_package/src/OldSlugbotDriver.cpp | 155 +++++++++++++++++++++++ slugbot_package/src/Simulation.cpp | 8 -- slugbot_package/src/SlugbotDriver.cpp | 21 +-- 3 files changed, 166 insertions(+), 18 deletions(-) create mode 100644 slugbot_package/src/OldSlugbotDriver.cpp diff --git a/slugbot_package/src/OldSlugbotDriver.cpp b/slugbot_package/src/OldSlugbotDriver.cpp new file mode 100644 index 0000000..f026482 --- /dev/null +++ b/slugbot_package/src/OldSlugbotDriver.cpp @@ -0,0 +1,155 @@ +/* +#include "slugbot_package/SlugbotDriver.hpp" + +#include "rclcpp/rclcpp.hpp" +#include +#include +#include "std_msgs/msg/float64.hpp" +#include "std_msgs/msg/string.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "messages/msg/controller_input.hpp" + +const double HALF_DISTANCE_BETWEEN_WHEELS = 0.24; +const double FORWARD_DISTANCE_BETWEEN_WHEELS = 0.30; +const double WHEEL_RADIUS = 0.06; +const double MAX_WHEEL_SPEED = 25.0; +const double MAX_LINEAR_SPEED = (MAX_WHEEL_SPEED * WHEEL_RADIUS); +const double MAX_TURN_ANGLE = (M_PI / 6.0); +const bool IGNORE_AVOID_MESSAGE = true; + +SlugbotDriver::SlugbotDriver() + : Node("slugbot_driver") { + // This assumes a 4 wheel drive with the front wheels turning, but it will also work with 6 wheels + back_left_wheel_publisher = this->create_publisher("/back_left_wheel", 10); + back_right_wheel_publisher = this->create_publisher("/back_right_wheel", 10); + front_left_wheel_publisher = this->create_publisher("/front_left_wheel", 10); + front_right_wheel_publisher = this->create_publisher("/front_right_wheel", 10); + left_turn_angle_publisher = this->create_publisher("/left_turn_angle", 10); + right_turn_angle_publisher = this->create_publisher("/right_turn_angle", 10); + + cmd_vel_subscription_avoid = this->create_subscription( + "/cmd_vel_avoid", 10, + [this](const geometry_msgs::msg::Twist::SharedPtr msg){ + this->cmd_vel_msg_avoid = *msg; + } + ); + + controller_subscription = this->create_subscription( + "/controller_input", 10, + [this](const messages::msg::ControllerInput::SharedPtr msg){ + this->controller_input = *msg; + this->recieved_input = true; + } + ); + + keyboard_subscription = this->create_subscription( + "/keyboard", 10, + [this](const std_msgs::msg::String::SharedPtr msg){ + this->keys_pressed = msg->data; + } + ); + + // Run at 50Hz + timer = this->create_wall_timer(std::chrono::milliseconds(20), + std::bind(&SlugbotDriver::update, this)); +} + +void SlugbotDriver::update() { + double speed = 0; + double angle = 0; + + if (recieved_input) { + double ly = controller_input.left_y; + double rx = controller_input.right_x; + speed = -MAX_LINEAR_SPEED * ly * std::abs(ly); + angle = rx * std::abs(rx); + } else { + if (keys_pressed.find('w') != std::string::npos){ + speed = MAX_LINEAR_SPEED; + } + if (keys_pressed.find('s') != std::string::npos){ + speed -= MAX_LINEAR_SPEED; + } + if (keys_pressed.find('a') != std::string::npos){ + angle = -1; + } + if (keys_pressed.find('d') != std::string::npos){ + angle += 1; + } + } + + if (IGNORE_AVOID_MESSAGE) { + cmd_vel_msg_avoid.linear.x = 0.0; + cmd_vel_msg_avoid.angular.z = 0.0; + } + + speed += cmd_vel_msg_avoid.linear.x; + speed = std::clamp(speed, -MAX_LINEAR_SPEED, MAX_LINEAR_SPEED); + double max_angular_speed = std::abs(speed) / (HALF_DISTANCE_BETWEEN_WHEELS + FORWARD_DISTANCE_BETWEEN_WHEELS/std::tan(MAX_TURN_ANGLE)); + angle *= max_angular_speed; + angle += cmd_vel_msg_avoid.angular.z; + angle = std::clamp(angle, -max_angular_speed, max_angular_speed); + + double back_left, back_right, front_left, front_right, left_angle, right_angle; + if(std::abs(angle) < 1e-5) { + back_left = speed; + back_right = speed; + front_left = speed; + front_right = speed; + left_angle = 0.0; + right_angle = 0.0; + }else{ + double radius = speed / angle; + double left_r = radius - HALF_DISTANCE_BETWEEN_WHEELS; + double right_r = radius + HALF_DISTANCE_BETWEEN_WHEELS; + back_left = angle * left_r; + back_right = angle * right_r; + double front_left_r = std::hypot(left_r, FORWARD_DISTANCE_BETWEEN_WHEELS); + double front_right_r = std::hypot(right_r, FORWARD_DISTANCE_BETWEEN_WHEELS); + front_left = angle * front_left_r * std::abs(left_r) / left_r; + front_right = angle * front_right_r * std::abs(right_r) / right_r; + left_angle = std::atan(FORWARD_DISTANCE_BETWEEN_WHEELS / left_r); + right_angle = std::atan(FORWARD_DISTANCE_BETWEEN_WHEELS / right_r); + double highest_speed = std::max({std::abs(back_left), std::abs(back_right), std::abs(front_left), std::abs(front_right)}); + if (highest_speed > MAX_LINEAR_SPEED) { + double scale = MAX_LINEAR_SPEED / highest_speed; + back_left *= scale; + back_right *= scale; + front_left *= scale; + front_right *= scale; + } + } + + auto back_left_msg = std_msgs::msg::Float64(); + back_left_msg.data = back_left / WHEEL_RADIUS; + back_left_wheel_publisher->publish(back_left_msg); + + auto back_right_msg = std_msgs::msg::Float64(); + back_right_msg.data = back_right / WHEEL_RADIUS; + back_right_wheel_publisher->publish(back_right_msg); + + auto front_left_msg = std_msgs::msg::Float64(); + front_left_msg.data = front_left / WHEEL_RADIUS; + front_left_wheel_publisher->publish(front_left_msg); + + auto front_right_msg = std_msgs::msg::Float64(); + front_right_msg.data = front_right / WHEEL_RADIUS; + front_right_wheel_publisher->publish(front_right_msg); + + auto left_angle_msg = std_msgs::msg::Float64(); + left_angle_msg.data = left_angle; + left_turn_angle_publisher->publish(left_angle_msg); + + auto right_angle_msg = std_msgs::msg::Float64(); + right_angle_msg.data = right_angle; + right_turn_angle_publisher->publish(right_angle_msg); +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} +*/ \ No newline at end of file diff --git a/slugbot_package/src/Simulation.cpp b/slugbot_package/src/Simulation.cpp index 0d00564..aef0795 100644 --- a/slugbot_package/src/Simulation.cpp +++ b/slugbot_package/src/Simulation.cpp @@ -13,8 +13,6 @@ #define WHEEL_RADIUS 0.06 #define WHEEL_COUNT 4 -void set_position(WbDeviceTag *side, float value); - namespace slugbot_driver { void Simulation::init( webots_ros2_driver::WebotsNode *node, @@ -93,12 +91,6 @@ void Simulation::step() { } } // namespace slugbot_driver -void set_position(WbDeviceTag *side, float value) { - for (int i = 0; i < 4; i++) { - wb_motor_set_position(*side, value); - } -} - #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(slugbot_driver::Simulation, webots_ros2_driver::PluginInterface) \ No newline at end of file diff --git a/slugbot_package/src/SlugbotDriver.cpp b/slugbot_package/src/SlugbotDriver.cpp index 69dfb98..f0623b5 100644 --- a/slugbot_package/src/SlugbotDriver.cpp +++ b/slugbot_package/src/SlugbotDriver.cpp @@ -8,12 +8,12 @@ #include "../../utils/math/Translation2d.hpp" #include "../../utils/math/Rotation2d.hpp" -const double TRACK_WIDTH = 0.5; -const double TRACK_LENGTH = 0.6; -const double WHEEL_RADIUS = 0.06; -const double MAX_WHEEL_SPEED = 25.0; -const double MAX_LINEAR_SPEED = (MAX_WHEEL_SPEED * WHEEL_RADIUS); -const double MAX_ROTATIONAL_SPEED = (MAX_LINEAR_SPEED / ((std::hypot(TRACK_WIDTH, TRACK_LENGTH) / 2))); +const double TRACK_WIDTH = 0.5; // Meters +const double TRACK_LENGTH = 0.6; // Meters +const double WHEEL_RADIUS = 0.06; // Meters +const double MAX_WHEEL_SPEED = 25.0; // Radians per second +const double MAX_LINEAR_SPEED = (MAX_WHEEL_SPEED * WHEEL_RADIUS); // Meters per second +const double MAX_ROTATIONAL_SPEED = (MAX_LINEAR_SPEED / ((std::hypot(TRACK_WIDTH, TRACK_LENGTH) / 2))); // Radians per second SlugbotDriver::SlugbotDriver() : Node("slugbot_driver") { @@ -55,6 +55,7 @@ void SlugbotDriver::update() { -lx * std::hypot(lx, ly) * MAX_LINEAR_SPEED); rotation = -rx * std::abs(rx) * MAX_ROTATIONAL_SPEED; } else { + // Percent speed, -1 to 1 double x = 0, y = 0; if (keys_pressed.find('w') != std::string::npos){ x = 1; @@ -63,16 +64,16 @@ void SlugbotDriver::update() { x -= 1; } if (keys_pressed.find('a') != std::string::npos){ - y = 0.5; + y = 1; } if (keys_pressed.find('d') != std::string::npos){ - y -= 0.5; + y -= 1; } if (keys_pressed.find('q') != std::string::npos){ - rotation = MAX_ROTATIONAL_SPEED; + rotation = MAX_ROTATIONAL_SPEED / 2; } if (keys_pressed.find('e') != std::string::npos){ - rotation -= MAX_ROTATIONAL_SPEED; + rotation -= MAX_ROTATIONAL_SPEED / 2; } speeds = Translation2d(x, y).normalized() * MAX_LINEAR_SPEED; }