def is_collided(objcm0, objcm1):
"""
check if two objcm are collided after converting the specified cdmesh_type
:param objcm0:
:param objcm1:
:return:
author: weiwei
date: 20210117
"""
# avoid using objcm.cdmesh -> be compatible with other cdhelpers
obj0 = gen_cdmesh_vvnf(*objcm0.extract_rotated_vvnf())
obj1 = gen_cdmesh_vvnf(*objcm1.extract_rotated_vvnf())
result = base.physicsworld.contactTestPair(obj0, obj1)
contacts = result.getContacts()
contact_points = [
da.pdv3_to_npv3(ct.getManifoldPoint().getPositionWorldOnB()) /
SCALE_FOR_PRECISION for ct in contacts
]
contact_points += [
da.pdv3_to_npv3(ct.getManifoldPoint().getPositionWorldOnA()) /
SCALE_FOR_PRECISION for ct in contacts
]
return (True,
contact_points) if len(contact_points) > 0 else (False,
contact_points)
def rayhit_closet(pfrom, pto, objcm):
"""
:param pfrom:
:param pto:
:param objcm:
:return:
author: weiwei
date: 20190805
"""
tgt_cdmesh = gen_cdmesh_vvnf(*objcm.extract_rotated_vvnf())
ray = OdeRayGeom(length=1)
length, dir = rm.unit_vector(pto - pfrom, toggle_length=True)
ray.set(pfrom[0], pfrom[1], pfrom[2], dir[0], dir[1], dir[2])
ray.setLength(length)
contact_entry = OdeUtil.collide(ray, tgt_cdmesh, max_contacts=10)
contact_points = [
da.pdv3_to_npv3(point) for point in contact_entry.getContactPoints()
]
min_id = np.argmin(np.linalg.norm(pfrom - np.array(contact_points),
axis=1))
contact_normals = [
da.pdv3_to_npv3(contact_entry.getContactGeom(i).getNormal())
for i in range(contact_entry.getNumContacts())
]
return contact_points[min_id], contact_normals[min_id]
def gen_cylindrical_cdnp(pdnp, name='cdnp_cylinder', radius=0.01):
"""
:param trimeshmodel:
:param name:
:param radius:
:return:
author: weiwei
date: 20200108
"""
bottom_left, top_right = pdnp.getTightBounds()
center = (bottom_left + top_right) / 2.0
# enlarge the bounding box
bottomleft_adjustvec = bottom_left - center
bottomleft_adjustvec[2] = 0
bottomleft_adjustvec.normalize()
bottom_left += bottomleft_adjustvec * radius
topright_adjustvec = top_right - center
topright_adjustvec[2] = 0
topright_adjustvec.normalize()
top_right += topright_adjustvec * radius
bottomleft_pos = da.pdv3_to_npv3(bottom_left)
topright_pos = da.pdv3_to_npv3(top_right)
collision_node = CollisionNode(name)
for angle in np.nditer(np.linspace(math.pi / 10, math.pi * 4 / 10, 4)):
ca = math.cos(angle)
sa = math.sin(angle)
new_bottomleft_pos = np.array([bottomleft_pos[0] * ca, bottomleft_pos[1] * sa, bottomleft_pos[2]])
new_topright_pos = np.array([topright_pos[0] * ca, topright_pos[1] * sa, topright_pos[2]])
new_bottomleft = da.npv3_to_pdv3(new_bottomleft_pos)
new_topright = da.npv3_to_pdv3(new_topright_pos)
collision_primitive = CollisionBox(new_bottomleft, new_topright)
collision_node.addSolid(collision_primitive)
return collision_node
def simulationTask(task):
# Step the simulation and set the new positions
world.quickStep(globalClock.getDt())
sphere_a.set_pos(da.pdv3_to_npv3(body_sphere_a.getPosition()))
sphere_b.set_pos(da.pdv3_to_npv3(body_sphere_b.getPosition()))
gm.gen_linesegs([[np.zeros(3), sphere_a.get_pos()]], thickness=.05, rgba=[0, 1, 0, 1]).attach_to(base)
gm.gen_linesegs([[sphere_a.get_pos(), sphere_b.get_pos()]], thickness=.05, rgba=[0, 0, 1, 1]).attach_to(base)
return task.cont
def is_collided(objcm_list0, objcm_list1, toggle_contact_points=False, toggle_plot_cdprimit=False):
"""
detect the collision between collision models
:param: objcm_list0, a single collision model or a list of collision models
:param: objcm_list1
:return: True or False
author: weiwei
date: 20190312osaka, 20201214osaka
"""
if not isinstance(objcm_list0, list):
objcm_list0 = [objcm_list0]
if not isinstance(objcm_list1, list):
objcm_list1 = [objcm_list1]
if toggle_plot_cdprimit:
for one_objcm in objcm_list0:
one_objcm.show_cdprimit()
for one_objcm in objcm_list1:
one_objcm.show_cdprimit()
tmpnp = NodePath("collision nodepath")
ctrav = CollisionTraverser()
chan = CollisionHandlerQueue()
for one_objcm in objcm_list0:
ctrav.addCollider(one_objcm.copy_cdnp_to(tmpnp), chan)
for one_objcm in objcm_list1:
one_objcm.copy_cdnp_to(tmpnp)
ctrav.traverse(tmpnp)
if chan.getNumEntries() > 0:
if toggle_contact_points:
contact_points = [da.pdv3_to_npv3(cd_entry.getSurfacePoint(base.render)) for cd_entry in chan.getEntries()]
return True, contact_points
else:
return True
else:
return False
def is_collided(obj_bullet_rbd0, obj_bullet_rbd1):
"""
check if two objcm are collided after converting the specified cdmesh_type
:param objcm0:
:param objcm1:
:return:
author: weiwei
date: 20210117, 20211215
"""
result = base.physicsworld.contactTestPair(obj_bullet_rbd0, obj_bullet_rbd1)
contacts = result.getContacts()
contact_points = [da.pdv3_to_npv3(ct.getManifoldPoint().getPositionWorldOnB()) / SCALE_FOR_PRECISION for ct in
contacts]
contact_points += [da.pdv3_to_npv3(ct.getManifoldPoint().getPositionWorldOnA()) / SCALE_FOR_PRECISION for ct in
contacts]
return (True, contact_points) if len(contact_points) > 0 else (False, contact_points)
def is_collided(self,
obstacle_list=[],
otherrobot_list=[],
toggle_contact_points=False):
"""
:param obstacle_list: staticgeometricmodel
:param otherrobot_list:
:return:
"""
for cdelement in self.all_cdelements:
pos = cdelement['gl_pos']
rotmat = cdelement['gl_rotmat']
cdnp = self.np.getChild(cdelement['cdprimit_childid'])
cdnp.setPosQuat(da.npv3_to_pdv3(pos), da.npmat3_to_pdquat(rotmat))
# print(da.npv3mat3_to_pdmat4(pos, rotmat))
# print("From", cdnp.node().getFromCollideMask())
# print("Into", cdnp.node().getIntoCollideMask())
# print("xxxx colliders xxxx")
# for collider in self.ctrav.getColliders():
# print(collider.getMat())
# print("From", collider.node().getFromCollideMask())
# print("Into", collider.node().getIntoCollideMask())
# attach obstacles
obstacle_parent_list = []
for obstacle in obstacle_list:
obstacle_parent_list.append(obstacle.objpdnp.getParent())
obstacle.objpdnp.reparentTo(self.np)
# attach other robots
for robot in otherrobot_list:
for cdnp in robot.cc.np.getChildren():
current_into_cdmask = cdnp.node().getIntoCollideMask()
new_into_cdmask = current_into_cdmask | self._bitmask_ext
cdnp.node().setIntoCollideMask(new_into_cdmask)
robot.cc.np.reparentTo(self.np)
# collision check
self.ctrav.traverse(self.np)
# clear obstacles
for i, obstacle in enumerate(obstacle_list):
obstacle.objpdnp.reparentTo(obstacle_parent_list[i])
# clear other robots
for robot in otherrobot_list:
for cdnp in robot.cc.np.getChildren():
current_into_cdmask = cdnp.node().getIntoCollideMask()
new_into_cdmask = current_into_cdmask & ~self._bitmask_ext
cdnp.node().setIntoCollideMask(new_into_cdmask)
robot.cc.np.detachNode()
if self.chan.getNumEntries() > 0:
collision_result = True
else:
collision_result = False
if toggle_contact_points:
contact_points = [
da.pdv3_to_npv3(cd_entry.getSurfacePoint(base.render))
for cd_entry in self.chan.getEntries()
]
return collision_result, contact_points
else:
return collision_result
def rayhit_all(pfrom, pto, objcm):
"""
:param pfrom:
:param pto:
:param objcm:
:return:
author: weiwei
date: 20190805
"""
tgt_cdmesh = gen_cdmesh_vvnf(*objcm.extract_rotated_vvnf())
ray = OdeRayGeom(length=1)
length, dir = rm.unit_vector(pto-pfrom, toggle_length=True)
ray.set(pfrom[0], pfrom[1], pfrom[2], dir[0], dir[1], dir[2])
ray.setLength(length)
hit_entry = OdeUtil.collide(ray, tgt_cdmesh)
hit_points = [da.pdv3_to_npv3(point) for point in hit_entry.getContactPoints()]
hit_normals = [da.pdv3_to_npv3(hit_entry.getContactGeom(i).getNormal()) for i in range(hit_entry.getNumContacts())]
return hit_points, hit_normals
def rayhit_all(pfrom, pto, objcm):
"""
:param pfrom:
:param pto:
:param objcm:
:return:
author: weiwei
date: 20190805, 20210118
"""
# avoid using objcm.cdmesh -> be compatible with other cdhelpers
tgt_cdmesh = gen_cdmesh_vvnf(*objcm.extract_rotated_vvnf())
base.physicsworld.attach(tgt_cdmesh)
result = base.physicsworld.rayTestAll(da.npv3_to_pdv3(pfrom), da.npv3_to_pdv3(pto))
base.physicsworld.removeRigidBody(tgt_cdmesh)
if result.hasHits():
return [[da.pdv3_to_npv3(hit.getHitPos()), da.pdv3_to_npv3(-hit.getHitNormal())] for hit in result.getHits()]
else:
return []
def getpos(self):
"""
:return: 1x3 nparray
"""
pdmat4 = self.getTransform().getMat()
pdv3 = pdmat4.xformPoint(Vec3(-self.com[0], -self.com[1], -self.com[2]))
# homomat = dh.pdmat4_to_npmat4(pdmat4)
pos = dh.pdv3_to_npv3(pdv3)
return pos
def is_collided(obj_ode_trimesh0, obj_ode_trimesh1):
"""
check if two odetrimesh are collided
:param obj_ode_trimesh00: an instance of OdeTriMeshGeom
:param obj_ode_trimesh1: an instance of OdeTriMeshGeom
:return:
author: weiwei
date: 20210118, 20211215
"""
contact_entry = OdeUtil.collide(obj_ode_trimesh0, obj_ode_trimesh1, max_contacts=10)
contact_points = [da.pdv3_to_npv3(point) for point in contact_entry.getContactPoints()]
return (True, contact_points) if len(contact_points) > 0 else (False, contact_points)
def gethomomat(self):
"""
get the homomat considering the original local frame
the dynamic body moves in a local frame defined at com (line 46 of this file), instead of returning the
homomat of the dynamic body, this file returns the pose of original local frame
the returned homomat can be used by collision bodies for rendering.
:return:
author: weiwei
date: 2019?, 20201119
"""
pdmat4 = self.getTransform().getMat()
pdv3 = pdmat4.xformPoint(Vec3(-self.com[0], -self.com[1], -self.com[2]))
homomat = dh.pdmat4_to_npmat4(pdmat4)
pos = dh.pdv3_to_npv3(pdv3)
homomat[:3, 3] = pos
return homomat
def is_collided(objcm0, objcm1):
"""
check if two objcm are collided after converting the specified cdmesh_type
:param objcm0: an instance of CollisionModel or CollisionModelCollection
:param objcm1: an instance of CollisionModel or CollisionModelCollection
:return:
author: weiwei
date: 20210118
"""
obj0 = gen_cdmesh_vvnf(*objcm0.extract_rotated_vvnf())
obj1 = gen_cdmesh_vvnf(*objcm1.extract_rotated_vvnf())
contact_entry = OdeUtil.collide(obj0, obj1, max_contacts=10)
contact_points = [
da.pdv3_to_npv3(point) for point in contact_entry.getContactPoints()
]
return (True,
contact_points) if len(contact_points) > 0 else (False,
contact_points)
def get_scale(self):
return da.pdv3_to_npv3(self._objpdnp.getScale())
def get_pos(self):
# return self.pos
# print("pose", self.pos)
# print("getpose", da.pdv3_to_npv3(self.cubenode.get_pos(self.node)))
return da.pdv3_to_npv3(self.cubenode.get_pos(self.node))
def __init__(self,
initor,
cdtype="triangles",
mass=.3,
restitution=0,
allow_deactivation=False,
allow_ccd=True,
friction=.2,
dynamic=True,
name="rbd"):
"""
TODO: triangles do not seem to work (very slow) in the github version (20210418)
Use convex if possible
:param initor: could be itself (copy), or an instance of collision model
:param type: triangle or convex
:param mass:
:param restitution: bounce parameter
:param friction:
:param dynamic: only applicable to triangle type, if an object does not move with force, it is not dynamic
:param name:
author: weiwei
date: 20190626, 20201119
"""
super().__init__(name)
if isinstance(initor, gm.GeometricModel):
if initor._objtrm is None:
raise ValueError("Only applicable to models with a trimesh!")
self.com = initor.objtrm.center_mass * base.physics_scale
self.setMass(mass)
self.setRestitution(restitution)
self.setFriction(friction)
self.setLinearDamping(.3)
self.setAngularDamping(.3)
if allow_deactivation:
self.setDeactivationEnabled(True)
self.setLinearSleepThreshold(.01 * base.physics_scale)
self.setAngularSleepThreshold(.01 * base.physics_scale)
else:
self.setDeactivationEnabled(False)
if allow_ccd: # continuous collision detection
self.setCcdMotionThreshold(1e-7)
self.setCcdSweptSphereRadius(0.0005 * base.physics_scale)
geom_np = initor.objpdnp.getChild(0).find("+GeomNode")
geom = copy.deepcopy(geom_np.node().getGeom(0))
vdata = geom.modifyVertexData()
vertices = copy.deepcopy(
np.frombuffer(vdata.modifyArrayHandle(0).getData(),
dtype=np.float32))
vertices.shape = (-1, 6)
vertices[:, :3] = vertices[:, :3] * base.physics_scale - self.com
vdata.modifyArrayHandle(0).setData(
vertices.astype(np.float32).tobytes())
geomtf = geom_np.getTransform()
geomtf = geomtf.setPos(geomtf.getPos() * base.physics_scale)
if cdtype == "triangles":
geombmesh = BulletTriangleMesh()
geombmesh.addGeom(geom)
bulletshape = BulletTriangleMeshShape(geombmesh,
dynamic=dynamic)
bulletshape.setMargin(1e-6)
self.addShape(bulletshape, geomtf)
elif cdtype == "convex":
bulletshape = BulletConvexHullShape(
) # TODO: compute a convex hull?
bulletshape.addGeom(geom, geomtf)
bulletshape.setMargin(1e-6)
self.addShape(bulletshape, geomtf)
elif cdtype == 'box':
minx = min(vertices[:, 0])
miny = min(vertices[:, 1])
minz = min(vertices[:, 2])
maxx = max(vertices[:, 0])
maxy = max(vertices[:, 1])
maxz = max(vertices[:, 2])
pcd_box = CollisionBox(Point3(minx, miny, minz),
Point3(maxx, maxy, maxz))
bulletshape = BulletBoxShape.makeFromSolid(pcd_box)
bulletshape.setMargin(1e-6)
self.addShape(bulletshape, geomtf)
else:
raise NotImplementedError
pd_homomat = geomtf.getMat()
pd_com_pos = pd_homomat.xformPoint(
Vec3(self.com[0], self.com[1], self.com[2]))
np_homomat = dh.pdmat4_to_npmat4(pd_homomat)
np_com_pos = dh.pdv3_to_npv3(pd_com_pos)
np_homomat[:3, 3] = np_com_pos # update center to com
self.setTransform(
TransformState.makeMat(dh.npmat4_to_pdmat4(np_homomat)))
elif isinstance(initor, BDBody):
self.com = initor.com.copy()
self.setMass(initor.getMass())
self.setRestitution(initor.restitution)
self.setFriction(initor.friction)
self.setLinearDamping(.3)
self.setAngularDamping(.3)
if allow_deactivation:
self.setDeactivationEnabled(True)
self.setLinearSleepThreshold(.01 * base.physics_scale)
self.setAngularSleepThreshold(.01 * base.physics_scale)
else:
self.setDeactivationEnabled(False)
if allow_ccd:
self.setCcdMotionThreshold(1e-7)
self.setCcdSweptSphereRadius(0.0005 * base.physics_scale)
np_homomat = copy.deepcopy(initor.get_homomat())
np_homomat[:3, 3] = np_homomat[:3, 3] * base.physics_scale
self.setTransform(
TransformState.makeMat(dh.npmat4_to_pdmat4(np_homomat)))
self.addShape(initor.getShape(0), initor.getShapeTransform(0))
def set_pos(self, npvec3):
self.setPos(dh.pdv3_to_npv3(npvec3) * base.physics_scale)
def get_pos(self):
pdmat4 = self.getTransform().getMat()
pdv3 = pdmat4.xformPoint(Vec3(-self.com[0], -self.com[1],
-self.com[2]))
pos = dh.pdv3_to_npv3(pdv3) / base.physics_scale
return pos
def get_homomat(self):
npv3 = da.pdv3_to_npv3(self._objpdnp.getPos())
npmat3 = da.pdquat_to_npmat3(self._objpdnp.getQuat())
return rm.homomat_from_posrot(npv3, npmat3)
def get_pos(self):
return da.pdv3_to_npv3(self._objpdnp.getPos())
def __init__(self, objinit, cdtype="triangle", mass=.3, restitution=0, allowdeactivation=False, allowccd=True,
friction=.2, dynamic=True, name="rbd"):
"""
:param objinit: could be itself (copy), or an instance of collision model
:param type: triangle or convex
:param mass:
:param restitution: bounce parameter
:param friction:
:param dynamic: only applicable to triangle type, if an object does not move with force, it is not dynamic
:param name:
author: weiwei
date: 20190626, 20201119
"""
super().__init__(name)
if isinstance(objinit, gm.GeometricModel):
if objinit._objtrm is None:
raise ValueError("Only applicable to models with a trimesh!")
self.com = objinit.objtrm.center_mass
self.setMass(mass)
self.setRestitution(restitution)
self.setFriction(friction)
self.setLinearDamping(.3)
self.setAngularDamping(.3)
if allowdeactivation:
self.setDeactivationEnabled(True)
self.setLinearSleepThreshold(0.001)
self.setAngularSleepThreshold(0.001)
else:
self.setDeactivationEnabled(False)
if allowccd: # continuous collision detection
self.setCcdMotionThreshold(1e-6)
self.setCcdSweptSphereRadius(0.0005)
gnd = objinit.objpdnp.getChild(0).find("+GeomNode")
geom = copy.deepcopy(gnd.node().getGeom(0))
vdata = geom.modifyVertexData()
vertrewritter = GeomVertexRewriter(vdata, 'vertex')
while not vertrewritter.isAtEnd(): # shift local coordinate to geom to correctly update dynamic changes
v = vertrewritter.getData3f()
vertrewritter.setData3f(v[0] - self.com[0], v[1] - self.com[1], v[2] - self.com[2])
geomtf = gnd.getTransform()
if cdtype is "triangle":
geombmesh = BulletTriangleMesh()
geombmesh.addGeom(geom)
bulletshape = BulletTriangleMeshShape(geombmesh, dynamic=dynamic)
bulletshape.setMargin(1e-6)
self.addShape(bulletshape, geomtf)
elif cdtype is "convex":
bulletshape = BulletConvexHullShape() # TODO: compute a convex hull?
bulletshape.addGeom(geom, geomtf)
bulletshape.setMargin(1e-6)
self.addShape(bulletshape, geomtf)
else:
raise NotImplementedError
pdmat4 = geomtf.getMat()
pdv3 = pdmat4.xformPoint(Vec3(self.com[0], self.com[1], self.com[2]))
homomat = dh.pdmat4_to_npmat4(pdmat4)
pos = dh.pdv3_to_npv3(pdv3)
homomat[:3, 3] = pos # update center to com
self.setTransform(TransformState.makeMat(dh.npmat4_to_pdmat4(homomat)))
elif isinstance(objinit, BDBody):
self.com = objinit.com.copy()
self.setMass(objinit.getMass())
self.setRestitution(objinit.restitution)
self.setFriction(objinit.friction)
self.setLinearDamping(.3)
self.setAngularDamping(.3)
if allowdeactivation:
self.setDeactivationEnabled(True)
self.setLinearSleepThreshold(0.001)
self.setAngularSleepThreshold(0.001)
else:
self.setDeactivationEnabled(False)
if allowccd:
self.setCcdMotionThreshold(1e-6)
self.setCcdSweptSphereRadius(0.0005)
self.setTransform(TransformState.makeMat(dh.npmat4_to_pdmat4(objinit.gethomomat())))
self.addShape(objinit.getShape(0), objinit.getShapeTransform(0))
def setpos(self, npvec3):
"""
:param npvec3: 1x3 nparray
:return:
"""
self.setPos(dh.pdv3_to_npv3(npvec3))
