changing the observation input, the angle is good for reward, but doe…

…s does not offer any groundingding
MADEAPPS · Sep 24, 2024 · a70b27b · a70b27b
1 parent e509736
commit a70b27b
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 8 deletions.
diff --git a/newton-4.00/applications/media/robot_old.fbx b/newton-4.00/applications/media/robot_old.fbx
diff --git a/newton-4.00/applications/ndSandbox/demos/ndAdvancedIndustrialRobot.cpp b/newton-4.00/applications/ndSandbox/demos/ndAdvancedIndustrialRobot.cpp
@@ -49,7 +49,9 @@ namespace ndAdvancedRobot
 		ndBrainFloat m_delta_x;
 		ndBrainFloat m_delta_z;
 		ndBrainFloat m_deltaAzimuth;
-		ndBrainFloat m_deltaRotation;
+		//ndBrainFloat m_deltaRotation;
+		ndBrainFloat m_sourcePin[3];
+		ndBrainFloat m_targetPin[3];
 	};
 
 	class ndControlParameters
@@ -579,6 +581,7 @@ namespace ndAdvancedRobot
 			ndFloat32 angleError = CalculateDeltaTargetRotation(currentEffectorMatrix);
 			ndFloat32 angularReward = rewardWeigh * GaussianReward((angleError + 1.0f) * 0.5f);
 			return angularReward + posit_xReward + posit_zReward + azimuthReward;
+			//return GaussianReward((angleError + 1.0f) * 0.5f);;
 		}
 
 		#pragma optimize( "", off )
@@ -608,8 +611,16 @@ namespace ndAdvancedRobot
 			observation->m_delta_z = ndBrainFloat(positError.m_z);
 			observation->m_deltaAzimuth = ndBrainFloat(positError.m_w);
 
-			ndFloat32 angleError(CalculateDeltaTargetRotation(currentEffectorMatrix));
-			observation->m_deltaRotation = ndBrainFloat(angleError);
+			//ndFloat32 angleError(CalculateDeltaTargetRotation(currentEffectorMatrix));
+			//observation->m_deltaRotation = ndBrainFloat(angleError);
+			observation->m_sourcePin[0] = ndBrainFloat(currentEffectorMatrix.m_front.m_x);
+			observation->m_sourcePin[1] = ndBrainFloat(currentEffectorMatrix.m_front.m_y);
+			observation->m_sourcePin[2] = ndBrainFloat(currentEffectorMatrix.m_front.m_z);
+
+			const ndMatrix targetMatrix(ndPitchMatrix(m_targetLocation.m_pitch) * ndYawMatrix(m_targetLocation.m_yaw) * ndRollMatrix(m_targetLocation.m_roll));
+			observation->m_targetPin[0] = ndBrainFloat(targetMatrix.m_front.m_x);
+			observation->m_targetPin[1] = ndBrainFloat(targetMatrix.m_front.m_y);
+			observation->m_targetPin[2] = ndBrainFloat(targetMatrix.m_front.m_z);
 		}
 
 		//#pragma optimize( "", off )
@@ -623,6 +634,9 @@ namespace ndAdvancedRobot
 				hinge->SetTargetAngle(targetAngle);
 			};
 
+			//SetParamter(m_arm_2, 2);
+			//SetParamter(m_arm_3, 3);
+
 			SetParamter(m_arm_0, 0);
 			SetParamter(m_arm_1, 1);
 			SetParamter(m_arm_2, 2);
@@ -696,7 +710,7 @@ namespace ndAdvancedRobot
 			ndFloat32 roll = ndFloat32(-ndPi * 0.35f + ndRand() * (ndPi * 0.9f - (-ndPi * 0.35f)));
 
 			//m_targetLocation.m_x = 0.0f;
-			//m_targetLocation.m_y = 0.0f;
+			//m_targetLocation.m_z = 0.0f;
 			//m_targetLocation.m_azimuth = 0.0f;
 			//yaw = 0.0f * ndDegreeToRad;
 			//roll = 0.0f * ndDegreeToRad;
@@ -1086,8 +1100,8 @@ namespace ndAdvancedRobot
 
 			ndInt32 countX = 22;
 			ndInt32 countZ = 23;
-			//countX = 1;
-			//countZ = 1;
+			//countX = 10;
+			//countZ = 11;
 
 			// add a hidden battery of model to generate trajectories in parallel
 			for (ndInt32 i = 0; i < countZ; ++i)

diff --git a/newton-4.00/applications/ndSandbox/ndDemoEntityManager.cpp b/newton-4.00/applications/ndSandbox/ndDemoEntityManager.cpp
@@ -57,8 +57,8 @@
 //#define DEFAULT_SCENE	17		// cart pole discrete controller
 //#define DEFAULT_SCENE	18		// cart pole continue controller
 //#define DEFAULT_SCENE	19		// unit cycle controller
-#define DEFAULT_SCENE	20		// simple industrial robot
-//#define DEFAULT_SCENE	21		// advanced industrial robot
+//#define DEFAULT_SCENE	20		// simple industrial robot
+#define DEFAULT_SCENE	21		// advanced industrial robot
 //#define DEFAULT_SCENE	22		// quadruped test 1
 //#define DEFAULT_SCENE	23		// quadruped test 2
 //#define DEFAULT_SCENE	24		// quadruped test 3