hexone2086/FLEXR_v0_IsaacLab
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

初步添加机器人并设定动作空间

Browse Source
This commit is contained in:
CaoWangrenbo
2025-06-16 12:16:07 +08:00
parent b03507ab4e
commit b5078e9c6d
6 changed files with 237 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
.vscode/extensions.json vendored
View File

@@ -2,11 +2,11 @@
// See http://go.microsoft.com/fwlink/?LinkId=827846
// for the documentation about the extensions.json format
"recommendations": [
"ms-python.python",
"ms-python.vscode-pylance",
"ban.spellright",
"ms-iot.vscode-ros",
"ms-python.black-formatter",
"ms-python.flake8",
// "ms-python.python",
// "ms-python.vscode-pylance",
// "ban.spellright",
// "ms-iot.vscode-ros",
// "ms-python.black-formatter",
// "ms-python.flake8",
]
}

BIN
scripts/rsl_rl/__pycache__/cli_args.cpython-310.pyc
View File

Binary file not shown.

70
source/FLEXR_v0/FLEXR_v0/robots/FLEXR_v0.py Normal file
View File

@@ -0,0 +1,70 @@
import isaaclab.sim as sim_utils
from isaaclab.assets import ArticulationCfg
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
import math
FLEXR_CONFIG = ArticulationCfg(
spawn=sim_utils.UsdFileCfg(
usd_path="/home/hexone/Documents/ftr.usd",
),
init_state=ArticulationCfg.InitialStateCfg(
joint_pos={
"arm_FL": math.radians(45.0),
"arm_FR": math.radians(45.0),
"arm_BL": math.radians(45.0),
"arm_BR": math.radians(45.0),
},
),
actuators={
# 摆臂执行器组
"arm_acts": ImplicitActuatorCfg(
joint_names_expr=["arm_(FL|FR|BL|BR)"],
effort_limit_sim=100.0, # 最大扭矩 (N·m)
velocity_limit_sim=100.0, # 最大速度 (rad/s)
stiffness=800.0, # 位置控制的刚度
damping=5.0, # 位置控制的阻尼
friction=0.1 # 可选：关节摩擦
),
# 轮组执行器 - 每个腿部一个执行器
"wheel_FL": ImplicitActuatorCfg(
joint_names_expr=["wheel_FL_[1-5]"],
effort_limit_sim=100.0, # 最大扭矩 (N·m)
# velocity_limit_sim=100.0, # 可选：最大速度限制
stiffness=0.0, # 设置为0表示纯速度/扭矩控制
damping=10000.0 # 高阻尼确保轮子自由转动但稳定
),
"wheel_FR": ImplicitActuatorCfg(
joint_names_expr=["wheel_FR_[1-5]"],
effort_limit_sim=100.0,
stiffness=0.0,
damping=10000.0
),
"wheel_BL": ImplicitActuatorCfg(
joint_names_expr=["wheel_BL_[1-5]"],
effort_limit_sim=100.0,
stiffness=0.0,
damping=10000.0
),
"wheel_BR": ImplicitActuatorCfg(
joint_names_expr=["wheel_BR_[1-5]"],
effort_limit_sim=100.0,
stiffness=0.0,
damping=10000.0
)
},
# 可选：物理材质覆盖
# physics_material=sim_utils.RigidBodyMaterialCfg(
# static_friction=1.0,
# dynamic_friction=0.5,
# restitution=0.1
# ),
# 可选：碰撞过滤器设置
# collision_props=sim_utils.CollisionPropertiesCfg(
# collision_enabled=True,
# contact_offset=0.02,
# rest_offset=0.002
# )
)

0
source/FLEXR_v0/FLEXR_v0/robots/__init__.py Normal file
View File

171
source/FLEXR_v0/FLEXR_v0/tasks/direct/flexr_v0/flexr_v0_env.py
View File

@@ -17,18 +17,62 @@ from isaaclab.utils.math import sample_uniform
from .flexr_v0_env_cfg import FlexrV0EnvCfg
import logging
# Set the default log level to WARNING
logging.basicConfig(level=logging.WARNING)
# Replace logging.debug statements with debug-level log logging.debugs
logging.debug("Your log message here")
class FlexrV0Env(DirectRLEnv):
cfg: FlexrV0EnvCfg
def __init__(self, cfg: FlexrV0EnvCfg, render_mode: str | None = None, **kwargs):
super().__init__(cfg, render_mode, **kwargs)
# 获取所有关节名称
joint_names = self.robot.joint_names
logging.debug(joint_names)
self._cart_dof_idx, _ = self.robot.find_joints(self.cfg.cart_dof_name)
self._pole_dof_idx, _ = self.robot.find_joints(self.cfg.pole_dof_name)
# 创建执行器组到关节索引列表的映射
self._actuator_joint_indices = {}
# 摆臂执行器组关节索引
self._arm_joint_indices = []
for name in ["arm_FL", "arm_FR", "arm_BL", "arm_BR"]:
idx, _ = self.robot.find_joints(name)
self._arm_joint_indices.extend(list(idx))
self._actuator_joint_indices["arm_acts"] = self._arm_joint_indices
# 轮组执行器关节索引
self._actuator_joint_indices["wheel_FL"] = self._get_wheel_joint_indices("wheel_FL", joint_names)
self._actuator_joint_indices["wheel_FR"] = self._get_wheel_joint_indices("wheel_FR", joint_names)
self._actuator_joint_indices["wheel_BL"] = self._get_wheel_joint_indices("wheel_BL", joint_names)
self._actuator_joint_indices["wheel_BR"] = self._get_wheel_joint_indices("wheel_BR", joint_names)
# 合并所有轮子关节索引（存储为列表）
self._all_wheel_joint_indices = (
self._actuator_joint_indices["wheel_FL"] +
self._actuator_joint_indices["wheel_FR"] +
self._actuator_joint_indices["wheel_BL"] +
self._actuator_joint_indices["wheel_BR"]
)
# 调试打印所有映射
logging.debug("Actuator Group to Joint Indices:", self._actuator_joint_indices)
# 初始化状态变量
self.joint_pos = self.robot.data.joint_pos
self.joint_vel = self.robot.data.joint_vel
def _get_wheel_joint_indices(self, prefix: str, joint_names: list[str]) -> list[int]:
"""获取指定腿部的所有轮子关节索引（返回整数列表）"""
indices = []
for i, name in enumerate(joint_names):
if name.startswith(prefix):
indices.append(i)
return indices
def _setup_scene(self):
self.robot = Articulation(self.cfg.robot_cfg)
@@ -46,57 +90,118 @@ class FlexrV0Env(DirectRLEnv):
self.actions = actions.clone()
def _apply_action(self) -> None:
self.robot.set_joint_effort_target(self.actions * self.cfg.action_scale, joint_ids=self._cart_dof_idx)
# 确保动作有正确形状 [num_envs, 8]
if self.actions.dim() == 1:
self.actions = self.actions.unsqueeze(0)
# 分离动作分量
arm_actions = self.actions[:, :4] # 前4个分量控制摆臂 (FL, FR, BL, BR)
wheel_actions = self.actions[:, 4:8] # 后4个分量控制轮组 (FL, FR, BL, BR)
# --- 摆臂控制部分 ---
arm_pos_target = torch.zeros_like(self.robot.data.joint_pos[:, self._arm_joint_indices])
arm_pos_target.copy_(arm_actions)
logging.debug(f"Total joints: {len(self.robot.joint_names)}")
logging.debug(f"Arm joint indices: {self._arm_joint_indices}")
logging.debug(f"Wheel joint counts: { {k:len(v) for k,v in self._actuator_joint_indices.items() if k.startswith('wheel')} }")
logging.debug(f"Initial arm positions: {self.robot.data.joint_pos[0, self._arm_joint_indices]}")
logging.debug(f"Arm actions: {arm_actions[0]}")
# 设置目标位置（只针对arm关节）
self.robot.set_joint_position_target(
arm_pos_target,
joint_ids=self._arm_joint_indices
)
# --- 轮组控制部分 ---
wheel_vel_target = torch.zeros_like(self.robot.data.joint_vel) # [num_envs, 24]
wheel_groups = ["wheel_FL", "wheel_FR", "wheel_BL", "wheel_BR"]
for i, group in enumerate(wheel_groups):
joint_indices = self._actuator_joint_indices[group] # 全局索引
wheel_vel_target[:, joint_indices] = wheel_actions[:, i].unsqueeze(-1)
self.robot.set_joint_velocity_target(
wheel_vel_target[:, self._all_wheel_joint_indices], # 只选择轮组部分
joint_ids=self._all_wheel_joint_indices
)
def _get_observations(self) -> dict:
# 获取摆臂关节的位置和速度
arm_pos = self.joint_pos[:, self._arm_joint_indices] # [num_envs, 4]
arm_vel = self.joint_vel[:, self._arm_joint_indices] # [num_envs, 4]
# 计算每个轮组的平均速度
wheel_FL_vel = self.joint_vel[:, self._actuator_joint_indices["wheel_FL"]].mean(dim=1, keepdim=True)
wheel_FR_vel = self.joint_vel[:, self._actuator_joint_indices["wheel_FR"]].mean(dim=1, keepdim=True)
wheel_BL_vel = self.joint_vel[:, self._actuator_joint_indices["wheel_BL"]].mean(dim=1, keepdim=True)
wheel_BR_vel = self.joint_vel[:, self._actuator_joint_indices["wheel_BR"]].mean(dim=1, keepdim=True)
# 组合轮组速度
wheel_vel = torch.cat([wheel_FL_vel, wheel_FR_vel, wheel_BL_vel, wheel_BR_vel], dim=1) # [num_envs, 4]
# 特权信息 - 暂不加入
base_pos = self.robot.data.root_pos_w
base_quat = self.robot.data.root_quat_w
base_lin_vel = self.robot.data.root_lin_vel_w
base_ang_vel = self.robot.data.root_ang_vel_w
#
# 组合所有观测
obs = torch.cat(
(
self.joint_pos[:, self._pole_dof_idx[0]].unsqueeze(dim=1),
self.joint_vel[:, self._pole_dof_idx[0]].unsqueeze(dim=1),
self.joint_pos[:, self._cart_dof_idx[0]].unsqueeze(dim=1),
self.joint_vel[:, self._cart_dof_idx[0]].unsqueeze(dim=1),
arm_pos, # 摆臂位置 [num_envs, 4]
arm_vel, # 摆臂速度 [num_envs, 4]
wheel_vel, # 轮组平均速度 [num_envs, 4]
),
dim=-1,
)
observations = {"policy": obs}
return observations
def _get_rewards(self) -> torch.Tensor:
total_reward = compute_rewards(
self.cfg.rew_scale_alive,
self.cfg.rew_scale_terminated,
self.cfg.rew_scale_pole_pos,
self.cfg.rew_scale_cart_vel,
self.cfg.rew_scale_pole_vel,
self.joint_pos[:, self._pole_dof_idx[0]],
self.joint_vel[:, self._pole_dof_idx[0]],
self.joint_pos[:, self._cart_dof_idx[0]],
self.joint_vel[:, self._cart_dof_idx[0]],
self.reset_terminated,
)
return total_reward
# total_reward = compute_rewards(
# self.cfg.rew_scale_alive,
# self.cfg.rew_scale_terminated,
# self.cfg.rew_scale_pole_pos,
# self.cfg.rew_scale_cart_vel,
# self.cfg.rew_scale_pole_vel,
# self.joint_pos[:, self._pole_dof_idx[0]],
# self.joint_vel[:, self._pole_dof_idx[0]],
# self.joint_pos[:, self._cart_dof_idx[0]],
# self.joint_vel[:, self._cart_dof_idx[0]],
# self.reset_terminated,
# )
# return total_reward
# 返回一个全为0的float张量
return torch.zeros_like(self.reset_terminated).float()
def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]:
self.joint_pos = self.robot.data.joint_pos
self.joint_vel = self.robot.data.joint_vel
time_out = self.episode_length_buf >= self.max_episode_length - 1
out_of_bounds = torch.any(torch.abs(self.joint_pos[:, self._cart_dof_idx]) > self.cfg.max_cart_pos, dim=1)
out_of_bounds = out_of_bounds | torch.any(torch.abs(self.joint_pos[:, self._pole_dof_idx]) > math.pi / 2, dim=1)
return out_of_bounds, time_out
# time_out = self.episode_length_buf >= self.max_episode_length - 1
# out_of_bounds = torch.any(torch.abs(self.joint_pos[:, self._cart_dof_idx]) > self.cfg.max_cart_pos, dim=1)
# out_of_bounds = out_of_bounds | torch.any(torch.abs(self.joint_pos[:, self._pole_dof_idx]) > math.pi / 2, dim=1)
# return out_of_bounds, time_out
return torch.zeros_like(self.reset_terminated), torch.zeros_like(self.reset_terminated)
def _reset_idx(self, env_ids: Sequence[int] | None):
if env_ids is None:
env_ids = self.robot._ALL_INDICES
super()._reset_idx(env_ids)
env_ids = self.robot._ALL_INDICES # type: ignore
super()._reset_idx(env_ids) # type: ignore
joint_pos = self.robot.data.default_joint_pos[env_ids]
joint_pos[:, self._pole_dof_idx] += sample_uniform(
self.cfg.initial_pole_angle_range[0] * math.pi,
self.cfg.initial_pole_angle_range[1] * math.pi,
joint_pos[:, self._pole_dof_idx].shape,
joint_pos.device,
)
# joint_pos[:, self._pole_dof_idx] += sample_uniform(
# self.cfg.initial_pole_angle_range[0] * math.pi,
# self.cfg.initial_pole_angle_range[1] * math.pi,
# joint_pos[:, self._pole_dof_idx].shape,
# joint_pos.device,
# )
joint_vel = self.robot.data.default_joint_vel[env_ids]
default_root_state = self.robot.data.default_root_state[env_ids]

43
source/FLEXR_v0/FLEXR_v0/tasks/direct/flexr_v0/flexr_v0_env_cfg.py
View File

@@ -3,7 +3,9 @@
#
# SPDX-License-Identifier: BSD-3-Clause
from isaaclab_assets.robots.cartpole import CARTPOLE_CFG
# from isaaclab_assets.robots.cartpole import CARTPOLE_CFG
from FLEXR_v0.robots.FLEXR_v0 import FLEXR_CONFIG
from isaaclab.assets import ArticulationCfg
from isaaclab.envs import DirectRLEnvCfg
@@ -18,31 +20,32 @@ class FlexrV0EnvCfg(DirectRLEnvCfg):
decimation = 2
episode_length_s = 5.0
# - spaces definition
action_space = 1
observation_space = 4
action_space = 8
observation_space = 12
state_space = 0
# simulation
sim: SimulationCfg = SimulationCfg(dt=1 / 120, render_interval=decimation)
# robot(s)
robot_cfg: ArticulationCfg = CARTPOLE_CFG.replace(prim_path="/World/envs/env_.*/Robot")
robot_cfg: ArticulationCfg = FLEXR_CONFIG.replace(prim_path="/World/envs/env_.*/Robot") # type: ignore
# scene
scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=4096, env_spacing=4.0, replicate_physics=True)
scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=128, env_spacing=4.0, replicate_physics=True)
# custom parameters/scales
# - controllable joint
cart_dof_name = "slider_to_cart"
pole_dof_name = "cart_to_pole"
# - action scale
action_scale = 100.0 # [N]
# - reward scales
rew_scale_alive = 1.0
rew_scale_terminated = -2.0
rew_scale_pole_pos = -1.0
rew_scale_cart_vel = -0.01
rew_scale_pole_vel = -0.005
# - reset states/conditions
initial_pole_angle_range = [-0.25, 0.25] # pole angle sample range on reset [rad]
max_cart_pos = 3.0 # reset if cart exceeds this position [m]
# 待增加的自定义参数
# # custom parameters/scales
# # - controllable joint
# cart_dof_name = "slider_to_cart"
# pole_dof_name = "cart_to_pole"
# # - action scale
# action_scale = 100.0 # [N]
# # - reward scales
# rew_scale_alive = 1.0
# rew_scale_terminated = -2.0
# rew_scale_pole_pos = -1.0
# rew_scale_cart_vel = -0.01
# rew_scale_pole_vel = -0.005
# # - reset states/conditions
# initial_pole_angle_range = [-0.25, 0.25] # pole angle sample range on reset [rad]
# max_cart_pos = 3.0 # reset if cart exceeds this position [m]