Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	tests/contacts.cpp	Lines:	47	47	100.0 %
Date:	2024-02-02 08:47:34	Branches:	191	382	50.0 %


//
// Copyright (c) 2017 CNRS
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

#include <pinocchio/algorithm/joint-configuration.hpp>

#include <tsid/contacts/contact-6d.hpp>
#include <tsid/robots/robot-wrapper.hpp>

using namespace tsid;
using namespace trajectories;
using namespace math;
using namespace contacts;
using namespace tsid::robots;
using namespace std;

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

#define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(isFinite(A), #A << ": " << A)

const string romeo_model_path = TSID_SOURCE_DIR "/models/romeo";

BOOST_AUTO_TEST_CASE(test_contact_6d) {
  const double lx = 0.07;
  const double ly = 0.12;
  const double lz = 0.105;
  const double mu = 0.3;
  const double fMin = 10.0;
  const double fMax = 1000.0;
  const std::string frameName = "r_sole_joint";

  vector<string> package_dirs;
  package_dirs.push_back(romeo_model_path);
  string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
  RobotWrapper robot(urdfFileName, package_dirs,
                     pinocchio::JointModelFreeFlyer(), false);

  BOOST_REQUIRE(robot.model().existFrame(frameName));

  Vector3 contactNormal = Vector3::UnitZ();
  Matrix3x contactPoints(3, 4);
  contactPoints << -lx, -lx, +lx, +lx, -ly, +ly, -ly, +ly, lz, lz, lz, lz;
  Contact6d contact("contact6d", robot, frameName, contactPoints, contactNormal,
                    mu, fMin, fMax);

  BOOST_CHECK(contact.n_motion() == 6);
  BOOST_CHECK(contact.n_force() == 12);

  Vector Kp = Vector::Ones(6);
  Vector Kd = 2 * Vector::Ones(6);
  contact.Kp(Kp);
  contact.Kd(Kd);
  BOOST_CHECK(contact.Kp().isApprox(Kp));
  BOOST_CHECK(contact.Kd().isApprox(Kd));

  Vector q = neutral(robot.model());
  Vector v = Vector::Zero(robot.nv());
  pinocchio::Data data(robot.model());
  robot.computeAllTerms(data, q, v);

  pinocchio::SE3 H_ref =
      robot.position(data, robot.model().getFrameId(frameName));
  contact.setReference(H_ref);

  double t = 0.0;
  contact.computeMotionTask(t, q, v, data);

  const ConstraintInequality& forceIneq =
      contact.computeForceTask(t, q, v, data);
  Vector3 f3;
  Vector f(12);
  f3 << 0.0, 0.0, 100.0;
  f << f3, f3, f3, f3;
  BOOST_CHECK(forceIneq.checkConstraint(f));
  BOOST_CHECK(
      ((forceIneq.matrix() * f).array() <= forceIneq.upperBound().array())
          .all());
  BOOST_CHECK(
      ((forceIneq.matrix() * f).array() >= forceIneq.lowerBound().array())
          .all());
  f(0) = f(2) * mu * 1.1;
  BOOST_CHECK(forceIneq.checkConstraint(f) == false);

  const Matrix& forceGenMat = contact.getForceGeneratorMatrix();
  BOOST_CHECK(forceGenMat.rows() == 6 && forceGenMat.cols() == 12);

  contact.computeForceRegularizationTask(t, q, v, data);
}

BOOST_AUTO_TEST_SUITE_END()


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2017 CNRS
3			//
4			// This file is part of tsid
5			// tsid is free software: you can redistribute it
6			// and/or modify it under the terms of the GNU Lesser General Public
7			// License as published by the Free Software Foundation, either version
8			// 3 of the License, or (at your option) any later version.
9			// tsid is distributed in the hope that it will be
10			// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11			// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12			// General Lesser Public License for more details. You should have
13			// received a copy of the GNU Lesser General Public License along with
14			// tsid If not, see
15			// <http://www.gnu.org/licenses/>.
16			//
17
18			#include <iostream>
19
20			#include <boost/test/unit_test.hpp>
21			#include <boost/utility/binary.hpp>
22
23			#include <pinocchio/algorithm/joint-configuration.hpp>
24
25			#include <tsid/contacts/contact-6d.hpp>
26			#include <tsid/robots/robot-wrapper.hpp>
27
28			using namespace tsid;
29			using namespace trajectories;
30			using namespace math;
31			using namespace contacts;
32			using namespace tsid::robots;
33			using namespace std;
34
35			BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
36
37			#define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(isFinite(A), #A << ": " << A)
38
39			const string romeo_model_path = TSID_SOURCE_DIR "/models/romeo";
40
41	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_contact_6d) {
42		2	const double lx = 0.07;
43		2	const double ly = 0.12;
44		2	const double lz = 0.105;
45		2	const double mu = 0.3;
46		2	const double fMin = 10.0;
47		2	const double fMax = 1000.0;
48	✓✗	4	const std::string frameName = "r_sole_joint";
49
50		4	vector<string> package_dirs;
51	✓✗	2	package_dirs.push_back(romeo_model_path);
52	✓✗	4	string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
53			RobotWrapper robot(urdfFileName, package_dirs,
54	✓✗✓✗ ✓✗	6	pinocchio::JointModelFreeFlyer(), false);
55
56	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_REQUIRE(robot.model().existFrame(frameName));
57
58	✓✗✓✗	2	Vector3 contactNormal = Vector3::UnitZ();
59	✓✗	4	Matrix3x contactPoints(3, 4);
60	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	contactPoints << -lx, -lx, +lx, +lx, -ly, +ly, -ly, +ly, lz, lz, lz, lz;
61			Contact6d contact("contact6d", robot, frameName, contactPoints, contactNormal,
62	✓✗✓✗ ✓✗✓✗	6	mu, fMin, fMax);
63
64	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(contact.n_motion() == 6);
65	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(contact.n_force() == 12);
66
67	✓✗✓✗	4	Vector Kp = Vector::Ones(6);
68	✓✗✓✗ ✓✗	4	Vector Kd = 2 * Vector::Ones(6);
69	✓✗✓✗	2	contact.Kp(Kp);
70	✓✗✓✗	2	contact.Kd(Kd);
71	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(contact.Kp().isApprox(Kp));
72	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(contact.Kd().isApprox(Kd));
73
74	✓✗✓✗	4	Vector q = neutral(robot.model());
75	✓✗✓✗ ✓✗	4	Vector v = Vector::Zero(robot.nv());
76	✓✗✓✗	4	pinocchio::Data data(robot.model());
77	✓✗	2	robot.computeAllTerms(data, q, v);
78
79			pinocchio::SE3 H_ref =
80	✓✗✓✗ ✓✗✓✗	2	robot.position(data, robot.model().getFrameId(frameName));
81	✓✗	2	contact.setReference(H_ref);
82
83		2	double t = 0.0;
84	✓✗✓✗ ✓✗	2	contact.computeMotionTask(t, q, v, data);
85
86			const ConstraintInequality& forceIneq =
87	✓✗✓✗ ✓✗	2	contact.computeForceTask(t, q, v, data);
88	✓✗	2	Vector3 f3;
89	✓✗	2	Vector f(12);
90	✓✗✓✗ ✓✗	2	f3 << 0.0, 0.0, 100.0;
91	✓✗✓✗ ✓✗✓✗	2	f << f3, f3, f3, f3;
92	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(forceIneq.checkConstraint(f));
93	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(
94			((forceIneq.matrix() * f).array() <= forceIneq.upperBound().array())
95			.all());
96	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(
97			((forceIneq.matrix() * f).array() >= forceIneq.lowerBound().array())
98			.all());
99	✓✗✓✗	2	f(0) = f(2) * mu * 1.1;
100	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(forceIneq.checkConstraint(f) == false);
101
102	✓✗	2	const Matrix& forceGenMat = contact.getForceGeneratorMatrix();
103	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(forceGenMat.rows() == 6 && forceGenMat.cols() == 12);
104
105	✓✗✓✗ ✓✗	2	contact.computeForceRegularizationTask(t, q, v, data);
106		2	}
107
108			BOOST_AUTO_TEST_SUITE_END()