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