soccerbot.rsg

rcssserver3d Robocup 3D比赛官方指定平台

;; -*- mode: lisp; -*-

;;
;; soccerbot with composite bodies
;;

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

 (def $scale 0.1)

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

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

 ;; shanks
 (def $ShankLength (eval 0.52 * $scale))
 (def $ShankWidth (eval 0.45 * $scale))
 (def $ShankHeight (eval 0.964 * $scale))
 (def $ShankMass 0.25)

 ;; thighs
 (def $ThighLength (eval 0.56 * $scale))
 (def $ThighWidth (eval 0.45 * $scale))
 (def $ThighHeight (eval 1.3 * $scale))
 (def $ThighMass 0.25)

 ;; hips
 (def $HipLength (eval 0.273 * $scale))
 (def $HipWidth (eval 0.273 * $scale))
 (def $HipHeight (eval 0.3 * $scale))
 (def $HipMass 0.1)

 ;; hand1
 (def $HandBox1Length (eval 0.082 * $scale))
 (def $HandBox1Width (eval 0.272 * $scale))
 (def $HandBox1Height (eval 0.57 * $scale))
 (def $HandBox1Mass 0.075)

 ;; hand2
 (def $HandBox2Length (eval 0.242 * $scale))
 (def $HandBox2Width (eval 0.265 * $scale))
 (def $HandBox2Height (eval 0.164 * $scale))

 ;; hand3
 (def $HandBox3Length (eval 0.074 * $scale))
 (def $HandBox3Width (eval 0.272 * $scale))
 (def $HandBox3Height (eval 0.2 * $scale))
 (def $HandBox3Mass 0.05)

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

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

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

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

 ;; head
 (def $HeadRadius (eval 0.39 * $scale))
 (def $HeadMass 0.3)

 ;; torso
 (def $UpperTorsoLength (eval 1.37 * $scale))
 (def $UpperTorsoWidth  (eval 0.96 * $scale))
 (def $UpperTorsoHeight (eval 1.00 * $scale))
 (def $UpperTorsoMass   1.2)

 (def $LowerTorsoLength (eval 1.32 * $scale))
 (def $LowerTorsoWidth  (eval 0.55 * $scale))
 (def $LowerTorsoHeight (eval 0.55 * $scale))
 (def $LowerTorsoMass   0.60)

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

 ;; 
 ;; define joint stops
 ;;
 (def $H1Min -120)
 (def $H1Max 120)
 (def $H2Min -45)
 (def $H2Max 45)

 (def $Rle1Min -90)       
 (def $Rle1Max 60)
 (def $Rle2Min -45)
 (def $Rle2Max 120)
 (def $Rle3Min -75)
 (def $Rle3Max 45)
 (def $Rle4Min -160)
 (def $Rle4Max 10)
 (def $Rle5Min -90)
 (def $Rle5Max 90)
 (def $Rle6Min -45)
 (def $Rle6Max 45)

 (def $Lle1Min -60)
 (def $Lle1Max 90)
 (def $Lle2Min -45)
 (def $Lle2Max 120)
 (def $Lle3Min -45)
 (def $Lle3Max 75)
 (def $Lle4Min -160)
 (def $Lle4Max 10)
 (def $Lle5Min -90)
 (def $Lle5Max 90)
 (def $Lle6Min -45)
 (def $Lle6Max 45)

 (def $Rae1Min -90)
 (def $Rae1Max 180)
 (def $Rae2Min -180)
 (def $Rae2Max 10)
 (def $Rae3Min -135)
 (def $Rae3Max 135)
 (def $Rae4Min -10)
 (def $Rae4Max 130)

 (def $Lae1Min -90)
 (def $Lae1Max 180)
 (def $Lae2Min -10)
 (def $Lae2Max 180)
 (def $Lae3Min -135)
 (def $Lae3Max 135)
 (def $Lae4Min -10)
 (def $Lae4Max 130)

 ;;
 ;; assemble the robot
 ;;

 (def $UpperTorsoPosX 5)
 (def $UpperTorsoPosY 5)
 (def $UpperTorsoPosZ 3.75)

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

 (nd Space
     (setName spaceman)
     ;;(disableInnerCollision true)

     ;; assemble the torso
     (nd AgentAspect
	 (setName body)
	 (setLocalPos $UpperTorsoPosX $UpperTorsoPosY $UpperTorsoPosZ)
	 (importScene rsg/agent/soccerbot058/soccerbottorso.rsg
		      $UpperTorsoLength $UpperTorsoWidth $UpperTorsoHeight
		      $UpperTorsoMass
		      $LowerTorsoLength $LowerTorsoWidth $LowerTorsoHeight
		      $LowerTorsoMass
		      $scale)

       
	 (nd SingleMatInitEffector) ;; we want to initialize

	 (nd TimePerceptor) ;; add a time perceptor

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

	 (nd BeamEffector) ;; add beam effector
					
	 (nd GyroRatePerceptor (setName torso) ) ;; add gyro rate sensor

	 (nd SayEffector) ;; add say effector

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

	 ;; attach left shoulder cylinder (virtual)
	 (nd Transform
	     (setLocalPos $offsetLeftShoulderCylX
			  $offsetLeftShoulderCylY
			  $offsetLeftShoulderCylZ)
	     (setLocalRotation 0 90 0)
	     (nd CCylinder
		 (setName leftshoulderpin)
		 (setMaterial matDarkGrey)
		 (setParams $TorsoCylinderRadius $TorsoCylinderLength)
		 )
	     )
	 ;; attach right shoulder cylinder (virtual)
	 (nd Transform
	     (setLocalPos $offsetRightShoulderCylX
			  $offsetRightShoulderCylY
			  $offsetRightShoulderCylZ)
	     (setLocalRotation 0 90 0)
	     (nd CCylinder
		 (setName rightshoulderpin)
		 (setMaterial matDarkGrey)
		 (setParams $TorsoCylinderRadius $TorsoCylinderLength)
		 )
	     )
      
	 )				
     ;; done upper torso

     (def $LowerTorsoPosX $UpperTorsoPosX)
     (def $LowerTorsoPosY $UpperTorsoPosY)
     (def $LowerTorsoPosZ (eval $UpperTorsoPosZ 
				- (eval $UpperTorsoHeight / 2.0) 
				- (eval $LowerTorsoHeight / 2.0) 
				+ (eval 0.25 * $scale)))