GraspablePredicateProvider.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 General Public License version 2 as
6 * published by the Free Software Foundation.
7 *
8 * ArmarX is distributed in the hope that it will be useful, but
9 * WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 *
13 * You should have received a copy of the GNU General Public License
14 * along with this program. If not, see <http://www.gnu.org/licenses/>.
15 *
16 * @package ArmarX::
17 * @author Valerij Wittenbeck ( valerij.wittenbeck at kit dot edu)
18 * @date 2015
19 * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22 
24 
31 #include <VirtualRobot/Grasping/GraspSet.h>
32 
33 #include <SimoxUtility/algorithm/string/string_tools.h>
34 
35 using namespace memoryx;
36 
38  leftColor(armarx::DrawColor {0.f, 0.f, 1.f, 0.25f}),
39  rightColor(armarx::DrawColor {0.f, 1.f, 0.f, 0.25f}),
40  leftGraspable(PredicateInfo {"leftgraspable", 1}),
41  rightGraspable(PredicateInfo {"rightgraspable", 1})
42 {
43 }
44 
45 std::vector<Box> GraspablePredicateProvider::stringToBoxes(const std::string& boxesString)
46 {
47  std::vector<Box> result;
48  std::vector<std::string> boxTuples = simox::alg::split(boxesString, ";");
49 
50  for (const auto& boxTupleString : boxTuples)
51  {
52  std::vector<std::string> boxTuple = simox::alg::split(boxTupleString, " ");
53  ARMARX_CHECK_EXPRESSION(boxTuple.size() == 6);
54 
55  result.push_back(Box
56  {
57  std::stof(boxTuple[0]),
58  std::stof(boxTuple[1]),
59  std::stof(boxTuple[2]),
60  std::stof(boxTuple[3]),
61  std::stof(boxTuple[4]),
62  std::stof(boxTuple[5])
63  });
64  }
65 
66  return result;
67 }
68 
70 {
71  boxesL = stringToBoxes(getProperty<std::string>("BoundingBoxesL").getValue());
72  boxesR = stringToBoxes(getProperty<std::string>("BoundingBoxesR").getValue());
73 
74  usingProxy("WorkingMemory");
75  usingProxy("PriorKnowledge");
76 
77  debugDrawerTopicName = getProperty<std::string>("DebugDrawerTopicName").getValue();
78 
79  if (!debugDrawerTopicName.empty())
80  {
81  offeringTopic(debugDrawerTopicName);
82  }
83 }
84 
86 {
87  wm = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");
88  objectInstances = wm->getObjectInstancesSegment();
89  prior = getProxy<PriorKnowledgeInterfacePrx>("PriorKnowledge");
90  objectClasses = prior->getObjectClassesSegment();
91  fileManager = memoryx::GridFileManagerPtr(new memoryx::GridFileManager(prior->getCommonStorage()));
92 
93  if (!debugDrawerTopicName.empty())
94  {
95  armarx::DebugDrawerInterfacePrx debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>(debugDrawerTopicName);
96 
97  for (size_t i = 0; i < boxesL.size(); ++i)
98  {
99  boxesL[i].drawTo(debugDrawer, getDefaultName(), leftGraspable.name + "_box_" + std::to_string(i), leftColor);
100  }
101 
102  for (size_t i = 0; i < boxesR.size(); ++i)
103  {
104  boxesR[i].drawTo(debugDrawer, getDefaultName(), rightGraspable.name + "_box_" + std::to_string(i), rightColor);
105  }
106  }
107 }
108 
110 {
111  return "GraspablePredicateProvider";
112 }
113 
114 PredicateInfoList GraspablePredicateProvider::getPredicateInfos(const Ice::Current&)
115 {
116  return {leftGraspable, rightGraspable};
117 }
118 
119 void GraspablePredicateProvider::addPredicateIfInside(PredicateInstanceList& result, const std::string& predicateName, const ObjectInstancePtr& obj, const std::vector<Box>& boxes)
120 {
121  bool inside = false;
122  auto pos = obj->getPosition();
123 
124  if (pos->getFrame() != armarx::GlobalFrame && !pos->getFrame().empty())
125  {
126  pos = armarx::FramedPosePtr::dynamicCast(wm->getAgentInstancesSegment()->convertToWorldPose(pos->agent, obj->getPose()))->getPosition();
127  }
128 
129  ARMARX_DEBUG << "Obj: " << obj->getName() << " pos: " << *pos;
130 
131  for (const auto& box : boxes)
132  {
133  inside |= box.inside(pos);
134  }
135 
136  if (inside)
137  {
138  ARMARX_DEBUG << obj->getName() << " " << predicateName;
139  result.push_back(PredicateInstance {predicateName, {objectInstances->getEntityRefById(obj->getId())}, true});
140  }
141 }
142 
143 PredicateInstanceList GraspablePredicateProvider::calcPredicates(const Ice::Current&)
144 {
145  PredicateInstanceList result;
146 
147  for (const auto& entity : objectInstances->getAllEntities())
148  {
149  ObjectInstancePtr object = ObjectInstancePtr::dynamicCast(entity);
150  const std::string className = object->getMostProbableClass();
151 
152  auto graspabilityDescriptorIt = std::find_if(graspabilityDescriptors.cbegin(), graspabilityDescriptors.cend(), [&](const GraspabilityDescriptor & gd)
153  {
154  return gd.className == className;
155  });
156 
157  if (graspabilityDescriptorIt == graspabilityDescriptors.cend())
158  {
159  auto parents = objectClasses->getObjectClassByNameWithAllParents(className)->getParentClasses();
160 
161  bool hasGraspableSuperclass = std::find(parents.cbegin(), parents.cend(), "graspable") != parents.cend();
162  hasGraspableSuperclass |= std::find(parents.cbegin(), parents.cend(), "bothhandsgraspable") != parents.cend();
163  GraspabilityDescriptor gd {className, hasGraspableSuperclass, false, false};
164 
165  ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(objectClasses->getEntityByName(className));
166  memoryx::EntityWrappers::SimoxObjectWrapperPtr simoxWrapper = objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
167  VirtualRobot::ManipulationObjectPtr mo = simoxWrapper->getManipulationObject();
168 
169  gd.hasLeftGrasp = false;
170  gd.hasRightGrasp = false;
171  if (mo)
172  {
173  auto hasGrasp = [](VirtualRobot::GraspSetPtr set)
174  {
175  if (!set)
176  {
177  return false;
178  }
179 
180  for (size_t i = 0; i < set->getSize(); ++i)
181  {
182  if (armarx::Contains(set->getGrasp(i)->getName(), "Grasp", true))
183  {
184  return true;
185  }
186  }
187  return false;
188  };
189  gd.hasLeftGrasp = hasGrasp(mo->getGraspSet("TCP L"));
190  gd.hasRightGrasp = hasGrasp(mo->getGraspSet("TCP R"));
191  }
192 
193  graspabilityDescriptors.push_back(gd);
194  graspabilityDescriptorIt = std::prev(graspabilityDescriptors.cend());
195 
196  ARMARX_INFO << "new graspability descriptor: [className: '" << className
197  << "'; child of graspable/bothhandsgraspable: " << gd.hasGraspableSuperclass
198  << "; has left grasp: " << gd.hasLeftGrasp
199  << "; has right grasp: " << gd.hasRightGrasp << "]";
200  }
201 
202  const auto& gd = *graspabilityDescriptorIt;
203  ARMARX_DEBUG << "checking " << className << "/" << gd.className << " " << gd.hasGraspableSuperclass << " " << gd.hasLeftGrasp << " " << gd.hasRightGrasp;
204 
205  if (!gd.hasGraspableSuperclass)
206  {
207  continue;
208  }
209 
210  if (gd.hasLeftGrasp)
211  {
212  addPredicateIfInside(result, leftGraspable.name, object, boxesL);
213  }
214 
215  if (gd.hasRightGrasp)
216  {
217  addPredicateIfInside(result, rightGraspable.name, object, boxesR);
218  }
219  }
220 
221  return result;
222 }
223 
224 
memoryx::GraspablePredicateProvider::getPredicateInfos
PredicateInfoList getPredicateInfos(const Ice::Current &=Ice::emptyCurrent) override
Definition: GraspablePredicateProvider.cpp:114
armarx::Contains
bool Contains(const ContainerType &container, const ElementType &searchElement)
Definition: algorithm.h:295
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:62
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
memoryx::GraspablePredicateProvider::GraspablePredicateProvider
GraspablePredicateProvider()
Definition: GraspablePredicateProvider.cpp:37
ObjectClass.h
StringHelpers.h
memoryx::GraspablePredicateProvider::getDefaultName
std::string getDefaultName() const override
Retrieve default name of component.
Definition: GraspablePredicateProvider.cpp:109
IceInternal::Handle< ObjectInstance >
FramedPose.h
memoryx::GraspablePredicateProvider::calcPredicates
PredicateInstanceList calcPredicates(const Ice::Current &=Ice::emptyCurrent) override
Definition: GraspablePredicateProvider.cpp:143
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
memoryx::GraspablePredicateProvider::onConnectWorldStateUpdater
void onConnectWorldStateUpdater() override
Definition: GraspablePredicateProvider.cpp:85
MemoryXCoreObjectFactories.h
memoryx::EntityWrappers::SimoxObjectWrapper
SimoxObjectWrapper offers a simplified access to the Simox ManipulationObject (i.e visualization,...
Definition: SimoxObjectWrapper.h:46
memoryx::GraspablePredicateProvider::onInitWorldStateUpdater
void onInitWorldStateUpdater() override
Definition: GraspablePredicateProvider.cpp:69
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:40
ObjectInstance.h
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
memoryx::Box
Definition: Box.hpp:32
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
memoryx::GridFileManagerPtr
std::shared_ptr< GridFileManager > GridFileManagerPtr
Definition: AbstractEntityWrapper.h:32
armarx::ManagedIceObject::offeringTopic
void offeringTopic(const std::string &name)
Registers a topic for retrival after initialization.
Definition: ManagedIceObject.cpp:290
set
set(LIBS ArmarXCoreInterfaces ${CMAKE_THREAD_LIBS_INIT} ${dl_LIBRARIES} ${rt_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES} BoostAssertionHandler ArmarXCPPUtility SimoxUtility) set(LIB_FILES ArmarXManager.cpp ArmarXMultipleObjectsScheduler.cpp ArmarXObjectScheduler.cpp ManagedIceObject.cpp ManagedIceObjectPlugin.cpp Component.cpp ComponentPlugin.cpp IceGridAdmin.cpp ArmarXObjectObserver.cpp IceManager.cpp PackagePath.cpp RemoteReferenceCount.cpp logging/LoggingUtil.cpp logging/Logging.cpp logging/LogSender.cpp logging/ArmarXLogBuf.cpp system/ArmarXDataPath.cpp system/DynamicLibrary.cpp system/ProcessWatcher.cpp system/FactoryCollectionBase.cpp system/cmake/CMakePackageFinder.cpp system/cmake/CMakePackageFinderCache.cpp system/cmake/ArmarXPackageToolInterface.cpp system/RemoteObjectNode.cpp services/sharedmemory/HardwareId.cpp services/tasks/RunningTask.cpp services/tasks/ThreadList.cpp services/tasks/ThreadPool.cpp services/profiler/Profiler.cpp services/profiler/FileLoggingStrategy.cpp services/profiler/IceLoggingStrategy.cpp application/Application.cpp application/ApplicationOptions.cpp application/ApplicationProcessFacet.cpp application/ApplicationNetworkStats.cpp application/properties/PropertyUser.cpp application/properties/Property.cpp application/properties/PropertyDefinition.cpp application/properties/PropertyDefinitionContainer.cpp application/properties/PropertyDefinitionHelpFormatter.cpp application/properties/PropertyDefinitionConfigFormatter.cpp application/properties/PropertyDefinitionBriefHelpFormatter.cpp application/properties/PropertyDefinitionXmlFormatter.cpp application/properties/PropertyDefinitionDoxygenFormatter.cpp application/properties/PropertyDefinitionDoxygenComponentPagesFormatter.cpp application/properties/PropertyDefinitionContainerBriefHelpFormatter.cpp application/properties/IceProperties.cpp exceptions/Exception.cpp exceptions/local/UnexpectedEnumValueException.cpp util/FileSystemPathBuilder.cpp util/StringHelpers.cpp util/IceReportSkipper.cpp util/Throttler.cpp util/distributed/AMDCallbackCollection.cpp util/distributed/RemoteHandle/ClientSideRemoteHandleControlBlock.cpp util/distributed/RemoteHandle/RemoteHandle.cpp util/distributed/RemoteHandle/RemoteHandleControlBlock.cpp time/ice_conversions.cpp time/json_conversions.cpp time/CallbackWaitLock.cpp time/Clock.cpp time/ClockType.cpp time/ClockTypeNames.cpp time/CycleUtil.cpp time/DateTime.cpp time/Duration.cpp time/Frequency.cpp time/LocalTimeServer.cpp time/Metronome.cpp time/ScopedStopWatch.cpp time/StopWatch.cpp time/Timer.cpp time/TimeKeeper.cpp time/TimeUtil.cpp csv/CsvWriter.cpp csv/CsvReader.cpp eigen/conversions.cpp eigen/ice_conversions.cpp) set(LIB_HEADERS ArmarXManager.h ArmarXDummyManager.h ArmarXMultipleObjectsScheduler.h ArmarXObjectObserver.h ArmarXObjectScheduler.h ArmarXFwd.h Component.h ComponentPlugin.h ComponentFactories.h CoreObjectFactories.h IceGridAdmin.h IceManager.h IceManagerImpl.h json_conversions.h ManagedIceObject.h ManagedIceObjectPlugin.h ManagedIceObjectImpl.h ManagedIceObjectDependency.h ManagedIceObjectRegistryInterface.h PackagePath.h RemoteReferenceCount.h system/ImportExport.h system/ImportExportComponent.h system/AbstractFactoryMethod.h system/FactoryCollectionBase.h system/Synchronization.h system/ArmarXDataPath.h system/DynamicLibrary.h system/ProcessWatcher.h system/ConditionSynchronization.h system/cmake/CMakePackageFinder.h system/cmake/CMakePackageFinderCache.h system/cmake/FindPackageX.cmake system/cmake/ArmarXPackageToolInterface.h system/RemoteObjectNode.h logging/LoggingUtil.h logging/LogSender.h logging/Logging.h logging/ArmarXLogBuf.h logging/SpamFilterData.h services/tasks/RunningTask.h services/tasks/PeriodicTask.h services/tasks/ThreadList.h services/tasks/TaskUtil.h services/tasks/ThreadPool.h services/sharedmemory/SharedMemoryProvider.h services/sharedmemory/SharedMemoryConsumer.h services/sharedmemory/IceSharedMemoryProvider.h services/sharedmemory/IceSharedMemoryConsumer.h services/sharedmemory/HardwareIdentifierProvider.h services/sharedmemory/HardwareId.h services/sharedmemory/exceptions/SharedMemoryExceptions.h services/profiler/Profiler.h services/profiler/LoggingStrategy.h services/profiler/FileLoggingStrategy.h services/profiler/IceLoggingStrategy.h application/Application.h application/ApplicationOptions.h application/ApplicationProcessFacet.h application/ApplicationNetworkStats.h application/properties/forward_declarations.h application/properties/Properties.h application/properties/Property.h application/properties/PluginEigen.h application/properties/PluginEnumNames.h application/properties/PluginCfgStruct.h application/properties/PluginAll.h application/properties/PropertyUser.h application/properties/PropertyDefinition.h application/properties/PropertyDefinition.hpp application/properties/PropertyDefinitionInterface.h application/properties/PropertyDefinitionContainer.h application/properties/PropertyDefinitionFormatter.h application/properties/PropertyDefinitionContainerFormatter.h application/properties/PropertyDefinitionConfigFormatter.h application/properties/PropertyDefinitionHelpFormatter.h application/properties/PropertyDefinitionBriefHelpFormatter.h application/properties/PropertyDefinitionXmlFormatter.h application/properties/PropertyDefinitionDoxygenFormatter.h application/properties/PropertyDefinitionDoxygenComponentPagesFormatter.h application/properties/PropertyDefinitionContainerBriefHelpFormatter.h application/properties/ProxyPropertyDefinition.h application/properties/IceProperties.h exceptions/Exception.h exceptions/LocalException.h exceptions/local/DynamicLibraryException.h exceptions/local/ExpressionException.h exceptions/local/FileIOException.h exceptions/local/InvalidPropertyValueException.h exceptions/local/MissingRequiredPropertyException.h exceptions/local/PropertyInheritanceCycleException.h exceptions/local/ProxyNotInitializedException.h exceptions/local/UnexpectedEnumValueException.h exceptions/local/UnmappedValueException.h exceptions/local/ValueRangeExceededException.h exceptions/user/NotImplementedYetException.h rapidxml/rapidxml.hpp rapidxml/rapidxml_print.hpp rapidxml/rapidxml_iterators.hpp rapidxml/rapidxml_utils.hpp rapidxml/wrapper/RapidXmlReader.h rapidxml/wrapper/RapidXmlWriter.h rapidxml/wrapper/DefaultRapidXmlReader.h rapidxml/wrapper/MultiNodeRapidXMLReader.h util/IceBlobToObject.h util/ObjectToIceBlob.h util/FileSystemPathBuilder.h util/FiniteStateMachine.h util/StringHelpers.h util/StringHelperTemplates.h util/algorithm.h util/OnScopeExit.h util/Predicates.h util/Preprocessor.h util/PropagateConst.h util/Registrar.h util/TemplateMetaProgramming.h util/TripleBuffer.h util/IceReportSkipper.h util/Throttler.h util/distributed/AMDCallbackCollection.h util/distributed/RemoteHandle/ClientSideRemoteHandleControlBlock.h util/distributed/RemoteHandle/RemoteHandle.h util/distributed/RemoteHandle/RemoteHandleControlBlock.h util/SimpleStatemachine.h time.h time_minimal.h time/forward_declarations.h time/ice_conversions.h time/json_conversions.h time/CallbackWaitLock.h time/Clock.h time/ClockType.h time/ClockTypeNames.h time/CycleUtil.h time/DateTime.h time/Duration.h time/Frequency.h time/LocalTimeServer.h time/Metronome.h time/ScopedStopWatch.h time/StopWatch.h time/Timer.h time/TimeUtil.h time/TimeKeeper.h csv/CsvWriter.h csv/CsvReader.h eigen/conversions.h eigen/ice_conversions.h ice_conversions.h ice_conversions/ice_conversions_boost_templates.h ice_conversions/ice_conversions_templates.h ice_conversions/ice_conversions_templates.tpp $
Definition: CMakeLists.txt:12
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
memoryx::GridFileManager
GridFileManager provides utility functions for working with files in Mongo GridFS and links to them s...
Definition: GridFileManager.h:42
MemoryXTypesObjectFactories.h
armarx::ManagedIceObject::usingProxy
bool usingProxy(const std::string &name, const std::string &endpoints="")
Registers a proxy for retrieval after initialization and adds it to the dependency list.
Definition: ManagedIceObject.cpp:151
GraspablePredicateProvider.h
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::split
std::vector< std::string > split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelpers.cpp:36