def draw_circle(center, radius, n=24, **kwargs):
vertices = []
for i in range(n):
theta = i * 2 * math.pi / n
unit = np.append(unit_from_theta(theta), [0])
vertices.append(center + radius * unit)
return add_segments(vertices, closed=True, **kwargs)
def sample_reachable_base(robot, point, reachable_range=(0.25, 1.0)):
from pybullet_planning.interfaces.env_manager.pose_transformation import unit_from_theta, point_from_pose
radius = np.random.uniform(*reachable_range)
x, y = radius * unit_from_theta(np.random.uniform(-np.pi,
np.pi)) + point[:2]
yaw = np.random.uniform(*CIRCULAR_LIMITS)
base_values = (x, y, yaw)
#set_base_values(robot, base_values)
return base_values
def grow_polygon(vertices, radius, n=8):
from pybullet_planning.interfaces.env_manager.pose_transformation import unit_from_theta
vertices2d = [vertex[:2] for vertex in vertices]
if not vertices2d:
return []
points = []
for vertex in convex_hull(vertices2d).vertices:
points.append(vertex)
if 0 < radius:
for theta in np.linspace(0, 2 * PI, num=n, endpoint=False):
points.append(vertex + radius * unit_from_theta(theta))
return convex_hull(points).vertices