def test_pair_equals(self):
c1 = pin.CollisionPair(1, 2)
c2 = pin.CollisionPair(1, 2)
c3 = pin.CollisionPair(3, 4)
self.assertEqual(c1, c2)
self.assertTrue(c1 == c2)
self.assertFalse(c1 != c2)
self.assertNotEqual(c1, c3)
self.assertTrue(c1 != c3)
self.assertFalse(c1 == c3)
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 appendUrdfModel(self, urdfFilename, frame_name, fMm, prefix=None):
if not isinstance(fMm, pinocchio.SE3):
fMm = pinocchio.XYZQUATToSE3(fMm)
id_parent_frame = self.model.getFrameId(frame_name)
model, gmodel = pinocchio.buildModelsFromUrdf(
hpp.rostools.retrieve_resource(urdfFilename),
geometry_types=pinocchio.GeometryType.COLLISION)
if prefix is not None:
for i in range(1, len(model.names)):
model.names[i] = prefix + model.names[i]
for f in model.frames:
f.name = prefix + f.name
for go in gmodel.geometryObjects:
go.name = prefix + go.name
igeom = self.gmodel.ngeoms
nmodel, ngmodel = pinocchio.appendModel(self.model, model, self.gmodel,
gmodel, id_parent_frame, fMm)
self.gid_field_of_view = ngmodel.getGeometryId("field_of_view")
for go in gmodel.geometryObjects:
ngmodel.addCollisionPair(
pinocchio.CollisionPair(self.gid_field_of_view,
ngmodel.getGeometryId(go.name)))
self.model, self.gmodel = nmodel, ngmodel
self.data = pinocchio.Data(self.model)
self.gdata = pinocchio.GeometryData(self.gmodel)
def test_pair_copy(self):
c1 = pin.CollisionPair(1, 2)
c2 = c1.copy()
self.assertEqual(c1, c2)
c2.second = 3
self.assertNotEqual(c1, c2)
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 add_obstacle(self, geom_obj, static):
if static:
geom_model = self.model_wrapper.geom_model
geom_model.addGeometryObject(geom_obj)
else:
raise NotImplementedError
# check collisions between the robot and obstacles
robot_n_joints = self.robot_n_joints
check_collision = range(robot_n_joints)
n_geom_model = len(geom_model.geometryObjects)
for collision_id in check_collision:
geom_model.addCollisionPair(
pin.CollisionPair(collision_id, n_geom_model - 1))
def initSolo():
robot = loadSolo(solo=False)
# Get robot model, data, and collision model
rmodel = robot.model
rdata = rmodel.createData()
gmodel = robot.collision_model
# Get the base_link and FL_UPPER_LEG meshes
base_link_geom = gmodel.getGeometryId("base_link_0")
leg_geom = gmodel.getGeometryId("FL_UPPER_LEG_0")
# Add the collision pair to the geometric model
gmodel.addCollisionPair(pio.CollisionPair(base_link_geom, leg_geom))
gdata = gmodel.createData()
return robot, rmodel, rdata, gmodel, gdata
def createRobotWithObstacles(robotname='ur5'):
### Robot
# Load the robot
robot = robex.load(robotname)
### Obstacle map
# Capsule obstacles will be placed at these XYZ-RPY parameters
oMobs = [[0.40, 0., 0.30, np.pi / 2, 0, 0],
[-0.08, -0., 0.69, np.pi / 2, 0, 0],
[0.23, -0., 0.04, np.pi / 2, 0, 0],
[-0.32, 0., -0.08, np.pi / 2, 0, 0]]
# Load visual objects and add them in collision/visual models
color = [1.0, 0.2, 0.2, 1.0] # color of the capsules
rad, length = .1, 0.4 # radius and length of capsules
for i, xyzrpy in enumerate(oMobs):
obs = pin.GeometryObject.CreateCapsule(
rad, length) # Pinocchio obstacle object
obs.meshColor = np.array(
[1.0, 0.2, 0.2,
1.0]) # Don't forget me, otherwise I am transparent ...
obs.name = "obs%d" % i # Set object name
obs.parentJoint = 0 # Set object parent = 0 = universe
obs.placement = XYZRPYtoSE3(xyzrpy) # Set object placement wrt parent
robot.collision_model.addGeometryObject(
obs) # Add object to collision model
robot.visual_model.addGeometryObject(obs) # Add object to visual model
### Collision pairs
nobs = len(oMobs)
nbodies = robot.collision_model.ngeoms - nobs
robotBodies = range(nbodies)
envBodies = range(nbodies, nbodies + nobs)
for a, b in itertools.product(robotBodies, envBodies):
robot.collision_model.addCollisionPair(pin.CollisionPair(a, b))
### Geom data
# Collision/visual models have been modified => re-generate corresponding data.
robot.collision_data = pin.GeometryData(robot.collision_model)
robot.visual_data = pin.GeometryData(robot.visual_model)
return robot
# Get the robot frames we're interested in
fl_upper_leg = rmodel.getFrameId("FL_UPPER_LEG")
fl_lower_leg = rmodel.getFrameId("FL_LOWER_LEG")
fr_upper_leg = rmodel.getFrameId("FR_UPPER_LEG")
fr_lower_leg = rmodel.getFrameId("FR_LOWER_LEG")
hl_upper_leg = rmodel.getFrameId("HL_UPPER_LEG")
hl_lower_leg = rmodel.getFrameId("HL_LOWER_LEG")
hr_upper_leg = rmodel.getFrameId("HR_UPPER_LEG")
hr_lower_leg = rmodel.getFrameId("HR_LOWER_LEG")
base_link = rmodel.getFrameId("base_link")
# Get the base_link and FL_UPPER_LEG meshes
base_link_geom = gmodel.getGeometryId("base_link_0")
fl_upper_geom = gmodel.getGeometryId("FL_UPPER_LEG_0")
# Add the collision pair to the geometric model
gmodel.addCollisionPair(pio.CollisionPair(base_link_geom, fl_upper_geom))
if(enableGUI):
# Display the robot
robot.rebuildData()
robot.displayCollisions(True)
robot.displayVisuals(False)
robot.display(robot_config)
for n in gv.getNodeList():
if 'collision' in n and 'simple' not in n and len(n)>27:
gv.setVisibility(n,'ON')
gv.setColor(n, [1,1,1,0.2])
if 'collision' in n and 'base_link' in n and len(n)>27:
gv.setColor(n, [1,0.5,0.2,1])
if 'collision' in n and 'FL_UPPER_LEG' in n and len(n)>27:
gv.setColor(n, [1,0.5,0.2,1])
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
collision_pair = pin.CollisionPair(height_field_collision_id,
panda_hand_collision_id)
collision_model.addCollisionPair(collision_pair)
viz = MeshcatVisualizer(model, collision_model, visual_model)
# Start a new MeshCat server and client.
# Note: the server can also be started separately using the "meshcat-server" command in a terminal:
# this enables the server to remain active after the current script ends.
#
# Option open=True pens the visualizer.
# Note: the visualizer can also be opened seperately by visiting the provided URL.
try:
viz.initViewer(open=True)
except ImportError as err:
print(
"Error while initializing the viewer. It seems you should install Python meshcat"
def addCollisionPairs(self):
# # Collision between finger 1 and right palm
# for nph in range(1,4):
# n= 'world/finger%d%d' % (1,nph)
# self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId(n),
# self.gmodel.getGeometryId('world/wpalmr')))
# # Collision between finger 2 and wirst
# for nph in range(1,4):
# n= 'world/finger%d%d' % (2,nph)
# self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId(n),
# self.gmodel.getGeometryId('world/wrist')))
# # Collision between finger 3 and left palm
# for nph in range(1,4):
# n= 'world/finger%d%d' % (3,nph)
# self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId(n),
# self.gmodel.getGeometryId('world/wpalml')))
# # Collision between phallange 2 and 3 and second palm
# for nf in range(1,4):
# for nph in range(2,4):
# n= 'world/finger%d%d' % (nf,nph)
# self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId(n),
# self.gmodel.getGeometryId('world/palm2')))
# self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId(n),
# self.gmodel.getGeometryId('world/wpalmfr')))
# # Collision between phallange 1 and phallange 3
# for nf in range(1,4):
# n1= 'world/finger%d%d' % (nf,1)
# n2= 'world/finger%d%d' % (nf,3)
# self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId(n1),
# self.gmodel.getGeometryId(n2)))
# self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId('world/wpalml'),
# self.gmodel.getGeometryId('world/thumb2')))
# self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId('world/wpalml'),
# self.gmodel.getGeometryId('world/thumb2')))
# for it in range(1,3):
# nt= 'world/thumb%d' % it
# self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId('world/wpalmfr'),
# self.gmodel.getGeometryId(nt)))
# self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId('world/palm2'),
# self.gmodel.getGeometryId(nt)))
# for iph in range(1,4):
# nph= 'world/finger%d%d' % (3,iph)
# self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId(nt),
# self.gmodel.getGeometryId(nph)))
pairs = [
['finger11','wpalmr'],
['finger12','wpalmr'],
['finger13','wpalmr'],
['finger21','wrist'],
['finger22','wrist'],
['finger23','wrist'],
['finger31','wpalml'],
['finger32','wpalml'],
['finger33','wpalml'],
['finger12','palm2'],
['finger12','wpalmfr'],
['finger13','palm2'],
['finger13','wpalmfr'],
['finger22','palm2'],
['finger22','wpalmfr'],
['finger23','palm2'],
['finger23','wpalmfr'],
['finger32','palm2'],
['finger32','wpalmfr'],
['finger33','palm2'],
['finger33','wpalmfr'],
['finger11','finger13'],
['finger21','finger23'],
['finger31','finger33'],
['wpalml','thumb2'],
['wpalmfr','thumb1'],
['palm2','thumb1'],
['thumb1','finger31'],
['thumb1','finger31'],
['thumb1','finger33'],
['wpalmfr','thumb2'],
['palm2','thumb2'],
['thumb2','finger31'],
['thumb2','finger32'],
['thumb2','finger33'],
]
for (n1,n2) in pairs:
self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId('world/'+n1),
self.gmodel.getGeometryId('world/'+n2)))
