GCC Code Coverage Report

Directory: ./
File: src/math/op-point-modifier.cpp
Date: 2024-11-13 12:35:17
Exec Total Coverage
Lines: 39 64 60.9%
Branches: 56 180 31.1%
Line Branch Exec Source
1 /*
2 * Copyright 2010,
3 * François Bleibel,
4 * Olivier Stasse,
5 *
6 * CNRS/AIST
7 *
8 */
9
10 #include <dynamic-graph/all-commands.h>
11 #include <dynamic-graph/all-signals.h>
12 #include <dynamic-graph/factory.h>
13
14 #include <sot/core/matrix-geometry.hh>
15 #include <sot/core/op-point-modifier.hh>
16
17 using namespace std;
18 using namespace dynamicgraph::sot;
19 using namespace dynamicgraph;
20
21
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(OpPointModifier, "OpPointModifier");
22
23 /* --------------------------------------------------------------------- */
24 /* --------------------------------------------------------------------- */
25 /* --------------------------------------------------------------------- */
26
27 4 OpPointModifier::OpPointModifier(const std::string &name)
28 : Entity(name),
29
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 jacobianSIN(NULL,
30
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 "OpPointModifior(" + name + ")::input(matrix)::jacobianIN"),
31
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 positionSIN(
32
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 NULL, "OpPointModifior(" + name + ")::input(matrixhomo)::positionIN"),
33
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 4 jacobianSOUT(
34 boost::bind(&OpPointModifier::jacobianSOUT_function, this, _1, _2),
35 jacobianSIN,
36
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 "OpPointModifior(" + name + ")::output(matrix)::jacobian"),
37
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 4 positionSOUT(
38 boost::bind(&OpPointModifier::positionSOUT_function, this, _1, _2),
39 positionSIN,
40
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 "OpPointModifior(" + name + ")::output(matrixhomo)::position"),
41
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 transformation(),
42
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 isEndEffector(true) {
43 sotDEBUGIN(15);
44
45
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 signalRegistration(jacobianSIN << positionSIN << jacobianSOUT
46
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 << positionSOUT);
47 {
48 using namespace dynamicgraph::command;
49
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 addCommand(
50 "getTransformation",
51
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 makeDirectGetter(*this, &transformation.matrix(),
52
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
 8 docDirectGetter("transformation", "matrix 4x4 homo")));
53
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 addCommand(
54 "setTransformation",
55
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 makeDirectSetter(*this, &transformation.matrix(),
56
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
 8 docDirectSetter("dimension", "matrix 4x4 homo")));
57
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 addCommand("getEndEffector",
58
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 makeDirectGetter(*this, &isEndEffector,
59
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
 8 docDirectGetter("end effector mode", "bool")));
60
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 addCommand("setEndEffector",
61
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 makeDirectSetter(*this, &isEndEffector,
62
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
 8 docDirectSetter("end effector mode", "bool")));
63 }
64
65 sotDEBUGOUT(15);
66 4 }
67
68 4 dynamicgraph::Matrix &OpPointModifier::jacobianSOUT_function(
69 dynamicgraph::Matrix &res, const int &iter) {
70
1/2
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
 4 if (isEndEffector) {
71
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 const dynamicgraph::Matrix &aJa = jacobianSIN(iter);
72 4 const MatrixHomogeneous &aMb = transformation;
73
74
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 MatrixTwist bVa;
75
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 buildFrom(aMb.inverse(), bVa);
76
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 res = bVa * aJa; // res := bJb
77 4 return res;
78 } else {
79 /* Consider that the jacobian of point A in frame A is given: J = aJa
80 * and that homogenous transformation from A to B is given aMb in
81 * getTransfo() and homo transfo from 0 to A is given oMa in positionSIN.
82 * Then return oJb, the jacobian of point B expressed in frame O:
83 * oJb = ( oRa 0 ; 0 oRa ) * bVa * aJa
84 * = [ I skew(oAB); 0 I ] * oJa
85 * with oAB = oRb bAB = oRb (-bRa aAB ) = -oRa aAB, and aAB =
86 * translation(aMb).
87 */
88
89 ✗ const dynamicgraph::Matrix &oJa = jacobianSIN(iter);
90 ✗ const MatrixHomogeneous &aMb = transformation;
91 ✗ const MatrixHomogeneous &oMa = positionSIN(iter);
92 ✗ MatrixRotation oRa;
93 ✗ oRa = oMa.linear();
94 ✗ dynamicgraph::Vector aAB(3);
95 ✗ aAB = aMb.translation();
96 ✗ dynamicgraph::Vector oAB = oRa * aAB;
97
98 ✗ const dynamicgraph::Matrix::Index nq = oJa.cols();
99 ✗ res.resize(6, oJa.cols());
100 ✗ for (int j = 0; j < nq; ++j) {
101 /* This is a I*Jtrans + skew*Jrot product, unrolled by hand ... */
102 ✗ res(0, j) = oJa(0, j) - oAB(1) * oJa(2 + 3, j) + oAB(2) * oJa(1 + 3, j);
103 ✗ res(1, j) = oJa(1, j) - oAB(2) * oJa(0 + 3, j) + oAB(0) * oJa(2 + 3, j);
104 ✗ res(2, j) = oJa(2, j) - oAB(0) * oJa(1 + 3, j) + oAB(1) * oJa(0 + 3, j);
105 ✗ for (int i = 0; i < 3; ++i) {
106 ✗ res(i + 3, j) = oJa(i + 3, j);
107 }
108 }
109 ✗ return res; // res := 0Jb
110 }
111 }
112
113 4 MatrixHomogeneous &OpPointModifier::positionSOUT_function(
114 MatrixHomogeneous &res, const int &iter) {
115 sotDEBUGIN(15);
116 sotDEBUGIN(15) << iter << " " << positionSIN.getTime()
117 << positionSOUT.getTime() << endl;
118 4 const MatrixHomogeneous &position = positionSIN(iter);
119
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 res = position * transformation;
120 sotDEBUGOUT(15);
121 4 return res;
122 }
123
124 ✗ void OpPointModifier::setTransformation(const Eigen::Matrix4d &tr) {
125 ✗ transformation.matrix() = tr;
126 }
127 ✗ const Eigen::Matrix4d &OpPointModifier::getTransformation(void) {
128 ✗ return transformation.matrix();
129 }
130
131 /* The following function needs an access to a specific signal via
132 * the pool, using the signal path <entity.signal>. this functionnality
133 * is deprecated, and the following function will have to be removed
134 * in a near future. A similar functionality is available using
135 * the <setTransformation> mthod, bound in python.
136 */
137 #include <dynamic-graph/pool.h>
138 [[deprecated("use setTransformation")]] void
139 ✗ OpPointModifier::setTransformationBySignalName(std::istringstream &cmdArgs) {
140 Signal<Eigen::Matrix4d, int> &sig =
141 ✗ dynamic_cast<Signal<Eigen::Matrix4d, int> &>(
142 ✗ PoolStorage::getInstance()->getSignal(cmdArgs));
143 ✗ setTransformation(sig.accessCopy());
144 }
145