Add option for payload object bounding ellipsoid constraint

[nepfaff]

Not sure if its working... It doesn't really seem to help, at least. Sometimes, it does make big differences, and it seems to bring the two ellipsoids (bounding and inertia) closer to better.

The ellipsoids than be visualized by adding the following code into solve_inertial_param_sdp.py after solving the program.

import trimesh

                from pydrake.all import Ellipsoid, Rgba

                M_BBo_B = SpatialInertia(
                    mass=payload_mass,
                    p_PScm_E=payload_com,
                    G_SP_E=UnitInertia(
                        Ixx=payload_rot_inertia[0, 0] / payload_mass,
                        Iyy=payload_rot_inertia[1, 1] / payload_mass,
                        Izz=payload_rot_inertia[2, 2] / payload_mass,
                        Ixy=payload_rot_inertia[0, 1] / payload_mass,
                        Ixz=payload_rot_inertia[0, 2] / payload_mass,
                        Iyz=payload_rot_inertia[1, 2] / payload_mass,
                    ),
                )
                (
                    radii,
                    X_BE,
                ) = M_BBo_B.CalcPrincipalSemiDiametersAndPoseForSolidEllipsoid()
                new_radii = np.clip(radii, a_min=1e-2 * radii.max(), a_max=None)
                density = 1000.0
                unit_inertia_ellipsoid_mass = (
                    density * 4.0 / 3.0 * np.pi * np.prod(new_radii)
                )
                volume_scale = payload_mass / unit_inertia_ellipsoid_mass
                abc = new_radii * np.cbrt(volume_scale)
                ellipsoid = Ellipsoid(abc)
               
                last_link = arm_plant_components_gt.plant.GetBodyByName(f"iiwa_link_7")
                X_WB = arm_plant_components_gt.plant.EvalBodyPoseInWorld(
                    arm_plant_components_gt.plant_context, last_link
                )
                ellipsoid = trimesh.creation.icosphere(subdivisions=3)
                ellipsoid.apply_scale(radii)  # Scale to the correct radii
                ellipsoid.visual.face_colors = [
                    255,
                    0,
                    0,
                    100,
                ]  # Red with 50% transparency

                center = payload_bounding_ellipsoid.center().flatten()
                B = payload_bounding_ellipsoid.B()
                radii = np.linalg.norm(B, axis=0)
                axes = B / radii
                ellipsoid1 = trimesh.creation.icosphere(subdivisions=3)
                ellipsoid1.apply_scale(radii)
                transform = np.eye(4)
                transform[:3, :3] = axes
                transform[:3, 3] = center
                ellipsoid1.apply_transform(transform)
                # Set ellipsoid transparency and color
                ellipsoid1.visual.face_colors = [
                    0,
                    0,
                    255,
                    100,
                ]  # Blue with transparency

                scene = trimesh.Scene([ellipsoid, ellipsoid1])
                scene.export("ellipsoid_scene.obj")