diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
index a4ee8ad724be379aee10d469fa72d8e8b3e5f180..c1bb7243955f6a7ef7559338c00e77809d1c2a98 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
@@ -75,6 +75,11 @@ void DebugDrawerHelper::drawText(const std::string& name, const Eigen::Vector3f&
debugDrawerPrx->setTextVisu(layerName, name, text, makeGlobal(p1), color, size);
}
+void DebugDrawerHelper::drawArrow(const std::string& name, const Eigen::Vector3f& pos, const Eigen::Vector3f& direction, const DrawColor& color, float length, float width)
+{
+ debugDrawerPrx->setArrowVisu(layerName, name, makeGlobal(pos), makeGlobalDirection(direction), color, length, width);
+}
+
PosePtr DebugDrawerHelper::makeGlobal(const Eigen::Matrix4f& pose)
{
return new Pose(rn->getGlobalPose(pose));
@@ -84,3 +89,8 @@ Vector3Ptr DebugDrawerHelper::makeGlobal(const Eigen::Vector3f& position)
{
return new Vector3(rn->getGlobalPosition(position));
}
+
+Vector3Ptr DebugDrawerHelper::makeGlobalDirection(const Eigen::Vector3f& direction)
+{
+ return new Vector3(math::Helpers::TransformDirection(rn->getGlobalPose(), direction));
+}
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
index 3bdaa4933cfb47767bf7e64575a01c32507ef7d3..5d95dd9c8e604b830bb7be33abab9ba96ff9d910 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
@@ -59,8 +59,11 @@ namespace armarx
void drawText(const std::string& name, const Eigen::Vector3f& p1, const std::string& text, const DrawColor& color, int size);
+ void drawArrow(const std::string& name, const Eigen::Vector3f& pos, const Eigen::Vector3f& direction, const DrawColor& color, float length, float width);
+
PosePtr makeGlobal(const Eigen::Matrix4f& pose);
Vector3Ptr makeGlobal(const Eigen::Vector3f& position);
+ Vector3Ptr makeGlobalDirection(const Eigen::Vector3f& direction);
Defaults defaults;
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
index 1b8ad6c412459cadbb171980839371f3461aef2a..f67b581dfbf35a5c64a11c7323458fbb291e0d37 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
@@ -25,7 +25,8 @@
namespace armarx
{
detail::RtMessageLogEntryDummy detail::RtMessageLogEntryDummy::Instance;
- thread_local ControlThreadOutputBuffer* ControlThreadOutputBuffer::RtLoggingInstance{nullptr};
+ detail::RtMessageLogEntryNull detail::RtMessageLogEntryNull::Instance;
+ thread_local ControlThreadOutputBuffer* ControlThreadOutputBuffer::RtLoggingInstance {nullptr};
ControlThreadOutputBuffer::Entry& ControlThreadOutputBuffer::getWriteBuffer()
{
@@ -192,27 +193,27 @@ namespace armarx
}
detail::RtMessageLogBuffer::RtMessageLogBuffer(const detail::RtMessageLogBuffer& other, bool minimize):
- initialBufferSize{other.initialBufferSize * minimize},
- initialBufferEntryNumbers{other.initialBufferEntryNumbers * minimize},
- bufferMaxSize{other.bufferMaxSize * minimize},
- bufferMaxNumberEntries{other.bufferMaxNumberEntries * minimize},
- buffer(
- minimize ?
- other.buffer.size() + other.maxAlign - 1 - other.bufferSpace :
- other.buffer.size(),
- 0),
- bufferSpace{buffer.size()},
- bufferPlace{buffer.data()},
- entries(
- minimize ?
- other.entriesWritten :
- other.entries.size(),
- nullptr),
- entriesWritten{0},
- requiredAdditionalBufferSpace{other.requiredAdditionalBufferSpace},
- requiredAdditionalEntries{other.requiredAdditionalEntries},
- messagesLost{other.messagesLost},
- maxAlign{1}
+ initialBufferSize {other.initialBufferSize * minimize},
+ initialBufferEntryNumbers {other.initialBufferEntryNumbers * minimize},
+ bufferMaxSize {other.bufferMaxSize * minimize},
+ bufferMaxNumberEntries {other.bufferMaxNumberEntries * minimize},
+ buffer(
+ minimize ?
+ other.buffer.size() + other.maxAlign - 1 - other.bufferSpace :
+ other.buffer.size(),
+ 0),
+ bufferSpace {buffer.size()},
+ bufferPlace {buffer.data()},
+ entries(
+ minimize ?
+ other.entriesWritten :
+ other.entries.size(),
+ nullptr),
+ entriesWritten {0},
+ requiredAdditionalBufferSpace {other.requiredAdditionalBufferSpace},
+ requiredAdditionalEntries {other.requiredAdditionalEntries},
+ messagesLost {other.messagesLost},
+ maxAlign {1}
{
for (std::size_t idx = 0; idx < other.entries.size() && other.entries.at(idx); ++idx)
{
@@ -223,19 +224,19 @@ namespace armarx
const auto hint = ARMARX_STREAM_PRINTER
{
out << "entry " << idx << " of " << other.entriesWritten
- << "\nbuffer first = " << static_cast<void*>(&buffer.front())
- << "\nbuffer last = " << static_cast<void*>(&buffer.back())
- << "\nbuffer size = " << buffer.size()
- << "\nbuffer place = " << bufferPlace
- << "\nbuffer space = " << bufferSpace
- << "\nentry size = " << entry->_sizeof()
- << "\nentry align = " << entry->_alignof()
- << "\n"
- << "\nother buffer size = " << other.buffer.size()
- << "\nother buffer space = " << other.bufferSpace
- << "\nother max align = " << other.maxAlign
- << "\n"
- << "\nthis = " << this;
+ << "\nbuffer first = " << static_cast<void*>(&buffer.front())
+ << "\nbuffer last = " << static_cast<void*>(&buffer.back())
+ << "\nbuffer size = " << buffer.size()
+ << "\nbuffer place = " << bufferPlace
+ << "\nbuffer space = " << bufferSpace
+ << "\nentry size = " << entry->_sizeof()
+ << "\nentry align = " << entry->_alignof()
+ << "\n"
+ << "\nother buffer size = " << other.buffer.size()
+ << "\nother buffer space = " << other.bufferSpace
+ << "\nother max align = " << other.maxAlign
+ << "\n"
+ << "\nthis = " << this;
};
ARMARX_CHECK_NOT_NULL_W_HINT(place, hint);
ARMARX_CHECK_LESS_EQUAL_W_HINT(static_cast<void*>(&buffer.front()), static_cast<void*>(place), hint);
@@ -326,9 +327,9 @@ namespace armarx
std::size_t messageBufferSize, std::size_t messageBufferNumberEntries,
std::size_t messageBufferMaxSize, std::size_t messageBufferMaxNumberEntries
):
- sensorValuesTimestamp{IceUtil::Time::microSeconds(0)},
- timeSinceLastIteration{IceUtil::Time::microSeconds(0)},
- messages{messageBufferSize, messageBufferNumberEntries, messageBufferMaxSize, messageBufferMaxNumberEntries}
+ sensorValuesTimestamp {IceUtil::Time::microSeconds(0)},
+ timeSinceLastIteration {IceUtil::Time::microSeconds(0)},
+ messages {messageBufferSize, messageBufferNumberEntries, messageBufferMaxSize, messageBufferMaxNumberEntries}
{
//calculate size in bytes for one buffer
const std::size_t bytes = [&]
@@ -398,12 +399,12 @@ namespace armarx
}
detail::ControlThreadOutputBufferEntry::ControlThreadOutputBufferEntry(const detail::ControlThreadOutputBufferEntry& other, bool minimize):
- writeTimestamp{other.writeTimestamp},
- sensorValuesTimestamp{other.sensorValuesTimestamp},
- timeSinceLastIteration{other.timeSinceLastIteration},
- iteration{other.iteration},
- messages{other.messages, minimize},
- buffer(other.buffer.size(), 0)
+ writeTimestamp {other.writeTimestamp},
+ sensorValuesTimestamp {other.sensorValuesTimestamp},
+ timeSinceLastIteration {other.timeSinceLastIteration},
+ iteration {other.iteration},
+ messages {other.messages, minimize},
+ buffer(other.buffer.size(), 0)
{
void* place = buffer.data();
std::size_t space = buffer.size();
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
index d7b0ad5c68a3bd6ff99749c7d995e3bad87c1f09..f57fec25d89690517b130bf3a8c5cafc560d80ce 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
@@ -26,6 +26,8 @@
#include <vector>
+#include <boost/format.hpp>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/PropagateConst.h>
#include <ArmarXCore/core/util/TripleBuffer.h>
@@ -40,6 +42,23 @@ namespace armarx
{
namespace detail
{
+ inline std::string armarx_sprintf_helper(boost::format& f)
+ {
+ return boost::str(f);
+ }
+
+ template<class T, class... Args>
+ inline std::string armarx_sprintf_helper(boost::format& f, T&& t, Args&& ... args)
+ {
+ return armarx_sprintf_helper(f % std::forward<T>(t), std::forward<Args>(args)...);
+ }
+ template<class... Args>
+ inline std::string armarx_sprintf(const std::string& formatString, Args&& ... args)
+ {
+ boost::format f(formatString);
+ return ::armarx::detail::armarx_sprintf_helper(f, std::forward<Args>(args)...);
+ }
+
struct RtMessageLogEntryBase
{
RtMessageLogEntryBase():
@@ -80,12 +99,36 @@ namespace armarx
ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER
};
+
+ struct RtMessageLogEntryNull : RtMessageLogEntryBase
+ {
+ static RtMessageLogEntryNull Instance;
+ protected:
+ std::string format() const final override
+ {
+ return "";
+ }
+ std::size_t line() const final override
+ {
+ return 0;
+ }
+ std::string file() const final override
+ {
+ return "";
+ }
+ std::string func() const final override
+ {
+ return "";
+ }
+
+ ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER
+ };
}
}
namespace armarx
{
class RobotUnit;
- class ControlThreadOutputBuffer;
+ struct ControlThreadOutputBuffer;
namespace RobotUnitModule
{
@@ -104,7 +147,7 @@ namespace armarx
RtMessageLogBuffer() = delete;
RtMessageLogBuffer(RtMessageLogBuffer&&) = delete;
- RtMessageLogBuffer& operator=(RtMessageLogBuffer&&) = delete;
+ RtMessageLogBuffer& operator=(RtMessageLogBuffer &&) = delete;
RtMessageLogBuffer& operator=(const RtMessageLogBuffer&) = delete;
void reset(std::size_t& bufferSize, std::size_t& numEntries, std::size_t iterationCount);
@@ -117,7 +160,7 @@ namespace armarx
void deleteAll();
friend struct ControlThreadOutputBufferEntry;
- friend class ::armarx::ControlThreadOutputBuffer;
+ friend struct ::armarx::ControlThreadOutputBuffer;
friend class ::armarx::RobotUnit;
friend class ::armarx::RobotUnitModule::Logging;
@@ -155,7 +198,7 @@ namespace armarx
ControlThreadOutputBufferEntry() = delete;
ControlThreadOutputBufferEntry(ControlThreadOutputBufferEntry&&) = delete;
- ControlThreadOutputBufferEntry& operator=(ControlThreadOutputBufferEntry&&) = delete;
+ ControlThreadOutputBufferEntry& operator=(ControlThreadOutputBufferEntry &&) = delete;
ControlThreadOutputBufferEntry& operator=(const ControlThreadOutputBufferEntry&) = delete;
std::size_t getDataBufferSize() const;
@@ -244,10 +287,20 @@ namespace armarx
}
-#define ARMARX_RT_LOGF(...) _detail_ARMARX_RT_LOGF(__FILE__, ARMARX_FUNCTION,__LINE__, __VA_ARGS__, true)
+// ARMARX_INFO << deactivateSpam(1) << "Redirected RT Logging:\n" << ::armarx::detail::armarx_printf_helper(f, __VA_ARGS__);
+
+#define _detail_ARMARX_RT_REDIRECT( ...) ([&]{ \
+ ::armarx::detail::armarx_sprintf(__VA_ARGS__); \
+ ARMARX_INFO << deactivateSpam(1) << "Redirected RT Logging:\n";\
+ return &::armarx::detail::RtMessageLogEntryNull::Instance;}())
+
+
+
+
+#define ARMARX_RT_LOGF(...) (*((::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance())? _detail_ARMARX_RT_LOGF(__FILE__, ARMARX_FUNCTION,__LINE__, __VA_ARGS__, true) : _detail_ARMARX_RT_REDIRECT(__VA_ARGS__)))
#define _detail_ARMARX_RT_LOGF(file_, func_, line_, FormatString, ...) \
- (*[&]{ \
+ ([&]{ \
using namespace ::armarx; \
using RtMessageLogEntryBase = ControlThreadOutputBuffer::RtMessageLogEntryBase; \
struct RtMessageLogEntry : RtMessageLogEntryBase \
@@ -268,7 +321,7 @@ namespace armarx
ARMARX_CHECK_NOT_NULL(::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance()); \
return ::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance() \
->addMessageToLog<RtMessageLogEntry>(__VA_ARGS__); \
- }())
+ }())
#define ARMARX_RT_LOGF_DEBUG(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::eDEBUG)
#define ARMARX_RT_LOGF_VERBOSE(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::eVERBOSE)
@@ -314,13 +367,13 @@ namespace armarx
std::size_t bufferMaxSize, std::size_t bufferMaxNumberEntries
):
initialBufferSize {bufferSize},
- initialBufferEntryNumbers {numEntries},
- bufferMaxSize {bufferMaxSize},
- bufferMaxNumberEntries {bufferMaxNumberEntries},
- buffer(bufferSize, 0),
- bufferSpace {buffer.size()},
- bufferPlace {buffer.data()},
- entries(numEntries, nullptr)
+ initialBufferEntryNumbers {numEntries},
+ bufferMaxSize {bufferMaxSize},
+ bufferMaxNumberEntries {bufferMaxNumberEntries},
+ buffer(bufferSize, 0),
+ bufferSpace {buffer.size()},
+ bufferPlace {buffer.data()},
+ entries(numEntries, nullptr)
{}
inline RtMessageLogBuffer::~RtMessageLogBuffer()
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
index 629d427a66fb670940910bcd559459e39939bd72..a54cf2fe86393f94a679b1d09d45cb5a7d48f292 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
@@ -27,6 +27,8 @@
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+#include <VirtualRobot/math/Helpers.h>
+
using namespace armarx;
PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const Eigen::Matrix4f& target)
@@ -57,7 +59,7 @@ void PositionControllerHelper::update()
}
Eigen::VectorXf cv = posController.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All);
velocityControllerHelper->setTargetVelocity(cv + feedForwardVelocity);
- if(autoClearFeedForward)
+ if (autoClearFeedForward)
{
clearFeedForwardVelocity();
}
@@ -147,6 +149,11 @@ const Eigen::Matrix4f& PositionControllerHelper::getCurrentTarget() const
return waypoints.at(currentWaypointIndex);
}
+const Eigen::Vector3f PositionControllerHelper::getCurrentTargetPosition() const
+{
+ return math::Helpers::GetPosition(waypoints.at(currentWaypointIndex));
+}
+
size_t PositionControllerHelper::skipToClosestWaypoint(float rad2mmFactor)
{
float dist = FLT_MAX;
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
index 92d20fa3133d90535234780b23c7a4c8899b1161..b90b2a615f0dafabae4a7a42fb4024a8f1e84d0d 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
@@ -75,6 +75,7 @@ namespace armarx
bool isLastWaypoint() const;
const Eigen::Matrix4f& getCurrentTarget() const;
+ const Eigen::Vector3f getCurrentTargetPosition() const;
size_t skipToClosestWaypoint(float rad2mmFactor);
@@ -84,7 +85,7 @@ namespace armarx
struct NullspaceOptimizationArgs
{
- NullspaceOptimizationArgs(){}
+ NullspaceOptimizationArgs() {}
int loops = 100;
float stepLength = 0.05f;
float eps = 0.001;