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