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/Action.h>
20 :
21 : /******************************** OPERATORS OVERLOAD FOR TYPEDEFS ***********************************/
22 :
23 : /** operator overload for JointPos so it is easier to print */
24 14915 : std::ostream& ROSEE::operator << (std::ostream& output, const ROSEE::JointPos jp) {
25 119821 : for (const auto &jsEl : jp) {
26 104906 : output << "\t\t"<<jsEl.first << " : "; //joint name
27 209812 : for(const auto &jValue : jsEl.second){
28 104906 : output << jValue << ", "; //joint position (vector because can have multiple dof)
29 : }
30 104906 : output.seekp (-2, output.cur); //to remove the last comma (and space)
31 104906 : output << std::endl;
32 : }
33 14915 : return output;
34 : }
35 :
36 37 : ROSEE::JointPos ROSEE::operator * ( double multiplier, ROSEE::JointPos jp) {
37 :
38 37 : return jp *= multiplier;
39 : }
40 :
41 60 : ROSEE::JointPos ROSEE::operator * ( ROSEE::JointPos jp, double multiplier ) {
42 :
43 60 : return jp *= multiplier;
44 : }
45 :
46 97 : ROSEE::JointPos& ROSEE::operator *= ( ROSEE::JointPos& jp, double multiplier) {
47 :
48 723 : for ( auto &jsEl : jp) {
49 1252 : for (int i = 0; i< jsEl.second.size(); i++) {
50 626 : jsEl.second.at(i) *= multiplier;
51 : }
52 : }
53 :
54 97 : return jp;
55 : }
56 :
57 0 : ROSEE::JointPos ROSEE::operator + ( ROSEE::JointPos jp1, ROSEE::JointPos jp2) {
58 :
59 0 : return jp1 += jp2;
60 : }
61 :
62 0 : ROSEE::JointPos& ROSEE::operator += (ROSEE::JointPos& jp1, ROSEE::JointPos jp2) {
63 :
64 0 : if ( ! ROSEE::Utils::keys_equal(jp1, jp2) ) {
65 0 : throw ROSEE::Utils::DifferentKeysException<ROSEE::JointPos, ROSEE::JointPos>(&jp1, &jp2);
66 : }
67 :
68 0 : for (auto &jsEl : jp1) {
69 0 : if (jsEl.second.size() != jp2.at(jsEl.first).size() ) {
70 0 : throw "Dofs not same";
71 : }
72 :
73 0 : for (int i = 0; i < jsEl.second.size(); i++) {
74 0 : jsEl.second.at(i) += jp2.at(jsEl.first).at(i);
75 : }
76 : }
77 :
78 0 : return jp1;
79 : }
80 :
81 360 : std::ostream& ROSEE::operator << (std::ostream& output, const ROSEE::JointsInvolvedCount jic) {
82 2732 : for (const auto &jicEl : jic) {
83 2372 : output << "\t"<< jicEl.first << " : " << jicEl.second;
84 2372 : output << std::endl;
85 : }
86 360 : return output;
87 : }
88 :
89 0 : std::ostream& ROSEE::operator<<(std::ostream& out, const ROSEE::Action::Type type){
90 :
91 0 : const char* s = 0;
92 : #define PROCESS_VAL(p) case(p): s = #p; break;
93 0 : switch(type){
94 0 : PROCESS_VAL(ROSEE::Action::Type::Primitive);
95 0 : PROCESS_VAL(ROSEE::Action::Type::Generic);
96 0 : PROCESS_VAL(ROSEE::Action::Type::Composed);
97 0 : PROCESS_VAL(ROSEE::Action::Type::Timed);
98 0 : PROCESS_VAL(ROSEE::Action::Type::None);
99 : }
100 : #undef PROCESS_VAL
101 :
102 0 : return out << s;
103 : }
104 :
105 : /********************************************* CLASS ACTION **************************************/
106 386 : ROSEE::Action::Action () {
107 :
108 386 : }
109 :
110 :
111 15767 : ROSEE::Action::Action ( std::string name, Action::Type type ) {
112 15767 : this->name = name;
113 15767 : this->type = type;
114 15767 : }
115 :
116 1163 : std::string ROSEE::Action::getName() const {
117 1163 : return name;
118 : }
119 :
120 500 : ROSEE::Action::Type ROSEE::Action::getType() const {
121 500 : return type;
122 : }
123 :
124 :
125 231 : std::set<std::string> ROSEE::Action::getFingersInvolved() const {
126 231 : return fingersInvolved;
127 : }
128 :
129 :
130 327 : ROSEE::JointsInvolvedCount ROSEE::Action::getJointsInvolvedCount() const {
131 327 : return jointsInvolvedCount;
132 : }
133 :
134 :
135 264 : void ROSEE::Action::print () const {
136 :
137 528 : std::stringstream output;
138 264 : output << "ActionName: " << name << std::endl;
139 :
140 264 : if (fingersInvolved.size() > 0 ){
141 264 : output << "FingersInvolved: [";
142 528 : for (auto fingName : fingersInvolved){
143 264 : output << fingName << ", " ;
144 : }
145 264 : output.seekp (-2, output.cur); //to remove the last comma (and space)
146 264 : output << "]" << std::endl;
147 :
148 : } else {
149 0 : output << "FingersInvolved: <not inserted>" << std::endl; // can happen, for ex for genericAction
150 : }
151 :
152 :
153 264 : output << "JointsInvolvedCount: " << std::endl;;
154 264 : output << jointsInvolvedCount << std::endl;
155 :
156 264 : output << "JointPos:" << std::endl;
157 264 : output << getJointPos() << std::endl;
158 :
159 264 : output << std::endl;
160 :
161 264 : std::cout << output.str();
162 447 : }
|
