def main(): world, robot, link_index, geom_index, obs_index = create_world() q0 = [0, -2, 0] qf = [3, 1, 3.14] config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'}, limit_joint=False) trajopt = KineTrajOpt(world, robot, q0=q0, qf=qf, config=config, link_index=link_index, geom_index=geom_index, obs_index=obs_index) # create guess and solve n_grid = 10 guess = np.linspace(q0, qf, n_grid) rst = trajopt.optimize(guess) print(rst) traj = rst['sol'] # show the results vis.add('world', world) robot.setConfig(traj[-1]) for i, qi in enumerate(traj[:-1]): name = 'ghost%d' % i vis.add(name, qi) vis.setAttribute(name, 'type', 'Config') vis.setColor(name, 0, 1, 0, 0.4) vis.spin(100)
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 sampling_9(serial_num): world_name = "flatworld" world_file = "../../data/robot_model_files/worlds/" + world_name + ".xml" robot_file = "../../data/robot_model_files/robots/irb120_icp.rob" world = get_world(world_file, robot_file, visualize_robot=True) robot = world.robot(0) est_config_list = cal_est_config_list(robot, serial_num) path = collision_checking(world, est_config_list, 0.001) if path is False: sys.exit("The calibration path exist self-collision.") else: print("The calibration path is collision free.") vis.clear() vis.add("world", world) for i in range(len(est_config_list)): vis.add("estimated configure" + str(i), est_config_list[i]) vis.setColor("estimated configure" + str(i), 1.0, 0.0, 0.1 * i, 0.5) vis.spin(float('inf')) df = pd.DataFrame(np.asarray(est_config_list)[:, 7:13] * (180. / np.pi)) df.to_csv("../../data/robot_configuration_files/configuration_record_" + serial_num + ".csv", header=False, index=False) time.sleep(1.) est_config_list = est_config_list[0:] controller = SampleController(est_config_list, robot, '192.168.1.178') controller.start()
def __init__(self,endeffectors,constraints,traj): vis.GLPluginInterface.__init__(self) self.endeffectors = endeffectors self.constraints = constraints self.robot = self.constraints[0].robot self.traj = traj self.refConfig = self.robot.getConfig() vis.add("ghost1",self.refConfig) vis.setColor("ghost1",0,1,0,0.5) #vis.hide("ghost1",False) def bumpTrajectory(): relative_xforms = [] robot.setConfig(self.refConfig) for e in self.endeffectors: xform = vis.getItemConfig("ee_"+robot.link(e).getName()) T = (xform[:9],xform[9:]) T0 = robot.link(e).getTransform() Trel = se3.mul(se3.inv(T0),T) print "Relative transform of",e,"is",Trel relative_xforms.append(Trel) bumpTraj = cartesian_trajectory.cartesian_bump(self.robot,self.traj,self.constraints,relative_xforms,ee_relative=True,closest=True) assert bumpTraj != None vis.animate(("world",world.robot(0).getName()),bumpTraj) self.refresh() self.add_action(bumpTrajectory,"Bump trajectory",'b')
def show_friction_cone(folder="FrictionCones/StanfordMicroSpineUnit"): V = Volume() V.load(folder) for C in V.convex_decomposition: print "Convex decomposition" print np.amin(C.points,axis=0) print np.amax(C.points,axis=0) Chull = np.vstack([V.convex_hull.points[u] for u in V.convex_hull.vertices]) #print Chull Chull *= 0.05 for i,C in enumerate(V.convex_decomposition): if len(V.convex_decomposition) == 1: g = 0 else: g = float(i)/(len(V.convex_decomposition)-1) #for j,v in enumerate(C.vertices): # vis.add("pt%d,%d"%(i,j),vectorops.mul(C.points[v,:].tolist(),0.05)) # vis.setColor("pt%d,%d"%(i,j),1,g,0) # vis.hideLabel("pt%d,%d"%(i,j)) for j,pt in enumerate(C.points): v = j vis.add("pt%d,%d"%(i,j),vectorops.mul(C.points[v,:].tolist(),0.05)) vis.setColor("pt%d,%d"%(i,j),1,g,0) vis.hideLabel("pt%d,%d"%(i,j)) for i in xrange(Chull.shape[0]): vis.add("CH%d"%(i,),Chull[i,:].tolist()) vis.hideLabel("CH%d"%(i,)) vis.add("origin",(0,0,0)) vis.setColor("origin",0,1,0) vis.dialog()
def RobotCOMPlot(SimRobot, vis): COMPos_start = SimRobot.getCom() COMPos_end = COMPos_start[:] COMPos_end[2] = COMPos_end[2] - 7.50 vis.add("COM", Trajectory([0, 1], [COMPos_start, COMPos_end])) vis.hideLabel("COM", True) vis.setColor("COM", 0.0, 204.0 / 255.0, 0.0, 1.0) vis.setAttribute("COM", 'width', 5.0)
def COM2IntersectionPlot(i, vis, COM_Pos, Intersection): # This function is used to plot PIP from COM to intersections Edge_Index = str(i) vis.add("PIPEdgefromCOM:" + Edge_Index, Trajectory([0, 1], [COM_Pos, Intersection])) vis.hideLabel("PIPEdgefromCOM:" + Edge_Index, True) vis.setAttribute("PIPEdgefromCOM:" + Edge_Index, 'width', 7.5) vis.setColor("PIPEdgefromCOM:" + Edge_Index, 65.0 / 255.0, 199.0 / 255.0, 244.0 / 255.0, 1.0)
def add_hm_to_klampt_vis(self, pcd_fname): pc_xyzs, non_floor_points_xyz = self.parse_hm(pcd_fname) pc_xyzs_as_list = pc_xyzs.flatten().astype("float") pcloud = PointCloud() pcloud.setPoints(int(len(pc_xyzs_as_list) / 3), pc_xyzs_as_list) vis.add("pcloud", pcloud) r, g, b, a = PCloudParser.PCLOUD_COLOR vis.setColor("pcloud", r, g, b, a)
def convert(geom,type,label): global a,b,atypes,btype if label=='A': vis.add(label,atypes[type]) vis.setColor(label,1,0,0,0.5) a = atypes[type] else: vis.add(label,btypes[type]) vis.setColor(label,0,1,0,0.5) b = btypes[type]
def runPlanner(runfunc, name): global lastPlan res = runfunc() if res: if lastPlan: vis.remove(lastPlan) vis.add(name, res) vis.setColor(name, 1, 0, 0) vis.animate(("world", robot.getName()), res) lastPlan = name
def remesh(geom, label): global a, b, Ta, Tb if label == 'A': a = a.convert(a.type(), 0.06) vis.add(label, a) vis.setColor(label, 1, 0, 0, 0.5) a.setCurrentTransform(*Ta) elif label == 'B': b = b.convert(b.type(), 0.06) vis.add(label, b) vis.setColor(label, 0, 1, 0, 0.5) b.setCurrentTransform(*Tb)
def convert(geom, type, label): global a, b, atypes, btypes, Ta, Tb if label == 'A': vis.add(label, atypes[type]) vis.setColor(label, 1, 0, 0, 0.5) a = atypes[type] a.setCurrentTransform(*Ta) else: vis.add(label, btypes[type]) vis.setColor(label, 0, 1, 0, 0.5) b = btypes[type] b.setCurrentTransform(*Tb)
def test_height_around_endeff_costfn(self): br_xyz = (8.759999999999984, 0.48, 0.0) ret = self.step_seq_planner.height_around_endeff_costfn(br_xyz[0], br_xyz[1], debug=True) sphere = klampt.GeometricPrimitive() sphere.setSphere([br_xyz[0], br_xyz[1], 0.1], parameters.STEPSEQ_HEIGHT_AROUND_ENDEFF_R) vis.add("sphere", sphere) vis.setColor("sphere", 0, 0, 1, 0.5) print(ret)
def main(): world, robot, link_index, geom_index, obs_index = create_world() # get start and target for the joints joint_start = [ 0.115, 0.6648, -0.3526, 1.6763, -1.9242, 2.9209, -1.2166, 1.3425, -0.6365, 0.0981, -1.226, -2.0264, -3.0125, -1.3958, -1.9289, 2.9295, 0.5748, -3.137 ] joint_target = [ 0.115, 0.6808, -0.3535, 1.4343, -1.8516, 2.7542, -1.2005, 1.5994, -0.6929, -0.3338, -1.292, -1.9048, -2.6915, -1.2908, -1.7152, 1.3155, 0.6877, -0.0041 ] assert (len(link_index) == len(joint_start) == len(joint_target)) # generate guess n_grid = 10 # resolution guess = np.linspace(joint_start, joint_target, n_grid) # create trajopt instance config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'}, limit_joint=False) trajopt = KineTrajOpt(world, robot, joint_start, joint_target, config=config, link_index=link_index, geom_index=geom_index, obs_index=obs_index) # create some stuff for visualization config = np.array(robot.getConfig()) def update_config(q): config[link_index] = q def get_config(q): cq = copy.copy(config) cq[link_index] = q return cq rst = trajopt.optimize(guess) sol = rst['sol'] vis.add('world', world) dists = [] for i in range(1, n_grid - 1): # set to -2 so the robot stops there with collision vis.add('ghost%d' % i, list(get_config(sol[i]))) vis.setColor('ghost%d' % i, 0, 1, 0, 0.3) robot.setConfig(get_config(sol[i])) update_config(sol[-1]) robot.setConfig(config) vis.spin(float('inf'))
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 __init__(self,endeffectors,constraints): vis.GLPluginInterface.__init__(self) self.endeffectors = endeffectors self.constraints = constraints self.robot = self.constraints[0].robot self.goalConfig = self.robot.getConfig() vis.add("ghost1",self.robot.getConfig()) vis.setColor("ghost1",0,1,0,0.5) vis.add("ghost2",self.robot.getConfig()) vis.setColor("ghost2",1,0,0,0.5) #vis.hide("ghost1",False) #vis.hide("ghost2",False) def goToWidget(): #first, set all constraints so they are fit at the robot's last solved configuration, and get the #current workspace coordinates vis.setItemConfig("ghost1",self.goalConfig) self.robot.setConfig(self.goalConfig) for e,d in zip(self.endeffectors,self.constraints): d.matchDestination(*self.robot.link(e).getTransform()) wcur = config.getConfig(self.constraints) wdest = [] for e in self.endeffectors: xform = vis.getItemConfig("ee_"+robot.link(e).getName()) wdest += xform print "Current workspace coords",wcur print "Dest workspace coords",wdest #Now interpolate self.robot.setConfig(self.goalConfig) traj1 = cartesian_trajectory.cartesian_interpolate_linear(robot,wcur,wdest,self.constraints,delta=1e-2,maximize=False) self.robot.setConfig(self.goalConfig) traj2 = cartesian_trajectory.cartesian_interpolate_bisect(robot,wcur,wdest,self.constraints,delta=1e-2) self.robot.setConfig(self.goalConfig) #traj3 = cartesian_trajectory.cartesian_path_interpolate(robot,[wcur,wdest],self.constraints,delta=1e-2,method='any',maximize=False) traj3 = None print "Method1 Method2 Method3:" print " ",(traj1 != None), (traj2 != None), (traj3 != None) if traj1: traj = traj1 elif traj2: traj = traj2 elif traj3: traj = traj3 else: traj = cartesian_trajectory.cartesian_interpolate_linear(robot,wcur,wdest,self.constraints,delta=1e-2,maximize=True) if traj: print "Result has",len(traj.milestones),"milestones" self.goalConfig = traj.milestones[-1] vis.setItemConfig(("world",world.robot(0).getName()),self.goalConfig) vis.animate(("world",world.robot(0).getName()),traj,speed=0.2,endBehavior='loop') vis.setItemConfig("ghost2",traj.milestones[-1]) vis.add("ee_trajectory",traj) self.refresh() self.add_action(goToWidget,"Go to widget",' ')
def main(): world, robot, link_index, geom_index, obs_index = create_world() joint_start = [-1.832, -0.332, -1.011, -1.437, -1.1, -1.926, 3.074] joint_target = [0.062, 1.287, 0.1, -1.554, -3.011, -0.268, 2.988] # compute the goal pose config0 = robot.getConfig() for lid, theta in zip(link_index, joint_target): config0[lid] = theta robot.setConfig(config0) tR, tt = robot.link(link_index[-1]).getTransform() print(f'target is {tR}, {tt}') # create the trajopt instance config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'}, limit_joint=False) trajopt = KineTrajOpt(world, robot, q0=joint_start, qf=None, config=config, link_index=link_index, geom_index=geom_index, obs_index=obs_index) FINAL_POSE = True if FINAL_POSE: trajopt.add_pose_constraint(-1, link_index[-1], (tR, tt)) # add keep constraint trajopt.add_orientation_constraint(None, link_index[-1], tR) # generate guess n_grid = 10 # resolution guess = np.linspace(joint_start, joint_target, n_grid) # create some stuff for visualization config = np.array(robot.getConfig()) def update_config(q): config[link_index] = q def get_config(q): cq = copy.copy(config) cq[link_index] = q return cq rst = trajopt.optimize(guess) sol = rst['sol'] vis.add('world', world) for i in range(1, n_grid - 2): # set to -2 so the robot stops there with collision vis.add('ghost%d' % i, list(get_config(sol[i]))) vis.setColor('ghost%d' % i, 0, 1, 0, 0.5) update_config(sol[-1]) vis.spin(float('inf'))
def sample_config(self): """ :return: None """ self.Clink_set = np.array( pd.read_csv(self.config_filename, header=None)).tolist() for i in range(len(self.Clink_set)): self.Clink_set[i] = np.multiply(self.Clink_set[i], np.pi / 180.).tolist() self.Clink_set[i] = [0] * 7 + self.Clink_set[i] + [0] for i in range(len(self.Clink_set)): vis.add("configure " + str(i), self.Clink_set[i]) vis.setColor("configure " + str(i), i * 0.15, 1, i * 0.05, 0.5) self.robot.setConfig(self.Clink_set[i])
def TransitionDataPlot(vis, StepNo, LimbNo, ReachableContacts_data): RowNo, ColumnNo = ReachableContacts_data.shape RowStart = 0 RowEnd = RowNo for i in range(RowStart, RowEnd): point_start = [0.0, 0.0, 0.0] ReachableContact_i = ReachableContacts_data[i] point_start[0] = ReachableContact_i[0] point_start[1] = ReachableContact_i[1] point_start[2] = ReachableContact_i[2] TransName = "Step" + str(StepNo) + "Limb" + str( LimbNo) + "TransPoint:" + str(i) vis.add(TransName, point_start) vis.hideLabel(TransName, True) vis.setColor(TransName, 255.0 / 255.0, 255.0 / 255.0, 51.0 / 255.0, 1.0)
def TransitionDataPlot(vis, StepNo, LimbNo, ReachableContacts_data): RowNo, ColumnNo = ReachableContacts_data.shape RowStart = 0 RowEnd = RowNo Traj = [] for i in range(RowStart, RowEnd): point_i = [0.0, 0.0, 0.0] ReachableContact_i = ReachableContacts_data[i] point_i[0] = ReachableContact_i[0] point_i[1] = ReachableContact_i[1] point_i[2] = ReachableContact_i[2] Traj.append(point_i) TransName = "Stage" + str(StepNo) + "LinkNo" + str(LimbNo) + "Path" vis.add(TransName, Trajectory([0, 1], Traj)) vis.hideLabel(TransName, True) vis.setColor(TransName, 255.0 / 255.0, 255.0 / 255.0, 51.0 / 255.0, 1.0)
def ContactDataPlot(vis, ReachableContacts_data): RowNo, ColumnNo = ReachableContacts_data.shape RowStart = 0 # RowEnd = min(RowNo, 99) RowEnd = RowNo for i in range(RowStart, RowEnd): point_start = [0.0, 0.0, 0.0] ReachableContact_i = ReachableContacts_data[i] point_start[0] = ReachableContact_i[0] point_start[1] = ReachableContact_i[1] point_start[2] = ReachableContact_i[2] vis.add("Point_" + str(i), point_start) vis.hideLabel("Point_" + str(i), True) vis.setColor("Point_" + str(i), 65.0 / 255.0, 199.0 / 255.0, 244.0 / 255.0, 1.0)
def ContactDataPlot(vis, StepNo, LimbNo, ReachableContacts_data): RowNo, ColumnNo = ReachableContacts_data.shape RowStart = 0 RowEnd = RowNo for i in range(RowStart, RowEnd): point_start = [0.0, 0.0, 0.0] ReachableContact_i = ReachableContacts_data[i] point_start[0] = ReachableContact_i[0] point_start[1] = ReachableContact_i[1] point_start[2] = ReachableContact_i[2] ContactName = "Stage" + str(StepNo) + "LinkNo" + str( LimbNo) + "Point:" + str(i) vis.add(ContactName, point_start) vis.hideLabel(ContactName, True) vis.setColor(ContactName, 65.0 / 255.0, 199.0 / 255.0, 244.0 / 255.0, 1.0)
def main(): world, robot = load_world() space = makeSpace(world, robot) collision_check = lambda: (space.selfCollision(robot.getConfig( ))) == False and (space.envCollision(robot.getConfig()) == False) # q0, qf = compute_initial_final(world, robot) q0 = np.array([ 0.0, 0.04143683792522413, -1.183867130629673, 2.0680756463305556, 3.745524327531242, 3.6939852943821956e-06, 0.08270468308944955, 0.0 ]) qf = np.array([ 0.0, 3.590769443639893, -0.8366423979290207, 1.5259988654642873, 5.59380779741848, 0.8157228966102285, 4.712401453217654, 0.0 ]) robot.setConfig(q0) q0 = q0[1:-1] qf = qf[1:-1] link_index = [1, 2, 3, 4, 5, 6] geom_index = link_index obs_index = [0, 1] n_grid = 10 # resolution guess = np.linspace(q0, qf, n_grid) # guess += np.random.uniform(-0.1, 0.1, size=guess.shape) vis.add("world", world) # this one has no additional constraint so it should be easy to solve... config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'}) trajopt = KineTrajOpt(world, robot, q0, qf, config=config, link_index=link_index, obs_index=obs_index, geom_index=geom_index) rst = trajopt.optimize(guess) sol = rst['sol'] for i in range(1, n_grid): robot.setConfig([0] + sol[i].tolist() + [0]) print(collision_check()) vis.add('ghost%d' % i, [0] + list(sol[i]) + [0]) vis.setColor('ghost%d' % i, 0, 1, 0, 0.5) vis.spin(float('inf'))
def visRoadMap(self): vis.show() edgeList = self.G.edges() cnt = 0 for e in edgeList: n1 = e[0] n2 = e[1] tr = trajectory.Trajectory() tr.milestones.append([n1.x, n1.y, n1.z]) tr.milestones.append([n2.x, n2.y, n2.z]) fName = "a" + str(cnt) vis.add(fName, tr) vis.hideLabel(fName) vis.setAttribute(fName, "width", 0.5) vis.setColor(fName, 0.4940, 0.1840, 0.5560) cnt = cnt + 1 self.rmCnt = cnt
def WeightedContactDataPlot(vis, OptimalContact_data, OptimalContactWeights_data): scale = 1.0 for i in range(OptimalContact_data.size / 3): point_start = [0.0, 0.0, 0.0] ReachableContact_i = OptimalContact_data[i] point_start[0] = ReachableContact_i[0] point_start[1] = ReachableContact_i[1] point_start[2] = ReachableContact_i[2] point_end = [0.0, 0.0, 0.0] ReachableContactWeight_i = OptimalContactWeights_data[i] point_end[0] = point_start[0] + scale * ReachableContactWeight_i[0] point_end[1] = point_start[1] + scale * ReachableContactWeight_i[1] point_end[2] = point_start[2] + scale * ReachableContactWeight_i[2] print i vis.add("PointWeights_" + str(i), Trajectory([0, 1], [point_start, point_end])) vis.hideLabel("PointWeights_" + str(i), True) vis.setColor("PointWeights_" + str(i), 0.0, 204.0 / 255.0, 0.0, 1.0) vis.setAttribute("PointWeights_" + str(i), 'width', 5.0)
def main(): world, robot, link_index, geom_index, obs_index = create_world() joint_start = [-1.832, -0.332, -1.011, -1.437, -1.1, -1.926, 3.074] joint_target = [0.062, 1.287, 0.1, -1.554, -3.011, -0.268, 2.988] # generate guess n_grid = 10 # resolution guess = np.linspace(joint_start, joint_target, n_grid) # create trajopt instance config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'}, limit_joint=False) trajopt = KineTrajOpt(world, robot, joint_start, joint_target, config=config, link_index=link_index, geom_index=geom_index, obs_index=obs_index) # create some stuff for visualization config = np.array(robot.getConfig()) def update_config(q): config[link_index] = q def get_config(q): cq = copy.copy(config) cq[link_index] = q return cq rst = trajopt.optimize(guess) sol = rst['sol'] vis.add('world', world) for i in range(1, n_grid - 2): # set to -2 so the robot stops there with collision vis.add('ghost%d' % i, list(get_config(sol[i]))) vis.setColor('ghost%d' % i, 0, 1, 0, 0.5) # dists = compute_distance(world, robot, link_index, obs_index) # print(dists) update_config(sol[-1]) vis.spin(float('inf'))
def visualize_in_klampt(self): from klampt import vis height = 0.25 for xyz in self.scatter: name = f"{xyz[0]}{xyz[1]}{xyz[2]}" traj = trajectory.Trajectory( milestones=[[xyz[0], xyz[1], xyz[2] + 0.001], [xyz[0], xyz[1], xyz[2] + height]]) vis.add(name, traj) vis.hideLabel(name) for i in range(len(self.scatter) - 4, len(self.scatter)): xyz = self.scatter[i] name = f"{xyz[0]}{xyz[1]}{xyz[2]}_2" traj = trajectory.Trajectory( milestones=[[xyz[0], xyz[1], xyz[2] + 0.001], [xyz[0], xyz[1], xyz[2] + 1]]) vis.add(name, traj) vis.setColor(name, 1, 0, 0, 1)
def ImpulsePlot(vis, SimRobot, SimulationTime, ImpulseObj): StartTime = ImpulseObj[0] EndTime = ImpulseObj[1] ImpulForce = ImpulseObj[2] DurationTime = EndTime - StartTime OriPoint = SimRobot.link(19).getWorldPosition([0.0, 0.0, 0.0]) ImpulseScale = 0.001 * (SimulationTime - StartTime) / DurationTime EndPoint = OriPoint[:] EndPoint[0] += ImpulForce[0] * ImpulseScale EndPoint[1] += ImpulForce[1] * ImpulseScale EndPoint[2] += ImpulForce[2] * ImpulseScale if SimulationTime < StartTime: pass elif SimulationTime > EndTime: vis.hide("Impulse", True) else: vis.add("Impulse", Trajectory([0, 1], [OriPoint, EndPoint])) vis.hideLabel("Impulse", True) vis.setColor("Impulse", 235.0 / 255.0, 52.0 / 255.0, 52.0 / 255.0, 1.0) vis.setAttribute("Impulse", 'width', 5.0)
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
def WeightedContactDataPlot(vis, StepNo, LimbNo, OptimalContact_data, OptimalContactWeights_data): scale = 1.0 for i in range(OptimalContact_data.size / 3): point_start = [0.0, 0.0, 0.0] ReachableContact_i = OptimalContact_data[i] point_start[0] = ReachableContact_i[0] point_start[1] = ReachableContact_i[1] point_start[2] = ReachableContact_i[2] point_end = [0.0, 0.0, 0.0] ReachableContactWeight_i = OptimalContactWeights_data[i] point_end[0] = point_start[0] + scale * ReachableContactWeight_i[0] point_end[1] = point_start[1] + scale * ReachableContactWeight_i[1] point_end[2] = point_start[2] + scale * ReachableContactWeight_i[2] ContactName = "Stage" + str(StepNo) + "LinkNo" + str( LimbNo) + "Point:" + str(i) print i vis.add(ContactName, Trajectory([0, 1], [point_start, point_end])) vis.hideLabel(ContactName, True) vis.setColor(ContactName, 0.0, 204.0 / 255.0, 0.0, 1.0) vis.setAttribute(ContactName, 'width', 5.0)