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:
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)
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)
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)
