class GLTransferPlanPlugin(GLPluginInterface):
def __init__(self, world):
GLPluginInterface.__init__(self)
self.world = world
self.robot = world.robot(0)
self.object = world.rigidObject(0)
#start robot config
self.qstart = self.robot.getConfig()
#start object transform
self.Tstart = self.object.getTransform()
#grasp transform
self.Tgrasp = Tgrasp = graspTransform(self.robot, Hand('l'),
self.qstart, self.Tstart)
#solution to planning problem
self.path = None
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
def test_trajectory_editing():
traj = SE3Trajectory([0, 1], [se3.identity(), se3.identity()])
saved, result = resource.edit("se3 traj",
traj,
'SE3Trajectory',
'Some spatial trajectory',
world=w,
referenceObject=r.link(7))
traj = Trajectory([0, 1], [[0, 0, 0], [0, 0, 1]])
saved, result = resource.edit("point traj",
traj,
'auto',
'Some point trajectory',
world=w,
referenceObject=r.link(7))
traj = RobotTrajectory(r, [0, 1, 2], [q0, qrand, qrand2])
saved, result = resource.edit("robot traj",
traj,
'auto',
'Random robot trajectory',
world=w,
referenceObject=r)
vis.kill()
def plan_pick_one(world,robot,object,gripper,grasp):
"""
Plans a picking motion for a given object and a specified grasp.
Arguments:
world (WorldModel): the world, containing robot, object, and other items that
will need to be avoided.
robot (RobotModel): the robot in its current configuration
object (RigidObjectModel): the object to pick.
gripper (GripperInfo): the gripper.
grasp (Grasp): the desired grasp. See common/grasp.py for more information.
Returns:
None or (transit,approach,lift): giving the components of the pick motion.
Each element is a RobotTrajectory. (Note: to convert a list of milestones
to a RobotTrajectory, use RobotTrajectory(robot,milestones=milestones)
Tip:
vis.debug(q,world=world) will show a configuration.
"""
qstart = robot.getConfig()
grasp.ik_constraint.robot = robot #this makes it more convenient to use the ik module
#TODO solve the IK problem for qgrasp?
def collision_free():
return is_collision_free_grasp(world, robot, object)
ik.solve_global([grasp.ik_constraint], iters=100, numRestarts=10, feasibilityCheck=collision_free)
qgrasp = robot.getConfig()
qgrasp = grasp.set_finger_config(qgrasp) #open the fingers the right amount
qopen = gripper.set_finger_config(qgrasp,gripper.partway_open_config(1)) #open the fingers further
qpregrasp = retract(robot, gripper, [0,0,-0.2]) #TODO solve the retraction problem for qpregrasp?
if qpregrasp is None:
return None
qstartopen = gripper.set_finger_config(qstart,gripper.partway_open_config(1)) #open the fingers of the start to match qpregrasp
robot.setConfig(qstartopen)
# print(qpregrasp)
transit = optimizing_plan(world,robot,qpregrasp) #decide whether to use feasible_plan or optimizing_plan
if not transit:
return None
#TODO: not a lot of collision checking going on either...
robot.setConfig(qgrasp)
qlift = retract(robot, gripper, [0,0,-0.2])
return (RobotTrajectory(robot,milestones=[qstart]+transit),RobotTrajectory(robot,milestones=[qpregrasp,qopen,qgrasp]),RobotTrajectory(robot,milestones=[qgrasp,qlift]))
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
def test_trajectory_vis():
#add a "world" item to the scene manager
vis.add("world", w)
#show qrand as a ghost configuration in transparent red
vis.add("qrand", qrand, color=(1, 0, 0, 0.5))
#show a Trajectory between q0 and qrand
vis.add("path_to_qrand", RobotTrajectory(r, [0, 1], [q0, qrand]))
#launch the vis loop and window
vis.show()
while vis.shown():
time.sleep(0.01)
def test_debug():
vis.debug(w.robot(0), centerCamera=True)
g = Geometry3D()
g.loadFile("../../data/objects/srimugsmooth.off")
vis.debug(g, centerCamera=True)
pt = [0, 0, 2]
traj = Trajectory([0, 1, 2], [[0, 0, 2], [1, 0, 2], [1, 1, 2]])
vis.debug('qrand', [qrand, qrand2, q0], {'color': [1, 0, 0, 0.5]},
pt,
world=w)
vis.debug('qrand',
qrand, {'color': [1, 0, 0, 0.5]},
pt,
world=w,
centerCamera=r.link(6))
vis.debug('qrand',
qrand, {'color': [1, 0, 0, 0.5]},
world=w,
followCamera=r.link(6))
vis.debug('qrand',
qrand, {'color': [1, 0, 0, 0.5]},
pt=pt,
world=w,
animation=traj)
vis.debug('qrand',
qrand, {'color': [1, 0, 0, 0.5]},
pt=pt,
world=w,
centerCamera='pt')
vis.debug('qrand',
qrand, {'color': [1, 0, 0, 0.5]},
pt=pt,
world=w,
animation=traj,
followCamera='pt')
milestones = []
for i in range(5):
r.randomizeConfig()
milestones.append(r.getConfig())
r.setConfig(q0)
qtraj = RobotTrajectory(r, [0, 1, 2, 3, 4], milestones)
vis.debug('qrand',
qrand, {'color': [1, 0, 0, 0.5]},
world=w,
animation=qtraj)
#this doesn't work -- qrand is not being tracked
vis.debug('qrand',
qrand, {'color': [1, 0, 0, 0.5]},
world=w,
animation=qtraj,
followCamera=r.link(6))
def assemble_result(self, plan):
transfer = plan['transfer']
qplace = plan['qplace']
qpreplace = plan['qpreplace']
retract = plan['retract']
#TODO: construct the RobotTrajectory tuple (transfer,lower,retract)
if self.robot0:
angle = -np.pi / 2
q_rotate = copy.deepcopy(self.qtransfer)
q_rotate[7] = q_rotate[7] - angle
qpreplace[7] = qpreplace[7] - angle
self.qplaceplate[7] = self.qplaceplate[7] - angle
self.qplaceplate_lower[7] = self.qplaceplate_lower[7] - angle
self.qplace_trash[7] = self.qplace_trash[7] - angle
self.qplace[7] = self.qplace[7] - angle
# plan a path between qpreplace and qlower
self.robot.setConfig(qpreplace)
self.lower_plan = transfer_plan(self.world, self.robot,
self.qlower, self.object,
self.Tobject_gripper)
return RobotTrajectory(self.robot, milestones=[self.qstart, self.qmouth, self.qmouth_in, self.qmouth] + transfer), \
RobotTrajectory(self.robot,
milestones=[self.qtransfer, q_rotate] + self.lower_plan + self.lower_plan[::-1]+
[self.qplaceplate,self.qplaceplate_lower, self.qplaceplate, self.qplace_trash,
self.qplace]), \
RobotTrajectory(self.robot, milestones=[self.qplace])
else:
return RobotTrajectory(self.robot, milestones=[self.qstart] + transfer), \
RobotTrajectory(self.robot, milestones=[self.qtransfer, qpreplace, qplace]), \
RobotTrajectory(self.robot, milestones=[qplace, retract])
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 loadedItem(self, fn, obj):
if fn in self.active:
print("klampt_browser: Re-loaded item", fn,
"so I'm first removing it")
self.remove(fn)
assert fn not in self.active
item = ResourceItem(obj)
self.active[fn] = item
item.plugin = GLVisualizationPlugin()
basename = os.path.basename(fn)
#determine whether it's being animated
if isinstance(obj, Trajectory) and len(obj.milestones) > 0:
d = len(obj.milestones[0])
if self.world.numRobots() > 0 and d == self.world.robot(
0).numLinks():
obj = RobotTrajectory(self.world.robot(0), obj.times,
obj.milestones)
robotpath = ('world', self.world.robot(0).getName())
item.animationBuddy = robotpath
elif d == 3:
item.plugin.add("anim_point", [0, 0, 0])
item.animationBuddy = "anim_point"
elif d == 12:
item.plugin.add("anim_xform", se3.identity())
item.animationBuddy = "anim_xform"
else:
print("klampt_browser: Can't interpret trajectory of length",
d)
elif isinstance(obj, MultiPath):
if self.world.numRobots() > 0:
robotpath = ('world', self.world.robot(0).getName())
item.animationBuddy = robotpath
item.plugin.add("world", self.world)
item.plugin.add(basename, obj)
item.plugin.addText("label", basename, position=(10, 10))
try:
type = vis.objectToVisType(obj, self.world)
except:
type = 'unknown'
if type in robot_override_types:
if self.world.numRobots() > 0:
path = ('world', self.world.robot(0).getName())
item.plugin.hide(path)
item.plugin.initialize()
def config_edit_template(world):
"""Shows how to edit Config, Configs, and Trajectory resources"""
config1 = resource.get("resourcetest1.config",
description="First config, always edited",
doedit=True,
editor='visual',
world=world)
print("Config 1:", config1)
config2 = resource.get("resourcetest1.config",
description="Trying this again...",
editor='visual',
world=world)
print("Config 2:", config2)
config3 = resource.get("resourcetest2.config",
description="Another configuration",
editor='visual',
world=world)
print("Config 3:", config3)
if config3 != None:
config3hi = config3[:]
config3hi[3] += 1.0
resource.set("resourcetest3_high.config", config3hi)
world.robot(0).setConfig(config3hi)
configs = []
if config1 != None: configs.append(config1)
if config2 != None: configs.append(config2)
if config3 != None: configs.append(config3)
print("Configs resource:", configs)
configs = resource.get("resourcetest.configs",
default=configs,
description="Editing config sequence",
doedit=True,
editor='visual',
world=world)
if configs is None:
print("To edit the trajectory, press OK next time")
else:
traj = RobotTrajectory(world.robot(0), list(range(len(configs))),
configs)
traj = resource.edit(name="Timed trajectory",
value=traj,
description="Editing trajectory",
world=world)
def from_JointTrajectory(ros_traj, robot=None, joint_link_indices=None):
"""Returns a Klamp't Trajectory or RobotTrajectory for a JointTrajectory
message.
Args:
ros_traj (sensor_msgs.msg.JointTrajectory): the JointTrajectory object
robot (RobotModel): the robot, optional
joint_link_indices (dict, optional): if given, maps ROS joint names to
Klampt link indices. Default uses the Klamp't link names.
"""
from klampt.model.trajectory import Trajectory, RobotTrajectory
if robot is None:
res = Trajectory()
for i in range(len(ros_traj.points)):
res.times.append(ros_traj.points[i].time_from_start.toSec())
res.milestones.append([v for v in ros_traj.points[i].positions])
return res
else:
res = RobotTrajectory(robot)
q0 = robot.getConfig()
if joint_link_indices is None:
joint_link_indices = dict()
for i in range(robot.numLinks()):
joint_link_indices[robot.link(i).getName()] = i
indices = []
if len(ros_traj.joint_names) > 0:
for n in ros_traj.joint_names:
if n not in joint_link_indices:
raise ValueError("ROS joint name %s is invalid" % (n, ))
indices = joint_link_indices[n]
for i in range(len(ros_traj.points)):
res.times.append(ros_traj.points[i].time_from_start.toSec())
for j, k in enumerate(indices):
q0[k] = ros_traj.points[i].positions[j]
res.milestones.append([v for v in q0])
return res
def plan_pick_one(world,robot,object,gripper,grasp, robot0=True):
"""
Plans a picking motion for a given object and a specified grasp.
Arguments:
world (WorldModel): the world, containing robot, object, and other items that
will need to be avoided.
robot (RobotModel): the robot in its current configuration
object (RigidObjectModel): the object to pick.
gripper (GripperInfo): the gripper.
grasp (Grasp): the desired grasp. See common/grasp.py for more information.
Returns:
None or (transit,approach,lift): giving the components of the pick motion.
Each element is a RobotTrajectory. (Note: to convert a list of milestones
to a RobotTrajectory, use RobotTrajectory(robot,milestones=milestones)
Tip:
vis.debug(q,world=world) will show a configuration.
"""
qstart = robot.getConfig()
qstartopen = gripper.set_finger_config(qstart, gripper.partway_open_config(1)) # open the fingers of the start to match qpregrasp
robot.setConfig(qstartopen)
grasp.ik_constraint.robot = robot #this makes it more convenient to use the ik module
def feasible():
return is_collision_free_grasp(world, robot, object)
# return True
solved = ik.solve_global(objectives=grasp.ik_constraint, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7],feasibilityCheck=feasible)
print('ik status:', solved)
if not solved: return None
qpregrasp = robot.getConfig()
robot.setConfig(qpregrasp)
if not feasible(): return None
qtransit = retract(robot=robot, gripper=gripper, amount=list(-0.1*np.array(gripper.primary_axis)), local=True)
# rotate the gripper along its primary axis to make its secondary axis perpendicular to the object
# so that it can grasp the object easily
secondary_axis_gripper = gripper.secondary_axis
if not isinstance(gripper,(int,str)):
gripper1 = gripper.base_link
else:
gripper1 = gripper
link = robot.link(gripper1)
R_gripper_w, _ = link.getTransform()
secondary_axis_world = so3.apply(R_gripper_w, secondary_axis_gripper)
secondary_axis_world_2d = np.array(secondary_axis_world)[:-1]
angle = np.arccos(np.dot(secondary_axis_world_2d, [0, 1]))
q_rotate = copy.deepcopy(qtransit)
q_rotate[7] = clip_angle(q_rotate[7] - angle)
qpregrasp[7] = clip_angle(qpregrasp[7] - angle)
if qtransit is None: return None
print(qtransit)
robot.setConfig(qtransit)
if not feasible(): return None
robot.setConfig(qpregrasp)
qopen = gripper.set_finger_config(qpregrasp, gripper.partway_open_config(1)) # open the fingers further
robot.setConfig(qopen)
if not feasible(): return None
if robot0:
close_amount = 0.1
else:
close_amount = 0.8
qgrasp = gripper.set_finger_config(qopen, gripper.partway_open_config(close_amount)) # close the gripper
robot.setConfig(qgrasp)
qlift = retract(robot=robot, gripper=gripper, amount=[0, 0, 0.1], local=False)
robot.setConfig(qlift)
if not feasible(): return None
robot.setConfig(qstart)
transit = feasible_plan(world, robot, object, qtransit) # decide whether to use feasible_plan or optimizing_plan
if not transit:
return None
return RobotTrajectory(robot,milestones=[qstart]+transit),\
RobotTrajectory(robot,milestones=[qtransit, q_rotate, qpregrasp, qopen, qgrasp]),\
RobotTrajectory(robot,milestones=[qgrasp,qlift])
trajcache.qend = [
0.0, -1.3000000000000003, 1.6199999999999997, 0.6999999999999997,
2.5200000000000014, -1.6000000000000008, 0.0
]
if EDIT_ENDPOINTS:
ok, res = resource.edit("Trajectory endpoints",
[trajcache.qstart, trajcache.qend],
world=world)
if ok:
trajcache.qstart, trajcache.qend = res
print(trajcache.qstart)
print(trajcache.qend)
xtraj = trajcache.straightLineState()
trajinit = trajcache.stateToTrajectory(xtraj)
else:
trajinit = RobotTrajectory(robot, trajinit.times, trajinit.milestones)
if MORE_SUBDIVISION > 1:
times = []
for i in range(len(trajinit.times) - 1):
t0 = trajinit.times[i]
t1 = trajinit.times[i + 1]
for k in range(MORE_SUBDIVISION):
times.append(t0 + float(k) / float(MORE_SUBDIVISION) *
(t1 - t0))
times.append(trajinit.times[-1])
print("NEW TIMES", times)
trajinit = trajinit.remesh(times)[0]
print("Trajectory has", len(trajinit.milestones), "milestones")
trajcache.qstart = trajinit.milestones[0][:]
trajcache.qend = trajinit.milestones[-1][:]
print("Time range", trajinit.times[0], trajinit.times[-1])
def planTriggered():
global world, robot, obj, target_gripper, solved_trajectory, trajectory_is_transfer, Tobject_grasp, obj, next_item_to_pick, qstart
if PROBLEM == '3a':
robot.setConfig(qstart)
res = place.transfer_plan(world, robot, qgoal, obj, Tobject_grasp)
if res is None:
print("Unable to plan transfer")
else:
traj = RobotTrajectory(robot, milestones=res)
vis.add("traj", traj, endEffectors=[gripper_link.index])
solved_trajectory = traj
robot.setConfig(qstart)
elif PROBLEM == '3b':
res = place.plan_place(world, robot, obj, Tobject_grasp,
target_gripper, goal_bounds)
if res is None:
print("Unable to plan place")
else:
(transfer, lower, retract) = res
traj = transfer
traj = traj.concat(lower, relative=True, jumpPolicy='jump')
traj = traj.concat(retract, relative=True, jumpPolicy='jump')
vis.add("traj", traj, endEffectors=[gripper_link.index])
solved_trajectory = traj
robot.setConfig(qstart)
else:
robot.setConfig(qstart)
obj = world.rigidObject(next_item_to_pick)
Tobj0 = obj.getTransform()
print("STARTING TO PICK OBJECT", obj.getName())
orig_grasps = db.object_to_grasps[obj.getName()]
grasps = [
grasp.get_transformed(obj.getTransform()).transfer(
source_gripper, target_gripper) for grasp in orig_grasps
]
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
qgrasp = approach.milestones[-1]
#get the grasp transform
robot.setConfig(qgrasp)
Tobj = obj.getTransform()
Tobject_grasp = se3.mul(se3.inv(gripper_link.getTransform()),
Tobj)
robot.setConfig(lift.milestones[-1])
res = place.plan_place(world, robot, obj, Tobject_grasp,
target_gripper, goal_bounds)
if res is None:
print("Unable to plan place")
else:
(transfer, lower, retract) = res
trajectory_is_transfer = Trajectory()
trajectory_is_transfer.times.append(0)
trajectory_is_transfer.milestones.append([0])
traj = transit
traj = traj.concat(approach,
relative=True,
jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([0])
trajectory_is_transfer.milestones.append([1])
traj = traj.concat(lift, relative=True, jumpPolicy='jump')
traj = traj.concat(transfer,
relative=True,
jumpPolicy='jump')
traj = traj.concat(lower, relative=True, jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([1])
trajectory_is_transfer.milestones.append([0])
traj = traj.concat(retract,
relative=True,
jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([0])
solved_trajectory = traj
obj.setTransform(*Tobj0)
vis.add("traj", traj, endEffectors=[gripper_link.index])
robot.setConfig(qstart)
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()
print("POINT CLOUD CONVERT")
pcnp = to_numpy(pc)
print("Numpy size", pcnp.shape)
pc2 = from_numpy(pcnp, 'PointCloud')
print(pc2.numPoints(), pc2.numProperties())
print("GEOMETRY CONVERT")
gnp = to_numpy(geom)
print("Numpy geometry has transform", gnp[0])
print("TRAJECTORY CONVERT")
resource.setDirectory('.')
traj = resource.get("../../data/motions/athlete_flex_opt.path")
trajnp = to_numpy(traj)
rtraj = RobotTrajectory(w.robot(0), traj.times, traj.milestones)
rtrajnp = to_numpy(rtraj)
rtraj2 = from_numpy(rtrajnp, template=rtraj)
print("Return type", rtraj2.__class__.__name__, "should be RobotTrajectory")
geom2 = w.robot(0).link(11).geometry()
vg = geom2.convert('VolumeGrid', 0.01)
print("VOLUME GRID CONVERT")
vgnp = to_numpy(vg.getVolumeGrid())
print(vgnp[0], vgnp[1])
import matplotlib.pyplot as plt
import numpy as np
grid = vgnp[2]
gridslice = grid.shape[0] / 2
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()
def planOptimizedTrajectory(world,
robot,
target,
maxTime=float('inf'),
numRestarts=1,
plannerSettings={
'type': 'sbl',
'shortcut': True
},
plannerMaxIters=3000,
plannerMaxTime=10.0,
plannerContinueOnRestart=False,
optimizationMaxIters=200,
kinematicsCache=None,
settings=None,
logFile=None):
"""
Args:
world (WorldModel): contains all objects
robot (RobotModel): the moving robot
target: a target configuration
maxTime (float, optional): a total max time for all planning / optimization calls
numRestarts (int): number of restarts of the randomized planner
plannerSettings (dict): a Klamp't planner settings dictionary
plannerMaxIters (int): max number of iterations to perform each planner call
plannerMaxTime (float): max time for each planner call
plannerContinueOnRestart (bool): if true, the planner doesn't get reset every restart
optimizationMaxIters (int): max number of total optimization iterations
kinematicsCache (RobotKinematicsCache, optional): modifies the kinematics cache.
settings (SemiInfiniteOptimizationSettings, optional): modifies the optimizer setting
logFile (file, optional): if given, this is a CSV file that logs the output
Returns a RobotTrajectory that (with more iterations) optimizes path length
"""
bestPath = None
bestPathCost = float('inf')
best_instantiated_params = None
t0 = time.time()
if kinematicsCache is None:
kinematicsCache = geometryopt.RobotKinematicsCache(robot)
if settings is None:
settings = geometryopt.SemiInfiniteOptimizationSettings()
settings.minimum_constraint_value = -0.02
optimizing = False
if 'optimizing' in plannerSettings:
optimizing = plannerSettings['optimizing']
plannerSettings = plannerSettings.copy()
del plannerSettings['optimizing']
obstacles = [
world.rigidObject(i) for i in xrange(world.numRigidObjects())
] + [world.terrain(i) for i in xrange(world.numTerrains())]
obstaclegeoms = [
geometryopt.PenetrationDepthGeometry(obs.geometry(), None, 0.04)
for obs in obstacles
]
t1 = time.time()
print("Preprocessing took time", t1 - t0)
if logFile:
logFile.write(
"numRestarts,plannerSettings,plannerMaxIters,plannerMaxTime,optimizationMaxIters\n"
)
logFile.write("%d,%s,%d,%g,%d\n" %
(numRestarts, str(plannerSettings), plannerMaxIters,
plannerMaxTime, optimizationMaxIters))
logFile.write("restart,iterations,time,method,path length\n")
tstart = time.time()
startConfig = robot.getConfig()
for restart in xrange(numRestarts):
print(
"***************************************************************")
#optimize existing best path, if available
if bestPath is not None and optimizationMaxIters > 0:
t0 = time.time()
#optimize the best path
settings.max_iters = optimizationMaxIters // (numRestarts * 2)
settings.instantiated_params = best_instantiated_params
print("Optimizing current best path with", settings.max_iters,
"iters over", len(bestPath.milestones), "milestones")
if logFile:
logFile.write("%d,0,%g,optimize-best,%g\n" %
(restart, t0 - tstart, bestPathCost))
trajCache = geometryopt.RobotTrajectoryCache(
kinematicsCache,
len(bestPath.milestones) - 2,
qstart=startConfig,
qend=target)
trajCache.tstart = 0
trajCache.tend = bestPath.times[-1]
obj = geometryopt.TrajectoryLengthObjective(trajCache)
constraint = geometryopt.RobotTrajectoryCollisionConstraint(
obstaclegeoms, trajCache)
#constraint = geometryopt.MultiSemiInfiniteConstraint([geometryopt.RobotLinkTrajectoryCollisionConstraint(robot.link(i),obstaclegeoms[0],trajCache) for i in range(robot.numLinks())])
traj, traj_trace, traj_times, constraintPts = geometryopt.optimizeCollFreeTrajectory(
trajCache,
bestPath,
obstacles,
constraints=[constraint],
settings=settings)
if logFile:
#write the whole trace
for i, (traji, ti) in enumerate(zip(traj_trace, traj_times)):
if i == 0: continue
logFile.write("%d,%d,%g,optimize-best,%g\n" %
(restart, i, t0 + ti - tstart,
min(bestPathCost, traji.length())))
xtraj = trajCache.trajectoryToState(traj)
#cost = obj.value(xtraj)
cost = traj.length()
constraint.setx(xtraj)
residual = constraint.minvalue(xtraj)
constraint.clearx()
print("Optimized cost", cost, "and residual", residual)
if cost < bestPathCost and residual[0] >= 0:
print("Got a better cost path by optimizing current best",
cost, "<", bestPathCost)
bestPathCost = cost
bestPath = traj
best_instantiated_params = constraintPts
t1 = time.time()
if t1 - tstart > maxTime:
break
#do sampling-based planningif bestPath is not None:
if restart == 0 or (not plannerContinueOnRestart
and planner.getPath()):
robot.setConfig(startConfig)
planner = robotplanning.planToConfig(world, robot, target,
**plannerSettings)
t0 = time.time()
if logFile:
logFile.write("%d,0,%g,sample,%g\n" %
(restart, t0 - tstart, bestPathCost))
path = None
oldpath = None
for it in xrange(0, plannerMaxIters, 50):
oldpath = path
planner.planMore(50)
t1 = time.time()
path = planner.getPath()
if path:
if oldpath is None:
print("Found a feasible path on restart", restart,
"iteration", it)
path = RobotTrajectory(robot, range(len(path)), path)
cost = path.length()
if cost < bestPathCost:
print("Got a better cost path from planner", cost, "<",
bestPathCost)
bestPathCost = cost
bestPath = path
if logFile:
if oldpath is not None:
logFile.write(
"%d,%d,%g,sample-optimize,%g\n" %
(restart, it + 50, t1 - tstart, bestPathCost))
else:
logFile.write(
"%d,%d,%g,sample,%g\n" %
(restart, it + 50, t1 - tstart, bestPathCost))
if not optimizing:
break
if t1 - t0 > plannerMaxTime:
break
if t1 - tstart > maxTime:
break
if logFile:
if oldpath is not None:
logFile.write("%d,%d,%g,sample-optimize,%g\n" %
(restart, it + 50, t1 - tstart, bestPathCost))
else:
logFile.write("%d,%d,%g,sample,%g\n" %
(restart, it + 50, t1 - tstart, bestPathCost))
if t1 - tstart > maxTime:
break
#optimize new planned path, if available
if path and len(path.milestones) == 2:
#straight line path works, this is certainly optimal
return path
elif path and len(path.milestones) > 2:
#found a feasible path
t0 = time.time()
if len(path.milestones) < 30:
traj0 = arc_length_refine(
path, min(30 - len(path.milestones), len(path.milestones)))
else:
traj0 = path
traj0.times = [
float(i) / (len(traj0.milestones) - 1)
for i in xrange(len(traj0.times))
]
trajCache = geometryopt.RobotTrajectoryCache(
kinematicsCache,
len(traj0.milestones) - 2,
qstart=startConfig,
qend=target)
trajCache.tstart = 0
trajCache.tend = traj0.times[-1]
print()
print("Planned trajectory with", len(traj0.milestones),
"milestones")
obj = geometryopt.TrajectoryLengthObjective(trajCache)
constraint = geometryopt.RobotTrajectoryCollisionConstraint(
obstaclegeoms, trajCache)
xtraj = trajCache.trajectoryToState(traj0)
#cost0 = obj.value(xtraj)
cost0 = traj0.length()
constraint.setx(xtraj)
residual0 = constraint.minvalue(xtraj)
constraint.clearx()
print("Planned cost", cost0, "and residual", residual0)
if bestPath:
settings.max_iters = optimizationMaxIters // (numRestarts * 2)
else:
settings.max_iters = optimizationMaxIters // (numRestarts)
settings.instantiated_params = None
if residual0[0] < 0:
print(
"Warning, the planned path has a negative constraint residual?",
residual0[0])
print(" Residual met @ parameter", residual0[1:])
#raw_input("Warning, the planned path has a negative constraint residual?")
if optimizationMaxIters > 0:
if logFile:
logFile.write("%d,0,%g,optimize,%g\n" %
(restart, t0 - tstart, bestPathCost))
print("Optimizing planned path with", settings.max_iters,
"iters")
traj, traj_trace, traj_times, constraintPts = geometryopt.optimizeCollFreeTrajectory(
trajCache,
traj0,
obstacles,
constraints=[constraint],
settings=settings)
if logFile:
#write the whole trace
for i, (traji,
ti) in enumerate(zip(traj_trace, traj_times)):
if i == 0: continue
xtraj = trajCache.trajectoryToState(traji)
constraint.setx(xtraj)
residual = constraint.minvalue(xtraj)
constraint.clearx()
if residual[0] >= 0:
logFile.write("%d,%d,%g,optimize,%g\n" %
(restart, i, t0 + ti - tstart,
min(bestPathCost, traji.length())))
xtraj = trajCache.trajectoryToState(traj)
#cost = obj.value(xtraj)
cost = traj.length()
constraint.setx(xtraj)
residual = constraint.minvalue(xtraj)
constraint.clearx()
print("Optimized cost", cost, "and residual", residual)
if cost < bestPathCost and residual[0] >= 0:
print("Got a better cost path from optimizer", cost, "<",
bestPathCost)
bestPathCost = cost
bestPath = traj
best_instantiated_params = constraintPts
else:
print("Optimizer produced a worse cost (", cost, "vs",
bestPathCost, ") or negative residual, skipping")
if residual[0] < 0:
#raw_input("Warning, the optimized path has a negative constraint residual...")
print(
"Warning, the optimized path has a negative constraint residual..."
)
t1 = time.time()
#if logFile:
# logFile.write("%d,%d,%g,optimize,%g\n"%(restart,settings.max_iters,t1-tstart,bestPathCost))
if t1 - tstart > maxTime:
break
else:
print("No feasible path was found by the planner on restart",
restart)
return bestPath
def main():
print(
"================================================================================"
)
print(sys.argv[0] + ": Simulates a robot file and Python controller")
if len(sys.argv) <= 1:
print(
"USAGE: klampt_sim [world_file] [trajectory (.traj/.path) or controller (.py)]"
)
print()
print(
" Try: klampt_sim athlete_plane.xml motions/athlete_flex_opt.path "
)
print(" [run from Klampt-examples/data/]")
print(
"================================================================================"
)
if len(sys.argv) <= 1:
exit()
world = WorldModel()
#load up any world items, control modules, or paths
control_modules = []
for fn in sys.argv[1:]:
path, base = os.path.split(fn)
mod_name, file_ext = os.path.splitext(base)
if file_ext == '.py' or file_ext == '.pyc':
sys.path.append(os.path.abspath(path))
mod = importlib.import_module(mod_name, base)
control_modules.append(mod)
elif file_ext == '.path':
control_modules.append(fn)
else:
res = world.readFile(fn)
if not res:
print("Unable to load model " + fn)
print("Quitting...")
sys.exit(1)
viewer = MyGLViewer(world)
for i, c in enumerate(control_modules):
if isinstance(c, str):
from klampt.control.blocks import trajectory_tracking
from klampt.model.trajectory import RobotTrajectory
from klampt.io import loader
traj = loader.load('Trajectory', c)
rtraj = RobotTrajectory(world.robot(i), traj.times,
traj.milestones)
#it's a path file, try to load it
controller = trajectory_tracking.TrajectoryPositionController(
rtraj)
else:
try:
maker = c.make
except AttributeError:
print("Module", c.__name__, "must have a make() method")
print("Quitting...")
sys.exit(1)
controller = maker(world.robot(i))
viewer.sim.setController(world.robot(i), controller)
vis.setWindowTitle("Klamp't Simulation Tester")
if SPLIT_SCREEN_TEST:
viewer2 = MyGLViewer(world)
vis.setPlugin(viewer)
vis.addPlugin(viewer2)
viewer2.window.broadcast = True
vis.show()
while vis.shown():
time.sleep(0.01)
vis.kill()
else:
vis.run(viewer)