GCC Code Coverage Report

Directory: ./
File: src/tools/gripper-control.cpp
Date: 2024-08-13 12:13:25
Exec Total Coverage
Lines: 0 71 0.0%
Branches: 0 256 0.0%
Line Branch Exec Source
1 /*
2 * Copyright 2010,
3 * François Bleibel,
4 * Olivier Stasse,
5 *
6 * CNRS/AIST
7 *
8 */
9
10 #define ENABLE_RT_LOG
11
12 #include <dynamic-graph/all-commands.h>
13 #include <dynamic-graph/real-time-logger.h>
14
15 #include <sot/core/debug.hh>
16 #include <sot/core/factory.hh>
17 #include <sot/core/gripper-control.hh>
18 #include <sot/core/macros-signal.hh>
19
20 using namespace dynamicgraph::sot;
21 using namespace dynamicgraph;
22
23 ✗ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(GripperControlPlugin, "GripperControl");
24
25 /* --- PLUGIN --------------------------------------------------------------- */
26 /* --- PLUGIN --------------------------------------------------------------- */
27 /* --- PLUGIN --------------------------------------------------------------- */
28
29 #define SOT_FULL_TO_REDUCED(sotName) \
30 sotName##FullSizeSIN(NULL, "GripperControl(" + name + \
31 ")::input(vector)::" + #sotName + "FullIN"), \
32 sotName##ReduceSOUT( \
33 SOT_INIT_SIGNAL_2(GripperControlPlugin::selector, \
34 sotName##FullSizeSIN, dynamicgraph::Vector, \
35 selectionSIN, Flags), \
36 "GripperControl(" + name + ")::input(vector)::" + #sotName + \
37 "ReducedOUT")
38
39 const double GripperControl::OFFSET_DEFAULT = 0.9;
40
41 // TODO: hard coded
42 const double DT = 0.005;
43
44 ✗ GripperControl::GripperControl(void)
45 ✗ : offset(GripperControl::OFFSET_DEFAULT), factor() {}
46
47 ✗ GripperControlPlugin::GripperControlPlugin(const std::string &name)
48 : Entity(name),
49 ✗ calibrationStarted(false),
50 ✗ positionSIN(NULL,
51 ✗ "GripperControl(" + name + ")::input(vector)::position"),
52 ✗ positionDesSIN(
53 ✗ NULL, "GripperControl(" + name + ")::input(vector)::positionDes"),
54 ✗ torqueSIN(NULL, "GripperControl(" + name + ")::input(vector)::torque"),
55 ✗ torqueLimitSIN(
56 ✗ NULL, "GripperControl(" + name + ")::input(vector)::torqueLimit"),
57 ✗ selectionSIN(NULL, "GripperControl(" + name + ")::input(vector)::selec")
58
59 ,
60 ✗ SOT_FULL_TO_REDUCED(position),
61 ✗ SOT_FULL_TO_REDUCED(torque),
62 ✗ SOT_FULL_TO_REDUCED(torqueLimit),
63 ✗ desiredPositionSOUT(
64 ✗ SOT_MEMBER_SIGNAL_4(GripperControl::computeDesiredPosition,
65 positionSIN, dynamicgraph::Vector, positionDesSIN,
66 dynamicgraph::Vector, torqueSIN,
67 dynamicgraph::Vector, torqueLimitSIN,
68 dynamicgraph::Vector),
69 ✗ "GripperControl(" + name + ")::output(vector)::reference") {
70 sotDEBUGIN(5);
71
72 ✗ positionSIN.plug(&positionReduceSOUT);
73 ✗ torqueSIN.plug(&torqueReduceSOUT);
74 ✗ torqueLimitSIN.plug(&torqueLimitReduceSOUT);
75
76 ✗ signalRegistration(
77 ✗ positionSIN << positionDesSIN << torqueSIN << torqueLimitSIN
78 ✗ << selectionSIN << desiredPositionSOUT << positionFullSizeSIN
79 ✗ << torqueFullSizeSIN << torqueLimitFullSizeSIN);
80 sotDEBUGOUT(5);
81
82 ✗ initCommands();
83 }
84
85 ✗ GripperControlPlugin::~GripperControlPlugin(void) {}
86
87 ✗ std::string GripperControlPlugin::getDocString() const {
88 ✗ std::string docstring = "Control of gripper.";
89 ✗ return docstring;
90 }
91
92 /* --- SIGNALS -------------------------------------------------------------- */
93 /* --- SIGNALS -------------------------------------------------------------- */
94 /* --- SIGNALS -------------------------------------------------------------- */
95
96 ✗ void GripperControl::computeIncrement(
97 const dynamicgraph::Vector &torques,
98 const dynamicgraph::Vector &torqueLimits,
99 const dynamicgraph::Vector &currentNormVel) {
100 ✗ const dynamicgraph::Vector::Index SIZE = currentNormVel.size();
101
102 // initialize factor, if needed.
103 ✗ if (factor.size() != SIZE) {
104 ✗ factor.resize(SIZE);
105 ✗ factor.fill(1.);
106 }
107
108 // Torque not provided?
109 ✗ if (torques.size() == 0) {
110 ✗ dgRTLOG() << "torque is not provided " << std::endl;
111 ✗ return;
112 }
113
114 ✗ for (int i = 0; i < SIZE; ++i) {
115 // apply a reduction factor if the torque limits are exceeded
116 // and the velocity goes in the same way
117 ✗ if ((torques(i) > torqueLimits(i)) && (currentNormVel(i) > 0)) {
118 ✗ factor(i) *= offset;
119 ✗ } else if ((torques(i) < -torqueLimits(i)) && (currentNormVel(i) < 0)) {
120 ✗ factor(i) *= offset;
121 }
122 // otherwise, release smoothly the reduction if possible/needed
123 else {
124 ✗ factor(i) /= offset;
125 }
126
127 // ensure factor is in )0,1(
128 ✗ factor(i) = std::min(1., std::max(factor(i), 0.));
129 }
130 }
131
132 ✗ dynamicgraph::Vector &GripperControl::computeDesiredPosition(
133 const dynamicgraph::Vector &currentPos,
134 const dynamicgraph::Vector &desiredPos, const dynamicgraph::Vector &torques,
135 const dynamicgraph::Vector &torqueLimits,
136 dynamicgraph::Vector &referencePos) {
137 ✗ const dynamicgraph::Vector::Index SIZE = currentPos.size();
138 // if( (SIZE==torques.size()) )
139 // { /* ERROR ... */ }
140
141 // compute the desired velocity
142 ✗ dynamicgraph::Vector velocity = (desiredPos - currentPos) * (1. / DT);
143
144 ✗ computeIncrement(torques, torqueLimits, velocity);
145
146 sotDEBUG(25) << " velocity " << velocity << std::endl;
147 sotDEBUG(25) << " factor " << factor << std::endl;
148
149 // multiply the velocity elmt per elmt
150 ✗ dynamicgraph::Vector weightedVel(SIZE);
151 ✗ weightedVel = velocity * factor;
152 sotDEBUG(25) << " weightedVel " << weightedVel << std::endl;
153
154 // integrate the desired velocity
155 ✗ referencePos.resize(SIZE);
156 ✗ referencePos = currentPos + weightedVel * DT;
157 ✗ return referencePos;
158 }
159
160 ✗ dynamicgraph::Vector &GripperControl::selector(
161 const dynamicgraph::Vector &fullsize, const Flags &selec,
162 dynamicgraph::Vector &desPos) {
163 ✗ int size = 0;
164 ✗ for (int i = 0; i < fullsize.size(); ++i) {
165 ✗ if (selec(i)) size++;
166 }
167
168 ✗ int curs = 0;
169 ✗ desPos.resize(size);
170 ✗ for (int i = 0; i < fullsize.size(); ++i) {
171 ✗ if (selec(i)) desPos(curs++) = fullsize(i);
172 }
173
174 ✗ return desPos;
175 }
176
177 /* --- COMMANDLINE ---------------------------------------------------------- */
178 /* --- COMMANDLINE ---------------------------------------------------------- */
179 /* --- COMMANDLINE ---------------------------------------------------------- */
180 ✗ void GripperControlPlugin::initCommands() {
181 namespace dc = ::dynamicgraph::command;
182 ✗ addCommand("offset",
183 ✗ dc::makeCommandVoid1(*this, &GripperControlPlugin::setOffset,
184 "set the offset (should be in )0, 1( )."));
185 }
186
187 ✗ void GripperControlPlugin::setOffset(const double &value) {
188 ✗ if ((value > 0) && (value < 1))
189 ✗ offset = value;
190 else
191 ✗ throw std::invalid_argument("The offset should be in )0, 1(.");
192 }
193