Skip to content

Commit

Permalink
setting up inverse dynamics robot that onle control 5 dof, the grripe…
Browse files Browse the repository at this point in the history 
…r with be done with foward dynamicss

hopeful this make for an easier to training robot with deep learning,
but that for next week.
  • Loading branch information
@JulioJerez
JulioJerez committed Sep 22, 2024
1 parent bbeb725 commit 156a542
Show file tree
Hide file tree
Showing 14 changed files with 401 additions and 418 deletions.
Binary file modified newton-4.00/applications/media/robot.fbx
View file
Binary file not shown.
Binary file added newton-4.00/applications/media/robot_old.fbx
View file
Binary file not shown.
2 changes: 1 addition & 1 deletion newton-4.00/applications/ndSandbox/demos/ndAdvancedIndustrialRobot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1277,7 +1277,7 @@ void ndAdvancedIndustrialRobot(ndDemoEntityManager* const scene)

ndVector origin1(0.0f, 0.0f, 0.0f, 1.0f);
ndMeshLoader loader;
ndSharedPtr<ndDemoEntity> modelMesh(loader.LoadEntity("robot.fbx", scene));
ndSharedPtr<ndDemoEntity> modelMesh(loader.LoadEntity("robot_old.fbx", scene));
ndMatrix matrix(ndYawMatrix(-ndPi * 0.5f));

#ifdef ND_TRAIN_MODEL
Expand Down
129 changes: 66 additions & 63 deletions newton-4.00/applications/ndSandbox/demos/ndQuadrupedTest_2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -863,31 +863,33 @@ namespace ndQuadruped_2
//#pragma optimize( "", off )
bool CalculateHolonomicReward() const
{
bool isHolonomic = true;
const ndModelArticulation* const model = GetModel()->GetAsModelArticulation();
for (ndModelArticulation::ndNode* node = model->GetRoot()->GetFirstIterator(); isHolonomic && node; node = node->GetNextIterator())
{
const ndVector veloc(node->m_body->GetVelocity());
const ndVector omega(node->m_body->GetOmega());
ndFloat32 vMag2 = veloc.DotProduct(veloc).GetScalar();
ndFloat32 wMag2 = omega.DotProduct(omega).GetScalar();
isHolonomic = isHolonomic && (vMag2 < 100.0f);
isHolonomic = isHolonomic && (wMag2 < 400.0f);
}
if (!isHolonomic)
{
// catastrophic penalty, results in a immediate kill.
ndMemSet(m_controllerTrainer->m_rewardsMemories, 0.0f, sizeof(m_controllerTrainer->m_rewardsMemories) / sizeof(m_controllerTrainer->m_rewardsMemories[0]));
}

for (ndInt32 i = 0; isHolonomic && (i < m_animPose.GetCount()); ++i)
{
const ndEffectorInfo* const info = &m_effectorsInfo[i];
const ndIkSwivelPositionEffector* const effector = (ndIkSwivelPositionEffector*)*info->m_effector;
bool holonomic = effector->IsHolonomic(m_timestep);
isHolonomic = isHolonomic && holonomic;
}
return isHolonomic;
ndAssert(0);
return false;
//bool isHolonomic = true;
//const ndModelArticulation* const model = GetModel()->GetAsModelArticulation();
//for (ndModelArticulation::ndNode* node = model->GetRoot()->GetFirstIterator(); isHolonomic && node; node = node->GetNextIterator())
//{
// const ndVector veloc(node->m_body->GetVelocity());
// const ndVector omega(node->m_body->GetOmega());
// ndFloat32 vMag2 = veloc.DotProduct(veloc).GetScalar();
// ndFloat32 wMag2 = omega.DotProduct(omega).GetScalar();
// isHolonomic = isHolonomic && (vMag2 < 100.0f);
// isHolonomic = isHolonomic && (wMag2 < 400.0f);
//}
//if (!isHolonomic)
//{
// // catastrophic penalty, results in a immediate kill.
// ndMemSet(m_controllerTrainer->m_rewardsMemories, 0.0f, sizeof(m_controllerTrainer->m_rewardsMemories) / sizeof(m_controllerTrainer->m_rewardsMemories[0]));
//}
//
//for (ndInt32 i = 0; isHolonomic && (i < m_animPose.GetCount()); ++i)
//{
// const ndEffectorInfo* const info = &m_effectorsInfo[i];
// const ndIkSwivelPositionEffector* const effector = (ndIkSwivelPositionEffector*)*info->m_effector;
// bool holonomic = effector->IsHolonomic(m_timestep);
// isHolonomic = isHolonomic && holonomic;
//}
//return isHolonomic;
}

ndReal GetReward() const
Expand Down Expand Up @@ -951,45 +953,46 @@ namespace ndQuadruped_2

void GetObservation(ndBrainFloat* const inputObservations)
{
ndAssert(0);
ndMemSet(inputObservations, 0.0f, ND_AGENT_INPUT_SIZE);
ndObservationVector& observation = *((ndObservationVector*)inputObservations);
for (ndInt32 i = 0; i < m_animPose.GetCount(); ++i)
{
const ndAnimKeyframe& keyFrame = m_animPose[i];
const ndEffectorInfo* const info = (ndEffectorInfo*)keyFrame.m_userData;

ndInt32 paramCount = 0;
ndJointBilateralConstraint::ndKinematicState kinematicState[16];
paramCount += info->m_thigh->GetKinematicState(&kinematicState[paramCount]);
paramCount += info->m_calf->GetKinematicState(&kinematicState[paramCount]);
paramCount += info->m_foot->GetKinematicState(&kinematicState[paramCount]);
for (ndInt32 j = 0; j < paramCount; ++j)
{
observation.m_legs[i].m_state[j * 2 + 0] = ndBrainFloat(kinematicState[j].m_posit);
observation.m_legs[i].m_state[j * 2 + 1] = ndBrainFloat(kinematicState[j].m_velocity);
}

const ndVector& posit = keyFrame.m_posit;
observation.m_legs[i].m_posit.m_x = ndReal (posit.m_x);
observation.m_legs[i].m_posit.m_y = ndReal (posit.m_y);
observation.m_legs[i].m_posit.m_z = ndReal (posit.m_z);

observation.m_legs[i].m_prevPosit.m_x = ndReal (m_animPrevPose[i].m_x);
observation.m_legs[i].m_prevPosit.m_y = ndReal (m_animPrevPose[i].m_y);
observation.m_legs[i].m_prevPosit.m_z = ndReal (m_animPrevPose[i].m_z);

m_animPrevPose[i] = posit;

ndBrainFloat contactDist = FindContactDist(i);
bool isHolonomic = info->m_effector->IsHolonomic(m_timestep);
observation.m_legs[i].m_hasContact = contactDist;
observation.m_legs[i].m_isHolonomic = ndBrainFloat(isHolonomic ? 0.0f : 1.0f);
observation.m_legs[i].m_animSequence = ndBrainFloat(keyFrame.m_userParamFloat);
}

observation.m_torso_x = m_control->m_x;
observation.m_torso_z = m_control->m_z;
observation.m_animSpeed = m_control->m_animSpeed;
//ndObservationVector& observation = *((ndObservationVector*)inputObservations);
//for (ndInt32 i = 0; i < m_animPose.GetCount(); ++i)
//{
// const ndAnimKeyframe& keyFrame = m_animPose[i];
// const ndEffectorInfo* const info = (ndEffectorInfo*)keyFrame.m_userData;
//
// ndInt32 paramCount = 0;
// ndJointBilateralConstraint::ndKinematicState kinematicState[16];
// paramCount += info->m_thigh->GetKinematicState(&kinematicState[paramCount]);
// paramCount += info->m_calf->GetKinematicState(&kinematicState[paramCount]);
// paramCount += info->m_foot->GetKinematicState(&kinematicState[paramCount]);
// for (ndInt32 j = 0; j < paramCount; ++j)
// {
// observation.m_legs[i].m_state[j * 2 + 0] = ndBrainFloat(kinematicState[j].m_posit);
// observation.m_legs[i].m_state[j * 2 + 1] = ndBrainFloat(kinematicState[j].m_velocity);
// }
//
// const ndVector& posit = keyFrame.m_posit;
// observation.m_legs[i].m_posit.m_x = ndReal (posit.m_x);
// observation.m_legs[i].m_posit.m_y = ndReal (posit.m_y);
// observation.m_legs[i].m_posit.m_z = ndReal (posit.m_z);
//
// observation.m_legs[i].m_prevPosit.m_x = ndReal (m_animPrevPose[i].m_x);
// observation.m_legs[i].m_prevPosit.m_y = ndReal (m_animPrevPose[i].m_y);
// observation.m_legs[i].m_prevPosit.m_z = ndReal (m_animPrevPose[i].m_z);
//
// m_animPrevPose[i] = posit;
//
// ndBrainFloat contactDist = FindContactDist(i);
// bool isHolonomic = info->m_effector->IsHolonomic(m_timestep);
// observation.m_legs[i].m_hasContact = contactDist;
// observation.m_legs[i].m_isHolonomic = ndBrainFloat(isHolonomic ? 0.0f : 1.0f);
// observation.m_legs[i].m_animSequence = ndBrainFloat(keyFrame.m_userParamFloat);
//}
//
//observation.m_torso_x = m_control->m_x;
//observation.m_torso_z = m_control->m_z;
//observation.m_animSpeed = m_control->m_animSpeed;
}

void ApplyActions(const ndBrainFloat* const actions)
Expand Down
Loading

0 comments on commit 156a542

Please sign in to comment.