achieved passive mode on unitree simulation
This commit is contained in:
Zhenbiao Huang
parent
acc6540534
commit
0cc499210a
|
|
@ -1,24 +0,0 @@
|
|||
quadruped_controller:
|
||||
ros__parameters:
|
||||
type: quadruped_ros2_control/QuadrupedController
|
||||
joints:
|
||||
- RF_HAA
|
||||
- LF_HAA
|
||||
- RH_HAA
|
||||
- LH_HAA
|
||||
- RF_HFE
|
||||
- LF_HFE
|
||||
- RH_HFE
|
||||
- LH_HFE
|
||||
- RF_KFE
|
||||
- LF_KFE
|
||||
- RH_KFE
|
||||
- LH_KFE
|
||||
|
||||
command_interfaces:
|
||||
- effort
|
||||
|
||||
state_interfaces:
|
||||
- effort
|
||||
- position
|
||||
- velocity
|
||||
|
|
@ -1,30 +0,0 @@
|
|||
//
|
||||
// Created by tlab-uav on 24-9-6.
|
||||
//
|
||||
|
||||
#ifndef FSMSTATE_H
|
||||
#define FSMSTATE_H
|
||||
|
||||
#include <string>
|
||||
#include "quadruped_ros2_control/common/enumClass.h"
|
||||
|
||||
class FSMState {
|
||||
public:
|
||||
virtual ~FSMState() = default;
|
||||
|
||||
FSMState(FSMStateName stateName, std::string stateNameString);
|
||||
|
||||
virtual void enter();
|
||||
|
||||
virtual void run();
|
||||
|
||||
virtual void exit();
|
||||
|
||||
virtual FSMStateName checkChange() { return FSMStateName::INVALID; }
|
||||
|
||||
FSMStateName state_name;
|
||||
std::string state_name_string;
|
||||
};
|
||||
|
||||
|
||||
#endif //FSMSTATE_H
|
||||
|
|
@ -1,22 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>quadruped_ros2_control</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="biao876990970@hotmail.com">tlab-uav</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>backward_ros</depend>
|
||||
<depend>controller_interface</depend>
|
||||
<depend>pluginlib</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
|
|
@ -1,8 +0,0 @@
|
|||
<library path="quadruped_ros2_control">
|
||||
<class name="quadruped_ros2_control/QuadrupedController" type="quadruped_ros2_control::QuadrupedController"
|
||||
base_class_type="controller_interface::ControllerInterface">
|
||||
<description>
|
||||
Basic Quadruped Controller based on Unitree Guide.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
|
|
@ -1,11 +0,0 @@
|
|||
//
|
||||
// Created by tlab-uav on 24-9-6.
|
||||
//
|
||||
|
||||
#include <quadruped_ros2_control/FSM/FSMState.h>
|
||||
|
||||
#include <utility>
|
||||
|
||||
FSMState::FSMState(const FSMStateName stateName, std::string stateNameString)
|
||||
: state_name(stateName), state_name_string(std::move(stateNameString)) {
|
||||
}
|
||||
|
|
@ -1,12 +0,0 @@
|
|||
//
|
||||
// Created by tlab-uav on 24-9-6.
|
||||
//
|
||||
|
||||
#include <quadruped_ros2_control/FSM/StatePassive.h>
|
||||
|
||||
StatePassive::StatePassive() : FSMState(FSMStateName::PASSIVE, "passive") {
|
||||
}
|
||||
|
||||
void StatePassive::enter() {
|
||||
|
||||
}
|
||||
|
|
@ -1,5 +1,5 @@
|
|||
cmake_minimum_required(VERSION 3.8)
|
||||
project(quadruped_ros2_control)
|
||||
project(unitree_guide_controller)
|
||||
|
||||
if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
|
|
@ -23,9 +23,9 @@ foreach (Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS})
|
|||
endforeach ()
|
||||
|
||||
add_library(${PROJECT_NAME} SHARED
|
||||
src/QuadrupedController.cpp
|
||||
src/FSM/FSMState.cpp
|
||||
src/UnitreeGuideController.cpp
|
||||
src/FSM/StatePassive.cpp
|
||||
src/FSM/StateFixedStand.cpp
|
||||
)
|
||||
target_include_directories(${PROJECT_NAME}
|
||||
PUBLIC
|
||||
|
|
@ -36,7 +36,7 @@ ament_target_dependencies(
|
|||
${CONTROLLER_INCLUDE_DEPENDS}
|
||||
)
|
||||
|
||||
pluginlib_export_plugin_description_file(controller_interface quadruped_ros2_control.xml)
|
||||
pluginlib_export_plugin_description_file(controller_interface unitree_guide_controller.xml)
|
||||
|
||||
install(
|
||||
DIRECTORY include/
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
unitree_guide_controller:
|
||||
ros__parameters:
|
||||
type: unitree_guide_controller/UnitreeGuideController
|
||||
joints:
|
||||
- FR_hip_joint
|
||||
- FR_thigh_joint
|
||||
- FR_calf_joint
|
||||
- FL_hip_joint
|
||||
- FL_thigh_joint
|
||||
- FL_calf_joint
|
||||
- RR_hip_joint
|
||||
- RR_thigh_joint
|
||||
- RR_calf_joint
|
||||
- RL_hip_joint
|
||||
- RL_thigh_joint
|
||||
- RL_calf_joint
|
||||
|
||||
command_interfaces:
|
||||
- effort
|
||||
- position
|
||||
- velocity
|
||||
- kp
|
||||
- kd
|
||||
|
||||
state_interfaces:
|
||||
- effort
|
||||
- position
|
||||
- velocity
|
||||
|
|
@ -0,0 +1,38 @@
|
|||
//
|
||||
// Created by tlab-uav on 24-9-6.
|
||||
//
|
||||
|
||||
#ifndef FSMSTATE_H
|
||||
#define FSMSTATE_H
|
||||
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <unitree_guide_controller/common/enumClass.h>
|
||||
#include <unitree_guide_controller/common/interface.h>
|
||||
|
||||
class FSMState {
|
||||
public:
|
||||
virtual ~FSMState() = default;
|
||||
|
||||
FSMState(const FSMStateName &stateName, std::string stateNameString, CtrlComponent ctrlComp)
|
||||
: state_name(stateName),
|
||||
state_name_string(std::move(stateNameString)),
|
||||
ctrlComp_(std::move(ctrlComp)) {
|
||||
}
|
||||
|
||||
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 ctrlComp_;
|
||||
};
|
||||
|
||||
#endif //FSMSTATE_H
|
||||
|
|
@ -0,0 +1,26 @@
|
|||
//
|
||||
// Created by biao on 24-9-10.
|
||||
//
|
||||
|
||||
#include "FSMState.h"
|
||||
|
||||
#ifndef STATEFIXEDSTAND_H
|
||||
#define STATEFIXEDSTAND_H
|
||||
|
||||
|
||||
|
||||
class StateFixedStand : public FSMState {
|
||||
explicit StateFixedStand(CtrlComponent ctrlComp);
|
||||
|
||||
void enter() override;
|
||||
|
||||
void run() override;
|
||||
|
||||
void exit() override;
|
||||
|
||||
FSMStateName checkChange() override;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //STATEFIXEDSTAND_H
|
||||
|
|
@ -9,7 +9,7 @@
|
|||
|
||||
class StatePassive final : public FSMState {
|
||||
public:
|
||||
explicit StatePassive();
|
||||
explicit StatePassive(CtrlComponent ctrlComp);
|
||||
|
||||
void enter() override;
|
||||
|
||||
|
|
@ -5,14 +5,25 @@
|
|||
#ifndef QUADRUPEDCONTROLLER_H
|
||||
#define QUADRUPEDCONTROLLER_H
|
||||
|
||||
#include "controller_interface/controller_interface.hpp"
|
||||
#include <controller_interface/controller_interface.hpp>
|
||||
#include <std_msgs/msg/string.hpp>
|
||||
#include <unitree_guide_controller/FSM/FSMState.h>
|
||||
#include <unitree_guide_controller/common/enumClass.h>
|
||||
|
||||
namespace quadruped_ros2_control {
|
||||
class QuadrupedController final : public controller_interface::ControllerInterface {
|
||||
#include "FSM/StateFixedStand.h"
|
||||
#include "FSM/StatePassive.h"
|
||||
|
||||
namespace unitree_guide_controller {
|
||||
struct FSMStateList {
|
||||
std::shared_ptr<FSMState> invalid;
|
||||
std::shared_ptr<StatePassive> passive;
|
||||
std::shared_ptr<StateFixedStand> fixedStand;
|
||||
};
|
||||
|
||||
class UnitreeGuideController final : public controller_interface::ControllerInterface {
|
||||
public:
|
||||
CONTROLLER_INTERFACE_PUBLIC
|
||||
QuadrupedController() = default;
|
||||
UnitreeGuideController() = default;
|
||||
|
||||
CONTROLLER_INTERFACE_PUBLIC
|
||||
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
|
||||
|
|
@ -51,6 +62,8 @@ namespace quadruped_ros2_control {
|
|||
controller_interface::CallbackReturn on_shutdown(
|
||||
const rclcpp_lifecycle::State &previous_state) override;
|
||||
|
||||
CtrlComponent ctrl_comp_;
|
||||
|
||||
protected:
|
||||
std::vector<std::string> joint_names_;
|
||||
std::vector<std::string> command_interface_types_;
|
||||
|
|
@ -58,29 +71,29 @@ namespace quadruped_ros2_control {
|
|||
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr state_command_subscriber_;
|
||||
|
||||
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
|
||||
joint_effort_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::unordered_map<
|
||||
std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> > *>
|
||||
command_interface_map_ = {
|
||||
{"effort", &joint_effort_command_interface_}
|
||||
{"effort", &ctrl_comp_.joint_effort_command_interface_},
|
||||
{"position", &ctrl_comp_.joint_position_command_interface_},
|
||||
{"velocity", &ctrl_comp_.joint_velocity_command_interface_},
|
||||
{"kp", &ctrl_comp_.joint_kp_command_interface_},
|
||||
{"kd", &ctrl_comp_.joint_kd_command_interface_}
|
||||
};
|
||||
|
||||
bool stand_ = false;
|
||||
FSMMode mode_ = FSMMode::NORMAL;
|
||||
std::string state_name_;
|
||||
FSMStateName next_state_name_ = FSMStateName::INVALID;
|
||||
FSMStateList state_list_;
|
||||
std::shared_ptr<FSMState> current_state_;
|
||||
std::shared_ptr<FSMState> next_state_;
|
||||
|
||||
std::unordered_map<
|
||||
std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> > *>
|
||||
state_interface_map_ = {
|
||||
{"position", &joint_position_state_interface_},
|
||||
{"effort", &joint_effort_state_interface_},
|
||||
{"velocity", &joint_velocity_state_interface_}
|
||||
{"position", &ctrl_comp_.joint_position_state_interface_},
|
||||
{"effort", &ctrl_comp_.joint_effort_state_interface_},
|
||||
{"velocity", &ctrl_comp_.joint_velocity_state_interface_}
|
||||
};
|
||||
};
|
||||
}
|
||||
|
|
@ -15,7 +15,11 @@ enum class FSMStateName {
|
|||
|
||||
SWINGTEST,
|
||||
BALANCETEST,
|
||||
};
|
||||
};
|
||||
|
||||
enum class FSMMode {
|
||||
NORMAL,
|
||||
CHANGE
|
||||
};
|
||||
|
||||
#endif //ENUMCLASS_H
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
//
|
||||
// Created by biao on 24-9-10.
|
||||
//
|
||||
|
||||
#ifndef INTERFACE_H
|
||||
#define INTERFACE_H
|
||||
|
||||
#include <vector>
|
||||
#include <hardware_interface/loaned_command_interface.hpp>
|
||||
#include <hardware_interface/loaned_state_interface.hpp>
|
||||
|
||||
struct CtrlComponent {
|
||||
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
|
||||
joint_effort_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_;
|
||||
|
||||
public:
|
||||
void clear() {
|
||||
joint_effort_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();
|
||||
}
|
||||
};
|
||||
|
||||
#endif //INTERFACE_H
|
||||
|
|
@ -24,16 +24,16 @@ def generate_launch_description():
|
|||
|
||||
robot_controllers = PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("quadruped_ros2_control"),
|
||||
FindPackageShare("unitree_guide_controller"),
|
||||
"config",
|
||||
"quadruped_controller.yaml",
|
||||
"controller.yaml",
|
||||
]
|
||||
)
|
||||
|
||||
robot_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["quadruped_controller", "--param-file", robot_controllers],
|
||||
arguments=["unitree_guide_controller", "--param-file", robot_controllers],
|
||||
)
|
||||
|
||||
nodes = [
|
||||
|
|
@ -0,0 +1,22 @@
|
|||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>unitree_guide_controller</name>
|
||||
<version>0.0.0</version>
|
||||
<description>A Ros2 Control Controller based on Unitree Guide</description>
|
||||
<maintainer email="biao876990970@hotmail.com">Huang Zhenbiao</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>backward_ros</depend>
|
||||
<depend>controller_interface</depend>
|
||||
<depend>pluginlib</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
# Unitree Guide Controller
|
||||
|
||||
This is a ros2-control controller based on unitree guide.
|
||||
|
||||
```bash
|
||||
cd ~/ros2_ws
|
||||
colcon build --packages-up-to unitree_guide_controller
|
||||
```
|
||||
|
||||
```bash
|
||||
source ~/ros2_ws/install/setup.bash
|
||||
ros2 launch unitree_guide_controller controller.launch.py
|
||||
```
|
||||
|
|
@ -0,0 +1,22 @@
|
|||
//
|
||||
// Created by biao on 24-9-10.
|
||||
//
|
||||
|
||||
#include <unitree_guide_controller/FSM/StateFixedStand.h>
|
||||
|
||||
StateFixedStand::StateFixedStand(CtrlComponent ctrlComp): FSMState(
|
||||
FSMStateName::FIXEDSTAND, "fixed stand", std::move(ctrlComp)) {
|
||||
}
|
||||
|
||||
void StateFixedStand::enter() {
|
||||
}
|
||||
|
||||
void StateFixedStand::run() {
|
||||
}
|
||||
|
||||
void StateFixedStand::exit() {
|
||||
}
|
||||
|
||||
FSMStateName StateFixedStand::checkChange() {
|
||||
return FSMStateName::FIXEDSTAND;
|
||||
}
|
||||
|
|
@ -0,0 +1,45 @@
|
|||
//
|
||||
// Created by tlab-uav on 24-9-6.
|
||||
//
|
||||
|
||||
#include <iostream>
|
||||
#include <ostream>
|
||||
#include <unitree_guide_controller/FSM/StatePassive.h>
|
||||
|
||||
#include <utility>
|
||||
|
||||
StatePassive::StatePassive(CtrlComponent ctrlComp) : FSMState(
|
||||
FSMStateName::PASSIVE, "passive", std::move(ctrlComp)) {
|
||||
}
|
||||
|
||||
void StatePassive::enter() {
|
||||
for (auto i: ctrlComp_.joint_effort_command_interface_) {
|
||||
i.get().set_value(0);
|
||||
}
|
||||
for (auto i: ctrlComp_.joint_position_command_interface_) {
|
||||
i.get().set_value(0);
|
||||
}
|
||||
for (auto i: ctrlComp_.joint_velocity_command_interface_) {
|
||||
i.get().set_value(0);
|
||||
}
|
||||
for (auto i: ctrlComp_.joint_kp_command_interface_) {
|
||||
i.get().set_value(0);
|
||||
}
|
||||
for (auto i: ctrlComp_.joint_kd_command_interface_) {
|
||||
i.get().set_value(3.5);
|
||||
}
|
||||
std::cout<<"passive"<<std::endl;
|
||||
}
|
||||
|
||||
void StatePassive::run() {
|
||||
}
|
||||
|
||||
void StatePassive::exit() {
|
||||
}
|
||||
|
||||
FSMStateName StatePassive::checkChange() {
|
||||
// if (controller_->state_name_ == "fixed_stand") {
|
||||
// return FSMStateName::FIXEDSTAND;
|
||||
// }
|
||||
return FSMStateName::PASSIVE;
|
||||
}
|
||||
|
|
@ -2,12 +2,13 @@
|
|||
// Created by tlab-uav on 24-9-6.
|
||||
//
|
||||
|
||||
#include <quadruped_ros2_control/QuadrupedController.h>
|
||||
#include <unitree_guide_controller/UnitreeGuideController.h>
|
||||
#include <unitree_guide_controller/FSM/StatePassive.h>
|
||||
|
||||
namespace quadruped_ros2_control {
|
||||
namespace unitree_guide_controller {
|
||||
using config_type = controller_interface::interface_configuration_type;
|
||||
|
||||
controller_interface::InterfaceConfiguration QuadrupedController::command_interface_configuration() const {
|
||||
controller_interface::InterfaceConfiguration UnitreeGuideController::command_interface_configuration() const {
|
||||
controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
|
||||
|
||||
conf.names.reserve(joint_names_.size() * command_interface_types_.size());
|
||||
|
|
@ -20,7 +21,7 @@ namespace quadruped_ros2_control {
|
|||
return conf;
|
||||
}
|
||||
|
||||
controller_interface::InterfaceConfiguration QuadrupedController::state_interface_configuration() const {
|
||||
controller_interface::InterfaceConfiguration UnitreeGuideController::state_interface_configuration() const {
|
||||
controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
|
||||
|
||||
conf.names.reserve(joint_names_.size() * state_interface_types_.size());
|
||||
|
|
@ -33,21 +34,16 @@ namespace quadruped_ros2_control {
|
|||
return conf;
|
||||
}
|
||||
|
||||
controller_interface::return_type QuadrupedController::
|
||||
controller_interface::return_type UnitreeGuideController::
|
||||
update(const rclcpp::Time &time, const rclcpp::Duration &period) {
|
||||
if (stand_) {
|
||||
for (auto i: joint_effort_command_interface_) {
|
||||
i.get().set_value(30);
|
||||
}
|
||||
} else {
|
||||
for (auto i: joint_effort_command_interface_) {
|
||||
i.get().set_value(0);
|
||||
}
|
||||
if (mode_ == FSMMode::NORMAL) {
|
||||
current_state_->run();
|
||||
}
|
||||
|
||||
return controller_interface::return_type::OK;
|
||||
}
|
||||
|
||||
controller_interface::CallbackReturn QuadrupedController::on_init() {
|
||||
controller_interface::CallbackReturn UnitreeGuideController::on_init() {
|
||||
try {
|
||||
joint_names_ = auto_declare<std::vector<std::string> >("joints", joint_names_);
|
||||
command_interface_types_ =
|
||||
|
|
@ -64,29 +60,21 @@ namespace quadruped_ros2_control {
|
|||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
controller_interface::CallbackReturn QuadrupedController::on_configure(
|
||||
controller_interface::CallbackReturn UnitreeGuideController::on_configure(
|
||||
const rclcpp_lifecycle::State &previous_state) {
|
||||
stand_ = false;
|
||||
state_command_subscriber_ = get_node()->create_subscription<std_msgs::msg::String>(
|
||||
"state_command", 10, [this](const std_msgs::msg::String::SharedPtr msg) {
|
||||
// Handle message
|
||||
RCLCPP_INFO(get_node()->get_logger(), "Received command: %s", msg->data.c_str());
|
||||
if (msg.get()->data == "stand") {
|
||||
stand_ = true;
|
||||
} else {
|
||||
stand_ = false;
|
||||
}
|
||||
RCLCPP_INFO(get_node()->get_logger(), "Switch State: %s", msg->data.c_str());
|
||||
state_name_ = msg->data;
|
||||
});
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
controller_interface::CallbackReturn
|
||||
QuadrupedController::on_activate(const rclcpp_lifecycle::State &previous_state) {
|
||||
UnitreeGuideController::on_activate(const rclcpp_lifecycle::State &previous_state) {
|
||||
// clear out vectors in case of restart
|
||||
joint_effort_command_interface_.clear();
|
||||
joint_effort_state_interface_.clear();
|
||||
joint_position_state_interface_.clear();
|
||||
joint_velocity_state_interface_.clear();
|
||||
ctrl_comp_.clear();
|
||||
|
||||
// assign command interfaces
|
||||
for (auto &interface: command_interfaces_) {
|
||||
|
|
@ -98,29 +86,40 @@ namespace quadruped_ros2_control {
|
|||
state_interface_map_[interface.get_interface_name()]->push_back(interface);
|
||||
}
|
||||
|
||||
state_list_.passive = std::make_shared<StatePassive>(ctrl_comp_);
|
||||
state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_comp_);
|
||||
|
||||
// Initialize FSM
|
||||
current_state_ = state_list_.passive;
|
||||
current_state_->enter();
|
||||
next_state_ = current_state_;
|
||||
next_state_name_ = current_state_->state_name;
|
||||
mode_ = FSMMode::NORMAL;
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
controller_interface::CallbackReturn QuadrupedController::on_deactivate(
|
||||
controller_interface::CallbackReturn UnitreeGuideController::on_deactivate(
|
||||
const rclcpp_lifecycle::State &previous_state) {
|
||||
release_interfaces();
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
controller_interface::CallbackReturn
|
||||
QuadrupedController::on_cleanup(const rclcpp_lifecycle::State &previous_state) {
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
controller_interface::CallbackReturn QuadrupedController::on_error(const rclcpp_lifecycle::State &previous_state) {
|
||||
UnitreeGuideController::on_cleanup(const rclcpp_lifecycle::State &previous_state) {
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
controller_interface::CallbackReturn
|
||||
QuadrupedController::on_shutdown(const rclcpp_lifecycle::State &previous_state) {
|
||||
UnitreeGuideController::on_error(const rclcpp_lifecycle::State &previous_state) {
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
controller_interface::CallbackReturn
|
||||
UnitreeGuideController::on_shutdown(const rclcpp_lifecycle::State &previous_state) {
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
}
|
||||
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
PLUGINLIB_EXPORT_CLASS(quadruped_ros2_control::QuadrupedController, controller_interface::ControllerInterface);
|
||||
PLUGINLIB_EXPORT_CLASS(unitree_guide_controller::UnitreeGuideController, controller_interface::ControllerInterface);
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
<library path="unitree_guide_controller">
|
||||
<class name="unitree_guide_controller/UnitreeGuideController"
|
||||
type="unitree_guide_controller::UnitreeGuideController"
|
||||
base_class_type="controller_interface::ControllerInterface">
|
||||
<description>
|
||||
Quadruped Controller based on Unitree Guide.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
|
|
@ -15,6 +15,12 @@ source ~/ros2_ws/install/setup.bash
|
|||
ros2 launch go2_description visualize.launch.py
|
||||
```
|
||||
|
||||
## Launch Hardware Interface
|
||||
```bash
|
||||
source ~/ros2_ws/install/setup.bash
|
||||
ros2 launch go2_description hardware.launch.py
|
||||
```
|
||||
|
||||
## When used for isaac gym or other similiar engine
|
||||
|
||||
Collision parameters in urdf can be amended to better train the robot:
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
# Controller Manager configuration
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 1000 # Hz
|
||||
update_rate: 50 # Hz
|
||||
use_sim_time: true # If running in simulation
|
||||
|
||||
# Define the available controllers
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 78
|
||||
Help Height: 138
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
|
|
@ -8,7 +8,7 @@ Panels:
|
|||
- /Status1
|
||||
- /RobotModel1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 542
|
||||
Tree Height: 832
|
||||
- Class: rviz_common/Selection
|
||||
Name: Selection
|
||||
- Class: rviz_common/Tool Properties
|
||||
|
|
@ -230,18 +230,18 @@ Visualization Manager:
|
|||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.42039796710014343
|
||||
Pitch: 0.5053979158401489
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 0.6653964519500732
|
||||
Yaw: 0.8203961253166199
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 846
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000156000002acfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002ac000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002acfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002ac000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000026f00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002ac00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
collapsed: true
|
||||
Height: 1194
|
||||
Hide Left Dock: true
|
||||
Hide Right Dock: true
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000001f40000043cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000700000043c0000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015d0000043cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000700000043c0000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006180000005efc0100000002fb0000000800540069006d00650100000000000006180000047a00fffffffb0000000800540069006d0065010000000000000450000000000000000000000618000003a000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
|
|
@ -249,7 +249,7 @@ Window Geometry:
|
|||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1200
|
||||
X: 523
|
||||
Y: 102
|
||||
collapsed: true
|
||||
Width: 1560
|
||||
X: 1327
|
||||
Y: 195
|
||||
|
|
|
|||
|
|
@ -96,6 +96,7 @@ std::vector<hardware_interface::CommandInterface> HardwareUnitree::export_comman
|
|||
command_interfaces.emplace_back(joint_name, "effort", &joint_effort_command_[ind]);
|
||||
command_interfaces.emplace_back(joint_name, "kp", &joint_kp_command_[ind]);
|
||||
command_interfaces.emplace_back(joint_name, "kd", &joint_kd_command_[ind]);
|
||||
ind++;
|
||||
}
|
||||
return command_interfaces;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue