Skip to content
Snippets Groups Projects
Commit 9fb96c16 authored by Simon Ottenhaus's avatar Simon Ottenhaus
Browse files

Pose convenience functions

parent 72fab436
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/libraries/core/FramedPose.cpp
+ 100
− 7
View file @ 9fb96c16
......@@ -34,10 +34,11 @@ namespace armarx
}
FramedDirection::FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string& frame) :
FramedDirection::FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string& frame, const string& agent) :
Vector3(x, y, z)
{
this->frame = frame;
this->agent = agent;
}
string FramedDirection::getFrame() const
......@@ -45,7 +46,7 @@ namespace armarx
return frame;
}
FramedDirectionPtr FramedDirection::ChangeFrame(const VirtualRobot::RobotPtr robot, const FramedDirection& framedVec, const string& newFrame)
FramedDirectionPtr FramedDirection::ChangeFrame(const VirtualRobot::RobotPtr& robot, const FramedDirection& framedVec, const string& newFrame)
{
if (framedVec.getFrame() == newFrame)
{
......@@ -71,7 +72,7 @@ namespace armarx
return result;
}
void FramedDirection::changeFrame(const VirtualRobot::RobotPtr robot, const string& newFrame)
void FramedDirection::changeFrame(const VirtualRobot::RobotPtr& robot, const string& newFrame)
{
if (getFrame() == newFrame)
{
......@@ -129,7 +130,6 @@ namespace armarx
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobal(sharedRobot);
}
FramedDirectionPtr FramedDirection::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedDirectionPtr newPos = FramedDirectionPtr::dynamicCast(this->clone());
......@@ -137,6 +137,30 @@ namespace armarx
return newPos;
}
FramedDirectionPtr FramedDirection::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toRootFrame(sharedRobot);
}
FramedDirectionPtr FramedDirection::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedDirectionPtr newDir = FramedDirectionPtr::dynamicCast(this->clone());
newDir->changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
return newDir;
}
Eigen::Vector3f FramedDirection::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toRootEigen(sharedRobot);
}
Eigen::Vector3f FramedDirection::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedDirection newDir(toEigen(), frame, agent);
newDir.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
return newDir.toEigen();
}
string FramedDirection::output(const Ice::Current& c) const
{
std::stringstream s;
......@@ -320,7 +344,6 @@ namespace armarx
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobal(sharedRobot);
}
FramedPosePtr FramedPose::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedPosePtr newPose = FramedPosePtr::dynamicCast(this->clone());
......@@ -328,6 +351,30 @@ namespace armarx
return newPose;
}
FramedPosePtr FramedPose::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toRootFrame(sharedRobot);
}
FramedPosePtr FramedPose::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedPosePtr newPose = FramedPosePtr::dynamicCast(this->clone());
newPose->changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
return newPose;
}
Eigen::Matrix4f FramedPose::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toRootEigen(sharedRobot);
}
Eigen::Matrix4f FramedPose::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedPose newPose(toEigen(), frame, agent);
newPose.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
return newPose.toEigen();
}
FramedPositionPtr FramedPose::getPosition() const
{
......@@ -482,7 +529,6 @@ namespace armarx
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobal(sharedRobot);
}
FramedPositionPtr FramedPosition::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedPositionPtr newPos = FramedPositionPtr::dynamicCast(this->clone());
......@@ -490,6 +536,30 @@ namespace armarx
return newPos;
}
FramedPositionPtr FramedPosition::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toRootFrame(sharedRobot);
}
FramedPositionPtr FramedPosition::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedPositionPtr newPosition = FramedPositionPtr::dynamicCast(this->clone());
newPosition->changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
return newPosition;
}
Eigen::Vector3f FramedPosition::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toRootEigen(sharedRobot);
}
Eigen::Vector3f FramedPosition::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedPosition newPosition(toEigen(), frame, agent);
newPosition.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
return newPosition.toEigen();
}
string FramedPosition::output(const Ice::Current& c) const
{
std::stringstream s;
......@@ -644,7 +714,6 @@ namespace armarx
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobal(sharedRobot);
}
FramedOrientationPtr FramedOrientation::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedOrientationPtr newPos = FramedOrientationPtr::dynamicCast(this->clone());
......@@ -652,6 +721,30 @@ namespace armarx
return newPos;
}
FramedOrientationPtr FramedOrientation::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toRootFrame(sharedRobot);
}
FramedOrientationPtr FramedOrientation::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedOrientationPtr newOrientation = FramedOrientationPtr::dynamicCast(this->clone());
newOrientation->changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
return newOrientation;
}
Eigen::Matrix3f FramedOrientation::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toRootEigen(sharedRobot);
}
Eigen::Matrix3f FramedOrientation::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
{
FramedOrientation newOrientation(toEigen(), frame, agent);
newOrientation.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
return newOrientation.toEigen();
}
void FramedOrientation::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
......
source/RobotAPI/libraries/core/FramedPose.h
+ 20
− 5
View file @ 9fb96c16
......@@ -84,16 +84,20 @@ namespace armarx
FramedDirection();
FramedDirection(const FramedDirection& source);
FramedDirection(const Eigen::Vector3f& vec, const std::string& frame, const std::string& agent);
FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string& frame);
FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string& frame, const std::string& agent);
std::string getFrame() const;
static FramedDirectionPtr ChangeFrame(const VirtualRobot::RobotPtr robot, const FramedDirection& framedVec, const std::string& newFrame);
void changeFrame(const VirtualRobot::RobotPtr robot, const std::string& newFrame);
static FramedDirectionPtr ChangeFrame(const VirtualRobot::RobotPtr& robot, const FramedDirection& framedVec, const std::string& newFrame);
void changeFrame(const VirtualRobot::RobotPtr& robot, const std::string& newFrame);
void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
FramedDirectionPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
FramedDirectionPtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
FramedDirectionPtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
FramedDirectionPtr toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const;
Eigen::Vector3f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
Eigen::Vector3f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
// inherited from VariantDataClass
Ice::ObjectPtr ice_clone() const
......@@ -123,8 +127,8 @@ namespace armarx
public: // serialization
virtual void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const;
virtual void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current());
private:
private:
static Eigen::Matrix4f __GetRotationBetweenFrames(const std::string& oldFrame, const std::string& newFrame, VirtualRobot::RobotPtr robotState);
};
......@@ -154,7 +158,10 @@ namespace armarx
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
FramedPositionPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
FramedPositionPtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
FramedPositionPtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
FramedPositionPtr toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const;
Eigen::Vector3f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
Eigen::Vector3f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
// inherited from VariantDataClass
Ice::ObjectPtr ice_clone() const
......@@ -232,6 +239,10 @@ namespace armarx
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
FramedOrientationPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
FramedOrientationPtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
FramedOrientationPtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
FramedOrientationPtr toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const;
Eigen::Matrix3f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
Eigen::Matrix3f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
friend std::ostream& operator<<(std::ostream& stream, const FramedOrientation& rhs)
{
......@@ -297,6 +308,10 @@ namespace armarx
void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
FramedPosePtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
FramedPosePtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
FramedPosePtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
FramedPosePtr toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const;
Eigen::Matrix4f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
Eigen::Matrix4f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
friend std::ostream& operator<<(std::ostream& stream, const FramedPose& rhs)
{
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment