6 #ifndef CNOID_BODYPLUGIN_BODY_ITEM_H_INCLUDED
7 #define CNOID_BODYPLUGIN_BODY_ITEM_H_INCLUDED
11 #include <boost/dynamic_bitset.hpp>
13 #include <cnoid/YamlNodes>
14 #include <cnoid/LazySignal>
15 #include <cnoid/LazyCaller>
17 #include <cnoid/ModelNodeSet>
18 #include <cnoid/ColdetLinkPair>
19 #include <cnoid/InverseKinematics>
20 #include <cnoid/LinkGroup>
51 bool loadModelFile(
const std::string& filename);
53 virtual void setName(
const std::string& name);
63 void setPresetPose(PresetPoseID
id);
66 void setCurrentBaseLink(
Link* link);
68 void calcForwardKinematics(
bool calcVelocity =
false,
bool calcAcceleration =
false);
70 void copyKinematicState();
71 void pasteKinematicState();
76 std::vector<double> q;
88 void beginKinematicStateEdit();
89 void acceptKinematicStateEdit();
90 bool undoKinematicState();
91 bool redoKinematicState();
105 return sigKinematicStateChanged_.signal();
108 void notifyKinematicStateChange(
109 bool requestFK =
false,
bool requestVelFK =
false,
bool requestAccFK =
false);
111 void notifyKinematicStateChange(
112 boost::signals::connection& connectionToBlock,
113 bool requestFK =
false,
bool requestVelFK =
false,
bool requestAccFK =
false);
116 return sigKinematicStateEdited_.signal();
124 void updateColdetModelPositions(
bool force =
false);
126 void enableSelfCollisionDetection(
bool on);
128 return isSelfCollisionDetectionEnabled_;
130 bool updateSelfCollisions(
bool force =
false);
131 void clearSelfCollisions();
137 return sigSelfCollisionsUpdated_;
140 return sigSelfCollisionLinkSetChanged_;
144 return worldColdetPairsOfLink_[linkIndex];
147 return worldColdetPairsOfLink_[linkIndex];
153 return sigWorldCollisionsUpdated_;
156 return sigWorldCollisionLinkSetChanged_;
159 sigWorldCollisionLinkSetChanged_();
162 sigWorldCollisionsUpdated_();
167 bool doLegIkToMoveCm(
const Vector3& c,
bool onlyProjectionToFloor =
false);
172 void editZmp(
const Vector3& zmp);
174 enum PositionType { CM_PROJECTION, HOME_COP, RIGHT_HOME_COP, LEFT_HOME_COP, ZMP };
176 boost::optional<Vector3> getParticularPosition(PositionType posType);
180 bool setStance(
double width);
187 virtual ItemPtr doDuplicate()
const;
188 virtual void doPutProperties(PutPropertyFunction& putProperty);
189 virtual bool store(Archive& archive);
190 virtual bool restore(
const Archive& archive);
196 std::string modelFilePath_;
197 std::string errorMessage_;
199 enum { UF_POSITIONS, UF_VELOCITIES, UF_ACCELERATIONS, UF_CM, UF_ZMP, NUM_UPUDATE_FLAGS };
200 std::bitset<NUM_UPUDATE_FLAGS> updateFlags;
202 typedef boost::shared_ptr<KinematicState> KinematicStatePtr;
203 std::deque<KinematicStatePtr> kinematicStateHistory;
204 size_t currentHistoryIndex;
205 bool isCurrentKinematicStateInHistory;
206 bool needToAppendKinematicStateToHistory;
208 LazySignal< boost::signal<void()> > sigKinematicStateChanged_;
209 LazySignal< boost::signal<void()> > sigKinematicStateEdited_;
210 LazySignal< boost::signal<void()> > sigStateUpdated_;
212 bool isCallingSlotsOnKinematicStateEdited;
214 bool isVelFkRequested;
215 bool isAccFkRequested;
216 Link* currentBaseLink_;
217 LinkTraverse fkTraverse;
221 WorldItem* worldItem_;
222 std::vector< std::vector<ColdetLinkPairPtr> > worldColdetPairsOfLink_;
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_;
235 void emitSigKinematicStateChanged();
236 void emitSigKinematicStateEdited();
237 void appendKinematicStateToHistory();
238 void updateSelfColdetPairs();
239 bool onSelfCollisionDetectionPropertyChanged(
bool on);
240 void onPositionChanged();