rough estimation of pr2's arm reachability sphere

[HiroIshida]

center: [-0.02280524 -0.2155183 0.80869328]
radius: 0.9923580511760968

import tqdm
import numpy as np
from plainmp.robot_spec import PR2DualarmSpec
from skrobot.models.pr2 import PR2
from skrobot.model.primitives import PointCloudLink, Sphere
from skrobot.viewers import PyrenderViewer
from scipy.optimize import minimize


def bounding_sphere(points):
    def objective(x):
        return x[3]  # We want to minimize the radius

    def constraint_single_point(x, i):
        """ Distance from center to point i must be <= r. """
        cx, cy, cz, r = x
        px, py, pz = points[i]
        dist = np.sqrt((px - cx)**2 + (py - cy)**2 + (pz - cz)**2)
        return r - dist  # must be >= 0 for a feasible solution

    c0 = np.mean(points, axis=0)
    r0 = np.max(np.linalg.norm(points - c0, axis=1))
    x0 = np.concatenate([c0, [r0]])

    cons = [
        {'type': 'ineq', 'fun': lambda x, i=i: constraint_single_point(x, i)}
        for i in range(len(points))
    ]
    result = minimize(objective, x0, method='SLSQP', constraints=cons)

    center = result.x[:3]
    radius = result.x[3]
    return center, radius


spec = PR2DualarmSpec()
kin = spec.get_kin()
lb, ub = spec.angle_bounds()  # lower and upper bounds of joint angles
n_sample = 10000
qs = np.random.uniform(lb, ub, (n_sample, len(lb)))

joint_ids = kin.get_joint_ids(spec.control_joint_names)

r_pos_arr = np.zeros((n_sample, 3))
l_pos_arr = np.zeros((n_sample, 3))

for i, q in enumerate(qs):
    kin.set_joint_positions(joint_ids, q)
    r_tip = kin.debug_get_link_pose("r_gripper_tool_frame")
    l_tip = kin.debug_get_link_pose("l_gripper_tool_frame")
    r_pos_arr[i] = r_tip[:3]
    l_pos_arr[i] = l_tip[:3]

# fit a minimum bounding sphere (r_pos_arr)
center, radius = bounding_sphere(r_pos_arr)
print(center)
print(radius)

# visualize
pr2 = PR2()
plink = PointCloudLink(r_pos_arr)
sphere = Sphere(radius=radius)
sphere.translate(center)
viewer = PyrenderViewer()
viewer.add(pr2)
viewer.add(sphere)
viewer.add(plink)
viewer.show()
import time; time.sleep(1000)