def callback():
if time.time() >= data['next_sim_time']:
sim.simulate(0.01)
if sim.controller(0).remainingTime() <= 0:
print("Executing timed trajectory")
trajectory.execute_trajectory(traj,sim.controller(0),smoothing='pause')
vis.setItemConfig("robot",sim.controller(0).getCommandedConfig())
data['next_sim_time'] += 0.01
def reset():
vis.lock()
robot.setConfig(q0)
base_link.setParentTransform(
*se3.mul(se3.inv(parent_xform), base_xform0))
vis.unlock()
vis.add("base_xform", base_xform0)
vis.edit("base_xform")
vis.setItemConfig("gripper", grob.getConfig())
def goToWidget():
#first, set all constraints so they are fit at the robot's last solved configuration, and get the
#current workspace coordinates
vis.setItemConfig("ghost1", self.goalConfig)
self.robot.setConfig(self.goalConfig)
for e, d in zip(self.endeffectors, self.constraints):
d.matchDestination(*self.robot.link(e).getTransform())
wcur = config.getConfig(self.constraints)
wdest = []
for e in self.endeffectors:
xform = vis.getItemConfig("ee_" + robot.link(e).getName())
wdest += xform
print "Current workspace coords", wcur
print "Dest workspace coords", wdest
#Now interpolate
self.robot.setConfig(self.goalConfig)
traj1 = cartesian_trajectory.cartesian_interpolate_linear(
robot,
wcur,
wdest,
self.constraints,
delta=1e-2,
maximize=False)
self.robot.setConfig(self.goalConfig)
traj2 = cartesian_trajectory.cartesian_interpolate_bisect(
robot, wcur, wdest, self.constraints, delta=1e-2)
self.robot.setConfig(self.goalConfig)
#traj3 = cartesian_trajectory.cartesian_path_interpolate(robot,[wcur,wdest],self.constraints,delta=1e-2,method='any',maximize=False)
traj3 = None
print "Method1 Method2 Method3:"
print " ", (traj1 != None), (traj2 != None), (traj3 != None)
if traj1: traj = traj1
elif traj2: traj = traj2
elif traj3: traj = traj3
else:
traj = cartesian_trajectory.cartesian_interpolate_linear(
robot,
wcur,
wdest,
self.constraints,
delta=1e-2,
maximize=True)
if traj:
print "Result has", len(traj.milestones), "milestones"
self.goalConfig = traj.milestones[-1]
vis.setItemConfig(("world", world.robot(0).getName()),
self.goalConfig)
vis.animate(("world", world.robot(0).getName()),
traj,
speed=0.2,
endBehavior='loop')
vis.setItemConfig("ghost2", traj.milestones[-1])
vis.add("ee_trajectory", traj)
self.refresh()
def rotate(self,radians):
robot = self.world.robot(0)
object = self.world.rigidObject(0)
robot.setConfig(vis.getItemConfig("robot"))
T = vis.getItemConfig("rigidObject")
object.setTransform(T[:9],T[9:])
self.angle += radians
T = (so3.rotation((0,1,0),radians),[0.0]*3)
Tbase = moving_base.get_moving_base_xform(robot)
moving_base.set_moving_base_xform(robot,*se3.mul(T,Tbase))
Tobj = object.getTransform()
object.setTransform(*se3.mul(T,Tobj))
#update visualization
vis.setItemConfig("robot",robot.getConfig())
vis.setItemConfig("rigidObject",sum(object.getTransform(),[]))
vis.setItemConfig("COM",se3.apply(object.getTransform(),object.getMass().getCom()))
coordinates.updateFromWorld()
self.refresh()
def load_config(self,name):
object = self.world.rigidObject(0)
self.config_name = name
print "Loading from",name+".config,",name+".xform","and",name+".array"
qrob = resource.get(name+".config",type="Config",doedit=False)
Tobj = resource.get(name+".xform",type="RigidTransform",doedit=False)
self.contact_units = resource.get(name+"_units.array",type="IntArray",default=[],doedit=False)
if len(self.contact_units) == 0:
print "COULDNT LOAD "+name+"_units.array, trying "+name+".array"
contact_links = resource.get(name+".array",type="IntArray",default=[],doedit=False)
if len(contact_links) > 0:
robot = self.world.robot(0)
contact_links = [robot.link(l).getName() for l in contact_links]
self.contact_units = []
for i,(link,unit) in enumerate(self.hand.all_units):
if link in contact_links:
self.contact_units.append(i)
print "UNITS",self.contact_units
object.setTransform(*Tobj)
qobj = vis.getItemConfig("rigidObject")
vis.setItemConfig("robot",qrob)
vis.setItemConfig("rigidObject",qobj)
vis.setItemConfig("COM",se3.apply(object.getTransform(),object.getMass().getCom()))
coordinates.updateFromWorld()
Tb = [euler_angle_to_rotation(eb),[1,0,1]]
def update_interpolation(u):
#linear interpolation with euler angles
e = interpolate_euler_angles(ea,eb,u)
t = interpolate_linear(Ta[1],Tb[1],u)
return (euler_angle_to_rotation(e),t)
if __name__ == "__main__":
#draw the world reference frame, the start and the goal, and the interpolated frame
vis.add("world",se3.identity())
vis.add("start",Ta)
vis.add("end",Tb)
vis.add("interpolated",Ta)
vis.setAttribute("world","length",0.25)
vis.setAttribute("interpolated","fancy",True)
vis.setAttribute("interpolated","width",0.03)
vis.setAttribute("interpolated","length",1.0)
period = 3.0
t0 = time.time()
vis.show()
while vis.shown():
#repeat the interpolation every 3 seconds
u = ((time.time()-t0)%period)/period
T = update_interpolation(u)
vis.setItemConfig("interpolated",T)
time.sleep(0.01)
vis.kill()
robot = world.robot(0)
link = robot.link(7)
#coordinates of the constrained IK point on the robot, in the link's local frame
localpos = [0.17,0,0]
#coordinates of the IK goal in world coordinates
position = [0,0,0]
t0 = time.time()
#draw the world, local point in yellow, and target point in white
vis.add("world",world)
vis.add("local_point",position)
vis.setColor("local_point",1,1,0)
vis.add("target",position)
vis.setColor("target",1,1,1)
vis.edit("target")
vis.show()
while vis.shown():
vis.lock()
#move the target and solve
t = time.time()-t0
r = 0.4
position = vis.getItemConfig("target")
position[1] = r*math.cos(t)
position[2] = 0.7+r*math.sin(t)
q = solve_ik(link,localpos,position)
robot.setConfig(q)
vis.setItemConfig("local_point",link.getWorldPosition(localpos))
vis.setItemConfig("target",position)
vis.unlock()
time.sleep(0.01)
vis.kill()
q = vis.getItemConfig(path)
T = (q[:9],q[9:])
for c,p in zip(constraints,pairs):
if p[1].getName() == path[1]:
c.env.setTransform(T)
q0 = robot.getConfig()
robot.setConfig(qinit)
#qcollfree,trace,cps = geometryopt.optimizeCollFreeRobot(robot,obstacles,constraints=constraints,qinit='random-collision-free',qdes=q0,verbose=VERBOSE,settings=settings,
# want_trace=True,want_times=False,want_constraints=True)
qcollfree,trace,cps = geometryopt.optimizeCollFreeRobot(robot,obstacles,constraints=constraints,qinit=None,qdes=q0,verbose=VERBOSE,settings=settings,
want_trace=True,want_times=False,want_constraints=True)
#vis.add("transformTrace",trace)
assert len(qcollfree) == robot.numLinks()
#robot.setConfig(qcollfree)
vis.setItemConfig("qsoln",qcollfree)
if geometryopt.TEST_PYCCD:
gx = [c(qcollfree) for c in constraints]
else:
gx = [c.eval_minimum(qcollfree) for c in constraints]
feasible = all([v >= 0 for v in gx])
if feasible:
vis.setColor("qsoln",0,1,0,0.5)
else:
vis.setColor("qsoln",1,0,0,0.5)
#initialize the next step from the last solved configuration
qinit = qcollfree
"""
#debug printing
sim.simulate(0)
trajectory.execute_path(traj.milestones,sim.controller(0))
vis.setWindowTitle("Simulating path using trajectory.execute_trajectory")
if vis.multithreaded():
#for some tricky Qt reason, need to sleep before showing a window again
#Perhaps the event loop must complete some extra cycles?
time.sleep(0.01)
vis.show()
t0 = time.time()
while vis.shown():
#print "Time",sim.getTime()
sim.simulate(0.01)
if sim.controller(0).remainingTime() <= 0:
print("Executing timed trajectory")
trajectory.execute_trajectory(traj,sim.controller(0),smoothing='pause')
vis.setItemConfig("robot",sim.controller(0).getCommandedConfig())
t1 = time.time()
time.sleep(max(0.01-(t1-t0),0.0))
t0 = t1
else:
data = {'next_sim_time':time.time()}
def callback():
if time.time() >= data['next_sim_time']:
sim.simulate(0.01)
if sim.controller(0).remainingTime() <= 0:
print("Executing timed trajectory")
trajectory.execute_trajectory(traj,sim.controller(0),smoothing='pause')
vis.setItemConfig("robot",sim.controller(0).getCommandedConfig())
data['next_sim_time'] += 0.01
vis.loop(callback=callback,setup=vis.show)
print("Ending vis.")
else:
dmin = qmin[driver.getAffectedLink()]
dmax = qmax[driver.getAffectedLink()]
if dmax - dmin > math.pi * 2:
if d0[index] > math.pi:
dmin = 0
dmax = math.pi * 2
else:
dmin = -math.pi
dmax = math.pi
uswitch1 = (dmax - d0[index]) / (dmax - dmin) * 0.5
uswitch2 = uswitch1 + 0.5
vis.lock()
if u < uswitch1:
driver.setValue(d0[index] + u / uswitch1 * (dmax - d0[index]))
elif u < uswitch2:
driver.setValue(dmax + (u - uswitch1) / 0.5 * (dmin - dmax))
elif u < 1:
driver.setValue(dmin + (u - uswitch1 - 0.5) / (1 - uswitch1 - 0.5) *
(d0[index] - dmin))
else:
driver.setValue(d0[index])
vis.setItemConfig("robot", r.getConfig())
vis.unlock()
if u > 1:
index += 1
index = index % r.numDrivers()
period_start += period
time.sleep(0.01)
vis.kill()
def keyboardfunc(self,c,x,y):
if c == 'h':
print "Help: "
print "- s: save grasp configuration to disk"
print "- l: load grasp configuration from disk"
print "- <: rotate left"
print "- >: rotate right"
print "- [space]: solve for stability"
print "- f: solve for the feasible force volume"
print "- w: solve for the feasible wrench space"
print "- f: solve for the robust force volume"
print "- m: save the current volume to a mesh"
print "- M: load the current volume from a mesh"
print "- 6: calculates the 6D wrench space"
print "- 7: loads the 6D wrench space"
print "- 0: zeros the robot's base transform"
print "- 9: hides the widgets"
if c == 's':
name = raw_input("Enter a name for this configuration > ")
if name.strip() == '':
print "Not saving..."
else:
self.save_config(name)
self.config_name = name
elif c == 'l':
vis.animate("rigidObject",None)
name = raw_input("Enter a name for the configuration to load > ")
if name.strip() == '':
print "Not loading..."
else:
self.load_config(name)
elif c == ',' or c == '<':
vis.animate("rigidObject",None)
self.rotate(math.radians(1))
print "Now at angle",math.degrees(self.angle)
elif c == '.' or c == '>':
vis.animate("rigidObject",None)
self.rotate(-math.radians(1))
print "Now at angle",math.degrees(self.angle)
elif c == ' ':
self.solve(self.contact_units)
elif c == 'f':
if self.wrenchSpace6D != None:
cm = se3.apply(self.world.rigidObject(0).getTransform(),self.world.rigidObject(0).getMass().getCom())
self.wrenchSpace = self.wrenchSpace6D.getSlice(cm).convex_decomposition[0]
self.wrench_space_gl_object = None
else:
self.wrenchSpace = self.calculate_force_volume((0,0,0))
self.wrenchSpace6D = None
self.wrench_space_gl_object = None
elif c == 'w':
self.wrenchSpace = self.calculate_3D_wrench_space()
self.wrenchSpace6D = None
self.wrench_space_gl_object = None
elif c == 'r':
dropout = raw_input("How much to drop out? (default 0) > ")
if dropout.strip() == '':
dropout = 0
else:
dropout = float(dropout.strip())
perturb = raw_input("How much to perturb? (default 0.1) > ")
if perturb.strip() == '':
perturb = 0.1
else:
perturb = float(perturb.strip())
N = raw_input("How many samples? (default 100) > ")
if N.strip() == '':
N = 100
else:
N = int(N.strip())
self.wrenchSpace = self.calculate_robust_force_volume((0,0,0),dropout=dropout,configurationPerturbation=perturb,maxVolumes=N)
self.wrenchSpace6D = None
self.wrench_space_gl_object = None
elif c == 'm':
assert self.wrenchSpace != None
from PyQt4.QtGui import QFileDialog
savedir = "data/"+self.world.robot(0).getName()+'/'+self.config_name
name = QFileDialog.getSaveFileName(caption="Mesh file (*.obj, *.off, etc)",directory=savedir,filter="Wavefront OBJ (*.obj);;Object File Format (*.off);;All files (*.*)")
g = polytope.hull_to_klampt_geom(self.wrenchSpace)
g.saveFile(str(name))
elif c == 'M':
from PyQt4.QtGui import QFileDialog
savedir = "data/"+self.world.robot(0).getName()+'/'+self.config_name
name = QFileDialog.getOpenFileName(caption="Mesh file (*.obj, *.off, etc)",directory=savedir,filter="Wavefront OBJ (*.obj);;Object File Format (*.off);;All files (*.*)")
g = Geometry3D()
g.loadFile(str(name))
self.wrenchSpace = polytope.klampt_geom_to_hull(g)
self.wrench_space_gl_object = None
#decomp = stability.approximate_convex_decomposition(g)
#print "Convex decomposition into",len(decomp),"pieces"
print len(polytope.pockets(g)),"pocket faces"
elif c == '6':
N = raw_input("How many samples? (default 100) > ")
if N.strip() == '':
N = 100
else:
N = int(N.strip())
self.wrenchSpace6D = self.calculate_6D_wrench_space(N)
self.wrenchSpace = None
self.wrench_space_gl_object = None
elif c == '7':
self.wrenchSpace6D = self.load_6D_wrench_space()
self.wrenchSpace = None
self.wrench_space_gl_object = None
elif c == '0':
robot = self.world.robot(0)
q = robot.getConfig()
q[0:6] = [0]*6
robot.setConfig(q)
vis.setItemConfig("robot",robot.getConfig())
coordinates.updateFromWorld()
elif c == '9':
vis.edit("robot",False)
vis.edit("rigidObject",False)
#vis.hide("COM")
#object = self.world.rigidObject(0)
#vis.edit("COM")
else:
return False
return True
def motionfunc(self,x,y,dx,dy):
res = GLPluginInterface.motionfunc(self,x,y,dx,dy)
coordinates.updateFromWorld()
object = self.world.rigidObject(0)
vis.setItemConfig("COM",se3.apply(object.getTransform(),object.getMass().getCom()))
return res
