Choreonoid  1.5
Pose.h
Go to the documentation of this file.
1 
6 #ifndef CNOID_POSE_SEQ_PLUGIN_POSE_H
7 #define CNOID_POSE_SEQ_PLUGIN_POSE_H
8 
9 #include <cnoid/Body>
10 #include <map>
11 #include "exportdecl.h"
12 
13 namespace cnoid {
14 
15 class Mapping;
16 
17 class PoseUnit;
18 class PoseSeq;
19 class PoseRef;
20 
22 
24 {
25 public:
26  PoseUnit();
27  PoseUnit(const PoseUnit& org);
28  virtual ~PoseUnit();
29 
30  virtual PoseUnit* duplicate() = 0;
31 
32  virtual bool restore(const Mapping& archive, const BodyPtr body) = 0;
33  virtual void store(Mapping& archive, const BodyPtr body) const = 0;
34  virtual bool hasSameParts(PoseUnitPtr unit) { return false; }
35 
39  inline const std::string& name() const {
40  return name_;
41  }
42 
43 private:
44  std::string name_;
45  PoseSeq* owner;
46  int seqLocalReferenceCounter;
47 
48  friend class PoseSeq;
49  friend class PoseRef;
50 };
51 
52 
53 class CNOID_EXPORT Pose : public PoseUnit
54 {
55  struct JointInfo {
56  inline JointInfo() : isValid(false), isStationaryPoint(false) { }
57  double q;
58  bool isValid;
59  bool isStationaryPoint;
60  };
61 
62 public:
63 
64  class LinkInfo {
65 
66  public:
67 
70 
71  inline LinkInfo() :
72  isBaseLink_(false),
73  isStationaryPoint_(false),
74  isTouching_(false),
75  isSlave_(false) { }
76  inline bool isBaseLink() const { return isBaseLink_; }
77  inline void setStationaryPoint(bool on){ isStationaryPoint_ = on; }
78  inline bool isStationaryPoint() const { return isStationaryPoint_; }
79  inline bool isTouching() const { return isTouching_; }
80  inline const Vector3& partingDirection() const { return partingDirection_; }
81  inline void setTouching(const Vector3& partingDirection) {
82  isTouching_ = true;
83  partingDirection_ = partingDirection;
84  }
85  inline void clearTouching() { isTouching_ = false; }
86  inline bool isSlave() const { return isSlave_; }
87  inline void setSlave(bool on) { isSlave_ = on; }
88 
89  private:
90  bool isBaseLink_;
91  bool isStationaryPoint_;
92  bool isTouching_;
93  bool isSlave_;
94  Vector3 partingDirection_;
95  friend class Pose;
96  };
97 
98  typedef std::map<int, LinkInfo> LinkInfoMap;
99 
100  Pose();
101  Pose(int numJoints);
102  Pose(const Pose& org);
103 
104  virtual ~Pose();
105 
106  bool empty();
107 
108  void clear();
109 
110  virtual PoseUnit* duplicate();
111 
112  virtual bool hasSameParts(PoseUnitPtr unit);
113 
114  virtual bool restore(const Mapping& archive, const BodyPtr body);
115  virtual void store(Mapping& archive, const BodyPtr body) const;
116 
117  inline void setNumJoints(int n){
118  jointInfos.resize(n);
119  }
120 
121  inline int numJoints() const {
122  return jointInfos.size();
123  }
124 
125  inline void setJointPosition(int jointId, double q){
126  if(jointId >= (int)jointInfos.size()){
127  setNumJoints(jointId + 1);
128  }
129  JointInfo& info = jointInfos[jointId];
130  info.q = q;
131  info.isValid = true;
132  }
133 
134  inline double jointPosition(int jointId) const {
135  return jointInfos[jointId].q;
136  }
137 
138  inline bool isJointValid(int jointId) const {
139  if(jointId < 0 || jointId >= (int)jointInfos.size()){
140  return false;
141  }
142  return jointInfos[jointId].isValid;
143  }
144 
145  inline void setJointStationaryPoint(int jointId, bool on = true){
146  if(jointId >= (int)jointInfos.size()){
147  setNumJoints(jointId + 1);
148  }
149  jointInfos[jointId].isStationaryPoint = on;
150  }
151 
152  inline bool isJointStationaryPoint(int jointId) const {
153  if(jointId >= (int)jointInfos.size()){
154  return false;
155  }
156  return jointInfos[jointId].isStationaryPoint;
157  }
158 
159  inline bool invalidateJoint(int jointId) {
160  if(jointId < (int)jointInfos.size()){
161  if(jointInfos[jointId].isValid){
162  jointInfos[jointId].isValid = false;
163  return true;
164  }
165  }
166  return false;
167  }
168 
169  void clearIkLinks();
170 
171  inline size_t numIkLinks(){
172  return ikLinks.size();
173  }
174 
175  inline LinkInfo* addIkLink(int linkIndex){
176  return &ikLinks[linkIndex];
177  }
178 
179  bool removeIkLink(int linkIndex);
180 
181  inline const LinkInfo* ikLinkInfo(int linkIndex) const {
182  LinkInfoMap::const_iterator p = ikLinks.find(linkIndex);
183  return (p != ikLinks.end()) ? &p->second : 0;
184  }
185 
186  inline LinkInfo* ikLinkInfo(int linkIndex) {
187  LinkInfoMap::iterator p = ikLinks.find(linkIndex);
188  return (p != ikLinks.end()) ? &p->second : 0;
189  }
190 
191  inline LinkInfoMap::iterator ikLinkBegin() { return ikLinks.begin(); }
192  inline const LinkInfoMap::const_iterator ikLinkBegin() const { return ikLinks.begin(); }
193  inline LinkInfoMap::iterator ikLinkEnd() { return ikLinks.end(); }
194  inline const LinkInfoMap::const_iterator ikLinkEnd() const { return ikLinks.end(); }
195 
196  LinkInfo& setBaseLink(int linkIndex);
197 
198  inline LinkInfo& setBaseLink(int linkIndex, const Vector3& p, const Matrix3& R){
199  LinkInfo& info = setBaseLink(linkIndex);
200  info.p = p;
201  info.R = R;
202  return info;
203  }
204 
205  inline int baseLinkIndex() const {
206  return (baseLinkIter != ikLinks.end()) ? baseLinkIter->first : -1;
207  }
208 
209  inline LinkInfo* baseLinkInfo() {
210  return (baseLinkIter != ikLinks.end()) ? &baseLinkIter->second : 0;
211  }
212 
214  if(baseLinkIter != ikLinks.end()){
215  baseLinkIter->second.isBaseLink_ = false;
216  baseLinkIter = ikLinks.end();
217  }
218  }
219 
220  inline void setZmp(const Vector3& p){
221  isZmpValid_ = true;
222  zmp_ = p;
223  }
224 
225  inline const Vector3 zmp() const {
226  return zmp_;
227  }
228 
229  inline bool isZmpValid() const {
230  return isZmpValid_;
231  }
232 
233  inline bool invalidateZmp() {
234  bool ret = isZmpValid_;
235  isZmpValid_ = false;
236  return ret;
237  }
238 
239  inline void setZmpStationaryPoint(bool on = true){
240  isZmpStationaryPoint_ = on;
241  }
242 
243  inline bool isZmpStationaryPoint() const {
244  return isZmpStationaryPoint_;
245  }
246 
247 private:
248 
249  std::vector<JointInfo> jointInfos;
250  LinkInfoMap ikLinks;
251  LinkInfoMap::iterator baseLinkIter;
252  Vector3 zmp_;
253  bool isZmpValid_;
254  bool isZmpStationaryPoint_;
255  void initializeMembers();
256 };
257 
259 }
260 
261 #endif
bool isStationaryPoint() const
Definition: Pose.h:78
int numJoints() const
Definition: Pose.h:121
void setJointStationaryPoint(int jointId, bool on=true)
Definition: Pose.h:145
const LinkInfoMap::const_iterator ikLinkEnd() const
Definition: Pose.h:194
Definition: PoseSeq.h:22
void setJointPosition(int jointId, double q)
Definition: Pose.h:125
bool isBaseLink() const
Definition: Pose.h:76
Definition: ValueTree.h:224
LinkInfoMap::iterator ikLinkEnd()
Definition: Pose.h:193
ref_ptr< PoseUnit > PoseUnitPtr
Definition: Pose.h:19
void setSlave(bool on)
Definition: Pose.h:87
LinkInfo()
Definition: Pose.h:71
Definition: Referenced.h:67
bool invalidateJoint(int jointId)
Definition: Pose.h:159
LinkInfo * baseLinkInfo()
Definition: Pose.h:209
void invalidateBaseLink()
Definition: Pose.h:213
Definition: Pose.h:64
ref_ptr< Pose > PosePtr
Definition: Pose.h:258
void setZmpStationaryPoint(bool on=true)
Definition: Pose.h:239
LinkInfo & setBaseLink(int linkIndex, const Vector3 &p, const Matrix3 &R)
Definition: Pose.h:198
bool invalidateZmp()
Definition: Pose.h:233
size_t numIkLinks()
Definition: Pose.h:171
const Vector3 & partingDirection() const
Definition: Pose.h:80
Matrix3 R
Definition: Pose.h:69
bool isZmpValid() const
Definition: Pose.h:229
bool isJointValid(int jointId) const
Definition: Pose.h:138
bool isTouching() const
Definition: Pose.h:79
const LinkInfoMap::const_iterator ikLinkBegin() const
Definition: Pose.h:192
bool isZmpStationaryPoint() const
Definition: Pose.h:243
double jointPosition(int jointId) const
Definition: Pose.h:134
void setZmp(const Vector3 &p)
Definition: Pose.h:220
LinkInfoMap::iterator ikLinkBegin()
Definition: Pose.h:191
Defines the minimum processing for performing pasing file for STL.
Definition: AbstractSceneLoader.h:9
void setNumJoints(int n)
Definition: Pose.h:117
int baseLinkIndex() const
Definition: Pose.h:205
void setTouching(const Vector3 &partingDirection)
Definition: Pose.h:81
Eigen::Vector3d Vector3
Definition: EigenTypes.h:58
virtual bool hasSameParts(PoseUnitPtr unit)
Definition: Pose.h:34
LinkInfo * ikLinkInfo(int linkIndex)
Definition: Pose.h:186
#define CNOID_EXPORT
Definition: Util/exportdecl.h:37
Definition: PoseSeq.h:77
LinkInfo * addIkLink(int linkIndex)
Definition: Pose.h:175
bool isSlave() const
Definition: Pose.h:86
const Vector3 zmp() const
Definition: Pose.h:225
bool isJointStationaryPoint(int jointId) const
Definition: Pose.h:152
const std::string & name() const
Definition: Pose.h:39
Definition: Pose.h:53
Definition: Pose.h:23
void clearTouching()
Definition: Pose.h:85
const LinkInfo * ikLinkInfo(int linkIndex) const
Definition: Pose.h:181
std::map< int, LinkInfo > LinkInfoMap
Definition: Pose.h:98
void setStationaryPoint(bool on)
Definition: Pose.h:77
Eigen::Matrix3d Matrix3
Definition: EigenTypes.h:57
Vector3 p
Definition: Pose.h:68