Choreonoid  1.1
BodyItem.h
[詳解]
1 
6 #ifndef CNOID_BODYPLUGIN_BODY_ITEM_H_INCLUDED
7 #define CNOID_BODYPLUGIN_BODY_ITEM_H_INCLUDED
8 
9 #include <deque>
10 #include <bitset>
11 #include <boost/dynamic_bitset.hpp>
12 #include <cnoid/Item>
13 #include <cnoid/YamlNodes>
14 #include <cnoid/LazySignal>
15 #include <cnoid/LazyCaller>
16 #include <cnoid/Body>
17 #include <cnoid/ModelNodeSet>
18 #include <cnoid/ColdetLinkPair>
19 #include <cnoid/InverseKinematics>
20 #include <cnoid/LinkGroup>
21 #include "exportdecl.h"
22 
23 namespace cnoid {
24 
25  class BodyItem;
26  typedef boost::intrusive_ptr<BodyItem> BodyItemPtr;
27 
28  class WorldItem;
29 
30  class PinDragIK;
31  typedef boost::shared_ptr<PinDragIK> PinDragIKptr;
32 
33  class PenetrationBlocker;
34  typedef boost::shared_ptr<PenetrationBlocker> PenetrationBlockerPtr;
35 
36  class LinkGroup;
37  class KinematicsBar;
38 
39  void initializeBodyItem(ExtensionManager& ext);
40 
41  class CNOID_EXPORT BodyItem : public Item
42  {
43  public:
44 
45  BodyItem();
46  BodyItem(const BodyItem& org);
47  virtual ~BodyItem();
48 
49  void init();
50 
51  bool loadModelFile(const std::string& filename);
52 
53  virtual void setName(const std::string& name);
54 
55  inline Body* body() { return body_.get(); }
56  inline ModelNodeSetPtr modelNodeSet() { return modelNodeSet_; }
57  const std::string& errorMessage() { return errorMessage_; }
58 
59  enum PresetPoseID { INITIAL_POSE, STANDARD_POSE };
60 
61  void moveToOrigin();
62 
63  void setPresetPose(PresetPoseID id);
64 
65  inline Link* currentBaseLink() const { return currentBaseLink_; }
66  void setCurrentBaseLink(Link* link);
67 
68  void calcForwardKinematics(bool calcVelocity = false, bool calcAcceleration = false);
69 
70  void copyKinematicState();
71  void pasteKinematicState();
72 
74  {
75  private:
76  std::vector<double> q;
77  Vector3 p;
78  Matrix3 R;
79  Vector3 zmp;
80  friend class BodyItem;
81  friend void cnoid::initializeBodyItem(ExtensionManager& ext);
82  };
83 
84  void storeKinematicState(KinematicState& state);
85  bool restoreKinematicState(const KinematicState& state);
86 
87  // for undo, redo operations
88  void beginKinematicStateEdit();
89  void acceptKinematicStateEdit();
90  bool undoKinematicState();
91  bool redoKinematicState();
92 
93  PinDragIKptr pinDragIK();
94  InverseKinematicsPtr getCurrentIK(Link* targetLink);
95  PenetrationBlockerPtr createPenetrationBlocker(Link* link, bool excludeSelfCollisions = false);
96 
104  SignalProxy< boost::signal<void()> > sigKinematicStateChanged() {
105  return sigKinematicStateChanged_.signal();
106  }
107 
108  void notifyKinematicStateChange(
109  bool requestFK = false, bool requestVelFK = false, bool requestAccFK = false);
110 
111  void notifyKinematicStateChange(
112  boost::signals::connection& connectionToBlock,
113  bool requestFK = false, bool requestVelFK = false, bool requestAccFK = false);
114 
115  SignalProxy< boost::signal<void()> > sigKinematicStateEdited() {
116  return sigKinematicStateEdited_.signal();
117  }
118 
122  WorldItem* worldItem() { return worldItem_; }
123 
124  void updateColdetModelPositions(bool force = false);
125 
126  void enableSelfCollisionDetection(bool on);
128  return isSelfCollisionDetectionEnabled_;
129  }
130  bool updateSelfCollisions(bool force = false);
131  void clearSelfCollisions();
132 
133  std::vector<ColdetLinkPairPtr> selfColdetPairs;
134  boost::dynamic_bitset<> selfCollisionLinkBitSet;
135 
136  SignalProxy< boost::signal<void()> > sigSelfCollisionsUpdated() {
137  return sigSelfCollisionsUpdated_;
138  }
139  SignalProxy< boost::signal<void()> > sigSelfCollisionLinkSetChanged() {
140  return sigSelfCollisionLinkSetChanged_;
141  }
142 
143  std::vector<ColdetLinkPairPtr>& worldColdetPairsOfLink(int linkIndex) {
144  return worldColdetPairsOfLink_[linkIndex];
145  }
146  const std::vector<ColdetLinkPairPtr>& worldColdetPairsOfLink(int linkIndex) const {
147  return worldColdetPairsOfLink_[linkIndex];
148  }
149 
150  boost::dynamic_bitset<> worldCollisionLinkBitSet;
151 
152  SignalProxy< boost::signal<void()> > sigWorldCollisionsUpdated() {
153  return sigWorldCollisionsUpdated_;
154  }
155  SignalProxy< boost::signal<void()> > sigWorldCollisionLinkSetChanged() {
156  return sigWorldCollisionLinkSetChanged_;
157  }
159  sigWorldCollisionLinkSetChanged_();
160  }
162  sigWorldCollisionsUpdated_();
163  }
164 
165  const Vector3& centerOfMass();
166 
167  bool doLegIkToMoveCm(const Vector3& c, bool onlyProjectionToFloor = false);
168 
169  inline const Vector3& zmp() { return zmp_; }
170  void setZmp(const Vector3& zmp) { zmp_ = zmp; }
171 
172  void editZmp(const Vector3& zmp);
173 
174  enum PositionType { CM_PROJECTION, HOME_COP, RIGHT_HOME_COP, LEFT_HOME_COP, ZMP };
175 
176  boost::optional<Vector3> getParticularPosition(PositionType posType);
177 
178  //void setZmp(ZmpPosition position);
179 
180  bool setStance(double width);
181 
182  const std::string modelFilePath() { return modelFilePath_; }
183 
184 
185  protected:
186 
187  virtual ItemPtr doDuplicate() const;
188  virtual void doPutProperties(PutPropertyFunction& putProperty);
189  virtual bool store(Archive& archive);
190  virtual bool restore(const Archive& archive);
191 
192  private:
193 
194  BodyPtr body_;
195  ModelNodeSetPtr modelNodeSet_;
196  std::string modelFilePath_;
197  std::string errorMessage_;
198 
199  enum { UF_POSITIONS, UF_VELOCITIES, UF_ACCELERATIONS, UF_CM, UF_ZMP, NUM_UPUDATE_FLAGS };
200  std::bitset<NUM_UPUDATE_FLAGS> updateFlags;
201 
202  typedef boost::shared_ptr<KinematicState> KinematicStatePtr;
203  std::deque<KinematicStatePtr> kinematicStateHistory;
204  size_t currentHistoryIndex;
205  bool isCurrentKinematicStateInHistory;
206  bool needToAppendKinematicStateToHistory;
207 
208  LazySignal< boost::signal<void()> > sigKinematicStateChanged_;
209  LazySignal< boost::signal<void()> > sigKinematicStateEdited_;
210  LazySignal< boost::signal<void()> > sigStateUpdated_;
211 
212  bool isCallingSlotsOnKinematicStateEdited;
213  bool isFkRequested;
214  bool isVelFkRequested;
215  bool isAccFkRequested;
216  Link* currentBaseLink_;
217  LinkTraverse fkTraverse;
218  PinDragIKptr pinDragIK_;
219  Vector3 zmp_;
220 
221  WorldItem* worldItem_;
222  std::vector< std::vector<ColdetLinkPairPtr> > worldColdetPairsOfLink_;
223 
224  bool isSelfCollisionDetectionEnabled_;
225  bool isSelfCollisionUpdateNeeded;
226  bool isColdetModelPositionUpdateNeeded;
227  KinematicsBar* kinematicsBar;
228  LazyCaller updateSelfCollisionsCaller;
229  boost::signal<void()> sigSelfCollisionsUpdated_;
230  boost::signal<void()> sigSelfCollisionLinkSetChanged_;
231  boost::signal<void()> sigWorldCollisionsUpdated_;
232  boost::signal<void()> sigWorldCollisionLinkSetChanged_;
233 
234  void initBody();
235  void emitSigKinematicStateChanged();
236  void emitSigKinematicStateEdited();
237  void appendKinematicStateToHistory();
238  void updateSelfColdetPairs();
239  bool onSelfCollisionDetectionPropertyChanged(bool on);
240  void onPositionChanged();
241  };
242 }
243 
244 #endif
void notifyWorldCollisionLinkSetChange()
Definition: BodyItem.h:158
boost::intrusive_ptr< Body > BodyPtr
Definition: Body.h:22
Definition: KinematicsBar.h:16
boost::shared_ptr< InverseKinematics > InverseKinematicsPtr
Definition: Body.h:28
Definition: LinkGroup.h:23
SignalProxy< boost::signal< void()> > sigSelfCollisionLinkSetChanged()
Definition: BodyItem.h:139
Definition: Body.h:45
Definition: PinDragIK.h:18
void initializeBodyItem(ExtensionManager &ext)
Definition: BodyItem.cpp:74
SignalProxy< boost::signal< void()> > sigWorldCollisionsUpdated()
Definition: BodyItem.h:152
boost::dynamic_bitset selfCollisionLinkBitSet
Definition: BodyItem.h:134
boost::shared_ptr< ModelNodeSet > ModelNodeSetPtr
Definition: BodyLoader.h:15
void setZmp(const Vector3 &zmp)
Definition: BodyItem.h:170
Definition: BodyItem.h:73
ModelNodeSetPtr modelNodeSet()
Definition: BodyItem.h:56
Body * body()
Definition: BodyItem.h:55
SignalProxy< boost::signal< void()> > sigKinematicStateEdited()
Definition: BodyItem.h:115
void notifyWorldCollisionUpdate()
Definition: BodyItem.h:161
const std::string modelFilePath()
Definition: BodyItem.h:182
SignalProxy< boost::signal< void()> > sigWorldCollisionLinkSetChanged()
Definition: BodyItem.h:155
Link * currentBaseLink() const
Definition: BodyItem.h:65
Definition: WorldItem.h:25
Definition: EasyScanner.h:16
const Vector3 & zmp()
Definition: BodyItem.h:169
PresetPoseID
Definition: BodyItem.h:59
bool isSelfCollisionDetectionEnabled()
Definition: BodyItem.h:127
std::vector< ColdetLinkPairPtr > selfColdetPairs
Definition: BodyItem.h:133
SignalProxy< boost::signal< void()> > sigSelfCollisionsUpdated()
Definition: BodyItem.h:136
std::vector< ColdetLinkPairPtr > & worldColdetPairsOfLink(int linkIndex)
Definition: BodyItem.h:143
boost::intrusive_ptr< BodyItem > BodyItemPtr
Definition: BodyItem.h:25
Eigen::Vector3d Vector3
Definition: EigenTypes.h:26
Definition: PenetrationBlocker.h:16
#define CNOID_EXPORT
Definition: Util/exportdecl.h:13
boost::dynamic_bitset worldCollisionLinkBitSet
Definition: BodyItem.h:150
Definition: BodyItem.h:41
PositionType
Definition: BodyItem.h:174
const std::string & errorMessage()
Definition: BodyItem.h:57
boost::shared_ptr< PenetrationBlocker > PenetrationBlockerPtr
Definition: PenetrationBlocker.h:28
const std::vector< ColdetLinkPairPtr > & worldColdetPairsOfLink(int linkIndex) const
Definition: BodyItem.h:146
SignalProxy< boost::signal< void()> > sigKinematicStateChanged()
Definition: BodyItem.h:104
boost::shared_ptr< PinDragIK > PinDragIKptr
Definition: PinDragIK.h:57
Eigen::Matrix3d Matrix3
Definition: EigenTypes.h:25
WorldItem * worldItem()
Definition: BodyItem.h:122