def plot(pose, ball_pos = None, png=False, frame=0, phase=0, bezier=None, mass=None, gplot=False):
"""Plottet den Roboter."""
robot = Robot()
robot.update(pose)
inverse = robot.get_l_foot_endpoint().get_chain_matrix(inverse=True)
ep = np.empty(4)
with open("plot/pos%i_%i.gp" % (phase,frame), "w") as plotfile:
for joint in robot.get_l_leg_chain():
ep[:3] = joint.get_endpoint()
ep[3] = 1
p = np.dot(inverse, ep)
plotfile.write("%f %f %f\n" % tuple(p[:-1]))
p = np.dot(robot.get_l_foot_endpoint().get_chain_matrix(inverse=True), np.dot(robot.get_l_foot_endpoint().get_chain_matrix(), v.trans3d(z=0.052)))
p = np.dot(p, v.orig)
plotfile.write("%f %f %f\n" % tuple(p[:-1]))
plotfile.write("\n")
for joint in robot.get_r_leg_chain():
ep[:3] = joint.get_endpoint()
ep[3] = 1
p = np.dot(inverse, ep)
plotfile.write("%f %f %f\n" % tuple(p[:-1]))
p = np.dot(robot.get_l_foot_endpoint().get_chain_matrix(inverse=True), np.dot(robot.get_r_foot_endpoint().get_chain_matrix(), v.trans3d(z=0.052)))
p = np.dot(p, v.orig)
plotfile.write("%f %f %f\n" % tuple(p[:-1]))
plotfile.write("\n")
for joint in robot.get_l_arm_chain():
ep[:3] = joint.get_endpoint()
ep[3] = 1
p = np.dot(inverse, ep)
plotfile.write("%f %f %f\n" % tuple(p[:-1]))
plotfile.write("\n")
for joint in robot.get_r_arm_chain():
ep[:3] = joint.get_endpoint()
ep[3] = 1
p = np.dot(inverse, ep)
plotfile.write("%f %f %f\n" % tuple(p[:-1]))
plotfile.write("\n")
plotfile.write("%f %f %f\n" % tuple((0.044,0,0.052)))
plotfile.write("%f %f %f\n" % tuple((-0.022,0,0.052)))
plotfile.write("%f %f %f\n" % tuple((-0.022,0,-0.052)))
plotfile.write("%f %f %f\n" % tuple((0.044,0,-0.052)))
plotfile.write("%f %f %f\n" % tuple((0.044,0,0.052)))
plotfile.write("\n")
if ball_pos is not None:
with open("plot/ball.gp", "w") as plotfile:
plotfile.write("%f %f %f\n" % tuple(ball_pos))
else:
open("plot/ball.gp","w") # remove old data
if mass is not None:
with open("plot/mass%i_%i.gp" % (phase, frame), "w") as plotfile:
ep[:3] = mass
ep[3] = 1
mass = np.dot(inverse, ep)
plotfile.write("%f %f %f\n" % (mass[0], mass[1], mass[2]))
plotfile.write("%f %f %f\n" % (mass[0], 0, mass[2]))
else:
open("plot/mass%i_%i.gp" % (phase, frame),"w") # remove old data
if bezier is not None:
bezier.visualize(100, only_data=True, phase=phase)
else:
open("plot/bezier_curve%i.gp" % phase,"w") # remove old data
if gplot:
g = Gnuplot.Gnuplot(debug=1)
if png:
g('set terminal png')
g('set output "plot/output/kick%i_%i.png"' % (phase,frame))
#g('set view 90,90')
else:
g('set terminal wxt persist')
g('set nokey')
g('set xrange [%.3f:%.3f]' % (-.3, .2))
g('set yrange [%.3f:%.3f]' % (-.1, .5))
g('set zrange [%.3f:%.3f]' % (-.2, .2))
g('set ytics 50')
g('set ztics 100')
g('set size square')
g('splot "plot/pos%i_%i.gp" every :::0::0 with lines, "plot/pos%i_%i.gp" every :::1::1 with lines, "plot/pos%i_%i.gp" every :::2::2 with lines, "plot/pos%i_%i.gp" every :::3::3 with lines, "plot/pos%i_%i.gp" every :::4::4 with lines, "plot/ball.gp", "plot/bezier_curve%i.gp" with lines, "plot/mass%i_%i.gp"' % (phase, frame, phase, frame, phase, frame, phase, frame, phase, frame, phase, phase, frame))
class KinematicDurchMergeWiederHierZumVergleichenDerImplementationen(object):
def __init__(self):
"""Konstruktor"""
self.ipc = SharedMemoryIPC() # IPC zum kommunizieren mit der Hardware
self.robot = Robot() # Modell des Roboters zum Bestimmen der Gliedpositionen
self.robot.update(self.ipc.get_pose())
self.phases = [] # Phasen aus denen die Bewegung zusammengesetzt ist
self.prev_dir = None # 3ter Bezier-Stuetzpunkt der vorherigen Phase
def kick(self, ball_pos, kick_angle, foot='right'):
"""Laesst den Roboter eine einfache Schussbewegung ausfuehren. Diese Bewegung besteht aus einer Phase.
Die dazugehoerige Bezierkurve beginnt bei der momentanen Position des Schussfusses und endet an der Ballposition.
Durch den Winkel ist es moeglich, die Richtung zu manipulieren, aus der sich der Fuss zur Ballposition bewegt.
Optional ist die angabe des Schussfusses. Im Normalfall wird der rechte Fuss genutzt. Die Koordinaten der Ballposition
muessen zudem relativ zum Standfuss angegeben werden."""
# Initialisiere das Modell mit der realen Roboterpose
self.robot.update(self.ipc.get_pose())
# Verschiebe das Roboterkoordinatensystem, so dass der Standfuss dem Nullpunkt entspricht
# und bestimme die Position des Schussfusses
if foot is 'right':
d = np.dot(self.robot.get_l_foot_endpoint().get_chain_matrix(inverse=True), np.dot(self.robot.get_r_foot_endpoint().get_chain_matrix(), v.trans3d(z=0.052)))
else:
d = np.dot(self.robot.get_r_foot_endpoint().get_chain_matrix(inverse=True), np.dot(self.robot.get_l_foot_endpoint().get_chain_matrix(), v.trans3d(z=0.052)))
pos = np.dot(d, v.orig)[:3]
# Bestimme 3ten Bezier-Stuetzpunkt
direction = ball_pos - np.dot(v.pitch(math.radians(kick_angle))[:-1,:-1], (ball_pos - pos) / 3)
# Bestimme Masse- und Distanzfunktionen, je nach Standfuss
dist_func = kick_r_foot_distance if foot is 'right' else kick_l_foot_distance
mass_func = kick_r_foot_cog if foot is 'right' else kick_l_foot_cog
mass_dist_func = kick_r_foot_cog_distance if foot is 'right' else kick_l_foot_cog_distance
val_func = kick_r_foot_validate if foot is 'right' else kick_l_foot_validate
self.add_phase(dist_func, mass_func, mass_dist_func, val_func, pos, ball_pos, direction)
def add_phase(self, dist_func, mass_func, mass_dist_func, val_func, start, target, direction=None):
"""Fuegt der Bewegung eine weitere Phase hinzu. Dafuer muessen Start- und Zielposition uebergeben werden,
sowie optional eine Position, aus deren Richtung sich der Zielposition genaehert werden soll.
Guetefunktionen fuer Distanz und Masse muessen angegeben werden, mit deren Hilfe der Abstand des Schussfusses zum Zielpunkt und
der Abstand des Masseschwerpunktes zum Zentrum des Stabilitaetsgebiet bewertet werden kann"""
self.phases.append(Phase(self.robot, dist_func, mass_func, mass_dist_func, val_func, start, target, self.prev_dir, direction, no_cog=True))
self.prev_dir = self.phases[-1].direction
def execute(self):
"""Fuehrt alle vorhandene Phasen der Reihe nach aus."""
self.phasenum = 0 # Phase-Nummer zu Debug-Zwecken
while len(self.phases) > 0:
self.executePhase(self.phases[0])
# Debug Ausgabe
iters = sum(self.phases[0].iter)/float(len(self.phases[0].iter))
with open('plot/iters', 'a') as f:
f.write("phase complete - average iteration: %.2f\n" % iters)
print "phase complete - average iteration: %.2f" % iters
self.phasenum += 1
self.phases = self.phases[1:]
def executePhase(self, phase, t_total=7.0, delta=1.0):
"""Führt eine Phase aus."""
self.frame = 0 # Frame-Nummer zu Debug-Zwecken
# Bewege Roboter zur Initialposition
t = 0
cur_pose = self.ipc.get_pose()
next_pose = phase.calc_angles(cur_pose, t)
self.update_pose(cur_pose, next_pose, delta)
start = time.time()
while t < t_total:
# Positionen auf der Bezierkurve abarbeiten
t+= delta
# Werte für die nächste Winkelangabe bestimmen
next_pose = phase.calc_angles(next_pose, t/t_total)
# Warten, wenn momentane Bewegung noch ausgefuehrt wird
sleep = delta - (time.time() - start)
if sleep > 0:
time.sleep(sleep)
start = time.time()
# Winkel setzen
cur_pose = self.ipc.get_pose()
self.update_pose(cur_pose, next_pose, delta)
def update_pose(self, cur_pose, new_pose, delta):
"""Uebergibt dem Roboter eine Pose. Passt dabei
die Winkelgeschwindigkeit der Zeit an, die fuer die Bewegung
zur Verfuegung steht."""
for name, joint in new_pose:
# Winkelgeschwindigkeit ergibt sich aus der Zeit und der
# Differenz zwischen momentanem Winkel und Zielwinkel
joint.speed = abs(cur_pose[name].position - joint.goal) / delta
# Pose dem Roboter uebergeben
self.ipc.update(new_pose)
# Debug Ausgabe
plot(new_pose,png=True,frame=self.frame,phase=self.phasenum, bezier=self.phases[0].bezier, mass=self.robot.get_center_of_gravity(), gplot=False)
self.frame += 1