Menu
▾
▴
[simspark-svn] SF.net SVN: simspark:[382] trunk/rcssserver3d/data/rsg/agent/nao/naolegtoe. rsg
|
From: <he...@us...> - 2014-06-24 20:55:07
|
Revision: 382
http://sourceforge.net/p/simspark/svn/382
Author: hedayat
Date: 2014-06-24 20:55:00 +0000 (Tue, 24 Jun 2014)
Log Message:
-----------
Add naolegtoe.rsg!
Added Paths:
-----------
trunk/rcssserver3d/data/rsg/agent/nao/naolegtoe.rsg
Added: trunk/rcssserver3d/data/rsg/agent/nao/naolegtoe.rsg
===================================================================
--- trunk/rcssserver3d/data/rsg/agent/nao/naolegtoe.rsg (rev 0)
+++ trunk/rcssserver3d/data/rsg/agent/nao/naolegtoe.rsg 2014-06-24 20:55:00 UTC (rev 382)
@@ -0,0 +1,447 @@
+; -*- mode: lisp; -*-
+
+(RSG 0 1)
+(
+ ;
+ ;Template
+ ;@param LRFactor is (left_leg ? -1 : 1)
+ ;
+ (templ $LRFactor $LeftOrRight
+ $Torso_X $Torso_Y $Torso_Z $loadObj
+ $type
+ )
+
+ ;
+ ;The Names
+ ;
+ (def $Hip1Name (join $LeftOrRight hip1))
+ (def $Hip2Name (join $LeftOrRight hip2))
+ (def $ThighName (join $LeftOrRight thigh))
+ (def $ShankName (join $LeftOrRight shank))
+ (def $AnkleName (join $LeftOrRight ankle))
+ (def $FootName (join $LeftOrRight foot))
+ (def $ToeName (join $LeftOrRight toe))
+
+ (def $Hip1BodyPath (join ../../ $Hip1Name /sphereBody))
+ (def $Hip2BodyPath (join ../../ $Hip2Name /sphereBody))
+ (def $ThighBodyPath (join ../../ $ThighName /boxBody))
+ (def $ShankBodyPath (join ../../ $ShankName /boxBody))
+ (def $AnkleBodyPath (join ../../ $AnkleName /sphereBody))
+ (def $FootBodyPath (join ../../ $FootName /boxBody))
+ (def $ToeBodyPath (join ../../ $ToeName /boxBody))
+
+ (def $Joint1PerName (join $LeftOrRight lj1))
+ (def $Joint2PerName (join $LeftOrRight lj2))
+ (def $Joint3PerName (join $LeftOrRight lj3))
+ (def $Joint4PerName (join $LeftOrRight lj4))
+ (def $Joint5PerName (join $LeftOrRight lj5))
+ (def $Joint6PerName (join $LeftOrRight lj6))
+ (def $Joint7PerName (join $LeftOrRight lj7))
+
+ (def $Joint1EffName (join $LeftOrRight le1))
+ (def $Joint2EffName (join $LeftOrRight le2))
+ (def $Joint3EffName (join $LeftOrRight le3))
+ (def $Joint4EffName (join $LeftOrRight le4))
+ (def $Joint5EffName (join $LeftOrRight le5))
+ (def $Joint6EffName (join $LeftOrRight le6))
+ (def $Joint7EffName (join $LeftOrRight le7))
+
+ (def $FRPName (join $LeftOrRight f))
+ (def $FRPToeName (join $LeftOrRight f1))
+
+ ;
+ ;Size and Mass Parameters
+ ;
+ (def $Hip1Radius 0.01)
+ (def $Hip1Mass 0.09)
+
+ (def $Hip2Radius 0.01)
+ (def $Hip2Mass 0.125)
+
+ (def $ThighSizeX 0.07)
+ (def $ThighSizeY 0.07)
+ (def $ThighSizeZ 0.14)
+ (def $ThighMass 0.275)
+
+ (def $ShankSizeX 0.08)
+ (def $ShankSizeY 0.07)
+ (def $ShankSizeZ 0.11)
+ (def $ShankMass 0.225)
+
+ (def $AnkleRadius 0.01)
+ (def $AnkleMass 0.125)
+
+ (def $ToeLength (eval "NaoType["$type"]['ToeLength']"))
+ (def $TotalFootLength 0.16)
+ (def $FootToeMassRatio (eval (eval $ToeLength / $TotalFootLength) * 0.5))
+ (def $FootTotalMass 0.2)
+
+ (def $FootSizeX 0.08)
+ (def $FootSizeY (eval $TotalFootLength - $ToeLength))
+ (def $FootSizeZ 0.02)
+ (def $FootMass (eval (eval 1 - $FootToeMassRatio) * $FootTotalMass))
+
+ (def $ToeSizeX 0.08)
+ (def $ToeSizeY $ToeLength)
+ (def $ToeSizeZ 0.01)
+ (def $ToeMass (eval $FootToeMassRatio * $FootTotalMass))
+
+
+ ;
+ ;Position Parameters
+ ;
+ ;(def $Hip1RelTorso_X (eval $LRFactor * 0.055))
+ (def $Hip1RelTorso_X (eval $LRFactor * (eval "NaoType["$type"]['Hip1RelTorso_X']")))
+ (def $Hip1RelTorso_Y -0.01)
+ ;(def $Hip1RelTorso_Z -0.115)
+ (def $Hip1RelTorso_Z (eval "NaoType["$type"]['Hip1RelTorso_Z']"))
+
+ (def $Hip2RelHip1_X 0)
+ (def $Hip2RelHip1_Y 0)
+ (def $Hip2RelHip1_Z 0)
+
+ (def $ThighRelHip2_X 0)
+ (def $ThighRelHip2_Y 0.01)
+ ;(def $ThighRelHip2_Z -0.04)
+ (def $ThighRelHip2_Z (eval "NaoType["$type"]['ThighRelHip2_Z']"))
+
+ (def $ShankRelThigh_X 0)
+ (def $ShankRelThigh_Y 0.005)
+ (def $ShankRelThigh_Z -0.125)
+
+ (def $AnkleRelShank_X 0)
+ (def $AnkleRelShank_Y -0.01)
+ ;(def $AnkleRelShank_Z -0.055)
+ (def $AnkleRelShank_Z (eval "NaoType["$type"]['AnkleRelShank_Z']"))
+
+ (def $FootRelAnkle_X 0)
+ (def $FootRelAnkle_Y (eval (eval $FootSizeY * 0.5) - 0.05))
+ (def $FootRelAnkle_Z -0.04)
+
+ (def $ToeRelFoot_X 0)
+ (def $ToeRelFoot_Y (eval $TotalFootLength * 0.5))
+ (def $ToeRelFoot_Z -0.005)
+
+ (def $Hip1_X (eval $Torso_X + $Hip1RelTorso_X))
+ (def $Hip1_Y (eval $Torso_Y + $Hip1RelTorso_Y))
+ (def $Hip1_Z (eval $Torso_Z + $Hip1RelTorso_Z))
+
+ (def $Hip2_X (eval $Hip1_X + $Hip2RelHip1_X))
+ (def $Hip2_Y (eval $Hip1_Y + $Hip2RelHip1_Y))
+ (def $Hip2_Z (eval $Hip1_Z + $Hip2RelHip1_Z))
+
+ (def $Thigh_X (eval $Hip2_X + $ThighRelHip2_X))
+ (def $Thigh_Y (eval $Hip2_Y + $ThighRelHip2_Y))
+ (def $Thigh_Z (eval $Hip2_Z + $ThighRelHip2_Z))
+
+ (def $Shank_X (eval $Thigh_X + $ShankRelThigh_X))
+ (def $Shank_Y (eval $Thigh_Y + $ShankRelThigh_Y))
+ (def $Shank_Z (eval $Thigh_Z + $ShankRelThigh_Z))
+
+ (def $Ankle_X (eval $Shank_X + $AnkleRelShank_X))
+ (def $Ankle_Y (eval $Shank_Y + $AnkleRelShank_Y))
+ (def $Ankle_Z (eval $Shank_Z + $AnkleRelShank_Z))
+
+ (def $Foot_X (eval $Ankle_X + $FootRelAnkle_X))
+ (def $Foot_Y (eval $Ankle_Y + $FootRelAnkle_Y))
+ (def $Foot_Z (eval $Ankle_Z + $FootRelAnkle_Z))
+
+ (def $Toe_X (eval $Foot_X + $ToeRelFoot_X))
+ (def $Toe_Y (eval $Foot_Y + $ToeRelFoot_Y))
+ (def $Toe_Z (eval $Foot_Z + $ToeRelFoot_Z))
+
+ ;
+ ;Joint Parameters
+ ;
+ (def $PI 3.14159265)
+ (def $MaxAbsJointSpeed (eval $PI * (eval 351.77 / 180.0)))
+
+ (def $lj1_min -90)
+ (def $lj1_max 1)
+ (def $lj1_max_abs_speed $MaxAbsJointSpeed)
+
+ (def $right_lj2_min -45)
+ (def $left_lj2_min -25)
+ (def $lj2_min_c1 (eval (eval $right_lj2_min + $left_lj2_min) * 0.5))
+ (def $lj2_min_c2 (eval (eval $right_lj2_min - $left_lj2_min) * 0.5))
+ (def $lj2_min (eval $lj2_min_c1 + (eval $LRFactor * $lj2_min_c2)))
+
+ (def $right_lj2_max 25)
+ (def $left_lj2_max 45)
+ (def $lj2_max_c1 (eval (eval $right_lj2_max + $left_lj2_max) * 0.5))
+ (def $lj2_max_c2 (eval (eval $right_lj2_max - $left_lj2_max) * 0.5))
+ (def $lj2_max (eval $lj2_max_c1 + (eval $LRFactor * $lj2_max_c2)))
+ (def $lj2_max_abs_speed $MaxAbsJointSpeed)
+
+ (def $lj3_min -25)
+ (def $lj3_max 100)
+ (def $lj3_max_abs_speed $MaxAbsJointSpeed)
+
+ (def $lj4_min -130)
+ (def $lj4_max 1)
+ (def $lj4_max_abs_speed $MaxAbsJointSpeed)
+
+ (def $lj5_min -45)
+ (def $lj5_max 75)
+ (def $lj5_max_abs_speed (eval "NaoType["$type"]['lj5_max_abs_speed']"))
+
+ (def $right_lj6_min -25)
+ (def $left_lj6_min -45)
+ (def $lj6_min_c1 (eval (eval $right_lj6_min + $left_lj6_min) * 0.5))
+ (def $lj6_min_c2 (eval (eval $right_lj6_min - $left_lj6_min) * 0.5))
+ (def $lj6_min (eval $lj6_min_c1 + (eval $LRFactor * $lj6_min_c2)))
+
+ (def $right_lj6_max 45)
+ (def $left_lj6_max 25)
+ (def $lj6_max_c1 (eval (eval $right_lj6_max + $left_lj6_max) * 0.5))
+ (def $lj6_max_c2 (eval (eval $right_lj6_max - $left_lj6_max) * 0.5))
+ (def $lj6_max (eval $lj6_max_c1 + (eval $LRFactor * $lj6_max_c2)))
+ (def $lj6_max_abs_speed (eval "NaoType["$type"]['lj6_max_abs_speed']"))
+
+ (def $lj7_min -1)
+ (def $lj7_max 70)
+ (def $lj7_max_abs_speed $MaxAbsJointSpeed)
+
+ (def $lj1_axis_x -0.7071)
+ (def $lj1_axis_y 0)
+ (def $lj1_axis_z (eval $LRFactor * 0.7071))
+
+ (def $lj3_Anchor_X (eval -1 * $ThighRelHip2_X))
+ (def $lj3_Anchor_Y (eval -1 * $ThighRelHip2_Y))
+ (def $lj3_Anchor_Z (eval -1 * $ThighRelHip2_Z))
+
+ (def $lj6_Anchor_X 0)
+ (def $lj6_Anchor_Y -0.03)
+ (def $lj6_Anchor_Z 0.035)
+
+ (def $lj7_Anchor_X 0)
+ (def $lj7_Anchor_Y (eval -0.5 * $ToeLength))
+ (def $lj7_Anchor_Z -0.005)
+
+ ;
+ ;Models obj
+ ;
+ (def $ThighObj (join models/ $LeftOrRight thigh.obj))
+ (def $ShankObj (join models/ $LeftOrRight shank.obj))
+ (def $FootObj (join models/ $LeftOrRight foot.obj))
+
+ ;
+ ;Construct Hip1
+ ;
+ (nd Transform
+ (setName $Hip1Name)
+ (setLocalPos $Hip1_X $Hip1_Y $Hip1_Z)
+
+ (importScene rsg/agent/nao/sphere_appearance.rsg $Hip1Radius matYellow)
+ ;(importScene rsg/agent/nao/sphere_physics $Hip1Radius $Hip1Mass)
+ (importScene rsg/agent/nao/sphere_physics_nocollider.rsg $Hip1Radius $Hip1Mass)
+
+
+ (importScene rsg/agent/nao/hingejoint.rsg
+ $Joint1PerName $Joint1EffName
+ ../sphereBody ../../body/boxBody
+ 0 0 0
+ $lj1_axis_x $lj1_axis_y $lj1_axis_z
+ $lj1_min $lj1_max
+ $lj1_max_abs_speed)
+ )
+
+ ;
+ ;Construct Hip2
+ ;
+ (nd Transform
+ (setName $Hip2Name)
+ (setLocalPos $Hip2_X $Hip2_Y $Hip2_Z)
+
+ (importScene rsg/agent/nao/sphere_appearance.rsg $Hip2Radius matYellow)
+ ;(importScene rsg/agent/nao/sphere_physics.rsg $Hip2Radius $Hip2Mass)
+ (importScene rsg/agent/nao/sphere_physics_nocollider.rsg $Hip2Radius $Hip2Mass)
+
+ (importScene rsg/agent/nao/hingejoint.rsg
+ $Joint2PerName $Joint2EffName
+ ../sphereBody $Hip1BodyPath
+ 0 0 0 ;anchor
+ 0 1 0 ;axis
+ $lj2_min $lj2_max
+ $lj2_max_abs_speed)
+ )
+
+ ;
+ ;Construct Thigh
+ ;
+ (nd Transform
+ (setName $ThighName)
+ (setLocalPos $Thigh_X $Thigh_Y $Thigh_Z)
+
+ (switch $loadObj
+ (true
+ (nd StaticMesh (load $ThighObj) (setScale $ThighSizeX $ThighSizeX $ThighSizeX))
+ )
+ (false
+ (importScene rsg/agent/nao/box_appearance.rsg $ThighSizeX $ThighSizeY $ThighSizeZ matBlue)
+ )
+ )
+
+ ;(importScene rsg/agent/nao/box_physics.rsg $ThighSizeX $ThighSizeY $ThighSizeZ $ThighMass)
+ (importScene rsg/agent/nao/box_physics_nocollider.rsg $ThighSizeX $ThighSizeY $ThighSizeZ $ThighMass)
+
+ (nd BoxCollider
+ (setName (join $LeftOrRight ThighCollider))
+ (addNotCollideWithColliderName TorsoCollider true)
+ (setBoxLengths $ThighSizeX $ThighSizeY $ThighSizeZ)
+ (importScene rsg/agent/nao/contactjointhandler.rsg)
+ )
+
+
+ (importScene rsg/agent/nao/hingejoint.rsg
+ $Joint3PerName $Joint3EffName
+ ../boxBody $Hip2BodyPath
+ $lj3_Anchor_X $lj3_Anchor_Y $lj3_Anchor_Z
+ 1 0 0 ;1 -1 -1
+ $lj3_min $lj3_max
+ $lj3_max_abs_speed)
+
+ )
+
+ ;
+ ;Construct Shank
+ ;
+ (nd Transform
+ (setName $ShankName)
+ (setLocalPos $Shank_X $Shank_Y $Shank_Z)
+
+ (switch $loadObj
+ (true
+ (nd StaticMesh (load $ShankObj) (setScale $ShankSizeX $ShankSizeX $ShankSizeX))
+ )
+ (false
+ (importScene rsg/agent/nao/box_appearance.rsg $ShankSizeX $ShankSizeY $ShankSizeZ matGrey)
+ )
+ )
+
+ ;(importScene rsg/agent/nao/box_physics.rsg $ShankSizeX $ShankSizeY $ShankSizeZ $ShankMass)
+ (importScene rsg/agent/nao/box_physics_nocollider.rsg $ShankSizeX $ShankSizeY $ShankSizeZ $ShankMass)
+
+ (nd BoxCollider
+ (setName (join $LeftOrRight ShankCollider))
+ (setBoxLengths $ShankSizeX $ShankSizeY $ShankSizeZ)
+ (importScene rsg/agent/nao/contactjointhandler.rsg)
+ )
+
+ (importScene rsg/agent/nao/hingejoint.rsg
+ $Joint4PerName $Joint4EffName
+ ../boxBody $ThighBodyPath
+ 0 -0.01 0.045
+ 1 0 0 ;0 -1 -1
+ $lj4_min $lj4_max
+ $lj4_max_abs_speed)
+
+ )
+
+ ;
+ ;Construct Ankle
+ ;
+ (nd Transform
+ (setName $AnkleName)
+ (setLocalPos $Ankle_X $Ankle_Y $Ankle_Z)
+
+ (importScene rsg/agent/nao/sphere_appearance.rsg $AnkleRadius matRed)
+ ;(importScene rsg/agent/nao/sphere_physics.rsg $AnkleRadius $AnkleMass)
+ (importScene rsg/agent/nao/sphere_physics_nocollider.rsg $AnkleRadius $AnkleMass)
+
+ (importScene rsg/agent/nao/hingejoint.rsg
+ $Joint5PerName $Joint5EffName
+ ../sphereBody $ShankBodyPath
+ 0 0 0
+ 1 0 0 ;0 -1 -1
+ $lj5_min $lj5_max
+ $lj5_max_abs_speed)
+
+ )
+
+ ;
+ ;Construct Foot
+ ;
+ (nd Transform
+ (setName $FootName)
+ (setLocalPos $Foot_X $Foot_Y $Foot_Z)
+
+ (nd ForceResistancePerceptor
+ (setName $FRPName)
+ )
+
+ (switch $loadObj
+ (true
+ (nd StaticMesh (load $FootObj) (setScale $FootSizeX (eval $FootSizeX * (eval $FootSizeY / $TotalFootLength)) $FootSizeX))
+ )
+ (false
+ (importScene rsg/agent/nao/box_appearance.rsg $FootSizeX $FootSizeY $FootSizeZ matBlue)
+ )
+ )
+
+ ;(importScene rsg/agent/nao/box_physics_with_handler.rsg $FootSizeX $FootSizeY $FootSizeZ $FootMass)
+ (importScene rsg/agent/nao/box_physics_nocollider.rsg $FootSizeX $FootSizeY $FootSizeZ $FootMass)
+
+ (nd BoxCollider
+ (setName (join $LeftOrRight FootCollider))
+ (addNotCollideWithColliderName (join $LeftOrRight ShankCollider) true)
+ (setBoxLengths $FootSizeX $FootSizeY $FootSizeZ)
+ (importScene rsg/agent/nao/touchperceptorhandler.rsg)
+ )
+
+ (importScene rsg/agent/nao/hingejoint.rsg
+ $Joint6PerName $Joint6EffName
+ ../boxBody $AnkleBodyPath
+ $lj6_Anchor_X $lj6_Anchor_Y $lj6_Anchor_Z
+ 0 1 0 ;1 -1 -1
+ $lj6_min $lj6_max
+ $lj6_max_abs_speed)
+
+ (nd ObjectState
+ (setID $FootName)
+ )
+
+
+ )
+
+
+
+ ;
+ ;Construct Toe
+ ;
+ (nd Transform
+ (setName $ToeName)
+ (setLocalPos $Toe_X $Toe_Y $Toe_Z)
+
+ (nd ForceResistancePerceptor
+ (setName $FRPToeName)
+ )
+
+ (switch $loadObj
+ (true
+ (nd StaticMesh (load $FootObj) (setScale $FootSizeX (eval $FootSizeX * (eval $ToeSizeY / $FootSizeY)) (eval $FootSizeX * (eval $ToeSizeZ / $FootSizeZ))))
+ )
+ (false
+ (importScene rsg/agent/nao/box_appearance.rsg $ToeSizeX $ToeSizeY $ToeSizeZ matGrey)
+ )
+ )
+
+ ;(importScene rsg/agent/nao/box_physics_with_handler.rsg $ToeSizeX $ToeSizeY $ToeSizeZ $ToeMass)
+ (importScene rsg/agent/nao/box_physics_nocollider.rsg $ToeSizeX $ToeSizeY $ToeSizeZ $ToeMass)
+
+ (nd BoxCollider
+ (setName (join $LeftOrRight ToeCollider))
+ (addNotCollideWithColliderName (join $LeftOrRight FootCollider) true)
+ (setBoxLengths $ToeSizeX $ToeSizeY $ToeSizeZ)
+ (importScene rsg/agent/nao/touchperceptorhandler.rsg)
+ )
+
+ (importScene rsg/agent/nao/hingejoint.rsg
+ $Joint7PerName $Joint7EffName
+ ../boxBody $FootBodyPath
+ $lj7_Anchor_X $lj7_Anchor_Y $lj7_Anchor_Z
+ 1 0 0 ;1 -1 -1
+ $lj7_min $lj7_max
+ $lj7_max_abs_speed)
+ )
+)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
