diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index 9681636da9a820a53711ed50f327205216a0fcc8..c34d94957311c775b378bcda7e0dda17f38fe1c3 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -24,6 +24,7 @@
#include "CartesianPositionController.h"
#include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
namespace armarx
{
@@ -35,12 +36,14 @@ namespace armarx
Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const
{
+ ARMARX_TRACE;
int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
Eigen::VectorXf cartesianVel(posLen + oriLen);
if (posLen)
{
+ ARMARX_TRACE;
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
Eigen::Vector3f currentPos = tcp->getPositionInFrame(referenceFrame);
Eigen::Vector3f errPos = targetPos - currentPos;
@@ -54,6 +57,7 @@ namespace armarx
if (oriLen)
{
+ ARMARX_TRACE;
Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
Eigen::Matrix3f tcpOri = tcp->getPoseInFrame(referenceFrame).block<3, 3>(0, 0);
Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
@@ -72,12 +76,14 @@ namespace armarx
Eigen::VectorXf CartesianPositionController::calculatePos(const Eigen::Vector3f& targetPos) const
{
+ ARMARX_TRACE;
Eigen::VectorXf cartesianVel(3);
Eigen::Vector3f currentPos = tcp->getPositionInFrame(referenceFrame);
Eigen::Vector3f errPos = targetPos - currentPos;
Eigen::Vector3f posVel = errPos * KpPos;
if (maxPosVel > 0)
{
+ ARMARX_TRACE;
posVel = math::MathUtils::LimitTo(posVel, maxPosVel);
}
cartesianVel.block<3, 1>(0, 0) = posVel;
@@ -98,6 +104,7 @@ namespace armarx
const VirtualRobot::RobotNodePtr& tcp,
const VirtualRobot::RobotNodePtr& referenceFrame)
{
+ ARMARX_TRACE;
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
Eigen::Vector3f tcpPos = referenceFrame ? tcp->getPositionInFrame(referenceFrame) : tcp->getPositionInRootFrame();
Eigen::Vector3f errPos = targetPos - tcpPos;
@@ -108,6 +115,7 @@ namespace armarx
const VirtualRobot::RobotNodePtr& tcp,
const VirtualRobot::RobotNodePtr& referenceFrame)
{
+ ARMARX_TRACE;
Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
Eigen::Matrix4f tcpPose = referenceFrame ? tcp->getPoseInFrame(referenceFrame) : tcp->getPoseInRootFrame();
Eigen::Matrix3f tcpOri = tcpPose.block<3, 3>(0, 0);
@@ -129,6 +137,7 @@ namespace armarx
bool CartesianPositionController::reached(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode, float thresholdPosReached, float thresholdOriReached)
{
+ ARMARX_TRACE;
if (mode & VirtualRobot::IKSolver::Position)
{
if (GetPositionError(targetPose, tcp, referenceFrame) > thresholdPosReached)
@@ -148,6 +157,7 @@ namespace armarx
Eigen::Vector3f CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose) const
{
+ ARMARX_TRACE;
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
return targetPos - tcp->getPositionInFrame(referenceFrame);
}
@@ -159,6 +169,7 @@ namespace armarx
Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Matrix4f& targetPose) const
{
+ ARMARX_TRACE;
Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
Eigen::Matrix3f tcpOri = tcp->getPoseInFrame(referenceFrame).block<3, 3>(0, 0);
Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index 9527835a6dd48107f5e11846ec882d30b056c54a..f7a7307bfb316db667d23ff59713d80551cd1e63 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -48,6 +48,7 @@ CartesianVelocityController::CartesianVelocityController(const VirtualRobot::Rob
void CartesianVelocityController::calculateJacobis(VirtualRobot::IKSolver::CartesianSelection mode)
{
+ ARMARX_TRACE;
jacobi = ik->getJacobianMatrix(_tcp, mode);
_jacobiWithCosts = Eigen::MatrixXf(jacobi.rows(), jacobi.cols());
for (int r = 0; r < jacobi.rows(); r++)
@@ -98,7 +99,7 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode)
{
-
+ ARMARX_TRACE;
calculateJacobis(mode);
@@ -106,6 +107,7 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
if (nullspaceVel.rows() > 0)
{
+ ARMARX_TRACE;
// ARMARX_IMPORTANT << "nullspaceVel " << nullspaceVel.transpose();
Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(_jacobiWithCosts);
//ARMARX_IMPORTANT << "The rank of the _jacobiWithCosts is " << lu_decomp.rank();
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
index a72fe97e50b3249595840b6fe18e1aca8be36542..ee06ab837f18e01764757e98e9aa372fc63e785f 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
@@ -77,6 +77,7 @@ namespace armarx
Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, float dt)
{
+ ARMARX_TRACE;
return controller.calculate(cartesianVelocityRamp.update(cartesianVel, dt), nullSpaceVelocityRamp.update(nullspaceVel, dt), mode);
}
diff --git a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
index fe9477986cee0a9d9a9a55b3f94d40206c010e3e..917877b7a5ce1556ece46a9d8353b274dba87465 100644
--- a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
@@ -32,6 +32,7 @@
#include <VirtualRobot/math/Helpers.h>
#include <VirtualRobot/MathTools.h>
#include <cfloat>
+#include <ArmarXCore/util/CPPUtility/trace.h>
namespace armarx
{
@@ -64,21 +65,25 @@ namespace armarx
const Eigen::VectorXf& CartesianWaypointController::calculate(float dt)
{
+ ARMARX_TRACE;
//calculate cartesian velocity + some management stuff
if (!isLastWaypoint() && isCurrentTargetNear())
{
+ ARMARX_TRACE;
currentWaypointIndex++;
}
cartesianVelocity = ctrlCartesianPos2Vel.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All) + feedForwardVelocity;
if (autoClearFeedForward)
{
+ ARMARX_TRACE;
clearFeedForwardVelocity();
}
//calculate joint velocity
if (nullSpaceControlEnabled)
{
+ ARMARX_TRACE;
//avoid joint limits
_jnv = KpJointLimitAvoidance * ctrlCartesianVelWithRamps.controller.calculateJointLimitAvoidance() +
ctrlCartesianVelWithRamps.controller.calculateNullspaceVelocity(
@@ -89,9 +94,11 @@ namespace armarx
}
else
{
+ ARMARX_TRACE;
//don't avoid joint limits
_jnv *= 0;
}
+ ARMARX_TRACE;
_out = ctrlCartesianVelWithRamps.calculate(cartesianVelocity, _jnv, dt);
return _out;
}
@@ -178,11 +185,13 @@ namespace armarx
const Eigen::Vector3f CartesianWaypointController::getCurrentTargetPosition() const
{
+ ARMARX_TRACE;
return ::math::Helpers::GetPosition(waypoints.at(currentWaypointIndex));
}
size_t CartesianWaypointController::skipToClosestWaypoint(float rad2mmFactor)
{
+ ARMARX_TRACE;
float dist = FLT_MAX;
size_t minIndex = 0;
for (size_t i = 0; i < waypoints.size(); i++)