Esempio n. 1
0
        q0 = robot.getConfig()
        for i in range(robot.numLinks()):
            if math.isinf(qmin[i]) or math.isinf(qmax[i]):
                #don't animate floating base
                continue
            traj.times.append(len(traj.times) * 0.5)
            q = q0[:]
            q[i] = qmin[i]
            traj.milestones.append(q)

            traj.times.append(len(traj.times) * 0.5)
            q[i] = qmax[i]
            traj.milestones.append(q)

        save, traj.milestones = resource.edit("trajectory",
                                              traj.milestones,
                                              world=world)
        visualization.animate("robot", traj)

    visualization.show()

    iteration = 0
    while visualization.shown():
        #visualization.lock()
        #can modify the visualization here
        #visualization.unlock()
        #time.sleep(0.01)
        #animationTime = visualization.animationTime()
        iteration += 1

    if len(sys.argv) > 1:
Esempio n. 2
0
    obj = ik.objective(robot.link(robot.numLinks() - 1),
                       local=[0, 0, 0],
                       world=[0.5, 0, 0.5])
    visualization.add("IK goal", obj)
    visualization.dialog()
    space = robotcspace.ClosedLoopRobotCSpace(robot, obj, collider)
    space.eps = 1e-2
    space.setup()

#Generate some waypoint configurations using the resource editor
configs = []
while True:
    (save,
     q) = resource.edit("plan config " + str(len(configs) + 1),
                        robot.getConfig(),
                        "Config",
                        world=world,
                        description="Press OK to add waypoint, cancel to stop")
    if save:
        if False and not space.feasible(q):
            print "Configuration is infeasible. Failures:"
            print " ", space.cspace.feasibilityFailures(q)
            print "Please pick another"
        else:
            configs.append(q)
            robot.setConfig(q)
    else:
        break
if len(configs) == 0:
    exit(0)
Esempio n. 3
0
        space.eps = 1e-2
        space.setup()
else:
    #TESTING: closed loop robot cspace
    collider = WorldCollider(world)
    obj = ik.objective(robot.link(robot.numLinks()-1),local=[0,0,0],world=[0.5,0,0.5])
    visualization.add("IK goal",obj)
    visualization.dialog()
    space = robotcspace.ClosedLoopRobotCSpace(robot,obj,collider)
    space.eps = 1e-2
    space.setup()

#Generate some waypoint configurations using the resource editor
configs = []
while True:
    (save,q) = resource.edit("plan config "+str(len(configs)+1),robot.getConfig(),"Config",world=world,description="Press OK to add waypoint, cancel to stop")
    if save:
        if False and not space.feasible(q):
            print "Configuration is infeasible. Failures:"
            print " ",space.cspace.feasibilityFailures(q)
            print "Please pick another"
        else:
            configs.append(q)
            robot.setConfig(q)
    else:
        break
if len(configs)==0:
    exit(0)

if CLOSED_LOOP_TEST:
    #need to project those onto the manifold
def show(world,desc=""):
    resource.edit(None,value=world.robot(0).getConfig(),type='Config',description=desc,world=world)
Esempio n. 5
0
        qmin,qmax = robot.getJointLimits()
        q0 = robot.getConfig()
        for i in range(robot.numLinks()):
            if math.isinf(qmin[i]) or math.isinf(qmax[i]):
                #don't animate floating base
                continue
            traj.times.append(len(traj.times)*0.5)
            q = q0[:]
            q[i] = qmin[i]
            traj.milestones.append(q)
            
            traj.times.append(len(traj.times)*0.5)
            q[i] = qmax[i]
            traj.milestones.append(q)

        save,traj.milestones = resource.edit("trajectory",traj.milestones,world=world)
        visualization.animate("robot",traj)
    
    visualization.show()
    
    iteration = 0
    while visualization.shown():
        #visualization.lock()
        #can modify the visualization here
        #visualization.unlock()
        #time.sleep(0.01)
        #animationTime = visualization.animationTime()
        iteration += 1

    if len(sys.argv)>1:
        visualization.animate("robot",None)