Merge branch 'master' of https://github.com/francescoriccio/Matilde

francescoriccio · Jan 28, 2014 · 2f74804 · 2f74804
2 parents 36a3532 + ace83da
commit 2f74804
Show file tree

Hide file tree

Showing 33 changed files with 2,992 additions and 496 deletions.
diff --git a/machineLearning/GoalieLearner.h b/machineLearning/GoalieLearner.h
@@ -4,6 +4,14 @@
 // Uncomment if you want to have debug information
 #define GOALIE_DEBUG_MODE
 
+#define DIVE_TYPE(x) \
+    if(x == 1) std::cerr << "\033[22;34;1m"<<"\t[Goalie] diveType: none"<<"\033[0m" << std::endl; \
+    else if(x == 2) std::cerr << "\033[22;34;1m"<<"\t[Goalie] diveType: long dive left"<<"\033[0m" << std::endl; \
+    else if(x == 3) std::cerr << "\033[22;34;1m"<<"\t[Goalie] diveType: long dive right"<<"\033[0m" << std::endl; \
+    else if(x == 4) std::cerr << "\033[22;34;1m"<<"\t[Goalie] diveType: close dive left"<<"\033[0m" << std::endl; \
+    else if(x == 5) std::cerr << "\033[22;34;1m"<<"\t[Goalie] diveType: close dive right"<<"\033[0m" << std::endl; \
+    else if(x == 6) std::cerr << "\033[22;34;1m"<<"\t[Goalie] diveType: stop ball"<<"\033[0m" << std::endl; \
+
 option GoalieLearner
 {
 private:
@@ -152,7 +160,7 @@ option GoalieLearner
                             if ((Timestamp() - lastTimeRoleSent).getMs() > 1000.0)
                             {
                                 cerr << "\033[22;34;1m\t[Goalie] DiveHandle::ballProjectionEstimate: " << theDiveHandle.ballProjectionEstimate << " \033[0m" << endl;
-                                cerr << "\033[22;34;1m\t[Goalie] DiveHandle::diveType: " << theDiveHandle.diveType << " \033[0m" << endl;
+                                DIVE_TYPE( (int)theDiveHandle.diveType );
                                 cerr << "\033[0;32;1m\t[Goalie] DiveHandle::diveTimer: " << theDiveHandle.diveTime << " \033[0m" << endl;
                             }
 #endif

diff --git a/naoqi/modules/R2module/CMakeLists.txt b/naoqi/modules/R2module/CMakeLists.txt
@@ -19,12 +19,14 @@ set(_srcs
   ${include_path}/libmath/Rutils.h
   ${include_path}/task.h
   ${include_path}/r2Module.h
+  ${include_path}/configParams.h
 
   ${src_path}/configReader.cpp
   ${src_path}/libmath/transform.cpp
   ${src_path}/libmath/kinChain.cpp
   ${src_path}/libmath/Rutils.cpp
   ${src_path}/task.cpp
+  ${src_path}/taskManager.cpp
   ${src_path}/r2ModuleMain.cpp
 
   ${src_path}/r2Module.cpp

diff --git a/naoqi/modules/R2module/config/CoM_head.cfg b/naoqi/modules/R2module/config/CoM_head.cfg
@@ -0,0 +1,4 @@
+[ T ] = 
+{
+T 0 0 126.5
+}
diff --git a/naoqi/modules/R2module/config/CoM_leftArm.cfg b/naoqi/modules/R2module/config/CoM_leftArm.cfg
@@ -0,0 +1,5 @@
+[ T ] = 
+{
+T 0 98 100
+R 1 0 0 -M_PI/2
+}
diff --git a/naoqi/modules/R2module/config/CoM_leftLeg.cfg b/naoqi/modules/R2module/config/CoM_leftLeg.cfg
@@ -0,0 +1,9 @@
+[ T ] = 
+{
+T 0 50 -85
+R 1 0 0 M_PI/4
+R 1 0 0 M_PI
+}
+H 0 0 0 q1 : LHipYawPitch
+R 1 0 0 -M_PI/4
+R 0 1 0 -M_PI/2
diff --git a/naoqi/modules/R2module/config/CoM_rightArm.cfg b/naoqi/modules/R2module/config/CoM_rightArm.cfg
@@ -0,0 +1,5 @@
+[ T ] = 
+{
+T 0 -98 100
+R 1 0 0 -M_PI/2
+}
diff --git a/naoqi/modules/R2module/config/CoM_rightLeg.cfg b/naoqi/modules/R2module/config/CoM_rightLeg.cfg
@@ -0,0 +1,9 @@
+[ T ] = 
+{
+T 0 -50 -85
+R 1 0 0 -M_PI/4
+}
+H 0 0 0 q1 : RHipYawPitch
+R 1 0 0 -M_PI/4
+R 1 0 0 -M_PI/2
+R 0 1 0 M_PI/2
diff --git a/naoqi/modules/R2module/config/dh_LAbase.cfg b/naoqi/modules/R2module/config/dh_LAbase.cfg
@@ -0,0 +1,8 @@
+[ chain ] = 
+{
+: LWristYaw
+: LElbowRoll
+: LElbowYaw
+: LShoulderRoll
+: LShoulderPitch
+}
diff --git a/naoqi/modules/R2module/config/dh_LLbase.cfg b/naoqi/modules/R2module/config/dh_LLbase.cfg
@@ -0,0 +1,17 @@
+[ chain ] = 
+{
+H 0 M_PI/2 0 q1 : LAnkleRoll
+H 102.9 0 0 q2 : LAnklePitch
+H 100 0 0 q3 : LKneePitch
+H 0 -M_PI/2 0 q4 : LHipPitch
+H 0 0 0 q5 : LHipRoll
+R 1 0 0 M_PI/2
+R 0 1 0 M_PI/4
+H 0 0 0 q6 : LHipYawPitch
+R 0 1 0 M_PI/4
+R 0 0 1 M_PI/2
+T 0 -50 85
+}
+
+
+
diff --git a/naoqi/modules/R2module/config/dh_RAbase.cfg b/naoqi/modules/R2module/config/dh_RAbase.cfg
@@ -0,0 +1,8 @@
+[ chain ] = 
+{
+: RWristYaw
+: RElbowRoll
+: RElbowYaw
+: RShoulderRoll
+: RShoulderPitch
+}
diff --git a/naoqi/modules/R2module/config/dh_RLbase.cfg b/naoqi/modules/R2module/config/dh_RLbase.cfg
@@ -0,0 +1,15 @@
+[ chain ] = 
+{
+H 0 M_PI/2 0 q1 : RAnkleRoll
+H 102.9 0 0 q2 : RAnklePitch
+H 100 0 0 q3 : RKneePitch
+H 0 -M_PI/2 0 q4 : RHipPitch
+H 0 0 0 q5 : RHipRoll
+R 1 0 0 M_PI/2
+H 0 0 0 q6 : RHipYawPitch
+R 0 1 0 M_PI/4
+R 0 1 0 M_PI/4
+R 0 0 1 M_PI/2
+T 0 50 85
+}
+
diff --git a/naoqi/modules/R2module/config/dh_head.cfg b/naoqi/modules/R2module/config/dh_head.cfg
@@ -1,7 +1,6 @@
 [ chain ] = 
 {
-T 0 0 126.5
-H 0 -M_PI/2 0 q1
-H 0 M_PI/2 0 q2
-T 53.9 0 67.9
+H 0 -M_PI/2 0 q1 : HeadYaw
+H 0 M_PI/2 0 q2 : HeadPitch
 }
+
diff --git a/naoqi/modules/R2module/config/dh_leftArm.cfg b/naoqi/modules/R2module/config/dh_leftArm.cfg
@@ -1,18 +1,14 @@
 [ chain ] = 
 {
-T 0 98 100
-R 1 0 0 -M_PI/2
-H 0 0 0 q1
-R 1 0 0 M_PI/2
-H 0 0 0 q2
-T 105 15 0
+H 0 M_PI/2 0 q1 : LShoulderPitch
+H 0 0 0 q2 : LShoulderRoll
 R 0 0 1 -M_PI/2
 R 1 0 0 -M_PI/2
-H 0 0 0 q3
+H -15 M_PI/2 105 q3 : LElbowYaw
 R 0 0 1 M_PI/2
-R 0 1 0 -M_PI/2
-H 0 0 0 q4
+H 0 0 0 q4 : LElbowRoll
 T 113.7 0 12.31
-R 0 1 0 M_PI/2
-H 0 0 0 q5
+R 0 0 1 -M_PI/2
+R 1 0 0 -M_PI/2
+H 0 0 0 q5 : LWristYaw
 }
diff --git a/naoqi/modules/R2module/config/dh_leftLeg.cfg b/naoqi/modules/R2module/config/dh_leftLeg.cfg
@@ -1,18 +1,23 @@
 [ chain ] = 
 {
+H 0 0 0 q1 : LHipYawPitch
+R 1 0 0 -M_PI/4
 R 0 1 0 M_PI/2
-R 0 0 1 M_PI
-T 45.19 0 0 
-H 0 M_PI/2 0 q1
-H 102.9 0 0 q2
-H 100 -M_PI/2 0 q3
-H 0 M_PI/2 0 q4
-H 0 0 0 q5
-R 0 1 0 -M_PI/4
-H 0 0 0 q6
-R 0 1 0 M_PI/4
+
+H 0 0 0 q1 : LHipRoll
+R 1 0 0 M_PI/2
+R 0 0 1 M_PI/2
+H 0 0 0 q2 : LHipPitch
+T 0 100 0
+H 0 0 0 q3 : LKneePitch
+T 0 102.9 0
+H 0 0 0 q4 : LAnklePitch
+R 1 0 0 M_PI/2
 R 0 0 1 M_PI/2
-T 0 0 -50
 R 1 0 0 M_PI/2
-T 0 0 85
+H 0 0 0 q5 : LAnkleRoll
+T 0 -45.19 0
+R 0 0 1 M_PI/2
 }
+
+
diff --git a/naoqi/modules/R2module/config/dh_rightArm.cfg b/naoqi/modules/R2module/config/dh_rightArm.cfg
@@ -1,11 +1,14 @@
 [ chain ] = 
 {
-T 0 -98 100 100
-H 0 -M_PI/2 0 q1
-H 105 M_PI/2 -15 q2
+H 0 M_PI/2 0 q1 : RShoulderPitch
+H 0 0 0 q2 : RShoulderRoll
 R 0 0 1 -M_PI/2
-H 0 -M_PI/2 0 q3
-T 0 -12.31 0
-H 0 0 0 q4
-H 0 0 113.7 q5
+R 1 0 0 -M_PI/2
+H 15 M_PI/2 105 q3 : RElbowYaw
+R 0 0 1 M_PI/2
+H 0 0 0 q4 : RElbowRoll
+T 113.7 0 12.31
+R 0 0 1 -M_PI/2
+R 1 0 0 -M_PI/2
+H 0 0 0 q5 : RWristYaw
 }
diff --git a/naoqi/modules/R2module/config/dh_rightLeg.cfg b/naoqi/modules/R2module/config/dh_rightLeg.cfg
@@ -1,14 +1,23 @@
 [ chain ] = 
 {
-T 0 -50 -85
+H 0 0 0 q1 : RHipYawPitch
 R 1 0 0 -M_PI/4
-H 0 0 0 q1
-R 1 0 0 -M_PI/4
-H 0 -M_PI/2 0 q2
-H -100 M_PI/2 0 q3
-H -102.9 0 0 q4
-H 0 -M_PI/2 0 q5
-H -45.19 0 0 q6
-R 0 0 1 -M_PI
-R 0 1 0 -M_PI/2
+R 1 0 0 -M_PI/2
+R 0 1 0 M_PI/2
+
+H 0 0 0 q1 : RHipRoll
+R 1 0 0 M_PI/2
+R 0 0 1 M_PI/2
+H 0 0 0 q2 : RHipPitch
+T 0 100 0
+H 0 0 0 q3 : RKneePitch
+T 0 102.9 0
+H 0 0 0 q4 : RAnklePitch
+R 1 0 0 M_PI/2
+R 0 0 1 M_PI/2
+R 1 0 0 M_PI/2
+H 0 0 0 q5 : RAnkleRoll
+T 0 -45.19 0
+R 0 0 1 M_PI/2
 }
+
diff --git a/naoqi/modules/R2module/config/joints_params.cfg b/naoqi/modules/R2module/config/joints_params.cfg
@@ -25,3 +25,4 @@
 0.0 -1.189516	0.922747 LAnklePitch
 0.0 -0.397880	0.769001 LAnkleRoll
 }
+
Original file line number	Diff line number	Diff line change
Expand Up		@@ -25,3 +25,4 @@
		0.0 -1.189516 0.922747 LAnklePitch
		0.0 -0.397880 0.769001 LAnkleRoll
		}