Code Release 2024 of Shared Autonomy Challenge

Co-authored-by: Leon B. Co-authored-by: Dominik Brämer <[email protected]> Co-authored-by: Alexandra Cloodt <[email protected]> Co-authored-by: Lisa Dasmann <[email protected]> Co-authored-by: Alicia Gayda <[email protected]> Co-authored-by: Robert Huber <[email protected]> Co-authored-by: Diana Kleingarn <[email protected]> Co-authored-by: Thomas Klute <[email protected]> Co-authored-by: Angelina Koch <[email protected]> Co-authored-by: Aaron Larisch <[email protected]> Co-authored-by: Mahdokht Mohammadi <[email protected]> Co-authored-by: Arne Moos <[email protected]> Co-authored-by: Tim Plotzki <[email protected]> Co-authored-by: Ingmar Schwarz <[email protected]> Co-authored-by: Alexander Voß <[email protected]>
NaoDevils · Oct 9, 2024 · 39a4ded · 39a4ded
1 parent de64b13
commit 39a4ded
Show file tree

Hide file tree

Showing 149 changed files with 1,774 additions and 3,107 deletions.
diff --git a/Config/KeyFrameEngine/freeArmsFront.kfm b/Config/KeyFrameEngine/freeArmsFront.kfm
@@ -1,7 +1,7 @@
 keyFrameID = freeArmsFront;
 initialKeyFrame = true;
 nextMotionConditions = [execute];
-nextKeyFrameMotionIDs = [standUpFront];
+nextKeyFrameMotionIDs = [lying];
 YstabilizationP = 0;
 YstabilizationI = 0;
 YstabilizationD = 0;

diff --git a/Config/KeyFrameEngine/lying.kfm b/Config/KeyFrameEngine/lying.kfm
@@ -23,7 +23,7 @@ keyFrames = [//stand copy
     armsAngles = [90deg,10deg,100deg,0deg,-90deg,0, 90deg,-10deg,-100deg,0deg,90deg,0];
     legsAngles = [0,0,-21deg,49deg,-27.5deg,0, 0,0,-21deg,49deg,-27.5deg,0];
     stiffnesses = [40,40, 40,40,40,40,40,40, 40,40,40,40,40,40, 60,60,60,60,60,60, 60,60,60,60,60,60];
-    duration = 700;
+    duration = 1000;
     intType = sine;
     angleAtKeyFrameTarget = 0;
     angleAtKeyFrameError = [-1,-1];

diff --git a/Config/KeyFrameEngine/saveFallFront.kfm b/Config/KeyFrameEngine/saveFallFront.kfm
@@ -40,7 +40,7 @@ keyFrames = [
   },
   {
     headAngles = [0deg,-38deg];
-    armsAngles = [15deg,0deg,110deg,-63.1917deg,-21.5359deg,0.252102deg, 40deg,-5deg,-103.861deg,27.7762deg,34.0117deg,0.435445deg];
+    armsAngles = [20deg,0deg,110deg,-63.1917deg,-21.5359deg,0.252102deg, 53.2648deg,-5deg,-103.861deg,27.7762deg,34.0117deg,0.435445deg];
     legsAngles = [0deg,0deg,-35.9687deg,79.188deg,-69.7884deg,-5.2711deg, 0deg,0deg,2.25942deg,41.6631deg,-70.6625deg,-5.88634deg];
     stiffnesses = [30,30, 30,15,15,15,15,15, 30,15,15,15,15,15, 20,20,20,20,20,20, 20,20,20,20,20,20];
     duration = 35;
@@ -60,7 +60,7 @@ keyFrames = [
   },
   {
     headAngles = [0deg,-38deg];
-    armsAngles = [35deg,-15deg,90deg,-45deg,-21.5359deg,0deg, 50deg,10deg,-88.861deg,27.7762deg,34.0117deg,0deg];
+    armsAngles = [23.5526deg,-11deg,91deg,-63.1917deg,-21.5359deg,0.252102deg, 53.2648deg,3.0738deg,-88.861deg,27.7762deg,34.0117deg,0.435445deg];
     legsAngles = [0deg,0deg,-32.453deg,74.7934deg,-69.8763deg,-4.56796deg, 0deg,0deg,13.5096deg,20.5691deg,-64.598deg,-6.2379deg];
     stiffnesses = [30,30, 20,15,15,15,15,15, 20,15,15,15,15,15, 20,20,20,20,20,20, 20,20,20,20,20,20];
     duration = 35;
@@ -72,7 +72,27 @@ keyFrames = [
     leavingPossible = false;
     useArmProblemDetection = [false,false];
     armAngleReached = false;
-    holdFrame = 500;
+    holdFrame = 128;
+    nextKeyFrameConditions = [none];
+    nextKeyFrameIDs = [none];
+    fallDownProtection = none;
+    holdFallDownProtection = 12;
+  },
+  {// move arms back 
+    headAngles = [0deg,-38deg];
+    armsAngles = [41.043deg,-6.15482deg,105.38deg,-60.6429deg,-21.5359deg,0.252102deg, 62.845deg,3.60116deg,-102.133deg,39.9931deg,34.1875deg,0.206265deg];
+    legsAngles = [0deg,0deg,-32.2772deg,61.6976deg,-69.7005deg,0deg, 0deg,0deg,-0.816792deg,20.3054deg,-59.764deg,0deg];
+    stiffnesses = [30,30, 15,15,15,15,15,15, 15,15,15,15,15,15, 20,20,20,20,20,20, 20,20,20,20,20,20];
+    duration = 35;
+    intType = linear;
+    angleAtKeyFrameTarget = 0;
+    angleAtKeyFrameError = [-1,-1];
+    stabilize = false;
+    waitForStable = false;
+    leavingPossible = false;
+    useArmProblemDetection = [false,false];
+    armAngleReached = false;
+    holdFrame = 36;
     nextKeyFrameConditions = [none];
     nextKeyFrameIDs = [none];
     fallDownProtection = none;

diff --git a/Config/KeyFrameEngine/stand.kfm b/Config/KeyFrameEngine/stand.kfm
@@ -27,7 +27,7 @@ keyFrames = [
     intType = linear;
     angleAtKeyFrameTarget = 0;
     angleAtKeyFrameError = [20deg,20deg];
-    stabilize = false;
+    stabilize = true;
     waitForStable = true;
     leavingPossible = false;
     useArmProblemDetection = [false,false];

diff --git a/Config/KeyFrameEngine/standUpBackLean.kfm b/Config/KeyFrameEngine/standUpBackLean.kfm
@@ -47,7 +47,7 @@ keyFrames = [
     duration = 200;
     intType = linear;
     angleAtKeyFrameTarget = 16deg;
-    angleAtKeyFrameError = [25deg,23deg];
+    angleAtKeyFrameError = [25deg,20deg];
     stabilize = false;
     waitForStable = true;
     leavingPossible = false;
@@ -68,7 +68,7 @@ keyFrames = [
     duration = 600;
     intType = sine;
     angleAtKeyFrameTarget = 11deg;
-    angleAtKeyFrameError = [35deg,23deg];
+    angleAtKeyFrameError = [35deg,20deg];
     stabilize = false;
     waitForStable = false;
     leavingPossible = false;

diff --git a/Config/KeyFrameEngine/standUpFrontLean.kfm b/Config/KeyFrameEngine/standUpFrontLean.kfm
@@ -89,7 +89,7 @@ keyFrames = [
     duration = 200;
     intType = linear;
     angleAtKeyFrameTarget = 14deg;
-    angleAtKeyFrameError = [40deg,21.5deg];
+    angleAtKeyFrameError = [40deg,20deg];
     stabilize = false;
     waitForStable = true;
     leavingPossible = false;

diff --git a/Config/Kicks/WalkingEngine/kickHack.cfg b/Config/Kicks/WalkingEngine/kickHack.cfg
@@ -48,9 +48,9 @@ steps = [
     kick = true;
     swingFootTraj = [
       {x = 0.0; y = 0.0; z = 0.0;},
-      {x = 0.0; y = 0.0; z = 0.020;},
-      {x = 0.02; y = 0.0; z = 0.020;},
-      {x = 0.0; y = 0.0; z = 0.020;},
+      {x = 0.0; y = 0.0; z = 0.025;},
+      {x = 0.025; y = 0.0; z = 0.025;},
+      {x = 0.0; y = 0.0; z = 0.025;},
       {x = 0.0; y = 0.0; z = 0.00;}
     ];
   }

diff --git a/Config/Robots/V6/sensorControlParams.cfg b/Config/Robots/V6/sensorControlParams.cfg
@@ -91,35 +91,32 @@ coMShifting = {
 };
 stepDurationAdjustment = {
   stepDuration = {
-    min = 0.5;
+    min = 0.43;
     max = 0.6;
   };
-  exponentialFactor = 0.999;
+  exponentialFactor = 0.99;
 };
 speedReduction = {
   angleY = {
-    min = -1.6deg;
-    max = 3.4deg;
+    min = -1.7deg;
+    max = 3.45deg;
   };
   angleX = {
-    min = -1.6deg;
-    max = 1.6deg;
+    min = -1.7deg;
+    max = 1.75deg;
   };
   speedReductionFactor = 1.05;
-  speedReductionExponentialBasis = 1.22;
-  emergencyStopSpeedReductionFactorThreshold = 4;
+  speedReductionExponentialBasis = 1.138;
+  emergencyStopSpeedReductionFactorThreshold = 1.9;
   emergencyStopGyroVariance = 1.25;
-  stepSpeedInfluenceFactor = {
-    x = 0.35;
-    y = 0.15;
-  };
-  customStepFactorStable = 3;
+  stepSpeedInfluenceFactor = 0.42;
+  customStepFactorStable = 2.5;
   anglesource = imuModel;
 };
 previewController = {
   freezeDSRefZMPUntilFSR = true;
   slowDSRefZMPUntilFSR = false;
-  maxFramesForDSExtension = 15;
+  maxFramesForDSExtension = 50;
 };
 flipmObserverGains = {
   activateSensorX = false;

diff --git a/Config/Robots/V6/walkingParamsFLIPM.cfg b/Config/Robots/V6/walkingParamsFLIPM.cfg
@@ -1,33 +1,33 @@
 acceleration = {
   maxAccR = 90deg;
-  maxAccXForward = 0.075;
-  maxAccXBackward = 0.05;
-  maxAccY = 0.05;
+  maxAccXForward = 0.1;
+  maxAccXBackward = 0.08;
+  maxAccY = 0.08;
 };
 comOffsets = {
   tiltSpeedDependent = [
     0deg,
     0deg
   ];
-  xArmContact = 0.0025;
-  xFixed = 0.02;
+  xArmContact = 0.0075;
+  xFixed = 0.022;
   xSpeedDependent = 0;
   yArmContact = [
     0,
     0
   ];
   yFixed = 0;
   ySpeedDependent = [
-    0.007,
-    -0.007
+    0.0055,
+    -0.006
   ];
 };
 footMovement = {
   doubleSupportRatio = 0.05;
   footPitch = 2deg;
   footRoll = 1deg;
-  footYDistance = 0.055;
-  leadingSideStepSpeedUp = 1;
+  footYDistance = 0.05;
+  leadingSideStepSpeedUp = 1.25;
   forwardPolygon = [
     0,
     0.2,
@@ -69,9 +69,9 @@ footMovement = {
     0
   ];
   stepHeight = [
-    0.023,
-    0.023,
-    0.026
+    0.022,
+    0.022,
+    0.025
   ];
 };
 jointCalibration = {
@@ -116,42 +116,42 @@ jointCalibration = {
     0
   ];
 };
-jointSensorDelayFrames = 4;
+jointSensorDelayFrames = 3;
 imuSensorDelayFrames = 4;
 outFilterOrder = 3;
-speedFactor = 1;
+speedFactor = 1.0;
 minSpeedLimits = {
-  r = 90deg;
-  rOnly = 120deg;
+  r = 60deg;
+  rOnly = 75deg;
   xBackward = 100;
-  xForward = 140;
+  xForward = 130;
   xForwardArmContact = 120;
   xForwardOmni = 120;
-  y = 130;
-  yArmContact = 120;
+  y = 110;
+  yArmContact = 100;
 };
 speedLimits = {
-  r = 90deg;
-  rOnly = 120deg;
+  r = 60deg;
+  rOnly = 75deg;
   xBackward = 130;
-  xForward = 180.489;
-  xForwardArmContact = 162.44;
-  xForwardOmni = 144.392;
+  xForward = 200;
+  xForwardArmContact = 180;
+  xForwardOmni = 160;
   y = 160;
   yArmContact = 130;
 };
 maxSpeedLimits = {
-  r = 90deg;
-  rOnly = 120deg;
-  xBackward = 200;
-  xForward = 250;
-  xForwardArmContact = 230;
-  xForwardOmni = 230;
-  y = 230;
-  yArmContact = 200;
+  r = 60deg;
+  rOnly = 75deg;
+  xBackward = 150;
+  xForward = 220;
+  xForwardArmContact = 200;
+  xForwardOmni = 200;
+  y = 180;
+  yArmContact = 150;
 };
 walkTransition = {
-  crouchingDownPhaseLength = 5;
+  crouchingDownPhaseLength = 10;
   stopSpeedThresholdX = 0.01;
   stopSpeedThresholdY = 0.01;
   fallDownAngleFront = 26deg;

diff --git a/Config/Scenes/Includes/SharedAutonomy.con b/Config/Scenes/Includes/SharedAutonomy.con
@@ -0,0 +1,21 @@
+vf worldState
+vfd worldState fieldLines
+vfd worldState goalFrame 
+vfd worldState fieldPolygons
+vfd worldState representation:GameInfo
+vfd worldState representation:RobotPose
+
+vfd worldState behavior:RemoteControlProvider 
+vfd worldState representation:RobotMap
+vfd worldState representation:BallChaserDecision
+vfd worldState representation:TeammateData
+vfd worldState origin:RobotPoseAfterPreview 
+vfd worldState representation:MotionRequest 
+vfd worldState origin:RobotPose
+vfd worldState representation:BallModel 
+vfd worldState representation:BallModel:endPosition
+
+vd module:BallChaserDecisionProvider:overwrite
+
+echo call Includes/JPEGImageViews
+echo set parameters:JPEGImageProvider quality = 15;
diff --git a/Config/Scenes/SharedAutonomyAttackers.con b/Config/Scenes/SharedAutonomyAttackers.con
@@ -0,0 +1,15 @@
+call Includes/Normal
+
+echo gc playing
+
+robot robot8
+dr module:BallChaserDecisionProvider:enable on
+robot robot9
+dr module:BallChaserDecisionProvider:enable on
+robot robot3
+rc offensive
+robot robot4
+rc offensive
+rc on
+
+call Includes/SharedAutonomy
diff --git a/Config/Scenes/SharedAutonomyAttackers.ros2 b/Config/Scenes/SharedAutonomyAttackers.ros2
@@ -0,0 +1,47 @@
+<Simulation>
+
+  <Include href="Includes/NaoV6H25.rsi2"/>
+  <Include href="Includes/Ball2016SPL.rsi2"/>
+  <Include href="Includes/Field2020SPL.rsi2"/>
+  <Include href="Includes/Boards.rsi2"/>
+
+  <Scene name="RoboCup" controller="SimulatedNao" stepLength="0.012" color="rgb(65%, 65%, 70%)" Kp="12500" Kd="10000" contactKp="1425" contactKd="7.5">
+   <Light z="9m" ambientColor="rgb(50%, 50%, 50%)"/>
+
+    <Compound name="robots">
+      <Body ref="Nao" name="robot3">
+        <Translation x="2.8" y="0" z="330mm"/>
+        <Rotation z="180degree"/>
+        <Set name="NaoColor" value="red"/>
+      </Body>
+      <Body ref="Nao" name="robot4">
+        <Translation x="0.9" z="330mm"/>
+        <Rotation z="180degree"/>
+        <Set name="NaoColor" value="yellow"/>
+      </Body>
+
+      <Body ref="Nao" name="robot8">
+        <Translation x="-4.5" z="330mm"/>
+        <Set name="NaoColor" value="blue"/>
+      </Body>
+      <Body ref="Nao" name="robot9">
+        <Translation x="-2" y = "0.4" z="330mm"/>
+        <Set name="NaoColor" value="black"/>
+      </Body>
+    </Compound>
+
+    <Compound name="balls">
+      <Body ref="ball">
+        <Translation z="0.05m"/>
+      </Body>
+    </Compound>
+
+    <Compound name="boards">
+      <Compound ref="boards"/>
+    </Compound>
+
+    <Compound name="field">
+      <Compound ref="field"/>
+    </Compound>
+  </Scene>
+</Simulation>
diff --git a/Config/Scenes/SharedAutonomyDefenders.con b/Config/Scenes/SharedAutonomyDefenders.con
@@ -0,0 +1,14 @@
+call Includes/Normal
+
+echo gc playing
+
+robot robot10
+dr module:BallChaserDecisionProvider:enable on
+robot robot11
+dr module:BallChaserDecisionProvider:enable on
+robot robot1
+rc defensive
+robot robot2
+rc defensive
+rc on
+call Includes/SharedAutonomy