Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/feature/feature-generic.cpp	Lines:	33	61	54.1 %
Date:	2023-03-13 12:09:37	Branches:	31	108	28.7 %


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

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* --- SOT --- */
#include <sot/core/debug.hh>
#include <sot/core/exception-feature.hh>
#include <sot/core/factory.hh>
#include <sot/core/feature-generic.hh>

using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureGeneric, "FeatureGeneric");

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

FeatureGeneric::FeatureGeneric(const string &pointName)
    : FeatureAbstract(pointName),
      dimensionDefault(0),
      errorSIN(NULL, "sotFeatureGeneric(" + name + ")::input(vector)::errorIN"),
      jacobianSIN(NULL,
                  "sotFeatureGeneric(" + name + ")::input(matrix)::jacobianIN")

{
  jacobianSOUT.addDependency(jacobianSIN);
  errorSOUT.addDependency(errorSIN);

  signalRegistration(errorSIN << jacobianSIN << errordotSIN << errordotSOUT);
}

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

void FeatureGeneric::addDependenciesFromReference(void) {
  assert(SP::isReferenceSet());
  errorSOUT.addDependency(getReference()->errorSIN);
  errordotSOUT.addDependency(getReference()->errordotSIN);
}

void FeatureGeneric::removeDependenciesFromReference(void) {
  assert(SP::isReferenceSet());
  errorSOUT.removeDependency(getReference()->errorSIN);
  errordotSOUT.removeDependency(getReference()->errordotSIN);
}

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

unsigned int &FeatureGeneric::getDimension(unsigned int &dim, int time) {
  sotDEBUG(25) << "# In {" << endl;

  const Flags &fl = selectionSIN.access(time);

  if (dimensionDefault == 0) dimensionDefault = errorSIN.access(time).size();

  dim = 0;
  for (unsigned int i = 0; i < dimensionDefault; ++i)
    if (fl(i)) dim++;

  sotDEBUG(25) << "# Out }" << endl;
  return dim;
}

Vector &FeatureGeneric::computeError(Vector &res, int time) {
  const Vector &err = errorSIN.access(time);
  const Flags &fl = selectionSIN.access(time);
  const int &dim = dimensionSOUT(time);

  unsigned int curr = 0;
  res.resize(dim);
  if (err.size() < dim) {
    SOT_THROW ExceptionFeature(
        ExceptionFeature::UNCOMPATIBLE_SIZE,
        "Error: dimension uncompatible with des->errorIN size."
        " (while considering feature <%s>).",
        getName().c_str());
  }

  sotDEBUG(15) << "Err = " << err;
  sotDEBUG(25) << "Dim = " << dim << endl;

  if (isReferenceSet()) {
    const Vector &errDes = getReference()->errorSIN(time);
    sotDEBUG(15) << "Err* = " << errDes;
    if (errDes.size() < dim) {
      SOT_THROW ExceptionFeature(
          ExceptionFeature::UNCOMPATIBLE_SIZE,
          "Error: dimension uncompatible with des->errorIN size."
          " (while considering feature <%s>).",
          getName().c_str());
    }

    for (int i = 0; i < err.size(); ++i)
      if (fl(i))
        if (fl(i)) res(curr++) = err(i) - errDes(i);
  } else {
    for (int i = 0; i < err.size(); ++i)
      if (fl(i)) res(curr++) = err(i);
  }

  return res;
}

Matrix &FeatureGeneric::computeJacobian(Matrix &res, int time) {
  sotDEBUGIN(15);

  const Matrix &Jac = jacobianSIN.access(time);
  const Flags &fl = selectionSIN.access(time);
  const unsigned int &dim = dimensionSOUT(time);

  unsigned int curr = 0;
  res.resize(dim, Jac.cols());

  for (unsigned int i = 0; curr < dim; ++i)
    if (fl(i)) {
      for (int j = 0; j < Jac.cols(); ++j) res(curr, j) = Jac(i, j);
      curr++;
    }

  sotDEBUGOUT(15);
  return res;
}

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

void FeatureGeneric::display(std::ostream &os) const {
  os << "Generic <" << name << ">: " << std::endl;

  try {
    os << "  error= " << errorSIN.accessCopy() << endl
       << "  J    = " << jacobianSIN.accessCopy() << endl;
  } catch (ExceptionSignal &e) {
    os << e.what();
  }
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2010,
3			* François Bleibel,
4			* Olivier Stasse,
5			*
6			* CNRS/AIST
7			*
8			*/
9
10			/* --------------------------------------------------------------------- */
11			/* --- INCLUDE --------------------------------------------------------- */
12			/* --------------------------------------------------------------------- */
13
14			/* --- SOT --- */
15			#include <sot/core/debug.hh>
16			#include <sot/core/exception-feature.hh>
17			#include <sot/core/factory.hh>
18			#include <sot/core/feature-generic.hh>
19
20			using namespace std;
21			using namespace dynamicgraph::sot;
22			using namespace dynamicgraph;
23			DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureGeneric, "FeatureGeneric");
24
25			/* --------------------------------------------------------------------- */
26			/* --- CLASS ----------------------------------------------------------- */
27			/* --------------------------------------------------------------------- */
28
29		2	FeatureGeneric::FeatureGeneric(const string &pointName)
30			: FeatureAbstract(pointName),
31			dimensionDefault(0),
32	✓✗	4	errorSIN(NULL, "sotFeatureGeneric(" + name + ")::input(vector)::errorIN"),
33			jacobianSIN(NULL,
34	✓✗✓✗ ✓✗✓✗ ✓✗	6	"sotFeatureGeneric(" + name + ")::input(matrix)::jacobianIN")
35
36			{
37	✓✗	2	jacobianSOUT.addDependency(jacobianSIN);
38	✓✗	2	errorSOUT.addDependency(errorSIN);
39
40	✓✗✓✗ ✓✗✓✗	2	signalRegistration(errorSIN << jacobianSIN << errordotSIN << errordotSOUT);
41		2	}
42
43			/* --------------------------------------------------------------------- */
44			/* --------------------------------------------------------------------- */
45			/* --------------------------------------------------------------------- */
46
47		1	void FeatureGeneric::addDependenciesFromReference(void) {
48	✗✓	1	assert(SP::isReferenceSet());
49		1	errorSOUT.addDependency(getReference()->errorSIN);
50		1	errordotSOUT.addDependency(getReference()->errordotSIN);
51		1	}
52
53			void FeatureGeneric::removeDependenciesFromReference(void) {
54			assert(SP::isReferenceSet());
55			errorSOUT.removeDependency(getReference()->errorSIN);
56			errordotSOUT.removeDependency(getReference()->errordotSIN);
57			}
58
59			/* --------------------------------------------------------------------- */
60			/* --------------------------------------------------------------------- */
61			/* --------------------------------------------------------------------- */
62
63		1	unsigned int &FeatureGeneric::getDimension(unsigned int &dim, int time) {
64			sotDEBUG(25) << "# In {" << endl;
65
66		1	const Flags &fl = selectionSIN.access(time);
67
68	✓✗	1	if (dimensionDefault == 0) dimensionDefault = errorSIN.access(time).size();
69
70		1	dim = 0;
71	✓✓	7	for (unsigned int i = 0; i < dimensionDefault; ++i)
72	✓✗✓✗	6	if (fl(i)) dim++;
73
74			sotDEBUG(25) << "# Out }" << endl;
75		1	return dim;
76			}
77
78		10	Vector &FeatureGeneric::computeError(Vector &res, int time) {
79		10	const Vector &err = errorSIN.access(time);
80		10	const Flags &fl = selectionSIN.access(time);
81		10	const int &dim = dimensionSOUT(time);
82
83		10	unsigned int curr = 0;
84		10	res.resize(dim);
85	✗✓	10	if (err.size() < dim) {
86			SOT_THROW ExceptionFeature(
87			ExceptionFeature::UNCOMPATIBLE_SIZE,
88			"Error: dimension uncompatible with des->errorIN size."
89			" (while considering feature <%s>).",
90			getName().c_str());
91			}
92
93			sotDEBUG(15) << "Err = " << err;
94			sotDEBUG(25) << "Dim = " << dim << endl;
95
96	✓✗	10	if (isReferenceSet()) {
97		10	const Vector &errDes = getReference()->errorSIN(time);
98			sotDEBUG(15) << "Err* = " << errDes;
99	✗✓	10	if (errDes.size() < dim) {
100			SOT_THROW ExceptionFeature(
101			ExceptionFeature::UNCOMPATIBLE_SIZE,
102			"Error: dimension uncompatible with des->errorIN size."
103			" (while considering feature <%s>).",
104			getName().c_str());
105			}
106
107	✓✗✓✓	70	for (int i = 0; i < err.size(); ++i)
108	✓✗✓✗	60	if (fl(i))
109	✓✗✓✗ ✓✗✓✗ ✓✗	60	if (fl(i)) res(curr++) = err(i) - errDes(i);
110			} else {
111			for (int i = 0; i < err.size(); ++i)
112			if (fl(i)) res(curr++) = err(i);
113			}
114
115		10	return res;
116			}
117
118			Matrix &FeatureGeneric::computeJacobian(Matrix &res, int time) {
119			sotDEBUGIN(15);
120
121			const Matrix &Jac = jacobianSIN.access(time);
122			const Flags &fl = selectionSIN.access(time);
123			const unsigned int &dim = dimensionSOUT(time);
124
125			unsigned int curr = 0;
126			res.resize(dim, Jac.cols());
127
128			for (unsigned int i = 0; curr < dim; ++i)
129			if (fl(i)) {
130			for (int j = 0; j < Jac.cols(); ++j) res(curr, j) = Jac(i, j);
131			curr++;
132			}
133
134			sotDEBUGOUT(15);
135			return res;
136			}
137
138			/* --------------------------------------------------------------------- */
139			/* --------------------------------------------------------------------- */
140			/* --------------------------------------------------------------------- */
141
142			void FeatureGeneric::display(std::ostream &os) const {
143			os << "Generic <" << name << ">: " << std::endl;
144
145			try {
146			os << " error= " << errorSIN.accessCopy() << endl
147			<< " J = " << jacobianSIN.accessCopy() << endl;
148			} catch (ExceptionSignal &e) {
149			os << e.what();
150			}
151			}