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

.vscode/extensions.json

@@ -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.python",
-		"ms-python.vscode-pylance",
+		// "ms-python.vscode-pylance",
-		"ban.spellright",
+		// "ban.spellright",
-		"ms-iot.vscode-ros",
+		// "ms-iot.vscode-ros",
-		"ms-python.black-formatter",
+		// "ms-python.black-formatter",
-		"ms-python.flake8",
+		// "ms-python.flake8",
 	]
 }

source/FLEXR_v0/FLEXR_v0/robots/FLEXR_v0.py

@@ -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
+    # )
+)

source/FLEXR_v0/FLEXR_v0/tasks/direct/flexr_v0/flexr_v0_env.py

@@ -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),
+                arm_pos,        # 摆臂位置 [num_envs, 4]
-                self.joint_vel[:, self._pole_dof_idx[0]].unsqueeze(dim=1),
+                arm_vel,        # 摆臂速度 [num_envs, 4]
-                self.joint_pos[:, self._cart_dof_idx[0]].unsqueeze(dim=1),
+                wheel_vel,      # 轮组平均速度 [num_envs, 4]
-                self.joint_vel[:, self._cart_dof_idx[0]].unsqueeze(dim=1),
             ),
             dim=-1,
         )
+        
         observations = {"policy": obs}
         return observations
-
+    
     def _get_rewards(self) -> torch.Tensor:
-        total_reward = compute_rewards(
+        # total_reward = compute_rewards(
-            self.cfg.rew_scale_alive,
+        #     self.cfg.rew_scale_alive,
-            self.cfg.rew_scale_terminated,
+        #     self.cfg.rew_scale_terminated,
-            self.cfg.rew_scale_pole_pos,
+        #     self.cfg.rew_scale_pole_pos,
-            self.cfg.rew_scale_cart_vel,
+        #     self.cfg.rew_scale_cart_vel,
-            self.cfg.rew_scale_pole_vel,
+        #     self.cfg.rew_scale_pole_vel,
-            self.joint_pos[:, self._pole_dof_idx[0]],
+        #     self.joint_pos[:, self._pole_dof_idx[0]],
-            self.joint_vel[:, self._pole_dof_idx[0]],
+        #     self.joint_vel[:, self._pole_dof_idx[0]],
-            self.joint_pos[:, self._cart_dof_idx[0]],
+        #     self.joint_pos[:, self._cart_dof_idx[0]],
-            self.joint_vel[:, self._cart_dof_idx[0]],
+        #     self.joint_vel[:, self._cart_dof_idx[0]],
-            self.reset_terminated,
+        #     self.reset_terminated,
-        )
+        # )
-        return total_reward
+        # 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
+        # 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 = 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)
+        # 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 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
+            env_ids = self.robot._ALL_INDICES # type: ignore
-        super()._reset_idx(env_ids)
+        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(
+        # joint_pos[:, self._pole_dof_idx] += sample_uniform(
-            self.cfg.initial_pole_angle_range[0] * math.pi,
+        #     self.cfg.initial_pole_angle_range[0] * math.pi,
-            self.cfg.initial_pole_angle_range[1] * math.pi,
+        #     self.cfg.initial_pole_angle_range[1] * math.pi,
-            joint_pos[:, self._pole_dof_idx].shape,
+        #     joint_pos[:, self._pole_dof_idx].shape,
-            joint_pos.device,
+        #     joint_pos.device,
-        )
+        # )
         joint_vel = self.robot.data.default_joint_vel[env_ids]
 
         default_root_state = self.robot.data.default_root_state[env_ids]

source/FLEXR_v0/FLEXR_v0/tasks/direct/flexr_v0/flexr_v0_env_cfg.py

@@ -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
+    action_space = 8
-    observation_space = 4
+    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
+    # # custom parameters/scales
-    cart_dof_name = "slider_to_cart"
+    # # - controllable joint
-    pole_dof_name = "cart_to_pole"
+    # cart_dof_name = "slider_to_cart"
-    # - action scale
+    # pole_dof_name = "cart_to_pole"
-    action_scale = 100.0  # [N]
+    # # - action scale
-    # - reward scales
+    # action_scale = 100.0  # [N]
-    rew_scale_alive = 1.0
+    # # - reward scales
-    rew_scale_terminated = -2.0
+    # rew_scale_alive = 1.0
-    rew_scale_pole_pos = -1.0
+    # rew_scale_terminated = -2.0
-    rew_scale_cart_vel = -0.01
+    # rew_scale_pole_pos = -1.0
-    rew_scale_pole_vel = -0.005
+    # rew_scale_cart_vel = -0.01
-    # - reset states/conditions
+    # rew_scale_pole_vel = -0.005
-    initial_pole_angle_range = [-0.25, 0.25]  # pole angle sample range on reset [rad]
+    # # - reset states/conditions
-    max_cart_pos = 3.0  # reset if cart exceeds this position [m]
+    # 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]