; -*- 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)			       )     )