def FromRaveTraj(robot, traj): N = traj.GetNumWaypoints() if N < 2: print "RAVE trajectory has less than 2 waypoints" return None timespec = openravepy.ConfigurationSpecification() timespec.AddGroup('deltatime', 1, 'linear') posspec = robot.GetActiveConfigurationSpecification() velspec = posspec.ConvertToVelocitySpecification() chunkslist = [] vel = traj.GetWaypoint(0, velspec) ndof = len(vel) for i in range(N - 1): pos = traj.GetWaypoint(i, posspec) deltatime = traj.GetWaypoint(i + 1, timespec)[0] if deltatime < 1e-5: continue nextvel = traj.GetWaypoint(i + 1, velspec) polynomialslist = [] for j in range(ndof): x = pos[j] v = vel[j] a = (nextvel[j] - vel[j]) / deltatime polynomialslist.append(Trajectory.Polynomial([x, v, a / 2])) chunkslist.append(Trajectory.Chunk(deltatime, polynomialslist)) vel = nextvel return Trajectory.PiecewisePolynomialTrajectory(chunkslist)
def processTrajects(frame, trajects, orig_frame, tick, direct, draw_flag, lower_cnt_radius=3, upper_cnt_radius=15): cnts = cv2.findContours(frame.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] for cnt in cnts: xy, r = cv2.minEnclosingCircle(cnt) if lower_cnt_radius <= r <= upper_cnt_radius: t = findTrajectoryFor(xy, r, trajects, tick) k = EncircledContour(cnt, xy, r, tick=tick) if t is None: newt = Trajectory() newt.add(k) trajects.append(newt) if draw_flag is True: ofc = orig_frame.copy() utils.drawTrajectOnFrame(newt, ofc, tick, direct, imwrite_flag=True) ofc2 = orig_frame.copy() utils.drawTrajectsOnFrame(trajects, ofc2, tick, direct) del ofc del ofc2 else: t.add(k) if draw_flag is True: ofc = orig_frame.copy() drawTrajectOnFrame(t, ofc, tick, direct, imwrite_flag=True) ofc2 = orig_frame.copy() drawTrajectsOnFrame(trajects, ofc2, tick, direct) del ofc del ofc2
def poseRefresh(): """Fonction refreshing robot pose """ global robot global camSensor global visionPeriod global traj objectNpLocation=None #Interruptions: if visionPeriod<1: pxLocation=camSensor.redDetect() if pxLocation is not None: objectNpLocation=camSensor.pixelWorkSpaceLocation(pxLocation) redTraj=Trajectory() location=np.array([0,5,6]) angle=np.array([0,3.14,0]) redTraj.addPlacement=(location,angle,None) robot.routineTrajectory=redTraj else: robot.routineTrajectory=traj visionPeriod=30 else: visionPeriod-=1 robot.refresh()
def addSample(self, timestamp, pose, vel, goal): """ Automatically appends the input state to the matching trajectory. If this measurement is part of a new trajectory, one will be created. """ # hack to catch situation of changing motion direction #if goal[0] < 0: # goal = self.last_goal if goal not in self.goals: # adding goal identifier self.goals.append(goal) self.trajectories.append( traj.Trajectory(goal=np.array([[goal[0], goal[1]]]))) if len(self.goals) > 1: self.traj_idx += 1 else: if len(self.goals) > 0: if not np.all(goal == self.last_goal): self.trajectories.append( traj.Trajectory(goal=np.array([[goal[0], goal[1]]]))) self.traj_idx += 1 # append data sample to trajectory (timestamp in [ns], pose and vel as np 1x3 arrays) self.trajectories[self.traj_idx].addData(timestamp, pose, vel, goal) self.last_goal = goal
def MakeTOPPChunk(vchunk, q0): polylist = [] for i in range(vchunk.ndof): polylist.append( Trajectory.Polynomial([ q0[i], vchunk.v0[i], 0.5 * (vchunk.v1[i] - vchunk.v0[i]) / vchunk.T ])) return Trajectory.Chunk(vchunk.T, polylist)
def sample_traj(K, L, numExps, overlap, acc, dec, delta_lane): traj = Trajectory(K, L, numExps, overlap, acc, dec, delta_lane) dt = 0.1 state_robot, js_noise, counters = traj.sample_trajectory() xs = state_robot[0, :] ys = state_robot[2, :] ts = np.array([dt * i for i in np.arange(overlap + numExps * K)]) return xs, ys, ts
def read_file(infile, segment_size, traj_angles, trajectories): """File input, one row per desire line with columns corresponding to line name, line weight, start x, start y, end x and end y, with comment lines (not to be included as desire lines starting with #""" fh = open (infile, 'r') for line in fh: if line.startswith("#"): continue linelist = line.split(); traj = Trajectory(name=linelist[0], weight = float(linelist[1]), startx=float(linelist[2]), starty=float(linelist[3]), endx=float(linelist[4]), endy=float(linelist[5])) traj.make_segments(segment_size) rounded_angle = round_to(traj.angle, 0.5); traj_angles[rounded_angle].append(traj) #add the trajectory to a list in the dictionary of angles trajectories.append(traj)
def testPrint(): s1 = stsegment(stpoint(0, 1, 2), stpoint(2, 3, 4)) s2 = stsegment(stpoint(0, 3, 4), stpoint(3, 2, 1)) s3 = stsegment(stpoint(1, 3, 6), stpoint(1, 3, 7)) t0 = Trajectory([]) t1 = Trajectory([s1]) t2 = Trajectory([s1, s2]) t3 = Trajectory([s1, s2, s3]) print("__str__") print(str(t1)) print(str(t2)) print(str(t3)) print("__repr__") print(repr(t1)) print(repr(t2)) print(repr(t3))
def run(int_inclination, ramp_length, time_step, sim_time): rocket_file = 'myRocket.dot' path = 'myRocket1/' rocket1 = RocketSimple.from_file(rocket_file, path) int_inclination = 10 / 180.0 * np.pi ramp_length = 2.5 * rocket1.getLength() time_step = 0.003 sim_time = 15 (t, position, euler, linearVelocity, angularVelocity, AoA, thrust, gravity, drag, lift) \ = Trajectory.calculateTrajectory(rocket1, int_inclination, ramp_length, time_step, sim_time) sample_rate = 1 / time_step t = 0 COM = [] COP = [] time_domain = [] while t < sim_time: time_domain = time_domain + [t] t += time_step for t in time_domain: COM = COM + [rocket1.getCOM(t)[0]] for i in range(len(AoA)): #COP = COP + [rocket1.getCOP(AoA[i], np.linalg.norm(linearVelocity[i]))[0]] COP = COP + [0] visual.launch(sample_rate, position, euler, COM, COP, thrust, gravity, lift, drag)
def simulate(self): self.t = self.start while self.t < self.end: self.t += self.t_step # Desired Trajectory [From Input or Trajectory] and State [From Dynamics] self.State_d = TR.trajectory(self.t, self.State_d) # Calculate Actions and Forces from Controller self.PhiC_last, Actions = C2D.control(self.PhiC_last, self.State, self.State_d, self.Gains, self.Params, self.t_step) # Use Dynamics to Simulate New State [Transform Axes, Estimate Derivatives self.State = DYN.dynamics(self.State, self.Params, Actions, self.t_step) ## Calculation time for above measured at around 0.5 milliseconds on average # Record trajectory against simulation time self.t_vec.append(self.t) self.y_vec.append(self.State[0]) self.z_vec.append(self.State[1]) self.phi_vec.append(self.State[4]) self.y_des_vec.append(self.State_d[0]) self.z_des_vec.append(self.State_d[1])
def __calcFlightResults(self, avId, launchTime): head = self.toonHeadDict[avId] startPos = head.getPos(render) startHpr = head.getHpr(render) hpr = self.cannonDict[avId][1].getHpr(render) towerPos = self.tower.getPos(render) rotation = self.__toRadians(hpr[0]) angle = self.__toRadians(hpr[1]) horizVel = INITIAL_VELOCITY * math.cos(angle) xVel = horizVel * -math.sin(rotation) yVel = horizVel * math.cos(rotation) zVel = INITIAL_VELOCITY * math.sin(angle) startVel = Vec3(xVel, yVel, zVel) trajectory = Trajectory.Trajectory(launchTime, startPos, startVel) towerList = [ towerPos + Point3(0, 0, BUCKET_HEIGHT), TOWER_RADIUS, TOWER_HEIGHT - BUCKET_HEIGHT ] self.notify.debug( 'calcFlightResults(%s): rotation(%s), angle(%s), horizVel(%s), xVel(%s), yVel(%s), zVel(%s), startVel(%s), trajectory(%s), towerList(%s)' % (avId, rotation, angle, horizVel, xVel, yVel, zVel, startVel, trajectory, towerList)) timeOfImpact, hitWhat = self.__calcToonImpact(trajectory, towerList) return { 'startPos': startPos, 'startHpr': startHpr, 'startVel': startVel, 'trajectory': trajectory, 'timeOfImpact': timeOfImpact, 'hitWhat': hitWhat }
def extract_features(self, seg, l, smooth=False): #try: t1 = tr.Trajectory(trajectory=seg, labels=[self.target], mood='df') ss = t1.rows() if ss > 10: # t1.get_features(smooth_=False) t1.get_features(smooth_=smooth) # t1.get_full_features() # print(t1.stat_label()) extracted_features = t1.get_full_features() # print(extracted_features) self.results.append(extracted_features) target_lbl = str(t1.descriptor.target_label) nstr_speed = t1.process_speed_sub_traj() nstr_acc = t1.process_acc_sub_traj() nstr_bearing = t1.process_bearing_sub_traj() self.traj_txts_speed = self.traj_txts_speed + " " + nstr_speed self.traj_txts_acc = self.traj_txts_acc + " " + nstr_acc self.traj_txts_bearing = self.traj_txts_bearing + " " + nstr_bearing self.traj_train_txts_speed = self.traj_train_txts_speed + " " + nstr_speed + "," + target_lbl + '\n' self.traj_train_txts_acc = self.traj_train_txts_acc + " " + nstr_acc + "," + target_lbl + '\n' self.traj_train_txts_bearing = self.traj_train_txts_bearing + " " + nstr_bearing + "," + target_lbl + '\n' print("processing ", l, "traj:", seg.shape, target_lbl, extracted_features) del t1
def Interpolate(qlist, vmax, amax, vcoeflist=[], acoeflist=[], tstart=0): npoints = len(qlist) assert (npoints >= 3) if (len(vcoeflist) == 0): vcoeflist = ones(npoints - 1) if (len(acoeflist) == 0): acoeflist = ones(npoints - 1) assert (len(vcoeflist) == npoints - 1 and len(acoeflist) == npoints - 1) q0 = qlist[0] qcur = qlist[1] reslist = ComputeChunks(q0, qcur, min(1, max(TINY, vcoeflist[0])) * vmax, min(1, max(TINY, acoeflist[0])) * amax, tstart) for i in range(2, npoints): qnext = qlist[i] chunkslistb = ComputeChunks(qcur, qnext, min(1, max(TINY, vcoeflist[i - 1])) * vmax, min(1, max(TINY, acoeflist[i - 1])) * amax, 0) chunka = reslist.pop(-1) listmerge = MergeChunks(chunka, chunkslistb) reslist.extend(listmerge) qcur = qnext # Make a TOPP Trajectory topplist = [] qcur = qlist[0] for i in range(len(reslist)): toppchunk = MakeTOPPChunk(reslist[i], qcur) qcur = toppchunk.Eval(toppchunk.duration) topplist.append(toppchunk) return Trajectory.PiecewisePolynomialTrajectory(topplist)
def defineConstants(self): self.notify.debug('defineConstants') self.DropPlacerType = RegionDropPlacer fruits = { ToontownGlobals.ToontownCentral: 'apple', ToontownGlobals.DonaldsDock: 'orange', ToontownGlobals.DaisyGardens: 'pear', ToontownGlobals.MinniesMelodyland: 'coconut', ToontownGlobals.TheBrrrgh: 'watermelon', ToontownGlobals.DonaldsDreamland: 'pineapple', ToontownGlobals.ToontownCentralBeta: 'apple', ToontownGlobals.DaisyGardensBeta: 'pear' } self.fruitName = fruits[self.getSafezoneId()] self.ShowObjSpheres = 0 self.ShowToonSpheres = 0 self.ShowSuitSpheres = 0 self.PredictiveSmoothing = 1 self.UseGravity = 1 self.TrickShadows = 1 self.WantSuits = 1 self.FirstDropDelay = 0.5 self.FasterDropDelay = int(2.0 / 3 * CatchGameGlobals.GameDuration) self.notify.debug('will start dropping fast after %s seconds' % self.FasterDropDelay) self.FasterDropPeriodMult = 0.5 self.calcDifficultyConstants(self.getDifficulty(), self.getNumPlayers()) self.notify.debug('ToonSpeed: %s' % self.ToonSpeed) self.notify.debug('total drops: %s' % self.totalDrops) self.notify.debug('numFruits: %s' % self.numFruits) self.notify.debug('numAnvils: %s' % self.numAnvils) self.ObjRadius = 1.0 dropGridDimensions = [[5, 5], [5, 5], [6, 6], [7, 7]] self.DropRows, self.DropColumns = dropGridDimensions[( self.getNumPlayers() - 1)] self.cameraPosTable = [[0, -29.36, 28.17]] * 2 + [[0, -32.87, 30.43], [0, -35.59, 32.1]] self.cameraHpr = [0, -35, 0] self.CameraPosHpr = self.cameraPosTable[(self.getNumPlayers() - 1)] + self.cameraHpr for objType in DropObjectTypes: self.notify.debug('*** Object Type: %s' % objType.name) objType.onscreenDuration = objType.onscreenDurMult * self.BaselineOnscreenDropDuration self.notify.debug('onscreenDuration=%s' % objType.onscreenDuration) v_0 = 0.0 t = objType.onscreenDuration x_0 = self.MinOffscreenHeight x = 0.0 g = 2.0 * (x - x_0 - v_0 * t) / (t * t) self.notify.debug('gravity=%s' % g) objType.trajectory = Trajectory.Trajectory( 0, Vec3(0, 0, x_0), Vec3(0, 0, v_0), gravMult=abs(g / Trajectory.Trajectory.gravity)) objType.fallDuration = objType.onscreenDuration + self.OffscreenTime
def ObjFunc(x, qstart, qend, ndof, nwaypoints, nsamples, weights, vmax, amax, trajref, robot): traj = MakeTraj(x, qstart, qend, ndof, nwaypoints, vmax, amax) dpos, dvel, dacc = Trajectory.Diff3(traj, trajref, nsamples, robot) cpos, cvel, cacc, cdur = weights cost = cpos * dpos * dpos + cvel * dvel * dvel + cacc * dacc * dacc + cdur * ( traj.duration - trajref.duration) print cost, cpos * dpos * dpos, cvel * dvel * dvel, cacc * dacc * dacc, cdur * ( traj.duration - trajref.duration) return cost
def createTrajectory(self, step): Str = "Vecteur aérien" va = QgsMapLayerRegistry.instance().mapLayersByName( Str.decode('utf-8'))[0] layerPointTraj = self.createTrajLayer(self.layer, step, va) self.layerMenu = QgsProject.instance().layerTreeRoot() self.rmTraj(self.layer.name()[11:]) self.traj = Trajectory(self.iface, self.layer.name()[11:], self.layerMenu, layerPointTraj, None, None, None)
def init_callback(self, start, stop): MovieCallbacks.init_callback(self, start, stop) self.ensemble = Amber(self.prmtop, self.trajectory, None, None) self.frames_done = 0 if self.start > len(self.ensemble) or self.stop > len(self.ensemble): raise RuntimeError( "Cannot set start (%d) or stop (%d) greater than length of ensemble (%d)" % (self.start, self.stop, len(self.ensemble))) if self.start < 0 or self.stop < 0 or self.start >= stop: raise RuntimeError( "Cannot set start (%d) or stop (%d) to less than 0 or start greater than or equil to stop" % (self.start, self.stop)) self.frames_total = self.stop - self.start self.molecule = Trajectory.Ensemble(self.ensemble) self.molecule.CreateMolecule() self.molecule.LoadFrame(1) self.molecule.AddMolecule() self.refmolecule = Trajectory.Ensemble(self.ensemble) self.refmolecule.CreateMolecule() self.refmolecule.LoadFrame(1) self.refmolecule.AddMolecule() Midas.turn("x", -25) Midas.turn("z", 25) Midas.turn("y", 40) Midas.undisplay("#1") Midas.ribbon("#0") Midas.undisplay("#0") Midas.ribcolor("red", "#0:/isHelix") Midas.ribcolor("blue", "#0:/isSheet") Midas.window("#1") Midas.scale(2) Midas.wait(1) self.t0 = time.time()
def InterpolateViapoints(path): nviapoints = len(path[0, :]) tv = linspace(0, 1, nviapoints) for i in range(nviapoints - 1): tv[i + 1] = tv[i] + linalg.norm(path[:, i] - path[:, i + 1]) tcklist = [] for idof in range(0, path.shape[0]): tcklist.append(interpolate.splrep(tv, path[idof, :], s=0)) t = tcklist[0][0] chunkslist = [] for i in range(len(t) - 1): polylist = [] if abs(t[i + 1] - t[i]) > 1e-5: for tck in tcklist: a = 1 / 6. * interpolate.splev(t[i], tck, der=3) b = 0.5 * interpolate.splev(t[i], tck, der=2) c = interpolate.splev(t[i], tck, der=1) d = interpolate.splev(t[i], tck, der=0) polylist.append(Trajectory.Polynomial([d, c, b, a])) chunkslist.append(Trajectory.Chunk(t[i + 1] - t[i], polylist)) return Trajectory.PiecewisePolynomialTrajectory(chunkslist)
def trajectoryFromArray(obj): stpoints = [] obj = sorted(obj, key=lambda coord: coord[2]) for x, y, t in obj: point = stpoint(x, y, t) stpoints.append(point) segments = [] # skip last point because can't make segment for i in range(0, len(stpoints) - 1): seg = stsegment(stpoints[i], stpoints[i + 1]) segments.append(seg) return Trajectory(segments)
def putOnTrajectory(self): """Load a trajectory in buffer to go to the closest point on routine trajectory. """ if self.routineTrajectory.length() > 0: #Find the closest point on trajectory tcpLocation, tcpAngle = self.getTcpPlacement() """ print(type(tcpLocation)) print(tcpLocation) print(type(tcpAngle)) print(tcpAngle) """ vectorId, minDist, closestPt = self.routineTrajectory.distLocation( tcpLocation) print("Travel to a point of routine trajectory on vector N {}". format(vectorId)) """ print("Closest point on trajectory: ",closestPt) print("Distance from trajectory: ",minDist) print("Closest vector ID :",vectorId) """ angle = self.routineTrajectory.angleArray[vectorId] #Transition trajectory transTraj = Trajectory() transTraj.addPlacement(tcpLocation, tcpAngle, None) transTraj.addPlacement(closestPt, angle, None) print(transTraj.actionList) #A verifier endLocation = self.routineTrajectory.locationArray[vectorId + 1] endAngle = self.routineTrajectory.angleArray[vectorId + 1] def endAction(rob=self, vectorId=vectorId): """Load routine trajectory in trajectory buffer """ print("Tcp is now back on routine trajectory") rob.bufferTrajectory = rob.routineTrajectory rob.bufferVectorId = vectorId transTraj.addPlacement(endLocation, endAngle, endAction) self.bufferTrajectory = transTraj self.onTrajectory = True #vectorId is -1 because the robot is starting first vector of buffer trajectory self.bufferVectorId = -1
def run(int_inclination, ramp_length, time_step, sim_time): sample_file = 'V13_CFD.txt' init_file = 'V13_data.dot' path = 'V13/' rocket = Rocket.from_file_with_AoAspeed(init_file, sample_file, path) initialInclination = int_inclination / 180.0 * np.pi (t, position, euler, linearVelocity, angularVelocity, AoA, thrust, gravity, drag, lift) \ = Trajectory.calculateTrajectory(rocket, int_inclination, ramp_length, time_step, sim_time) xs, ys, zs = position.T[0], position.T[1], position.T[2] hs = -zs rs = [np.sqrt(xs[i]**2 + ys[i]**2) for i in range(len(xs))] return hs, rs
def __init__(self, beatMap, parent=None): super().__init__(parent) self.timer = QTimer(self) self.timer.setTimerType(Qt.PreciseTimer) self.timer.setInterval(1) self.timer.timeout.connect(self.checkOutput) self.beatMap = beatMap self.iNextPoint = 0 self.iNextHit = 0 self.beginTime = None self.tc = Trajectory.Converter() self.ht = Hit.Converter() self.timeList, self.pointList = self.tc(beatMap) self.eventList = self.ht(beatMap, self.timeList, self.pointList) self.started = False
def LoadFile(self): self.UpdateData(1) self.UpdateData(2) if self.fileName == None: self.valueStatus.setText("Error: file is not found") else: self.count = 0 self.RB.listPoints = np.array([[0,0,0]]) self.RB.color=np.array([0]) self.AllPoints = np.array([[None, None, None]]) self.AllJVars = np.array([[None, None, None, None]]) self.toolstatus = np.array([None]) self.valueStatus.setText("Processing...") self.processLoadFile.setValue(30) listPoint = LoadGCode(self.fileName, self.objRB.EVars[0], self.objRB.EVars[1], self.objRB.EVars[2]) self.RB.updateGL() trj = Trajectory() listPoint = np.insert(listPoint, 0, [self.objRB.EVars[0], self.objRB.EVars[1], self.objRB.EVars[2], 0], axis = 0) listPoint = np.append(listPoint, [[self.objRB.EVars[0], self.objRB.EVars[1], self.objRB.EVars[2], 0]], axis = 0) toolstt_tmp = np.array([None]) trj.SetSpTime(0.1) for i in np.arange(len(listPoint)-1): p1 = listPoint[i][:3] p2 = listPoint[i+1][:3] if i==0 or i == len(listPoint)-2: trj.SetPoint(p1,p2,100) else: trj.SetPoint(p1, p2, 100) points = trj.Calculate() if points[0] == False: pass else: self.AllPoints = np.append(self.AllPoints, points[1], axis = 0) self.toolstatus = np.append(self.toolstatus, [listPoint[i+1][3]]*len(points[1])) self.toolstatus = np.delete(self.toolstatus, 0) self.AllPoints = np.delete(self.AllPoints, 0, axis = 0) toolstt_tmp = np.delete(toolstt_tmp, 0) q1P = self.objRB.q1P q2P = self.objRB.q2P for p in self.AllPoints: EVars = np.append(p, [self.objRB.EVars[3], self.objRB.EVars[4], self.objRB.EVars[5]]) JVar = self.objRB.CalInvPositionEx(EVars, q1P, q2P) if JVar[0] == False: break self.AllJVars = np.append(self.AllJVars, [JVar[1]], axis = 0) q2P = q1P q1P = JVar[1] self.AllJVars = np.delete(self.AllJVars, 0, axis = 0) self.processLoadFile.setValue(100) if len(self.AllJVars) == len(self.AllPoints): self.valueStatus.setText("All done") else: self.valueStatus.setText("Some points is missing")
def get_update(self, i): start = time.time() self.t += self.t_step # Desired Trajectory [From Input or Trajectory] and State [From Dynamics] self.State_d = TR.trajectory(self.t, self.State_d) # Calculate Actions and Forces from Controller self.PhiC_last, Actions = C2D.control(self.PhiC_last, self.State, self.State_d, self.Gains, self.Params, self.t_step) # Use Dynamics to Simulate New State [Transform Axes, Estimate Derivatives self.State = DYN.dynamics(self.State, self.Params, Actions, self.t_step) ## Calculation time for above measured at around 0.5 milliseconds on average end = time.time() print("Simulation time") print(end - start) # Update Plot self.t_vec.append(self.t) self.p_vec.append(self.State[0]) self.p_des_vec.append(self.State_d[0]) start = time.time() self.line.set_data(self.t_vec, self.p_vec) #self.fig.canvas.draw() self.fig.canvas.blit(self.ax.bbox) end = time.time() print("Plotting time") print(end - start) if self.t > 10: self.terminate() return self.line,
def __init__(self, name, mass=1., coords=[0., 0.], vel=[0., 0.], isTest=False, color='red'): self.mass = mass self.pos = np.array(coords) self.vel = np.array(vel) self.force = np.zeros(2) self.isTest = isTest self.visual_radius = 2000 * mass**(1. / 3) #wip self.color = color self.name = name self.traj = Trajectory(dimension=2, color=self.color)
def __init__(self, ctrlOwner, tcpPBone, toolPBone=None, speed=0.1): """Create a robotArm object. Parameters ---------- tcpPBone: Pose bone used as inverse kinematic target speed: Robot translation speed in dist unit per frame """ #Base referential self.refLocation = None self.refAngle = None #ControlerOwner (use to access armature pose bone) self.ctrlOwner = ctrlOwner #Bone where is attached the tool self.toolPBone = toolPBone #Tool center point pose bone self.tcpPBone = tcpPBone #print("Target location",tcpPBone.location) #Tools #Dictionnary of tools mounted ont he robot self.tools = {} #Robotic arm base position in global referential self.restTcpPosition = tcpPBone.location #Robotic arm base orientation in global referential self.restTcpRotation = tcpPBone.joint_rotation #displacmeent speed (in dist unit /frame) self.speed = 0.1 #Routine trajectory self.routineTrajectory = Trajectory() #Non interpolated trajectory to be executed by the robot self.bufferTrajectory = Trajectory() #Interpolated trajectory for the on going routine trajectory vector or vector #If None the robot freeze self.bufferInterTrajectory = Trajectory() #Id of the interpolated vector self.bufferVectorId = -1 #On trajectory flag self.onTrajectory = False #Progress on routine trajectory self.onRoutine = False #Dictionnary which can be used by skills algorithm self.customDic = {} #Skills self.skillDic = {}
def __init__(self, ensemble, **kw): self.title = "MD Movie: %s" % ensemble.name self.ensemble = ensemble self.model = Trajectory.Ensemble(self.ensemble) self.model.CreateMolecule() endFrame = ensemble.endFrame if endFrame == "pipe": fn = 1 while True: replyobj.status("Loading trajectory from pipe:" " frame %d\n" % fn) try: self.model.LoadFrame(fn, makeCurrent=False) except NoFrameError: break fn += 1 replyobj.status("Loading trajectory from pipe: done\n") endFrame = fn - 1 if ensemble.startFrame == None: self.startFrame = 1 else: self.startFrame = ensemble.startFrame if self.startFrame > len(ensemble): replyobj.warning("Start frame > number of" " trajectory frames; changing start" " frame to 1\n") self.startFrame = 1 if endFrame == None: self.endFrame = len(ensemble) else: self.endFrame = endFrame if self.endFrame > len(ensemble): replyobj.warning("End frame > number of" " trajectory frames; changing end" " frame to last frame\n") self.endFrame = len(ensemble) self.molTrigID = chimera.triggers.addHandler("Molecule", self._molTrigCB, None) self.openOpts = kw self._inTriggerCB = False ModelessDialog.__init__(self) del self.openOpts chimera.extension.manager.registerInstance(self)
def tel(): n = 250 r = 0.5 x0 = -0.25 y0 = 1 # star = Trajectory.Star2D(n,5,r,x0,y0) circle = Trajectory.Circle2D(n, r, x0, y0, arclength=2 * np.pi) # nPoly = Trajectory.nPoly2D(n,4,r,x0,y0) # spiral = Trajectory.Spiral2D(n,0.6,0.1,x0,y0,6*np.pi) x_tel = circle.xtest pub = rospy.Publisher('teleop', numpy_msg(Floats), queue_size=10) pub2 = rospy.Publisher('point', Point, queue_size=10) R = 10 # R = rospy.get_param('~pub_rate') rospy.init_node('Teleop_node') rate = rospy.Rate(R) i = 0 N = len(x_tel) while not rospy.is_shutdown(): if i > N - 1: i = 0 # x = np.array(x_tel[i],dtype = np.float64) # x = np.array([float(x_tel[i,0]),float(x_tel[i,1]),float(0)]) # x.astype(np.float64) # x = np.array([trans[0],trans[1]], dtype=numpy.float32) # pub.publish(x) newpoint = Point() newpoint.x = x_tel[i, 0] newpoint.y = x_tel[i, 1] newpoint.z = float(i) pub2.publish(newpoint) # rospy.loginfo(newpoint) i += 1 rate.sleep()
def exec_traj(p_sphere_pos,v_sphere_pos,params): global thread_data global thread_lock robot=params['robot'] #Reach the current position of p_sphere HRP4.SetConfig(robot,thread_data['initial_q']) config=Reach(params['linkindex'],params['linkindex2'],p_sphere_pos,params['n_steps'],params) HRP4.SetConfig(robot,config) #If Move_p, update the position of the v_sphere if thread_data['state']=='Move_p': Trans=eye(4) Trans[0:3,3]=p_sphere_pos+thread_data['cur_vit']*params['dist'] thread_data['v_sphere'].SetTransform(Trans) #Else, i.e. Move_v, update the current v else: thread_data['cur_vit']=(v_sphere_pos-p_sphere_pos)/params['dist'] #COmpute the deformed trajectory and move robot 2 traj=params['traj'] tau=params['tau'] T=params['T'] q_T=thread_data['initial_q'] q_Td=config qd_T=thread_data['initial_qd'] J=compute_Jacobian_speed(params) qd_Td=qd_T+dot(linalg.pinv(J),thread_data['cur_vit']-dot(J,qd_T)) if params['with_velocity']: traj_def=Affine.deform_traj(traj,tau,T,q_Td,params['active_dofs'],qd_T,qd_Td) else: traj_def=Affine.deform_traj(traj,tau,T,q_Td,params['active_dofs']) for k in range(traj_def.n_steps): traj_def.q_vect[[0,1,2],k]+=params['offset'] robot2=params['robot2'] Trajectory.Execute(robot2,traj_def,0.001) thread_data['traj_def']=traj_def
def deform_traj(traj, tau, T, q_Td, active_dofs, v_T=None, v_Td=None): q_red = traj.q_vect[active_dofs, :] v_tau_red = q_red[:, tau + 1] - q_red[:, tau] q_Td_red = q_Td[active_dofs] if v_T != None: M = compute_mat_c1(v_tau_red, q_red[:, tau], q_red[:, T], q_Td_red, v_T[active_dofs], v_Td[active_dofs]) else: M = compute_mat_c1(v_tau_red, q_red[:, tau], q_red[:, T], q_Td_red) q_fork_red = fork(q_red, tau, M) q_vect2 = zeros(traj.q_vect.shape) for i in range(traj.dim): if not (i in active_dofs): q_vect2[i, :] = traj.q_vect[i, :] for i in range(len(active_dofs)): q_vect2[active_dofs[i], :] = q_fork_red[i, :] res_traj = Trajectory.SampleTrajectory(q_vect2) res_traj.t_vect = traj.t_vect return res_traj
def run(gl): #Read reactant definition if gl.StartType == 'file': Reac = read(gl.Start) elif gl.StartType == 'Smile': Reac = tl.getMolFromSmile(gl.Start) # Set up calculator if gl.trajMethod == "openMM": Reac = tl.setCalc(Reac, "DOS/", gl.trajMethod, gl) else: Reac = tl.setCalc(Reac, "DOS/", gl.trajMethod, gl.trajLevel) # Do we want to minimise the reactant if gl.GenBXDrelax: min = BFGS(Reac) try: min.run(fmax=0.1, steps=150) except: min.run(fmax=0.1, steps=50) t = Trajectory.Trajectory(Reac, gl, os.getcwd(), 0, False) t.runBXDEconvergence(gl.maxHits, gl.maxAdapSteps, gl.eneAdaptive, gl.decorrelationSteps, gl.histogramLevel, gl.runsThrough, gl.numberOfBoxes, gl.grainSize)
nativeclist = eval(fnative.readline()) fnative.close() if VERBOSE: print "NATIVE CLIST:", nativeclist # Make a list of config.NREPLICAS Replica objects replicas = [] # a list of Replica objects for i in range(0, config.NREPLICAS): replicas.append(Replica(config, i)) # Each replica is just a container for the objects: # replica[i].chain <-- the HP chain # replica[i].mc <-- A Monte Carlo move set object that performs operations on the chain # a trajectory object to handle the work of writing trajectories traj = Trajectory(replicas, config) if VERBOSE: print "Trajectory REPFILES:", traj.repfiles_trj # Keep track of statistics for the replica exchange simulation, for each replica: ### Monte Carlo stats steps = [] # number of total move steps attempted (viable or not) viablesteps = [] # number of viable steps acceptedsteps = [] # number of viable steps that were accepted under Metropolis criterion moveacceptance = [] # fraction of acceptance = [] # fraction of MC moves accepted ### Replica exchange stats swaps = [] # the number of attemped replica swaps viableswaps = [] # the number of viable swaps
if __name__ == '__main__': if len(sys.argv) < 2: print usage sys.exit(1) configfile = sys.argv[1] config = Config( filename=configfile) if VERBOSE: config.print_config() # create a single Replica replicas = [ Replica(config,0) ] traj = Trajectory(replicas,config) # a trajectory object to write out trajectories nconfs = 0 contact_states = {} # dictionary of {repr{contact state}: number of conformations} contacts = {} # dictionary of {number of contacts: number of conformations} microstates = [] # a list of microstate vec lists, in order microContactStates = [] # a list of contact states, in order, for each microstate microindex = {} # dictionary of {"011121321": microstate index} lookup table ################# # # This is a useful subroutine for enumerating all conformations of an HP chain
max_dist = float(sys.argv[2]) min_density = float(sys.argv[3]) max_angle = float(sys.argv[4]) segment_size = float(sys.argv[5]) # For each line, get candidate lines from angle dictionary, find within distance traj_angles = defaultdict(list) #look up angles fast, each angle (in degrees) will have a list of Trajectory objects trajectories = [] # list of trajectories, keep it? fh = open (infile, 'r') segment_list_oh = open(infile + "." + str(max_dist) + "." + str(min_density) + "." + str(max_angle) + "." + str(segment_size) + "_seg_list.csv", "w") corridor_list_oh = open (infile + "." + str(max_dist) + "." + str(min_density) + "." + str(max_angle) + "." + str(segment_size) + "_cor_list.csv", "w") corridor_list_oh.write("cor_id\tcor_weight\tcor_coordinates\n"); segment_list_oh.write("dl_id\tdl_weight\tdl_angle\tcor_id\tseg_coordinates\n"); for line in fh: linelist = line.split(); traj = Trajectory(name=linelist[0], weight = float(linelist[1]), startx=float(linelist[2]), starty=float(linelist[3]), endx=float(linelist[4]), endy=float(linelist[5])) traj.make_segments(segment_size) rounded_angle = round_to(traj.angle, 0.5); traj_angles[rounded_angle].append(traj) #add the trajectory to a list in the dictionary of angles trajectories.append(traj) for line in trajectories: for segment in line.segments: (sumweight, segments ) = DBScan(segment, traj_angles, max_dist, min_density, max_angle) if sumweight >= min_density: add_cluster(segment, segments, int(sumweight*100), sum_pairwise(segments))
V=env.GetViewer() M=array([[ -5.61713e-02, 1.80862e-01, -9.81903e-01, 2.77983e+00], [ 9.98354e-01, -1.21427e-03, -5.73361e-02, 9.60523e-02], [ -1.15622e-02, -9.83508e-01, -1.80496e-01, 1.35762e+00], [ 0.00000e+00, 0.00000e+00, 0.00000e+00, 1.00000e+00]]) V.SetCamera(M) n_snaps=11 box=[250,0,530,480] ct=traj ni=0 for i in [int(round(k)) for k in linspace(0,ct.n_steps-1,n_snaps)]: with robot: robot.GetLinks()[0].SetTransform(Trajectory.v2t(ct.q_vect[0:3,i])) robot.SetDOFValues(ct.q_vect[3:ct.dim,i]) I=V.GetCameraImage(640,480,M,[640,640,320,240]) scipy.misc.imsave('tmp.jpg',I) im=Image.open('tmp.jpg') im2=im.crop(box) im2.save('../../Reda/mintime/fig/hrp4-'+str(ni)+'.eps') ni+=1
def test_segmentation(self): #horizontal line: hor = Trajectory(startx=200,starty=300.0,endx=300.0,endy=300.0) self.assertEqual(hor.angle, 0) neg_hor = Trajectory(startx=200,starty=300.0,endx=100.0,endy=300.0) self.assertEqual(abs(neg_hor.angle), 180) ver = Trajectory(startx=200,starty=300.0,endx=200.0,endy=400.0) self.assertEqual(ver.angle, 90) neg_ver = Trajectory(startx=200,starty=300.0,endx=200.0,endy=200.0) self.assertEqual(neg_ver.angle, -90) ang20 = Trajectory(startx=200,starty=300,endx=293.969262092233, endy = 334.202014295086) self.assertAlmostEqual(ang20.angle,20, 7) ang_minus20 = Trajectory(startx=200,starty=300,endx=293.969262092233, endy = 265.797985704914) self.assertAlmostEqual(ang_minus20.angle,-20, 7) hor.make_segments(segment_length=12) neg_hor.make_segments(segment_length=12) ver.make_segments(segment_length=12) neg_ver.make_segments(segment_length=12) ang20.make_segments(segment_length=12) ang_minus20.make_segments(segment_length=12) self.assertEqual(len(hor.segments),9) self.assertEqual(len(neg_hor.segments),9) self.assertEqual(len(ver.segments),9) self.assertEqual(len(neg_ver.segments),9) self.assertEqual(len(ang20.segments),9) self.assertEqual(len(ang_minus20.segments),9) for i in range(9): self.assertAlmostEqual(hor.segments[i].startx, 200+cos(radians(0))*i*12.) self.assertAlmostEqual(hor.segments[i].starty, 300+sin(radians(0))*i*12.) self.assertAlmostEqual(neg_hor.segments[i].startx, 200+cos(radians(180))*i*12.) self.assertAlmostEqual(neg_hor.segments[i].starty, 300+sin(radians(180))*i*12.) self.assertAlmostEqual(ver.segments[i].startx, 200+cos(radians(90))*i*12.) self.assertAlmostEqual(ver.segments[i].starty, 300+sin(radians(90))*i*12.) self.assertAlmostEqual(neg_ver.segments[i].startx, 200+cos(radians(-90))*i*12.) self.assertAlmostEqual(neg_ver.segments[i].starty, 300+sin(radians(-90))*i*12.) self.assertAlmostEqual(ang20.segments[i].startx, 200+cos(radians(20))*i*12.) self.assertAlmostEqual(ang20.segments[i].starty, 300+sin(radians(20))*i*12.) self.assertAlmostEqual(ang_minus20.segments[i].startx, 200+cos(radians(-20))*i*12.) self.assertAlmostEqual(ang_minus20.segments[i].starty, 300+sin(radians(-20))*i*12.)
VERBOSE = 1 # make an MDTraj trajectory to store the coordinate frames as PDBs t = md.load('template.pdb') # note the first frame is junk if __name__ == '__main__': configfile = sys.argv[1] config = Config( filename=configfile) if VERBOSE: config.print_config() # create a single Replica replicas = [ Replica(config,0) ] traj = Trajectory(replicas,config) # a trajectory object to write out trajectories nconfs = 0 contact_states = {} # dictionary of {repr{contact state}: number of conformations} contacts = {} # dictionary of {number of contacts: number of conformations} contact_state_keys = [] # a list of unique contact states, e.g. elements "[(0,9),(0,11)]", whose order defines the # contact state indices assignments = [] # a list of contact state indices for every frame in the trajectory ################# # # This is a useful subroutine for enumerating all conformations of an HP chain # # NOTE: in order for this to work correctly, the initial starting vector must be [0,0,0,....,0]