def aabb_contained_ratio(aabb1, aabb2) -> float:
"""Returns how much aabb2 is contained by aabb1."""
import pybullet
if isinstance(aabb1, int):
aabb1 = pybullet.getAABB(aabb1)
if isinstance(aabb2, int):
aabb2 = pybullet.getAABB(aabb2)
aabb1_min, aabb1_max = aabb1
aabb1_min = np.array(aabb1_min)
aabb1_max = np.array(aabb1_max)
aabb2_min, aabb2_max = aabb2
aabb2_min = np.array(aabb2_min)
aabb2_max = np.array(aabb2_max)
def get_volume(aabb_min, aabb_max):
aabb_extents = aabb_max - aabb_min
if np.any(aabb_extents <= 0):
return 0
return np.prod(aabb_extents)
volume_intersect = get_volume(
np.maximum(aabb1_min, aabb2_min), np.minimum(aabb1_max, aabb2_max),
)
volume2 = get_volume(aabb2_min, aabb2_max)
# volume1 = get_volume(aabb1_min, aabb1_max)
# iou = volume_intersect / (volume1 + volume2 - volume_intersect)
ratio = volume_intersect / volume2
if ratio < 0:
ratio = 0
return ratio
def get_handcraft_reward (self):
distance = sum ([(x - y) ** 2 for x, y in zip (self.start_pos, self.target_pos)]) ** 0.5
box = p.getAABB (self.box_id, -1)
box_center = [(x + y) * 0.5 for x, y in zip (box[0], box[1])]
obj = p.getAABB (self.obj_id, -1)
obj_center = [(x + y) * 0.5 for x, y in zip (obj[0], obj[1])]
aabb_dist = sum ([(x - y) ** 2 for x, y in zip (box_center, obj_center)]) ** 0.5
if self.opt.video_reward:
if ((self.seq_num-1)%self.opt.give_reward_num==self.opt.give_reward_num-1) \
and self.seq_num>=self.opt.cut_frame_num:
self.eval.get_caption()
rank,probability = self.eval.eval()
reward = probability
self.info += 'rank: {}\n'.format(rank)
self.eval.update(img_path=self.log_path,start_id=self.seq_num-1-self.opt.cut_frame_num)
else:
reward = 0
else:
if self.opt.reward_diff:
reward = (self.last_aabb_dist - aabb_dist) * 100
else:
reward = (0.5-aabb_dist)*100
# reward = (self.last_aabb_dist-aabb_dist)*100
self.last_aabb_dist = aabb_dist
# calculate whether it is done
self.info += 'now distance:{}\n'.format (distance)
self.info += 'AABB distance:{}\n'.format (aabb_dist)
# check whether the seqence number is out of limit
if self.seq_num >= self.max_seq_num:
done = True
else:
done = False
# check whether the object are out of order
for axis_dim in range (3):
if self.start_pos[axis_dim] < self.axis_limit[axis_dim][0] or \
self.start_pos[axis_dim] > self.axis_limit[axis_dim][1]:
done = True
reward = self.opt.out_reward
# check whether the object is still in the gripper
left_closet_info = p.getContactPoints (self.robotId, self.obj_id, 13, -1)
right_closet_info = p.getContactPoints (self.robotId, self.obj_id, 17, -1)
if self.opt.obj_away_loss:
if len (left_closet_info)==0 and len (right_closet_info)==0:
done = True
# let the model learn the reward automatically, so delete the following line
reward = self.opt.away_reward
self.info += 'reward: {}\n\n'.format (reward)
self.log_info.write (self.info)
print (self.info)
return self.observation, reward, done, self.info
def get_put_on_pose(self, topid, bottomid):
bot_pose = pyplan.get_point(bottomid)
aabb = p.getAABB(bottomid)
botheight = aabb[1][2] - aabb[0][2]
top_pose = list(bot_pose)
top_pose[2] += botheight / 1.8
aabb = p.getAABB(topid)
topheight = aabb[1][2] - aabb[0][2]
top_pose[2] += topheight / 1.8
return top_pose
def debug_get_full_aabb(sim_id: int, robot_id: int):
"""
Get full AABB of a robot including all of its constituent links.
"""
num_joints = pb.getNumJoints(robot_id, physicsClientId=sim_id)
aabb = np.asarray(pb.getAABB(robot_id, -1, physicsClientId=sim_id),
dtype=np.float32)
for i in range(num_joints):
link_aabb = pb.getAABB(robot_id, i, physicsClientId=sim_id)
aabb[0] = np.minimum(aabb[0], link_aabb[0], out=aabb[0])
aabb[1] = np.maximum(aabb[1], link_aabb[1], out=aabb[1])
return aabb
def get_handcraft_reward(self):
distance = sum([(x - y)**2
for x, y in zip(self.start_pos, self.target_pos)])**0.5
box = p.getAABB(self.box_id, -1)
box_center = [(x + y) * 0.5 for x, y in zip(box[0], box[1])]
obj = p.getAABB(self.obj_id, -1)
obj_center = [(x + y) * 0.5 for x, y in zip(obj[0], obj[1])]
aabb_dist = sum([(x - y)**2
for x, y in zip(box_center, obj_center)])**0.5
if self.opt.reward_diff:
reward = (self.last_aabb_dist - aabb_dist) * 100
else:
reward = (0.5 - aabb_dist) * 100
# calculate whether it is done
self.info += 'now distance:{}\n'.format(distance)
self.info += 'AABB distance:{}\n'.format(aabb_dist)
if self.seq_num >= self.max_seq_num:
done = True
else:
done = False
# check whether the object are out of order
for axis_dim in range(3):
if self.start_pos[axis_dim] < self.axis_limit[axis_dim][0] or \
self.start_pos[axis_dim] > self.axis_limit[axis_dim][1]:
done = True
reward = self.opt.out_reward
# check whether the object is still in the gripper
left_closet_info = p.getContactPoints(self.robotId, self.obj_id, 13,
-1)
right_closet_info = p.getContactPoints(self.robotId, self.obj_id, 17,
-1)
if self.opt.obj_away_loss:
if len(left_closet_info) == 0 and len(right_closet_info) == 0:
done = True
# let the model learn the reward automatically, so delete the following line
# reward = self.opt.away_reward
# if aabb_dist<self.opt.end_distance and abs(max(box[0][2],box[1][2])-min(obj[0][2],obj[1][2]))<0.05:
if aabb_dist < self.opt.end_distance:
done = True
if (not self.opt.video_reward):
reward = 100
# reward = -1
self.info += 'reward: {}\n\n'.format(reward)
self.log_info.write(self.info)
print(self.info)
return self.observation, reward, done, self.info
def get_full_aabb(obj):
aabb = np.array(p.getAABB(obj.pid))
aabb_mins = [aabb[:1, :]]
aabb_maxs = [aabb[1:, :]]
for i in range(p.getNumJoints(obj.pid)):
aabb = np.array(p.getAABB(obj.pid, i))
aabb_mins.append(aabb[:1, :])
aabb_maxs.append(aabb[1:, :])
aabb_mins = np.vstack(aabb_mins)
aabb_maxs = np.vstack(aabb_maxs)
amin = np.amin(aabb_mins, axis=0)
amax = np.amax(aabb_maxs, axis=0)
return np.array([amin, amax])
def generateHorseShoeinPath():
ObstacleIDs = []
ObstacleIDs.append(
pbl.loadURDF("/Misc/HorseShoeinPath/blockback.urdf", [0, 5, 3]))
bbox = pbl.getAABB(ObstacleIDs[-1])
dims = np.subtract(bbox[1], bbox[0])
disk = Rectangle(bbox[0][0::2], dims[0], dims[2], color='r', fill=False)
ax.add_patch(disk)
art3d.pathpatch_2d_to_3d(disk, z=bbox[1][1], zdir='y')
ObstacleIDs.append(
pbl.loadURDF("/Misc/HorseShoeinPath/blockSide.urdf", [0, 3, 5]))
bbox = pbl.getAABB(ObstacleIDs[-1])
dims = np.subtract(bbox[1], bbox[0])
disk = Rectangle(bbox[0][0:2], dims[0], dims[1], color='r', fill=False)
ax.add_patch(disk)
art3d.pathpatch_2d_to_3d(disk, z=bbox[1][2], zdir='z')
ObstacleIDs.append(
pbl.loadURDF("/Misc/HorseShoeinPath/blockSide.urdf", [0, 3, 1]))
bbox = pbl.getAABB(ObstacleIDs[-1])
dims = np.subtract(bbox[1], bbox[0])
disk = Rectangle(bbox[0][0:2], dims[0], dims[1], color='r', fill=False)
ax.add_patch(disk)
art3d.pathpatch_2d_to_3d(disk, z=bbox[0][2], zdir='z')
ObstacleIDs.append(
pbl.loadURDF("/Misc/HorseShoeinPath/blockSideActual.urdf", [2, 3, 3]))
bbox = pbl.getAABB(ObstacleIDs[-1])
dims = np.subtract(bbox[1], bbox[0])
disk = Rectangle(bbox[0][1:], dims[1], dims[2], color='r', fill=False)
ax.add_patch(disk)
art3d.pathpatch_2d_to_3d(disk, z=bbox[1][0], zdir='x')
ObstacleIDs.append(
pbl.loadURDF("/Misc/HorseShoeinPath/blockSideActual.urdf", [-2, 3, 3]))
bbox = pbl.getAABB(ObstacleIDs[-1])
dims = np.subtract(bbox[1], bbox[0])
disk = Rectangle(bbox[0][1:], dims[1], dims[2], color='r', fill=False)
ax.add_patch(disk)
art3d.pathpatch_2d_to_3d(disk, z=bbox[0][0], zdir='x')
ObstacleIDs.append(pbl.loadURDF('plane.urdf'))
return ObstacleIDs
def get_object_pointcloud(self, obid):
cloud = []
aabb = p.getAABB(obid)
minx, miny, minz = aabb[0]
maxx, maxy, maxz = aabb[1]
num_x = max(int((maxx - minx) * 50), 10)
xs = np.linspace(minx, maxx, num=num_x)
for px in xs:
cloud.append([px, miny, minz])
cloud.append([px, maxy, maxz])
cloud.append([px, miny, maxz])
cloud.append([px, maxy, minz])
num_y = max(int((maxy - miny) * 50), 10)
ys = np.linspace(miny, maxy, num=num_y)
for py in ys:
cloud.append([minx, py, minz])
cloud.append([maxx, py, maxz])
cloud.append([minx, py, maxz])
cloud.append([maxx, py, minz])
num_z = max(int((maxz - minz) * 50), 10)
zs = np.linspace(minz, maxz, num=num_z)
for pz in zs:
cloud.append([minx, miny, pz])
cloud.append([maxx, maxy, pz])
cloud.append([minx, maxy, pz])
cloud.append([maxx, miny, pz])
return cloud
def step_3d_simulation_shooter(player_body,
bullet_body,
goals,
bodies,
nsteps=1,
physicsClientId=0):
done = False
hit = False
reward = 0
for i in range(nsteps):
p.stepSimulation(physicsClientId=physicsClientId)
bullet_aabb = p.getAABB(bullet_body, physicsClientId=physicsClientId)
obs = p.getOverlappingObjects(*bullet_aabb,
physicsClientId=physicsClientId)
if obs is not None:
obs = [ob[0] for ob in obs]
for ob in obs:
if ob in goals:
x, y = np.random.uniform(0, 10, size=(2))
vx, vy = np.random.uniform(-20.0, 20.0, size=[2])
p.resetBasePositionAndOrientation(
ob, [x, y, 3.0], [0, 0, 0, 1],
physicsClientId=physicsClientId)
p.resetBaseVelocity(ob, [vx, vy, 0],
physicsClientId=physicsClientId)
reward += 1
hit = True
elif ob in bodies:
x, y = np.random.uniform(0, 10, size=(2))
vx, vy = np.random.uniform(-20.0, 20.0, size=[2])
p.resetBasePositionAndOrientation(
ob, [x, y, 3.0], [0, 0, 0, 1],
physicsClientId=physicsClientId)
p.resetBaseVelocity(ob, [vx, vy, 0],
physicsClientId=physicsClientId)
reward -= 1
hit = True
if hit:
p.resetBasePositionAndOrientation(bullet_body, [30, 30, 0.5],
[0, 0, 0, 1],
physicsClientId=physicsClientId)
vel, _ = p.getBaseVelocity(player_body, physicsClientId=physicsClientId)
vel = vel[:2]
tot = sum([abs(v) for v in list(vel)])
if tot > 1:
done = True
else:
done = False
return done, reward
def _set_size(self, pybullet_client_ids, position, orientation):
"""
:param pybullet_client_ids: (list) specifies the pybullet clients.
:param position: (list float) specifies the positions in x,y,z
:param orientation: (list float) specifies the quaternion of the goal.
:return: (list) the size of the bounding box of the object.
"""
temp_shape_id = pybullet.createCollisionShape(
shapeType=pybullet.GEOM_MESH,
meshScale=self._scale,
fileName=self._filename,
physicsClientId=pybullet_client_ids[0])
temp_block_id = pybullet.createMultiBody(
baseCollisionShapeIndex=temp_shape_id,
basePosition=position,
baseOrientation=orientation,
baseMass=0.1,
physicsClientId=pybullet_client_ids[0])
bb = pybullet.getAABB(temp_block_id,
physicsClientId=pybullet_client_ids[0])
size = np.array([(bb[1][0] - bb[0][0]), (bb[1][1] - bb[0][1]),
(bb[1][2] - bb[0][2])])
pybullet.removeBody(temp_block_id,
physicsClientId=pybullet_client_ids[0])
return size
def init_grasp(self):
self.robot.gripperControl(255)
pos_traj = np.load (os.path.join (self.env_root, 'init', 'pos.npy'))
orn_traj = np.load (os.path.join (self.env_root, 'init', 'orn.npy'))
self.fix_orn = np.load (os.path.join (self.env_root, 'init', 'orn.npy'))
for j in range (7):
p.resetJointState(self.robotId, j, self.data_q[0][j], self.data_dq[0][j])
for init_t in range(100):
box = p.getAABB(self.obj_id,-1)
center = [(x+y)*0.5 for x,y in zip(box[0],box[1])]
center[0] -= 0.05
center[1] -= 0.05
center[2] += 0.03
# center = (box[0]+box[1])*0.5
points = np.array ([pos_traj[0], center])
start_id = 0
init_traj = point2traj(points)
start_id = self.move(init_traj,orn_traj,start_id)
p.stepSimulation()
self.start_pos = p.getLinkState (self.robotId, 7)[0]
def get_handcraft_reward(self):
distance = sum([(x - y)**2
for x, y in zip(self.start_pos, self.target_pos)])**0.5
obj = p.getAABB(self.obj_id, -1)
obj_center = [(x + y) * 0.5 for x, y in zip(obj[0], obj[1])]
aabb_dist = sum([(x)**2 for x in obj_center])**0.5
reward = (0.5 - aabb_dist) * 10000
# calculate whether it is done
self.info += 'now distance:{}\n'.format(distance)
self.info += 'AABB distance:{}\n'.format(aabb_dist)
if self.seq_num >= self.max_seq_num:
done = True
else:
done = False
for axis_dim in range(3):
if self.start_pos[axis_dim] < self.axis_limit[axis_dim][0] or \
self.start_pos[axis_dim] > self.axis_limit[axis_dim][1]:
done = True
reward = -10000
if aabb_dist < 0.1:
done = True
reward = 10000
# reward = -1
self.info += 'reward: {}\n\n'.format(reward)
self.log_info.write(self.info)
print(self.info)
return self.observation, reward, done, 1 #self.info
def aabb_info(self):
"""Returns information on the aligned axis bounding box of the body."""
aabb = p.getAABB(self.bid)
x_min = aabb[0][0]
x_max = aabb[1][0]
y_min = aabb[0][1]
y_max = aabb[1][1]
z_min = aabb[0][2]
z_max = aabb[1][2]
long_edge_length = abs(max(x_max - x_min, y_max - y_min))
short_edge_length = abs(min(x_max - x_min, y_max - y_min))
diagonal_length = np.sqrt(((x_max - x_min) ** 2) + ((y_max - y_min) ** 2))
return {
"min x": x_min,
"max x": x_max,
"min y": y_min,
"max y": y_max,
"min z": z_min,
"max z": z_max,
"2D long edge length": long_edge_length,
"2D short edge length": short_edge_length,
"2D diagonal length": diagonal_length,
}
def reset(obj_id):
# remove if already exists
if obj_id is not None:
p.removeBody(obj_id)
# select rand file
rand_file = random.choice(model_list)
# load obj
print('Loading: ', rand_file)
# flags = p.URDF_INITIALIZE_SAT_FEATURES
obj_id = p.loadURDF(os.path.join(data_path, rand_file, "model.urdf"),
start_pos)
if auto_scale:
# Could use trimesh to get bounding box before loading in pybullet
# to avoid remove/reload
targ_diag = 0.15
obj_aabb = p.getAABB(obj_id)
aabb_min, aabb_max = obj_aabb[0], obj_aabb[1]
long_diag = np.linalg.norm(np.array(aabb_min) - np.array(aabb_max))
ratio = targ_diag / long_diag
p.removeBody(obj_id)
obj_id = p.loadURDF(
os.path.join(data_path, rand_file, "model.urdf"),
start_pos,
# flags=flags,
globalScaling=ratio)
return obj_id
def check_collisions(self):
"""Проверяет объект робота на пересечения с объектами внешней
среды и самопересечения
Возвращает true, если существует хотя бы одно пересечение
Возвращает false, если пересечений нет"""
# множество объектов, для которых необходима обработка
# в узкой фазе
obj_set = set()
# широкая фаза
for i in range(-1, pb.getNumJoints(self._id, physicsClientId=self.s)):
# координаты AABB
box_min, box_max = pb.getAABB(self._id, i, physicsClientId=self.s)
# getOverlappingObjects возвращает кортежи,
# первый элемент которых -- id объекта,
# а второй -- id "звеньев" объекта
# нужен только первый элемент
ids = [
obj[0] for obj in pb.getOverlappingObjects(
box_min, box_max, physicsClientId=self.s)
]
obj_set |= set(ids)
# узкая фаза
contacts = []
for o in obj_set:
contacts += pb.getContactPoints(self._id,
o,
physicsClientId=self.s)
return len(contacts) > 0
def load_obj(self, path, pos, yaw, mod_orn=False, mod_stiffness=False):
orn = p.getQuaternionFromEuler([0, 0, yaw])
obj_id = p.loadURDF(path, pos, orn)
# adjust position according to height
aabb = p.getAABB(obj_id, -1)
if mod_orn:
minm, maxm = aabb[0][1], aabb[1][1]
orn = p.getQuaternionFromEuler([0, np.pi * 0.5, yaw])
else:
minm, maxm = aabb[0][2], aabb[1][2]
pos[2] += (maxm - minm) / 2
p.resetBasePositionAndOrientation(obj_id, pos, orn)
# change dynamics
if mod_stiffness:
p.changeDynamics(obj_id,
-1,
lateralFriction=1,
rollingFriction=0.001,
spinningFriction=0.002,
restitution=0.01,
contactStiffness=100000,
contactDamping=0.0)
else:
p.changeDynamics(obj_id,
-1,
lateralFriction=1,
rollingFriction=0.002,
spinningFriction=0.001,
restitution=0.01)
self.obj_ids.append(obj_id)
self.obj_positions.append(pos)
self.obj_orientations.append(orn)
return obj_id, pos, orn
def init_space(self):
import pybullet
cad = morefusion.extra.trimesh.bin_model(
extents=self._extents,
thickness=self._thickness,
)
cad_file = self._temp_dir / f"{cad.md5()}.obj"
if not cad_file.exists():
trimesh.exchange.export.export_mesh(cad, str(cad_file))
unique_id = morefusion.extra.pybullet.add_model(
visual_file=cad_file,
collision_file=morefusion.utils.get_collision_file(cad_file),
position=(0, 0, self._extents[2] / 2),
base_mass=10,
)
self._simulate(nstep=100)
aabb_min, aabb_max = pybullet.getAABB(unique_id)
aabb_min, aabb_max = self._shrink_aabb(aabb_min, aabb_max, ratio=0.1)
aabb_max[2] *= 1.1
self._objects[unique_id] = dict(class_id=0, cad_file=cad_file)
self._aabb = aabb_min, aabb_max
def get_top_grasp(self, object_id):
aabb = p.getAABB(object_id)
height = aabb[1][2] - aabb[0][2]
position, _ = pyplan.get_pose(object_id)
position = list(position)
position[2] += (height / 1.6)
orientation = p.getQuaternionFromEuler((0, 1.57, 0))
return position, orientation
def setup(use_gui):
#TODO: get rid of random sampling
n_obj = 0
n_stack = 0
connect_type = p.GUI if use_gui else p.DIRECT
p.connect(connect_type)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0., 0., -10.)
plane, table, robot = init_world()
table_aabb = np.array(p.getAABB(table)) - np.divide(
((-DIM, -DIM, 0), (DIM, DIM, 0)), 2)
reject_aabbs = [get_full_aabb(robot)]
objects, mass_bool = gen_objects(table_aabb, 1, reject_aabbs)
push_obj = objects[0]
region_wlh = (DIM, DIM, 0)
region = rejection_sample_region(table_aabb, region_wlh,
[p.getAABB(push_obj.pid)])
#goal_pos = sample_aabb(np.array(region.aabb))
push_vec = np.subtract((0.68, 0.3, 0.67), push_obj.pose.pos)
yaw = get_yaw(push_vec)
goal_ori = eul_to_quat((0, 0, yaw))
goal_pose = Pose(((0.68, 0.3, 0.67), goal_ori))
visual_id = p.createVisualShape(p.GEOM_BOX,
halfExtents=[0.05, 0.05, 0.05],
rgbaColor=(1, 0, 0, 0.75))
goal_block_id = p.createMultiBody(
baseVisualShapeIndex=visual_id,
basePosition=list(goal_pose.pos),
)
start_pose = Pose((push_obj.pose.pos, goal_ori))
from_conf = robot.conf
set_pose(push_obj, start_pose)
objects.extend(gen_stack(push_obj, n_stack))
objects.extend(gen_objects(table_aabb, n_obj, reject_aabbs))
step_simulation(steps=20)
end_poses = [get_pose(obj) for obj in objects]
full_pose = end_poses + [from_conf]
return full_pose, robot, objects, use_gui, goal_pose, mass_bool
def get_side_grasp(self, object_id):
aabb = p.getAABB(object_id)
width = aabb[1][0] - aabb[0][0]
position, _ = pyplan.get_pose(object_id)
position = list(position)
position[0] -= (width / 2.0)
orientation = p.getQuaternionFromEuler((1.57, 0, 0))
return position, orientation
def __init__(self, ID, plane=False, sensorNoise=0):
self.ID = ID
self.plane = plane
self.sensorNoise = sensorNoise
self.bbox = np.subtract(
pbl.getAABB(self.ID)[1],
pbl.getAABB(self.ID)[0])
self.bbox2 = pbl.getAABB(self.ID)
self.pos, orient = pbl.getBasePositionAndOrientation(ID)
self.distance = np.inf
self.angle = 0
self.pos = np.array(self.pos)
self.xdot = np.zeros(3)
def get_bounding_box(self):
bb_min, bb_max = p.getAABB(self.id)
bb_min = np.array(bb_min)
bb_max = np.array(bb_max)
# sometimes the bounding box is below plane, which causes trouble.
# see https://github.com/bulletphysics/bullet3/issues/2820
bb_min[2] = 0
return bb_min, bb_max
def get_obstacles(objects):
margin = GRIPPER_Y / 2.
obstacles = list()
for obj in objects:
obj_aabb = p.getAABB(obj.pid)
obstacles.append((obj_aabb[0][0] - margin, obj_aabb[0][1] - margin,
obj_aabb[1][0] + margin, obj_aabb[1][1] + margin))
return tuple(obstacles)
def check_target_reached(self, obj_id):
aabb = p.getAABB(self.target_id, -1)
x_min, x_max = aabb[0][0], aabb[1][0]
y_min, y_max = aabb[0][1], aabb[1][1]
pos = p.getBasePositionAndOrientation(obj_id)
x, y = pos[0][0], pos[0][1]
if x > x_min and x < x_max and y > y_min and y < y_max:
return True
return False
def CheckContact(self, player):
aabbMin, aabbMax = p.getAABB(player.colId, -1)
collision_list = p.getOverlappingObjects(aabbMin, aabbMax)
if collision_list is not None and len(collision_list) != 0:
for objId in collision_list:
colId = objId[0]
if colId != 0 and colId in self.finalScene.activatedBodies:
self.firstFinalContact = True
break
def get_AABB(self, linkId=None):
"""Returns the bounding box of a named link.
:type linkId: str, NoneType
:rtype: AABB
"""
res = pb.getAABB(self.__bulletId,
self.link_index_map[linkId],
physicsClientId=self.__client_id)
return AABB(Vector3(*res[0]), Vector3(*res[1]))
def checkContact(self, obj_list):
aabbMin, aabbMax = p.getAABB(self.colId, -1)
collision_list = p.getOverlappingObjects(aabbMin, aabbMax)
if collision_list is not None and len(collision_list) != 0:
for objId in collision_list:
obj = obj_list.get(str(objId[0]))
if objId[0] != self.colId and objId[
0] != self.lastContactId and obj is not None:
self.lastContactId = objId[0]
self.onContact(obj)
def _isHitEverything(self, drone_id):
a, b = p.getAABB(drone_id, physicsClientId=self.CLIENT
) # Melihat batas posisi collision drone ke i
list_obj = p.getOverlappingObjects(
a, b, physicsClientId=self.CLIENT
) # Melihat objek2 yang ada di batas posisi collision
if (list_obj != None
and len(list_obj) > 6): # 1 Quadcopter memiliki 6 link/bagian
# print("Drone {}: _isHitEverything".format(drone_id))
return True
return False
def get_overlapping_nodes(self, scene_node, margin=0.0):
if scene_node.id in self.nodes_map:
nodes_overlapping = []
sim_id = self.entity_id_map[scene_node.id]
aabb = p.getAABB(sim_id)
sim_ids_overlapping = p.getOverlappingObjects(aabb[0], aabb[1])
for sim_id in sim_ids_overlapping:
object_id = self.reverse_entity_id_map[sim_id]
nodes_overlapping.append(self.nodes_map[object_id])
return True, nodes_overlapping
return False, []
def get_overlapping_objects(body_or_link):
""" Return all the unique ids of objects that have axis aligned
bounding box overlap with a axis aligned bounding box of
given body/link. """
client_id = body_or_link.client_id
kwargs = _compute_args(body_or_link, 'bodyUniqueId', 'linkIndex')
aa, bb = pb.getAABB(physicsClientId=client_id, **kwargs)
obs = pb.getOverlappingObjects(aa, bb, physticsClient=client_id)
if obs is None:
return []
return [link.Link(o[0], o[1]) for o in obs]
f = [aabbMin[0], aabbMax[1], aabbMin[2]]
t = [aabbMin[0], aabbMax[1], aabbMax[2]]
p.addUserDebugLine(f, t, [1, 1, 1])
f = [aabbMax[0], aabbMax[1], aabbMax[2]]
t = [aabbMin[0], aabbMax[1], aabbMax[2]]
p.addUserDebugLine(f, t, [1.0, 0.5, 0.5])
f = [aabbMax[0], aabbMax[1], aabbMax[2]]
t = [aabbMax[0], aabbMin[1], aabbMax[2]]
p.addUserDebugLine(f, t, [1, 1, 1])
f = [aabbMax[0], aabbMax[1], aabbMax[2]]
t = [aabbMax[0], aabbMax[1], aabbMin[2]]
p.addUserDebugLine(f, t, [1, 1, 1])
aabb = p.getAABB(r2d2)
aabbMin = aabb[0]
aabbMax = aabb[1]
if (printtext):
print(aabbMin)
print(aabbMax)
if (draw == 1):
drawAABB(aabb)
for i in range(p.getNumJoints(r2d2)):
aabb = p.getAABB(r2d2, i)
aabbMin = aabb[0]
aabbMax = aabb[1]
if (printtext):
print(aabbMin)
print(aabbMax)
