[simspark-cvs] simspark/simulations/parts/rsg/agent hoap2.rsg, NONE, 1.1.2.1

[Markus R. <rol...@us...>]

Update of /cvsroot/simspark/simspark/simulations/parts/rsg/agent
In directory sc8-pr-cvs8.sourceforge.net:/tmp/cvs-serv31950

Added Files:
      Tag: WIN32
	hoap2.rsg 
Log Message:
- add the hoap 2 bot

--- NEW FILE: hoap2.rsg ---
; -*- mode: lisp; -*-

;
; approximation of the Fujitsu Hoap-2 robot
;

(RubySceneGraph 0 1)
( 
    ;
    ; define constants for the robot parts
    ;

    ; feet
    (define $FootLength 0.6)
    (define $FootWidth 0.956)
    (define $FootHeight 0.095)
    (define $FootMass 0.1)

    ; static box at the ankles
    (define $AnkleBoxLength 0.355)
    (define $AnkleBoxWidth 0.143)
    (define $AnkleBoxHeight 0.476)

    ; shanks
    (define $ShankLength 0.56)
    (define $ShankWidth 0.56)
    (define $ShankHeight 0.964)
    (define $ShankMass 0.25)

    ; thighs
    (define $ThighLength 0.56)
    (define $ThighWidth 0.56)
    (define $ThighHeight 1.3)
    (define $ThighMass 0.25)

    ; hips
    (define $HipLength 0.273)
    (define $HipWidth 0.273)
    (define $HipHeight 0.2)
    (define $HipMass 0.1)

    ; hand1
    (define $HandBox1Length 0.082)
    (define $HandBox1Width 0.272) 
    (define $HandBox1Height 0.57)

    ; hand2
    (define $HandBox2Length 0.242)
    (define $HandBox2Width 0.272)
    (define $HandBox2Height 0.164)

    ; hand3
    (define $HandBox3Length 0.074)
    (define $HandBox3Width 0.272)
    (define $HandBox3Height 0.2)

    ; lower arms
    (define $LowerarmLength 0.445)
    (define $LowerarmWidth 0.316)
    (define $LowerarmHeight 0.6)
    (define $LowerarmMass 0.2)

    ; elbows
    (define $ElbowDirection 1)
    (define $ElbowRadius 0.134)
    (define $ElbowLength 0.3)

    ; upper arms
    (define $UpperarmLength 0.445)
    (define $UpperarmWidth 0.398)
    (define $UpperarmHeight 0.506)
    (define $UpperarmMass 0.2)

    ; shoulders
    (define $ShoulderLength 0.445)
    (define $ShoulderWidth 1.017)
    (define $ShoulderHeight 0.536)
    (define $ShoulderMass 0.5) 

    ; head
    (define $HeadRadius 0.39)
    (define $HeadMass 0.3)

    ; neck
    (define $NeckLength 0.2)
    (define $NeckWidth 0.2)
    (define $NeckHeight 0.4)
    (define $NeckMass 0.1)

    ; torso1
    (define $TorsoBox1Length 1.37)
    (define $TorsoBox1Width 0.96)
    (define $TorsoBox1Height 1.41)
    (define $TorsoBox1Mass 1.8)

    ; torso2
    (define $TorsoBox2Length 1.37)
    (define $TorsoBox2Width 0.54)
    (define $TorsoBox2Height 1.81)
    (define $TorsoBox2Mass 1.8)

    (define $TorsoCylinderDirection 1)   ; oriented along x-axis
    (define $TorsoCylinderRadius 0.1)
    (define $TorsoCylinderLength 0.3)

    ;
    ; assemble the robot
    ;

    ; torso position (absolute)
    (define $TorsoPosX 0)
    (define $TorsoPosY 0)
    (define $TorsoPosZ 3.5)

    ; calculate offset of the battery pack
    (define $offsetTorsoBox2X 0.0)
    (define $offsetTorsoBox2Y (eval -1 * (eval (eval $TorsoBox1Width / 2.0) + (eval $TorsoBox2Width / 2.0))))
    (define $offsetTorsoBox2Z (eval -1 * (eval (eval $TorsoBox1Height / 2.0) - (eval $TorsoBox2Height / 2.0))))

    ; calculate offset for the left shoulder cylinder
    (define $offsetLeftShoulderCylX (eval -1 * (eval $TorsoBox1Length / 2.0)))
    (define $offsetLeftShoulderCylY 0.0)
    (define $offsetLeftShoulderCylZ (eval (eval $TorsoBox1Height / 2.0) - (eval $ShoulderHeight / 2.0)))
 
    ; calculate offset for the right shoulder cylinder
    (define $offsetRightShoulderCylX (eval $TorsoBox1Length / 2.0))
    (define $offsetRightShoulderCylY 0.0)
    (define $offsetRightShoulderCylZ (eval (eval $TorsoBox1Height / 2.0) - (eval $ShoulderHeight / 2.0)))

    ; create the torso
    (node AgentAspect
        (setName body)
        (setLocalPos $TorsoPosX $TorsoPosY $TorsoPosZ)
        (importScene rsg/boxspheres/box.rsg $TorsoBox1Length $TorsoBox1Width $TorsoBox1Height $TorsoBox1Mass matGrey)

	; install a node holding agent state data
        (node AgentState
	     (setName AgentState)
	)

	; static cylinders for the shoulders
	(node Transform
	     (setLocalPos $offsetLeftShoulderCylX $offsetLeftShoulderCylY $offsetLeftShoulderCylZ)
	     (setLocalRotation 0 90 0)
	     (node CCylinder
		   (setMaterial matGrey)
		   (setParams $TorsoCylinderRadius $TorsoCylinderLength)
	     )
	)
	
	(node Transform
	     (setLocalPos $offsetRightShoulderCylX $offsetRightShoulderCylY $offsetRightShoulderCylZ)
	     (setLocalRotation 0 90 0)
	     (node CCylinder
		   (setMaterial matGrey)
		   (setParams $TorsoCylinderRadius $TorsoCylinderLength)
	     )
	)
    )
	
    ;;; DEBUG
    ;(node FixedJoint
	;  (attach ../body/boxBody 0) ; attach to the static environment
	;  (setFixed)
    ;)
    
    ; add battery backpack
    (node Transform
	(setName battery)
	(setLocalPos (eval $TorsoPosX + $offsetTorsoBox2X) (eval $TorsoPosY + $offsetTorsoBox2Y + 0.01) (eval $TorsoPosZ + $offsetTorsoBox2Z))
	(importScene rsg/boxspheres/box.rsg $TorsoBox2Length $TorsoBox2Width $TorsoBox2Height $TorsoBox2Mass matGrey)

	; +++
	; THIS IS A HACK SINCE WE DON'T HAVE COMPOUND OBJECTS YET !
	; +++

	; install hinge joint to connect to the torso
        (node HingeJoint
            (attach ../boxBody ../../body/boxBody)
            (setAnchor 0.0 0.0 0.0)   
            (setAxis 2) ; move around z-axis
	    (setLowStopDeg 0 0.01)
	    (setHighStopDeg 0 0.01)
	)
    )

    ;;; -------------------------------------
    ;;; HEAD
    ;;; -------------------------------------
    
    ; attach neck
    (node Transform
        (setName neck)
        (setLocalPos $TorsoPosX (eval $TorsoPosY - 0.1) (eval $TorsoPosZ + (eval $TorsoBox1Height / 2.0) + (eval $NeckHeight / 2.0)))
        (importScene rsg/boxspheres/box.rsg $NeckLength $NeckWidth $NeckHeight $NeckMass matGrey)

        ; install hinge joint to connect to the torso
        (node HingeJoint
            (attach ../boxBody ../../body/boxBody)
            (setAnchor $TorsoPosX (eval $TorsoPosY - 0.1) (eval $TorsoPosZ + (eval $TorsoBox1Height / 2.0)))   
            (setAxis 2) ; move around z-axis
            
            ; enable the joint motors
            (setMaxMotorForce 0 100)

            ; install a perceptor
            (node HingePerceptor
                (setName head_joint_2)
            )

            ; install an effector to control the joint motors
            (node HingeEffector
                (setName head_eff_2)
            )
        )
    )

    ; attach head
    (node Transform
        (setName head)
        (setLocalPos $TorsoPosX 
		     (eval $TorsoPosY + 0.05) 
		     (eval $TorsoPosZ + (eval $TorsoBox1Height / 2.0) + (eval $NeckHeight / 2.0) + (eval $HeadRadius / 2.0)))
        (importScene rsg/boxspheres/sphere.rsg $HeadRadius $HeadMass matLightBlue)

        ; install hinge joint to connect to the neck
        (node HingeJoint
            (attach ../sphereBody ../../neck/boxBody)
            (setAnchor $TorsoPosX (eval $TorsoPosY - 0.1) (eval $TorsoPosZ + (eval $TorsoBox1Height / 2.0) + $NeckHeight))
            (setAxis 0) ; move around the x-axis

            ; enable the joint motor
            (setMaxMotorForce 0 100)

            ; install a perceptor
            (node HingePerceptor
                (setName head_joint_1)
            )

            ; install an effector to control the joint motor
            (node HingeEffector
                (setName head_eff_1)
            )
        )
    )

    ;;; -------------------------------------
    ;;; ARMS
    ;;; -------------------------------------

    (define $LeftShoulderPosX (eval $TorsoPosX - 
				    (eval $TorsoBox1Length / 2.0) - 
				    (eval $ShoulderLength / 2.0) - 
				    (eval $TorsoCylinderLength / 2.0)
			       )
    )
    (define $LeftShoulderPosY $TorsoPosY)
    (define $LeftShoulderPosZ (eval $TorsoPosZ + 
				    (eval $TorsoBox1Height / 2.0) -
				    (eval $ShoulderHeight / 2.0)
			       )
    )

    (define $RightShoulderPosX (eval $TorsoPosX +
				    (eval $TorsoBox1Length / 2.0) + 
				    (eval $ShoulderLength / 2.0) + 
				    (eval $TorsoCylinderLength / 2.0)
			       )
    )
    (define $RightShoulderPosY $TorsoPosY)
    (define $RightShoulderPosZ $LeftShoulderPosZ)
 
    ; left shoulder
    (node Transform
        (setName leftshoulder)
        (setLocalPos $LeftShoulderPosX $LeftShoulderPosY $LeftShoulderPosZ)
        (importScene rsg/boxspheres/box.rsg $ShoulderLength $ShoulderWidth $ShoulderHeight $ShoulderMass matGrey)

        ; install universal joint to connect to the torso
        (node UniversalJoint
            (attach ../boxBody ../../body/boxBody)
            (setAnchor 0.0 0.0 0.0)   
            (setAxis1 1.0 0.0 0.0) ; move around the x-axis
            (setAxis2 0.0 1.0 0.0) ; move around the y-axis

            ; enable the joint motors
            (setMaxMotorForce 0 100)
            (setMaxMotorForce 1 100)           

            ; install a perceptor
            (node UniversalJointPerceptor
                (setName larm_joint_1_2)
            )

            ; install an effector to control the joint motors
            (node UniversalJointEffector
                (setName larm_eff_1_2)
            )
        )
    )

    ; right shoulder
    (node Transform
        (setName rightshoulder)
        (setLocalPos $RightShoulderPosX $RightShoulderPosY $RightShoulderPosZ)
        (importScene rsg/boxspheres/box.rsg $ShoulderLength $ShoulderWidth $ShoulderHeight $ShoulderMass matGrey)
        (setLocalRotation 0 0 180)

            ; install universal joint to connect to the torso
	    (node UniversalJoint		
                (attach ../boxBody ../../body/boxBody)
		(setAnchor 0.0 0.0 0.0)  
		(setAxis1 1.0 0.0 0.0) ; move around the x-axis
		(setAxis2 0.0 1.0 0.0) ; move around the y-axis

                ; enable the joint motors
		(setMaxMotorForce 0 100)
		(setMaxMotorForce 1 100)           

                ; install a perceptor
                (node UniversalJointPerceptor
                    (setName rarm_joint_1_2)
		)

                ; install an effector to control the joint motors
                (node UniversalJointEffector
                    (setName rarm_eff_1_2)
                )
            )
    )

    (define $LeftUpperArmPosX $LeftShoulderPosX)
    (define $LeftUpperArmPosY (eval $LeftShoulderPosY + (eval $UpperarmWidth / 2.0)))
    (define $LeftUpperArmPosZ (eval $LeftShoulderPosZ - (eval $ShoulderHeight / 2.0) - (eval $UpperarmHeight / 2.0)))

    (define $RightUpperArmPosX $RightShoulderPosX)
    (define $RightUpperArmPosY (eval $RightShoulderPosY + (eval $UpperarmWidth / 2.0)))
    (define $RightUpperArmPosZ (eval $RightShoulderPosZ - (eval $ShoulderHeight / 2.0) - (eval $UpperarmHeight / 2.0)))
    
    ; left upper arm
    (node Transform
        (setName leftupperarm)
        (setLocalPos $LeftUpperArmPosX $LeftUpperArmPosY $LeftUpperArmPosZ)
        (importScene rsg/boxspheres/box.rsg $UpperarmLength $UpperarmWidth $UpperarmHeight $UpperarmMass matGrey)

        ; install hinge joint to connect to the shoulder
        (node HingeJoint
            (attach ../boxBody ../../leftshoulder/boxBody)
            (setAnchor 0.0 0.0 (eval $UpperarmHeight / 2.0))   
            (setAxis 2) ; move around z-axis
            
            ; enable the joint motors
            (setMaxMotorForce 0 100)

            ; install a perceptor
            (node HingePerceptor
                (setName larm_joint_3)
            )

            ; install an effector to control the joint motors
            (node HingeEffector
                (setName larm_eff_3)
            )
        )

	; static cylinder for the elbow
	(node Transform
	     (setLocalPos 0 0 (eval -1 * (eval (eval $UpperarmHeight / 2.0) + (eval $ElbowRadius / 2.0))))
	     (setLocalRotation 0 90 0)
	     (node CCylinder
		   (setMaterial matGrey)
		   (setParams $ElbowRadius $ElbowLength)
	     )
	)
    )

    ; right upper arm
    (node Transform
        (setName rightupperarm)
        (setLocalPos $RightUpperArmPosX $RightUpperArmPosY $RightUpperArmPosZ)
	(importScene rsg/boxspheres/box.rsg $UpperarmLength $UpperarmWidth $UpperarmHeight $UpperarmMass matGrey)
	
        ; install hinge joint to connect to the shoulder
        (node HingeJoint
            (attach ../boxBody ../../rightshoulder/boxBody)
            (setAnchor 0 0 (eval $UpperarmHeight / 2.0))   
            (setAxis 2) ; move around z-axis
            
            ; enable the joint motors
            (setMaxMotorForce 0 100)

            ; install a perceptor
            (node HingePerceptor
                (setName rarm_joint_3)
            )

            ; install an effector to control the joint motors
            (node HingeEffector
                (setName rarm_eff_3)
            )
        )

	; static cylinder for the elbow
	(node Transform
	     (setLocalPos 0 0 (eval -1 * (eval (eval $UpperarmHeight / 2.0) + (eval $ElbowRadius / 2.0))))
	     (setLocalRotation 0 90 0)
	     (node CCylinder
		   (setMaterial matGrey)
		   (setParams $ElbowRadius $ElbowLength)
	     )
	)
    )

    (define $LeftLowerArmPosX $LeftUpperArmPosX)
    (define $LeftLowerArmPosY $LeftUpperArmPosY)
    (define $LeftLowerArmPosZ (eval $LeftUpperArmPosZ -
				    (eval $UpperarmHeight / 2.0) -
				    $ElbowRadius -
				    (eval $LowerarmHeight / 2.0)
			       )
     )

    (define $RightLowerArmPosX $RightUpperArmPosX)
    (define $RightLowerArmPosY $RightUpperArmPosY)
    (define $RightLowerArmPosZ (eval $RightUpperArmPosZ -
				    (eval $UpperarmHeight / 2.0) -
				    $ElbowRadius -
				    (eval $LowerarmHeight / 2.0)
			       )
     )

    ; left lower arm
    (node Transform
        (setName leftlowerarm)
        (setLocalPos $LeftLowerArmPosX $LeftLowerArmPosY $LeftLowerArmPosZ)
        (importScene rsg/boxspheres/box.rsg $LowerarmLength $LowerarmWidth $LowerarmHeight $LowerarmMass matGrey)

        ; install hinge joint to connect to the upper arm
        (node HingeJoint
            (attach ../boxBody ../../leftupperarm/boxBody)
            (setAnchor 0 0 (eval $LowerarmHeight / 2.0))   
            (setAxis 0) ; move around x-axis
            
            ; enable the joint motors
            (setMaxMotorForce 0 100)

            ; install a perceptor
            (node HingePerceptor
                (setName larm_joint_4)
            )

            ; install an effector to control the joint motors
            (node HingeEffector
                (setName larm_eff_4)
            )
        )

	; static hand
	(node Transform
	     (setLocalPos (eval -1 * (eval (eval $LowerarmLength / 2.0) - (eval $HandBox1Length / 2.0) - 0.06))
			  0.0
			  (eval -1 * (eval (eval $LowerarmHeight / 2.0) + (eval $HandBox1Height / 2.0)))
	      )
	     (node Box
		   (setMaterial matGrey)
		   (setExtents $HandBox1Length $HandBox1Width $HandBox1Height)
	     )	     
        )
	
	(node Transform
	     (setLocalPos (eval (eval $LowerarmLength / 2.0) - (eval $HandBox2Length / 2.0) - 0.06)
			  0.0
			  (eval -1 * (eval (eval $LowerarmHeight / 2.0) + (eval $HandBox2Height / 2.0)))
	      )
	     (node Box
		   (setMaterial matGrey)
		   (setExtents $HandBox2Length $HandBox2Width $HandBox2Height)
	     )
	)

	(node Transform
	     (setLocalPos (eval (eval $LowerarmLength / 2.0) - (eval $HandBox3Length / 2.0) - 0.06)
			  0.0
			  (eval -1 * (eval (eval $LowerarmHeight / 2.0) + $HandBox2Height + (eval $HandBox3Height / 2.0)))
	      )
	     (node Box
		   (setMaterial matGrey)
		   (setExtents $HandBox3Length $HandBox3Width $HandBox3Width)
	     )
	)
    )

    ; right lower arm
    (node Transform
        (setName rightlowerarm)
        (setLocalPos $RightLowerArmPosX $RightLowerArmPosY $RightLowerArmPosZ)
	;(setLocalRotation 0 0 180)
        (importScene rsg/boxspheres/box.rsg $LowerarmLength $LowerarmWidth $LowerarmHeight $LowerarmMass matGrey)

        ; install hinge joint to connect to the upper arm
        (node HingeJoint
            (attach ../boxBody ../../rightupperarm/boxBody)
            (setAnchor 0 0 (eval $LowerarmHeight / 2.0))   
            (setAxis 0) ; move around x-axis
            
            ; enable the joint motors
            (setMaxMotorForce 0 100)

            ; install a perceptor
            (node HingePerceptor
                (setName rarm_joint_4)
            )

            ; install an effector to control the joint motors
            (node HingeEffector
                (setName rarm_eff_4)
            )
        )

	; static hand
	(node Transform
	      (setLocalRotation 0 0 180)
	      
	      (node Transform
		    (setLocalPos (eval -1 * (eval (eval $LowerarmLength / 2.0) - (eval $HandBox1Length / 2.0) - 0.06))
				 0.0
				 (eval -1 * (eval (eval $LowerarmHeight / 2.0) + (eval $HandBox1Height / 2.0)))
				 )
		    (node Box
			  (setMaterial matGrey)
			  (setExtents $HandBox1Length $HandBox1Width $HandBox1Height)
			  )	     
		    )
	      
	      (node Transform
		    (setLocalPos (eval (eval $LowerarmLength / 2.0) - (eval $HandBox2Length / 2.0) - 0.06)
				 0.0
				 (eval -1 * (eval (eval $LowerarmHeight / 2.0) + (eval $HandBox2Height / 2.0)))
				 )
		    (node Box
			  (setMaterial matGrey)
			  (setExtents $HandBox2Length $HandBox2Width $HandBox2Height)
			  )
		    )
	      
	      (node Transform
		    (setLocalPos (eval (eval $LowerarmLength / 2.0) - (eval $HandBox3Length / 2.0) - 0.06)
				 0.0
				 (eval -1 * (eval (eval $LowerarmHeight / 2.0) + $HandBox2Height + (eval $HandBox3Height / 2.0)))
				 )
		    (node Box
			  (setMaterial matGrey)
			  (setExtents $HandBox3Length $HandBox3Width $HandBox3Width)
			  )
		    )
	)
    )

    ;;; -------------------------------------
    ;;; LEGS
    ;;; -------------------------------------
    
    (define $LeftHipPosX (eval $TorsoPosX - (eval $TorsoBox1Length / 4.0) - 0.06))
    (define $LeftHipPosY $TorsoPosY)
    (define $LeftHipPosZ (eval $TorsoPosZ - (eval $TorsoBox1Height / 2.0) - (eval $HipHeight / 2.0)))

    (define $RightHipPosX (eval $TorsoPosX + (eval $TorsoBox1Length / 4.0) + 0.06))
    (define $RightHipPosY $TorsoPosY)
    (define $RightHipPosZ (eval $TorsoPosZ - (eval $TorsoBox1Height / 2.0) - (eval $HipHeight / 2.0)))


    ; left hip
    (node Transform
        (setName lefthip)
        (setLocalPos $LeftHipPosX $LeftHipPosY $LeftHipPosZ)
        (importScene rsg/boxspheres/box.rsg $HipLength $HipWidth $HipHeight $HipMass matGrey)

        ; install hinge joint to connect to the torso
        (node HingeJoint
            (attach ../boxBody ../../body/boxBody)
            (setAnchor 0 0 (eval $HipHeight / 2.0))   
            (setAxis 2) ; move around z-axis
            
            ; enable the joint motors
            (setMaxMotorForce 0 100)

            ; install a perceptor
            (node HingePerceptor
                (setName lleg_joint_1)
            )

            ; install an effector to control the joint motors
            (node HingeEffector
                (setName lleg_eff_1)
            )
        )
    )

    ; right hip
    (node Transform
        (setName righthip)
        (setLocalPos $RightHipPosX $RightHipPosY $RightHipPosZ)
        (importScene rsg/boxspheres/box.rsg $HipLength $HipWidth $HipHeight $HipMass matGrey)

        ; install hinge joint to connect to the torso
        (node HingeJoint
            (attach ../boxBody ../../body/boxBody)
            (setAnchor 0 0 (eval $HipHeight / 2.0))   
            (setAxis 2) ; move around z-axis
            
            ; enable the joint motors
            (setMaxMotorForce 0 100)

            ; install a perceptor
            (node HingePerceptor
                (setName rleg_joint_1)
            )

            ; install an effector to control the joint motors
            (node HingeEffector
                (setName rleg_eff_1)
            )
        )
    )

    (define $LeftThighPosX $LeftHipPosX)
    (define $LeftThighPosY $LeftHipPosY)
    (define $LeftThighPosZ (eval $LeftHipPosZ - (eval $HipHeight / 2.0) - (eval $ThighHeight / 2.0)))

    (define $RightThighPosX $RightHipPosX)
    (define $RightThighPosY $RightHipPosY)
    (define $RightThighPosZ (eval $RightHipPosZ - (eval $HipHeight / 2.0) - (eval $ThighHeight / 2.0)))

    ; left thigh
    (node Transform
        (setName leftthigh)
        (setLocalPos $LeftThighPosX $LeftThighPosY $LeftThighPosZ)
        (importScene rsg/boxspheres/box.rsg $ThighLength $ThighWidth $ThighHeight $ThighMass matGrey)

        ; install hinge joint to connect to the torso
        (node UniversalJoint
            (attach ../boxBody ../../lefthip/boxBody)
            (setAnchor 0.0 0.0 (eval $ThighHeight / 2.0))   
            (setAxis1 1.0 0.0 0.0) ; move around the x-axis
            (setAxis2 0.0 1.0 0.0) ; move around the y-axis

            ; enable the joint motors
            (setMaxMotorForce 0 100)
            (setMaxMotorForce 1 100)           

            ; install a perceptor
            (node UniversalJointPerceptor
                (setName lleg_joint_2_3)
            )

            ; install an effector to control the joint motors
            (node UniversalJointEffector
                (setName lleg_eff_2_3)
            )
        )
    )

    ; right thigh
    (node Transform
        (setName rightthigh)
        (setLocalPos $RightThighPosX $RightThighPosY $RightThighPosZ)
        (importScene rsg/boxspheres/box.rsg $ThighLength $ThighWidth $ThighHeight $ThighMass matGrey)

        ; install hinge joint to connect to the torso
        (node UniversalJoint
            (attach ../boxBody ../../righthip/boxBody)
            (setAnchor 0.0 0.0 (eval $ThighHeight / 2.0))   
            (setAxis1 1.0 0.0 0.0) ; move around the x-axis
            (setAxis2 0.0 1.0 0.0) ; move around the y-axis

            ; enable the joint motors
            (setMaxMotorForce 0 100)
            (setMaxMotorForce 1 100)           

            ; install a perceptor
            (node UniversalJointPerceptor
                (setName rleg_joint_2_3)
            )

            ; install an effector to control the joint motors
            (node UniversalJointEffector
                (setName rleg_eff_2_3)
            )
        )
    )

    (define $LeftShankPosX $LeftThighPosX)
    (define $LeftShankPosY $LeftThighPosY)
    (define $LeftShankPosZ (eval $LeftThighPosZ - (eval $ThighHeight / 2.0) - (eval $ShankHeight / 2.0) - 0.05))

    (define $RightShankPosX $RightThighPosX)
    (define $RightShankPosY $RightThighPosY)
    (define $RightShankPosZ (eval $RightThighPosZ - (eval $ThighHeight / 2.0) - (eval $ShankHeight / 2.0) - 0.05))

    ; left shank
    (node Transform
        (setName leftshank)
        (setLocalPos $LeftShankPosX $LeftShankPosY $LeftShankPosZ)
        (importScene rsg/boxspheres/box.rsg $ShankLength $ShankWidth $ShankHeight $ShankMass matGrey)

        ; install hinge joint to connect to the thigh
        (node HingeJoint
            (attach ../boxBody ../../leftthigh/boxBody)
            (setAnchor 0.0 0.0 (eval $ShankHeight / 2.0))   
            (setAxis 0) ; move around x-axis
            
            ; enable the joint motors
            (setMaxMotorForce 0 100)

            ; install a perceptor
            (node HingePerceptor
                (setName lleg_joint_4)
            )

            ; install an effector to control the joint motors
            (node HingeEffector
                (setName lleg_eff_4)
            )
        )
    )

    ; right shank
    (node Transform
        (setName rightshank)
        (setLocalPos $RightShankPosX $RightShankPosY $RightShankPosZ)
        (importScene rsg/boxspheres/box.rsg $ShankLength $ShankWidth $ShankHeight $ShankMass matGrey)

        ; install hinge joint to connect to the thigh
        (node HingeJoint
            (attach ../boxBody ../../rightthigh/boxBody)
            (setAnchor 0.0 0.0 (eval $ShankHeight / 2.0))
            (setAxis 0) ; move around x-axis
            
            ; enable the joint motors
            (setMaxMotorForce 0 100)

            ; install a perceptor
            (node HingePerceptor
                (setName rleg_joint_4)
            )

            ; install an effector to control the joint motors
            (node HingeEffector
                (setName rleg_eff_4)
            )
        )
    )

    ; left foot
    (node Transform
        (setName leftfoot)
        (setLocalPos $LeftShankPosX 
		     $LeftShankPosY
		     (eval $LeftShankPosZ - (eval $ShankHeight / 2.0) - (eval $FootHeight / 2.0) - 0.05)
	 )
        (importScene rsg/boxspheres/box.rsg $FootLength $FootWidth $FootHeight $FootMass matGrey)

        ; install hinge joint to connect to the shank
        (node UniversalJoint
            (attach ../boxBody ../../leftshank/boxBody)
            (setAnchor 0.0 0.0 (eval $FootHeight / 2.0))   
            (setAxis1 1.0 0.0 0.0) ; move around the x-axis
            (setAxis2 0.0 1.0 0.0) ; move around the y-axis

            ; enable the joint motors
           (setMaxMotorForce 0 100)
           (setMaxMotorForce 1 100)           

            ; install a perceptor
          (node UniversalJointPerceptor
              (setName lleg_joint_5_6)
         )

            ; install an effector to control the joint motors
            (node UniversalJointEffector
                (setName lleg_eff_5_6)
            )
        )

	; static box at the back of the foot
	(node Transform
	     (setLocalPos 0.0
			  (eval -1 * (eval (eval $FootWidth / 2.0) - (eval $AnkleBoxWidth / 2.0)))
			  (eval (eval $FootHeight / 2.0) + (eval $AnkleBoxHeight / 2.0))
	      )
	     (node Box
		   (setMaterial matGrey)
		   (setExtents $AnkleBoxLength $AnkleBoxWidth $AnkleBoxHeight)
	     )
	)
    )

    ; right foot
    (node Transform
        (setName rightfoot)
        (setLocalPos $RightShankPosX 
		     $RightShankPosY
		     (eval $RightShankPosZ - (eval $ShankHeight / 2.0) - (eval $FootHeight / 2.0) - 0.05)
	 )
        (importScene rsg/boxspheres/box.rsg $FootLength $FootWidth $FootHeight $FootMass matGrey)

        ; install hinge joint to connect to the shank
        (node UniversalJoint
            (attach ../boxBody ../../rightshank/boxBody)
            (setAnchor 0.0 0.0 (eval $FootHeight / 2.0))   
            (setAxis1 1.0 0.0 0.0) ; move around the x-axis
            (setAxis2 0.0 1.0 0.0) ; move around the y-axis

            ; enable the joint motors
            (setMaxMotorForce 0 100)
            (setMaxMotorForce 1 100)           

            ; install a perceptor
            (node UniversalJointPerceptor
                (setName rleg_joint_5_6)
            )

            ; install an effector to control the joint motors
            (node UniversalJointEffector
                (setName rleg_eff_5_6)
            )
        )

	; static box at the back of the foot
	(node Transform
	     (setLocalPos 0.0
			  (eval -1 * (eval (eval $FootWidth / 2.0) - (eval $AnkleBoxWidth / 2.0)))
			  (eval (eval $FootHeight / 2.0) + (eval $AnkleBoxHeight / 2.0))
	      )
	     (node Box
		   (setMaterial matGrey)
		   (setExtents $AnkleBoxLength $AnkleBoxWidth $AnkleBoxHeight)
	     )
	)
    )

) ; EOF