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 __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))
class KinoPlanner:
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 addDefaultObjects(self, sim):
sim.addObject('table',self.setup['table_type'],[1,1,1,1])
sim.setObjectPose('table',0.63,0,0)
sim.addObject(self.name_actor,self.type_actor,self.color_actor)
def createState(self, x, y, yaw):
state = ob.State(self.space)
state().setX(x)
state().setY(y)
state().setYaw(yaw)
return state
@staticmethod
def action2key(action):
return tuple(action.items()) if type(action)==dict else action
def genActions(self, cells, interval):
cell_size = cells['cell_size']
meter_per_pixel = 0.001
img = np.zeros([500,500],dtype=np.uint8)
for cell in cells['cells']:
pt1 = np.array([-cell['pos'][1],-cell['pos'][0]]) - cell_size
pt2 = pt1 + cell_size*2
pt1 = (pt1/float(meter_per_pixel) + np.array([img.shape[1],img.shape[0]])*0.5).astype(int)
pt2 = (pt2/float(meter_per_pixel) + np.array([img.shape[1],img.shape[0]])*0.5).astype(int)
cv2.rectangle(img, tuple(pt1), tuple(pt2), (255,255,255))
points = {(1,0):[], (-1,0):[], (0,1):[], (0,-1):[]}
for c in range(img.shape[1]):
for r in range(img.shape[0]):
if img[r,c] > 0:
x = (-(r-img.shape[0]*0.5)*meter_per_pixel)
y = (-(c-img.shape[1]*0.5)*meter_per_pixel)
points[(-1,0)].append((x,y))
break
for c in range(img.shape[1]):
for r in range(img.shape[0]-1,-1,-1):
if img[r,c] > 0:
x = (-(r-img.shape[0]*0.5)*meter_per_pixel)
y = (-(c-img.shape[1]*0.5)*meter_per_pixel)
points[(1,0)].append((x,y))
break
for r in range(img.shape[0]):
for c in range(img.shape[1]):
if img[r,c] > 0:
x = (-(r-img.shape[0]*0.5)*meter_per_pixel)
y = (-(c-img.shape[1]*0.5)*meter_per_pixel)
points[(0,-1)].append((x,y))
break
for r in range(img.shape[0]):
for c in range(img.shape[1]-1,-1,-1):
if img[r,c] > 0:
x = (-(r-img.shape[0]*0.5)*meter_per_pixel)
y = (-(c-img.shape[1]*0.5)*meter_per_pixel)
points[(0,1)].append((x,y))
break
interval_px = int(interval/float(meter_per_pixel))
for key in points:
n_points = len(points[key])
n_contacts = n_points // interval_px - 2
if n_contacts <= 0:
n_contacts = 1
# [KUKA]
#if key==(0,-1) or key==(0,1):
# n_contacts = 1
i_beg = n_points//2 - (n_contacts-1)//2*interval_px
i_end = n_points//2 + (n_contacts-1)//2*interval_px
contacts = []
for i in range(i_beg,i_end+1,interval_px):
contacts.append(points[key][i])
points[key] = contacts
print('Generating Actions ... ')
actions = {}
for direction in points:
actions[direction] = []
for pos in points[direction]:
actions[direction].append([])
for length in self.pushing_dists:
velocity = tuple(np.array(direction) * length)
action = {'pos':pos, 'velocity':velocity, 'duration':1.0,
'direction':direction,
'idx':len(actions[direction])-1,
'ldx':len(actions[direction][-1]) }
actions[direction][-1].append(action)
print('direction {:<10}: {:>4} x {:>4} (# of contacts) x (# of pushing length)'.format(direction,len(actions[direction]),len(self.pushing_dists)))
return actions
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 updateObjectProperties(self, name, cellprop):
self.trans_objs[name] = {} # clear
self.sim_cols[name].setCellBodyProperties(name, cellprop)
self.sim_stbs[name].setCellBodyProperties(name, cellprop)
self.sim_objs[name].setCellBodyProperties(name, cellprop)
if self.debug:
self.sim_debug.setCellBodyProperties(name, cellprop)
com = np.array([0,0])
mass_sum = 0
self.sim_objs[name].setObjectPose(name,0,0,0)
cellpos = self.sim_objs[name].getCellBodyPose(name)
cellprop = self.sim_objs[name].getCellBodyProperties(name)
for pos, prop in zip(cellpos, cellprop):
com[0] = pos[0] * prop['mass']
com[1] = pos[1] * prop['mass']
mass_sum = mass_sum + prop['mass']
com[0] = com[0] / float(mass_sum)
com[1] = com[1] / float(mass_sum)
self.com_objs[name] = com
for direction in self.actions_objs[name]:
uvecs = []
for actions in self.actions_objs[name][direction]:
vec = com - np.array(actions[-1]['pos'])
uvecs.append(vec/np.linalg.norm(vec))
self.uvec_contact2com[name][direction] = np.array(uvecs)
def updateModelProbabilities(self, name, probs):
probs_sum = sum(probs)
self.name2probs[name] = [prob/float(probs_sum) for prob in probs]
@staticmethod
def applyAction(sim, name, x, y, yaw, action, name_actor, dims_actor):
for key, value in action.items() if type(action)==dict else action:
if key=='pos':
pos = [0,0]
pos[0] = np.cos(yaw)*value[0]-np.sin(yaw)*value[1] + x
pos[1] = np.sin(yaw)*value[0]+np.cos(yaw)*value[1] + y
elif key=='velocity':
velocity = [0,0,0]
velocity[0] = np.cos(yaw)*value[0]-np.sin(yaw)*value[1]
velocity[1] = np.sin(yaw)*value[0]+np.cos(yaw)*value[1]
elif key=='duration':
duration = value
length = np.linalg.norm(velocity)
sim.setObjectPose(name,x,y,yaw)
sim.setObjectPose(name_actor,
pos[0]-velocity[0]/float(length)*dims_actor[0],\
pos[1]-velocity[1]/float(length)*dims_actor[1],yaw)
#duration = duration + self.dims_actor[0] / length
sim.applyConstantVelocity(name_actor,velocity,duration)
def getCellPos(self, name, x, y, yaw):
self.sim_objs[name].setObjectPose(name, x, y, yaw)
return self.sim_objs[name].getCellBodyPose(name)
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 computeTransition_models(self, name, action):
names = self.name2names[name]
if self.use_multiproc:
ret = self.pool.map(computeTransition_multiproc,
[(name_i,
self.name2cells[name],
action,
self.name_actor,
self.type_actor,
self.dims_actor) for name_i in names])
else:
ret = []
for name_i in names:
ret.append(self.computeTransition(name_i, action))
return ret
def buildTransitionTable(self, name):
print('Building a transition table for \"%s\" ...' % name)
prog = progressbar.ProgressBar(widgets=[progressbar.Percentage(), progressbar.Bar()], maxval=self.n_actions_objs[name])
prog.start()
if self.use_multiproc:
actions_all = []
for direction in self.actions_objs[name]:
for actions in self.actions_objs[name][direction]:
actions_all = actions_all + actions
ret = self.pool.map(computeTransition_multiproc,
[(name,
self.name2cells[name],
action,
self.name_actor,
self.type_actor,
self.dims_actor) for action in actions_all])
for i, action in enumerate(actions_all):
if ret[i] is None:
continue
key = self.action2key(action)
self.trans_objs[name][key] = ret[i]
prog.update(len(self.trans_objs[name])-1)
else:
for direction in self.actions_objs[name]:
for actions in self.actions_objs[name][direction]:
for action in actions:
key = self.action2key(action)
tran = self.computeTransition(name, action)
if tran is not None:
self.trans_objs[name][key] = tran
prog.update(len(self.trans_objs[name])-1)
prog.finish()
def lookupTransitionTable(self, name, x, y, yaw, action):
key = self.action2key(action)
if key in self.trans_objs[name]:
tran = self.trans_objs[name][key]
else:
tran = self.computeTransition(name, action)
if tran is not None:
self.trans_objs[name][key] = tran
else:
cellpos = self.getCellPos(name,x,y,yaw)
return (x,y,yaw,cellpos)
vec = tran['vec']
x_next = np.cos(yaw)*vec[0]-np.sin(yaw)*vec[1] + x
y_next = np.sin(yaw)*vec[0]+np.cos(yaw)*vec[1] + y
yaw_next = yaw + tran['yaw']
cellpos_next = self.getCellPos(name,x_next,y_next,yaw_next)
return (x_next,y_next,yaw_next,cellpos_next)
def costAction(self, action_prv, action):
for key, value in action.items() if type(action)==dict else action:
if key=='pos':
pos = value
elif key=='velocity':
velocity = value
elif key=='duration':
duration = value
#cost = np.linalg.norm(velocity) * duration
cost = 0.1
if action_prv is None:
return cost
for key, value in action_prv.items() if type(action_prv)==dict else action_prv:
if key=='pos':
pos_prv = value
break
length = np.linalg.norm(np.array(pos)-np.array(pos_prv))
return 0.1 + length + 0.1 + cost
def centerOfMass(self, name, x, y, yaw):
cx = np.cos(yaw)*self.com_objs[name][0]-np.sin(yaw)*self.com_objs[name][1] + x
cy = np.sin(yaw)*self.com_objs[name][0]+np.cos(yaw)*self.com_objs[name][1] + y
return (cx,cy)
def isStateValid(self, state):
x = state.getX()
y = state.getY()
yaw = state.getYaw()
# [KUKA]
#dist = np.linalg.norm([x,y])
#if dist < 0.25 or dist > 0.80:
# return False
cx,cy = self.centerOfMass(self.name_target, x,y,yaw)
dist = np.linalg.norm([cx-0.63,cy])
if dist > self.setup['table_radius'] + 0.25:
return False
self.sim_cols[self.name_target].setObjectPose(self.name_target, x, y, yaw)
if self.sim_cols[self.name_target].isObjectCollide(self.name_target)==True:
return False
return True
def isObjectStable(self, name, x,y,yaw):
if (x,y,yaw) in self.stbs_objs[name]:
return self.stbs_objs[name][(x,y,yaw)]
cx,cy = self.centerOfMass(name, x,y,yaw)
dist = np.linalg.norm([cx-0.63,cy])
if dist < 0.40:
return True
self.sim_stbs[name].setObjectPose(name,x,y,yaw)
(x0,y0,z0),q0 = self.sim_stbs[name].getObjectPose(name,ndims=3)
for i in range(100):
self.sim_stbs[name].stepSimulation()
(x1,y1,z1),q1 = self.sim_stbs[name].getObjectPose(name,ndims=3)
q0 = q0.normalised
q1 = q1.normalised
dist_q = 1 - np.dot(q0.q,q1.q)**2
ret = z1>0 or dist_q<0.003 # <10degree
self.stbs_objs[name][(x,y,yaw)] = ret
return ret
def isActionStable(self, name, x,y,yaw, action):
x_next, y_next, yaw_next, cellpos_next\
= self.lookupTransitionTable(name,x,y,yaw,action)
if np.linalg.norm([x -0.63,y ]) < 0.40 and\
np.linalg.norm([x_next-0.63,y_next]) < 0.40:
return True
self.sim_stbs[name].setObjectPose(name,x,y,yaw)
(x0,y0,z0),q0 = self.sim_stbs[name].getObjectPose(name,ndims=3)
self.applyAction(self.sim_stbs[name], \
name,x,y,yaw, \
action,self.name_actor,self.dims_actor)
(x1,y1,z1),q1 = self.sim_stbs[name].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 selectGoals(self, name, goals):
names = self.name2names[name]
probs = self.name2probs[name]
goals_ret = []
for goal in goals:
prob = 0
for i in range(len(names)):
if self.isObjectStable(names[i], goal[0], goal[1], goal[2]):
prob = prob + probs[i]
if prob > 0.90:
goals_ret.append(goal)
return goals_ret
@staticmethod
def split( path, xy_min=0.05, yaw_min=15.0/180.0*np.pi, method='rotNpushNrot' ):
res = [path[0]]
for i in range(1,len(path)):
x0,y0,yaw0 = path[i-1]
x1,y1,yaw1 = path[i]
if method=='smooth':
vec_xy = np.array([x1-x0,y1-y0])
vec_yaw = distance_angle(yaw1,yaw0)
dist_xy = np.linalg.norm(vec_xy)
dist_yaw = abs(vec_yaw)
n_xy = int(np.ceil(dist_xy / xy_min))
n_yaw = int(np.ceil(dist_yaw / yaw_min))
n_split = n_xy if n_xy > n_yaw else n_yaw
vec_xy = vec_xy / n_split
vec_yaw = vec_yaw / n_split
for i in range(n_split):
res.append([x0+vec_xy[0]*(i+1),
y0+vec_xy[1]*(i+1),
yaw0+vec_yaw*(i+1)])
elif method=='rotNpushNrot':
vec_xy = np.array([x1-x0,y1-y0])
dist_xy = np.linalg.norm(vec_xy)
yaw01 = math.acos(vec_xy[1]/dist_xy)
vec_yaw = distance_angle(yaw01,yaw0)
dist_yaw = abs(vec_yaw)
n_yaw = int(np.ceil(dist_yaw / yaw_min))
if n_yaw > 0:
vec_yaw = vec_yaw / n_yaw
for i in range(n_yaw):
res.append([x0,y0,yaw0+vec_yaw*(i+1)])
vec_xy = np.array([x1-x0,y1-y0])
dist_xy = np.linalg.norm(vec_xy)
n_xy = int(np.ceil(dist_xy / xy_min))
if n_xy > 0:
vec_xy = vec_xy / n_xy
for i in range(n_xy):
res.append([x0+vec_xy[0]*(i+1),
y0+vec_xy[1]*(i+1), yaw01])
vec_yaw = distance_angle(yaw1,yaw01)
dist_yaw = abs(vec_yaw)
n_yaw = int(np.ceil(dist_yaw / yaw_min))
if n_yaw > 0:
vec_yaw = vec_yaw / n_yaw
for i in range(n_yaw):
res.append([x1,y1,yaw01+vec_yaw*(i+1)])
return res
def plan_rrt(self, name, init, goal):
init = self.createState(*tuple(init))
goal = self.createState(*tuple(goal))
self.name_target = name
planner = og.RRTstar(self.si)
ss = og.SimpleSetup(self.si)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(self.isStateValid))
ss.setPlanner(planner)
ss.setStartAndGoalStates(init, goal)
solved = ss.solve(1.0)
if solved:
ss.simplifySolution()
path = ss.getSolutionPath()
path_res = []
for state in path.getStates():
path_res.append((state.getX(),state.getY(),state.getYaw()))
return path_res
else:
return None
def plan_guided(self, name, path_guide, pred_step=np.inf):
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
waypoints = self.split(path_guide)
path_res = [path_guide[0]]
actions_res = []
i_cur = 0
x_cur, y_cur, yaw_cur = path_guide[0]
while i_cur < len(waypoints)-1:
i_min = -1
err_min = -1
for i_next in range(min(i_cur+5,len(waypoints)-1),i_cur,-1):
x,y,yaw = waypoints[i_next]
cellpos = self.getCellPos(name,x,y,yaw)
action_next, err, x_next, y_next, yaw_next, cellpos_next\
= findBestAction(name,x_cur,y_cur,yaw_cur,cellpos)
if err_min==-1 or err_min > err:
err_min = err
i_min = i_next
next_min = (action_next,x_next,y_next,yaw_next,cellpos_next)
cellpos_guide = cellpos
i_cur = i_min
action_cur,x_cur,y_cur,yaw_cur,cellpos_next = next_min
path_res.append([x_cur,y_cur,yaw_cur])
actions_res.append(action_cur)
if len(actions_res) >= pred_step:
break
return path_res, actions_res
def plan_bfs(self, name, waypoints, horizon=1, thres=0.03):
if horizon==0:
return [waypoints[0]], [], 0, 0
if len(self.trans_objs[name])==0:
self.buildTransitionTable(obj)
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)
x0,y0,yaw0 = waypoints[0]
x1,y1,yaw1 = waypoints[1]
cellpos0 = self.getCellPos(name,x0,y0,yaw0)
cellpos1 = self.getCellPos(name,x1,y1,yaw1)
path = [waypoints[0]]
actions = []
cost = 0
i = 1
err_prv = np.inf
while True:
err_res, path_res, actions_res, cost_res\
= get_best_action(x0,y0,yaw0,cellpos1,[],[],0,horizon)
if self.debug:
for action in actions_res:
self.applyAction(self.sim_debug,name,x0,y0,yaw0,action,self.name_actor,self.dims_actor)
(x0,y0,yaw0) = self.sim_debug.getObjectPose(name)
actions.append(action)
path.append((x0,y0,yaw0))
cost = cost + cost_res
else:
x0,y0,yaw0 = path_res[-1]
actions = actions + actions_res
path = path + path_res
cost = cost + cost_res
if len(actions) >= horizon:
break
cellpos0 = self.getCellPos(name,x0,y0,yaw0)
if err_res < thres:
i = i + 1
if i<len(waypoints):
x1,y1,yaw1 = waypoints[i]
cellpos1 = self.getCellPos(name,x1,y1,yaw1)
else:
break
elif i>=len(waypoints)-1:
if err_prv < err_res:
break
else:
err_prv = err_res
return path, actions
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
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 draw_cellpos(self, ax, cellpos, color=[0,0,0], marker='s'):
cellpos = np.array(cellpos)
ax.plot(-cellpos[:,2],cellpos[:,1],'.',marker=marker,color=color)
def draw_action(self, ax, x,y,yaw, action, color='g', marker='o'):
for key, value in action.items() if type(action)==dict else action:
if key=='pos':
pos = value
elif key=='velocity':
velocity = value
tmp = [0,0]
tmp[0] = np.cos(yaw)*pos[0]-np.sin(yaw)*pos[1] + x
tmp[1] = np.sin(yaw)*pos[0]+np.cos(yaw)*pos[1] + y
ax.plot(-tmp[1],tmp[0],'.',color=color,marker=marker)
tmp2 = [0,0]
tmp2[0] = np.cos(yaw)*velocity[0]-np.sin(yaw)*velocity[1] + tmp[0]
tmp2[1] = np.sin(yaw)*velocity[0]+np.cos(yaw)*velocity[1] + tmp[1]
ax.plot([-tmp[1],-tmp2[1]],[tmp[0],tmp2[0]],'-',color=color,marker=marker)
def draw_plan(self, ax, name, path, actions):
circle = plt.Circle((0,0.63),0.5, color='k', fill=False)
ax.add_artist(circle)
clrs = np.linspace(0.3,1,len(path)+1)
for i in range(len(path)-1):
cellpos = self.getCellPos(name,path[i][0],path[i][1],path[i][2])
self.draw_cellpos(ax,cellpos,color=(clrs[i+1],0,0))
action = actions[i]
self.draw_action(ax,path[i][0],path[i][1],path[i][2], action )
cellpos = self.getCellPos(name,path[-1][0],path[-1][1],path[-1][2])
self.draw_cellpos(ax,cellpos,color=(clrs[-1],0,0))
ax.set_aspect('equal')
def plan(self, name, init, goal, method='diff', pred_step=np.inf, thres=0.03, n_plans=10, waypoints=None, actions_rejected=[]):
if waypoints is None:
if method=='prop':
waypoints = self.plan_rrt(name+'_0', init, goal)
else:
waypoints = self.plan_rrt(name, init, goal)
#print('waypoints: ',waypoints)
if method=='guided':
path_res, actions_res = self.plan_guided(name, waypoints, horizon=pred_step, thres=thres)
elif method=='bfs':
path_res, actions_res = self.plan_bfs(name, waypoints, horizon=pred_step, thres=thres)
elif method=='diff':
path_res, actions_res = self.plan_diff(name, waypoints, horizon=pred_step, thres=thres)
elif method=='prop':
plans = self.plan_prop(name, waypoints, horizon=1, thres=thres, n_plans=n_plans, actions_rejected=actions_rejected)
for plan in plans:
if plan['succ'] >= 1.00:
return [init, plan['nexts'][0][1:4]], [plan['action']]
plan_max = max(plans, key=lambda x: -x['succ'])
return [init, plan_max['nexts'][0][:3]], [plan_max['action']]
if self.debug:
fig = plt.figure()
ax = fig.add_subplot(111)
self.draw_plan(ax, name, path_res, actions_res)
plt.show()
fig.savefig(os.path.join('./',method+'.png'))
self.sim_debug.recordStart(os.path.join('./',method+'.mp4'))
self.sim_debug.setupCamera(0.75,-90,-89.99, 0.63,0,0)
x,y,yaw = init
for action in actions_res:
self.applyAction(self.sim_debug,name,x,y,yaw,action,self.name_actor,self.dims_actor)
x,y,yaw = self.sim_debug.getObjectPose(name)
self.sim_debug.recordStop()
return (path_res, actions_res)
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 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 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):
'targets': [obj],
obj: {
'pos': [0, 0],
'yaw': 0,
'pos_res': [x_res, y_res],
'yaw_res': yaw_res
}
}
setup['real'].append(action)
if True:
print(file)
print('push_x:', push_x)
print('push_y:', push_y)
sim = SimBullet(gui=False)
sim._p.setGravity(0, 0, -9.8)
sim.setupRendering(0.75, -90, -89.99, 500, 500)
sim.addObject('finger0', 'finger', [1, 1, 0, 1])
sim.addObject('finger1', 'finger', [1, 0, 1, 1])
sim.addObject('table_rect', 'table_rect', [1, 1, 1, 1])
img_label = cv2.imread(fp_label, cv2.IMREAD_UNCHANGED)
cells = CellPhysicsBase.createCells(
img_label, [0, 0, 0, 0], meter_per_pixel=0.53 / 640.0)
sim.addObject(obj + '0', cells, [0, 0, 1, 1])
sim.addObject(obj + '1', cells, [1, 0, 0, 1])
sim.setObjectPose('finger0', push_x, push_y, 0)
sim.setObjectPose('finger1',
push_x + float(node['push']['vec']['x']),
push_y, 0)
sim.setObjectPose(obj + '0', 0, 0, 0)
init = initgoals[idx_initgoal][0]
goal = initgoals[idx_initgoal][1]
for method in args.method:
fp_save = os.path.join(args.outdir,
'%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)