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 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)
qmin, qmax = robot.getJointLimits()
for i in range(len(qmin)):
if qmax[i] - qmin[i] > math.pi * 2:
qmin[i] = -float('inf')
qmax[i] = float('inf')
robot.setJointLimits(qmin, qmax)
qstart = resource.get("start.config", world=world)
#add the world elements individually to the visualization
vis.add("world", world)
vis.add("start", qstart, color=(0, 1, 0, 0.5))
# qgoal = resource.get("goal.config",world=world)
qgoal = resource.get("goal_easy.config", world=world)
robot.setConfig(qgoal)
vis.edit(vis.getItemName(robot))
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