Robot2 - simple humanoid

11軸の単純な人間型ロボットを作ってみました。胴体を中心とした全体回転１軸、頭２軸、手足各２軸ｘ４本です。これも"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()