def test_robot_is_created_from_config(self):
try:
robot=Robot()
larm = robot.get_l_arm_chain()
self.assertTrue(larm is not None)
except Exception, e:
self.assertEquals(e, None)
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))
def test_Darwin_config(self):
print "### Darwin ###"
config = get_config()
config["RobotTypeName"] = "Darwin"
robot = Robot()
larm = robot.get_l_arm_chain()
def test_GOAL_config(self):
print "### Goal ###"
config = get_config()
config["RobotTypeName"] = "Hambot"
robot = Robot()
larm = robot.get_l_arm_chain()
