Skip to content
Snippets Groups Projects
Commit 6755fe9c authored by Mirko Wächter's avatar Mirko Wächter
Browse files

TCP Observer now reports tcp velocities

parent 81c0e9e8
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/units/TCPControlUnit.cpp
+ 118
− 59
View file @ 6755fe9c
......@@ -32,6 +32,8 @@
#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <Eigen/Core>
using namespace VirtualRobot;
using namespace Eigen;
......@@ -52,26 +54,28 @@ namespace armarx
{
usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
usingProxy("RobotStateComponent");
usingProxy("DebugObserver");
offeringTopic("TCPControlUnitState");
cycleTime = getProperty<int>("CycleTime").getValue();
if(getProperty<float>("MaxJointVelocity").isSet())
maxJointVelocity = getProperty<float>("MaxJointVelocity").getValue();
}
void TCPControlUnit::onConnectComponent()
{
ScopedLock lock(dataMutex);
if(getProperty<float>("MaxJointVelocity").isSet())
maxJointVelocity = getProperty<float>("MaxJointVelocity").getValue();
debugObs = getProxy<DebugObserverInterfacePrx>("DebugObserver");
kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
// retrieve Jacobian pseudo inverse for TCP and chain
remoteRobotPrx = robotStateComponentPrx->getSynchronizedRobot();
// localRobot.reset(new RemoteRobot(remoteRobotPrx));
// ARMARX_INFO << "hi limit : " << localRobot->getRobotNode("LeftArm_Joint1")->getJointLimitHi();
// localRobot = localRobot->clone("localRobot");
// ARMARX_INFO << "hi limit after: " << localRobot->getRobotNode("LeftArm_Joint1")->getJointLimitHi();
std::string robotFile = getProperty<std::string>("RobotFileName").getValue();
if (!ArmarXDataPath::getAbsolutePath(robotFile,robotFile))
......@@ -108,7 +112,7 @@ namespace armarx
requested = false;
if(execTask)
execTask->stop();
execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, cycleTime, true, "TCPVelocityCalculator");
execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
execTask->start();
}
......@@ -201,7 +205,7 @@ namespace armarx
if(execTask)
execTask->stop();
lastReportTime = IceUtil::Time::now();
execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, cycleTime, true, "TCPVelocityCalculator");
execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
execTask->start();
ARMARX_IMPORTANT << "Requested TCPControlUnit";
}
......@@ -217,45 +221,114 @@ namespace armarx
}
void TCPControlUnit::periodicExec()
{
// ARMARX_INFO << "SPAM";
// ARMARX_INFO << deactivateSpam(4) << "periodicExec for " << tcpVelocitiesMap.size();
// IceUtil::Time startTime = IceUtil::Time::now();
ScopedLock lock(dataMutex);
double tDelta = (IceUtil::Time::now() - lastReportTime).toSecondsDouble();
lastReportTime = IceUtil::Time::now();
NameValueMap robotConfigMap = remoteRobotPrx->getConfig();
// NameValueMap::iterator it = robotConfigMap.begin();
std::map<std::string, float> jointValues(robotConfigMap.begin(), robotConfigMap.end());
localRobot->setJointValues(jointValues);
periodicReport();
periodicReport(tDelta);
if(!requested)
return;
calcAndSetVelocities();
}
void TCPControlUnit::periodicReport(double tDelta)
{
std::vector<RobotNodeSetPtr > nodeSets = localRobot->getRobotNodeSets();
FramedPoseBaseMap tcpPoses;
FramedVector3Map tcpTranslationVelocities;
FramedVector3Map tcpOrientationVelocities;
std::string rootFrame = localRobot->getRootNode()->getName();
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 &currentPose = nodeSet->getTCP()->getGlobalPose();
tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame);
FramedPoseBaseMap::iterator it = lastTCPPoses.find(tcpName);
if(it != lastTCPPoses.end())
{
FramedPosePtr lastPose = FramedPosePtr::dynamicCast(it->second);
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);
it->second = new FramedPose(currentPose, rootFrame);
}
}
ARMARX_INFO << deactivateSpam(5) << "TCP Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
listener->begin_reportTCPPose(tcpPoses);
listener->begin_reportTCPVelocities(tcpTranslationVelocities, tcpOrientationVelocities);
lastTCPPoses = tcpPoses;
}
void TCPControlUnit::calcAndSetVelocities()
{
TCPVelocityDataMap::iterator it = tcpVelocitiesMap.begin();
for(; it != tcpVelocitiesMap.end(); it++)
{
const TCPVelocityData& data = it->second;
RobotNodeSetPtr nodeSet = localRobot->getRobotNodeSet(data.nodeSetName);
std::string refFrame = nodeSet->getTCP()->getName();
// std::string refFrame = nodeSet->getTCP()->getName();
DIKMap::iterator itDIK = dIKMap.find(data.nodeSetName);
if(itDIK == dIKMap.end())
dIKMap[data.nodeSetName].reset(new EDifferentialIK(nodeSet, localRobot->getRootNode()/*, VirtualRobot::JacobiProvider::eTranspose*/));
EDifferentialIKPtr dIk = boost::shared_dynamic_cast<EDifferentialIK>(dIKMap[data.nodeSetName]);
dIk->clearGoals();
// Eigen::VectorXf dofWeights(nodeSet->getSize());
// dofWeights.setOnes();
// dofWeights(0) = 0.2;
// dofWeights(1) = 0.0;
// dIk->setDOFWeights(dofWeights);
// Eigen::VectorXf tcpWeights(3);
// tcpWeights.setOnes();
// tcpWeights(0) = 0.2;
// tcpWeights(2) = 0.3;
// dIk->setTCPWeights(tcpWeights);
}
using namespace Eigen;
......@@ -265,7 +338,7 @@ namespace armarx
{
TCPVelocityData& data = it->second;
RobotNodeSetPtr nodeSet = localRobot->getRobotNodeSet(data.nodeSetName);
// RobotNodeSetPtr nodeSet = localRobot->getRobotNodeSet(data.nodeSetName);
std::string refFrame = localRobot->getRootNode()->getName();
IKSolver::CartesianSelection mode;
if(data.translationVelocity && data.orientationVelocity)
......@@ -389,49 +462,35 @@ namespace armarx
kinematicUnitPrx->switchControlMode(controlModes);
kinematicUnitPrx->setJointVelocities(targetVelocities);
}
void TCPControlUnit::periodicReport()
{
// ScopedLock lock(dataMutex);
// NameValueMap robotConfigMap = remoteRobotPrx->getConfig();
//// NameValueMap::iterator it = robotConfigMap.begin();
// std::map<std::string, float> jointValues(robotConfigMap.begin(), robotConfigMap.end());
// localRobot->setJointValues(jointValues);
std::vector<RobotNodeSetPtr > nodeSets = localRobot->getRobotNodeSets();
FramedPoseBaseMap tcpPoses;
FramedVector3Map tcpTranslationVelocities;
FramedVector3Map tcpOrientationVelocities;
std::string rootFrame = localRobot->getRootNode()->getName();
double timeDelta = (IceUtil::Time::now() - lastReportTime).toSecondsDouble();
for(unsigned int i=0; i < nodeSets.size(); i++)
void TCPControlUnit::ContinuousAngles(const Eigen::AngleAxisf& oldAngle, Eigen::AngleAxisf& newAngle, double & offset)
{
// if(oldAngle.axis().isApprox(newAngle.axis()*-1))
const Eigen::Vector3f& v1 = oldAngle.axis();
const Eigen::Vector3f& v2 = newAngle.axis();
const Eigen::Vector3f& v2i = newAngle.axis()*-1;
double angle = acos(v1.dot(v2) / (v1.norm() * v2.norm()));
double angleInv = acos(v1.dot(v2i) / (v1.norm() * v2i.norm()));
// std::cout << "angle1: " << angle << std::endl;
// std::cout << "angleInv: " << angleInv << std::endl;
// Eigen::AngleAxisd result;
if(angle > angleInv)
{
RobotNodeSetPtr nodeSet = nodeSets.at(i);
const std::string &tcpName = nodeSet->getTCP()->getName();
const Eigen::Matrix4f &currentPose = nodeSet->getTCP()->getGlobalPose();
tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame);
FramedPoseBaseMap::iterator it = lastTCPPoses.find(tcpName);
if(it != lastTCPPoses.end())
{
ARMARX_IMPORTANT_S << "inversion needed" << std::endl;
newAngle = Eigen::AngleAxisf(2.0*M_PI - newAngle.angle(), newAngle.axis()*-1);
}
// else newAngle = newAngle;
FramedPosePtr lastPose = FramedPosePtr::dynamicCast(it->second);
tcpTranslationVelocities[tcpName] = new FramedVector3((currentPose.block(0,3,3,1) - lastPose->toEigen().block(0,3,3,1))/timeDelta, rootFrame);
if(fabs(newAngle.angle()+offset - oldAngle.angle()) > fabs(newAngle.angle()+offset - (oldAngle.angle() + M_PI*2)) )
offset -= M_PI*2;
else if(fabs(newAngle.angle()+offset - oldAngle.angle()) > fabs((newAngle.angle()+M_PI*2+offset) - oldAngle.angle()) )
offset += M_PI*2;
newAngle.angle() += offset;
const Eigen::Matrix3f rot = currentPose.block(0,0,3,3) * lastPose->toEigen().block(0,0,3,3).inverse();
Eigen::AngleAxis<float> orient(rot.block<3,3>(0,0));
tcpOrientationVelocities[tcpName] = new FramedVector3(orient.axis() * (orient.angle()/timeDelta), rootFrame);
it->second = new FramedPose(currentPose, rootFrame);
}
}
listener->reportTCPPose(tcpPoses);
listener->reportTCPVelocities(tcpTranslationVelocities, tcpOrientationVelocities);
lastTCPPoses = tcpPoses;
lastReportTime = IceUtil::Time::now();
}
......
source/RobotAPI/units/TCPControlUnit.h
+ 8
− 1
View file @ 6755fe9c
......@@ -31,6 +31,7 @@
#include <VirtualRobot/IK/DifferentialIK.h>
#include <Core/robotstate/remote/RemoteRobot.h>
#include <Core/interface/observers/ObserverInterface.h>
namespace armarx {
......@@ -45,6 +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");
}
};
......@@ -83,9 +85,11 @@ namespace armarx {
static Eigen::VectorXf CalcNullspaceJointDeltas(const Eigen::VectorXf &desiredJointDeltas, const Eigen::MatrixXf &jacobian, const Eigen::MatrixXf &jacobianInverse);
static Eigen::VectorXf CalcJointLimitAvoidanceDeltas(VirtualRobot::RobotNodeSetPtr robotNodeSet, const Eigen::MatrixXf &jacobian, const Eigen::MatrixXf &jacobianInverse, Eigen::VectorXf desiredJointValues = Eigen::VectorXf());
void calcAndSetVelocities();
private:
static void ContinuousAngles(const Eigen::AngleAxisf &oldAngle, Eigen::AngleAxisf &newAngle, double & offset);
void periodicExec();
void periodicReport();
void periodicReport(double tDelta);
Eigen::VectorXf calcJointVelocities(VirtualRobot::RobotNodeSetPtr robotNodeSet, Eigen::VectorXf tcpDelta, VirtualRobot::RobotNodePtr refFrame, VirtualRobot::IKSolver::CartesianSelection mode);
Eigen::VectorXf applyMaxJointVelocity(const Eigen::VectorXf &jointVelocity, float maxJointVelocity);
......@@ -119,13 +123,16 @@ namespace armarx {
TCPControlUnitListenerPrx listener;
// VirtualRobot::RobotNodeSetPtr robotNodeSet;
// VirtualRobot::DifferentialIKPtr dIK;
DebugObserverInterfacePrx debugObs;
bool requested;
std::map<std::string, double> oriOffsets;
FramedPoseBaseMap lastTCPPoses;
IceUtil::Time lastReportTime;
Mutex dataMutex;
Mutex taskMutex;
......
source/RobotAPI/units/TCPControlUnitObserver.cpp
+ 18
− 16
View file @ 6755fe9c
......@@ -125,32 +125,34 @@ namespace armarx {
if(itOri != tcpOrientationVelocities.end())
vecOri = FramedVector3Ptr::dynamicCast(itOri->second);
const std::string &tcpName = it->first;
ARMARX_CHECK_EXPRESSION(vec->frame == vecOri->frame);
if(!existsDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName))
offerDataFieldWithDefault(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
else
setDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second));
if(!existsChannel(tcpName))
const std::string channelName = tcpName+"Velocities";
if(!existsChannel(channelName))
{
offerChannel(tcpName, "pose components of " + tcpName);
offerDataFieldWithDefault(tcpName,"x", Variant(vec->x), "X axis");
offerDataFieldWithDefault(tcpName,"y", Variant(vec->y), "Y axis");
offerDataFieldWithDefault(tcpName,"z", Variant(vec->z), "Z axis");
offerDataFieldWithDefault(tcpName,"roll", Variant(vecOri->x), "Roll");
offerDataFieldWithDefault(tcpName,"pitch", Variant(vecOri->y), "Pitch");
offerDataFieldWithDefault(tcpName,"yaw", Variant(vecOri->z), "Yaw");
offerDataFieldWithDefault(tcpName,"frame", Variant(vecOri->frame), "Reference Frame");
offerChannel(channelName, "pose components of " + tcpName);
offerDataFieldWithDefault(channelName,"x", Variant(vec->x), "X axis");
offerDataFieldWithDefault(channelName,"y", Variant(vec->y), "Y axis");
offerDataFieldWithDefault(channelName,"z", Variant(vec->z), "Z axis");
offerDataFieldWithDefault(channelName,"roll", Variant(vecOri->x), "Roll");
offerDataFieldWithDefault(channelName,"pitch", Variant(vecOri->y), "Pitch");
offerDataFieldWithDefault(channelName,"yaw", Variant(vecOri->z), "Yaw");
offerDataFieldWithDefault(channelName,"frame", Variant(vecOri->frame), "Reference Frame");
}
else
{
setDataField(tcpName,"x", Variant(vec->x));
setDataField(tcpName,"y", Variant(vec->y));
setDataField(tcpName,"z", Variant(vec->z));
setDataField(tcpName,"roll", Variant(vecOri->x));
setDataField(tcpName,"pitch", Variant(vecOri->y));
setDataField(tcpName,"yaw", Variant(vecOri->z));
setDataField(tcpName,"frame", Variant(vec->frame));
setDataField(channelName,"x", Variant(vec->x));
setDataField(channelName,"y", Variant(vec->y));
setDataField(channelName,"z", Variant(vec->z));
setDataField(channelName,"roll", Variant(vecOri->x));
setDataField(channelName,"pitch", Variant(vecOri->y));
setDataField(channelName,"yaw", Variant(vecOri->z));
setDataField(channelName,"frame", Variant(vec->frame));
}
}
......
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