Choreonoid  1.8
MprPosition.h
Go to the documentation of this file.
1 #ifndef CNOID_MANIPULATOR_PLUGIN_MPR_POSITION_H
2 #define CNOID_MANIPULATOR_PLUGIN_MPR_POSITION_H
3 
4 #include <cnoid/CoordinateFrameList>
5 #include <cnoid/ClonableReferenced>
6 #include <cnoid/GeneralId>
7 #include <cnoid/EigenTypes>
8 #include <cnoid/Signal>
9 #include <string>
10 #include <array>
11 #include <bitset>
12 #include "exportdecl.h"
13 
14 namespace cnoid {
15 
16 class Body;
17 class LinkKinematicsKit;
18 class MprIkPosition;
19 class MprFkPosition;
20 class MprPositionList;
21 class Mapping;
22 
23 class CNOID_EXPORT MprPosition : public ClonableReferenced
24 {
25 public:
26  static constexpr int MaxNumJoints = 8;
27 
28  enum PositionType { IK, FK };
29 
30  MprPosition& operator=(const MprPosition& rhs) = delete;
31 
32  MprPosition* clone() const {
33  return static_cast<MprPosition*>(doClone(nullptr));
34  }
35 
36  const GeneralId& id() const { return id_; }
37 
39  void setId(const GeneralId& id);
40 
41  int positionType() const { return positionType_; }
42  bool isIK() const { return (positionType_ == IK); };
43  bool isFK() const { return (positionType_ == FK); };
44 
45  MprIkPosition* ikPosition();
46  MprFkPosition* fkPosition();
47 
48  MprPositionList* ownerPositionList();
49 
50  virtual bool fetch(LinkKinematicsKit* kinematicsKit) = 0;
51  virtual bool apply(LinkKinematicsKit* kinematicsKit) const = 0;
52 
53  const std::string& note() const { return note_; }
54  void setNote(const std::string& note) { note_ = note; }
55 
56  virtual bool read(const Mapping& archive);
57  virtual bool write(Mapping& archive) const;
58 
59  enum UpdateFlag {
60  IdUpdate = 1 << 0,
61  NoteUpdate = 1 << 1,
62  PositionUpdate = 1 << 2,
63  ObjectReplaced = 1 << 3
64  };
65  SignalProxy<void(int flags)> sigUpdated() { return sigUpdated_; }
66  void notifyUpdate(int flags);
67 
68 protected:
69  MprPosition(PositionType type);
70  MprPosition(PositionType type, const GeneralId& id);
71  MprPosition(const MprPosition& org);
72 
73 private:
74  PositionType positionType_;
75  GeneralId id_;
76  std::string note_;
77  weak_ref_ptr<MprPositionList> ownerPositionList_;
78  Signal<void(int flags)> sigUpdated_;
79 
80  friend class MprPositionList;
81 };
82 
84 
85 
86 class CNOID_EXPORT MprIkPosition : public MprPosition
87 {
88 public:
89  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
90 
91  MprIkPosition();
92  MprIkPosition(const GeneralId& id);
93  MprIkPosition(const MprIkPosition& org);
94  MprIkPosition& operator=(const MprIkPosition& rhs) = delete;
95 
96  const Isometry3& position() const { return T; }
97  void setPosition(const Isometry3& T) { this->T = T; }
98  Vector3 rpy() const;
99  void setRpy(const Vector3& rpy);
100  const Vector3 referenceRpy() const { return referenceRpy_; }
101  void setReferenceRpy(const Vector3& rpy);
102  void resetReferenceRpy();
103 
104  void setBaseFrameId(const GeneralId& id){ baseFrameId_ = id; }
105  void setOffsetFrameId(const GeneralId& id){ offsetFrameId_ = id; }
106  const GeneralId& baseFrameId() const { return baseFrameId_; }
107  const GeneralId& offsetFrameId() const { return offsetFrameId_; }
108 
109  enum FrameType { BaseFrame = 0, OffsetFrame = 1 };
110  const GeneralId& frameId(int frameType) const {
111  return (frameType == BaseFrame) ? baseFrameId_ : offsetFrameId_;
112  }
113 
115  return baseFrames->findFrame(baseFrameId_);
116  }
118  return offsetFrames->findFrame(offsetFrameId_);
119  }
121  return (frameType == BaseFrame) ? findBaseFrame(frames) : findOffsetFrame(frames);
122  }
123 
124  int configuration() const { return configuration_; }
125 
127  virtual bool fetch(LinkKinematicsKit* kinematicsKit) override;
128  virtual bool apply(LinkKinematicsKit* kinematicsKit) const override;
129  virtual bool read(const Mapping& archive) override;
130  virtual bool write(Mapping& archive) const override;
131 
132 protected:
133  virtual Referenced* doClone(CloneMap* cloneMap) const override;
134 
135 private:
136  Isometry3 T;
137  Vector3 referenceRpy_;
138  GeneralId baseFrameId_;
139  GeneralId offsetFrameId_;
140  int configuration_;
141  std::array<int, MaxNumJoints> phase_;
142 };
143 
145 
146 
147 class CNOID_EXPORT MprFkPosition : public MprPosition
148 {
149  typedef std::array<double, MaxNumJoints> JointDisplacementArray;
150 
151 public:
152  MprFkPosition();
153  MprFkPosition(const GeneralId& id);
154  MprFkPosition(const MprFkPosition& org);
155  MprFkPosition& operator=(const MprFkPosition& rhs) = delete;
156 
157  virtual bool fetch(LinkKinematicsKit* kinematicsKit) override;
158  virtual bool apply(LinkKinematicsKit* kinematicsKit) const override;
159  virtual bool read(const Mapping& archive) override;
160  virtual bool write(Mapping& archive) const override;
161 
162  int numJoints() const { return numJoints_; }
163 
164  typedef JointDisplacementArray::iterator iterator;
165  typedef JointDisplacementArray::const_iterator const_iterator;
166 
167  iterator begin() { return jointDisplacements_.begin(); }
168  const_iterator begin() const { return jointDisplacements_.cbegin(); }
169  iterator end() { return jointDisplacements_.begin() + numJoints_; }
170  const_iterator end() const { return jointDisplacements_.cbegin() + numJoints_; }
171 
172  double& jointDisplacement(int index) { return jointDisplacements_[index]; }
173  double jointDisplacement(int index) const { return jointDisplacements_[index]; }
174  double& q(int index) { return jointDisplacements_[index]; }
175  double q(int index) const { return jointDisplacements_[index]; }
176 
177  bool checkIfPrismaticJoint(int index) const { return prismaticJointFlags_[index]; }
178  bool checkIfRevoluteJoint(int index) const { return !prismaticJointFlags_[index]; }
179 
180 protected:
181  virtual Referenced* doClone(CloneMap* cloneMap) const override;
182 
183 private:
184  JointDisplacementArray jointDisplacements_;
185  std::bitset<MaxNumJoints> prismaticJointFlags_;
186  int numJoints_;
187 };
188 
190 
191 }
192 
193 #endif
cnoid::MprFkPosition::begin
iterator begin()
Definition: MprPosition.h:167
cnoid::Mapping
Definition: ValueTree.h:253
cnoid::MprPosition::clone
MprPosition * clone() const
Definition: MprPosition.h:32
cnoid::MprIkPosition::setBaseFrameId
void setBaseFrameId(const GeneralId &id)
Definition: MprPosition.h:104
cnoid::GeneralId
Definition: GeneralId.h:11
cnoid::MprPosition::id
const GeneralId & id() const
Definition: MprPosition.h:36
cnoid::Vector3
Eigen::Vector3d Vector3
Definition: EigenTypes.h:57
cnoid::write
Listing * write(Mapping *mapping, const std::string &key, const Eigen::MatrixBase< Derived > &x)
Definition: EigenArchive.h:145
cnoid::MprFkPosition::q
double & q(int index)
Definition: MprPosition.h:174
cnoid::MprFkPosition::q
double q(int index) const
Definition: MprPosition.h:175
cnoid::MprIkPosition
Definition: MprPosition.h:86
cnoid::MprPosition::sigUpdated
SignalProxy< void(int flags)> sigUpdated()
Definition: MprPosition.h:65
cnoid::MprIkPosition::referenceRpy
const Vector3 referenceRpy() const
Definition: MprPosition.h:100
cnoid::MprFkPosition
Definition: MprPosition.h:147
cnoid::Isometry3
Eigen::Isometry3d Isometry3
Definition: EigenTypes.h:73
cnoid::MprFkPosition::jointDisplacement
double & jointDisplacement(int index)
Definition: MprPosition.h:172
cnoid::weak_ref_ptr
Definition: Referenced.h:276
cnoid::MprIkPosition::findFrame
CoordinateFrame * findFrame(CoordinateFrameList *frames, int frameType)
Definition: MprPosition.h:120
cnoid::MprFkPosition::numJoints
int numJoints() const
Definition: MprPosition.h:162
cnoid::MprIkPositionPtr
ref_ptr< MprIkPosition > MprIkPositionPtr
Definition: MprPosition.h:144
cnoid::MprFkPosition::checkIfPrismaticJoint
bool checkIfPrismaticJoint(int index) const
Definition: MprPosition.h:177
cnoid::MprIkPosition::findBaseFrame
CoordinateFrame * findBaseFrame(CoordinateFrameList *baseFrames)
Definition: MprPosition.h:114
cnoid::CoordinateFrameList::findFrame
CoordinateFrame * findFrame(const GeneralId &id) const
Definition: CoordinateFrameList.cpp:160
cnoid::read
bool read(const Mapping *mapping, const std::string &key, Eigen::MatrixBase< Derived > &x)
Definition: EigenArchive.h:43
cnoid::MprPosition::note
const std::string & note() const
Definition: MprPosition.h:53
cnoid::MprPosition::setNote
void setNote(const std::string &note)
Definition: MprPosition.h:54
cnoid::MprPositionPtr
ref_ptr< MprPosition > MprPositionPtr
Definition: MprPosition.h:83
cnoid::ref_ptr
Definition: Referenced.h:103
cnoid::MprIkPosition::setOffsetFrameId
void setOffsetFrameId(const GeneralId &id)
Definition: MprPosition.h:105
cnoid::MprIkPosition::configuration
int configuration() const
Definition: MprPosition.h:124
cnoid::CoordinateFrame
Definition: CoordinateFrame.h:16
cnoid::MprPosition::UpdateFlag
UpdateFlag
Definition: MprPosition.h:59
cnoid::MprIkPosition::frameId
const GeneralId & frameId(int frameType) const
Definition: MprPosition.h:110
cnoid
Definition: AbstractSceneLoader.h:11
cnoid::MprFkPosition::end
const_iterator end() const
Definition: MprPosition.h:170
cnoid::CoordinateFrameList
Definition: CoordinateFrameList.h:12
cnoid::ClonableReferenced
Definition: ClonableReferenced.h:10
cnoid::LinkKinematicsKit
Definition: LinkKinematicsKit.h:23
cnoid::CloneMap
Definition: CloneMap.h:13
cnoid::MprPosition::positionType
int positionType() const
Definition: MprPosition.h:41
cnoid::MprIkPosition::offsetFrameId
const GeneralId & offsetFrameId() const
Definition: MprPosition.h:107
cnoid::MprFkPosition::begin
const_iterator begin() const
Definition: MprPosition.h:168
cnoid::MprIkPosition::FrameType
FrameType
Definition: MprPosition.h:109
cnoid::MprPosition
Definition: MprPosition.h:23
cnoid::MprPosition::isIK
bool isIK() const
Definition: MprPosition.h:42
cnoid::Referenced
Definition: Referenced.h:54
cnoid::MprIkPosition::setPosition
void setPosition(const Isometry3 &T)
Definition: MprPosition.h:97
cnoid::MprPosition::isFK
bool isFK() const
Definition: MprPosition.h:43
cnoid::MprIkPosition::baseFrameId
const GeneralId & baseFrameId() const
Definition: MprPosition.h:106
cnoid::MprFkPosition::end
iterator end()
Definition: MprPosition.h:169
cnoid::MprFkPosition::checkIfRevoluteJoint
bool checkIfRevoluteJoint(int index) const
Definition: MprPosition.h:178
cnoid::MprPosition::PositionType
PositionType
Definition: MprPosition.h:28
cnoid::MprFkPosition::iterator
JointDisplacementArray::iterator iterator
Definition: MprPosition.h:164
cnoid::Signal
Definition: Signal.h:165
cnoid::MprFkPosition::jointDisplacement
double jointDisplacement(int index) const
Definition: MprPosition.h:173
cnoid::SignalProxy
Definition: Signal.h:470
cnoid::MprPositionList
Definition: MprPositionList.h:14
cnoid::MprIkPosition::findOffsetFrame
CoordinateFrame * findOffsetFrame(CoordinateFrameList *offsetFrames)
Definition: MprPosition.h:117
cnoid::MprFkPosition::const_iterator
JointDisplacementArray::const_iterator const_iterator
Definition: MprPosition.h:165
cnoid::MprIkPosition::position
const Isometry3 & position() const
Definition: MprPosition.h:96
cnoid::MprFkPositionPtr
ref_ptr< MprFkPosition > MprFkPositionPtr
Definition: MprPosition.h:189