diff --git a/CMakeLists.txt b/CMakeLists.txt
index 99314721ad44b94b9eac660a20139fb65ad16640..043e79fb024a648724e9333574e00b72c9705a7f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -20,7 +20,7 @@ set(ARMARX_ENABLE_AUTO_CODE_FORMATTING TRUE)
armarx_project("RobotAPI")
depends_on_armarx_package(ArmarXGui)
-set(ArmarX_Simox_VERSION 2.3.71)
+set(ArmarX_Simox_VERSION 2.3.72)
option(REQUIRE_SIMOX "If enabled the Simox dependency is a required dependency" TRUE)
if(REQUIRE_SIMOX)
diff --git a/data/RobotAPI/robots/Head/KA-Head.xml b/data/RobotAPI/robots/Head/KA-Head.xml
index 4fbdcda252ecae4a50952d0cedd8e1d1bc5c5bd9..6609e8058c5a70a243a941124307f1ab21487856 100644
--- a/data/RobotAPI/robots/Head/KA-Head.xml
+++ b/data/RobotAPI/robots/Head/KA-Head.xml
@@ -473,6 +473,8 @@
<Node name="Cameras"/>
<Node name="Eye_Left"/>
<Node name="Eye_Right"/>
+ <Node name="EyeLeftCamera"/>
+ <Node name="EyeRightCamera"/>
</RobotNodeSet>
diff --git a/etc/cmake/ArmarXPackageVersion.cmake b/etc/cmake/ArmarXPackageVersion.cmake
index 855fc4134baf23365fbd04ad5674dabd14b4a1e2..8fd081103ec8a10e0558f8089ed7e076c3082a11 100644
--- a/etc/cmake/ArmarXPackageVersion.cmake
+++ b/etc/cmake/ArmarXPackageVersion.cmake
@@ -1,7 +1,7 @@
# armarx version file
set(ARMARX_PACKAGE_LIBRARY_VERSION_MAJOR "0")
-set(ARMARX_PACKAGE_LIBRARY_VERSION_MINOR "9")
-set(ARMARX_PACKAGE_LIBRARY_VERSION_PATCH "2")
+set(ARMARX_PACKAGE_LIBRARY_VERSION_MINOR "10")
+set(ARMARX_PACKAGE_LIBRARY_VERSION_PATCH "0")
set(ARMARX_PACKAGE_LIBRARY_VERSION "${ARMARX_PACKAGE_LIBRARY_VERSION_MAJOR}.${ARMARX_PACKAGE_LIBRARY_VERSION_MINOR}.${ARMARX_PACKAGE_LIBRARY_VERSION_PATCH}")
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/Units/TCPControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
index d82b7167c7c4aa385f3eff4164ced67688817e2f..ef3cb1009b0c1caf2ceab65a7bb4b5143d992e94 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
@@ -70,23 +70,7 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
ARMARX_CHECK_EXPRESSION_W_HINT(coordinateTransformationRobot->hasRobotNode(tcpName), "The robot does not have the node: " + tcpName);
tcp = tcpName;
}
- auto controllerName = this->getName() + "_" + tcp + "_" + nodeSetName;
- auto activeNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
- NJointCartesianVelocityControllerWithRampPtr tcpController;
- bool nodeSetAlreadyControlled = false;
- for (NJointControllerPtr controller : activeNJointControllers)
- {
- tcpController = NJointCartesianVelocityControllerWithRampPtr::dynamicCast(controller);
- if (!tcpController)
- {
- continue;
- }
- if (tcpController->getNodeSetName() == nodeSetName && tcpController->getInstanceName() == controllerName)
- {
- nodeSetAlreadyControlled = true;
- break;
- }
- }
+
robotUnit->updateVirtualRobot(coordinateTransformationRobot);
float xVel = 0.f;
@@ -135,7 +119,33 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
noMode = true;
}
ARMARX_DEBUG << "CartesianSelection-Mode: " << (int)mode;
+ auto controllerName = this->getName() + "_" + tcp + "_" + nodeSetName + "_mode_" + std::to_string((int)mode);
+ auto NJointControllers = robotUnit->getNJointControllerNames();
+ NJointCartesianVelocityControllerWithRampPtr tcpController;
+ bool nodeSetAlreadyControlled = false;
+ for (auto& name : NJointControllers)
+ {
+ NJointControllerPtr controller;
+ try
+ {
+ controller = robotUnit->getNJointControllerNotNull(name);
+ }
+ catch (...)
+ {
+ continue;
+ }
+ tcpController = NJointCartesianVelocityControllerWithRampPtr::dynamicCast(controller);
+ if (!tcpController)
+ {
+ continue;
+ }
+ if (tcpController->getNodeSetName() == nodeSetName && tcpController->getInstanceName() == controllerName)
+ {
+ nodeSetAlreadyControlled = true;
+ break;
+ }
+ }
if (!nodeSetAlreadyControlled)
{
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
index 1b8ad6c412459cadbb171980839371f3461aef2a..dea0cefc31e5558e73fa18bf13d16d0fb670af61 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},
+ 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()},
+ 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}
+ 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)
{
@@ -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,11 +399,11 @@ 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},
+ 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();
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
index 6d760be0fda039b74870660a410378d724bb83de..c2c4459d03c31fdc67a2a973608eb4796acbb53d 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
{
@@ -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;
@@ -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 \
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;
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
index c062e9509533da3b3705b2821b6c75bb4272c5e2..c1a2369f9c01964bf8282af46edbb34c97ba26d6 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
@@ -79,5 +79,14 @@ void VelocityControllerHelper::setNullSpaceControl(bool enabled)
void VelocityControllerHelper::cleanup()
{
- controller->deactivateAndDeleteController();
+ if (controllerCreated)
+ {
+ // delete controller only if it was created
+ controller->deactivateAndDeleteController();
+ }
+ else
+ {
+ // if the controller existed, only deactivate it
+ controller->deactivateController();
+ }
}
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index de66c0591d66119057275be998bbfa6faccc854b..496ad995fe58b5672c2b9189475b8bd64535e9eb 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -32,8 +32,9 @@
using namespace armarx;
-CartesianVelocityController::CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod)
- : rns(rns)
+CartesianVelocityController::CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod, bool considerJointLimits)
+ : rns(rns),
+ considerJointLimits(considerJointLimits)
{
//ARMARX_IMPORTANT << VAROUT(rns->getRobot()->getRootNode());
ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), invJacMethod));
@@ -55,9 +56,6 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
//ARMARX_IMPORTANT << VAROUT(tcp.get());
//ARMARX_IMPORTANT << VAROUT(ik.get());
jacobi = ik->getJacobianMatrix(tcp, mode);
- //ARMARX_IMPORTANT << "hi";
- //ARMARX_IMPORTANT << jacobi;
- inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
// ARMARX_IMPORTANT << "nullspaceVel " << nullspaceVel.transpose();
Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
@@ -79,12 +77,22 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
//nsv /= kernel.cols();
- Eigen::VectorXf jointVel = inv * cartesianVel;
+
+ inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
+
+
+ if (considerJointLimits)
+ {
+ adjustJacobiForJointsAtJointLimits(mode, cartesianVel);
+ }
+
+
+ Eigen::VectorXf jointVel = inv * cartesianVel;
jointVel += nsv;
- if(maximumJointVelocities.rows() > 0)
+ if (maximumJointVelocities.rows() > 0)
{
jointVel = ::math::Helpers::LimitVectorLength(jointVel, maximumJointVelocities);
}
@@ -155,3 +163,52 @@ Eigen::VectorXf CartesianVelocityController::calculateRegularization(VirtualRobo
}
return regularization.topRows(i);
}
+
+bool CartesianVelocityController::adjustJacobiForJointsAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, float jointLimitCheckAccuracy)
+{
+
+ bool modifiedJacobi = false;
+
+ Eigen::VectorXf jointVel = inv * cartesianVel;
+ size_t size = rns->getSize();
+
+ for (size_t i = 0; i < size; ++i)
+ {
+ auto& node = rns->getNode(static_cast<int>(i));
+
+ if (node->isLimitless() || // limitless joint cannot be out of limits
+ std::abs(jointVel(i)) < 0.001f // If it the jacobi doesnt want this joint to move anyway, there is no point in recalculating the inverse jacobi
+ )
+ {
+ continue;
+ }
+
+ if ((node->getJointValue() >= node->getJointLimitHigh() - jointLimitCheckAccuracy && jointVel(i) > 0)
+ || (node->getJointValue() <= node->getJointLimitLow() + jointLimitCheckAccuracy && jointVel(i) < 0))
+ {
+ for (int k = 0; k < jacobi.rows(); ++k) // memory allocation free resetting of column
+ {
+ jacobi(k, i) = 0.0f;
+ }
+ modifiedJacobi = true;
+ }
+ }
+
+ if (modifiedJacobi)
+ {
+ // calculate inverse jacobi again since atleast one joint would be driven into the limit
+ inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
+ }
+
+ return modifiedJacobi;
+}
+
+bool CartesianVelocityController::getConsiderJointLimits() const
+{
+ return considerJointLimits;
+}
+
+void CartesianVelocityController::setConsiderJointLimits(bool value)
+{
+ considerJointLimits = value;
+}
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.h b/source/RobotAPI/libraries/core/CartesianVelocityController.h
index 01398dd59581ffd1a36749e95f8a3287fbde476f..3826e0471d9eada001b8cb13b07cdde7bde8b25c 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.h
@@ -40,7 +40,8 @@ namespace armarx
{
public:
CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp = nullptr,
- const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = VirtualRobot::JacobiProvider::eSVDDamped);
+ const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = VirtualRobot::JacobiProvider::eSVDDamped,
+ bool considerJointLimits = true);
Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, float KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection mode);
Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode);
@@ -49,6 +50,9 @@ namespace armarx
void setCartesianRegularization(float cartesianMMRegularization, float cartesianRadianRegularization);
+ bool getConsiderJointLimits() const;
+ void setConsiderJointLimits(bool value);
+
Eigen::MatrixXf jacobi;
Eigen::MatrixXf inv;
VirtualRobot::RobotNodeSetPtr rns;
@@ -58,7 +62,8 @@ namespace armarx
private:
Eigen::VectorXf calculateRegularization(VirtualRobot::IKSolver::CartesianSelection mode);
-
+ bool adjustJacobiForJointsAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, float jointLimitCheckAccuracy = 0.001f);
+ bool considerJointLimits = true;
float cartesianMMRegularization;
float cartesianRadianRegularization;
};
diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index 1d85531ea8537c614aea2c6a4d98ff8f7bb0c964..404379ab6d8c533a55a12dcea1c46ea80fe50dfd 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -131,6 +131,13 @@ namespace armarx
return new Trajectory(*this);
}
+ TrajectoryPtr Trajectory::cloneMetaData() const
+ {
+ TrajectoryPtr t = new Trajectory();
+ CopyMetaData(*this, *t);
+ return t;
+ }
+
std::string Trajectory::output(const Ice::Current&) const
{
@@ -1647,11 +1654,14 @@ namespace armarx
}
- void Trajectory::clear()
+ void Trajectory::clear(bool keepMetaData)
{
dataMap.erase(dataMap.begin(), dataMap.end());
- dimensionNames.clear();
- limitless.clear();
+ if (!keepMetaData)
+ {
+ dimensionNames.clear();
+ limitless.clear();
+ }
}
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index 5f7613bdca048ee60f3efdcc43b53e7d9f79ca62..15de38e073b2924f2178df40c98d9160dfc03f9b 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -398,7 +398,7 @@ namespace armarx
static void CopyData(const Trajectory& source, Trajectory& destination);
static void CopyMetaData(const Trajectory& source, Trajectory& destination);
- void clear();
+ void clear(bool keepMetaData = false);
void setPositionEntry(const double t, const size_t dimIndex, const double& y);
void setEntries(const double t, const size_t dimIndex, const Ice::DoubleSeq& y);
bool dataExists(double t, size_t dimension = 0, size_t derivation = 0) const;
@@ -417,6 +417,7 @@ namespace armarx
// VariantDataClass interface
VariantDataClassPtr clone(const Ice::Current& c = Ice::Current()) const override;
+ TrajectoryPtr cloneMetaData() const;
std::string output(const Ice::Current& c = Ice::Current()) const override;
Ice::Int getType(const Ice::Current& c = Ice::Current()) const override;
bool validate(const Ice::Current& c = Ice::Current()) override;
diff --git a/source/RobotAPI/libraries/core/TrajectoryController.cpp b/source/RobotAPI/libraries/core/TrajectoryController.cpp
index ef74dd1fc47b1f67dbc44bb97f87f5dc5346a39c..5328d534a2b6ce76915a343e2cdad3014a4ee1b4 100644
--- a/source/RobotAPI/libraries/core/TrajectoryController.cpp
+++ b/source/RobotAPI/libraries/core/TrajectoryController.cpp
@@ -48,6 +48,7 @@ namespace armarx
//}
positions.resize(traj->dim(), 1);
veloctities.resize(traj->dim(), 1);
+ currentError.resize(traj->dim(), 1);
}
const Eigen::VectorXf& TrajectoryController::update(double deltaT, const Eigen::VectorXf& currentPosition)
@@ -82,7 +83,18 @@ namespace armarx
veloctities(i) = 0;
}
}
- currentError = positions - currentPosition;
+ for (size_t i = 0; i < dim; ++i)
+ {
+ if (pid->limitless.size() > i && pid->limitless.at(i))
+ {
+ currentError(i) = math::MathUtils::AngleDelta(currentPosition(i), positions(i));
+ }
+ else
+ {
+ currentError(i) = positions(i) - currentPosition(i);
+ }
+
+ }
pid->update(std::abs(deltaT), currentPosition, positions);
veloctities += pid->getControlValue();
diff --git a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
index f50dedc362145b2ef1aeb812f98233ba0f6da8d4..216c48d906af3f8ea6e14f3a2a89424a1ce13c22 100644
--- a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
+++ b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
@@ -76,3 +76,82 @@ BOOST_AUTO_TEST_CASE(test1)
BOOST_CHECK_LE((targetTcpVel - tcpVel).norm(), 0.01f);
}
+
+BOOST_AUTO_TEST_CASE(testJointLimitAwareness)
+{
+ const std::string project = "RobotAPI";
+ armarx::CMakePackageFinder finder(project);
+
+ if (!finder.packageFound())
+ {
+ ARMARX_ERROR_S << "ArmarX Package " << project << " has not been found!";
+ }
+ else
+ {
+ armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
+ }
+ std::string fn = "RobotAPI/robots/Armar3/ArmarIII.xml";
+ ArmarXDataPath::getAbsolutePath(fn, fn);
+ std::string robotFilename = fn;
+ VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFilename, VirtualRobot::RobotIO::eStructure);
+ VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet("TorsoRightArm");
+ CartesianVelocityControllerPtr h(new CartesianVelocityController(rns, nullptr, VirtualRobot::JacobiProvider::eSVDDamped));
+ const Eigen::VectorXf oldJV = rns->getJointValuesEigen();
+ ARMARX_INFO << VAROUT(oldJV);
+ Eigen::VectorXf cartesianVel(6);
+ srand(123);
+
+ for (int i = 0; i < 10000; ++i)
+ {
+
+
+ cartesianVel << rand() % 100, rand() % 100, rand() % 100, 0.01 * (rand() % 100), 0.01 * (rand() % 100), 0.01 * (rand() % 100);
+ // cartesianVel << 100,0,0;//, 0.01*(rand()%100), 0.01*(rand()%100), 0.01*(rand()%100);
+ auto mode = VirtualRobot::IKSolver::CartesianSelection::All;
+ Eigen::MatrixXf jacobi = h->ik->getJacobianMatrix(rns->getTCP(), mode);
+ Eigen::MatrixXf inv = h->ik->computePseudoInverseJacobianMatrix(jacobi, h->ik->getJacobiRegularization(mode));
+ int jointIndex = rand() % rns->getSize();
+ jacobi.block(0, jointIndex, jacobi.rows(), 1) = Eigen::VectorXf::Zero(jacobi.rows());
+ Eigen::MatrixXf inv2 = h->ik->computePseudoInverseJacobianMatrix(jacobi, h->ik->getJacobiRegularization(mode));
+
+ // ARMARX_INFO << "diff\n" << (inv-inv2);
+ rns->setJointValues(oldJV);
+ Eigen::Vector3f posBefore = h->tcp->getPositionInRootFrame();
+
+ float accuracy = 0.001f;
+
+ Eigen::VectorXf jointVel = inv * cartesianVel;
+ rns->setJointValues(oldJV + jointVel * accuracy);
+ Eigen::VectorXf resultCartesianVel = ((h->tcp->getPositionInRootFrame() - posBefore) / accuracy);
+
+
+
+ Eigen::VectorXf jointVel2 = inv2 * cartesianVel;
+ rns->setJointValues(oldJV + jointVel2 * accuracy);
+ Eigen::VectorXf resultCartesianVel2 = ((h->tcp->getPositionInRootFrame() - posBefore) / accuracy);
+
+ Eigen::Vector3f diff = (resultCartesianVel - cartesianVel);
+ Eigen::Vector3f diff2 = (resultCartesianVel2 - cartesianVel);
+
+ if (!((diff - diff2).norm() < 0.5 || diff2.norm() < diff.norm()))
+ {
+ ARMARX_INFO << "Target cartesian velo:\n" << cartesianVel;
+ ARMARX_INFO << "jacobi\n" << jacobi;
+ ARMARX_INFO << "inv\n" << inv;
+ ARMARX_INFO << "inv2\n" << inv2;
+ ARMARX_INFO << "\n\nResulting joint cart velocity: " << resultCartesianVel << "\n jointVel:\n" << jointVel;
+ ARMARX_INFO << "\n\nResulting joint cart velocity with joint limit avoidance: " << resultCartesianVel2 << "\n jointVel:\n" << jointVel2;
+
+ ARMARX_INFO << "Diff:\n" << diff;
+ ARMARX_INFO << "Diff2:\n" << diff2;
+ }
+ else
+ {
+ ARMARX_INFO << "Success for \n" << cartesianVel;
+ }
+
+
+ BOOST_REQUIRE((diff - diff2).norm() < 1 || diff2.norm() < diff.norm());
+
+ }
+}