LinkedPose.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-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
19  * @author
20  * @date
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 #include "LinkedPose.h"
25 
27 
30 
31 #include <Eigen/Geometry>
32 #include <Eigen/Core>
33 
34 #include <VirtualRobot/LinkedCoordinate.h>
35 #include <VirtualRobot/VirtualRobot.h>
36 
37 #include <Ice/ObjectAdapter.h>
38 
39 
40 namespace armarx
41 {
42 
44  Pose(),
45  FramedPose()
46  {
47  this->referenceRobot = nullptr;
48  }
49 
51  IceUtil::Shared(other),
52  armarx::Serializable(other),
53  armarx::VariantDataClass(other),
54  PoseBase(other),
55  FramedPoseBase(other),
56  LinkedPoseBase(other),
57  Pose(other),
58  FramedPose(other)
59  {
60  if (referenceRobot)
61  {
62  //ARMARX_WARNING_S << "Calling referenceRobot->ref() in cctor of LinkedPose";
63  referenceRobot->ref();
64  }
65  }
66 
67  LinkedPose::LinkedPose(const FramedPose& other, const SharedRobotInterfacePrx& referenceRobot) :
68  IceUtil::Shared(other),
69  PoseBase(other),
70  FramedPoseBase(other),
71  Pose(other),
72  FramedPose(other)
73  {
74  ARMARX_CHECK_EXPRESSION(referenceRobot) << "The robot proxy must not be zero";
75  this->referenceRobot = referenceRobot;
76 
77  if (referenceRobot)
78  {
79  //ARMARX_WARNING_S << "Calling referenceRobot->ref() in cctor of LinkedPose";
80  referenceRobot->ref();
81  }
82 
83  }
84 
85  LinkedPose::LinkedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& s, const SharedRobotInterfacePrx& referenceRobot) :
86  Pose(m, v),
87  FramedPose(m, v, s, referenceRobot->getName())
88  {
89  ARMARX_CHECK_EXPRESSION(referenceRobot) << "The robot proxy must not be zero";
90  referenceRobot->ref();
91  this->referenceRobot = referenceRobot;
92  }
93 
94  LinkedPose::LinkedPose(const Eigen::Matrix4f& m, const std::string& s, const SharedRobotInterfacePrx& referenceRobot) :
95  Pose(m),
96  FramedPose(m, s, referenceRobot->getName())
97  {
98  ARMARX_CHECK_EXPRESSION(referenceRobot) << "The robot proxy must not be zero";
99  referenceRobot->ref();
100  this->referenceRobot = referenceRobot;
101  }
102 
104  {
105  try
106  {
107  if (referenceRobot)
108  {
109  referenceRobot->unref();
110  }
111  }
112  catch (...)
113  {
115  }
116  }
117 
118 
119  VirtualRobot::LinkedCoordinate LinkedPose::createLinkedCoordinate()
120  {
121  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
122  VirtualRobot::LinkedCoordinate c(sharedRobot);
123  std::string frame = this->getFrame();
124 
126 
127  pose.block<3, 3>(0, 0) = QuaternionPtr::dynamicCast(orientation)->toEigen();
128  pose.block<3, 1>(0, 3) = Vector3Ptr::dynamicCast(position)->toEigen();
129 
130  c.set(frame, pose);
131 
132  return c;
133  }
134 
136  {
137  return this->clone();
138  }
139 
140  VariantDataClassPtr LinkedPose::clone(const Ice::Current& c) const
141  {
142  return new LinkedPose(*this);
143  }
144 
145  std::string LinkedPose::output(const Ice::Current& c) const
146  {
147  std::stringstream s;
148  s << FramedPose::output() << std::endl << "reference robot: " << referenceRobot->ice_toString();
149  return s.str();
150  }
151 
152  VariantTypeId LinkedPose::getType(const Ice::Current& c) const
153  {
155  }
156 
157  bool LinkedPose::validate(const Ice::Current& c)
158  {
159  return true;
160  }
161 
162  void LinkedPose::changeFrame(const std::string& newFrame, const Ice::Current& c)
163  {
164  FramedPose::changeFrame(referenceRobot, newFrame);
165  }
166 
168  {
169  FramedPose::changeToGlobal(referenceRobot);
170  }
171 
173  {
174  FramedPosePtr fp = this->FramedPose::toGlobal(referenceRobot);
175  LinkedPosePtr newPose = new LinkedPose(fp->toEigen(), fp->frame, referenceRobot);
176  return newPose;
177  }
178 
179 
180  void LinkedPose::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
181  {
182  AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
183 
184  Pose::serialize(obj, c);
185  obj->setString("referenceRobot", "");
186  }
187 
188  void LinkedPose::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
189  {
190  AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
191 
193 
194  std::string remoteRobotId = obj->getString("referenceRobot");
195  referenceRobot = SharedRobotInterfacePrx::uncheckedCast(c.adapter->getCommunicator()->stringToProxy(remoteRobotId));
196 
197  if (!referenceRobot)
198  {
199  ARMARX_ERROR_S << "ReferenceRobot for LinkedPose not registered: " << remoteRobotId << flush;
200  }
201  }
202 
204  {
205  if (referenceRobot)
206  {
207  //ARMARX_WARNING_S << "Calling referenceRobot->ref() in __read(IceInternal::BasicStream *__is, bool __rid) of LinkedPose";
208  referenceRobot->ref();
209  }
210 
212  }
213 
214 
216  = default;
217 
219  IceUtil::Shared(source),
220  armarx::Serializable(source),
221  Vector3Base(source),
222  FramedDirectionBase(source),
223  LinkedDirectionBase(source),
224  Vector3(source),
226  {
227  referenceRobot = source.referenceRobot;
228 
229  if (referenceRobot)
230  {
231  //ARMARX_WARNING_S << "Calling referenceRobot->ref() in cctor of LinkedPose";
232  referenceRobot->ref();
233  }
234  }
235 
236  LinkedDirection::LinkedDirection(const Eigen::Vector3f& v, const std::string& frame, const SharedRobotInterfacePrx& referenceRobot) :
237  FramedDirection(v, frame, referenceRobot->getName())
238  {
239  referenceRobot->ref();
240  this->referenceRobot = referenceRobot;
241  }
242 
244  {
245  try
246  {
247  if (referenceRobot)
248  {
249  referenceRobot->unref();
250  }
251  }
252  catch (...)
253  {
255  }
256  }
257 
258  void LinkedDirection::changeFrame(const std::string& newFrame, const Ice::Current& c)
259  {
260  if (newFrame == frame)
261  {
262  return;
263  }
264 
265  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
266 
267  FramedDirectionPtr frVec = ChangeFrame(sharedRobot, *this, newFrame);
268  x = frVec->x;
269  y = frVec->y;
270  z = frVec->z;
271  frame = frVec->frame;
272  }
273 
274 
275  void LinkedDirection::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
276  {
277  throw LocalException("LinkedDirection cannot be serialized! Serialize FramedDirection");
278  }
279 
280  void LinkedDirection::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
281  {
282  throw LocalException("LinkedDirection cannot be deserialized! Deserialize FramedDirection");
283  }
284 
286  {
287  if (referenceRobot)
288  {
289  // ARMARX_WARNING_S << "Calling referenceRobot->ref() in __read(IceInternal::BasicStream *__is, bool __rid) of LinkedPose";
290  referenceRobot->ref();
291  }
292 
293  FramedDirection::ice_postUnmarshal();
294  }
295 
296 
298  {
301  }
302 
303 }
armarx::LinkedPose::clone
VariantDataClassPtr clone(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: LinkedPose.cpp:140
RemoteRobot.h
armarx::LinkedDirection::LinkedDirection
LinkedDirection()
LinkedPose.h
armarx::LinkedDirection::ice_postUnmarshal
void ice_postUnmarshal() override
Definition: LinkedPose.cpp:285
armarx::VariantType::suppressWarningUnusedVariableForLinkedPoseAndDirection
void suppressWarningUnusedVariableForLinkedPoseAndDirection()
Definition: LinkedPose.cpp:297
armarx::FramedPose
The FramedPose class.
Definition: FramedPose.h:258
armarx::LinkedDirection
The LinkedDirection class.
Definition: LinkedPose.h:113
armarx::LinkedDirection::serialize
void serialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: LinkedPose.cpp:275
armarx::LinkedPose::changeFrame
void changeFrame(const std::string &newFrame, const Ice::Current &c=Ice::emptyCurrent) override
Definition: LinkedPose.cpp:162
armarx::LinkedPose::createLinkedCoordinate
VirtualRobot::LinkedCoordinate createLinkedCoordinate()
Definition: LinkedPose.cpp:119
armarx::LinkedPose::serialize
void serialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: LinkedPose.cpp:180
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::LinkedDirection::changeFrame
void changeFrame(const std::string &newFrame, const Ice::Current &c=Ice::emptyCurrent) override
Definition: LinkedPose.cpp:258
IceUtil
Definition: Instance.h:21
armarx::FramedPose::toGlobal
FramedPosePtr toGlobal(const SharedRobotInterfacePrx &referenceRobot) const
Definition: FramedPose.cpp:500
armarx::LinkedPose::LinkedPose
LinkedPose()
Definition: LinkedPose.cpp:43
IceInternal::Handle
Definition: forward_declarations.h:8
armarx::FramedDirection::ChangeFrame
static FramedDirectionPtr ChangeFrame(const VirtualRobot::RobotPtr &robot, const FramedDirection &framedVec, const std::string &newFrame)
Definition: FramedPose.cpp:86
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::FramedPose::output
std::string output(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: FramedPose.cpp:398
armarx::VariantType::LinkedPose
const VariantTypeId LinkedPose
Definition: LinkedPose.h:43
ARMARX_DEBUG_S
#define ARMARX_DEBUG_S
Definition: Logging.h:198
armarx::LinkedPose::~LinkedPose
~LinkedPose() override
Definition: LinkedPose.cpp:103
ARMARX_ERROR_S
#define ARMARX_ERROR_S
Definition: Logging.h:209
armarx::flush
const LogSender::manipulator flush
Definition: LogSender.h:251
armarx::LinkedDirection::deserialize
void deserialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: LinkedPose.cpp:280
armarx::Vector3
The Vector3 class.
Definition: Pose.h:112
armarx::Pose::serialize
void serialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.cpp:353
armarx::LinkedPose::ice_clone
Ice::ObjectPtr ice_clone() const override
Definition: LinkedPose.cpp:135
armarx::LinkedPose::changeToGlobal
void changeToGlobal()
Definition: LinkedPose.cpp:167
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
ExpressionException.h
armarx::Pose
The Pose class.
Definition: Pose.h:242
armarx::RemoteRobot
Mimics the behaviour of the VirtualRobot::Robot class while redirecting everything to an Ice proxy.
Definition: RemoteRobot.h:139
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
armarx::FramedDirection
FramedDirection is a 3 dimensional direction vector with a reference frame. The reference frame can b...
Definition: FramedPose.h:83
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
armarx::LinkedPose::getType
VariantTypeId getType(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: LinkedPose.cpp:152
IceInternal::ProxyHandle< ::IceProxy::armarx::SharedRobotInterface >
armarx::Pose::ice_postUnmarshal
void ice_postUnmarshal() override
Definition: Pose.cpp:376
armarx::LinkedPose::deserialize
void deserialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: LinkedPose.cpp:188
armarx::LinkedPose::ice_postUnmarshal
void ice_postUnmarshal() override
Definition: LinkedPose.cpp:203
armarx::LinkedPose::output
std::string output(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: LinkedPose.cpp:145
armarx::LinkedPose::toGlobal
LinkedPosePtr toGlobal() const
Definition: LinkedPose.cpp:172
armarx::LinkedPose
The LinkedPose class.
Definition: LinkedPose.h:60
armarx::LinkedPose::validate
bool validate(const Ice::Current &c=Ice::emptyCurrent) override
Definition: LinkedPose.cpp:157
armarx::aron::type::ObjectPtr
std::shared_ptr< Object > ObjectPtr
Definition: Object.h:36
armarx::FramedPose::getFrame
std::string getFrame() const
Definition: FramedPose.cpp:393
Logging.h
armarx::FramedPose::changeToGlobal
void changeToGlobal(const SharedRobotInterfacePrx &referenceRobot)
Definition: FramedPose.cpp:467
armarx::handleExceptions
void handleExceptions()
Definition: Exception.cpp:141
armarx::LinkedDirection::~LinkedDirection
~LinkedDirection() override
Definition: LinkedPose.cpp:243
armarx::FramedPose::deserialize
void deserialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: FramedPose.cpp:590
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::FramedPose::changeFrame
void changeFrame(const SharedRobotInterfacePrx &referenceRobot, const std::string &newFrame)
Definition: FramedPose.cpp:406
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18