From 07deb04c1c95dda4675bcd2ad4bbe747bcdebca3 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <waechter@kit.edu>
Date: Fri, 14 Mar 2014 12:17:46 +0100
Subject: [PATCH] Working TCP Velocity Observer
---
source/RobotAPI/units/TCPControlUnit.cpp | 19 +++++++++++--------
source/RobotAPI/units/TCPControlUnit.h | 3 ++-
2 files changed, 13 insertions(+), 9 deletions(-)
diff --git a/source/RobotAPI/units/TCPControlUnit.cpp b/source/RobotAPI/units/TCPControlUnit.cpp
index be236d187..48ec1595c 100644
--- a/source/RobotAPI/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/units/TCPControlUnit.cpp
@@ -1137,7 +1137,7 @@ void armarx::TCPControlUnit::reportJointAngles(const NameValueMap &jointAngles,
tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame);
}
- ARMARX_INFO << deactivateSpam(5) << "TCP Pose Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
+// ARMARX_INFO << deactivateSpam(5) << "TCP Pose Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
listener->reportTCPPose(tcpPoses);
}
@@ -1147,6 +1147,8 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap &jointVel,
return;
lastTopicReportTime = IceUtil::Time::now();
ScopedLock lock(reportMutex);
+ if(!localVelReportRobot)
+ localVelReportRobot = localReportRobot->clone("VelRobot");
// ARMARX_INFO << jointVel;
std::vector<RobotNodeSetPtr > nodeSets = localReportRobot->getRobotNodeSets();
FramedPoseBaseMap tcpPoses;
@@ -1169,11 +1171,11 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap &jointVel,
{
NameValueMap::const_iterator itSrc = jointVel.find(it->first);
if(itSrc != jointVel.end())
- it->second += it->second * tDelta;
+ it->second += itSrc->second * tDelta;
}
- VirtualRobot::RobotPtr localVelRobot = localReportRobot->clone("VelRobot");
- nodeSets = localVelRobot->getRobotNodeSets();
- localVelRobot->setJointValues(tempJointAngles);
+
+ nodeSets = localVelReportRobot->getRobotNodeSets();
+ localVelReportRobot->setJointValues(tempJointAngles);
IceUtil::Time start = IceUtil::Time::now();
for(unsigned int i=0; i < nodeSets.size(); i++)
{
@@ -1183,9 +1185,10 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap &jointVel,
-// if(tcpName == "Wrist 2 R")
-// currentPose.block(0,3,3,1);
+
+
FramedPosePtr lastPose = FramedPosePtr::dynamicCast(tcpPoses[tcpName]);
+
tcpTranslationVelocities[tcpName] = new FramedVector3((currentPose.block(0,3,3,1) - lastPose->toEigen().block(0,3,3,1))/tDelta, rootFrame);
const Eigen::Matrix3f rot = currentPose.block(0,0,3,3) * lastPose->toEigen().block(0,0,3,3).inverse();
@@ -1221,7 +1224,7 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap &jointVel,
}
- ARMARX_INFO << deactivateSpam(5) << "TCP Pose Vel Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
+// ARMARX_INFO << deactivateSpam(5) << "TCP Pose Vel Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
listener->reportTCPVelocities(tcpTranslationVelocities, tcpOrientationVelocities);
}
diff --git a/source/RobotAPI/units/TCPControlUnit.h b/source/RobotAPI/units/TCPControlUnit.h
index 786f8ec4c..122b21b62 100644
--- a/source/RobotAPI/units/TCPControlUnit.h
+++ b/source/RobotAPI/units/TCPControlUnit.h
@@ -46,7 +46,7 @@ namespace armarx {
defineRequiredProperty<std::string>("KinematicUnitName","Name of the KinematicUnit Proxy");
defineOptionalProperty<float>("MaxJointVelocity",30.f/180*3.141, "Maximal joint velocity in rad/sec");
defineRequiredProperty<std::string>("RobotFileName", "Robot file name, e.g. robot_model.xml");
- defineOptionalProperty<int>("CycleTime", 200, "Cycle time of the tcp control in ms");
+ defineOptionalProperty<int>("CycleTime", 50, "Cycle time of the tcp control in ms");
}
};
@@ -122,6 +122,7 @@ namespace armarx {
SharedRobotInterfacePrx remoteRobotPrx;
VirtualRobot::RobotPtr localRobot;
VirtualRobot::RobotPtr localReportRobot;
+ VirtualRobot::RobotPtr localVelReportRobot;
TCPControlUnitListenerPrx listener;
// VirtualRobot::RobotNodeSetPtr robotNodeSet;
// VirtualRobot::DifferentialIKPtr dIK;
--
GitLab