-
Notifications
You must be signed in to change notification settings - Fork 0
/
ContainingMap.h
94 lines (78 loc) · 2.63 KB
/
ContainingMap.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
//
// Created by hejia on 8/12/19.
//
#ifndef WECOOK_CONTAININGMAP_H
#define WECOOK_CONTAININGMAP_H
#include <aikido/planner/World.hpp>
#include "Task.h"
#include "Container.h"
#include "ObjectMgr.h"
namespace wecook {
class ContainingMap {
public:
ContainingMap(const Task &task,
const std::shared_ptr<ObjectMgr> &objMgr,
const aikido::planner::WorldPtr &env);
void unconnectAll();
void connect(const dart::dynamics::SkeletonPtr &bodyToConn,
const dart::dynamics::SkeletonPtr &bodyThatConns,
const std::string &containerName,
const std::string &objectName);
void unconnect(const dart::dynamics::SkeletonPtr &bodyToConn,
const dart::dynamics::SkeletonPtr &bodyThatConns,
const std::string &containerName,
const std::string &objectName);
inline Eigen::Isometry3d getContainedPose(const std::string &containedName) {
for (const auto &container : m_containers) {
if (container.contains(containedName)) {
return container.getContainedPose();
}
}
}
inline dart::dynamics::BodyNode *getContainedBodyNode(const std::string &containedName) {
for (const auto &container : m_containers) {
if (container.contains(containedName)) {
return container.getContainedBodyNode();
}
}
return nullptr;
}
inline std::vector<dart::dynamics::BodyNode *> getContainedBodyNodes(const std::string &containerName) {
std::vector<dart::dynamics::BodyNode *> ret;
for (const auto &container : m_containers) {
if (container.getContainerName() == containerName) {
ret.emplace_back(container.getContainedBodyNode());
}
}
return ret;
}
inline bool hasTuple(const std::string &containerName, const std::string &containedName) {
for (const auto &container : m_containers) {
if (container.getContainerName() == containerName && container.getContainedName() == containedName) {
return true;
}
}
return false;
}
inline bool hasObject(const std::string &containedName) {
for (const auto &container : m_containers) {
if (container.getContainedName() == containedName) {
return true;
}
}
return false;
}
inline Container getContainer(const std::string &containedName) {
for (const auto &container : m_containers) {
if (container.getContainedName() == containedName) {
return container;
}
}
}
private:
std::vector<Container> m_containers;
aikido::planner::WorldPtr m_env = nullptr;
std::shared_ptr<ObjectMgr> m_objMgr = nullptr;
};
}
#endif //WECOOK_CONTAININGMAP_H