Robot Self-Collision

[harryzhangOG]

Hi, we are using the OSC control interface to control the robot arm to reach a target point and rpy pose. However, the twist motion generated results in self-collision. Is self-collision prevention not enforced in the current OSC interface? Thanks.

Here is the code to reproduce:

        rot = R.from_quat(env.agent.get_pose().q).as_matrix()
        hand_xyz = env.agent.get_pose().p
        T_world_robot = np.vstack([np.hstack([rot, hand_xyz.reshape(3, 1)]), np.array([[0, 0, 0, 1]])])
        T_robot_world = np.linalg.inv(T_world_robot)
        T_world_goal = np.vstack([np.hstack([goal_rpy_mat, goal_xyz.reshape(3, 1)]), np.array([[0, 0, 0, 1]])])
        T_robot_goal = T_robot_world@T_world_goal

        from pytransform3d import transformations
        twist_mat = transformations.transform_log_from_transform(T_robot_goal)

        deltaV = twist_mat[:3, -1]
        omg_wedge = twist_mat[:3, :3]
        deltaOmg = np.array([omg_wedge[-1, 1], -omg_wedge[-1, 0], omg_wedge[1, 0]])

        hand_forward = np.zeros(osc_interface.osc_dim)
        hand_forward[6:9] = deltaV
        hand_forward[9:12] = deltaOmg
        print(hand_forward)
        keep_open = osc_interface.operational_space_and_null_space_to_joint_space(
                            qpos, hand_forward, action[:osc_interface.null_space_dim])
        action = keep_open

[tongzhoumu]

Hi Harry,

The OSC interface is used to convert EE's 6D velocity to joint space velocity, so it is not designed to prevent self-collision. For example, if you control the EE to hit the robot arm/body, the EE will just go ahead to hit. Preventing self-collision should be handled by users.

[harryzhangOG]

Thanks. Would the srdf of the robot be provided? Thanks. https://gramaziokohler.github.io/compas_fab/latest/examples/03_backends_ros/08_ros_create_moveit_package_from_custom_urdf.html

[tongzhoumu]

Hi Harry,

We provided the URDF files.

• Single-arm robot
• Dual-arm robot

[fbxiang]

What srdf properties are you looking for? You can certainly use MoveIt to generate the srdf if it works. I believe disabling collision for neighbor links would be enough for motion planning. We also recommend using https://sapien.ucsd.edu/docs/latest/tutorial/motion_planning/getting_started.html for motion planning.

[harryzhangOG]

Hi Fanbo. I was using the mplib library from sapien but found out that an srdf file is required (when setting up the planner, mplib gave a not valid urdf error.)

[fbxiang]

I have talked to mplib authors and posted an issue haosulab/MPlib#2. They should implement this feature soon. While it is being fixed, I believe adding effort tags to the limits and manually creating SRDF to ignore collisions between neighboring links would make mplib to work.

[fbxiang]

See the MPlib issue above. MPlib should be able to parse URDF files and generate SRDF files now.