Line data Source code
1 : /*
2 : * Copyright 2020 <copyright holder> <email>
3 : *
4 : * Licensed under the Apache License, Version 2.0 (the "License");
5 : * you may not use this file except in compliance with the License.
6 : * You may obtain a copy of the License at
7 : *
8 : * http://www.apache.org/licenses/LICENSE-2.0
9 : *
10 : * Unless required by applicable law or agreed to in writing, software
11 : * distributed under the License is distributed on an "AS IS" BASIS,
12 : * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 : * See the License for the specific language governing permissions and
14 : * limitations under the License.
15 : */
16 :
17 : #include <ros_end_effector/GraspingActions/ActionTimed.h>
18 :
19 90 : ROSEE::ActionTimed::ActionTimed() {
20 90 : type = Action::Type::Timed;
21 90 : }
22 :
23 12 : ROSEE::ActionTimed::ActionTimed (std::string name ) : Action(name, Action::Type::Timed) {
24 :
25 12 : }
26 :
27 67 : ROSEE::JointPos ROSEE::ActionTimed::getJointPos() const {
28 67 : return jointPosFinal;
29 : }
30 :
31 107 : std::vector<ROSEE::JointPos> ROSEE::ActionTimed::getAllJointPos() const {
32 :
33 107 : std::vector<ROSEE::JointPos> jpVect;
34 107 : jpVect.reserve (actionsNamesOrdered.size());
35 558 : for (auto actName : actionsNamesOrdered) {
36 451 : jpVect.push_back( actionsJointPosMap.at (actName) );
37 : }
38 107 : return jpVect;
39 : }
40 :
41 111 : std::vector<ROSEE::JointsInvolvedCount> ROSEE::ActionTimed::getAllJointCountAction() const {
42 :
43 111 : std::vector<ROSEE::JointsInvolvedCount> jcVect;
44 111 : jcVect.reserve (actionsNamesOrdered.size());
45 581 : for (auto actName : actionsNamesOrdered) {
46 470 : jcVect.push_back( actionsJointCountMap.at (actName) );
47 : }
48 111 : return jcVect;
49 : }
50 :
51 166 : std::vector<std::pair<double, double> > ROSEE::ActionTimed::getAllActionMargins() const {
52 :
53 166 : std::vector <std::pair <double,double> > timeVect;
54 166 : timeVect.reserve (actionsNamesOrdered.size());
55 667 : for (auto actName : actionsNamesOrdered) {
56 501 : timeVect.push_back( actionsTimeMarginsMap.at (actName) );
57 : }
58 166 : return timeVect;
59 : }
60 :
61 0 : ROSEE::JointPos ROSEE::ActionTimed::getJointPosAction (std::string actionName) const {
62 :
63 0 : auto it = actionsJointPosMap.find(actionName);
64 :
65 0 : if ( it != actionsJointPosMap.end() ) {
66 0 : return (it->second);
67 :
68 : } else {
69 0 : std::cerr << "[ACTIONTIMED:: " << __func__ << "] ERROR: action " << actionName << " not present in this composed timed action" << std::endl;
70 0 : return ROSEE::JointPos();
71 : }
72 : }
73 :
74 4 : ROSEE::JointsInvolvedCount ROSEE::ActionTimed::getJointCountAction(std::string actionName) const {
75 :
76 4 : auto it = actionsJointCountMap.find(actionName);
77 :
78 4 : if ( it != actionsJointCountMap.end() ) {
79 4 : return (it->second);
80 :
81 : } else {
82 0 : std::cerr << "[ACTIONTIMED:: " << __func__ << "] ERROR: action " << actionName << " not present in this composed timed action" << std::endl;
83 0 : return ROSEE::JointsInvolvedCount();
84 : }
85 : }
86 :
87 :
88 0 : std::pair <double, double> ROSEE::ActionTimed::getActionMargins ( std::string actionName ) const {
89 :
90 0 : auto it = actionsTimeMarginsMap.find(actionName);
91 :
92 0 : if ( it != actionsTimeMarginsMap.end() ) {
93 0 : return ( it->second );
94 :
95 : } else {
96 0 : std::cerr << "[ACTIONTIMED:: " << __func__ << "] ERROR: action " << actionName << " not present in this composed timed action" << std::endl;
97 0 : return std::make_pair(-1, -1);
98 : }
99 : }
100 :
101 989 : std::vector<std::string> ROSEE::ActionTimed::getInnerActionsNames() const {
102 989 : return actionsNamesOrdered;
103 : }
104 :
105 :
106 0 : void ROSEE::ActionTimed::print() const {
107 :
108 0 : std::stringstream output;
109 :
110 0 : output << "Timed Action '" << name << "'" << std::endl;
111 :
112 0 : output << "\tNice TimeLine:" << std::endl << "\t\t";
113 0 : for ( auto it : actionsNamesOrdered ) {
114 0 : output << actionsTimeMarginsMap.at(it).first << " -----> " << it << " -----> " << actionsTimeMarginsMap.at(it).second << " + ";
115 : }
116 0 : output.seekp (-3, output.cur); //to remove the last --+--
117 0 : output << std::endl;
118 :
119 0 : output << "\tJointPos of each actions, in order of execution:\n";
120 0 : for ( auto actionName : actionsNamesOrdered ) {
121 0 : output << "\t" << actionName << std::endl;
122 0 : output << actionsJointPosMap.at(actionName) << std::endl;
123 : }
124 :
125 0 : output << "\tJointPos final, the sum of all joint pos of each inner action:\n";
126 0 : output << jointPosFinal << std::endl;
127 :
128 0 : output << "\tFingers involved: [" ;
129 0 : for (auto it : fingersInvolved) {
130 0 : output << it << ", ";
131 : }
132 0 : output.seekp (-2, output.cur); //to remove the last comma (and space)
133 0 : output << "]" << std::endl;
134 :
135 0 : output << "\tEach joint influenced by x inner action:" << std::endl;
136 0 : output << jointsInvolvedCount << std::endl;
137 :
138 0 : std::cout << output.str();
139 :
140 0 : }
141 :
142 12 : void ROSEE::ActionTimed::emitYaml(YAML::Emitter& out) const {
143 :
144 12 : out << YAML::BeginMap << YAML::Key << name << YAML::Value << YAML::BeginMap ;
145 24 : std::string timeline;
146 49 : for ( auto it : actionsNamesOrdered ) {
147 111 : timeline += (std::to_string(actionsTimeMarginsMap.at(it).first) + " -----> " +
148 148 : it + " -----> " + std::to_string(actionsTimeMarginsMap.at(it).second) + " + ");
149 : }
150 12 : if (! timeline.empty() ) {
151 12 : timeline.pop_back(); timeline.pop_back(); timeline.pop_back(); //to delete last " + "
152 : }
153 :
154 12 : out << YAML::Comment(timeline);
155 12 : out << YAML::Key << "Type" << YAML::Value << type;
156 12 : out << YAML::Key << "FingersInvolved" << YAML::Value << YAML::Flow << fingersInvolved;
157 :
158 12 : out << YAML::Key << "JointsInvolvedCount" << YAML::Value << YAML::BeginMap;
159 72 : for (const auto &jointCount : jointsInvolvedCount ) {
160 60 : out << YAML::Key << jointCount.first;
161 60 : out << YAML::Value << jointCount.second;
162 : }
163 12 : out << YAML::EndMap;
164 :
165 12 : out << YAML::Key << "ActionsJointPosFinal" << YAML::Value << YAML::BeginMap;
166 72 : for ( const auto joint : jointPosFinal ) {
167 60 : out << YAML::Key << joint.first;
168 60 : out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
169 : }
170 12 : out << YAML::EndMap;
171 :
172 12 : out << YAML::Key << "ActionsNamesOrdered" << YAML::Value << YAML::Flow << actionsNamesOrdered;
173 :
174 12 : out << YAML::Key << "ActionsTimeMargins" << YAML::Value << YAML::BeginMap;
175 49 : for (const std::string action : actionsNamesOrdered ){
176 :
177 37 : out << YAML::Key << action << YAML::Value << YAML::BeginMap;
178 :
179 : out << YAML::Key << "marginBefore" <<
180 37 : YAML::Value << actionsTimeMarginsMap.at(action).first;
181 :
182 : out << YAML::Key << "marginAfter" <<
183 37 : YAML::Value << actionsTimeMarginsMap.at(action).second;
184 37 : out << YAML::EndMap;
185 : }
186 12 : out << YAML::EndMap;
187 :
188 12 : out << YAML::Key << "ActionsJointPos" << YAML::Value << YAML::BeginMap;
189 49 : for (const std::string action : actionsNamesOrdered ){
190 :
191 37 : out << YAML::Key << action << YAML::Value << YAML::BeginMap;
192 :
193 261 : for ( const auto joint : actionsJointPosMap.at(action) ) {
194 224 : out << YAML::Key << joint.first;
195 224 : out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
196 : }
197 37 : out << YAML::EndMap;
198 : }
199 12 : out << YAML::EndMap;
200 :
201 :
202 12 : out << YAML::Key << "ActionsJointCount" << YAML::Value << YAML::BeginMap;
203 49 : for (const std::string action : actionsNamesOrdered ){
204 :
205 37 : out << YAML::Key << action << YAML::Value << YAML::BeginMap;
206 :
207 261 : for ( const auto joint : actionsJointCountMap.at(action) ) {
208 224 : out << YAML::Key << joint.first;
209 224 : out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
210 : }
211 37 : out << YAML::EndMap;
212 : }
213 12 : out << YAML::EndMap;
214 :
215 :
216 :
217 :
218 12 : out << YAML::EndMap; // map began at the beginning of the function
219 12 : out << YAML::EndMap;// map began at the beginning of the function
220 12 : }
221 :
222 90 : bool ROSEE::ActionTimed::fillFromYaml(YAML::const_iterator yamlIt){
223 :
224 90 : name = yamlIt->first.as<std::string>();
225 90 : type = Action::Type::Timed;
226 :
227 810 : for (auto keyValue = yamlIt->second.begin(); keyValue != yamlIt->second.end(); ++keyValue ) {
228 :
229 1440 : std::string key = keyValue->first.as<std::string>();
230 :
231 720 : if ( key.compare ("FingersInvolved") == 0 ) {
232 180 : auto tempVect = keyValue->second.as <std::vector <std::string> > ();
233 90 : fingersInvolved.insert ( tempVect.begin(), tempVect.end() );
234 :
235 630 : } else if ( key.compare ("Type") == 0 ) {
236 90 : if (ROSEE::Action::Type::Timed != static_cast<ROSEE::Action::Type> ( keyValue->second.as <unsigned int>() )) {
237 0 : std::cout << "[Timed ACTION::" << __func__ << "] Error, found type " << keyValue->second.as <unsigned int>()
238 0 : << "instead of Timed type (" << ROSEE::Action::Type::Timed << ")" << std::endl;
239 0 : return false;
240 : }
241 90 : type = ROSEE::Action::Type::Timed;
242 :
243 540 : } else if ( key.compare ("ActionsNamesOrdered") == 0 ) {
244 90 : actionsNamesOrdered = keyValue->second.as < std::vector <std::string> > ();
245 :
246 450 : } else if ( key.compare ("JointsInvolvedCount") == 0 ) {
247 90 : jointsInvolvedCount = keyValue->second.as < JointsInvolvedCount >();
248 :
249 360 : } else if ( key.compare("ActionsTimeMargins") == 0 ) {
250 :
251 367 : for (auto tMargins = keyValue->second.begin(); tMargins != keyValue->second.end(); ++tMargins ) {
252 :
253 554 : std::string actNAme = tMargins->first.as<std::string>();
254 277 : double before = tMargins->second["marginBefore"].as<double>();
255 277 : double after = tMargins->second["marginAfter"].as<double>();
256 277 : actionsTimeMarginsMap.insert (std::make_pair (actNAme, std::make_pair(before, after) ) ) ;
257 : }
258 :
259 270 : } else if ( key.compare("ActionsJointPos") == 0) {
260 :
261 367 : for (auto jPos = keyValue->second.begin(); jPos != keyValue->second.end(); ++jPos ) {
262 :
263 554 : std::string actName = jPos->first.as<std::string>();
264 554 : JointPos jp = jPos->second.as<ROSEE::JointPos>();
265 277 : actionsJointPosMap.insert (std::make_pair (actName, jp) );
266 : }
267 :
268 180 : } else if ( key.compare("ActionsJointCount") == 0) {
269 :
270 367 : for (auto jCount = keyValue->second.begin(); jCount != keyValue->second.end(); ++jCount ) {
271 :
272 554 : std::string actName = jCount->first.as<std::string>();
273 554 : JointsInvolvedCount jc = jCount->second.as<ROSEE::JointsInvolvedCount>();
274 277 : actionsJointCountMap.insert (std::make_pair (actName, jc) );
275 : }
276 :
277 :
278 90 : } else if ( key.compare("ActionsJointPosFinal") == 0) {
279 :
280 90 : jointPosFinal = keyValue->second.as < JointPos >();
281 :
282 : } else {
283 0 : std::cerr << "[TIMEDACTION::" << __func__ << "] Error, not known key " << key << std::endl;
284 0 : return false;
285 : }
286 : }
287 :
288 90 : return true;
289 : }
290 :
291 37 : bool ROSEE::ActionTimed::insertAction(ROSEE::Action::Ptr action, double marginBefore, double marginAfter,
292 : unsigned int jointPosIndex, double percentJointPos, std::string newActionName) {
293 :
294 74 : std::string usedName = (newActionName.empty()) ? action->getName() : newActionName;
295 :
296 37 : unsigned int count = actionsJointPosMap.count( usedName );
297 :
298 37 : if (count > 0) {
299 0 : std::cerr << "[ACTIONTIMED:: " << __func__ << "] ERROR: " << usedName << " already present. Failed Insertion" << std::endl;
300 0 : return false;
301 : }
302 :
303 37 : if (marginAfter < 0 || marginBefore < 0) {
304 0 : std::cerr << "[ACTIONTIMED:: " << __func__ << "] ERROR: Please pass positive time margins" << std::endl;
305 0 : return false;
306 : }
307 :
308 37 : if ( jointPosIndex > action->getAllJointPos().size()-1 ) {
309 0 : std::cerr << "[ACTIONTIMED:: " << __func__ << "] ERROR: you pass index "<< jointPosIndex <<
310 0 : " but there are only " << action->getAllJointPos().size() << " JointPos in the action passed as argument" << std::endl;
311 0 : return false;
312 : }
313 :
314 37 : if (percentJointPos < 0 || percentJointPos > 1) {
315 0 : std::cerr << "[ACTIONTIMED:: " << __func__ << "] Please insert percentage for jointpos between 0 and 1. Passed: "
316 0 : << percentJointPos << std::endl;
317 0 : return false;
318 : }
319 :
320 124 : if ( actionsNamesOrdered.size() > 0 &&
321 87 : (! ROSEE::Utils::keys_equal(action->getAllJointPos().at(jointPosIndex), actionsJointPosMap.begin()->second)) ) {
322 : //we need only to compare the first jointPos (.begin())
323 : std::cerr << "[ACTIONTIMED:: " << __func__ << "] The action passed as argument has different keys in jointPosmap"
324 0 : << " respect to the others inserted in this timed action " << std::endl;
325 0 : return false;
326 : }
327 :
328 74 : ROSEE::JointPos insertingJP = (percentJointPos)*(action->getAllJointPos().at( jointPosIndex )) ;
329 37 : actionsJointPosMap.insert (std::make_pair ( usedName, insertingJP) );
330 37 : actionsTimeMarginsMap.insert ( std::make_pair( usedName, std::make_pair(marginBefore, marginAfter)));
331 37 : actionsNamesOrdered.push_back ( usedName );
332 37 : actionsJointCountMap.insert (std::make_pair (usedName, action->getJointsInvolvedCount()));
333 :
334 : //father member
335 93 : for ( auto it: action->getFingersInvolved() ) {
336 56 : fingersInvolved.insert ( it );
337 : }
338 :
339 37 : if (actionsNamesOrdered.size() == 1 ) { //We are inserting first action, we have to init the JointsInvolvedCount map
340 :
341 12 : jointsInvolvedCount = action->getJointsInvolvedCount();
342 12 : jointPosFinal = insertingJP;
343 :
344 : } else {
345 : // add the action.jointInvolvedCount to the timed jointCount
346 : // and update jointPosFinal
347 189 : for (auto jic : action->getJointsInvolvedCount() ) {
348 164 : jointsInvolvedCount.at(jic.first) += jic.second;
349 :
350 164 : if (jic.second > 0) {
351 : //if so, we must overwrite the pos of this joint in jointPosFina
352 95 : jointPosFinal.at(jic.first) = insertingJP.at(jic.first);
353 : }
354 : }
355 : }
356 :
357 37 : return true;
358 183 : }
359 :
360 :
|
