HandPredicateProvider.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 2014
19 * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22 
23 #include "HandPredicateProvider.h"
24 
28 
31 
32 namespace memoryx
33 {
35  graspedPredicate(PredicateInfo {"grasped", 3}),
36  graspedBothPredicate(PredicateInfo {"graspedBoth", 2}),
37  handEmptyPredicate(PredicateInfo {"handEmpty", 2})
38  {
39  }
40 
41 
43  {
44  usingProxy("WorkingMemory");
45  usingProxy("PriorKnowledge");
46  }
47 
49  {
50  wm = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");
51  prior = getProxy<PriorKnowledgeInterfacePrx>("PriorKnowledge");
52  distanceThreshold = getProperty<float>("DistanceThreshold").getValue();
53 
54  agentInstances = wm->getAgentInstancesSegment();
55  objectInstances = wm->getObjectInstancesSegment();
56  objectClasses = prior->getObjectClassesSegment();
57  }
58 
60  {
61  return "HandPredicateProvider";
62  }
63 
64  PredicateInfoList HandPredicateProvider::getPredicateInfos(const Ice::Current&)
65  {
66  return {graspedPredicate, graspedBothPredicate, handEmptyPredicate};
67  }
68 
69  PredicateInstanceList HandPredicateProvider::calcPredicates(const Ice::Current&)
70  {
71  PredicateInstanceList result;
72  // const std::string agentName = "testAgent";
73  //problem: hands can't be associated with the agent, this PP will try every agent with every hand
74 
75  auto agents = agentInstances->getAllAgentInstances();
76 
77  if (agents.empty())
78  {
79  ARMARX_WARNING << "No Agents found!";
80  }
81 
82  for (const auto& agent : agents)
83  {
84  ARMARX_INFO << "Checking agent: " << agent->getName();
85  if (armarx::Contains(agent->getName(), "human", true))
86  {
87  continue;
88  }
89 
90  auto robot = agent->getSharedRobot();
91  EntityRefBasePtr agentRef = agentInstances->getEntityRefById(agent->getId());
92 
93  std::vector<ObjectInstancePtr> hands;
94  std::vector<ObjectInfo> graspableObjects;
95 
96  for (const auto& entity : objectInstances->getAllEntities())
97  {
98  ObjectInstancePtr object = ObjectInstancePtr::dynamicCast(entity);
99  auto classes = objectClasses->getObjectClassByNameWithAllParents(object->getMostProbableClass())->getParentClasses();
100 
101  const bool isGraspable = std::find(classes.cbegin(), classes.cend(), "graspable") != classes.cend();
102  const bool isBothHandGraspable = std::find(classes.cbegin(), classes.cend(), "bothhandsgraspable") != classes.cend();
103 
104  if (isGraspable)
105  {
106  graspableObjects.push_back(ObjectInfo {object, ObjectInfo::ObjectType::OneHandGraspable});
107  }
108  else if (isBothHandGraspable)
109  {
110  graspableObjects.push_back(ObjectInfo {object, ObjectInfo::ObjectType::TwoHandGraspable});
111  }
112  else if (object->getName() == "handleft3a" || object->getName() == "handright3a")
113  {
114  hands.push_back(object);
115  }
116  }
117 
118  std::vector<std::pair<EntityRefBasePtr, ObjectInfo>> graspCandidates;
119  std::vector<std::pair<EntityRefBasePtr, ObjectInfo>> bothHandsGraspCandidates;
120 
121  for (const auto& hand : hands)
122  {
123  EntityRefBasePtr handRef = objectInstances->getEntityRefById(hand->getId());
124  const auto handPos = hand->getPosition()->toGlobal(robot)->toEigen();
125  float minDistance = std::numeric_limits<float>::max();
126  ObjectInfo bestObject {ObjectInstancePtr(), ObjectInfo::ObjectType::Other};
127 
128  for (const auto& object : graspableObjects)
129  {
130  // auto classes = prior->getObjectClassesSegment()->getObjectClassByNameWithAllParents(object->getMostProbableClass())->getParentClasses();
131  // const bool isGraspable = std::find(classes.begin(), classes.end(), "graspable") != classes.end();
132  // if (!isGraspable) continue;
133 
134 
135  armarx::FramedPositionPtr objPos = object.objectInstance->getPosition();
136  objPos = objPos->toGlobal(robot);
137  ARMARX_DEBUG << object.objectInstance->getName() << VAROUT(objPos);
138  ARMARX_DEBUG << hand->getName() << VAROUT(handPos);
139  const float dist = (objPos->toEigen() - handPos).norm();
140  ARMARX_DEBUG << "distance: " << dist;
141 
142  if (dist < minDistance)
143  {
144  bestObject = object;
145  minDistance = dist;
146  }
147  }
148 
149  if (bestObject.objectInstance)
150  {
151  ARMARX_INFO << "Closest object for grasped: '" << bestObject.objectInstance->getName() << "' in hand '" << hand->getName() << "' with distance: " << minDistance;
152  }
153 
154  if (minDistance < distanceThreshold)
155  {
156  ARMARX_INFO << "grasp candidate: hand '" << handRef->entityName << "' and object '" << bestObject.objectInstance->getName() << "' with dist " << minDistance;
157  graspCandidates.push_back({handRef, bestObject});
158  }
159  if (minDistance < distanceThreshold * 1.5)
160  {
161  ARMARX_INFO << "grasp candidate for both hand graspable: hand '" << handRef->entityName << "' and object '" << bestObject.objectInstance->getName() << "' with dist " << minDistance;
162  bothHandsGraspCandidates.push_back({handRef, bestObject});
163  }
164 
165  }
166 
167  std::map<std::string, bool> handOccupiedMap;
168  for (const auto& hand : hands)
169  {
170  handOccupiedMap[hand->getId()] = false;
171  }
172 
173  for (auto firstIt = graspCandidates.cbegin(); firstIt != graspCandidates.cend(); ++firstIt)
174  {
175  const auto& candidate = *firstIt;
176  const auto hand = candidate.first;
177  const auto objectId = candidate.second.objectInstance->getId();
178 
179  // if (candidate.second.type == ObjectInfo::ObjectType::TwoHandGraspable)
180  // {
181  // for (auto secondIt = std::next(firstIt); secondIt != graspCandidates.cend(); ++secondIt)
182  // {
183  // const auto handOther = secondIt->first;
184  // const auto objectIdOther = secondIt->second.objectInstance->getId();
185  // if (hand->entityId != handOther->entityId && objectId == objectIdOther)
186  // {
187  // result.push_back(PredicateInstance {graspedBothPredicate.name, {agentRef, objectInstances->getEntityRefById(objectId)}, true});
188  // handOccupiedMap[hand->entityId] = true;
189  // handOccupiedMap[handOther->entityId] = true;
190  // break;
191  // }
192  // }
193  // }
194  // else
195  if (candidate.second.type != ObjectInfo::ObjectType::TwoHandGraspable)
196  {
197  result.push_back(PredicateInstance {graspedPredicate.name, {agentRef, hand, objectInstances->getEntityRefById(objectId)}, true});
198  handOccupiedMap[hand->entityId] = true;
199  }
200  }
201 
202  for (auto firstIt = bothHandsGraspCandidates.cbegin(); firstIt != bothHandsGraspCandidates.cend(); ++firstIt)
203  {
204  const auto& candidate = *firstIt;
205  const auto hand = candidate.first;
206  const auto objectId = candidate.second.objectInstance->getId();
207 
208  if (candidate.second.type == ObjectInfo::ObjectType::TwoHandGraspable)
209  {
210  for (auto secondIt = std::next(firstIt); secondIt != bothHandsGraspCandidates.cend(); ++secondIt)
211  {
212  const auto handOther = secondIt->first;
213  const auto objectIdOther = secondIt->second.objectInstance->getId();
214  if (hand->entityId != handOther->entityId && objectId == objectIdOther)
215  {
216  result.push_back(PredicateInstance {graspedBothPredicate.name, {agentRef, objectInstances->getEntityRefById(objectId)}, true});
217  handOccupiedMap[hand->entityId] = true;
218  handOccupiedMap[handOther->entityId] = true;
219  break;
220  }
221  }
222  }
223  }
224 
225  for (const auto& hand : handOccupiedMap)
226  {
227  if (!hand.second)
228  {
229  result.push_back(PredicateInstance {handEmptyPredicate.name, {agentRef, objectInstances->getEntityRefById(hand.first)}, true});
230  }
231  }
232  }
233 
234  return result;
235  }
236 }
armarx::Contains
bool Contains(const ContainerType &container, const ElementType &searchElement)
Definition: algorithm.h:295
memoryx::HandPredicateProvider::getDefaultName
std::string getDefaultName() const override
Retrieve default name of component.
Definition: HandPredicateProvider.cpp:59
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
StringHelpers.h
IceInternal::Handle< ObjectInstance >
memoryx::HandPredicateProvider::onConnectWorldStateUpdater
void onConnectWorldStateUpdater() override
Definition: HandPredicateProvider.cpp:48
memoryx::HandPredicateProvider::calcPredicates
PredicateInstanceList calcPredicates(const Ice::Current &=Ice::emptyCurrent) override
Definition: HandPredicateProvider.cpp:69
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
memoryx::ObjectInstancePtr
IceInternal::Handle< ObjectInstance > ObjectInstancePtr
Definition: ObjectInstance.h:42
MemoryXCoreObjectFactories.h
HandPredicateProvider.h
max
T max(T t1, T t2)
Definition: gdiam.h:48
memoryx::HandPredicateProvider::HandPredicateProvider
HandPredicateProvider()
Definition: HandPredicateProvider.cpp:34
ObjectInstance.h
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
MemoryXTypesObjectFactories.h
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
memoryx::HandPredicateProvider::getPredicateInfos
PredicateInfoList getPredicateInfos(const Ice::Current &=Ice::emptyCurrent) override
Definition: HandPredicateProvider.cpp:64
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
memoryx::HandPredicateProvider::onInitWorldStateUpdater
void onInitWorldStateUpdater() override
Definition: HandPredicateProvider.cpp:42
ChannelRef.h
norm
double norm(const Point &a)
Definition: point.hpp:94