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/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/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..40dad31076f78f8753bfee2599e0649cb7dba6f1 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());
+
+ }
+}