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 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, 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 simulationTask(task): space.autoCollide() # Setup the contact joints # Step the simulation and set the new positions world.step(globalClock.getDt()) for cmobj, body in boxes: cmobj.set_homomat( rm.homomat_from_posrot(da.npv3_to_pdv3(body.getPosition()), da.pdmat3_to_npmat3(body.getRotation()))) contactgroup.empty() # Clear the contact joints return task.cont
def update_pose(obj_bullet_rbd, objnp): """ update obj_bullet_rbd using the pos, nd quat of objnp :param obj_bullet_rbd: :param objnp: :return: author: weiwei date: 20211215 """ obj_bullet_rbd.setTransform( TransformState.makePosQuatScale(objnp.getPos(), objnp.getQuat(), da.npv3_to_pdv3(np.array([1, 1, 1]))))
def gen_plane_cdmesh(updirection=np.array([0, 0, 1]), offset=0, name='autogen'): """ generate a plane bulletrigidbody node :param updirection: the normal parameter of bulletplaneshape at panda3d :param offset: the d parameter of bulletplaneshape at panda3d :param name: :return: bulletrigidbody author: weiwei date: 20170202, tsukuba """ bulletplnode = BulletRigidBodyNode(name) bulletplshape = BulletPlaneShape(da.npv3_to_pdv3(updirection), offset) bulletplshape.setMargin(0) bulletplnode.addShape(bulletplshape) return bulletplnode
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()
sphere_b.set_pos([0, 1.25, -.7]) sphere_b.set_rgba([.3, .2, 1, 1]) sphere_b.attach_to(base) 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) # Setup our physics world and the body world = OdeWorld() world.setGravity(0, 0, -9.81) body_sphere_a = OdeBody(world) M = OdeMass() M.setSphere(7874, radius) body_sphere_a.setMass(M) body_sphere_a.setPosition(da.npv3_to_pdv3(sphere_a.get_pos())) body_sphere_b = OdeBody(world) M = OdeMass() M.setSphere(7874, radius) body_sphere_b.setMass(M) body_sphere_b.setPosition(da.npv3_to_pdv3(sphere_b.get_pos())) # Create the joints earth_a_jnt = OdeBallJoint(world) earth_a_jnt.attach(body_sphere_a, None) # Attach it to the environment earth_a_jnt.setAnchor(0, 0, 0) earth_b_jnt = OdeBallJoint(world) earth_b_jnt.attach(body_sphere_a, body_sphere_b) earth_b_jnt.setAnchor(0, .3, -.3)
def gen_frame_box(extent=[.02, .02, .02], homomat=np.eye(4), rgba=[0, 0, 0, 1], thickness=.001): """ draw a 3D box, only show edges :param extent: :param homomat: :return: """ M_TO_PIXEL = 3779.53 # Create a set of line segments ls = LineSegs() ls.setThickness(thickness * M_TO_PIXEL) ls.setColor(rgba[0], rgba[1], rgba[2], rgba[3]) center_pos = homomat[:3, 3] x_axis = homomat[:3, 0] y_axis = homomat[:3, 1] z_axis = homomat[:3, 2] x_min, x_max = -x_axis * extent[0] / 2, x_axis * extent[0] / 2 y_min, y_max = -y_axis * extent[1] / 2, y_axis * extent[1] / 2 z_min, z_max = -z_axis * extent[2] / 2, z_axis * extent[2] / 2 # max, max, max print(center_pos + np.array([x_max, y_max, z_max])) ls.moveTo(da.npv3_to_pdv3(center_pos + x_max + y_max + z_max)) ls.drawTo(da.npv3_to_pdv3(center_pos + x_max + y_max + z_min)) ls.drawTo(da.npv3_to_pdv3(center_pos + x_max + y_min + z_min)) ls.drawTo(da.npv3_to_pdv3(center_pos + x_max + y_min + z_max)) ls.drawTo(da.npv3_to_pdv3(center_pos + x_max + y_max + z_max)) ls.drawTo(da.npv3_to_pdv3(center_pos + x_min + y_max + z_max)) ls.drawTo(da.npv3_to_pdv3(center_pos + x_min + y_min + z_max)) ls.drawTo(da.npv3_to_pdv3(center_pos + x_min + y_min + z_min)) ls.drawTo(da.npv3_to_pdv3(center_pos + x_min + y_max + z_min)) ls.drawTo(da.npv3_to_pdv3(center_pos + x_min + y_max + z_max)) ls.moveTo(da.npv3_to_pdv3(center_pos + x_max + y_max + z_min)) ls.drawTo(da.npv3_to_pdv3(center_pos + x_min + y_max + z_min)) ls.moveTo(da.npv3_to_pdv3(center_pos + x_max + y_min + z_min)) ls.drawTo(da.npv3_to_pdv3(center_pos + x_min + y_min + z_min)) ls.moveTo(da.npv3_to_pdv3(center_pos + x_max + y_min + z_max)) ls.drawTo(da.npv3_to_pdv3(center_pos + x_min + y_min + z_max)) # Create and return a node with the segments lsnp = NodePath(ls.create()) lsnp.setTransparency(TransparencyAttrib.MDual) lsnp.setLightOff() ls_sgm = StaticGeometricModel(lsnp) return ls_sgm
def set_homomat(self, npmat4): self._objpdnp.setPosQuat(da.npv3_to_pdv3(npmat4[:3, 3]), da.npmat3_to_pdquat(npmat4[:3, :3]))
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_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)
new_box = box.copy() 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))