Could not avoid obstacle with pointcloud using update_world, or 'Start state is colliding with world'

[Bailey-24]

Thanks for your amazing code.
The functionality I'm trying to achieve is to key out the point cloud of the target object, convert the point clouds of other objects on the table to mesh, and use them as obstacles that the robotic arm has to avoid when grasping this target object.
But the problem is it can not avoid the obstacle.
I have no idea how to solve, please help me out.

class MotionPlanner(object):
        
    def __init__(self,
                 robot_file: str = "galbot_one_left_arm_config.yml",
                 world_file: str = "galbot_one_world_config_table.yml"):

        self.logger = logger
        n_obstacle_cuboids = 10
        n_obstacle_mesh = 2
        
        # load configuration files
        self.robot_config_file = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
        self.robot_cfg = RobotConfig.from_dict(self.robot_config_file)
        self.world_config_file = load_yaml(join_path(get_world_configs_path(), world_file))
        self.world_cfg = WorldConfig.from_dict(self.world_config_file)

        self.robot_file = robot_file
        self.world_file = world_file
        self.tensor_args = TensorDeviceType(device=torch.device("cuda:0"), dtype=torch.float32)
        self.joint_names = self.robot_cfg.kinematics.cspace.joint_names
        
        # config trajectory generation solver
        motion_gen_config = MotionGenConfig.load_from_robot_config(
            robot_file,
            world_file,
            self.tensor_args,
            num_graph_seeds = 8,
            trajopt_tsteps=32,
            collision_checker_type=CollisionCheckerType.MESH,
            use_cuda_graph=True,
            interpolation_dt = 0.0125,
            collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh},
            position_threshold = 0.008,
            rotation_threshold = 0.05,
            maximum_trajectory_dt = 0.1,
        )
        self.motion_gen = MotionGen(motion_gen_config) 
        self.motion_gen.warmup(enable_graph=True,
                               warmup_js_trajopt=True,
                               parallel_finetune=True
                               )
        self.motion_gen_plan_config = MotionGenPlanConfig(
            enable_graph = True,
            enable_opt = True,
            timeout = 10.0,
            use_nn_ik_seed = True,
            )

    def plan_obstacle(self, 
            start_joint_angles: List[float],
            goal_end_effector_pose: List[float] = None,
            goal_joint_angles: List[float] = None,
            constraint: List[float] = None,
            generate_trajectory_animation=False,
            pcd_background=None
            ):
        """Generate robot arm motion.

        Following modes are supported:
        1. start_joint_angles to goal_joint_angles mode
        2. start_joint_angles to goal_end_effector_pose mode

        Args:
            start_joint_angles:
                start joint angles in format of [q1, q2, q3, q4, q5, q6, q7,...]
            goal_end_effector_pose:
                goal end effector pose in format of [x, y, z, qx, qy, qz, qw]
            goal_joint_angles:
                goal joint angles in format of [q1, q2, q3, q4, q5, q6, q7,...]
            constraint:
                constraint hold_vec_weight for constraint in format of [roll, pitch, yaw, x, y, z] to goal frame.
            generate_trajectory_animation:
                If True, generates trajectory animation in .usd format,
                which can be viewed in Nvidia Omniverse Isaac Sim.
            pcd_background:
                point cloud background for collision checking.

        """
        
        pcd_bg = np.asarray(pcd_background.points)
        obstacle_pose = [0, 0, -3.5, 1, 0, 0, 0]
        obstacle_mesh = Mesh.from_pointcloud(pcd_bg, pose=obstacle_pose)
        obstacle_mesh.save_as_mesh("test_mesh.stl")
        world_obstacle = WorldConfig(
            mesh=[obstacle_mesh])
        self.motion_gen.update_world(world_obstacle) 

        # Start joint angles to goal end effector pose mode
        start_joint_angles is not None and goal_end_effector_pose is not None and goal_joint_angles is None:
            # Get start joint angles
            start_joint_angles = start_joint_angles
            start_joint_angles = JointState.from_position(position=self.tensor_args.to_device([start_joint_angles]),
                joint_names=self.joint_names[0:len(start_joint_angles)])
            # Get goal pose
            goal_pose = goal_end_effector_pose
            goal_pose = Pose(
                # [x,y,z]
                position=self.tensor_args.to_device(
                    [goal_pose[0], goal_pose[1], goal_pose[2]]),
                # [qx,qy,qz,qw] to [qw,qx,qy,qz]
                quaternion=self.tensor_args.to_device(
                    [goal_pose[6], goal_pose[3], goal_pose[4], goal_pose[5]]),
            )
            # Set constrained planning config
            if constraint is not None:
                self.set_constraint(constraint)
            # Plan single(cartesian space)
           
            motion_gen_result = self.motion_gen.plan_single(start_joint_angles, goal_pose, self.motion_gen_plan_config)
            solution_dict_to_return = {"success": motion_gen_result.success.item(),
                                       "joint_names": interpolated_solution.joint_names,
                                       "positions": interpolated_solution.position.cpu().squeeze().numpy().tolist(),
                                       "velocities": interpolated_solution.velocity.cpu().squeeze().numpy().tolist(),
                                       "accelerations": interpolated_solution.acceleration.cpu().squeeze().numpy().tolist(),
                                       "jerks": interpolated_solution.jerk.cpu().squeeze().numpy().tolist(),
                                       "interpolation_dt": motion_gen_result.interpolation_dt}
            return solution_dict_to_return

from this galbot_one_world_config_table is ( the table height is 0.45 meters )

cuboid:
  table:
    dims: [1.0, 2.0, 0.45] # x, y, z
    pose: [0.9, 0.0, 0.22, 1.0, 0.0, 0.0, 0.0] # x, y, z, qw, qx, qy, qz

and the start_joint_angles is [-0.8820, 1.8702, -0.6193, -1.9703, 1.5688, 1.0399, -1.3189], goal_end_effector_pose is [0.6046829274873973, 0.13720613159460218, 0.4853204518659768, 0.9441690084792496, -0.06547099091108394, 0.22824335211985328, -0.22839309313007716].
It successfully generate the motion trajctory, but it can not avoid the obstacle, how to solve the problem.

but when I set the obstacle_pose to [0, 0, -0.2, 1, 0, 0, 0] or [0, 0, 0.2, 1, 0, 0, 0] or more higher, the motion_gen_result.status is always is 'Start state is colliding with world'.

The video could not avoid is showing like this. I'm all about grasping that little yellow toy.

20240913-170611.mp4

The other scene is like this.

[balakumar-s]

Is it possible for you to render the robot spheres? You can use this api: https://curobo.org/_api/curobo.cuda_robot_model.cuda_robot_model.html#curobo.cuda_robot_model.cuda_robot_model.CudaRobotModel.get_robot_as_spheres

The issue could be that the spheres of the robot at your start configuration is in collision with the world.