コード例 #1
0
ファイル: vistemplate.py プロジェクト: paperstiger/Klampt
        print "USAGE: vistemplate.py [world_file]"
        exit()

    #creates a world and loads all the items on the command line
    world = WorldModel()
    for fn in sys.argv[1:]:
        res = world.readFile(fn)
        if not res:
            raise RuntimeError("Unable to load model " + fn)

    coordinates.setWorldModel(world)

    #add the world to the visualizer
    visualization.add("world", world)
    #add the coordinate Manager to the visualizer
    visualization.add("coordinates", coordinates.manager())
    #test a point
    pt = [2, 5, 1]
    visualization.add("some point", pt)
    #test a rigid transform
    visualization.add("some blinking transform", [so3.identity(), [1, 3, 0.5]])
    #test an IKObjective
    link = world.robot(0).link(world.robot(0).numLinks() - 1)
    #point constraint
    #obj = ik.objective(link,local=[[0,0,0]],world=[pt])
    #hinge constraint
    obj = ik.objective(link,
                       local=[[0, 0, 0], [0, 0, 0.1]],
                       world=[pt, [pt[0], pt[1], pt[2] + 0.1]])
    #transform constraint
    #obj = ik.objective(link,R=link.getTransform()[0],t=pt)
コード例 #2
0
ファイル: vistemplate.py プロジェクト: arocchi/Klampt
        print "USAGE: vistemplate.py [world_file]"
        exit()

    #creates a world and loads all the items on the command line
    world = WorldModel()
    for fn in sys.argv[1:]:
        res = world.readFile(fn)
        if not res:
            raise RuntimeError("Unable to load model "+fn)

    coordinates.setWorldModel(world)

    #add the world to the visualizer
    visualization.add("world",world)
    #add the coordinate Manager to the visualizer
    visualization.add("coordinates",coordinates.manager())
    #test a point
    pt = [2,5,1]
    visualization.add("some point",pt)
    #test a rigid transform
    visualization.add("some blinking transform",[so3.identity(),[1,3,0.5]])
    #test an IKObjective
    link = world.robot(0).link(world.robot(0).numLinks()-1)
    #point constraint
    #obj = ik.objective(link,local=[[0,0,0]],world=[pt])
    #hinge constraint
    obj = ik.objective(link,local=[[0,0,0],[0,0,0.1]],world=[pt,[pt[0],pt[1],pt[2]+0.1]])
    #transform constraint
    #obj = ik.objective(link,R=link.getTransform()[0],t=pt)
    visualization.add("ik objective",obj)
コード例 #3
0
ファイル: ex3.py プロジェクト: paperstiger/Klampt
      
if __name__ == "__main__":
    world = WorldModel()
    res = world.readFile("ex2_file.xml")
    if not res: raise RuntimeError("Unable to load world file")

    linkindex = 7
    localpos = (0.17,0,0)
    robot = world.robot(0)
    link = robot.link(linkindex)

    coordinates.setWorldModel(world)
    goalpoint = [0,0,0]
    ptlocal = coordinates.addPoint("ik-constraint-local",localpos,robot.getName()+":"+link.getName())
    ptworld = coordinates.addPoint("ik-constraint-world",goalpoint,"world")
    print coordinates.manager().frames.keys()
    
    visualization.add("robot",robot)
    visualization.add("coordinates",coordinates.manager())
    visualization.show()
    iteration = 0
    while visualization.shown():
        visualization.lock()
        #set the desired position goalpoint to move in a circle
        r = 0.4
        t = visualization.animationTime()
        goalpoint[0],goalpoint[1],goalpoint[2] = 0.8,r*math.cos(t),0.7+r*math.sin(t)
        q = solve_ik(link,localpos,goalpoint)
        robot.setConfig(q)
        #this updates the coordinates module
        coordinates.updateFromWorld()