Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	tests/features/test_feature_point6d.cpp	Lines:	125	133	94.0 %
Date:	2023-03-13 12:09:37	Branches:	120	252	47.6 %


/*
 * Copyright 2019,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
#include <dynamic-graph/linear-algebra.h>

#include <iostream>
#include <sot/core/debug.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/feature-point6d.hh>
#include <sot/core/gain-adaptive.hh>
#include <sot/core/macros.hh>
#include <sot/core/sot.hh>
#include <sot/core/task.hh>
using namespace std;
using namespace dynamicgraph::sot;

class TestPoint6d {
 public:
  SOT_CORE_DISABLE_WARNING_PUSH
  SOT_CORE_DISABLE_WARNING_DEPRECATED
  FeaturePoint6d feature_, featureDes_;
  SOT_CORE_DISABLE_WARNING_POP
  Task task_;
  int time_;
  int dim_, robotDim_, featureDim_;
  dynamicgraph::Vector manual_;

  TestPoint6d(unsigned dim, std::string &name)
      : feature_("feature" + name),
        featureDes_("featureDes" + name),
        task_("task" + name),
        time_(0)

  {
    feature_.computationFrame("desired");
    feature_.setReference(&featureDes_);
    feature_.selectionSIN = Flags(true);

    task_.addFeature(feature_);
    task_.setWithDerivative(true);
    dim_ = dim;
    robotDim_ = featureDim_ = dim;

    dynamicgraph::Matrix Jq(dim, dim);
    Jq.setIdentity();
    feature_.articularJacobianSIN.setReference(&Jq);

    manual_.resize(dim);
  }

  void setInputs(MatrixHomogeneous &s, MatrixHomogeneous &sd,
                 dynamicgraph::Vector &vd, double gain) {
    feature_.positionSIN = s;
    feature_.positionSIN.access(time_);
    feature_.positionSIN.setReady();

    featureDes_.positionSIN = sd;
    featureDes_.positionSIN.access(time_);
    featureDes_.positionSIN.setReady();

    featureDes_.velocitySIN = vd;
    featureDes_.velocitySIN.access(time_);
    featureDes_.velocitySIN.setReady();

    task_.controlGainSIN = gain;
    task_.controlGainSIN.access(time_);
    task_.controlGainSIN.setReady();
  }

  void printInputs() {
    std::cout << "----- inputs -----" << std::endl;
    std::cout << "feature_.position: " << feature_.positionSIN(time_)
              << std::endl;
    std::cout << "featureDes_.position: " << featureDes_.positionSIN(time_)
              << std::endl;
    std::cout << "featureDes_.velocity: "
              << featureDes_.velocitySIN(time_).transpose() << std::endl;
    std::cout << "task.controlGain: " << task_.controlGainSIN(time_)
              << std::endl;
  }

  int recompute() {
    feature_.errorSOUT.recompute(time_);
    feature_.errordotSOUT.recompute(time_);
    task_.taskSOUT.recompute(time_);
    task_.errorSOUT.recompute(time_);
    task_.errorTimeDerivativeSOUT.recompute(time_);
    time_++;
    MatrixHomogeneous s = feature_.positionSIN;
    MatrixHomogeneous sd = featureDes_.positionSIN;
    dynamicgraph::Vector vd = featureDes_.velocitySIN;
    double gain = task_.controlGainSIN;
    dynamicgraph::Vector manual;
    const std::vector<dynamicgraph::sot::MultiBound> &taskTaskSOUT =
        task_.taskSOUT(time_);

    /// Verify the computation of the desired frame.
    /// -gain *(s-sd) - ([w]x (sd -s)-vd)
    dynamicgraph::Matrix aM;
    dynamicgraph::Vector aV;
    aM.resize(3, 3);
    aV.resize(3);
    aM(0, 0) = 0.0;
    aM(0, 1) = -vd(5);
    aM(0, 2) = vd(4);
    aM(1, 0) = vd(5);
    aM(1, 1) = 0.0;
    aM(1, 2) = -vd(3);
    aM(2, 0) = -vd(4);
    aM(2, 1) = vd(3);
    aM(2, 2) = 0.0;
    for (unsigned int i = 0; i < 3; i++) aV(i) = sd(i, 3) - s(i, 3);

    aV = aM * aV;

    /// Recompute error_th.
    for (unsigned int i = 0; i < 3; i++) {
      manual_[i] = -gain * (s(i, 3) - sd(i, 3)) - (aV(i) - vd(i));
      if (manual_[i] != taskTaskSOUT[i].getSingleBound()) return -1;
    }
    return 0;
  }

  void printOutput() {
    std::cout << "----- output -----" << std::endl;
    std::cout << "time: " << time_ << std::endl;
    std::cout << "feature.errorSOUT: " << feature_.errorSOUT(time_).transpose()
              << std::endl;
    std::cout << "feature.errordotSOUT: "
              << feature_.errordotSOUT(time_).transpose() << std::endl;
    std::cout << "task.taskSOUT: " << task_.taskSOUT(time_) << std::endl;
    //    std::cout << "task.errorSOUT: " << task_.errorSOUT(time_)
    //<< std::endl;
    // std::cout << "task.errordtSOUT: " << task_.errorTimeDerivativeSOUT(time_)
    //<< std::endl;
    std::cout << "manual: " << manual_.transpose() << std::endl;
  }

  int runTest(MatrixHomogeneous &s, MatrixHomogeneous &sd,
              dynamicgraph::Vector &vd, double gain) {
    setInputs(s, sd, vd, gain);
    printInputs();
    int r = recompute();
    printOutput();
    return r;
  }
};

int main(void) {
  // Name of the robot
  std::string srobot("Robot");
  // Dimension of the robot.
  unsigned int dim = 6;
  // Feature and Desired Feature
  MatrixHomogeneous s, sd;
  // Desired velocity
  dynamicgraph::Vector vd(6);
  // Task gain.
  double gain;
  // Result of test
  int r;

  TestPoint6d testFeaturePoint6d(dim, srobot);

  std::cout << " ----- Test Velocity -----" << std::endl;
  s.setIdentity();
  sd.setIdentity();
  vd.setConstant(1.);
  gain = 0.0;

  if ((r = testFeaturePoint6d.runTest(s, sd, vd, gain)) < 0) {
    std::cerr << "Failure on 1st test." << std::endl;
    return r;
  }

  std::cout << " ----- Test Position -----" << std::endl;
  s.setIdentity();
  sd.setIdentity();
  sd.translation()[2] = 2.0;
  vd.setZero();
  gain = 1.0;

  if ((r = testFeaturePoint6d.runTest(s, sd, vd, gain)) < 0) {
    std::cerr << "Failure on 2nd test." << std::endl;
    return r;
  }

  std::cout << " ----- Test both -----" << std::endl;
  s.setIdentity();
  sd.setIdentity();
  sd.translation()[2] = 2.0;
  vd.setConstant(1.);
  gain = 3.0;

  if ((r = testFeaturePoint6d.runTest(s, sd, vd, gain)) < 0) {
    std::cerr << "Failure on 3th test." << std::endl;
    return r;
  }

  std::cout << " ----- Test both again -----" << std::endl;
  s.setIdentity();
  sd.setIdentity();
  sd.translation()[2] = 2.0;
  vd.setConstant(1.);
  gain = 3.0;

  if ((r = testFeaturePoint6d.runTest(s, sd, vd, gain)) < 0) {
    std::cerr << "Failure on 4th test." << std::endl;
    return r;
  }

  std::cout << "Test successfull !" << std::endl;
  return 0;
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2019,
3			* François Bleibel,
4			* Olivier Stasse,
5			*
6			* CNRS/AIST
7			*
8			*/
9
10			/* -------------------------------------------------------------------------- */
11			/* --- INCLUDES ------------------------------------------------------------- */
12			/* -------------------------------------------------------------------------- */
13			#include <dynamic-graph/linear-algebra.h>
14
15			#include <iostream>
16			#include <sot/core/debug.hh>
17			#include <sot/core/feature-abstract.hh>
18			#include <sot/core/feature-point6d.hh>
19			#include <sot/core/gain-adaptive.hh>
20			#include <sot/core/macros.hh>
21			#include <sot/core/sot.hh>
22			#include <sot/core/task.hh>
23			using namespace std;
24			using namespace dynamicgraph::sot;
25
26			class TestPoint6d {
27			public:
28			SOT_CORE_DISABLE_WARNING_PUSH
29			SOT_CORE_DISABLE_WARNING_DEPRECATED
30			FeaturePoint6d feature_, featureDes_;
31			SOT_CORE_DISABLE_WARNING_POP
32			Task task_;
33			int time_;
34			int dim_, robotDim_, featureDim_;
35			dynamicgraph::Vector manual_;
36
37		1	TestPoint6d(unsigned dim, std::string &name)
38		1	: feature_("feature" + name),
39		1	featureDes_("featureDes" + name),
40		1	task_("task" + name),
41	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	4	time_(0)
42
43			{
44	✓✗✓✗	1	feature_.computationFrame("desired");
45	✓✗	1	feature_.setReference(&featureDes_);
46	✓✗✓✗	1	feature_.selectionSIN = Flags(true);
47
48	✓✗	1	task_.addFeature(feature_);
49	✓✗	1	task_.setWithDerivative(true);
50		1	dim_ = dim;
51		1	robotDim_ = featureDim_ = dim;
52
53	✓✗	2	dynamicgraph::Matrix Jq(dim, dim);
54	✓✗	1	Jq.setIdentity();
55	✓✗	1	feature_.articularJacobianSIN.setReference(&Jq);
56
57	✓✗	1	manual_.resize(dim);
58		1	}
59
60		4	void setInputs(MatrixHomogeneous &s, MatrixHomogeneous &sd,
61			dynamicgraph::Vector &vd, double gain) {
62		4	feature_.positionSIN = s;
63		4	feature_.positionSIN.access(time_);
64		4	feature_.positionSIN.setReady();
65
66		4	featureDes_.positionSIN = sd;
67		4	featureDes_.positionSIN.access(time_);
68		4	featureDes_.positionSIN.setReady();
69
70		4	featureDes_.velocitySIN = vd;
71		4	featureDes_.velocitySIN.access(time_);
72		4	featureDes_.velocitySIN.setReady();
73
74		4	task_.controlGainSIN = gain;
75		4	task_.controlGainSIN.access(time_);
76		4	task_.controlGainSIN.setReady();
77		4	}
78
79		4	void printInputs() {
80		4	std::cout << "----- inputs -----" << std::endl;
81	✓✗	4	std::cout << "feature_.position: " << feature_.positionSIN(time_)
82	✓✗	4	<< std::endl;
83	✓✗	4	std::cout << "featureDes_.position: " << featureDes_.positionSIN(time_)
84	✓✗	4	<< std::endl;
85		4	std::cout << "featureDes_.velocity: "
86	✓✗✓✗	4	<< featureDes_.velocitySIN(time_).transpose() << std::endl;
87		4	std::cout << "task.controlGain: " << task_.controlGainSIN(time_)
88		4	<< std::endl;
89		4	}
90
91		4	int recompute() {
92	✓✗	4	feature_.errorSOUT.recompute(time_);
93	✓✗	4	feature_.errordotSOUT.recompute(time_);
94	✓✗	4	task_.taskSOUT.recompute(time_);
95	✓✗	4	task_.errorSOUT.recompute(time_);
96	✓✗	4	task_.errorTimeDerivativeSOUT.recompute(time_);
97		4	time_++;
98	✓✗✓✗	4	MatrixHomogeneous s = feature_.positionSIN;
99	✓✗✓✗	4	MatrixHomogeneous sd = featureDes_.positionSIN;
100	✓✗✓✗	8	dynamicgraph::Vector vd = featureDes_.velocitySIN;
101	✓✗	4	double gain = task_.controlGainSIN;
102	✓✗	8	dynamicgraph::Vector manual;
103			const std::vector<dynamicgraph::sot::MultiBound> &taskTaskSOUT =
104	✓✗	4	task_.taskSOUT(time_);
105
106			/// Verify the computation of the desired frame.
107			/// -gain *(s-sd) - ([w]x (sd -s)-vd)
108	✓✗	8	dynamicgraph::Matrix aM;
109	✓✗	8	dynamicgraph::Vector aV;
110	✓✗	4	aM.resize(3, 3);
111	✓✗	4	aV.resize(3);
112	✓✗	4	aM(0, 0) = 0.0;
113	✓✗✓✗	4	aM(0, 1) = -vd(5);
114	✓✗✓✗	4	aM(0, 2) = vd(4);
115	✓✗✓✗	4	aM(1, 0) = vd(5);
116	✓✗	4	aM(1, 1) = 0.0;
117	✓✗✓✗	4	aM(1, 2) = -vd(3);
118	✓✗✓✗	4	aM(2, 0) = -vd(4);
119	✓✗✓✗	4	aM(2, 1) = vd(3);
120	✓✗	4	aM(2, 2) = 0.0;
121	✓✓✓✗ ✓✗✓✗	16	for (unsigned int i = 0; i < 3; i++) aV(i) = sd(i, 3) - s(i, 3);
122
123	✓✗✓✗	4	aV = aM * aV;
124
125			/// Recompute error_th.
126	✓✓	16	for (unsigned int i = 0; i < 3; i++) {
127	✓✗✓✗ ✓✗✓✗ ✓✗	12	manual_[i] = -gain * (s(i, 3) - sd(i, 3)) - (aV(i) - vd(i));
128	✓✗✓✗ ✗✓	12	if (manual_[i] != taskTaskSOUT[i].getSingleBound()) return -1;
129			}
130		4	return 0;
131			}
132
133		4	void printOutput() {
134		4	std::cout << "----- output -----" << std::endl;
135		4	std::cout << "time: " << time_ << std::endl;
136	✓✗	4	std::cout << "feature.errorSOUT: " << feature_.errorSOUT(time_).transpose()
137	✓✗	4	<< std::endl;
138		4	std::cout << "feature.errordotSOUT: "
139	✓✗✓✗	4	<< feature_.errordotSOUT(time_).transpose() << std::endl;
140		4	std::cout << "task.taskSOUT: " << task_.taskSOUT(time_) << std::endl;
141			// std::cout << "task.errorSOUT: " << task_.errorSOUT(time_)
142			//<< std::endl;
143			// std::cout << "task.errordtSOUT: " << task_.errorTimeDerivativeSOUT(time_)
144			//<< std::endl;
145	✓✗✓✗	4	std::cout << "manual: " << manual_.transpose() << std::endl;
146		4	}
147
148		4	int runTest(MatrixHomogeneous &s, MatrixHomogeneous &sd,
149			dynamicgraph::Vector &vd, double gain) {
150		4	setInputs(s, sd, vd, gain);
151		4	printInputs();
152		4	int r = recompute();
153		4	printOutput();
154		4	return r;
155			}
156			};
157
158		1	int main(void) {
159			// Name of the robot
160	✓✗	2	std::string srobot("Robot");
161			// Dimension of the robot.
162		1	unsigned int dim = 6;
163			// Feature and Desired Feature
164	✓✗✓✗	1	MatrixHomogeneous s, sd;
165			// Desired velocity
166	✓✗	2	dynamicgraph::Vector vd(6);
167			// Task gain.
168			double gain;
169			// Result of test
170			int r;
171
172	✓✗	2	TestPoint6d testFeaturePoint6d(dim, srobot);
173
174	✓✗✓✗	1	std::cout << " ----- Test Velocity -----" << std::endl;
175	✓✗	1	s.setIdentity();
176	✓✗	1	sd.setIdentity();
177	✓✗	1	vd.setConstant(1.);
178		1	gain = 0.0;
179
180	✓✗✗✓	1	if ((r = testFeaturePoint6d.runTest(s, sd, vd, gain)) < 0) {
181			std::cerr << "Failure on 1st test." << std::endl;
182			return r;
183			}
184
185	✓✗✓✗	1	std::cout << " ----- Test Position -----" << std::endl;
186	✓✗	1	s.setIdentity();
187	✓✗	1	sd.setIdentity();
188	✓✗✓✗	1	sd.translation()[2] = 2.0;
189	✓✗	1	vd.setZero();
190		1	gain = 1.0;
191
192	✓✗✗✓	1	if ((r = testFeaturePoint6d.runTest(s, sd, vd, gain)) < 0) {
193			std::cerr << "Failure on 2nd test." << std::endl;
194			return r;
195			}
196
197	✓✗✓✗	1	std::cout << " ----- Test both -----" << std::endl;
198	✓✗	1	s.setIdentity();
199	✓✗	1	sd.setIdentity();
200	✓✗✓✗	1	sd.translation()[2] = 2.0;
201	✓✗	1	vd.setConstant(1.);
202		1	gain = 3.0;
203
204	✓✗✗✓	1	if ((r = testFeaturePoint6d.runTest(s, sd, vd, gain)) < 0) {
205			std::cerr << "Failure on 3th test." << std::endl;
206			return r;
207			}
208
209	✓✗✓✗	1	std::cout << " ----- Test both again -----" << std::endl;
210	✓✗	1	s.setIdentity();
211	✓✗	1	sd.setIdentity();
212	✓✗✓✗	1	sd.translation()[2] = 2.0;
213	✓✗	1	vd.setConstant(1.);
214		1	gain = 3.0;
215
216	✓✗✗✓	1	if ((r = testFeaturePoint6d.runTest(s, sd, vd, gain)) < 0) {
217			std::cerr << "Failure on 4th test." << std::endl;
218			return r;
219			}
220
221	✓✗✓✗	1	std::cout << "Test successfull !" << std::endl;
222		1	return 0;
223			}