From ca9b45ca2a2cc79e066aea8e11e2b675dd3c0441 Mon Sep 17 00:00:00 2001 From: JulioJerez Date: Mon, 9 Sep 2024 16:48:51 -0700 Subject: [PATCH] adding debug checks --- .../demos/ndAdvancedIndustrialRobot.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/newton-4.00/applications/ndSandbox/demos/ndAdvancedIndustrialRobot.cpp b/newton-4.00/applications/ndSandbox/demos/ndAdvancedIndustrialRobot.cpp index d1337b7e3..4d9974e3b 100644 --- a/newton-4.00/applications/ndSandbox/demos/ndAdvancedIndustrialRobot.cpp +++ b/newton-4.00/applications/ndSandbox/demos/ndAdvancedIndustrialRobot.cpp @@ -502,7 +502,7 @@ namespace ndAdvancedRobot return false; } - //#pragma optimize( "", off ) + #pragma optimize( "", off ) ndReal GetReward() const { if (IsTerminal()) @@ -510,8 +510,13 @@ namespace ndAdvancedRobot return ND_DEAD_PENALTY; } - const ndMatrix invBaseMatrix(m_base_rotator->CalculateGlobalMatrix1().OrthoInverse()); - const ndMatrix effectorMatrix(m_effectorMatrixOffset * m_arm_4->CalculateGlobalMatrix0() * invBaseMatrix); + const ndMatrix globalBaseMatrix(m_base_rotator->CalculateGlobalMatrix1()); + const ndMatrix globalEffectorMatrix(m_effectorMatrixOffset * m_arm_4->CalculateGlobalMatrix0()); + const ndMatrix globalTargetMatrix(CalculateTargetMatrix() * globalBaseMatrix); + + //const ndMatrix invBaseMatrix(m_base_rotator->CalculateGlobalMatrix1().OrthoInverse()); + //const ndMatrix effectorMatrix(m_effectorMatrixOffset * m_arm_4->CalculateGlobalMatrix0() * invBaseMatrix); + const ndMatrix effectorMatrix(m_effectorMatrixOffset * m_arm_4->CalculateGlobalMatrix0() * globalBaseMatrix.OrthoInverse()); ndFloat32 azimuth = 0.0f; const ndVector& posit = effectorMatrix.m_posit; @@ -526,9 +531,6 @@ namespace ndAdvancedRobot ndFloat32 dy = m_targetLocation.m_y - currenPosit.m_y; ndFloat32 dAzimuth = ndAnglesSub(m_targetLocation.m_azimuth, azimuth); - //ndFloat32 azimuth2 = dAzimuth * dAzimuth; - //ndFloat32 positError2 = dx * dx + dy * dy; - #ifdef ND_USE_EULERS ndVector euler1; ndVector euler(effectorMatrix.CalcPitchYawRoll(euler1)); @@ -543,7 +545,7 @@ namespace ndAdvancedRobot ndFloat32 rollReward = ndExp(-100.0f * deltaRoll * deltaRoll); ndFloat32 pitchReward = ndExp(-100.0f * deltaPitch * deltaPitch); - ndFloat32 rewardWeight = 1.0 / 6.0f; + const ndFloat32 rewardWeight = 1.0 / 6.0f; return rewardWeight * (posit_xReward + posit_yReward + azimuthReward + yawReward + rollReward + pitchReward); #else