def sphere_grid_test(N=5, staggered=True):
    from klampt import vis
    from klampt.model import trajectory
    if staggered:
        G = sphere_staggered_grid(N)
    else:
        G = sphere_grid(N)
    for n in G.nodes():
        x = G.node[n]['params']
        vis.add(str(n), x)
        vis.hideLabel(str(n))
    #draw edges?
    minDist = float('inf')
    maxDist = 0.0
    for i, j in G.edges():
        xi = G.node[i]['params']
        xj = G.node[j]['params']
        vis.add(str(i) + '-' + str(j), trajectory.Trajectory([0, 1], [xi, xj]))
        vis.hideLabel(str(i) + '-' + str(j))
        dist = vectorops.distance(xi, xj)
        if dist > maxDist:
            maxDist = dist
            print "Max dispersion at", i, j, ":", dist
        if dist < minDist:
            minDist = dist
    print "Number of points:", G.number_of_nodes()
    print "Min/max dispersion:", minDist, maxDist
    vis.run()
Exemplo n.º 2
0
def main():
    print("============================================================")
    print(sys.argv[0] + ": Simulates a robot file and Python controller")
    if len(sys.argv) <= 1:
        print("USAGE: simtest.py [world_file] [controller files (.py)]")
    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):
            sys.path.append(os.path.abspath("../control"))
            import trajectory_controller
            #it's a path file, try to load it
            controller = trajectory_controller.make(world.robot(i), c)
        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)
Exemplo n.º 3
0
def grasp_plan_main():
    world = WorldModel()
    world.readFile("camera.rob")
    robot = world.robot(0)
    sensor = robot.sensor(0)
    world.readFile("table.xml")
    xform = resource.get("table_camera_00.xform",type='RigidTransform')
    sensing.set_sensor_xform(sensor,xform)
    box = GeometricPrimitive()
    box.setAABB([0,0,0],[1,1,1])
    box = resource.get('table.geom','GeometricPrimitive',default=box,world=world,doedit='auto')
    bmin,bmax = [v for v in box.properties[0:3]],[v for v in box.properties[3:6]]
    nobj = 5
    obj_fns = []
    for o in range(nobj):
        fn = random.choice(ycb_objects)
        obj = make_ycb_object(fn,world)
        #TODO: you might want to mess with colors here too
        obj.appearance().setSilhouette(0)
        obj_fns.append(fn)
    for i in range(world.numTerrains()):
        #TODO: you might want to mess with colors here too
        world.terrain(i).appearance().setSilhouette(0)
    problem1.arrange_objects(world,obj_fns,bmin,bmax)

    intrinsics = dict()
    w = int(sensor.getSetting('xres'))
    h = int(sensor.getSetting('yres'))
    xfov = float(sensor.getSetting('xfov'))
    yfov = float(sensor.getSetting('yfov'))
    intrinsics['cx'] = w/2
    intrinsics['cy'] = h/2
    intrinsics['fx'] = math.tan(xfov*0.5)*h*2
    intrinsics['fy'] = math.tan(xfov*0.5)*h*2
    print("Intrinsics",intrinsics)
    planner = ImageBasedGraspPredictor()
    def do_grasp_plan(event=None,world=world,sensor=sensor,planner=planner,camera_xform=xform,camera_intrinsics=intrinsics):
        sensor.kinematicReset()
        sensor.kinematicSimulate(world,0.01)
        rgb,depth = sensing.camera_to_images(sensor)
        grasps,scores = planner.generate(camera_xform,camera_intrinsics,rgb,depth)
        for i,(g,s) in enumerate(zip(grasps,scores)):
            color = (1-s,s,0,1)
            g.add_to_vis("grasp_"+str(i),color=color)

    def resample_objects(event=None,world=world,obj_fns=obj_fns,bmin=bmin,bmax=bmax):
        problem1.arrange_objects(world,obj_fns,bmin,bmax)

    vis.add("world",world)
    vis.add("sensor",sensor)
    vis.addAction(do_grasp_plan,'Run grasp planner','p')
    vis.addAction(resample_objects,'Sample new arrangement','s')
    vis.run()
def so3_grid_test(N=5, staggered=True):
    from klampt import vis
    from klampt.model import trajectory
    if staggered:
        G = so3_staggered_grid(N)
    else:
        G = so3_grid(N)
    vispt = [1, 0, 0]
    for n in G.nodes():
        R = G.node[n]['params']
        trans = so3.apply(R, vispt)
        #trans = so3.axis_angle(R)[0]
        #trans = vectorops.unit(so3.quaternion(R)[:3])
        vis.add(str(n), [R, trans])
        vis.hideLabel(str(n))
    #draw edges?
    minDist = float('inf')
    maxDist = 0.0
    for i, j in G.edges():
        Ri = G.node[i]['params']
        Rj = G.node[j]['params']
        tmax = 9
        times = range(tmax + 1)
        milestones = []
        for t in times:
            u = float(t) / tmax
            trans = so3.apply(so3.interpolate(Ri, Rj, u), vispt)
            milestones.append(trans)
        vis.add(
            str(i) + '-' + str(j), trajectory.Trajectory(times, milestones))
        vis.hideLabel(str(i) + '-' + str(j))
        dist = so3.distance(Ri, Rj)
        if dist > maxDist:
            maxDist = dist
            print "Max dispersion at", i, j, ":", dist
        if dist < minDist:
            minDist = dist
    print "Number of points:", G.number_of_nodes()
    print "Min/max dispersion:", minDist, maxDist
    vis.run()
Exemplo n.º 5
0
    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
            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])

    vis.addAction(planTriggered, "Plan to target", 'p')
    vis.run()
Exemplo n.º 6
0
    def print_config(self):
        config = self.robotWidget.get()
        print("Config:",config)
        self.world.robot(0).setConfig(config)
    
    def save_world(self):
        fn = "widgets_test_world.xml"
        print("Saving file to",fn)
        self.world.saveFile(fn)



if __name__ == "__main__":
    print("==============================================================================")
    print("gl_vis_widgets.py: This example demonstrates how to manually add visualization")
    print("widgets to pose a robot using the GLWidgetPlugin interface")
    if len(sys.argv)<=1:
        print()
        print("USAGE: gl_vis_widgets.py [world_file]")
    print("==============================================================================")
    if len(sys.argv)<=1:
        exit()
    world = klampt.WorldModel()
    for fn in sys.argv[1:]:
        res = world.readFile(fn)
        if not res:
            raise RuntimeError("Unable to load model "+fn)
    viewer = MyGLViewer(world)
    vis.run(viewer)
Exemplo n.º 7
0
def main():
    # This funciton is used for the multi-contact humanoid push recovery
    # The default robot to be loaded is the HRP2 robot in this same folder
    print "This funciton is used for the multi-contact humanoid push recovery"
    if len(sys.argv) <= 1:
        print "USAGE: The default robot to be loaded is the HRP2 robot in this same folder"
        exit()
    world = WorldModel()  # WorldModel is a pre-defined class
    input_files = sys.argv[1:]
    # sys.argv will automatically capture the input files' names

    for fn in input_files:
        result = world.readFile(
            fn
        )  # Here result is a boolean variable indicating the result of this loading operation
        if not result:
            raise RuntimeError("Unable to load model " + fn)

    global Terr_No
    Terr_No = world.numTerrains()

    # This system call will rewrite the robot_angle_init.config into the robot_angle_init.txt
    # However, the initiali angular velocities can be customerized by the user in robot_angvel_init.txt

    # # Now world has already read the world file

    # The first step is to validate the feasibility of the given initial condition

    Sigma_Init = [
        1, 1, 0, 0
    ]  # This is the initial contact status:  1__------> contact constraint is active,
    #                                      0--------> contact constraint is inactive
    #                                      [left foot, right foot, left hand, right hand]

    DOF_val, Config_Init = Configuration_Loader(
    )  # This is the initial condition: joint angle and joint angular velocities

    # The initial joint angular velocties will be of the same length and value as the config_init
    global DOF
    DOF = DOF_val

    Velocity_Init = np.zeros(len(Config_Init))
    for i in range(0, len(Config_Init)):
        Velocity_Init[i] = Config_Init[i]
    State_Init = np.append(Config_Init, Velocity_Init)

    # Now it is the validation of the feasibility of the given initial condition

    Init_Config, Init_Velocity = Robot_Init_Opt_fn(world, Sigma_Init,
                                                   State_Init)
    #
    # Init_Config_File = open('Init_Config.txt', 'w')
    # for Config in Init_Config:
    #     print>>Init_Config_File, Config
    # Init_Config_File.close()
    #
    # Init_Velocity_File = open('Init_Velocity.txt', 'w')
    # for Velocity in Init_Velocity:
    #     print>>Init_Velocity_File, Velocity
    # Init_Velocity_File.close()

    # However, the easy method is to directly read in the initial configuration and the initial velocity
    # with open('./Init_Config.txt') as Init_Config_File:
    #     Init_Config = Init_Config_File.read().splitlines()
    # Init_Config = [float(i) for i in Init_Config]
    #
    # with open('./Init_Velocity.txt') as Init_Velocity_File:
    #     Init_Velocity = Init_Velocity_File.read().splitlines()
    # Init_Velocity = [float(i) for i in Init_Velocity]

    # ipdb.set_trace()

    # Here the Initial robot state has been optimized

    # since the optimization of the initial configuration takes longer time, we save the optimized result and directly use it
    # ipdb.set_trace()

    sim_robot = world.robot(0)

    viewer = MyGLViewer(world, Init_Config, Init_Velocity, Sigma_Init)

    # This part of the code  is used to find the extremities in the local coordinate

    # Local_Extremeties = [0.1, 0, -0.1, -0.15 , 0, -0.1, 0.1, 0, -0.1, -0.15 , 0, -0.1, 0, 0, -0.22, 0, 0, -0.205]         # This 6 * 3 vector describes the local coordinate of the contact extremeties in their local coordinate

    # The left foot: 4 points!

    # vis.pushPlugin(viewer)
    #add the world to the visualizer
    # vis.add("world", world)
    # sim_robot.setConfig(Init_Config)
    #
    # vis.show()

    # vis.lock()

    # # left_foot_offset1 = [0.13, 0.055, -0.105]
    # left_foot_offset1 = [-0.015, 0.0, -0.205]
    # # left_foot_offset2 = [0.1, 0, -0.1]
    # # left_foot_offset3 = [0.1, 0, -0.1]
    # # left_foot_offset4 = [0.1, 0, -0.1]
    #
    # low_point = left_foot_offset1
    # up_point = low_point[:]
    # up_point[2] = up_point[2] - 5
    #
    # current_link = sim_robot.link(End_Link_Ind[3])
    # current_link_appear = current_link.appearance()
    #
    # # print current_link.getAxis()
    #
    # link_color = random_colors()
    #
    # # print current_link.getWorldDirection([ 0, 1, 0])
    #
    # current_link_appear.setColor(link_color[0], link_color[1], link_color[2])
    #
    # low_point_coor = current_link.getWorldPosition(low_point)
    # up_point_coor = current_link.getWorldPosition(up_point)
    # print low_point_coor
    #
    # left_ft, rght_ft, left_hd, rght_hd, left_ft_end, rght_ft_end, left_hd_end, rght_hd_end = Contact_Force_vec(robot, Contact_Force_Traj_i, norm)
    # vis.add("foot force", Trajectory([0, 1], [low_point_coor, up_point_coor]))
    # vis.add("right foot force", Trajectory([0, 1], [rght_ft, rght_ft_end]))
    # vis.add("left hand force", Trajectory([0, 1], [left_hd, left_hd_end]))
    # vis.add("right hand force", Trajectory([0, 1], [rght_hd, rght_hd_end]))
    # COMPos_start = robot.getCom()
    # COMPos_end = COMPos_start
    # COMPos_end[2] = COMPos_end[2] + 100
    # vis.add("Center of Mass",  Trajectory([0, 1], [COMPos_start, COMPos_end]))

    # vis.unlock()

    # ipdb.set_trace()

    vis.run(viewer)
Exemplo n.º 8
0
def main():
    print("""
===============================================================================
A program to quickly browse Klamp't objects. 

USAGE: %s [item1 item2 ...]

where the given items are world, robot, terrain, object, or geometry files. Run
it without arguments

   %s

for an empty reference world. You may add items to the reference world using
the `Add to World` button.  If you know what items to use in the reference
world, run it with

   %s world.xml

or 

   %s item1 item2 ...

where the items are world, robot, terrain, object, or geometry files.
===============================================================================
""" % (sys.argv[0], sys.argv[0], sys.argv[0], sys.argv[0]))
    #must be explicitly deleted for some reason in PyQt5...
    g_browser = None

    def makefunc(gl_backend):
        global g_browser
        browser = ResourceBrowser(gl_backend)
        g_browser = browser
        dw = QtWidgets.QDesktopWidget()
        x = dw.width() * 0.8
        y = dw.height() * 0.8
        browser.setFixedSize(x, y)
        for fn in sys.argv[1:]:
            res = browser.world.readFile(fn)
            if not res:
                print("Unable to load model", fn)
                print("Quitting...")
                sys.exit(1)
            print("Added", fn, "to world")
        if len(sys.argv) > 1:
            browser.emptyVisPlugin.add("world", browser.world)
        return browser

    vis.customUI(makefunc)
    vis.setWindowTitle("Klamp't Resource Browser")
    vis.run()
    del g_browser
    return

    #this code below is incorrect...
    app = QtWidgets.QApplication(sys.argv)
    browser = ResourceBrowser()
    for fn in sys.argv[1:]:
        res = browser.world.readFile(fn)
        if not res:
            print("Unable to load model", fn)
            print("Quitting...")
            sys.exit(1)
        print("Added", fn, "to world")
    if len(sys.argv) > 1:
        browser.emptyVisPlugin.add("world", browser.world)
    dw = QtWidgets.QDesktopWidget()
    x = dw.width() * 0.8
    y = dw.height() * 0.8
    browser.setFixedSize(x, y)
    #browser.splitter.setWindowState(QtCore.Qt.WindowMaximized)
    browser.setWindowTitle("Klamp't Resource Browser")
    browser.show()
    # Start the main loop.
    res = app.exec_()
    return res
Exemplo n.º 9
0
from klampt import *
from klampt import vis
from klampt.vis.glrobotprogram import GLWorldPlugin

class MyPlugin(GLWorldPlugin):
  def __init__(self,world):
    GLWorldPlugin.__init__(self,world)
  def mousefunc(self,button,state,x,y):
    #Put your mouse handler here
    #the current example prints out the list of objects clicked whenever
    #you right click
    print "mouse",button,state,x,y
    if button==2:
      if state==0:
        print [o.getName() for o in self.click_world(x,y)]
        return
    GLWorldPlugin.mousefunc(self,button,state,x,y)

  def motionfunc(self,x,y,dx,dy):
    return GLWorldPlugin.motionfunc(self,x,y,dx,dy)

world = WorldModel()
if not world.readFile("../../../Klampt-examples/data/athlete_plane.xml"):
  raise RuntimeError("Couldn't load world")
vis.run(MyPlugin(world))
Exemplo n.º 10
0
def simulation_template(world):
    """Runs a custom simulation plugin"""
    viewer = MyGLSimulationViewer(world)
    vis.run(viewer)
    vis.kill()
Exemplo n.º 11
0
                glVertex3f(V[j][0], V[j][1], V[j][2])
            glEnd()
            glDisable(GL_BLEND)
            glEnable(GL_LIGHTING)
        return True


if __name__ == '__main__':
    if problem == "3":
        from klampt import WorldModel
        from klampt import vis
        world = WorldModel()
        world.readFile("../../data/tx90cuptable.xml")
        plugin = RigidObjectCSpacePlugin(world, world.rigidObject(0))
        vis.setWindowTitle("Rigid object planning")
        vis.run(plugin)
        exit()
    space = None
    start = None
    goal = None
    if problem == "1":
        space = CircleObstacleCSpace()
        space.addObstacle(Circle(0.5, 0.5, 0.36))
        start = (0.06, 0.5)
        goal = (0.94, 0.5)
    elif problem == "2":
        space = RigidBarCSpace()
        space.addObstacle(Circle(0.5, 0.5, 0.4))
        start = (0.1, 0.1, 0.0)
        goal = (0.9, 0.9, 6.20)
    program = CSpaceObstacleProgram(space, start, goal)
Exemplo n.º 12
0
        GLPluginInterface.__init__(self)
        self.world = world
        self.q = world.robot(0).getConfig()

    def display(self):
        self.world.drawGL()

    def motionfunc(self, x, y, dx, dy):
        if 'shift' in self.modifiers():
            self.q[2] = float(y) / 400
            self.q[3] = float(x) / 400
            self.world.robot(0).setConfig(self.q)
            return True
        return False

    def idle(self):
        return True


if __name__ == "__main__":
    print """mousetest.py: A simple program where the mouse motion, when
    shift-clicking, gets translated into joint values for an animated robot."""

    world = WorldModel()
    res = world.readFile("../../data/tx90blocks.xml")
    if not res:
        raise RuntimeError("Unable to load world")
    #set a custom initial configuration of the world
    vis.setWindowTitle("mousetest.py")
    vis.run(GLTest(world))
Exemplo n.º 13
0
from klampt import *
from klampt import vis
from klampt.vis.glrobotprogram import GLWorldPlugin


class MyPlugin(GLWorldPlugin):
    def __init__(self, world):
        GLWorldPlugin.__init__(self, world)

    def mousefunc(self, button, state, x, y):
        #Put your mouse handler here
        #the current example prints out the list of objects clicked whenever
        #you right click
        print "mouse", button, state, x, y
        if button == 2:
            if state == 0:
                print[o.getName() for o in self.click_world(x, y)]
                return
        GLWorldPlugin.mousefunc(self, button, state, x, y)

    def motionfunc(self, x, y, dx, dy):
        return GLWorldPlugin.motionfunc(self, x, y, dx, dy)


world = WorldModel()
if not world.readFile("../../../Klampt-examples/data/athlete_plane.xml"):
    raise RuntimeError("Couldn't load world")
vis.run(MyPlugin(world))
Exemplo n.º 14
0
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)
Exemplo n.º 15
0
        self.sim = sim

    def display(self):
        self.sim.updateWorld()
        self.world.drawGL()

    def idle(self):
        rfs = sim.controller(0).sensor("RF_ForceSensor")
        print("Sensor values:", rfs.getMeasurements())
        sim.simulate(self.dt)
        return


if __name__ == "__main__":
    print("================================================================")
    print(
        "gl_vis.py: This example demonstrates how to use the GL visualization interface"
    )
    print("   to tie directly into the GUI.")
    print()
    print("   The demo simulates a world and reads a force sensor")
    print("================================================================")
    world = klampt.WorldModel()
    res = world.readFile("../../data/hubo_plane.xml")
    if not res:
        raise RuntimeError("Unable to load world")
    sim = klampt.Simulator(world)
    print("STARTING vis.run()")
    vis.run(GLTest(world, sim))
    print("END OF vis.run()")
Exemplo n.º 16
0
def stability_vis():
	program = GLStabilityPlugin()
	vis.run(program)