Skip to content

Commit

Permalink
prioritize rewrads.
Browse files Browse the repository at this point in the history 
trying rewards only valid when the azimuth angle is small enough.
  • Loading branch information
@JulioJerez
JulioJerez committed Sep 18, 2024
1 parent 7c837ff commit f853016
Show file tree
Hide file tree
Showing 2 changed files with 23 additions and 15 deletions.
34 changes: 21 additions & 13 deletions newton-4.00/applications/ndSandbox/demos/ndAdvancedIndustrialRobot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ namespace ndAdvancedRobot
{
public:
//ndBrainFloat m_actions[6];
ndBrainFloat m_actions[4];
ndBrainFloat m_actions[5];
};

class ndObservationVector
Expand Down Expand Up @@ -110,7 +110,7 @@ namespace ndAdvancedRobot
static ndDefinition jointsDefinition[] =
{
{ "base", ndDefinition::m_root, 100.0f, 0.0f, 0.0f},
{ "base_rotator", ndDefinition::m_hinge, 50.0f, -1.0e10f, 1.0e10f},
{ "base_rotator", ndDefinition::m_hinge, 50.0f, -ndPi * 2.0f, ndPi * 2.0f},
{ "arm_0", ndDefinition::m_hinge , 5.0f, -120.0f * ndDegreeToRad, 60.0f * ndDegreeToRad},
{ "arm_1", ndDefinition::m_hinge , 5.0f, -90.0f * ndDegreeToRad, 60.0f * ndDegreeToRad},
{ "arm_2", ndDefinition::m_hinge , 5.0f, -1.0e10f, 1.0e10f},
Expand Down Expand Up @@ -556,8 +556,14 @@ namespace ndAdvancedRobot
ndFloat32 omega_xReward = ndExp(-invSigma2 * rotationError2.m_x);
ndFloat32 omega_yReward = ndExp(-invSigma2 * rotationError2.m_y);
ndFloat32 omega_zReward = ndExp(-invSigma2 * rotationError2.m_z);

return (omega_xReward + omega_yReward + omega_zReward + posit_xReward + posit_yReward + azimuthReward) / 6.0f;
if (azimuthReward > 1.0e-5f)
{
return (omega_xReward + omega_yReward + omega_zReward + posit_xReward + posit_yReward + azimuthReward) / 6.0f;
}
else
{
return azimuthReward / 6.0f;
}
}

#pragma optimize( "", off )
Expand Down Expand Up @@ -614,8 +620,10 @@ namespace ndAdvancedRobot

SetParamter(m_arm_0, 0);
SetParamter(m_arm_1, 1);
SetParamter(m_arm_3, 2);
SetParamter(m_base_rotator, 3);
SetParamter(m_arm_2, 2);
SetParamter(m_arm_3, 3);
//SetParamter(m_arm_4, 3);
SetParamter(m_base_rotator, 4);
}

void CheckModelStability()
Expand Down Expand Up @@ -677,7 +685,7 @@ namespace ndAdvancedRobot
m_targetLocation.m_y = ndClamp(m_targetLocation.m_y, ndReal(ND_MIN_Y_SPAND + 0.05f), ndReal(ND_MAX_Y_SPAND - 0.05f));
m_targetLocation.m_azimuth = ndClamp(m_targetLocation.m_azimuth, ndReal(-ndPi + 0.09f), ndReal(ndPi - 0.09f));

ndFloat32 yaw = ndFloat32((1.0f * ndRand() - 0.5f) * ndPi);
ndFloat32 yaw = ndFloat32((2.0f * ndRand() - 1.0f) * ndPi);
ndFloat32 pitch = ndFloat32((2.0f * ndRand() - 1.0f) * ndPi);
ndFloat32 roll = ndFloat32(-ndPi * 0.35f + ndRand() * (ndPi * 0.9f - (-ndPi * 0.35f)));

Expand All @@ -687,7 +695,7 @@ namespace ndAdvancedRobot

yaw = 0.0f * ndDegreeToRad;
//roll = 0.0f * ndDegreeToRad;
pitch = 0.0f * ndDegreeToRad;
//pitch = 0.0f * ndDegreeToRad;

m_targetLocation.m_yaw = yaw;
m_targetLocation.m_roll = roll;
Expand Down Expand Up @@ -812,7 +820,7 @@ namespace ndAdvancedRobot
change = change | ndInt8(ImGui::SliderFloat("y", &m_robot->m_targetLocation.m_y, ND_MIN_X_SPAND, ND_MAX_X_SPAND));
change = change | ndInt8 (ImGui::SliderFloat("azimuth", &m_robot->m_targetLocation.m_azimuth, -ndPi, ndPi));
change = change | ndInt8(ImGui::SliderFloat("pitch", &pitch, -ndPi, ndPi));
change = change | ndInt8(ImGui::SliderFloat("yaw", &yaw, -ndPi * 0.5f, ndPi * 0.5f));
change = change | ndInt8(ImGui::SliderFloat("yaw", &yaw, -ndPi, ndPi));
change = change | ndInt8(ImGui::SliderFloat("roll", &roll, -ndPi * 0.35f, ndPi * 0.9f));
change = change | ndInt8 (ImGui::SliderFloat("gripper", &m_robot->m_targetLocation.m_gripperPosit, -0.2f, 0.4f));

Expand All @@ -821,7 +829,7 @@ namespace ndAdvancedRobot
{
change = 1;
pitch = ndReal((2.0f * ndRand() - 1.0f) * ndPi);
yaw = ndReal((2.0f * ndRand() - 1.0f) * ndPi * 0.5f);
yaw = ndReal((2.0f * ndRand() - 1.0f) * ndPi);
roll = ndReal(-ndPi * 0.35f + ndRand() * (ndPi * 0.9f - (-ndPi * 0.35f)));

m_robot->m_targetLocation.m_azimuth = ndReal((2.0f * ndRand() - 1.0f) * ndPi);
Expand All @@ -830,7 +838,7 @@ namespace ndAdvancedRobot

yaw = 0.0f;
//roll = 0.0f;
pitch = 0.0f;
//pitch = 0.0f;
//m_robot->m_targetLocation.m_x = 0.0f;
//m_robot->m_targetLocation.m_y = 0.0f;
//m_robot->m_targetLocation.m_azimuth = ndReal((2.0f * ndRand() - 1.0f) * ndPi);
Expand Down Expand Up @@ -1076,8 +1084,8 @@ namespace ndAdvancedRobot

ndInt32 countX = 22;
ndInt32 countZ = 23;
//countX = 1;
//countZ = 1;
countX = 10;
countZ = 10;

// add a hidden battery of model to generate trajectories in parallel
for (ndInt32 i = 0; i < countZ; ++i)
Expand Down
4 changes: 2 additions & 2 deletions newton-4.00/applications/ndSandbox/demos/ndSimpleIndustrialRobot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -325,7 +325,7 @@ namespace ndSimpleRobot
change = change | ndInt8(ImGui::SliderFloat("y", &m_robot->m_y, ND_MIN_Y_SPAND, ND_MAX_Y_SPAND));
change = change | ndInt8(ImGui::SliderFloat("azimuth", &m_robot->m_azimuth, -ndPi, ndPi));
change = change | ndInt8(ImGui::SliderFloat("pitch", &m_robot->m_pitch, -ndPi, ndPi));
change = change | ndInt8(ImGui::SliderFloat("yaw", &m_robot->m_yaw, -ndPi * 0.5f, ndPi * 0.5f));
change = change | ndInt8(ImGui::SliderFloat("yaw", &m_robot->m_yaw, -ndPi, ndPi));
change = change | ndInt8(ImGui::SliderFloat("roll", &m_robot->m_roll, -ndPi * 0.35f, ndPi * 0.9f));
change = change | ndInt8(ImGui::SliderFloat("gripper", &m_robot->m_gripperPosit, -0.2f, 0.4f));
bool newTarget = ndInt8(ImGui::Button("random target"));
Expand All @@ -334,7 +334,7 @@ namespace ndSimpleRobot
change = 1;

m_robot->m_pitch = ndReal((2.0f * ndRand() - 1.0f) * ndPi);
m_robot->m_yaw = ndReal((2.0f * ndRand() - 1.0f) * ndPi * 0.5f);
m_robot->m_yaw = ndReal((2.0f * ndRand() - 1.0f) * ndPi);
m_robot->m_roll = ndReal(-ndPi * 0.35f + ndRand() * (ndPi * 0.9f - (-ndPi * 0.35f)));

m_robot->m_azimuth = ndReal((2.0f * ndRand() - 1.0f) * ndPi);
Expand Down

0 comments on commit f853016

Please sign in to comment.