def test_box1(self):
box = hppfcl.Box(np.matrix([1., 2., 3.]).T)
self.assertIsInstance(box, hppfcl.Box)
self.assertIsInstance(box, hppfcl.ShapeBase)
self.assertIsInstance(box, hppfcl.CollisionGeometry)
self.assertEqual(box.getNodeType(), hppfcl.NODE_TYPE.GEOM_BOX)
self.assertTrue(
np.array_equal(box.halfSide,
np.matrix([.5, 1., 1.5]).T))
box.halfSide = np.matrix([4., 5., 6.]).T
self.assertTrue(np.array_equal(box.halfSide,
np.matrix([4., 5., 6.]).T))
com = box.computeCOM()
self.assertApprox(com, np.zeros(3))
V = box.computeVolume()
x = float(2 * box.halfSide[0])
y = float(2 * box.halfSide[1])
z = float(2 * box.halfSide[2])
V_ref = x * y * z
self.assertApprox(V, V_ref)
I0 = box.computeMomentofInertia()
Ix = V_ref * (y * y + z * z) / 12.
Iy = V_ref * (x * x + z * z) / 12.
Iz = V_ref * (y * y + x * x) / 12.
I0_ref = np.diag([Ix, Iy, Iz])
self.assertApprox(I0, I0_ref)
Ic = box.computeMomentofInertiaRelatedToCOM()
self.assertApprox(Ic, I0_ref)
def get_bounds_geom_objs(pos_bounds):
"""
Generate 6 faces corresponding to the agent deplacement bounds
"""
size = pos_bounds[1] - pos_bounds[0]
center = np.mean(pos_bounds, axis=0)
thickness = 0.05
color = (1, 1, 1, 0.3)
geom_objs = []
aas = [
eigenpy.AngleAxis(0, np.array([1, 0, 0])),
eigenpy.AngleAxis(np.pi / 2, np.array([0, 1, 0])),
eigenpy.AngleAxis(np.pi / 2, np.array([0, 0, 1])),
]
placement = pin.SE3.Identity()
for i, angle_axis in enumerate(aas):
placement.rotation = angle_axis.matrix()
size_bound = size.copy()
translation = np.zeros(3)
size_bound[i] = thickness
translation[i] = -size[i] / 2 - thickness / 2.1
for j in range(2):
geom = hppfcl.Box(*size_bound)
placement.translation = translation
mesh = Mesh(
name=f"bound{i}{j}",
geometry=geom,
placement=placement,
color=color,
)
geom_objs.append(mesh.geom_obj())
translation *= -1
return geom_objs
def extract_obstacles(condition, num_obstacles):
w_dim = 3
dw = 0.1
gap1 = condition[0:w_dim]
gap2 = condition[w_dim:2 * w_dim]
gap3 = condition[2 * w_dim:3 * w_dim]
num_obstacles = 5
obst1 = [0, gap1[1] - dw, -0.5, gap1[0], gap1[1], 1.5]
obst2 = [gap2[0] - dw, 0, -0.5, gap2[0], gap2[1], 1.5]
obst3 = [gap2[0] - dw, gap2[1] + dw, -0.5, gap2[0], 1, 1.5]
obst4 = [gap1[0] + dw, gap1[1] - dw, -0.5, gap3[0], gap1[1], 1.5]
obst5 = [gap3[0] + dw, gap1[1] - dw, -0.5, 1, gap1[1], 1.5]
obstacles = [obst1, obst2, obst3, obst4, obst5]
obstacles = obstacles[:num_obstacles]
for i, obst in enumerate(obstacles):
x0, y0, x1, y1 = obst[0], obst[1], obst[3], obst[4]
box_size = [x1 - x0, y1 - y0, 0.1]
pos = [(x0 + x1) / 2, (y0 + y1) / 2, 0]
placement = pin.SE3(np.eye(3), np.array(pos))
mesh = Mesh(
name=f"obstacle{i}",
geometry=hppfcl.Box(*box_size),
placement=placement,
color=(0, 0, 1, 0.8),
)
obstacles[i] = mesh.geom_obj()
return obstacles
def sample_geom(np_random, obst_type, size, index=None):
geom = None
path = ""
scale = np.ones(3)
if obst_type != "boxes":
size /= 2
if obst_type == "boxes":
geom = hppfcl.Box(*size)
elif obst_type == "shapes":
j = np.random.randint(3)
if j == 0:
r = 1.3 * size[0]
geom = hppfcl.Sphere(r)
elif j == 1:
geom = hppfcl.Cylinder(size[0] / 1.5, size[1] * 3)
elif j == 2:
# geom = hppfcl.Cone(size[0] * 1.5, 3 * size[1])
geom = hppfcl.Capsule(size[0], 2 * size[1])
elif obst_type == "ycb":
dataset_path = "YCB_PATH"
files = os.listdir(dataset_path)
idx = np.random.randint(len(files))
path = os.path.join(dataset_path, files[idx])
scale = 3.5 * np.ones(3)
return geom, path, scale
def test_box2(self):
box = hppfcl.Box(1., 2., 3)
self.assertIsInstance(box, hppfcl.Box)
self.assertIsInstance(box, hppfcl.ShapeBase)
self.assertIsInstance(box, hppfcl.CollisionGeometry)
self.assertEqual(box.getNodeType(), hppfcl.NODE_TYPE.GEOM_BOX)
self.assertEqual(box.halfSide[0], 0.5)
self.assertEqual(box.halfSide[1], 1.0)
self.assertEqual(box.halfSide[2], 1.5)
def extract_obstacles(maze, thickness):
scx = 1 / maze.nx
scy = 1 / maze.ny
obstacles_coord = []
for x in range(maze.nx):
obstacles_coord.append((x / maze.nx, 0, (x + 1) / maze.nx, 0))
for y in range(maze.ny):
obstacles_coord.append((0, y / maze.ny, 0, (y + 1) / maze.ny))
# Draw the "South" and "East" walls of each cell, if present (these
# are the "North" and "West" walls of a neighbouring cell in
# general, of course).
for x in range(maze.nx):
for y in range(maze.ny):
if maze.cell_at(x, y).walls["S"]:
x1, y1, x2, y2 = (
x * scx,
(y + 1) * scy,
(x + 1) * scx,
(y + 1) * scy,
)
obstacles_coord.append((x1, y1, x2, y2))
if maze.cell_at(x, y).walls["E"]:
x1, y1, x2, y2 = (
(x + 1) * scx,
y * scy,
(x + 1) * scx,
(y + 1) * scy,
)
obstacles_coord.append((x1, y1, x2, y2))
obstacles = []
for i, obst_coord in enumerate(obstacles_coord):
x1, y1, x2, y2 = obst_coord[0], obst_coord[1], obst_coord[
2], obst_coord[3]
x1 -= thickness / 2
x2 += thickness / 2
y1 -= thickness / 2
y2 += thickness / 2
box_size = [x2 - x1, y2 - y1, 0.1]
pos = [(x1 + x2) / 2, (y1 + y2) / 2, 0]
placement = pin.SE3(np.eye(3), np.array(pos))
mesh = Mesh(
name=f"obstacle{i}",
geometry=hppfcl.Box(*box_size),
placement=placement,
color=(0, 0, 1, 0.8),
)
obstacles.append(mesh.geom_obj())
return obstacles
def dict_to_geom(props):
geom_name = props["name"]
if geom_name == "capsule":
geom = hppfcl.Capsule(props["radius"], props["halfLength"])
elif geom_name == "cylinder":
geom = hppfcl.Cylinder(props["radius"], props["halfLength"])
elif geom_name == "box":
geom = hppfcl.Box(2 * props["halfSide"][:, None])
elif geom_name == "sphere":
geom = hppfcl.Sphere(props["radius"])
elif geom_name == "cone":
geom = hppfcl.Cone(props["radius"], props["halfLength"])
elif geom_name == "mesh":
geom = None
else:
raise ValueError(f"Unsupported geometry type for {geom_name}.")
return geom
def show_robot_aabb(self):
def hex_to_rgb(value):
value = value.lstrip("#")
lv = len(value)
return tuple(
int(value[i:i + lv // 3], 16) for i in range(0, lv, lv // 3))
# geom_objs = self.geom_model.geometryObjects[1:7]
geom_objs = self.geom_model.geometryObjects
geometries = [geom_obj.geometry for geom_obj in geom_objs]
parents = [geom_obj.parentJoint for geom_obj in geom_objs]
i = 0
colors = cycle([
"#377eb8",
"#e41a1c",
"#4daf4a",
"#984ea3",
"#ff7f00",
"#ffff33",
"#a65628",
"#f781bf",
])
for geom, parent_id, color in zip(geometries, parents, colors):
aabb = geom.aabb_local
w, h, d = aabb.width(), aabb.height(), aabb.depth()
box = hppfcl.Box(w, h, d)
placement = pin.SE3(np.eye(3), aabb.center())
geom_obj = pin.GeometryObject(f"aabb{i}", 0, parent_id, box,
placement)
color = np.array(hex_to_rgb(color)) / 255
geom_obj.meshColor = np.array((color[0], color[1], color[2], 0.5))
self.viz_model.addGeometryObject(geom_obj)
i += 1
print("show aabb")
self._create_data()
self._create_viz()
try:
import hppfcl
except ImportError:
print("This example requires hppfcl")
sys.exit(0)
from pinocchio.visualize import GepettoVisualizer
pin.switchToNumpyArray()
model = pin.Model()
geom_model = pin.GeometryModel()
geometries = [
hppfcl.Capsule(0.1, 0.8),
hppfcl.Sphere(0.5),
hppfcl.Box(1, 1, 1),
hppfcl.Cylinder(0.1, 1.0),
hppfcl.Cone(0.5, 1.0),
]
for i, geom in enumerate(geometries):
placement = pin.SE3(np.eye(3), np.array([i, 0, 0]))
geom_obj = pin.GeometryObject("obj{}".format(i), 0, 0, geom, placement)
color = np.random.uniform(0, 1, 4)
color[3] = 1
geom_obj.meshColor = color
geom_model.addGeometryObject(geom_obj)
viz = GepettoVisualizer(
model=model,
collision_model=geom_model,
visual_model=geom_model,