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 get_best_action(x,y,yaw,cellpos_goal,path,actions,cost,depth):
if depth==0:
self.sim_objs[name].setObjectPose(name,x,y,yaw)
cellpos = self.sim_objs[name].getCellBodyPose(name)
err = CellPhysicsBase.distanceCellPos(cellpos,cellpos_goal)
return (err,list(path),list(actions),cost)
actions_sorted = []
action_prv = actions[-1] if len(actions)>0 else None
for action in self.trans_objs[name]:
cost_cur = self.costAction(action_prv,action)
actions_sorted.append((action,cost_cur))
actions_sorted = sorted(actions_sorted, key=lambda a: a[1])
err_min = -1
path_min = None
actions_min = None
cost_min = None
for action, cost_cur in actions_sorted:
x_next,y_next,yaw_next,cellpos_next\
= self.lookupTransitionTable(name,x,y,yaw,action)
err_next = CellPhysicsBase.distanceCellPos(cellpos_next,cellpos_goal)
path_next = list(path) + [(x_next,y_next,yaw_next)]
actions_next = list(actions) + [action]
cost_next = cost + cost_cur
if err_next < thres:
err_res, path_res, actions_res, cost_res\
= (err_next, path_next, actions_next, cost_next)
else:
err_res, path_res, actions_res, cost_res\
= get_best_action( x_next,y_next,yaw_next,cellpos_goal,
path_next, actions_next, cost_next, depth-1 )
if err_res==-1:
continue
if err_res < thres:
return (err_res, path_res, actions_res, cost_res)
elif err_min==-1 or err_min > err_res:
err_min = err_res
path_min = path_res
actions_min = actions_res
cost_min = cost_res
return (err_min, path_min, actions_min, cost_min)
def addObservation(self, pos_begin, pos_final, pos_push, vec_push,
duration):
x_tmp = pos_final.x - pos_begin.x
y_tmp = pos_final.y - pos_begin.y
x_res = np.cos(-pos_begin.theta) * x_tmp - np.sin(
-pos_begin.theta) * y_tmp
y_res = np.sin(-pos_begin.theta) * x_tmp + np.cos(
-pos_begin.theta) * y_tmp
yaw_res = distance_angle(pos_final.theta, pos_begin.theta)
self.estimator.sim.setObjectPose(self.name, x_res, y_res, yaw_res)
cellpos_res = self.estimator.sim.getCellBodyPose(self.name)
pos = [0, 0]
x_tmp = pos_push.x - pos_begin.x
y_tmp = pos_push.y - pos_begin.y
pos[0] = np.cos(-pos_begin.theta) * x_tmp - np.sin(
-pos_begin.theta) * y_tmp
pos[1] = np.sin(-pos_begin.theta) * x_tmp + np.cos(
-pos_begin.theta) * y_tmp
velocity = [0, 0, 0]
vel_x = vec_push.x / duration
vel_y = vec_push.y / duration
velocity[0] = np.cos(-pos_begin.theta) * vel_x - np.sin(
-pos_begin.theta) * vel_y
velocity[1] = np.sin(-pos_begin.theta) * vel_x + np.cos(
-pos_begin.theta) * vel_y
action = {
'pos': pos,
'yaw': 0,
'velocity': velocity,
'duration': duration
}
trans = planner.computeTransition_models(self.name, action)
cellpos0 = planner.getCellPos(self.name + '_0', 0, 0, 0)
probs = []
for i, tran in enumerate(trans):
cellpos1 = planner.getCellPos(self.name + '_0', x_res, y_res,
yaw_res)
err_cell = CellPhysicsBase.distanceCellPos(cellpos0, cellpos1)
probs.append(error2prob(err_cell))
probs_sum = sum(probs)
self.probs_obs.append([prob / float(probs_sum) for prob in probs])
probs = []
for m, prob_model in enumerate(self.probs_models):
prob_obs = 1
for o in range(len(self.probs_obs)):
prob_obs = prob_obs * self.probs_obs[o][m]
probs.append(prob_model * prob_obs)
planner.updateModelProbabilities(self.name, probs)
def findBestAction(name, x, y, yaw, cellpos_goal):
err_min = -1
for action in self.trans_objs[name]:
x_next, y_next, yaw_next, cellpos_next\
= self.lookupTransitionTable(name,x,y,yaw,action)
err = CellPhysicsBase.distanceCellPos(cellpos_next,cellpos_goal)
if err_min==-1 or err_min > err:
err_min = err
ret = (action, err, x_next, y_next, yaw_next, cellpos_next)
return ret
def computeTransition(self, name, action):
key = self.action2key(action)
if key in self.trans_objs[name]:
return self.trans_objs[name][key]
self.sim_objs[name].setObjectPose(name,0,0,0)
cellpos0 = self.sim_objs[name].getCellBodyPose(name)
self.applyAction(self.sim_objs[name],name,0,0,0,action,self.name_actor,self.dims_actor)
x1,y1,yaw1 = self.sim_objs[name].getObjectPose(name)
cellpos1 = self.sim_objs[name].getCellBodyPose(name)
dist = CellPhysicsBase.distanceCellPos(cellpos0,cellpos1)
if dist < 0.001:
return None
return {'vec':[x1,y1],'yaw':yaw1,'cellpos':cellpos1}
def plan_prop(self, name, waypoints, horizon=1, thres=0.03, method='bfs', n_plans=10, actions_rejected=[]):
for name_i in self.name2names[name]:
if len(self.trans_objs[name_i])==0:
self.buildTransitionTable(name_i)
keys_rejected = []
for action in actions_rejected:
keys_rejected.append(self.action2key(action))
x0,y0,yaw0 = waypoints[0]
x1,y1,yaw1 = waypoints[1]
cellpos0 = self.getCellPos(name+'_0',x0,y0,yaw0)
cellpos1 = self.getCellPos(name+'_0',x1,y1,yaw1)
actions = [action for direction in self.actions_objs[name+'_0']\
for actions in self.actions_objs[name+'_0'][direction]\
for action in actions ]
plan_all = []
for action in actions:
if self.action2key(action) in keys_rejected:
continue
cost = self.costAction(None,action)
probs = self.name2probs[name]
err = 0
nexts = []
for i, name_i in enumerate(self.name2names[name]):
x_i,y_i,yaw_i,cellpos_i\
= self.lookupTransitionTable(name_i,x0,y0,yaw0,action)
err_i = CellPhysicsBase.distanceCellPos(cellpos_i,cellpos1)
err = err + err_i*probs[i]
nexts.append((name_i,x_i,y_i,yaw_i,probs[i]))
nexts = sorted(nexts, key=lambda x: -x[-1])
plan_all.append({'action': action, 'error': err, 'nexts':nexts})
plan_all = sorted(plan_all, key=lambda x: x['error'])
plan_top = []
for plan in plan_all:
succ = 0
if self.use_multiproc:
ret = self.pool.map(isActionStable_multiproc,
[(name_i,
self.name2cells[name_i],
x_i, y_i, yaw_i,
plan['action'],
self.name_actor,
self.type_actor,
self.dims_actor)
for name_i,x_i,y_i,yaw_i,_ in plan['nexts']])
for i, is_stable in enumerate(ret):
if is_stable:
prob_i = plan['nexts'][i][-1]
succ = succ + prob_i
else:
for i, nexts in enumerate(plan['nexts']):
name_i, x_i, y_i, yaw_i, prob_i = nexts
if self.isActionStable(name_i,x0,y0,yaw0, plan['action']):
succ = succ + prob_i
plan['succ'] = succ
plan_top.append(plan)
if len(plan_top)>=n_plans:
break
return plan_top
def plan_diff(self, name, waypoints, horizon=1, thres=0.03, pred_step=np.inf):
path = [waypoints[0]]
actions = []
for i in range(1,len(waypoints)):
x0,y0,yaw0 = waypoints[i-1]
x1,y1,yaw1 = waypoints[i]
cellpos0 = self.getCellPos(name,x0,y0,yaw0)
cellpos1 = self.getCellPos(name,x1,y1,yaw1)
if i==len(waypoints)-1:
thres = 0.01
err = np.inf
while err > thres:
# cached simulation result
errs = []
for action in self.trans_objs[name]:
for key, value in action:
if key=='direction':
direction = value
elif key=='idx':
idx = value
elif key=='ldx':
ldx = value
(x_cur,y_cur,yaw_cur,cellpos_cur)\
= self.lookupTransitionTable(name,x0,y0,yaw0,action)
err = CellPhysicsBase.distanceCellPos(cellpos_cur,cellpos1)
errs.append((err,(direction,idx,ldx),(x_cur,y_cur,yaw_cur)))
if len(errs)>0:
err_min = min(errs,key=lambda x: x[0])
errs = [err_min]
# fix the pushing length (ldx)
dist = np.sqrt((x1-x0)*(x1-x0)+(y1-y0)*(y1-y0))
for ldx, length in enumerate(self.pushing_dists):
if dist < length:
break
vec_com2goal = np.array(self.centerOfMass(name,x1,y1,yaw1))\
- np.array(self.centerOfMass(name,x0,y0,yaw0))
np.array(self.centerOfMass(name,x1,y1,yaw1))
for direction in self.actions_objs[name]:
# ignore the opposite direction
vec = [0,0]
vec[0] = np.cos(yaw0)*direction[0]-np.cos(yaw0)*direction[1]
vec[1] = np.sin(yaw0)*direction[0]+np.cos(yaw0)*direction[1]
if vec_com2goal.dot(np.array(vec)) < 0:
continue
# start searching from the point that is closest to the line between center of mass
uvec_c2c = self.uvec_contact2com[name][direction]
vec = np.zeros(uvec_c2c.shape)
vec[:,0] = np.cos(yaw0)*uvec_c2c[:,0]-np.sin(yaw0)*uvec_c2c[:,1]
vec[:,1] = np.sin(yaw0)*uvec_c2c[:,0]+np.cos(yaw0)*uvec_c2c[:,1]
projs = vec.dot(vec_com2goal)
idx = np.argmax(projs)
action = self.actions_objs[name][direction][idx][ldx]
(x_cur,y_cur,yaw_cur,cellpos_cur)\
= self.lookupTransitionTable(name,x0,y0,yaw0,action)
err_cur = CellPhysicsBase.distanceCellPos(cellpos_cur,cellpos1)
errs.append((err_cur,(direction,idx,ldx),(x_cur,y_cur,yaw_cur)))
for incr in [-1,1]:
j = idx + incr
while 0<=j and j<len(self.actions_objs[name][direction]):
actioni = self.actions_objs[name][direction][j][ldx]
(xi,yi,yawi,cellposi)\
= self.lookupTransitionTable(name,x0,y0,yaw0,actioni)
erri = CellPhysicsBase.distanceCellPos(cellposi,cellpos1)
if erri > err_cur:
break
errs.append((erri,(direction,j,ldx),(xi,yi,yawi)))
err_cur = erri
j = j + incr
# fix the contact point (direction,idx)
err_min = min(errs,key=lambda x: x[0])
(err,(direction,idx,ldx),(x,y,yaw)) = err_min
errs = [err_min]
err_cur = err
for incr in [-1,1]:
l = ldx + incr
while 0<=l and l<len(self.pushing_dists):
actionl = self.actions_objs[name][direction][idx][l]
(xl,yl,yawl,cellposl)\
= self.lookupTransitionTable(name,x0,y0,yaw0,actionl)
errl = CellPhysicsBase.distanceCellPos(cellposl,cellpos1)
if errl > err_cur:
break
errs.append((errl,(direction,idx,l),(xl,yl,yawl)))
err_cur = errl
l = l + incr
err_min = min(errs,key=lambda x: x[0])
(err,(direction,idx,ldx),(x,y,yaw)) = err_min
action = self.actions_objs[name][direction][idx][ldx]
self.applyAction(self.sim,name,x0,y0,yaw0,action,self.name_actor,self.dims_actor)
(x,y,yaw) = self.sim.getObjectPose(name)
actions.append(action)
path.append((x,y,yaw))
(x0,y0,yaw0) = (x,y,yaw)
cellpos0 = self.getCellPos(name,x0,y0,yaw0)
print('# of simulation: {}/{}, err: {}'.format(len(self.trans_objs[name]),self.n_actions_objs[name],err))
if len(actions) >= pred_step:
break
if len(actions) >= pred_step:
break
return path, actions
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))
fp_save = os.path.join(args.outdir,'%s_%s_init%d.pkl'%(method,obj,i))
if args.skip and os.path.isfile(fp_save):
print('[SKIP] %s' % fp_save)
with open(fp_save,'rb') as f:
tmp = pickle.load(f)
succs.append(tmp['success'])
'%s_%s_param%d_initgoal%d.pkl'%\
(method,obj,idx_param,idx_initgoal))
if args.skip and os.path.isfile(fp_save):
print('[SKIP] %s' % fp_save)
continue
print('method: {}, param: {}, init: {}, goal: {}'.format(method,param,init,goal))
time_beg = time.time()
planner = CellPhysicsPlanner(obj, img, [5-0.1]*4, meter_per_pixel)
sim = SimBullet(gui=False)
sim.setupCamera(0.75,-90,-89.99, 0.63,0,0)
sim.addObject('plane','plane',[1,1,1,1])
sim.addObject('finger','finger',[1,1,0,1])
cells = CellPhysicsBase.createCells(img,param['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])
history = {'path':[], 'actions':[]}
while True:
x0,y0,yaw0 = sim.getObjectPose(obj)
cellpos0 = sim.getCellBodyPose(obj)
err_xy = np.linalg.norm([goal[0]-x0,goal[1]-y0])
err_yaw = distance_angle(goal[2],yaw0)
err_cell = CellPhysicsBase.distanceCellPos(cellpos_goal, cellpos0)
cellinfos = planner.getProperties()
cellinfos_copy = [info.copy() for info in cellinfos]
history['path'].append({'x':x0,'y':y0,'yaw':yaw0,'cellinfos':cellinfos_copy,