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 show_cdprimit(self): """ Copy the current nodepath to base.render to show collision states TODO: maintain a list to allow unshow :return: author: weiwei date: 20220404 """ # print("call show_cdprimit") snp_cpy = self.np.copyTo(base.render) for cdelement in self.all_cdelements: pos = cdelement['gl_pos'] rotmat = cdelement['gl_rotmat'] cdnp = snp_cpy.getChild(cdelement['cdprimit_childid']) cdnp.setPosQuat(da.npv3_to_pdv3(pos), da.npmat3_to_pdquat(rotmat)) cdnp.show()
def set_homomat(self, npmat4): self._objpdnp.setPosQuat(da.npv3_to_pdv3(npmat4[:3, 3]), da.npmat3_to_pdquat(npmat4[:3, :3]))
def set_rotmat(self, npmat3): self._objpdnp.setQuat(da.npmat3_to_pdquat(npmat3))
def set_homomat(self, npmat4): self._objpdnp.setPosQuat(da.npv3_to_pdv3(npmat4[:3, 3]), da.npmat3_to_pdquat(npmat4[:3, :3])) mcd.update_pose(self._cdmesh, self._objpdnp)
def set_rotmat(self, npmat3): self._objpdnp.setQuat(da.npmat3_to_pdquat(npmat3)) mcd.update_pose(self._cdmesh, self._objpdnp)
def set_pose(self, pos: npt.NDArray = np.zeros(3), rotmat: npt.NDArray = np.eye(3)): self._objpdnp.setPosQuat(da.npv3_to_pdv3(pos), da.npmat3_to_pdquat(rotmat)) mcd.update_pose(self._cdmesh, self._objpdnp)
def set_rotmat(self, rotmat: npt.NDArray = np.eye(3)): self._objpdnp.setQuat(da.npmat3_to_pdquat(rotmat)) mcd.update_pose(self._cdmesh, self._objpdnp)
new_box.set_pos( np.array([random() * 10 - 5, random() * 10 - 5, 1 + random()])) new_box.set_rgba([random(), random(), random(), 1]) new_box.set_rotmat( rm.rotmat_from_euler(random() * math.pi / 4, random() * math.pi / 4, random() * math.pi / 4)) new_box.attach_to(base) # Create the body and set the mass boxBody = OdeBody(world) M = OdeMass() M.setBox(3, .3, .3, .3) boxBody.setMass(M) boxBody.setPosition(da.npv3_to_pdv3(new_box.get_pos())) boxBody.setQuaternion(da.npmat3_to_pdquat(new_box.get_rotmat())) # Create a BoxGeom boxGeom = OdeBoxGeom(space, .3, .3, .3) # boxGeom = OdeTriMeshGeom(space, OdeTriMeshData(new_box.objpdnp, True)) boxGeom.setCollideBits(BitMask32(0x00000002)) boxGeom.setCategoryBits(BitMask32(0x00000001)) boxGeom.setBody(boxBody) boxes.append((new_box, boxBody)) # Add a plane to collide with ground = cm.gen_box(extent=[20, 20, 1], rgba=[.3, .3, .3, 1]) ground.set_pos(np.array([0, 0, -1.5])) ground.attach_to(base) # groundGeom = OdeTriMeshGeom(space, OdeTriMeshData(ground.objpdnp, True)) groundGeom = OdePlaneGeom(space, Vec4(0, 0, 1, -1)) groundGeom.setCollideBits(BitMask32(0x00000001))