Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	tests/test_biped_ig.cpp	Lines:	149	151	98.7 %
Date:	2023-12-03 14:15:28	Branches:	672	1342	50.1 %


#include <boost/test/unit_test.hpp>

#include "aig/biped_ig.hpp"
#include "aig/unittests/pyrene_settings.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/frames.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/parsers/srdf.hpp"
#include "pinocchio/parsers/urdf.hpp"

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_biped_ig_talos_settings) {
  const std::string path_to_robots = EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data";
  aig::BipedIGSettings talos_settings =
      aig::makeSettingsFor(path_to_robots, "talos");
  aig::BipedIG biped_ig(talos_settings);
  BOOST_CHECK_EQUAL(biped_ig.get_settings(), talos_settings);
}

BOOST_AUTO_TEST_CASE(test_biped_ig_default_constructor) {
  aig::BipedIG biped_ig;
  aig::BipedIGSettings default_settings;
  BOOST_CHECK_EQUAL(biped_ig.get_settings(), default_settings);
}

BOOST_AUTO_TEST_CASE(test_biped_ig_init_constructor) {
  aig::BipedIGSettings settings = aig::unittests::bipeds;

  aig::BipedIG biped_ig(settings);
  BOOST_CHECK_EQUAL(biped_ig.get_settings(), settings);
}

BOOST_AUTO_TEST_CASE(test_biped_ig_init_constructor_urdf_content) {
  aig::BipedIGSettings settings = aig::unittests::bipeds;
  aig::BipedIG biped_ig_1(settings);

  // read the urdf:
  std::ifstream file(settings.urdf.c_str());
  std::stringstream buffer;
  buffer << file.rdbuf();
  // Save the urdf content into a string
  settings.urdf = buffer.str();
  aig::BipedIG biped_ig_2(settings);

  // Check that both pinocchio model are equal.
  BOOST_CHECK_EQUAL(biped_ig_1.get_model(), biped_ig_2.get_model());
}

BOOST_AUTO_TEST_CASE(test_biped_ig_init_constructor_srdf_content) {
  aig::BipedIGSettings settings = aig::unittests::bipeds;
  aig::BipedIG biped_ig_1(settings);

  // read the urdf:
  std::ifstream file(settings.srdf.c_str());
  std::stringstream buffer;
  buffer << file.rdbuf();
  // Save the urdf content into a string
  settings.srdf = buffer.str();
  aig::BipedIG biped_ig_2(settings);

  // Check that both pinocchio model are equal.
  BOOST_CHECK_EQUAL(biped_ig_1.get_model(), biped_ig_2.get_model());
}

BOOST_AUTO_TEST_CASE(test_leg_ig_init_through_biped_ig) {
  aig::BipedIGSettings settings = aig::unittests::bipeds;

  aig::BipedIG biped_ig(settings);

  const aig::LegIGSettings& llegs = biped_ig.get_left_leg_settings();
  const aig::LegIGSettings& rlegs = biped_ig.get_right_leg_settings();

  aig::LegIGSettings test_llegs = aig::unittests::llegs;
  aig::LegIGSettings test_rlegs = aig::unittests::rlegs;

  BOOST_CHECK_EQUAL(llegs, test_llegs);
  BOOST_CHECK_EQUAL(rlegs, test_rlegs);
}

enum Mode { ZERO, HALF_SITTING, RANDOM };

void generate_references(Eigen::Vector3d& com, pinocchio::SE3& base,
                         pinocchio::SE3& lf, pinocchio::SE3& rf,
                         Eigen::VectorXd& q, const Mode& mode) {
  // Get the model and data
  pinocchio::Model model;
  pinocchio::urdf::buildModel(aig::unittests::urdf,
                              pinocchio::JointModelFreeFlyer(), model);
  pinocchio::Data data = pinocchio::Data(model);
  pinocchio::srdf::loadReferenceConfigurations(model, aig::unittests::srdf,
                                               false);

  // Generate a robot configuration.
  switch (mode) {
    case Mode::ZERO:
      q = Eigen::VectorXd::Zero(model.nq);
      q(6) = 1.0;
      break;
    case Mode::HALF_SITTING:
      q = model.referenceConfigurations["half_sitting"];
      break;
    case Mode::RANDOM:
      model.lowerPositionLimit.head<3>().fill(-0.1);
      model.upperPositionLimit.head<3>().fill(0.1);
      q = randomConfiguration(model);
      break;
    default:
      throw std::runtime_error(
          "tes_leg_ig: generate_references():Switch default ask, not "
          "implemented.");
      break;
  }

  // Forward Kinematics.
  pinocchio::forwardKinematics(model, data, q);
  pinocchio::updateFramePlacements(model, data);
  pinocchio::FrameIndex lf_id =
      model.getFrameId(aig::unittests::left_foot_frame_name);
  pinocchio::FrameIndex rf_id =
      model.getFrameId(aig::unittests::right_foot_frame_name);

  // outputs
  lf = data.oMf[lf_id];
  rf = data.oMf[rf_id];
  Eigen::Quaterniond quat = Eigen::Quaterniond(q[6], q[3], q[4], q[5]);
  quat.normalize();
  base = pinocchio::SE3(quat.toRotationMatrix(), q.head<3>());
  com = pinocchio::centerOfMass(model, data, q);
}

void test_solve(Mode mode) {
  // create the solver
  aig::BipedIGSettings settings = aig::unittests::bipeds;
  aig::BipedIG biped_ig(settings);

  // perform a forward kinematics on a configuration
  Eigen::VectorXd q_test, q_ig_com, q_ig_com_baserot, q_ig_base;
  Eigen::Vector3d com;
  pinocchio::SE3 base, lf, rf;
  generate_references(com, base, lf, rf, q_test, mode);
  double precision = mode == Mode::RANDOM ? 1.0 : 1e-4;

  // Compute inverse geometry and tests
  biped_ig.solve(com, lf, rf, q_test, q_ig_com);
  BOOST_CHECK_EQUAL(q_test.size(), q_ig_com.size());
  // BOOST_CHECK_LE((q_test - q_ig_com).norm(), precision);
  //
  biped_ig.solve(com, base.rotation(), lf, rf, q_test, q_ig_com_baserot);
  BOOST_CHECK_EQUAL(q_test.size(), q_ig_com_baserot.size());
  // BOOST_CHECK_LE((q_test - q_ig_com_baserot).norm(), precision);
  //
  biped_ig.solve(base, lf, rf, q_test, q_ig_base);
  BOOST_CHECK_EQUAL(q_test.size(), q_ig_base.size());
  BOOST_CHECK_LE((q_test - q_ig_base).norm(), precision);
}

BOOST_AUTO_TEST_CASE(test_solve_zero) { test_solve(Mode::ZERO); }

BOOST_AUTO_TEST_CASE(test_solve_half_sitting) {
  test_solve(Mode::HALF_SITTING);
}

BOOST_AUTO_TEST_CASE(test_solve_random) { test_solve(Mode::RANDOM); }

void test_solve_derivatives(Mode mode) {
  // create the solver
  aig::BipedIGSettings settings = aig::unittests::bipeds;
  aig::BipedIG biped_ig(settings);

  // perform a forward kinematics on a configuration
  Eigen::VectorXd q_test, q_ig_com, q_ig_com_baserot, q_ig_base, v_ig_com,
      a_ig_com;
  Eigen::Vector3d com;
  pinocchio::SE3 base, lf, rf;  //
  generate_references(com, base, lf, rf, q_test, mode);
  // double precision = mode == Mode::RANDOM ? 1.0 : 1e-3;

  double dt = 1e-5;
  // std::array<pinocchio::SE3, 3> bases{ {base, base, base} };
  std::array<Eigen::Vector3d, 3> coms{{com, com, com}};
  std::array<pinocchio::SE3, 3> lfs{{lf, lf, lf}};
  std::array<pinocchio::SE3, 3> rfs{{rf, rf, rf}};

  // Compute inverse geometry and tests
  biped_ig.solve(coms, lfs, rfs, q_test, q_ig_com, v_ig_com, a_ig_com, dt);
  BOOST_CHECK_EQUAL(q_test.size(), q_ig_com.size());
  BOOST_CHECK_EQUAL(q_test.size() - 1, v_ig_com.size());
  BOOST_CHECK_EQUAL(q_test.size() - 1, a_ig_com.size());

  // Compute with Eigen::Isometry3d instead of pinocchio::SE3
  Eigen::Isometry3d LF, RF;  //()//(lf.toHomogeneousMatrix())
  LF.rotate(lf.rotation());
  LF.translate(lf.translation());
  RF.rotate(rf.rotation());
  RF.translate(rf.translation());
  std::array<Eigen::Isometry3d, 3> LFs{{LF, LF, LF}};
  std::array<Eigen::Isometry3d, 3> RFs{{RF, RF, RF}};

  biped_ig.solve(coms, LFs, RFs, q_test, q_ig_com, v_ig_com, a_ig_com, dt);
  BOOST_CHECK_EQUAL(q_test.size(), q_ig_com.size());
  BOOST_CHECK_EQUAL(q_test.size() - 1, v_ig_com.size());
  BOOST_CHECK_EQUAL(q_test.size() - 1, a_ig_com.size());

  /*BOOST_CHECK_LE((q_test - q_ig_com).norm(), precision);

  biped_ig.solve(com, base.rotation(), lf, rf, q_test, q_ig_com_baserot);
  BOOST_CHECK_EQUAL(q_test.size(), q_ig_com_baserot.size());
  // BOOST_CHECK_LE((q_test - q_ig_com_baserot).norm(), precision);
  //
  biped_ig.solve(base, lf, rf, q_test, q_ig_base);
  BOOST_CHECK_EQUAL(q_test.size(), q_ig_base.size());
  BOOST_CHECK_LE((q_test - q_ig_base).norm(), precision);*/
}

BOOST_AUTO_TEST_CASE(test_solve_half_sitting_derivatives) {
  test_solve_derivatives(Mode::HALF_SITTING);
}

BOOST_AUTO_TEST_CASE(test_compute_dynamics) {
  // create the solver
  aig::BipedIGSettings settings = aig::unittests::bipeds;
  aig::BipedIG biped_ig(settings);

  // perform a forward kinematics on a configuration
  Eigen::VectorXd q_test, q_ig_com, v_ig_com, a_ig_com;
  Eigen::Vector3d com;
  pinocchio::SE3 base, lf, rf;  //
  generate_references(com, base, lf, rf, q_test, Mode::HALF_SITTING);

  double dt = 1e-5;
  // std::array<pinocchio::SE3, 3> bases{ {base, base, base} };
  std::array<Eigen::Vector3d, 3> coms{{com, com, com}};
  std::array<pinocchio::SE3, 3> lfs{{lf, lf, lf}};
  std::array<pinocchio::SE3, 3> rfs{{rf, rf, rf}};

  // Compute inverse geometry and tests
  biped_ig.solve(coms, lfs, rfs, q_test, q_ig_com, v_ig_com, a_ig_com, dt);
  BOOST_CHECK_EQUAL(q_test.size(), q_ig_com.size());
  BOOST_CHECK_EQUAL(q_test.size() - 1, v_ig_com.size());
  BOOST_CHECK_EQUAL(q_test.size() - 1, a_ig_com.size());
  BOOST_CHECK(v_ig_com.isMuchSmallerThan(1));
  BOOST_CHECK(a_ig_com.isMuchSmallerThan(1));

  biped_ig.computeNL(3.3, q_ig_com, v_ig_com, a_ig_com);

  BOOST_CHECK(biped_ig.getAM().isMuchSmallerThan(1));
  BOOST_CHECK(biped_ig.getAMVariation().isMuchSmallerThan(1));
  BOOST_CHECK(biped_ig.getNL().isMuchSmallerThan(1));
  // BOOST_CHECK(
  //     (biped_ig.getCoM().head<2>() -
  //     biped_ig.getCoP()).isMuchSmallerThan(1));
}

BOOST_AUTO_TEST_SUITE_END()


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			#include <boost/test/unit_test.hpp>
2
3			#include "aig/biped_ig.hpp"
4			#include "aig/unittests/pyrene_settings.hpp"
5			#include "pinocchio/algorithm/center-of-mass.hpp"
6			#include "pinocchio/algorithm/frames.hpp"
7			#include "pinocchio/algorithm/joint-configuration.hpp"
8			#include "pinocchio/algorithm/kinematics.hpp"
9			#include "pinocchio/parsers/srdf.hpp"
10			#include "pinocchio/parsers/urdf.hpp"
11
12			BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
13
14	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_biped_ig_talos_settings) {
15	✓✗	4	const std::string path_to_robots = EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data";
16			aig::BipedIGSettings talos_settings =
17	✓✗✓✗	6	aig::makeSettingsFor(path_to_robots, "talos");
18	✓✗	4	aig::BipedIG biped_ig(talos_settings);
19	✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK_EQUAL(biped_ig.get_settings(), talos_settings);
20		2	}
21
22	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_biped_ig_default_constructor) {
23	✓✗	4	aig::BipedIG biped_ig;
24	✓✗	4	aig::BipedIGSettings default_settings;
25	✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK_EQUAL(biped_ig.get_settings(), default_settings);
26		2	}
27
28	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_biped_ig_init_constructor) {
29	✓✗	4	aig::BipedIGSettings settings = aig::unittests::bipeds;
30
31	✓✗	4	aig::BipedIG biped_ig(settings);
32	✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK_EQUAL(biped_ig.get_settings(), settings);
33		2	}
34
35	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_biped_ig_init_constructor_urdf_content) {
36	✓✗	4	aig::BipedIGSettings settings = aig::unittests::bipeds;
37	✓✗	4	aig::BipedIG biped_ig_1(settings);
38
39			// read the urdf:
40	✓✗	4	std::ifstream file(settings.urdf.c_str());
41	✓✗	4	std::stringstream buffer;
42	✓✗✓✗	2	buffer << file.rdbuf();
43			// Save the urdf content into a string
44	✓✗	2	settings.urdf = buffer.str();
45	✓✗	4	aig::BipedIG biped_ig_2(settings);
46
47			// Check that both pinocchio model are equal.
48	✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK_EQUAL(biped_ig_1.get_model(), biped_ig_2.get_model());
49		2	}
50
51	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_biped_ig_init_constructor_srdf_content) {
52	✓✗	4	aig::BipedIGSettings settings = aig::unittests::bipeds;
53	✓✗	4	aig::BipedIG biped_ig_1(settings);
54
55			// read the urdf:
56	✓✗	4	std::ifstream file(settings.srdf.c_str());
57	✓✗	4	std::stringstream buffer;
58	✓✗✓✗	2	buffer << file.rdbuf();
59			// Save the urdf content into a string
60	✓✗	2	settings.srdf = buffer.str();
61	✓✗	4	aig::BipedIG biped_ig_2(settings);
62
63			// Check that both pinocchio model are equal.
64	✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK_EQUAL(biped_ig_1.get_model(), biped_ig_2.get_model());
65		2	}
66
67	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_leg_ig_init_through_biped_ig) {
68	✓✗	4	aig::BipedIGSettings settings = aig::unittests::bipeds;
69
70	✓✗	4	aig::BipedIG biped_ig(settings);
71
72		2	const aig::LegIGSettings& llegs = biped_ig.get_left_leg_settings();
73		2	const aig::LegIGSettings& rlegs = biped_ig.get_right_leg_settings();
74
75	✓✗	2	aig::LegIGSettings test_llegs = aig::unittests::llegs;
76	✓✗	2	aig::LegIGSettings test_rlegs = aig::unittests::rlegs;
77
78	✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK_EQUAL(llegs, test_llegs);
79	✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK_EQUAL(rlegs, test_rlegs);
80		2	}
81
82			enum Mode { ZERO, HALF_SITTING, RANDOM };
83
84		5	void generate_references(Eigen::Vector3d& com, pinocchio::SE3& base,
85			pinocchio::SE3& lf, pinocchio::SE3& rf,
86			Eigen::VectorXd& q, const Mode& mode) {
87			// Get the model and data
88	✓✗	10	pinocchio::Model model;
89			pinocchio::urdf::buildModel(aig::unittests::urdf,
90	✓✗✓✗ ✓✗	5	pinocchio::JointModelFreeFlyer(), model);
91	✓✗	10	pinocchio::Data data = pinocchio::Data(model);
92	✓✗	5	pinocchio::srdf::loadReferenceConfigurations(model, aig::unittests::srdf,
93			false);
94
95			// Generate a robot configuration.
96	✓✓✓✗	5	switch (mode) {
97		1	case Mode::ZERO:
98	✓✗✓✗	1	q = Eigen::VectorXd::Zero(model.nq);
99	✓✗	1	q(6) = 1.0;
100		1	break;
101		3	case Mode::HALF_SITTING:
102	✓✗✓✗ ✓✗	3	q = model.referenceConfigurations["half_sitting"];
103		3	break;
104		1	case Mode::RANDOM:
105	✓✗✓✗	1	model.lowerPositionLimit.head<3>().fill(-0.1);
106	✓✗✓✗	1	model.upperPositionLimit.head<3>().fill(0.1);
107	✓✗	1	q = randomConfiguration(model);
108		1	break;
109			default:
110			throw std::runtime_error(
111			"tes_leg_ig: generate_references():Switch default ask, not "
112			"implemented.");
113			break;
114			}
115
116			// Forward Kinematics.
117	✓✗	5	pinocchio::forwardKinematics(model, data, q);
118	✓✗	5	pinocchio::updateFramePlacements(model, data);
119			pinocchio::FrameIndex lf_id =
120	✓✗✓✗	5	model.getFrameId(aig::unittests::left_foot_frame_name);
121			pinocchio::FrameIndex rf_id =
122	✓✗	5	model.getFrameId(aig::unittests::right_foot_frame_name);
123
124			// outputs
125	✓✗	5	lf = data.oMf[lf_id];
126	✓✗	5	rf = data.oMf[rf_id];
127	✓✗✓✗ ✓✗✓✗ ✓✗	5	Eigen::Quaterniond quat = Eigen::Quaterniond(q[6], q[3], q[4], q[5]);
128	✓✗	5	quat.normalize();
129	✓✗✓✗ ✓✗	5	base = pinocchio::SE3(quat.toRotationMatrix(), q.head<3>());
130	✓✗✓✗	5	com = pinocchio::centerOfMass(model, data, q);
131		5	}
132
133		3	void test_solve(Mode mode) {
134			// create the solver
135	✓✗	6	aig::BipedIGSettings settings = aig::unittests::bipeds;
136	✓✗	6	aig::BipedIG biped_ig(settings);
137
138			// perform a forward kinematics on a configuration
139	✓✗✓✗ ✓✗✓✗	6	Eigen::VectorXd q_test, q_ig_com, q_ig_com_baserot, q_ig_base;
140	✓✗	3	Eigen::Vector3d com;
141	✓✗✓✗ ✓✗	3	pinocchio::SE3 base, lf, rf;
142	✓✗	3	generate_references(com, base, lf, rf, q_test, mode);
143	✓✓✓✗	3	double precision = mode == Mode::RANDOM ? 1.0 : 1e-4;
144
145			// Compute inverse geometry and tests
146	✓✗	3	biped_ig.solve(com, lf, rf, q_test, q_ig_com);
147	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	3	BOOST_CHECK_EQUAL(q_test.size(), q_ig_com.size());
148			// BOOST_CHECK_LE((q_test - q_ig_com).norm(), precision);
149			//
150	✓✗✓✗	3	biped_ig.solve(com, base.rotation(), lf, rf, q_test, q_ig_com_baserot);
151	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	3	BOOST_CHECK_EQUAL(q_test.size(), q_ig_com_baserot.size());
152			// BOOST_CHECK_LE((q_test - q_ig_com_baserot).norm(), precision);
153			//
154	✓✗	3	biped_ig.solve(base, lf, rf, q_test, q_ig_base);
155	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	3	BOOST_CHECK_EQUAL(q_test.size(), q_ig_base.size());
156	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	3	BOOST_CHECK_LE((q_test - q_ig_base).norm(), precision);
157		3	}
158
159	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_solve_zero) { test_solve(Mode::ZERO); }
160
161	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_solve_half_sitting) {
162		2	test_solve(Mode::HALF_SITTING);
163		2	}
164
165	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_solve_random) { test_solve(Mode::RANDOM); }
166
167		1	void test_solve_derivatives(Mode mode) {
168			// create the solver
169	✓✗	2	aig::BipedIGSettings settings = aig::unittests::bipeds;
170	✓✗	2	aig::BipedIG biped_ig(settings);
171
172			// perform a forward kinematics on a configuration
173	✓✗✓✗ ✓✗✓✗ ✓✗	2	Eigen::VectorXd q_test, q_ig_com, q_ig_com_baserot, q_ig_base, v_ig_com,
174	✓✗	2	a_ig_com;
175	✓✗	1	Eigen::Vector3d com;
176	✓✗✓✗ ✓✗	1	pinocchio::SE3 base, lf, rf; //
177	✓✗	1	generate_references(com, base, lf, rf, q_test, mode);
178			// double precision = mode == Mode::RANDOM ? 1.0 : 1e-3;
179
180		1	double dt = 1e-5;
181			// std::array<pinocchio::SE3, 3> bases{ {base, base, base} };
182	✓✗✓✗ ✓✗	1	std::array<Eigen::Vector3d, 3> coms{{com, com, com}};
183	✓✗✓✗ ✓✗	1	std::array<pinocchio::SE3, 3> lfs{{lf, lf, lf}};
184	✓✗✓✗ ✓✗	1	std::array<pinocchio::SE3, 3> rfs{{rf, rf, rf}};
185
186			// Compute inverse geometry and tests
187	✓✗	1	biped_ig.solve(coms, lfs, rfs, q_test, q_ig_com, v_ig_com, a_ig_com, dt);
188	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK_EQUAL(q_test.size(), q_ig_com.size());
189	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK_EQUAL(q_test.size() - 1, v_ig_com.size());
190	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK_EQUAL(q_test.size() - 1, a_ig_com.size());
191
192			// Compute with Eigen::Isometry3d instead of pinocchio::SE3
193	✓✗✓✗	1	Eigen::Isometry3d LF, RF; //()//(lf.toHomogeneousMatrix())
194	✓✗✓✗	1	LF.rotate(lf.rotation());
195	✓✗✓✗	1	LF.translate(lf.translation());
196	✓✗✓✗	1	RF.rotate(rf.rotation());
197	✓✗✓✗	1	RF.translate(rf.translation());
198	✓✗✓✗ ✓✗	1	std::array<Eigen::Isometry3d, 3> LFs{{LF, LF, LF}};
199	✓✗✓✗ ✓✗	1	std::array<Eigen::Isometry3d, 3> RFs{{RF, RF, RF}};
200
201	✓✗	1	biped_ig.solve(coms, LFs, RFs, q_test, q_ig_com, v_ig_com, a_ig_com, dt);
202	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK_EQUAL(q_test.size(), q_ig_com.size());
203	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK_EQUAL(q_test.size() - 1, v_ig_com.size());
204	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK_EQUAL(q_test.size() - 1, a_ig_com.size());
205
206			/*BOOST_CHECK_LE((q_test - q_ig_com).norm(), precision);
207
208			biped_ig.solve(com, base.rotation(), lf, rf, q_test, q_ig_com_baserot);
209			BOOST_CHECK_EQUAL(q_test.size(), q_ig_com_baserot.size());
210			// BOOST_CHECK_LE((q_test - q_ig_com_baserot).norm(), precision);
211			//
212			biped_ig.solve(base, lf, rf, q_test, q_ig_base);
213			BOOST_CHECK_EQUAL(q_test.size(), q_ig_base.size());
214			BOOST_CHECK_LE((q_test - q_ig_base).norm(), precision);*/
215		1	}
216
217	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_solve_half_sitting_derivatives) {
218		2	test_solve_derivatives(Mode::HALF_SITTING);
219		2	}
220
221	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_compute_dynamics) {
222			// create the solver
223	✓✗	4	aig::BipedIGSettings settings = aig::unittests::bipeds;
224	✓✗	4	aig::BipedIG biped_ig(settings);
225
226			// perform a forward kinematics on a configuration
227	✓✗✓✗ ✓✗✓✗	4	Eigen::VectorXd q_test, q_ig_com, v_ig_com, a_ig_com;
228	✓✗	2	Eigen::Vector3d com;
229	✓✗✓✗ ✓✗	2	pinocchio::SE3 base, lf, rf; //
230	✓✗	2	generate_references(com, base, lf, rf, q_test, Mode::HALF_SITTING);
231
232		2	double dt = 1e-5;
233			// std::array<pinocchio::SE3, 3> bases{ {base, base, base} };
234	✓✗✓✗ ✓✗	2	std::array<Eigen::Vector3d, 3> coms{{com, com, com}};
235	✓✗✓✗ ✓✗	2	std::array<pinocchio::SE3, 3> lfs{{lf, lf, lf}};
236	✓✗✓✗ ✓✗	2	std::array<pinocchio::SE3, 3> rfs{{rf, rf, rf}};
237
238			// Compute inverse geometry and tests
239	✓✗	2	biped_ig.solve(coms, lfs, rfs, q_test, q_ig_com, v_ig_com, a_ig_com, dt);
240	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK_EQUAL(q_test.size(), q_ig_com.size());
241	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK_EQUAL(q_test.size() - 1, v_ig_com.size());
242	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK_EQUAL(q_test.size() - 1, a_ig_com.size());
243	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(v_ig_com.isMuchSmallerThan(1));
244	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(a_ig_com.isMuchSmallerThan(1));
245
246	✓✗✓✗ ✓✗	2	biped_ig.computeNL(3.3, q_ig_com, v_ig_com, a_ig_com);
247
248	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(biped_ig.getAM().isMuchSmallerThan(1));
249	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(biped_ig.getAMVariation().isMuchSmallerThan(1));
250	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(biped_ig.getNL().isMuchSmallerThan(1));
251			// BOOST_CHECK(
252			// (biped_ig.getCoM().head<2>() -
253			// biped_ig.getCoP()).isMuchSmallerThan(1));
254		2	}
255
256			BOOST_AUTO_TEST_SUITE_END()