def __init__(self):
'''
Initialize a mesh-mesh collision manager.
'''
if not _fcl_exists:
raise ValueError('No FCL Available!')
self._objs = {}
self._manager = fcl.DynamicAABBTreeCollisionManager()
self._manager.setup()
def __init__(self, x_min, x_max, v_min, v_max, Pset):
self.x_min = x_min
self.x_max = x_max
self.v_min = v_min
self.v_max = v_max
self.Pset = Pset
self.fcl_manager = fcl.DynamicAABBTreeCollisionManager()
objs = []
for p in self.Pset:
objs.append(self.rec2fcl(p))
self.fcl_manager.registerObjects(objs)
self.fcl_manager.setup()
def __init__(self):
"""
Initialize a mesh-mesh collision manager.
"""
if not _fcl_exists:
raise ValueError('No FCL Available!')
# {name: {geom:, obj}}
self._objs = {}
# {id(bvh) : str, name}
# unpopulated values will return None
self._names = collections.defaultdict(lambda: None)
# cache BVH objects
# {mesh.md5(): fcl.BVHModel object}
self._bvh = {}
self._manager = fcl.DynamicAABBTreeCollisionManager()
self._manager.setup()
def checkLidarCollistion(lidar, verts, tris):
mesh = fcl.BVHModel()
mesh.beginModel(len(verts), len(tris))
mesh.addSubModel(verts, tris)
mesh.endModel()
mesh_obj = fcl.CollisionObject(mesh)
objs = [mesh_obj, lidar]
manager = fcl.DynamicAABBTreeCollisionManager()
manager.registerObjects(objs)
manager.setup()
crequest = fcl.CollisionRequest(enable_contact=True)
drequest = fcl.DistanceRequest(enable_nearest_points=True)
cdata = fcl.CollisionData(crequest, fcl.CollisionResult())
ddata = fcl.DistanceData(drequest, fcl.DistanceResult())
manager.collide(cdata, fcl.defaultCollisionCallback)
manager.distance(ddata, fcl.defaultDistanceCallback)
minimum_distance = ddata.result.min_distance
return minimum_distance
def test_many_objects(self):
manager = fcl.DynamicAABBTreeCollisionManager()
objs = [
fcl.CollisionObject(self.geometry['sphere']),
fcl.CollisionObject(self.geometry['sphere'],
fcl.Transform(np.array([0.0, 0.0, -5.0]))),
fcl.CollisionObject(self.geometry['sphere'],
fcl.Transform(np.array([0.0, 0.0, 5.0])))
]
manager.registerObjects(objs)
manager.setup()
self.assertTrue(len(manager.getObjects()) == 3)
# Many-to-many, internal
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager.collide(cdata, fcl.defaultCollisionCallback)
self.assertFalse(cdata.result.is_collision)
objs[1].setTranslation(np.array([0.0, 0.0, -0.3]))
manager.update(objs[1])
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager.collide(cdata, fcl.defaultCollisionCallback)
self.assertTrue(cdata.result.is_collision)
objs[1].setTranslation(np.array([0.0, 0.0, -5.0]))
manager.update(objs[1])
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager.collide(cdata, fcl.defaultCollisionCallback)
self.assertFalse(cdata.result.is_collision)
objs[2].setTranslation(np.array([0.0, 0.0, 0.3]))
manager.update(objs[2])
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager.collide(cdata, fcl.defaultCollisionCallback)
self.assertTrue(cdata.result.is_collision)
objs[2].setTranslation(np.array([0.0, 0.0, 5.0]))
manager.update(objs[2])
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager.collide(cdata, fcl.defaultCollisionCallback)
self.assertFalse(cdata.result.is_collision)
def obs2fcl(obs, dimW):
obs_fcl = []
for i in range(0, obs.shape[0] // (2 * dimW)): # plot obstacle patches
width = obs[i * 2 * dimW + dimW] - obs[i * 2 * dimW]
height = obs[i * 2 * dimW + dimW + 1] - obs[i * 2 * dimW + 1]
# width = height = 1
ob = fcl.Box(width, height, 1)
x = obs[i * 2 * dimW] + width/2
y = obs[i * 2 * dimW + 1] + height/2
# x = y = 0
T = np.array([x, y, 0])
t = fcl.Transform(T)
co = fcl.CollisionObject(ob, t)
obs_fcl.append(co)
# print("x: ", x, " y: ", y, " width: ", width, " height: ", height)
obs_manager = fcl.DynamicAABBTreeCollisionManager()
obs_manager.registerObjects(obs_fcl)
obs_manager.setup()
return obs_manager
def _distance_check(tube_manager, environment):
"""Checks distance between given collision manager and object list.
Parameters
----------
tube_manager : DynamicAABBTreeCollisionManager
environment : list of CollisionObjects
Returns
-------
float
minimum distance between the two collections of collision objects
"""
env_manager = fcl.DynamicAABBTreeCollisionManager()
env_manager.registerObjects(environment)
env_manager.setup()
data = fcl.DistanceData()
tube_manager.distance(env_manager, data, fcl.defaultDistanceCallback)
return data.result.min_distance
def CheckCollison(rotate_direction, verts, tris, stick, rota_degree, trans_x,
trans_y, tolerance):
global previous_minimum_distance
mesh = fcl.BVHModel()
mesh.beginModel(len(verts), len(tris))
mesh.addSubModel(verts, tris)
mesh.endModel()
mesh_obj = fcl.CollisionObject(mesh)
objs = [mesh_obj, stick]
manager = fcl.DynamicAABBTreeCollisionManager()
manager.registerObjects(objs)
manager.setup()
crequest = fcl.CollisionRequest(enable_contact=True)
drequest = fcl.DistanceRequest(enable_nearest_points=True)
cdata = fcl.CollisionData(crequest, fcl.CollisionResult())
ddata = fcl.DistanceData(drequest, fcl.DistanceResult())
manager.collide(cdata, fcl.defaultCollisionCallback)
manager.distance(ddata, fcl.defaultDistanceCallback)
#print("Is collision: ", cdata.result.is_collision)
minimum_distance = ddata.result.min_distance
nearest_point = ddata.result.nearest_points[0][:2]
nearest_point_stick = ddata.result.nearest_points[1]
nearest_point_stick = Transform(rota_degree, trans_x, trans_y,
nearest_point_stick)
collision = -10000
if previous_minimum_distance != -10000:
if minimum_distance == -1:
collision = -1
print("The tip of the stick is inside the soft tissues.")
elif minimum_distance >= tolerance:
collision = 0
print("No collision")
elif minimum_distance >= 0 and minimum_distance < tolerance and minimum_distance < previous_minimum_distance:
print("Collision")
collision = 1
# if nearest_point in verts:
# print("nearest point of the stick:", nearest_point_stick)
tmp = previous_minimum_distance
previous_minimum_distance = minimum_distance
return nearest_point, collision, minimum_distance
def check_collision(self, curve, rad):
"""Determine if the curve given collides with obstacles or goal.
The curve is translated to discretized cylinders with the given radius
and checked for collisions with the obstacles and goal. Minimum distance
to obstacles and tip distance to goal are returned, with a negative
value denoting a collision.
Parameters
----------
curve : list of list of 4x4 numpy arrays
The SE3 g values for each curve
rad : list of float
radii of the tubes
Returns
-------
float
minimum distance between curve and obstacles
float
minimum distance between curve and goal
"""
tube = self._build_tube(curve, rad)
tube_manager = fcl.DynamicAABBTreeCollisionManager()
tube_manager.registerObjects(tube)
tube_manager.setup()
obstacle_min = self._distance_check(tube_manager, self.obstacles)
s = fcl.Sphere(rad[-1])
final_point = curve[-1][-1][0:3, 3]
t = fcl.Transform(final_point) # coordinates of last point of tube
tip = fcl.CollisionObject(s, t)
request = fcl.DistanceRequest()
result = fcl.DistanceResult()
goal_dist = fcl.distance(tip, self.goal, request, result)
return obstacle_min, goal_dist
def isValid(self, q_set):
# check validity for multiple points.
# will be used for piecewize path consited of multiple points
# for q in q_set:
# if not self.isValidPoint(q):
# return False
# return True
if len(q_set.shape) < 2:
return self.isValidPoint(q_set)
manager_q = fcl.DynamicAABBTreeCollisionManager()
qs = []
for q in q_set:
qs.append(self.point2fcl(q))
manager_q.registerObjects(qs)
req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
rdata = fcl.CollisionData(request=req)
self.fcl_manager.collide(manager_q, rdata,
fcl.defaultCollisionCallback)
# print('Collision between manager 1 and Mesh?: {}'.format(rdata.result.is_collision))
# print('Contacts:')
# for c in rdata.result.contacts:
# print('\tO1: {}, O2: {}'.format(c.o1, c.o2))
return not rdata.result.is_collision
box1 = fcl.CollisionObject(fcl.Box(1, 1, 0.0), Transform) #x,y,z length center at the origin
#box.setTranslation(np.array([0.0, 1.05000, 0.0])) #useful
#box.setRotation(rotation) #useful
#other options: setTransform setQuatRotation
rota_degree = 0
rotation = np.array([[np.cos(rota_degree/180*np.pi), -np.sin(rota_degree/180*np.pi), 0.0],
[np.sin(rota_degree/180*np.pi), np.cos(rota_degree/180*np.pi), 0.0],
[0.0, 0.0, 1.0]])
translation = np.array([1.4999, 0, 0.0])
Transform = fcl.Transform(rotation, translation)
box2 = fcl.CollisionObject(fcl.Box(3, 3, 0.0), Transform) #x,y,z length center at the origin
cylinder = fcl.CollisionObject(fcl.Cylinder(1, 0.0),Transform) #radius = 1
objs = [box1,cylinder]
manager = fcl.DynamicAABBTreeCollisionManager()
manager.registerObjects(objs)
manager.setup()
crequest = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
cdata = fcl.CollisionData(crequest, fcl.CollisionResult())
manager.collide(cdata, fcl.defaultCollisionCallback)
print(cdata.result.contacts)
for contact in cdata.result.contacts:
print(contact.pos)
print(contact.normal)
print(contact.penetration_depth)
def generate_one(robot,
obs_num,
folder,
label_type='binary',
num_class=None,
num_points=8000,
env_id='',
vis=True):
obstacles = []
types = ['rect', 'circle']
link_length = robot.link_length[0].item()
for i in range(obs_num):
obs_t = types[randint(2)]
if types[0] in obs_t: # rectangle, size = 0.5-3.5, pos = -7~7
while True:
s = rand(2) * 3 + 0.5
if obs_num <= 2:
p = rand(2) * 10 - 5
else:
p = rand(2) * 14 - 7
if any(p - s / 2 > link_length) or any(
p +
s / 2 < -link_length): # the near-origin area is clear
break
elif types[1] in obs_t: # circle, size = 0.25-2, pos = -7~7
while True:
s = rand() * 1.75 + 0.25
if obs_num <= 2:
p = rand(2) * 10 - 5
else:
p = rand(2) * 14 - 7
if np.linalg.norm(p) > s + link_length:
break
obstacles.append((obs_t, p, s))
fcl_obs = [FCLObstacle(*param) for param in obstacles]
fcl_collision_obj = [fobs.cobj for fobs in fcl_obs]
# geom2instnum = {id(g): i for i, (_, g) in enumerate(fcl_obs)}
cfgs = torch.rand((num_points, robot.dof), dtype=torch.float32)
cfgs = cfgs * (robot.limits[:, 1] - robot.limits[:, 0]) + robot.limits[:,
0]
if label_type == 'binary':
labels = torch.zeros(num_points, 1, dtype=torch.float)
dists = torch.zeros(num_points, 1, dtype=torch.float)
obs_managers = [fcl.DynamicAABBTreeCollisionManager()]
obs_managers[0].registerObjects(fcl_collision_obj)
obs_managers[0].setup()
elif label_type == 'instance':
labels = torch.zeros(num_points, len(obstacles), dtype=torch.float)
dists = torch.zeros(num_points, len(obstacles), dtype=torch.float)
obs_managers = [fcl.DynamicAABBTreeCollisionManager() for _ in fcl_obs]
for mng, cobj in zip(obs_managers, fcl_collision_obj):
mng.registerObjects([cobj])
elif label_type == 'class':
labels = torch.zeros(num_points, num_class, dtype=torch.float)
dists = torch.zeros(num_points, num_class, dtype=torch.float)
obs_managers = [
fcl.DynamicAABBTreeCollisionManager() for _ in range(num_class)
]
obj_by_cls = [[] for _ in range(num_class)]
for obj in fcl_obs:
obj_by_cls[obj.category].append(obj.cobj)
for mng, obj_group in zip(obs_managers, obj_by_cls):
mng.registerObjects(obj_group)
robot_links = robot.update_polygons(cfgs[0])
robot_manager = fcl.DynamicAABBTreeCollisionManager()
robot_manager.registerObjects(robot_links)
robot_manager.setup()
for mng in obs_managers:
mng.setup()
req = fcl.CollisionRequest(num_max_contacts=1000, enable_contact=True)
times = []
st = time()
for i, cfg in enumerate(cfgs):
st1 = time()
robot.update_polygons(cfg)
robot_manager.update()
assert len(robot_manager.getObjects()) == robot.dof
for cat, obs_mng in enumerate(obs_managers):
rdata = fcl.CollisionData(request=req)
robot_manager.collide(obs_mng, rdata, fcl.defaultCollisionCallback)
in_collision = rdata.result.is_collision
ddata = fcl.DistanceData()
robot_manager.distance(obs_mng, ddata, fcl.defaultDistanceCallback)
depths = torch.FloatTensor(
[c.penetration_depth for c in rdata.result.contacts])
labels[i, cat] = 1 if in_collision else -1
dists[i, cat] = depths.abs().max(
) if in_collision else -ddata.result.min_distance
end1 = time()
times.append(end1 - st1)
end = time()
times = np.array(times)
print('std: {}, mean {}, avg {}'.format(times.std(), times.mean(),
(end - st) / len(cfgs)))
in_collision = (labels == 1).sum(1) > 0
if label_type == 'binary':
labels = labels.squeeze_(1)
dists = dists.squeeze_(1)
print('env_id {}, {} collisons, {} free'.format(
env_id, torch.sum(in_collision == 1), torch.sum(in_collision == 0)))
if torch.sum(in_collision == 1) == 0:
print('0 Collision. You may want to regenerate env {}obs{}'.format(
obs_num, env_id))
dataset = {
'data': cfgs,
'label': labels,
'dist': dists,
'obs': obstacles,
'robot': robot.__class__,
'rparam': [
robot.link_length,
robot.link_width,
robot.dof,
]
}
torch.save(
dataset,
os.path.join(
folder, '2d_{}dof_{}obs_{}_{}.pt'.format(robot.dof, obs_num,
label_type, env_id)))
if vis:
create_plots(robot, obstacles, cfg=None)
plt.savefig(
os.path.join(
folder,
'2d_{}dof_{}obs_{}_{}.png'.format(robot.dof, obs_num,
label_type, env_id)))
plt.close()
return
def test_managed_distances(self):
manager1 = fcl.DynamicAABBTreeCollisionManager()
manager2 = fcl.DynamicAABBTreeCollisionManager()
manager3 = fcl.DynamicAABBTreeCollisionManager()
objs1 = [
fcl.CollisionObject(self.geometry['box']),
fcl.CollisionObject(self.geometry['cylinder'])
]
objs2 = [
fcl.CollisionObject(self.geometry['cone'],
fcl.Transform(np.array([0.0, 0.0, 5.0]))),
fcl.CollisionObject(self.geometry['cylinder'],
fcl.Transform(np.array([0.0, 0.0, -5.0])))
]
objs3 = [
fcl.CollisionObject(self.geometry['mesh']),
fcl.CollisionObject(self.geometry['box'],
fcl.Transform(np.array([0.0, 0.0, -5.0])))
]
manager1.registerObjects(objs1)
manager2.registerObjects(objs2)
manager3.registerObjects(objs3)
manager1.setup()
manager2.setup()
manager3.setup()
self.assertTrue(len(manager1.getObjects()) == 2)
self.assertTrue(len(manager2.getObjects()) == 2)
self.assertTrue(len(manager3.getObjects()) == 2)
# One-to-many
o1 = fcl.CollisionObject(self.geometry['box'])
o2 = fcl.CollisionObject(self.geometry['cylinder'],
fcl.Transform(np.array([0.0, 0.0, -4.6])))
cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
manager1.distance(o1, cdata, fcl.defaultDistanceCallback)
self.assertTrue(cdata.result.min_distance < 0)
cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
manager1.distance(o2, cdata, fcl.defaultDistanceCallback)
assert abs(cdata.result.min_distance - 3.6) < 1e-4
cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
manager2.distance(o1, cdata, fcl.defaultDistanceCallback)
self.assertAlmostEqual(cdata.result.min_distance, 4.0)
cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
manager2.distance(o2, cdata, fcl.defaultDistanceCallback)
self.assertTrue(cdata.result.min_distance < 0)
# Many-to-many, internal
cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
manager1.distance(cdata, fcl.defaultDistanceCallback)
self.assertTrue(cdata.result.min_distance < 0)
cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
manager2.distance(cdata, fcl.defaultDistanceCallback)
self.assertAlmostEqual(cdata.result.min_distance, 9.0)
# Many-to-many, grouped
cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
manager1.distance(manager2, cdata, fcl.defaultDistanceCallback)
self.assertAlmostEqual(cdata.result.min_distance, 4.0)
cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
manager2.distance(manager3, cdata, fcl.defaultDistanceCallback)
self.assertTrue(cdata.result.min_distance < 0)
cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
manager1.distance(manager3, cdata, fcl.defaultDistanceCallback)
self.assertTrue(cdata.result.min_distance < 0)
def collision(ball1, ball2, ball3, ball4):
geom1 = fcl.Sphere(1.0)
geom2 = fcl.Sphere(1.0)
geom3 = fcl.Sphere(1.0)
geom4 = fcl.Sphere(1.0)
T1 = [ball1.pos.x, ball1.pos.y, ball1.pos.z]
t1 = fcl.Transform(T1)
obj1 = fcl.CollisionObject(geom1, t1)
T2 = [ball2.pos.x, ball2.pos.y, ball2.pos.z]
t2 = fcl.Transform(T2)
obj2 = fcl.CollisionObject(geom2, t2)
T3 = [ball3.pos.x, ball3.pos.y, ball3.pos.z]
t3 = fcl.Transform(T3)
obj3 = fcl.CollisionObject(geom3, t3)
T4 = [ball4.pos.x, ball4.pos.y, ball4.pos.z]
t4 = fcl.Transform(T4)
obj4 = fcl.CollisionObject(geom4, t4)
geoms = [geom1, geom2, geom3, geom4]
objs = [obj1, obj2, obj3, obj4]
names = ['obj1', 'obj2', 'obj3', 'obj4']
# Create map from geometry IDs to objects
geom_id_to_obj = {id(geom): obj for geom, obj in zip(geoms, objs)}
# Create map from geometry IDs to string names
geom_id_to_name = {id(geom): name for geom, name in zip(geoms, names)}
# Create manager
manager = fcl.DynamicAABBTreeCollisionManager()
manager.registerObjects(objs)
manager.setup()
# Create collision request structure
crequest = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
cdata = fcl.CollisionData(crequest, fcl.CollisionResult())
# Run collision request
manager.collide(cdata, fcl.defaultCollisionCallback)
# Extract collision data from contacts and use that to infer set of
# objects that are in collision
objs_in_collision = set()
for contact in cdata.result.contacts:
# Extract collision geometries that are in contact
coll_geom_0 = contact.o1
coll_geom_1 = contact.o2
# Get their names
coll_names = [
geom_id_to_name[id(coll_geom_0)], geom_id_to_name[id(coll_geom_1)]
]
coll_names = tuple(sorted(coll_names))
objs_in_collision.add(coll_names)
for coll_pair in objs_in_collision:
print('Object {} in collision with object {}!'.format(
coll_pair[0], coll_pair[1]))
return objs_in_collision
return self.fkine_backup
if __name__ == "__main__":
lw_data = 0.3
robot = RevolutePlanarRobot(1, dof=7, link_width=lw_data)
num_frames = 100
q = 2 * (torch.rand(num_frames, 7) - 0.5) * np.pi / 1.5
points = robot.fkine(q)
points = torch.cat([torch.zeros(num_frames, 1, 2), points], axis=1)
print(points)
robot_links = robot.update_polygons(q[0])
cir_pos = torch.FloatTensor([2, 2, 0])
obs = [fcl.CollisionObject(fcl.Cylinder(1, 1), fcl.Transform(cir_pos))]
robot_manager = fcl.DynamicAABBTreeCollisionManager()
obs_manager = fcl.DynamicAABBTreeCollisionManager()
robot_manager.registerObjects(robot_links)
obs_manager.registerObjects(obs)
robot_manager.setup()
obs_manager.setup()
req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
rdata = fcl.CollisionData(request=req)
robot_manager.collide(obs_manager, rdata, fcl.defaultCollisionCallback)
in_collision = rdata.result.is_collision
from matplotlib import pyplot as plt
import seaborn as sns
sns.set()
import matplotlib.patheffects as path_effects
num_init_points = 8000
if 'compare' not in env_name or DOF > 2:
cfgs = 2 * (torch.rand(
(num_init_points, DOF), dtype=torch.float32) - 0.5) * np.pi
else:
# --- only for compare with gt distance
size = [400, 400]
yy, xx = torch.meshgrid(torch.linspace(-np.pi, np.pi, size[0]),
torch.linspace(-np.pi, np.pi, size[1]))
cfgs = torch.stack([xx, yy], axis=2).reshape((-1, DOF))
num_init_points = len(cfgs)
# --- only for compare with gt distance
if label_type == 'binary':
labels = torch.zeros(num_init_points, 1, dtype=torch.float)
dists = torch.zeros(num_init_points, 1, dtype=torch.float)
obs_managers = [fcl.DynamicAABBTreeCollisionManager()]
obs_managers[0].registerObjects(fcl_collision_obj)
obs_managers[0].setup()
elif label_type == 'instance':
labels = torch.zeros(num_init_points,
len(obstacles),
dtype=torch.float)
dists = torch.zeros(num_init_points, len(obstacles), dtype=torch.float)
obs_managers = [fcl.DynamicAABBTreeCollisionManager() for _ in fcl_obs]
for mng, cobj in zip(obs_managers, fcl_collision_obj):
mng.registerObjects([cobj])
elif label_type == 'class':
labels = torch.zeros(num_init_points, num_class, dtype=torch.float)
dists = torch.zeros(num_init_points, num_class, dtype=torch.float)
obs_managers = [
fcl.DynamicAABBTreeCollisionManager() for _ in range(num_class)
def test_one_env(env_name, method, folder, args: ExpConfigs, prev_rec={}):
assert (args.use_previous_solution and prev_rec != {}) or \
((not args.use_previous_solution) and prev_rec == {}), \
"args.use_previous_solution does not match the existence of prev_rec."
print(env_name, method, 'Begin')
# Prepare distance estimator ====================
dataset = torch.load('{}/{}.pt'.format(folder, env_name))
cfgs = dataset['data'].double()
labels = dataset['label'].double() #.max(1).values
dists = dataset['dist'].double() #.reshape(-1, 1) #.max(1).values
obstacles = dataset['obs']
# obstacles = [obs+(i, ) for i, obs in enumerate(obstacles)]
robot = dataset['robot'](*dataset['rparam'])
width = robot.link_width
train_num = 6000
fkine = robot.fkine
train_t = time()
checker = DiffCo(obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)), beta=1.0)
# checker = MultiDiffCo(obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)), beta=1.0)
if not args.use_previous_solution:
checker.train(cfgs[:train_num], labels[:train_num], max_iteration=len(cfgs[:train_num]), distance=dists[:train_num])
# Check DiffCo test ACC
# test_preds = (checker.score(cfgs[train_num:]) > 0) * 2 - 1
# test_acc = torch.sum(test_preds == labels[train_num:], dtype=torch.float32)/len(test_preds.view(-1))
# test_tpr = torch.sum(test_preds[labels[train_num:]==1] == 1, dtype=torch.float32) / len(test_preds[labels[train_num:]==1])
# test_tnr = torch.sum(test_preds[labels[train_num:]==-1] == -1, dtype=torch.float32) / len(test_preds[labels[train_num:]==-1])
# print('Test acc: {}, TPR {}, TNR {}'.format(test_acc, test_tpr, test_tnr))
# if test_acc < 0.9:
# print('test acc is only {}'.format(test_acc))
fitting_target = 'label' # {label, dist, hypo}
Epsilon = 1 #0.01
checker.fit_poly(kernel_func=kernel.Polyharmonic(1, Epsilon), target=fitting_target, fkine=fkine)#, reg=0.09) # epsilon=Epsilon,
# checker.fit_full_poly(epsilon=Epsilon, target=fitting_target, fkine=fkine)#, lmbd=80)
# ========================
# ONLY for additional training timing exp
# fcl_obs = [FCLObstacle(*param) for param in obstacles]
# fcl_collision_obj = [fobs.cobj for fobs in fcl_obs]
# obs_managers = [fcl.DynamicAABBTreeCollisionManager()]
# obs_managers[0].registerObjects(fcl_collision_obj)
# obs_managers[0].setup()
# robot_links = robot.update_polygons(cfgs[0])
# robot_manager = fcl.DynamicAABBTreeCollisionManager()
# robot_manager.registerObjects(robot_links)
# robot_manager.setup()
# for mng in obs_managers:
# mng.setup()
# gt_checker = FCLChecker(obstacles, robot, robot_manager, obs_managers)
# gt_checker.predict(cfgs[:train_num], distance=False)
# return time() - train_t
# END ========================
dist_est = checker.rbf_score
# dist_est = checker.poly_score
min_score = dist_est(cfgs[train_num:]).min().item()
# print('MIN_SCORE = {:.6f}'.format(min_score))
else:
dist_est = None
min_score = 0
# ==============================================
# FCL checker =====================
fcl_obs = [FCLObstacle(*param) for param in obstacles]
fcl_collision_obj = [fobs.cobj for fobs in fcl_obs]
label_type = 'binary'
num_class = 1
if label_type == 'binary':
obs_managers = [fcl.DynamicAABBTreeCollisionManager()]
obs_managers[0].registerObjects(fcl_collision_obj)
obs_managers[0].setup()
elif label_type == 'instance':
obs_managers = [fcl.DynamicAABBTreeCollisionManager() for _ in fcl_obs]
for mng, cobj in zip(obs_managers, fcl_collision_obj):
mng.registerObjects([cobj])
elif label_type == 'class':
obs_managers = [fcl.DynamicAABBTreeCollisionManager() for _ in range(num_class)]
obj_by_cls = [[] for _ in range(num_class)]
for obj in fcl_obs:
obj_by_cls[obj.category].append(obj.cobj)
for mng, obj_group in zip(obs_managers, obj_by_cls):
mng.registerObjects(obj_group)
else:
raise NotImplementedError('Unsupported label_type {}'.format(label_type))
robot_links = robot.update_polygons(cfgs[0])
robot_manager = fcl.DynamicAABBTreeCollisionManager()
robot_manager.registerObjects(robot_links)
robot_manager.setup()
for mng in obs_managers:
mng.setup()
fcl_checker = FCLChecker(obstacles, robot, robot_manager, obs_managers)
# =================================
options = {
'N_WAYPOINTS': 20,
'NUM_RE_TRIALS': 3,
'MAXITER': 200,
'safety_margin': max(1/5*min_score, -0.5),
'seed': 1234,
'history': False
}
repair_options = {
'N_WAYPOINTS': 20,
'NUM_RE_TRIALS': 1, # just one trial
'MAXITER': 200,
'seed': 1234, # actually not used due to only one trial
'history': False,
}
test_rec = {
'start_cfg': [],
'target_cfg': [],
'cnt_check': [],
'repair_cnt_check': [],
'cost': [],
'repair_cost': [],
'time': [],
'val_time': [],
'repair_time': [],
'success': [],
'repair_success': [],
'seed': [],
'solution': [],
'repair_solution': [],
}
with open('{}/{}_testcfgs.json'.format(folder, env_name), 'r') as f:
test_cfg_dataset = json.load(f)
s_cfgs = torch.FloatTensor(test_cfg_dataset['start_cfgs'])[:10]
t_cfgs = torch.FloatTensor(test_cfg_dataset['target_cfgs'])[:10]
assert env_name == test_cfg_dataset['env_name']
if prev_rec != {}:
rec_s_cfgs = torch.FloatTensor(prev_rec['solution'])[:, 0]
rec_t_cfgs = torch.FloatTensor(prev_rec['solution'])[:, -1]
assert torch.all(torch.isclose(rec_s_cfgs, s_cfgs[:len(rec_s_cfgs)])) and torch.all(torch.isclose(rec_t_cfgs, t_cfgs[:len(rec_t_cfgs)]))
for test_it, (start_cfg, target_cfg) in tqdm(enumerate(zip(s_cfgs, t_cfgs)), desc='Test Query'):
options['seed'] += 1 # Otherwise the random initialization will stay the same every problem
if prev_rec != {} and test_it < len(prev_rec['success']):
print('using saved solution {}'.format(test_it))
tmp_rec = {k: prev_rec[k][test_it] for k in prev_rec}
elif method == 'fclgradfree':
print('solving query {} with fclgradfree'.format(test_it))
tmp_rec = gradient_free_traj_optimize(robot, lambda cfg: fcl_checker.predict(cfg, distance=False), start_cfg, target_cfg, options=options)
elif method == 'fcldist':
tmp_rec = gradient_free_traj_optimize(robot, fcl_checker.score, start_cfg, target_cfg, options=options)
elif method == 'adamdiffco':
tmp_rec = adam_traj_optimize(robot, dist_est, start_cfg, target_cfg, options=options)
elif method == 'bidiffco':
tmp_rec = gradient_free_traj_optimize(robot, lambda cfg: 2*(dist_est(cfg)>=0).type(torch.FloatTensor)-1, start_cfg, target_cfg, options=options)
elif method == 'diffcogradfree':
with torch.no_grad():
tmp_rec = gradient_free_traj_optimize(robot, dist_est, start_cfg, target_cfg, options=options)
elif method == 'margindiffcogradfree':
with torch.no_grad():
tmp_rec = gradient_free_traj_optimize(robot, lambda cfg: dist_est(cfg)-options['safety_margin'], start_cfg, target_cfg, options=options)
elif method == 'givengrad':
tmp_rec = givengrad_traj_optimize(robot, dist_est, start_cfg, target_cfg, options=options)
else:
raise NotImplementedError('Method = {} not implemented'.format(method))
# Verification
# if tmp_rec['success']:
def con_max_move(p):
control_points = robot.fkine(p)
return torch.all((control_points[1:]-control_points[:-1]).pow(2).sum(dim=2)-1.5**2 <= 0).item()
def con_collision_free(p):
return torch.all(fcl_checker.predict(p, distance=False) < 0).item()
def con_dist_collision_free(p):
return torch.all(fcl_checker.score(p) < 0).item()
# return True
def con_joint_limit(p):
return (torch.all(robot.limits[:, 0]-p <= 0) and torch.all(p-robot.limits[:, 1] <= 0)).item()
def validate(solution, method=None):
veri_cfgs = [utils.anglin(q1, q2, args.validate_density, endpoint=False)\
for q1, q2 in zip(solution[:-1], solution[1:])]
veri_cfgs = torch.cat(veri_cfgs, 0)
collision_free = con_collision_free(veri_cfgs) if method != 'fcldist' else con_dist_collision_free(veri_cfgs)
sol_tensor = torch.FloatTensor(solution)
within_jointlimit = con_joint_limit(sol_tensor)
within_movelimit = con_max_move(sol_tensor)
return collision_free and within_jointlimit and within_movelimit
if 'fcl' in method and args.validate_density == 1: # skip validation if using fcl and density is only 1
val_t = 0
# tmp_rec['success'] = validate(tmp_rec['solution'], method=method) # This is only temporary
else:
val_t = time()
tmp_rec['success'] = validate(tmp_rec['solution'])
val_t = time() - val_t
tmp_rec['val_time'] = val_t
for k in tmp_rec:
if 'repair_' in k:
continue
test_rec[k].append(tmp_rec[k])
# Repair
if not tmp_rec['success'] and 'fcl' not in method:
repair_rec = gradient_free_traj_optimize(robot, fcl_checker.score, start_cfg, target_cfg,
options={**repair_options, 'init_solution': torch.DoubleTensor(tmp_rec['solution'])})
# repair_rec['success'] = validate(repair_rec['solution']) # validation not needed
else:
repair_rec = {
'cnt_check': 0,
'cost': tmp_rec['cost'],
'time': 0,
'success': tmp_rec['success'],
'solution': tmp_rec['solution'],
}
for k in ['cnt_check', 'cost', 'time', 'success', 'solution']:
test_rec['repair_'+k].append(repair_rec[k])
# ================Visualize for debugging purposes===================
# cfg_path_plots = []
# if robot.dof > 2:
# fig, ax, link_plot, joint_plot, eff_plot = create_plots(robot, obstacles, dist_est, checker)
# elif robot.dof == 2:
# fig, ax, link_plot, joint_plot, eff_plot, cfg_path_plots = create_plots(robot, obstacles, dist_est, checker)
# single_plot(robot, torch.FloatTensor(test_rec['repair_solution'][-1]), fig, link_plot, joint_plot, eff_plot, cfg_path_plots=cfg_path_plots, ax=ax)
# debug_dir = join('debug', exp_name, method)
# if not isdir(debug_dir):
# os.makedirs(debug_dir)
# plt.savefig(join(debug_dir, 'debug_view_{}.png'.format(test_it)), dpi=500)
# plt.close()
# break # debugging
return test_rec
m.addSubModel(verts, tris)
m.endModel()
objs1 = [
fcl.CollisionObject(
b,
fcl.Transform(
Rotation.from_euler(
'XYZ', [0, 0, np.arccos(2.0 / 3) - np.arctan(0.5)]).as_quat()[[
3, 0, 1, 2
]], [1.5, 0, 0]))
] #, fcl.CollisionObject(s)] np.arccos(2.0/3) Rotation.from_euler('XYZ', [0, 0, np.pi/2]).as_quat()[[3,0,1,2]], [1.5, 0, 0]
print(Rotation.from_rotvec([0, 0, np.arccos(2.0 / 3)]).as_quat()[[3, 0, 1, 2]])
objs2 = [fcl.CollisionObject(c)] #, fcl.CollisionObject(m)]
manager1 = fcl.DynamicAABBTreeCollisionManager()
manager2 = fcl.DynamicAABBTreeCollisionManager()
manager1.registerObjects(objs1)
manager2.registerObjects(objs2)
manager1.setup()
manager2.setup()
# #=====================================================================
# # Managed internal (sub-n^2) collision checking
# #=====================================================================
# cdata = fcl.CollisionData()
# manager1.collide(cdata, fcl.defaultCollisionCallback)
# print('Collision within manager 1?: {}'.format(cdata.result.is_collision))
def test_managed_collisions(self):
manager1 = fcl.DynamicAABBTreeCollisionManager()
manager2 = fcl.DynamicAABBTreeCollisionManager()
manager3 = fcl.DynamicAABBTreeCollisionManager()
objs1 = [
fcl.CollisionObject(self.geometry['box']),
fcl.CollisionObject(self.geometry['cylinder'])
]
objs2 = [
fcl.CollisionObject(self.geometry['cone'],
fcl.Transform(np.array([0.0, 0.0, 5.0]))),
fcl.CollisionObject(self.geometry['cylinder'],
fcl.Transform(np.array([0.0, 0.0, -5.0])))
]
objs3 = [
fcl.CollisionObject(self.geometry['mesh']),
fcl.CollisionObject(self.geometry['box'],
fcl.Transform(np.array([0.0, 0.0, -5.0])))
]
manager1.registerObjects(objs1)
manager2.registerObjects(objs2)
manager3.registerObjects(objs3)
manager1.setup()
manager2.setup()
manager3.setup()
self.assertTrue(len(manager1.getObjects()) == 2)
self.assertTrue(len(manager2.getObjects()) == 2)
self.assertTrue(len(manager3.getObjects()) == 2)
# One-to-many
o1 = fcl.CollisionObject(self.geometry['box'])
o2 = fcl.CollisionObject(self.geometry['cylinder'],
fcl.Transform(np.array([0.0, 0.0, -4.6])))
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager1.collide(o1, cdata, fcl.defaultCollisionCallback)
self.assertTrue(cdata.result.is_collision)
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager1.collide(o2, cdata, fcl.defaultCollisionCallback)
self.assertFalse(cdata.result.is_collision)
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager2.collide(o1, cdata, fcl.defaultCollisionCallback)
self.assertFalse(cdata.result.is_collision)
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager2.collide(o2, cdata, fcl.defaultCollisionCallback)
self.assertTrue(cdata.result.is_collision)
# Many-to-many, internal
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager1.collide(cdata, fcl.defaultCollisionCallback)
self.assertTrue(cdata.result.is_collision)
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager2.collide(cdata, fcl.defaultCollisionCallback)
self.assertFalse(cdata.result.is_collision)
# Many-to-many, grouped
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager1.collide(manager2, cdata, fcl.defaultCollisionCallback)
self.assertFalse(cdata.result.is_collision)
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager2.collide(manager3, cdata, fcl.defaultCollisionCallback)
self.assertTrue(cdata.result.is_collision)
cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
manager1.collide(manager3, cdata, fcl.defaultCollisionCallback)
self.assertTrue(cdata.result.is_collision)
def main(checking_method='diffco'):
DOF = 2
env_name = '1rect_active' # '2rect' # '1rect_1circle' '1rect' 'narrow' '2instance'
dataset = torch.load('data/2d_{}dof_{}.pt'.format(DOF, env_name))
cfgs = dataset['data'].double()
labels = dataset['label'].reshape(-1, 1).double() #.max(1).values
dists = dataset['dist'].reshape(-1, 1).double() #.max(1).values
obstacles = dataset['obs']
obstacles = [list(o) for o in obstacles]
robot = dataset['robot'](*dataset['rparam'])
width = robot.link_width
#=================================================================================================================================
fcl_obs = [FCLObstacle(*param) for param in obstacles]
fcl_collision_obj = [fobs.cobj for fobs in fcl_obs]
label_type = 'binary'
num_class = 1
T = 11
nu = 5 #5
kai = 1500
sigma = 0.3
seed = 1918
torch.manual_seed(seed)
np.random.seed(seed)
num_init_points = 8000
if label_type == 'binary':
obs_managers = [fcl.DynamicAABBTreeCollisionManager()]
obs_managers[0].registerObjects(fcl_collision_obj)
obs_managers[0].setup()
elif label_type == 'instance':
obs_managers = [fcl.DynamicAABBTreeCollisionManager() for _ in fcl_obs]
for mng, cobj in zip(obs_managers, fcl_collision_obj):
mng.registerObjects([cobj])
elif label_type == 'class':
obs_managers = [
fcl.DynamicAABBTreeCollisionManager() for _ in range(num_class)
]
obj_by_cls = [[] for _ in range(num_class)]
for obj in fcl_obs:
obj_by_cls[obj.category].append(obj.cobj)
for mng, obj_group in zip(obs_managers, obj_by_cls):
mng.registerObjects(obj_group)
robot_links = robot.update_polygons(cfgs[0])
robot_manager = fcl.DynamicAABBTreeCollisionManager()
robot_manager.registerObjects(robot_links)
robot_manager.setup()
for mng in obs_managers:
mng.setup()
gt_checker = FCLChecker(obstacles, robot, robot_manager, obs_managers)
train_num = 6000
fkine = robot.fkine
# checker = DiffCo(obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)), beta=1.0)
from time import time
init_train_t = time()
checker = MultiDiffCo(obstacles,
kernel_func=kernel.FKKernel(fkine,
kernel.RQKernel(10)),
beta=1.0)
labels, dists = gt_checker.predict(cfgs[:train_num], distance=True)
labels = labels.double()
dists = dists.double()
checker.train(cfgs[:train_num],
labels[:train_num],
max_iteration=len(cfgs[:train_num]),
distance=dists[:train_num])
# Check DiffCo test ACC
# test_preds = (checker.score(cfgs[train_num:]) > 0) * 2 - 1
# test_acc = torch.sum(test_preds == labels[train_num:], dtype=torch.float32)/len(test_preds.view(-1))
# test_tpr = torch.sum(test_preds[labels[train_num:]==1] == 1, dtype=torch.float32) / len(test_preds[labels[train_num:]==1])
# test_tnr = torch.sum(test_preds[labels[train_num:]==-1] == -1, dtype=torch.float32) / len(test_preds[labels[train_num:]==-1])
# print('Test acc: {}, TPR {}, TNR {}'.format(test_acc, test_tpr, test_tnr))
# assert(test_acc > 0.9)
fitting_target = 'dist' # {label, dist, hypo}
Epsilon = 0.01
checker.fit_poly(kernel_func=kernel.Polyharmonic(1, Epsilon),
target=fitting_target,
fkine=fkine) # epsilon=Epsilon,
# checker.fit_poly(kernel_func=kernel.MultiQuadratic(Epsilon), target=fitting_target, fkine=fkine)
# checker.fit_full_poly(epsilon=Epsilon, target=fitting_target, fkine=fkine, lmbd=10)
dist_est = checker.rbf_score
init_train_t = time() - init_train_t
# dist_est = checker.score
# dist_est = checker.poly_score
print('MIN_SCORE = {:.6f}'.format(dist_est(cfgs[train_num:]).min()))
positions = torch.FloatTensor(np.linspace(obstacles[0][1], [4, 3], T))
start_cfg = torch.zeros(robot.dof,
dtype=cfgs.dtype) # free_cfgs[indices[0]] #
target_cfg = torch.zeros(robot.dof,
dtype=cfgs.dtype) # free_cfgs[indices[1]] #
start_cfg[0] = np.pi / 2 # -np.pi/16
start_cfg[1] = -np.pi / 6
target_cfg[0] = 0 # -np.pi/2 # -15*np.pi/16
target_cfg[1] = np.pi / 7
update_ts = []
plan_ts = []
for t, trans in zip(range(T), positions):
ut = time()
fcl_collision_obj[0].setTransform(
fcl.Transform(
# Rotation.from_rotvec([0, 0, angle]).as_quat()[[3,0,1,2]],
[trans[0], trans[1], 0]))
for obs_mng in obs_managers:
obs_mng.update()
# if checking_method == 'diffco':
exploit_samples = torch.randn(nu,
len(checker.gains),
robot.dof,
dtype=checker.support_points.dtype
) * sigma + checker.support_points
exploit_samples = utils.wrap2pi(exploit_samples).reshape(-1, robot.dof)
explore_samples = torch.rand(
kai, robot.dof,
dtype=checker.support_points.dtype) * 2 * np.pi - np.pi
cfgs = torch.cat(
[exploit_samples, explore_samples, checker.support_points])
labels, dists = gt_checker.predict(cfgs, distance=True)
dists = dists.double()
print('Collision {}, Free {}\n'.format((labels == 1).sum(),
(labels == -1).sum()))
gains = torch.cat([
torch.zeros(len(exploit_samples) + len(explore_samples),
checker.num_class,
dtype=checker.gains.dtype), checker.gains
]) #None #
#TODO: bug: not calculating true hypothesis for new points
added_hypothesis = checker.score(cfgs[:-len(checker.support_points)])
hypothesis = torch.cat(
[added_hypothesis, checker.hypothesis]
) # torch.cat([torch.zeros(len(exploit_samples)+len(explore_samples), checker.num_class), checker.hypothesis]) # None #
# kernel_matrix = torch.zeros(len(cfgs), len(cfgs)) #None #
# kernel_matrix[-len(checker.kernel_matrix):, -len(checker.kernel_matrix):] = checker.kernel_matrix
checker.train(cfgs,
labels,
gains=gains,
hypothesis=hypothesis,
distance=dists) #, kernel_matrix=kernel_matrix
print('Num of support points {}'.format(len(checker.support_points)))
checker.fit_poly(kernel_func=kernel.Polyharmonic(1, Epsilon),
target=fitting_target,
fkine=fkine,
reg=0.1)
update_ts.append(time() - ut)
if checking_method == 'fcl':
fcl_options = {
'N_WAYPOINTS': 20,
'NUM_RE_TRIALS': 5, # Debugging
'MAXITER': 200,
'seed': seed,
'history': False
}
elif checking_method == 'diffco':
diffco_options = {
'N_WAYPOINTS': 20,
'NUM_RE_TRIALS': 5, # Debugging
'MAXITER': 200,
'safety_margin': -0.5, #max(1/5*min_score, -0.5),
'seed': seed,
'history': False
}
print('t = {}'.format(t))
if t % 1 == 0 and not torch.any(
checker.predict(torch.stack([start_cfg, target_cfg], dim=0)) ==
1):
obstacles[0][1] = (trans[0], trans[1])
cfg_path_plots = []
if robot.dof > 2:
fig, ax, link_plot, joint_plot, eff_plot = create_plots(
robot, obstacles, dist_est, checker)
elif robot.dof == 2:
fig, ax, link_plot, joint_plot, eff_plot, cfg_path_plots = create_plots(
robot, obstacles, dist_est, checker)
ot = time()
# Begin optimization==========
if checking_method == 'diffco':
# p, path_history, num_trial, num_step = traj_optimize(
# robot, dist_est, start_cfg, target_cfg, history=False)
solution_rec = givengrad_traj_optimize(robot,
dist_est,
start_cfg,
target_cfg,
options=diffco_options)
p = torch.FloatTensor(solution_rec['solution'])
elif checking_method == 'fcl':
solution_rec = gradient_free_traj_optimize(
robot,
lambda cfg: gt_checker.predict(cfg, distance=False),
start_cfg,
target_cfg,
options=fcl_options)
p = torch.FloatTensor(solution_rec['solution'])
# ============================
plan_ts.append(time() - ot)
# path_dir = 'results/active_learning/path_2d_{}dof_{}_seed{}_step{:02d}.json'.format(robot.dof, env_name, seed, t) # DEBUGGING
path_dir = 'results/active_learning/path_2d_{}dof_{}_checker={}_seed{}_step{:02d}.json'.format(
robot.dof, env_name, checking_method, seed, t)
with open(path_dir, 'w') as f:
json.dump({
'path': p.data.numpy().tolist(),
}, f, indent=1)
print('Plan recorded in {}'.format(f.name))
# Use saved path ======
# if not isfile(path_dir):
# continue
# with open(path_dir, 'r') as f:
# path_dict = json.load(f)
# p = torch.FloatTensor(path_dict['path'])
# =====================
p = utils.make_continue(p)
#animation
# vid_name = None #'results/maual_trajopt_2d_{}dof_{}_fitting_{}_eps_{}_dif_{}_updates_{}_steps_{}.mp4'.format(
# # robot.dof, env_name, fitting_target, Epsilon, dif_weight, UPDATE_STEPS, N_STEPS)
# if robot.dof == 2:
# animation_demo(
# robot, p, fig, link_plot, joint_plot, eff_plot,
# cfg_path_plots=cfg_path_plots, path_history=path_history, save_dir=vid_name)
# elif robot.dof == 7:
# animation_demo(robot, p, fig, link_plot, joint_plot, eff_plot, save_dir=vid_name)
# single shot
single_plot(robot,
p,
fig,
link_plot,
joint_plot,
eff_plot,
cfg_path_plots=cfg_path_plots,
ax=ax)
# plt.show()
# plt.savefig('figs/path_2d_{}dof_{}.png'.format(robot.dof, env_name), dpi=500)
# plt.savefig('figs/active_2d_{}dof_{}_{}'.format(robot.dof, env_name, t), dpi=500)
fig_dir = 'figs/active/{random_seed}/{checking_method}'.format(
random_seed=seed, checking_method=checking_method)
if not isdir(fig_dir):
makedirs(fig_dir)
plt.savefig(join(
fig_dir,
'2d_{DOF}dof_{ename}_{checking_method}_{step:02d}'.format(
DOF=robot.dof,
ename=env_name,
checking_method=checking_method,
step=t)),
dpi=300)
print('{} summary'.format(checking_method))
print('Initial training {} sec.'.format(init_train_t))
print('Update {} sec.'.format(update_ts))
print('Planning {} sec.'.format(plan_ts))
