From 3ed3e47d430de34d8eef331346af6669695b2d4d Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 12 Sep 2019 09:30:13 +0200
Subject: [PATCH] Move method defs
---
.../RobotState/RobotStateComponent.cpp | 328 +++++++++---------
1 file changed, 164 insertions(+), 164 deletions(-)
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 083a9e811..5fc79d828 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -272,170 +272,6 @@ namespace armarx
}
- bool RobotStateComponent::interpolate(double time, RobotStateConfig& config) const
- {
- bool success = interpolateJoints(time, config.jointMap);
-
- FramedPosePtr globalPose = FramedPosePtr::dynamicCast(config.globalPose);
- ARMARX_CHECK(globalPose);
- success = success && interpolatePose(time, *globalPose);
-
- config.timestamp = time;
- return success;
- }
-
-
- bool RobotStateComponent::interpolateJoints(double time, NameValueMap& jointAngles) const
- {
- boost::shared_lock<SharedMutex> lock(jointHistoryMutex);
- // Get the older newer entry of time.
- auto nextIt = jointHistory.lower_bound(time);
- if (jointHistory.empty())
- {
- ARMARX_WARNING << deactivateSpam(1) << "Joint history of robot state component is empty!";
- return false;
- }
- if (nextIt == jointHistory.end())
- {
- ARMARX_WARNING << deactivateSpam(1)
- << "Could not find or interpolate a robot joint angles for time "
- << IceUtil::Time::microSeconds(static_cast<int>(time)).toDateTime()
- << " - oldest available value: "
- << IceUtil::Time::microSeconds(static_cast<int>(jointHistory.begin()->first)).toDateTime();
- return false;
- }
-
- // Found an entry.
-
- const double newestTimeInHistory = jointHistory.rbegin()->first;
- if (time > newestTimeInHistory)
- {
- double maxOffset = 2000000;
- if (time > newestTimeInHistory + maxOffset)
- {
- ARMARX_WARNING << deactivateSpam(1) << "Requested timestamp is substantially newer (>"
- << maxOffset << " usec) than newest available timestamp!\nrequested timestamp: "
- << IceUtil::Time::microSecondsDouble(time).toDateTime() << " newest timestamp: "
- << IceUtil::Time::microSecondsDouble(jointHistory.rbegin()->first).toDateTime();
- return false;
- }
- jointAngles = jointHistory.rbegin()->second;
- return true;
- }
-
-
- const NameValueMap& next = nextIt->second;
- auto prevIt = std::prev(nextIt);
-
- if (prevIt == jointHistory.end())
- {
- // Next was oldest entry.
- jointAngles = next;
- return true;
- }
-
- // Interpolate.
-
- double prevTime = prevIt->first;
- double nextTime = nextIt->first;
-
- float t = static_cast<float>((time - prevTime) / (nextTime - prevTime));
-
- jointAngles.clear();
- auto prevJointIt = prevIt->second.begin();
- for (const auto& [name, nextAngle] : next)
- {
- float value = (1 - t) * prevJointIt->second + t * nextAngle;
- prevJointIt++;
-
- jointAngles.emplace(name, value);
- }
-
- return true;
- }
-
-
- bool RobotStateComponent::interpolatePose(double time, FramedPose& pose) const
- {
- boost::shared_lock<SharedMutex> lock(poseHistoryMutex);
- // Get the older newer entry of time.
- auto nextIt = poseHistory.lower_bound(time);
- if (poseHistory.empty())
- {
- ARMARX_WARNING << deactivateSpam(1) << "Pose history of robot state component is empty!";
- return false;
- }
- if (nextIt == poseHistory.end())
- {
- ARMARX_WARNING << deactivateSpam(1)
- << "Could not find or interpolate a robot pose for time "
- << IceUtil::Time::microSeconds(static_cast<int>(time)).toDateTime()
- << " - oldest available value: "
- << IceUtil::Time::microSeconds(static_cast<int>(poseHistory.begin()->first)).toDateTime();
- return false;
- }
-
- // Found an entry.
-
- const double newestTimeInHistory = poseHistory.rbegin()->first;
- if (time > newestTimeInHistory)
- {
- double maxOffset = 2000000;
- if (time > newestTimeInHistory + maxOffset)
- {
- ARMARX_WARNING << deactivateSpam(1) << "Requested timestamp is substantially newer (>"
- << maxOffset << " usec) than newest available timestamp!\nrequested timestamp: "
- << IceUtil::Time::microSecondsDouble(time).toDateTime() << " newest timestamp: "
- << IceUtil::Time::microSecondsDouble(poseHistory.rbegin()->first).toDateTime();
- return false;
- }
- pose = *poseHistory.rbegin()->second;
- return true;
- }
-
-
- auto prevIt = std::prev(nextIt);
- ARMARX_CHECK_EXPRESSION(prevIt->second);
-
- const FramedPosePtr& next = nextIt->second;
-
- if (prevIt == poseHistory.end())
- {
- // Next was oldest entry.
- pose = *next;
- return true;
- }
-
- const FramedPosePtr& prev = prevIt->second;
-
-
- // Interpolate.
-
- double prevTime = prevIt->first;
- double nextTime = nextIt->first;
-
- float t = static_cast<float>((time - prevTime) / (nextTime - prevTime));
-
- Eigen::Matrix4f globalPoseNext = next->toEigen();
- Eigen::Matrix4f globalPosePrev = prev->toEigen();
-
-
- Eigen::Matrix4f globalPose;
-
- math::Helpers::Position(globalPose) =
- (1 - t) * math::Helpers::Position(globalPosePrev) + t * math::Helpers::Position(globalPoseNext);
-
- Eigen::Quaternionf rotNext(math::Helpers::Orientation(globalPoseNext));
- Eigen::Quaternionf rotPrev(math::Helpers::Orientation(globalPosePrev));
- Eigen::Quaternionf rotNew = rotPrev.slerp(t, rotNext);
-
- math::Helpers::Orientation(globalPose) = rotNew.toRotationMatrix();
-
- pose = FramedPose(globalPose, armarx::GlobalFrame, "");
- return true;
- }
-
-
void RobotStateComponent::reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Current&)
{
if (timestamp <= 0)
@@ -630,4 +466,168 @@ namespace armarx
return robotStateTopicName;
}
+
+ bool RobotStateComponent::interpolate(double time, RobotStateConfig& config) const
+ {
+ bool success = interpolateJoints(time, config.jointMap);
+
+ FramedPosePtr globalPose = FramedPosePtr::dynamicCast(config.globalPose);
+ ARMARX_CHECK(globalPose);
+ success = success && interpolatePose(time, *globalPose);
+
+ config.timestamp = time;
+ return success;
+ }
+
+
+ bool RobotStateComponent::interpolateJoints(double time, NameValueMap& jointAngles) const
+ {
+ boost::shared_lock<SharedMutex> lock(jointHistoryMutex);
+ // Get the older newer entry of time.
+ auto nextIt = jointHistory.lower_bound(time);
+ if (jointHistory.empty())
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Joint history of robot state component is empty!";
+ return false;
+ }
+ if (nextIt == jointHistory.end())
+ {
+ ARMARX_WARNING << deactivateSpam(1)
+ << "Could not find or interpolate a robot joint angles for time "
+ << IceUtil::Time::microSeconds(static_cast<int>(time)).toDateTime()
+ << " - oldest available value: "
+ << IceUtil::Time::microSeconds(static_cast<int>(jointHistory.begin()->first)).toDateTime();
+ return false;
+ }
+
+ // Found an entry.
+
+ const double newestTimeInHistory = jointHistory.rbegin()->first;
+ if (time > newestTimeInHistory)
+ {
+ double maxOffset = 2000000;
+ if (time > newestTimeInHistory + maxOffset)
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Requested timestamp is substantially newer (>"
+ << maxOffset << " usec) than newest available timestamp!\nrequested timestamp: "
+ << IceUtil::Time::microSecondsDouble(time).toDateTime() << " newest timestamp: "
+ << IceUtil::Time::microSecondsDouble(jointHistory.rbegin()->first).toDateTime();
+ return false;
+ }
+ jointAngles = jointHistory.rbegin()->second;
+ return true;
+ }
+
+
+ const NameValueMap& next = nextIt->second;
+ auto prevIt = std::prev(nextIt);
+
+ if (prevIt == jointHistory.end())
+ {
+ // Next was oldest entry.
+ jointAngles = next;
+ return true;
+ }
+
+ // Interpolate.
+
+ double prevTime = prevIt->first;
+ double nextTime = nextIt->first;
+
+ float t = static_cast<float>((time - prevTime) / (nextTime - prevTime));
+
+ jointAngles.clear();
+ auto prevJointIt = prevIt->second.begin();
+ for (const auto& [name, nextAngle] : next)
+ {
+ float value = (1 - t) * prevJointIt->second + t * nextAngle;
+ prevJointIt++;
+
+ jointAngles.emplace(name, value);
+ }
+
+ return true;
+ }
+
+
+ bool RobotStateComponent::interpolatePose(double time, FramedPose& pose) const
+ {
+ boost::shared_lock<SharedMutex> lock(poseHistoryMutex);
+ // Get the older newer entry of time.
+ auto nextIt = poseHistory.lower_bound(time);
+ if (poseHistory.empty())
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Pose history of robot state component is empty!";
+ return false;
+ }
+ if (nextIt == poseHistory.end())
+ {
+ ARMARX_WARNING << deactivateSpam(1)
+ << "Could not find or interpolate a robot pose for time "
+ << IceUtil::Time::microSeconds(static_cast<int>(time)).toDateTime()
+ << " - oldest available value: "
+ << IceUtil::Time::microSeconds(static_cast<int>(poseHistory.begin()->first)).toDateTime();
+ return false;
+ }
+
+ // Found an entry.
+
+ const double newestTimeInHistory = poseHistory.rbegin()->first;
+ if (time > newestTimeInHistory)
+ {
+ double maxOffset = 2000000;
+ if (time > newestTimeInHistory + maxOffset)
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Requested timestamp is substantially newer (>"
+ << maxOffset << " usec) than newest available timestamp!\nrequested timestamp: "
+ << IceUtil::Time::microSecondsDouble(time).toDateTime() << " newest timestamp: "
+ << IceUtil::Time::microSecondsDouble(poseHistory.rbegin()->first).toDateTime();
+ return false;
+ }
+ pose = *poseHistory.rbegin()->second;
+ return true;
+ }
+
+
+ auto prevIt = std::prev(nextIt);
+ ARMARX_CHECK_EXPRESSION(prevIt->second);
+
+ const FramedPosePtr& next = nextIt->second;
+
+ if (prevIt == poseHistory.end())
+ {
+ // Next was oldest entry.
+ pose = *next;
+ return true;
+ }
+
+ const FramedPosePtr& prev = prevIt->second;
+
+
+ // Interpolate.
+
+ double prevTime = prevIt->first;
+ double nextTime = nextIt->first;
+
+ float t = static_cast<float>((time - prevTime) / (nextTime - prevTime));
+
+ Eigen::Matrix4f globalPoseNext = next->toEigen();
+ Eigen::Matrix4f globalPosePrev = prev->toEigen();
+
+
+ Eigen::Matrix4f globalPose;
+
+ math::Helpers::Position(globalPose) =
+ (1 - t) * math::Helpers::Position(globalPosePrev) + t * math::Helpers::Position(globalPoseNext);
+
+ Eigen::Quaternionf rotNext(math::Helpers::Orientation(globalPoseNext));
+ Eigen::Quaternionf rotPrev(math::Helpers::Orientation(globalPosePrev));
+ Eigen::Quaternionf rotNew = rotPrev.slerp(t, rotNext);
+
+ math::Helpers::Orientation(globalPose) = rotNew.toRotationMatrix();
+
+ pose = FramedPose(globalPose, armarx::GlobalFrame, "");
+ return true;
+ }
+
}
--
GitLab