# -*- coding: utf-8 -*-
# -*- Python -*-

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


from numpy import *

import ode  
import sys
import time

import numpy





def prepare_GL():
 
    glViewport(0,0,640,480)

    glClearColor(0.8,0.8,0.9,0)
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glEnable(GL_DEPTH_TEST)
    glDisable(GL_LIGHTING)
    glEnable(GL_LIGHTING)
    glEnable(GL_NORMALIZE)
    glShadeModel(GL_FLAT)

    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    gluPerspective (45,1.3333,0.2,20)

    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()

    glLightfv(GL_LIGHT0,GL_POSITION,[0,0,1,0])
    glLightfv(GL_LIGHT0,GL_DIFFUSE,[1,1,1,1])
    glLightfv(GL_LIGHT0,GL_SPECULAR,[1,1,1,1])
    glEnable(GL_LIGHT0)

    gluLookAt (1.5*3.0, 1.5*3.0, 6.0, 0.0, 1.0, 0.0, -1, -1, 0)

def myCylinder(r, h, dn):
    PI2 = 3.141592*2
    step = PI2 / 25.0
    if dn == 1:
        glNormal3d(-1.0, 0.0, 0.0)
        glBegin(GL_TRIANGLE_FAN)
        for i in range(0, 25):
            t = step * i
            glVertex3d(h, r * sin(t), r * cos(t))
        glEnd()
        glNormal3d(1.0, 0.0, 0.0);
        glBegin(GL_TRIANGLE_FAN);
        for i in range(0, 25):
            t = step * (25 - i)
            glVertex3d(h, r * sin(t), r * cos(t))
        glEnd()

        glBegin(GL_QUAD_STRIP)
        for i in range(0, 25):
            t = step * i;
            z = sin(t);
            x = cos(t);

            glNormal3d(0.0, x, z)
            glVertex3f(h, r * x, r * z)
            glVertex3f(0.0, r * x, r * z)
        
        glEnd()
    elif dn == 2:
        glNormal3d(0.0, 1.0, 0.0)
        glBegin(GL_TRIANGLE_FAN)
        for i in range(0, 25):
            t = step * i
            glVertex3d(r * sin(t), h, r * cos(t))
        glEnd()
        glNormal3d(0.0, -1.0, 0.0)
        glBegin(GL_TRIANGLE_FAN);
        for i in range(0, 25):
            t = step * (25 - i)
            glVertex3d(r * sin(t), h, r * cos(t))
        glEnd()

        glBegin(GL_QUAD_STRIP)
        for i in range(0, 25):
            t = step * i
            x = sin(t)
            z = cos(t)

            glNormal3d(x, 0.0, z)
            glVertex3f(r * x, h, r * z)
            glVertex3f(r * x, 0.0, r * z)
        
        glEnd();
    elif dn == 3:
        glNormal3d(0.0, 0.0, -1.0)
        glBegin(GL_TRIANGLE_FAN)
        for i in range(0, 25):
            t = step * i
            glVertex3d(r * sin(t), r * cos(t), h)
        glEnd()
        glNormal3d(0.0, 0.0, 1.0)
        glBegin(GL_TRIANGLE_FAN)
        for i in range(0, 25):
            t = step * (25 - i)
            glVertex3d(r * sin(t), r * cos(t), h)
        glEnd()

        glBegin(GL_QUAD_STRIP)
        for i in range(0, 25):
            t = step * i
            z = sin(t)
            x = cos(t)

            glNormal3d(x, z, 0.0)
            glVertex3f(r * x, r * z, h)
            glVertex3f(r * x, r * z, 0.0)
        
        glEnd()
def myCapsule(r, h, dn):
    myCylinder(r, h, dn)
    if dn == 1:
        x = h
        y = 0
        z = 0
    elif dn == 2:
        x = 0
        y = h
        z = 0
    elif dn == 3:
        x = 0
        y = 0
        z = h
        
    glTranslatef(x, y, z)
    glutSolidSphere(r, 10, 10)
    
    glTranslatef(-x, -y, -z)
    glutSolidSphere(r, 10, 10)
    
class Function:
    def __init__(self):
        self.bodynum = 0
        self.jointnum = 0
        self.bodies = []
        self.joints = []
        self.world = ode.World() #動力学の世界の創造
        self.space = ode.Space()
        self.lasttime = 0.
        self.floor = ode.GeomPlane(self.space, (0,0,1), 0)
        self.contactgroup = ode.JointGroup()
        self.data =[]
        self.datanum = 0
        self.dnum = []
        self.td = 0
        self.f = open('data.dat', 'w')
        self.tmptheta = [0,0,0,0,0]
        self.tmpw = [0,-0.35,0]
        self.goalx = 1.0
        self.goaly = -5.0
        self.l = 0.7
        self.lg = 0.35
        self.dt = 0.01
        
        
        
    def draw_body(self):
        green = [0.0, 1.0, 0.0, 1.0]
        glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, green)
        for i in range(0, self.bodynum):
            x,y,z = self.bodies[i].getPosition()
            R = self.bodies[i].getRotation()
            rot = [R[0], R[3], R[6], 0.,
                   R[1], R[4], R[7], 0.,
                   R[2], R[5], R[8], 0.,
                   x, y, z, 1.0]
            glPushMatrix()
            glMultMatrixd(rot)
            if self.bodies[i].shape == "box":
                sx = self.bodies[i].lx
                sy = self.bodies[i].ly
                sz = self.bodies[i].lz
                glScale(sx, sy, sz)
                glutSolidCube(1)
            elif self.bodies[i].shape == "sphere":
                black = [0.2, 0.2, 0.2, 1.0]
                glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, black)
                sr = self.bodies[i].r
                glutSolidSphere(sr, 10, 10)
            elif self.bodies[i].shape == "cylinder":
                black = [0.2, 0.2, 0.2, 1.0]
                glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, black)
                myCylinder(self.bodies[i].r, self.bodies[i].l, self.bodies[i].dn)
            elif self.bodies[i].shape == "capsule":
                myCapsule(self.bodies[i].r, self.bodies[i].l, self.bodies[i].dn)

            glPopMatrix()

    def draw_ground(self):
        black = [0.8, 0.4, 0.2, 1.0]
        glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, black)
        glPushMatrix()
        sx,sy,sz = [100, 100, 1]
        glScale(sx, sy, sz)
        glTranslatef(0.0, 0.0, -0.5) 
        glutSolidCube(1)
        glPopMatrix()
    

    def create_box(self, lx, ly, lz, px, py, pz, Mass):
        link = ode.Body(self.world)
        link.MASS = Mass
        link.lx = lx
        link.ly = ly
        link.lz = lz
        link.px = px
        link.py = py
        link.pz = pz
        link.shape = "box"

        self.bodies.append(link)
    

        mass = ode.Mass()
        mass.setBox(self.bodies[self.bodynum].MASS, self.bodies[self.bodynum].lx, self.bodies[self.bodynum].ly, self.bodies[self.bodynum].lz)
        link.setMass(mass)
        link.setPosition((self.bodies[self.bodynum].px, self.bodies[self.bodynum].py, self.bodies[self.bodynum].pz))
        link.boxsize = (self.bodies[self.bodynum].lx, self.bodies[self.bodynum].ly, self.bodies[self.bodynum].lz)
        geom = ode.GeomBox(self.space, self.bodies[self.bodynum].boxsize)
        geom.setBody(self.bodies[self.bodynum])
        self.bodynum += 1
        
    def create_sphere(self, r, px, py, pz, Mass):
        link = ode.Body(self.world)
        link.MASS = Mass
        link.r = r
        link.px = px
        link.py = py
        link.pz = pz
        link.shape = "sphere"

        self.bodies.append(link)
    

        mass = ode.Mass()
        mass.setSphere(self.bodies[self.bodynum].MASS, self.bodies[self.bodynum].r)
        link.setMass(mass)
        link.setPosition((self.bodies[self.bodynum].px, self.bodies[self.bodynum].py, self.bodies[self.bodynum].pz))
        geom = ode.GeomSphere(self.space, self.bodies[self.bodynum].r)
        geom.setBody(self.bodies[self.bodynum])
        self.bodynum += 1

    def create_cylinder(self, dn,  l, r ,px, py, pz, Mass):
        link = ode.Body(self.world)
        link.MASS = Mass
        link.l = l
        link.r = r
        link.px = px
        link.py = py
        link.pz = pz
        link.dn = dn
        link.shape = "cylinder"
        self.bodies.append(link)
        
        mass = ode.Mass()
        mass.setCylinder(self.bodies[self.bodynum].MASS, self.bodies[self.bodynum].dn, self.bodies[self.bodynum].r, self.bodies[self.bodynum].l)
        link.setMass(mass)
        link.setPosition((self.bodies[self.bodynum].px, self.bodies[self.bodynum].py, self.bodies[self.bodynum].pz))
        geom = ode.GeomCylinder(self.space, self.bodies[self.bodynum].r, self.bodies[self.bodynum].l)
        geom.setBody(self.bodies[self.bodynum])
        self.bodynum += 1
    def create_capsule(self, dn,  l, r, px, py, pz,Mass):
        link = ode.Body(self.world)
        link.MASS = Mass
        link.l = l
        link.r = r
        link.px = px
        link.py = py
        link.pz = pz
        link.dn = dn
        link.shape = "capsule"
        self.bodies.append(link)
    

        mass = ode.Mass()
        mass.setCappedCylinder(self.bodies[self.bodynum].MASS, self.bodies[self.bodynum].dn, self.bodies[self.bodynum].r, self.bodies[self.bodynum].l)
        link.setMass(mass)
        link.setPosition((self.bodies[self.bodynum].px, self.bodies[self.bodynum].py, self.bodies[self.bodynum].pz))
        geom = ode.GeomCapsule(self.space, self.bodies[self.bodynum].r, self.bodies[self.bodynum].l)
        geom.setBody()
        self.bodynum += 1
        
    def create_Fix_joint(self, link1, link2):

        joint = ode.FixedJoint(self.world)
        joint.name = "FIXED"
        self.joints.append(joint)
        self.joints[self.jointnum].attach(link1, link2)
        self.joints[self.jointnum].setFixed()
        self.jointnum += 1
    
    def create_Hinge_joint(self, link1, link2, c_x, c_y, c_z, axis_x, axis_y, axis_z, control):

        joint = ode.HingeJoint(self.world)
        joint.name = "HINGE"
        joint.control = control
        joint.theta = 0
        self.joints.append(joint)
        self.joints[self.jointnum].attach(link1, link2)
        self.joints[self.jointnum].setAnchor( (c_x,c_y,c_z) )
        self.joints[self.jointnum].setAxis((axis_x,axis_y,axis_z))
        self.joints[self.jointnum].setParam(ode.ParamFudgeFactor, 0.1)
        self.joints[self.jointnum].setParam(ode.ParamSuspensionCFM,0.015)
        self.joints[self.jointnum].setParam(ode.ParamSuspensionERP,0.8)
        self.jointnum += 1

    def create_Slider_joint(self, link1, link2, axis_x, axis_y, axis_z):

        joint = ode.SliderJoint(self.world)
        joint.name = "SLIDER"
        joint.theta = 0
        self.joints.append(joint)
        self.joints[self.jointnum].attach(link1, link2)
        self.joints[self.jointnum].setAxis((axis_x,axis_y,axis_z))
        self.joints[self.jointnum].setParam(ode.ParamHiStop, 0.1)
        self.joints[self.jointnum].setParam(ode.ParamLoStop, -0.1)
        self.joints[self.jointnum].setParam(ode.ParamFudgeFactor, 0.1)
        self.jointnum += 1

    def create_Ball_joint(self, link1, link2, c_x, c_y, c_z):

        joint = ode.BallJoint(self.world)
        joint.name = "BALL"
        joint.theta = 0
        self.joints.append(joint)
        self.joints[self.jointnum].attach(link1, link2)
        self.joints[self.jointnum].setAnchor( (c_x,c_y,c_z) )
        self.joints[self.jointnum].setParam(ode.ParamFudgeFactor, 0.1)
        self.jointnum += 1
        
    def create_Hinge2_joint(self, link1, link2, c_x, c_y, c_z, axis_x1, axis_y1, axis_z1, axis_x2, axis_y2, axis_z2):

        joint = ode.Hinge2Joint(self.world)
        joint.name = "HINGE2"
        joint.theta = 0
        joint.theta2 = 0
        self.joints.append(joint)
        self.joints[self.jointnum].attach(link1, link2)
        self.joints[self.jointnum].setAnchor( (c_x,c_y,c_z) )
        self.joints[self.jointnum].setAxis1((axis_x1,axis_y1,axis_z1))
        self.joints[self.jointnum].setAxis2((axis_x2,axis_y2,axis_z2))
        self.joints[self.jointnum].setParam(ode.ParamFudgeFactor, 0.1)
        self.jointnum += 1
        
    def create_Universal_joint(self, link1, link2, c_x, c_y, c_z, axis_x1, axis_y1, axis_z1, axis_x2, axis_y2, axis_z2):

        joint = ode.UniversalJoint(self.world)
        joint.name = "UNIVERSAL"
        joint.theta = 0
        joint.theta2 = 0
        self.joints.append(joint)
        self.joints[self.jointnum].attach(link1, link2)
        self.joints[self.jointnum].setAnchor( (c_x,c_y,c_z) )
        self.joints[self.jointnum].setAxis1((axis_x1,axis_y1,axis_z1))
        self.joints[self.jointnum].setAxis2((axis_x2,axis_y2,axis_z2))
        self.joints[self.jointnum].setParam(ode.ParamFudgeFactor, 0.1)
        self.jointnum += 1

    def create_Amotor(self, link1, link2, axis_x, axis_y, axis_z, anum, rel, num_axis):

        joint = ode.AMotor(self.world)
        joint.name = "AMOTOR"
        joint.theta = 0
        joint.anum = anum
        self.joints.append(joint)
        self.joints[self.jointnum].attach(link1, link2)
        #self.joints[self.jointnum].setMode(AMotorUser)
        self.joints[self.jointnum].setNumAxes(num_axis)
        self.joints[self.jointnum].setAxis(anum, rel, (axis_x,axis_y,axis_z))
        self.joints[self.jointnum].setParam(ode.ParamFudgeFactor, 0.1)
        self.jointnum += 1

    def Vcontrol(self):
        kp = 2.0
        kv = 0.0
        k = 10
        fMax = 0.2*30.
        for i in range(0, self.jointnum):
            if self.joints[i].name == "HINGE":
                if self.joints[i].control == "T":
                    self.joints[i].setParam(ode.ParamVel, self.joints[i].theta)
                    self.joints[i].setParam(ode.ParamFMax, fMax)
                    #self.joints[i].addTorque(kp*z-kv*d)
            if self.joints[i].name == "SLIDER":
                self.joints[i].setParam(ode.ParamVel, self.joints[i].theta)
                self.joints[i].setParam(ode.ParamFMax, fMax)
                #self.joints[i].addForce(kp*z-kv*d)
            if self.joints[i].name == "BALL":
                pass
            if self.joints[i].name == "HINGE2":
                tmp = self.joints[i].getAngle1()
                z = self.joints[i].theta - tmp
                self.joints[i].setParam(ode.ParamVel, 0)
                self.joints[i].setParam(ode.ParamFMax, fMax)
                self.joints[i].setParam(ode.ParamSuspensionERP, 0.4)
                self.joints[i].setParam(ode.ParamSuspensionCFM, 0.8)
            if self.joints[i].name == "UNIVERSAL":
                self.joints[i].setParam(ode.ParamVel, 0)
                self.joints[i].setParam(ode.ParamFMax, fMax)
                self.joints[i].setParam(ode.ParamVel2, 0)
                self.joints[i].setParam(ode.ParamFMax2, fMax)
            if self.joints[i].name == "AMOTOR":
                tmp = self.joints[i].getAngle(self.joints[i].anum)
                z = self.joints[i].theta - tmp
                #d = self.joints[i].getAngleRate(self.joints[i].anum)
                #self.joints[i].setParam(ode.ParamVel, k*z)
                #self.joints[i].setParam(ode.ParamFMax, fMax)
                #self.joints[i].addTorque(kp*z-kv*d)
        
    def Pcontrol(self):
        kp = 2.0
        kv = 0.0
        k = 10
        fMax = 0.2*30.
        for i in range(0, self.jointnum):
            if self.joints[i].name == "HINGE":
                if self.joints[i].control == "T":
                    tmp = self.joints[i].getAngle()
                    z = self.joints[i].theta - tmp
                    d = self.joints[i].getAngleRate()
                    self.joints[i].setParam(ode.ParamVel, k*z)
                    self.joints[i].setParam(ode.ParamFMax, fMax)
                    #self.joints[i].addTorque(kp*z-kv*d)
            if self.joints[i].name == "SLIDER":
                tmp = self.joints[i].getPosition()
                z = self.joints[i].theta - tmp
                d = self.joints[i].getPositionRate()
                self.joints[i].setParam(ode.ParamVel, k*z)
                self.joints[i].setParam(ode.ParamFMax, fMax)
                #self.joints[i].addForce(kp*z-kv*d)
            if self.joints[i].name == "BALL":
                pass
            if self.joints[i].name == "HINGE2":
                tmp = self.joints[i].getAngle1()
                z = self.joints[i].theta - tmp
                self.joints[i].setParam(ode.ParamVel, 0)
                self.joints[i].setParam(ode.ParamFMax, fMax)
                self.joints[i].setParam(ode.ParamSuspensionERP, 0.4)
                self.joints[i].setParam(ode.ParamSuspensionCFM, 0.8)
            if self.joints[i].name == "UNIVERSAL":
                self.joints[i].setParam(ode.ParamVel, 0)
                self.joints[i].setParam(ode.ParamFMax, fMax)
                self.joints[i].setParam(ode.ParamVel2, 0)
                self.joints[i].setParam(ode.ParamFMax2, fMax)
            if self.joints[i].name == "AMOTOR":
                tmp = self.joints[i].getAngle(self.joints[i].anum)
                z = self.joints[i].theta - tmp
                #d = self.joints[i].getAngleRate(self.joints[i].anum)
                #self.joints[i].setParam(ode.ParamVel, k*z)
                #self.joints[i].setParam(ode.ParamFMax, fMax)
                #self.joints[i].addTorque(kp*z-kv*d)
    
                
    def snake_control(self, t):
        PI = 3.141592
        Kn = 1 #S-形状の数
        ay0 = 30 * PI / 180#初期くねり角(yaw)
        L = 0.7*6 #体幹全長
        n = 6 #リンク数



        for i in range(0, 5):
            if t > 3:
                self.joints[i].theta =  -2 * ay0 * sin(Kn*PI/n) * sin(2*Kn*PI/L*t - 2*Kn*PI/n*i)
                #print self.joints[i].theta
        
        for i in range(6, 10):
            if t > 3:
                self.joints[i].theta =  0.
                #print self.joints[i].theta

            
    def near_callback(self, args, geom1, geom2):

        contacts = ode.collide(geom1, geom2)

        self.world,self.contactgroup = args
        if geom1 == self.floor or geom2 == self.floor:
            for c in contacts:
                c.setBounce(0.0)
                c.setMu(200000000)
                j = ode.ContactJoint(self.world, self.contactgroup, c)
                j.attach(geom1.getBody(), geom2.getBody())
                
    def init_force_data(self, num):
        self.joints[num].setFeedback(True)
        d = []
        self.data.append(d)
        self.data[self.datanum] = self.joints[num].getFeedback()
        self.dnum.append(num)
        self.datanum += 1
    def get_force_data(self):
        for i in range(0, self.datanum):
            d = self.joints[self.dnum[i]].getFeedback()
            self.data[i] = d
        
        
    def init_GL(self):
    
        self.world.setGravity((0,0,-9.8))    #重力設定
        self.world.setERP(1.0)
        self.world.setCFM(1E-3)

        self.create_box(0.2, 0.4, 0.2, 0.0, 0.0, 0.3, 2.0)
        self.create_box(0.2, 0.4, 0.2, 0.0, self.l, 0.3, 2.0)
        self.create_box(0.2, 0.4, 0.2, 0.0, self.l*2, 0.3, 2.0)
        self.create_box(0.2, 0.4, 0.2, 0.0, self.l*3, 0.3, 2.0)
        self.create_box(0.2, 0.4, 0.2, 0.0, self.l*4, 0.3, 2.0)
        self.create_box(0.2, 0.4, 0.2, 0.0, self.l*5, 0.3, 2.0)

        
        
        self.create_sphere(0.15, -0.2, 0.0, 0.19, 0.3)
        self.create_sphere(0.15, 0.2, 0.0, 0.19, 0.3)
        self.create_sphere(0.15, -0.2, self.l, 0.19, 0.3)
        self.create_sphere(0.15, 0.2, self.l, 0.19, 0.3)
        self.create_sphere(0.15, -0.2, self.l*2, 0.19, 0.3)
        self.create_sphere(0.15, 0.2, self.l*2, 0.19, 0.3)
        self.create_sphere(0.15, -0.2, self.l*3, 0.19, 0.3)
        self.create_sphere(0.15, 0.2, self.l*3, 0.19, 0.3)
        self.create_sphere(0.15, -0.2, self.l*4, 0.19, 0.3)
        self.create_sphere(0.15, 0.2, self.l*4, 0.19, 0.3)
        self.create_sphere(0.15, -0.2, self.l*5, 0.19, 0.3)
        self.create_sphere(0.15, 0.2, self.l*5, 0.19, 0.3)

        self.create_box(0.1, 0.1, 0.1, 0.0, self.lg, 0.3, 0.1)
        self.create_box(0.1, 0.1, 0.1, 0.0, self.l+self.lg, 0.3, 0.1)
        self.create_box(0.1, 0.1, 0.1, 0.0, self.l*2+self.lg, 0.3, 0.1)
        self.create_box(0.1, 0.1, 0.1, 0.0, self.l*3+self.lg, 0.3, 0.1)
        self.create_box(0.1, 0.1, 0.1, 0.0, self.l*4+self.lg, 0.3, 0.1)

        
        self.create_Hinge_joint(self.bodies[0], self.bodies[18], 0, self.lg, 0.3, 0, 0, 1, "T")
        self.create_Hinge_joint(self.bodies[1], self.bodies[19], 0, self.l+self.lg, 0.3, 0, 0, 1, "T")
        self.create_Hinge_joint(self.bodies[2], self.bodies[20], 0, self.l*2+self.lg, 0.3, 0, 0, 1, "T")
        self.create_Hinge_joint(self.bodies[3], self.bodies[21], 0, self.l*3+self.lg, 0.3, 0, 0, 1, "T")
        self.create_Hinge_joint(self.bodies[4], self.bodies[22], 0, self.l*4+self.lg, 0.3, 0, 0, 1, "T")
        
        self.create_Hinge_joint(self.bodies[1], self.bodies[18], 0, self.lg, 0.3, 1, 0, 0, "T")
        self.create_Hinge_joint(self.bodies[2], self.bodies[19], 0, self.l+self.lg, 0.3, 1, 0, 0, "T")
        self.create_Hinge_joint(self.bodies[3], self.bodies[20], 0, self.l*2+self.lg, 0.3, 1, 0, 0, "T")
        self.create_Hinge_joint(self.bodies[4], self.bodies[21], 0, self.l*3+self.lg, 0.3, 1, 0, 0, "T")
        self.create_Hinge_joint(self.bodies[5], self.bodies[22], 0, self.l*4+self.lg, 0.3, 1, 0, 0, "T")


        self.create_Hinge_joint(self.bodies[0], self.bodies[6], 0.1, 0.00, 0.19, 1, 0, 0, "F")
        self.create_Hinge_joint(self.bodies[0], self.bodies[7], -0.1, 0.00, 0.19, 1, 0, 0, "F")
        self.create_Hinge_joint(self.bodies[1], self.bodies[8], 0.1, self.l, 0.19, 1, 0, 0, "F")
        self.create_Hinge_joint(self.bodies[1], self.bodies[9], -0.1, self.l, 0.19, 1, 0, 0, "F")
        self.create_Hinge_joint(self.bodies[2], self.bodies[10], 0.1, self.l*2, 0.19, 1, 0, 0, "F")
        self.create_Hinge_joint(self.bodies[2], self.bodies[11], -0.1, self.l*2, 0.19, 1, 0, 0, "F")
        self.create_Hinge_joint(self.bodies[3], self.bodies[12], 0.1, self.l*3, 0.19, 1, 0, 0, "F")
        self.create_Hinge_joint(self.bodies[3], self.bodies[13], -0.1, self.l*3, 0.19, 1, 0, 0, "F")
        self.create_Hinge_joint(self.bodies[4], self.bodies[14], 0.1, self.l*4, 0.19, 1, 0, 0, "F")
        self.create_Hinge_joint(self.bodies[4], self.bodies[15], -0.1, self.l*4, 0.19, 1, 0, 0, "F")
        self.create_Hinge_joint(self.bodies[5], self.bodies[16], 0.1, self.l*5, 0.19, 1, 0, 0, "F")
        self.create_Hinge_joint(self.bodies[5], self.bodies[17], -0.1, self.l*5, 0.19, 1, 0, 0, "F")

        self.create_box(0.2, 0.2, 10, self.goalx, self.goaly, 5.0, 2.0)

        

        
        glutInit ([])

        glutInitDisplayMode (GLUT_RGB | GLUT_DOUBLE)

        x = 0
        y = 0
        width = 640
        height = 480
        glutInitWindowPosition (x, y);
        glutInitWindowSize (width, height);
        glutCreateWindow ("test")

        glEnable(GL_CULL_FACE);
        glCullFace(GL_BACK);
        


    def _drawfunc (self):
        
        prepare_GL()
        self.draw_body()
        self.draw_ground()

        glutSwapBuffers ()


    def _idlefunc (self):
        
        

        self.td += self.dt
    
        t = self.dt - (time.time() - self.lasttime)
        if (t > 0):
            time.sleep(t)


        self.Control()
        
        self.get_force_data()
        
        self.space.collide((self.world,self.contactgroup), self.near_callback)
    
        self.world.step(self.dt)          #シミュレーションを1ステップ進める。
    
        self.contactgroup.empty()
    
        glutPostRedisplay ()

        self.lasttime = time.time()

    def Control (self):
        x,y,z = self.bodies[0].getPosition()
        
        R = self.bodies[0].getRotation()



        #self.joints[i].theta

        theZ = asin(-R[1]/cos(asin(R[2])))
        phi = []
        for i in range(0,6):
            tmp = theZ
            for j in range(0,i):
                tmp -= self.joints[j].getAngle()
            phi.append(tmp)

        FA = numpy.matrix([[self.lg,0,0,0,0,0],[self.l*cos(phi[0]+phi[1]),self.lg,0,0,0,0],[self.l*cos(phi[0]+phi[2]),self.l*cos(phi[1]+phi[2]),self.lg,0,0,0],[self.l*cos(phi[0]+phi[3]),self.l*cos(phi[1]+phi[3]),self.l*cos(phi[2]+phi[3]),self.lg,0,0],[self.l*cos(phi[0]+phi[4]),self.l*cos(phi[1]+phi[4]),self.l*cos(phi[2]+phi[4]),self.l*cos(phi[3]+phi[4]),self.lg,0],[self.l*cos(phi[0]+phi[5]),self.l*cos(phi[1]+phi[5]),self.l*cos(phi[2]+phi[5]),self.l*cos(phi[3]+phi[5]),self.l*cos(phi[4]+phi[5]),self.lg]])
        FB = numpy.matrix([[cos(phi[0]),sin(phi[0])],[cos(phi[1]),sin(phi[1])],[cos(phi[2]),sin(phi[2])],[cos(phi[3]),sin(phi[3])],[cos(phi[4]),sin(phi[4])],[cos(phi[5]),sin(phi[5])]])
        E = numpy.matrix([[-1,1,0,0,0,0],[0,-1,1,0,0,0],[0,0,-1,1,0,0],[0,0,0,-1,1,0],[0,0,0,0,-1,1]])
        invFA = FA.I
        tmp = numpy.dot(E,invFA)
        H = numpy.dot(tmp,FB)

        

        invH = H.I

        print self.td

        x0 = x + 0.35*sin(phi[0])
        y0 = y - 0.35*cos(phi[0])

        
        dw = numpy.matrix([[(self.goalx - x0)*0.3],[(self.goaly - y0)*0.3]])

        dthe = numpy.dot(H,dw)

        if(self.td > 0.3):
            for i in range(0,5):
                self.joints[i].theta = -dthe[i][0]
            for i in range(6,10):
                tmp = self.joints[i].getAngle()
                self.joints[i].theta = 10*(0 - tmp)
            self.Vcontrol()
        else:
            self.snake_control(10)
            self.Pcontrol()
        

        dthed = numpy.matrix([[(self.joints[0].getAngle() - self.tmptheta[0])/self.dt],[(self.joints[1].getAngle() - self.tmptheta[1])/self.dt],[(self.joints[2].getAngle() - self.tmptheta[2])/self.dt],[(self.joints[3].getAngle() - self.tmptheta[3])/self.dt],[(self.joints[4].getAngle() - self.tmptheta[4])/self.dt]])
        dwd = numpy.matrix([[(x0 - self.tmpw[0])/self.dt],[(y0 - self.tmpw[1])/self.dt]])


        
        

        self.f.write(str(self.td) + "\t")
        self.f.write(str(self.goalx) + "\t")
        self.f.write(str(self.goaly) + "\t")
        
        self.f.write(str(dw[0,0]) + "\t")
        self.f.write(str(dw[1,0]) + "\t")
        self.f.write(str(dwd[0,0]) + "\t")
        self.f.write(str(dwd[1,0]) + "\t")
        self.f.write(str(x0) + "\t")
        self.f.write(str(y0) + "\n")

        for i in range(0,5):
            self.tmptheta[i] = self.joints[i].getAngle()
        self.tmpw[0] = x0
        self.tmpw[1] = y0

    def _keyfunc (self, key, x, y):
        PI = 3.141592
        ESCAPE = 27
        if key == ESCAPE:
            sys.exit (0)
        elif key == "1":
            time.sleep(10)

            
def main():
    Func = Function()
    Func.init_GL()
    glutKeyboardFunc(Func._keyfunc)
    glutSpecialFunc(Func._keyfunc)
    glutDisplayFunc (Func._drawfunc)
    glutIdleFunc (Func._idlefunc)
    glutMainLoop ()

if __name__ == "__main__":
    main()