diff --git a/controllers/rl_quadruped_controller/CMakeLists.txt b/controllers/rl_quadruped_controller/CMakeLists.txt index f744e63..c1524a0 100644 --- a/controllers/rl_quadruped_controller/CMakeLists.txt +++ b/controllers/rl_quadruped_controller/CMakeLists.txt @@ -27,6 +27,7 @@ set(dependencies control_input_msgs kdl_parser ament_index_cpp + controller_common ) # find dependencies @@ -39,9 +40,7 @@ add_library(${PROJECT_NAME} SHARED src/common/ObservationBuffer.cpp - src/FSM/StatePassive.cpp src/FSM/StateFixedStand.cpp - src/FSM/StateFixedDown.cpp src/FSM/StateRL.cpp src/control/LowPassFilter.cpp diff --git a/controllers/rl_quadruped_controller/README.md b/controllers/rl_quadruped_controller/README.md index baa01cf..7109145 100644 --- a/controllers/rl_quadruped_controller/README.md +++ b/controllers/rl_quadruped_controller/README.md @@ -32,7 +32,7 @@ echo 'export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:~/CLionProjects/libtorch/lib' >> ~ ```bash cd ~/ros2_ws -colcon build --packages-up-to rl_quadruped_controller +colcon build --packages-up-to rl_quadruped_controller --symlink-install ``` ## 3. Launch diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/FSMState.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/FSMState.h deleted file mode 100644 index 32b780d..0000000 --- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/FSMState.h +++ /dev/null @@ -1,40 +0,0 @@ -// -// Created by tlab-uav on 24-9-6. -// - -#ifndef FSMSTATE_H -#define FSMSTATE_H - -#include -#include -#include - -#include "rl_quadruped_controller/common/enumClass.h" -#include "rl_quadruped_controller/control/CtrlComponent.h" - -class FSMState { -public: - virtual ~FSMState() = default; - - FSMState(const FSMStateName &state_name, std::string state_name_string, CtrlComponent &ctrl_comp) - : state_name(state_name), - state_name_string(std::move(state_name_string)), - ctrl_comp_(ctrl_comp) { - } - - virtual void enter() = 0; - - virtual void run() = 0; - - virtual void exit() = 0; - - virtual FSMStateName checkChange() { return FSMStateName::INVALID; } - - FSMStateName state_name; - std::string state_name_string; - -protected: - CtrlComponent &ctrl_comp_; -}; - -#endif //FSMSTATE_H diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedDown.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedDown.h deleted file mode 100644 index 012fee5..0000000 --- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedDown.h +++ /dev/null @@ -1,38 +0,0 @@ -// -// Created by tlab-uav on 24-9-11. -// - -#ifndef STATEFIXEDDOWN_H -#define STATEFIXEDDOWN_H - -#include "FSMState.h" - -class StateFixedDown final : public FSMState { -public: - explicit StateFixedDown(CtrlComponent &ctrlComp, - const std::vector &target_pos, - double kp, - double kd - ); - - void enter() override; - - void run() override; - - void exit() override; - - FSMStateName checkChange() override; -private: - double target_pos_[12] = {}; - double start_pos_[12] = {}; - rclcpp::Time start_time_; - - double kp_, kd_; - - double duration_ = 600; // steps - double percent_ = 0; //% - double phase = 0.0; -}; - - -#endif //STATEFIXEDDOWN_H diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedStand.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedStand.h index b9f4cd8..70085b2 100644 --- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedStand.h +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedStand.h @@ -5,33 +5,16 @@ #ifndef STATEFIXEDSTAND_H #define STATEFIXEDSTAND_H -#include "FSMState.h" +#include -class StateFixedStand final : public FSMState { +class StateFixedStand final : public BaseFixedStand { public: - explicit StateFixedStand(CtrlComponent &ctrlComp, + explicit StateFixedStand(CtrlInterfaces &ctrl_interfaces, const std::vector &target_pos, double kp, double kd); - void enter() override; - - void run() override; - - void exit() override; - FSMStateName checkChange() override; - -private: - double target_pos_[12] = {}; - double start_pos_[12] = {}; - rclcpp::Time start_time_; - - double kp_, kd_; - - double duration_ = 600; // steps - double percent_ = 0; //% - double phase = 0.0; }; diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StatePassive.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StatePassive.h deleted file mode 100644 index 9b186c4..0000000 --- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StatePassive.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// Created by tlab-uav on 24-9-6. -// - -#ifndef STATEPASSIVE_H -#define STATEPASSIVE_H -#include "FSMState.h" - - -class StatePassive final : public FSMState { -public: - explicit StatePassive(CtrlComponent &ctrlComp); - - void enter() override; - - void run() override; - - void exit() override; - - FSMStateName checkChange() override; -}; - - -#endif //STATEPASSIVE_H diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h index c4b24b1..aa43e14 100644 --- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h @@ -6,12 +6,16 @@ #define STATERL_H #include +#include #include -#include "FSMState.h" +#include "controller_common/FSM/FSMState.h" + +struct CtrlComponent; template -void executeAndSleep(Functor f, const double frequency) { +void executeAndSleep(Functor f, const double frequency) +{ using clock = std::chrono::high_resolution_clock; const auto start = clock::now(); @@ -27,9 +31,28 @@ void executeAndSleep(Functor f, const double frequency) { std::this_thread::sleep_until(sleepTill); } -template -struct RobotCommand { - struct MotorCommand { +inline void setThreadPriority(int priority, std::thread& thread) +{ + sched_param sched{}; + sched.sched_priority = priority; + + if (priority != 0) + { + if (pthread_setschedparam(thread.native_handle(), SCHED_FIFO, &sched) != 0) + { + std::cerr << "WARNING: Failed to set threads priority (one possible reason could be " + "that the user and the group permissions are not set properly.)" + << std::endl; + } + } +} + + +template +struct RobotCommand +{ + struct MotorCommand + { std::vector q = std::vector(32, 0.0); std::vector dq = std::vector(32, 0.0); std::vector tau = std::vector(32, 0.0); @@ -38,15 +61,18 @@ struct RobotCommand { } motor_command; }; -template -struct RobotState { - struct IMU { +template +struct RobotState +{ + struct IMU + { std::vector quaternion = {1.0, 0.0, 0.0, 0.0}; // w, x, y, z std::vector gyroscope = {0.0, 0.0, 0.0}; std::vector accelerometer = {0.0, 0.0, 0.0}; } imu; - struct MotorState { + struct MotorState + { std::vector q = std::vector(32, 0.0); std::vector dq = std::vector(32, 0.0); std::vector ddq = std::vector(32, 0.0); @@ -62,7 +88,8 @@ struct Control double yaw = 0.0; }; -struct ModelParams { +struct ModelParams +{ std::string model_name; std::string framework; int decimation; @@ -101,13 +128,17 @@ struct Observations torch::Tensor actions; }; -class StateRL final : public FSMState { +class StateRL final : public FSMState +{ public: - explicit StateRL(CtrlComponent &ctrl_component, const std::string &config_path, const std::vector &target_pos); + explicit StateRL(CtrlInterfaces& ctrl_interfaces, + CtrlComponent& ctrl_component, const std::string& config_path, + const std::vector& target_pos); void enter() override; - void run() override; + void run(const rclcpp::Time& time, + const rclcpp::Duration& period) override; void exit() override; @@ -116,9 +147,10 @@ public: private: torch::Tensor computeObservation(); - void loadYaml(const std::string &config_path); + void loadYaml(const std::string& config_path); - static torch::Tensor quatRotateInverse(const torch::Tensor &q, const torch::Tensor& v, const std::string& framework); + static torch::Tensor quatRotateInverse(const torch::Tensor& q, const torch::Tensor& v, + const std::string& framework); /** * @brief Forward the RL model to get the action @@ -131,6 +163,9 @@ private: void setCommand() const; + bool enable_estimator_; + std::shared_ptr& estimator_; + // Parameters ModelParams params_; Observations obs_; diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/enumClass.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/enumClass.h deleted file mode 100644 index 4b2fc0e..0000000 --- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/enumClass.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// Created by tlab-uav on 24-9-6. -// - -#ifndef ENUMCLASS_H -#define ENUMCLASS_H - -enum class FSMStateName { - // EXIT, - INVALID, - PASSIVE, - FIXEDDOWN, - FIXEDSTAND, - RL, -}; - -enum class FSMMode { - NORMAL, - CHANGE -}; - -#endif //ENUMCLASS_H diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/CtrlComponent.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/CtrlComponent.h index a7aee5c..6544160 100644 --- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/CtrlComponent.h +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/CtrlComponent.h @@ -2,65 +2,18 @@ // Created by biao on 24-9-10. // -#ifndef INTERFACE_H -#define INTERFACE_H - -#include -#include -#include -#include +#ifndef CtrlComponent_H +#define CtrlComponent_H #include "Estimator.h" struct CtrlComponent { - std::vector > - joint_torque_command_interface_; - std::vector > - joint_position_command_interface_; - std::vector > - joint_velocity_command_interface_; - std::vector > - joint_kp_command_interface_; - std::vector > - joint_kd_command_interface_; - - std::vector > - joint_effort_state_interface_; - std::vector > - joint_position_state_interface_; - std::vector > - joint_velocity_state_interface_; - - std::vector > - imu_state_interface_; - - std::vector > - foot_force_state_interface_; - - control_input_msgs::msg::Inputs control_inputs_; - int frequency_{}; bool enable_estimator_ = false; - std::shared_ptr robot_model_; std::shared_ptr estimator_; CtrlComponent() = default; - - void clear() { - joint_torque_command_interface_.clear(); - joint_position_command_interface_.clear(); - joint_velocity_command_interface_.clear(); - joint_kd_command_interface_.clear(); - joint_kp_command_interface_.clear(); - - joint_effort_state_interface_.clear(); - joint_position_state_interface_.clear(); - joint_velocity_state_interface_.clear(); - - imu_state_interface_.clear(); - foot_force_state_interface_.clear(); - } }; -#endif //INTERFACE_H +#endif //CtrlComponent_H diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/Estimator.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/Estimator.h index 66b6704..353785b 100644 --- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/Estimator.h +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/Estimator.h @@ -6,7 +6,7 @@ #define ESTIMATOR_H #include #include -#include +#include #include #include "LowPassFilter.h" @@ -16,7 +16,7 @@ struct CtrlComponent; class Estimator { public: - explicit Estimator(CtrlComponent &ctrl_component); + explicit Estimator(CtrlInterfaces &ctrl_interfaces, CtrlComponent &ctrl_component); ~Estimator() = default; @@ -104,7 +104,7 @@ public: void update(); private: - CtrlComponent &ctrl_component_; + CtrlInterfaces &ctrl_interfaces_; std::shared_ptr &robot_model_; Eigen::Matrix x_hat_; // The state of estimator, position(3)+velocity(3)+feet position(3x4) diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/QuadrupedRobot.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/QuadrupedRobot.h index db62b3f..da6bd01 100644 --- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/QuadrupedRobot.h +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/QuadrupedRobot.h @@ -7,16 +7,16 @@ #define QUADRUPEDROBOT_H #include #include -#include +#include #include "RobotLeg.h" -struct CtrlComponent; +struct CtrlInterfaces; class QuadrupedRobot { public: - explicit QuadrupedRobot(CtrlComponent &ctrl_component, const std::string &robot_description, + explicit QuadrupedRobot(CtrlInterfaces &ctrl_interfaces, const std::string &robot_description, const std::vector &feet_names, const std::string &base_name); ~QuadrupedRobot() = default; @@ -91,7 +91,7 @@ public: void update(); private: - CtrlComponent &ctrl_component_; + CtrlInterfaces &ctrl_interfaces_; std::vector > robot_legs_; KDL::Chain fr_chain_; diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/RobotLeg.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/RobotLeg.h index 029dd1e..3f150b8 100644 --- a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/RobotLeg.h +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/RobotLeg.h @@ -9,7 +9,7 @@ #include #include #include -#include +#include class RobotLeg { public: diff --git a/controllers/rl_quadruped_controller/launch/gazebo.launch.py b/controllers/rl_quadruped_controller/launch/gazebo.launch.py index af0b4c7..0866e04 100644 --- a/controllers/rl_quadruped_controller/launch/gazebo.launch.py +++ b/controllers/rl_quadruped_controller/launch/gazebo.launch.py @@ -34,7 +34,7 @@ def launch_setup(context, *args, **kwargs): executable='create', output='screen', arguments=['-topic', 'robot_description', '-name', - 'robot', '-allow_renaming', 'true', '-z', init_height] + 'robot', '-allow_renaming', 'true', '-z', init_height], ) robot_state_publisher = Node( @@ -65,20 +65,12 @@ def launch_setup(context, *args, **kwargs): "--controller-manager", "/controller_manager"], ) - leg_pd_controller = Node( + rl_quadruped_controller = Node( package="controller_manager", executable="spawner", - arguments=["leg_pd_controller", - "--controller-manager", "/controller_manager"], + arguments=["rl_quadruped_controller", "--controller-manager", "/controller_manager"], ) - controller = Node( - package="controller_manager", - executable="spawner", - arguments=["rl_quadruped_controller", "--controller-manager", "/controller_manager"] - ) - - return [ rviz, Node( @@ -95,11 +87,16 @@ def launch_setup(context, *args, **kwargs): launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]), robot_state_publisher, gz_spawn_entity, - leg_pd_controller, RegisterEventHandler( event_handler=OnProcessExit( - target_action=leg_pd_controller, - on_exit=[imu_sensor_broadcaster, joint_state_publisher, controller], + target_action=gz_spawn_entity, + on_exit=[imu_sensor_broadcaster, joint_state_publisher], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_publisher, + on_exit=[rl_quadruped_controller], ) ), ] diff --git a/controllers/rl_quadruped_controller/package.xml b/controllers/rl_quadruped_controller/package.xml index a0f0bcd..5bcb1f6 100644 --- a/controllers/rl_quadruped_controller/package.xml +++ b/controllers/rl_quadruped_controller/package.xml @@ -9,6 +9,7 @@ ament_cmake + controller_common backward_ros controller_interface pluginlib diff --git a/controllers/rl_quadruped_controller/src/FSM/StateFixedDown.cpp b/controllers/rl_quadruped_controller/src/FSM/StateFixedDown.cpp deleted file mode 100644 index 8057bbc..0000000 --- a/controllers/rl_quadruped_controller/src/FSM/StateFixedDown.cpp +++ /dev/null @@ -1,62 +0,0 @@ -// -// Created by tlab-uav on 24-9-11. -// - -#include "rl_quadruped_controller/FSM/StateFixedDown.h" - -#include - -StateFixedDown::StateFixedDown(CtrlComponent &ctrlComp, - const std::vector &target_pos, - const double kp, - const double kd) - : FSMState(FSMStateName::FIXEDDOWN, "fixed down", ctrlComp), - kp_(kp), kd_(kd) { - duration_ = ctrl_comp_.frequency_ * 1.2; - for (int i = 0; i < 12; i++) { - target_pos_[i] = target_pos[i]; - } -} - -void StateFixedDown::enter() { - for (int i = 0; i < 12; i++) { - start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value(); - } - ctrl_comp_.control_inputs_.command = 0; - for (int i = 0; i < 12; i++) { - ctrl_comp_.joint_position_command_interface_[i].get().set_value(start_pos_[i]); - ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0); - ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0); - ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_); - ctrl_comp_.joint_kd_command_interface_[i].get().set_value(kd_); - } -} - -void StateFixedDown::run() { - percent_ += 1 / duration_; - phase = std::tanh(percent_); - for (int i = 0; i < 12; i++) { - ctrl_comp_.joint_position_command_interface_[i].get().set_value( - phase * target_pos_[i] + (1 - phase) * start_pos_[i]); - } -} - -void StateFixedDown::exit() { - percent_ = 0; -} - -FSMStateName StateFixedDown::checkChange() { - if (percent_ < 1.5) { - return FSMStateName::FIXEDDOWN; - } - switch (ctrl_comp_.control_inputs_.command) { - case 1: - return FSMStateName::PASSIVE; - case 2: - return FSMStateName::FIXEDSTAND; - case 3: - return FSMStateName::RL; - default: - return FSMStateName::FIXEDDOWN; - } -} diff --git a/controllers/rl_quadruped_controller/src/FSM/StateFixedStand.cpp b/controllers/rl_quadruped_controller/src/FSM/StateFixedStand.cpp index d2ac404..4d6d78d 100644 --- a/controllers/rl_quadruped_controller/src/FSM/StateFixedStand.cpp +++ b/controllers/rl_quadruped_controller/src/FSM/StateFixedStand.cpp @@ -4,51 +4,17 @@ #include "rl_quadruped_controller/FSM/StateFixedStand.h" -#include - -StateFixedStand::StateFixedStand(CtrlComponent &ctrlComp, const std::vector &target_pos, +StateFixedStand::StateFixedStand(CtrlInterfaces &ctrl_interfaces, const std::vector &target_pos, const double kp, const double kd) - : FSMState(FSMStateName::FIXEDSTAND, "fixed stand", ctrlComp), - kp_(kp), kd_(kd) { - duration_ = ctrl_comp_.frequency_ * 1.2; - for (int i = 0; i < 12; i++) { - target_pos_[i] = target_pos[i]; - } -} - -void StateFixedStand::enter() { - for (int i = 0; i < 12; i++) { - start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value(); - } - for (int i = 0; i < 12; i++) { - ctrl_comp_.joint_position_command_interface_[i].get().set_value(start_pos_[i]); - ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0); - ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0); - ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_); - ctrl_comp_.joint_kd_command_interface_[i].get().set_value(kd_); - } - ctrl_comp_.control_inputs_.command = 0; -} - -void StateFixedStand::run() { - percent_ += 1 / duration_; - phase = std::tanh(percent_); - for (int i = 0; i < 12; i++) { - ctrl_comp_.joint_position_command_interface_[i].get().set_value( - phase * target_pos_[i] + (1 - phase) * start_pos_[i]); - } -} - -void StateFixedStand::exit() { - percent_ = 0; + : BaseFixedStand(ctrl_interfaces, target_pos, kp, kd) { } FSMStateName StateFixedStand::checkChange() { if (percent_ < 1.5) { return FSMStateName::FIXEDSTAND; } - switch (ctrl_comp_.control_inputs_.command) { + switch (ctrl_interfaces_.control_inputs_.command) { case 1: return FSMStateName::PASSIVE; case 2: diff --git a/controllers/rl_quadruped_controller/src/FSM/StatePassive.cpp b/controllers/rl_quadruped_controller/src/FSM/StatePassive.cpp deleted file mode 100644 index 9622cc6..0000000 --- a/controllers/rl_quadruped_controller/src/FSM/StatePassive.cpp +++ /dev/null @@ -1,43 +0,0 @@ -// -// Created by tlab-uav on 24-9-6. -// - -#include "rl_quadruped_controller/FSM/StatePassive.h" - -#include - -StatePassive::StatePassive(CtrlComponent &ctrlComp) : FSMState( - FSMStateName::PASSIVE, "passive", ctrlComp) { -} - -void StatePassive::enter() { - for (auto i: ctrl_comp_.joint_torque_command_interface_) { - i.get().set_value(0); - } - for (auto i: ctrl_comp_.joint_position_command_interface_) { - i.get().set_value(0); - } - for (auto i: ctrl_comp_.joint_velocity_command_interface_) { - i.get().set_value(0); - } - for (auto i: ctrl_comp_.joint_kp_command_interface_) { - i.get().set_value(0); - } - for (auto i: ctrl_comp_.joint_kd_command_interface_) { - i.get().set_value(1); - } - ctrl_comp_.control_inputs_.command = 0; -} - -void StatePassive::run() { -} - -void StatePassive::exit() { -} - -FSMStateName StatePassive::checkChange() { - if (ctrl_comp_.control_inputs_.command == 2) { - return FSMStateName::FIXEDDOWN; - } - return FSMStateName::PASSIVE; -} diff --git a/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp b/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp index 1f58df0..ccee726 100644 --- a/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp +++ b/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp @@ -7,42 +7,55 @@ #include #include -template -std::vector ReadVectorFromYaml(const YAML::Node &node) { +template +std::vector ReadVectorFromYaml(const YAML::Node& node) +{ std::vector values; - for (const auto &val: node) { + for (const auto& val : node) + { values.push_back(val.as()); } return values; } -template -std::vector ReadVectorFromYaml(const YAML::Node &node, const std::string &framework, const int &rows, - const int &cols) { +template +std::vector ReadVectorFromYaml(const YAML::Node& node, const std::string& framework, const int& rows, + const int& cols) +{ std::vector values; - for (const auto &val: node) { + for (const auto& val : node) + { values.push_back(val.as()); } - if (framework == "isaacsim") { + if (framework == "isaacsim") + { std::vector transposed_values(cols * rows); - for (int r = 0; r < rows; ++r) { - for (int c = 0; c < cols; ++c) { + for (int r = 0; r < rows; ++r) + { + for (int c = 0; c < cols; ++c) + { transposed_values[c * rows + r] = values[r * cols + c]; } } return transposed_values; } - if (framework == "isaacgym") { + if (framework == "isaacgym") + { return values; } throw std::invalid_argument("Unsupported framework: " + framework); } -StateRL::StateRL(CtrlComponent &ctrl_component, const std::string &config_path, - const std::vector &target_pos) : FSMState( - FSMStateName::RL, "rl", ctrl_component) { - for (int i = 0; i < 12; i++) { +StateRL::StateRL(CtrlInterfaces& ctrl_interfaces, + CtrlComponent& ctrl_component, const std::string& config_path, + const std::vector& target_pos) : FSMState( + FSMStateName::RL, "rl", ctrl_interfaces), + estimator_(ctrl_component.estimator_), + enable_estimator_(ctrl_component.enable_estimator_) +{ + for (int i = 0; i < 12; i++) + { init_pos_[i] = target_pos[i]; } @@ -50,7 +63,8 @@ StateRL::StateRL(CtrlComponent &ctrl_component, const std::string &config_path, loadYaml(config_path); // history - if (!params_.observations_history.empty()) { + if (!params_.observations_history.empty()) + { history_obs_buf_ = std::make_shared(1, params_.num_observations, params_.observations_history.size()); } @@ -59,31 +73,39 @@ StateRL::StateRL(CtrlComponent &ctrl_component, const std::string &config_path, model_ = torch::jit::load(config_path + "/" + params_.model_name); - // for (const auto ¶m: model_.parameters()) { // std::cout << "Parameter dtype: " << param.dtype() << std::endl; // } - rl_thread_ = std::thread([&] { - while (true) { - try { + rl_thread_ = std::thread([&] + { + while (true) + { + try + { executeAndSleep( - [&] { - if (running_) { + [&] + { + if (running_) + { runModel(); } }, - ctrl_comp_.frequency_ / params_.decimation); - } catch (const std::exception &e) { + ctrl_interfaces_.frequency_ / params_.decimation); + } + catch (const std::exception& e) + { running_ = false; RCLCPP_ERROR(rclcpp::get_logger("StateRL"), "Error in RL thread: %s", e.what()); } } }); + setThreadPriority(50, rl_thread_); } -void StateRL::enter() { +void StateRL::enter() +{ // Init observations obs_.lin_vel = torch::tensor({{0.0, 0.0, 0.0}}); obs_.ang_vel = torch::tensor({{0.0, 0.0, 0.0}}); @@ -106,44 +128,63 @@ void StateRL::enter() { running_ = true; } -void StateRL::run() { +void StateRL::run(const rclcpp::Time&/*time*/, const rclcpp::Duration&/*period*/) +{ getState(); setCommand(); } -void StateRL::exit() { +void StateRL::exit() +{ running_ = false; } -FSMStateName StateRL::checkChange() { - switch (ctrl_comp_.control_inputs_.command) { - case 1: - return FSMStateName::PASSIVE; - case 2: - return FSMStateName::FIXEDDOWN; - default: - return FSMStateName::RL; +FSMStateName StateRL::checkChange() +{ + switch (ctrl_interfaces_.control_inputs_.command) + { + case 1: + return FSMStateName::PASSIVE; + case 2: + return FSMStateName::FIXEDDOWN; + default: + return FSMStateName::RL; } } -torch::Tensor StateRL::computeObservation() { +torch::Tensor StateRL::computeObservation() +{ std::vector obs_list; - for (const std::string &observation: params_.observations) { - if (observation == "lin_vel") { + for (const std::string& observation : params_.observations) + { + if (observation == "lin_vel") + { obs_list.push_back(obs_.lin_vel * params_.lin_vel_scale); - } else if (observation == "ang_vel") { + } + else if (observation == "ang_vel") + { obs_list.push_back( quatRotateInverse(obs_.base_quat, obs_.ang_vel, params_.framework) * params_.ang_vel_scale); - } else if (observation == "gravity_vec") { + } + else if (observation == "gravity_vec") + { obs_list.push_back(quatRotateInverse(obs_.base_quat, obs_.gravity_vec, params_.framework)); - } else if (observation == "commands") { + } + else if (observation == "commands") + { obs_list.push_back(obs_.commands * params_.commands_scale); - } else if (observation == "dof_pos") { + } + else if (observation == "dof_pos") + { obs_list.push_back((obs_.dof_pos - params_.default_dof_pos) * params_.dof_pos_scale); - } else if (observation == "dof_vel") { + } + else if (observation == "dof_vel") + { obs_list.push_back(obs_.dof_vel * params_.dof_vel_scale); - } else if (observation == "actions") { + } + else if (observation == "actions") + { obs_list.push_back(obs_.actions); } } @@ -155,11 +196,15 @@ torch::Tensor StateRL::computeObservation() { return clamped_obs; } -void StateRL::loadYaml(const std::string &config_path) { +void StateRL::loadYaml(const std::string& config_path) +{ YAML::Node config; - try { + try + { config = YAML::LoadFile(config_path + "/config.yaml"); - } catch ([[maybe_unused]] YAML::BadFile &e) { + } + catch ([[maybe_unused]] YAML::BadFile& e) + { RCLCPP_ERROR(rclcpp::get_logger("StateRL"), "The file '%s' does not exist", config_path.c_str()); return; } @@ -170,19 +215,25 @@ void StateRL::loadYaml(const std::string &config_path) { params_.framework = config["framework"].as(); const int rows = config["rows"].as(); const int cols = config["cols"].as(); - if (config["observations_history"].IsNull()) { + if (config["observations_history"].IsNull()) + { params_.observations_history = {}; - } else { + } + else + { params_.observations_history = ReadVectorFromYaml(config["observations_history"]); } params_.decimation = config["decimation"].as(); params_.num_observations = config["num_observations"].as(); params_.observations = ReadVectorFromYaml(config["observations"]); params_.clip_obs = config["clip_obs"].as(); - if (config["clip_actions_lower"].IsNull() && config["clip_actions_upper"].IsNull()) { + if (config["clip_actions_lower"].IsNull() && config["clip_actions_upper"].IsNull()) + { params_.clip_actions_upper = torch::tensor({}).view({1, -1}); params_.clip_actions_lower = torch::tensor({}).view({1, -1}); - } else { + } + else + { params_.clip_actions_upper = torch::tensor( ReadVectorFromYaml(config["clip_actions_upper"], params_.framework, rows, cols)).view({1, -1}); params_.clip_actions_lower = torch::tensor( @@ -213,13 +264,17 @@ void StateRL::loadYaml(const std::string &config_path) { // ReadVectorFromYaml(config["default_dof_pos"], params_.framework, rows, cols)).view({1, -1}); } -torch::Tensor StateRL::quatRotateInverse(const torch::Tensor &q, const torch::Tensor &v, const std::string &framework) { +torch::Tensor StateRL::quatRotateInverse(const torch::Tensor& q, const torch::Tensor& v, const std::string& framework) +{ torch::Tensor q_w; torch::Tensor q_vec; - if (framework == "isaacsim") { + if (framework == "isaacsim") + { q_w = q.index({torch::indexing::Slice(), 0}); q_vec = q.index({torch::indexing::Slice(), torch::indexing::Slice(1, 4)}); - } else if (framework == "isaacgym") { + } + else if (framework == "isaacgym") + { q_w = q.index({torch::indexing::Slice(), 3}); q_vec = q.index({torch::indexing::Slice(), torch::indexing::Slice(0, 3)}); } @@ -231,63 +286,75 @@ torch::Tensor StateRL::quatRotateInverse(const torch::Tensor &q, const torch::Te return a - b + c; } -torch::Tensor StateRL::forward() { +torch::Tensor StateRL::forward() +{ torch::autograd::GradMode::set_enabled(false); torch::Tensor clamped_obs = computeObservation(); torch::Tensor actions; - if (!params_.observations_history.empty()) { + if (!params_.observations_history.empty()) + { history_obs_buf_->insert(clamped_obs); history_obs_ = history_obs_buf_->getObsVec(params_.observations_history); actions = model_.forward({history_obs_}).toTensor(); - } else { + } + else + { actions = model_.forward({clamped_obs}).toTensor(); } - if (params_.clip_actions_upper.numel() != 0 && params_.clip_actions_lower.numel() != 0) { + if (params_.clip_actions_upper.numel() != 0 && params_.clip_actions_lower.numel() != 0) + { return clamp(actions, params_.clip_actions_lower, params_.clip_actions_upper); } return actions; } -void StateRL::getState() { - if (params_.framework == "isaacgym") { - robot_state_.imu.quaternion[3] = ctrl_comp_.imu_state_interface_[0].get().get_value(); - robot_state_.imu.quaternion[0] = ctrl_comp_.imu_state_interface_[1].get().get_value(); - robot_state_.imu.quaternion[1] = ctrl_comp_.imu_state_interface_[2].get().get_value(); - robot_state_.imu.quaternion[2] = ctrl_comp_.imu_state_interface_[3].get().get_value(); - } else if (params_.framework == "isaacsim") { - robot_state_.imu.quaternion[0] = ctrl_comp_.imu_state_interface_[0].get().get_value(); - robot_state_.imu.quaternion[1] = ctrl_comp_.imu_state_interface_[1].get().get_value(); - robot_state_.imu.quaternion[2] = ctrl_comp_.imu_state_interface_[2].get().get_value(); - robot_state_.imu.quaternion[3] = ctrl_comp_.imu_state_interface_[3].get().get_value(); +void StateRL::getState() +{ + if (params_.framework == "isaacgym") + { + robot_state_.imu.quaternion[3] = ctrl_interfaces_.imu_state_interface_[0].get().get_value(); + robot_state_.imu.quaternion[0] = ctrl_interfaces_.imu_state_interface_[1].get().get_value(); + robot_state_.imu.quaternion[1] = ctrl_interfaces_.imu_state_interface_[2].get().get_value(); + robot_state_.imu.quaternion[2] = ctrl_interfaces_.imu_state_interface_[3].get().get_value(); + } + else if (params_.framework == "isaacsim") + { + robot_state_.imu.quaternion[0] = ctrl_interfaces_.imu_state_interface_[0].get().get_value(); + robot_state_.imu.quaternion[1] = ctrl_interfaces_.imu_state_interface_[1].get().get_value(); + robot_state_.imu.quaternion[2] = ctrl_interfaces_.imu_state_interface_[2].get().get_value(); + robot_state_.imu.quaternion[3] = ctrl_interfaces_.imu_state_interface_[3].get().get_value(); } - robot_state_.imu.gyroscope[0] = ctrl_comp_.imu_state_interface_[4].get().get_value(); - robot_state_.imu.gyroscope[1] = ctrl_comp_.imu_state_interface_[5].get().get_value(); - robot_state_.imu.gyroscope[2] = ctrl_comp_.imu_state_interface_[6].get().get_value(); + robot_state_.imu.gyroscope[0] = ctrl_interfaces_.imu_state_interface_[4].get().get_value(); + robot_state_.imu.gyroscope[1] = ctrl_interfaces_.imu_state_interface_[5].get().get_value(); + robot_state_.imu.gyroscope[2] = ctrl_interfaces_.imu_state_interface_[6].get().get_value(); - robot_state_.imu.accelerometer[0] = ctrl_comp_.imu_state_interface_[7].get().get_value(); - robot_state_.imu.accelerometer[1] = ctrl_comp_.imu_state_interface_[8].get().get_value(); - robot_state_.imu.accelerometer[2] = ctrl_comp_.imu_state_interface_[9].get().get_value(); + robot_state_.imu.accelerometer[0] = ctrl_interfaces_.imu_state_interface_[7].get().get_value(); + robot_state_.imu.accelerometer[1] = ctrl_interfaces_.imu_state_interface_[8].get().get_value(); + robot_state_.imu.accelerometer[2] = ctrl_interfaces_.imu_state_interface_[9].get().get_value(); - for (int i = 0; i < 12; i++) { - robot_state_.motor_state.q[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value(); - robot_state_.motor_state.dq[i] = ctrl_comp_.joint_velocity_state_interface_[i].get().get_value(); - robot_state_.motor_state.tauEst[i] = ctrl_comp_.joint_effort_state_interface_[i].get().get_value(); + for (int i = 0; i < 12; i++) + { + robot_state_.motor_state.q[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_value(); + robot_state_.motor_state.dq[i] = ctrl_interfaces_.joint_velocity_state_interface_[i].get().get_value(); + robot_state_.motor_state.tauEst[i] = ctrl_interfaces_.joint_effort_state_interface_[i].get().get_value(); } - control_.x = ctrl_comp_.control_inputs_.ly; - control_.y = -ctrl_comp_.control_inputs_.lx; - control_.yaw = -ctrl_comp_.control_inputs_.rx; + control_.x = ctrl_interfaces_.control_inputs_.ly; + control_.y = -ctrl_interfaces_.control_inputs_.lx; + control_.yaw = -ctrl_interfaces_.control_inputs_.rx; updated_ = true; } -void StateRL::runModel() { - if (ctrl_comp_.enable_estimator_) { - obs_.lin_vel = torch::from_blob(ctrl_comp_.estimator_->getVelocity().data(), {3}, torch::kDouble).clone(). - to(torch::kFloat).unsqueeze(0); +void StateRL::runModel() +{ + if (enable_estimator_) + { + obs_.lin_vel = torch::from_blob(estimator_->getVelocity().data(), {3}, torch::kDouble).clone(). + to(torch::kFloat).unsqueeze(0); } obs_.ang_vel = torch::tensor(robot_state_.imu.gyroscope).unsqueeze(0); obs_.commands = torch::tensor({{control_.x, control_.y, control_.yaw}}); @@ -297,7 +364,8 @@ void StateRL::runModel() { const torch::Tensor clamped_actions = forward(); - for (const int i: params_.hip_scale_reduction_indices) { + for (const int i : params_.hip_scale_reduction_indices) + { clamped_actions[0][i] *= params_.hip_scale_reduction; } @@ -309,7 +377,8 @@ void StateRL::runModel() { output_dof_pos_ = actions_scaled + params_.default_dof_pos; - for (int i = 0; i < params_.num_of_dofs; ++i) { + for (int i = 0; i < params_.num_of_dofs; ++i) + { robot_command_.motor_command.q[i] = output_dof_pos_[0][i].item(); robot_command_.motor_command.dq[i] = 0; robot_command_.motor_command.kp[i] = params_.rl_kp[0][i].item(); @@ -318,12 +387,21 @@ void StateRL::runModel() { } } -void StateRL::setCommand() const { - for (int i = 0; i < 12; i++) { - ctrl_comp_.joint_position_command_interface_[i].get().set_value(robot_command_.motor_command.q[i]); - ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(robot_command_.motor_command.dq[i]); - ctrl_comp_.joint_kp_command_interface_[i].get().set_value(robot_command_.motor_command.kp[i]); - ctrl_comp_.joint_kd_command_interface_[i].get().set_value(robot_command_.motor_command.kd[i]); - ctrl_comp_.joint_torque_command_interface_[i].get().set_value(robot_command_.motor_command.tau[i]); +void StateRL::setCommand() const +{ + for (int i = 0; i < 12; i++) + { + std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get(). + set_value( + robot_command_.motor_command.q[i]); + std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value( + robot_command_.motor_command.dq[i]); + std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value( + robot_command_.motor_command.kp[i]); + std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value( + robot_command_.motor_command.kd[i]); + std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get(). + set_value( + robot_command_.motor_command.tau[i]); } } diff --git a/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp b/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp index 30b58f9..a396936 100644 --- a/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp +++ b/controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp @@ -47,18 +47,18 @@ namespace rl_quadruped_controller { } controller_interface::return_type LeggedGymController:: - update(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - if (ctrl_comp_.enable_estimator_) { - if (ctrl_comp_.robot_model_ == nullptr) { + update(const rclcpp::Time & time, const rclcpp::Duration & period) { + if (ctrl_component_.enable_estimator_) { + if (ctrl_component_.robot_model_ == nullptr) { return controller_interface::return_type::OK; } - ctrl_comp_.robot_model_->update(); - ctrl_comp_.estimator_->update(); + ctrl_component_.robot_model_->update(); + ctrl_component_.estimator_->update(); } if (mode_ == FSMMode::NORMAL) { - current_state_->run(); + current_state_->run(time, period); next_state_name_ = current_state_->checkChange(); if (next_state_name_ != current_state_->state_name) { mode_ = FSMMode::CHANGE; @@ -108,12 +108,12 @@ namespace rl_quadruped_controller { stand_kp_ = auto_declare("stand_kp", stand_kp_); stand_kd_ = auto_declare("stand_kd", stand_kd_); - get_node()->get_parameter("update_rate", ctrl_comp_.frequency_); - RCLCPP_INFO(get_node()->get_logger(), "Controller Update Rate: %d Hz", ctrl_comp_.frequency_); + get_node()->get_parameter("update_rate", ctrl_interfaces_.frequency_); + RCLCPP_INFO(get_node()->get_logger(), "Controller Update Rate: %d Hz", ctrl_interfaces_.frequency_); if (foot_force_interface_types_.size() == 4) { - ctrl_comp_.enable_estimator_ = true; - ctrl_comp_.estimator_ = std::make_shared(ctrl_comp_); + ctrl_component_.enable_estimator_ = true; + ctrl_component_.estimator_ = std::make_shared(ctrl_interfaces_, ctrl_component_); } } catch (const std::exception &e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); @@ -128,9 +128,9 @@ namespace rl_quadruped_controller { robot_description_subscription_ = get_node()->create_subscription( "/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(), [this](const std_msgs::msg::String::SharedPtr msg) { - if (ctrl_comp_.enable_estimator_) { - ctrl_comp_.robot_model_ = std::make_shared( - ctrl_comp_, msg->data, feet_names_, base_name_); + if (ctrl_component_.enable_estimator_) { + ctrl_component_.robot_model_ = std::make_shared( + ctrl_interfaces_, msg->data, feet_names_, base_name_); } }); @@ -138,11 +138,11 @@ namespace rl_quadruped_controller { control_input_subscription_ = get_node()->create_subscription( "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) { // Handle message - ctrl_comp_.control_inputs_.command = msg->command; - ctrl_comp_.control_inputs_.lx = msg->lx; - ctrl_comp_.control_inputs_.ly = msg->ly; - ctrl_comp_.control_inputs_.rx = msg->rx; - ctrl_comp_.control_inputs_.ry = msg->ry; + ctrl_interfaces_.control_inputs_.command = msg->command; + ctrl_interfaces_.control_inputs_.lx = msg->lx; + ctrl_interfaces_.control_inputs_.ly = msg->ly; + ctrl_interfaces_.control_inputs_.rx = msg->rx; + ctrl_interfaces_.control_inputs_.ry = msg->ry; }); return CallbackReturn::SUCCESS; @@ -151,7 +151,7 @@ namespace rl_quadruped_controller { controller_interface::CallbackReturn LeggedGymController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // clear out vectors in case of restart - ctrl_comp_.clear(); + ctrl_interfaces_.clear(); // assign command interfaces for (auto &interface: command_interfaces_) { @@ -166,24 +166,24 @@ namespace rl_quadruped_controller { // assign state interfaces for (auto &interface: state_interfaces_) { if (interface.get_prefix_name() == imu_name_) { - ctrl_comp_.imu_state_interface_.emplace_back(interface); + ctrl_interfaces_.imu_state_interface_.emplace_back(interface); } else if (interface.get_prefix_name() == foot_force_name_) { - ctrl_comp_.foot_force_state_interface_.emplace_back(interface); + ctrl_interfaces_.foot_force_state_interface_.emplace_back(interface); } else { state_interface_map_[interface.get_interface_name()]->push_back(interface); } } // Create FSM List - state_list_.passive = std::make_shared(ctrl_comp_); - state_list_.fixedDown = std::make_shared(ctrl_comp_, down_pos_, stand_kp_, stand_kd_); - state_list_.fixedStand = std::make_shared(ctrl_comp_, stand_pos_, stand_kp_, stand_kd_); + state_list_.passive = std::make_shared(ctrl_interfaces_); + state_list_.fixedDown = std::make_shared(ctrl_interfaces_, down_pos_, stand_kp_, stand_kd_); + state_list_.fixedStand = std::make_shared(ctrl_interfaces_, stand_pos_, stand_kp_, stand_kd_); RCLCPP_INFO(get_node()->get_logger(), "Using robot model from %s", robot_pkg_.c_str()); const std::string package_share_directory = ament_index_cpp::get_package_share_directory(robot_pkg_); std::string model_path = package_share_directory + "/config/" + model_folder_; - state_list_.rl = std::make_shared(ctrl_comp_, model_path, stand_pos_); + state_list_.rl = std::make_shared(ctrl_interfaces_, ctrl_component_, model_path, stand_pos_); // Initialize FSM current_state_ = state_list_.passive; diff --git a/controllers/rl_quadruped_controller/src/RlQuadrupedController.h b/controllers/rl_quadruped_controller/src/RlQuadrupedController.h index abc7ccd..acba961 100644 --- a/controllers/rl_quadruped_controller/src/RlQuadrupedController.h +++ b/controllers/rl_quadruped_controller/src/RlQuadrupedController.h @@ -9,9 +9,9 @@ #include #include "rl_quadruped_controller/control/CtrlComponent.h" -#include "rl_quadruped_controller/FSM/StateFixedDown.h" +#include "controller_common/FSM/StateFixedDown.h" #include "rl_quadruped_controller/FSM/StateFixedStand.h" -#include "rl_quadruped_controller/FSM/StatePassive.h" +#include "controller_common//FSM/StatePassive.h" namespace rl_quadruped_controller { struct FSMStateList { @@ -56,7 +56,8 @@ namespace rl_quadruped_controller { private: std::shared_ptr getNextState(FSMStateName stateName) const; - CtrlComponent ctrl_comp_; + CtrlComponent ctrl_component_; + CtrlInterfaces ctrl_interfaces_; std::vector joint_names_; std::string base_name_ = "base"; std::vector feet_names_; @@ -96,19 +97,19 @@ namespace rl_quadruped_controller { std::unordered_map< std::string, std::vector > *> command_interface_map_ = { - {"effort", &ctrl_comp_.joint_torque_command_interface_}, - {"position", &ctrl_comp_.joint_position_command_interface_}, - {"velocity", &ctrl_comp_.joint_velocity_command_interface_}, - {"kp", &ctrl_comp_.joint_kp_command_interface_}, - {"kd", &ctrl_comp_.joint_kd_command_interface_} + {"effort", &ctrl_interfaces_.joint_torque_command_interface_}, + {"position", &ctrl_interfaces_.joint_position_command_interface_}, + {"velocity", &ctrl_interfaces_.joint_velocity_command_interface_}, + {"kp", &ctrl_interfaces_.joint_kp_command_interface_}, + {"kd", &ctrl_interfaces_.joint_kd_command_interface_} }; std::unordered_map< std::string, std::vector > *> state_interface_map_ = { - {"position", &ctrl_comp_.joint_position_state_interface_}, - {"effort", &ctrl_comp_.joint_effort_state_interface_}, - {"velocity", &ctrl_comp_.joint_velocity_state_interface_} + {"position", &ctrl_interfaces_.joint_position_state_interface_}, + {"effort", &ctrl_interfaces_.joint_effort_state_interface_}, + {"velocity", &ctrl_interfaces_.joint_velocity_state_interface_} }; rclcpp::Subscription::SharedPtr control_input_subscription_; diff --git a/controllers/rl_quadruped_controller/src/control/Estimator.cpp b/controllers/rl_quadruped_controller/src/control/Estimator.cpp index 5c64ec8..5f05a5c 100644 --- a/controllers/rl_quadruped_controller/src/control/Estimator.cpp +++ b/controllers/rl_quadruped_controller/src/control/Estimator.cpp @@ -4,14 +4,15 @@ #include "rl_quadruped_controller/control/Estimator.h" -#include +#include +#include #include "rl_quadruped_controller/control/CtrlComponent.h" -Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_component), - robot_model_(ctrl_component.robot_model_) { +Estimator::Estimator(CtrlInterfaces &ctrl_interfaces, CtrlComponent &ctrl_component) : ctrl_interfaces_(ctrl_interfaces), + robot_model_(ctrl_component.robot_model_){ g_ << 0, 0, -9.81; - dt_ = 1.0 / ctrl_component_.frequency_; + dt_ = 1.0 / ctrl_interfaces_.frequency_; std::cout << "dt: " << dt_ << std::endl; large_variance_ = 100; @@ -158,7 +159,7 @@ void Estimator::update() { // Adjust the covariance based on foot contact and phase. for (int i(0); i < 4; ++i) { - if (ctrl_component_.foot_force_state_interface_[i].get().get_value() < 1) { + if (ctrl_interfaces_.foot_force_state_interface_[i].get().get_value() < 1) { // foot not contact Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = large_variance_ * Eigen::MatrixXd::Identity(3, 3); R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = large_variance_ * Eigen::MatrixXd::Identity(3, 3); @@ -180,19 +181,19 @@ void Estimator::update() { } Quat quat; - quat << ctrl_component_.imu_state_interface_[0].get().get_value(), - ctrl_component_.imu_state_interface_[1].get().get_value(), - ctrl_component_.imu_state_interface_[2].get().get_value(), - ctrl_component_.imu_state_interface_[3].get().get_value(); + quat << ctrl_interfaces_.imu_state_interface_[0].get().get_value(), + ctrl_interfaces_.imu_state_interface_[1].get().get_value(), + ctrl_interfaces_.imu_state_interface_[2].get().get_value(), + ctrl_interfaces_.imu_state_interface_[3].get().get_value(); rotation_ = quatToRotMat(quat); - gyro_ << ctrl_component_.imu_state_interface_[4].get().get_value(), - ctrl_component_.imu_state_interface_[5].get().get_value(), - ctrl_component_.imu_state_interface_[6].get().get_value(); + gyro_ < -#include "rl_quadruped_controller/control/CtrlComponent.h" +#include "controller_common/CtrlInterfaces.h" #include "rl_quadruped_controller/robot/QuadrupedRobot.h" -QuadrupedRobot::QuadrupedRobot(CtrlComponent &ctrl_component, const std::string &robot_description, +QuadrupedRobot::QuadrupedRobot(CtrlInterfaces &ctrl_interfaces, const std::string &robot_description, const std::vector &feet_names, - const std::string &base_name) : ctrl_component_(ctrl_component) { + const std::string &base_name) : ctrl_interfaces_(ctrl_interfaces) { KDL::Tree robot_tree; kdl_parser::treeFromString(robot_description, robot_tree); @@ -114,15 +114,15 @@ void QuadrupedRobot::update() { if (mass_ == 0) return; for (int i = 0; i < 4; i++) { KDL::JntArray pos_array(3); - pos_array(0) = ctrl_component_.joint_position_state_interface_[i * 3].get().get_value(); - pos_array(1) = ctrl_component_.joint_position_state_interface_[i * 3 + 1].get().get_value(); - pos_array(2) = ctrl_component_.joint_position_state_interface_[i * 3 + 2].get().get_value(); + pos_array(0) = ctrl_interfaces_.joint_position_state_interface_[i * 3].get().get_value(); + pos_array(1) = ctrl_interfaces_.joint_position_state_interface_[i * 3 + 1].get().get_value(); + pos_array(2) = ctrl_interfaces_.joint_position_state_interface_[i * 3 + 2].get().get_value(); current_joint_pos_[i] = pos_array; KDL::JntArray vel_array(3); - vel_array(0) = ctrl_component_.joint_velocity_state_interface_[i * 3].get().get_value(); - vel_array(1) = ctrl_component_.joint_velocity_state_interface_[i * 3 + 1].get().get_value(); - vel_array(2) = ctrl_component_.joint_velocity_state_interface_[i * 3 + 2].get().get_value(); + vel_array(0) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3].get().get_value(); + vel_array(1) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3 + 1].get().get_value(); + vel_array(2) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3 + 2].get().get_value(); current_joint_vel_[i] = vel_array; } } diff --git a/controllers/rl_quadruped_controller/src/robot/RobotLeg.cpp b/controllers/rl_quadruped_controller/src/robot/RobotLeg.cpp index 134472c..6074230 100644 --- a/controllers/rl_quadruped_controller/src/robot/RobotLeg.cpp +++ b/controllers/rl_quadruped_controller/src/robot/RobotLeg.cpp @@ -5,8 +5,6 @@ #include #include "rl_quadruped_controller/robot/RobotLeg.h" -#include - RobotLeg::RobotLeg(const KDL::Chain &chain) { chain_ = chain; diff --git a/descriptions/unitree/a1_description/config/gazebo.yaml b/descriptions/unitree/a1_description/config/gazebo.yaml index e81c73d..9eb26b8 100644 --- a/descriptions/unitree/a1_description/config/gazebo.yaml +++ b/descriptions/unitree/a1_description/config/gazebo.yaml @@ -160,17 +160,16 @@ rl_quadruped_controller: update_rate: 200 # Hz robot_pkg: "a1_description" model_folder: "legged_gym" - command_prefix: "leg_pd_controller" joints: - FL_hip_joint - FL_thigh_joint - FL_calf_joint - - FR_hip_joint - - FR_thigh_joint - - FR_calf_joint - RL_hip_joint - RL_thigh_joint - RL_calf_joint + - FR_hip_joint + - FR_thigh_joint + - FR_calf_joint - RR_hip_joint - RR_thigh_joint - RR_calf_joint diff --git a/descriptions/unitree/a1_description/config/legged_gym/config.yaml b/descriptions/unitree/a1_description/config/legged_gym/config.yaml index 40df453..54abe69 100644 --- a/descriptions/unitree/a1_description/config/legged_gym/config.yaml +++ b/descriptions/unitree/a1_description/config/legged_gym/config.yaml @@ -15,10 +15,10 @@ clip_actions_upper: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] -rl_kp: [20.1, 20.1, 20.1, - 20.1, 20.1, 20.1, - 20.1, 20.1, 20.1, - 20.1, 20.1, 20.1] +rl_kp: [30, 30, 30, + 30, 30, 30, + 30, 30, 30, + 30, 30, 30] rl_kd: [0.754, 0.754, 0.754, 0.754, 0.754, 0.754, 0.754, 0.754, 0.754, diff --git a/descriptions/unitree/a1_description/xacro/gazebo.xacro b/descriptions/unitree/a1_description/xacro/gazebo.xacro index ba9dd39..4f48485 100644 --- a/descriptions/unitree/a1_description/xacro/gazebo.xacro +++ b/descriptions/unitree/a1_description/xacro/gazebo.xacro @@ -1,90 +1,138 @@ - + - gz_ros2_control/GazeboSimSystem + gz_quadruped_hardware/GazeboSimSystem - - + + + + + + - + + + + + - + + + + + - + + + + + - + + + + + - + + + + + - + + + + + - + + + + + - + + + + + - + + + + + - + + + + + - + + + + + @@ -102,11 +150,19 @@ + + + + + + + + - - $(find a1_description)/config/gazebo.yaml + + $(find go2_description)/config/gazebo.yaml @@ -115,7 +171,7 @@ name="gz::sim::systems::OdometryPublisher"> odom base - 500 + 1000 odom 3 odom_with_covariance @@ -194,4 +250,10 @@ true + + + + + + diff --git a/descriptions/unitree/go2_description/config/gazebo.yaml b/descriptions/unitree/go2_description/config/gazebo.yaml index e1af91f..cb4e753 100644 --- a/descriptions/unitree/go2_description/config/gazebo.yaml +++ b/descriptions/unitree/go2_description/config/gazebo.yaml @@ -135,8 +135,8 @@ unitree_guide_controller: rl_quadruped_controller: ros__parameters: update_rate: 200 # Hz - command_prefix: "leg_pd_controller" robot_pkg: "go2_description" + model_folder: "legged_gym" joints: - FL_hip_joint - FL_thigh_joint diff --git a/descriptions/unitree/go2_description/config/legged_gym/config.yaml b/descriptions/unitree/go2_description/config/legged_gym/config.yaml index 89f5400..ac43a20 100644 --- a/descriptions/unitree/go2_description/config/legged_gym/config.yaml +++ b/descriptions/unitree/go2_description/config/legged_gym/config.yaml @@ -15,14 +15,14 @@ clip_actions_upper: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] -rl_kp: [20, 20, 20, - 20, 20, 20, - 20, 20, 20, - 20, 20, 20] -rl_kd: [0.5, 0.5, 0.5, - 0.5, 0.5, 0.5, - 0.5, 0.5, 0.5, - 0.5, 0.5, 0.5] +rl_kp: [40, 40, 40, + 40, 40, 40, + 40, 40, 40, + 40, 40, 40] +rl_kd: [1, 1, 1, + 1, 1, 1, + 1, 1, 1, + 1, 1, 1] hip_scale_reduction: 1.0 hip_scale_reduction_indices: [0, 3, 6, 9] num_of_dofs: 12 diff --git a/descriptions/unitree/go2_description/config/legged_gym/himloco.pt b/descriptions/unitree/go2_description/config/legged_gym/himloco.pt new file mode 100644 index 0000000..b3886b1 Binary files /dev/null and b/descriptions/unitree/go2_description/config/legged_gym/himloco.pt differ diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/mathTools.h b/libraries/controller_common/include/controller_common/common/mathTools.h similarity index 100% rename from controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/mathTools.h rename to libraries/controller_common/include/controller_common/common/mathTools.h diff --git a/controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/mathTypes.h b/libraries/controller_common/include/controller_common/common/mathTypes.h similarity index 100% rename from controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/mathTypes.h rename to libraries/controller_common/include/controller_common/common/mathTypes.h diff --git a/libraries/gz_quadruped_playground/models/Lidar3DV1/materials/textures/lidar_3d_v1.jpg b/libraries/gz_quadruped_playground/models/Lidar3DV1/materials/textures/lidar_3d_v1.jpg new file mode 100644 index 0000000..328d3dc Binary files /dev/null and b/libraries/gz_quadruped_playground/models/Lidar3DV1/materials/textures/lidar_3d_v1.jpg differ diff --git a/libraries/gz_quadruped_playground/models/Lidar3DV1/meshes/lidar_3d_v1.dae b/libraries/gz_quadruped_playground/models/Lidar3DV1/meshes/lidar_3d_v1.dae new file mode 100644 index 0000000..da9eab8 --- /dev/null +++ b/libraries/gz_quadruped_playground/models/Lidar3DV1/meshes/lidar_3d_v1.dae @@ -0,0 +1,738 @@ + + + FBX COLLADA exporter2018-09-20T22:13:36Z2018-09-20T22:13:36ZZ_UP + + lidar_3d_v1.jpg + + + + + + + + + + + + + 0.000000 0.000000 0.000000 1.000000 + + + 0.588235 0.588235 0.588235 1.000000 + + + + + + TRUE + TRUE + ADD + + + + + + 0.000000 0.000000 0.000000 1.000000 + + + 2.000000 + + + 0.000000 0.000000 0.000000 1.000000 + + + 1.000000 + + + 1.000000 1.000000 1.000000 1.000000 + + + 0.000000 + + + + + + + + + + + +5.165000 0.000000 0.000000 +4.989007 1.336800 0.000000 +4.473021 2.582500 0.000000 +3.652206 3.652206 0.000000 +2.582500 4.473022 0.000000 +1.336800 4.989007 0.000000 +0.000000 5.165000 0.000000 +-1.336800 4.989007 0.000000 +-2.582499 4.473022 0.000000 +-3.652205 3.652207 0.000000 +-4.473021 2.582501 0.000000 +-4.989007 1.336802 0.000000 +-5.165000 0.000002 0.000000 +-4.989007 -1.336798 0.000000 +-4.473022 -2.582498 0.000000 +-3.652209 -3.652205 0.000000 +-2.582501 -4.473020 0.000000 +-1.336802 -4.989006 0.000000 +-0.000002 -5.165000 0.000000 +1.336798 -4.989007 0.000000 +2.582497 -4.473022 0.000000 +3.652204 -3.652209 0.000000 +4.473020 -2.582503 0.000000 +4.989006 -1.336804 0.000000 +5.077889 0.000000 7.170000 +4.904864 1.314255 7.170000 +4.397581 2.538944 7.170000 +3.590610 3.590609 7.170000 +2.538945 4.397585 7.170000 +1.314255 4.904868 7.170000 +0.000000 5.077893 7.170000 +-1.314254 4.904868 7.170000 +-2.538944 4.397585 7.170000 +-3.590610 3.590610 7.170000 +-4.397581 2.538945 7.170000 +-4.904864 1.314257 7.170000 +-5.077890 0.000002 7.170000 +-4.904865 -1.314251 7.170000 +-4.397583 -2.538944 7.170000 +-3.590612 -3.590610 7.170000 +-2.538947 -4.397577 7.170000 +-1.314256 -4.904860 7.170000 +-0.000002 -5.077887 7.170000 +1.314252 -4.904862 7.170000 +2.538942 -4.397579 7.170000 +3.590608 -3.590614 7.170000 +4.397579 -2.538949 7.170000 +4.904863 -1.314257 7.170000 +5.165000 0.000000 7.026600 +4.989007 1.336800 7.026600 +4.473021 2.582500 7.026600 +3.652206 3.652206 7.026600 +2.582500 4.473022 7.026600 +1.336800 4.989007 7.026600 +0.000000 5.165000 7.026600 +-1.336800 4.989007 7.026600 +-2.582499 4.473022 7.026600 +-3.652205 3.652207 7.026600 +-4.473021 2.582501 7.026600 +-4.989007 1.336802 7.026600 +-5.165000 0.000002 7.026600 +-4.989007 -1.336798 7.026600 +-4.473022 -2.582498 7.026600 +-3.652209 -3.652205 7.026600 +-2.582501 -4.473020 7.026600 +-1.336802 -4.989006 7.026600 +-0.000002 -5.165000 7.026600 +1.336798 -4.989007 7.026600 +2.582497 -4.473022 7.026600 +3.652204 -3.652209 7.026600 +4.473020 -2.582503 7.026600 +4.989006 -1.336804 7.026600 + + + + + + + + + + + +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +1.000000 -0.000000 0.000000 +0.965926 0.258819 0.000000 +0.965926 0.258819 0.000000 +0.965926 0.258819 0.000000 +1.000000 -0.000000 0.000000 +1.000000 -0.000000 0.000000 +0.866025 0.500000 0.000000 +0.866025 0.500000 0.000000 +0.965926 0.258819 0.000000 +0.965926 0.258819 0.000000 +0.965926 0.258819 0.000000 +0.866025 0.500000 0.000000 +0.707107 0.707107 0.000000 +0.707107 0.707107 0.000000 +0.866025 0.500000 0.000000 +0.866025 0.500000 0.000000 +0.866025 0.500000 0.000000 +0.707107 0.707107 0.000000 +0.500000 0.866025 0.000000 +0.500000 0.866025 0.000000 +0.707107 0.707107 0.000000 +0.707107 0.707107 0.000000 +0.707107 0.707107 0.000000 +0.500000 0.866025 0.000000 +0.258819 0.965926 0.000000 +0.258819 0.965926 0.000000 +0.500000 0.866025 0.000000 +0.500000 0.866025 0.000000 +0.500000 0.866025 0.000000 +0.258819 0.965926 0.000000 +-0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.258819 0.965926 0.000000 +0.258819 0.965926 0.000000 +0.258819 0.965926 0.000000 +-0.000000 1.000000 0.000000 +-0.258819 0.965926 0.000000 +-0.258819 0.965926 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +-0.000000 1.000000 0.000000 +-0.258819 0.965926 0.000000 +-0.500000 0.866025 0.000000 +-0.500000 0.866025 0.000000 +-0.258819 0.965926 0.000000 +-0.258819 0.965926 0.000000 +-0.258819 0.965926 0.000000 +-0.500000 0.866025 0.000000 +-0.707107 0.707107 0.000000 +-0.707107 0.707107 0.000000 +-0.500000 0.866025 0.000000 +-0.500000 0.866025 0.000000 +-0.500000 0.866025 0.000000 +-0.707107 0.707107 0.000000 +-0.866025 0.500000 0.000000 +-0.866025 0.500000 0.000000 +-0.707107 0.707107 0.000000 +-0.707107 0.707107 0.000000 +-0.707107 0.707107 0.000000 +-0.866025 0.500000 0.000000 +-0.965926 0.258819 0.000000 +-0.965926 0.258819 0.000000 +-0.866025 0.500000 0.000000 +-0.866025 0.500000 0.000000 +-0.866025 0.500000 0.000000 +-0.965926 0.258819 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-0.965926 0.258819 0.000000 +-0.965926 0.258819 0.000000 +-0.965926 0.258819 0.000000 +-1.000000 0.000000 0.000000 +-0.965926 -0.258819 0.000000 +-0.965926 -0.258819 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-0.965926 -0.258819 0.000000 +-0.866026 -0.500000 0.000000 +-0.866026 -0.500000 0.000000 +-0.965926 -0.258819 0.000000 +-0.965926 -0.258819 0.000000 +-0.965926 -0.258819 0.000000 +-0.866026 -0.500000 0.000000 +-0.707107 -0.707107 0.000000 +-0.707107 -0.707107 0.000000 +-0.866026 -0.500000 0.000000 +-0.866026 -0.500000 0.000000 +-0.866026 -0.500000 0.000000 +-0.707107 -0.707107 0.000000 +-0.500000 -0.866025 0.000000 +-0.500000 -0.866025 0.000000 +-0.707107 -0.707107 0.000000 +-0.707107 -0.707107 0.000000 +-0.707107 -0.707107 0.000000 +-0.500000 -0.866025 0.000000 +-0.258820 -0.965926 0.000000 +-0.258820 -0.965926 0.000000 +-0.500000 -0.866025 0.000000 +-0.500000 -0.866025 0.000000 +-0.500000 -0.866025 0.000000 +-0.258820 -0.965926 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +-0.258820 -0.965926 0.000000 +-0.258820 -0.965926 0.000000 +-0.258820 -0.965926 0.000000 +-0.000001 -1.000000 0.000000 +0.258819 -0.965926 0.000000 +0.258819 -0.965926 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +0.258819 -0.965926 0.000000 +0.499999 -0.866026 0.000000 +0.499999 -0.866026 0.000000 +0.258819 -0.965926 0.000000 +0.258819 -0.965926 0.000000 +0.258819 -0.965926 0.000000 +0.499999 -0.866026 0.000000 +0.707106 -0.707107 0.000000 +0.707106 -0.707107 0.000000 +0.499999 -0.866026 0.000000 +0.499999 -0.866026 0.000000 +0.499999 -0.866026 0.000000 +0.707106 -0.707107 0.000000 +0.866025 -0.500001 0.000000 +0.866025 -0.500001 0.000000 +0.707106 -0.707107 0.000000 +0.707106 -0.707107 0.000000 +0.707106 -0.707107 0.000000 +0.866025 -0.500001 0.000000 +0.965926 -0.258819 0.000000 +0.965926 -0.258819 0.000000 +0.866025 -0.500001 0.000000 +0.866025 -0.500001 0.000000 +0.866025 -0.500001 0.000000 +0.965926 -0.258819 0.000000 +1.000000 -0.000000 0.000000 +1.000000 -0.000000 0.000000 +0.965926 -0.258819 0.000000 +0.965926 -0.258819 0.000000 +0.965926 -0.258819 0.000000 +1.000000 -0.000000 0.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.825543 0.221204 0.519179 +0.854666 -0.000000 0.519179 +0.854666 -0.000000 0.519179 +0.854666 -0.000000 0.519179 +0.825543 0.221203 0.519180 +0.825543 0.221204 0.519179 +0.740161 0.427332 0.519181 +0.825543 0.221204 0.519179 +0.825543 0.221203 0.519180 +0.825543 0.221203 0.519180 +0.740161 0.427332 0.519181 +0.740161 0.427332 0.519181 +0.604340 0.604338 0.519182 +0.740161 0.427332 0.519181 +0.740161 0.427332 0.519181 +0.740161 0.427332 0.519181 +0.604340 0.604338 0.519181 +0.604340 0.604338 0.519182 +0.427336 0.740169 0.519166 +0.604340 0.604338 0.519182 +0.604340 0.604338 0.519181 +0.604340 0.604338 0.519181 +0.427337 0.740169 0.519166 +0.427336 0.740169 0.519166 +0.221206 0.825552 0.519165 +0.427336 0.740169 0.519166 +0.427337 0.740169 0.519166 +0.427337 0.740169 0.519166 +0.221206 0.825552 0.519165 +0.221206 0.825552 0.519165 +-0.000000 0.854676 0.519162 +0.221206 0.825552 0.519165 +0.221206 0.825552 0.519165 +0.221206 0.825552 0.519165 +-0.000000 0.854676 0.519162 +-0.000000 0.854676 0.519162 +-0.221206 0.825552 0.519164 +-0.000000 0.854676 0.519162 +-0.000000 0.854676 0.519162 +-0.000000 0.854676 0.519162 +-0.221206 0.825552 0.519164 +-0.221206 0.825552 0.519164 +-0.427337 0.740169 0.519166 +-0.221206 0.825552 0.519164 +-0.221206 0.825552 0.519164 +-0.221206 0.825552 0.519164 +-0.427338 0.740168 0.519166 +-0.427337 0.740169 0.519166 +-0.604340 0.604339 0.519180 +-0.427337 0.740169 0.519166 +-0.427338 0.740168 0.519166 +-0.427338 0.740168 0.519166 +-0.604339 0.604340 0.519180 +-0.604340 0.604339 0.519180 +-0.740162 0.427333 0.519179 +-0.604340 0.604339 0.519180 +-0.604339 0.604340 0.519180 +-0.604339 0.604340 0.519180 +-0.740162 0.427333 0.519179 +-0.740162 0.427333 0.519179 +-0.825544 0.221205 0.519178 +-0.740162 0.427333 0.519179 +-0.740162 0.427333 0.519179 +-0.740162 0.427333 0.519179 +-0.825544 0.221204 0.519177 +-0.825544 0.221205 0.519178 +-0.854668 0.000000 0.519175 +-0.825544 0.221205 0.519178 +-0.825544 0.221204 0.519177 +-0.825544 0.221204 0.519177 +-0.854668 0.000000 0.519175 +-0.854668 0.000000 0.519175 +-0.825544 -0.221202 0.519179 +-0.854668 0.000000 0.519175 +-0.854668 0.000000 0.519175 +-0.854668 0.000000 0.519175 +-0.825543 -0.221203 0.519179 +-0.825544 -0.221202 0.519179 +-0.740165 -0.427334 0.519174 +-0.825544 -0.221202 0.519179 +-0.825543 -0.221203 0.519179 +-0.825543 -0.221203 0.519179 +-0.740165 -0.427334 0.519174 +-0.740165 -0.427334 0.519174 +-0.604341 -0.604343 0.519174 +-0.740165 -0.427334 0.519174 +-0.740165 -0.427334 0.519174 +-0.740165 -0.427334 0.519174 +-0.604342 -0.604342 0.519174 +-0.604341 -0.604343 0.519174 +-0.427330 -0.740156 0.519190 +-0.604341 -0.604343 0.519174 +-0.604342 -0.604342 0.519174 +-0.604342 -0.604342 0.519174 +-0.427329 -0.740156 0.519190 +-0.427330 -0.740156 0.519190 +-0.221202 -0.825535 0.519193 +-0.427330 -0.740156 0.519190 +-0.427329 -0.740156 0.519190 +-0.427329 -0.740156 0.519190 +-0.221202 -0.825535 0.519193 +-0.221202 -0.825535 0.519193 +-0.000000 -0.854658 0.519192 +-0.221202 -0.825535 0.519193 +-0.221202 -0.825535 0.519193 +-0.221202 -0.825535 0.519193 +-0.000000 -0.854658 0.519191 +-0.000000 -0.854658 0.519192 +0.221202 -0.825536 0.519192 +-0.000000 -0.854658 0.519192 +-0.000000 -0.854658 0.519191 +-0.000000 -0.854658 0.519191 +0.221201 -0.825536 0.519193 +0.221202 -0.825536 0.519192 +0.427328 -0.740156 0.519191 +0.221202 -0.825536 0.519192 +0.221201 -0.825536 0.519193 +0.221201 -0.825536 0.519193 +0.427327 -0.740157 0.519190 +0.427328 -0.740156 0.519191 +0.604341 -0.604343 0.519174 +0.427328 -0.740156 0.519191 +0.427327 -0.740157 0.519190 +0.427327 -0.740157 0.519190 +0.604342 -0.604342 0.519174 +0.604341 -0.604343 0.519174 +0.740162 -0.427334 0.519178 +0.604341 -0.604343 0.519174 +0.604342 -0.604342 0.519174 +0.604342 -0.604342 0.519174 +0.740163 -0.427333 0.519178 +0.740162 -0.427334 0.519178 +0.825542 -0.221203 0.519181 +0.740162 -0.427334 0.519178 +0.740163 -0.427333 0.519178 +0.740163 -0.427333 0.519178 +0.825542 -0.221204 0.519182 +0.825542 -0.221203 0.519181 +0.854666 -0.000000 0.519179 +0.825542 -0.221203 0.519181 +0.825542 -0.221204 0.519182 +0.825542 -0.221204 0.519182 +0.854666 -0.000000 0.519179 +0.854666 -0.000000 0.519179 + + + + + + + + + + + +0.442003 0.737271 +0.462553 0.687661 +0.495242 0.645059 +0.537844 0.612370 +0.587454 0.591821 +0.640693 0.584812 +0.693931 0.591821 +0.000503 0.000500 +0.054201 0.000501 +0.054198 0.286050 +0.000500 0.286049 +0.107899 0.000502 +0.107897 0.286051 +0.161597 0.000502 +0.161595 0.286051 +0.215294 0.000503 +0.215293 0.286051 +0.268992 0.000503 +0.268992 0.286051 +0.322688 0.580495 +0.268990 0.580495 +0.268990 0.294947 +0.322688 0.294947 +0.215292 0.580495 +0.743542 0.612370 +0.786143 0.645059 +0.818832 0.687661 +0.839382 0.737271 +0.846391 0.790510 +0.839382 0.843749 +0.818832 0.893359 +0.322689 0.000503 +0.322690 0.286051 +0.376387 0.000503 +0.376388 0.286051 +0.430084 0.000503 +0.430086 0.286050 +0.483782 0.000502 +0.483784 0.286050 +0.537479 0.000502 +0.537482 0.286049 +0.591177 0.000501 +0.591181 0.286048 +0.215292 0.294947 +0.161594 0.580495 +0.161594 0.294947 +0.107896 0.580495 +0.786143 0.935960 +0.644875 0.000500 +0.644878 0.286048 +0.743541 0.968650 +0.693931 0.989199 +0.640693 0.996208 +0.587454 0.989199 +0.537843 0.968650 +0.495242 0.935960 +0.462553 0.893359 +0.442003 0.843749 +0.434994 0.790510 +0.404887 0.739823 +0.411896 0.793062 +0.404887 0.846301 +0.384338 0.895911 +0.351649 0.938513 +0.309047 0.971202 +0.259437 0.991751 +0.206198 0.998760 +0.152960 0.991751 +0.103349 0.971202 +0.060748 0.938513 +0.028058 0.895911 +0.007509 0.846301 +0.000500 0.793062 +0.007509 0.739824 +0.028058 0.690213 +0.060747 0.647612 +0.103349 0.614922 +0.152959 0.594373 +0.206198 0.587364 +0.259437 0.594373 +0.309047 0.614922 +0.351649 0.647611 +0.384338 0.690213 +0.107896 0.294947 +0.054198 0.580495 +0.054198 0.294947 +0.000500 0.580495 +0.000500 0.294947 +0.644876 0.580495 +0.591178 0.580495 +0.591179 0.294947 +0.644876 0.294947 +0.537480 0.580495 +0.537481 0.294947 +0.483782 0.580495 +0.483783 0.294947 +0.430084 0.580495 +0.430085 0.294947 +0.376386 0.580495 +0.376386 0.294947 +0.322688 0.300658 +0.268990 0.300658 +0.215292 0.300658 +0.161594 0.300658 +0.107896 0.300658 +0.054198 0.300658 +0.000500 0.300658 +0.000500 0.280338 +0.054198 0.280339 +0.107897 0.280340 +0.161595 0.280340 +0.215293 0.280340 +0.268992 0.280340 +0.322690 0.280340 +0.376388 0.280340 +0.430086 0.280339 +0.483784 0.280339 +0.537482 0.280338 +0.591181 0.280337 +0.644878 0.280337 +0.644876 0.300658 +0.591179 0.300658 +0.537481 0.300658 +0.483783 0.300658 +0.430085 0.300658 +0.376386 0.300658 + + + + + + + + + + + + 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3

22 0 1 21 1 2 20 2 3 20 3 3 19 4 4 18 5 5 18 6 5 17 7 6 16 8 24 20 9 3 18 10 5 16 11 24 16 12 24 15 13 25 14 14 26 14 15 26 13 16 27 12 17 28 16 18 24 14 19 26 12 20 28 12 21 28 11 22 29 10 23 30 10 24 30 9 25 47 8 26 50 12 27 28 10 28 30 8 29 50 16 30 24 12 31 28 8 32 50 8 33 50 7 34 51 6 35 52 6 36 52 5 37 53 4 38 54 8 39 50 6 40 52 4 41 54 4 42 54 3 43 55 2 44 56 2 45 56 1 46 57 0 47 58 4 48 54 2 49 56 0 50 58 8 51 50 4 52 54 0 53 58 16 54 24 8 55 50 0 56 58 20 57 3 16 58 24 0 59 58 22 60 1 20 61 3 0 62 58 23 63 0 22 64 1 0 65 58 0 66 19 1 67 20 49 68 101 49 69 101 48 70 100 0 71 19 2 72 23 50 73 102 49 74 101 49 75 101 1 76 20 2 77 23 3 78 44 51 79 103 50 80 102 50 81 102 2 82 23 3 83 44 4 84 46 52 85 104 51 86 103 51 87 103 3 88 44 4 89 46 5 90 84 53 91 105 52 92 104 52 93 104 4 94 46 5 95 84 6 96 86 54 97 106 53 98 105 53 99 105 5 100 84 6 101 86 7 102 8 55 103 108 54 104 107 54 105 107 6 106 7 7 107 8 8 108 11 56 109 109 55 110 108 55 111 108 7 112 8 8 113 11 9 114 13 57 115 110 56 116 109 56 117 109 8 118 11 9 119 13 10 120 15 58 121 111 57 122 110 57 123 110 9 124 13 10 125 15 11 126 17 59 127 112 58 128 111 58 129 111 10 130 15 11 131 17 12 132 31 60 133 113 59 134 112 59 135 112 11 136 17 12 137 31 13 138 33 61 139 114 60 140 113 60 141 113 12 142 31 13 143 33 14 144 35 62 145 115 61 146 114 61 147 114 13 148 33 14 149 35 15 150 37 63 151 116 62 152 115 62 153 115 14 154 35 15 155 37 16 156 39 64 157 117 63 158 116 63 159 116 15 160 37 16 161 39 17 162 41 65 163 118 64 164 117 64 165 117 16 166 39 17 167 41 18 168 48 66 169 119 65 170 118 65 171 118 17 172 41 18 173 48 19 174 89 67 175 121 66 176 120 66 177 120 18 178 88 19 179 89 20 180 92 68 181 122 67 182 121 67 183 121 19 184 89 20 185 92 21 186 94 69 187 123 68 188 122 68 189 122 20 190 92 21 191 94 22 192 96 70 193 124 69 194 123 69 195 123 21 196 94 22 197 96 23 198 98 71 199 125 70 200 124 70 201 124 22 202 96 23 203 98 0 204 19 48 205 100 71 206 125 71 207 125 23 208 98 0 209 19 24 210 60 25 211 61 26 212 62 26 213 62 27 214 63 28 215 64 28 216 64 29 217 65 30 218 66 26 219 62 28 220 64 30 221 66 30 222 66 31 223 67 32 224 68 32 225 68 33 226 69 34 227 70 30 228 66 32 229 68 34 230 70 34 231 70 35 232 71 36 233 72 36 234 72 37 235 73 38 236 74 34 237 70 36 238 72 38 239 74 30 240 66 34 241 70 38 242 74 38 243 74 39 244 75 40 245 76 40 246 76 41 247 77 42 248 78 38 249 74 40 250 76 42 251 78 42 252 78 43 253 79 44 254 80 44 255 80 45 256 81 46 257 82 42 258 78 44 259 80 46 260 82 38 261 74 42 262 78 46 263 82 30 264 66 38 265 74 46 266 82 26 267 62 30 268 66 46 269 82 24 270 60 26 271 62 46 272 82 47 273 59 24 274 60 46 275 82 25 276 21 24 277 22 48 278 100 48 279 100 49 280 101 25 281 21 26 282 43 25 283 21 49 284 101 49 285 101 50 286 102 26 287 43 27 288 45 26 289 43 50 290 102 50 291 102 51 292 103 27 293 45 28 294 83 27 295 45 51 296 103 51 297 103 52 298 104 28 299 83 29 300 85 28 301 83 52 302 104 52 303 104 53 304 105 29 305 85 30 306 87 29 307 85 53 308 105 53 309 105 54 310 106 30 311 87 31 312 9 30 313 10 54 314 107 54 315 107 55 316 108 31 317 9 32 318 12 31 319 9 55 320 108 55 321 108 56 322 109 32 323 12 33 324 14 32 325 12 56 326 109 56 327 109 57 328 110 33 329 14 34 330 16 33 331 14 57 332 110 57 333 110 58 334 111 34 335 16 35 336 18 34 337 16 58 338 111 58 339 111 59 340 112 35 341 18 36 342 32 35 343 18 59 344 112 59 345 112 60 346 113 36 347 32 37 348 34 36 349 32 60 350 113 60 351 113 61 352 114 37 353 34 38 354 36 37 355 34 61 356 114 61 357 114 62 358 115 38 359 36 39 360 38 38 361 36 62 362 115 62 363 115 63 364 116 39 365 38 40 366 40 39 367 38 63 368 116 63 369 116 64 370 117 40 371 40 41 372 42 40 373 40 64 374 117 64 375 117 65 376 118 41 377 42 42 378 49 41 379 42 65 380 118 65 381 118 66 382 119 42 383 49 43 384 90 42 385 91 66 386 120 66 387 120 67 388 121 43 389 90 44 390 93 43 391 90 67 392 121 67 393 121 68 394 122 44 395 93 45 396 95 44 397 93 68 398 122 68 399 122 69 400 123 45 401 95 46 402 97 45 403 95 69 404 123 69 405 123 70 406 124 46 407 97 47 408 99 46 409 97 70 410 124 70 411 124 71 412 125 47 413 99 24 414 22 47 415 99 71 416 125 71 417 125 48 418 100 24 419 22

 + + + + + + + + 0.149020 0.149020 0.149020 + + + + + + + 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.0000001.000000 + 30.0000000.0000003.333333 + + + + + + + + + diff --git a/libraries/gz_quadruped_playground/models/Lidar3DV1/meshes/lidar_3d_v1.jpg b/libraries/gz_quadruped_playground/models/Lidar3DV1/meshes/lidar_3d_v1.jpg new file mode 100644 index 0000000..328d3dc Binary files /dev/null and b/libraries/gz_quadruped_playground/models/Lidar3DV1/meshes/lidar_3d_v1.jpg differ diff --git a/libraries/gz_quadruped_playground/models/Lidar3DV1/model.config b/libraries/gz_quadruped_playground/models/Lidar3DV1/model.config new file mode 100644 index 0000000..7d0d5dc --- /dev/null +++ b/libraries/gz_quadruped_playground/models/Lidar3DV1/model.config @@ -0,0 +1,15 @@ + + + + Lidar 3d v1 + 1.0 + model.sdf + + + Cole Biesemeyer + + + + A generic 3d lidar. + + diff --git a/libraries/gz_quadruped_playground/models/Lidar3DV1/model.sdf b/libraries/gz_quadruped_playground/models/Lidar3DV1/model.sdf new file mode 100644 index 0000000..c643510 --- /dev/null +++ b/libraries/gz_quadruped_playground/models/Lidar3DV1/model.sdf @@ -0,0 +1,36 @@ + + + + + 0 0 0 0 0 0 + + 0 0 0.03585 0 0 0 + 0.83 + + 0.0009080594249999999 + 0 + 0 + 0.0009080594249999999 + 0 + 0.0011049624 + + + + 0 0 0.03585 0 0 0 + + + 0.0516 + 0.0717 + + + + + + + model://lidar_3d_v1/meshes/lidar_3d_v1.dae + + + + + + diff --git a/libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/1.jpg b/libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/1.jpg new file mode 100644 index 0000000..0f1ed89 Binary files /dev/null and b/libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/1.jpg differ diff --git a/libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/2.jpg b/libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/2.jpg new file mode 100644 index 0000000..2b569e4 Binary files /dev/null and b/libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/2.jpg differ diff --git a/libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/3.jpg b/libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/3.jpg new file mode 100644 index 0000000..d4de4b0 Binary files /dev/null and b/libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/3.jpg differ diff --git a/libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/4.jpg b/libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/4.jpg new file mode 100644 index 0000000..72e82c3 Binary files /dev/null and b/libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/4.jpg differ diff --git a/libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/5.jpg b/libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/5.jpg new file mode 100644 index 0000000..2ef5f86 Binary files /dev/null and b/libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/5.jpg differ