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/ActionMultiplePinchTight.h>
20 :
21 12 : ROSEE::ActionMultiplePinchTight::ActionMultiplePinchTight() :
22 12 : ActionPinchGeneric ("multiplePinchTight", 3, ActionPrimitive::Type::MultiplePinchTight) { }
23 :
24 0 : ROSEE::ActionMultiplePinchTight::ActionMultiplePinchTight(unsigned int maxStoredActionStates) :
25 0 : ActionPinchGeneric ("multiplePinchTight", maxStoredActionStates, ActionPrimitive::Type::MultiplePinchTight) { }
26 :
27 2 : ROSEE::ActionMultiplePinchTight::ActionMultiplePinchTight (
28 : std::set <std::string> fingerNamesSet,
29 2 : JointPos jp, double depthSum) :
30 4 : ActionPinchGeneric ( ("multiplePinchTight_" + std::to_string(fingerNamesSet.size())),
31 6 : fingerNamesSet.size(), 3, ActionPrimitive::Type::MultiplePinchTight ) {
32 :
33 : //different from insertState, here we are sure the set is empty (we are in costructor)
34 2 : fingersInvolved = fingerNamesSet;
35 2 : actionStates.insert (std::make_pair (jp, depthSum) );
36 2 : }
37 :
38 :
39 10 : ROSEE::JointPos ROSEE::ActionMultiplePinchTight::getJointPos() const {
40 10 : return (actionStates.begin()->first);
41 : }
42 :
43 0 : ROSEE::JointPos ROSEE::ActionMultiplePinchTight::getJointPos( unsigned int index) const {
44 0 : auto it = actionStates.begin();
45 0 : unsigned int i = 1;
46 0 : while (i < index ) {
47 0 : ++ it;
48 : }
49 0 : return (it->first);
50 : }
51 :
52 0 : std::vector < ROSEE::JointPos > ROSEE::ActionMultiplePinchTight::getAllJointPos() const{
53 :
54 0 : std::vector < JointPos > retVect;
55 0 : retVect.reserve ( actionStates.size() );
56 :
57 0 : for (auto it : actionStates ) {
58 0 : retVect.push_back(it.first);
59 : }
60 :
61 0 : return retVect;
62 : }
63 :
64 :
65 0 : std::vector < ROSEE::ActionMultiplePinchTight::StateWithDepth > ROSEE::ActionMultiplePinchTight::getActionStates () const {
66 :
67 0 : std::vector < ROSEE::ActionMultiplePinchTight::StateWithDepth > retVect;
68 0 : retVect.reserve ( actionStates.size() );
69 :
70 0 : for (auto it : actionStates ) {
71 0 : retVect.push_back(it);
72 : }
73 :
74 0 : return retVect;
75 :
76 : }
77 :
78 1 : bool ROSEE::ActionMultiplePinchTight::insertActionState (
79 : ROSEE::JointPos jp, double depthSum) {
80 :
81 1 : auto pairRet = actionStates.insert ( std::make_pair (jp, depthSum) ) ;
82 :
83 1 : if (! pairRet.second ) {
84 : //TODO print error no insertion because depth is equal... very improbable
85 0 : return false;
86 : }
87 :
88 1 : if (actionStates.size() > maxStoredActionStates) {
89 : //max capacity reached, we have to delete the last one
90 0 : auto it = pairRet.first;
91 :
92 0 : if ( (++it) == actionStates.end() ){
93 : // the new inserted is the last one and has to be erased
94 0 : actionStates.erase(pairRet.first);
95 0 : return false;
96 : }
97 :
98 : // the new inserted is not the last one that has to be erased
99 0 : auto lastElem = actionStates.end();
100 0 : --lastElem;
101 0 : actionStates.erase(lastElem);
102 : }
103 :
104 1 : return true;
105 : }
106 :
107 :
108 0 : void ROSEE::ActionMultiplePinchTight::print () const {
109 :
110 0 : std::stringstream output;
111 0 : output << "ActionName: " << name << std::endl;
112 :
113 0 : output << "FingersInvolved: [";
114 0 : for (auto fingName : fingersInvolved){
115 0 : output << fingName << ", " ;
116 : }
117 0 : output.seekp (-2, output.cur); //to remove the last comma (and space)
118 0 : output << "]" << std::endl;
119 :
120 0 : output << "JointsInvolvedCount: " << std::endl;;
121 0 : output << jointsInvolvedCount << std::endl;
122 :
123 0 : unsigned int nActState = 1;
124 0 : for (auto itemSet : actionStates) { //the element in the set
125 0 : output << "Action_State_" << nActState << " :" << std::endl;
126 :
127 0 : output << "\t" << "JointStates:" << std::endl;
128 0 : output << itemSet.first;
129 0 : output << "\t" << "DepthSum:" << itemSet.second <<std::endl;
130 :
131 0 : nActState++;
132 : }
133 0 : output << std::endl;
134 :
135 0 : std::cout << output.str();
136 :
137 0 : }
138 :
139 :
140 1 : void ROSEE::ActionMultiplePinchTight::emitYaml ( YAML::Emitter& out ) const {
141 :
142 1 : out << YAML::Key << YAML::Flow << fingersInvolved;
143 :
144 1 : unsigned int nCont = 1;
145 1 : out << YAML::Value << YAML::BeginMap;
146 1 : out << YAML::Key << "PrimitiveType" << YAML::Value << primitiveType;
147 1 : out << YAML::Key << "ActionName" << YAML::Value << name;
148 1 : out << YAML::Key << "JointsInvolvedCount" << YAML::Value << YAML::BeginMap;
149 10 : for (const auto &jointCount : jointsInvolvedCount ) {
150 9 : out << YAML::Key << jointCount.first;
151 9 : out << YAML::Value << jointCount.second;
152 : }
153 1 : out << YAML::EndMap;
154 :
155 3 : for (const auto & actionState : actionStates) { //.second is the set of ActionState
156 :
157 4 : std::string contSeq = "ActionState_" + std::to_string(nCont);
158 2 : out << YAML::Key << contSeq;
159 :
160 2 : out << YAML::Value << YAML::BeginMap;
161 : //actionState.first, the jointstates map
162 2 : out << YAML::Key << "JointPos" << YAML::Value << YAML::BeginMap;
163 20 : for (const auto &joint : actionState.first) {
164 18 : out << YAML::Key << joint.first;
165 18 : out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
166 : }
167 2 : out << YAML::EndMap;
168 :
169 : //actionState.second, the optional
170 2 : out << YAML::Key << "Optional" << YAML::Value << YAML::BeginMap;
171 2 : out << YAML::Key << "DepthSum" << YAML::Value << actionState.second;
172 2 : out << YAML::EndMap;
173 :
174 2 : out << YAML::EndMap;
175 2 : nCont++;
176 : }
177 1 : out << YAML::EndMap;
178 :
179 1 : }
180 :
181 :
182 12 : bool ROSEE::ActionMultiplePinchTight::fillFromYaml ( YAML::const_iterator yamlIt ) {
183 :
184 24 : std::vector <std::string> fingInvolvedVect = yamlIt->first.as <std::vector < std::string >> ();
185 48 : for (const auto &it : fingInvolvedVect) {
186 36 : fingersInvolved.insert(it);
187 : }
188 :
189 79 : for ( YAML::const_iterator keyValue = yamlIt->second.begin(); keyValue != yamlIt->second.end(); ++keyValue) {
190 :
191 134 : std::string key = keyValue->first.as<std::string>();
192 67 : if (key.compare("JointsInvolvedCount") == 0) {
193 12 : jointsInvolvedCount = keyValue->second.as < JointsInvolvedCount > ();
194 :
195 55 : } else if (key.compare ("ActionName") == 0 ) {
196 12 : name = keyValue->second.as <std::string> ();
197 :
198 43 : } else if (key.compare ("PrimitiveType") == 0) {
199 : ROSEE::ActionPrimitive::Type parsedType = static_cast<ROSEE::ActionPrimitive::Type> (
200 12 : keyValue->second.as <unsigned int>() );
201 12 : if (parsedType != primitiveType ) {
202 0 : std::cerr << "[ERROR ActionMultPinch::" << __func__ << " parsed a type " << parsedType <<
203 0 : " but this object has primitive type " << primitiveType << std::endl;
204 0 : return false;
205 : }
206 :
207 31 : } else if (key.compare(0, 12, "ActionState_") == 0) { //compare 12 caracters from index 0 of key
208 :
209 62 : JointPos jointPos;
210 : double depthSum;
211 93 : for(YAML::const_iterator asEl = keyValue->second.begin(); asEl != keyValue->second.end(); ++asEl) {
212 :
213 : //asEl can be the map JointPos or the map Optional
214 62 : if (asEl->first.as<std::string>().compare ("JointPos") == 0 ) {
215 31 : jointPos = asEl->second.as < JointPos >();
216 :
217 31 : } else if (asEl->first.as<std::string>().compare ("Optional") == 0 ) {
218 31 : depthSum = asEl->second["DepthSum"].as < double >();
219 :
220 : } else {
221 : //ERRROr, only JointPos and Optional at this level
222 : std::cerr << "[ERROR ActionMultPinch::" << __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 31 : actionStates.insert ( std::make_pair (jointPos, depthSum));
229 :
230 : } else {
231 : std::cerr << "[ERROR ActionMultPinch::" << __func__ << "not know key " << key <<
232 0 : " found in the yaml file" << std::endl;
233 0 : return false;
234 : }
235 : }
236 :
237 12 : nFingersInvolved = fingersInvolved.size();
238 :
239 12 : return true;
240 183 : }
|
