<commit_before>#include <cxxtest/TestDrive.h>

#include <openma/body/inversedynamicsmatrix.h>
#include <openma/body/model.h>
#include <openma/body/segment.h>
#include <openma/body/joint.h>
#include <openma/body/inertialparameters.h>
#include <openma/body/enums.h>
#include <openma/body/anchor.h>
#include <openma/body/chain.h>
#include <openma/base/timesequence.h>

#include <openma/math.h>

void dyninv_compare_data(ma::body::Model* model, const std::string& jntname, unsigned samples, const double* frc, const double* trq)
{
  auto force = model->joints()->findChild<const ma::TimeSequence*>(jntname+".Force",{{"type",ma::TimeSequence::Force},{"components",4}});
  auto moment = model->joints()->findChild<const ma::TimeSequence*>(jntname+".Moment",{{"type",ma::TimeSequence::Moment},{"components",4}});
  TS_ASSERT_DIFFERS(force, nullptr);
  TS_ASSERT_EQUALS(force->samples(), samples);
  TS_ASSERT_DIFFERS(moment, nullptr);
  TS_ASSERT_EQUALS(moment->samples(), samples);
  const double deltaF = 1e-5;
  const double deltaM = 1e-5;
  for (unsigned i = 0 ; i < samples ; ++i)
  {
    TSM_ASSERT_DELTA(force->name()+".X #"+std::to_string(i), force->data()[i]            , frc[i]           , deltaF);
    TSM_ASSERT_DELTA(force->name()+".Y #"+std::to_string(i), force->data()[i +   samples], frc[ +   samples], deltaF);
    TSM_ASSERT_DELTA(force->name()+".Z #"+std::to_string(i), force->data()[i + 2*samples], frc[ + 2*samples], deltaF);
    TSM_ASSERT_DELTA(moment->name()+".X #"+std::to_string(i), moment->data()[i]            , trq[i]           , deltaM);
    TSM_ASSERT_DELTA(moment->name()+".Y #"+std::to_string(i), moment->data()[i +   samples], trq[ +   samples], deltaM);
    TSM_ASSERT_DELTA(moment->name()+".Z #"+std::to_string(i), moment->data()[i + 2*samples], trq[ + 2*samples], deltaM);    
  }
}

CXXTEST_SUITE(InverseDynamicMatrixTest)
{
  CXXTEST_TEST(gait)
  {
    ma::Node root("Root");
    ma::body::Model model("M1",&root);
    double g[3] = {0., -9810., 0.};
    model.setGravity(g); // mm.s^-2
    
    auto segments = model.segments();
    auto joints = model.joints();
    auto chains = model.chains();
    
    ma::body::Segment foot("R.Foot", ma::body::Part::Foot, ma::body::Side::Right, segments);
    ma::body::Segment shank("R.Shank", ma::body::Part::Shank, ma::body::Side::Right, segments);
    ma::body::Segment thigh("R.Thigh", ma::body::Part::Thigh, ma::body::Side::Right, segments);
    ma::body::Segment pelvis("Pelvis", ma::body::Part::Pelvis, ma::body::Side::Center, segments);
    
    ma::body::Joint ankle("R.Ankle", &shank, ma::body::Anchor::origin(&foot), &foot, joints);
    ma::body::Joint knee("R.Knee", &thigh, ma::body::Anchor::origin(&shank), &shank, joints);
    ma::body::Joint hip("R.Hip", &pelvis, ma::body::Anchor::origin(&thigh), &thigh, joints);
    
    ma::body::Chain lowerlimb("R.LowerLimb", {{&hip, &knee, &ankle}}, chains);
    
    double relcom[4][3] = {
      { 54.0526,   -21.3663 ,   3.6790 }, // foot
      {-22.1367,  -189.084  ,   3.2283 }, // shank
      {-18.622 ,  -194.846  ,  14.988  }, // thigh
      {  2.5438,   -25.43787,  -0.54510}  // pelvis
    };
    
    double relinertia[4][9] = {
      {6.24925470579212e+02, 0., 0., 0., 2.96028709073682e+03, 0., 0., 0., 2.80243394418913e+03},
      {7.20347164309006e+04, 0., 0., 0., 9.18810158557406e+03, 0., 0., 0., 7.20347164309006e+04},
      {1.92048657677671e+05, 0., 0., 0., 5.13804375475340e+04, 0., 0., 0., 2.05521750190136e+05},
      {1.07601694913786e+05, 0., 0., 0., 1.18519031864651e+05, 0., 0., 0., 9.51970685812099e+04}
    };
    
    new ma::body::InertialParameters("R.Foot.BSIP",   1.08, relcom[0], relinertia[0], &foot);
    new ma::body::InertialParameters("R.Shank.BSIP",  4.32, relcom[1], relinertia[1], &shank);
    new ma::body::InertialParameters("R.Thigh.BSIP", 11.07, relcom[2], relinertia[2], &thigh);
    new ma::body::InertialParameters("Pelvis.BSIP",  12.78, relcom[3], relinertia[3], &pelvis);
    
    const unsigned numeltsscs = 130;
    auto tss = ma::make_nodes<ma::TimeSequence*>(4, 13, 10, 100.0, 0.0, ma::TimeSequence::Pose, "");
    
    // Data for the foot
    tss[0]->setName("R.Foot.SCS");
    tss[0]->addParent(&foot);
    const double footscsdata[numeltsscs] = {
      0.90926, 0.9095, 0.90888, 0.9074, 0.90533, 0.90319, 0.90164, 0.90107, 0.90141, 0.9023,
      0.14403, 0.12366, 0.09823, 0.07004, 0.0417, 0.01558, -0.00674, -0.0246, -0.03807, -0.04769,
      0.39051, 0.39689, 0.40533, 0.41438, 0.42266, 0.42895, 0.43242, 0.43299, 0.43129, 0.42846,
      -0.12315, -0.09854, -0.06712, -0.03158, 0.00472, 0.03852, 0.06753, 0.09074, 0.10827, 0.12089,
      0.98931, 0.99165, 0.99365, 0.99459, 0.99412, 0.99237, 0.98981, 0.98699, 0.9843, 0.98198,
      -0.07815, -0.08316, -0.09031, -0.09895, -0.10819, -0.11716, -0.12538, -0.13276, -0.13941, -0.14528,
      -0.39759, -0.40386, -0.41163, -0.41907, -0.42469, -0.4275, -0.42717, -0.42408, -0.41922, -0.41381,
      0.02297, 0.03653, 0.05487, 0.07671, 0.09994, 0.12234, 0.14225, 0.15891, 0.17236, 0.18288,
      0.91728, 0.91409, 0.9097, 0.90471, 0.89981, 0.8957, 0.89291, 0.89157, 0.89138, 0.8918,
      826.32837, 832.89762, 838.19049, 842.49298, 846.065, 849.08362, 851.62637, 853.71509, 855.38028, 856.69122,
      81.19634, 79.14663, 77.07691, 75.17602, 73.58755, 72.3748, 71.52374, 70.97495, 70.65585, 70.49444,<commit_msg>Memory leak in the unit test as the generated nodes (allocated on the heap) have no parent.<commit_after>#include <cxxtest/TestDrive.h>

#include <openma/body/inversedynamicsmatrix.h>
#include <openma/body/model.h>
#include <openma/body/segment.h>
#include <openma/body/joint.h>
#include <openma/body/inertialparameters.h>
#include <openma/body/enums.h>
#include <openma/body/anchor.h>
#include <openma/body/chain.h>
#include <openma/base/timesequence.h>

#include <openma/math.h>

void dyninv_compare_data(ma::body::Model* model, const std::string& jntname, unsigned samples, const double* frc, const double* trq)
{
  auto force = model->joints()->findChild<const ma::TimeSequence*>(jntname+".Force",{{"type",ma::TimeSequence::Force},{"components",4}});
  auto moment = model->joints()->findChild<const ma::TimeSequence*>(jntname+".Moment",{{"type",ma::TimeSequence::Moment},{"components",4}});
  TS_ASSERT_DIFFERS(force, nullptr);
  TS_ASSERT_EQUALS(force->samples(), samples);
  TS_ASSERT_DIFFERS(moment, nullptr);
  TS_ASSERT_EQUALS(moment->samples(), samples);
  const double deltaF = 1e-5;
  const double deltaM = 1e-5;
  for (unsigned i = 0 ; i < samples ; ++i)
  {
    TSM_ASSERT_DELTA(force->name()+".X #"+std::to_string(i), force->data()[i]            , frc[i]           , deltaF);
    TSM_ASSERT_DELTA(force->name()+".Y #"+std::to_string(i), force->data()[i +   samples], frc[ +   samples], deltaF);
    TSM_ASSERT_DELTA(force->name()+".Z #"+std::to_string(i), force->data()[i + 2*samples], frc[ + 2*samples], deltaF);
    TSM_ASSERT_DELTA(moment->name()+".X #"+std::to_string(i), moment->data()[i]            , trq[i]           , deltaM);
    TSM_ASSERT_DELTA(moment->name()+".Y #"+std::to_string(i), moment->data()[i +   samples], trq[ +   samples], deltaM);
    TSM_ASSERT_DELTA(moment->name()+".Z #"+std::to_string(i), moment->data()[i + 2*samples], trq[ + 2*samples], deltaM);    
  }
}

CXXTEST_SUITE(InverseDynamicMatrixTest)
{
  CXXTEST_TEST(gait)
  {
    ma::Node root("Root");
    ma::body::Model model("M1",&root);
    double g[3] = {0., -9810., 0.};
    model.setGravity(g); // mm.s^-2
    
    auto segments = model.segments();
    auto joints = model.joints();
    auto chains = model.chains();
    
    ma::body::Segment foot("R.Foot", ma::body::Part::Foot, ma::body::Side::Right, segments);
    ma::body::Segment shank("R.Shank", ma::body::Part::Shank, ma::body::Side::Right, segments);
    ma::body::Segment thigh("R.Thigh", ma::body::Part::Thigh, ma::body::Side::Right, segments);
    ma::body::Segment pelvis("Pelvis", ma::body::Part::Pelvis, ma::body::Side::Center, segments);
    
    ma::body::Joint ankle("R.Ankle", &shank, ma::body::Anchor::origin(&foot), &foot, joints);
    ma::body::Joint knee("R.Knee", &thigh, ma::body::Anchor::origin(&shank), &shank, joints);
    ma::body::Joint hip("R.Hip", &pelvis, ma::body::Anchor::origin(&thigh), &thigh, joints);
    
    ma::body::Chain lowerlimb("R.LowerLimb", {{&hip, &knee, &ankle}}, chains);
    
    double relcom[4][3] = {
      { 54.0526,   -21.3663 ,   3.6790 }, // foot
      {-22.1367,  -189.084  ,   3.2283 }, // shank
      {-18.622 ,  -194.846  ,  14.988  }, // thigh
      {  2.5438,   -25.43787,  -0.54510}  // pelvis
    };
    
    double relinertia[4][9] = {
      {6.24925470579212e+02, 0., 0., 0., 2.96028709073682e+03, 0., 0., 0., 2.80243394418913e+03},
      {7.20347164309006e+04, 0., 0., 0., 9.18810158557406e+03, 0., 0., 0., 7.20347164309006e+04},
      {1.92048657677671e+05, 0., 0., 0., 5.13804375475340e+04, 0., 0., 0., 2.05521750190136e+05},
      {1.07601694913786e+05, 0., 0., 0., 1.18519031864651e+05, 0., 0., 0., 9.51970685812099e+04}
    };
    
    new ma::body::InertialParameters("R.Foot.BSIP",   1.08, relcom[0], relinertia[0], &foot);
    new ma::body::InertialParameters("R.Shank.BSIP",  4.32, relcom[1], relinertia[1], &shank);
    new ma::body::InertialParameters("R.Thigh.BSIP", 11.07, relcom[2], relinertia[2], &thigh);
    new ma::body::InertialParameters("Pelvis.BSIP",  12.78, relcom[3], relinertia[3], &pelvis);
    
    ma::Node tmp("tmp");
    const unsigned numeltsscs = 130;
    auto tss = ma::make_nodes<ma::TimeSequence*>(4, 13, 10, 100.0, 0.0, ma::TimeSequence::Pose, "", &tmp);
    
    // Data for the foot
    tss[0]->setName("R.Foot.SCS");
    tss[0]->addParent(&foot);
    const double footscsdata[numeltsscs] = {
      0.90926, 0.9095, 0.90888, 0.9074, 0.90533, 0.90319, 0.90164, 0.90107, 0.90141, 0.9023,
      0.14403, 0.12366, 0.09823, 0.07004, 0.0417, 0.01558, -0.00674, -0.0246, -0.03807, -0.04769,
      0.39051, 0.39689, 0.40533, 0.41438, 0.42266, 0.42895, 0.43242, 0.43299, 0.43129, 0.42846,
      -0.12315, -0.09854, -0.06712, -0.03158, 0.00472, 0.03852, 0.06753, 0.09074, 0.10827, 0.12089,
      0.98931, 0.99165, 0.99365, 0.99459, 0.99412, 0.99237, 0.98981, 0.98699, 0.9843, 0.98198,
      -0.07815, -0.08316, -0.09031, -0.09895, -0.10819, -0.11716, -0.12538, -0.13276, -0.13941, -0.14528,
      -0.39759, -0.40386, -0.41163, -0.41907, -0.42469, -0.4275, -0.42717, -0.42408, -0.41922, -0.41381,
      0.02297, 0.03653, 0.05487, 0.07671, 0.09994, 0.12234, 0.14225, 0.15891, 0.17236, 0.18288,
      0.91728, 0.91409, 0.9097, 0.90471, 0.89981, 0.8957, 0.89291, 0.89157, 0.89138, 0.8918,
      826.32837, 832.89762, 838.19049, 842.49298, 846.065, 849.08362, 851.62637, 853.71509, 855.38028, 856.69122,
      81.19634, 79.14663, 77.07691, 75.17602, 73.58755, 72.3748, 71.52374, 70.97495, 70.65585, 70.49444,
{'fraction_non_alphanumeric': 0.21771730074979173, 'fraction_numerical': 0.19161344071091363, 'mean_word_length': 3.691272253582284, 'pattern_counts': {'":': 0, '<': 33, '<?xml version=': 0, '>': 71, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6810355', 'n_tokens_mistral': 5880, 'n_tokens_neox': 4602, 'n_words': 782}