def get_next_state(self, action, step_count, query=False):
"""
This method implements the take action functionality by asking the robot to take the action, and return the
next robot configuration
:param action: action to take
:param step_count: Number of steps taken by the robot so far
:param query: A boolean variable denotes weather it slearning phase or query phase, if it is a query phase
add a delay for better visualization
:return:
"""
pc = self.robot.getConfig()
old_pc = copy.deepcopy(pc)
# Compute the new positions
pc[0] = pc[0] + self.step_size * np.cos(self.actions[action])
pc[1] = pc[1] + self.step_size * np.sin(self.actions[action])
pc[2] = pc[2]
pc[3] = 0
pc[4] = 0
pc[5] = 0
self.robot.setConfig(pc)
boundary_reached = False
if np.abs(pc[0]) > self.terrain_limit[0] or np.abs(pc[1]) > self.terrain_limit[1]:
boundary_reached = True
q2f = ['{0:.2f}'.format(elem) for elem in pc]
label = "Steps: " + str(step_count)
vis.addText("textStep", label)
cpc = self.robot.getConfig()
# compute the distance
distance = self.check_goal_distance(cpc)
collision, is_goal = self.check_collision()
goal_reached = False
# Update the robot positions
if boundary_reached:
self.robot.setConfig(old_pc)
reward = self.reward_system['boundary']
# print("Hit Boundary", reward)
pc = old_pc
elif collision:
self.robot.setConfig(old_pc)
reward = self.reward_system['collision']
# print("Collision", reward)
# Don't let it cross the boundary
pc = old_pc
elif distance==0:
reward = self.reward_system['goal']
goal_reached = True
else:
reward = self.reward_system['free']
# send only the x and y position
state = np.array([[pc[0], pc[1]]])
time.sleep(0.05)
if query:
time.sleep(0)
return reward, state, goal_reached
def shift_grasp(amt):
global cur_object,cur_grasp,shown_grasps,db
for i,grasp in shown_grasps:
grasp.remove_from_vis("grasp"+str(i))
shown_grasps = []
all_grasps = db.object_to_grasps[db.objects[cur_object]]
if amt == None:
cur_grasp = -1
else:
cur_grasp += amt
if cur_grasp >= len(all_grasps):
cur_grasp = -1
elif cur_grasp < -1:
cur_grasp = len(all_grasps)-1
if cur_grasp==-1:
for i,grasp in enumerate(all_grasps):
grasp.ik_constraint.robot = robot
grasp.add_to_vis("grasp"+str(i))
shown_grasps.append((i,grasp))
print("Showing",len(shown_grasps),"grasps")
else:
grasp = all_grasps[cur_grasp]
grasp.ik_constraint.robot = robot
grasp.add_to_vis("grasp"+str(cur_grasp))
Tbase = grasp.ik_constraint.closestMatch(*se3.identity())
g.add_to_vis(robot,animate=False,base_xform=Tbase)
robot.setConfig(grasp.set_finger_config(robot.getConfig()))
shown_grasps.append((cur_grasp,grasp))
if grasp.score is not None:
vis.addText("score","Score %.3f"%(grasp.score,),position=(10,10))
else:
vis.addText("score","",position=(10,10))
def callback(robot_controller=robot_controller,
drawing_controller=drawing_controller,
storage=[sim_world, planning_world, sim, controller_vis]):
start_clock = time.time()
dt = 1.0 / robot_controller.controlRate()
#advance the controller
robot_controller.startStep()
if robot_controller.status() == 'ok':
drawing_controller.advance(dt)
vis.addText("Status", drawing_controller.state, (10, 20))
robot_controller.endStep()
#update the visualization
sim_robot.setConfig(
robot_controller.configToKlampt(robot_controller.sensedPosition()))
Tee = sim_robot.link(ee_link).getTransform()
vis.add(
"pen axis",
Trajectory([0, 1], [
se3.apply(Tee, ee_local_pos),
se3.apply(Tee,
vectorops.madd(ee_local_pos, ee_local_axis, lifth))
]),
color=(0, 1, 0, 1))
controller_vis.update()
cur_clock = time.time()
duration = cur_clock - start_clock
time.sleep(max(0, dt - duration))
def setParabolic():
traj = trajectory.path_to_trajectory(traj0,
velocities='parabolic',
timing='Linf',
speed=1.0)
print "*** Resulting duration", traj.endTime(), "***"
vis.animate("robot", traj)
vis.addText("label", "Parabolic velocity profile")
def setCosine():
traj = trajectory.path_to_trajectory(traj0,
velocities='cosine',
timing='Linf',
speed=1.0,
stoptol=0.0)
print "*** Resulting duration", traj.endTime(), "***"
vis.animate("robot", traj)
vis.addText("label", "Start/stop cosine velocity profile")
def setStartStop():
traj = trajectory.path_to_trajectory(traj0,
velocities='trapezoidal',
timing='Linf',
speed=1.0,
zerotol=0.0)
print "*** Resulting duration", traj.endTime(), "***"
vis.animate("robot", traj)
vis.addText("label", "Start/stop trapezoidal velocity profile")
def setMinJerk():
traj = trajectory.path_to_trajectory(traj0,
velocities='minimum-jerk',
timing='Linf',
speed=1.0,
stoptol=0.0)
print("*** Resulting duration", traj.endTime(), "***")
vis.animate("robot", traj)
vis.addText("label", "Start/stop minimum-jerk velocity profile")
def multiwindow_template(world):
"""Tests multiple windows and views."""
vis.add("world", world)
vp = vis.getViewport()
vp.w, vp.h = 400, 600
vis.setViewport(vp)
vis.addText("label1", "This is Window 1", (20, 20))
vis.setWindowTitle("Window 1")
vis.show()
id1 = vis.getWindow()
print("First window ID:", id1)
id2 = vis.createWindow("Window 2")
vis.add("Lone point", [0, 0, 0])
vis.setViewport(vp)
vis.addText("label1", "This is Window 2", (20, 20))
print("Second window ID:", vis.getWindow())
vis.setWindow(id2)
vis.spin(float('inf'))
#restore back to 1 window, clear the text
vis.setWindow(id1)
vis.clearText()
vp = vis.getViewport()
vp.w, vp.h = 800, 800
vis.setViewport(vp)
vis.setWindowTitle("vis.spin test: will close in 5 seconds...")
vis.spin(5.0)
#Now testing ability to re-launch windows
vis.setWindowTitle("Shown again. Close me to proceed.")
vis.spin(float('inf'))
vis.setWindowTitle("Dialog test. Close me to proceed.")
vp = vis.getViewport()
vp.w, vp.h = 400, 600
vis.setViewport(vp)
vis.dialog()
vp.w, vp.h = 640, 480
vis.setViewport(vp)
for i in range(3):
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
vis.addPlugin(widgets)
vis.setWindowTitle("Split screen test")
vis.spin(float('inf'))
vis.setPlugin(None)
vis.setWindowTitle("Back to normal. Close me to quit.")
vis.dialog()
vis.kill()
def testCartesianDrive():
w = WorldModel()
#w.readFile("../../data/tx90scenario0.xml")
w.readFile("../../data/robots/jaco.rob")
r = w.robot(0)
solver = CartesianDriveSolver(r)
#set a non-singular configuration
q = r.getConfig()
q[3] = 0.5
r.setConfig(q)
solver.start(q, 6)
vis.add("world", w)
vis.addPlot("timing")
vis.addPlot("info")
vis.show()
time.sleep(0.1)
dt = 0.01
t = 0
while t < 20 and vis.shown():
vis.lock()
if t < 2:
v = [0, 0, 0.25]
elif t < 3:
v = [0, 0, -0.1]
elif t < 3.2:
v = [0, 0, -1]
elif t < 8:
v = [0, 0, 0]
elif t < 10:
v = [-1, 0, 0]
else:
v = [1, 0, 0]
if t < 4:
w = [0, 0, 0]
elif t < 10:
w = [0, -0.25, 0]
else:
w = None
t0 = time.time()
progress, qnext = solver.drive(q, w, v, dt)
t1 = time.time()
vis.addText("debug", "Vel %s" % (str(v), ))
vis.logPlot("timing", "t", t1 - t0)
vis.logPlot("info", "progress", progress)
vis.logPlot("info", "adj", solver.driveSpeedAdjustment)
r.setConfig(qnext)
q = qnext
vis.unlock()
vis.add("tgt", solver.driveTransforms[0])
t += dt
time.sleep(max(0.005 - (t1 - t0), 0))
vis.show(False)
vis.clear()
def check_collision(self):
for iT in range(self.world.numTerrains()):
collRT0 = self.collision_checker.robotTerrainCollisions(
self.world.robot(0), iT)
collision_flag = False
for i, j in collRT0:
collision_flag = True
strng = "Robot collides with " + j.getName()
vis.addText("textCol", strng)
vis.setColor("textCol", 1, 0, 0)
break
for iR in range(self.world.numRigidObjects()):
collRT2 = self.collision_checker.robotObjectCollisions(
self.world.robot(0), iR)
for i, j in collRT2:
if j.getName() != "goal":
collision_flag = True
strng = self.world.robot(
0).getName() + " collides with " + j.getName()
vis.addText("textCol", strng)
vis.setColor("textCol", 1, 0, 0)
if collision_flag:
vis.addText("textCol", "Collision")
vis.setColor("textCol", 1, 0.0, 0.0)
if not collision_flag:
vis.addText("textCol", "No collision")
vis.setColor("textCol", 0.4660, 0.6740, 0.1880)
return collision_flag
def __call__(self):
vis.lock()
#TODO: you may modify the world here. This line tests a sin wave.
pt[2] = 1 + math.sin(self.iteration*0.03)
vis.unlock()
#changes to the visualization with vis.X functions can done outside the lock
if (self.iteration % 100) == 0:
if (self.iteration / 100)%2 == 0:
vis.hide("some blinking transform")
vis.addText("text4","The transform was hidden")
vis.logPlotEvent('plot','hide')
else:
vis.hide("some blinking transform",False)
vis.addText("text4","The transform was shown")
vis.logPlotEvent('plot','show')
#this is another way of changing the point's data without needing a lock/unlock
#vis.add("some point",[2,5,1 + math.sin(iteration*0.03)],keepAppearance=True)
#or
#vis.setItemConfig("some point",[2,5,1 + math.sin(iteration*0.03)])
if self.iteration == 200:
vis.addText("text2","Going to hide the text for a second...")
if self.iteration == 400:
#use this to remove text
vis.clearText()
if self.iteration == 500:
vis.addText("text2","Text added back again")
vis.setColor("text2",1,0,0)
self.iteration += 1
def setHermite():
smoothInput = trajectory.HermiteTrajectory()
smoothInput.makeSpline(traj0)
dtraj = smoothInput.discretize(0.1)
dtraj = trajectory.RobotTrajectory(robot, dtraj.times, dtraj.milestones)
traj = trajectory.path_to_trajectory(
dtraj,
velocities='constant',
timing='limited',
smoothing=None,
stoptol=10.0,
vmax=robot.getVelocityLimits(),
amax=robot.getAccelerationLimits(),
speed=1.0,
)
print("*** Resulting duration", traj.endTime(), "***")
#vis.animate("robot",ltraj)
#vis.animate("robot",dtraj)
vis.animate("robot", traj)
vis.addText("label", "Hermite trajectory")
def loop_callback():
global was_grasping, Tobject_grasp, solved_trajectory, trajectory_is_transfer, executing_plan, execute_start_time, qstart, next_item_to_pick
if not executing_plan:
return
if PHYSICS_SIMULATION:
execute_start_time += sim_dt
t = execute_start_time
else:
t = time.time() - execute_start_time
vis.addText("time", "Time %.3f" % (t), position=(10, 10))
if PROBLEM == '3c':
qstart = solved_trajectory.eval(t)
if PHYSICS_SIMULATION:
#sim.controller(0).setPIDCommand(qstart,solved_trajectory.deriv(t))
#sim.controller(0).setMilestone(qstart)
sim.controller(0).setLinear(qstart, sim_dt * 5)
sim.simulate(sim_dt)
sim.updateWorld()
else:
robot.setConfig(qstart)
during_transfer = trajectory_is_transfer.eval(t)[0]
if not was_grasping:
#pick up object
Tobject_grasp = se3.mul(
se3.inv(gripper_link.getTransform()),
obj.getTransform())
was_grasping = True
if during_transfer:
obj.setTransform(
*se3.mul(robot.link(9).getTransform(), Tobject_grasp))
else:
was_grasping = False
if t > solved_trajectory.duration():
executing_plan = False
solved_trajectory = None
next_item_to_pick += 1
else:
robot.setConfig(solved_trajectory.eval(t, 'loop'))
obj.setTransform(
*se3.mul(robot.link(9).getTransform(), Tobject_grasp))
def checkCollision(self):
vis.lock()
## Checking collision
collisionFlag = False
for iR in range(self.n):
collRT0 = self.collisionChecker.robotTerrainCollisions(
self.world.robot(iR), self.world.terrain(0))
for i, j in collRT0:
collisionFlag = True
strng = "Robot collides with " + j.getName()
if self.showVis:
vis.addText("textCol", strng)
vis.setColor("textCol", 0.8500, 0.3250, 0.0980)
vis.unlock()
return collisionFlag
break
for iR in range(self.n):
collRT2 = self.collisionChecker.robotObjectCollisions(
self.world.robot(iR))
for i, j in collRT2:
collisionFlag = True
strng = self.world.robot(
iR).getName() + " collides with " + j.getName()
if self.showVis:
print(strng)
vis.addText("textCol", strng)
vis.setColor("textCol", 0.8500, 0.3250, 0.0980)
vis.unlock()
return collisionFlag
break
collRT3 = self.collisionChecker.robotSelfCollisions()
for i, j in collRT3:
collisionFlag = True
strng = i.getName() + " collides with " + j.getName()
if self.showVis:
vis.addText("textCol", strng)
vis.setColor("textCol", 0.8500, 0.3250, 0.0980)
vis.unlock()
return collisionFlag
break
if not collisionFlag:
if self.showVis:
vis.addText("textCol", "No collision")
vis.setColor("textCol", 0.4660, 0.6740, 0.1880)
vis.unlock()
return collisionFlag
drawExtra.add("cpb" + str(i))
drawExtra.add("n" + str(i))
if abs(abs(res.depths[i]) - vectorops.distance(p1, p2)) > 1e-7:
print("ERROR IN DEPTH?", res.depths[i],
vectorops.distance(p1, p2))
vis.unlock()
elem, pt = a.rayCast_ext(ray[0], ray[1])
if elem >= 0:
vis.add("hitpt", pt)
vis.setColor("hitpt", 0, 0, 0)
else:
vis.setColor("hitpt", 1, 0, 1)
time.sleep(0.001)
t1 = time.time()
last_cycle_times.append(t1 - t0)
while len(last_cycle_times) > Ntimes:
last_cycle_times.popleft()
last_query_times.append(tq1 - tq0)
while len(last_query_times) > Ntimes:
last_query_times.popleft()
counter += 1
if counter % 30 == 0:
vis.addText(
"counter", "Cycle time %.3fms, query time %.3fms" %
(1000 * sum(last_cycle_times) / len(last_cycle_times),
1000 * sum(last_query_times) / len(last_query_times)), (10, 10))
t0 = t1
vis.kill()
def setDoubleSpeed():
traj = trajectory.path_to_trajectory(traj0, speed=2.0)
print "*** Resulting duration", traj.endTime(), "***"
vis.animate("robot", traj)
vis.addText("label", "2x speed")
world=world,
doedit=True)
if len(configs) < 2:
print "Didn't add 2 or more milestones, quitting"
exit(-1)
resource.set("pathtest.configs", configs)
traj0 = trajectory.RobotTrajectory(robot,
times=range(len(configs)),
milestones=configs)
#show the path in the visualizer, repeating for 60 seconds
traj = trajectory.path_to_trajectory(traj0, speed=1.0)
print "*** Resulting duration", traj.endTime(), "***"
vis.animate("robot", traj)
vis.addText("label", "Default values")
vis.addPlot("plot")
vis.addPlotItem("plot", "robot")
#action callbacks
def setHalfSpeed():
traj = trajectory.path_to_trajectory(traj0, speed=0.5)
print "*** Resulting duration", traj.endTime(), "***"
vis.animate("robot", traj)
vis.addText("label", "0.5x speed")
def setDoubleSpeed():
traj = trajectory.path_to_trajectory(traj0, speed=2.0)
print "*** Resulting duration", traj.endTime(), "***"
def loop_callback():
global was_grasping, Tobject_grasp, solved_trajectory, trajectory_is_transfer, executing_plan, \
execute_start_time, qstart, next_robot_to_move, swab_time, swab_init_height, swab_height
global sensor
sensor.kinematicReset()
sensor.kinematicSimulate(world, 0.01)
if next_robot_to_move == 0:
cur_robot = robot
cur_obj = swab
elif next_robot_to_move == 1:
cur_robot = robot2
cur_obj = plate
if not executing_plan:
return
if PHYSICS_SIMULATION:
execute_start_time += sim_dt
t = execute_start_time
else:
t = time.time() - execute_start_time
vis.addText("time", "Time %.3f" % (t), position=(10, 10))
qstart = solved_trajectory.eval(t)
if PHYSICS_SIMULATION:
# sim.controller(0).setPIDCommand(qstart,solved_trajectory.deriv(t))
# sim.controller(0).setMilestone(qstart)
sim.controller(0).setLinear(qstart, sim_dt * 5)
sim.simulate(sim_dt)
sim.updateWorld()
else:
cur_robot.setConfig(qstart)
during_transfer = trajectory_is_transfer.eval(t)[0]
if not was_grasping:
# pick up object
Tobject_grasp = se3.mul(
se3.inv(cur_robot.link(9).getTransform()),
cur_obj.getTransform())
was_grasping = True
if during_transfer:
cur_obj.setTransform(
*se3.mul(cur_robot.link(9).getTransform(), Tobject_grasp))
else:
was_grasping = False
if t > solved_trajectory.duration() - 0.8:
swab_init_height = swab.getTransform()[1][2]
swab_height = copy.deepcopy(swab_init_height)
if t > solved_trajectory.duration() + 1:
executing_plan = False
solved_trajectory = None
vis.add('gripper end', cur_robot.link(9).getTransform())
next_robot_to_move += 1
# let swab drop into trash can
if solved_trajectory is not None and next_robot_to_move == 0:
if solved_trajectory.duration(
) - 0.8 < t < solved_trajectory.duration() + 1:
if swab_height > 0:
swab_height = swab_init_height - 0.5 * 1 * swab_time**2
swab_time = swab_time + sim_dt
Rs, ts = swab.getTransform()
ts[2] = swab_height
swab.setTransform(Rs, ts)
print("and")
print(so3.matrix(Rb))
print("is", so3.distance(Ra, Rb))
Ta = [Ra, [-1, 0, 1]]
Tb = [Rb, [1, 0, 1]]
if __name__ == "__main__":
vis.add("world", se3.identity())
vis.add("start", Ta)
vis.add("end", Tb)
vis.add("interpolated", Ta)
vis.edit("start")
vis.edit("end")
vis.setAttribute("world", "length", 0.25)
vis.setAttribute("interpolated", "fancy", True)
vis.setAttribute("interpolated", "width", 0.03)
vis.setAttribute("interpolated", "length", 1.0)
vis.addText("angle_display", "")
vis.show()
t0 = time.time()
while vis.shown():
#interpolate with a period of 3 seconds
period = 3.0
u = ((time.time() - t0) % period) / period
T = interpolate_transform(Ta, Tb, u)
#uncomment for question 3
#vis.addText("angle_display","Angle to a: %f, b: %f"%(so3.distance(Ta[0],T[0]),so3.distance(Tb[0],T[0])))
vis.setItemConfig("interpolated", T)
time.sleep(0.01)
def modification_template(world):
"""Tests a variety of miscellaneous vis functions"""
vis.add("world",world)
robot = world.robot(0)
vis.setColor(("world",world.terrain(0).getName()),1,0,0,0.5) #turn the terrain red and 50% opaque
import random
for i in range(10):
#set some links to random colors
randlink = random.randint(0,robot.numLinks()-1)
color = (random.random(),random.random(),random.random())
vis.setColor(("world",robot.getName(),robot.link(randlink).getName()),*color)
#test the on-screen text display
vis.addText("text2","Here's some red text")
vis.setColor("text2",1,0,0)
vis.addText("text3","Here's bigger text")
vis.setAttribute("text3","size",24)
vis.addText("text4","Transform status")
vis.addText("textbottom","Text anchored to bottom of screen",(20,-30))
#test a point
pt = [2,5,1]
vis.add("some point",pt)
#test a rigid transform
vis.add("some blinking transform",[so3.identity(),[1,3,0.5]])
vis.edit("some point")
#vis.edit("some blinking transform")
#vis.edit("coordinates:ATHLETE:ankle roll 3")
#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)
vis.add("ik objective",obj)
#enable plotting
vis.addPlot('plot')
vis.addPlotItem('plot','some point')
vis.setPlotDuration('plot',10.0)
#run the visualizer, which runs in a separate thread
vis.setWindowTitle("Manual animation visualization test")
class MyCallback:
def __init__(self):
self.iteration = 0
def __call__(self):
vis.lock()
#TODO: you may modify the world here. This line tests a sin wave.
pt[2] = 1 + math.sin(self.iteration*0.03)
vis.unlock()
#changes to the visualization with vis.X functions can done outside the lock
if (self.iteration % 100) == 0:
if (self.iteration / 100)%2 == 0:
vis.hide("some blinking transform")
vis.addText("text4","The transform was hidden")
vis.logPlotEvent('plot','hide')
else:
vis.hide("some blinking transform",False)
vis.addText("text4","The transform was shown")
vis.logPlotEvent('plot','show')
#this is another way of changing the point's data without needing a lock/unlock
#vis.add("some point",[2,5,1 + math.sin(iteration*0.03)],keepAppearance=True)
#or
#vis.setItemConfig("some point",[2,5,1 + math.sin(iteration*0.03)])
if self.iteration == 200:
vis.addText("text2","Going to hide the text for a second...")
if self.iteration == 400:
#use this to remove text
vis.clearText()
if self.iteration == 500:
vis.addText("text2","Text added back again")
vis.setColor("text2",1,0,0)
self.iteration += 1
callback = MyCallback()
if not MULTITHREADED:
vis.loop(callback=callback,setup=vis.show)
else:
vis.show()
while vis.shown():
callback()
time.sleep(0.01)
#use this to remove a plot
vis.remove("plot")
vis.kill()
def update(self):
from klampt import vis
t = 0
try:
t = self.interface.clock()
vis.addText(self.tag + "clock", '%.3f' % (t, ),
(self.text_x, self.text_y))
except NotImplementedError:
vis.addText(self.tag + "clock",
str(self.interface) + " provides no clock",
(self.text_x, self.text_y))
try:
stat = self.interface.status()
if stat != 'ok':
vis.addText(self.tag + "status",
'Status: ' + stat, (self.text_x, self.text_y + 15),
color=(1, 0, 0))
else:
try:
vis.remove(self.tag + "status")
except Exception:
pass
except NotImplementedError:
vis.addText(self.tag + "status",
str(self.interface) + " provides no status",
(self.text_x, self.text_y + 15))
moving = False
endTime = 0
try:
moving = self.interface.isMoving()
if moving:
endTime = self.interface.destinationTime()
if moving:
if endTime > t:
vis.addText(self.tag + "moving",
"Moving, %.3fs left" % (endTime - t, ),
(self.text_x, self.text_y + 30))
else:
vis.addText(self.tag + "moving", "Moving",
(self.text_x, self.text_y + 30))
else:
try:
vis.remove(self.tag + "moving")
except Exception:
pass
except NotImplementedError:
pass
try:
qsns = self.interface.configToKlampt(
self.interface.sensedPosition())
vis.add(self.tag + "q_sns",
qsns,
robot=self.visRobotIndex,
color=(0, 1, 0, 0.5))
except NotImplementedError:
qsns = None
try:
qcmd = self.interface.configToKlampt(
self.interface.commandedPosition())
if qcmd != qsns:
vis.add(self.tag + "q_cmd",
qcmd,
robot=self.visRobotIndex,
color=(1, 1, 0, 0.5))
except NotImplementedError:
pass
try:
if moving and endTime > t:
qdes = self.interface.configToKlampt(
self.interface.destinationPosition())
vis.add(self.tag + "q_dest",
qdes,
robot=self.visRobotIndex,
color=(1, 0, 0, 0.5))
else:
try:
vis.remove(self.tag + "q_dest")
except Exception:
pass
except NotImplementedError:
pass
def __init__(self, fn):
## Creates a world and loads all the items on the command line
self.world = WorldModel()
self.robotSystemName = 'O'
for f in fn:
print(f)
res = self.world.readFile(f)
if not res:
raise RuntimeError("Unable to load model " + fn)
self.showVis = False
coordinates.setWorldModel(self.world)
vis.lock()
bW.getDoubleRoomWindow(self.world, 8, 8, 1)
vis.unlock()
## Add the world to the visualizer
vis.add("world", self.world)
vp = vis.getViewport()
vp.w, vp.h = 1800, 800
vis.setViewport(vp)
self.robots = []
self.n = self.world.numRobots()
for i in range(self.n):
self.robots.append(
sphero6DoF(self.world.robot(i),
self.world.robot(i).getName()))
self.eps = 0.000001
self.sj = [[0, 0, 0], [0.2, 0, 0]]
self.sjStar = [[-0.070534, -0.097082, 0], [0, -0.06, 0],
[0.070534, -0.097082, 0], [0.057063, -0.018541, 0],
[0.11413, 0.037082, 0], [0.035267, 0.048541, 0],
[0, 0.12, 0], [-0.035267, 0.048541, 0],
[-0.11413, 0.037082, 0], [-0.057063, -0.018541, 0]]
self.sjL = [[0, 0, 0], [0, 0.2, 0], [0, 0.4, 0], [0, 0.6, 0],
[0, 0.8, 0], [0, 1, 0], [0.2, 0, 0], [0.4, 0, 0],
[0.6, 0, 0], [0.8, 0, 0]]
self.sj = self.sjL
self.xB = [-4, 4]
self.yB = [-4, 4]
self.zB = [0.02, 1]
self.rad = 0.04
self.currConfig = [0, 0, 1, 1, 0]
self.scMin = 1
self.scXMin = 1
self.scYMin = 2
self.sumDist = 0
if self.n > 1:
minSij = vectorops.norm(vectorops.sub(self.sj[0], self.sj[1]))
minSijX = math.fabs(self.sj[0][0] - self.sj[1][0])
minSijY = math.fabs(self.sj[0][1] - self.sj[1][1])
for i in range(self.n):
for j in range(self.n):
if i != j:
dist = vectorops.norm(
vectorops.sub(self.sj[i], self.sj[j]))
if dist < minSij:
minSij = dist
dist = math.fabs(self.sj[i][0] - self.sj[j][0])
if dist < minSijX:
minSijX = dist
dist = math.fabs(self.sj[i][1] - self.sj[j][1])
if dist < minSijY:
minSijY = dist
for i in range(self.n):
self.sumDist += vectorops.norm(self.sj[i])
if minSij > self.eps:
self.scMin = 2 * math.sqrt(2) * self.rad / minSij
if minSijX > self.eps:
self.scXMin = 2 * math.sqrt(2) * self.rad / minSijX
if minSijY > self.eps:
self.scYMin = 2 * math.sqrt(2) * self.rad / minSijY
self.scMax = max(2, self.scMin)
self.scB = [self.scMin, 2 * self.scMin]
print('Minimum scale')
print(self.scMin)
vis.add(self.robotSystemName, [so3.identity(), [10, 10, -10]])
vis.setAttribute(self.robotSystemName, "size", 12)
vis.edit(self.robotSystemName)
vis.add("WCS", [so3.identity(), [0, 0, 0]])
vis.setAttribute("WCS", "size", 32)
vis.edit("WCS")
self.collisionChecker = collide.WorldCollider(self.world)
if self.showVis:
## Display the world coordinate system
vis.addText("textCol", "No collision")
vis.setAttribute("textCol", "size", 24)
## On-screen text display
vis.addText("textConfig", "Robot configuration: ")
vis.setAttribute("textConfig", "size", 24)
vis.addText("textbottom",
"WCS: X-axis Red, Y-axis Green, Z-axis Blue",
(20, -30))
print "Starting visualization window#..."
## Run the visualizer, which runs in a separate thread
vis.setWindowTitle("Visualization for kinematic simulation")
vis.show()
simTime = 60
startTime = time.time()
oldTime = startTime
self.setConfig(0, 0, 1, 1, 0)
q = self.robots[0].getConfig()
if self.showVis:
q2f = ['{0:.2f}'.format(elem) for elem in q]
strng = "Robot configuration: " + str(q2f)
vis.addText("textConfig", strng)
colFlag = self.checkCollision()
print(colFlag)
if self.showVis:
time.sleep(10)
vis.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)
vis.add("ik objective", obj)
vis.edit("some point")
#vis.edit("some blinking transform")
#vis.edit("coordinates:ATHLETE:ankle roll 3")
#test the on-screen text display
vis.addText("text2", "Here's some red text")
vis.setColor("text2", 1, 0, 0)
vis.addText("text3", "Here's bigger text")
vis.setAttribute("text3", "size", 24)
vis.addText("text4", "Transform status")
vis.addText("textbottom", "Text anchored to bottom of screen", (20, -30))
vis.addPlot('plot')
vis.addPlotItem('plot', 'some point')
vis.setPlotDuration('plot', 10.0)
print "Visualization items:"
vis.listItems(indent=2)
vis.autoFitCamera()
def setHalfSpeed():
traj = trajectory.path_to_trajectory(traj0, speed=0.5)
print("*** Resulting duration", traj.endTime(), "***")
vis.animate("robot", traj)
vis.addText("label", "0.5x speed")
#robot.setAltitude(0.01)
robot = turtlebot(world.robot(0), "turtle", vis)
robot.setAltitude(0.12)
#robot = sphero6DoF(world.robot(0), "sphero", vis)
## Display the world coordinate system
vis.add("WCS", [so3.identity(), [0, 0, 0]])
vis.setAttribute("WCS", "size", 24)
#print "Visualization items:"
#vis.listItems(indent=2)
#vis.autoFitCamera()
vis.addText("textCol", "No collision")
vis.setAttribute("textCol", "size", 24)
collisionFlag = False
collisionChecker = collide.WorldCollider(world)
## On-screen text display
vis.addText("textConfig", "Robot configuration: ")
vis.setAttribute("textConfig", "size", 24)
vis.addText("textbottom", "WCS: X-axis Red, Y-axis Green, Z-axis Blue",
(20, -30))
print "Starting visualization window#..."
## Run the visualizer, which runs in a separate thread
vis.setWindowTitle("Visualization for kinematic simulation")
def create(self, start_pc, goal_pc):
"""
This method cretes the simulation
:param start_pc: robot's initial position coordinate
:param goal_pc: goal position coordinate
:return:
"""
print "Creating the Simulator"
object_dir = "/home/jeet/PycharmProjects/DeepQMotionPlanning/"
self.start_pc = start_pc
self.goal_pc = goal_pc
coordinates.setWorldModel(self.world)
getDoubleRoomDoor(self.world, 8, 8, 1)
builder = Builder(object_dir)
# Create a goal cube
n_objects = 1
width = 0.1
depth = 0.1
height = 0.1
x = goal_pc[0]
y = goal_pc[1]
z = goal_pc[2] / 2
thickness = 0.005
color = (0.2, 0.6, 0.3, 1.0)
builder.make_objects(self.world, n_objects, "goal", width, depth,
height, thickness, self.terrain_limit, color,
self.goal_pc)
# Create a obstacle cube
n_objects = 4
width = 0.2
depth = 0.2
height = 0.2
x = 2.3
y = 0.8
z = goal_pc[2] / 2
thickness = 0.001
color = (0.8, 0.2, 0.2, 1.0)
builder.make_objects(self.world, n_objects, "rigid", width, depth,
height, thickness, self.terrain_limit, color)
self.vis = vis
vis.add("world", self.world)
# Create the view port
vp = vis.getViewport()
vp.w, vp.h = 1200, 900
vp.x, vp.y = 0, 0
vis.setViewport(vp)
# Create the robot
self.robot = sphero6DoF(self.world.robot(0), "", None)
self.robot.setConfig(start_pc)
# Create the axis representation
# vis.add("WCS", [so3.identity(), [0, 0, 0]])
# vis.setAttribute("WCS", "size", 24)
# Add text messages component for collision check and robot position
vis.addText("textCol", "No collision")
vis.setAttribute("textCol", "size", 24)
vis.addText("textStep", "Steps: ")
vis.setAttribute("textStep", "size", 24)
vis.addText("textGoalDistance", "Goal Distance: ")
vis.setAttribute("textGoalDistance", "size", 24)
vis.addText("textConfig", "Robot configuration: ")
vis.setAttribute("textConfig", "size", 24)
self.collision_checker = collide.WorldCollider(self.world)
vis.setWindowTitle("Simulator")
vis.show()
return vis, self.robot, self.world
