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/ActionComposed.h>
20 :
21 87 : ROSEE::ActionComposed::ActionComposed() : ActionGeneric() {
22 87 : independent = true;
23 87 : nInnerActions = 0;
24 87 : type = Action::Type::Composed;
25 87 : }
26 :
27 8 : ROSEE::ActionComposed::ActionComposed(std::string name) : ActionGeneric(name) {
28 8 : independent = true;
29 8 : nInnerActions = 0;
30 8 : type = Action::Type::Composed;
31 8 : }
32 :
33 16 : ROSEE::ActionComposed::ActionComposed(std::string name, bool independent) : ActionGeneric(name) {
34 16 : this->independent = independent;
35 16 : nInnerActions = 0;
36 16 : type = Action::Type::Composed;
37 16 : }
38 :
39 26 : unsigned int ROSEE::ActionComposed::numberOfInnerActions() const {
40 26 : return nInnerActions;
41 : }
42 :
43 10 : bool ROSEE::ActionComposed::isIndependent() const {
44 10 : return independent;
45 : }
46 :
47 8 : std::vector<std::string> ROSEE::ActionComposed::getInnerActionsNames() const {
48 8 : return innerActionsNames;
49 : }
50 :
51 0 : bool ROSEE::ActionComposed::empty() {
52 0 : return (nInnerActions == 0);
53 : }
54 :
55 :
56 60 : bool ROSEE::ActionComposed::sumAction ( ROSEE::Action::Ptr action, double jointPosScaleFactor, unsigned int jointPosIndex )
57 : {
58 :
59 60 : if ( ! checkIndependency(action) ) {
60 0 : return false; //cant add this primitive
61 : }
62 :
63 60 : if ( jointPosIndex > action->getAllJointPos().size()-1 ) {
64 0 : std::cerr << "[ACTIONCOMPOSED:: " << __func__ << "] The given jointPosindex " << jointPosIndex
65 0 : << " exceed the number " << action->getAllJointPos().size() << " of jointpos of passed action" << std::endl;
66 0 : return false;
67 : }
68 :
69 204 : if ( nInnerActions > 0 &&
70 144 : (! ROSEE::Utils::keys_equal(action->getAllJointPos().at(jointPosIndex), jointPos)) ) {
71 : std::cerr << "[ACTIONCOMPOSED:: " << __func__ << "] The action passed as argument has different keys in jointPosmap"
72 0 : << " respect to the others inserted in this composed action " << std::endl;
73 0 : return false;
74 : }
75 :
76 60 : if (jointPosScaleFactor < 0) {
77 : std::cerr << "[ACTIONCOMPOSED:: " << __func__ << "] You can not scale the joint position of the action to be inserted by a "
78 0 : << "value less than 0; jointPosScaleFactor passed is: " << jointPosScaleFactor << std::endl;
79 0 : return false;
80 : }
81 :
82 60 : if (jointPosScaleFactor > 1) {
83 : std::wcerr << "[ACTIONCOMPOSED:: " << __func__ << "] WARNING, You are scaling with a value greater than 1 "
84 0 : << " this could cause to command position over the joint limits; jointPosScaleFactor passed is: " << jointPosScaleFactor << std::endl;
85 : }
86 :
87 120 : JointPos actionJP = action->getAllJointPos().at(jointPosIndex) * jointPosScaleFactor;
88 120 : JointsInvolvedCount actionJIC = action->getJointsInvolvedCount();
89 :
90 60 : if (nInnerActions == 0) { //first primitive inserted
91 :
92 132 : for (auto joint : actionJP ){
93 114 : jointsInvolvedCount.insert ( std::make_pair (joint.first, actionJIC.at(joint.first) ) );
94 114 : jointPos.insert ( std::make_pair (joint.first, joint.second) );
95 : }
96 :
97 : } else {
98 :
99 42 : if (independent) {
100 :
101 : //if here, action is independent, we can add the joints states
102 195 : for ( auto joint : actionJP ){
103 :
104 170 : if ( actionJIC.at (joint.first ) > 0 ) {
105 :
106 60 : jointPos.at(joint.first) = joint.second;
107 60 : jointsInvolvedCount.at(joint.first) += actionJIC.at (joint.first );
108 : //+= or = is the same for the checks done before
109 : }
110 : }
111 :
112 : } else {
113 : // for each joint, add the state's value to the mean
114 : // (number of element in the previous mean is given by jointsInvolvedCount.at(x))
115 :
116 59 : for ( auto joint : actionJP ) {
117 118 : if ( actionJIC.at( joint.first ) == 0 ) {
118 76 : continue; //if the action that is being added has this joint not setted, not consider it
119 : }
120 :
121 : //update the count
122 42 : jointsInvolvedCount.at(joint.first) += actionJIC.at (joint.first );
123 :
124 : //iterate all dofs of jointPos
125 84 : for (unsigned int dof = 0; dof < joint.second.size(); dof++ ) {
126 :
127 42 : double mean = jointPos.at( joint.first ).at(dof) +
128 84 : ( (joint.second.at(dof) - jointPos.at(joint.first).at(dof)) /
129 84 : jointsInvolvedCount.at(joint.first) );
130 :
131 42 : jointPos.at(joint.first).at(dof) = mean;
132 : }
133 : }
134 : }
135 : }
136 :
137 60 : innerActionsNames.push_back ( action->getName() );
138 124 : for ( auto it: action->getFingersInvolved() ) {
139 64 : fingersInvolved.insert ( it );
140 : }
141 :
142 60 : nInnerActions ++;
143 :
144 60 : return true;
145 : }
146 :
147 60 : bool ROSEE::ActionComposed::checkIndependency ( ROSEE::Action::Ptr action ) {
148 :
149 60 : if (!independent) {
150 :
151 40 : } else if (nInnerActions == 0 ) {
152 :
153 110 : for ( auto jic : action->getJointsInvolvedCount() ){
154 95 : if ( jic.second > 1 ) {
155 : // if action is dependent we check that all its joints are setted only once
156 : // so we can teoretically add a "dipendent" action if all its joints are setted once
157 0 : return false; //cant add this primitive
158 : }
159 : }
160 :
161 : } else {
162 :
163 195 : for ( auto jic : action->getJointsInvolvedCount() ){
164 170 : if ( jic.second + jointsInvolvedCount.at(jic.first) > 1 ) {
165 : // we use the sum so also if action is dependent we check that all its joints are setted once
166 : // so we can teoretically add a "dipendent" action if all its joints are setted once
167 0 : return false; //cant add this primitive
168 : }
169 : }
170 : }
171 :
172 60 : return true;
173 : }
174 :
175 :
176 33 : void ROSEE::ActionComposed::print () const {
177 :
178 66 : std::stringstream output;
179 :
180 33 : output << "Composed Action '" << name;
181 33 : independent ? output << "' (independent):" : output << "' (not independent):" ;
182 33 : output << std::endl;
183 :
184 33 : output << "Composed by " << nInnerActions << " inner action: [" ;
185 121 : for (auto it : innerActionsNames) {
186 88 : output << it << ", ";
187 : }
188 33 : output.seekp (-2, output.cur); //to remove the last comma (and space)
189 33 : output << "]" << std::endl;
190 :
191 33 : output << "Fingers involved: [" ;
192 121 : for (auto it : fingersInvolved) {
193 88 : output << it << ", ";
194 : }
195 33 : output.seekp (-2, output.cur); //to remove the last comma (and space)
196 33 : output << "]" << std::endl;
197 :
198 33 : output << "Each joint influenced by x inner action:" << std::endl;
199 33 : output << jointsInvolvedCount;
200 :
201 33 : output << "JointPos:" << std::endl;
202 33 : output << jointPos << std::endl;
203 :
204 33 : std::cout << output.str();
205 33 : }
206 :
207 :
208 9 : void ROSEE::ActionComposed::emitYaml ( YAML::Emitter& out) const {
209 :
210 9 : out << YAML::BeginMap << YAML::Key << name << YAML::Value << YAML::BeginMap ;
211 9 : out << YAML::Key << "Type" << YAML::Value << type;
212 9 : out << YAML::Key << "Independent" << YAML::Value << independent;
213 9 : out << YAML::Key << "NInnerActions" << YAML::Value << nInnerActions;
214 9 : out << YAML::Key << "InnerActionsNames" << YAML::Value << YAML::Flow << innerActionsNames;
215 9 : out << YAML::Key << "FingersInvolved" << YAML::Value << YAML::Flow << fingersInvolved;
216 9 : out << YAML::Key << "JointsInvolvedCount" << YAML::Value << YAML::BeginMap;
217 66 : for (const auto &jointCount : jointsInvolvedCount ) {
218 57 : out << YAML::Key << jointCount.first;
219 57 : out << YAML::Value << jointCount.second;
220 : }
221 9 : out << YAML::EndMap;
222 :
223 9 : out << YAML::Key << "JointPos" << YAML::Value << YAML::BeginMap;
224 66 : for (const auto &joint : jointPos) {
225 57 : out << YAML::Key << joint.first;
226 57 : out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
227 : }
228 9 : out << YAML::EndMap;
229 9 : out << YAML::EndMap;
230 9 : out << YAML::EndMap;
231 9 : }
232 :
233 : // if parsed, we dont have the link with the primitives... so primitiveObjects is empty
234 75 : bool ROSEE::ActionComposed::fillFromYaml ( YAML::const_iterator yamlIt ) {
235 :
236 75 : name = yamlIt->first.as<std::string>();
237 :
238 501 : for (auto keyValue = yamlIt->second.begin(); keyValue != yamlIt->second.end(); ++keyValue ) {
239 :
240 852 : std::string key = keyValue->first.as<std::string>();
241 :
242 426 : if ( key.compare ("Independent") == 0 ) {
243 42 : independent = keyValue->second.as<bool>();
244 :
245 384 : } else if ( key.compare ("NInnerActions") == 0 ) {
246 42 : nInnerActions = keyValue->second.as <unsigned int>();
247 :
248 342 : } else if ( key.compare ("Type") == 0 ) {
249 75 : if (ROSEE::Action::Type::Composed != static_cast<ROSEE::Action::Type> ( keyValue->second.as <unsigned int>() )) {
250 0 : std::cout << "[COMPOSED ACTION::" << __func__ << "] Error, found type " << keyValue->second.as <unsigned int>()
251 0 : << "instead of Composed type (" << ROSEE::Action::Type::Composed << ")" << std::endl;
252 0 : return false;
253 : }
254 75 : type = ROSEE::Action::Type::Composed;
255 :
256 267 : } else if ( key.compare ("InnerActionsNames") == 0 ) {
257 42 : innerActionsNames = keyValue->second.as <std::vector <std::string> >();
258 :
259 225 : } else if ( key.compare ("FingersInvolved") == 0 ) {
260 150 : auto tempVect = keyValue->second.as <std::vector <std::string> > ();
261 75 : fingersInvolved.insert ( tempVect.begin(), tempVect.end() );
262 :
263 150 : } else if ( key.compare ("JointsInvolvedCount") == 0 ) {
264 75 : jointsInvolvedCount = keyValue->second.as < JointsInvolvedCount >();
265 :
266 75 : } else if ( key.compare ("JointPos") == 0 ) {
267 75 : jointPos = keyValue->second.as < JointPos >();
268 :
269 : } else {
270 0 : std::cout << "[COMPOSED ACTION PARSER] Error, not known key " << key << std::endl;
271 0 : return false;
272 : }
273 : }
274 75 : return true;
275 183 : }
276 :
277 :
278 :
|
