[Question] Unable to Pick Up Objects in the Simulator

[yilin404]

I am facing an issue where the robotic arm in the simulator cannot pick up objects. The arm successfully moves to the target grasp position, closes the gripper, but the object remains stationary and is not lifted.
https://github.com/user-attachments/assets/159beff9-dd53-4e5b-ab5a-6d46b2e56d0e

The agent class is:

# ManiSkill
from mani_skill.agents.base_agent import BaseAgent, Keyframe
from mani_skill.agents.registration import register_agent
from mani_skill.agents.controllers import PDJointPosControllerConfig, PDJointPosMimicControllerConfig, \
                                          PDJointPosVelControllerConfig, \
                                          deepcopy_dict
from mani_skill.agents.utils import get_active_joint_indices
from mani_skill.utils import common, sapien_utils

import torch

# Typing
from mani_skill.utils.structs.actor import Actor

@register_agent()
class A1Arm(BaseAgent):
    uid = "a1arm"
    urdf_path = "./data/robot_description/a1arm_description/urdf/a1arm.urdf"
    urdf_config = dict(
        _materials=dict(
            gripper=dict(static_friction=2.0, dynamic_friction=2.0, restitution=0.1)
        ),
        link=dict(
            gripper1_axis=dict(
                material="gripper", patch_radius=0.1, min_patch_radius=0.1
            ),
            gripper2_axis=dict(
                material="gripper", patch_radius=0.1, min_patch_radius=0.1
            ),
        ),
    )

    fix_root_link = True
    load_multiple_collisions = True

    keyframes = dict()
    arm_joint_names = [
        "arm_joint1",
        "arm_joint2",
        "arm_joint3",
        "arm_joint4",
        "arm_joint5",
        "arm_joint6",
    ]
    gripper_joint_names = [
        "gripper1_axis", 
        "gripper2_axis"
    ]
    ee_link_name = "arm_seg6"

    arm_stiffness = 1e3
    arm_damping = 2e2
    arm_force_limit = 100.0

    gripper_stiffness = 1e3
    gripper_damping = 2e2
    gripper_force_limit = 100.0

    @property
    def _controller_configs(self):
        # -------------------------------------------------------------------------- #
        # Arm
        # -------------------------------------------------------------------------- #
        arm_pd_joint_pos = PDJointPosControllerConfig(
            self.arm_joint_names,
            lower=None,
            upper=None,
            stiffness=self.arm_stiffness,
            damping=self.arm_damping,
            force_limit=self.arm_force_limit,
            normalize_action=False,
        )
        arm_pd_joint_pos_vel = PDJointPosVelControllerConfig(
            self.arm_joint_names,
            lower=-0.1,
            upper=0.1,
            stiffness=self.arm_stiffness,
            damping=self.arm_damping,
            force_limit=self.arm_force_limit,
            normalize_action=False,
        )
        # -------------------------------------------------------------------------- #
        # Gripper
        # -------------------------------------------------------------------------- #
        # NOTE(jigu): IssacGym uses large P and D but with force limit
        # However, tune a good force limit to have a good mimic behavior
        gripper_pd_joint_pos = PDJointPosMimicControllerConfig(
            self.gripper_joint_names,
            lower=None,  # a trick to have force when the object is thin
            upper=None,
            stiffness=self.gripper_stiffness,
            damping=self.gripper_damping,
            force_limit=self.gripper_force_limit,
            normalize_action=False,
        )

        controller_configs = dict(
            pd_joint_pos=dict(arm=arm_pd_joint_pos, gripper=gripper_pd_joint_pos),
            pd_joint_pos_vel=dict(arm=arm_pd_joint_pos_vel, gripper=gripper_pd_joint_pos),
        )

        return deepcopy_dict(controller_configs)

    def _after_init(self):
        self.gripper1_link = sapien_utils.get_obj_by_name(
            self.robot.get_links(), "gripper1"
        )
        self.gripper2_link = sapien_utils.get_obj_by_name(
            self.robot.get_links(), "gripper2"
        )
        # Disable collision between the gripper
        for link in [self.gripper1_link, self.gripper2_link]:
            link.set_collision_group_bit(group=2, bit_idx=30, bit=1)

        self.arm_joint_indices = get_active_joint_indices(self.robot, self.arm_joint_names)
        self.gripper_joint_indices = get_active_joint_indices(self.robot, self.gripper_joint_names)

        self.ee_link = sapien_utils.get_obj_by_name(
            self.robot.get_links(), self.ee_link_name
        )
    
    def is_grasping(self, object: Actor, min_force: float = 0.5, max_angle: float = 85):
        """Check if the robot is grasping an object

        Args:
            object (Actor): The object to check if the robot is grasping
            min_force (float, optional): Minimum force before the robot is considered to be grasping the object in Newtons. Defaults to 0.5.
            max_angle (int, optional): Maximum angle of contact to consider grasping. Defaults to 85.
        """
        l_contact_forces = self.scene.get_pairwise_contact_forces(
            self.gripper1_link, object
        )
        r_contact_forces = self.scene.get_pairwise_contact_forces(
            self.gripper2_link, object
        )
        lforce = torch.linalg.norm(l_contact_forces, axis=1)
        rforce = torch.linalg.norm(r_contact_forces, axis=1)

        # direction to open the gripper
        ldirection = self.gripper1_link.pose.to_transformation_matrix()[..., :3, 1]
        rdirection = -self.gripper2_link.pose.to_transformation_matrix()[..., :3, 1]
        langle = common.compute_angle_between(ldirection, l_contact_forces)
        rangle = common.compute_angle_between(rdirection, r_contact_forces)
        lflag = torch.logical_and(
            lforce >= min_force, torch.rad2deg(langle) <= max_angle
        )
        rflag = torch.logical_and(
            rforce >= min_force, torch.rad2deg(rangle) <= max_angle
        )
        return torch.logical_and(lflag, rflag)

The urdf file is:
a1arm_description_.zip

The gripper property is:

I use such code to create actor:

# create obj model asset
def create_obj(
    scene: ManiSkillScene,
    scene_idxs: list[int],
    pose: Union[Pose, sapien.Pose],
    modelname: str,
    convex: bool = False, # 凸分解
    is_static: bool = False, # 静态物体
    model_id: int = 0, # 模型id
    z_axis_protect: bool = False, # 将物体平放在桌子表面
    z_axis_protect_value: float = 0.74, # 将物体平放在桌子表面
) -> Actor:
    file_name = get_obj_path(modelname, model_id)
    json_file_path = get_model_data_path(modelname, model_id)

    try:
        with open(json_file_path, 'r') as file:
            model_data = json.load(file)
        scale = model_data["scale"]
    except:
        model_data = None
    
    if z_axis_protect:
        if isinstance(pose, Pose):
            xy = pose.get_p()[..., :2]
            rot_matrix = quaternion_to_matrix(pose.get_q())
            N = xy.size(0)

            extents = torch.from_numpy(np.asarray(model_data["extents"])).unsqueeze(0).repeat([N, 1])
            scale = torch.from_numpy(np.asarray(model_data["scale"])).unsqueeze(0).repeat([N, 1])
            z = z_axis_protect_value +  (rot_matrix @ (extents * scale).t()).t()[..., [2]] / 2
            pose.set_p(torch.cat([xy, z], dim=-1))
        if isinstance(pose, sapien.Pose):
            xy = pose.get_p()[:2].tolist()
            z = z_axis_protect + (t3d.quaternions.quat2mat(pose.get_q()) @ (np.array(model_data["extents"]) * np.array(model_data["scale"])))[2] / 2
            pose.set_p([*xy, z])

    builder = scene.create_actor_builder()
    builder.set_scene_idxs(scene_idxs)
    if is_static:
        builder.set_physx_body_type("static")
    else:
        builder.set_physx_body_type("dynamic")
    if convex:
        builder.add_multiple_convex_collisions_from_file(filename=file_name, scale=scale)
    else:
        builder.add_nonconvex_collision_from_file(filename=file_name, scale=scale)
    builder.add_visual_from_file(filename=file_name, scale=scale)
    builder.set_initial_pose(pose)
    mesh = builder.build(name=modelname)
    mesh.set_pose(pose)

    return mesh, model_data

# create glb model asset
def create_glb(
    scene: ManiSkillScene,
    scene_idxs: list[int],
    pose: Union[Pose, sapien.Pose],
    modelname: str,
    convex: bool = False, # 凸分解
    is_static: bool = False, # 静摩擦 or 动摩擦
    model_id: int = 0, # 模型id
    z_axis_protect: bool = False, # 将物体平放在桌子表面
    z_axis_protect_value: float = 0.74, # 将物体平放在桌子表面
) -> tuple[Actor, dict]:
    file_name = get_glb_path(modelname, model_id)
    json_file_path = get_model_data_path(modelname, model_id)
    
    try:
        with open(json_file_path, 'r') as file:
            model_data = json.load(file)
        scale = model_data["scale"]
    except:
        model_data = None

    if z_axis_protect:
        if isinstance(pose, Pose):
            xy = pose.get_p()[..., :2]
            rot_matrix = quaternion_to_matrix(pose.get_q())
            N = xy.size(0)

            extents = torch.from_numpy(np.asarray(model_data["extents"])).unsqueeze(0).repeat([N, 1])
            scale = torch.from_numpy(np.asarray(model_data["scale"])).unsqueeze(0).repeat([N, 1])
            z = z_axis_protect_value +  (rot_matrix @ (extents * scale).t()).t()[..., [2]] / 2
            pose.set_p(torch.cat([xy, z], dim=-1))
        if isinstance(pose, sapien.Pose):
            xy = pose.get_p()[:2].tolist()
            z = z_axis_protect + (t3d.quaternions.quat2mat(pose.get_q()) @ (np.array(model_data["extents"]) * np.array(model_data["scale"])))[2] / 2
            pose.set_p([*xy, z])
    
    builder = scene.create_actor_builder()
    builder.set_scene_idxs(scene_idxs)
    if is_static:
        builder.set_physx_body_type("static")
    else:
        builder.set_physx_body_type("dynamic")
    if convex:
        builder.add_multiple_convex_collisions_from_file(filename=file_name, scale=scale)
    else:
        builder.add_nonconvex_collision_from_file(filename=file_name, scale=scale,)
    builder.add_visual_from_file(filename=file_name, scale=scale)
    builder.set_initial_pose(pose)
    mesh = builder.build(name=modelname)
    mesh.set_pose(pose)

    return mesh, model_data

def create_actor(
    scene: ManiSkillScene,
    scene_idxs: list[int],
    pose: Union[Pose, sapien.Pose],
    modelname: str,
    convex: bool = False, # 凸分解
    is_static: bool = False, # 静摩擦 or 动摩擦
    model_id: int = 0, # 模型id
    z_axis_protect: bool = False, # 将物体平放在桌子表面
    z_axis_protect_value: float = 0.74, # 将物体平放在桌子表面
) -> tuple[Actor, dict]:
    try:
        return create_glb(
            scene = scene,
            scene_idxs=scene_idxs,
            pose = pose,
            modelname = modelname,
            convex = convex,
            is_static = is_static,
            model_id = model_id,
            z_axis_protect=z_axis_protect,
            z_axis_protect_value=z_axis_protect_value,
        )
    except:
        try:
            return create_obj(
                scene = scene,
                scene_idxs=scene_idxs,
                pose = pose,
                modelname = modelname,
                convex = convex,
                is_static = is_static,
                model_id = model_id,
                z_axis_protect=z_axis_protect,
                z_val_protect_value = z_axis_protect_value,
            )
        except:
            print(modelname, 'is not exsist model file!')
            return None, None



self.container, self.container_data = create_actor(
            self.scene,
            [0],
            container_pose,
            "002_container",
            model_id=np.random.choice(list(range(10))),
            z_axis_protect=True,
        )
        self.container.set_mass(0.01)

The object file is:
7.zip

The object property is:

[yilin404]

I sincerely apologize for the mistake in the code I previously uploaded. I've corrected the error and updated the repository with the modified code.
Unfortunately, it seems that the problem still persists even after the fix.

The agent class is:

# ManiSkill
from mani_skill.agents.base_agent import BaseAgent, Keyframe
from mani_skill.agents.registration import register_agent
from mani_skill.agents.controllers import PDJointPosControllerConfig, PDJointPosMimicControllerConfig, \
                                          PDJointPosVelControllerConfig, \
                                          deepcopy_dict
from mani_skill.agents.utils import get_active_joint_indices
from mani_skill.utils import common, sapien_utils

import torch

# Typing
from mani_skill.utils.structs.actor import Actor

@register_agent()
class A1Arm(BaseAgent):
    uid = "a1arm"
    urdf_path = "./data/robot_description/a1arm_description/urdf/a1arm.urdf"
    urdf_config = dict(
        _materials=dict(
            gripper=dict(static_friction=2.0, dynamic_friction=2.0, restitution=0.1)
        ),
        link=dict(
            gripper1=dict( # ----- I fix this error -----
                material="gripper", patch_radius=0.1, min_patch_radius=0.1
            ),
            gripper2=dict( # ----- I fix this error -----
                material="gripper", patch_radius=0.1, min_patch_radius=0.1
            ),
        ),
    )

    fix_root_link = True
    load_multiple_collisions = True

    keyframes = dict()
    arm_joint_names = [
        "arm_joint1",
        "arm_joint2",
        "arm_joint3",
        "arm_joint4",
        "arm_joint5",
        "arm_joint6",
    ]
    gripper_joint_names = [
        "gripper1_axis", 
        "gripper2_axis"
    ]
    ee_link_name = "arm_seg6"

    arm_stiffness = 1e3
    arm_damping = 2e2
    arm_force_limit = 100.0

    gripper_stiffness = 1e3
    gripper_damping = 2e2
    gripper_force_limit = 100.0

    @property
    def _controller_configs(self):
        # -------------------------------------------------------------------------- #
        # Arm
        # -------------------------------------------------------------------------- #
        arm_pd_joint_pos = PDJointPosControllerConfig(
            self.arm_joint_names,
            lower=None,
            upper=None,
            stiffness=self.arm_stiffness,
            damping=self.arm_damping,
            force_limit=self.arm_force_limit,
            normalize_action=False,
        )
        arm_pd_joint_pos_vel = PDJointPosVelControllerConfig(
            self.arm_joint_names,
            lower=-0.1,
            upper=0.1,
            stiffness=self.arm_stiffness,
            damping=self.arm_damping,
            force_limit=self.arm_force_limit,
            normalize_action=False,
        )
        # -------------------------------------------------------------------------- #
        # Gripper
        # -------------------------------------------------------------------------- #
        # NOTE(jigu): IssacGym uses large P and D but with force limit
        # However, tune a good force limit to have a good mimic behavior
        gripper_pd_joint_pos = PDJointPosMimicControllerConfig(
            self.gripper_joint_names,
            lower=None,  # a trick to have force when the object is thin
            upper=None,
            stiffness=self.gripper_stiffness,
            damping=self.gripper_damping,
            force_limit=self.gripper_force_limit,
            normalize_action=False,
        )

        controller_configs = dict(
            pd_joint_pos=dict(arm=arm_pd_joint_pos, gripper=gripper_pd_joint_pos),
            pd_joint_pos_vel=dict(arm=arm_pd_joint_pos_vel, gripper=gripper_pd_joint_pos),
        )

        return deepcopy_dict(controller_configs)

    def _after_init(self):
        self.gripper1_link = sapien_utils.get_obj_by_name(
            self.robot.get_links(), "gripper1"
        )
        self.gripper2_link = sapien_utils.get_obj_by_name(
            self.robot.get_links(), "gripper2"
        )
        # Disable collision between the gripper
        for link in [self.gripper1_link, self.gripper2_link]:
            link.set_collision_group_bit(group=2, bit_idx=30, bit=1)

        self.arm_joint_indices = get_active_joint_indices(self.robot, self.arm_joint_names)
        self.gripper_joint_indices = get_active_joint_indices(self.robot, self.gripper_joint_names)

        self.ee_link = sapien_utils.get_obj_by_name(
            self.robot.get_links(), self.ee_link_name
        )
    
    def is_grasping(self, object: Actor, min_force: float = 0.5, max_angle: float = 85):
        """Check if the robot is grasping an object

        Args:
            object (Actor): The object to check if the robot is grasping
            min_force (float, optional): Minimum force before the robot is considered to be grasping the object in Newtons. Defaults to 0.5.
            max_angle (int, optional): Maximum angle of contact to consider grasping. Defaults to 85.
        """
        l_contact_forces = self.scene.get_pairwise_contact_forces(
            self.gripper1_link, object
        )
        r_contact_forces = self.scene.get_pairwise_contact_forces(
            self.gripper2_link, object
        )
        lforce = torch.linalg.norm(l_contact_forces, axis=1)
        rforce = torch.linalg.norm(r_contact_forces, axis=1)

        # direction to open the gripper
        ldirection = self.gripper1_link.pose.to_transformation_matrix()[..., :3, 1]
        rdirection = -self.gripper2_link.pose.to_transformation_matrix()[..., :3, 1]
        langle = common.compute_angle_between(ldirection, l_contact_forces)
        rangle = common.compute_angle_between(rdirection, r_contact_forces)
        lflag = torch.logical_and(
            lforce >= min_force, torch.rad2deg(langle) <= max_angle
        )
        rflag = torch.logical_and(
            rforce >= min_force, torch.rad2deg(rangle) <= max_angle
        )
        return torch.logical_and(lflag, rflag)

The robot property is:

[StoneT2000]

Can you share a video of what occurs?

And is the object you are picking up dynamic or static or kinematic?

[yilin404]

The video is:
https://github.com/user-attachments/assets/59143fef-a155-49ad-b78d-321d9a9b849c

And the object is dynamic i think, becaure i use such code to create it builder.set_physx_body_type("dynamic")

[StoneT2000]

I tried with a cube (density 1000, volume 0.04^3).

Looks fine to me, lifted up correctly. It is possible the object you are trying to pick up is too heavy. If you really want to pick up something that is heavy you may need to modify controller.

Note is that if you are using position control, you should try to set the gripper position to a smaller value (e.g. -0.01 is okay as well, we do this for panda), it should try and exert more force to get a closed grip on the object.

Another note is that the gripper's collision mesh is overly complicated. Try to simplify it to a flat box shape. It may also improve the grasping, I didn't look too carefully but the gripper mesh could be somewhat wrong.

[yilin404]

I’ve noticed another issue: in my code based on SAPIEN, I can easily grasp the object. Below are the parameter differences between the configurations in SAPIEN and ManiSkill:
The Gripper property in SAPIEN is:

The Gripper property in ManiSkill is:

The Object to Pick property in SAPIEN is:

The Object to Pick property in ManiSkill is:

I would like to ask which of these parameters might significantly affect the grasping performance? Are there any specific ones I should prioritize when tuning?
Additionally, I plan to migrate the code to the ManiSkill framework for further experiments. Could you provide guidance on how to modify these parameters within ManiSkill?
Your insights would be highly valuable for narrowing down the potential cause. Thank you!

[yilin404]

@StoneT2000