sot-core
4.11.6
Hierarchical task solver plug-in for dynamic-graph.
|
Go to the documentation of this file.
14 #include <boost/mpl/if.hpp>
15 #include <boost/type_traits/is_same.hpp>
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>
22 #include <pinocchio/multibody/liegroup/liegroup.hpp>
33 typedef pinocchio::CartesianProductOperation<
34 pinocchio::VectorSpaceOperationTpl<3, double>,
35 pinocchio::SpecialOrthogonalOperationTpl<3, double> >
37 typedef pinocchio::SpecialEuclideanOperationTpl<3, double>
SE3_t;
40 template <Representation_t representation>
struct LG_t {
52 template <Representation_t representation>
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"),
62 CLASS_NAME +
"(" + name +
")::input(matrixHomo)::faMfbDes"),
64 CLASS_NAME +
"(" + name +
")::input(vector)::faNufafbDes"),
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,
71 faMfb, CLASS_NAME +
"(" + name +
")::output(vector7)::q_faMfb"),
72 q_faMfbDes(boost::bind(&
FeaturePose<representation>::computeQfaMfbDes,
75 CLASS_NAME +
"(" + name +
")::output(vector7)::q_faMfbDes") {
77 jaMfa.setConstant(Id);
78 jbMfb.setConstant(Id);
96 using namespace dynamicgraph::command;
101 "modify the desired position to servo at current pos.",
106 template <Representation_t representation>
113 template <Representation_t representation>
116 assert(ft.
oMja.isPlugged());
117 assert(ft.
jaMfa.isPlugged());
118 assert(ft.
oMjb.isPlugged());
119 assert(ft.
jbMfb.isPlugged());
124 template <Representation_t representation>
127 sotDEBUG(25) <<
"# In {" << std::endl;
132 for (
int i = 0; i < 6; ++i)
136 sotDEBUG(25) <<
"# Out }" << std::endl;
141 v.head<3>() = M.translation();
151 template <Representation_t representation>
163 const Matrix &_jbJjb =
jbJjb(time);
166 (
jbMfb.isPlugged() ?
jbMfb.accessCopy() : Id);
172 Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jminus;
174 buildFrom(_jbMfb.inverse(Eigen::Affine), X);
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>(
186 for (
unsigned int r = 0; r < 6; ++r)
188 J.row(rJ++) = (Jminus * X).row(r) * _jbJjb;
190 if (
jaJja.isPlugged()) {
191 const Matrix &_jaJja =
jaJja(time);
193 (
jaMfa.isPlugged() ?
jaMfa.accessCopy() : Id),
194 _faMfb =
faMfb.accessCopy();
196 buildFrom((_jaMfa * _faMfb).inverse(Eigen::Affine), X);
197 if (boost::is_same<LieGroup_t, R3xSO3_t>::value)
198 X.topRows<3>().applyOnTheLeft(faRfb);
202 for (
unsigned int r = 0; r < 6; ++r)
204 J.row(rJ++).noalias() -= (Jminus * X).row(r) * _jaJja;
210 template <Representation_t representation>
215 res = (
oMja(time) *
jaMfa(time)).inverse(Eigen::Affine) *
oMjb(time) *
220 template <Representation_t representation>
221 Vector7 &FeaturePose<representation>::computeQfaMfb(
Vector7 &res,
int time) {
228 template <Representation_t representation>
229 Vector7 &FeaturePose<representation>::computeQfaMfbDes(
Vector7 &res,
int time) {
236 template <Representation_t representation>
243 Eigen::Matrix<double, 6, 1> v;
247 unsigned int cursor = 0;
248 for (
unsigned int i = 0; i < 6; ++i)
250 error(cursor++) = v(i);
264 buildFrom(Mdes.inverse(Eigen::Affine), X);
274 nu.tail<3>() = Mdes.rotation().transpose() *
faNufafbDes.tail<3>();
278 template <Representation_t representation>
297 Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jminus;
299 LieGroup_t().template dDifference<pinocchio::ARG0>(
301 Vector6d nu = convertVelocity<LieGroup_t>(
faMfb(time), _faMfbDes,
303 unsigned int cursor = 0;
304 for (
unsigned int i = 0; i < 6; ++i)
306 errordot(cursor++) = Jminus.row(i) * nu;
314 template <Representation_t representation>
320 (
jaMfa.isPlugged() ?
jaMfa.access(time) : Id),
321 _oMjb =
oMjb.access(time),
323 (
jbMfb.isPlugged() ?
jbMfb.access(time) : Id);
324 faMfbDes = (_oMja * _jaMfa).inverse(Eigen::Affine) * _oMjb * _jbMfb;
327 static const char *featureNames[] = {
"X ",
"Y ",
"Z ",
"RX",
"RY",
"RZ"};
328 template <Representation_t representation>
335 for (
int i = 0; i < 6; ++i)
342 os << featureNames[i];
346 os <<
" selectSIN not set.";
dynamicgraph::SignalPtr< Matrix, int > jaJja
Jacobian of the input Joint A, expressed in Joint A
Definition: feature-pose.hh:81
Eigen::Map< Quaternion > SOT_CORE_EXPORT QuaternionMap
Definition: matrix-geometry.hh:85
unsigned int getDimension(void) const
Shortest method.
Definition: feature-abstract.hh:120
virtual dynamicgraph::Vector & computeErrorDot(dynamicgraph::Vector &res, int time)
Definition: feature-pose.hxx:279
dynamicgraph::SignalPtr< Matrix, int > jbJjb
Jacobian of the input Joint B, expressed in Joint B
Definition: feature-pose.hh:83
@ SE3Representation
Definition: feature-pose.hh:29
virtual dynamicgraph::Matrix & computeJacobian(dynamicgraph::Matrix &res, int time)
Definition: feature-pose.hxx:152
Eigen::Transform< double, 3, Eigen::Affine > SOT_CORE_EXPORT MatrixHomogeneous
Definition: matrix-geometry.hh:74
SignalTimeDependent< dynamicgraph::Matrix, int > jacobianSOUT
Jacobian of the error wrt the robot state: .
Definition: feature-abstract.hh:192
SignalTimeDependent< dynamicgraph::Vector, int > errorSOUT
This signal returns the error between the desired value and the current value : .
Definition: feature-abstract.hh:184
Definition: exception-abstract.hh:35
Definition: abstract-sot-external-interface.hh:17
virtual void display(std::ostream &os) const
Definition: feature-pose.hxx:329
Eigen::VectorXd::Index Index
Definition: robot-utils.hh:39
pinocchio::SpecialEuclideanOperationTpl< 3, double > SE3_t
Definition: feature-pose.hxx:37
SignalTimeDependent< Vector7, int > q_faMfbDes
Definition: feature-pose.hh:104
void toVector(const MatrixHomogeneous &M, Vector7 &v)
Definition: feature-pose.hxx:140
SignalTimeDependent< Vector7, int > q_faMfb
Definition: feature-pose.hh:100
Vector6d convertVelocity< SE3_t >(const MatrixHomogeneous &M, const MatrixHomogeneous &Mdes, const Vector &faNufafbDes)
Definition: feature-pose.hxx:259
dynamicgraph::SignalPtr< Vector, int > faNufafbDes
Definition: feature-pose.hh:89
boost::mpl::if_c< representation==SE3Representation, SE3_t, R3xSO3_t >::type type
Definition: feature-pose.hxx:42
This class gives the abstract definition of a feature.
Definition: feature-abstract.hh:75
pinocchio::CartesianProductOperation< pinocchio::VectorSpaceOperationTpl< 3, double >, pinocchio::SpecialOrthogonalOperationTpl< 3, double > > R3xSO3_t
Definition: feature-pose.hxx:36
SignalTimeDependent< dynamicgraph::Vector, int > errordotSOUT
Derivative of the error with respect to time: .
Definition: feature-abstract.hh:188
dynamicgraph::SignalPtr< MatrixHomogeneous, int > jbMfb
Input pose of Frame B wrt to Joint B.
Definition: feature-pose.hh:79
void buildFrom(const MatrixHomogeneous &MH, MatrixTwist &MT)
Definition: matrix-geometry.hh:87
Vector6d convertVelocity< R3xSO3_t >(const MatrixHomogeneous &M, const MatrixHomogeneous &Mdes, const Vector &faNufafbDes)
Definition: feature-pose.hxx:268
dynamicgraph::SignalPtr< MatrixHomogeneous, int > jaMfa
Input pose of Frame A wrt to Joint A.
Definition: feature-pose.hh:75
Feature that controls the relative (or absolute) pose between two frames A (or world) and B.
Definition: feature-pose.hh:62
void servoCurrentPosition(const int &time)
Definition: feature-pose.hxx:315
static const std::string CLASS_NAME
Definition: feature-pose.hh:65
Definition: feature-pose.hxx:40
Eigen::Matrix< double, 6, 6 > SOT_CORE_EXPORT MatrixTwist
Definition: matrix-geometry.hh:81
dynamicgraph::SignalPtr< MatrixHomogeneous, int > oMja
Input pose of Joint A wrt to world frame.
Definition: feature-pose.hh:73
Eigen::Matrix< double, 3, 3 > SOT_CORE_EXPORT MatrixRotation
Definition: matrix-geometry.hh:75
Eigen::Matrix< double, 7, 1 > SOT_CORE_EXPORT Vector7
Definition: matrix-geometry.hh:83
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
SignalTimeDependent< MatrixHomogeneous, int > faMfb
Pose of Frame B wrt to Frame A.
Definition: feature-pose.hh:96
dynamicgraph::SignalPtr< MatrixHomogeneous, int > oMjb
Input pose of Joint B wrt to world frame.
Definition: feature-pose.hh:77
virtual dynamicgraph::Vector & computeError(dynamicgraph::Vector &res, int time)
Computes .
Definition: feature-pose.hxx:237
SignalTimeDependent< unsigned int, int > dimensionSOUT
Returns the dimension of the feature as an output signal.
Definition: feature-abstract.hh:195
FeaturePose(const std::string &name)
Definition: feature-pose.hxx:53
virtual ~FeaturePose(void)
Definition: feature-pose.hxx:107
dynamicgraph::SignalPtr< MatrixHomogeneous, int > faMfbDes
The desired pose of Frame B wrt to Frame A.
Definition: feature-pose.hh:86
#define sotDEBUG(level)
Definition: debug.hh:167