FramedPose.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 
25 
28 
32 
33 #include <VirtualRobot/Robot.h>
34 #include <VirtualRobot/LinkedCoordinate.h>
35 #include <VirtualRobot/VirtualRobot.h>
36 #include <VirtualRobot/RobotConfig.h>
37 
38 #include <Eigen/Core>
39 #include <Eigen/Geometry>
40 
41 
42 
43 using namespace Eigen;
44 
45 template class ::IceInternal::Handle<::armarx::FramedPose>;
46 template class ::IceInternal::Handle<::armarx::FramedPosition>;
47 template class ::IceInternal::Handle<::armarx::FramedDirection>;
48 template class ::IceInternal::Handle<::armarx::FramedOrientation>;
49 
50 namespace armarx
51 {
52 
53 
55  = default;
56 
58  IceUtil::Shared(source),
59  armarx::Serializable(source),
60  Vector3Base(source),
61  FramedDirectionBase(source),
63  {
64  }
65 
66  FramedDirection::FramedDirection(const Eigen::Vector3f& vec, const std::string& frame, const std::string& agent) :
67  Vector3(vec)
68  {
69  this->frame = frame;
70  this->agent = agent;
71 
72  }
73 
74  FramedDirection::FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string& frame, const std::string& agent) :
75  Vector3(x, y, z)
76  {
77  this->frame = frame;
78  this->agent = agent;
79  }
80 
81  std::string FramedDirection::getFrame() const
82  {
83  return frame;
84  }
85 
86  FramedDirectionPtr FramedDirection::ChangeFrame(const VirtualRobot::RobotPtr& robot, const FramedDirection& framedVec, const std::string& newFrame)
87  {
88  ARMARX_CHECK_NOT_NULL(robot);
89  return ChangeFrame(*robot, framedVec, newFrame);
90  }
91 
92  FramedDirectionPtr FramedDirection::ChangeFrame(const VirtualRobot::Robot& robot, const FramedDirection& framedVec, const std::string& newFrame)
93  {
94  if (framedVec.getFrame() == newFrame)
95  {
96  return FramedDirectionPtr::dynamicCast(framedVec.clone());
97  }
98 
99  if (!robot.hasRobotNode(newFrame))
100  {
101  throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << robot.getName();
102  }
103 
104  Eigen::Matrix4f rotToNewFrame = __GetRotationBetweenFrames(framedVec.frame, newFrame, robot);
105 
106  Eigen::Vector3f vecOldFrame = framedVec.Vector3::toEigen();
107 
108  Eigen::Vector3f newVec = rotToNewFrame.block(0, 0, 3, 3).inverse() * vecOldFrame;
109 
110  FramedDirectionPtr result = new FramedDirection();
111  result->x = newVec(0);
112  result->y = newVec(1);
113  result->z = newVec(2);
114  result->frame = newFrame;
115  result->agent = robot.getName();
116  return result;
117  }
118 
119  void FramedDirection::changeFrame(const VirtualRobot::RobotPtr& robot, const std::string& newFrame)
120  {
121  ARMARX_CHECK_NOT_NULL(robot);
122  changeFrame(*robot, newFrame);
123  }
124 
125  void FramedDirection::changeFrame(const VirtualRobot::Robot& robot, const std::string& newFrame)
126  {
127  if (frame == "")
128  {
129  frame = GlobalFrame;
130  }
131 
132 
133  if (getFrame() == newFrame)
134  {
135  return;
136  }
137 
138  if (newFrame == GlobalFrame)
139  {
140  changeToGlobal(robot);
141  return;
142  }
143 
144  if (!robot.hasRobotNode(newFrame))
145  {
146  throw LocalException() << "The requested frame '" << newFrame << "' does not exist in the robot " << robot.getName();
147  }
148 
149  if (frame != GlobalFrame && !robot.hasRobotNode(frame))
150  {
151  throw LocalException() << "The current frame '" << frame << "' does not exist in the robot " << robot.getName();
152  }
153 
154 
155  Eigen::Matrix4f rotToNewFrame = __GetRotationBetweenFrames(frame, newFrame, robot);
156 
157  Eigen::Vector3f vecOldFrame = Vector3::toEigen();
158 
159  Eigen::Vector3f newVec = rotToNewFrame.block(0, 0, 3, 3).inverse() * vecOldFrame;
160 
161 
162  x = newVec(0);
163  y = newVec(1);
164  z = newVec(2);
165  frame = newFrame;
166  agent = robot.getName();
167  }
168 
170  {
171  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
172  changeToGlobal(sharedRobot);
173  }
174 
175  void FramedDirection::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
176  {
177  ARMARX_CHECK_NOT_NULL(referenceRobot);
178  changeToGlobal(*referenceRobot);
179  }
180 
182  {
183  if (frame == GlobalFrame)
184  {
185  return;
186  }
187 
188  changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
189  Eigen::Vector3f pos = referenceRobot.getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen();
190  x = pos[0];
191  y = pos[1];
192  z = pos[2];
193  frame = GlobalFrame;
194  agent = "";
195  }
196 
198  {
199  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
200  return toGlobal(sharedRobot);
201  }
202  FramedDirectionPtr FramedDirection::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
203  {
204  ARMARX_CHECK_NOT_NULL(referenceRobot);
205  return toGlobal(*referenceRobot);
206  }
208  {
209  FramedDirectionPtr newDirection = FramedDirectionPtr::dynamicCast(this->clone());
210  newDirection->changeToGlobal(referenceRobot);
211  return newDirection;
212  }
213 
214  Eigen::Vector3f FramedDirection::toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
215  {
216  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
217  return toGlobalEigen(sharedRobot);
218  }
219  Eigen::Vector3f FramedDirection::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
220  {
221  ARMARX_CHECK_NOT_NULL(referenceRobot);
222  return toGlobalEigen(*referenceRobot);
223  }
224  Eigen::Vector3f FramedDirection::toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const
225  {
226  FramedDirection newDirection(toEigen(), frame, agent);
227  newDirection.changeToGlobal(referenceRobot);
228  return newDirection.toEigen();
229  }
230 
232  {
233  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
234  return toRootFrame(sharedRobot);
235  }
236  FramedDirectionPtr FramedDirection::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
237  {
238  ARMARX_CHECK_NOT_NULL(referenceRobot);
239  return toRootFrame(*referenceRobot);
240  }
242  {
243  FramedDirectionPtr newDirection = FramedDirectionPtr::dynamicCast(this->clone());
244  newDirection->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
245  return newDirection;
246  }
247 
248  Eigen::Vector3f FramedDirection::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
249  {
250  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
251  return toRootEigen(sharedRobot);
252  }
253  Eigen::Vector3f FramedDirection::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
254  {
255  ARMARX_CHECK_NOT_NULL(referenceRobot);
256  return toRootEigen(*referenceRobot);
257  }
258  Eigen::Vector3f FramedDirection::toRootEigen(const VirtualRobot::Robot& referenceRobot) const
259  {
260  FramedDirection newDirection(toEigen(), frame, agent);
261  newDirection.changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
262  return newDirection.toEigen();
263  }
264 
265  std::string FramedDirection::output(const Ice::Current& c) const
266  {
267  std::stringstream s;
268  s << toEigen().format(ArmarXEigenFormat) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
269  return s.str();
270  }
271 
272  Eigen::Matrix4f FramedDirection::__GetRotationBetweenFrames(const std::string& oldFrame, const std::string& newFrame, const VirtualRobot::Robot& robotState)
273  {
274  Eigen::Matrix4f toNewFrame;
275 
276  if (oldFrame == GlobalFrame)
277  {
278  auto node = robotState.getRobotNode(newFrame);
279  ARMARX_CHECK_EXPRESSION(node) << newFrame;
280  toNewFrame = node->getGlobalPose();
281  }
282  else
283  {
284  auto node = robotState.getRobotNode(oldFrame);
285  ARMARX_CHECK_EXPRESSION(node) << "old frame: " + oldFrame + ValueToString(robotState.getConfig()->getRobotNodeJointValueMap());
286  auto newNode = robotState.getRobotNode(newFrame);
287  ARMARX_CHECK_EXPRESSION(newNode) << newFrame;
288  toNewFrame = node->getTransformationTo(newNode);
289  }
290 
291  toNewFrame(0, 3) = 0;
292  toNewFrame(1, 3) = 0;
293  toNewFrame(2, 3) = 0;
294 
295  return toNewFrame;
296  }
297 
298 
299  void FramedDirection::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
300  {
301  AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
302  Vector3::serialize(serializer);
303  obj->setString("frame", frame);
304  obj->setString("agent", agent);
305 
306  }
307 
308  void FramedDirection::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
309  {
310  AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
311  Vector3::deserialize(serializer);
312  frame = obj->getString("frame");
313 
314  if (obj->hasElement("agent"))
315  {
316  agent = obj->getString("agent");
317  }
318  }
319 
321  {
322  return this->clone();
323  }
324 
325  VariantDataClassPtr FramedDirection::clone(const Ice::Current& c) const
326  {
327  return new FramedDirection(*this);
328  }
329 
330  VariantTypeId FramedDirection::getType(const Ice::Current& c) const
331  {
333  }
334 
335  bool FramedDirection::validate(const Ice::Current& c)
336  {
337  return true;
338  }
339 
340  std::ostream& operator<<(std::ostream& stream, const FramedDirection& rhs)
341  {
342  stream << "FramedDirection: " << std::endl << rhs.output() << std::endl;
343  return stream;
344  }
345 
346 
348  Pose()
349  {
350  frame = "";
351  }
352 
354  IceUtil::Shared(pose),
355  armarx::Serializable(pose),
356  armarx::VariantDataClass(pose),
357  PoseBase(pose),
358  FramedPoseBase(pose),
359  Pose(pose)
360  {
361 
362  }
363 
364  FramedPose::FramedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& s, const std::string& agent) :
365  Pose(m, v)
366  {
367  frame = s;
368  this->agent = agent;
369  }
370 
371  FramedPose::FramedPose(const Eigen::Matrix4f& m, const std::string& s, const std::string& agent) :
372  Pose(m)
373  {
374  frame = s;
375  this->agent = agent;
376  }
377 
378  FramedPose::FramedPose(const armarx::Vector3BasePtr pos, const armarx::QuaternionBasePtr ori, const std::string& frame, const std::string& agent) :
379  Pose(pos, ori)
380  {
381  this->frame = frame;
382  this->agent = agent;
383  }
384 
385  std::string FramedPose::getFrame() const
386  {
387  return frame;
388  }
389 
390  std::string FramedPose::output(const Ice::Current& c) const
391  {
392  std::stringstream s;
393  const Eigen::IOFormat ArmarXEigenFormat(Eigen::StreamPrecision, 4, " ", "\n", "", "");
394  s << toEigen().format(ArmarXEigenFormat) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
395  return s.str();
396  }
397 
398  void FramedPose::changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame)
399  {
400  if (newFrame == frame)
401  {
402  return;
403  }
404 
405  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
406  changeFrame(sharedRobot, newFrame);
407  }
408 
409  void FramedPose::changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame)
410  {
411  ARMARX_CHECK_NOT_NULL(referenceRobot);
412  changeFrame(*referenceRobot, newFrame);
413  }
414 
415  void FramedPose::changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame)
416  {
417  if (newFrame == frame)
418  {
419  return;
420  }
421 
422  if (newFrame == GlobalFrame)
423  {
424  changeToGlobal(referenceRobot);
425  return;
426  }
427 
428  Eigen::Matrix4f oldFrameTransformation = Eigen::Matrix4f::Identity();
429 
430  if (frame != GlobalFrame and !referenceRobot.hasRobotNode(frame))
431  {
432  throw LocalException() << "The current frame '" << frame << "' does not exists in the robot " << referenceRobot.getName();
433  }
434  if (!referenceRobot.hasRobotNode(newFrame))
435  {
436  throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot.getName();
437  }
438 
439  if (frame != GlobalFrame)
440  {
441  oldFrameTransformation = referenceRobot.getRobotNode(frame)->getPoseInRootFrame();
442  }
443  else
444  {
445  oldFrameTransformation = referenceRobot.getRootNode()->getGlobalPose().inverse();
446  }
447 
448  Eigen::Matrix4f newPose =
449  (referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * toEigen();
450 
451  orientation = new Quaternion(newPose);
452  Eigen::Vector3f pos = newPose.block<3, 1>(0, 3);
453  position = new Vector3(pos);
454  frame = newFrame;
455  agent = referenceRobot.getName();
456  init();
457  }
458 
460  {
461  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
462  changeToGlobal(sharedRobot);
463 
464  }
465  void FramedPose::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
466  {
467  ARMARX_CHECK_NOT_NULL(referenceRobot);
468  changeToGlobal(*referenceRobot);
469  }
471  {
472  if (frame == GlobalFrame)
473  {
474  return;
475  }
476 
477  changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
478  Eigen::Matrix4f global = referenceRobot.getRootNode()->getGlobalPose();
479  const Eigen::Matrix4f pose = global * toEigen();
480  position->x = pose(0, 3);
481  position->y = pose(1, 3);
482  position->z = pose(2, 3);
483  Eigen::Quaternionf q(pose.block<3, 3>(0, 0));
484  orientation->qw = q.w();
485  orientation->qx = q.x();
486  orientation->qy = q.y();
487  orientation->qz = q.z();
488  frame = GlobalFrame;
489  agent = "";
490  }
491 
493  {
494  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
495  return toGlobal(sharedRobot);
496  }
497  FramedPosePtr FramedPose::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
498  {
499  ARMARX_CHECK_NOT_NULL(referenceRobot);
500  return toGlobal(*referenceRobot);
501  }
503  {
504  FramedPosePtr newPose = FramedPosePtr::dynamicCast(this->clone());
505  newPose->changeToGlobal(referenceRobot);
506  return newPose;
507  }
508 
510  {
511  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
512  return toGlobalEigen(sharedRobot);
513  }
514  Eigen::Matrix4f FramedPose::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
515  {
516  ARMARX_CHECK_NOT_NULL(referenceRobot);
517  return toGlobalEigen(*referenceRobot);
518  }
520  {
521  FramedPose newPose(toEigen(), frame, agent);
522  newPose.changeToGlobal(referenceRobot);
523  return newPose.toEigen();
524  }
525 
527  {
528  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
529  return toRootFrame(sharedRobot);
530  }
531  FramedPosePtr FramedPose::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
532  {
533  ARMARX_CHECK_NOT_NULL(referenceRobot);
534  return toRootFrame(*referenceRobot);
535  }
537  {
538  FramedPosePtr newPose = FramedPosePtr::dynamicCast(this->clone());
539  newPose->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
540  return newPose;
541  }
542 
544  {
545  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
546  return toRootEigen(sharedRobot);
547  }
548  Eigen::Matrix4f FramedPose::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
549  {
550  ARMARX_CHECK_NOT_NULL(referenceRobot);
551  return toRootEigen(*referenceRobot);
552  }
554  {
555  FramedPose newPose(toEigen(), frame, agent);
556  newPose.changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
557  return newPose.toEigen();
558  }
559 
560 
562  {
563  FramedPositionPtr result = new FramedPosition(Vector3Ptr::dynamicCast(position)->toEigen(), frame, agent);
564  return result;
565  }
566 
568  {
569  FramedOrientationPtr result = new FramedOrientation(QuaternionPtr::dynamicCast(this->orientation)->toEigen(), frame, agent);
570  return result;
571  }
572 
573  void FramedPose::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
574  {
575  AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
576 
577  Pose::serialize(obj, c);
578  obj->setString("frame", frame);
579  obj->setString("agent", agent);
580  }
581 
582  void FramedPose::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
583  {
584  AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
585 
586  Pose::deserialize(obj);
587  frame = obj->getString("frame");
588 
589  if (obj->hasElement("agent"))
590  {
591  agent = obj->getString("agent");
592  }
593  }
594 
596  Vector3()
597  {
598  frame = "";
599  }
600 
601  FramedPosition::FramedPosition(const Eigen::Vector3f& v, const std::string& s, const std::string& agent) :
602  Vector3(v)
603  {
604  frame = s;
605  this->agent = agent;
606  }
607 
608  FramedPosition::FramedPosition(const Eigen::Matrix4f& m, const std::string& s, const std::string& agent) :
609  Vector3(m)
610  {
611  frame = s;
612  this->agent = agent;
613  }
614 
615  // this doesnt work for unknown reasons
616  // FramedPosition::FramedPosition(const Vector3BasePtr pos, const std::string &frame )
617  // {
618  // this->x = pos->x;
619  // this->y = pos->y;
620  // this->z = pos->z;
621  // this->frame = frame;
622  // }
623 
624  std::string FramedPosition::getFrame() const
625  {
626  return this->frame;
627  }
628 
629  void FramedPosition::changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame)
630  {
631  if (newFrame == frame)
632  {
633  return;
634  }
635 
636  if (!referenceRobot->hasRobotNode(newFrame))
637  {
638  throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot->getName();
639  }
640 
641  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
642  changeFrame(sharedRobot, newFrame);
643  }
644 
645  void FramedPosition::changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame)
646  {
647  ARMARX_CHECK_NOT_NULL(referenceRobot);
648  changeFrame(*referenceRobot, newFrame);
649  }
650 
651  void FramedPosition::changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame)
652  {
653  if (newFrame == frame)
654  {
655  return;
656  }
657 
658  if (newFrame == GlobalFrame)
659  {
660  changeToGlobal(referenceRobot);
661  return;
662  }
663 
664  if (newFrame.empty())
665  {
666  ARMARX_WARNING_S << "Frame Conversion: Frame is empty, always set frame names! ...assuming GlobalFrame";
667  changeToGlobal(referenceRobot);
668  return;
669  }
670 
671  Eigen::Matrix4f oldFrameTransformation = Eigen::Matrix4f::Identity();
672 
673  if (!referenceRobot.hasRobotNode(newFrame))
674  {
675  throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot.getName();
676  }
677 
678  if (frame != GlobalFrame)
679  {
680  oldFrameTransformation = referenceRobot.getRobotNode(frame)->getPoseInRootFrame();
681  }
682  else
683  {
684  oldFrameTransformation = referenceRobot.getRootNode()->getGlobalPose().inverse();
685  }
686 
688  oldPose.block<3, 1>(0, 3) = toEigen();
689  Eigen::Matrix4f newPose =
690  (referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * oldPose;
691 
692  Eigen::Vector3f pos = newPose.block<3, 1>(0, 3);
693  x = pos[0];
694  y = pos[1];
695  z = pos[2];
696  agent = referenceRobot.getName();
697  frame = newFrame;
698  }
699 
701  {
702  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
703  changeToGlobal(sharedRobot);
704  }
705  void FramedPosition::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
706  {
707  ARMARX_CHECK_NOT_NULL(referenceRobot);
708  changeToGlobal(*referenceRobot);
709  }
711  {
712  if (frame == GlobalFrame)
713  {
714  return;
715  }
716 
717  changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
718  Eigen::Vector3f pos = referenceRobot.getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen()
719  + referenceRobot.getRootNode()->getGlobalPose().block<3, 1>(0, 3);
720  x = pos[0];
721  y = pos[1];
722  z = pos[2];
723  frame = GlobalFrame;
724  agent = "";
725  }
726 
728  {
729  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
730  return toGlobal(sharedRobot);
731  }
732  FramedPositionPtr FramedPosition::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
733  {
734  ARMARX_CHECK_NOT_NULL(referenceRobot);
735  return toGlobal(*referenceRobot);
736  }
738  {
739  FramedPositionPtr newPosition = FramedPositionPtr::dynamicCast(this->clone());
740  newPosition->changeToGlobal(referenceRobot);
741  return newPosition;
742  }
743 
744  Eigen::Vector3f FramedPosition::toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
745  {
746  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
747  return toGlobalEigen(sharedRobot);
748  }
749  Eigen::Vector3f FramedPosition::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
750  {
751  ARMARX_CHECK_NOT_NULL(referenceRobot);
752  return toGlobalEigen(*referenceRobot);
753  }
754  Eigen::Vector3f FramedPosition::toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const
755  {
756  FramedPosition newPosition(toEigen(), frame, agent);
757  newPosition.changeToGlobal(referenceRobot);
758  return newPosition.toEigen();
759  }
760 
762  {
763  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
764  return toRootFrame(sharedRobot);
765  }
766  FramedPositionPtr FramedPosition::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
767  {
768  ARMARX_CHECK_NOT_NULL(referenceRobot);
769  return toRootFrame(*referenceRobot);
770  }
772  {
773  FramedPositionPtr newPosition = FramedPositionPtr::dynamicCast(this->clone());
774  newPosition->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
775  return newPosition;
776  }
777 
778  Eigen::Vector3f FramedPosition::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
779  {
780  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
781  return toRootEigen(sharedRobot);
782  }
783  Eigen::Vector3f FramedPosition::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
784  {
785  ARMARX_CHECK_NOT_NULL(referenceRobot);
786  return toRootEigen(*referenceRobot);
787  }
788  Eigen::Vector3f FramedPosition::toRootEigen(const VirtualRobot::Robot& referenceRobot) const
789  {
790  FramedPosition newPosition(toEigen(), frame, agent);
791  newPosition.changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
792  return newPosition.toEigen();
793  }
794 
795  std::string FramedPosition::output(const Ice::Current& c) const
796  {
797  std::stringstream s;
798  s << toEigen().format(ArmarXEigenFormat) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
799  return s.str();
800  }
801 
802  void FramedPosition::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
803  {
804  AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
805 
806  Vector3::serialize(obj, c);
807  obj->setString("frame", frame);
808  obj->setString("agent", agent);
809  }
810 
811  void FramedPosition::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
812  {
813  AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
814 
816  frame = obj->getString("frame");
817 
818  if (obj->hasElement("agent"))
819  {
820  agent = obj->getString("agent");
821  }
822  }
823 
825  Shared(other),
826  armarx::Serializable(other),
827  Vector3Base(other.x, other.y, other.z),
828  FramedPositionBase(other.x, other.y, other.z, other.frame, other.agent),
829  Vector3(other.x, other.y, other.z)
830  {
831  }
832 
834  {
835  x = other.x;
836  y = other.y;
837  z = other.z;
838  frame = other.frame;
839  agent = other.agent;
840  return *this;
841  }
842 
844  {
845  return this->clone();
846  }
847 
848  VariantDataClassPtr FramedPosition::clone(const Ice::Current& c) const
849  {
850  return new FramedPosition(*this);
851  }
852 
853  VariantTypeId FramedPosition::getType(const Ice::Current& c) const
854  {
856  }
857 
858  bool FramedPosition::validate(const Ice::Current& c)
859  {
860  return true;
861  }
862 
863  std::ostream& operator<<(std::ostream& stream, const FramedPosition& rhs)
864  {
865  stream << "FramedPosition: " << std::endl << rhs.output() << std::endl;
866  return stream;
867  }
868 
869 
871  Quaternion()
872  {
873  frame = "";
874  }
875 
876  FramedOrientation::FramedOrientation(const Eigen::Matrix3f& m, const std::string& s, const std::string& agent) :
877  Quaternion(m)
878  {
879  frame = s;
880  this->agent = agent;
881  }
882 
883  FramedOrientation::FramedOrientation(const Quaternionf& q, const std::string& frame, const std::string& agent):
884  Quaternion(q)
885  {
886  this->frame = frame;
887  this->agent = agent;
888  }
889 
890  FramedOrientation::FramedOrientation(const Eigen::Matrix4f& m, const std::string& s, const std::string& agent) :
891  Quaternion(m)
892  {
893  frame = s;
894  this->agent = agent;
895  }
896 
897  // this doesnt work for an unknown reason
898  // FramedOrientation::FramedOrientation(const QuaternionBasePtr ori, const std::string &frame )
899  // {
900  // Matrix3f rot;
901  // rot = Quaternionf(ori->qw, ori->qx, ori->qy, ori->qz);
902  // FramedOrientation(rot, frame);
903  // }
904 
905  std::string FramedOrientation::getFrame() const
906  {
907  return this->frame;
908  }
909 
910  std::string FramedOrientation::output(const Ice::Current& c) const
911  {
912  std::stringstream s;
913  s << toEigen()/*.format(ArmarXEigenFormat)*/ << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
914  return s.str();
915  }
916 
917  void FramedOrientation::changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame)
918  {
919  if (newFrame == frame)
920  {
921  return;
922  }
923 
924  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
925 
926  changeFrame(sharedRobot, newFrame);
927  }
928 
929  void FramedOrientation::changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame)
930  {
931  ARMARX_CHECK_NOT_NULL(referenceRobot);
932  changeFrame(*referenceRobot, newFrame);
933  }
934 
935  void FramedOrientation::changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame)
936  {
937  if (newFrame == frame)
938  {
939  return;
940  }
941 
942  if (newFrame == GlobalFrame)
943  {
944  changeToGlobal(referenceRobot);
945  return;
946  }
947 
948  Eigen::Matrix4f oldFrameTransformation = Eigen::Matrix4f::Identity();
949 
950  if (!referenceRobot.hasRobotNode(newFrame))
951  {
952  throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot.getName();
953  }
954 
955  if (frame != GlobalFrame)
956  {
957  oldFrameTransformation = referenceRobot.getRobotNode(frame)->getPoseInRootFrame();
958  }
959  else
960  {
961  oldFrameTransformation = referenceRobot.getRootNode()->getGlobalPose().inverse();
962  }
963 
965  oldPose.block<3, 3>(0, 0) = toEigen();
966 
967  Eigen::Matrix4f newPose =
968  (referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * oldPose;
969 
970  Eigen::Quaternionf quat(newPose.block<3, 3>(0, 0));
971  qw = quat.w();
972  qx = quat.x();
973  qy = quat.y();
974  qz = quat.z();
975  agent = referenceRobot.getName();
976  frame = newFrame;
977  }
978 
980  {
981  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
982  changeToGlobal(sharedRobot);
983  }
984 
985  void FramedOrientation::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
986  {
987  ARMARX_CHECK_NOT_NULL(referenceRobot);
988  changeToGlobal(*referenceRobot);
989  }
991  {
992  if (frame == GlobalFrame)
993  {
994  return;
995  }
996 
997  changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
998  Eigen::Matrix3f rot = referenceRobot.getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen();
999  Eigen::Quaternionf quat(rot);
1000  qw = quat.w();
1001  qx = quat.x();
1002  qy = quat.y();
1003  qz = quat.z();
1004  frame = GlobalFrame;
1005  agent = "";
1006  }
1007 
1009  {
1010  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
1011  return toGlobal(sharedRobot);
1012  }
1013  FramedOrientationPtr FramedOrientation::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
1014  {
1015  ARMARX_CHECK_NOT_NULL(referenceRobot);
1016  return toGlobal(*referenceRobot);
1017  }
1019  {
1020  FramedOrientationPtr newOrientation = FramedOrientationPtr::dynamicCast(this->clone());
1021  newOrientation->changeToGlobal(referenceRobot);
1022  return newOrientation;
1023  }
1024 
1026  {
1027  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
1028  return toGlobalEigen(sharedRobot);
1029  }
1030  Eigen::Matrix3f FramedOrientation::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
1031  {
1032  ARMARX_CHECK_NOT_NULL(referenceRobot);
1033  return toGlobalEigen(*referenceRobot);
1034  }
1036  {
1037  FramedOrientation newOrientation(toEigen(), frame, agent);
1038  newOrientation.changeToGlobal(referenceRobot);
1039  return newOrientation.toEigen();
1040  }
1041 
1043  {
1044  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
1045  return toRootFrame(sharedRobot);
1046  }
1047  FramedOrientationPtr FramedOrientation::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
1048  {
1049  ARMARX_CHECK_NOT_NULL(referenceRobot);
1050  return toRootFrame(*referenceRobot);
1051  }
1053  {
1054  FramedOrientationPtr newOrientation = FramedOrientationPtr::dynamicCast(this->clone());
1055  newOrientation->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
1056  return newOrientation;
1057  }
1058 
1060  {
1061  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
1062  return toRootEigen(sharedRobot);
1063  }
1064  Eigen::Matrix3f FramedOrientation::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
1065  {
1066  ARMARX_CHECK_NOT_NULL(referenceRobot);
1067  return toRootEigen(*referenceRobot);
1068  }
1070  {
1071  FramedOrientation newOrientation(toEigen(), frame, agent);
1072  newOrientation.changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
1073  return newOrientation.toEigen();
1074  }
1075 
1076  void FramedOrientation::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
1077  {
1078  AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
1079 
1080  Quaternion::serialize(obj, c);
1081  obj->setString("frame", frame);
1082  obj->setString("agent", agent);
1083  }
1084 
1085  void FramedOrientation::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
1086  {
1087  AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
1088 
1090  frame = obj->getString("frame");
1091 
1092  if (obj->hasElement("agent"))
1093  {
1094  agent = obj->getString("agent");
1095  }
1096  }
1097 
1099  {
1100  return this->clone();
1101  }
1102 
1103  VariantDataClassPtr FramedOrientation::clone(const Ice::Current& c) const
1104  {
1105  return new FramedOrientation(*this);
1106  }
1107 
1108  VariantTypeId FramedOrientation::getType(const Ice::Current& c) const
1109  {
1111  }
1112 
1113  bool FramedOrientation::validate(const Ice::Current& c)
1114  {
1115  return true;
1116  }
1117 
1118  std::ostream& operator<<(std::ostream& stream, const FramedOrientation& rhs)
1119  {
1120  stream << "FramedOrientation: " << std::endl << rhs.output() << std::endl;
1121  return stream;
1122  }
1123 
1124 
1125  VirtualRobot::LinkedCoordinate FramedPose::createLinkedCoordinate(const VirtualRobot::RobotPtr& virtualRobot, const FramedPositionPtr& position, const FramedOrientationPtr& orientation)
1126  {
1127  VirtualRobot::LinkedCoordinate c(virtualRobot);
1128  std::string frame;
1129 
1130  if (position)
1131  {
1132  frame = position->getFrame();
1133 
1134  if (orientation)
1135  {
1136  if (!frame.empty() && frame != orientation->getFrame())
1137  {
1138  throw armarx::UserException("Error: frames mismatch");
1139  }
1140  }
1141  }
1142  else
1143  {
1144  if (!orientation)
1145  {
1146  armarx::UserException("createLinkedCoordinate: orientation and position are both NULL");
1147  }
1148  else
1149  {
1150  frame = orientation->getFrame();
1151  }
1152  }
1153 
1155 
1156  if (orientation)
1157  {
1158  pose.block<3, 3>(0, 0) = orientation->toEigen();
1159  }
1160 
1161  if (position)
1162  {
1163  pose.block<3, 1>(0, 3) = position->toEigen();
1164  }
1165 
1166  c.set(frame, pose);
1167 
1168  return c;
1169  }
1170 
1171  FramedPosePtr FramedPose::toFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame) const
1172  {
1173  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
1174  return toFrame(sharedRobot, newFrame);
1175  }
1176 
1177  FramedPosePtr FramedPose::toFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame) const
1178  {
1179  FramedPosePtr newPose = FramedPosePtr::dynamicCast(this->clone());
1180  newPose->changeFrame(referenceRobot, newFrame);
1181  return newPose;
1182  }
1183 
1185  FramedPose::toFrameEigen(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame) const
1186  {
1187  VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
1188  return toFrameEigen(sharedRobot, newFrame);
1189  }
1190 
1192  FramedPose::toFrameEigen(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame) const
1193  {
1194  return toFrame(referenceRobot, newFrame)->toEigen();
1195  }
1196 
1198  {
1199  return this->clone();
1200  }
1201 
1202  VariantDataClassPtr FramedPose::clone(const Ice::Current& c) const
1203  {
1204  return new FramedPose(*this);
1205  }
1206 
1207  VariantTypeId FramedPose::getType(const Ice::Current& c) const
1208  {
1209  return VariantType::FramedPose;
1210  }
1211 
1212  bool FramedPose::validate(const Ice::Current& c)
1213  {
1214  return true;
1215  }
1216 
1217  std::ostream& operator<<(std::ostream& stream, const FramedPose& rhs)
1218  {
1219  stream << "FramedPose: " << std::endl << rhs.output() << std::endl;
1220  return stream;
1221  }
1222 
1223  bool operator==(const FramedPose& pose1, const FramedPose& pose2)
1224  {
1225  if (pose1.frame != pose2.frame || pose1.agent != pose2.agent) return false;
1226  return (pose1.toEigen().isApprox(pose2.toEigen()));
1227  }
1228 
1229  bool operator!=(const FramedPose& pose1, const FramedPose& pose2)
1230  {
1231  return !(pose1 == pose2);
1232  }
1233 
1234 
1235 }
armarx::FramedDirection::output
std::string output(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: FramedPose.cpp:265
RemoteRobot.h
armarx::FramedPose::serialize
void serialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: FramedPose.cpp:573
Eigen
Definition: Elements.h:36
armarx::VariantType::Float
const VariantTypeId Float
Definition: Variant.h:918
armarx::FramedPosition::serialize
void serialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: FramedPose.cpp:802
armarx::operator!=
bool operator!=(const RemoteHandle< PrxTA > &fst, const RemoteHandle< PrxTB > &snd)
Definition: RemoteHandle.h:220
armarx::FramedOrientation::getFrame
std::string getFrame() const
Definition: FramedPose.cpp:905
armarx::FramedPosition::operator=
FramedPosition & operator=(const armarx::FramedPosition &other)
Definition: FramedPose.cpp:833
armarx::FramedPose
The FramedPose class.
Definition: FramedPose.h:258
armarx::VariantType::FramedPose
const VariantTypeId FramedPose
Definition: FramedPose.h:37
armarx::FramedPose::getType
VariantTypeId getType(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: FramedPose.cpp:1207
armarx::FramedPosition::getFrame
std::string getFrame() const
Definition: FramedPose.cpp:624
armarx::FramedPose::createLinkedCoordinate
static VirtualRobot::LinkedCoordinate createLinkedCoordinate(const VirtualRobot::RobotPtr &virtualRobot, const FramedPositionPtr &position, const FramedOrientationPtr &orientation)
Definition: FramedPose.cpp:1125
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition: ExpressionException.h:206
armarx::FramedPose::toFrame
FramedPosePtr toFrame(const SharedRobotInterfacePrx &referenceRobot, const std::string &newFrame) const
Definition: FramedPose.cpp:1171
armarx::FramedPose::clone
VariantDataClassPtr clone(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: FramedPose.cpp:1202
armarx::FramedPose::toFrameEigen
Eigen::Matrix4f toFrameEigen(const SharedRobotInterfacePrx &referenceRobot, const std::string &newFrame) const
Definition: FramedPose.cpp:1185
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:62
AbstractObjectSerializer.h
armarx::FramedDirection::ice_clone
Ice::ObjectPtr ice_clone() const override
Definition: FramedPose.cpp:320
armarx::FramedDirection::toRootEigen
Eigen::Vector3f toRootEigen(const SharedRobotInterfacePrx &referenceRobot) const
Definition: FramedPose.cpp:248
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::Pose::init
void init()
Definition: Pose.cpp:362
armarx::FramedDirection::getType
VariantTypeId getType(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: FramedPose.cpp:330
armarx::Quaternion::deserialize
void deserialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: Pose.cpp:269
armarx::FramedPosition::FramedPosition
FramedPosition()
Definition: FramedPose.cpp:595
armarx::FramedPosition::toRootEigen
Eigen::Vector3f toRootEigen(const SharedRobotInterfacePrx &referenceRobot) const
Definition: FramedPose.cpp:778
IceUtil
Definition: Instance.h:21
armarx::FramedOrientation::toRootFrame
FramedOrientationPtr toRootFrame(const SharedRobotInterfacePrx &referenceRobot) const
Definition: FramedPose.cpp:1042
armarx::FramedPose::toGlobal
FramedPosePtr toGlobal(const SharedRobotInterfacePrx &referenceRobot) const
Definition: FramedPose.cpp:492
armarx::Vector3::deserialize
void deserialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: Pose.cpp:168
armarx::FramedOrientation::getType
VariantTypeId getType(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: FramedPose.cpp:1108
armarx::FramedPosition::toGlobalEigen
Eigen::Vector3f toGlobalEigen(const SharedRobotInterfacePrx &referenceRobot) const
Definition: FramedPose.cpp:744
armarx::FramedPosition::ice_clone
Ice::ObjectPtr ice_clone() const override
Definition: FramedPose.cpp:843
armarx::FramedPosition::output
std::string output(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: FramedPose.cpp:795
armarx::VariantType::Quaternion
const VariantTypeId Quaternion
Definition: Pose.h:39
armarx::FramedDirection::FramedDirection
FramedDirection()
armarx::FramedPosition::deserialize
void deserialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: FramedPose.cpp:811
armarx::FramedDirection::toRootFrame
FramedDirectionPtr toRootFrame(const SharedRobotInterfacePrx &referenceRobot) const
Definition: FramedPose.cpp:231
armarx::FramedOrientation::output
std::string output(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: FramedPose.cpp:910
IceInternal::Handle< FramedDirection >
armarx::FramedOrientation::toGlobal
FramedOrientationPtr toGlobal(const SharedRobotInterfacePrx &referenceRobot) const
Definition: FramedPose.cpp:1008
armarx::FramedPosition::clone
VariantDataClassPtr clone(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: FramedPose.cpp:848
armarx::FramedPose::validate
bool validate(const Ice::Current &c=Ice::emptyCurrent) override
Definition: FramedPose.cpp:1212
armarx::ArmarXEigenFormat
const Eigen::IOFormat ArmarXEigenFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", "\n", "", "")
armarx::FramedDirection::toGlobalEigen
Eigen::Vector3f toGlobalEigen(const SharedRobotInterfacePrx &referenceRobot) const
Definition: FramedPose.cpp:214
armarx::FramedDirection::serialize
void serialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: FramedPose.cpp:299
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::FramedOrientation::ice_clone
Ice::ObjectPtr ice_clone() const override
Definition: FramedPose.cpp:1098
armarx::FramedOrientation::toGlobalEigen
Eigen::Matrix3f toGlobalEigen(const SharedRobotInterfacePrx &referenceRobot) const
Definition: FramedPose.cpp:1025
armarx::SharedRobotInterfacePrx
::IceInternal::ProxyHandle< ::IceProxy::armarx::SharedRobotInterface > SharedRobotInterfacePrx
Definition: FramedPose.h:57
armarx::FramedPose::output
std::string output(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: FramedPose.cpp:390
armarx::FramedDirection::toGlobal
FramedDirectionPtr toGlobal(const SharedRobotInterfacePrx &referenceRobot) const
Definition: FramedPose.cpp:197
armarx::FramedPose::getPosition
FramedPositionPtr getPosition() const
Definition: FramedPose.cpp:561
armarx::FramedPose::toGlobalEigen
Eigen::Matrix4f toGlobalEigen(const SharedRobotInterfacePrx &referenceRobot) const
Definition: FramedPose.cpp:509
armarx::FramedOrientation::changeToGlobal
void changeToGlobal(const SharedRobotInterfacePrx &referenceRobot)
Definition: FramedPose.cpp:979
armarx::FramedDirection::validate
bool validate(const Ice::Current &c=Ice::emptyCurrent) override
Definition: FramedPose.cpp:335
FramedPose.h
armarx::ValueToString
std::string ValueToString(const T &value)
Definition: StringHelpers.h:58
armarx::armem::human::Robot
@ Robot
Definition: util.h:14
armarx::FramedPosition
The FramedPosition class.
Definition: FramedPose.h:142
armarx::FramedOrientation::serialize
void serialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: FramedPose.cpp:1076
armarx::FramedPosition::validate
bool validate(const Ice::Current &c=Ice::emptyCurrent) override
Definition: FramedPose.cpp:858
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::FramedPose::FramedPose
FramedPose()
Definition: FramedPose.cpp:347
armarx::FramedOrientation::validate
bool validate(const Ice::Current &c=Ice::emptyCurrent) override
Definition: FramedPose.cpp:1113
armarx::FramedDirection::getFrame
std::string getFrame() const
Definition: FramedPose.cpp:81
armarx::operator==
bool operator==(const RemoteHandle< PrxTA > &fst, const RemoteHandle< PrxTB > &snd)
Definition: RemoteHandle.h:216
armarx::FramedPosition::changeToGlobal
void changeToGlobal(const SharedRobotInterfacePrx &referenceRobot)
Definition: FramedPose.cpp:700
armarx::Pose::deserialize
void deserialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: Pose.cpp:353
armarx::FramedOrientation
The FramedOrientation class.
Definition: FramedPose.h:199
armarx::VariantTypeId
Ice::Int VariantTypeId
Definition: Variant.h:44
armarx::FramedPosition::changeFrame
void changeFrame(const SharedRobotInterfacePrx &referenceRobot, const std::string &newFrame)
Definition: FramedPose.cpp:629
boost::source
Vertex source(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:681
armarx::Quaternion::toEigen
Eigen::Matrix3f toEigen() const
Definition: Pose.cpp:204
q
#define q
ARMARX_WARNING_S
#define ARMARX_WARNING_S
Definition: Logging.h:206
armarx::FramedPosition::getType
VariantTypeId getType(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: FramedPose.cpp:853
armarx::FramedDirection::clone
VariantDataClassPtr clone(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: FramedPose.cpp:325
armarx::FramedPose::toRootEigen
Eigen::Matrix4f toRootEigen(const SharedRobotInterfacePrx &referenceRobot) const
Definition: FramedPose.cpp:543
ExpressionException.h
armarx::FramedPosition::toRootFrame
FramedPositionPtr toRootFrame(const SharedRobotInterfacePrx &referenceRobot) const
Definition: FramedPose.cpp:761
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::changeToGlobal
void changeToGlobal(const SharedRobotInterfacePrx &referenceRobot)
Definition: FramedPose.cpp:169
armarx::VariantType::FramedOrientation
const VariantTypeId FramedOrientation
Definition: FramedPose.h:40
armarx::FramedDirection
FramedDirection is a 3 dimensional direction vector with a reference frame. The reference frame can b...
Definition: FramedPose.h:83
armarx::VariantType::FramedDirection
const VariantTypeId FramedDirection
Definition: FramedPose.h:38
armarx::FramedPose::getOrientation
FramedOrientationPtr getOrientation() const
Definition: FramedPose.cpp:567
armarx::Vector3::toEigen
virtual Eigen::Vector3f toEigen() const
Definition: Pose.cpp:134
armarx::Quaternion< float, 0 >
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:41
armarx::operator<<
std::ostream & operator<<(std::ostream &os, const PythonApplicationManager::Paths &paths)
Definition: PythonApplicationManager.cpp:221
armarx::Vector3::serialize
void serialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.cpp:158
armarx::FramedPose::toRootFrame
FramedPosePtr toRootFrame(const SharedRobotInterfacePrx &referenceRobot) const
Definition: FramedPose.cpp:526
armarx::FramedOrientation::FramedOrientation
FramedOrientation()
Definition: FramedPose.cpp:870
armarx::aron::type::ObjectPtr
std::shared_ptr< Object > ObjectPtr
Definition: Object.h:36
armarx::FramedPose::getFrame
std::string getFrame() const
Definition: FramedPose.cpp:385
Logging.h
armarx::FramedDirection::changeFrame
void changeFrame(const VirtualRobot::RobotPtr &robot, const std::string &newFrame)
Definition: FramedPose.cpp:119
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:39
armarx::FramedPose::changeToGlobal
void changeToGlobal(const SharedRobotInterfacePrx &referenceRobot)
Definition: FramedPose.cpp:459
armarx::FramedPose::ice_clone
Ice::ObjectPtr ice_clone() const override
Definition: FramedPose.cpp:1197
armarx::FramedOrientation::clone
VariantDataClassPtr clone(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: FramedPose.cpp:1103
armarx::FramedPosition::toGlobal
FramedPositionPtr toGlobal(const SharedRobotInterfacePrx &referenceRobot) const
Definition: FramedPose.cpp:727
armarx::FramedPose::deserialize
void deserialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: FramedPose.cpp:582
armarx::FramedOrientation::toRootEigen
Eigen::Matrix3f toRootEigen(const SharedRobotInterfacePrx &referenceRobot) const
Definition: FramedPose.cpp:1059
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
armarx::FramedOrientation::deserialize
void deserialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: FramedPose.cpp:1085
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::FramedOrientation::changeFrame
void changeFrame(const SharedRobotInterfacePrx &referenceRobot, const std::string &newFrame)
Definition: FramedPose.cpp:917
armarx::FramedPose::changeFrame
void changeFrame(const SharedRobotInterfacePrx &referenceRobot, const std::string &newFrame)
Definition: FramedPose.cpp:398
armarx::Quaternion::serialize
void serialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: Pose.cpp:258
armarx::FramedDirection::deserialize
void deserialize(const armarx::ObjectSerializerBasePtr &serializer, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: FramedPose.cpp:308