soccerbot055.rsg

robocup rcssserver 运行防真机器人足球比赛所用的服务器端

; -*- mode: lisp; -*-

;
; possible base for the Atlanta 2007 robot
;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    ;
    ; assemble the robot
    ;

    ; torso position (absolute)
    (def $TorsoPosX 5)
    (def $TorsoPosY 5)
    (def $TorsoPosZ 3.5)

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

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

    (nd Space
	(setName spaceman)
					; create the torso
    (nd AgentAspect
        (setName body)
        (setLocalPos $TorsoPosX $TorsoPosY $TorsoPosZ)
        ; (importScene rsg/boxspheres/box.rsg $TorsoBox1Length $TorsoBox1Width $TorsoBox1Height $TorsoBox1Mass matDarkGrey)
	(importScene rsg/agent/soccerbottorso.rsg $TorsoBox1Length $TorsoBox1Width $TorsoBox1Height $TorsoBox1Mass)
	
	; we want to initialize
	;(nd InitEffector)
	(nd SingleMatInitEffector)

	;add a time perceptor
	(nd TimePerceptor)

	; install a node holding agent state data
        (nd AgentState
	     (setName AgentState)
	    ; add a gamestateperceptor
	    (nd GameStatePerceptor)
	)

	; add beam effector
	(nd BeamEffector)

	; add gyro rate sensor
	(nd GyroRatePerceptor
	      (setName torso)
	      )

	(nd VisionPerceptor
	     (setSenseMyPos false)
	     (setStaticSenseAxis false)
	     (addNoise false)
	     )

	; static cylinders for the shoulders
	(nd Transform
	     (setLocalPos $offsetLeftShoulderCylX $offsetLeftShoulderCylY $offsetLeftShoulderCylZ)
	     (setLocalRotation 0 90 0)
	     (nd CCylinder
		   (setName leftshoulderpin)
		   (setMaterial matDarkGrey)
		   (setParams $TorsoCylinderRadius $TorsoCylinderLength)
	     )
	)
	
	(nd Transform
	     (setLocalPos $offsetRightShoulderCylX $offsetRightShoulderCylY $offsetRightShoulderCylZ)
	     (setLocalRotation 0 90 0)
	     (nd CCylinder
		   (setName rightshoulderpin)
		   (setMaterial matDarkGrey)
		   (setParams $TorsoCylinderRadius $TorsoCylinderLength)
	     )
	)
    )
;) ;xxx	
    ;;; DEBUG
    ;(nd FixedJoint
;	  (attach ../body/boxBody 0) ; attach to the static environment
;	  (setFixed)
;    )

    ;;; -------------------------------------
    ;;; HEAD
    ;;; -------------------------------------
    
    ; attach head
    (nd Transform
        (setName head)
        (setLocalPos (eval $TorsoPosX)
		     (eval $TorsoPosY + 0.05) 
		     (eval $TorsoPosZ + 0.1 + (eval $TorsoBox1Height / 2.0) + (eval $HeadRadius / 2.0)))
        (importScene rsg/boxspheres/sphere.rsg $HeadRadius $HeadMass matGrey)

        ; install hinge joint to connect to the neck
        (nd FixedJoint
            (attach ../sphereBody ../../body/boxBody)
	    (setFixed)
        )
    )

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

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

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

        ; install universal joint to connect to the torso
        (nd 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
            (nd UniversalJointPerceptor
                (setName laj1_2)
            )

            ; install an effector to control the joint motors
            (nd UniversalJointEffector
                (setName lae1_2)
            )
        )
    )

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

            ; install universal joint to connect to the torso
	    (nd 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
                (nd UniversalJointPerceptor
                    (setName raj1_2)
		)

                ; install an effector to control the joint motors
                (nd UniversalJointEffector
                    (setName rae1_2)
                )
            )
    )

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

    (def $RightUpperArmPosX $RightShoulderPosX)
    ;(def $RightUpperArmPosY (eval $RightShoulderPosY + (eval $UpperarmWidth / 2.0)))
    (def $RightUpperArmPosY (eval $RightShoulderPosY))
    (def $RightUpperArmPosZ (eval $RightShoulderPosZ - (eval $ShoulderHeight / 2.0) - (eval $UpperarmHeight / 2.0)))
    
    ; left upper arm
    (nd 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
        (nd 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
            (nd HingePerceptor
                (setName laj3)
            )

            ; install an effector to control the joint motors
            (nd HingeEffector
                (setName lae3)
            )
        )

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

    ; right upper arm
    (nd Transform
        (setName rightupperarm)
        (setLocalPos $RightUpperArmPosX $RightUpperArmPosY $RightUpperArmPosZ)
        (importScene rsg/boxspheres/box.rsg $UpperarmLength $UpperarmWidth $UpperarmHeight $UpperarmMass matGrey)
	;(setLocalRotation 180 0 0)

        ; install hinge joint to connect to the shoulder
        (nd 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
            (nd HingePerceptor
                (setName raj3)
            )

            ; install an effector to control the joint motors
            (nd HingeEffector
                (setName rae3)
            )
        )

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

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

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

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

        ; install hinge joint to connect to the upper arm
        (nd 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
            (nd HingePerceptor
                (setName laj4)
            )

            ; install an effector to control the joint motors
            (nd HingeEffector
                (setName lae4)
            )
        )

	; static hand
	(nd Transform