sot-core  4.10.1
Hierarchical task solver plug-in for dynamic-graph.
feature-pose.hxx
Go to the documentation of this file.
1 /*
2  * Copyright 2019
3  * Joseph Mirabel
4  *
5  * LAAS-CNRS
6  *
7  */
8 
9 /* --------------------------------------------------------------------- */
10 /* --- INCLUDE --------------------------------------------------------- */
11 /* --------------------------------------------------------------------- */
12 
13 /* --- SOT --- */
14 #include <boost/mpl/if.hpp>
15 #include <boost/type_traits/is_same.hpp>
16 
17 #include <dynamic-graph/command-bind.h>
18 #include <dynamic-graph/command-getter.h>
19 #include <dynamic-graph/command-setter.h>
20 #include <dynamic-graph/command.h>
21 
22 #include <pinocchio/multibody/liegroup/liegroup.hpp>
23 
24 #include <Eigen/LU>
25 
26 #include <sot/core/debug.hh>
27 #include <sot/core/factory.hh>
28 #include <sot/core/feature-pose.hh>
29 
30 namespace dynamicgraph {
31 namespace sot {
32 
33 typedef pinocchio::CartesianProductOperation<
34  pinocchio::VectorSpaceOperationTpl<3, double>,
35  pinocchio::SpecialOrthogonalOperationTpl<3, double> >
37 typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t;
38 
39 namespace internal {
40 template <Representation_t representation> struct LG_t {
41  typedef typename boost::mpl::if_c<representation == SE3Representation, SE3_t,
43 };
44 } // namespace internal
45 
46 /* --------------------------------------------------------------------- */
47 /* --- CLASS ----------------------------------------------------------- */
48 /* --------------------------------------------------------------------- */
49 
50 static const MatrixHomogeneous Id(MatrixHomogeneous::Identity());
51 
52 template <Representation_t representation>
53 FeaturePose<representation>::FeaturePose(const std::string &pointName)
54  : FeatureAbstract(pointName),
55  oMja(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMja"),
56  jaMfa(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jaMfa"),
57  oMjb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMjb"),
58  jbMfb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jbMfb"),
59  jaJja(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jaJja"),
60  jbJjb(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jbJjb"),
61  faMfbDes(NULL,
62  CLASS_NAME + "(" + name + ")::input(matrixHomo)::faMfbDes"),
63  faNufafbDes(NULL,
64  CLASS_NAME + "(" + name + ")::input(vector)::faNufafbDes"),
65  faMfb(
66  boost::bind(&FeaturePose<representation>::computefaMfb, this, _1, _2),
67  oMja << jaMfa << oMjb << jbMfb,
68  CLASS_NAME + "(" + name + ")::output(matrixHomo)::faMfb"),
69  q_faMfb(boost::bind(&FeaturePose<representation>::computeQfaMfb, this, _1,
70  _2),
71  faMfb, CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfb"),
72  q_faMfbDes(boost::bind(&FeaturePose<representation>::computeQfaMfbDes,
73  this, _1, _2),
74  faMfbDes,
75  CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfbDes") {
76  oMja.setConstant(Id);
77  jaMfa.setConstant(Id);
78  jbMfb.setConstant(Id);
79  faMfbDes.setConstant(Id);
80  faNufafbDes.setConstant(Vector::Zero(6));
81 
82  jacobianSOUT.addDependencies(q_faMfbDes << q_faMfb << jaJja << jbJjb);
83 
84  errorSOUT.addDependencies(q_faMfbDes << q_faMfb);
85 
86  signalRegistration(oMja << jaMfa << oMjb << jbMfb << jaJja << jbJjb);
87  signalRegistration(faMfb << errordotSOUT << faMfbDes << faNufafbDes);
88 
89  errordotSOUT.setFunction(
90  boost::bind(&FeaturePose<representation>::computeErrorDot, this, _1, _2));
91  errordotSOUT.addDependencies(q_faMfbDes << q_faMfb << faNufafbDes);
92 
93  // Commands
94  //
95  {
96  using namespace dynamicgraph::command;
97  addCommand("keep",
98  makeCommandVoid1(
100  docCommandVoid1(
101  "modify the desired position to servo at current pos.",
102  "time")));
103  }
104 }
105 
106 template <Representation_t representation>
108 
109 /* --------------------------------------------------------------------- */
110 /* --------------------------------------------------------------------- */
111 /* --------------------------------------------------------------------- */
112 
113 template <Representation_t representation>
114 static inline void check(const FeaturePose<representation> &ft) {
115  (void)ft;
116  assert(ft.oMja.isPlugged());
117  assert(ft.jaMfa.isPlugged());
118  assert(ft.oMjb.isPlugged());
119  assert(ft.jbMfb.isPlugged());
120  assert(ft.faMfbDes.isPlugged());
121  assert(ft.faNufafbDes.isPlugged());
122 }
123 
124 template <Representation_t representation>
125 unsigned int &FeaturePose<representation>::getDimension(unsigned int &dim,
126  int time) {
127  sotDEBUG(25) << "# In {" << std::endl;
128 
129  const Flags &fl = selectionSIN.access(time);
130 
131  dim = 0;
132  for (int i = 0; i < 6; ++i)
133  if (fl(i))
134  dim++;
135 
136  sotDEBUG(25) << "# Out }" << std::endl;
137  return dim;
138 }
139 
140 void toVector(const MatrixHomogeneous &M, Vector7 &v) {
141  v.head<3>() = M.translation();
142  QuaternionMap(v.tail<4>().data()) = M.linear();
143 }
144 
146  Vector7 ret;
147  toVector(M, ret);
148  return ret;
149 }
150 
151 template <Representation_t representation>
152 Matrix &FeaturePose<representation>::computeJacobian(Matrix &J, int time) {
153  typedef typename internal::LG_t<representation>::type LieGroup_t;
154 
155  check(*this);
156 
157  q_faMfb.recompute(time);
158  q_faMfbDes.recompute(time);
159 
160  const unsigned int &dim = dimensionSOUT(time);
161  const Flags &fl = selectionSIN(time);
162 
163  const Matrix &_jbJjb = jbJjb(time);
164 
165  const MatrixHomogeneous &_jbMfb =
166  (jbMfb.isPlugged() ? jbMfb.accessCopy() : Id);
167 
168  const Matrix::Index cJ = _jbJjb.cols();
169  J.resize(dim, cJ);
170 
171  MatrixTwist X;
172  Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jminus;
173 
174  buildFrom(_jbMfb.inverse(Eigen::Affine), X);
175  MatrixRotation faRfb = jaMfa.access(time).rotation().transpose() *
176  oMja.access(time).rotation().transpose() *
177  oMjb.access(time).rotation() * _jbMfb.rotation();
178  if (boost::is_same<LieGroup_t, R3xSO3_t>::value)
179  X.topRows<3>().applyOnTheLeft(faRfb);
180  LieGroup_t().template dDifference<pinocchio::ARG1>(
181  q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus);
182 
183  // Contribution of b:
184  // J = Jminus * X * jbJjb;
185  unsigned int rJ = 0;
186  for (unsigned int r = 0; r < 6; ++r)
187  if (fl((int)r))
188  J.row(rJ++) = (Jminus * X).row(r) * _jbJjb;
189 
190  if (jaJja.isPlugged()) {
191  const Matrix &_jaJja = jaJja(time);
192  const MatrixHomogeneous &_jaMfa =
193  (jaMfa.isPlugged() ? jaMfa.accessCopy() : Id),
194  _faMfb = faMfb.accessCopy();
195 
196  buildFrom((_jaMfa * _faMfb).inverse(Eigen::Affine), X);
197  if (boost::is_same<LieGroup_t, R3xSO3_t>::value)
198  X.topRows<3>().applyOnTheLeft(faRfb);
199 
200  // J -= (Jminus * X) * jaJja(time);
201  rJ = 0;
202  for (unsigned int r = 0; r < 6; ++r)
203  if (fl((int)r))
204  J.row(rJ++).noalias() -= (Jminus * X).row(r) * _jaJja;
205  }
206 
207  return J;
208 }
209 
210 template <Representation_t representation>
213  check(*this);
214 
215  res = (oMja(time) * jaMfa(time)).inverse(Eigen::Affine) * oMjb(time) *
216  jbMfb(time);
217  return res;
218 }
219 
220 template <Representation_t representation>
222  check(*this);
223 
224  toVector(faMfb(time), res);
225  return res;
226 }
227 
228 template <Representation_t representation>
230  check(*this);
231 
232  toVector(faMfbDes(time), res);
233  return res;
234 }
235 
236 template <Representation_t representation>
237 Vector &FeaturePose<representation>::computeError(Vector &error, int time) {
238  typedef typename internal::LG_t<representation>::type LieGroup_t;
239  check(*this);
240 
241  const Flags &fl = selectionSIN(time);
242 
243  Eigen::Matrix<double, 6, 1> v;
244  LieGroup_t().difference(q_faMfbDes(time), q_faMfb(time), v);
245 
246  error.resize(dimensionSOUT(time));
247  unsigned int cursor = 0;
248  for (unsigned int i = 0; i < 6; ++i)
249  if (fl((int)i))
250  error(cursor++) = v(i);
251 
252  return error;
253 }
254 
255 // This function is responsible of converting the input velocity expressed with
256 // SE(3) convention onto a velocity expressed with the convention of this
257 // feature (R^3xSO(3) or SE(3)), in the right frame.
258 template <>
260  const MatrixHomogeneous &Mdes,
261  const Vector &faNufafbDes) {
262  (void)M;
263  MatrixTwist X;
264  buildFrom(Mdes.inverse(Eigen::Affine), X);
265  return X * faNufafbDes;
266 }
267 template <>
269  const MatrixHomogeneous &Mdes,
270  const Vector &faNufafbDes) {
271  Vector6d nu;
272  nu.head<3>() =
273  faNufafbDes.head<3>() - M.translation().cross(faNufafbDes.tail<3>());
274  nu.tail<3>() = Mdes.rotation().transpose() * faNufafbDes.tail<3>();
275  return nu;
276 }
277 
278 template <Representation_t representation>
280  int time) {
281  typedef typename internal::LG_t<representation>::type LieGroup_t;
282  check(*this);
283 
284  errordot.resize(dimensionSOUT(time));
285  const Flags &fl = selectionSIN(time);
286  if (!faNufafbDes.isPlugged()) {
287  errordot.setZero();
288  return errordot;
289  }
290 
291  q_faMfb.recompute(time);
292  q_faMfbDes.recompute(time);
293  faNufafbDes.recompute(time);
294 
295  const MatrixHomogeneous &_faMfbDes = faMfbDes(time);
296 
297  Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jminus;
298 
299  LieGroup_t().template dDifference<pinocchio::ARG0>(
300  q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus);
301  Vector6d nu = convertVelocity<LieGroup_t>(faMfb(time), _faMfbDes,
302  faNufafbDes.accessCopy());
303  unsigned int cursor = 0;
304  for (unsigned int i = 0; i < 6; ++i)
305  if (fl((int)i))
306  errordot(cursor++) = Jminus.row(i) * nu;
307 
308  return errordot;
309 }
310 
311 /* Modify the value of the reference (sdes) so that it corresponds
312  * to the current position. The effect on the servo is to maintain the
313  * current position and correct any drift. */
314 template <Representation_t representation>
316  check(*this);
317 
318  const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.access(time) : Id),
319  _jaMfa =
320  (jaMfa.isPlugged() ? jaMfa.access(time) : Id),
321  _oMjb = oMjb.access(time),
322  _jbMfb =
323  (jbMfb.isPlugged() ? jbMfb.access(time) : Id);
324  faMfbDes = (_oMja * _jaMfa).inverse(Eigen::Affine) * _oMjb * _jbMfb;
325 }
326 
327 static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"};
328 template <Representation_t representation>
329 void FeaturePose<representation>::display(std::ostream &os) const {
330  os << CLASS_NAME << "<" << name << ">: (";
331 
332  try {
333  const Flags &fl = selectionSIN.accessCopy();
334  bool first = true;
335  for (int i = 0; i < 6; ++i)
336  if (fl(i)) {
337  if (first) {
338  first = false;
339  } else {
340  os << ",";
341  }
342  os << featureNames[i];
343  }
344  os << ") ";
345  } catch (ExceptionAbstract e) {
346  os << " selectSIN not set.";
347  }
348 }
349 
350 } // namespace sot
351 } // namespace dynamicgraph
dynamicgraph::SignalPtr< MatrixHomogeneous, int > jaMfa
Input pose of Frame A wrt to Joint A.
Definition: feature-pose.hh:75
Eigen::Transform< double, 3, Eigen::Affine > SOT_CORE_EXPORT MatrixHomogeneous
Definition: matrix-geometry.hh:74
SignalTimeDependent< dynamicgraph::Vector, int > errordotSOUT
Derivative of the error with respect to time: .
Definition: feature-abstract.hh:188
SignalPtr< Flags, int > selectionSIN
This vector specifies which dimension are used to perform the computation. For instance let us assume...
Definition: feature-abstract.hh:172
virtual dynamicgraph::Vector & computeError(dynamicgraph::Vector &res, int time)
Computes .
Definition: feature-pose.hxx:237
Eigen::VectorXd::Index Index
Definition: robot-utils.hh:66
virtual dynamicgraph::Matrix & computeJacobian(dynamicgraph::Matrix &res, int time)
Definition: feature-pose.hxx:152
SignalTimeDependent< unsigned int, int > dimensionSOUT
Returns the dimension of the feature as an output signal.
Definition: feature-abstract.hh:195
SignalTimeDependent< Vector7, int > q_faMfb
Definition: feature-pose.hh:100
pinocchio::SpecialEuclideanOperationTpl< 3, double > SE3_t
Definition: feature-pose.hxx:37
void toVector(const MatrixHomogeneous &M, Vector7 &v)
Definition: feature-pose.hxx:140
dynamicgraph::SignalPtr< MatrixHomogeneous, int > oMja
Input pose of Joint A wrt to world frame.
Definition: feature-pose.hh:73
SignalTimeDependent< MatrixHomogeneous, int > faMfb
Pose of Frame B wrt to Frame A.
Definition: feature-pose.hh:96
void buildFrom(const MatrixHomogeneous &MH, MatrixTwist &MT)
Definition: matrix-geometry.hh:87
SignalTimeDependent< dynamicgraph::Matrix, int > jacobianSOUT
Jacobian of the error wrt the robot state: .
Definition: feature-abstract.hh:192
Vector6d convertVelocity< SE3_t >(const MatrixHomogeneous &M, const MatrixHomogeneous &Mdes, const Vector &faNufafbDes)
Definition: feature-pose.hxx:259
unsigned int getDimension(void) const
Shortest method.
Definition: feature-abstract.hh:120
dynamicgraph::SignalPtr< MatrixHomogeneous, int > jbMfb
Input pose of Frame B wrt to Joint B.
Definition: feature-pose.hh:79
Definition: exception-abstract.hh:35
pinocchio::CartesianProductOperation< pinocchio::VectorSpaceOperationTpl< 3, double >, pinocchio::SpecialOrthogonalOperationTpl< 3, double > > R3xSO3_t
Definition: feature-pose.hxx:36
SignalTimeDependent< Vector7, int > q_faMfbDes
Definition: feature-pose.hh:104
const std::string CLASS_NAME
Definition: feature-pose.hh:171
boost::mpl::if_c< representation==SE3Representation, SE3_t, R3xSO3_t >::type type
Definition: feature-pose.hxx:42
Vector6d convertVelocity< R3xSO3_t >(const MatrixHomogeneous &M, const MatrixHomogeneous &Mdes, const Vector &faNufafbDes)
Definition: feature-pose.hxx:268
void servoCurrentPosition(const int &time)
Definition: feature-pose.hxx:315
Eigen::Matrix< double, 6, 6 > SOT_CORE_EXPORT MatrixTwist
Definition: matrix-geometry.hh:81
#define sotDEBUG(level)
Definition: debug.hh:167
dynamicgraph::SignalPtr< Matrix, int > jaJja
Jacobian of the input Joint A, expressed in Joint A
Definition: feature-pose.hh:81
FeaturePose(const std::string &name)
Definition: feature-pose.hxx:53
Definition: flags.hh:31
This class gives the abstract definition of a feature.
Definition: feature-abstract.hh:75
Eigen::Matrix< double, 3, 3 > SOT_CORE_EXPORT MatrixRotation
Definition: matrix-geometry.hh:75
virtual void display(std::ostream &os) const
Definition: feature-pose.hxx:329
Eigen::Matrix< double, 7, 1 > SOT_CORE_EXPORT Vector7
Definition: matrix-geometry.hh:83
Feature that controls the relative (or absolute) pose between two frames A (or world) and B...
Definition: feature-pose.hh:62
dynamicgraph::SignalPtr< Matrix, int > jbJjb
Jacobian of the input Joint B, expressed in Joint B
Definition: feature-pose.hh:83
virtual dynamicgraph::Vector & computeErrorDot(dynamicgraph::Vector &res, int time)
Definition: feature-pose.hxx:279
dynamicgraph::SignalPtr< MatrixHomogeneous, int > faMfbDes
The desired pose of Frame B wrt to Frame A.
Definition: feature-pose.hh:86
virtual ~FeaturePose(void)
Definition: feature-pose.hxx:107
dynamicgraph::SignalPtr< MatrixHomogeneous, int > oMjb
Input pose of Joint B wrt to world frame.
Definition: feature-pose.hh:77
dynamicgraph::SignalPtr< Vector, int > faNufafbDes
Definition: feature-pose.hh:89
SignalTimeDependent< dynamicgraph::Vector, int > errorSOUT
This signal returns the error between the desired value and the current value : . ...
Definition: feature-abstract.hh:184
Definition: feature-pose.hxx:40
Eigen::Map< Quaternion > SOT_CORE_EXPORT QuaternionMap
Definition: matrix-geometry.hh:85
Definition: abstract-sot-external-interface.hh:17
Definition: feature-pose.hh:29