How to do accurate PDBasePosControl?

[Jaraxxus-Me]

Hi, I am following the official mani_skill/envs/tasks/mobile_manipulation/open_cabinet_drawer.py and mani_skill/agents/robots/fetch/fetch.py for mobile manipulation and the velocity control works very well.

Now I hope to do something similar to arm_joint_delta_pos control for the base, i.e., I hope the input action is the dx,dy,dtheta, instead of their velocities.

I tried to implement a controller like this:

class PDBasePosController(PDJointPosController):
    """PDJointVelController for ego-centric base movement."""

    def _initialize_action_space(self):
        # At least support xy-plane translation and z-axis rotation
        assert len(self.joints) >= 3, len(self.joints)
        super()._initialize_action_space()

    def set_action(self, action: Array):
        action = self._preprocess_action(action)
        # Convert to ego-centric action
        # Assume the 3rd DoF stands for orientation
        self._step = 0
        self._start_qpos = self.qpos
        if self.config.use_delta:
            if self.config.use_target:
                self._target_qpos = self._target_qpos + action
            else:
                self._target_qpos = self._start_qpos + action
        else:
            # Compatible with mimic controllers. Need to clone here otherwise cannot do in-place replacements in the reset function
            self._target_qpos = torch.broadcast_to(
                action, self._start_qpos.shape
            ).clone()
        if self.config.interpolate:
            self._step_size = (self._target_qpos - self._start_qpos) / self._sim_steps
        else:
            self.set_drive_targets(self._target_qpos)


class PDBasePosControllerConfig(PDJointPosControllerConfig):
    controller_cls = PDBasePosController

And use it like this:

        # -------------------------------------------------------------------------- #
        # Base
        # -------------------------------------------------------------------------- #
        base_pd_joint_delta_pos = PDBasePosControllerConfig(
            self.base_joint_names,
            lower=-CFG.body_joint_delta,
            upper=CFG.body_joint_delta,
            stiffness=1000,
            damping=100,
            force_limit=500,
            use_delta=True,
            normalize_action=CFG.normalize_action,
        )

It seems that the robot moves, but there exist a very large position error, where I used 0.01 as dx action per step, but the actual x after one step simulation is very small (see video below for 50 steps). How can I get accurate position control for base, like the arm?

slow_moving.mp4

Thanks a lot for any guidance.
Some work-arounds I tried:

• URDF dynamics is like this:

  <link name="root">
    <inertial>
      <mass value="0"/>
      <inertia ixx="0.00" ixy="0.00" ixz="0.00" iyy="0.00" iyz="0.00" izz="0.00"/>
    </inertial>
  </link>
  <link name="root_arm_1_link_1">
    <inertial>
      <mass value="0"/>
      <inertia ixx="0.00" ixy="0.00" ixz="0.00" iyy="0.00" iyz="0.00" izz="0.00"/>
    </inertial>
  </link>
  <link name="root_arm_1_link_2">
    <inertial>
      <mass value="0"/>
      <inertia ixx="0.00" ixy="0.00" ixz="0.00" iyy="0.00" iyz="0.00" izz="0.00"/>
    </inertial>
  </link>
  <joint name="root_x_axis_joint" type="prismatic">
    <parent link="root"/>
    <child link="root_arm_1_link_1"/>
    <axis xyz="1 0 0"/>
    <limit effort="10000" lower="-10" upper="10" velocity="10000"/>
    <dynamics damping="10.0" friction="10"/>
  </joint>
  <joint name="root_y_axis_joint" type="prismatic">
    <parent link="root_arm_1_link_1"/>
    <child link="root_arm_1_link_2"/>
    <axis xyz="0 1 0"/>
    <limit effort="1000" lower="-10" upper="10" velocity="10000"/>
    <dynamics damping="10.0" friction="10"/>
  </joint>
  <joint name="root_z_rotation_joint" type="continuous">
    <parent link="root_arm_1_link_2"/>
    <child link="body"/>
    <axis xyz="0 0 1"/>
    <dynamics damping="10.0" friction="1"/>
  </joint>

• I also tried to set the mass in the URDF to a reasonably small value.

[StoneT2000]

What is in CFG? I'm guessing one reason is with normalization sending an action of 0.01 might correspond to something different.