import numpy as np
from pybullet_industrial.interpolation import linear_interpolation, circular_interpolation
[docs]
def build_box_path(center_position: np.array, dimensions: np.array,
radius: float, orientation: np.array, samples: int):
"""Build a box shaped path with rounded corners
Args:
center_position (np.array): The 3D center position of the box
dimensions (np.array): the length and width of the box
radius (float): the radius of the corners
orientation (np.array): A quaternion describing the orientation of the box
samples (int): The number of points in the box path
Returns:
ToolPath: The resulting Toolpath
"""
corner_point_00 = np.array(
[-0.5*dimensions[0], -0.5*dimensions[1]+radius, 0])
corner_point_01 = np.array(
[-0.5*dimensions[0], 0.5*dimensions[1]-radius, 0])
corner_point_10 = np.array(
[-0.5*dimensions[0]+radius, 0.5*dimensions[1], 0])
corner_point_11 = np.array(
[0.5*dimensions[0]-radius, 0.5*dimensions[1], 0])
corner_point_20 = np.array(
[0.5*dimensions[0], 0.5*dimensions[1]-radius, 0])
corner_point_21 = np.array(
[0.5*dimensions[0], -0.5*dimensions[1]+radius, 0])
corner_point_30 = np.array(
[0.5*dimensions[0]-radius, -0.5*dimensions[1], 0])
corner_point_31 = np.array(
[-0.5*dimensions[0]+radius, -0.5*dimensions[1], 0])
linear_length = (np.linalg.norm(corner_point_00-corner_point_01) +
np.linalg.norm(corner_point_10-corner_point_11) +
np.linalg.norm(corner_point_20-corner_point_21) +
np.linalg.norm(corner_point_30-corner_point_31))
circle_length = 2*np.pi*radius
linear_samples = int(0.25*linear_length /
(linear_length+circle_length)*samples)
circle_samples = int(0.25*circle_length /
(linear_length+circle_length)*samples)
box_path = linear_interpolation(
corner_point_00, corner_point_01, linear_samples)
side_1 = linear_interpolation(
corner_point_10, corner_point_11, linear_samples)
side_2 = linear_interpolation(
corner_point_20, corner_point_21, linear_samples)
side_3 = linear_interpolation(
corner_point_30, corner_point_31, linear_samples)
corner_0 = circular_interpolation(
corner_point_01, corner_point_10, radius, circle_samples)
corner_1 = circular_interpolation(
corner_point_11, corner_point_20, radius, circle_samples)
corner_2 = circular_interpolation(
corner_point_21, corner_point_30, radius, circle_samples)
corner_3 = circular_interpolation(
corner_point_31, corner_point_00, radius, circle_samples)
box_path.append(corner_0)
box_path.append(side_1)
box_path.append(corner_1)
box_path.append(side_2)
box_path.append(corner_2)
box_path.append(side_3)
box_path.append(corner_3)
box_path.rotate(orientation)
box_path.translate(center_position)
return box_path