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