Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 10 additions & 4 deletions src/humanoid_loco_mujoco/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,13 @@ https://github.com/robfiras/loco-mujoco/tree/master/loco_mujoco/models/unitree_h

# _文件说明_

- `main`:使用locomujoco的简单示例
- `mujoco`:使用mujoco的简单实例
- `robot_RL`:使用mujoco进行的强化学习训练
- `mujoco_ros2_sim`:使用mujoco和ros2进行仿真
```
locomujoco
├── ros2 #ros2接口
│ ├── mujoco_ros2_sim # 使用mujoco进行的强化学习训练
│ └── robot_controller # 控制节点
├── main # 使用locomujoco的简单示例
├── mujoco #使用mujoco的简单实例
├── robot_RL # 强化学习算法

```
97 changes: 91 additions & 6 deletions src/humanoid_loco_mujoco/main.py
Original file line number Diff line number Diff line change
@@ -1,16 +1,101 @@
import numpy as np
from loco_mujoco.task_factories import ImitationFactory, LAFAN1DatasetConf, DefaultDatasetConf, AMASSDatasetConf
from loco_mujoco.core import ObservationType
randomization_config = {
# gravity
"randomize_gravity": True,
"gravity_range": [9.51, 10.11],

# geom properties
"randomize_geom_friction_tangential": True,
"geom_friction_tangential_range": [0.8, 1.2],
"randomize_geom_friction_torsional": True,
"geom_friction_torsional_range": [0.003, 0.007],
"randomize_geom_friction_rolling": True,
"geom_friction_rolling_range": [0.00008, 0.00012],
"randomize_geom_damping": True,
"geom_damping_range": [72, 88],
"randomize_geom_stiffness": True,
"geom_stiffness_range": [900, 1100],

# joint properties
"randomize_joint_damping": True,
"joint_damping_range": [0.3, 1.5],
"randomize_joint_stiffness": True,
"joint_stiffness_range": [0.9, 1.1],
"randomize_joint_friction_loss": True,
"joint_friction_loss_range": [0.0, 0.2],
"randomize_joint_armature": True,
"joint_armature_range": [0.08, 0.12],

# base mass
"randomize_base_mass": True,
"base_mass_to_add_range": [-2.0, 2.0],

# COM
"randomize_com_displacement": True,
"com_displacement_range": [-0.15, 0.15],

# link mass
"randomize_link_mass": True,
"link_mass_multiplier_range": {
"root_body": [0.5, 1.9],
"other_bodies": [0.8, 1.2],
},

# PD Gains (if PDControl is used)
"add_p_gains_noise": True,
"add_d_gains_noise": True,
"p_gains_noise_scale": 0.1,
"d_gains_noise_scale": 0.1,

# Observation Noise
"add_joint_pos_noise": True,
"joint_pos_noise_scale": 0.003,
"add_joint_vel_noise": True,
"joint_vel_noise_scale": 0.08,
"add_gravity_noise": True,
"gravity_noise_scale": 0.015,
"add_free_joint_lin_vel_noise": True,
"lin_vel_noise_scale": 0.1,
"add_free_joint_ang_vel_noise": True,
"ang_vel_noise_scale": 0.02,
}
observation_spec = [
# prioritized observations
ObservationType.FreeJointPosNoXY(obs_name="free_joint", xml_name="root", group="prioritized", allow_randomization=False),
ObservationType.FreeJointVel(obs_name="free_joint_vel", xml_name="root", group="prioritized", allow_randomization=False),
# 修正左髋关节名称(之前的KeyError问题)
ObservationType.JointPos(obs_name="joint_pos", xml_name="hip_flexion_l", group="prioritized", allow_randomization=False),
# 修正右髋关节名称
ObservationType.JointVel(obs_name="joint_vel1", xml_name="hip_flexion_r", group="prioritized", allow_randomization=False),
# 修正左膝关节名称
ObservationType.JointVel(obs_name="joint_vel2", xml_name="knee_angle_l", group="prioritized", allow_randomization=False),
# 将head替换为有效的身体名称(例如torso_link)
ObservationType.BodyPos(obs_name="head_pos", xml_name="torso_link", group="prioritized", allow_randomization=False),
# policy observations --> these will be noisy
ObservationType.ProjectedGravityVector(obs_name="proj_grav", xml_name="root", group="policy", allow_randomization=True),
ObservationType.FreeJointVel(obs_name="free_joint_vel_pi", xml_name="root", group="policy", allow_randomization=True),
# 修正左髋关节名称
ObservationType.JointPos(obs_name="joint_pos_pi", xml_name="hip_flexion_l", group="policy", allow_randomization=True),
# 修正右髋关节名称
ObservationType.JointVel(obs_name="joint_vel1_pi", xml_name="hip_flexion_r", group="policy", allow_randomization=True),
# 修正左膝关节名称
ObservationType.JointVel(obs_name="joint_vel2_pi", xml_name="knee_angle_l", group="policy", allow_randomization=True),
# 将head替换为有效的身体名称(例如torso_link)
ObservationType.BodyPos(obs_name="head_pos_pi", xml_name="torso_link", group="policy", allow_randomization=True),
ObservationType.LastAction(obs_name="last_action_pi", group="policy", allow_randomization=True)
]

# # example --> you can add as many datasets as you want in the lists!
env = ImitationFactory.make("UnitreeH1",
default_dataset_conf=DefaultDatasetConf(["squat", "walk"]),
lafan1_dataset_conf=LAFAN1DatasetConf(["dance2_subject4"]),
# if SMPL and AMASS are installed, you can use the following:
# amass_dataset_conf=AMASSDatasetConf(["DanceDB/DanceDB/20120911_TheodorosSourmelis/Capoeira_Theodoros_v2_C3D_poses",
# "KIT/12/WalkInClockwiseCircle11_poses",
# "HUMAN4D/HUMAN4D/Subject3_Medhi/INF_JumpingJack_S3_01_poses",
# 'KIT/359/walking_fast05_poses']),
terrain_type="RoughTerrain",
terrain_params=dict(random_min_height=-0.1,random_max_height=0.1,random_downsampled_scale=0.5),
domain_randomization_type="DefaultRandomizer",
domain_randomization_params=randomization_config,
observation_spec=observation_spec,
control_type="PDControl", control_params=dict(p_gain=100, d_gain=1),
n_substeps=20)

env.play_trajectory(n_episodes=3, n_steps_per_episode=500, render=True)
139 changes: 139 additions & 0 deletions src/humanoid_loco_mujoco/ros2/mujoco_ros2_sim.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,139 @@
#include "rclcpp/rclcpp.hpp"
#include "my_interfaces/srv/robot_ctrl.hpp"
#include "my_interfaces/msg/robot_state.hpp"
#include <mujoco/mujoco.h>
#include <fstream>
#include <string>
#include <thread>

// 全局变量
std::string current_action = "stop";

class MujocoRos2Sim : public rclcpp::Node {
public:
MujocoRos2Sim() : Node("mujoco_ros2_sim") {
RCLCPP_INFO(this->get_logger(), "节点已启动:mujoco_ros2_sim!");

// 加载模型
char error[1000];
model_ = mj_loadXML("RobotH.xml", nullptr, error, 1000);
if (!model_) {
RCLCPP_ERROR(this->get_logger(), "模型加载失败: %s", error);
exit(1);
}
data_ = mj_makeData(model_);

// 加载轨迹数据(这里简化处理,实际需要根据npz格式解析)
load_trajectory_data();

// 创建viewer
viewer_ = mjViewerCreate();
mjv_defaultCamera(&cam_);
mjv_defaultOption(&opt_);
mjr_defaultContext(&con_);
mjv_makeScene(model_, &scene_, 1000);
mjr_makeContext(model_, &con_, mjFONTSCALE_150);
mjViewerSetup(viewer_, &scene_, &cam_, &opt_, &con_);

// 创建服务
service_ = this->create_service<my_interfaces::srv::RobotCtrl>(
"robot_control",
std::bind(&MujocoRos2Sim::control_callback, this, std::placeholders::_1, std::placeholders::_2)
);

// 创建发布者
publisher_ = this->create_publisher<my_interfaces::msg::RobotState>("robot_state", 10);

// 启动模拟线程
sim_thread_ = std::thread(&MujocoRos2Sim::run_sim, this);
}

~MujocoRos2Sim() {
if (sim_thread_.joinable()) {
sim_thread_.join();
}
mjViewerClose(viewer_);
mjr_freeContext(&con_);
mjv_freeScene(&scene_);
mj_deleteData(data_);
mj_deleteModel(model_);
}

private:
mjModel* model_ = nullptr;
mjData* data_ = nullptr;
mjViewer* viewer_ = nullptr;
mjvScene scene_;
mjvCamera cam_;
mjvOption opt_;
mjrContext con_;
rclcpp::Service<my_interfaces::srv::RobotCtrl>::SharedPtr service_;
rclcpp::Publisher<my_interfaces::msg::RobotState>::SharedPtr publisher_;
std::thread sim_thread_;

// 轨迹数据(示例)
std::vector<double> walk_qpos, walk_qvel;
std::vector<double> squat_qpos, squat_qvel;

void load_trajectory_data() {
// 实际应用中需要解析npz文件
// 这里仅做示例初始化
walk_qpos.push_back(0.0);
walk_qvel.push_back(0.1);
squat_qpos.push_back(-0.5);
squat_qvel.push_back(0.0);
}

void control_callback(
const std::shared_ptr<my_interfaces::srv::RobotCtrl::Request> request,
std::shared_ptr<my_interfaces::srv::RobotCtrl::Response> response
) {
RCLCPP_INFO(this->get_logger(), "收到指令: %s", request->command.c_str());
current_action = request->command;
response->result = true;
response->message = "指令已接收";
}

void publish_status() {
auto msg = my_interfaces::msg::RobotState();
msg.action = current_action;
publisher_->publish(msg);
}

void run_sim() {
while (rclcpp::ok()) {
// 控制动作
if (current_action == "squat" && !squat_qpos.empty()) {
data_->qpos[0] = squat_qpos[0];
data_->qvel[0] = squat_qvel[0];
} else if (current_action == "walk" && !walk_qpos.empty()) {
data_->qpos[0] = walk_qpos[0];
data_->qvel[0] = walk_qvel[0];
} else if (current_action == "stop") {
data_->qpos[0] = 0;
data_->qvel[0] = 0;
}

// 模拟一步
mj_step(model_, data_);

// 渲染
mjv_updateScene(model_, data_, &opt_, nullptr, &cam_, mjCAT_ALL, &scene_);
mjViewerRender(viewer_, &scene_, &con_);

// 发布状态
publish_status();

// 小延迟
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}
};

int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
auto node = std::make_shared<MujocoRos2Sim>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
95 changes: 95 additions & 0 deletions src/humanoid_loco_mujoco/ros2/robot_controller.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,95 @@
import sys
import select
import rclpy
from rclpy.node import Node
from my_interfaces.srv import RobotCtrl

# 全局变量(提前初始化,避免未绑定)
current_action_key = 'stop'
action_switch_flag = False


class RobotController(Node):
def __init__(self, name):
super().__init__(name)
# 创建服务客户端,连接机器人控制服务
self.client = self.create_client(RobotCtrl, 'robot_control')
# 等待服务上线
while not self.client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('等待服务 "robot_control" 启动...')
self.get_logger().info("操作提示:w=行走 | s=下蹲 | q=退出 | 其他键=停止")

def result_callback(self, future):
"""服务调用结果的回调函数"""

response = future.result()
self.get_logger().info(f'指令执行成功:{response.message}')


def send_request(self, action):
"""发送控制指令到服务端"""

request = RobotCtrl.Request()
request.command = action # 传递动作指令
self.client.call_async(request).add_done_callback(self.result_callback)
self.get_logger().info(f'已发送指令:{action}')



def read_keypress_non_blocking():
"""非阻塞读取终端按键"""
if select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []):
key = sys.stdin.read(1)
return key.strip()
return None


def main(args=None):
# 关键:声明使用全局变量(修复核心错误)
global current_action_key, action_switch_flag

rclpy.init(args=args)
robot_controller = RobotController("robot_controller")

# 初始发送停止指令(此时current_action_key已绑定全局值'stop')
robot_controller.send_request(current_action_key)

try:
# 主循环:非阻塞读取按键 + 处理ROS事件
while rclpy.ok():
# 1. 读取终端按键
key = read_keypress_non_blocking()
if key:
# 按键逻辑
if key == 'w' and current_action_key != "walk":
current_action_key = "walk"
action_switch_flag = True
elif key == 's' and current_action_key != "squat":
current_action_key = "squat"
action_switch_flag = True
elif key == 'q': # q键退出
robot_controller.get_logger().info("收到退出指令,程序终止")
break
elif key not in ['w', 's']: # 其他键停止
if current_action_key != "stop":
current_action_key = "stop"
action_switch_flag = True

# 2. 检测动作切换并发送指令
if action_switch_flag:
robot_controller.send_request(current_action_key)
action_switch_flag = False # 重置标记

# 3. 非阻塞处理ROS回调
rclpy.spin_once(robot_controller, timeout_sec=0.01)

except KeyboardInterrupt:
robot_controller.get_logger().info("用户强制退出")
finally:
robot_controller.destroy_node()
rclpy.shutdown()
print("程序已退出")


if __name__ == '__main__':
main()
Loading