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))