How can I make the joint static even if it bumps into the ground?

Hi,

I have the UR5e that has a tool at the flange and it is connected with one prismatic and three perpendicular angular joints.

I want the joint not to move when the episode starts even if it bumps into the ground. When I check the joint values, the tool does not bump into the ground, the four joints’ poses are (almost) the same as the initial setting. However, when the joint hits the ground, the joint values are changed.

Here is an example figure and value.

robot_dof_pos:
tensor([[-5.2402e-01, -7.2713e-01,  1.0427e+00, -1.7659e+00, -1.5033e+00,
          1.6473e+00,  9.5430e-02,  1.2001e+00,  8.1442e-05, -7.2722e-02],
        [-6.0413e-01, -6.8301e-01,  1.1180e+00, -1.7992e+00, -1.3264e+00,
          1.5484e+00,  9.0777e-02,  1.2002e+00, -7.6110e-05, -2.2959e-01],
        [-6.7578e-01, -8.0045e-01,  1.1788e+00, -1.8395e+00, -1.5079e+00,
          1.5020e+00,  9.8078e-02,  1.2001e+00, -1.5564e-05, -1.0823e-01]],
       device='cuda:0')

The top image is the robot of the middle one and the robot_dof_pos is the corresponding value with the image. I initialized that the 7th joint equals 0.09 and the 8th joint equals 1.2 then, the rest 9th, and 10th are 0.

I think the initial value that I put on the methods works well because the initialize value keeps until the robots’ tool hits the ground.

Does it seem the below robot class that I use in the environment code is wrong? I input extremely high stiffness, damping, and max force value but it did not work.

class UR5eTool(Robot):
    def __init__(
        self,
        prim_path: str,
        name: Optional[str] = "ur5e_tool",
        usd_path: Optional[str] = None,
        translation: Optional[torch.tensor] = None,
        orientation: Optional[torch.tensor] = None,
        rgb_cam_prim_name: Optional[str] = None,    ### BSH
        depth_cam_prim_name: Optional[str] = None,  ### BSH
    ) -> None:
        """[summary]
        """

        self._usd_path = usd_path
        self._name = name

        self._position = torch.tensor([0, 0.0, 0.0]) if translation is None else translation
        # self._orientation = torch.tensor([0.0, 0.0, 0.0, 1.0]) if orientation is None else orientation
        self._orientation = torch.tensor([1.0, 0.0, 0.0, 0.0]) if orientation is None else orientation

        if self._usd_path is None:
            isaac_root_path = "/home/bak/.local/share/ov/pkg/isaac_sim-2022.2.1/"
            if isaac_root_path is None:
                carb.log_error("Could not find Isaac Sim assets folder")
            self._usd_path = isaac_root_path + "OmniIsaacGymEnvsAILAB/omniisaacgymenvs/skrl_examples/moving_target_point_cloud/ur5e/usd/ur5e_tool.usd"
            # self._usd_path = isaac_root_path + "OmniIsaacGymEnvsAILAB/omniisaacgymenvs/skrl_examples/moving_target_point_cloud/ur5e/usd/ur5e_tool_wo_rigidbody.usd"

        add_reference_to_stage(self._usd_path, prim_path)
        
        super().__init__(
            prim_path=prim_path,
            name=name,
            translation=self._position,
            orientation=self._orientation,
            articulation_controller=None,
        )

        dof_paths = [
            "base_link/shoulder_pan_joint",
            "shoulder_link/shoulder_lift_joint",
            "upper_arm_link/elbow_joint",
            "forearm_link/wrist_1_joint",
            "wrist_1_link/wrist_2_joint",
            "wrist_2_link/wrist_3_joint",
            "wrist_3_link/wrist_3_link_flange_prismatic",
            "flange_tool_rot_x/flange_revolute_x",
            "flange_tool_rot_z/flange_revolute_z",
            "flange_tool_rot_y/flange_revolute_y",
        ]

        drive_type = ["angular"] * 6 + ["linear"] + ["angular"] * 3
        default_dof_pos = [math.degrees(x) for x in [0.0, -1.75, 1.05, -1.57, -1.57, 1.57,
                                                     0.0, 0.0, 0.0, 0.0]]
        stiffness = [400*np.pi/180] * 6
        stiffness.extend([1000] * 4)
        damping = [80*np.pi/180] * 6
        damping.extend([10000] * 4)
        max_force = [87, 87, 87, 12, 12, 12,
                     100, 100, 100, 100]
        max_velocity = [math.degrees(x) for x in [2.175, 2.175, 2.175, 2.61, 2.61, 2.61,
                                                  2.61, 2.61, 2.61, 2.61]]


        for i, dof in enumerate(dof_paths):
            if i > 5: continue
            set_drive(
                prim_path=f"{self.prim_path}/{dof}",
                drive_type=drive_type[i],
                target_type="position",
                target_value=default_dof_pos[i],
                stiffness=stiffness[i],
                damping=damping[i],
                max_force=max_force[i]
            )

            PhysxSchema.PhysxJointAPI(get_prim_at_path(f"{self.prim_path}/{dof}")).CreateMaxJointVelocityAttr().Set(max_velocity[i])

I also attached my USD file.

usd.zip (3.4 MB)

Is there anyone who can solve the problem?

I am curious why you would need such a joint configuration between the last link and the tool? Why can’t you use a D6 joint with only two translational dofs?
Have you tried adding a fixed joint for the initial phase of your simulation and disable it later on? you can probably do the same thing with D6 joint as well, i.e. to disable all the dofs and only enable them once you need the joint to be active

Hi, @Milad-Rakhsha

Q1: why you would need such a joint configuration between the last link and the tool?
A1: I want to create a situation where the robot grasps a tool with an unstructured pose.

Q2: Why can’t you use a D6 joint with only two translational dofs?
A2: I tried to use a spherical joint at first but, I could not find any modules that make control the joints on the Isaac Sim codes.

Q3: Have you tried adding a fixed joint for the initial phase of your simulation and disable it later on?
A3: As I mentioned in A1, I want to create an unstructured grasping pose. I think I cannot make the same situation with a fixed joint. Isn’t it??

Hi, @Milad-Rakhsha

Is there a solution to making the joint static?