Choreonoid  1.5
BodyMotion.h
Go to the documentation of this file.
1 
6 #ifndef CNOID_BODY_BODY_MOTION_H
7 #define CNOID_BODY_BODY_MOTION_H
8 
9 #include <cnoid/MultiValueSeq>
10 #include <cnoid/MultiSE3Seq>
11 #include <cnoid/Signal>
12 #include <boost/make_shared.hpp>
13 #include <map>
14 #include "exportdecl.h"
15 
16 namespace cnoid {
17 
18 class Body;
19 
21 {
22 public:
23  BodyMotion();
24  BodyMotion(const BodyMotion& org);
25 
26  BodyMotion& operator=(const BodyMotion& rhs);
27  virtual AbstractSeqPtr cloneSeq() const;
28 
29  virtual void setDimension(int numFrames, int numJoints, bool clearNewArea = false);
30 
31  void setDimension(int numFrames, int numJoints, int numLinks, bool clearNewArea = false);
32 
33  virtual void setNumParts(int numParts, bool clearNewElements = false);
34  virtual int getNumParts() const;
35 
36  int numJoints() const { return jointPosSeq_->numParts(); }
37  int numLinks() const { return linkPosSeq_->numParts(); }
38 
39  double frameRate() const { return jointPosSeq_->frameRate(); }
40  virtual double getFrameRate() const;
41  virtual void setFrameRate(double frameRate);
42 
43  double timeStep() const { return jointPosSeq_->timeStep(); }
44 
45  virtual int getOffsetTimeFrame() const;
46 
47  int numFrames() const {
48  return std::max(jointPosSeq_->numFrames(), linkPosSeq_->numFrames());
49  }
50  virtual int getNumFrames() const;
51  virtual void setNumFrames(int n, bool clearNewArea = false);
52 
54  return jointPosSeq_;
55  }
56 
57  const MultiValueSeqPtr& jointPosSeq() const {
58  return jointPosSeq_;
59  }
60 
62  return linkPosSeq_;
63  }
64 
65  const MultiSE3SeqPtr& linkPosSeq() const {
66  return linkPosSeq_;
67  }
68 
69  class Frame {
70  BodyMotion& motion_;
71  const int frame_;
72  Frame(BodyMotion& motion, int frame) : motion_(motion), frame_(frame) { }
73  public:
74  Frame(const Frame& org) : motion_(org.motion_), frame_(org.frame_) { }
75  BodyMotion& motion() { return motion_; }
76  int frame() const { return frame_; }
77  friend class BodyMotion;
78  };
79 
80  class ConstFrame {
81  const BodyMotion& motion_;
82  const int frame_;
83  ConstFrame(const BodyMotion& motion, int frame) : motion_(motion), frame_(frame) { }
84  public:
85  ConstFrame(const Frame& org) : motion_(org.motion_), frame_(org.frame_) { }
86  const BodyMotion& motion() const { return motion_; }
87  int frame() const { return frame_; }
88  friend class BodyMotion;
89  };
90 
91  Frame frame(int frame) { return Frame(*this, frame); }
92  ConstFrame frame(int frame) const { return ConstFrame(*this, frame); }
93 
94  virtual bool read(const Mapping& archive);
95  virtual bool write(YAMLWriter& writer);
96 
97  bool loadStandardYAMLformat(const std::string& filename);
98  bool saveAsStandardYAMLformat(const std::string& filename);
99 
100  typedef std::map<std::string, AbstractSeqPtr> ExtraSeqMap;
101  typedef ExtraSeqMap::const_iterator ConstSeqIterator;
102 
103  ConstSeqIterator extraSeqBegin() const { return extraSeqs.begin(); }
104  ConstSeqIterator extraSeqEnd() const { return extraSeqs.end(); }
105 
106  template <class SeqType>
107  boost::shared_ptr<SeqType> extraSeq(const std::string& contentName) const {
108  ExtraSeqMap::const_iterator p = extraSeqs.find(contentName);
109  return ((p != extraSeqs.end()) ?
110  boost::dynamic_pointer_cast<SeqType>(p->second) : boost::shared_ptr<SeqType>());
111  }
112 
113  void setExtraSeq(AbstractSeqPtr seq);
114 
115  template <class SeqType>
116  boost::shared_ptr<SeqType> getOrCreateExtraSeq(const std::string& contentName) {
117  AbstractSeqPtr& base = extraSeqs[contentName];
118  boost::shared_ptr<SeqType> seq;
119  if(base){
120  seq = boost::dynamic_pointer_cast<SeqType>(base);
121  }
122  if(!seq){
123  seq = boost::make_shared<SeqType>(numFrames());
124  seq->setFrameRate(frameRate());
125  base = seq;
126  sigExtraSeqsChanged_();
127  }
128  return seq;
129  }
130 
131  void clearExtraSeq(const std::string& contentName);
132 
134  return sigExtraSeqsChanged_;
135  }
136 
137 private:
138 
139  MultiValueSeqPtr jointPosSeq_;
140  MultiSE3SeqPtr linkPosSeq_;
141 
142  ExtraSeqMap extraSeqs;
143 
144  Signal<void()> sigExtraSeqsChanged_;
145 };
146 
147 typedef boost::shared_ptr<BodyMotion> BodyMotionPtr;
148 
149 class Body;
150 
156 CNOID_EXPORT const Body& operator>>(const Body& body, BodyMotion::Frame frame);
157 
158 }
159 
160 #endif
ConstSeqIterator extraSeqEnd() const
Definition: BodyMotion.h:104
CNOID_EXPORT std::ostream & operator<<(std::ostream &os, const BoundingBox &bb)
Definition: BoundingBox.cpp:153
double frameRate() const
Definition: BodyMotion.h:39
boost::shared_ptr< SeqType > extraSeq(const std::string &contentName) const
Definition: BodyMotion.h:107
Definition: Body.h:28
int frame() const
Definition: BodyMotion.h:76
ref_ptr< T > dynamic_pointer_cast(ref_ptr< U > const &p)
Definition: Referenced.h:280
double timeStep() const
Definition: BodyMotion.h:43
Definition: ValueTree.h:224
int numJoints() const
Definition: BodyMotion.h:36
MultiValueSeq::Ptr MultiValueSeqPtr
Definition: MultiValueSeq.h:36
boost::shared_ptr< BodyMotion > BodyMotionPtr
Definition: BodyMotion.h:147
int numLinks() const
Definition: BodyMotion.h:37
CNOID_EXPORT EasyScanner & operator>>(EasyScanner &scanner, double &value)
Definition: EasyScanner.cpp:758
Definition: YAMLWriter.h:18
SignalProxy< void()> sigExtraSeqsChanged()
Definition: BodyMotion.h:133
Definition: AbstractSeq.h:118
Definition: BodyMotion.h:80
Listing & write(Mapping &mapping, const std::string &key, const Eigen::MatrixBase< Derived > &x)
Definition: EigenArchive.h:62
boost::shared_ptr< AbstractSeq > AbstractSeqPtr
Definition: AbstractSeq.h:19
int frame() const
Definition: BodyMotion.h:87
MultiSE3SeqPtr & linkPosSeq()
Definition: BodyMotion.h:61
MultiSE3Seq::Ptr MultiSE3SeqPtr
Definition: MultiSE3Seq.h:50
Definition: BodyMotion.h:20
const MultiValueSeqPtr & jointPosSeq() const
Definition: BodyMotion.h:57
int numFrames() const
Definition: BodyMotion.h:47
void read(const Listing &listing, Eigen::MatrixBase< Derived > &x)
Definition: EigenArchive.h:17
std::map< std::string, AbstractSeqPtr > ExtraSeqMap
Definition: BodyMotion.h:100
Defines the minimum processing for performing pasing file for STL.
Definition: AbstractSceneLoader.h:9
const BodyMotion & motion() const
Definition: BodyMotion.h:86
boost::shared_ptr< SeqType > getOrCreateExtraSeq(const std::string &contentName)
Definition: BodyMotion.h:116
ConstFrame(const Frame &org)
Definition: BodyMotion.h:85
Frame(const Frame &org)
Definition: BodyMotion.h:74
ConstSeqIterator extraSeqBegin() const
Definition: BodyMotion.h:103
Frame frame(int frame)
Definition: BodyMotion.h:91
BodyMotion & motion()
Definition: BodyMotion.h:75
Definition: BodyMotion.h:69
#define CNOID_EXPORT
Definition: Util/exportdecl.h:37
MultiValueSeqPtr & jointPosSeq()
Definition: BodyMotion.h:53
ConstFrame frame(int frame) const
Definition: BodyMotion.h:92
Definition: Signal.h:380
ExtraSeqMap::const_iterator ConstSeqIterator
Definition: BodyMotion.h:101
const MultiSE3SeqPtr & linkPosSeq() const
Definition: BodyMotion.h:65