feature-generic.cpp
Go to the documentation of this file.
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>
17 #include <sot/core/factory.hh>
19 
20 using namespace std;
21 using namespace dynamicgraph::sot;
22 using namespace dynamicgraph;
24 
25 /* --------------------------------------------------------------------- */
26 /* --- CLASS ----------------------------------------------------------- */
27 /* --------------------------------------------------------------------- */
28 
29 FeatureGeneric::FeatureGeneric(const string &pointName)
30  : FeatureAbstract(pointName),
31  dimensionDefault(0),
32  errorSIN(NULL, "sotFeatureGeneric(" + name + ")::input(vector)::errorIN"),
33  jacobianSIN(NULL,
34  "sotFeatureGeneric(" + name + ")::input(matrix)::jacobianIN")
35 
36 {
39 
41 }
42 
43 /* --------------------------------------------------------------------- */
44 /* --------------------------------------------------------------------- */
45 /* --------------------------------------------------------------------- */
46 
48  assert(SP::isReferenceSet());
51 }
52 
54  assert(SP::isReferenceSet());
57 }
58 
59 /* --------------------------------------------------------------------- */
60 /* --------------------------------------------------------------------- */
61 /* --------------------------------------------------------------------- */
62 
64  sotDEBUG(25) << "# In {" << endl;
65 
66  const Flags &fl = selectionSIN.access(time);
67 
68  if (dimensionDefault == 0) dimensionDefault = errorSIN.access(time).size();
69 
70  dim = 0;
71  for (size_type i = 0; i < dimensionDefault; ++i)
72  if (fl(i)) dim++;
73 
74  sotDEBUG(25) << "# Out }" << endl;
75  return dim;
76 }
77 
79  const Vector &err = errorSIN.access(time);
80  const Flags &fl = selectionSIN.access(time);
81  const size_type &dim = dimensionSOUT(time);
82 
83  std::size_t curr = 0;
84  res.resize(dim);
85  if (err.size() < dim) {
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  if (isReferenceSet()) {
97  const Vector &errDes = getReference()->errorSIN(time);
98  sotDEBUG(15) << "Err* = " << errDes;
99  if (errDes.size() < dim) {
102  "Error: dimension uncompatible with des->errorIN size."
103  " (while considering feature <%s>).",
104  getName().c_str());
105  }
106 
107  for (size_type i = 0; i < err.size(); ++i)
108  if (fl(i))
109  if (fl(i)) res(curr++) = err(i) - errDes(i);
110  } else {
111  for (size_type i = 0; i < err.size(); ++i)
112  if (fl(i)) res(curr++) = err(i);
113  }
114 
115  return res;
116 }
117 
119  sotDEBUGIN(15);
120 
121  const Matrix &Jac = jacobianSIN.access(time);
122  const Flags &fl = selectionSIN.access(time);
123  const std::size_t &dim = dimensionSOUT(time);
124 
125  std::size_t curr = 0;
126  res.resize(dim, Jac.cols());
127 
128  for (std::size_t i = 0; curr < dim; ++i)
129  if (fl(i)) {
130  for (size_type 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 }
dynamicgraph::sot::FeatureAbstract::addDependenciesFromReference
virtual void addDependenciesFromReference(void)=0
factory.hh
dynamicgraph::SignalPtr::accessCopy
virtual const T & accessCopy() const
dynamicgraph::sot::FeatureAbstract::errorSOUT
SignalTimeDependent< dynamicgraph::Vector, sigtime_t > errorSOUT
This signal returns the error between the desired value and the current value : .
Definition: feature-abstract.hh:185
dynamicgraph
dynamicgraph::sot::FeatureReferenceHelper< FeatureGeneric >::getReference
FeatureGeneric * getReference(void)
Definition: feature-abstract.hh:240
dynamicgraph::sot::FeatureGeneric::jacobianSIN
dynamicgraph::SignalPtr< dynamicgraph::Matrix, sigtime_t > jacobianSIN
Input for the Jacobian.
Definition: feature-generic.hh:80
dynamicgraph::sot::FeatureGeneric::computeJacobian
virtual dynamicgraph::Matrix & computeJacobian(dynamicgraph::Matrix &res, sigtime_t time)
Compute the Jacobian of the value according to the robot state..
Definition: feature-generic.cpp:118
i
int i
dynamicgraph::sot::FeatureGeneric::display
virtual void display(std::ostream &os) const
Display the information related to this generic implementation.
Definition: feature-generic.cpp:142
dynamicgraph::sot::FeatureAbstract::selectionSIN
SignalPtr< Flags, sigtime_t > selectionSIN
This vector specifies which dimension are used to perform the computation. For instance let us assume...
Definition: feature-abstract.hh:173
dynamicgraph::Entity::name
std::string name
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureGeneric, "FeatureGeneric")
dynamicgraph::sot::FeatureGeneric
Class that defines a generic implementation of the abstract interface for features.
Definition: feature-generic.hh:56
dynamicgraph::Matrix
Eigen::MatrixXd Matrix
dynamicgraph::sot::FeatureAbstract::jacobianSOUT
SignalTimeDependent< dynamicgraph::Matrix, sigtime_t > jacobianSOUT
Jacobian of the error wrt the robot state: .
Definition: feature-abstract.hh:193
dynamicgraph::sot::FeatureAbstract
This class gives the abstract definition of a feature.
Definition: feature-abstract.hh:76
dynamicgraph::Entity::getName
const std::string & getName() const
debug.hh
res
res
dynamicgraph::SignalPtr::access
virtual const T & access(const Time &t)
sotDEBUGOUT
#define sotDEBUGOUT(level)
Definition: debug.hh:215
dim
int dim
dynamicgraph::sigtime_t
int64_t sigtime_t
dynamicgraph::sot::FeatureAbstract::errordotSOUT
SignalTimeDependent< dynamicgraph::Vector, sigtime_t > errordotSOUT
Derivative of the error with respect to time: .
Definition: feature-abstract.hh:189
dynamicgraph::SignalTimeDependent::addDependency
virtual void addDependency(const SignalBase< Time > &signal)
sotDEBUGIN
#define sotDEBUGIN(level)
Definition: debug.hh:214
dynamicgraph::sot::FeatureGeneric::dimensionDefault
dynamicgraph::Vector::Index dimensionDefault
Definition: feature-generic.hh:66
dynamicgraph::SignalTimeDependent::removeDependency
virtual void removeDependency(const SignalBase< Time > &signal)
dynamicgraph::size_type
Matrix::Index size_type
dynamicgraph::Vector
Eigen::VectorXd Vector
dynamicgraph::sot::FeatureGeneric::errorSIN
dynamicgraph::SignalPtr< dynamicgraph::Vector, sigtime_t > errorSIN
Input for the error.
Definition: feature-generic.hh:77
dynamicgraph::sot::FeatureAbstract::removeDependenciesFromReference
virtual void removeDependenciesFromReference(void)=0
dynamicgraph::ExceptionSignal
exception-feature.hh
dynamicgraph::sot::FeatureGeneric::computeError
virtual dynamicgraph::Vector & computeError(dynamicgraph::Vector &res, sigtime_t time)
Compute the error between the desired value and the value itself.
Definition: feature-generic.cpp:78
dynamicgraph::sot::ExceptionFeature
Definition: exception-feature.hh:29
feature-generic.hh
err
err
dynamicgraph::sot
dynamicgraph::sot::FeatureAbstract::getDimension
size_type getDimension(void) const
Shortest method.
Definition: feature-abstract.hh:121
dynamicgraph::sot::FeatureAbstract::dimensionSOUT
SignalTimeDependent< size_type, sigtime_t > dimensionSOUT
Returns the dimension of the feature as an output signal.
Definition: feature-abstract.hh:196
dynamicgraph::sot::FeatureAbstract::errordotSIN
SignalPtr< dynamicgraph::Vector, sigtime_t > errordotSIN
Derivative of the reference value.
Definition: feature-abstract.hh:176
dynamicgraph::sot::Flags
Definition: flags.hh:33
dynamicgraph::ExceptionAbstract::what
virtual const char * what() const
SOT_THROW
#define SOT_THROW
Definition: exception-abstract.hh:130
dynamicgraph::sot::FeatureAbstract::isReferenceSet
virtual bool isReferenceSet(void) const
Definition: feature-abstract.hh:217
dynamicgraph::Entity::signalRegistration
void signalRegistration(const SignalArray< sigtime_t > &signals)
compile.name
name
Definition: compile.py:23
dynamicgraph::sot::ExceptionFeature::UNCOMPATIBLE_SIZE
@ UNCOMPATIBLE_SIZE
Definition: exception-feature.hh:36
sotDEBUG
#define sotDEBUG(level)
Definition: debug.hh:168


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Tue Oct 24 2023 02:26:31