def test_cone(self):
cone = hppfcl.Cone(1., 2.)
self.assertIsInstance(cone, hppfcl.Cone)
self.assertIsInstance(cone, hppfcl.ShapeBase)
self.assertIsInstance(cone, hppfcl.CollisionGeometry)
self.assertEqual(cone.getNodeType(), hppfcl.NODE_TYPE.GEOM_CONE)
self.assertEqual(cone.radius, 1.)
self.assertEqual(cone.halfLength, 1.)
cone.radius = 3.
cone.halfLength = 4.
self.assertEqual(cone.radius, 3.)
self.assertEqual(cone.halfLength, 4.)
com = cone.computeCOM()
self.assertApprox(com, np.matrix([0., 0., -0.5 * cone.halfLength]).T)
V = cone.computeVolume()
V_ref = np.pi * cone.radius**2 * 2. * cone.halfLength / 3.
self.assertApprox(V, V_ref)
I0 = cone.computeMomentofInertia()
Ix_ref = V_ref * (3. / 20. * cone.radius**2 + 0.4 * cone.halfLength**2)
Iz_ref = 0.3 * V_ref * cone.radius**2
I0_ref = np.diag([Ix_ref, Ix_ref, Iz_ref])
self.assertApprox(I0, I0_ref)
Ic = cone.computeMomentofInertiaRelatedToCOM()
Icx_ref = V_ref * 3. / 20. * (cone.radius**2 + cone.halfLength**2)
Ic_ref = np.diag([Icx_ref, Icx_ref, Iz_ref])
self.assertApprox(Ic, Ic_ref)
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
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,
)