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();
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: 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
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