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 __init__(
self,
urdfString=None,
urdfFilename=None,
fov=np.radians((60., 90.)),
# fov = np.radians((49.5, 60)),
geoms=["arm_3_link_0"],
):
if isinstance(fov, str):
self.fov = fov
fov_fcl = hppfcl.MeshLoader().load(
hpp.rostools.retrieve_resource(fov))
else:
# build FOV tetahedron
dx, dy = np.sin(fov)
pts = hppfcl.StdVec_Vec3f()
self.fov = [
(0, 0, 0),
(dx, dy, 1),
(-dx, dy, 1),
(-dx, -dy, 1),
(dx, -dy, 1),
]
pts.extend([np.array(e) for e in self.fov])
self.fov.append(self.fov[1])
fov_fcl = hppfcl.BVHModelOBBRSS()
fov_fcl.beginModel(4, 5)
fov_fcl.addSubModel(pts, _tetahedron_tris)
fov_fcl.endModel()
self.cos_angle_thr = np.cos(np.radians(70))
if urdfFilename is None:
assert urdfString is not None
# Pinocchio does not allow to build a GeometryModel from a XML string.
urdfFilename = '/tmp/tmp.urdf'
with open(urdfFilename, 'w') as f:
f.write(urdfString)
self.model, self.gmodel = pinocchio.buildModelsFromUrdf(
hpp.rostools.retrieve_resource(urdfFilename),
root_joint=pinocchio.JointModelPlanar(),
geometry_types=pinocchio.GeometryType.COLLISION)
id_parent_frame = self.model.getFrameId("xtion_rgb_optical_frame")
parent_frame = self.model.frames[id_parent_frame]
go = pinocchio.GeometryObject("field_of_view", id_parent_frame,
parent_frame.parent, fov_fcl,
parent_frame.placement)
self.gid_field_of_view = self.gmodel.addGeometryObject(go, self.model)
for n in geoms:
assert self.gmodel.existGeometryName(n)
self.gmodel.addCollisionPair(
pinocchio.CollisionPair(self.gmodel.getGeometryId(n),
self.gid_field_of_view))
self.data = pinocchio.Data(self.model)
self.gdata = pinocchio.GeometryData(self.gmodel)
def addDriller(self, mesh, frame_name, fMm):
if not isinstance(fMm, pinocchio.SE3):
fMm = pinocchio.XYZQUATToSE3(fMm)
id_parent_frame = self.model.getFrameId(frame_name)
parent_frame = self.model.frames[id_parent_frame]
go = pinocchio.GeometryObject("driller", id_parent_frame, parent_frame.parent,
hppfcl.MeshLoader().load(hpp.rostools.retrieve_resource(mesh)),
parent_frame.placement * fMm)
self.gmodel.addGeometryObject(go, self.model)
self.gmodel.addCollisionPair(pinocchio.CollisionPair(self.gid_field_of_view, self.gmodel.ngeoms-1))
self.gdata = pinocchio.GeometryData(self.gmodel)
def dict_to_geom_obj(props):
geom = dict_to_geom(props["geom"])
placement = pin.SE3(props["placement"])
geom_obj = pin.GeometryObject(
name=props["name"],
parent_joint=props["parentJoint"],
parent_frame=props["parentFrame"],
collision_geometry=geom,
placement=placement,
mesh_path=props["meshPath"],
mesh_scale=props["meshScale"],
override_material=props["overrideMaterial"],
mesh_color=props["meshColor"],
)
geom_obj.meshScale = props["meshScale"]
geom_obj.meshColor = props["meshColor"]
# geom_obj.overrideMaterial = props["overrideMaterial"]
# geom_obj.meshTexturePath = props["meshTexturePath"]
return geom_obj
def addCapsule(name,
length,
radius,
linkFrameId,
jointId,
fMcaps,
rmodel,
gmodel,
gui=None):
caps = gmodel.addGeometryObject(
pio.GeometryObject(
name,
linkFrameId,
jointId,
hppfcl.Capsule(
radius,
length), # WARNING : FCL takses the capsule halfLength !
fMcaps),
rmodel)
if (gui != None):
gui.addCapsule("world/pinocchio/collisions/" + name, radius, length,
[1, 0, 0, .1])
return caps
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()
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,
)
# Initialize the viewer.
try:
viz.initViewer()
except ImportError as error:
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)
shape1 = fcl.Sphere(body_radius)
y_dim = y_grid[-1] - y_grid[0]
point_bins = y_bins * nx + x_bins
heights = np.zeros((ny, nx))
np.maximum.at(heights.ravel(), point_bins, Z)
point_cloud = fcl.BVHModelOBBRSS()
point_cloud.beginModel(0, num_points)
point_cloud.addVertices(points.T)
height_field = fcl.HeightFieldOBBRSS(x_dim, y_dim, heights, min(Z))
height_field_placement = point_cloud_placement * pin.SE3(
np.eye(3),
0.5 * np.array([x_grid[0] + x_grid[-1], y_grid[0] + y_grid[-1], 0.]))
go_point_cloud = pin.GeometryObject("point_cloud", 0, point_cloud,
point_cloud_placement)
go_point_cloud.meshColor = np.ones((4))
collision_model.addGeometryObject(go_point_cloud)
visual_model.addGeometryObject(go_point_cloud)
go_height_field = pin.GeometryObject("height_field", 0, height_field,
height_field_placement)
go_height_field.meshColor = np.ones((4))
height_field_collision_id = collision_model.addGeometryObject(go_height_field)
visual_model.addGeometryObject(go_height_field)
# Add colllision pair between the height field and the panda_hand geometry
panda_hand_collision_id = collision_model.getGeometryId("panda_hand_0")
go_panda_hand = collision_model.geometryObjects[panda_hand_collision_id]
go_panda_hand.geometry.buildConvexRepresentation(False)
go_panda_hand.geometry = go_panda_hand.geometry.convex # We need to work with the convex hull of the real mesh
print(err)
sys.exit(0)
# Load the robot in the viewer.
viz.loadViewerModel()
# Display a robot configuration.
q0 = pin.neutral(model)
viz.display(q0)
viz.displayCollisions(True)
viz.displayVisuals(False)
mesh = visual_model.geometryObjects[0].geometry
mesh.buildConvexRepresentation(True)
convex = mesh.convex
if convex is not None:
placement = pin.SE3.Identity()
placement.translation[0] = 2.
geometry = pin.GeometryObject("convex", 0, convex, placement)
geometry.meshColor = np.ones((4))
visual_model.addGeometryObject(geometry)
# Display another robot.
viz2 = MeshcatVisualizer(model, collision_model, visual_model)
viz2.initViewer(viz.viewer)
viz2.loadViewerModel(rootNodeName="pinocchio2")
q = q0.copy()
q[1] = 1.0
viz2.display(q)
geometry_placement.rotation = pin.Quaternion(np.array(
[0., 0., 1.]), np.array([0., 1., 0.])).toRotationMatrix()
joint_id = model.addJoint(parent_id, pin.JointModelPY(),
pin.SE3.Identity(), joint_name)
body_inertia = pin.Inertia.FromCylinder(cart_mass, cart_radius,
cart_length)
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
