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]
