def addObject(self, name, fp_label, masses, meter_per_pixel, color=[0.5,0.5,0.5,1], x=0, y=0, yaw=0, probs=None):
if type(fp_label)==str:
img = cv2.imread(fp_label, cv2.IMREAD_UNCHANGED)
else:
img = fp_label
if type(masses[0])==list:
names = [name+'_'+str(i) for i in range(len(masses))]
if probs is None:
probs = [1/float(len(names))] * len(names)
else:
names = [name+'_0']
probs = [1]
masses = [masses]
self.name2names[name] = names
self.name2probs[name] = probs
for i, name in enumerate(names):
if type(masses[i][0])==dict:
cells = CellPhysicsBase.createCells(img,[1,1,1,1],meter_per_pixel=meter_per_pixel)
else:
cells = CellPhysicsBase.createCells(img,masses[i],meter_per_pixel=meter_per_pixel)
self.name2cells[name] = cells
self.sim_cols[name] = SimBullet(gui=False)
self.sim_cols[name].addObject(name, cells, color, x, y, yaw)
self.sim_stbs[name] = SimBullet(gui=False)
self.sim_stbs[name].addObject(name, cells, color, x, y, yaw)
self.addDefaultObjects(self.sim_stbs[name])
self.sim_objs[name] = SimBullet(gui=False)
self.sim_objs[name].setupCamera(0.50,-90,-89.99)
self.sim_objs[name].addObject('plane','plane')
self.sim_objs[name].addObject(name, cells, color, x, y, yaw)
self.sim_objs[name].addObject(self.name_actor,self.type_actor,self.color_actor)
if self.debug:
self.sim_debug.addObject(name, cells, color, x, y, yaw)
self.trans_objs[name] = {}
self.actions_objs[name] = self.genActions(cells, self.resolution_contact)
self.n_actions_objs[name] = sum([len(self.actions_objs[name][key]) for key in self.actions_objs[name]]) * len(self.pushing_dists)
self.uvec_contact2com[name] = {}
if type(masses[i][0])==dict:
cellprop = masses[i]
else:
cellprop = self.sim_objs[name].getCellBodyProperties(name)
self.updateObjectProperties(name, cellprop)
def computeTransition_multiproc(args):
name, cells, action, name_actor, type_actor, dims_actor = args
sim = SimBullet(gui=False)
sim.addObject('plane','plane')
sim.addObject(name, cells, [0,0,0,1], 0,0,0)
sim.addObject(name_actor,type_actor,[0,0,0,1])
KinoPlanner.applyAction(sim,name,0,0,0,action,name_actor,dims_actor)
x,y,yaw = sim.getObjectPose(name)
cellpos = sim.getCellBodyPose(name)
return {'vec':[x,y],'yaw':yaw,'cellpos':cellpos}
def simulations_multiproc(args):
name, cells, cellinfo, actions = args
dims_actor = [0.03, 0.03, 0.10]
sim = SimBullet()
sim.addObject('plane', 'plane')
sim.addObject('finger', 'finger')
sim.addObject(name, cells)
sim.setCellBodyProperties(name, cellinfo)
res = []
for action in actions:
DiffPhysics.simulate(sim, action, 'finger', dims_actor)
x, y, yaw = sim.getObjectPose(name)
cellpos = sim.getCellBodyPose(name)
res.append({'x': x, 'y': y, 'yaw': yaw, 'cellpos': cellpos})
return res
def __init__(self, setup=None, contact_resolution=0.02, pushing_dists=[0.02,0.04,0.06,0.08,0.10], debug=False, use_multiproc=False):
self.name2cells = {}
self.space = ob.SE2StateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(-1)
bounds.setHigh(1)
self.space.setBounds(bounds)
self.si = ob.SpaceInformation(self.space)
self.name_actor = 'finger'
self.type_actor = 'finger'
self.color_actor = [1,1,0,1]
self.dims_actor = [0.03, 0.03]
self.name2names = {}
self.name2probs = {}
self.sim_cols = {}
self.sim_stbs = {}
self.sim_objs = {}
self.stbs_objs = {}
self.trans_objs = {}
self.actions_objs = {}
self.n_actions_objs = {}
self.uvec_contact2com = {}
self.com_objs = {}
if setup is None:
setup = {'table_type','table_round'}
self.setup = setup
self.setup['table_radius'] = SimBullet.get_shapes()[self.setup['table_type']]['radius']
self.debug = debug
if self.debug:
self.sim_debug = SimBullet(gui=self.debug)
self.addDefaultObjects(self.sim_debug)
self.resolution_contact = contact_resolution
self.pushing_dists = sorted(pushing_dists)
self.use_multiproc = use_multiproc
if self.use_multiproc:
self.pool = Pool(int(multiprocessing.cpu_count()*0.8+0.5))
def isActionStable_multiproc(args):
name, cells, x, y, yaw, action, name_actor, type_actor, dims_actor = args
sim = SimBullet(gui=False)
sim.addObject('plane','plane')
sim.addObject(name, cells, [0,0,0,1], 0,0,0)
sim.addObject(name_actor,type_actor,[0,0,0,1])
sim.setObjectPose(name,x,y,yaw)
(x0,y0,z0),q0 = sim.getObjectPose(name,ndims=3)
KinoPlanner.applyAction(sim, name,x,y,yaw, \
action,name_actor,dims_actor)
(x1,y1,z1),q1 = sim.getObjectPose(name,ndims=3)
q0 = q0.normalised
q1 = q1.normalised
dist_q = 1 - np.dot(q0.q,q1.q)**2
return z1>0 or dist_q<0.003 # <10degree
def draw_result(fp_label,
fp_res,
fp_massmap,
fp_posres,
setups,
masses_gt=[1, 1, 1, 1],
iters=[
-1,
0,
1,
2,
]):
cmap = matplotlib.cm.coolwarm
#cnorm = matplotlib.colors.Normalize(vmin=0.1,vmax=4.9)
obj = os.path.split(fp_label)[1].split('_')[0]
sim = SimBullet(gui=False)
#sim._p.setGravity(0,0,-9.8)
sim.addObject('table_rect', 'table_rect', [1, 1, 1, 0])
sim.setupRendering(0.75, -90, -60, 500, 500)
img_label = cv2.imread(fp_label, cv2.IMREAD_UNCHANGED)
labels = np.unique(img_label)
cells = CellPhysicsBase.createCells(img_label, masses_gt, 0.53 / 640.0)
sim.addObject(obj, cells, [0, 0, 1, 1])
sim.setObjectPose(obj, 0, 0, 0)
cellinfos = sim.getCellBodyProperties(obj)
vmax = max([info['mass'] for info in cellinfos])
vmin = min([info['mass'] for info in cellinfos])
cnorm = matplotlib.colors.Normalize(vmin=vmin, vmax=vmax)
for info in cellinfos:
info['color'] = cmap(cnorm(info['mass']))
sim.setCellBodyProperties(obj, cellinfos)
img = sim.draw()
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
cv2.imwrite(fp_massmap % -3, img)
f = open(fp_res, 'rb')
res = pickle.load(f)
f.close()
# estimation example
cellinfos = res['history'][-1]['cellinfos']
#cnorm = matplotlib.colors.Normalize(vmin=0.1,vmax=5)
for info in cellinfos[obj]:
info['color'] = cmap(cnorm((vmax - vmin) * 0.5))
sim.setCellBodyProperties(obj, cellinfos[obj])
img = sim.draw()
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
cv2.imwrite(fp_massmap % -2, img)
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
for it in iters:
if it >= len(res['history']):
continue
cellinfos = res['history'][it]['cellinfos']
#vmax = max([info['mass'] for info in cellinfos[obj]])
#vmin = min([info['mass'] for info in cellinfos[obj]])
#cnorm = matplotlib.colors.Normalize(vmin=vmin,vmax=vmax)
for info in cellinfos[obj]:
info['color'] = cmap(cnorm(info['mass']))
sim.setCellBodyProperties(obj, cellinfos[obj])
img = sim.draw()
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
cv2.imwrite(fp_massmap % it, img)
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
return
# debug image
sim.setupRendering(0.75, 90, -90, 500, 500)
sim.addObject('finger1', 'finger', [1, 1, 0.9, 1])
sim.addObject('finger2', 'finger', [1, 1, 0.6, 1])
sim.addObject('finger3', 'finger', [1, 1, 0.3, 1])
sim.addObject('init', cells, [0.7, 0.7, 0.7, 1])
pos, rot = sim.getPosRot(sim.objects['init']['shape'], 0, 0, 0)
pos[2] = pos[2] - 0.002
sim._p.resetBasePositionAndOrientation(sim.objects['init']['bodyId'], pos,
rot)
sim.addObject('gt', cells, [0, 1, 0, 1])
for i in range(len(res['history'][-1]['test_poses'])):
if 'real' not in setups:
pixel2meter(cells, setups['test'][i])
sim.resetObject('finger1',setups['test'][i]['finger']['pos'][0] - 0.03,\
setups['test'][i]['finger']['pos'][1],0)
sim.resetObject('finger2',setups['test'][i]['finger']['pos'][0] + setups['test'][i]['finger']['velocity'][0]*0.40 - 0.03,\
setups['test'][i]['finger']['pos'][1] + setups['test'][i]['finger']['velocity'][1]*0.40,0)
sim.resetObject('finger3',setups['test'][i]['finger']['pos'][0] + setups['test'][i]['finger']['velocity'][0]*0.90 - 0.03,\
setups['test'][i]['finger']['pos'][1] + setups['test'][i]['finger']['velocity'][1]*0.90,0)
else:
sim.resetObject('finger1',setups['real'][i]['finger']['pos'][0] - 0.03,\
setups['real'][i]['finger']['pos'][1],0)
sim.resetObject('finger2',setups['real'][i]['finger']['pos'][0] + setups['real'][i]['finger']['velocity'][0]*0.40 - 0.03,\
setups['real'][i]['finger']['pos'][1] + setups['real'][i]['finger']['velocity'][1]*0.40,0)
sim.resetObject('finger3',setups['real'][i]['finger']['pos'][0] + setups['real'][i]['finger']['velocity'][0]*0.90 - 0.03,\
setups['real'][i]['finger']['pos'][1] + setups['real'][i]['finger']['velocity'][1]*0.90,0)
pose_gt = res['history'][-1]['test_poses'][i]['gt']
#sim.resetObject(obj,100,100,0)
sim.resetObject('gt', pose_gt[obj]['x'], pose_gt[obj]['y'],
pose_gt[obj]['yaw'])
#img = sim.draw()
#img = cv2.cvtColor(img,cv2.COLOR_RGB2BGR)
#cv2.imwrite(fp_posres % (i+20),img)
#img = cv2.cvtColor(img,cv2.COLOR_BGR2RGB)
#sim.resetObject('gt',100,100,0)
pose_est = res['history'][-1]['test_poses'][i]['estimation']
sim.resetObject(obj, pose_est[obj]['x'], pose_est[obj]['y'],
pose_est[obj]['yaw'])
pos, rot = sim.getPosRot(sim.objects['gt']['shape'], pose_gt[obj]['x'],
pose_gt[obj]['y'], pose_gt[obj]['yaw'])
pos[0] = pos[0] - 0.001
pos[2] = pos[2] - 0.001
sim._p.resetBasePositionAndOrientation(sim.objects['gt']['bodyId'],
pos, rot)
img = sim.draw()
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
cv2.imwrite(fp_posres % i, img)
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
if method=='uniform':
n_plans = 10
elif method=='none':
n_plans = 1
# create planner
planner = ProbPhysicsPlanner(obj,fp_label,models,meter_per_pixel,
setup=setup,
contact_resolution=0.02, \
pushing_dists=[0.20,0.10,0.05,0.04,0.03,0.02,0.01])
# select goals
goals_sel = planner.selectGoals(obj, goals)
print('Selected Goals: ', goals_sel)
sim = SimBullet(gui=args.debug)
sim.setupCamera(0.75,-90,-89.99, 0.63,0,0)
sim.addObject('table_round','table_round',[1,1,1,1])
sim.setObjectPose('table_round',0.63,0,0)
sim.addObject('finger','finger',[1,1,0,1])
sim.setObjectPose('finger',10,0,0)
cells = CellPhysicsBase.createCells(fp_label,gts[obj]['masses'],meter_per_pixel=meter_per_pixel)
sim.addObject(obj, cells, [0.5,0.5,0.5,1], 0, 0, 0)
succs = []
plans = []
n_actions = []
for i in args.inits:
init = inits[i]
print('method:%s obj:%s init#:%d'%(method,obj,i))
dir_sav = 'sav_tmp4'
for obj in [
'book', 'box', 'crimp', 'hammer', 'ranch', 'snack', 'toothpaste',
'windex'
]:
if not os.path.isdir(dir_sav + '/fig'):
os.makedirs(dir_sav + '/fig')
fp_label = '../../dataset/objects/%s_label.pgm' % obj
img_label = cv2.imread(fp_label, cv2.IMREAD_UNCHANGED)
cells = CellPhysicsBase.createCells(img_label, [1, 1, 1, 1],
0.53 / 640.0)
sim = SimBullet(gui=False)
sim.setupRendering(0.75, -90, -60, 500, 500)
sim.addObject(obj, cells, [0, 0, 1, 1])
sim.setObjectPose(obj, 0, 0, 0)
files = glob.glob(dir_sav + '/' + obj + '*.pkl')
for file in files:
filename = os.path.split(file)[1].split('.')[0]
f = open(file, 'rb')
sav = pickle.load(f)
f.close()
cellinfo = sav['cellinfo']
error_cell = sav['error_cell']
return img_plan
if __name__ == '__main__':
obj = 'hammer'
init = [0.63, 0.00, 0.5 * np.pi]
goal = [0.40, -0.40, -30.0 / 180.0 * np.pi]
meter_per_pixel = 0.53 / 640.0
fp_label = '../../dataset/objects/' + obj + '_label.pgm'
img = cv2.imread(fp_label, cv2.IMREAD_UNCHANGED)
planner = CellPhysicsPlanner(obj, img, [1, 1, 1, 1], meter_per_pixel)
sim = SimBullet(gui=True)
sim.setupCamera(0.75, -90, -89.99, 0.63, 0, 0)
sim.addObject('table_round', 'table_round', [1, 1, 1, 1])
sim.setObjectPose('table_round', 0.63, 0, 0)
sim.addObject('finger', 'finger', [1, 1, 0, 1])
sim.setObjectPose('finger', 10, 0, 0)
cells = DiffPhysics.createCells(img,
params['hammer'][6]['masses'],
meter_per_pixel=meter_per_pixel)
sim.addObject(obj, cells, [0.5, 0.5, 0.5, 1], 0, 0, 0)
sim.setObjectPose(obj, goal[0], goal[1], goal[2])
cellpos_goal = sim.getCellBodyPose(obj)
sim.setObjectPose(obj, init[0], init[1], init[2])
class Pose2D:
def __init__(self, x, y, theta):