EmbodiedAI
/
quadruped_ros2_control
mirror of https://github.com/legubiao/quadruped_ros2_control.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

simplified structure for rl controller

This commit is contained in:
Huang Zhenbiao 2025-03-03 23:37:42 +08:00
parent 354e235fdf
commit f2062983fd
41 changed files with 1201 additions and 568 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
controllers/rl_quadruped_controller/CMakeLists.txt
View File

@ -27,6 +27,7 @@ set(dependencies
control_input_msgs control_input_msgs
kdl_parser kdl_parser
ament_index_cpp ament_index_cpp
controller_common
) )
# find dependencies # find dependencies
@ -39,9 +40,7 @@ add_library(${PROJECT_NAME} SHARED
src/common/ObservationBuffer.cpp src/common/ObservationBuffer.cpp
src/FSM/StatePassive.cpp
src/FSM/StateFixedStand.cpp src/FSM/StateFixedStand.cpp
src/FSM/StateFixedDown.cpp
src/FSM/StateRL.cpp src/FSM/StateRL.cpp
src/control/LowPassFilter.cpp src/control/LowPassFilter.cpp

2
controllers/rl_quadruped_controller/README.md
View File

@ -32,7 +32,7 @@ echo 'export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:~/CLionProjects/libtorch/lib' >> ~
```bash ```bash
cd ~/ros2_ws cd ~/ros2_ws
colcon build --packages-up-to rl_quadruped_controller colcon build --packages-up-to rl_quadruped_controller --symlink-install
``` ```
## 3. Launch ## 3. Launch

40
controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/FSMState.h
View File

@ -1,40 +0,0 @@
//
// Created by tlab-uav on 24-9-6.
//
#ifndef FSMSTATE_H
#define FSMSTATE_H
#include <string>
#include <utility>
#include <rclcpp/time.hpp>
#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

38
controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedDown.h
View File

@ -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<double> &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

23
controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedStand.h
View File

@ -5,33 +5,16 @@
#ifndef STATEFIXEDSTAND_H #ifndef STATEFIXEDSTAND_H
#define STATEFIXEDSTAND_H #define STATEFIXEDSTAND_H
#include "FSMState.h" #include <controller_common/FSM/BaseFixedStand.h>
class StateFixedStand final : public FSMState { class StateFixedStand final : public BaseFixedStand {
public: public:
explicit StateFixedStand(CtrlComponent &ctrlComp, explicit StateFixedStand(CtrlInterfaces &ctrl_interfaces,
const std::vector<double> &target_pos, const std::vector<double> &target_pos,
double kp, double kp,
double kd); double kd);
void enter() override;
void run() override;
void exit() override;
FSMStateName checkChange() 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;
}; };

24
controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StatePassive.h
View File

@ -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

65
controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h
View File

@ -6,12 +6,16 @@
#define STATERL_H #define STATERL_H
#include <common/ObservationBuffer.h> #include <common/ObservationBuffer.h>
#include <rl_quadruped_controller/control/CtrlComponent.h>
#include <torch/script.h> #include <torch/script.h>
#include "FSMState.h" #include "controller_common/FSM/FSMState.h"
struct CtrlComponent;
template <typename Functor> template <typename Functor>
void executeAndSleep(Functor f, const double frequency) { void executeAndSleep(Functor f, const double frequency)
{
using clock = std::chrono::high_resolution_clock; using clock = std::chrono::high_resolution_clock;
const auto start = clock::now(); const auto start = clock::now();
@ -27,9 +31,28 @@ void executeAndSleep(Functor f, const double frequency) {
std::this_thread::sleep_until(sleepTill); std::this_thread::sleep_until(sleepTill);
} }
template<typename T> inline void setThreadPriority(int priority, std::thread& thread)
struct RobotCommand { {
struct MotorCommand { 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 <typename T>
struct RobotCommand
{
struct MotorCommand
{
std::vector<T> q = std::vector<T>(32, 0.0); std::vector<T> q = std::vector<T>(32, 0.0);
std::vector<T> dq = std::vector<T>(32, 0.0); std::vector<T> dq = std::vector<T>(32, 0.0);
std::vector<T> tau = std::vector<T>(32, 0.0); std::vector<T> tau = std::vector<T>(32, 0.0);
@ -38,15 +61,18 @@ struct RobotCommand {
} motor_command; } motor_command;
}; };
template<typename T> template <typename T>
struct RobotState { struct RobotState
struct IMU { {
struct IMU
{
std::vector<T> quaternion = {1.0, 0.0, 0.0, 0.0}; // w, x, y, z std::vector<T> quaternion = {1.0, 0.0, 0.0, 0.0}; // w, x, y, z
std::vector<T> gyroscope = {0.0, 0.0, 0.0}; std::vector<T> gyroscope = {0.0, 0.0, 0.0};
std::vector<T> accelerometer = {0.0, 0.0, 0.0}; std::vector<T> accelerometer = {0.0, 0.0, 0.0};
} imu; } imu;
struct MotorState { struct MotorState
{
std::vector<T> q = std::vector<T>(32, 0.0); std::vector<T> q = std::vector<T>(32, 0.0);
std::vector<T> dq = std::vector<T>(32, 0.0); std::vector<T> dq = std::vector<T>(32, 0.0);
std::vector<T> ddq = std::vector<T>(32, 0.0); std::vector<T> ddq = std::vector<T>(32, 0.0);
@ -62,7 +88,8 @@ struct Control
double yaw = 0.0; double yaw = 0.0;
}; };
struct ModelParams { struct ModelParams
{
std::string model_name; std::string model_name;
std::string framework; std::string framework;
int decimation; int decimation;
@ -101,13 +128,17 @@ struct Observations
torch::Tensor actions; torch::Tensor actions;
}; };
class StateRL final : public FSMState { class StateRL final : public FSMState
{
public: public:
explicit StateRL(CtrlComponent &ctrl_component, const std::string &config_path, const std::vector<double> &target_pos); explicit StateRL(CtrlInterfaces& ctrl_interfaces,
CtrlComponent& ctrl_component, const std::string& config_path,
const std::vector<double>& target_pos);
void enter() override; void enter() override;
void run() override; void run(const rclcpp::Time& time,
const rclcpp::Duration& period) override;
void exit() override; void exit() override;
@ -116,9 +147,10 @@ public:
private: private:
torch::Tensor computeObservation(); 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 * @brief Forward the RL model to get the action
@ -131,6 +163,9 @@ private:
void setCommand() const; void setCommand() const;
bool enable_estimator_;
std::shared_ptr<Estimator>& estimator_;
// Parameters // Parameters
ModelParams params_; ModelParams params_;
Observations obs_; Observations obs_;

22
controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/enumClass.h
View File

@ -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

53
controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/CtrlComponent.h
View File

@ -2,65 +2,18 @@
// Created by biao on 24-9-10. // Created by biao on 24-9-10.
// //
#ifndef INTERFACE_H #ifndef CtrlComponent_H
#define INTERFACE_H #define CtrlComponent_H
#include <vector>
#include <hardware_interface/loaned_command_interface.hpp>
#include <hardware_interface/loaned_state_interface.hpp>
#include <control_input_msgs/msg/inputs.hpp>
#include "Estimator.h" #include "Estimator.h"
struct CtrlComponent { struct CtrlComponent {
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_torque_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_position_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_velocity_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_kp_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_kd_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
joint_effort_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
joint_position_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
joint_velocity_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
imu_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
foot_force_state_interface_;
control_input_msgs::msg::Inputs control_inputs_;
int frequency_{};
bool enable_estimator_ = false; bool enable_estimator_ = false;
std::shared_ptr<QuadrupedRobot> robot_model_; std::shared_ptr<QuadrupedRobot> robot_model_;
std::shared_ptr<Estimator> estimator_; std::shared_ptr<Estimator> estimator_;
CtrlComponent() = default; 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

6
controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/Estimator.h
View File

@ -6,7 +6,7 @@
#define ESTIMATOR_H #define ESTIMATOR_H
#include <memory> #include <memory>
#include <kdl/frames.hpp> #include <kdl/frames.hpp>
#include <rl_quadruped_controller/common/mathTypes.h> #include <controller_common/common/mathTypes.h>
#include <rl_quadruped_controller/robot/QuadrupedRobot.h> #include <rl_quadruped_controller/robot/QuadrupedRobot.h>
#include "LowPassFilter.h" #include "LowPassFilter.h"
@ -16,7 +16,7 @@ struct CtrlComponent;
class Estimator { class Estimator {
public: public:
explicit Estimator(CtrlComponent &ctrl_component); explicit Estimator(CtrlInterfaces &ctrl_interfaces, CtrlComponent &ctrl_component);
~Estimator() = default; ~Estimator() = default;
@ -104,7 +104,7 @@ public:
void update(); void update();
private: private:
CtrlComponent &ctrl_component_; CtrlInterfaces &ctrl_interfaces_;
std::shared_ptr<QuadrupedRobot> &robot_model_; std::shared_ptr<QuadrupedRobot> &robot_model_;
Eigen::Matrix<double, 18, 1> x_hat_; // The state of estimator, position(3)+velocity(3)+feet position(3x4) Eigen::Matrix<double, 18, 1> x_hat_; // The state of estimator, position(3)+velocity(3)+feet position(3x4)

8
controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/QuadrupedRobot.h
View File

@ -7,16 +7,16 @@
#define QUADRUPEDROBOT_H #define QUADRUPEDROBOT_H
#include <string> #include <string>
#include <kdl_parser/kdl_parser.hpp> #include <kdl_parser/kdl_parser.hpp>
#include <rl_quadruped_controller/common/mathTypes.h> #include <controller_common/common/mathTypes.h>
#include "RobotLeg.h" #include "RobotLeg.h"
struct CtrlComponent; struct CtrlInterfaces;
class QuadrupedRobot { class QuadrupedRobot {
public: public:
explicit QuadrupedRobot(CtrlComponent &ctrl_component, const std::string &robot_description, explicit QuadrupedRobot(CtrlInterfaces &ctrl_interfaces, const std::string &robot_description,
const std::vector<std::string> &feet_names, const std::string &base_name); const std::vector<std::string> &feet_names, const std::string &base_name);
~QuadrupedRobot() = default; ~QuadrupedRobot() = default;
@ -91,7 +91,7 @@ public:
void update(); void update();
private: private:
CtrlComponent &ctrl_component_; CtrlInterfaces &ctrl_interfaces_;
std::vector<std::shared_ptr<RobotLeg> > robot_legs_; std::vector<std::shared_ptr<RobotLeg> > robot_legs_;
KDL::Chain fr_chain_; KDL::Chain fr_chain_;

2
controllers/rl_quadruped_controller/include/rl_quadruped_controller/robot/RobotLeg.h
View File

@ -9,7 +9,7 @@
#include <kdl/chainfksolverpos_recursive.hpp> #include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainiksolverpos_lma.hpp> #include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainjnttojacsolver.hpp> #include <kdl/chainjnttojacsolver.hpp>
#include <rl_quadruped_controller/common/mathTypes.h> #include <controller_common/common/mathTypes.h>
class RobotLeg { class RobotLeg {
public: public:

25
controllers/rl_quadruped_controller/launch/gazebo.launch.py
View File

@ -34,7 +34,7 @@ def launch_setup(context, *args, **kwargs):
executable='create', executable='create',
output='screen', output='screen',
arguments=['-topic', 'robot_description', '-name', arguments=['-topic', 'robot_description', '-name',
'robot', '-allow_renaming', 'true', '-z', init_height] 'robot', '-allow_renaming', 'true', '-z', init_height],
) )
robot_state_publisher = Node( robot_state_publisher = Node(
@ -65,20 +65,12 @@ def launch_setup(context, *args, **kwargs):
"--controller-manager", "/controller_manager"], "--controller-manager", "/controller_manager"],
) )
leg_pd_controller = Node( rl_quadruped_controller = Node(
package="controller_manager", package="controller_manager",
executable="spawner", executable="spawner",
arguments=["leg_pd_controller", arguments=["rl_quadruped_controller", "--controller-manager", "/controller_manager"],
"--controller-manager", "/controller_manager"],
) )
controller = Node(
package="controller_manager",
executable="spawner",
arguments=["rl_quadruped_controller", "--controller-manager", "/controller_manager"]
)
return [ return [
rviz, rviz,
Node( Node(
@ -95,11 +87,16 @@ def launch_setup(context, *args, **kwargs):
launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]), launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
robot_state_publisher, robot_state_publisher,
gz_spawn_entity, gz_spawn_entity,
leg_pd_controller,
RegisterEventHandler( RegisterEventHandler(
event_handler=OnProcessExit( event_handler=OnProcessExit(
target_action=leg_pd_controller, target_action=gz_spawn_entity,
on_exit=[imu_sensor_broadcaster, joint_state_publisher, controller], on_exit=[imu_sensor_broadcaster, joint_state_publisher],
)
),
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_publisher,
on_exit=[rl_quadruped_controller],
) )
), ),
] ]

1
controllers/rl_quadruped_controller/package.xml
View File

@ -9,6 +9,7 @@
<buildtool_depend>ament_cmake</buildtool_depend> <buildtool_depend>ament_cmake</buildtool_depend>
<depend>controller_common</depend>
<depend>backward_ros</depend> <depend>backward_ros</depend>
<depend>controller_interface</depend> <depend>controller_interface</depend>
<depend>pluginlib</depend> <depend>pluginlib</depend>

62
controllers/rl_quadruped_controller/src/FSM/StateFixedDown.cpp
View File

@ -1,62 +0,0 @@
//
// Created by tlab-uav on 24-9-11.
//
#include "rl_quadruped_controller/FSM/StateFixedDown.h"
#include <cmath>
StateFixedDown::StateFixedDown(CtrlComponent &ctrlComp,
const std::vector<double> &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;
}
}

40
controllers/rl_quadruped_controller/src/FSM/StateFixedStand.cpp
View File

@ -4,51 +4,17 @@
#include "rl_quadruped_controller/FSM/StateFixedStand.h" #include "rl_quadruped_controller/FSM/StateFixedStand.h"
#include <cmath> StateFixedStand::StateFixedStand(CtrlInterfaces &ctrl_interfaces, const std::vector<double> &target_pos,
StateFixedStand::StateFixedStand(CtrlComponent &ctrlComp, const std::vector<double> &target_pos,
const double kp, const double kp,
const double kd) const double kd)
: FSMState(FSMStateName::FIXEDSTAND, "fixed stand", ctrlComp), : BaseFixedStand(ctrl_interfaces, target_pos, kp, kd) {
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;
} }
FSMStateName StateFixedStand::checkChange() { FSMStateName StateFixedStand::checkChange() {
if (percent_ < 1.5) { if (percent_ < 1.5) {
return FSMStateName::FIXEDSTAND; return FSMStateName::FIXEDSTAND;
} }
switch (ctrl_comp_.control_inputs_.command) { switch (ctrl_interfaces_.control_inputs_.command) {
case 1: case 1:
return FSMStateName::PASSIVE; return FSMStateName::PASSIVE;
case 2: case 2:

43
controllers/rl_quadruped_controller/src/FSM/StatePassive.cpp
View File

@ -1,43 +0,0 @@
//
// Created by tlab-uav on 24-9-6.
//
#include "rl_quadruped_controller/FSM/StatePassive.h"
#include <iostream>
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;
}

254
controllers/rl_quadruped_controller/src/FSM/StateRL.cpp
View File

@ -7,42 +7,55 @@
#include <rclcpp/logging.hpp> #include <rclcpp/logging.hpp>
#include <yaml-cpp/yaml.h> #include <yaml-cpp/yaml.h>
template<typename T> template <typename T>
std::vector<T> ReadVectorFromYaml(const YAML::Node &node) { std::vector<T> ReadVectorFromYaml(const YAML::Node& node)
{
std::vector<T> values; std::vector<T> values;
for (const auto &val: node) { for (const auto& val : node)
{
values.push_back(val.as<T>()); values.push_back(val.as<T>());
} }
return values; return values;
} }
template<typename T> template <typename T>
std::vector<T> ReadVectorFromYaml(const YAML::Node &node, const std::string &framework, const int &rows, std::vector<T> ReadVectorFromYaml(const YAML::Node& node, const std::string& framework, const int& rows,
const int &cols) { const int& cols)
{
std::vector<T> values; std::vector<T> values;
for (const auto &val: node) { for (const auto& val : node)
{
values.push_back(val.as<T>()); values.push_back(val.as<T>());
} }
if (framework == "isaacsim") { if (framework == "isaacsim")
{
std::vector<T> transposed_values(cols * rows); std::vector<T> transposed_values(cols * rows);
for (int r = 0; r < rows; ++r) { for (int r = 0; r < rows; ++r)
for (int c = 0; c < cols; ++c) { {
for (int c = 0; c < cols; ++c)
{
transposed_values[c * rows + r] = values[r * cols + c]; transposed_values[c * rows + r] = values[r * cols + c];
} }
} }
return transposed_values; return transposed_values;
} }
if (framework == "isaacgym") { if (framework == "isaacgym")
{
return values; return values;
} }
throw std::invalid_argument("Unsupported framework: " + framework); throw std::invalid_argument("Unsupported framework: " + framework);
} }
StateRL::StateRL(CtrlComponent &ctrl_component, const std::string &config_path, StateRL::StateRL(CtrlInterfaces& ctrl_interfaces,
const std::vector<double> &target_pos) : FSMState( CtrlComponent& ctrl_component, const std::string& config_path,
FSMStateName::RL, "rl", ctrl_component) { const std::vector<double>& target_pos) : FSMState(
for (int i = 0; i < 12; i++) { 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]; init_pos_[i] = target_pos[i];
} }
@ -50,7 +63,8 @@ StateRL::StateRL(CtrlComponent &ctrl_component, const std::string &config_path,
loadYaml(config_path); loadYaml(config_path);
// history // history
if (!params_.observations_history.empty()) { if (!params_.observations_history.empty())
{
history_obs_buf_ = std::make_shared<ObservationBuffer>(1, params_.num_observations, history_obs_buf_ = std::make_shared<ObservationBuffer>(1, params_.num_observations,
params_.observations_history.size()); 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); model_ = torch::jit::load(config_path + "/" + params_.model_name);
// for (const auto &param: model_.parameters()) { // for (const auto &param: model_.parameters()) {
// std::cout << "Parameter dtype: " << param.dtype() << std::endl; // std::cout << "Parameter dtype: " << param.dtype() << std::endl;
// } // }
rl_thread_ = std::thread([&] { rl_thread_ = std::thread([&]
while (true) { {
try { while (true)
{
try
{
executeAndSleep( executeAndSleep(
[&] { [&]
if (running_) { {
if (running_)
{
runModel(); runModel();
} }
}, },
ctrl_comp_.frequency_ / params_.decimation); ctrl_interfaces_.frequency_ / params_.decimation);
} catch (const std::exception &e) { }
catch (const std::exception& e)
{
running_ = false; running_ = false;
RCLCPP_ERROR(rclcpp::get_logger("StateRL"), "Error in RL thread: %s", e.what()); 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 // Init observations
obs_.lin_vel = torch::tensor({{0.0, 0.0, 0.0}}); obs_.lin_vel = torch::tensor({{0.0, 0.0, 0.0}});
obs_.ang_vel = torch::tensor({{0.0, 0.0, 0.0}}); obs_.ang_vel = torch::tensor({{0.0, 0.0, 0.0}});
@ -106,17 +128,21 @@ void StateRL::enter() {
running_ = true; running_ = true;
} }
void StateRL::run() { void StateRL::run(const rclcpp::Time&/*time*/, const rclcpp::Duration&/*period*/)
{
getState(); getState();
setCommand(); setCommand();
} }
void StateRL::exit() { void StateRL::exit()
{
running_ = false; running_ = false;
} }
FSMStateName StateRL::checkChange() { FSMStateName StateRL::checkChange()
switch (ctrl_comp_.control_inputs_.command) { {
switch (ctrl_interfaces_.control_inputs_.command)
{
case 1: case 1:
return FSMStateName::PASSIVE; return FSMStateName::PASSIVE;
case 2: case 2:
@ -126,24 +152,39 @@ FSMStateName StateRL::checkChange() {
} }
} }
torch::Tensor StateRL::computeObservation() { torch::Tensor StateRL::computeObservation()
{
std::vector<torch::Tensor> obs_list; std::vector<torch::Tensor> obs_list;
for (const std::string &observation: params_.observations) { for (const std::string& observation : params_.observations)
if (observation == "lin_vel") { {
if (observation == "lin_vel")
{
obs_list.push_back(obs_.lin_vel * params_.lin_vel_scale); 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( obs_list.push_back(
quatRotateInverse(obs_.base_quat, obs_.ang_vel, params_.framework) * params_.ang_vel_scale); 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)); 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); 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); 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); 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); obs_list.push_back(obs_.actions);
} }
} }
@ -155,11 +196,15 @@ torch::Tensor StateRL::computeObservation() {
return clamped_obs; return clamped_obs;
} }
void StateRL::loadYaml(const std::string &config_path) { void StateRL::loadYaml(const std::string& config_path)
{
YAML::Node config; YAML::Node config;
try { try
{
config = YAML::LoadFile(config_path + "/config.yaml"); 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()); RCLCPP_ERROR(rclcpp::get_logger("StateRL"), "The file '%s' does not exist", config_path.c_str());
return; return;
} }
@ -170,19 +215,25 @@ void StateRL::loadYaml(const std::string &config_path) {
params_.framework = config["framework"].as<std::string>(); params_.framework = config["framework"].as<std::string>();
const int rows = config["rows"].as<int>(); const int rows = config["rows"].as<int>();
const int cols = config["cols"].as<int>(); const int cols = config["cols"].as<int>();
if (config["observations_history"].IsNull()) { if (config["observations_history"].IsNull())
{
params_.observations_history = {}; params_.observations_history = {};
} else { }
else
{
params_.observations_history = ReadVectorFromYaml<int>(config["observations_history"]); params_.observations_history = ReadVectorFromYaml<int>(config["observations_history"]);
} }
params_.decimation = config["decimation"].as<int>(); params_.decimation = config["decimation"].as<int>();
params_.num_observations = config["num_observations"].as<int>(); params_.num_observations = config["num_observations"].as<int>();
params_.observations = ReadVectorFromYaml<std::string>(config["observations"]); params_.observations = ReadVectorFromYaml<std::string>(config["observations"]);
params_.clip_obs = config["clip_obs"].as<double>(); params_.clip_obs = config["clip_obs"].as<double>();
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_upper = torch::tensor({}).view({1, -1});
params_.clip_actions_lower = torch::tensor({}).view({1, -1}); params_.clip_actions_lower = torch::tensor({}).view({1, -1});
} else { }
else
{
params_.clip_actions_upper = torch::tensor( params_.clip_actions_upper = torch::tensor(
ReadVectorFromYaml<double>(config["clip_actions_upper"], params_.framework, rows, cols)).view({1, -1}); ReadVectorFromYaml<double>(config["clip_actions_upper"], params_.framework, rows, cols)).view({1, -1});
params_.clip_actions_lower = torch::tensor( params_.clip_actions_lower = torch::tensor(
@ -213,13 +264,17 @@ void StateRL::loadYaml(const std::string &config_path) {
// ReadVectorFromYaml<double>(config["default_dof_pos"], params_.framework, rows, cols)).view({1, -1}); // ReadVectorFromYaml<double>(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_w;
torch::Tensor q_vec; torch::Tensor q_vec;
if (framework == "isaacsim") { if (framework == "isaacsim")
{
q_w = q.index({torch::indexing::Slice(), 0}); q_w = q.index({torch::indexing::Slice(), 0});
q_vec = q.index({torch::indexing::Slice(), torch::indexing::Slice(1, 4)}); 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_w = q.index({torch::indexing::Slice(), 3});
q_vec = q.index({torch::indexing::Slice(), torch::indexing::Slice(0, 3)}); q_vec = q.index({torch::indexing::Slice(), torch::indexing::Slice(0, 3)});
} }
@ -231,62 +286,74 @@ torch::Tensor StateRL::quatRotateInverse(const torch::Tensor &q, const torch::Te
return a - b + c; return a - b + c;
} }
torch::Tensor StateRL::forward() { torch::Tensor StateRL::forward()
{
torch::autograd::GradMode::set_enabled(false); torch::autograd::GradMode::set_enabled(false);
torch::Tensor clamped_obs = computeObservation(); torch::Tensor clamped_obs = computeObservation();
torch::Tensor actions; torch::Tensor actions;
if (!params_.observations_history.empty()) { if (!params_.observations_history.empty())
{
history_obs_buf_->insert(clamped_obs); history_obs_buf_->insert(clamped_obs);
history_obs_ = history_obs_buf_->getObsVec(params_.observations_history); history_obs_ = history_obs_buf_->getObsVec(params_.observations_history);
actions = model_.forward({history_obs_}).toTensor(); actions = model_.forward({history_obs_}).toTensor();
} else { }
else
{
actions = model_.forward({clamped_obs}).toTensor(); 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 clamp(actions, params_.clip_actions_lower, params_.clip_actions_upper);
} }
return actions; return actions;
} }
void StateRL::getState() { void StateRL::getState()
if (params_.framework == "isaacgym") { {
robot_state_.imu.quaternion[3] = ctrl_comp_.imu_state_interface_[0].get().get_value(); if (params_.framework == "isaacgym")
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[3] = ctrl_interfaces_.imu_state_interface_[0].get().get_value();
robot_state_.imu.quaternion[2] = ctrl_comp_.imu_state_interface_[3].get().get_value(); robot_state_.imu.quaternion[0] = ctrl_interfaces_.imu_state_interface_[1].get().get_value();
} else if (params_.framework == "isaacsim") { robot_state_.imu.quaternion[1] = ctrl_interfaces_.imu_state_interface_[2].get().get_value();
robot_state_.imu.quaternion[0] = ctrl_comp_.imu_state_interface_[0].get().get_value(); robot_state_.imu.quaternion[2] = ctrl_interfaces_.imu_state_interface_[3].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(); else if (params_.framework == "isaacsim")
robot_state_.imu.quaternion[3] = ctrl_comp_.imu_state_interface_[3].get().get_value(); {
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[0] = ctrl_interfaces_.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[1] = ctrl_interfaces_.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[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[0] = ctrl_interfaces_.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[1] = ctrl_interfaces_.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[2] = ctrl_interfaces_.imu_state_interface_[9].get().get_value();
for (int i = 0; i < 12; i++) { 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.q[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_value();
robot_state_.motor_state.tauEst[i] = ctrl_comp_.joint_effort_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_.x = ctrl_interfaces_.control_inputs_.ly;
control_.y = -ctrl_comp_.control_inputs_.lx; control_.y = -ctrl_interfaces_.control_inputs_.lx;
control_.yaw = -ctrl_comp_.control_inputs_.rx; control_.yaw = -ctrl_interfaces_.control_inputs_.rx;
updated_ = true; updated_ = true;
} }
void StateRL::runModel() { void StateRL::runModel()
if (ctrl_comp_.enable_estimator_) { {
obs_.lin_vel = torch::from_blob(ctrl_comp_.estimator_->getVelocity().data(), {3}, torch::kDouble).clone(). if (enable_estimator_)
{
obs_.lin_vel = torch::from_blob(estimator_->getVelocity().data(), {3}, torch::kDouble).clone().
to(torch::kFloat).unsqueeze(0); to(torch::kFloat).unsqueeze(0);
} }
obs_.ang_vel = torch::tensor(robot_state_.imu.gyroscope).unsqueeze(0); obs_.ang_vel = torch::tensor(robot_state_.imu.gyroscope).unsqueeze(0);
@ -297,7 +364,8 @@ void StateRL::runModel() {
const torch::Tensor clamped_actions = forward(); 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; clamped_actions[0][i] *= params_.hip_scale_reduction;
} }
@ -309,7 +377,8 @@ void StateRL::runModel() {
output_dof_pos_ = actions_scaled + params_.default_dof_pos; 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<double>(); robot_command_.motor_command.q[i] = output_dof_pos_[0][i].item<double>();
robot_command_.motor_command.dq[i] = 0; robot_command_.motor_command.dq[i] = 0;
robot_command_.motor_command.kp[i] = params_.rl_kp[0][i].item<double>(); robot_command_.motor_command.kp[i] = params_.rl_kp[0][i].item<double>();
@ -318,12 +387,21 @@ void StateRL::runModel() {
} }
} }
void StateRL::setCommand() const { 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]); for (int i = 0; i < 12; 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]); std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(robot_command_.motor_command.kd[i]); set_value(
ctrl_comp_.joint_torque_command_interface_[i].get().set_value(robot_command_.motor_command.tau[i]); 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]);
} }
} }

50
controllers/rl_quadruped_controller/src/RlQuadrupedController.cpp
View File

@ -47,18 +47,18 @@ namespace rl_quadruped_controller {
} }
controller_interface::return_type LeggedGymController:: controller_interface::return_type LeggedGymController::
update(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { update(const rclcpp::Time & time, const rclcpp::Duration & period) {
if (ctrl_comp_.enable_estimator_) { if (ctrl_component_.enable_estimator_) {
if (ctrl_comp_.robot_model_ == nullptr) { if (ctrl_component_.robot_model_ == nullptr) {
return controller_interface::return_type::OK; return controller_interface::return_type::OK;
} }
ctrl_comp_.robot_model_->update(); ctrl_component_.robot_model_->update();
ctrl_comp_.estimator_->update(); ctrl_component_.estimator_->update();
} }
if (mode_ == FSMMode::NORMAL) { if (mode_ == FSMMode::NORMAL) {
current_state_->run(); current_state_->run(time, period);
next_state_name_ = current_state_->checkChange(); next_state_name_ = current_state_->checkChange();
if (next_state_name_ != current_state_->state_name) { if (next_state_name_ != current_state_->state_name) {
mode_ = FSMMode::CHANGE; mode_ = FSMMode::CHANGE;
@ -108,12 +108,12 @@ namespace rl_quadruped_controller {
stand_kp_ = auto_declare<double>("stand_kp", stand_kp_); stand_kp_ = auto_declare<double>("stand_kp", stand_kp_);
stand_kd_ = auto_declare<double>("stand_kd", stand_kd_); stand_kd_ = auto_declare<double>("stand_kd", stand_kd_);
get_node()->get_parameter("update_rate", ctrl_comp_.frequency_); get_node()->get_parameter("update_rate", ctrl_interfaces_.frequency_);
RCLCPP_INFO(get_node()->get_logger(), "Controller Update Rate: %d Hz", ctrl_comp_.frequency_); RCLCPP_INFO(get_node()->get_logger(), "Controller Update Rate: %d Hz", ctrl_interfaces_.frequency_);
if (foot_force_interface_types_.size() == 4) { if (foot_force_interface_types_.size() == 4) {
ctrl_comp_.enable_estimator_ = true; ctrl_component_.enable_estimator_ = true;
ctrl_comp_.estimator_ = std::make_shared<Estimator>(ctrl_comp_); ctrl_component_.estimator_ = std::make_shared<Estimator>(ctrl_interfaces_, ctrl_component_);
} }
} catch (const std::exception &e) { } catch (const std::exception &e) {
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); 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<std_msgs::msg::String>( robot_description_subscription_ = get_node()->create_subscription<std_msgs::msg::String>(
"/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(), "/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(),
[this](const std_msgs::msg::String::SharedPtr msg) { [this](const std_msgs::msg::String::SharedPtr msg) {
if (ctrl_comp_.enable_estimator_) { if (ctrl_component_.enable_estimator_) {
ctrl_comp_.robot_model_ = std::make_shared<QuadrupedRobot>( ctrl_component_.robot_model_ = std::make_shared<QuadrupedRobot>(
ctrl_comp_, msg->data, feet_names_, base_name_); 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_msgs::msg::Inputs>( control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
"/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) { "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) {
// Handle message // Handle message
ctrl_comp_.control_inputs_.command = msg->command; ctrl_interfaces_.control_inputs_.command = msg->command;
ctrl_comp_.control_inputs_.lx = msg->lx; ctrl_interfaces_.control_inputs_.lx = msg->lx;
ctrl_comp_.control_inputs_.ly = msg->ly; ctrl_interfaces_.control_inputs_.ly = msg->ly;
ctrl_comp_.control_inputs_.rx = msg->rx; ctrl_interfaces_.control_inputs_.rx = msg->rx;
ctrl_comp_.control_inputs_.ry = msg->ry; ctrl_interfaces_.control_inputs_.ry = msg->ry;
}); });
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
@ -151,7 +151,7 @@ namespace rl_quadruped_controller {
controller_interface::CallbackReturn LeggedGymController::on_activate( controller_interface::CallbackReturn LeggedGymController::on_activate(
const rclcpp_lifecycle::State & /*previous_state*/) { const rclcpp_lifecycle::State & /*previous_state*/) {
// clear out vectors in case of restart // clear out vectors in case of restart
ctrl_comp_.clear(); ctrl_interfaces_.clear();
// assign command interfaces // assign command interfaces
for (auto &interface: command_interfaces_) { for (auto &interface: command_interfaces_) {
@ -166,24 +166,24 @@ namespace rl_quadruped_controller {
// assign state interfaces // assign state interfaces
for (auto &interface: state_interfaces_) { for (auto &interface: state_interfaces_) {
if (interface.get_prefix_name() == imu_name_) { 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_) { } 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 { } else {
state_interface_map_[interface.get_interface_name()]->push_back(interface); state_interface_map_[interface.get_interface_name()]->push_back(interface);
} }
} }
// Create FSM List // Create FSM List
state_list_.passive = std::make_shared<StatePassive>(ctrl_comp_); state_list_.passive = std::make_shared<StatePassive>(ctrl_interfaces_);
state_list_.fixedDown = std::make_shared<StateFixedDown>(ctrl_comp_, down_pos_, stand_kp_, stand_kd_); state_list_.fixedDown = std::make_shared<StateFixedDown>(ctrl_interfaces_, down_pos_, stand_kp_, stand_kd_);
state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_comp_, stand_pos_, stand_kp_, stand_kd_); state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_interfaces_, stand_pos_, stand_kp_, stand_kd_);
RCLCPP_INFO(get_node()->get_logger(), "Using robot model from %s", robot_pkg_.c_str()); 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_); 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_; std::string model_path = package_share_directory + "/config/" + model_folder_;
state_list_.rl = std::make_shared<StateRL>(ctrl_comp_, model_path, stand_pos_); state_list_.rl = std::make_shared<StateRL>(ctrl_interfaces_, ctrl_component_, model_path, stand_pos_);
// Initialize FSM // Initialize FSM
current_state_ = state_list_.passive; current_state_ = state_list_.passive;

23
controllers/rl_quadruped_controller/src/RlQuadrupedController.h
View File

@ -9,9 +9,9 @@
#include <std_msgs/msg/string.hpp> #include <std_msgs/msg/string.hpp>
#include "rl_quadruped_controller/control/CtrlComponent.h" #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/StateFixedStand.h"
#include "rl_quadruped_controller/FSM/StatePassive.h" #include "controller_common//FSM/StatePassive.h"
namespace rl_quadruped_controller { namespace rl_quadruped_controller {
struct FSMStateList { struct FSMStateList {
@ -56,7 +56,8 @@ namespace rl_quadruped_controller {
private: private:
std::shared_ptr<FSMState> getNextState(FSMStateName stateName) const; std::shared_ptr<FSMState> getNextState(FSMStateName stateName) const;
CtrlComponent ctrl_comp_; CtrlComponent ctrl_component_;
CtrlInterfaces ctrl_interfaces_;
std::vector<std::string> joint_names_; std::vector<std::string> joint_names_;
std::string base_name_ = "base"; std::string base_name_ = "base";
std::vector<std::string> feet_names_; std::vector<std::string> feet_names_;
@ -96,19 +97,19 @@ namespace rl_quadruped_controller {
std::unordered_map< std::unordered_map<
std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> > *> std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> > *>
command_interface_map_ = { command_interface_map_ = {
{"effort", &ctrl_comp_.joint_torque_command_interface_}, {"effort", &ctrl_interfaces_.joint_torque_command_interface_},
{"position", &ctrl_comp_.joint_position_command_interface_}, {"position", &ctrl_interfaces_.joint_position_command_interface_},
{"velocity", &ctrl_comp_.joint_velocity_command_interface_}, {"velocity", &ctrl_interfaces_.joint_velocity_command_interface_},
{"kp", &ctrl_comp_.joint_kp_command_interface_}, {"kp", &ctrl_interfaces_.joint_kp_command_interface_},
{"kd", &ctrl_comp_.joint_kd_command_interface_} {"kd", &ctrl_interfaces_.joint_kd_command_interface_}
}; };
std::unordered_map< std::unordered_map<
std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> > *> std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> > *>
state_interface_map_ = { state_interface_map_ = {
{"position", &ctrl_comp_.joint_position_state_interface_}, {"position", &ctrl_interfaces_.joint_position_state_interface_},
{"effort", &ctrl_comp_.joint_effort_state_interface_}, {"effort", &ctrl_interfaces_.joint_effort_state_interface_},
{"velocity", &ctrl_comp_.joint_velocity_state_interface_} {"velocity", &ctrl_interfaces_.joint_velocity_state_interface_}
}; };
rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_; rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_;

31
controllers/rl_quadruped_controller/src/control/Estimator.cpp
View File

@ -4,14 +4,15 @@
#include "rl_quadruped_controller/control/Estimator.h" #include "rl_quadruped_controller/control/Estimator.h"
#include <rl_quadruped_controller/common/mathTools.h> #include <controller_common/CtrlInterfaces.h>
#include <controller_common/common/mathTools.h>
#include "rl_quadruped_controller/control/CtrlComponent.h" #include "rl_quadruped_controller/control/CtrlComponent.h"
Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_component), Estimator::Estimator(CtrlInterfaces &ctrl_interfaces, CtrlComponent &ctrl_component) : ctrl_interfaces_(ctrl_interfaces),
robot_model_(ctrl_component.robot_model_) { robot_model_(ctrl_component.robot_model_){
g_ << 0, 0, -9.81; g_ << 0, 0, -9.81;
dt_ = 1.0 / ctrl_component_.frequency_; dt_ = 1.0 / ctrl_interfaces_.frequency_;
std::cout << "dt: " << dt_ << std::endl; std::cout << "dt: " << dt_ << std::endl;
large_variance_ = 100; large_variance_ = 100;
@ -158,7 +159,7 @@ void Estimator::update() {
// Adjust the covariance based on foot contact and phase. // Adjust the covariance based on foot contact and phase.
for (int i(0); i < 4; ++i) { 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 // foot not contact
Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = large_variance_ * Eigen::MatrixXd::Identity(3, 3); 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); 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 quat;
quat << ctrl_component_.imu_state_interface_[0].get().get_value(), quat << ctrl_interfaces_.imu_state_interface_[0].get().get_value(),
ctrl_component_.imu_state_interface_[1].get().get_value(), ctrl_interfaces_.imu_state_interface_[1].get().get_value(),
ctrl_component_.imu_state_interface_[2].get().get_value(), ctrl_interfaces_.imu_state_interface_[2].get().get_value(),
ctrl_component_.imu_state_interface_[3].get().get_value(); ctrl_interfaces_.imu_state_interface_[3].get().get_value();
rotation_ = quatToRotMat(quat); rotation_ = quatToRotMat(quat);
gyro_ << ctrl_component_.imu_state_interface_[4].get().get_value(), gyro_ <<ctrl_interfaces_.imu_state_interface_[4].get().get_value(),
ctrl_component_.imu_state_interface_[5].get().get_value(), ctrl_interfaces_.imu_state_interface_[5].get().get_value(),
ctrl_component_.imu_state_interface_[6].get().get_value(); ctrl_interfaces_.imu_state_interface_[6].get().get_value();
acceleration_ << ctrl_component_.imu_state_interface_[7].get().get_value(), acceleration_ <<ctrl_interfaces_.imu_state_interface_[7].get().get_value(),
ctrl_component_.imu_state_interface_[8].get().get_value(), ctrl_interfaces_.imu_state_interface_[8].get().get_value(),
ctrl_component_.imu_state_interface_[9].get().get_value(); ctrl_interfaces_.imu_state_interface_[9].get().get_value();
u_ = rotation_ * acceleration_ + g_; u_ = rotation_ * acceleration_ + g_;
x_hat_ = A * x_hat_ + B * u_; x_hat_ = A * x_hat_ + B * u_;

18
controllers/rl_quadruped_controller/src/robot/QuadrupedRobot.cpp
View File

@ -3,12 +3,12 @@
// //
#include <iostream> #include <iostream>
#include "rl_quadruped_controller/control/CtrlComponent.h" #include "controller_common/CtrlInterfaces.h"
#include "rl_quadruped_controller/robot/QuadrupedRobot.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<std::string> &feet_names, const std::vector<std::string> &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::Tree robot_tree;
kdl_parser::treeFromString(robot_description, robot_tree); kdl_parser::treeFromString(robot_description, robot_tree);
@ -114,15 +114,15 @@ void QuadrupedRobot::update() {
if (mass_ == 0) return; if (mass_ == 0) return;
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
KDL::JntArray pos_array(3); KDL::JntArray pos_array(3);
pos_array(0) = ctrl_component_.joint_position_state_interface_[i * 3].get().get_value(); pos_array(0) = ctrl_interfaces_.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(1) = ctrl_interfaces_.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(2) = ctrl_interfaces_.joint_position_state_interface_[i * 3 + 2].get().get_value();
current_joint_pos_[i] = pos_array; current_joint_pos_[i] = pos_array;
KDL::JntArray vel_array(3); KDL::JntArray vel_array(3);
vel_array(0) = ctrl_component_.joint_velocity_state_interface_[i * 3].get().get_value(); vel_array(0) = ctrl_interfaces_.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(1) = ctrl_interfaces_.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(2) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3 + 2].get().get_value();
current_joint_vel_[i] = vel_array; current_joint_vel_[i] = vel_array;
} }
} }

2
controllers/rl_quadruped_controller/src/robot/RobotLeg.cpp
View File

@ -5,8 +5,6 @@
#include <memory> #include <memory>
#include "rl_quadruped_controller/robot/RobotLeg.h" #include "rl_quadruped_controller/robot/RobotLeg.h"
#include <rl_quadruped_controller/common/mathTypes.h>
RobotLeg::RobotLeg(const KDL::Chain &chain) { RobotLeg::RobotLeg(const KDL::Chain &chain) {
chain_ = chain; chain_ = chain;

7
descriptions/unitree/a1_description/config/gazebo.yaml
View File

@ -160,17 +160,16 @@ rl_quadruped_controller:
update_rate: 200 # Hz update_rate: 200 # Hz
robot_pkg: "a1_description" robot_pkg: "a1_description"
model_folder: "legged_gym" model_folder: "legged_gym"
command_prefix: "leg_pd_controller"
joints: joints:
- FL_hip_joint - FL_hip_joint
- FL_thigh_joint - FL_thigh_joint
- FL_calf_joint - FL_calf_joint
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- RL_hip_joint - RL_hip_joint
- RL_thigh_joint - RL_thigh_joint
- RL_calf_joint - RL_calf_joint
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- RR_hip_joint - RR_hip_joint
- RR_thigh_joint - RR_thigh_joint
- RR_calf_joint - RR_calf_joint

8
descriptions/unitree/a1_description/config/legged_gym/config.yaml
View File

@ -15,10 +15,10 @@ clip_actions_upper: [100, 100, 100,
100, 100, 100, 100, 100, 100,
100, 100, 100, 100, 100, 100,
100, 100, 100] 100, 100, 100]
rl_kp: [20.1, 20.1, 20.1, rl_kp: [30, 30, 30,
20.1, 20.1, 20.1, 30, 30, 30,
20.1, 20.1, 20.1, 30, 30, 30,
20.1, 20.1, 20.1] 30, 30, 30]
rl_kd: [0.754, 0.754, 0.754, rl_kd: [0.754, 0.754, 0.754,
0.754, 0.754, 0.754, 0.754, 0.754, 0.754,
0.754, 0.754, 0.754, 0.754, 0.754, 0.754,

98
descriptions/unitree/a1_description/xacro/gazebo.xacro
View File

@ -1,90 +1,138 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"> <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find gz_quadruped_hardware)/xacro/foot_force_sensor.xacro"/>
<ros2_control name="GazeboSystem" type="system"> <ros2_control name="GazeboSystem" type="system">
<hardware> <hardware>
<plugin>gz_ros2_control/GazeboSimSystem</plugin> <plugin>gz_quadruped_hardware/GazeboSimSystem</plugin>
</hardware> </hardware>
<joint name="FR_hip_joint"> <joint name="FR_hip_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="FR_thigh_joint"> <joint name="FR_thigh_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="FR_calf_joint"> <joint name="FR_calf_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="FL_hip_joint"> <joint name="FL_hip_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="FL_thigh_joint"> <joint name="FL_thigh_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="FL_calf_joint"> <joint name="FL_calf_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="RR_hip_joint"> <joint name="RR_hip_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="RR_thigh_joint"> <joint name="RR_thigh_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="RR_calf_joint"> <joint name="RR_calf_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="RL_hip_joint"> <joint name="RL_hip_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="RL_thigh_joint"> <joint name="RL_thigh_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="RL_calf_joint"> <joint name="RL_calf_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
@ -102,11 +150,19 @@
<state_interface name="linear_acceleration.y"/> <state_interface name="linear_acceleration.y"/>
<state_interface name="linear_acceleration.z"/> <state_interface name="linear_acceleration.z"/>
</sensor> </sensor>
<sensor name="foot_force">
<state_interface name="FR_foot_force"/>
<state_interface name="FL_foot_force"/>
<state_interface name="RR_foot_force"/>
<state_interface name="RL_foot_force"/>
</sensor>
</ros2_control> </ros2_control>
<gazebo> <gazebo>
<plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin"> <plugin filename="gz_quadruped_hardware-system" name="gz_quadruped_hardware::GazeboSimQuadrupedPlugin">
<parameters>$(find a1_description)/config/gazebo.yaml</parameters> <parameters>$(find go2_description)/config/gazebo.yaml</parameters>
</plugin> </plugin>
<plugin filename="gz-sim-imu-system" <plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu"> name="gz::sim::systems::Imu">
@ -115,7 +171,7 @@
name="gz::sim::systems::OdometryPublisher"> name="gz::sim::systems::OdometryPublisher">
<odom_frame>odom</odom_frame> <odom_frame>odom</odom_frame>
<robot_base_frame>base</robot_base_frame> <robot_base_frame>base</robot_base_frame>
<odom_publish_frequency>500</odom_publish_frequency> <odom_publish_frequency>1000</odom_publish_frequency>
<odom_topic>odom</odom_topic> <odom_topic>odom</odom_topic>
<dimensions>3</dimensions> <dimensions>3</dimensions>
<odom_covariance_topic>odom_with_covariance</odom_covariance_topic> <odom_covariance_topic>odom_with_covariance</odom_covariance_topic>
@ -194,4 +250,10 @@
<gazebo reference="imu_joint"> <gazebo reference="imu_joint">
<disableFixedJointLumping>true</disableFixedJointLumping> <disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo> </gazebo>
<xacro:foot_force_sensor name="FL" suffix="foot_fixed"/>
<xacro:foot_force_sensor name="RL" suffix="foot_fixed"/>
<xacro:foot_force_sensor name="FR" suffix="foot_fixed"/>
<xacro:foot_force_sensor name="RR" suffix="foot_fixed"/>
</robot> </robot>

2
descriptions/unitree/go2_description/config/gazebo.yaml
View File

@ -135,8 +135,8 @@ unitree_guide_controller:
rl_quadruped_controller: rl_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 200 # Hz update_rate: 200 # Hz
command_prefix: "leg_pd_controller"
robot_pkg: "go2_description" robot_pkg: "go2_description"
model_folder: "legged_gym"
joints: joints:
- FL_hip_joint - FL_hip_joint
- FL_thigh_joint - FL_thigh_joint

16
descriptions/unitree/go2_description/config/legged_gym/config.yaml
View File

@ -15,14 +15,14 @@ clip_actions_upper: [100, 100, 100,
100, 100, 100, 100, 100, 100,
100, 100, 100, 100, 100, 100,
100, 100, 100] 100, 100, 100]
rl_kp: [20, 20, 20, rl_kp: [40, 40, 40,
20, 20, 20, 40, 40, 40,
20, 20, 20, 40, 40, 40,
20, 20, 20] 40, 40, 40]
rl_kd: [0.5, 0.5, 0.5, rl_kd: [1, 1, 1,
0.5, 0.5, 0.5, 1, 1, 1,
0.5, 0.5, 0.5, 1, 1, 1,
0.5, 0.5, 0.5] 1, 1, 1]
hip_scale_reduction: 1.0 hip_scale_reduction: 1.0
hip_scale_reduction_indices: [0, 3, 6, 9] hip_scale_reduction_indices: [0, 3, 6, 9]
num_of_dofs: 12 num_of_dofs: 12

BIN
descriptions/unitree/go2_description/config/legged_gym/himloco.pt Normal file
View File

Binary file not shown.

0
controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/mathTools.h → libraries/controller_common/include/controller_common/common/mathTools.h
View File

0
controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/mathTypes.h → libraries/controller_common/include/controller_common/common/mathTypes.h
View File

BIN
libraries/gz_quadruped_playground/models/Lidar3DV1/materials/textures/lidar_3d_v1.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 94 KiB

738
libraries/gz_quadruped_playground/models/Lidar3DV1/meshes/lidar_3d_v1.dae Normal file
View File

@ -0,0 +1,738 @@
<?xml version="1.0" encoding="utf-8"?>
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
<asset><contributor><author></author><authoring_tool>FBX COLLADA exporter</authoring_tool><comments></comments></contributor><created>2018-09-20T22:13:36Z</created><keywords></keywords><modified>2018-09-20T22:13:36Z</modified><revision></revision><subject></subject><title></title><unit meter="0.010000" name="meter"></unit><up_axis>Z_UP</up_axis></asset>
<library_images>
<image id="Lidar_01_ncl1_2-image" name="Lidar_01_ncl1_2"><init_from>lidar_3d_v1.jpg</init_from></image>
</library_images>
<library_materials>
<material id="Lidar_01_ncl1_1" name="Lidar_01_ncl1_1">
<instance_effect url="#Lidar_01_ncl1_1-fx"/>
</material>
</library_materials>
<library_effects>
<effect id="Lidar_01_ncl1_1-fx" name="Lidar_01_ncl1_1">
<profile_COMMON>
<technique sid="standard">
<phong>
<emission>
<color sid="emission">0.000000 0.000000 0.000000 1.000000</color>
</emission>
<ambient>
<color sid="ambient">0.588235 0.588235 0.588235 1.000000</color>
</ambient>
<diffuse>
<texture texture="Lidar_01_ncl1_2-image" texcoord="CHANNEL0">
<extra>
<technique profile="MAYA">
<wrapU sid="wrapU0">TRUE</wrapU>
<wrapV sid="wrapV0">TRUE</wrapV>
<blend_mode>ADD</blend_mode>
</technique>
</extra>
</texture>
</diffuse>
<specular>
<color sid="specular">0.000000 0.000000 0.000000 1.000000</color>
</specular>
<shininess>
<float sid="shininess">2.000000</float>
</shininess>
<reflective>
<color sid="reflective">0.000000 0.000000 0.000000 1.000000</color>
</reflective>
<reflectivity>
<float sid="reflectivity">1.000000</float>
</reflectivity>
<transparent opaque="RGB_ZERO">
<color sid="transparent">1.000000 1.000000 1.000000 1.000000</color>
</transparent>
<transparency>
<float sid="transparency">0.000000</float>
</transparency>
</phong>
</technique>
</profile_COMMON>
</effect>
</library_effects>
<library_geometries>
<geometry id="Lidar_01-lib" name="Lidar_01Mesh">
<mesh>
<source id="Lidar_01-POSITION">
<float_array id="Lidar_01-POSITION-array" count="216">
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
</float_array>
<technique_common>
<accessor source="#Lidar_01-POSITION-array" count="72" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="Lidar_01-Normal0">
<float_array id="Lidar_01-Normal0-array" count="1260">
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
</float_array>
<technique_common>
<accessor source="#Lidar_01-Normal0-array" count="420" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="Lidar_01-UV0">
<float_array id="Lidar_01-UV0-array" count="252">
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
</float_array>
<technique_common>
<accessor source="#Lidar_01-UV0-array" count="126" stride="2">
<param name="S" type="float"/>
<param name="T" type="float"/>
</accessor>
</technique_common>
</source>
<vertices id="Lidar_01-VERTEX">
<input semantic="POSITION" source="#Lidar_01-POSITION"/>
</vertices>
<polylist count="140" material="Lidar_01_ncl1_1"><input semantic="VERTEX" offset="0" source="#Lidar_01-VERTEX"/><input semantic="NORMAL" offset="1" source="#Lidar_01-Normal0"/><input semantic="TEXCOORD" offset="2" set="0" source="#Lidar_01-UV0"/><vcount>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 </vcount><p> 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</p></polylist>
</mesh>
</geometry>
</library_geometries>
<library_lights>
<light id="SceneAmbient" name="SceneAmbient">
<technique_common>
<ambient>
<color>0.149020 0.149020 0.149020</color>
</ambient>
</technique_common>
</light>
</library_lights>
<library_visual_scenes>
<visual_scene id="Lidar_01" name="Lidar_01">
<node name="Lidar_01" id="Lidar_01" layer="VLP-16" sid="Lidar_01"><matrix sid="matrix">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.000000</matrix><instance_geometry url="#Lidar_01-lib"><bind_material><technique_common><instance_material symbol="Lidar_01_ncl1_1" target="#Lidar_01_ncl1_1"/></technique_common></bind_material></instance_geometry><extra><technique profile="FCOLLADA"><visibility>1.000000</visibility></technique></extra></node>
<extra><technique profile="MAX3D"><frame_rate>30.000000</frame_rate></technique><technique profile="FCOLLADA"><start_time>0.000000</start_time><end_time>3.333333</end_time></technique></extra>
<node>
<instance_light url="#SceneAmbient"></instance_light>
</node>
</visual_scene>
</library_visual_scenes>
<scene>
<instance_visual_scene url="#Lidar_01"></instance_visual_scene>
</scene>
</COLLADA>

BIN
libraries/gz_quadruped_playground/models/Lidar3DV1/meshes/lidar_3d_v1.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 94 KiB

15
libraries/gz_quadruped_playground/models/Lidar3DV1/model.config Normal file
View File

@ -0,0 +1,15 @@
<?xml version="1.0"?>
<model>
<name>Lidar 3d v1</name>
<version>1.0</version>
<sdf version="1.6">model.sdf</sdf>
<author>
<name>Cole Biesemeyer</name>
</author>
<description>
A generic 3d lidar.
</description>
</model>

36
libraries/gz_quadruped_playground/models/Lidar3DV1/model.sdf Normal file
View File

@ -0,0 +1,36 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<model name="lidar_3d_v1">
<link name="link">
<pose>0 0 0 0 0 0</pose>
<inertial>
<pose>0 0 0.03585 0 0 0</pose>
<mass>0.83</mass>
<inertia>
<ixx>0.0009080594249999999</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0009080594249999999</iyy>
<iyz>0</iyz>
<izz>0.0011049624</izz>
</inertia>
</inertial>
<collision name="collision">
<pose>0 0 0.03585 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.0516</radius>
<length>0.0717</length>
</cylinder>
</geometry>
</collision>
<visual name="visual">
<geometry>
<mesh>
<uri>model://lidar_3d_v1/meshes/lidar_3d_v1.dae</uri>
</mesh>
</geometry>
</visual>
</link>
</model>
</sdf>

BIN
libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/1.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 20 KiB

BIN
libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/2.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 30 KiB

BIN
libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/3.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 18 KiB

BIN
libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/4.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 18 KiB

BIN
libraries/gz_quadruped_playground/models/Lidar3DV1/thumbnails/5.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 18 KiB