From 47ff66dc3f2cab7193d626819824cfcd1044a8a2 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <waechter@kit.edu>
Date: Fri, 14 Mar 2014 11:34:30 +0100
Subject: [PATCH] TCP Velocities half fixed
---
interface/slice/units/TCPControlUnit.ice | 2 +-
source/RobotAPI/units/TCPControlUnit.cpp | 172 ++++++++++++++++++++---
source/RobotAPI/units/TCPControlUnit.h | 19 ++-
3 files changed, 170 insertions(+), 23 deletions(-)
diff --git a/interface/slice/units/TCPControlUnit.ice b/interface/slice/units/TCPControlUnit.ice
index 6b7517f5f..f3db320fb 100644
--- a/interface/slice/units/TCPControlUnit.ice
+++ b/interface/slice/units/TCPControlUnit.ice
@@ -35,7 +35,7 @@
module armarx
{
- interface TCPControlUnitInterface extends armarx::SensorActorUnitInterface
+ interface TCPControlUnitInterface extends armarx::SensorActorUnitInterface, armarx::KinematicUnitListener
{
void setCycleTime(int milliseconds);
// void setTCPVelocity(string robotNodeSetName, FramedVector3Base translationVelocity, FramedVector3Base orientationVelocityRPY);
diff --git a/source/RobotAPI/units/TCPControlUnit.cpp b/source/RobotAPI/units/TCPControlUnit.cpp
index 382f3af2d..be236d187 100644
--- a/source/RobotAPI/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/units/TCPControlUnit.cpp
@@ -52,10 +52,12 @@ namespace armarx
void TCPControlUnit::onInitComponent()
{
+ topicName = getName() + "State";
usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
usingProxy("RobotStateComponent");
usingProxy("DebugObserver");
- offeringTopic("TCPControlUnitState");
+ offeringTopic(topicName);
+ usingTopic("RobotState");
cycleTime = getProperty<int>("CycleTime").getValue();
if(getProperty<float>("MaxJointVelocity").isSet())
@@ -85,6 +87,7 @@ namespace armarx
localRobot = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);//localRobot->clone("LocalRobot");
localRobot->setUpdateVisualization(false);
localRobot->setThreadsafe(false);
+ localReportRobot = localRobot->clone("reportRobot");
std::vector<RobotNodePtr> nodes = localRobot->getRobotNodes();
for(unsigned int i=0; i < nodes.size(); i++)
{
@@ -105,7 +108,7 @@ namespace armarx
// setTCPVelocity("LeftLeg", tcpVel, NULL);
// request();
- listener = getTopic<TCPControlUnitListenerPrx>("TCPControlUnitState");
+ listener = getTopic<TCPControlUnitListenerPrx>(topicName);
// topicTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicReport,cycleTime, false, "TCPDataProvider");
// topicTask->start();
@@ -241,7 +244,7 @@ namespace armarx
localRobot->setJointValues(jointValues);
- periodicReport(tDelta);
+// periodicReport(tDelta);
if(!requested)
return;
@@ -278,27 +281,27 @@ namespace armarx
const Eigen::Matrix3f rot = currentPose.block(0,0,3,3) * lastPose->toEigen().block(0,0,3,3).inverse();
Eigen::AngleAxisf orient(rot.block<3,3>(0,0));
- Eigen::Matrix3f currentRot = currentPose.block(0,0,3,3);
- Eigen::AngleAxisf currentAA(currentRot);
- Eigen::Quaternionf quat(currentAA);
+// Eigen::Matrix3f currentRot = currentPose.block(0,0,3,3);
+// Eigen::AngleAxisf currentAA(currentRot);
+// Eigen::Quaternionf quat(currentAA);
// debugObs->begin_setDebugDatafield(tcpName+ "rotation","w", new Variant(quat.w()));
// debugObs->begin_setDebugDatafield(tcpName+ "rotation","x", new Variant(quat.x()));
// debugObs->begin_setDebugDatafield(tcpName+ "rotation","y", new Variant(quat.y()));
// debugObs->begin_setDebugDatafield(tcpName+ "rotation","z", new Variant(quat.z()));
- debugObs->begin_setDebugDatafield(tcpName+ "rotation","w", new Variant(currentAA.angle()));
- debugObs->begin_setDebugDatafield(tcpName+ "rotation","x", new Variant(currentAA.axis()[0]));
- debugObs->begin_setDebugDatafield(tcpName+ "rotation","y", new Variant(currentAA.axis()[1]));
- debugObs->begin_setDebugDatafield(tcpName+ "rotation","z", new Variant(currentAA.axis()[2]));
- if(oriOffsets.find(tcpName) == oriOffsets.end())
- oriOffsets[tcpName] = 0.0;
- double& offset = oriOffsets[tcpName];
- ContinuousAngles(Eigen::AngleAxisf(lastPose->toEigen().block<3,3>(0,0)),currentAA, offset);
- Eigen::Quaternionf quatFixed(currentAA);
-
- debugObs->begin_setDebugDatafield(tcpName+ "rotation","wFixed", new Variant(currentAA.angle()));
- debugObs->begin_setDebugDatafield(tcpName+ "rotation","xFixed", new Variant(currentAA.axis()[0]));
- debugObs->begin_setDebugDatafield(tcpName+ "rotation","yFixed", new Variant(currentAA.axis()[1]));
- debugObs->begin_setDebugDatafield(tcpName+ "rotation","zFixed", new Variant(currentAA.axis()[2]));
+// debugObs->begin_setDebugDatafield(tcpName+ "rotation","w", new Variant(currentAA.angle()));
+// debugObs->begin_setDebugDatafield(tcpName+ "rotation","x", new Variant(currentAA.axis()[0]));
+// debugObs->begin_setDebugDatafield(tcpName+ "rotation","y", new Variant(currentAA.axis()[1]));
+// debugObs->begin_setDebugDatafield(tcpName+ "rotation","z", new Variant(currentAA.axis()[2]));
+// if(oriOffsets.find(tcpName) == oriOffsets.end())
+// oriOffsets[tcpName] = 0.0;
+// double& offset = oriOffsets[tcpName];
+// ContinuousAngles(Eigen::AngleAxisf(lastPose->toEigen().block<3,3>(0,0)),currentAA, offset);
+// Eigen::Quaternionf quatFixed(currentAA);
+
+// debugObs->begin_setDebugDatafield(tcpName+ "rotation","wFixed", new Variant(currentAA.angle()));
+// debugObs->begin_setDebugDatafield(tcpName+ "rotation","xFixed", new Variant(currentAA.axis()[0]));
+// debugObs->begin_setDebugDatafield(tcpName+ "rotation","yFixed", new Variant(currentAA.axis()[1]));
+// debugObs->begin_setDebugDatafield(tcpName+ "rotation","zFixed", new Variant(currentAA.axis()[2]));
// debugObs->begin_setDebugDatafield(tcpName+ "rotation","wFixed", new Variant(quatFixed.w()));
@@ -480,7 +483,7 @@ namespace armarx
// Eigen::AngleAxisd result;
if(angle > angleInv)
{
- ARMARX_IMPORTANT_S << "inversion needed" << std::endl;
+// ARMARX_IMPORTANT_S << "inversion needed" << std::endl;
newAngle = Eigen::AngleAxisf(2.0*M_PI - newAngle.angle(), newAngle.axis()*-1);
}
// else newAngle = newAngle;
@@ -1111,3 +1114,130 @@ namespace armarx
}
+
+
+void armarx::TCPControlUnit::reportControlModeChanged(const NameControlModeMap &, bool, const Ice::Current &)
+{
+}
+
+void armarx::TCPControlUnit::reportJointAngles(const NameValueMap &jointAngles, bool, const Ice::Current &)
+{
+ ScopedLock lock(reportMutex);
+ std::vector<RobotNodeSetPtr > nodeSets = localReportRobot->getRobotNodeSets();
+ FramedPoseBaseMap tcpPoses;
+
+ std::string rootFrame = localReportRobot->getRootNode()->getName();
+ localReportRobot->setJointValues(jointAngles);
+ IceUtil::Time start = IceUtil::Time::now();
+ for(unsigned int i=0; i < nodeSets.size(); i++)
+ {
+ RobotNodeSetPtr nodeSet = nodeSets.at(i);
+ const std::string &tcpName = nodeSet->getTCP()->getName();
+ const Eigen::Matrix4f ¤tPose = nodeSet->getTCP()->getGlobalPose();
+ tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame);
+
+ }
+ ARMARX_INFO << deactivateSpam(5) << "TCP Pose Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
+ listener->reportTCPPose(tcpPoses);
+}
+
+void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap &jointVel, bool, const Ice::Current &)
+{
+ if((IceUtil::Time::now()-lastTopicReportTime).toMilliSeconds() < cycleTime )
+ return;
+ lastTopicReportTime = IceUtil::Time::now();
+ ScopedLock lock(reportMutex);
+// ARMARX_INFO << jointVel;
+ std::vector<RobotNodeSetPtr > nodeSets = localReportRobot->getRobotNodeSets();
+ FramedPoseBaseMap tcpPoses;
+ FramedVector3Map tcpTranslationVelocities;
+ FramedVector3Map tcpOrientationVelocities;
+ std::string rootFrame = localReportRobot->getRootNode()->getName();
+ NameValueMap tempJointAngles = localReportRobot->getConfig()->getRobotNodeJointValueMap();
+
+ for(unsigned int i=0; i < nodeSets.size(); i++)
+ {
+ RobotNodeSetPtr nodeSet = nodeSets.at(i);
+ const std::string &tcpName = nodeSet->getTCP()->getName();
+ const Eigen::Matrix4f ¤tPose = nodeSet->getTCP()->getGlobalPose();
+ tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame);
+
+ }
+
+ double tDelta = 0.001;
+ for(NameValueMap::iterator it = tempJointAngles.begin(); it != tempJointAngles.end(); it++)
+ {
+ NameValueMap::const_iterator itSrc = jointVel.find(it->first);
+ if(itSrc != jointVel.end())
+ it->second += it->second * tDelta;
+ }
+ VirtualRobot::RobotPtr localVelRobot = localReportRobot->clone("VelRobot");
+ nodeSets = localVelRobot->getRobotNodeSets();
+ localVelRobot->setJointValues(tempJointAngles);
+ IceUtil::Time start = IceUtil::Time::now();
+ for(unsigned int i=0; i < nodeSets.size(); i++)
+ {
+ RobotNodeSetPtr nodeSet = nodeSets.at(i);
+ const std::string &tcpName = nodeSet->getTCP()->getName();
+ const Eigen::Matrix4f ¤tPose = nodeSet->getTCP()->getGlobalPose();
+
+
+
+// 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();
+ Eigen::AngleAxisf orient(rot.block<3,3>(0,0));
+ // Eigen::Matrix3f currentRot = currentPose.block(0,0,3,3);
+ // Eigen::AngleAxisf currentAA(currentRot);
+ // Eigen::Quaternionf quat(currentAA);
+ // debugObs->begin_setDebugDatafield(tcpName+ "rotation","w", new Variant(quat.w()));
+ // debugObs->begin_setDebugDatafield(tcpName+ "rotation","x", new Variant(quat.x()));
+ // debugObs->begin_setDebugDatafield(tcpName+ "rotation","y", new Variant(quat.y()));
+ // debugObs->begin_setDebugDatafield(tcpName+ "rotation","z", new Variant(quat.z()));
+ // debugObs->begin_setDebugDatafield(tcpName+ "rotation","w", new Variant(currentAA.angle()));
+ // debugObs->begin_setDebugDatafield(tcpName+ "rotation","x", new Variant(currentAA.axis()[0]));
+ // debugObs->begin_setDebugDatafield(tcpName+ "rotation","y", new Variant(currentAA.axis()[1]));
+ // debugObs->begin_setDebugDatafield(tcpName+ "rotation","z", new Variant(currentAA.axis()[2]));
+ // if(oriOffsets.find(tcpName) == oriOffsets.end())
+ // oriOffsets[tcpName] = 0.0;
+ // double& offset = oriOffsets[tcpName];
+ // ContinuousAngles(Eigen::AngleAxisf(lastPose->toEigen().block<3,3>(0,0)),currentAA, offset);
+ // Eigen::Quaternionf quatFixed(currentAA);
+
+ // debugObs->begin_setDebugDatafield(tcpName+ "rotation","wFixed", new Variant(currentAA.angle()));
+ // debugObs->begin_setDebugDatafield(tcpName+ "rotation","xFixed", new Variant(currentAA.axis()[0]));
+ // debugObs->begin_setDebugDatafield(tcpName+ "rotation","yFixed", new Variant(currentAA.axis()[1]));
+ // debugObs->begin_setDebugDatafield(tcpName+ "rotation","zFixed", new Variant(currentAA.axis()[2]));
+
+
+ // debugObs->begin_setDebugDatafield(tcpName+ "rotation","wFixed", new Variant(quatFixed.w()));
+ // debugObs->begin_setDebugDatafield(tcpName+ "rotation","xFixed", new Variant(quatFixed.x()));
+ // debugObs->begin_setDebugDatafield(tcpName+ "rotation","yFixed", new Variant(quatFixed.y()));
+ // debugObs->begin_setDebugDatafield(tcpName+ "rotation","zFixed", new Variant(quatFixed.z()));
+ tcpOrientationVelocities[tcpName] = new FramedVector3(orient.axis() * (orient.angle()/tDelta), rootFrame);
+
+
+ }
+ ARMARX_INFO << deactivateSpam(5) << "TCP Pose Vel Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
+ listener->reportTCPVelocities(tcpTranslationVelocities, tcpOrientationVelocities);
+
+}
+
+void armarx::TCPControlUnit::reportJointTorques(const NameValueMap &, bool, const Ice::Current &)
+{
+}
+
+void armarx::TCPControlUnit::reportJointCurrents(const NameValueMap &, bool, const Ice::Current &)
+{
+}
+
+void armarx::TCPControlUnit::reportJointMotorTemperatures(const NameValueMap &, bool, const Ice::Current &)
+{
+}
+
+void armarx::TCPControlUnit::reportJointStatuses(const NameStatusMap &, bool, const Ice::Current &)
+{
+}
diff --git a/source/RobotAPI/units/TCPControlUnit.h b/source/RobotAPI/units/TCPControlUnit.h
index abc6939c2..786f8ec4c 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", 20, "Cycle time of the tcp control in ms");
+ defineOptionalProperty<int>("CycleTime", 200, "Cycle time of the tcp control in ms");
}
};
@@ -54,6 +54,7 @@ namespace armarx {
class TCPControlUnit :
virtual public Component,
virtual public TCPControlUnitInterface
+
{
public:
TCPControlUnit();
@@ -120,6 +121,7 @@ namespace armarx {
KinematicUnitInterfacePrx kinematicUnitPrx;
SharedRobotInterfacePrx remoteRobotPrx;
VirtualRobot::RobotPtr localRobot;
+ VirtualRobot::RobotPtr localReportRobot;
TCPControlUnitListenerPrx listener;
// VirtualRobot::RobotNodeSetPtr robotNodeSet;
// VirtualRobot::DifferentialIKPtr dIK;
@@ -131,10 +133,25 @@ namespace armarx {
FramedPoseBaseMap lastTCPPoses;
IceUtil::Time lastReportTime;
+ IceUtil::Time lastTopicReportTime;
Mutex dataMutex;
Mutex taskMutex;
+ Mutex reportMutex;
+ std::string topicName;
+
+
+
+ // KinematicUnitListener interface
+ public:
+ void reportControlModeChanged(const NameControlModeMap &, bool, const Ice::Current &);
+ void reportJointAngles(const NameValueMap &jointAngles, bool, const Ice::Current &);
+ void reportJointVelocities(const NameValueMap &jointVel, bool, const Ice::Current &);
+ void reportJointTorques(const NameValueMap &, bool, const Ice::Current &);
+ void reportJointCurrents(const NameValueMap &, bool, const Ice::Current &);
+ void reportJointMotorTemperatures(const NameValueMap &, bool, const Ice::Current &);
+ void reportJointStatuses(const NameStatusMap &, bool, const Ice::Current &);
};
--
GitLab