Pose.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package RobotAPI::Core
19  * @author ( stefan dot ulbrich at kit dot edu)
20  * @date
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #pragma once
26 
27 #include <Eigen/Core>
28 #include <Eigen/Geometry>
29 
31 
32 #include <RobotAPI/interface/core/PoseBase.h>
33 
34 namespace armarx::VariantType
35 {
36  // variant types
37  const VariantTypeId Vector2 = Variant::addTypeName("::armarx::Vector2Base");
38  const VariantTypeId Vector3 = Variant::addTypeName("::armarx::Vector3Base");
39  const VariantTypeId Quaternion = Variant::addTypeName("::armarx::QuaternionBase");
40  const VariantTypeId Pose = Variant::addTypeName("::armarx::PoseBase");
41 } // namespace armarx::VariantType
42 
43 namespace armarx
44 {
45  const Eigen::IOFormat
46  ArmarXEigenFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", "\n", "", "");
47 
48  /**
49  * @brief The Vector2 class
50  * @ingroup VariantsGrp
51  */
52  class Vector2 : virtual public Vector2Base
53  {
54  public:
55  Vector2();
56  Vector2(const Eigen::Vector2f&);
57  Vector2(::Ice::Float x, ::Ice::Float y);
58 
59  void operator=(const Eigen::Vector2f& ves);
60 
61  virtual Eigen::Vector2f toEigen() const;
62 
63  // inherited from VariantDataClass
65  ice_clone() const override
66  {
67  return this->clone();
68  }
69 
70  VariantDataClassPtr
71  clone(const Ice::Current& = Ice::emptyCurrent) const override
72  {
73  return new Vector2(*this);
74  }
75 
76  std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
77 
79  getType(const Ice::Current& = Ice::emptyCurrent) const override
80  {
81  return VariantType::Vector2;
82  }
83 
84  bool
85  validate(const Ice::Current& = Ice::emptyCurrent) override
86  {
87  return true;
88  }
89 
90  friend std::ostream&
91  operator<<(std::ostream& stream, const Vector2& rhs)
92  {
93  stream << "Vector2: " << std::endl << rhs.output() << std::endl;
94  return stream;
95  }
96 
97  public: // serialization
98  void serialize(const armarx::ObjectSerializerBasePtr& serializer,
99  const ::Ice::Current& = Ice::emptyCurrent) const override;
100  void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
101  const ::Ice::Current& = Ice::emptyCurrent) override;
102  };
103 
105 
106  /**
107  * @class Vector3
108  * @ingroup VariantsGrp
109  * @ingroup RobotAPI-FramedPose
110  * @brief The Vector3 class
111  */
112  class Vector3 : virtual public Vector3Base
113  {
114  public:
115  Vector3();
116  Vector3(const Eigen::Vector3f&);
117  Vector3(const Eigen::Matrix4f&);
118  Vector3(::Ice::Float x, ::Ice::Float y, ::Ice::Float z);
119 
120  void operator=(const Eigen::Vector3f& vec);
121 
122  virtual Eigen::Vector3f toEigen() const;
123 
124  // inherited from VariantDataClass
126  ice_clone() const override
127  {
128  return this->clone();
129  }
130 
131  VariantDataClassPtr
132  clone(const Ice::Current& c = Ice::emptyCurrent) const override
133  {
134  return new Vector3(*this);
135  }
136 
137  std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
138 
140  getType(const Ice::Current& = Ice::emptyCurrent) const override
141  {
142  return VariantType::Vector3;
143  }
144 
145  bool
146  validate(const Ice::Current& = Ice::emptyCurrent) override
147  {
148  return true;
149  }
150 
151  friend std::ostream&
152  operator<<(std::ostream& stream, const Vector3& rhs)
153  {
154  stream << "Vector3: " << std::endl << rhs.output() << std::endl;
155  return stream;
156  }
157 
158  public: // serialization
159  void serialize(const armarx::ObjectSerializerBasePtr& serializer,
160  const ::Ice::Current& = Ice::emptyCurrent) const override;
161  void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
162  const ::Ice::Current& = Ice::emptyCurrent) override;
163  };
164 
166 
167  /**
168  * @class Quaternion
169  * @ingroup VariantsGrp
170  * @ingroup RobotAPI-FramedPose
171  * @brief The Quaternion class
172  */
173  class Quaternion : virtual public QuaternionBase
174  {
175  public:
176  /// Construct an identity quaternion.
177  Quaternion();
178  Quaternion(const Eigen::Matrix4f&);
179  Quaternion(const Eigen::Matrix3f&);
181  Quaternion(::Ice::Float qw, ::Ice::Float qx, ::Ice::Float qy, ::Ice::Float qz);
182 
183  Eigen::Matrix3f toEigen() const;
185  Eigen::Matrix3f slerp(float, const Eigen::Matrix3f&);
186 
187  static Eigen::Matrix3f slerp(float, const Eigen::Matrix3f&, const Eigen::Matrix3f&);
188 
189  // inherited from VariantDataClass
191  ice_clone() const override
192  {
193  return this->clone();
194  }
195 
196  VariantDataClassPtr
197  clone(const Ice::Current& = Ice::emptyCurrent) const override
198  {
199  return new Quaternion(*this);
200  }
201 
202  std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
203 
205  getType(const Ice::Current& = Ice::emptyCurrent) const override
206  {
208  }
209 
210  bool
211  validate(const Ice::Current& = Ice::emptyCurrent) override
212  {
213  return true;
214  }
215 
216  friend std::ostream&
217  operator<<(std::ostream& stream, const Quaternion& rhs)
218  {
219  stream << "Quaternion: " << std::endl << rhs.output() << std::endl;
220  return stream;
221  }
222 
223  public: // serialization
224  void serialize(const armarx::ObjectSerializerBasePtr& serializer,
225  const ::Ice::Current& = Ice::emptyCurrent) const override;
226  void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
227  const ::Ice::Current& = Ice::emptyCurrent) override;
228 
229  private:
230  void init(const Eigen::Matrix3f&);
231  void init(const Eigen::Quaternionf&);
232  };
233 
235 
236  /**
237  * @class Pose
238  * @ingroup VariantsGrp
239  * @ingroup RobotAPI-FramedPose
240  * @brief The Pose class
241  */
242  class Pose : virtual public PoseBase
243  {
244  public:
245  Pose();
246  Pose(const Pose& source);
247  Pose(const Eigen::Matrix4f&);
248  Pose(const Eigen::Matrix3f&, const Eigen::Vector3f&);
249  Pose(const armarx::Vector3BasePtr pos, const armarx::QuaternionBasePtr ori);
250  Pose& operator=(const Pose&) = default;
251 
252  void operator=(const Eigen::Matrix4f& matrix);
253  virtual Eigen::Matrix4f toEigen() const;
254 
255  // inherited from VariantDataClass
257  ice_clone() const override
258  {
259  return this->clone();
260  }
261 
262  VariantDataClassPtr
263  clone(const Ice::Current& = Ice::emptyCurrent) const override
264  {
265  return new Pose(*this);
266  }
267 
268  std::string output(const Ice::Current& = Ice::emptyCurrent) const override;
269 
271  getType(const Ice::Current& = Ice::emptyCurrent) const override
272  {
273  return VariantType::Pose;
274  }
275 
276  bool
277  validate(const Ice::Current& = Ice::emptyCurrent) override
278  {
279  return true;
280  }
281 
282  friend std::ostream&
283  operator<<(std::ostream& stream, const Pose& rhs)
284  {
285  stream << "Pose: " << std::endl << rhs.output() << std::endl;
286  return stream;
287  }
288 
289  public: // serialization
290  void serialize(const armarx::ObjectSerializerBasePtr& serializer,
291  const ::Ice::Current& = Ice::emptyCurrent) const override;
292  void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
293  const ::Ice::Current& = Ice::emptyCurrent) override;
294 
295  protected:
296  void init();
297  void ice_postUnmarshal() override;
298 
299  private:
300  //! To void unnecessary upcasts
301  QuaternionPtr c_orientation;
302  Vector3Ptr c_position;
303  };
304 
306 
307 
308  // Ice conversion functions
309 
310  void fromIce(const Vector3BasePtr& ice, Eigen::Vector3f& vector);
311  void fromIce(const QuaternionBasePtr& ice, Eigen::Quaternionf& quaternion);
312  void fromIce(const QuaternionBasePtr& ice, Eigen::Matrix3f& rotation);
313  void fromIce(const PoseBasePtr& ice, Eigen::Matrix4f& pose);
314  void fromIce(const QuaternionBasePtr& ice,
316  void fromIce(const PoseBasePtr& ice, Eigen::Matrix<float, 4, 4, Eigen::RowMajor>& pose);
317 
318  Eigen::Vector3f fromIce(const Vector3BasePtr& position);
319  Eigen::Quaternionf fromIce(const QuaternionBasePtr& rotation);
320  Eigen::Matrix4f fromIce(const PoseBasePtr& pose);
321 
322  void toIce(Vector3BasePtr& ice, const Eigen::Vector3f& vector);
323  void toIce(QuaternionBasePtr& ice, const Eigen::Matrix3f& rotation);
324  void toIce(QuaternionBasePtr& ice, const Eigen::Matrix<float, 3, 3, Eigen::RowMajor>& rotation);
325  void toIce(QuaternionBasePtr& ice, const Eigen::Quaternionf& quaternion);
326  void toIce(PoseBasePtr& ice, const Eigen::Matrix4f& pose);
327  void toIce(PoseBasePtr& ice, const Eigen::Matrix<float, 4, 4, Eigen::RowMajor>& pose);
328 
329  Vector3Ptr toIce(const Eigen::Vector3f& vector);
330  QuaternionPtr toIce(const Eigen::Matrix3f& rotation);
332  QuaternionPtr toIce(const Eigen::Quaternionf& quaternion);
333  PosePtr toIce(const Eigen::Matrix4f& pose);
335  PosePtr toIce(const Eigen::Isometry3f& pose);
336 
337 } // namespace armarx
338 
339 extern template class ::IceInternal::Handle<::armarx::Pose>;
340 extern template class ::IceInternal::Handle<::armarx::Vector2>;
341 extern template class ::IceInternal::Handle<::armarx::Vector3>;
342 extern template class ::IceInternal::Handle<::armarx::Quaternion>;
armarx::Vector2::clone
VariantDataClassPtr clone(const Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.h:71
armarx::Vector3::clone
VariantDataClassPtr clone(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: Pose.h:132
armarx::VariantType::Float
const VariantTypeId Float
Definition: Variant.h:918
armarx::Pose::clone
VariantDataClassPtr clone(const Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.h:263
armarx::Vector3::getType
VariantTypeId getType(const Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.h:140
armarx::Pose::Pose
Pose()
Definition: Pose.cpp:293
armarx::Quaternion::toEigenQuaternion
Eigen::Quaternionf toEigenQuaternion() const
Definition: Pose.cpp:210
armarx::Vector2::output
std::string output(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: Pose.cpp:80
armarx::Pose::getType
VariantTypeId getType(const Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.h:271
armarx::Quaternion::output
std::string output(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: Pose.cpp:250
armarx::Vector3::operator=
void operator=(const Eigen::Vector3f &vec)
Definition: Pose.cpp:142
armarx::Quaternion::operator<<
friend std::ostream & operator<<(std::ostream &stream, const Quaternion &rhs)
Definition: Pose.h:217
armarx::Vector3::Vector3
Vector3()
Definition: Pose.cpp:105
armarx::Vector2::ice_clone
Ice::ObjectPtr ice_clone() const override
Definition: Pose.h:65
armarx::Vector2::serialize
void serialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.cpp:88
armarx::VariantType::Vector3
const VariantTypeId Vector3
Definition: Pose.h:38
armarx::Vector2::operator=
void operator=(const Eigen::Vector2f &ves)
Definition: Pose.cpp:73
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::Pose::init
void init()
Definition: Pose.cpp:362
armarx::Quaternion::deserialize
void deserialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: Pose.cpp:269
armarx::VariantType::Vector2
const VariantTypeId Vector2
Definition: Pose.h:37
armarx::Pose::validate
bool validate(const Ice::Current &=Ice::emptyCurrent) override
Definition: Pose.h:277
armarx::Vector2
The Vector2 class.
Definition: Pose.h:52
armarx::Vector3::validate
bool validate(const Ice::Current &=Ice::emptyCurrent) override
Definition: Pose.h:146
armarx::Vector3::deserialize
void deserialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: Pose.cpp:168
armarx::Vector2::operator<<
friend std::ostream & operator<<(std::ostream &stream, const Vector2 &rhs)
Definition: Pose.h:91
armarx::Quaternion::getType
VariantTypeId getType(const Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.h:205
armarx::VariantType::Quaternion
const VariantTypeId Quaternion
Definition: Pose.h:39
armarx::Pose::operator=
Pose & operator=(const Pose &)=default
IceInternal::Handle
Definition: forward_declarations.h:8
armarx::ArmarXEigenFormat
const Eigen::IOFormat ArmarXEigenFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", "\n", "", "")
armarx::Quaternion::slerp
Eigen::Matrix3f slerp(float, const Eigen::Matrix3f &)
Definition: Pose.cpp:233
armarx::toIce
void toIce(std::map< IceKeyT, IceValueT > &iceMap, const boost::container::flat_map< CppKeyT, CppValueT > &cppMap)
Definition: ice_conversions_boost_templates.h:15
armarx::Vector2::getType
VariantTypeId getType(const Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.h:79
armarx::Pose::operator<<
friend std::ostream & operator<<(std::ostream &stream, const Pose &rhs)
Definition: Pose.h:283
armarx::Vector3
The Vector3 class.
Definition: Pose.h:112
armarx::Pose::toEigen
virtual Eigen::Matrix4f toEigen() const
Definition: Pose.cpp:327
armarx::Pose::serialize
void serialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.cpp:346
armarx::VariantType
Definition: ChannelRef.h:160
armarx::Quaternion::validate
bool validate(const Ice::Current &=Ice::emptyCurrent) override
Definition: Pose.h:211
Eigen::Quaternionf
Quaternion< float, 0 > Quaternionf
Definition: EigenForwardDeclarations.h:61
armarx::Pose::deserialize
void deserialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: Pose.cpp:353
armarx::VariantTypeId
Ice::Int VariantTypeId
Definition: Variant.h:44
boost::source
Vertex source(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:681
armarx::Vector3::ice_clone
Ice::ObjectPtr ice_clone() const override
Definition: Pose.h:126
armarx::Quaternion::toEigen
Eigen::Matrix3f toEigen() const
Definition: Pose.cpp:204
armarx::fromIce
void fromIce(const std::map< IceKeyT, IceValueT > &iceMap, boost::container::flat_map< CppKeyT, CppValueT > &cppMap)
Definition: ice_conversions_boost_templates.h:26
armarx::Vector3::output
std::string output(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: Pose.cpp:150
armarx::Pose
The Pose class.
Definition: Pose.h:242
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::Vector2::Vector2
Vector2()
Definition: Pose.cpp:46
armarx::Vector2::validate
bool validate(const Ice::Current &=Ice::emptyCurrent) override
Definition: Pose.h:85
armarx::Vector2::deserialize
void deserialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: Pose.cpp:97
armarx::Vector3::toEigen
virtual Eigen::Vector3f toEigen() const
Definition: Pose.cpp:134
armarx::Vector3::operator<<
friend std::ostream & operator<<(std::ostream &stream, const Vector3 &rhs)
Definition: Pose.h:152
armarx::Quaternion
The Quaternion class.
Definition: Pose.h:173
armarx::Quaternion::clone
VariantDataClassPtr clone(const Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.h:197
armarx::Vector2::toEigen
virtual Eigen::Vector2f toEigen() const
Definition: Pose.cpp:65
armarx::Pose::ice_postUnmarshal
void ice_postUnmarshal() override
Definition: Pose.cpp:369
armarx::Quaternion::ice_clone
Ice::ObjectPtr ice_clone() const override
Definition: Pose.h:191
Eigen::Matrix
Definition: EigenForwardDeclarations.h:27
armarx::Vector3::serialize
void serialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.cpp:158
armarx::aron::type::ObjectPtr
std::shared_ptr< Object > ObjectPtr
Definition: Object.h:36
armarx::Quaternion::Quaternion
Quaternion()
Construct an identity quaternion.
Variant.h
armarx::Pose::output
std::string output(const Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.cpp:338
armarx::Pose::ice_clone
Ice::ObjectPtr ice_clone() const override
Definition: Pose.h:257
armarx::VariantType::Pose
const VariantTypeId Pose
Definition: Pose.h:40
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::Variant::addTypeName
static VariantTypeId addTypeName(const std::string &typeName)
Register a new type for the use in a Variant.
Definition: Variant.cpp:751
armarx::Quaternion::serialize
void serialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.cpp:258