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