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