SceneArmarX.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as
6  * published by the Free Software Foundation; either version 2 of
7  * the License, or (at your option) any later version.
8  *
9  * ArmarX is distributed in the hope that it will be useful, but
10  * WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  *
17  * @package AffordanceKitArmarX
18  * @author Peter Kaiser ( peter dot kaiser at kit dot edu )
19  * @date 2016
20  * @copyright http://www.gnu.org/licenses/gpl.txt
21  * GNU General Public License
22  */
23 
24 #include "SceneArmarX.h"
25 #include "PrimitiveSetArmarX.h"
26 
27 #include <AffordanceKit/affordances/PlatformGraspAffordance.h>
28 #include <AffordanceKit/affordances/PrismaticGraspAffordance.h>
29 #include <AffordanceKit/affordances/GraspAffordance.h>
30 #include <AffordanceKit/affordances/SupportAffordance.h>
31 #include <AffordanceKit/affordances/LeanAffordance.h>
32 #include <AffordanceKit/affordances/HoldAffordance.h>
33 #include <AffordanceKit/affordances/LiftAffordance.h>
34 #include <AffordanceKit/affordances/TurnAffordance.h>
35 #include <AffordanceKit/affordances/PushAffordance.h>
36 #include <AffordanceKit/affordances/PullAffordance.h>
37 #include <AffordanceKit/affordances/BimanualPlatformGraspAffordance.h>
38 #include <AffordanceKit/affordances/BimanualPrismaticGraspAffordance.h>
39 #include <AffordanceKit/affordances/BimanualGraspAffordance.h>
40 #include <AffordanceKit/affordances/BimanualOpposedGraspAffordance.h>
41 #include <AffordanceKit/affordances/BimanualAlignedGraspAffordance.h>
42 #include <AffordanceKit/affordances/BimanualOpposedPlatformGraspAffordance.h>
43 #include <AffordanceKit/affordances/BimanualOpposedPrismaticGraspAffordance.h>
44 #include <AffordanceKit/affordances/BimanualAlignedPlatformGraspAffordance.h>
45 #include <AffordanceKit/affordances/BimanualAlignedPrismaticGraspAffordance.h>
46 #include <AffordanceKit/affordances/BimanualTurnAffordance.h>
47 #include <AffordanceKit/affordances/BimanualLiftAffordance.h>
48 
49 #include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
50 #include <ArmarXCore/observers/variant/TimestampVariant.h>
51 
52 #include <RobotAPI/interface/core/PoseBase.h>
53 
54 #include <MemoryX/libraries/memorytypes/segment/AffordanceSegment.h>
55 #include <MemoryX/libraries/memorytypes/segment/EnvironmentalPrimitiveSegment.h>
56 
57 namespace AffordanceKitArmarX
58 {
59  SceneArmarX::SceneArmarX()
60  {
61  generateAffordanceTypes();
62  }
63 
64  SceneArmarX::SceneArmarX(const AffordanceKit::PrimitiveSetPtr& primitives, const std::vector<AffordanceKit::UnimanualAffordancePtr>& unimanualAffordances, const std::vector<AffordanceKit::BimanualAffordancePtr>& bimanualAffordances) :
65  AffordanceKit::Scene(primitives, unimanualAffordances, bimanualAffordances)
66  {
67  generateAffordanceTypes();
68  }
69 
70  SceneArmarX::SceneArmarX(memoryx::EnvironmentalPrimitiveSegmentBasePrx& primitiveSegment, memoryx::AffordanceSegmentBasePrx& affordanceSegment)
71  {
72  auto t = IceUtil::Time::now();
73 
74  // Fetch affordances from memory
75  memoryx::AffordanceBaseList affordances = affordanceSegment->getAffordances();
76 
77  if (affordances.size() == 0)
78  {
79  // If there are no affordances in memory, just fetch the most recent primitives
80  primitives.reset(new PrimitiveSetArmarX(primitiveSegment));
81  return;
82  }
83 
84  long timestamp = (*affordances.begin())->getTime()->timestamp;
85 
86  // Fetch primitives from memory (only those with appropriate timestamp)
87  primitives.reset(new PrimitiveSetArmarX(primitiveSegment, timestamp));
88 
89  std::cout << "Creating a primitive set from memory took: " << (IceUtil::Time::now() - t).toMilliSeconds() << " ms" << std::endl;
90 
91  t = IceUtil::Time::now();
92 
93  #pragma omp parallel for
94  for (unsigned int i = 0; i < affordances.size(); i++)
95  {
96  AffordanceKit::UnimanualAffordancePtr uni;
97  AffordanceKit::BimanualAffordancePtr bi;
98 
99  switch (affordances[i]->getType())
100  {
101  case memoryx::eAffordanceTypeGraspPlatform:
102  uni.reset(new AffordanceKit::PlatformGraspAffordance());
103  break;
104 
105  case memoryx::eAffordanceTypeGraspPrismatic:
106  uni.reset(new AffordanceKit::PrismaticGraspAffordance());
107  break;
108 
109  case memoryx::eAffordanceTypeGrasp:
110  uni.reset(new AffordanceKit::GraspAffordance());
111  break;
112 
113  case memoryx::eAffordanceTypeSupport:
114  uni.reset(new AffordanceKit::SupportAffordance());
115  break;
116 
117  case memoryx::eAffordanceTypeLean:
118  uni.reset(new AffordanceKit::LeanAffordance());
119  break;
120 
121  case memoryx::eAffordanceTypeHold:
122  uni.reset(new AffordanceKit::HoldAffordance());
123  break;
124 
125  case memoryx::eAffordanceTypeLift:
126  uni.reset(new AffordanceKit::LiftAffordance());
127  break;
128 
129  case memoryx::eAffordanceTypeTurn:
130  uni.reset(new AffordanceKit::TurnAffordance());
131  break;
132 
133  case memoryx::eAffordanceTypePush:
134  uni.reset(new AffordanceKit::PushAffordance());
135  break;
136 
137  case memoryx::eAffordanceTypePull:
138  uni.reset(new AffordanceKit::PullAffordance());
139  break;
140 
141  case memoryx::eAffordanceTypeBimanualGraspPlatform:
142  bi.reset(new AffordanceKit::BimanualPlatformGraspAffordance());
143  break;
144 
145  case memoryx::eAffordanceTypeBimanualGraspPrismatic:
146  bi.reset(new AffordanceKit::BimanualPrismaticGraspAffordance());
147  break;
148 
149  case memoryx::eAffordanceTypeBimanualGrasp:
150  bi.reset(new AffordanceKit::BimanualGraspAffordance());
151  break;
152 
153  case memoryx::eAffordanceTypeBimanualGraspOpposed:
154  bi.reset(new AffordanceKit::BimanualOpposedGraspAffordance());
155  break;
156 
157  case memoryx::eAffordanceTypeBimanualGraspAligned:
158  bi.reset(new AffordanceKit::BimanualAlignedGraspAffordance());
159  break;
160 
161  case memoryx::eAffordanceTypeBimanualOpposedGraspPlatform:
162  bi.reset(new AffordanceKit::BimanualOpposedPlatformGraspAffordance());
163  break;
164 
165  case memoryx::eAffordanceTypeBimanualOpposedGraspPrismatic:
166  bi.reset(new AffordanceKit::BimanualOpposedPrismaticGraspAffordance());
167  break;
168 
169  case memoryx::eAffordanceTypeBimanualAlignedGraspPlatform:
170  bi.reset(new AffordanceKit::BimanualAlignedPlatformGraspAffordance());
171  break;
172 
173  case memoryx::eAffordanceTypeBimanualAlignedGraspPrismatic:
174  bi.reset(new AffordanceKit::BimanualAlignedPrismaticGraspAffordance());
175  break;
176 
177  case memoryx::eAffordanceTypeBimanualTurn:
178  bi.reset(new AffordanceKit::BimanualTurnAffordance());
179  break;
180 
181  case memoryx::eAffordanceTypeBimanualLift:
182  bi.reset(new AffordanceKit::BimanualLiftAffordance());
183  break;
184 
185  case memoryx::eAffordanceTypeBimanualSupport:
186  case memoryx::eAffordanceTypeBimanualLean:
187  case memoryx::eAffordanceTypeBimanualHold:
188  case memoryx::eAffordanceTypeBimanualPush:
189  case memoryx::eAffordanceTypeBimanualPull:
190  ARMARX_WARNING_S << "Affordance type " << affordances[i]->getType() << " not yet included in AffordanceKitArmarX";
191  break;
192  }
193 
194  AffordanceKit::PrimitivePtr p = primitives->getPrimitiveById(affordances[i]->getPrimitiveId());
195  if (!p)
196  {
197  std::cout << "Warning: Affordance relates to unknown primitive Id" << std::endl;
198  continue;
199  }
200 
201  AffordanceKit::ThetaFunctionPtr theta(new AffordanceKit::ThetaFunction());
202  (*theta)[AffordanceKit::PrimitivePair(p)] = armarx::MatrixFloatPtr::dynamicCast(affordances[i]->getCertaintyFunction())->toEigen();
203 
204  for (auto& observation : affordances[i]->getObservations())
205  {
206  AffordanceKit::ThetaFunctionPtr theta(new AffordanceKit::ThetaFunction());
207  (*theta)[AffordanceKit::PrimitivePair(p)] = armarx::MatrixFloatPtr::dynamicCast(observation)->toEigen();
208 
209  if (uni)
210  {
211  uni->addObservation(AffordanceKit::ObservationPtr(new AffordanceKit::Observation(theta)));
212  }
213  else
214  {
215  bi->addObservation(AffordanceKit::ObservationPtr(new AffordanceKit::Observation(theta)));
216  }
217  }
218 
219  if (uni)
220  {
221  uni->setTheta(theta);
222  uni->setTimestamp(affordances[i]->getTime()->timestamp);
223 
224  #pragma omp critical
225  {
226  unimanualAffordances.push_back(uni);
227  AffordanceKit::Scene::affordances.push_back(uni);
228  }
229  }
230  else if (bi)
231  {
232  bi->setTheta(theta);
233  bi->setTimestamp(affordances[i]->getTime()->timestamp);
234 
235  #pragma omp critical
236  {
237  bimanualAffordances.push_back(bi);
238  AffordanceKit::Scene::affordances.push_back(bi);
239  }
240  }
241  }
242 
243  std::cout << "Creating a scene from memory took: " << (IceUtil::Time::now() - t).toMilliSeconds() << " ms (" << unimanualAffordances.size() << " unimanual affordances and " << bimanualAffordances.size() << " bimanual affordances)" << std::endl;
244  }
245 
246  void SceneArmarX::writeToMemory(const memoryx::EnvironmentalPrimitiveSegmentBasePrx& primitiveSegment, const memoryx::AffordanceSegmentBasePrx& affordanceSegment) const
247  {
248  // Write primitives to memory
249  PrimitiveSetArmarX p(primitives);
250  p.writeToMemory(primitiveSegment);
251 
252  // Write affordances to memory
253  std::vector<memoryx::EntityBasePtr> unimanualAffordanceList;
254  unimanualAffordanceList.reserve(unimanualAffordances.size());
255 
256  for (auto& primitive : *primitives)
257  {
258  // Write unimanual affordances to memory
259  for (std::vector<AffordanceKit::UnimanualAffordancePtr>::const_iterator a = unimanualAffordances.begin(); a < unimanualAffordances.end(); a++)
260  {
261  AffordanceKit::UnimanualAffordancePtr affordance = *a;
262  if (!affordance->existsForPrimitive(primitive))
263  {
264  continue;
265  }
266 
267  AffordanceKit::Belief value;
268  Eigen::Matrix4f pose = affordance->getMostCertainPoseForPrimitive(primitive, value);
269 
270  if (affordanceTypes.find(affordance->getName()) != affordanceTypes.end())
271  {
272  memoryx::AffordanceBasePtr a_mx(new memoryx::Affordance);
273  a_mx->setType(affordanceTypes.at(affordance->getName()));
274  a_mx->setPosition(new armarx::Vector3(Eigen::Vector3f(pose.block<3, 1>(0, 3))));
275  a_mx->setPrimitiveId(primitive->getId());
276  a_mx->setValidationStatus(memoryx::eAffordanceNotValidated);
277  a_mx->setTime(new armarx::TimestampVariant(primitive->getTimestamp()));
278  a_mx->setCertaintyFunction(new armarx::MatrixFloat(affordance->getTheta(AffordanceKit::PrimitivePair(primitive))));
279 
280  memoryx::AffordanceObservationList observations;
281  for (auto& observation : affordance->getObservations())
282  {
283  if (observation->existsForPrimitive(primitive))
284  {
285  observations.push_back(new armarx::MatrixFloat(observation->getTheta(primitive)));
286  }
287  }
288  a_mx->setObservations(observations);
289 
290  unimanualAffordanceList.push_back(a_mx);
291  }
292  }
293  }
294 
295  std::vector<memoryx::EntityBasePtr> bimanualAffordanceList;
296  bimanualAffordanceList.reserve(bimanualAffordances.size());
297 
298  for (auto& primitive : *primitives)
299  {
300  // Write bimanual affordances to memory
301  for (std::vector<AffordanceKit::BimanualAffordancePtr>::const_iterator a = bimanualAffordances.begin(); a < bimanualAffordances.end(); a++)
302  {
303  AffordanceKit::BimanualAffordancePtr affordance = *a;
304  if (!affordance->existsForPrimitive(primitive))
305  {
306  continue;
307  }
308 
309  AffordanceKit::Belief value;
310  std::pair<Eigen::Matrix4f, Eigen::Matrix4f> poses = affordance->getMostCertainPosesForPrimitive(primitive, value);
311  if (poses.first.isZero())
312  {
313  continue;
314  }
315 
316  if (affordanceTypes.find(affordance->getName()) != affordanceTypes.end())
317  {
318  memoryx::AffordanceBasePtr a_mx(new memoryx::Affordance);
319  std::cout << affordance->getName() << std::endl;
320  a_mx->setType(affordanceTypes.at(affordance->getName()));
321  a_mx->setPosition(new armarx::Vector3(Eigen::Vector3f(poses.first.block<3, 1>(0, 3))));
322  a_mx->setPrimitiveId(primitive->getId());
323  a_mx->setValidationStatus(memoryx::eAffordanceNotValidated);
324  a_mx->setTime(new armarx::TimestampVariant(primitive->getTimestamp()));
325  a_mx->setCertaintyFunction(new armarx::MatrixFloat(affordance->getTheta(AffordanceKit::PrimitivePair(primitive, primitive))));
326 
327  memoryx::AffordanceObservationList observations;
328  for (auto& observation : affordance->getObservations())
329  {
330  if (observation->existsForPrimitive(primitive))
331  {
332  observations.push_back(new armarx::MatrixFloat(observation->getTheta(primitive)));
333  }
334  }
335  a_mx->setObservations(observations);
336 
337  bimanualAffordanceList.push_back(a_mx);
338  }
339  }
340  }
341 
342  affordanceSegment->clear();
343  affordanceSegment->upsertEntityList(unimanualAffordanceList);
344  affordanceSegment->upsertEntityList(bimanualAffordanceList);
345  }
346 
347  void SceneArmarX::generateAffordanceTypes()
348  {
349  affordanceTypes["G"] = memoryx::eAffordanceTypeGrasp;
350  affordanceTypes["G-Pl"] = memoryx::eAffordanceTypeGraspPlatform;
351  affordanceTypes["G-Pr"] = memoryx::eAffordanceTypeGraspPrismatic;
352  affordanceTypes["Sp"] = memoryx::eAffordanceTypeSupport;
353  affordanceTypes["Ln"] = memoryx::eAffordanceTypeLean;
354  affordanceTypes["Hd"] = memoryx::eAffordanceTypeHold;
355  affordanceTypes["Ps"] = memoryx::eAffordanceTypePush;
356  affordanceTypes["Lf"] = memoryx::eAffordanceTypeLift;
357  affordanceTypes["Tn"] = memoryx::eAffordanceTypeTurn;
358  affordanceTypes["Pl"] = memoryx::eAffordanceTypePull;
359  affordanceTypes["Bi-G-Pl"] = memoryx::eAffordanceTypeBimanualGraspPlatform;
360  affordanceTypes["Bi-G-Pr"] = memoryx::eAffordanceTypeBimanualGraspPrismatic;
361  affordanceTypes["Bi-Op-G-Pl"] = memoryx::eAffordanceTypeBimanualOpposedGraspPlatform;
362  affordanceTypes["Bi-Op-G-Pr"] = memoryx::eAffordanceTypeBimanualOpposedGraspPrismatic;
363  affordanceTypes["Bi-Tn"] = memoryx::eAffordanceTypeBimanualTurn;
364  affordanceTypes["Bi-Lf"] = memoryx::eAffordanceTypeBimanualLift;
365  }
366 }
AffordanceKitArmarX::SceneArmarX::generateAffordanceTypes
void generateAffordanceTypes()
Definition: SceneArmarX.cpp:347
memoryx::Affordance
Definition: Affordance.h:42
MatrixVariant.h
AffordanceKitArmarX::SceneArmarX::affordanceTypes
std::map< std::string, memoryx::AffordanceType > affordanceTypes
Definition: SceneArmarX.h:46
armarx::TimestampVariant
Definition: TimestampVariant.h:54
AffordanceKitArmarX::SceneArmarX::writeToMemory
void writeToMemory(const memoryx::EnvironmentalPrimitiveSegmentBasePrx &primitiveSegment, const memoryx::AffordanceSegmentBasePrx &affordanceSegment) const
Definition: SceneArmarX.cpp:246
EnvironmentalPrimitiveSegment.h
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
TimestampVariant.h
armarx::MatrixFloat
The MatrixFloat class.
Definition: MatrixVariant.h:48
AffordanceKitArmarX::PrimitiveSetArmarX
Definition: PrimitiveSetArmarX.h:32
armarx::Vector3
The Vector3 class.
Definition: Pose.h:112
AffordanceSegment.h
SceneArmarX.h
ARMARX_WARNING_S
#define ARMARX_WARNING_S
Definition: Logging.h:206
AffordanceKitArmarX::SceneArmarX::SceneArmarX
SceneArmarX()
Definition: SceneArmarX.cpp:59
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
AffordanceKit
Definition: PrimitiveExtractor.h:109
PrimitiveSetArmarX.h
AffordanceKitArmarX
Definition: BimanualAffordanceArmarX.cpp:30
AffordanceKitArmarX::PrimitiveSetArmarX::writeToMemory
void writeToMemory(const memoryx::EnvironmentalPrimitiveSegmentBasePrx &segment) const
Definition: PrimitiveSetArmarX.cpp:136