/tests/xrhex.py

# Copyright (c) 2010 Galen Clark Haynes
#               2010 Dexteris Robotics, LLC
# All rights reserved.
# 
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
# 
#     * Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer.
#     * Redistributions in binary form must reproduce the above copyright
#       notice, this list of conditions and the following disclaimer in the
#       documentation and/or other materials provided with the distribution.
#     * Neither the name of Dexteris Robotics, LLC nor the names of its
#       contributors may be used to endorse or promote products derived from
#       this software without specific prior written permission.
# 
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

import copy
from math import pi
import forma
import forma.gl

class RHexLeg(forma.Forma):
    def __init__(self,**kwargs):
        # initialize
        self.length = 0.175
        self.width = 0.03
        self.color = [0.8,0.7,0.4]
        super(type(self),self).__init__(kwargs)
        
        # design leg
        r1 = forma.Rotation(y=pi/2)
        self.rotation = forma.Rotation(z=0)
        basecyl = forma.SolidCylinder(radius=self.width/3,height=self.width)
        t = forma.Translation(x=self.length/2)
        c = forma.Cylinder(arc=pi,radius=self.length/2,height=self.width,color=self.color)
        self.attach(r1.attach(self.rotation.attach(basecyl.attach(t.attach(c)))))
    
    def set_angle(self,a):
        self.rotation.z = a
        
    def get_angle(self):
        return self.rotation.z
        
    angle = property(get_angle,set_angle)


class RHex(forma.Forma):
    def __init__(self,**kwargs):
        # initialize
        self.width = 0.3
        self.height = 0.08
        self.length = 0.57
        self.color = [0.1,0.1,0.1]
        super(type(self),self).__init__(kwargs)

        boxlength = self.length - self.height

        # design body
        self.attach(forma.Box(width=self.width,height=self.height,length=boxlength,color=self.color))
        c_front = forma.SolidCylinder(radius=self.height/2,height=self.width,arc=pi,color=self.color)
        c_back = copy.copy(c_front)
        self.attach(forma.Translation(y=boxlength/2).attach(forma.Rotation(y=pi/2).attach(c_front)))
        self.attach(forma.Translation(y=-boxlength/2).attach(forma.Rotation(y=pi/2,z=pi).attach(c_back)))
        
        # add legs
        w = self.width/2
        l = boxlength/2
        self.leg = []
        for s in [w,-w]:
            for p in [-l,0,l]:
                t = forma.Translation(x=s+0.02*s/abs(s),y=p)
                self.attach(t)
                if p == 0:
                    offset = 0.04 * s / abs(s)
                    c = forma.Box(width=offset,length=offset,height=offset,color=self.color)
                    #c = forma.Cube(size=offset)
                    t.attach(c)
                    newt = forma.Translation(x=offset)
                    t.attach(newt)
                    t = newt
                ll = RHexLeg()
                self.leg.append(ll)
                t.attach(ll)

class EduBot(forma.Forma):
    def __init__(self,**kwargs):
        self.width = 0.25
        self.height = 0.108
        self.length = 0.36
        self.color = [0.4,0.4,0.4]
        self.shell_color = [0.1,0.1,0.1]
        super(type(self),self).__init__(kwargs)

        boxlength = self.length - self.height
        # front to back base piece
        base = forma.Box(height=0.01,width=self.width*0.5,length=self.length,color=self.color)
        self.attach(base)
        # middle pieces
        middle = forma.Box(height=0.01,width=self.width+0.04,length=self.length/5,color=self.color)
        c = forma.SolidCylinder(height=self.width+0.03,radius=self.length/10,arc=pi,color=self.shell_color)
        r = forma.Rotation(y=pi/2)
        self.attach(middle)
        self.attach(r.attach(forma.Rotation(z=pi/2).attach(c)))
        # front piece
        ends = forma.Box(height=0.01,width=self.width,length=self.length/5,color=self.color)
        t = forma.Translation(y=boxlength/2)
        c = forma.SolidCylinder(height=self.width-0.01,radius=self.length/10,arc=pi,color=self.shell_color)
        r = forma.Rotation(y=pi/2)
        self.attach(t.attach(r.attach(forma.Rotation(z=pi/2).attach(c))))
        self.attach(t.attach(ends))
        # back piece
        t = forma.Translation(y=-boxlength/2)
        self.attach(t.attach(r.attach(forma.Rotation(z=pi/2).attach(copy.copy(c)))))
        self.attach(t.attach(copy.copy(ends)))
        r = forma.Rotation(x=pi/2)
        c = forma.SolidCylinder(height=self.length,radius = self.width*0.25,arc=pi,color=self.shell_color)
        self.attach(r.attach(c))
        w = self.width/2
        l = boxlength/2
        self.leg = []
        for s in [-w,w]:
            for p in [-l,0,l]:
                t = forma.Translation(x=s+0.02*s/abs(s),y=p)
                self.attach(t)
                if p == 0:
                    offset = 0.02 * s / abs(s)
                    newt = forma.Translation(x=offset)
                    t.attach(newt)
                    t = newt
                ll = RHexLeg(length=0.117,width=0.02,color=[0.1,0.1,0.1])
                t.attach(ll)
                self.leg.append(ll)

if __name__ == '__main__':
    s = forma.Scene()
    #xrhex = RHex(color=[0.8,0.7,0.4])
    xrhex = RHex()
    s.attach(xrhex)
    t = forma.Translation(x=0.5)
    rhex = RHex(length=0.543,width=0.335,height=0.139,color=[0.0,0.1,0.05])
    #rhex = RHex(height=0.15,width=0.33,length=0.53,color=[0.7,0.7,0.7])
    s.attach(t.attach(rhex))
    t = forma.Translation(x=-0.5)
    edubot = EduBot()
    s.attach(t.attach(edubot))
    for i in [1,3,5]:
        xrhex.leg[i].angle = 6*pi/5
        rhex.leg[i].angle = 6*pi/5
        edubot.leg[i].angle = 6*pi/5
    win = forma.gl.GLUTWindow()
    win.scene = s
    win.run()