Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	tests/test_leg_ig.cpp	Lines:	83	85	97.6 %
Date:	2023-12-03 14:15:28	Branches:	366	724	50.6 %


#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "aig/leg_ig.hpp"
#include "aig/unittests/pyrene_settings.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_default_constructor) {
  aig::LegIG leg_ig;
  aig::LegIGSettings default_settings;
  BOOST_CHECK_EQUAL(leg_ig.get_settings(), default_settings);
}

BOOST_AUTO_TEST_CASE(test_init_constructor) {
  aig::LegIGSettings settings;

  // Randomize the Matrices.
  srand((unsigned int)time(0));
  settings.knee_from_hip = Eigen::Vector3d::Random();
  settings.ankle_from_knee = Eigen::Vector3d::Random();
  settings.hip_from_waist = Eigen::Vector3d::Random();
  settings.ankle_from_foot = Eigen::Vector3d::Random();

  aig::LegIG leg_ig(settings);
  BOOST_CHECK_EQUAL(leg_ig.get_settings(), settings);
}

enum Mode { ZERO, HALF_SITTING, RANDOM };

void generate_references(pinocchio::SE3& base, pinocchio::SE3& lf,
                         pinocchio::SE3& rf, aig::LegJoints& ll_q,
                         aig::LegJoints& rl_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.
  Eigen::VectorXd q;
  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);

  // Get the legs joints configuration for the test
  int lleg_idx_qs =
      model.idx_qs[model.getJointId(aig::unittests::left_hip_joint_name)];
  int rleg_idx_qs =
      model.idx_qs[model.getJointId(aig::unittests::right_hip_joint_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>());
  ll_q = q.segment<6>(lleg_idx_qs);
  rl_q = q.segment<6>(rleg_idx_qs);
}

void test_solve(bool left, Mode mode) {
  // create the solver
  aig::LegIGSettings settings;
  if (left) {
    settings = aig::unittests::llegs;
  } else {
    settings = aig::unittests::rlegs;
  }
  aig::LegIG leg_ig(settings);

  // perform a forward kinematics on a configuration
  Eigen::VectorXd q;
  aig::LegJoints test_ll_q, test_rl_q;
  pinocchio::SE3 base, lf, rf;
  generate_references(base, lf, rf, test_ll_q, test_rl_q, mode);
  double precision = mode == Mode::RANDOM ? 1.0 : 1e-3;
  // Compute inverse geometry.
  aig::LegJoints q_leg;
  if (left) {
    q_leg = leg_ig.solve(base, lf);
  } else {
    q_leg = leg_ig.solve(base, rf);
  }

  // Tests.
  if (left) {
    BOOST_CHECK_LE((q_leg - test_ll_q).norm(), precision);
  } else {
    BOOST_CHECK_LE((q_leg - test_rl_q).norm(), precision);
  }
}

BOOST_AUTO_TEST_CASE(test_solve_left_zero) { test_solve(true, Mode::ZERO); }

BOOST_AUTO_TEST_CASE(test_solve_left_half_sitting) {
  test_solve(true, Mode::HALF_SITTING);
}

BOOST_AUTO_TEST_CASE(test_solve_left_random) {
  // Randomize the Matrices.
  srand((unsigned int)time(0));
  test_solve(true, Mode::RANDOM);
}

BOOST_AUTO_TEST_CASE(test_solve_right_zero) { test_solve(false, Mode::ZERO); }

BOOST_AUTO_TEST_CASE(test_solve_right_half_sitting) {
  test_solve(false, Mode::HALF_SITTING);
}

BOOST_AUTO_TEST_CASE(test_solve_right_random) {
  // Randomize the Matrices.
  srand((unsigned int)time(0));
  test_solve(false, Mode::RANDOM);
}

BOOST_AUTO_TEST_SUITE_END()


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			#include <boost/test/tools/floating_point_comparison.hpp>
2			#include <boost/test/unit_test.hpp>
3
4			#include "aig/leg_ig.hpp"
5			#include "aig/unittests/pyrene_settings.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_default_constructor) {
15	✓✗	2	aig::LegIG leg_ig;
16	✓✗	2	aig::LegIGSettings default_settings;
17	✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK_EQUAL(leg_ig.get_settings(), default_settings);
18		2	}
19
20	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_init_constructor) {
21	✓✗	2	aig::LegIGSettings settings;
22
23			// Randomize the Matrices.
24		2	srand((unsigned int)time(0));
25	✓✗✓✗	2	settings.knee_from_hip = Eigen::Vector3d::Random();
26	✓✗✓✗	2	settings.ankle_from_knee = Eigen::Vector3d::Random();
27	✓✗✓✗	2	settings.hip_from_waist = Eigen::Vector3d::Random();
28	✓✗✓✗	2	settings.ankle_from_foot = Eigen::Vector3d::Random();
29
30	✓✗	2	aig::LegIG leg_ig(settings);
31	✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK_EQUAL(leg_ig.get_settings(), settings);
32		2	}
33
34			enum Mode { ZERO, HALF_SITTING, RANDOM };
35
36		6	void generate_references(pinocchio::SE3& base, pinocchio::SE3& lf,
37			pinocchio::SE3& rf, aig::LegJoints& ll_q,
38			aig::LegJoints& rl_q, const Mode& mode) {
39			// Get the model and data
40	✓✗	12	pinocchio::Model model;
41			pinocchio::urdf::buildModel(aig::unittests::urdf,
42	✓✗✓✗ ✓✗	6	pinocchio::JointModelFreeFlyer(), model);
43	✓✗	12	pinocchio::Data data = pinocchio::Data(model);
44	✓✗	6	pinocchio::srdf::loadReferenceConfigurations(model, aig::unittests::srdf,
45			false);
46
47			// Generate a robot configuration.
48	✓✗	6	Eigen::VectorXd q;
49	✓✓✓✗	6	switch (mode) {
50		2	case Mode::ZERO:
51	✓✗✓✗	2	q = Eigen::VectorXd::Zero(model.nq);
52	✓✗	2	q(6) = 1.0;
53		2	break;
54		2	case Mode::HALF_SITTING:
55	✓✗✓✗ ✓✗	2	q = model.referenceConfigurations["half_sitting"];
56		2	break;
57		2	case Mode::RANDOM:
58	✓✗✓✗	2	model.lowerPositionLimit.head<3>().fill(-0.1);
59	✓✗✓✗	2	model.upperPositionLimit.head<3>().fill(0.1);
60	✓✗	2	q = randomConfiguration(model);
61		2	break;
62			default:
63			throw std::runtime_error(
64			"tes_leg_ig: generate_references():Switch default ask, not "
65			"implemented.");
66			break;
67			}
68
69			// Forward Kinematics.
70	✓✗	6	pinocchio::forwardKinematics(model, data, q);
71	✓✗	6	pinocchio::updateFramePlacements(model, data);
72			pinocchio::FrameIndex lf_id =
73	✓✗✓✗	6	model.getFrameId(aig::unittests::left_foot_frame_name);
74			pinocchio::FrameIndex rf_id =
75	✓✗✓✗	6	model.getFrameId(aig::unittests::right_foot_frame_name);
76
77			// Get the legs joints configuration for the test
78			int lleg_idx_qs =
79	✓✗	6	model.idx_qs[model.getJointId(aig::unittests::left_hip_joint_name)];
80			int rleg_idx_qs =
81	✓✗	6	model.idx_qs[model.getJointId(aig::unittests::right_hip_joint_name)];
82
83			// outputs
84	✓✗	6	lf = data.oMf[lf_id];
85	✓✗	6	rf = data.oMf[rf_id];
86	✓✗✓✗ ✓✗✓✗ ✓✗	6	Eigen::Quaterniond quat = Eigen::Quaterniond(q[6], q[3], q[4], q[5]);
87	✓✗	6	quat.normalize();
88	✓✗✓✗ ✓✗	6	base = pinocchio::SE3(quat.toRotationMatrix(), q.head<3>());
89	✓✗✓✗	6	ll_q = q.segment<6>(lleg_idx_qs);
90	✓✗✓✗	6	rl_q = q.segment<6>(rleg_idx_qs);
91		6	}
92
93		6	void test_solve(bool left, Mode mode) {
94			// create the solver
95	✓✗	6	aig::LegIGSettings settings;
96	✓✓	6	if (left) {
97	✓✗	3	settings = aig::unittests::llegs;
98			} else {
99	✓✗	3	settings = aig::unittests::rlegs;
100			}
101	✓✗	6	aig::LegIG leg_ig(settings);
102
103			// perform a forward kinematics on a configuration
104	✓✗	12	Eigen::VectorXd q;
105	✓✗✓✗	6	aig::LegJoints test_ll_q, test_rl_q;
106	✓✗✓✗ ✓✗	6	pinocchio::SE3 base, lf, rf;
107	✓✗	6	generate_references(base, lf, rf, test_ll_q, test_rl_q, mode);
108	✓✓	6	double precision = mode == Mode::RANDOM ? 1.0 : 1e-3;
109			// Compute inverse geometry.
110	✓✗	6	aig::LegJoints q_leg;
111	✓✓	6	if (left) {
112	✓✗	3	q_leg = leg_ig.solve(base, lf);
113			} else {
114	✓✗	3	q_leg = leg_ig.solve(base, rf);
115			}
116
117			// Tests.
118	✓✓	6	if (left) {
119	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	3	BOOST_CHECK_LE((q_leg - test_ll_q).norm(), precision);
120			} else {
121	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	3	BOOST_CHECK_LE((q_leg - test_rl_q).norm(), precision);
122			}
123		6	}
124
125	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_solve_left_zero) { test_solve(true, Mode::ZERO); }
126
127	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_solve_left_half_sitting) {
128		2	test_solve(true, Mode::HALF_SITTING);
129		2	}
130
131	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_solve_left_random) {
132			// Randomize the Matrices.
133		2	srand((unsigned int)time(0));
134		2	test_solve(true, Mode::RANDOM);
135		2	}
136
137	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_solve_right_zero) { test_solve(false, Mode::ZERO); }
138
139	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_solve_right_half_sitting) {
140		2	test_solve(false, Mode::HALF_SITTING);
141		2	}
142
143	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_solve_right_random) {
144			// Randomize the Matrices.
145		2	srand((unsigned int)time(0));
146		2	test_solve(false, Mode::RANDOM);
147		2	}
148
149			BOOST_AUTO_TEST_SUITE_END()