33 #include <VirtualRobot/Robot.h>
34 #include <VirtualRobot/LinkedCoordinate.h>
35 #include <VirtualRobot/VirtualRobot.h>
36 #include <VirtualRobot/RobotConfig.h>
39 #include <Eigen/Geometry>
43 using namespace Eigen;
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>;
61 FramedDirectionBase(
source),
94 if (framedVec.
getFrame() == newFrame)
96 return FramedDirectionPtr::dynamicCast(framedVec.
clone());
99 if (!robot.hasRobotNode(newFrame))
101 throw LocalException() <<
"The requested frame '" << newFrame <<
"' does not exists in the robot " << robot.getName();
104 Eigen::Matrix4f rotToNewFrame = __GetRotationBetweenFrames(framedVec.frame, newFrame, robot);
106 Eigen::Vector3f vecOldFrame = framedVec.Vector3::toEigen();
108 Eigen::Vector3f newVec = rotToNewFrame.block(0, 0, 3, 3).inverse() * vecOldFrame;
111 result->x = newVec(0);
112 result->y = newVec(1);
113 result->z = newVec(2);
114 result->frame = newFrame;
115 result->agent = robot.getName();
144 if (!robot.hasRobotNode(newFrame))
146 throw LocalException() <<
"The requested frame '" << newFrame <<
"' does not exist in the robot " << robot.getName();
149 if (frame !=
GlobalFrame && !robot.hasRobotNode(frame))
151 throw LocalException() <<
"The current frame '" << frame <<
"' does not exist in the robot " << robot.getName();
155 Eigen::Matrix4f rotToNewFrame = __GetRotationBetweenFrames(frame, newFrame, robot);
159 Eigen::Vector3f newVec = rotToNewFrame.block(0, 0, 3, 3).inverse() * vecOldFrame;
166 agent = robot.getName();
171 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
188 changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
189 Eigen::Vector3f pos = referenceRobot.getRootNode()->getGlobalPose().block<3, 3>(0, 0) *
toEigen();
199 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
210 newDirection->changeToGlobal(referenceRobot);
216 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
233 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
244 newDirection->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
250 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
261 newDirection.
changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
278 auto node = robotState.getRobotNode(newFrame);
280 toNewFrame = node->getGlobalPose();
284 auto node = robotState.getRobotNode(oldFrame);
286 auto newNode = robotState.getRobotNode(newFrame);
288 toNewFrame = node->getTransformationTo(newNode);
291 toNewFrame(0, 3) = 0;
292 toNewFrame(1, 3) = 0;
293 toNewFrame(2, 3) = 0;
303 obj->setString(
"frame", frame);
304 obj->setString(
"agent", agent);
312 frame = obj->getString(
"frame");
314 if (obj->hasElement(
"agent"))
316 agent = obj->getString(
"agent");
322 return this->
clone();
342 stream <<
"FramedDirection: " << std::endl << rhs.
output() << std::endl;
355 armarx::Serializable(pose),
356 armarx::VariantDataClass(pose),
358 FramedPoseBase(pose),
378 FramedPose::FramedPose(
const armarx::Vector3BasePtr pos,
const armarx::QuaternionBasePtr ori,
const std::string& frame,
const std::string& agent) :
393 const Eigen::IOFormat
ArmarXEigenFormat(Eigen::StreamPrecision, 4,
" ",
"\n",
"",
"");
400 if (newFrame == frame)
405 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
417 if (newFrame == frame)
430 if (frame !=
GlobalFrame and !referenceRobot.hasRobotNode(frame))
432 throw LocalException() <<
"The current frame '" << frame <<
"' does not exists in the robot " << referenceRobot.getName();
434 if (!referenceRobot.hasRobotNode(newFrame))
436 throw LocalException() <<
"The requested frame '" << newFrame <<
"' does not exists in the robot " << referenceRobot.getName();
441 oldFrameTransformation = referenceRobot.getRobotNode(frame)->getPoseInRootFrame();
445 oldFrameTransformation = referenceRobot.getRootNode()->getGlobalPose().inverse();
449 (referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) *
toEigen();
452 Eigen::Vector3f pos = newPose.block<3, 1>(0, 3);
455 agent = referenceRobot.getName();
461 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
477 changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
478 Eigen::Matrix4f global = referenceRobot.getRootNode()->getGlobalPose();
480 position->x = pose(0, 3);
481 position->y = pose(1, 3);
482 position->z = pose(2, 3);
484 orientation->qw =
q.w();
485 orientation->qx =
q.x();
486 orientation->qy =
q.y();
487 orientation->qz =
q.z();
494 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
505 newPose->changeToGlobal(referenceRobot);
511 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
528 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
539 newPose->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
545 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
556 newPose.
changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
578 obj->setString(
"frame", frame);
579 obj->setString(
"agent", agent);
587 frame = obj->getString(
"frame");
589 if (obj->hasElement(
"agent"))
591 agent = obj->getString(
"agent");
631 if (newFrame == frame)
636 if (!referenceRobot->hasRobotNode(newFrame))
638 throw LocalException() <<
"The requested frame '" << newFrame <<
"' does not exists in the robot " << referenceRobot->getName();
641 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
653 if (newFrame == frame)
664 if (newFrame.empty())
666 ARMARX_WARNING_S <<
"Frame Conversion: Frame is empty, always set frame names! ...assuming GlobalFrame";
673 if (!referenceRobot.hasRobotNode(newFrame))
675 throw LocalException() <<
"The requested frame '" << newFrame <<
"' does not exists in the robot " << referenceRobot.getName();
680 oldFrameTransformation = referenceRobot.getRobotNode(frame)->getPoseInRootFrame();
684 oldFrameTransformation = referenceRobot.getRootNode()->getGlobalPose().inverse();
688 oldPose.block<3, 1>(0, 3) =
toEigen();
690 (referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * oldPose;
692 Eigen::Vector3f pos = newPose.block<3, 1>(0, 3);
696 agent = referenceRobot.getName();
702 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
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);
729 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
740 newPosition->changeToGlobal(referenceRobot);
746 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
763 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
774 newPosition->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
780 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
791 newPosition.
changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
807 obj->setString(
"frame", frame);
808 obj->setString(
"agent", agent);
816 frame = obj->getString(
"frame");
818 if (obj->hasElement(
"agent"))
820 agent = obj->getString(
"agent");
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)
845 return this->
clone();
865 stream <<
"FramedPosition: " << std::endl << rhs.
output() << std::endl;
913 s <<
toEigen() << std::endl <<
"frame: " <<
getFrame() << (agent.empty() ?
"" : (
" agent: " + agent));
919 if (newFrame == frame)
924 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
937 if (newFrame == frame)
950 if (!referenceRobot.hasRobotNode(newFrame))
952 throw LocalException() <<
"The requested frame '" << newFrame <<
"' does not exists in the robot " << referenceRobot.getName();
957 oldFrameTransformation = referenceRobot.getRobotNode(frame)->getPoseInRootFrame();
961 oldFrameTransformation = referenceRobot.getRootNode()->getGlobalPose().inverse();
965 oldPose.block<3, 3>(0, 0) =
toEigen();
968 (referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * oldPose;
975 agent = referenceRobot.getName();
981 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
997 changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
1010 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
1021 newOrientation->changeToGlobal(referenceRobot);
1022 return newOrientation;
1027 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
1039 return newOrientation.
toEigen();
1044 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
1055 newOrientation->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
1056 return newOrientation;
1061 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
1072 newOrientation.
changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
1073 return newOrientation.
toEigen();
1081 obj->setString(
"frame", frame);
1082 obj->setString(
"agent", agent);
1090 frame = obj->getString(
"frame");
1092 if (obj->hasElement(
"agent"))
1094 agent = obj->getString(
"agent");
1100 return this->
clone();
1120 stream <<
"FramedOrientation: " << std::endl << rhs.
output() << std::endl;
1127 VirtualRobot::LinkedCoordinate
c(virtualRobot);
1132 frame = position->getFrame();
1136 if (!frame.empty() && frame != orientation->getFrame())
1138 throw armarx::UserException(
"Error: frames mismatch");
1146 armarx::UserException(
"createLinkedCoordinate: orientation and position are both NULL");
1150 frame = orientation->getFrame();
1158 pose.block<3, 3>(0, 0) = orientation->toEigen();
1163 pose.block<3, 1>(0, 3) = position->toEigen();
1173 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
1174 return toFrame(sharedRobot, newFrame);
1180 newPose->changeFrame(referenceRobot, newFrame);
1187 VirtualRobot::RobotPtr sharedRobot(
new RemoteRobot(referenceRobot));
1194 return toFrame(referenceRobot, newFrame)->toEigen();
1199 return this->
clone();
1219 stream <<
"FramedPose: " << std::endl << rhs.
output() << std::endl;
1225 if (pose1.frame != pose2.frame || pose1.agent != pose2.agent)
return false;
1231 return !(pose1 == pose2);