def add_robot(self, robot_name, bounds):
model_wrapper = self.model_wrapper
color = self.robot_color
if robot_name == "sphere":
radius = 0.03
geom = hppfcl.Sphere(radius)
sphere_mesh = Mesh(name="robot", geometry=geom, color=color)
robot = FreeFlyer(model_wrapper, sphere_mesh, bounds)
elif robot_name == "sphere2d":
radius = 0.01
geom = hppfcl.Sphere(radius)
sphere_mesh = Mesh(name="robot", geometry=geom, color=color)
robot = FreeFlyer(model_wrapper, sphere_mesh, bounds)
elif robot_name == "s_shape":
mesh_path = "../assets/s_shape_description/s_shape.stl"
scale = (0.2, 0.2, 0.2)
s_mesh = Mesh(name="robot",
geometry_path=mesh_path,
color=color,
scale=scale)
robot = FreeFlyer(model_wrapper, s_mesh, bounds)
else:
raise ValueError(f"Unknown robot: {robot_name}")
self.robot_n_joints = robot.n_joints
return robot
def addTerrainsToGeomModel(gmodel, terrain, obstacles):
"""
Add a list of stepstones and obstacles to the robot geometry object.
Each step stone is defined by its SE3 placement. It is added as a red disk of 20cm radius.
Each obstacles is defined by its 3d position. It is added as a white sphere of radius 20cm.
- gmodel is a pinocchio geometry model
- terrain is a list of SE3 placement of the step stones
- obstacles is a list of 3d position of the sphere centers.
"""
# Create pinocchio 3d objects for each step stone
for i, d in enumerate(terrain):
# The step stones have name "debris0X" and are attached to the world (jointId=0).
g2 = pin.GeometryObject("debris%02d" % i, 0, hppfcl.Cylinder(.2, .01), d)
g2.meshColor = np.array([1, 0, 0, 1.])
gmodel.addGeometryObject(g2)
# Create Pinocchio 3d objects for the obstacles.
for i, obs in enumerate(obstacles):
# The obstacles have name "obs0X" and are attached to the world (jointId=0).
g2 = pin.GeometryObject("obs%02d" % i, 0, hppfcl.Sphere(.2), pin.SE3(np.eye(3), obs))
g2.meshColor = np.array([1, 1, 1, 1.])
gmodel.addGeometryObject(g2)
# Add the collision pair to check the robot collision against the step stones and the obstacles.
# For simplicity, does not activate the self-collision pairs.
ngeomRobot = len(gmodel.geometryObjects) - len(terrain) - len(obstacles)
for irobot in range(ngeomRobot):
for ienv in range(len(terrain) + len(obstacles)):
gmodel.addCollisionPair(pin.CollisionPair(irobot, ngeomRobot + ienv))
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 show_joints(self):
raise ValueError("To be reimplemented")
goal_jts = utils.get_oMi(self.model, self.data, self.goal_state["q"])
for i, jt_se3 in enumerate(goal_jts):
self.add_mesh(
f"jt{i}",
geom=hppfcl.Sphere(0.07),
placement=jt_se3,
check_collision=False,
color=(0, 0, 1, 1.0),
)
self._create_data()
self._create_viz()
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 test_sphere(self):
sphere = hppfcl.Sphere(1.)
self.assertIsInstance(sphere, hppfcl.Sphere)
self.assertIsInstance(sphere, hppfcl.ShapeBase)
self.assertIsInstance(sphere, hppfcl.CollisionGeometry)
self.assertEqual(sphere.getNodeType(), hppfcl.NODE_TYPE.GEOM_SPHERE)
self.assertEqual(sphere.radius, 1.)
sphere.radius = 2.
self.assertEqual(sphere.radius, 2.)
com = sphere.computeCOM()
self.assertApprox(com, np.zeros(3))
V = sphere.computeVolume()
V_ref = 4. * np.pi / 3 * sphere.radius**3
self.assertApprox(V, V_ref)
I0 = sphere.computeMomentofInertia()
I0_ref = 0.4 * V_ref * sphere.radius * sphere.radius * np.identity(3)
self.assertApprox(I0, I0_ref)
Ic = sphere.computeMomentofInertiaRelatedToCOM()
self.assertApprox(Ic, I0_ref)
import pinocchio as pin
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,
import hppfcl as fcl
import numpy as np
import math
import time
import sys
N = 10 # number of pendulums
model = pin.Model()
geom_model = pin.GeometryModel()
parent_id = 0
joint_placement = pin.SE3.Identity()
body_mass = 1.
body_radius = 0.1
shape0 = fcl.Sphere(body_radius)
geom0_obj = pin.GeometryObject("base", 0, shape0, pin.SE3.Identity())
geom0_obj.meshColor = np.array([1., 0.1, 0.1, 1.])
geom_model.addGeometryObject(geom0_obj)
for k in range(N):
joint_name = "joint_" + str(k + 1)
joint_id = model.addJoint(parent_id, pin.JointModelRY(), joint_placement,
joint_name)
body_inertia = pin.Inertia.FromSphere(body_mass, body_radius)
body_placement = joint_placement.copy()
body_placement.translation[2] = 1.
model.appendBodyToJoint(joint_id, body_inertia, body_placement)
geom1_name = "ball_" + str(k + 1)
def make_geom_obj(self, name, radius):
geom = hppfcl.Sphere(radius)
mesh = Mesh(name=name, geometry=geom)
return mesh.geom_obj()
def make_geom_obj(self, name):
geom = hppfcl.Sphere(self.radius)
mesh = Mesh(name=name, geometry=geom, color=self.color)
return mesh.geom_obj()
body_placement = geometry_placement
model.appendBodyToJoint(
joint_id, body_inertia, body_placement
) # We need to rotate the inertia as it is expressed in the LOCAL frame of the geometry
shape_cart = fcl.Cylinder(cart_radius, cart_length)
geom_cart = pin.GeometryObject("shape_cart", joint_id, shape_cart,
geometry_placement)
geom_cart.meshColor = np.array([1., 0.1, 0.1, 1.])
geom_model.addGeometryObject(geom_cart)
parent_id = joint_id
else:
base_radius = 0.2
shape_base = fcl.Sphere(base_radius)
geom_base = pin.GeometryObject("base", 0, shape_base, pin.SE3.Identity())
geom_base.meshColor = np.array([1., 0.1, 0.1, 1.])
geom_model.addGeometryObject(geom_base)
joint_placement = pin.SE3.Identity()
body_mass = 1.
body_radius = 0.1
for k in range(N):
joint_name = "joint_" + str(k + 1)
joint_id = model.addJoint(parent_id, pin.JointModelRX(), joint_placement,
joint_name)
body_inertia = pin.Inertia.FromSphere(body_mass, body_radius)
body_placement = joint_placement.copy()