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 <RobotAPI/interface/core/PoseBase.h>
28 
30 
31 #include <Eigen/Core>
32 #include <Eigen/Geometry>
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 }
42 
43 namespace armarx
44 {
45  const Eigen::IOFormat ArmarXEigenFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", "\n", "", "");
46 
51  class Vector2 : virtual public Vector2Base
52  {
53  public:
54  Vector2();
55  Vector2(const Eigen::Vector2f&);
57 
58  void operator=(const Eigen::Vector2f& ves);
59 
60  virtual Eigen::Vector2f toEigen() const;
61 
62  // inherited from VariantDataClass
63  Ice::ObjectPtr ice_clone() const override
64  {
65  return this->clone();
66  }
67  VariantDataClassPtr clone(const Ice::Current& = Ice::emptyCurrent) const override
68  {
69  return new Vector2(*this);
70  }
71  std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
72  VariantTypeId getType(const Ice::Current& = Ice::emptyCurrent) const override
73  {
74  return VariantType::Vector2;
75  }
76  bool validate(const Ice::Current& = Ice::emptyCurrent) override
77  {
78  return true;
79  }
80 
81  friend std::ostream& operator<<(std::ostream& stream, const Vector2& rhs)
82  {
83  stream << "Vector2: " << std::endl << rhs.output() << std::endl;
84  return stream;
85  }
86 
87  public: // serialization
88  void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
89  void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
90 
91  };
92 
94 
101  class Vector3 :
102  virtual public Vector3Base
103  {
104  public:
105  Vector3();
106  Vector3(const Eigen::Vector3f&);
107  Vector3(const Eigen::Matrix4f&);
109 
110  void operator=(const Eigen::Vector3f& vec);
111 
112  virtual Eigen::Vector3f toEigen() const;
113 
114  // inherited from VariantDataClass
115  Ice::ObjectPtr ice_clone() const override
116  {
117  return this->clone();
118  }
119  VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
120  {
121  return new Vector3(*this);
122  }
123  std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
124  VariantTypeId getType(const Ice::Current& = Ice::emptyCurrent) const override
125  {
126  return VariantType::Vector3;
127  }
128  bool validate(const Ice::Current& = Ice::emptyCurrent) override
129  {
130  return true;
131  }
132 
133 
134  friend std::ostream& operator<<(std::ostream& stream, const Vector3& rhs)
135  {
136  stream << "Vector3: " << std::endl << rhs.output() << std::endl;
137  return stream;
138  }
139 
140  public: // serialization
141  void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
142  void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
143  };
144 
146 
147 
154  class Quaternion :
155  virtual public QuaternionBase
156  {
157  public:
158 
160  Quaternion();
161  Quaternion(const Eigen::Matrix4f&);
162  Quaternion(const Eigen::Matrix3f&);
164  Quaternion(::Ice::Float qw, ::Ice::Float qx, ::Ice::Float qy, ::Ice::Float qz);
165 
166  Eigen::Matrix3f toEigen() const;
168  Eigen::Matrix3f slerp(float, const Eigen::Matrix3f&);
169 
170  static Eigen::Matrix3f slerp(float, const Eigen::Matrix3f&, const Eigen::Matrix3f&);
171 
172  // inherited from VariantDataClass
173  Ice::ObjectPtr ice_clone() const override
174  {
175  return this->clone();
176  }
177  VariantDataClassPtr clone(const Ice::Current& = Ice::emptyCurrent) const override
178  {
179  return new Quaternion(*this);
180  }
181  std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
182  VariantTypeId getType(const Ice::Current& = Ice::emptyCurrent) const override
183  {
185  }
186  bool validate(const Ice::Current& = Ice::emptyCurrent) override
187  {
188  return true;
189  }
190 
191 
192  friend std::ostream& operator<<(std::ostream& stream, const Quaternion& rhs)
193  {
194  stream << "Quaternion: " << std::endl << rhs.output() << std::endl;
195  return stream;
196  }
197 
198  public: // serialization
199  void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
200  void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
201 
202  private:
203  void init(const Eigen::Matrix3f&);
204  void init(const Eigen::Quaternionf&);
205  };
206 
208 
209 
216  class Pose :
217  virtual public PoseBase
218  {
219  public:
220  Pose();
221  Pose(const Pose& source);
222  Pose(const Eigen::Matrix4f&);
223  Pose(const Eigen::Matrix3f&, const Eigen::Vector3f&);
224  Pose(const armarx::Vector3BasePtr pos, const armarx::QuaternionBasePtr ori);
225  Pose& operator=(const Pose&) = default;
226 
227  void operator=(const Eigen::Matrix4f& matrix);
228  virtual Eigen::Matrix4f toEigen() const;
229 
230  // inherited from VariantDataClass
231  Ice::ObjectPtr ice_clone() const override
232  {
233  return this->clone();
234  }
235  VariantDataClassPtr clone(const Ice::Current& = Ice::emptyCurrent) const override
236  {
237  return new Pose(*this);
238  }
239  std::string output(const Ice::Current& = Ice::emptyCurrent) const override;
240  VariantTypeId getType(const Ice::Current& = Ice::emptyCurrent) const override
241  {
242  return VariantType::Pose;
243  }
244  bool validate(const Ice::Current& = Ice::emptyCurrent) override
245  {
246  return true;
247  }
248 
249  friend std::ostream& operator<<(std::ostream& stream, const Pose& rhs)
250  {
251  stream << "Pose: " << std::endl << rhs.output() << std::endl;
252  return stream;
253  }
254 
255  public: // serialization
256  void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
257  void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
258 
259  protected:
260  void init();
261  void ice_postUnmarshal() override;
262  private:
264  QuaternionPtr c_orientation;
265  Vector3Ptr c_position;
266 
267  };
268 
270 
271 
272  // Ice conversion functions
273 
274  void fromIce(const Vector3BasePtr& ice, Eigen::Vector3f& vector);
275  void fromIce(const QuaternionBasePtr& ice, Eigen::Quaternionf& quaternion);
276  void fromIce(const QuaternionBasePtr& ice, Eigen::Matrix3f& rotation);
277  void fromIce(const PoseBasePtr& ice, Eigen::Matrix4f& pose);
278 
279  Eigen::Vector3f fromIce(const Vector3BasePtr& position);
280  Eigen::Quaternionf fromIce(const QuaternionBasePtr& rotation);
281  Eigen::Matrix4f fromIce(const PoseBasePtr& pose);
282 
283  void toIce(Vector3BasePtr& ice, const Eigen::Vector3f& vector);
284  void toIce(QuaternionBasePtr& ice, const Eigen::Matrix3f& rotation);
285  void toIce(QuaternionBasePtr& ice, const Eigen::Quaternionf& quaternion);
286  void toIce(PoseBasePtr& ice, const Eigen::Matrix4f& pose);
287 
288  Vector3Ptr toIce(const Eigen::Vector3f& vector);
289  QuaternionPtr toIce(const Eigen::Matrix3f& rotation);
290  QuaternionPtr toIce(const Eigen::Quaternionf& quaternion);
291  PosePtr toIce(const Eigen::Matrix4f& pose);
292  PosePtr toIce(const Eigen::Isometry3f& pose);
293 
294 }
295 
296 extern template class ::IceInternal::Handle< ::armarx::Pose>;
297 extern template class ::IceInternal::Handle< ::armarx::Vector2>;
298 extern template class ::IceInternal::Handle< ::armarx::Vector3>;
299 extern template class ::IceInternal::Handle< ::armarx::Quaternion>;
armarx::Vector2::clone
VariantDataClassPtr clone(const Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.h:67
armarx::Vector3::clone
VariantDataClassPtr clone(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: Pose.h:119
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:235
armarx::Vector3::getType
VariantTypeId getType(const Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.h:124
armarx::Pose::Pose
Pose()
Definition: Pose.cpp:276
armarx::Quaternion::toEigenQuaternion
Eigen::Quaternionf toEigenQuaternion() const
Definition: Pose.cpp:200
armarx::Vector2::output
std::string output(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: Pose.cpp:77
armarx::Pose::getType
VariantTypeId getType(const Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.h:240
armarx::Quaternion::output
std::string output(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: Pose.cpp:235
armarx::Vector3::operator=
void operator=(const Eigen::Vector3f &vec)
Definition: Pose.cpp:135
armarx::Quaternion::operator<<
friend std::ostream & operator<<(std::ostream &stream, const Quaternion &rhs)
Definition: Pose.h:192
armarx::Vector3::Vector3
Vector3()
Definition: Pose.cpp:100
armarx::Vector2::ice_clone
Ice::ObjectPtr ice_clone() const override
Definition: Pose.h:63
armarx::Vector2::serialize
void serialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.cpp:84
armarx::VariantType::Vector3
const VariantTypeId Vector3
Definition: Pose.h:38
armarx::Vector2::operator=
void operator=(const Eigen::Vector2f &ves)
Definition: Pose.cpp:71
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::Pose::init
void init()
Definition: Pose.cpp:340
armarx::Quaternion::deserialize
void deserialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: Pose.cpp:252
armarx::VariantType::Vector2
const VariantTypeId Vector2
Definition: Pose.h:37
armarx::Pose::validate
bool validate(const Ice::Current &=Ice::emptyCurrent) override
Definition: Pose.h:244
armarx::Vector2
The Vector2 class.
Definition: Pose.h:51
armarx::Vector3::validate
bool validate(const Ice::Current &=Ice::emptyCurrent) override
Definition: Pose.h:128
armarx::Vector3::deserialize
void deserialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: Pose.cpp:158
armarx::Vector2::operator<<
friend std::ostream & operator<<(std::ostream &stream, const Vector2 &rhs)
Definition: Pose.h:81
armarx::Quaternion::getType
VariantTypeId getType(const Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.h:182
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:220
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:72
armarx::Pose::operator<<
friend std::ostream & operator<<(std::ostream &stream, const Pose &rhs)
Definition: Pose.h:249
armarx::Vector3
The Vector3 class.
Definition: Pose.h:101
armarx::Pose::toEigen
virtual Eigen::Matrix4f toEigen() const
Definition: Pose.cpp:308
armarx::Pose::serialize
void serialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.cpp:326
armarx::VariantType
Definition: ChannelRef.h:160
armarx::Quaternion::validate
bool validate(const Ice::Current &=Ice::emptyCurrent) override
Definition: Pose.h:186
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:332
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:115
armarx::Quaternion::toEigen
Eigen::Matrix3f toEigen() const
Definition: Pose.cpp:195
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:142
armarx::Pose
The Pose class.
Definition: Pose.h:216
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:76
armarx::Vector2::deserialize
void deserialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: Pose.cpp:92
armarx::Vector3::toEigen
virtual Eigen::Vector3f toEigen() const
Definition: Pose.cpp:128
armarx::Vector3::operator<<
friend std::ostream & operator<<(std::ostream &stream, const Vector3 &rhs)
Definition: Pose.h:134
armarx::Quaternion
The Quaternion class.
Definition: Pose.h:154
armarx::Quaternion::clone
VariantDataClassPtr clone(const Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.h:177
armarx::Vector2::toEigen
virtual Eigen::Vector2f toEigen() const
Definition: Pose.cpp:64
armarx::Pose::ice_postUnmarshal
void ice_postUnmarshal() override
Definition: Pose.cpp:346
armarx::Quaternion::ice_clone
Ice::ObjectPtr ice_clone() const override
Definition: Pose.h:173
armarx::Vector3::serialize
void serialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.cpp:149
armarx::aron::type::ObjectPtr
std::shared_ptr< Object > ObjectPtr
Definition: Object.h:37
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:318
armarx::Pose::ice_clone
Ice::ObjectPtr ice_clone() const override
Definition: Pose.h:231
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:242