硬件配置

两台松灵Piper机械臂，Realsense L515
iDP3 项目链接

复现思路

1. 用遥操作构建数据集
2. 训练模型
3. 将模型部署到机械臂上

本人为imitation learning初学者，如有不对的地方请多指教

数据集收集

刚开始通过用键鼠操作机械臂去收集数据集。本来想着用moveit 逆解去操纵机械臂采集数据,但是实际控制起来由于worksapce的原因难以做到连续控制，所以采用了键盘控制关节角度采集。
后来为了速度和连贯性开始使用机械臂自带的主从模式去收集数据集（还是这样方便）

1. 调用了repo中的MultiRealSense class去获得点云信息
2. 用piper sdk 得到机械臂的 joint pose 和 夹爪角度（稍微修改了一下sdk）
3. 使用h5py保存为h5文件
4. 调用teleoperation repo中的convert_demos.py转换为用来训练的zarray格式

模型训练

添加了新的task yaml 文件, 主要改动了agent_pos适配piper机械臂

shape_meta: &shape_meta
  # acceptable types: rgb, low_dim
  obs:
    point_cloud:
      shape: [4096, 6]
      type: point_cloud
    agent_pos:
      shape: [7]
      type: low_dim
  action:
    shape: [7]

其他训练参数稍微进行了改动 降低了lr和lr_setup等
然后就开始训练了

deploy程序

首先是对仓库中deploy.py代码的分析

第一段是初始化，包括相机，设置观测/动作 horizon还有建立和机器人，灵巧手和IK求解器的通讯。
注意：原文使用的是Fourier GR1机器人和因时的灵巧手，论文作者的另一个repo里有和这个deploy.py 适配的控制程序和teleoperation。

class GR1DexEnvInference:
    """
    The deployment is running on the local computer of the robot.
    """
    def __init__(self, obs_horizon=2, action_horizon=8, device="gpu",
                use_point_cloud=True, use_image=True, img_size=224,
                 num_points=4096,
                 use_waist=False):
        
        # obs/action
        self.use_point_cloud = use_point_cloud
        self.use_image = use_image
        
        self.use_waist = use_waist
        
        # camera
        self.camera = MultiRealSense(use_front_cam=True, # by default we use single cam. but we also support multi-cam
                            front_num_points=num_points,
                            img_size=img_size)

        # horizon
        self.obs_horizon = obs_horizon
        self.action_horizon = action_horizon

        # inference device
        if device == "gpu":
            self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
        else:
            self.device = torch.device("cpu")
        
        # robot comm
        self.upbody_comm = UpperBodyCommunication()
        self.hand_comm = HandCommunication()
        self.arm_solver = ArmRetarget("AVP")

下面是执行函数

    def step(self, action_list):
        
        for action_id in range(self.action_horizon):
            act = action_list[action_id]
            self.action_array.append(act)
            #模型输出25维转换为控制需要的32维
            act = action_util.joint25_to_joint32(act)
            
            filtered_act = act.copy()
            
            #区分控制手和身体的动作
            filtered_pos = filtered_act[:-12]
            filtered_handpos = filtered_act[-12:]
            if not self.use_waist:
                filtered_pos[0:6] = 0. #锁住腰
            
            #发送命令给手和上半身
            self.upbody_comm.set_pos(filtered_pos)
            self.hand_comm.send_hand_cmd(filtered_handpos[6:], filtered_handpos[:6])
            
            #采集新的相机数据
            cam_dict = self.camera()
            self.cloud_array.append(cam_dict['point_cloud'])
            self.color_array.append(cam_dict['color'])
            self.depth_array.append(cam_dict['depth'])
            
            #获得手部关节信息
            try:
                hand_qpos = self.hand_comm.get_qpos()
            except:
                cprint("fail to fetch hand qpos. use default.", "red")
                hand_qpos = np.ones(12)
            env_qpos = np.concatenate([self.upbody_comm.get_pos(), hand_qpos])
            self.env_qpos_array.append(env_qpos)
            
        
        agent_pos = np.stack(self.env_qpos_array[-self.obs_horizon:], axis=0)
    
        obs_cloud = np.stack(self.cloud_array[-self.obs_horizon:], axis=0)
        obs_img = np.stack(self.color_array[-self.obs_horizon:], axis=0)
        #储存agent关节信息，相机和点云数据到字典里
        obs_dict = {
            'agent_pos': torch.from_numpy(agent_pos).unsqueeze(0).to(self.device),
        }
        if self.use_point_cloud:
            obs_dict['point_cloud'] = torch.from_numpy(obs_cloud).unsqueeze(0).to(self.device)
        if self.use_image:
            obs_dict['image'] = torch.from_numpy(obs_img).permute(0, 3, 1, 2).unsqueeze(0)

        return obs_dict

然后是reset函数，不做详细解释，用途是清空array和初始化各种数据和机器人姿态

def main(cfg: OmegaConf):
    torch.manual_seed(42)
    # resolve immediately so all the ${now:} resolvers
    # will use the same time.
    OmegaConf.resolve(cfg)
    cls = hydra.utils.get_class(cfg._target_)
    workspace: BaseWorkspace = cls(cfg) # 使用hydra库一键配置模型

    if workspace.__class__.__name__ == 'DPWorkspace': #选择使用2D版（image）还是3D版（点云）
        use_image = True
        use_point_cloud = False
    else:
        use_image = False
        use_point_cloud = True
        
    # fetch policy model
    policy = workspace.get_model()
    action_horizon = policy.horizon - policy.n_obs_steps + 1 #预测长度 总长度-观测长度+1

    # pour 定义不同任务步长
    roll_out_length_dict = {
        "pour": 300,
        "grasp": 1000,
        "wipe": 300,
    }
    # task = "wipe"
    task = "grasp"
    # task = "pour"
    roll_out_length = roll_out_length_dict[task]
    
    img_size = 224
    num_points = 4096
    use_waist = True
    first_init = True
    record_data = True

	#初始化 class
    env = GR1DexEnvInference(obs_horizon=2, action_horizon=action_horizon, device="cpu",
                             use_point_cloud=use_point_cloud,
                             use_image=use_image,
                             img_size=img_size,
                             num_points=num_points,
                             use_waist=use_waist)

    
    obs_dict = env.reset(first_init=first_init)

    step_count = 0
    #开始循环
    while step_count < roll_out_length:
        with torch.no_grad():
            action = policy(obs_dict)[0]
            action_list = [act.numpy() for act in action]
        
        obs_dict = env.step(action_list)
        step_count += action_horizon
        print(f"step: {step_count}")
	#记录数据
    if record_data:
        import h5py
        root_dir = "/home/gr1p24ap0049/projects/gr1-learning-real/"
        save_dir = root_dir + "deploy_dir"
        os.makedirs(save_dir, exist_ok=True)
        
        record_file_name = f"{save_dir}/demo.h5"
        color_array = np.array(env.color_array)
        depth_array = np.array(env.depth_array)
        cloud_array = np.array(env.cloud_array)
        qpos_array = np.array(env.qpos_array)
        with h5py.File(record_file_name, "w") as f:
            f.create_dataset("color", data=np.array(color_array))
            f.create_dataset("depth", data=np.array(depth_array))
            f.create_dataset("cloud", data=np.array(cloud_array))
            f.create_dataset("qpos", data=np.array(qpos_array))
        
        choice = input("whether to rename: y/n")
        if choice == "y":
            renamed = input("file rename:")
            os.rename(src=record_file_name, dst=record_file_name.replace("demo.h5", renamed+'.h5'))
            new_name = record_file_name.replace("demo.h5", renamed+'.h5')
            cprint(f"save data at step: {roll_out_length} in {new_name}", "yellow")
        else:
            cprint(f"save data at step: {roll_out_length} in {record_file_name}", "yellow")

至此deploy.py分析完毕，我们要做的是

1. 改变控制函数去适配piper臂
2. 更改机械臂初始化配置

这里遇到了一个问题也是之前项目存在过的一个问题是：piper和相机的控制程序基于ros2，而ros2又与conda环境不兼容，之前的解决方案是用TCP socket传输数据，但是在这个项目中因为是deploy的代码，环境比较简单我就直接装到主环境里面了。在寻找的过程中我还发现可以用robostack把ros2装到conda里去管理环境，最后尝试也能成功编译和运行。
二编：鉴于各种原因，最后并没有用ros。。。使用了piper python sdk去适配这个程序 发现省时省力很多 ：））总体来说部署还算简单

结果

第一次只采集了单臂的数据进行训练, 采集了6组，部署之后的问题是机械臂老是空夹空放，不能精准定位物体位置和下降到足够的高度去夹取魔方，最重要的原因我们数据集样本太少，因为论文中是用了10组 x 10个rollout 虽然有点没搞懂他rollout是怎么做的。

第二次采集了50组数据进行训练，机械臂能做到夹取和放置 但是夹爪夹取时候的力度不太对 还有对物体位置敏感度低，往往不能正确找到物体。检查完数据集之后发现物体的点云比较少而且模糊，相机摆放位置有点问题。

第三次同样50组 调整了相机位置以更清晰的捕捉点云。能成功夹取，但是成功率并不是很高。

整个复现过程断断续续做了一个星期，感觉效果并没有达到预期。