2008/05/09

Robot2 - simple humanoid

11軸の単純な人間型ロボットを作ってみました。胴体を中心とした全体回転1軸、頭2軸、手足各2軸x4本です。これも"OpenGL入門" エドワード・エンジェル著、滝沢徹・牧野祐子訳、ピアソン・エデュケーション刊を参考にして作ってみました。軸は多くなりましたが、glPush/PopMatrixが追加されただけで、原理は昨日のサンプルと全く同じです。Pythonはインタープリタでありデバッグが簡単なので、これくらいのコードなら数時間で動かせます。とても手軽です。キーボードで操作できます。大文字は逆方向に動きます。
e/E r/R (頭)
a/A s/S d/D f/F g/G (腕、胴体)
z/Z x/X c/C v/V (足)

Simple humanoid type robot with 11 moving axis, 1 for entire body, 2 for head, 2 for each leg, by refering a book "OpenGL, A PRIMER" by Edward Angel. glPush/PopMatrix is added to handle this complex structure. Python is pretty powerfull to debug my source code because it's an interpreter. You can operate the humanoid by using keyboard.

e/E r/R (head)
a/A s/S d/D f/F g/G (arms, entire body)
z/Z x/X c/C v/V (legs)


import sys
import math
from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *

TORSO_RADIUS=0.1
TORSO_HEIGHT=0.4

UPPER_ARM_HEIGHT=0.2
UPPER_ARM_WIDTH=0.07

LOWER_ARM_HEIGHT=0.2
LOWER_ARM_WIDTH=0.05

UPPER_LEG_HEIGHT=0.2
UPPER_LEG_WIDTH=0.08

LOWER_LEG_HEIGHT=0.2
LOWER_LEG_WIDTH=0.06

SHOLDER_WIDTH = 0.2
HIP_WIDTH = 0.2

HEADX=0.1
HEADY=TORSO_HEIGHT

LUAX=-1.0 * SHOLDER_WIDTH / 2
RUAX=SHOLDER_WIDTH / 2
LUAY=RUAY=TORSO_HEIGHT
LLAY=RLAY=LOWER_ARM_HEIGHT

LULX=-1.0 * HIP_WIDTH / 2
RULX=HIP_WIDTH / 2
LULY=RULY=0
LLLY=RLLY=LOWER_LEG_HEIGHT

t0=0.0
t1=0.0
t2=0.0
t3=90.0
t4=0.0
t5=90.0
t6=0.0
t7=180.0
t8=0.0
t9=180.0
t10=0.0

def display():
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glMatrixMode(GL_MODELVIEW)

#TORSO
glLoadIdentity()
gluLookAt(2.0, 2.0, 2.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0)
gray()
glRotatef(t0, 0.0, 1.0, 0.0)
torso()
glPushMatrix()

#HEAD
cyan()
glTranslatef(0.0, HEADX, 0.0)
glRotatef(t1, 1.0, 0.0, 0.0)
glRotatef(t2, 0.0, 1.0, 0.0)
glTranslatef(0.0, HEADY, 0.0)
head()

#LEFT UPPER ARM
glPopMatrix()
glPushMatrix()
red()
glTranslatef(LUAX, LUAY, 0.0)
glRotatef(t3, 1.0, 0.0, 0.0)
upper_arm()

#LEFT LOWER ARM
green()
glTranslatef(0.0, LLAY, 0.0)
glRotatef(t4, 1.0, 0.0, 0.0)
lower_arm()

#RIGHT UPPER ARM
glPopMatrix()
glPushMatrix()
blue()
glTranslatef(RUAX, RUAY, 0.0)
glRotatef(t5, 1.0, 0.0, 0.0)
upper_arm()

#RIGHT UPPER ARM
cyan()
glTranslatef(0.0, RLAY, 0.0)
glRotatef(t6, 1.0, 0.0, 0.0)
lower_arm()

#LEFT UPPER LEG
glPopMatrix()
glPushMatrix()
magenta()
glTranslatef(LULX, LULY, 0.0)
glRotatef(t7, 1.0, 0.0, 0.0)
upper_leg()

#LEFT LOWER LEG
yellow()
glTranslatef(0.0, LLLY, 0.0)
glRotatef(t8, 1.0, 0.0, 0.0)
lower_leg()

#RIGHT UPPER LEG
glPopMatrix()
glPushMatrix()
pink()
glTranslatef(RULX, RULY, 0.0)
glRotatef(t9, 1.0, 0.0, 0.0)
upper_leg()

#RIGHT LOWER LEG
gray()
glTranslatef(0.0, RLLY, 0.0)
glRotatef(t10, 1.0, 0.0, 0.0)
lower_leg()

glPopMatrix()
glFlush()

def torso():
glPushMatrix()
glRotatef(-90.0, 1.0, 0.0, 0.0)
gluCylinder(p, TORSO_RADIUS, TORSO_RADIUS, TORSO_HEIGHT, 10, 10)
glPopMatrix()

def head():
glPushMatrix()
glRotatef(-90.0, 1.0, 0.0, 0.0)
glutWireSphere(0.1, 10, 10)
glPopMatrix()

def upper_arm():
glPushMatrix()
glTranslatef(0.0, 0.5*UPPER_ARM_HEIGHT, 0.0)
glScalef(UPPER_ARM_WIDTH, UPPER_ARM_HEIGHT, UPPER_ARM_WIDTH)
glutSolidCube(1.0)
glPopMatrix()

def lower_arm():
glPushMatrix()
glTranslatef(0.0, 0.5*UPPER_ARM_HEIGHT, 0.0)
glScalef(LOWER_ARM_WIDTH, LOWER_ARM_HEIGHT, LOWER_ARM_WIDTH)
glutSolidCube(1.0)
glPopMatrix()

def upper_leg():
glPushMatrix()
glTranslatef(0.0, 0.5*LOWER_ARM_HEIGHT, 0.0)
glScalef(UPPER_LEG_WIDTH, UPPER_LEG_HEIGHT, UPPER_LEG_WIDTH)
glutSolidCube(1.0)
glPopMatrix()

def lower_leg():
glPushMatrix()
glTranslatef(0.0, 0.5*LOWER_ARM_HEIGHT, 0.0)
glScalef(LOWER_LEG_WIDTH, LOWER_LEG_HEIGHT, LOWER_LEG_WIDTH)
glutSolidCube(1.0)
glPopMatrix()

def red():
glColor3f(1.0, 0.0, 0.0)
def green():
glColor3f(0.0, 1.0, 0.0)
def blue():
glColor3f(0.0, 0.0, 1.0)
def cyan():
glColor3f(0.0, 1.0, 1.0)
def magenta():
glColor3f(1.0, 1.0, 0.0)
def yellow():
glColor3f(1.0, 0.0, 1.0)
def pink():
glColor3f(1.0, 0.5, 0.5)
def gray():
glColor3f(0.7, 0.7, 0.7)

def mykey(key, x, y):
global t0, t1, t2, t3, t4, t5, t6, t7, t8, t9, t10
if key=='d': # TORSO
t0 = t0 + 10.0
elif key=='D':
t0 = t0 - 10.0
elif key=='e': # HEAD 1
t1 = t1 + 10.0
elif key=='E':
t1 = t1 - 10.0
elif key=='r': # HEAD 2
t2 = t2 + 10.0
elif key=='R':
t2 = t2 - 10.0
elif key=='s': # LUA
t3 = t3 + 10.0
elif key=='S':
t3 = t3 - 10.0
elif key=='a': # LLA
t4 = t4 + 10.0
elif key=='A':
t4 = t4 - 10.0
elif key=='f': # RUA
t5 = t5 + 10.0
elif key=='F':
t5 = t5 - 10.0
elif key=='g': # RLA
t6 = t6 + 10.0
elif key=='G':
t6 = t6 -10.0
elif key=='x': # LUL
t7 = t7 + 10.0
elif key=='X':
t7 = t7 - 10.0
elif key=='z': # LLL
t8 = t8 + 10.0
elif key=='Z':
t8 = t8 -10.0
elif key=='c': # RUL
t9 = t9 + 10.0
elif key=='C':
t9 = t9 - 10.0
elif key=='v': # RLL
t10 = t10 + 10.0
elif key=='V':
t10 = t10 - 10.0
elif key=='q':
sys.exit()

print "params: ", t0, t1, t2, t3, t4, t5, t6, t7, t8, t9, t10
glutPostRedisplay()

glutInit( sys.argv )
glutInitDisplayMode( GLUT_SINGLE | GLUT_RGB )
glutInitWindowSize( 500, 500 )
glutInitWindowPosition(0,0)
glutCreateWindow( 'robot' )
glutDisplayFunc( display )
glutKeyboardFunc(mykey)
p=gluNewQuadric()
gluQuadricDrawStyle(p, GLU_LINE)

glClearColor(0.0, 0.0, 0.0, 0.0)

glMatrixMode(GL_PROJECTION)
glLoadIdentity()
gluPerspective(30, 1.0, 0.0, 100.0)

glutMainLoop()

0 件のコメント: