Skip to content
Snippets Groups Projects
Commit b46ab4b5 authored by David Schiebener's avatar David Schiebener
Browse files

added TCPControlUnit proxy for statecharts; removed spammy debug outputs

parent 906fc8e8
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
data/RobotAPI/VariantInfo-RobotAPI.xml
+ 8
− 0
View file @ b46ab4b5
......@@ -34,6 +34,14 @@
propertyName="ForceTorqueUnitObserverName"
propertyIsOptional="true"
propertyDefaultValue="ForceTorqueUnitObserver" />
<Proxy include="RobotAPI/interface/units/TCPControlUnit.h"
humanName="TCPControlUnit"
typeName="TCPControlUnitInterfacePrx"
memberName="tcpControlUnit"
getterName="getTCPControlUnit"
propertyName="TCPControlUnitName"
propertyIsOptional="true"
propertyDefaultValue="TCPControlUnit" />
<Proxy include="RobotAPI/interface/core/RobotState.h"
humanName="Robot State Component"
typeName="RobotStateComponentInterfacePrx"
......
source/RobotAPI/components/units/TCPControlUnit.cpp
+ 33
− 33
View file @ b46ab4b5
......@@ -271,10 +271,10 @@ namespace armarx
localDIKMap[itDIK->first] = itDIK->second;
}
//ARMARX_INFO << "RN TCP R pose1:" << localRobot->getRobotNode("TCP R")->getGlobalPose();
//ARMARX_DEBUG << "RN TCP R pose1:" << localRobot->getRobotNode("TCP R")->getGlobalPose();
RemoteRobot::synchronizeLocalClone(localRobot,robotStateComponentPrx);
//ARMARX_INFO << "RN TCP R pose2:" << localRobot->getRobotNode("TCP R")->getGlobalPose();
//ARMARX_DEBUG << "RN TCP R pose2:" << localRobot->getRobotNode("TCP R")->getGlobalPose();
calculationRunning = true;
......@@ -326,7 +326,7 @@ namespace armarx
if(data.translationVelocity && data.orientationVelocity)
{
mode = IKSolver::All;
// ARMARX_INFO << deactivateSpam(4) << "FullMode";
// ARMARX_DEBUG << deactivateSpam(4) << "FullMode";
}
else if(data.translationVelocity && !data.orientationVelocity)
mode = IKSolver::Position;
......@@ -344,7 +344,7 @@ namespace armarx
if(data.orientationVelocity)
{
//ARMARX_INFO << deactivateSpam(1) << "orientationVelocity before ChangeFrame: " << data.orientationVelocity->output();
//ARMARX_DEBUG << deactivateSpam(1) << "orientationVelocity before ChangeFrame: " << data.orientationVelocity->output();
data.orientationVelocity = FramedVector3::ChangeFrame(localRobot, *data.orientationVelocity, refFrame);
ARMARX_INFO << deactivateSpam(1) << "Orientation in " << refFrame << ": " << data.orientationVelocity->output();
Eigen::Matrix3f rot;
......@@ -440,7 +440,7 @@ namespace armarx
// const Eigen::Matrix3f rot = currentPose.block(0,0,3,3) * lastPose->toEigen().block(0,0,3,3).inverse();
// Eigen::Vector3f rpy;
// VirtualRobot::MathTools::eigen4f2rpy(mat, rpy);
// ARMARX_INFO << deactivateSpam(2) << "Current TCP Position: \n" << currentTCPPosition;
// ARMARX_DEBUG << deactivateSpam(2) << "Current TCP Position: \n" << currentTCPPosition;
// ARMARX_VERBOSE << deactivateSpam(2)<< "ActualTCPVelocity: " << (rpy/(0.001*cycleTime));
lastTCPPose = robotNodeSet->getTCP()->getGlobalPose();
......@@ -670,7 +670,7 @@ namespace armarx
for (unsigned int i=0; i<nodes.size();i++)
{
ARMARX_INFO_S << VAROUT(nodes[i]->getJointValue()) << VAROUT(dTheta[i]);
ARMARX_DEBUG << VAROUT(nodes[i]->getJointValue()) << VAROUT(dTheta[i]);
jv[i] = (nodes[i]->getJointValue() + dTheta[i]);
if (boost::math::isnan(jv[i]) || boost::math::isinf(jv[i]))
{
......@@ -686,7 +686,7 @@ namespace armarx
VectorXf newJointValues;
rns->getJointValues(newJointValues);
resultJointDelta = newJointValues - oldJointValues;
// ARMARX_INFO << "joint angle deltas:\n" << dThetaSum;
// ARMARX_DEBUG << "joint angle deltas:\n" << dThetaSum;
// check tolerances
if (checkTolerances())
......@@ -733,7 +733,7 @@ namespace armarx
}
robot->setJointValues(rns,oldJointValues + resultJointDelta);
// ARMARX_INFO << "best try: " << bestIndex << " with error: " << bestJVError;
// ARMARX_DEBUG << "best try: " << bestIndex << " with error: " << bestJVError;
// rns->setJointValues(oldJointValues);
// Matrix4f oldPose = rns->getTCP()->getGlobalPose();
// rns->setJointValues(oldJointValues+dThetaSum);
......@@ -766,10 +766,10 @@ namespace armarx
if (this->targets.find(tcp)!=this->targets.end())
{
Eigen::VectorXf delta = getDeltaToGoal(tcp);
//ARMARX_INFO_S << VAROUT(delta);
//ARMARX_DEBUG << VAROUT(delta);
IKSolver::CartesianSelection mode = this->modes[tcp];
MatrixXf partJacobian = this->getJacobianMatrix(tcp,mode);
//ARMARX_INFO_S << VAROUT(partJacobian);
//ARMARX_DEBUG << VAROUT(partJacobian);
Jacobian.block(index,0,partJacobian.rows(),nDoF) = partJacobian;
Vector3f position = delta.head(3);
......@@ -803,10 +803,10 @@ namespace armarx
}
// applyDOFWeightsToJacobian(Jacobian);
ARMARX_INFO_S << VAROUT(Jacobian);
ARMARX_DEBUG << VAROUT(Jacobian);
MatrixXf pseudo = computePseudoInverseJacobianMatrix(Jacobian);
ARMARX_INFO_S << VAROUT(Jacobian);
ARMARX_INFO_S << VAROUT(error);
ARMARX_DEBUG << VAROUT(Jacobian);
ARMARX_DEBUG << VAROUT(error);
return pseudo * error;
}
......@@ -842,7 +842,7 @@ namespace armarx
index = 0;
VectorXf partError(tcpDOF);
Vector3f position = delta.head(3);
// ARMARX_INFO_S << tcp->getName() << "'s error: " << position << " weighted error: " << position*stepSize;
// ARMARX_DEBUG << tcp->getName() << "'s error: " << position << " weighted error: " << position*stepSize;
position *=stepSize;
if (mode & IKSolver::X)
{
......@@ -866,10 +866,10 @@ namespace armarx
}
ARMARX_INFO << deactivateSpam(0.05) << "step size adjusted error to goal:\n" << partError;
ARMARX_DEBUG << deactivateSpam(0.05) << "step size adjusted error to goal:\n" << partError;
applyDOFWeightsToJacobian(partJacobian);
// ARMARX_INFO << "Jac:\n" << partJacobian;
// ARMARX_DEBUG << "Jac:\n" << partJacobian;
MatrixXf pseudo = computePseudoInverseJacobianMatrix(partJacobian);
......@@ -904,10 +904,10 @@ namespace armarx
Eigen::MatrixXf weightMat(tcpWeightVec.rows(), tcpWeightVec.rows());
weightMat.setIdentity();
weightMat.diagonal() = tcpWeightVec;
// ARMARX_INFO << /*deactivateSpam(1) <<*/ "tcpWeightVec:\n" << tcpWeightVec;
// ARMARX_INFO << /*deactivateSpam(1) << */"InvJac Before:\n" << invJacobian;
// ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "tcpWeightVec:\n" << tcpWeightVec;
// ARMARX_DEBUG << /*deactivateSpam(1) << */"InvJac Before:\n" << invJacobian;
pseudo = pseudo * weightMat;
// ARMARX_INFO << /*deactivateSpam(1) <<*/ "InvJac after:\n" << pseudo;
// ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "InvJac after:\n" << pseudo;
}
else
ARMARX_WARNING << deactivateSpam(3) << "Wrong dimension of tcp weights: " << tcpWeightVec.rows() << ", but should be: " << pseudo.cols();
......@@ -949,10 +949,10 @@ namespace armarx
Eigen::MatrixXf weightMat(dofWeights.rows(), dofWeights.rows());
weightMat.setIdentity();
weightMat.diagonal() = dofWeights;
// ARMARX_INFO << deactivateSpam(1) << "Jac before:\n" << Jacobian;
// ARMARX_INFO << deactivateSpam(1) << "Weights:\n" << weightMat;
// ARMARX_DEBUG << deactivateSpam(1) << "Jac before:\n" << Jacobian;
// ARMARX_DEBUG << deactivateSpam(1) << "Weights:\n" << weightMat;
Jacobian = Jacobian * weightMat;
// ARMARX_INFO << deactivateSpam(1) << "Jac after:\n" << Jacobian;
// ARMARX_DEBUG << deactivateSpam(1) << "Jac after:\n" << Jacobian;
}
}
......@@ -970,9 +970,9 @@ namespace armarx
Eigen::MatrixXf weightMat(tcpWeightVec.rows(), tcpWeightVec.rows());
weightMat.setIdentity();
weightMat.diagonal() = tcpWeightVec;
ARMARX_INFO << deactivateSpam(1) << "Jac Before: " << partJacobian;
ARMARX_DEBUG << deactivateSpam(1) << "Jac Before: " << partJacobian;
partJacobian = weightMat * partJacobian;
ARMARX_INFO << deactivateSpam(1) << "Jac after: " << partJacobian;
ARMARX_DEBUG << deactivateSpam(1) << "Jac after: " << partJacobian;
}
else
ARMARX_WARNING << deactivateSpam(3) << "Wrong dimension of tcp weights: " << tcpWeightVec.rows() << ", but should be: " << partJacobian.rows();
......@@ -1021,10 +1021,10 @@ namespace armarx
Eigen::MatrixXf weightMat(tcpWeightVec.rows(), tcpWeightVec.rows());
weightMat.setIdentity();
weightMat.diagonal() = tcpWeightVec;
// ARMARX_INFO << /*deactivateSpam(1) <<*/ "tcpWeightVec:\n" << tcpWeightVec;
// ARMARX_INFO << /*deactivateSpam(1) << */"InvJac Before:\n" << invJacobian;
// ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "tcpWeightVec:\n" << tcpWeightVec;
// ARMARX_DEBUG << /*deactivateSpam(1) << */"InvJac Before:\n" << invJacobian;
invJacobian = invJacobian * weightMat;
ARMARX_INFO << /*deactivateSpam(1) <<*/ "InvJac after:\n" << invJacobian;
ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "InvJac after:\n" << invJacobian;
}
else
ARMARX_WARNING << deactivateSpam(3) << "Wrong dimension of tcp weights: " << tcpWeightVec.rows() << ", but should be: " << invJacobian.cols();
......@@ -1061,17 +1061,17 @@ namespace armarx
int weightIndex = 0;
if (modes[tcp] & IKSolver::X)
{
// ARMARX_INFO << "error x: " << position(0)*tcpWeight(weightIndex);
// ARMARX_DEBUG << "error x: " << position(0)*tcpWeight(weightIndex);
result += position(0) * position(0) * tcpWeight(weightIndex++);
}
if (modes[tcp] & IKSolver::Y)
{
// ARMARX_INFO << "error y: " << position(1)*tcpWeight(weightIndex);
// ARMARX_DEBUG << "error y: " << position(1)*tcpWeight(weightIndex);
result += position(1) * position(1) * tcpWeight(weightIndex++);
}
if (modes[tcp] & IKSolver::Z)
{
// ARMARX_INFO << "error z: " << position(2)*tcpWeight(weightIndex);
// ARMARX_DEBUG << "error z: " << position(2)*tcpWeight(weightIndex);
result += position(2) * position(2) * tcpWeight(weightIndex++);
}
......@@ -1127,7 +1127,7 @@ void armarx::TCPControlUnit::reportJointAngles(const NameValueMap &jointAngles,
tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, localReportRobot->getName());
}
// ARMARX_INFO << deactivateSpam(5) << "TCP Pose Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
// ARMARX_DEBUG << deactivateSpam(5) << "TCP Pose Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
listener->reportTCPPose(tcpPoses);
}
......@@ -1139,7 +1139,7 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap &jointVel,
ScopedLock lock(reportMutex);
if(!localVelReportRobot)
localVelReportRobot = localReportRobot->clone(localReportRobot->getName());
// ARMARX_INFO << jointVel; FramedPoseBaseMap tcpPoses;
// ARMARX_DEBUG << jointVel; FramedPoseBaseMap tcpPoses;
FramedVector3Map tcpTranslationVelocities;
FramedVector3Map tcpOrientationVelocities;
std::string rootFrame = localReportRobot->getRootNode()->getName();
......@@ -1185,7 +1185,7 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap &jointVel,
}
// ARMARX_INFO << deactivateSpam(5) << "TCP Pose Vel Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
// ARMARX_DEBUG << deactivateSpam(5) << "TCP Pose Vel Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
listener->reportTCPVelocities(tcpTranslationVelocities, tcpOrientationVelocities);
}
......
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