Line data Source code
1 : /*
2 : * Copyright (C) 2020 IIT-HHCM
3 : * Author: Davide Torielli
4 : * email: davide.torielli@iit.it
5 : *
6 : * Licensed under the Apache License, Version 2.0 (the "License");
7 : * you may not use this file except in compliance with the License.
8 : * You may obtain a copy of the License at
9 : *
10 : * http://www.apache.org/licenses/LICENSE-2.0
11 : *
12 : * Unless required by applicable law or agreed to in writing, software
13 : * distributed under the License is distributed on an "AS IS" BASIS,
14 : * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 : * See the License for the specific language governing permissions and
16 : * limitations under the License.
17 : */
18 :
19 : #include <ros_end_effector/GraspingActions/ActionPinchLoose.h>
20 :
21 19 : ROSEE::ActionPinchLoose::ActionPinchLoose() :
22 19 : ActionPinchGeneric ("pinchLoose", 2, 3, ActionPrimitive::Type::PinchLoose) { }
23 :
24 0 : ROSEE::ActionPinchLoose::ActionPinchLoose (unsigned int maxStoredActionStates) :
25 0 : ActionPinchGeneric ("pinchLoose", 2, maxStoredActionStates, ActionPrimitive::Type::PinchLoose) { }
26 :
27 272 : ROSEE::ActionPinchLoose::ActionPinchLoose (std::string tip1, std::string tip2) :
28 272 : ActionPinchGeneric ("pinchLoose", 2, 3, ActionPrimitive::Type::PinchLoose) {
29 272 : fingersInvolved.insert (tip1);
30 272 : fingersInvolved.insert (tip2);
31 272 : }
32 :
33 0 : ROSEE::ActionPinchLoose::ActionPinchLoose (std::pair <std::string, std::string> tipNames,
34 0 : JointPos jp, double distance) :
35 0 : ActionPinchGeneric ("pinchLoose", 2, 3, ActionPrimitive::Type::PinchLoose ) {
36 :
37 0 : fingersInvolved.insert (tipNames.first);
38 0 : fingersInvolved.insert (tipNames.second);
39 :
40 : //different from insertState, here we are sure the set is empty (we are in costructor)
41 0 : actionStates.insert (std::make_pair (jp, distance) );
42 0 : }
43 :
44 7 : std::vector < ROSEE::ActionPinchLoose::StateWithDistance > ROSEE::ActionPinchLoose::getActionStates() const {
45 :
46 7 : std::vector < ROSEE::ActionPinchLoose::StateWithDistance > retVect;
47 7 : retVect.reserve ( actionStates.size() );
48 :
49 28 : for (auto it : actionStates ) {
50 21 : retVect.push_back(it);
51 : }
52 :
53 7 : return retVect;
54 : }
55 :
56 5 : ROSEE::JointPos ROSEE::ActionPinchLoose::getJointPos() const {
57 5 : return (actionStates.begin()->first);
58 : }
59 :
60 0 : ROSEE::JointPos ROSEE::ActionPinchLoose::getJointPos(unsigned int index) const {
61 0 : auto it = actionStates.begin();
62 0 : unsigned int i = 1;
63 0 : while (i < index ) {
64 0 : ++ it;
65 0 : ++ i;
66 : }
67 0 : return (it->first);
68 : }
69 :
70 14 : std::vector < ROSEE::JointPos > ROSEE::ActionPinchLoose::getAllJointPos() const {
71 :
72 14 : std::vector < JointPos > retVect;
73 14 : retVect.reserve(actionStates.size());
74 :
75 56 : for (auto it : actionStates ) {
76 42 : retVect.push_back(it.first);
77 : }
78 :
79 14 : return retVect;
80 : }
81 :
82 85000 : bool ROSEE::ActionPinchLoose::insertActionState (ROSEE::JointPos jp, double dist) {
83 :
84 85000 : auto pairRet = actionStates.insert ( std::make_pair (jp, dist) ) ;
85 :
86 85000 : if (actionStates.size() > maxStoredActionStates) {
87 : //max capacity reached, we have to delete the last one
88 84949 : auto it = pairRet.first;
89 :
90 84949 : if (++(it) == actionStates.end() ){
91 : // the new inserted is the last one and has to be erased
92 84582 : actionStates.erase(pairRet.first);
93 84582 : return false;
94 : }
95 :
96 : // the new inserted is not the last one that has to be erased
97 367 : auto lastElem = actionStates.end();
98 367 : --lastElem;
99 367 : actionStates.erase(lastElem);
100 : }
101 :
102 418 : return true;
103 : }
104 :
105 :
106 8 : void ROSEE::ActionPinchLoose::print () const {
107 :
108 16 : std::stringstream output;
109 :
110 8 : output << "ActionName: " << name << std::endl;
111 :
112 8 : output << "FingersInvolved: [";
113 24 : for (auto fingName : fingersInvolved){
114 16 : output << fingName << ", " ;
115 : }
116 8 : output.seekp (-2, output.cur); //to remove the last comma (and space)
117 8 : output << "]" << std::endl;
118 :
119 8 : output << "JointsInvolvedCount: " << std::endl;;
120 8 : output << jointsInvolvedCount << std::endl;
121 :
122 8 : unsigned int nActState = 1;
123 32 : for (auto itemSet : actionStates) { //the element in the set
124 24 : output << "Action_State_" << nActState << " :" << std::endl;
125 :
126 24 : output << "\t" << "JointStates:" << std::endl;
127 24 : output << itemSet.first;
128 24 : output << "\t" << "Distance:" << std::endl;
129 24 : output << "\t\tdistance " << itemSet.second << std::endl;
130 :
131 24 : nActState++;
132 : }
133 8 : output << std::endl;
134 :
135 8 : std::cout << output.str();
136 :
137 8 : }
138 :
139 :
140 17 : void ROSEE::ActionPinchLoose::emitYaml ( YAML::Emitter& out ) const {
141 :
142 17 : out << YAML::Key << YAML::Flow << fingersInvolved;
143 :
144 17 : unsigned int nCont = 1;
145 17 : out << YAML::Value << YAML::BeginMap;
146 17 : out << YAML::Key << "PrimitiveType" << YAML::Value << primitiveType;
147 17 : out << YAML::Key << "ActionName" << YAML::Value << name;
148 17 : out << YAML::Key << "JointsInvolvedCount" << YAML::Value << YAML::BeginMap;
149 34 : for (const auto &jointCount : jointsInvolvedCount ) {
150 17 : out << YAML::Key << jointCount.first;
151 17 : out << YAML::Value << jointCount.second;
152 : }
153 17 : out << YAML::EndMap;
154 :
155 68 : for (const auto & actionState : actionStates) { //.second is the set of ActionState
156 :
157 102 : std::string contSeq = "ActionState_" + std::to_string(nCont);
158 51 : out << YAML::Key << contSeq;
159 :
160 51 : out << YAML::Value << YAML::BeginMap;
161 : //actionState.first, the jointstates map
162 51 : out << YAML::Key << "JointStates" << YAML::Value << YAML::BeginMap;
163 102 : for (const auto &joint : actionState.first) {
164 51 : out << YAML::Key << joint.first;
165 51 : out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
166 : }
167 51 : out << YAML::EndMap;
168 :
169 : //actionState.second, the optional
170 51 : out << YAML::Key << "Optional" << YAML::Value << YAML::BeginMap;
171 51 : out << YAML::Key << "distance" << YAML::Value << actionState.second;
172 51 : out << YAML::EndMap;
173 :
174 51 : out << YAML::EndMap;
175 51 : nCont++;
176 : }
177 17 : out << YAML::EndMap;
178 :
179 17 : }
180 :
181 :
182 19 : bool ROSEE::ActionPinchLoose::fillFromYaml ( YAML::const_iterator yamlIt ) {
183 :
184 38 : std::vector <std::string> fingInvolvedVect = yamlIt->first.as <std::vector < std::string >> ();
185 57 : for (const auto &it : fingInvolvedVect) {
186 38 : fingersInvolved.insert(it);
187 : }
188 :
189 133 : for ( YAML::const_iterator keyValue = yamlIt->second.begin(); keyValue != yamlIt->second.end(); ++keyValue) {
190 :
191 228 : std::string key = keyValue->first.as<std::string>();
192 114 : if ( key.compare("JointsInvolvedCount") == 0 ) {
193 19 : jointsInvolvedCount = keyValue->second.as < JointsInvolvedCount > ();
194 :
195 95 : } else if (key.compare ("ActionName") == 0 ) {
196 19 : name = keyValue->second.as <std::string> ();
197 :
198 76 : } else if (key.compare ("PrimitiveType") == 0) {
199 : ROSEE::ActionPrimitive::Type parsedType = static_cast<ROSEE::ActionPrimitive::Type> (
200 19 : keyValue->second.as <unsigned int>() );
201 19 : if (parsedType != primitiveType ) {
202 0 : std::cerr << "[ERROR ActionPinchLoose::" << __func__ << " parsed a type " << parsedType <<
203 0 : " but this object has primitive type " << primitiveType << std::endl;
204 0 : return false;
205 : }
206 :
207 57 : } else if (key.compare(0, 12, "ActionState_") == 0) { //compare 12 caracters from index 0 of key
208 :
209 114 : JointPos jointPos;
210 : double distance;
211 171 : for(YAML::const_iterator asEl = keyValue->second.begin(); asEl != keyValue->second.end(); ++asEl) {
212 :
213 : //asEl can be the map JointStates or the map Optional
214 114 : if (asEl->first.as<std::string>().compare ("JointStates") == 0 ) {
215 57 : jointPos = asEl->second.as < JointPos >();
216 :
217 57 : } else if (asEl->first.as<std::string>().compare ("Optional") == 0 ) {
218 57 : distance = asEl->second["distance"].as < double >();
219 :
220 : } else {
221 : //ERRROr, only joinstates and optional at this level
222 : std::cerr << "[ERROR ActionPinchLoose::" << __func__ << "not know key "
223 0 : << asEl->first.as<std::string>() <<
224 0 : " found in the yaml file at this level" << std::endl;
225 0 : return false;
226 : }
227 : }
228 57 : actionStates.insert ( std::make_pair (jointPos, distance));
229 :
230 : } else {
231 : std::cerr << "[ERROR ActionPinchLoose::" << __func__ << "not know key " << key <<
232 0 : " found in the yaml file" << std::endl;
233 0 : return false;
234 : }
235 : }
236 :
237 19 : return true;
238 183 : }
239 :
240 :
|
