/usr/include/choreonoid-1.1/cnoid/src/PoseSeqPlugin/Pose.h is in libcnoid-dev 1.1.0+dfsg-6.1+b4.
This file is owned by root:root, with mode 0o644.
The actual contents of the file can be viewed below.
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 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 | /**
@file
@author Shin'ichiro Nakaoka
*/
#ifndef CNOID_CHOREOGRAPHY_POSE_H_INCLUDED
#define CNOID_CHOREOGRAPHY_POSE_H_INCLUDED
#include <cnoid/Body>
#include "exportdecl.h"
namespace cnoid {
class YamlMapping;
class PoseUnit;
class PoseSeq;
class PoseRef;
typedef boost::intrusive_ptr<PoseUnit> PoseUnitPtr;
class CNOID_EXPORT PoseUnit : public Referenced
{
public:
PoseUnit();
PoseUnit(const PoseUnit& org);
virtual ~PoseUnit();
virtual PoseUnit* duplicate() = 0;
virtual bool restore(const YamlMapping& archive, const BodyPtr body) = 0;
virtual void store(YamlMapping& archive, const BodyPtr body) const = 0;
virtual bool hasSameParts(PoseUnitPtr unit) { return false; }
/**
@note A name can be only set by PoseSeq::rename().
*/
inline const std::string& name() const {
return name_;
}
private:
std::string name_;
PoseSeq* owner;
int seqLocalReferenceCounter;
friend class PoseSeq;
friend class PoseRef;
};
class CNOID_EXPORT Pose : public PoseUnit
{
struct JointInfo {
inline JointInfo() : isValid(false), isStationaryPoint(false) { }
double q;
bool isValid;
bool isStationaryPoint;
};
public:
class LinkInfo {
public:
Vector3 p;
Matrix3 R;
inline LinkInfo() :
isBaseLink_(false),
isStationaryPoint_(false),
isTouching_(false),
isSlave_(false) { }
inline bool isBaseLink() const { return isBaseLink_; }
inline void setStationaryPoint(bool on){ isStationaryPoint_ = on; }
inline bool isStationaryPoint() const { return isStationaryPoint_; }
inline bool isTouching() const { return isTouching_; }
inline const Vector3& partingDirection() const { return partingDirection_; }
inline void setTouching(const Vector3& partingDirection) {
isTouching_ = true;
partingDirection_ = partingDirection;
}
inline void clearTouching() { isTouching_ = false; }
inline bool isSlave() const { return isSlave_; }
inline void setSlave(bool on) { isSlave_ = on; }
private:
bool isBaseLink_;
bool isStationaryPoint_;
bool isTouching_;
bool isSlave_;
Vector3 partingDirection_;
friend class Pose;
};
typedef std::map<int, LinkInfo> LinkInfoMap;
Pose();
Pose(int numJoints);
Pose(const Pose& org);
virtual ~Pose();
bool empty();
void clear();
virtual PoseUnit* duplicate();
virtual bool hasSameParts(PoseUnitPtr unit);
virtual bool restore(const YamlMapping& archive, const BodyPtr body);
virtual void store(YamlMapping& archive, const BodyPtr body) const;
inline void setNumJoints(int n){
jointInfos.resize(n);
}
inline int numJoints() const {
return jointInfos.size();
}
inline void setJointPosition(int jointId, double q){
if(jointId >= (int)jointInfos.size()){
setNumJoints(jointId + 1);
}
JointInfo& info = jointInfos[jointId];
info.q = q;
info.isValid = true;
}
inline double jointPosition(int jointId) const {
return jointInfos[jointId].q;
}
inline bool isJointValid(int jointId) const {
if(jointId < 0 || jointId >= (int)jointInfos.size()){
return false;
}
return jointInfos[jointId].isValid;
}
inline void setJointStationaryPoint(int jointId, bool on = true){
if(jointId >= (int)jointInfos.size()){
setNumJoints(jointId + 1);
}
jointInfos[jointId].isStationaryPoint = on;
}
inline bool isJointStationaryPoint(int jointId) const {
if(jointId >= (int)jointInfos.size()){
return false;
}
return jointInfos[jointId].isStationaryPoint;
}
inline bool invalidateJoint(int jointId) {
if(jointId < (int)jointInfos.size()){
if(jointInfos[jointId].isValid){
jointInfos[jointId].isValid = false;
return true;
}
}
return false;
}
void clearIkLinks();
inline size_t numIkLinks(){
return ikLinks.size();
}
inline LinkInfo* addIkLink(int linkIndex){
return &ikLinks[linkIndex];
}
bool removeIkLink(int linkIndex);
inline const LinkInfo* ikLinkInfo(int linkIndex) const {
LinkInfoMap::const_iterator p = ikLinks.find(linkIndex);
return (p != ikLinks.end()) ? &p->second : 0;
}
inline LinkInfo* ikLinkInfo(int linkIndex) {
LinkInfoMap::iterator p = ikLinks.find(linkIndex);
return (p != ikLinks.end()) ? &p->second : 0;
}
inline LinkInfoMap::iterator ikLinkBegin() { return ikLinks.begin(); }
inline const LinkInfoMap::const_iterator ikLinkBegin() const { return ikLinks.begin(); }
inline LinkInfoMap::iterator ikLinkEnd() { return ikLinks.end(); }
inline const LinkInfoMap::const_iterator ikLinkEnd() const { return ikLinks.end(); }
LinkInfo& setBaseLink(int linkIndex);
inline LinkInfo& setBaseLink(int linkIndex, const Vector3& p, const Matrix3& R){
LinkInfo& info = setBaseLink(linkIndex);
info.p = p;
info.R = R;
return info;
}
inline int baseLinkIndex() const {
return (baseLinkIter != ikLinks.end()) ? baseLinkIter->first : -1;
}
inline LinkInfo* baseLinkInfo() {
return (baseLinkIter != ikLinks.end()) ? &baseLinkIter->second : 0;
}
void invalidateBaseLink() {
if(baseLinkIter != ikLinks.end()){
baseLinkIter->second.isBaseLink_ = false;
baseLinkIter = ikLinks.end();
}
}
inline void setZmp(const Vector3& p){
isZmpValid_ = true;
zmp_ = p;
}
inline const Vector3 zmp() const {
return zmp_;
}
inline bool isZmpValid() const {
return isZmpValid_;
}
inline bool invalidateZmp() {
bool ret = isZmpValid_;
isZmpValid_ = false;
return ret;
}
inline void setZmpStationaryPoint(bool on = true){
isZmpStationaryPoint_ = on;
}
inline bool isZmpStationaryPoint() const {
return isZmpStationaryPoint_;
}
private:
std::vector<JointInfo> jointInfos;
LinkInfoMap ikLinks;
LinkInfoMap::iterator baseLinkIter;
Vector3 zmp_;
bool isZmpValid_;
bool isZmpStationaryPoint_;
void initializeMembers();
};
typedef boost::intrusive_ptr<Pose> PosePtr;
}
#endif
|