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

can visualize and control gazebo robots

This commit is contained in:
Huang Zhenbiao 2024-09-20 12:48:45 +08:00
parent 5825c3cac2
commit f127ff144f
17 changed files with 155 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
controllers/leg_pd_controller/include/leg_pd_controller/LegPdController.h
View File

@ -9,7 +9,7 @@
namespace leg_pd_controller {
class LegPdController : public controller_interface::ChainableControllerInterface {
class LegPdController final : public controller_interface::ChainableControllerInterface {
public:
controller_interface::CallbackReturn on_init() override;

33
controllers/leg_pd_controller/src/LegPdController.cpp
View File

@ -18,6 +18,14 @@ namespace leg_pd_controller {
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
return controller_interface::CallbackReturn::ERROR;
}
const size_t joint_num = joint_names_.size();
joint_effort_command_.assign(joint_num, 0);
joint_position_command_.assign(joint_num, 0);
joint_velocities_command_.assign(joint_num, 0);
joint_kp_command_.assign(joint_num, 0);
joint_kd_command_.assign(joint_num, 0);
return CallbackReturn::SUCCESS;
}
@ -50,6 +58,11 @@ namespace leg_pd_controller {
controller_interface::CallbackReturn LegPdController::on_activate(
const rclcpp_lifecycle::State & /*previous_state*/) {
joint_effort_command_interface_.clear();
joint_position_state_interface_.clear();
joint_velocity_state_interface_.clear();
// assign effort command interface
for (auto &interface: command_interfaces_) {
joint_effort_command_interface_.emplace_back(interface);
@ -75,6 +88,24 @@ namespace leg_pd_controller {
controller_interface::return_type LegPdController::update_and_write_commands(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
if (joint_names_.size() != joint_effort_command_.size() ||
joint_names_.size() != joint_kp_command_.size() ||
joint_names_.size() != joint_position_command_.size() ||
joint_names_.size() != joint_position_state_interface_.size() ||
joint_names_.size() != joint_velocity_state_interface_.size() ||
joint_names_.size() != joint_effort_command_interface_.size()) {
std::cout << "joint_names_.size() = " << joint_names_.size() << std::endl;
std::cout << "joint_effort_command_.size() = " << joint_effort_command_.size() << std::endl;
std::cout << "joint_kp_command_.size() = " << joint_kp_command_.size() << std::endl;
std::cout << "joint_position_command_.size() = " << joint_position_command_.size() << std::endl;
std::cout << "joint_position_state_interface_.size() = " << joint_position_state_interface_.size() << std::endl;
std::cout << "joint_velocity_state_interface_.size() = " << joint_velocity_state_interface_.size() << std::endl;
std::cout << "joint_effort_command_interface_.size() = " << joint_effort_command_interface_.size() << std::endl;
throw std::runtime_error("Mismatch in vector sizes in update_and_write_commands");
}
for (size_t i = 0; i < joint_names_.size(); ++i) {
// PD Controller
const double torque = joint_effort_command_[i] + joint_kp_command_[i] * (
@ -98,7 +129,7 @@ namespace leg_pd_controller {
reference_interfaces.emplace_back(controller_name, joint_name + "/position", &joint_position_command_[ind]);
reference_interfaces.emplace_back(controller_name, joint_name + "/velocity",
&joint_velocities_command_[ind]);
reference_interfaces.emplace_back(controller_name, joint_name + "/torque", &joint_effort_command_[ind]);
reference_interfaces.emplace_back(controller_name, joint_name + "/effort", &joint_effort_command_[ind]);
reference_interfaces.emplace_back(controller_name, joint_name + "/kp", &joint_kp_command_[ind]);
reference_interfaces.emplace_back(controller_name, joint_name + "/kd", &joint_kd_command_[ind]);
ind++;

4
controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h
View File

@ -81,7 +81,9 @@ namespace unitree_guide_controller {
std::vector<std::string> state_interface_types_;
std::string imu_name_;
std::string command_prefix_;
std::vector<std::string> imu_interface_types_;
std::vector<std::string> feet_names_;
rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
@ -89,7 +91,7 @@ namespace unitree_guide_controller {
std::unordered_map<
std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> > *>
command_interface_map_ = {
{"torque", &ctrl_comp_.joint_torque_command_interface_},
{"effort", &ctrl_comp_.joint_torque_command_interface_},
{"position", &ctrl_comp_.joint_position_command_interface_},
{"velocity", &ctrl_comp_.joint_velocity_command_interface_},
{"kp", &ctrl_comp_.joint_kp_command_interface_},

2
controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h
View File

@ -21,7 +21,7 @@ public:
~QuadrupedRobot() = default;
void init(const std::string &robot_description);
void init(const std::string &robot_description, const std::vector<std::string> &feet_names);
/**
* Calculate the joint positions based on the foot end position

20
controllers/unitree_guide_controller/src/UnitreeGuideController.cpp
View File

@ -15,7 +15,11 @@ namespace unitree_guide_controller {
conf.names.reserve(joint_names_.size() * command_interface_types_.size());
for (const auto &joint_name: joint_names_) {
for (const auto &interface_type: command_interface_types_) {
conf.names.push_back(joint_name + "/" += interface_type);
if (!command_prefix_.empty()) {
conf.names.push_back(command_prefix_ + "/" + joint_name + "/" += interface_type);
} else {
conf.names.push_back(joint_name + "/" += interface_type);
}
}
}
@ -84,6 +88,9 @@ namespace unitree_guide_controller {
// imu sensor
imu_name_ = auto_declare<std::string>("imu_name", imu_name_);
imu_interface_types_ = auto_declare<std::vector<std::string> >("imu_interfaces", state_interface_types_);
command_prefix_ = auto_declare<std::string>("command_prefix", command_prefix_);
feet_names_ =
auto_declare<std::vector<std::string> >("feet_names", feet_names_);
} catch (const std::exception &e) {
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
return controller_interface::CallbackReturn::ERROR;
@ -105,9 +112,9 @@ namespace unitree_guide_controller {
});
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) {
ctrl_comp_.robot_model_.init(msg->data);
ctrl_comp_.robot_model_.init(msg->data, feet_names_);
ctrl_comp_.balance_ctrl_.init(ctrl_comp_.robot_model_);
});
@ -126,7 +133,12 @@ namespace unitree_guide_controller {
// assign command interfaces
for (auto &interface: command_interfaces_) {
command_interface_map_[interface.get_interface_name()]->push_back(interface);
std::string interface_name = interface.get_interface_name();
if (const size_t pos = interface_name.find('/'); pos != std::string::npos) {
command_interface_map_[interface_name.substr(pos + 1)]->push_back(interface);
} else {
command_interface_map_[interface_name]->push_back(interface);
}
}
// assign state interfaces

12
controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp
View File

@ -6,14 +6,15 @@
#include "unitree_guide_controller/control/CtrlComponent.h"
#include "unitree_guide_controller/robot/QuadrupedRobot.h"
void QuadrupedRobot::init(const std::string &robot_description) {
void QuadrupedRobot::init(const std::string &robot_description, const std::vector<std::string> &feet_names) {
KDL::Tree robot_tree;
kdl_parser::treeFromString(robot_description, robot_tree);
robot_tree.getChain("base", "FR_foot", fr_chain_);
robot_tree.getChain("base", "FL_foot", fl_chain_);
robot_tree.getChain("base", "RR_foot", rr_chain_);
robot_tree.getChain("base", "RL_foot", rl_chain_);
robot_tree.getChain("base", feet_names[0], fr_chain_);
robot_tree.getChain("base", feet_names[1], fl_chain_);
robot_tree.getChain("base", feet_names[2], rr_chain_);
robot_tree.getChain("base", feet_names[3], rl_chain_);
robot_legs_.resize(4);
robot_legs_[0] = std::make_shared<RobotLeg>(fr_chain_);
@ -70,6 +71,7 @@ std::vector<KDL::Frame> QuadrupedRobot::getFeet2BPositions() const {
result.resize(4);
for (int i = 0; i < 4; i++) {
result[i] = robot_legs_[i]->calcPEe2B(current_joint_pos_[i]);
result[i].M = KDL::Rotation::Identity();
}
return result;
}

1
controllers/unitree_guide_controller/src/robot/RobotLeg.cpp
View File

@ -18,7 +18,6 @@ RobotLeg::RobotLeg(const KDL::Chain &chain) {
KDL::Frame RobotLeg::calcPEe2B(const KDL::JntArray &joint_positions) const {
KDL::Frame pEe;
fk_pose_solver_->JntToCart(joint_positions, pEe);
pEe.M = KDL::Rotation::Identity();
return pEe;
}

8
descriptions/go2_description/config/robot_control.yaml
View File

@ -36,7 +36,7 @@ unitree_guide_controller:
- RL_calf_joint
command_interfaces:
- torque
- effort
- position
- velocity
- kp
@ -47,6 +47,12 @@ unitree_guide_controller:
- position
- velocity
feet_names:
- FR_FOOT
- FL_FOOT
- RR_FOOT
- RL_FOOT
imu_name: "imu_sensor"
imu_interfaces:

3
descriptions/go2_description/launch/hardware.launch.py
View File

@ -48,9 +48,6 @@ def launch_setup(context, *args, **kwargs):
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_controllers],
remappings=[
("~/robot_description", "/robot_description"),
],
output="both",
)

24
descriptions/go2_description/xacro/ros2_control.xacro
View File

@ -10,7 +10,7 @@
<joint name="FR_hip_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="torque"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
@ -22,7 +22,7 @@
<joint name="FR_thigh_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="torque"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
@ -34,7 +34,7 @@
<joint name="FR_calf_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="torque"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
@ -46,7 +46,7 @@
<joint name="FL_hip_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="torque"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
@ -58,7 +58,7 @@
<joint name="FL_thigh_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="torque"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
@ -70,7 +70,7 @@
<joint name="FL_calf_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="torque"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
@ -82,7 +82,7 @@
<joint name="RR_hip_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="torque"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
@ -94,7 +94,7 @@
<joint name="RR_thigh_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="torque"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
@ -106,7 +106,7 @@
<joint name="RR_calf_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="torque"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
@ -118,7 +118,7 @@
<joint name="RL_hip_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="torque"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
@ -130,7 +130,7 @@
<joint name="RL_thigh_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="torque"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
@ -142,7 +142,7 @@
<joint name="RL_calf_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="torque"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>

0
descriptions/quadruped_gazebo/readme.md → descriptions/quadruped_gazebo/README.md
View File

17
descriptions/quadruped_gazebo/config/jtc.yaml
View File

@ -13,6 +13,9 @@ controller_manager:
leg_pd_controller:
type: leg_pd_controller/LegPdController
unitree_guide_controller:
type: unitree_guide_controller/UnitreeGuideController
imu_sensor_broadcaster:
ros__parameters:
sensor_name: "imu_sensor"
@ -20,10 +23,9 @@ imu_sensor_broadcaster:
leg_pd_controller:
ros__parameters:
update_rate: 500 # Hz
joints:
- RF_HAA
- RH_HFE
- RF_HFE
- RF_KFE
- LF_HAA
- LF_HFE
@ -44,9 +46,10 @@ leg_pd_controller:
unitree_guide_controller:
ros__parameters:
command_prefix: "leg_pd_controller"
joints:
- RF_HAA
- RH_HFE
- RF_HFE
- RF_KFE
- LF_HAA
- LF_HFE
@ -59,7 +62,7 @@ unitree_guide_controller:
- LH_KFE
command_interfaces:
- torque
- effort
- position
- velocity
- kp
@ -70,6 +73,12 @@ unitree_guide_controller:
- position
- velocity
feet_names:
- RF_FOOT
- LF_FOOT
- RH_FOOT
- LH_FOOT
imu_name: "imu_sensor"
imu_interfaces:

11
descriptions/quadruped_gazebo/launch/gazebo.launch.py
View File

@ -66,6 +66,11 @@ def launch_setup(context, *args, **kwargs):
"--controller-manager", "/controller_manager"],
)
unitree_guide_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
)
return [
robot_state_publisher,
@ -83,6 +88,12 @@ def launch_setup(context, *args, **kwargs):
on_exit=[leg_pd_controller],
)
),
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=leg_pd_controller,
on_exit=[unitree_guide_controller],
)
),
]

35
descriptions/quadruped_gazebo/urdf/common/leg.xacro
View File

@ -8,7 +8,7 @@
<joint name="${prefix}_HAA" type="revolute">
<xacro:insert_block name="origin"/>
<parent link="base"/>
<parent link="trunk"/>
<child link="${prefix}_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
@ -40,7 +40,7 @@
<mesh filename="${mesh_path}/hip.dae"
scale="1 1 1"/>
</geometry>
<material name="orange"/>
<!-- <material name="orange"/>-->
</visual>
<collision>
<origin rpy="${pi/2.0} 0 0" xyz="0 0 0"/>
@ -81,7 +81,7 @@
scale="1 1 1"/>
</xacro:if>
</geometry>
<material name="orange"/>
<!-- <material name="orange"/>-->
</visual>
<collision>
<origin rpy="0 ${pi/2.0+0.1} 0" xyz="${thigh_x_offset} 0 ${-thigh_length/2.0}"/>
@ -121,7 +121,7 @@
<mesh filename="${mesh_path}/calf.dae"
scale="1 1 1"/>
</geometry>
<material name="orange"/>
<!-- <material name="orange"/>-->
</visual>
<collision>
<origin rpy="0 ${pi/2.0} 0" xyz="${calf_x_offset} 0 ${-calf_length/2.0}"/>
@ -151,7 +151,7 @@
<geometry>
<sphere radius="${foot_radius-0.01}"/>
</geometry>
<material name="orange"/>
<!-- <material name="orange"/>-->
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
@ -178,26 +178,45 @@
<gazebo reference="${prefix}_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
<material>
<ambient>.175 .175 .175 1.0</ambient>
<diffuse>.175 .175 .175 1.0</diffuse>
<specular>.175 .175 .175 1.000000 1.500000</specular>
</material>
</gazebo>
<gazebo reference="${prefix}_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>0</self_collide>
<material>Gazebo/DarkGrey</material>
<material>
<ambient>.175 .175 .175 1.0</ambient>
<diffuse>.175 .175 .175 1.0</diffuse>
<specular>.175 .175 .175 1.000000 1.500000</specular>
</material>
<kp value="1000000.0"/>
<kd value="100.0"/>
</gazebo>
<gazebo reference="${prefix}_calf">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>
<ambient>.175 .175 .175 1.0</ambient>
<diffuse>.175 .175 .175 1.0</diffuse>
<specular>.175 .175 .175 1.000000 1.500000</specular>
</material>
</gazebo>
<gazebo reference="${prefix}_FOOT">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<material>
<ambient>.175 .175 .175 1.0</ambient>
<diffuse>.175 .175 .175 1.0</diffuse>
<specular>.175 .175 .175 1.000000 1.500000</specular>
</material>
<kp value="1000000.0"/>
<kd value="100.0"/>

8
descriptions/quadruped_gazebo/urdf/go1/const.xacro
View File

@ -128,4 +128,12 @@
<!-- foot -->
<xacro:property name="foot_mass" value="0.06"/>
<gazebo reference="trunk">
<material>
<ambient>.175 .175 .175 1.0</ambient>
<diffuse>.175 .175 .175 1.0</diffuse>
<specular>.175 .175 .175 1.000000 1.500000</specular>
</material>
</gazebo>
</robot>

17
descriptions/quadruped_gazebo/urdf/robot.xacro
View File

@ -13,6 +13,21 @@
<xacro:property name="mesh_path" value="file://$(find quadruped_gazebo)/meshes/$(arg robot_type)"/>
<link name="base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base"/>
<child link="trunk"/>
</joint>
<link name="trunk">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
@ -35,7 +50,7 @@
</inertial>
</link>
<xacro:IMU connected_to="base" imu_name="base_imu" xyz="0. 0. 0." rpy="0. 0. 0."/>
<xacro:IMU connected_to="trunk" imu_name="base_imu" xyz="0. 0. 0." rpy="0. 0. 0."/>
<xacro:leg prefix="LF" mesh_path="${mesh_path}" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True">

2
hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp
View File

@ -100,7 +100,7 @@ std::vector<hardware_interface::CommandInterface> HardwareUnitree::export_comman
ind = 0;
for (const auto &joint_name: joint_interfaces["effort"]) {
command_interfaces.emplace_back(joint_name, "torque", &joint_torque_command_[ind]);
command_interfaces.emplace_back(joint_name, "effort", &joint_torque_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++;