Mimicgen基本环境介绍Stack_d0

---

前言

仅介绍现有环境中提供的资源，鉴于 demo 中案例众多，本文以解析 case Stack_d0 为切入点进行介绍。

1. 机器人介绍

                                    0:Sawyer                                                    1:Panda                                                              2:Jaco

                                       3:Kinova3                                                      4:IIWA                                                     5:UR5e

2. 夹具介绍

0:RethinkGripper                                      1:PandaGripper                                         2:JacoThreeFingerGripper

               3:JacoThreeFingerDexterousGripper                          4:WipingGripper                                         5:Robotiq85Gripper

                   6:Robotiq140Gripper                                   7:RobotiqThreeFingerGripper              8:RobotiqThreeFingerDexterousGripper

3. 交叉测试

针对现有数据集,训练一个模型,同时进行更换机器人/夹具进行测试:
训练数据中机器人及夹具型号:

机器人: Panda
夹具: PandaGripper

更换机械人/夹具

测试场景

机器人	夹具	horizon	50次成功次数
Panda	PandaGripper	1000	48
Sawyer	PandaGripper	1000	46
Jaco	PandaGripper	1000	44
Kinova3	PandaGripper	1000	14
Panda	RethinkGripper	1000	5
Panda	WipingGripper	1000	/
Panda	RobotiqThreeFingerGripper	1000	/
Panda	Robotiq140Gripper	1000	/
Panda	RobotiqThreeFingerDexterousGripper	1000	/
Panda	Robotiq85Gripper	1000	/

修改物体属性

size=0.02*0.05 acc=43/50

size=random([0.01,0.03] *[0.02,0.05]) acc=45/50

size=random([0.01,0.03] *[0.02,0.05], [0.025,0.05]]) acc=185/200

acc=48/50

4. 取放逻辑解析

首先定位到主要代码文件：
robosuite/environments/manipulation/stack.py
为增加测试准确性，现将物体A和物体B的尺寸都改成长宽分别在[0.01, 0.03], [0.02 , 0.04]区间内的随机参数。

物体位姿存储

首先要明确场景中的物体，经代码测试发现，物体的姿态可从 _load_model 中设置，可以实现的功能有：倒入模型( xml )文件，设置长方体，圆柱、球体等基本集合图形的尺寸，位置，颜色，纹理等等

取放逻辑

def _setup_observables(self):
	@sensor(modality=modality)
	def cubeA_to_cubeB(obs_cache):
     	 return (
         	 obs_cache["cubeB_pos"] - obs_cache["cubeA_pos"]
         	if "cubeA_pos" in obs_cache and "cubeB_pos" in obs_cache
         	else np.zeros(3)
    	)
sensors = [cubeA_pos, cubeA_quat, cubeB_pos, cubeB_quat, gripper_to_cubeA, gripper_to_cubeB, cubeA_to_cubeB]

初步推测，cubeA_to_cubeB 即为生成取放逻辑的代码，为验证推测，将上述代码修改为：

# add new function
 @sensor(modality=modality)
 def cubeB_to_cubeA(obs_cache):
        return (
           obs_cache["cubeA_pos"] - obs_cache["cubeB_pos"]
           if "cubeB_pos" in obs_cache and "cubeA_pos" in obs_cache
           else np.zeros(3)
    )
 sensors = [cubeB_pos, cubeB_quat, cubeA_pos, cubeA_quat, gripper_to_cubeB, gripper_to_cubeA, cubeB_to_cubeA]

重点在 sensors 这行，除了需要将 cubeA_to_cubeB 修改为 cubeB_to_cubeA 外，还需要将前面对应的 pos, quat 顺序修改，测试效果如下：

cubeA_to_cubeB（将红色的放到绿色上面）

cubeB_to_cubeA（将绿色的放到红色上面）

存在问题：将取放逻辑修改后，出现 截止判断异常的情况，无法正常判断是否完成放置。

放置成功判断

在数据集中，每个 demo_? 的子数据中存在一个关键词为 dones 的数据，该数据为 0/1，且只有在最后几位是 1，其余全是 0，初步推测，此为训练时判断是否成功的依据，但在 train 的过程中，也调用的了任务类 Stack ，进行了 _check_success 的判断。
修改判断逻辑后，成功率显著提高（10%–>80%）：

cubeB_to_cubeA（将绿色的放到红色上面，修改判断条件后的）

修改后代码如下所示，可与官方提供的 staged_rewards函数作对比：

function: staged_rewards_test
def staged_rewards_test(self):
        cubeA_pos = self.sim.data.body_xpos[self.cubeA_body_id]
        cubeB_pos = self.sim.data.body_xpos[self.cubeB_body_id]
        gripper_site_pos = self.sim.data.site_xpos[self.robots[0].eef_site_id]
        
        dist = np.linalg.norm(gripper_site_pos - cubeB_pos) #
        r_reach = (1 - np.tanh(10.0 * dist)) * 0.25

        # grasping reward
        grasping_cubeB = self._check_grasp(gripper=self.robots[0].gripper, object_geoms=self.cubeB)
        if grasping_cubeB:
            r_reach += 0.25

        # lifting is successful when the cube is above the table top by a margin
        cubeB_height = cubeB_pos[2]
        table_height = self.table_offset[2]
        cubeB_lifted = cubeB_height > table_height + 0.04
        r_lift = 1.0 if cubeB_lifted else 0.0

        # Aligning is successful when cubeA is right above cubeB
        if cubeB_lifted:
            horiz_dist = np.linalg.norm(np.array(cubeB_pos[:2]) - np.array(cubeA_pos[:2]))
            r_lift += 0.5 * (1 - np.tanh(horiz_dist))

        # stacking is successful when the block is lifted and the gripper is not holding the object
        r_stack = 0
        cubeB_touching_cubeA = self.check_contact(self.cubeB, self.cubeA)
        if not grasping_cubeB and r_lift > 0 and cubeB_touching_cubeA:
            r_stack = 2.0

        return r_reach, r_lift, r_stack

增加障碍物

在现有场景中随机增加 n 个小球作为障碍物进行测试：

增加红色小球作为障碍物

结论：从视频效果分析，在进行抓取时，虽然有碰撞检测，但并未影响到抓取任务，初步怀疑可能是有些地方没有设置

后续探究方向

1：对于场景中存在障碍物的情况，是否可以实现避障操作，若可以，那么数据集与障碍物函数该怎么设计，若数据集中没有障碍物，但现实场景中有障碍物，该怎么处理；
2：给定夹具模型和物体模型，是否能自动实现抓取动作的判断与生成；
3：若 train 的物体与 test 物体的模型有细微区别，是否能自动判断夹具的抓取位置和抓取姿态，比如，把手在侧面的杯子和把手在顶部的杯子；
4：在取放逻辑相同，模型总体区别很大，但约束区域基本一致的物体，是否能实现取放流程的迁移，比如，冰箱门体与机器的装配，虽然外表千差万别，但铰链处是一样的；