def setParabolic():
traj = trajectory.path_to_trajectory(traj0,
velocities='parabolic',
timing='Linf',
speed=1.0)
print "*** Resulting duration", traj.endTime(), "***"
vis.animate("robot", traj)
vis.addText("label", "Parabolic velocity profile")
def setCosine():
traj = trajectory.path_to_trajectory(traj0,
velocities='cosine',
timing='Linf',
speed=1.0,
stoptol=0.0)
print "*** Resulting duration", traj.endTime(), "***"
vis.animate("robot", traj)
vis.addText("label", "Start/stop cosine velocity profile")
def setStartStop():
traj = trajectory.path_to_trajectory(traj0,
velocities='trapezoidal',
timing='Linf',
speed=1.0,
zerotol=0.0)
print "*** Resulting duration", traj.endTime(), "***"
vis.animate("robot", traj)
vis.addText("label", "Start/stop trapezoidal velocity profile")
def setMinJerk():
traj = trajectory.path_to_trajectory(traj0,
velocities='minimum-jerk',
timing='Linf',
speed=1.0,
stoptol=0.0)
print("*** Resulting duration", traj.endTime(), "***")
vis.animate("robot", traj)
vis.addText("label", "Start/stop minimum-jerk velocity profile")
def runPlanner(runfunc, name):
global lastPlan
res = runfunc()
if res:
if lastPlan:
vis.remove(lastPlan)
vis.add(name, res)
vis.setColor(name, 1, 0, 0)
vis.animate(("world", robot.getName()), res)
lastPlan = name
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 animation_template(world):
"""Shows how to animate a robot."""
#first, build a trajectory with 10 random configurations
robot = world.robot(0)
times = range(10)
milestones = []
for t in times:
robot.randomizeConfig()
milestones.append(robot.getConfig())
traj = trajectory.RobotTrajectory(robot, times, milestones)
vis.add("world", world)
robotPath = ("world", world.robot(0).getName()
) #compound item reference: refers to robot 0 in the world
#we're also going to visualize the end effector trajectory
#eetraj = traj.getLinkTrajectory(robot.numLinks()-1,0.05)
#vis.add("end effector trajectory",eetraj)
#uncomment this to automatically visualize the end effector trajectory
vis.add("robot trajectory", traj)
vis.setAttribute("robot trajectory", "endeffectors", [13, 20])
vis.setWindowTitle("Animation test")
MANUAL_ANIMATION = False
if not MANUAL_ANIMATION:
#automatic animation, just call vis.animate
vis.animate(robotPath, traj)
if not MULTITHREADED:
#need to set up references to function-local variables manually, and the easiest way is to use a default argument
def callback(robot=robot):
if MANUAL_ANIMATION:
#with manual animation, you just set the robot's configuration based on the current time.
t = vis.animationTime()
q = traj.eval(t, endBehavior='loop')
robot.setConfig(q)
pass
vis.loop(callback=callback, setup=vis.show)
else:
vis.show()
while vis.shown():
vis.lock()
if MANUAL_ANIMATION:
#with manual animation, you just set the robot's configuration based on the current time.
t = vis.animationTime()
q = traj.eval(t, endBehavior='loop')
robot.setConfig(q)
vis.unlock()
time.sleep(0.01)
#quit the visualization thread nicely
vis.kill()
def bumpTrajectory():
relative_xforms = []
robot.setConfig(self.refConfig)
for e in self.endeffectors:
xform = vis.getItemConfig("ee_"+robot.link(e).getName())
T = (xform[:9],xform[9:])
T0 = robot.link(e).getTransform()
Trel = se3.mul(se3.inv(T0),T)
print "Relative transform of",e,"is",Trel
relative_xforms.append(Trel)
bumpTraj = cartesian_trajectory.cartesian_bump(self.robot,self.traj,self.constraints,relative_xforms,ee_relative=True,closest=True)
assert bumpTraj != None
vis.animate(("world",world.robot(0).getName()),bumpTraj)
self.refresh()
def planTriggered():
global world,robot
qtgt = vis.getItemConfig(vis.getItemName(robot))
qstart = vis.getItemConfig("start")
robot.setConfig(qstart)
if PROBLEM == '1a':
path = planning.feasible_plan(world,robot,qtgt)
else:
path = planning.optimizing_plan(world,robot,qtgt)
if path is not None:
ptraj = trajectory.RobotTrajectory(robot,milestones=path)
ptraj.times = [t / len(ptraj.times) * 5.0 for t in ptraj.times]
#this function should be used for creating a C1 path to send to a robot controller
traj = trajectory.path_to_trajectory(ptraj,timing='robot',smoothing=None)
#show the path in the visualizer, repeating for 60 seconds
vis.animate("start",traj)
vis.add("traj",traj,endeffectors=[9])
def visualize(world, robot, path=None, start=None, goal=None):
"""Visualize a path"""
# visualize start/goal as spheres if exist
r = 0.04
if start != None:
sphere = create.primitives.sphere(r, start, world=world, name='start')
# vis.add('start', sphere)
if goal != None:
create.primitives.sphere(r, goal, world=world, name='goal')
vis.add("world", world)
# animate path if exist
if path != None:
traj = trajectory.RobotTrajectory(robot, range(len(path)), path)
vis.animate(("world", robot.getName()), path, speed=0.5)
vis.add("trajectory", traj)
vis.spin(float('inf'))
def setHermite():
smoothInput = trajectory.HermiteTrajectory()
smoothInput.makeSpline(traj0)
dtraj = smoothInput.discretize(0.1)
dtraj = trajectory.RobotTrajectory(robot, dtraj.times, dtraj.milestones)
traj = trajectory.path_to_trajectory(
dtraj,
velocities='constant',
timing='limited',
smoothing=None,
stoptol=10.0,
vmax=robot.getVelocityLimits(),
amax=robot.getAccelerationLimits(),
speed=1.0,
)
print("*** Resulting duration", traj.endTime(), "***")
#vis.animate("robot",ltraj)
#vis.animate("robot",dtraj)
vis.animate("robot", traj)
vis.addText("label", "Hermite trajectory")
def planTriggered():
global world, robot, obj, target_gripper, grasp, grasps
qstart = robot.getConfig()
if PROBLEM == '2a':
res = pick.plan_pick_one(world, robot, obj, target_gripper, grasp)
elif PROBLEM == '2b':
res = pick.plan_pick_grasps(world, robot, obj, target_gripper,
grasps)
else:
res = pick.plan_pick_multistep(world, robot, obj, target_gripper,
grasps)
if res is None:
print("Unable to plan pick")
else:
(transit, approach, lift) = res
traj = transit
traj = traj.concat(approach, relative=True, jumpPolicy='jump')
traj = traj.concat(lift, relative=True, jumpPolicy='jump')
vis.add("traj", traj, endEffectors=[9])
vis.animate(vis.getItemName(robot), traj)
robot.setConfig(qstart)
def keyboardfunc(self, key, x, y):
if key == 'l':
self.robot.setConfig(self.qstart)
self.path = planTransit(self.world, 0, Hand('l'))
if self.path:
#convert to a timed path for animation's sake
self.path = RobotTrajectory(self.robot, range(len(self.path)),
self.path)
#reset the animation
vis.animate(("world", self.robot.getName()), self.path)
return True
elif key == 'r':
self.robot.setConfig(self.qstart)
self.path = planTransit(self.world, 0, Hand('r'))
if self.path:
#convert to a timed path for animation's sake
self.path = RobotTrajectory(self.robot, range(len(self.path)),
self.path)
#reset the animation
vis.animate(("world", self.robot.getName()), self.path)
return True
return False
def keyboardfunc(self, key, x, y):
if key == 'r':
self.robot.setConfig(self.qstart)
self.object.setTransform(*self.Tstart)
self.path = planTransfer(self.world, 0, Hand('l'), (0, -0.15, 0))
if self.path:
#convert to a timed path for animation's sake
self.path = RobotTrajectory(self.robot,
list(range(len(self.path))),
self.path)
#compute object trajectory
resolution = 0.05
self.objectPath = self.path.getLinkTrajectory(
Hand('l').link, resolution)
self.objectPath.postTransform(self.Tgrasp)
else:
self.path = None
#reset the animation
vis.animate(("world", self.robot.getName()), self.path)
vis.animate(("world", self.object.getName()), self.objectPath)
return True
elif key == 'f':
self.robot.setConfig(self.qstart)
self.object.setTransform(*self.Tstart)
self.path = planTransfer(self.world, 0, Hand('l'), (0.15, 0, 0))
if self.path:
#convert to a timed path for animation's sake
self.path = RobotTrajectory(self.robot,
list(range(len(self.path))),
self.path)
#compute object trajectory
resolution = 0.05
self.objectPath = self.path.getLinkTrajectory(
Hand('l').link, resolution)
self.objectPath.postTransform(self.Tgrasp)
else:
self.path = None
#reset the animation
vis.animate(("world", self.robot.getName()), self.path)
vis.animate(("world", self.object.getName()), self.objectPath)
return True
return False
print "CSpace stats:"
spacestats = space.getStats()
for k in sorted(spacestats.keys()):
print " ",k,":",spacestats[k]
print "Planner stats:"
planstats = planner.getStats()
for k in sorted(planstats.keys()):
print " ",k,":",planstats[k]
if path:
#save planned milestone path to disk
print "Saving to my_plan.configs"
resource.set("my_plan.configs",path,"Configs")
#visualize path as a Trajectory resource
from klampt.model.trajectory import RobotTrajectory
traj = RobotTrajectory(robot,range(len(path)),path)
#resource.edit("Planned trajectory",traj,world=world)
#visualize path in the vis module
from klampt import vis
vis.add("world",world)
vis.animate(("world",robot.getName()),path)
vis.add("trajectory",traj)
vis.spin(float('inf'))
#play nice with garbage collection
planner.space.close()
planner.close()
def solve(self,units,mass=None,wext=None,mode='equilibrium'):
"""mode can be 'equilibrium', 'energy_minimization', 'wrench_maximization', or 'wrench_dot_maximization'
"""
if not units:
print "No contact units specified?"
return False
#create and configure the solver
t0 = time.time()
robot = self.world.robot(0)
if mass == None:
mass = self.world.rigidObject(0).getMass().mass
#center the origin at the object's COM
cm = se3.apply(self.world.rigidObject(0).getTransform(),self.world.rigidObject(0).getMass().getCom())
#print "Cm",cm
gravity = (0,0,-9.8)
origin = cm
if mode in ['wrench_maximization','wrench_dot_maximization'] or wext != None:
gravity = (0.0,0.0,0.0)
(solver,J) = self.make_solver(units,origin,useTorqueLimits=True,gravity=gravity)
#set up the external forcing
if wext is not None:
solver.setExternalWrench(wext)
else:
solver.setGravityWrench(mass,[0,0,0],gravity)
if mode == 'energy_minimization':
#TODO: mass and inertia matrix
solver.setWrenchObjective(1,1)
elif mode == 'wrench_maximization':
solver.setExternalWrench(wext)
solver.maximizeWrenchMagnitude = True
elif mode == 'wrench_dot_maximization':
solver.setExternalWrench(wext)
solver.maximizeWrenchDot = True
else:
#TESTING
#solver.maximizeWrenchMagnitude = True
pass
t1 = time.time()
print "Problem setup time",t1-t0
t0 = time.time()
self.torques = None
#perform the solve
try:
self.wrenches = solver.solve()
except ValueError as e:
print e
raise
print "Probably don't have a valid static equilibrium solution"
print "Equality constraints:"
print solver.A_eq_extra
self.wrenches = None
t1 = time.time()
self.last_solver = solver
self.feasible = (self.wrenches is not None)
print
print "Solve time",t1-t0,"Feasible:",self.feasible
if self.feasible:
assert len(solver.forces) == len(J)
#assert len(self.wrenches) == len(units)
self.torques = np.array(robot.getGravityForces(gravity)) + sum(np.dot(Ji.T,fi) for fi,Ji in zip(solver.forces,J))
return True
print "Contact patch wrenches"
for w in self.wrenches:
print " ",w
#print "Contact patch forces",solver.forces
#print solver.wrenchMatrices
#print "Gravity torques",G
w = sum(self.wrenches)
if mode not in ['wrench_maximization','wrench_dot_maximization']:
w += solver.wext
#print "Resultant joint torques:"
#for i in range(6,robot.numLinks()):
# print " ",robot.link(i).getName(),":",self.torques[i]
print "Resultant body wrench:",w
f = w[0:3]
t = w[3:6]
if mode == 'energy_minimization':
#TODO: look at the object's inertia matrix?
Tobj = self.world.rigidObject(0).getTransform()
v,w = vectorops.div(f,mass),vectorops.div(t,mass)
dR = vectorops.mul(w,1.0/mass)
dt = vectorops.mul(v,1.0/mass)
Tnew = se3.mul((so3.from_moment(dR),dt),Tobj)
traj = SE3Trajectory([0,1],[Tobj,Tnew])
vis.animate("rigidObject",traj)
#raw_input()
if solver.minimize:
return solver.objectiveValue
self.feasible = (solver.objectiveValue < 1e-5)
return self.feasible
return True
else:
#try again, without torque bounds
solver.A_ineq_extra = []
solver.b_ineq_extra = []
self.wrenches = solver.solve()
if self.wrenches is not None:
print "Limiting factor is joint torques"
self.torques = G + sum(np.dot(Ji.T,fi) for fi,Ji in zip(solver.forces,J))
print "Resultant joint torques:",self.torques[6:]
return False
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 setDoubleSpeed():
traj = trajectory.path_to_trajectory(traj0, speed=2.0)
print "*** Resulting duration", traj.endTime(), "***"
vis.animate("robot", traj)
vis.addText("label", "2x speed")
vis.add("point spline",traj4.discretize(0.05),color=(0.4,0.4,1,1))
traj5 = trajectory.HermiteTrajectory()
traj5.makeSpline(traj,preventOvershoot=False)
for m in traj5.milestones:
m[1] = 0.75
vis.add("point spline, overshoot allowed",traj5.discretize(0.05),color=(0.6,0.6,1,1))
traj6 = trajectory.HermiteTrajectory()
traj6.makeBezier([0,3,6,9],traj.milestones)
for m in traj6.milestones:
m[1] = 1
vis.add("point bezier",traj6.discretize(0.05),color=(0.8,0.8,1,1))
#which one to animate?
vis.animate("point",traj2)
#add a transform to the visualizer and animate it
xform = vis.add("xform",se3.identity())
traj = trajectory.SE3Trajectory()
x = 0
for i in range(10):
rrot = so3.sample()
rpoint = [x + random.uniform(0.1,0.3),0,random.uniform(-1,1)]
x = rpoint[0]
traj.milestones.append(rrot+rpoint)
traj.times = list(range(len(traj.milestones)))
vis.add("xform_milestones",traj,color=(1,1,0,1))
traj2 = trajectory.SE3HermiteTrajectory()
traj2.makeSpline(traj)
vis.edit("ee_"+robot.link(e).getName())
obj = coordinates.ik_fixed_objective(f)
eeobjectives.append(obj)
#this tests the cartesian interpolation stuff
print "***** BEGINNING CARTESIAN INTERPOLATION TEST *****"
vis.setWindowTitle("Klamp't Cartesian interpolation test")
vis.pushPlugin(InterpKeyCapture(endeffectors,eeobjectives))
vis.show()
while vis.shown():
coordinates.updateFromWorld()
time.sleep(0.1)
vis.popPlugin()
vis.hide("ghost1")
vis.hide("ghost2")
vis.animate(("world",world.robot(0).getName()),None)
print
print
#this tests the "bump" function stuff
print "***** BEGINNING BUMP FUNCTION TEST *****"
configs = resource.get("cartesian_test"+world.robot(0).getName()+".configs",description="Make a reference trajectory for bump test",world=world)
if configs is None:
print "To test the bump function, you need to create a reference trajectory"
vis.kill()
exit(0)
print "Found trajectory with",len(configs),"configurations"
traj = trajectory.RobotTrajectory(robot,range(len(configs)),configs)
vis.setWindowTitle("Klamp't Trajectory bump test")
vis.pushPlugin(BumpKeyCapture(endeffectors,eeobjectives,traj))
vis.show()
R0 = so3.identity()
R1 = so3.rotation([0, 0, 1], math.pi / 2)
dR0 = [0.0] * 9
#TODO: for some reason the interpolation doesn't work very well...
#vis.add("Camera smooth traj",circle_smooth_traj.discretize_se3(0.1))
#for m in circle_smooth_traj.milestones:
# T = m[:12]
# vT = m[12:]
# print("Orientation velocity",vT[:9])
#print("SMOOTH")
#for R in circle_smooth_traj.discretize_se3(0.1).milestones:
# print(so3.apply(R,[0,1,0]))
vis.add("xform", se3.identity())
vis.animate("xform", circle_smooth_traj)
#vis.add("Camera traj",circle_traj.discretize(0.25))
vis.addAction(lambda: vis.followCamera(None), "stop folllowing")
vis.addAction(lambda: vis.followCamera(cam), "robot camera")
vis.addAction(
lambda: vis.followCamera(
("world", robot.getName(), link.getName()), True, False, True),
"link, translate")
vis.addAction(
lambda: vis.followCamera(
("world", robot.getName(), link.getName()), False, True, True),
"link, rotate")
vis.addAction(
lambda: vis.followCamera(
("world", robot.getName(), link.getName()), True, True, True),
"link, full pose")
trajsolved.times = [timescale * v for v in trajsolved.times]
vis.add(
"Initial trajectory",
trajinit.getLinkTrajectory(
END_EFFECTOR_LINK,
0.1).getPositionTrajectory(END_EFFECTOR_LOCAL_POSITION))
#vis.add("Initial trajectory",trajinit)
vis.setColor("Initial trajectory", 1, 1, 0, 0.5)
vis.hideLabel("Initial trajectory")
vis.setAttribute("Initial trajectory", "width", 2)
eetrajopt = trajsolved.getLinkTrajectory(
END_EFFECTOR_LINK, 0.1).getPositionTrajectory(END_EFFECTOR_LOCAL_POSITION)
vis.add("Solution trajectory", eetrajopt)
#vis.add("Solution trajectory",trajsolved)
vis.hideLabel("Solution trajectory")
vis.animate(("world", robot.getName()), trajsolved)
params[0] = sorted(params[0], key=lambda x: x[2])
for i, p in enumerate(params[0]):
link = p[0]
env = p[1]
t = p[2]
wpt = p[3:6]
eept = eetrajopt.eval(t * timescale)
vis.add("support" + str(i), wpt)
vis.add("tsupport" + str(i), eept)
r = float(i) / len(params[0])
if i % 2 == 0:
b = 0
else:
b = 0.5
def add_to_vis(self,robot=None,animate=True,base_xform=None):
"""Adds the gripper to the klampt.vis scene."""
from klampt import vis
from klampt import WorldModel,Geometry3D,GeometricPrimitive
from klampt.model.trajectory import Trajectory
prefix = "gripper_"+self.name
if robot is None and self.klampt_model is not None:
w = WorldModel()
if w.readFile(self.klampt_model):
robot = w.robot(0)
vis.add(prefix+"_gripper",w)
robotPath = (prefix+"_gripper",robot.getName())
elif robot is not None:
vis.add(prefix+"_gripper",robot)
robotPath = prefix+"_gripper"
if robot is not None:
assert self.base_link >= 0 and self.base_link < robot.numLinks()
robot.link(self.base_link).appearance().setColor(1,0.75,0.5)
if base_xform is None:
base_xform = robot.link(self.base_link).getTransform()
else:
if robot.link(self.base_link).getParent() >= 0:
print("Warning, setting base link transform for an attached gripper base")
#robot.link(self.base_link).setParent(-1)
robot.link(self.base_link).setParentTransform(*base_xform)
robot.setConfig(robot.getConfig())
for l in self.finger_links:
assert l >= 0 and l < robot.numLinks()
robot.link(l).appearance().setColor(1,1,0.5)
if robot is not None and animate:
q0 = robot.getConfig()
for i in self.finger_drivers:
if isinstance(i,(list,tuple)):
for j in i:
robot.driver(j).setValue(1)
else:
robot.driver(i).setValue(1)
traj = Trajectory([0],[robot.getConfig()])
for i in self.finger_drivers:
if isinstance(i,(list,tuple)):
for j in i:
robot.driver(j).setValue(0)
traj.times.append(traj.endTime()+0.5)
traj.milestones.append(robot.getConfig())
else:
robot.driver(i).setValue(0)
traj.times.append(traj.endTime()+1)
traj.milestones.append(robot.getConfig())
traj.times.append(traj.endTime()+1)
traj.milestones.append(traj.milestones[0])
traj.times.append(traj.endTime()+1)
traj.milestones.append(traj.milestones[0])
assert len(traj.times) == len(traj.milestones)
traj.checkValid()
vis.animate(robotPath,traj)
robot.setConfig(q0)
if self.center is not None:
vis.add(prefix+"_center",se3.apply(base_xform,self.center))
center_point = (0,0,0) if self.center is None else self.center
outer_point = (0,0,0)
if self.primary_axis is not None:
length = 0.1 if self.finger_length is None else self.finger_length
outer_point = vectorops.madd(self.center,self.primary_axis,length)
line = Trajectory([0,1],[self.center,outer_point])
line.milestones = [se3.apply(base_xform,m) for m in line.milestones]
vis.add(prefix+"_primary",line,color=(1,0,0,1))
if self.secondary_axis is not None:
width = 0.1 if self.maximum_span is None else self.maximum_span
line = Trajectory([0,1],[vectorops.madd(outer_point,self.secondary_axis,-0.5*width),vectorops.madd(outer_point,self.secondary_axis,0.5*width)])
line.milestones = [se3.apply(base_xform,m) for m in line.milestones]
vis.add(prefix+"_secondary",line,color=(0,1,0,1))
elif self.maximum_span is not None:
#assume vacuum gripper?
p = GeometricPrimitive()
p.setSphere(outer_point,self.maximum_span)
g = Geometry3D()
g.set(p)
vis.add(prefix+"_opening",g,color=(0,1,0,0.25))
def setHalfSpeed():
traj = trajectory.path_to_trajectory(traj0, speed=0.5)
print("*** Resulting duration", traj.endTime(), "***")
vis.animate("robot", traj)
vis.addText("label", "0.5x speed")
print 3,":",traj.eval(3)
print 4,":",traj.eval(4)
print 5,":",traj.eval(5)
print 6,":",traj.eval(6)
#print some interpolated points
print 0.5,":",traj.eval(0.5)
print 2.5,":",traj.eval(2.5)
#print some stuff after the end of trajectory
print 7,":",traj.eval(6)
print 100.3,":",traj.eval(100.3)
print -2,":",traj.eval(-2)
from klampt import vis
vis.add("point",[0,0,0])
vis.animate("point",traj)
vis.add("traj",traj)
#vis.spin(float('inf')) #show the window until you close it
traj2 = trajectory.HermiteTrajectory()
traj2.makeSpline(traj)
vis.animate("point",traj2)
vis.hide("traj")
vis.add("traj2",traj2.configTrajectory().discretize(0.1))
#vis.spin(float('inf'))
traj_timed = trajectory.path_to_trajectory(traj,vmax=2,amax=4)
#next, try this line instead
#traj_timed = trajectory.path_to_trajectory(traj,timing='sqrt-L2',speed='limited',vmax=2,amax=4)
#or this line
def keyboardfunc(self, key, x, y):
h = 0.932
targets = {
'a': (0.35, 0.25, h),
'b': (0.35, 0.05, h),
'c': (0.35, -0.1, h),
'd': (0.35, -0.1, h)
}
if key in targets:
dest = targets[key]
shift = vectorops.sub(dest, self.Tstart[1])
self.robot.setConfig(self.qstart)
self.object.setTransform(*self.Tstart)
self.hand = Hand('l')
#plan transit path to grasp object
self.transitPath = planTransit(self.world, 0, self.hand)
if self.transitPath:
#plan transfer path
self.Tgrasp = graspTransform(self.robot, self.hand,
self.transitPath[-1], self.Tstart)
self.robot.setConfig(self.transitPath[-1])
self.transferPath = planTransfer(self.world, 0, self.hand,
shift)
if self.transferPath:
self.Tgoal = graspedObjectTransform(
self.robot, self.hand, self.transferPath[0],
self.Tstart, self.transferPath[-1])
#plan free path to retract arm
self.robot.setConfig(self.transferPath[-1])
self.object.setTransform(*self.Tgoal)
self.retractPath = planFree(self.world, self.hand,
self.qstart)
#reset the animation
if self.transitPath and self.transferPath and self.retractPath:
milestones = self.transitPath + self.transferPath + self.retractPath
self.path = RobotTrajectory(self.robot, range(len(milestones)),
milestones)
resolution = 0.05
xfertraj = RobotTrajectory(self.robot,
range(len(self.transferPath)),
self.transferPath)
xferobj = xfertraj.getLinkTrajectory(self.hand.link,
resolution)
xferobj.postTransform(self.Tgrasp)
#offset times to be just during the transfer stage
for i in xrange(len(xferobj.times)):
xferobj.times[i] += len(self.transitPath)
self.objectPath = xferobj
vis.animate(("world", self.robot.getName()), self.path)
vis.animate(("world", self.object.getName()), self.objectPath)
else:
vis.animate(("world", self.robot.getName()), None)
vis.animate(("world", self.object.getName()), None)
if key == 'n':
print "Moving to next action"
if self.transitPath and self.transferPath and self.retractPath:
#update start state
self.qstart = self.retractPath[-1]
self.Tstart = self.Tgoal
self.robot.setConfig(self.qstart)
self.object.setTransform(*self.Tstart)
self.transitPath = None
self.transferPath = None
self.hand = None
self.Tgrasp = None
self.refresh()
print(plan.space.cspace.feasibilityQueryOrder())
print("Plan stats:")
print(plan.getStats())
print("CSpace stats:")
print(plan.space.getStats())
#to be nice to the C++ module, do this to free up memory
plan.space.close()
plan.close()
if len(wholepath) > 1:
#print "Path:"
#for q in wholepath:
# print " ",q
#if you want to save the path to disk, uncomment the following line
#wholepath.save("test.path")
#draw the path as a RobotTrajectory (you could just animate wholepath, but for robots with non-standard joints
#the results will often look odd). Animate with 5-second duration
times = [i * 5.0 / (len(wholepath) - 1) for i in range(len(wholepath))]
traj = trajectory.RobotTrajectory(robot, times=times, milestones=wholepath)
#show the path in the visualizer, repeating for 60 seconds
vis.animate("robot", traj)
vis.spin(60)
else:
print("Failed to generate a plan")
vis.kill()
if not res:
raise RuntimeError("Unable to load model "+fn)
world.enableInitCollisions(True)
#the plugin is pushed on the visualizer stack to get interactivity with the visualizer... if you
#don't care about interactivity, you may leave it out. See vistemplate.py for
#an example of this
plugin = MyGLPlugin(world)
vis.pushPlugin(plugin)
#add the world to the visualizer
vis.add("world",world)
vis.listItems()
vis.setColor(("world","Terrain0"),1,0,0)
vis.setColor(("world","ATHLETE","hex pitch"),0.5,0.5,0.5,0.5)
q0 = world.robot(0).getConfig()
q1 = q0[:]
q1[0] += 1
vis.animate(("world","ATHLETE"),[q0,q1],speed=0.2)
print "Press 'q' to exit"
#run the visualizer in a separate thread
vis.show()
while vis.shown() and not plugin.quit:
vis.lock()
#TODO: you may modify the world here
vis.unlock()
#changes to the visualization must be done outside the lock
time.sleep(0.01)
print "Ending visualization."
vis.kill()