From 6aa0349ec1348fbb73fbd4cf835b4dbfa9925994 Mon Sep 17 00:00:00 2001
From: Meixner <andre.meixner@kit.edu>
Date: Fri, 5 Mar 2021 12:44:57 +0100
Subject: [PATCH] Added manipulability tracking, composite diff ik, soechting
nullspace constraints and an example graphical user interface in RobotViewer
---
VirtualRobot/CMakeLists.txt | 27 +
.../CartesianPositionController.cpp | 158 ++++
.../Controller/CartesianPositionController.h | 62 ++
.../CartesianVelocityController.cpp | 287 ++++++
.../Controller/CartesianVelocityController.h | 74 ++
.../IK/CompositeDiffIK/CompositeDiffIK.cpp | 525 +++++++++++
.../IK/CompositeDiffIK/CompositeDiffIK.h | 240 +++++
.../ManipulabilityNullspaceGradient.cpp | 62 ++
.../ManipulabilityNullspaceGradient.h | 47 +
VirtualRobot/IK/CompositeDiffIK/Soechting.h | 121 +++
.../SoechtingNullspaceGradient.cpp | 114 +++
.../SoechtingNullspaceGradient.h | 64 ++
.../Manipulability/AbstractManipulability.cpp | 197 ++++
.../Manipulability/AbstractManipulability.h | 102 ++
.../AbstractManipulabilityTracking.cpp | 200 ++++
.../AbstractManipulabilityTracking.h | 103 +++
.../Manipulability/BimanualManipulability.cpp | 195 ++++
.../Manipulability/BimanualManipulability.h | 105 +++
.../BimanualManipulabilityTracking.cpp | 166 ++++
.../BimanualManipulabilityTracking.h | 71 ++
.../SingleChainManipulability.cpp | 140 +++
.../SingleChainManipulability.h | 120 +++
.../SingleChainManipulabilityTracking.cpp | 119 +++
.../SingleChainManipulabilityTracking.h | 68 ++
VirtualRobot/Workspace/Manipulability.h | 2 +-
.../examples/RobotViewer/CMakeLists.txt | 8 +-
.../examples/RobotViewer/DiffIKWidget.cpp | 664 +++++++++++++
.../examples/RobotViewer/DiffIKWidget.h | 108 +++
.../examples/RobotViewer/DiffIKWidget.ui | 871 ++++++++++++++++++
.../examples/RobotViewer/RobotViewer.cpp | 2 +-
.../examples/RobotViewer/RobotViewer.ui | 25 +-
.../examples/RobotViewer/showRobotWindow.cpp | 14 +-
.../examples/RobotViewer/showRobotWindow.h | 1 +
VirtualRobot/math/Helpers.cpp | 10 +
VirtualRobot/math/Helpers.h | 2 +
VirtualRobot/tests/CMakeLists.txt | 2 +
.../tests/VirtualRobotManipulabilityTest.cpp | 223 +++++
37 files changed, 5286 insertions(+), 13 deletions(-)
create mode 100644 VirtualRobot/Controller/CartesianPositionController.cpp
create mode 100644 VirtualRobot/Controller/CartesianPositionController.h
create mode 100644 VirtualRobot/Controller/CartesianVelocityController.cpp
create mode 100644 VirtualRobot/Controller/CartesianVelocityController.h
create mode 100644 VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp
create mode 100644 VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h
create mode 100644 VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.cpp
create mode 100644 VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.h
create mode 100644 VirtualRobot/IK/CompositeDiffIK/Soechting.h
create mode 100644 VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.cpp
create mode 100644 VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h
create mode 100644 VirtualRobot/Manipulability/AbstractManipulability.cpp
create mode 100644 VirtualRobot/Manipulability/AbstractManipulability.h
create mode 100644 VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp
create mode 100644 VirtualRobot/Manipulability/AbstractManipulabilityTracking.h
create mode 100644 VirtualRobot/Manipulability/BimanualManipulability.cpp
create mode 100644 VirtualRobot/Manipulability/BimanualManipulability.h
create mode 100644 VirtualRobot/Manipulability/BimanualManipulabilityTracking.cpp
create mode 100644 VirtualRobot/Manipulability/BimanualManipulabilityTracking.h
create mode 100644 VirtualRobot/Manipulability/SingleChainManipulability.cpp
create mode 100644 VirtualRobot/Manipulability/SingleChainManipulability.h
create mode 100644 VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp
create mode 100644 VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h
create mode 100644 VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp
create mode 100644 VirtualRobot/examples/RobotViewer/DiffIKWidget.h
create mode 100644 VirtualRobot/examples/RobotViewer/DiffIKWidget.ui
create mode 100644 VirtualRobot/tests/VirtualRobotManipulabilityTest.cpp
diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index 0b958dc4b..13c07881e 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -192,6 +192,9 @@ SET(SOURCES
Compression/CompressionBZip2.cpp
Compression/CompressionRLE.cpp
+ Controller/CartesianPositionController.cpp
+ Controller/CartesianVelocityController.cpp
+
EndEffector/EndEffector.cpp
EndEffector/EndEffectorActor.cpp
@@ -229,11 +232,21 @@ SET(SOURCES
IK/constraints/PositionConstraint.cpp
IK/constraints/ReferenceConfigurationConstraint.cpp
IK/constraints/TSRConstraint.cpp
+ IK/CompositeDiffIK/CompositeDiffIK.cpp
+ IK/CompositeDiffIK/ManipulabilityNullspaceGradient.cpp
+ IK/CompositeDiffIK/SoechtingNullspaceGradient.cpp
Import/RobotImporterFactory.cpp
Import/SimoxXMLFactory.cpp
Import/MeshImport/AssimpReader.cpp
+ Manipulability/AbstractManipulability.cpp
+ Manipulability/SingleChainManipulability.cpp
+ Manipulability/BimanualManipulability.cpp
+ Manipulability/AbstractManipulabilityTracking.cpp
+ Manipulability/SingleChainManipulabilityTracking.cpp
+ Manipulability/BimanualManipulabilityTracking.cpp
+
math/AbstractFunctionR1R2.cpp
math/AbstractFunctionR1R3.cpp
math/AbstractFunctionR1R6.cpp
@@ -388,6 +401,9 @@ SET(INCLUDES
Compression/CompressionBZip2.h
Compression/CompressionRLE.h
+ Controller/CartesianPositionController.h
+ Controller/CartesianVelocityController.h
+
DataStructures/nanoflann.hpp
DataStructures/KdTreePointCloud.h
@@ -428,11 +444,22 @@ SET(INCLUDES
IK/constraints/PositionConstraint.h
IK/constraints/ReferenceConfigurationConstraint.h
IK/constraints/TSRConstraint.h
+ IK/CompositeDiffIK/CompositeDiffIK.h
+ IK/CompositeDiffIK/ManipulabilityNullspaceGradient.h
+ IK/CompositeDiffIK/Soechting.h
+ IK/CompositeDiffIK/SoechtingNullspaceGradient.h
Import/RobotImporterFactory.h
Import/SimoxXMLFactory.h
Import/MeshImport/AssimpReader.h
+ Manipulability/AbstractManipulability.h
+ Manipulability/SingleChainManipulability.h
+ Manipulability/BimanualManipulability.h
+ Manipulability/AbstractManipulabilityTracking.h
+ Manipulability/SingleChainManipulabilityTracking.h
+ Manipulability/BimanualManipulabilityTracking.h
+
math/AbstractFunctionR1Ori.h
math/AbstractFunctionR1R2.h
math/AbstractFunctionR1R3.h
diff --git a/VirtualRobot/Controller/CartesianPositionController.cpp b/VirtualRobot/Controller/CartesianPositionController.cpp
new file mode 100644
index 000000000..59149787c
--- /dev/null
+++ b/VirtualRobot/Controller/CartesianPositionController.cpp
@@ -0,0 +1,158 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "CartesianPositionController.h"
+
+#include "VirtualRobot/math/Helpers.h"
+#include <Eigen/Dense>
+
+namespace VirtualRobot
+{
+ CartesianPositionController::CartesianPositionController(const RobotNodePtr& tcp)
+ : tcp(tcp)
+ {
+ }
+
+ Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& targetPose, IKSolver::CartesianSelection mode) const
+ {
+ int posLen = mode & IKSolver::Position ? 3 : 0;
+ int oriLen = mode & IKSolver::Orientation ? 3 : 0;
+ Eigen::VectorXf cartesianVel(posLen + oriLen);
+
+ if (posLen)
+ {
+ Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
+ Eigen::Vector3f currentPos = tcp->getPositionInRootFrame();
+ Eigen::Vector3f errPos = targetPos - currentPos;
+ Eigen::Vector3f posVel = errPos * KpPos;
+ if (maxPosVel > 0)
+ {
+ posVel = math::Helpers::LimitTo(posVel, maxPosVel);
+ }
+ cartesianVel.block<3, 1>(0, 0) = posVel;
+ }
+
+ if (oriLen)
+ {
+ Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+ Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
+ Eigen::AngleAxisf aa(oriDir);
+ Eigen::Vector3f errOri = aa.axis() * aa.angle();
+ Eigen::Vector3f oriVel = errOri * KpOri;
+
+ if (maxOriVel > 0)
+ {
+ oriVel = math::Helpers::LimitTo(oriVel, maxOriVel);
+ }
+ cartesianVel.block<3, 1>(posLen, 0) = oriVel;
+ }
+ return cartesianVel;
+ }
+
+ Eigen::VectorXf CartesianPositionController::calculatePos(const Eigen::Vector3f& targetPos) const
+ {
+ Eigen::VectorXf cartesianVel(3);
+ Eigen::Vector3f currentPos = tcp->getPositionInRootFrame();
+ Eigen::Vector3f errPos = targetPos - currentPos;
+ Eigen::Vector3f posVel = errPos * KpPos;
+ if (maxPosVel > 0)
+ {
+ posVel = math::Helpers::LimitTo(posVel, maxPosVel);
+ }
+ cartesianVel.block<3, 1>(0, 0) = posVel;
+ return cartesianVel;
+ }
+
+ float CartesianPositionController::getPositionError(const Eigen::Matrix4f& targetPose) const
+ {
+ return GetPositionError(targetPose, tcp);
+ }
+
+ float CartesianPositionController::getOrientationError(const Eigen::Matrix4f& targetPose) const
+ {
+ return GetOrientationError(targetPose, tcp);
+ }
+
+ float CartesianPositionController::GetPositionError(const Eigen::Matrix4f& targetPose, const RobotNodePtr& tcp)
+ {
+ Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
+ Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
+ return errPos.norm();
+ }
+
+ float CartesianPositionController::GetOrientationError(const Eigen::Matrix4f& targetPose, const RobotNodePtr& tcp)
+ {
+ Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+ Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
+ Eigen::AngleAxisf aa(oriDir);
+ return aa.angle();
+ }
+
+ bool CartesianPositionController::Reached(const Eigen::Matrix4f& targetPose, const RobotNodePtr& tcp, bool checkOri, float thresholdPosReached, float thresholdOriReached)
+ {
+ return GetPositionError(targetPose, tcp) < thresholdPosReached
+ && (!checkOri || GetOrientationError(targetPose, tcp) < thresholdOriReached);
+ }
+
+ bool CartesianPositionController::reached(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode, float thresholdPosReached, float thresholdOriReached)
+ {
+ if (mode & VirtualRobot::IKSolver::Position)
+ {
+ if (GetPositionError(targetPose, tcp) > thresholdPosReached)
+ {
+ return false;
+ }
+ }
+ if (mode & VirtualRobot::IKSolver::Orientation)
+ {
+ if (GetOrientationError(targetPose, tcp) > thresholdOriReached)
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+
+ Eigen::Vector3f CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose) const
+ {
+ Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
+ return targetPos - tcp->getPositionInRootFrame();
+ }
+
+ Eigen::Vector3f CartesianPositionController::getPositionDiffVec3(const Eigen::Vector3f& targetPosition) const
+ {
+ return targetPosition - tcp->getPositionInRootFrame();
+ }
+
+ Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Matrix4f& targetPose) const
+ {
+ Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+ Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
+ Eigen::AngleAxisf aa(oriDir);
+ return aa.axis() * aa.angle();
+ }
+
+ RobotNodePtr CartesianPositionController::getTcp() const
+ {
+ return tcp;
+ }
+}
diff --git a/VirtualRobot/Controller/CartesianPositionController.h b/VirtualRobot/Controller/CartesianPositionController.h
new file mode 100644
index 000000000..f174e9878
--- /dev/null
+++ b/VirtualRobot/Controller/CartesianPositionController.h
@@ -0,0 +1,62 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include "VirtualRobot/IK/IKSolver.h"
+#include "VirtualRobot/VirtualRobot.h"
+
+namespace VirtualRobot
+{
+ class CartesianPositionController;
+ using CartesianPositionControllerPtr = std::shared_ptr<CartesianPositionController>;
+
+ // This is a P-controller. In: Target pose, out Cartesian velocity.
+ class CartesianPositionController
+ {
+ public:
+ CartesianPositionController(const RobotNodePtr& tcp);
+
+ CartesianPositionController(CartesianPositionController&&) = default;
+ CartesianPositionController& operator=(CartesianPositionController&&) = default;
+
+ Eigen::VectorXf calculate(const Eigen::Matrix4f& targetPose, IKSolver::CartesianSelection mode) const;
+ Eigen::VectorXf calculatePos(const Eigen::Vector3f& targetPos) const;
+
+ float getPositionError(const Eigen::Matrix4f& targetPose) const;
+ float getOrientationError(const Eigen::Matrix4f& targetPose) const;
+ static float GetPositionError(const Eigen::Matrix4f& targetPose, const RobotNodePtr& tcp);
+ static float GetOrientationError(const Eigen::Matrix4f& targetPose, const RobotNodePtr& tcp);
+ static bool Reached(const Eigen::Matrix4f& targetPose, const RobotNodePtr& tcp, bool checkOri, float thresholdPosReached, float thresholdOriReached);
+ bool reached(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode, float thresholdPosReached, float thresholdOriReached);
+
+ Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose) const;
+ Eigen::Vector3f getPositionDiffVec3(const Eigen::Vector3f& targetPosition) const;
+ Eigen::Vector3f getOrientationDiff(const Eigen::Matrix4f& targetPose) const;
+ RobotNodePtr getTcp() const;
+
+ float KpPos = 1;
+ float KpOri = 1;
+ float maxPosVel = -1;
+ float maxOriVel = -1;
+
+
+ private:
+ RobotNodePtr tcp;
+ };
+}
diff --git a/VirtualRobot/Controller/CartesianVelocityController.cpp b/VirtualRobot/Controller/CartesianVelocityController.cpp
new file mode 100644
index 000000000..1fda6c086
--- /dev/null
+++ b/VirtualRobot/Controller/CartesianVelocityController.cpp
@@ -0,0 +1,287 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author ()
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "CartesianVelocityController.h"
+
+#include "VirtualRobot/Robot.h"
+#include "VirtualRobot/math/Helpers.h"
+#include "VirtualRobot/IK/DifferentialIK.h"
+
+namespace VirtualRobot
+{
+
+CartesianVelocityController::CartesianVelocityController(const RobotNodeSetPtr& rns, const RobotNodePtr& tcp, const JacobiProvider::InverseJacobiMethod invJacMethod, bool considerJointLimits)
+ : rns(rns),
+ _considerJointLimits(considerJointLimits)
+{
+ ik.reset(new DifferentialIK(rns, rns->getRobot()->getRootNode(), invJacMethod));
+ _tcp = tcp ? tcp : rns->getTCP();
+
+ _cartesianMMRegularization = 100;
+ _cartesianRadianRegularization = 1;
+ _jointCosts = Eigen::VectorXf::Constant(rns->getSize(), 1);
+}
+
+void CartesianVelocityController::calculateJacobis(IKSolver::CartesianSelection mode)
+{
+ jacobi = ik->getJacobianMatrix(_tcp, mode);
+ _jacobiWithCosts = Eigen::MatrixXf(jacobi.rows(), jacobi.cols());
+ for (int r = 0; r < jacobi.rows(); r++)
+ {
+ for (int c = 0; c < jacobi.cols(); c++)
+ {
+ _jacobiWithCosts(r, c) = jacobi(r, c) / _jointCosts(c);
+ }
+ }
+ _inv = ik->computePseudoInverseJacobianMatrix(_jacobiWithCosts, ik->getJacobiRegularization(mode));
+}
+
+Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, IKSolver::CartesianSelection mode)
+{
+ return calculate(cartesianVel, Eigen::VectorXf::Zero(0), mode);
+ /*calculateJacobis(mode);
+
+ if (_considerJointLimits)
+ {
+ clampJacobiAtJointLimits(mode, cartesianVel, _jacobiWithCosts, _inv);
+ }
+
+ Eigen::VectorXf jointVel = _inv * cartesianVel;
+ jointVel += nsv;
+ for (int r = 0; r < jointVel.rows(); r++)
+ {
+ jointVel(r) = jointVel(r) / _jointCosts(r);
+ }
+
+ if (maximumJointVelocities.rows() > 0)
+ {
+ jointVel = ::math::Helpers::LimitVectorLength(jointVel, maximumJointVelocities);
+ }
+
+ return jointVel;*/
+}
+
+Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, float KpJointLimitAvoidanceScale, IKSolver::CartesianSelection mode)
+{
+ Eigen::VectorXf nullspaceVel = calculateNullspaceVelocity(cartesianVel, KpJointLimitAvoidanceScale, mode);
+ return calculate(cartesianVel, nullspaceVel, mode);
+}
+
+Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, IKSolver::CartesianSelection mode)
+{
+
+ calculateJacobis(mode);
+
+
+ Eigen::VectorXf nsv = Eigen::VectorXf::Zero(rns->getSize());
+
+ if (nullspaceVel.rows() > 0)
+ {
+ Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(_jacobiWithCosts);
+ Eigen::MatrixXf nullspace = lu_decomp.kernel();
+
+ for (int i = 0; i < nullspace.cols(); i++)
+ {
+ float squaredNorm = nullspace.col(i).squaredNorm();
+ // Prevent division by zero
+ if (squaredNorm > 1.0e-32f)
+ {
+ nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
+ }
+ }
+ }
+
+
+ if (_considerJointLimits)
+ {
+ clampJacobiAtJointLimits(mode, cartesianVel, _jacobiWithCosts, _inv);
+ }
+
+
+ Eigen::VectorXf jointVel = _inv * cartesianVel;
+ jointVel += nsv;
+ for (int r = 0; r < jointVel.rows(); r++)
+ {
+ jointVel(r) = jointVel(r) / _jointCosts(r);
+ }
+
+ if (maximumJointVelocities.rows() > 0)
+ {
+ jointVel = math::Helpers::LimitVectorLength(jointVel, maximumJointVelocities);
+ }
+
+ return jointVel;
+}
+
+Eigen::VectorXf CartesianVelocityController::calculateJointLimitAvoidance()
+{
+ Eigen::VectorXf r(rns->getSize());
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ RobotNodePtr rn = rns->getNode(i);
+ if (rn->isLimitless())
+ {
+ r(i) = 0;
+ }
+ else
+ {
+ float f = math::Helpers::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
+ r(i) = cos(f * M_PI);
+ //r(i) = math::MathUtils::Lerp(1.f, -1.f, f);
+ }
+ }
+ return r;
+}
+
+/**
+ * @brief CartesianVelocityController::calculateJointLimitAvoidanceWithMargins
+ * @param margins Vector with same size as rns. Values between 0 (no margin) and 1 (50% low and 50% high margin).
+ * @return
+ */
+Eigen::VectorXf CartesianVelocityController::calculateJointLimitAvoidanceWithMargins(const Eigen::VectorXf& margins)
+{
+ Eigen::VectorXf r(rns->getSize());
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ RobotNodePtr rn = rns->getNode(i);
+ if (rn->isLimitless() || margins(i) <= 0)
+ {
+ r(i) = 0;
+ }
+ else
+ {
+ float lo = rn->getJointLimitLo();
+ float hi = rn->getJointLimitHi();
+ float range = hi - lo;
+ float mrg = math::Helpers::Clamp(0.f, 1.f, margins(i) * range / 2);
+ r(i) = math::Helpers::Clamp(0.f, 1.f, math::Helpers::ILerp(lo + mrg, lo, rn->getJointValue()))
+ + math::Helpers::Clamp(0.f, 1.f, math::Helpers::ILerp(hi, hi - mrg, rn->getJointValue()));
+ }
+ }
+ return r;
+}
+
+Eigen::VectorXf CartesianVelocityController::calculateNullspaceVelocity(const Eigen::VectorXf& cartesianVel, float KpScale, IKSolver::CartesianSelection mode)
+{
+ Eigen::VectorXf regularization = calculateRegularization(mode);
+ // Eigen does not allow product between two column vectors (this fails in Debug mode)
+ // In Release this causes cartesianVel to be only multiplied by the first element of regularization
+ // Eigen::VectorXf vel = cartesianVel * regularization;
+ Eigen::VectorXf vel = cartesianVel.cwiseProduct(regularization);
+
+ return KpScale * vel.norm() * calculateJointLimitAvoidance();
+
+}
+
+void CartesianVelocityController::setCartesianRegularization(float cartesianMMRegularization, float cartesianRadianRegularization)
+{
+ _cartesianMMRegularization = cartesianMMRegularization;
+ _cartesianRadianRegularization = cartesianRadianRegularization;
+}
+
+Eigen::VectorXf CartesianVelocityController::calculateRegularization(IKSolver::CartesianSelection mode)
+{
+ Eigen::VectorXf regularization(6);
+
+ int i = 0;
+
+ if (mode & IKSolver::X)
+ {
+ regularization(i++) = 1 / _cartesianMMRegularization;
+ }
+
+ if (mode & IKSolver::Y)
+ {
+ regularization(i++) = 1 / _cartesianMMRegularization;
+ }
+
+ if (mode & IKSolver::Z)
+ {
+ regularization(i++) = 1 / _cartesianMMRegularization;
+ }
+
+ if (mode & IKSolver::Orientation)
+ {
+ regularization(i++) = 1 / _cartesianRadianRegularization;
+ regularization(i++) = 1 / _cartesianRadianRegularization;
+ regularization(i++) = 1 / _cartesianRadianRegularization;
+ }
+ return regularization.topRows(i);
+}
+
+bool CartesianVelocityController::clampJacobiAtJointLimits(IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, Eigen::MatrixXf& jacobi, Eigen::MatrixXf& inv, 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)
+ {
+ inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
+ }
+
+
+ return modifiedJacobi;
+}
+
+bool CartesianVelocityController::getConsiderJointLimits() const
+{
+ return _considerJointLimits;
+}
+
+void CartesianVelocityController::setConsiderJointLimits(bool value)
+{
+ _considerJointLimits = value;
+}
+
+void CartesianVelocityController::setJointCosts(const std::vector<float>& jointCosts)
+{
+ VR_ASSERT((int)jointCosts.size() == _jointCosts.rows());
+ for (size_t i = 0; i < jointCosts.size(); i++)
+ {
+ _jointCosts(i) = jointCosts.at(i);
+ }
+}
+
+}
+
diff --git a/VirtualRobot/Controller/CartesianVelocityController.h b/VirtualRobot/Controller/CartesianVelocityController.h
new file mode 100644
index 000000000..75af396cb
--- /dev/null
+++ b/VirtualRobot/Controller/CartesianVelocityController.h
@@ -0,0 +1,74 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author ()
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include "VirtualRobot/IK/JacobiProvider.h"
+#include "VirtualRobot/VirtualRobot.h"
+
+namespace VirtualRobot
+{
+ class CartesianVelocityController;
+ using CartesianVelocityControllerPtr = std::shared_ptr<CartesianVelocityController>;
+
+ class CartesianVelocityController
+ {
+ public:
+ CartesianVelocityController(const RobotNodeSetPtr& rns, const RobotNodePtr& tcp = nullptr,
+ const JacobiProvider::InverseJacobiMethod invJacMethod = JacobiProvider::eSVDDamped,
+ bool _considerJointLimits = true);
+
+ CartesianVelocityController(CartesianVelocityController&&) = default;
+ CartesianVelocityController& operator=(CartesianVelocityController&&) = default;
+
+ Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, IKSolver::CartesianSelection mode);
+ Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, float KpJointLimitAvoidanceScale, IKSolver::CartesianSelection mode);
+ Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, IKSolver::CartesianSelection mode);
+ Eigen::VectorXf calculateJointLimitAvoidance();
+ Eigen::VectorXf calculateJointLimitAvoidanceWithMargins(const Eigen::VectorXf& margins);
+ Eigen::VectorXf calculateNullspaceVelocity(const Eigen::VectorXf& cartesianVel, float KpScale, IKSolver::CartesianSelection mode);
+
+ void setCartesianRegularization(float cartesianMMRegularization, float cartesianRadianRegularization);
+
+ bool getConsiderJointLimits() const;
+ void setConsiderJointLimits(bool value);
+
+ Eigen::MatrixXf jacobi;
+ RobotNodeSetPtr rns;
+ DifferentialIKPtr ik;
+ RobotNodePtr _tcp;
+ Eigen::VectorXf maximumJointVelocities;
+
+ void setJointCosts(const std::vector<float>& _jointCosts);
+ Eigen::VectorXf calculateRegularization(VirtualRobot::IKSolver::CartesianSelection mode);
+
+ private:
+ void calculateJacobis(IKSolver::CartesianSelection mode);
+ Eigen::MatrixXf _jacobiWithCosts;
+ Eigen::MatrixXf _inv;
+ bool clampJacobiAtJointLimits(IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, Eigen::MatrixXf& jacobi, Eigen::MatrixXf& _inv, float jointLimitCheckAccuracy = 0.001f);
+ bool _considerJointLimits = true;
+ float _cartesianMMRegularization;
+ float _cartesianRadianRegularization;
+ Eigen::VectorXf _jointCosts;
+ };
+}
+
diff --git a/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp b/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp
new file mode 100644
index 000000000..2af62e09d
--- /dev/null
+++ b/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp
@@ -0,0 +1,525 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "CompositeDiffIK.h"
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/math/Helpers.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <cfloat>
+#include <SimoxUtility/error/SimoxError.h>
+
+namespace VirtualRobot {
+
+CompositeDiffIK::CompositeDiffIK(const RobotNodeSetPtr& rns)
+ : rns(rns), robot(rns->getRobot())
+{
+ ik.reset(new DifferentialIK(rns, rns->getRobot()->getRootNode(), JacobiProvider::eSVDDamped));
+}
+
+void CompositeDiffIK::addTarget(const TargetPtr& target)
+{
+ targets.emplace_back(target);
+}
+
+CompositeDiffIK::TargetPtr CompositeDiffIK::addTarget(const RobotNodePtr& tcp, const Eigen::Matrix4f& target, IKSolver::CartesianSelection mode)
+{
+ TargetPtr t = std::make_shared<Target>(tcp, target, mode);
+ addTarget(t);
+ return t;
+}
+
+void CompositeDiffIK::addNullspaceGradient(const CompositeDiffIK::NullspaceGradientPtr& gradient)
+{
+ nullspaceGradients.emplace_back(gradient);
+ gradient->setMapping(rns);
+}
+
+CompositeDiffIK::NullspaceTargetPtr CompositeDiffIK::addNullspacePositionTarget(const RobotNodePtr& tcp, const Eigen::Vector3f& target)
+{
+ CompositeDiffIK::NullspaceTargetPtr nst(new CompositeDiffIK::NullspaceTarget(rns, tcp, target));
+ addNullspaceGradient(nst);
+ return nst;
+}
+
+
+CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params) {
+ SolveState s;
+ return solve(params, s);
+}
+
+CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params, SolveState &s)
+{
+ if (params.resetRnsValues)
+ {
+ for (RobotNodePtr rn : rns->getAllRobotNodes())
+ {
+ if (rn->isLimitless())
+ {
+ rn->setJointValue(0);
+ }
+ else
+ {
+ rn->setJointValue((rn->getJointLimitHi() + rn->getJointLimitLo()) / 2);
+ }
+ }
+ }
+
+ s.rows = 0;
+ s.cols = rns->getSize();
+
+ for (const TargetPtr& target : targets)
+ {
+ s.rows += CartesianSelectionToSize(target->mode);
+ }
+
+ s.jointRegularization = Eigen::VectorXf::Zero(s.cols);
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ s.jointRegularization(i) = rns->getNode(i)->isTranslationalJoint() ? params.jointRegularizationTranslation : params.jointRegularizationRotation;
+ }
+
+ s.cartesianRegularization = Eigen::VectorXf::Zero(s.rows);
+ {
+ CartesianVelocityController tmpVC(rns);
+ int row = 0;
+ for (const TargetPtr& target : targets)
+ {
+ int h = CartesianSelectionToSize(target->mode);
+ target->jacobi = Eigen::MatrixXf::Zero(h, s.cols);
+ s.cartesianRegularization.block(row, 0, h, 1) = tmpVC.calculateRegularization(target->mode);
+ row += h;
+ }
+ }
+
+ s.jointValues = rns->getJointValuesEigen();
+
+ for (size_t i = 0; i < params.steps; i++)
+ {
+ step(params, s, i);
+ }
+
+ return getLastResult();
+}
+
+CompositeDiffIK::Result CompositeDiffIK::getLastResult() {
+ bool allReached = true;
+ for (const TargetPtr& target : targets)
+ {
+ allReached = allReached && target->pCtrl.reached(target->target, target->mode, target->maxPosError, target->maxOriError);
+ }
+
+ Result result;
+ result.jointValues = rns->getJointValuesEigen();
+ result.reached = allReached;
+ result.jointLimitMargins = Eigen::VectorXf::Zero(rns->getSize());
+ result.minimumJointLimitMargin = FLT_MAX;
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ RobotNodePtr rn = rns->getNode(i);
+ if (rn->isLimitless())
+ {
+ result.jointLimitMargins(i) = M_PI;
+ }
+ else
+ {
+ result.jointLimitMargins(i) = std::min(rn->getJointValue() - rn->getJointLimitLo(), rn->getJointLimitHi() - rn->getJointValue());
+ result.minimumJointLimitMargin = std::min(result.minimumJointLimitMargin, result.jointLimitMargins(i));
+ }
+ }
+
+ return result;
+}
+
+Eigen::MatrixXf CompositeDiffIK::CalculateNullspaceSVD(const Eigen::Matrix4f& jacobi)
+{
+ Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
+ Eigen::MatrixXf nullspaceLU = lu_decomp.kernel();
+ return nullspaceLU;
+}
+
+Eigen::MatrixXf CompositeDiffIK::CalculateNullspaceLU(const Eigen::Matrix4f& jacobi)
+{
+ // cols >= rows
+ int rows = jacobi.rows();
+ int cols = jacobi.cols();
+ Eigen::JacobiSVD<Eigen::MatrixXf> svd(jacobi, Eigen::ComputeFullU | Eigen::ComputeFullV);
+ ///
+ /// \brief V contains right singular vector and nullspace:
+ /// V.shape: (cols,cols)
+ /// singular vectors: V[:,0:rows]
+ /// nullspace: V[:,rows:cols-rows]
+ ///
+ Eigen::MatrixXf V = svd.matrixV();
+ Eigen::MatrixXf nullspaceSVD = V.block(0, rows, cols, cols - rows);
+ return nullspaceSVD;
+}
+
+void CompositeDiffIK::step(CompositeDiffIK::Parameters& params, SolveState& s, int stepNr)
+{
+ s.jacobi = Eigen::MatrixXf::Zero(s.rows, s.cols);
+ s.invJac = Eigen::MatrixXf::Zero(s.cols, s.rows);
+
+ Eigen::VectorXf cartesianVel = Eigen::VectorXf::Zero(s.rows);
+ {
+ int row = 0;
+ for (const TargetPtr& target : targets)
+ {
+ ik->updateJacobianMatrix(target->jacobi, target->tcp, target->mode);
+ int h = CartesianSelectionToSize(target->mode);
+ s.jacobi.block(row, 0, h, s.cols) = target->jacobi;
+ Eigen::Matrix4f targetPose = target->target;
+ Eigen::VectorXf cv = target->pCtrl.calculate(targetPose, target->mode);
+ cartesianVel.block(row, 0, h, 1) = cv;
+ row += h;
+ if (params.returnIKSteps)
+ {
+ TargetStep step;
+ step.tcpPose = target->tcp->getPoseInRootFrame();
+ step.cartesianVel = cv;
+ step.posDiff = target->pCtrl.getPositionDiff(targetPose);
+ step.oriDiff = target->pCtrl.getOrientationDiff(targetPose);
+ target->ikSteps.emplace_back(step);
+ }
+ }
+ }
+
+ for (int row = 0; row < s.rows; row++)
+ {
+ s.jacobi.block(row, 0, 1, s.cols) = s.jacobi.block(row, 0, 1, s.cols).cwiseProduct(s.jointRegularization.transpose());
+ }
+
+ ik->updatePseudoInverseJacobianMatrix(s.invJac, s.jacobi, 0, s.cartesianRegularization);
+
+
+ Eigen::VectorXf nullspaceVel = Eigen::VectorXf::Zero(s.cols);
+
+ for (const NullspaceGradientPtr& nsGradient : nullspaceGradients)
+ {
+ Eigen::VectorXf nsgrad = nsGradient->kP * nsGradient->getGradientAdjusted(params, stepNr);
+ nullspaceVel += nsgrad;
+ }
+ //LimitInfNormTo(nullspaceVel, params.maxJointAngleStep);
+
+ Eigen::JacobiSVD<Eigen::MatrixXf> svd(s.jacobi, Eigen::ComputeFullU | Eigen::ComputeFullV);
+ Eigen::MatrixXf V = svd.matrixV();
+ Eigen::MatrixXf nullspaceSVD = V.block(0, s.rows, s.cols, s.cols - s.rows);
+
+ s.nullspace = nullspaceSVD; // CalculateNullspaceSVD(s.jacobi);
+
+ Eigen::VectorXf nsv = Eigen::VectorXf::Zero(s.cols);
+ for (int i = 0; i < s.nullspace.cols(); i++)
+ {
+ float squaredNorm = s.nullspace.col(i).squaredNorm();
+ // Prevent division by zero
+ if (squaredNorm > 1.0e-32f)
+ {
+ nsv += s.nullspace.col(i) * s.nullspace.col(i).dot(nullspaceVel) / s.nullspace.col(i).squaredNorm();
+ }
+ }
+
+ Eigen::VectorXf jv = s.invJac * cartesianVel;
+ jv = jv + nsv;
+ jv = jv * params.stepSize;
+ jv = LimitInfNormTo(jv, params.maxJointAngleStep, params.maxJointAngleStepIgnore);
+ jv = jv.cwiseProduct(s.jointRegularization);
+
+ Eigen::VectorXf newJointValues = s.jointValues + jv;
+
+ rns->setJointValues(newJointValues);
+ s.jointValues = newJointValues;
+}
+
+int CompositeDiffIK::CartesianSelectionToSize(IKSolver::CartesianSelection mode)
+{
+ switch (mode)
+ {
+ case IKSolver::CartesianSelection::Position:
+ return 3;
+ case IKSolver::CartesianSelection::Orientation:
+ return 3;
+ case IKSolver::CartesianSelection::All:
+ return 6;
+ default:
+ throw std::runtime_error("mode not supported");
+ }
+}
+
+RobotNodeSetPtr CompositeDiffIK::getRobotNodeSet() {
+ return rns;
+}
+
+RobotPtr CompositeDiffIK::getRobot() {
+ return robot;
+}
+
+CompositeDiffIK::Target::Target(const RobotNodePtr& tcp, const Eigen::Matrix4f& target, IKSolver::CartesianSelection mode)
+ : tcp(tcp), target(target), mode(mode), pCtrl(tcp)
+{
+ jacobi = Eigen::MatrixXf::Zero(0, 0);
+}
+
+CompositeDiffIK::NullspaceTarget::NullspaceTarget(const RobotNodeSetPtr& rns, const RobotNodePtr& tcp, const Eigen::Matrix4f& target, IKSolver::CartesianSelection mode)
+ : NullspaceGradient(rns->getNodeNames()), tcp(tcp), target(target), mode(mode), pCtrl(tcp), vCtrl(rns, tcp)
+{
+}
+
+CompositeDiffIK::NullspaceTarget::NullspaceTarget(const RobotNodeSetPtr& rns, const RobotNodePtr& tcp, const Eigen::Vector3f& target)
+ : CompositeDiffIK::NullspaceTarget(rns, tcp, math::Helpers::CreatePose(target, Eigen::Matrix3f::Identity()), IKSolver::CartesianSelection::Position)
+{
+}
+
+void CompositeDiffIK::NullspaceTarget::init(Parameters&)
+{
+ ikSteps.clear();
+}
+
+Eigen::VectorXf CompositeDiffIK::NullspaceTarget::getGradient(Parameters& params, int /*stepNr*/)
+{
+ Eigen::VectorXf cv = pCtrl.calculate(target, mode);
+ Eigen::VectorXf jv = vCtrl.calculate(cv, mode);
+ if (params.returnIKSteps)
+ {
+ NullspaceTargetStep step;
+ step.tcpPose = tcp->getPoseInRootFrame();
+ step.posDiff = pCtrl.getPositionDiff(target);
+ step.oriDiff = pCtrl.getOrientationDiff(target);
+ step.cartesianVel = cv;
+ step.jointVel = jv;
+ ikSteps.emplace_back(step);
+ }
+ return jv;
+}
+
+CompositeDiffIK::AdaptableNullspaceTarget::AdaptableNullspaceTarget(const RobotNodeSetPtr& rns, const RobotNodePtr& tcp, const Eigen::Matrix4f& target, IKSolver::CartesianSelection mode, int startStepNr, int endStepNr) :
+ NullspaceTarget(rns, tcp, target, mode),
+ startStepNr(startStepNr),
+ endStepNr(endStepNr)
+{
+ assert(startStepNr < endStepNr);
+}
+
+CompositeDiffIK::AdaptableNullspaceTarget::AdaptableNullspaceTarget(const RobotNodeSetPtr& rns, const RobotNodePtr& tcp, const Eigen::Vector3f& target, int startStepNr, int endStepNr) :
+ NullspaceTarget(rns, tcp, target),
+ startStepNr(startStepNr),
+ endStepNr(endStepNr)
+{
+ assert(startStepNr < endStepNr);
+}
+
+Eigen::VectorXf CompositeDiffIK::AdaptableNullspaceTarget::getGradient(Parameters& params, int stepNr) {
+ Eigen::VectorXf gradient = NullspaceTarget::getGradient(params, stepNr);
+ float factor = 0;
+ if ((int)params.steps < (endStepNr - startStepNr) || stepNr >= (int)params.steps - startStepNr)
+ factor = 1;
+ else if (stepNr >= startStepNr) {
+ factor = ((float)(stepNr - startStepNr)) / ((int)params.steps - (endStepNr - startStepNr));
+ }
+ return gradient * factor;
+}
+
+Eigen::VectorXf CompositeDiffIK::LimitInfNormTo(Eigen::VectorXf vec, float maxValue, const std::set<std::string> &ignore)
+{
+ Eigen::VectorXf newVec(vec.rows() - ignore.size());
+ int j = 0;
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ auto node = rns->getNode(i);
+ if (ignore.find(node->getName()) == ignore.end()) {
+ newVec(j) = vec(i);
+ j++;
+ }
+ }
+
+ float infNorm = newVec.lpNorm<Eigen::Infinity>();
+ if (infNorm > maxValue)
+ {
+ for (int i = 0; i < vec.rows(); i++)
+ vec(i) = vec(i) / infNorm * maxValue;
+ }
+ return vec;
+}
+
+
+Eigen::VectorXf CompositeDiffIK::LimitInfNormTo(Eigen::VectorXf vec, float maxValue)
+{
+ float infNorm = vec.lpNorm<Eigen::Infinity>();
+ if (infNorm > maxValue)
+ {
+ vec = vec / infNorm * maxValue;
+ }
+ return vec;
+}
+
+CompositeDiffIK::NullspaceJointTarget::NullspaceJointTarget(const RobotNodeSetPtr& rns)
+ : NullspaceGradient(rns->getNodeNames()), rns(rns), target(rns->getJointValuesEigen()), weight(Eigen::VectorXf::Zero(rns->getSize()))
+{
+
+}
+
+CompositeDiffIK::NullspaceJointTarget::NullspaceJointTarget(const RobotNodeSetPtr& rns, const Eigen::VectorXf& target, const Eigen::VectorXf& weight)
+ : NullspaceGradient(rns->getNodeNames()), rns(rns), target(target), weight(weight)
+{
+
+}
+
+void CompositeDiffIK::NullspaceJointTarget::set(int index, float target, float weight)
+{
+ this->target(index) = target;
+ this->weight(index) = weight;
+}
+
+void CompositeDiffIK::NullspaceJointTarget::set(const std::string& jointName, float target, float weight)
+{
+ int index = rns->getRobotNodeIndex(jointName);
+ if (index < 0)
+ {
+ throw std::runtime_error("RobotNodeSet has no joint " + jointName);
+ }
+ set(index, target, weight);
+}
+
+void CompositeDiffIK::NullspaceJointTarget::set(const RobotNodePtr& rn, float target, float weight)
+{
+ int index = rns->getRobotNodeIndex(rn);
+ if (index < 0)
+ {
+ throw std::runtime_error("RobotNodeSet has no joint " + rn->getName());
+ }
+ set(index, target, weight);
+}
+
+void CompositeDiffIK::NullspaceJointTarget::init(CompositeDiffIK::Parameters&)
+{
+
+}
+
+Eigen::VectorXf CompositeDiffIK::NullspaceJointTarget::getGradient(Parameters& /*params*/, int /*stepNr*/)
+{
+ return (target - rns->getJointValuesEigen()).cwiseProduct(weight);
+}
+
+CompositeDiffIK::NullspaceJointLimitAvoidance::NullspaceJointLimitAvoidance(const RobotNodeSetPtr& rns)
+ : NullspaceGradient(rns->getNodeNames()), rns(rns), weight(Eigen::VectorXf::Constant(rns->getSize(), 1))
+{
+
+}
+
+CompositeDiffIK::NullspaceJointLimitAvoidance::NullspaceJointLimitAvoidance(const RobotNodeSetPtr& rns, const Eigen::VectorXf& weight)
+ : NullspaceGradient(rns->getNodeNames()), rns(rns), weight(weight)
+{
+
+}
+
+void CompositeDiffIK::NullspaceJointLimitAvoidance::setWeight(int index, float weight)
+{
+ this->weight(index) = weight;
+}
+
+void CompositeDiffIK::NullspaceJointLimitAvoidance::setWeight(const std::string& jointName, float weight)
+{
+ int index = rns->getRobotNodeIndex(jointName);
+ if (index < 0)
+ {
+ throw std::runtime_error("RobotNodeSet has no joint " + jointName);
+ }
+ setWeight(index, weight);
+}
+
+void CompositeDiffIK::NullspaceJointLimitAvoidance::setWeight(const RobotNodePtr& rn, float weight)
+{
+ int index = rns->getRobotNodeIndex(rn);
+ if (index < 0)
+ {
+ throw std::runtime_error("RobotNodeSet has no joint " + rn->getName());
+ }
+ setWeight(index, weight);
+}
+
+void CompositeDiffIK::NullspaceJointLimitAvoidance::setWeights(const RobotNodeSetPtr& rns, float weight)
+{
+ for (const RobotNodePtr& rn : rns->getAllRobotNodes())
+ {
+ setWeight(rn, weight);
+ }
+}
+
+void CompositeDiffIK::NullspaceJointLimitAvoidance::init(CompositeDiffIK::Parameters&)
+{
+ // Do nothing
+}
+
+Eigen::VectorXf CompositeDiffIK::NullspaceJointLimitAvoidance::getGradient(Parameters& /*params*/, int /*stepNr*/)
+{
+ Eigen::VectorXf r(rns->getSize());
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ RobotNodePtr rn = rns->getNode(i);
+ if (rn->isLimitless())
+ {
+ r(i) = 0;
+ }
+ else
+ {
+ float f = math::Helpers::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
+ r(i) = cos(f * M_PI);
+ }
+ }
+ return r.cwiseProduct(weight);
+}
+
+CompositeDiffIK::NullspaceGradient::NullspaceGradient(const std::vector<std::string> &jointNames) : jointNames(jointNames)
+{
+}
+
+Eigen::VectorXf CompositeDiffIK::NullspaceGradient::getGradientAdjusted(CompositeDiffIK::Parameters ¶ms, int stepNr) {
+ Eigen::VectorXf gradient = getGradient(params, stepNr);
+ if (mapping.empty()) return gradient;
+ else {
+ Eigen::VectorXf adjustedGradient = Eigen::VectorXf::Zero(ikSize);
+ for (const auto &m : mapping) {
+ adjustedGradient(m.second) = gradient(m.first);
+ }
+ return adjustedGradient;
+ }
+}
+
+void CompositeDiffIK::NullspaceGradient::setMapping(const RobotNodeSetPtr &rns) {
+ ikSize = rns->getSize();
+ mapping = getMapping(rns);
+}
+
+std::map<int, int> CompositeDiffIK::NullspaceGradient::getMapping(const RobotNodeSetPtr &rns) {
+ std::map<int, int> mapping;
+ if (this->jointNames != rns->getNodeNames()) {
+ for (unsigned int i = 0; i < jointNames.size(); i++) {
+ int index = rns->getRobotNodeIndex(jointNames.at(i));
+ if (index >= 0) {
+ mapping[i] = index;
+ }
+ else std::cout << "Ignoring null space node " << jointNames.at(i) << " which is not part of ik" << std::endl;
+ }
+ }
+ return mapping;
+}
+
+std::vector<std::string> CompositeDiffIK::NullspaceGradient::getJointNames() const {
+ return jointNames;
+}
+
+}
diff --git a/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h b/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h
new file mode 100644
index 000000000..c341f2ce5
--- /dev/null
+++ b/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h
@@ -0,0 +1,240 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* @author André Meixner (andre dot meixner at kit dot edu)
+* @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Controller/CartesianPositionController.h>
+#include <VirtualRobot/Controller/CartesianVelocityController.h>
+#include <VirtualRobot/Manipulability/AbstractManipulabilityTracking.h>
+
+#include <memory>
+#include <set>
+
+namespace VirtualRobot
+{
+ typedef std::shared_ptr<class CompositeDiffIK> CompositeDiffIKPtr;
+ class CompositeDiffIK
+ {
+ public:
+ struct Parameters
+ {
+ Parameters() {}
+ // IK params
+ size_t steps = 40;
+
+ float maxJointAngleStep = 0.2f;
+ float stepSize = 0.5f;
+ bool resetRnsValues = true;
+ bool returnIKSteps = false;
+ float jointRegularizationTranslation = 1000; // in mm
+ float jointRegularizationRotation = 1;
+
+ std::set<std::string> maxJointAngleStepIgnore = {};
+ };
+
+ class NullspaceGradient
+ {
+ public:
+ NullspaceGradient(const std::vector<std::string> &jointNames);
+
+ virtual void init(Parameters& params) = 0;
+
+ virtual Eigen::VectorXf getGradient(Parameters& params, int stepNr) = 0;
+
+ //! Adjusts the gradient when using different robot node sets for ik solver and nullspace
+ virtual Eigen::VectorXf getGradientAdjusted(Parameters& params, int stepNr);
+
+ void setMapping(const RobotNodeSetPtr &rns);
+
+ //! Returns a mapping from the nullspace robot node set to the robot node set used in the ik solver
+ std::map<int, int> getMapping(const RobotNodeSetPtr &rns);
+
+ float kP = 1;
+
+ std::vector<std::string> getJointNames() const;
+
+ protected:
+ std::vector<std::string> jointNames;
+
+ private:
+ size_t ikSize;
+ std::map<int, int> mapping;
+ };
+ typedef std::shared_ptr<class NullspaceGradient> NullspaceGradientPtr;
+
+ struct NullspaceTargetStep
+ {
+ Eigen::Vector3f posDiff;
+ Eigen::Vector3f oriDiff;
+ Eigen::Matrix4f tcpPose;
+ Eigen::VectorXf cartesianVel;
+ Eigen::VectorXf jointVel;
+ };
+
+ class NullspaceTarget : public NullspaceGradient
+ {
+ public:
+ NullspaceTarget(const RobotNodeSetPtr& rns, const RobotNodePtr& tcp, const Eigen::Matrix4f& target, IKSolver::CartesianSelection mode);
+ NullspaceTarget(const RobotNodeSetPtr& rns, const RobotNodePtr& tcp, const Eigen::Vector3f& target);
+
+ RobotNodePtr tcp;
+ Eigen::Matrix4f target;
+ IKSolver::CartesianSelection mode;
+ CartesianPositionController pCtrl;
+ CartesianVelocityController vCtrl;
+
+ void init(Parameters&) override;
+ Eigen::VectorXf getGradient(Parameters& params, int stepNr) override;
+
+ std::vector<NullspaceTargetStep> ikSteps;
+ };
+ typedef std::shared_ptr<class NullspaceTarget> NullspaceTargetPtr;
+
+ class AdaptableNullspaceTarget : public NullspaceTarget
+ {
+ public:
+ AdaptableNullspaceTarget(const RobotNodeSetPtr& rns, const RobotNodePtr& tcp, const Eigen::Matrix4f& target, IKSolver::CartesianSelection mode, int startStepNr, int endStepNr);
+ AdaptableNullspaceTarget(const RobotNodeSetPtr& rns, const RobotNodePtr& tcp, const Eigen::Vector3f& target, int startStepNr, int endStepNr);
+
+ int startStepNr;
+ int endStepNr;
+
+ Eigen::VectorXf getGradient(Parameters& params, int stepNr) override;
+ };
+
+ typedef std::shared_ptr<class AdaptableNullspaceTarget> AdaptableNullspaceTargetPtr;
+
+ class NullspaceJointTarget : public NullspaceGradient
+ {
+ public:
+ NullspaceJointTarget(const RobotNodeSetPtr& rns);
+ NullspaceJointTarget(const RobotNodeSetPtr& rns, const Eigen::VectorXf& target, const Eigen::VectorXf& weight);
+ void set(int index, float target, float weight);
+ void set(const std::string& jointName, float target, float weight);
+ void set(const RobotNodePtr& rn, float target, float weight);
+
+ RobotNodeSetPtr rns;
+ Eigen::VectorXf target;
+ Eigen::VectorXf weight;
+
+ void init(Parameters&) override;
+ Eigen::VectorXf getGradient(Parameters& params, int stepNr) override;
+ };
+ typedef std::shared_ptr<class NullspaceJointTarget> NullspaceJointTargetPtr;
+
+ class NullspaceJointLimitAvoidance : public NullspaceGradient
+ {
+ public:
+ NullspaceJointLimitAvoidance(const RobotNodeSetPtr& rns);
+ NullspaceJointLimitAvoidance(const RobotNodeSetPtr& rns, const Eigen::VectorXf& weight);
+ void setWeight(int index, float weight);
+ void setWeight(const std::string& jointName, float weight);
+ void setWeight(const RobotNodePtr& rn, float weight);
+ void setWeights(const RobotNodeSetPtr& rns, float weight);
+
+ RobotNodeSetPtr rns;
+ Eigen::VectorXf weight;
+
+ void init(Parameters&) override;
+ Eigen::VectorXf getGradient(Parameters& params, int stepNr) override;
+ };
+ typedef std::shared_ptr<class NullspaceJointLimitAvoidance> NullspaceJointLimitAvoidancePtr;
+
+ struct TargetStep
+ {
+ Eigen::Vector3f posDiff;
+ Eigen::Vector3f oriDiff;
+ Eigen::Matrix4f tcpPose; // in mm or m
+ Eigen::VectorXf cartesianVel;
+ };
+
+ class Target
+ {
+ public:
+ Target(const RobotNodePtr& tcp, const Eigen::Matrix4f& target, IKSolver::CartesianSelection mode);
+
+ RobotNodePtr tcp;
+ Eigen::Matrix4f target;
+ IKSolver::CartesianSelection mode;
+ Eigen::MatrixXf jacobi;
+ CartesianPositionController pCtrl;
+ float maxPosError = 10.f;
+ float maxOriError = 0.05f;
+ std::vector<TargetStep> ikSteps;
+ };
+ typedef std::shared_ptr<Target> TargetPtr;
+
+ struct Result
+ {
+ Eigen::VectorXf jointValues;
+ Eigen::Vector3f posDiff;
+ Eigen::Vector3f oriDiff;
+ float posError;
+ float oriError;
+ bool reached;
+ Eigen::VectorXf jointLimitMargins;
+ float minimumJointLimitMargin;
+ };
+
+ struct SolveState
+ {
+ int rows = 0;
+ int cols = 0;
+ Eigen::VectorXf jointRegularization;
+ Eigen::VectorXf cartesianRegularization;
+ Eigen::VectorXf jointValues;
+ Eigen::MatrixXf jacobi;
+ Eigen::MatrixXf invJac;
+ Eigen::MatrixXf nullspace;
+
+ };
+
+ static Eigen::VectorXf LimitInfNormTo(Eigen::VectorXf vec, float maxValue);
+ Eigen::VectorXf LimitInfNormTo(Eigen::VectorXf vec, float maxValue, const std::set<std::string> &ignore);
+
+ CompositeDiffIK(const RobotNodeSetPtr& rns);
+
+ void addTarget(const TargetPtr& target);
+ TargetPtr addTarget(const RobotNodePtr& tcp, const Eigen::Matrix4f& target, IKSolver::CartesianSelection mode);
+ void addNullspaceGradient(const NullspaceGradientPtr& gradient);
+ NullspaceTargetPtr addNullspacePositionTarget(const RobotNodePtr& tcp, const Eigen::Vector3f& target);
+
+
+ Result solve(Parameters params);
+ Result solve(Parameters params, SolveState &s);
+ static Eigen::MatrixXf CalculateNullspaceSVD(const Eigen::Matrix4f& jacobi);
+ static Eigen::MatrixXf CalculateNullspaceLU(const Eigen::Matrix4f& jacobi);
+ void step(Parameters& params, SolveState& s, int stepNr);
+
+ Result getLastResult();
+
+ int CartesianSelectionToSize(IKSolver::CartesianSelection mode);
+
+ RobotNodeSetPtr getRobotNodeSet();
+
+ RobotPtr getRobot();
+
+ private:
+ RobotNodeSetPtr rns;
+ RobotPtr robot;
+ std::vector<TargetPtr> targets;
+ std::vector<NullspaceGradientPtr> nullspaceGradients;
+ DifferentialIKPtr ik;
+ };
+}
diff --git a/VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.cpp b/VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.cpp
new file mode 100644
index 000000000..f032925f4
--- /dev/null
+++ b/VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.cpp
@@ -0,0 +1,62 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* @author André Meixner (andre dot meixner at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "ManipulabilityNullspaceGradient.h"
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/math/Helpers.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <cfloat>
+
+namespace VirtualRobot
+{
+
+NullspaceManipulability::NullspaceManipulability(AbstractManipulabilityTrackingPtr manipulabilityTracking, float k_gain, const Eigen::MatrixXd& manipulabilityDesired) :
+ CompositeDiffIK::NullspaceGradient(manipulabilityTracking->getJointNames()),
+ manipulabilityTracking(manipulabilityTracking),
+ k_gain(k_gain),
+ manipulabilityDesired(manipulabilityDesired)
+{
+}
+
+double NullspaceManipulability::computeDistance() {
+ return manipulabilityTracking->computeDistance(manipulabilityDesired);
+}
+
+void NullspaceManipulability::init(CompositeDiffIK::Parameters&)
+{
+ // Does not need any initial parameters
+}
+
+Eigen::VectorXf NullspaceManipulability::getGradient(CompositeDiffIK::Parameters& /*params*/, int /*stepNr*/)
+{
+ Eigen::VectorXf velocities = manipulabilityTracking->calculateVelocity(manipulabilityDesired);
+ // check if nan
+ unsigned int nan = 0;
+ for (unsigned int i = 0; i < velocities.rows(); i++) {
+ if (std::isnan(velocities(i))) {
+ velocities(i) = 0;
+ nan++;
+ }
+ }
+ if (nan > 0)
+ std::cout << "Nan in nullspace manipulability velocities: " << nan << "/" << velocities.rows() << " \n";
+ return k_gain * velocities;
+}
+
+}
diff --git a/VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.h b/VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.h
new file mode 100644
index 000000000..3a1b9c6fe
--- /dev/null
+++ b/VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.h
@@ -0,0 +1,47 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* @author André Meixner (andre dot meixner at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+#include "CompositeDiffIK.h"
+#include <VirtualRobot/Manipulability/AbstractManipulabilityTracking.h>
+
+#include <memory>
+#include <set>
+
+
+namespace VirtualRobot
+{
+ class NullspaceManipulability : public CompositeDiffIK::NullspaceGradient
+ {
+ public:
+ NullspaceManipulability(AbstractManipulabilityTrackingPtr manipulabilityTracking,
+ float k_gain, const Eigen::MatrixXd& manipulabilityDesired);
+
+ AbstractManipulabilityTrackingPtr manipulabilityTracking;
+ float k_gain;
+ Eigen::MatrixXd manipulabilityDesired;
+
+ double computeDistance();
+
+ void init(CompositeDiffIK::Parameters&) override;
+ Eigen::VectorXf getGradient(CompositeDiffIK::Parameters& params, int stepNr) override;
+
+ std::vector<CompositeDiffIK::NullspaceTargetStep> ikSteps;
+ };
+ typedef std::shared_ptr<class NullspaceManipulability> NullspaceManipulabilityPtr;
+}
diff --git a/VirtualRobot/IK/CompositeDiffIK/Soechting.h b/VirtualRobot/IK/CompositeDiffIK/Soechting.h
new file mode 100644
index 000000000..03ed4ef4f
--- /dev/null
+++ b/VirtualRobot/IK/CompositeDiffIK/Soechting.h
@@ -0,0 +1,121 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* @author André Meixner (andre dot meixner at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <Eigen/Geometry>
+#include <VirtualRobot/MathTools.h>
+
+namespace VirtualRobot
+{
+struct Soechting
+{
+ enum ArmType
+ {
+ Right = 0, Left = 1
+ };
+
+ struct Arm
+ {
+ Eigen::Vector3f shoulder;
+ float upperArmLength;
+ float forearmLength;
+
+ ArmType armType;
+ };
+
+ struct SoechtingAngles
+ {
+ float SE;
+ float SY;
+ float EE;
+ float EY;
+ };
+
+ struct ForwardPositions
+ {
+ Eigen::Vector3f shoulder;
+ Eigen::Vector3f elbow;
+ Eigen::Vector3f wrist;
+ };
+
+ static SoechtingAngles CalculateAngles(const Eigen::Vector3f& target, const Arm& arm, float scale = 1.0f, bool accuratePointing = false)
+ {
+ Eigen::Vector3f soechtingTarget = ((target - arm.shoulder) / scale);
+ soechtingTarget /= 10.0f; // Soechting is defined in cm
+
+ if (arm.armType == ArmType::Left)
+ {
+ soechtingTarget(0) *= -1;
+ }
+
+ const float R = soechtingTarget.norm();
+ const float Chi = MathTools::rad2deg(std::atan2(soechtingTarget.x(), soechtingTarget.y()));
+ const float Psi = MathTools::rad2deg(std::atan2(soechtingTarget.z(), soechtingTarget.head<2>().norm()));
+
+ SoechtingAngles sa;
+ if (accuratePointing) {
+ // Angles derived from accurate pointing
+ sa.SE = -6.7 + 1.09*R + 1.10*Psi;
+ sa.EE = 47.6 + 0.33*R - 0.95*Psi;
+ sa.EY = -11.5 + 1.27*Chi - 0.54*Psi;
+ sa.SY = 67.7 + 1.00*Chi - 0.68*R;
+ }
+ else {
+ // Angles derived from pointing in the dark
+ sa.SE = -4.0 + 1.10 * R + 0.90 * Psi;
+ sa.EE = 39.4 + 0.54 * R - 1.06 * Psi;
+ sa.SY = 13.2 + 0.86 * Chi + 0.11 * Psi;
+ sa.EY = -10.0 + 1.08 * Chi - 0.35 * Psi;
+ }
+
+ // convert to radian
+ sa.SE = MathTools::deg2rad(sa.SE);
+ sa.SY = MathTools::deg2rad(sa.SY);
+ sa.EE = MathTools::deg2rad(sa.EE);
+ sa.EY = MathTools::deg2rad(sa.EY);
+
+ if (arm.armType == ArmType::Left)
+ {
+ sa.SY = -sa.SY;
+ sa.EY = -sa.EY;
+ }
+
+ return sa;
+ }
+
+ static ForwardPositions CalculateForwardPositions(const SoechtingAngles& sa, const Arm& arm)
+ {
+ const Eigen::AngleAxisf aaSE(sa.SE, Eigen::Vector3f::UnitX());
+ const Eigen::AngleAxisf aaSY(-sa.SY, Eigen::Vector3f::UnitZ());
+ const Eigen::AngleAxisf aaEE(-sa.EE, Eigen::Vector3f::UnitX());
+ const Eigen::AngleAxisf aaEY(-sa.EY, Eigen::Vector3f::UnitZ());
+
+ const Eigen::Vector3f elb = aaSY * aaSE * -Eigen::Vector3f::UnitZ() * arm.upperArmLength;
+ const Eigen::Vector3f wri = aaEY * aaEE * Eigen::Vector3f::UnitZ() * arm.forearmLength;
+
+ ForwardPositions res;
+ res.shoulder = arm.shoulder;
+ res.elbow = arm.shoulder + elb;
+ res.wrist = arm.shoulder + elb + wri;
+
+ return res;
+ }
+};
+
+}
diff --git a/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.cpp b/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.cpp
new file mode 100644
index 000000000..533b14904
--- /dev/null
+++ b/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.cpp
@@ -0,0 +1,114 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* @author André Meixner (andre dot meixner at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "SoechtingNullspaceGradient.h"
+#include "Soechting.h"
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+
+namespace VirtualRobot
+{
+SoechtingNullspaceGradient::SoechtingNullspaceGradient(const CompositeDiffIK::TargetPtr& target, const std::string& shoulderName, const Soechting::ArmType &arm, const ArmJoints& joints) :
+ CompositeDiffIK::NullspaceGradient(joints.getRobotNodeNames()),
+ rns(joints.createRobotNodeSet("Soechting" + std::to_string(arm))),
+ target(target),
+ shoulder(rns->getRobot()->getRobotNode(shoulderName)),
+ arm(arm),
+ joints(joints)
+{
+}
+
+void SoechtingNullspaceGradient::init(CompositeDiffIK::Parameters &) {
+ // Do nothing
+}
+
+Eigen::VectorXf SoechtingNullspaceGradient::getGradient(CompositeDiffIK::Parameters ¶ms, int /*stepNr*/) {
+ const size_t dim = rns->getSize();
+
+ auto sa = calcShoulderAngles(params);
+ Eigen::VectorXf weights = Eigen::VectorXf::Zero(dim);
+ Eigen::VectorXf target = Eigen::VectorXf::Zero(dim);
+
+#define SET(joint, w, t)\
+ if (joint)\
+ {\
+ auto id = rns->getRobotNodeIndex(joint);\
+ weights[id] = w;\
+ target[id] = t;\
+ }
+ SET(joints.clavicula, 0.5f, sa.C)
+ SET(joints.shoulder1, 1, sa.SE)
+ SET(joints.shoulder2, 2, sa.SR)
+ SET(joints.shoulder3, 0.5f, -M_PI / 4)
+ SET(joints.elbow, 0.5f, sa.E)
+#undef SET
+ std::cout << target << std::endl;
+
+ return (target - rns->getJointValuesEigen()).cwiseProduct(weights);
+}
+
+SoechtingNullspaceGradient::ShoulderAngles SoechtingNullspaceGradient::calcShoulderAngles(const CompositeDiffIK::Parameters& /*params*/) const
+{
+ const Eigen::Matrix4f currentShoulder = shoulder->getPoseInRootFrame();
+
+ Soechting::Arm arm;
+ arm.armType = this->arm;
+ arm.shoulder = currentShoulder.block<3, 1>(0, 3);
+ arm.upperArmLength = 0.f;
+ arm.forearmLength = 0.f;
+
+ Eigen::Vector3f targetPosition = target->target.block<3, 1>(0, 3);
+ const auto sa = Soechting::CalculateAngles(targetPosition, arm);
+
+ const Eigen::AngleAxisf aaSE(sa.SE, Eigen::Vector3f::UnitX());
+ const Eigen::AngleAxisf aaSY(-sa.SY, Eigen::Vector3f::UnitZ());
+ const Eigen::AngleAxisf aaEE(-sa.EE, Eigen::Vector3f::UnitX());
+ const Eigen::AngleAxisf aaEY(-sa.EY, Eigen::Vector3f::UnitZ());
+
+ const Eigen::Vector3f elb = Eigen::AngleAxisf(-sa.SE, Eigen::Vector3f::UnitX())
+ * aaSY * aaSE * -Eigen::Vector3f::UnitZ();
+ const float SR = std::atan2(elb(0), -elb(2));
+
+ ShoulderAngles res;
+ res.SR = std::max(-0.1f, SR);
+ res.SE = sa.SE;
+ res.E = sa.EE;
+ res.C = 0;
+ return res;
+}
+
+RobotNodeSetPtr SoechtingNullspaceGradient::ArmJoints::createRobotNodeSet(const std::string &name) const {
+ std::vector<RobotNodePtr> robotNodes;
+ for (auto node : { clavicula, shoulder1, shoulder2, shoulder3, elbow }) {
+ if (node) robotNodes.push_back(node);
+ }
+ if (robotNodes.empty()) return nullptr;
+
+ auto frontNode = robotNodes.front();
+ return RobotNodeSet::createRobotNodeSet(frontNode->getRobot(), name, robotNodes, frontNode);
+}
+
+std::vector<std::string> SoechtingNullspaceGradient::ArmJoints::getRobotNodeNames() const {
+ std::vector<std::string> nodeNames;
+ for (auto node : { clavicula, shoulder1, shoulder2, shoulder3, elbow }) {
+ if (node) nodeNames.push_back(node->getName());
+ }
+ return nodeNames;
+}
+
+}
diff --git a/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h b/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h
new file mode 100644
index 000000000..493082cf3
--- /dev/null
+++ b/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h
@@ -0,0 +1,64 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* @author André Meixner (andre dot meixner at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include "CompositeDiffIK.h"
+#include "Soechting.h"
+
+namespace VirtualRobot
+{
+
+class SoechtingNullspaceGradient : public CompositeDiffIK::NullspaceGradient
+{
+public:
+ struct ArmJoints
+ {
+ RobotNodePtr clavicula = nullptr;
+ RobotNodePtr shoulder1 = nullptr;
+ RobotNodePtr shoulder2 = nullptr;
+ RobotNodePtr shoulder3 = nullptr;
+ RobotNodePtr elbow = nullptr;
+
+ RobotNodeSetPtr createRobotNodeSet(const std::string &name) const;
+ std::vector<std::string> getRobotNodeNames() const;
+ };
+
+ struct ShoulderAngles
+ {
+ float SE, SR, E, C;
+ };
+
+ SoechtingNullspaceGradient(const CompositeDiffIK::TargetPtr& target, const std::string& shoulderName, const Soechting::ArmType &arm, const ArmJoints& joints);
+ virtual ~SoechtingNullspaceGradient() = default;
+
+ void init(CompositeDiffIK::Parameters&) override;
+ Eigen::VectorXf getGradient(CompositeDiffIK::Parameters& params, int stepNr) override;
+
+ RobotNodeSetPtr rns;
+ CompositeDiffIK::TargetPtr target;
+ VirtualRobot::RobotNodePtr shoulder;
+ Soechting::ArmType arm;
+ ArmJoints joints;
+
+ ShoulderAngles calcShoulderAngles(const CompositeDiffIK::Parameters& params) const;
+};
+
+typedef std::shared_ptr<SoechtingNullspaceGradient> SoechtingNullspaceGradientPtr;
+
+}
diff --git a/VirtualRobot/Manipulability/AbstractManipulability.cpp b/VirtualRobot/Manipulability/AbstractManipulability.cpp
new file mode 100644
index 000000000..80773025b
--- /dev/null
+++ b/VirtualRobot/Manipulability/AbstractManipulability.cpp
@@ -0,0 +1,197 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Andre Meixner
+* @author Noémie Jaquier
+* @copyright 2021 Andre Meixner
+* GNU Lesser General Public License
+*
+*/
+#include "AbstractManipulability.h"
+
+#include <Eigen/Dense>
+#include <SimoxUtility/math/convert/pos_quat_to_mat4f.h>
+#include "VirtualRobot/Visualization/VisualizationFactory.h"
+#include "VirtualRobot/Visualization/VisualizationNode.h"
+
+
+namespace VirtualRobot
+{
+
+AbstractManipulability::AbstractManipulability(AbstractManipulability::Mode mode, AbstractManipulability::Type type) :
+ mode(mode),
+ type(type)
+{
+}
+
+Eigen::MatrixXd AbstractManipulability::computeJacobian() {
+ return computeJacobian(getCartesianSelection());
+}
+
+Eigen::MatrixXd AbstractManipulability::computeFullJacobian() {
+ return computeJacobian(VirtualRobot::IKSolver::All);
+}
+
+Eigen::MatrixXd AbstractManipulability::getJacobianSubMatrix(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian) {
+ return GetJacobianSubMatrix(jacobian, mode);
+}
+
+Eigen::MatrixXd AbstractManipulability::getManipulabilitySubMatrix(const Eigen::Matrix<double, 6, 6> &manipulability) {
+ switch (mode) {
+ case AbstractManipulability::Whole:
+ return manipulability;
+ case AbstractManipulability::Position:
+ return manipulability.block(0, 0, 3, 3);
+ case AbstractManipulability::Orientation:
+ return manipulability.block(3, 3, 3, 3);
+ default:
+ throw std::runtime_error("Mode not supported");
+ }
+}
+
+
+Eigen::MatrixXd AbstractManipulability::computeManipulability() {
+ return computeManipulability(computeJacobian());
+}
+
+Eigen::MatrixXd AbstractManipulability::computeManipulability(const Eigen::MatrixXd &jacobian) {
+ return computeManipulability(jacobian, type);
+}
+
+VisualizationNodePtr AbstractManipulability::getManipulabilityVis(const std::string &visualizationType, double scaling) {
+ return getManipulabilityVis(computeManipulability(), visualizationType, scaling);
+}
+
+VisualizationNodePtr AbstractManipulability::getManipulabilityVis(const Eigen::MatrixXd &manipulability, const std::string &visualizationType, double scaling) {
+ return getManipulabilityVis(manipulability, getGlobalPosition(), visualizationType, scaling);
+}
+
+VisualizationNodePtr AbstractManipulability::getManipulabilityVis(const Eigen::MatrixXd &manipulability, const Eigen::Vector3f &position, const std::string &visualizationType, double scaling) {
+ VisualizationFactoryPtr visualizationFactory;
+ if (visualizationType.empty())
+ {
+ visualizationFactory = VisualizationFactory::first(NULL);
+ }
+ else
+ {
+ visualizationFactory = VisualizationFactory::fromName(visualizationType, NULL);
+ }
+ if (!visualizationFactory)
+ {
+ VR_WARNING << "No visualization factory for name " << visualizationType << std::endl;
+ return nullptr;
+ }
+
+ Eigen::Quaternionf orientation;
+ Eigen::Vector3d scale;
+ getEllipsoidOrientationAndScale(manipulability, orientation, scale);
+ scale *= scaling;
+ auto vis = visualizationFactory->createEllipse(scale(0), scale(1), scale(2), false);
+ vis->setGlobalPose(simox::math::pos_quat_to_mat4f(position, orientation));
+ return vis;
+}
+
+void AbstractManipulability::getEllipsoidOrientationAndScale(const Eigen::MatrixXd &manipulability, Eigen::Quaternionf &orientation, Eigen::Vector3d &scale) {
+ Eigen::MatrixXd reduced_manipulability = (mode != Mode::Orientation || manipulability.rows() == 3) ? manipulability.block(0, 0, 3, 3) : manipulability.block(3, 3, 3, 3);
+ Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(reduced_manipulability);
+ Eigen::MatrixXd eigenVectors = eigenSolver.eigenvectors();
+ Eigen::VectorXd eigenValues = eigenSolver.eigenvalues();
+ Eigen::VectorXd v1 = eigenVectors.col(eigenVectors.cols() - 1);
+ Eigen::VectorXd v2 = eigenVectors.col(eigenVectors.cols() - 2);
+
+ orientation = Eigen::Quaterniond::FromTwoVectors(v1, v2).cast<float>();
+
+ // normalize singular values for scaling
+ eigenValues /= eigenValues.sum();
+ for (int i = 0; i < eigenValues.rows(); i++) {
+ if (eigenValues(i) < 0.005) // 5mm
+ eigenValues(i) = 0.005;
+ }
+
+ scale(0) = eigenValues(eigenValues.rows() - 1);
+ scale(1) = eigenValues(eigenValues.rows() - 2);
+ scale(2) = eigenValues(eigenValues.rows() - 3);
+}
+
+void AbstractManipulability::getEllipsoidOrientationAndScale(Eigen::Quaternionf &orientation, Eigen::Vector3d &scale) {
+ getEllipsoidOrientationAndScale(computeManipulability(), orientation, scale);
+}
+
+IKSolver::CartesianSelection AbstractManipulability::getCartesianSelection() {
+ return GetCartesianSelection(mode);
+}
+
+AbstractManipulability::Mode AbstractManipulability::getMode() {
+ return mode;
+}
+
+AbstractManipulability::Type AbstractManipulability::getType() {
+ return type;
+}
+
+int AbstractManipulability::getTaskVars() {
+ switch(mode) {
+ case Whole:
+ return 6;
+ case Position:
+ case Orientation:
+ return 3;
+ default:
+ throw std::runtime_error("Unkown manipulability mode");
+ }
+}
+
+Eigen::MatrixXd AbstractManipulability::GetJacobianSubMatrix(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian, IKSolver::CartesianSelection mode) {
+ switch (mode) {
+ case IKSolver::All:
+ return jacobian;
+ case IKSolver::Position:
+ return jacobian.block(0, 0, 3, jacobian.cols());
+ case IKSolver::Orientation:
+ return jacobian.block(3, 0, 3, jacobian.cols());
+ default:
+ throw std::runtime_error("Mode not supported");
+ }
+}
+
+Eigen::MatrixXd AbstractManipulability::GetJacobianSubMatrix(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian, Mode mode) {
+ switch (mode) {
+ case AbstractManipulability::Whole:
+ return jacobian;
+ case AbstractManipulability::Position:
+ return jacobian.block(0, 0, 3, jacobian.cols());
+ case AbstractManipulability::Orientation:
+ return jacobian.block(3, 0, 3, jacobian.cols());
+ default:
+ throw std::runtime_error("Mode not supported");
+ }
+}
+
+IKSolver::CartesianSelection AbstractManipulability::GetCartesianSelection(AbstractManipulability::Mode mode) {
+ switch(mode) {
+ case Whole:
+ return IKSolver::All;
+ case Position:
+ return IKSolver::Position;
+ case Orientation:
+ return IKSolver::Orientation;
+ default:
+ throw std::runtime_error("Unkown manipulability mode");
+ }
+}
+
+}
diff --git a/VirtualRobot/Manipulability/AbstractManipulability.h b/VirtualRobot/Manipulability/AbstractManipulability.h
new file mode 100644
index 000000000..7b3470473
--- /dev/null
+++ b/VirtualRobot/Manipulability/AbstractManipulability.h
@@ -0,0 +1,102 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Andre Meixner
+* @author Noémie Jaquier
+* @copyright 2021 Andre Meixner
+* GNU Lesser General Public License
+*
+*/
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <unsupported/Eigen/CXX11/Tensor>
+#include "VirtualRobot/IK/IKSolver.h"
+
+namespace VirtualRobot
+{
+
+class AbstractManipulability
+{
+public:
+ enum Type {
+ Velocity = 1, Force = 2
+ };
+
+ enum Mode {
+ Whole = 1, Position = 2, Orientation = 3
+ };
+
+ AbstractManipulability(Mode mode, Type type);
+
+ Eigen::MatrixXd computeJacobian();
+
+ Eigen::MatrixXd computeFullJacobian();
+
+ Eigen::MatrixXd getJacobianSubMatrix(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian);
+
+ Eigen::MatrixXd getManipulabilitySubMatrix(const Eigen::Matrix<double, 6, 6> &manipulability);
+
+ virtual Eigen::MatrixXd computeManipulability();
+
+ Eigen::MatrixXd computeManipulability(const Eigen::MatrixXd &jacobian);
+
+ virtual Eigen::MatrixXd computeManipulability(const Eigen::MatrixXd &jacobian, Type type) = 0;
+
+ VisualizationNodePtr getManipulabilityVis(const std::string &visualizationType = "", double scaling = 1000.0);
+
+ VisualizationNodePtr getManipulabilityVis(const Eigen::MatrixXd &manipulability, const std::string &visualizationType = "", double scaling = 1000.0);
+
+ VisualizationNodePtr getManipulabilityVis(const Eigen::MatrixXd &manipulability, const Eigen::Vector3f &position, const std::string &visualizationType = "", double scaling = 1000.0);
+
+ void getEllipsoidOrientationAndScale(const Eigen::MatrixXd &manipulability, Eigen::Quaternionf &orientation, Eigen::Vector3d &scale);
+
+ void getEllipsoidOrientationAndScale(Eigen::Quaternionf &orientation, Eigen::Vector3d &scale);
+
+ IKSolver::CartesianSelection getCartesianSelection();
+
+ Mode getMode();
+
+ Type getType();
+
+ int getTaskVars();
+
+ virtual void setConvertMMtoM(bool value) = 0;
+
+ virtual Eigen::Vector3f getGlobalPosition() = 0;
+
+ virtual Eigen::Vector3f getLocalPosition() = 0;
+
+ virtual std::vector<std::string> getJointNames() = 0;
+
+ static Eigen::MatrixXd GetJacobianSubMatrix(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian, IKSolver::CartesianSelection mode);
+
+ static Eigen::MatrixXd GetJacobianSubMatrix(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian, Mode mode);
+
+ static IKSolver::CartesianSelection GetCartesianSelection(Mode mode);
+
+protected:
+ virtual Eigen::MatrixXd computeJacobian(IKSolver::CartesianSelection mode) = 0;
+
+ Mode mode;
+ Type type;
+};
+
+typedef std::shared_ptr<AbstractManipulability> AbstractManipulabilityPtr;
+
+}
diff --git a/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp
new file mode 100644
index 000000000..0c8126653
--- /dev/null
+++ b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp
@@ -0,0 +1,200 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Andre Meixner
+* @author Noémie Jaquier
+* @copyright 2021 Andre Meixner
+* GNU Lesser General Public License
+*
+*/
+#include "AbstractManipulabilityTracking.h"
+
+#include <VirtualRobot/Robot.h>
+#include <Eigen/Dense>
+
+#include "AbstractManipulability.h"
+
+namespace VirtualRobot
+{
+
+std::map<std::string, float> AbstractManipulabilityTracking::calculateVelocityMap(const Eigen::MatrixXd &manipulabilityDesired, const std::vector<std::string> &jointNames) {
+ Eigen::VectorXf velocities = calculateVelocity(manipulabilityDesired);
+ std::map<std::string, float> velocityMap;
+ if ((unsigned int)velocities.rows() == jointNames.size()) {
+ for (unsigned int index = 0; index < jointNames.size(); index++) {
+ velocityMap[jointNames.at(index)] = velocities(index);
+ }
+ }
+ return velocityMap;
+}
+
+Eigen::Tensor<double, 3> AbstractManipulabilityTracking::computeJacobianDerivative(const Eigen::MatrixXd &jacobian) {
+ int rows = jacobian.rows(); // task space dim
+ int columns = jacobian.cols(); // joint space dim
+
+ // Calculate derivative of Jacobian, see "Symbolic differentiation of the velocity mapping for a serial kinematic chain", Bruyninck et al., Mechanism and Machine Theory. 1996
+ Eigen::Tensor<double, 3> dJdq(rows,columns,columns);
+ dJdq.setZero();
+ for(int i = 0; i < columns; i++) {
+ for(int j = 0; j < columns; j++) {
+ Eigen::VectorXd J_i = jacobian.col(i);
+ Eigen::VectorXd J_j = jacobian.col(j);
+ if (j < i) {
+ dJdq(0, i, j) = J_j(4) * J_i(2) - J_j(5) * J_i(1);
+ dJdq(1, i, j) = J_j(5) * J_i(0) - J_j(3) * J_i(2);
+ dJdq(2, i, j) = J_j(3) * J_i(1) - J_j(4) * J_i(0);
+ dJdq(3, i, j) = J_j(4) * J_i(5) - J_j(5) * J_i(4);
+ dJdq(4, i, j) = J_j(5) * J_i(3) - J_j(3) * J_i(5);
+ dJdq(5, i, j) = J_j(3) * J_i(4) - J_j(4) * J_i(3);
+ }
+ else if (j > i) {
+ dJdq(0, i, j) = - J_j(1) * J_i(5) + J_j(2) * J_i(4);
+ dJdq(1, i, j) = - J_j(2) * J_i(3) + J_j(0) * J_i(5);
+ dJdq(2, i, j) = - J_j(0) * J_i(4) + J_j(1) * J_i(3);
+ }
+ else {
+ dJdq(0, i, j) = J_i(4) * J_i(2) - J_i(5) * J_i(1);
+ dJdq(1, i, j) = J_i(5) * J_i(0) - J_i(3) * J_i(2);
+ dJdq(2, i, j) = J_i(3) * J_i(1) - J_i(4) * J_i(0);
+ }
+ }
+ }
+ return dJdq;
+}
+
+Eigen::Tensor<double, 3> AbstractManipulabilityTracking::tensorMatrixProduct(const Eigen::Tensor<double, 3> &tensor, const Eigen::MatrixXd &matrix) {
+ Eigen::array<long,2> shape = {tensor.dimension(0), tensor.dimension(1) * tensor.dimension(2)};
+ Eigen::Tensor<double, 2> reshapedTensor = tensor.reshape(shape);
+ Eigen::MatrixXd mat_mult = matrix * Tensor_to_Matrix(reshapedTensor, reshapedTensor.dimension(0), reshapedTensor.dimension(1));
+ Eigen::array<long,3> shape2 = {matrix.rows(), tensor.dimension(1), tensor.dimension(2)};
+ return Matrix_to_Tensor(mat_mult, mat_mult.rows(), mat_mult.cols()).reshape(shape2);
+}
+
+Eigen::Tensor<double, 3> AbstractManipulabilityTracking::tensorMatrixProductPermutate(const Eigen::Tensor<double, 3> &tensor, const Eigen::MatrixXd &matrix) {
+ Eigen::array<int, 3> shuffling({1, 0, 2}); // Permutation of first two dimensions
+ Eigen::Tensor<double, 3> perm_tensor = tensor.shuffle(shuffling);
+ Eigen::array<long,2> shape = {perm_tensor.dimension(0), perm_tensor.dimension(1) * perm_tensor.dimension(2)};
+ Eigen::Tensor<double, 2> reshapedTensor = perm_tensor.reshape(shape);
+ Eigen::MatrixXd mat_mult = matrix * Tensor_to_Matrix(reshapedTensor, reshapedTensor.dimension(0), reshapedTensor.dimension(1));
+ Eigen::array<long,3> shape2 = {matrix.rows(), perm_tensor.dimension(1), perm_tensor.dimension(2)};
+ return Matrix_to_Tensor(mat_mult, mat_mult.rows(), mat_mult.cols()).reshape(shape2).shuffle(shuffling);
+}
+
+Eigen::Tensor<double, 3> AbstractManipulabilityTracking::subCube(Eigen::Tensor<double, 3> &manipulationJacobian) {
+ switch (getMode()) {
+ case AbstractManipulability::Whole:
+ return manipulationJacobian;
+ case AbstractManipulability::Position:
+ {
+ Eigen::array<long,3> offset = {0, 0, 0};
+ Eigen::array<long,3> extent = {3, 3, manipulationJacobian.dimension(2)};
+ return manipulationJacobian.slice(offset, extent);
+ }
+ case AbstractManipulability::Orientation:
+ {
+ Eigen::array<long,3> offset = {3, 3, 0};
+ Eigen::array<long,3> extent = {3, 3, manipulationJacobian.dimension(2)};
+ return manipulationJacobian.slice(offset, extent);
+ }
+ default:
+ throw std::runtime_error("Mode not supported");
+ }
+}
+
+Eigen::MatrixXd AbstractManipulabilityTracking::getDefaultGainMatrix(){
+ Eigen::MatrixXd gainMatrix;
+ switch (getMode()) {
+ case AbstractManipulability::Whole:
+ gainMatrix.setIdentity(21, 21);
+ return gainMatrix;
+ case AbstractManipulability::Position:
+ {
+ gainMatrix.setIdentity(6, 6);
+ return gainMatrix;
+ }
+ case AbstractManipulability::Orientation:
+ {
+ gainMatrix.setIdentity(6, 6);
+ return gainMatrix;
+ }
+ default:
+ throw std::runtime_error("Mode not supported");
+ }
+}
+
+Eigen::MatrixXd AbstractManipulabilityTracking::dampedLeastSquaresInverse(const Eigen::MatrixXd &jacobian, double dampingFactor) {
+ Eigen::MatrixXd k2I = dampingFactor/*std::pow(dampingFactor, 2)*/ * Eigen::MatrixXd::Identity(jacobian.rows(), jacobian.rows());
+ Eigen::MatrixXd JJT = jacobian * jacobian.transpose();
+ Eigen::MatrixXd JJT_k2I = JJT + k2I;
+ Eigen::MatrixXd dampedLeastSquaresInverse = jacobian.transpose() * JJT_k2I.inverse();
+ return dampedLeastSquaresInverse;
+}
+
+Eigen::MatrixXd AbstractManipulabilityTracking::logMap(const Eigen::MatrixXd &manipulabilityDesired, const Eigen::MatrixXd &manipulabilityCurrent) {
+ Eigen::MatrixXd m = manipulabilityCurrent.inverse() * manipulabilityDesired;
+ Eigen::EigenSolver<Eigen::MatrixXd> eigenSolver(m);
+ Eigen::MatrixXd eigenVectors = eigenSolver.eigenvectors().real();
+ Eigen::MatrixXd diagonalM = eigenSolver.eigenvalues().real().array().log().matrix().asDiagonal();
+ return manipulabilityCurrent * eigenVectors * diagonalM * eigenVectors.inverse();
+}
+
+double AbstractManipulabilityTracking::computeDistance(const Eigen::MatrixXd &manipulabilityDesired) {
+ return logMap(manipulabilityDesired, computeCurrentManipulability()).norm();
+}
+
+double AbstractManipulabilityTracking::getDamping(const Eigen::MatrixXd &matrix) {
+ auto svd = Eigen::JacobiSVD(matrix, Eigen::ComputeThinU | Eigen::ComputeThinV);
+ auto sV = svd.singularValues();
+ double minEigenValue = sV(sV.rows()-1, sV.cols()-1);
+ double damping = minEigenValue < 1e-2 ? 1e-2 : 1e-8; // c++ code sets damping to min singular value if smaller
+ return damping;
+}
+
+Eigen::MatrixXd AbstractManipulabilityTracking::computeManipulabilityJacobianMandelNotation(const Eigen::Tensor<double, 3> &manipulabilityJacobian) {
+ int num_task_vars = getTaskVars();
+
+ Eigen::MatrixXd jm_red((num_task_vars * num_task_vars + num_task_vars) / 2, manipulabilityJacobian.dimension(2));
+ jm_red.setZero();
+
+ for (int i = 0; i < manipulabilityJacobian.dimension(2); i++) {
+ Eigen::Tensor<double,2> tensor = manipulabilityJacobian.chip(i, 2);
+ Eigen::MatrixXd sym_matrix = Tensor_to_Matrix(tensor, num_task_vars, num_task_vars);
+ jm_red.col(i) = symMatrixToVector(sym_matrix);
+ }
+ return jm_red;
+}
+
+Eigen::VectorXd AbstractManipulabilityTracking::symMatrixToVector(const Eigen::MatrixXd &sym_matrix) {
+ int num_task_vars = sym_matrix.rows();
+ Eigen::VectorXd v((num_task_vars * num_task_vars + num_task_vars) / 2);
+ // sym matrix to Mandel notation vector
+ Eigen::VectorXd diagonal = sym_matrix.diagonal();
+ int diagSize = diagonal.size();
+ int nextIndex = 0;
+ v.block(nextIndex, 0, diagSize, 1) = diagonal;
+ nextIndex += diagSize;
+ double scalar = std::pow(2, 0.5);
+ for (int i = 1; i < sym_matrix.rows(); i++) {
+ Eigen::VectorXd diagonal_i = sym_matrix.diagonal(i);
+ diagSize = diagonal_i.size();
+ v.block(nextIndex, 0, diagonal_i.size(), 1) = scalar * diagonal_i;
+ nextIndex += diagonal_i.size();
+ }
+ return v;
+}
+
+}
diff --git a/VirtualRobot/Manipulability/AbstractManipulabilityTracking.h b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.h
new file mode 100644
index 000000000..772cd34c4
--- /dev/null
+++ b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.h
@@ -0,0 +1,103 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Andre Meixner
+* @author Noémie Jaquier
+* @copyright 2021 Andre Meixner
+* GNU Lesser General Public License
+*
+*/
+#pragma once
+
+#include <unsupported/Eigen/CXX11/Tensor>
+#include <memory>
+#include "AbstractManipulability.h"
+
+namespace VirtualRobot {
+
+template<typename T>
+using MatrixType = Eigen::Matrix<T,Eigen::Dynamic, Eigen::Dynamic>;
+
+template<typename Scalar,int rank, typename sizeType>
+auto Tensor_to_Matrix(const Eigen::Tensor<Scalar,rank> &tensor,const sizeType rows,const sizeType cols)
+{
+ return Eigen::Map<const MatrixType<Scalar>> (tensor.data(), rows,cols);
+}
+
+template<typename Scalar, typename... Dims>
+auto Matrix_to_Tensor(const MatrixType<Scalar> &matrix, Dims... dims)
+{
+ constexpr int rank = sizeof... (Dims);
+ return Eigen::TensorMap<Eigen::Tensor<const Scalar, rank>>(matrix.data(), {dims...});
+}
+
+/**
+ * Implementation of manipulability tracking, see
+ * N. Jaquier, L. Rozo, D. G. Caldwell and S. Calinon. Geometry-aware Tracking of Manipulability Ellipsoids, in Robotics: Science and Systems (R:SS), 2018. (http://www.roboticsproceedings.org/rss14/p27.pdf)
+ * N. Jaquier, L. Rozo, D. G. Caldwell and S. Calinon. Geometry-aware Manipulability Learning, Tracking and Transfer, International Journal of Robotics Research (IJRR), 2020.
+ * @brief The ManipulabilityTracking class
+ */
+class AbstractManipulabilityTracking
+{
+public:
+ virtual Eigen::VectorXf calculateVelocity(const Eigen::MatrixXd &manipulabilityDesired, Eigen::MatrixXd gainMatrix=Eigen::MatrixXd()) = 0;
+
+ std::map<std::string, float> calculateVelocityMap(const Eigen::MatrixXd &manipulabilityDesired, const std::vector<std::string> &jointNames);
+
+ Eigen::Tensor<double, 3> computeJacobianDerivative(const Eigen::MatrixXd &jacobian);
+
+ Eigen::Tensor<double, 3> tensorMatrixProduct(const Eigen::Tensor<double, 3> &tensor, const Eigen::MatrixXd &matrix);
+
+ Eigen::Tensor<double, 3> tensorMatrixProductPermutate(const Eigen::Tensor<double, 3> &tensor, const Eigen::MatrixXd &matrix);
+
+ Eigen::Tensor<double, 3> subCube(Eigen::Tensor<double, 3> &manipulationJacobian);
+
+ Eigen::MatrixXd getDefaultGainMatrix();
+
+ /*! @brief Calculates the damped least square inverse by J^T * (JJ^T + k^2 I)^{-1}
+ * @jacobian Jacobian J
+ * @dampingFactor Damping factor k
+ */
+ Eigen::MatrixXd dampedLeastSquaresInverse(const Eigen::MatrixXd &jacobian, double dampingFactor = 0.01);
+
+ Eigen::MatrixXd logMap(const Eigen::MatrixXd &manipulabilityDesired, const Eigen::MatrixXd &manipulabilityCurrent);
+
+ double computeDistance(const Eigen::MatrixXd &manipulabilityDesired);
+
+ /*! Calculates damping factor for singularity avoidance */
+ double getDamping(const Eigen::MatrixXd &matrix);
+
+ Eigen::MatrixXd computeManipulabilityJacobianMandelNotation(const Eigen::Tensor<double, 3> &manipulabilityJacobian);
+
+ Eigen::VectorXd symMatrixToVector(const Eigen::MatrixXd &sym_matrix);
+
+ virtual int getTaskVars() = 0;
+
+ virtual AbstractManipulability::Mode getMode() = 0;
+
+ virtual Eigen::MatrixXd computeCurrentManipulability() = 0;
+
+ virtual std::vector<std::string> getJointNames() = 0;
+
+ virtual VisualizationNodePtr getManipulabilityVis(const Eigen::MatrixXd &manipulability, const std::string &visualizationType = "", double scaling = 1000.0) = 0;
+
+ virtual void setConvertMMtoM(bool value) = 0;
+};
+
+typedef std::shared_ptr<AbstractManipulabilityTracking> AbstractManipulabilityTrackingPtr;
+
+}
diff --git a/VirtualRobot/Manipulability/BimanualManipulability.cpp b/VirtualRobot/Manipulability/BimanualManipulability.cpp
new file mode 100644
index 000000000..c7c187cfe
--- /dev/null
+++ b/VirtualRobot/Manipulability/BimanualManipulability.cpp
@@ -0,0 +1,195 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Andre Meixner
+* @author Noémie Jaquier
+* @copyright 2021 Andre Meixner
+* GNU Lesser General Public License
+*
+*/
+
+#include "BimanualManipulability.h"
+
+#include <Visualization/VisualizationFactory.h>
+#include <Visualization/VisualizationNode.h>
+#include "../IK/DifferentialIK.h"
+#include "VirtualRobot/RobotNodeSet.h"
+#include "VirtualRobot/Robot.h"
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <unsupported/Eigen/CXX11/Tensor>
+#include <SimoxUtility/math/convert/pos_quat_to_mat4f.h>
+
+namespace VirtualRobot
+{
+
+BimanualManipulability::BimanualManipulability(const RobotNodeSetPtr &rnsLeft, const RobotNodeSetPtr &rnsRight, Mode mode, Type type, bool convertMMtoM)
+ : BimanualManipulability(rnsLeft, rnsRight, rnsLeft->getTCP(), rnsRight->getTCP(), mode, type, SceneObjectPtr(), RobotNodePtr(), convertMMtoM)
+{
+}
+
+BimanualManipulability::BimanualManipulability(const RobotNodeSetPtr &rnsLeft, const RobotNodeSetPtr &rnsRight, const RobotNodePtr &nodeLeft, const RobotNodePtr &nodeRight, Mode mode, Type type, const SceneObjectPtr& object, const RobotNodePtr &coordSystem, bool convertMMtoM)
+ : AbstractManipulability(mode, type), rnsLeft(rnsLeft), rnsRight(rnsRight), nodeLeft(nodeLeft), nodeRight(nodeRight), object(object), coordSystem(coordSystem)
+{
+ if (rnsLeft->getRobot() != rnsRight->getRobot()) {
+ throw VirtualRobotException("Left and right robot node set of bimanual manipulability do not map to the same robot!");
+ }
+ ikLeft.reset(new DifferentialIK(rnsLeft, coordSystem, JacobiProvider::eSVDDamped));
+ ikLeft->convertModelScalingtoM(convertMMtoM);
+ ikRight.reset(new DifferentialIK(rnsRight, coordSystem, JacobiProvider::eSVDDamped));
+ ikRight->convertModelScalingtoM(convertMMtoM);
+}
+
+Eigen::MatrixXd BimanualManipulability::computeFullJacobianLeft() {
+ return computeJacobianLeft(IKSolver::All);
+}
+
+Eigen::MatrixXd BimanualManipulability::computeFullJacobianRight() {
+ return computeJacobianRight(IKSolver::All);
+}
+
+Eigen::MatrixXd BimanualManipulability::computeBimanualJacobian(const Eigen::MatrixXd &jacobianLeft, const Eigen::MatrixXd &jacobianRight){
+ int nbVarsLeft = jacobianLeft.rows(); // task space dim
+ int nbDofsLeft = jacobianLeft.cols(); // joint space dim
+ int nbVarsRight = jacobianRight.rows(); // task space dim
+ int nbDofsRight = jacobianRight.cols(); // joint space dim
+
+ // Build a block-diagonal matrix with the two Jacobians
+ // Note: we assume here that the kinematic chains of the two arms are independent!
+ Eigen::MatrixXd jacobianBimanual = Eigen::MatrixXd::Zero(nbVarsLeft + nbVarsRight, nbDofsLeft + nbDofsRight);
+ jacobianBimanual.block(0, 0, nbVarsLeft, nbDofsLeft) = jacobianLeft;
+ jacobianBimanual.block(nbVarsLeft, nbDofsLeft, nbVarsRight, nbDofsRight) = jacobianRight;
+
+ return jacobianBimanual;
+}
+
+Eigen::MatrixXd BimanualManipulability::computeManipulability() {
+ return computeManipulability(computeFullJacobian());
+}
+
+Eigen::MatrixXd BimanualManipulability::computeManipulability(const Eigen::MatrixXd &jacobian, Type type) {
+ if (jacobian.rows() != 6)
+ std::runtime_error("computeManipulability(...) of Bimanual Maniulability requires the full jacobian!");
+ return getManipulabilitySubMatrix(computeBimanualManipulability(jacobian, computeBimanualGraspMatrix(), type));
+}
+
+Eigen::Vector3f BimanualManipulability::getLocalPosition() {
+ return rnsLeft->getRobot()->toGlobalCoordinateSystemVec(getGlobalPosition());
+}
+
+Eigen::Vector3f BimanualManipulability::getGlobalPosition() {
+ if (object) return object->getCoMGlobal();
+ else return (nodeLeft->getGlobalPosition() + nodeRight->getGlobalPosition()) / 2.0f;
+}
+
+Eigen::MatrixXd BimanualManipulability::computeBimanualGraspMatrix() {
+ Eigen::Vector3f endeffectorLeft = nodeLeft->getPositionInRootFrame();
+ Eigen::Vector3f endeffectorRight = nodeRight->getPositionInRootFrame();
+ Eigen::MatrixXd rotation = Eigen::MatrixXd::Identity(3, 3);
+ Eigen::MatrixXd bimanualGraspMatrix = computeBimanualGraspMatrix(endeffectorLeft, endeffectorRight, rotation);
+ return bimanualGraspMatrix;
+}
+
+RobotNodeSetPtr BimanualManipulability::getLeftRobotNodeSet() {
+ return rnsLeft;
+}
+
+RobotNodeSetPtr BimanualManipulability::getRightRobotNodeSet() {
+ return rnsRight;
+}
+
+std::vector<std::string> BimanualManipulability::getJointNames() {
+ std::vector<std::string> robotNodeNames = rnsLeft->getNodeNames();
+ auto rightRobotNodeNames = rnsRight->getNodeNames();
+ robotNodeNames.insert(robotNodeNames.end(), rightRobotNodeNames.begin(), rightRobotNodeNames.end());
+ return robotNodeNames;
+}
+
+RobotPtr BimanualManipulability::getRobot() {
+ return rnsLeft->getRobot();
+}
+
+RobotNodeSetPtr BimanualManipulability::createRobotNodeSet(const std::string &name) {
+ return RobotNodeSet::createRobotNodeSet(getRobot(), name, getJointNames(), getRobot()->getRootNode()->getName());
+}
+
+void BimanualManipulability::setConvertMMtoM(bool value) {
+ ikLeft->convertModelScalingtoM(value);
+ ikRight->convertModelScalingtoM(value);
+}
+
+Eigen::MatrixXd BimanualManipulability::computeBimanualManipulability(const Eigen::MatrixXd &bimanualJacobian, const Eigen::MatrixXd &bimanualGraspMatrix) {
+ return computeBimanualManipulability(bimanualJacobian, bimanualGraspMatrix, type);
+}
+
+Eigen::MatrixXd BimanualManipulability::computeBimanualManipulability(const Eigen::MatrixXd &bimanualJacobian, const Eigen::MatrixXd &bimanualGraspMatrix, Type type) {
+ // Compute pseudo-inverse of grasp matrix
+ Eigen::MatrixXd pinverseBimanualGraspMatrix = bimanualGraspMatrix.transpose() * (bimanualGraspMatrix * bimanualGraspMatrix.transpose()).inverse();
+ // Compute manipulability
+ Eigen::MatrixXd velocityManipulability = pinverseBimanualGraspMatrix.transpose() * (bimanualJacobian * bimanualJacobian.transpose()) * pinverseBimanualGraspMatrix;
+
+ switch (type) {
+ case Velocity:
+ return velocityManipulability;
+ case Force:
+ return velocityManipulability.inverse();
+ default:
+ throw std::runtime_error("Unkown manipulability type");
+ }
+}
+
+Eigen::MatrixXd BimanualManipulability::computeJacobian(IKSolver::CartesianSelection mode) {
+ return computeBimanualJacobian(computeJacobianLeft(mode), computeJacobianRight(mode));
+}
+
+Eigen::MatrixXd BimanualManipulability::computeJacobianLeft(IKSolver::CartesianSelection mode) {
+ return ikLeft->getJacobianMatrix(nodeLeft, mode).cast<double>();
+}
+
+Eigen::MatrixXd BimanualManipulability::computeJacobianRight(IKSolver::CartesianSelection mode) {
+ return ikRight->getJacobianMatrix(nodeRight, mode).cast<double>();
+}
+
+Eigen::MatrixXd BimanualManipulability::computeBimanualGraspMatrix(const Eigen::Vector3f &endeffectorLeftPosition, const Eigen::Vector3f &endeffectorRightPosition, const Eigen::MatrixXd &rotation) {
+ Eigen::Vector3f objectPosition = getLocalPosition();
+ // Compute left and right grasp matrices
+ Eigen::MatrixXd graspMatrixLeft = computeGraspMatrix(endeffectorLeftPosition, objectPosition, rotation);
+ Eigen::MatrixXd graspMatrixRight = computeGraspMatrix(endeffectorRightPosition, objectPosition, rotation);
+
+ // Build the bimanual grasp matrix
+ Eigen::MatrixXd graspMatrix = Eigen::MatrixXd::Zero(6, 12);
+ graspMatrix.block(0, 0, 6, 6) = graspMatrixLeft;
+ graspMatrix.block(0, 6, 6, 6) = graspMatrixRight;
+ return graspMatrix;
+}
+
+Eigen::MatrixXd BimanualManipulability::computeGraspMatrix(const Eigen::VectorXf &endeffectorPosition, const Eigen::VectorXf &objectPosition, const Eigen::MatrixXd &rotation) {
+ Eigen::MatrixXd crossProductPosition = Eigen::MatrixXd::Zero(3, 3);
+ crossProductPosition(0, 1) = endeffectorPosition(2) - objectPosition(2);
+ crossProductPosition(0, 2) = objectPosition(1) - endeffectorPosition(1);
+ crossProductPosition(1, 0) = objectPosition(2) - endeffectorPosition(2);
+ crossProductPosition(1, 2) = endeffectorPosition(0) - objectPosition(0);
+ crossProductPosition(2, 0) = endeffectorPosition(1) - objectPosition(1);
+ crossProductPosition(2, 1) = objectPosition(0) - endeffectorPosition(0);
+
+ Eigen::MatrixXd graspMatrix = Eigen::MatrixXd::Zero(6, 6);
+ graspMatrix.block(0, 0, 3, 3) = rotation;
+ graspMatrix.block(3, 3, 3, 3) = rotation;
+ graspMatrix.block(3, 0, 3, 3) = crossProductPosition;
+
+ return graspMatrix;
+}}
diff --git a/VirtualRobot/Manipulability/BimanualManipulability.h b/VirtualRobot/Manipulability/BimanualManipulability.h
new file mode 100644
index 000000000..3d8ac22f8
--- /dev/null
+++ b/VirtualRobot/Manipulability/BimanualManipulability.h
@@ -0,0 +1,105 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Andre Meixner
+* @author Noémie Jaquier
+* @copyright 2021 Andre Meixner
+* GNU Lesser General Public License
+*
+*/
+#pragma once
+
+#include "AbstractManipulability.h"
+
+namespace VirtualRobot
+{
+
+/**
+ * Implementation of manipulability tracking, see
+ * N. Jaquier, L. Rozo, D. G. Caldwell and S. Calinon. Geometry-aware Tracking of Manipulability Ellipsoids, in Robotics: Science and Systems (R:SS), 2018. (http://www.roboticsproceedings.org/rss14/p27.pdf)
+ * N. Jaquier, L. Rozo, D. G. Caldwell and S. Calinon. Geometry-aware Manipulability Learning, Tracking and Transfer, International Journal of Robotics Research (IJRR), 2020.
+ * @brief The Manipulability class
+ */
+class BimanualManipulability : public AbstractManipulability
+{
+public:
+ BimanualManipulability(const RobotNodeSetPtr& rnsLeft, const RobotNodeSetPtr& rnsRight, Mode mode, Type type, bool convertMMtoM = true);
+
+ /**
+ * @param rns The robotNodes (i.e., joints) for which the Jacobians should be calculated.
+ * @param node
+ * @param coordSystem The coordinate system in which the Jacobians are defined. By default the global coordinate system is used.
+ */
+ BimanualManipulability(const RobotNodeSetPtr& rnsLeft, const RobotNodeSetPtr& rnsRight, const RobotNodePtr& nodeLeft, const RobotNodePtr& nodeRight, Mode mode, Type type, const SceneObjectPtr& object = SceneObjectPtr(), const RobotNodePtr& coordSystem = RobotNodePtr(), bool convertMMtoM = true);
+
+ Eigen::MatrixXd computeFullJacobianLeft();
+
+ Eigen::MatrixXd computeFullJacobianRight();
+
+ Eigen::MatrixXd computeBimanualJacobian(const Eigen::MatrixXd &jacobianLeft, const Eigen::MatrixXd &jacobianRight);
+
+ using AbstractManipulability::computeManipulability;
+
+ virtual Eigen::MatrixXd computeManipulability() override;
+
+ virtual Eigen::MatrixXd computeManipulability(const Eigen::MatrixXd &jacobian, Type type) override;
+
+ Eigen::MatrixXd computeBimanualManipulability(const Eigen::MatrixXd &bimanualJacobian, const Eigen::MatrixXd &bimanualGraspMatrix);
+
+ Eigen::MatrixXd computeBimanualManipulability(const Eigen::MatrixXd &bimanualJacobian, const Eigen::MatrixXd &bimanualGraspMatrix, Type type);
+
+ virtual Eigen::Vector3f getLocalPosition() override;
+
+ virtual Eigen::Vector3f getGlobalPosition() override;
+
+ Eigen::MatrixXd computeBimanualGraspMatrix();
+
+ RobotNodeSetPtr getLeftRobotNodeSet();
+
+ RobotNodeSetPtr getRightRobotNodeSet();
+
+ virtual std::vector<std::string> getJointNames() override;
+
+ RobotPtr getRobot();
+
+ RobotNodeSetPtr createRobotNodeSet(const std::string &name = "BimanualManipulabilityTracking");
+
+ virtual void setConvertMMtoM(bool value) override;
+
+protected:
+ virtual Eigen::MatrixXd computeJacobian(IKSolver::CartesianSelection mode) override;
+ Eigen::MatrixXd computeJacobianLeft(IKSolver::CartesianSelection mode);
+
+ Eigen::MatrixXd computeJacobianRight(IKSolver::CartesianSelection mode);
+
+private:
+ Eigen::MatrixXd computeBimanualGraspMatrix(const Eigen::Vector3f &endeffectorLeftPosition, const Eigen::Vector3f &endeffectorRightPosition, const Eigen::MatrixXd &rotation);
+ Eigen::MatrixXd computeGraspMatrix(const Eigen::VectorXf &endeffectorPosition, const Eigen::VectorXf &objectPosition, const Eigen::MatrixXd &rotation);
+
+ RobotNodeSetPtr rnsLeft;
+ RobotNodeSetPtr rnsRight;
+ RobotNodePtr nodeLeft;
+ RobotNodePtr nodeRight;
+ SceneObjectPtr object;
+ RobotNodePtr coordSystem;
+ DifferentialIKPtr ikLeft;
+ DifferentialIKPtr ikRight;
+};
+
+typedef std::shared_ptr<BimanualManipulability> BimanualManipulabilityPtr;
+
+}
diff --git a/VirtualRobot/Manipulability/BimanualManipulabilityTracking.cpp b/VirtualRobot/Manipulability/BimanualManipulabilityTracking.cpp
new file mode 100644
index 000000000..86616c667
--- /dev/null
+++ b/VirtualRobot/Manipulability/BimanualManipulabilityTracking.cpp
@@ -0,0 +1,166 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Andre Meixner
+* @author Noémie Jaquier
+* @copyright 2021 Andre Meixner
+* GNU Lesser General Public License
+*
+*/
+#include "BimanualManipulabilityTracking.h"
+
+#include <SimoxUtility/math/convert/pos_quat_to_mat4f.h>
+#include "BimanualManipulability.h"
+
+namespace VirtualRobot
+{
+
+BimanualManipulabilityTracking::BimanualManipulabilityTracking(BimanualManipulabilityPtr manipulability) : manipulability(manipulability)
+{
+}
+
+Eigen::VectorXf BimanualManipulabilityTracking::calculateVelocity(const Eigen::MatrixXd &manipulabilityDesired, Eigen::MatrixXd gainMatrix) {
+ // Compute left and right Jacobians
+ Eigen::MatrixXd jacobianLeft = manipulability->computeFullJacobianLeft();
+ Eigen::MatrixXd jacobianRight = manipulability->computeFullJacobianRight();
+
+ // Compute bimanual Jacobian
+ Eigen::MatrixXd jacobian = manipulability->computeBimanualJacobian(jacobianLeft, jacobianRight);
+
+ // Compute grasp matrix
+ Eigen::MatrixXd bimanualGraspMatrix = manipulability->computeBimanualGraspMatrix();
+
+ // Compute manipulability
+ Eigen::MatrixXd manipulabilityCurrent = manipulability->getManipulabilitySubMatrix(manipulability->computeBimanualManipulability(jacobian, bimanualGraspMatrix));
+
+ Eigen::MatrixXd manipulability_velocity = logMap(manipulabilityDesired, manipulabilityCurrent);
+ Eigen::MatrixXd manipulability_mandel = symMatrixToVector(manipulability_velocity);
+
+ // Compute manipulability Jacobian
+ Eigen::Tensor<double, 3> manipJ = computeBimanualManipulabilityJacobian(jacobianLeft, jacobianRight, bimanualGraspMatrix);
+ Eigen::Tensor<double, 3> manipJ_sub = subCube(manipJ);
+ Eigen::MatrixXd matManipJ = computeManipulabilityJacobianMandelNotation(manipJ_sub);
+
+ // Compute damped pseudo-inverse of manipulability Jacobian
+ double dampingFactor = getDamping(matManipJ);
+ Eigen::MatrixXd dampedLSI = dampedLeastSquaresInverse(matManipJ, dampingFactor);
+
+ // Set gain matrix
+ if (gainMatrix.rows() == 0)
+ {
+ gainMatrix = getDefaultGainMatrix();
+ }
+
+ // Compute joint velocity
+ Eigen::VectorXd dq = dampedLSI * gainMatrix * manipulability_mandel;
+ return dq.cast<float>();
+}
+
+Eigen::Tensor<double, 3> BimanualManipulabilityTracking::computeBimanualManipulabilityJacobian(const Eigen::MatrixXd &jacobianLeft, const Eigen::MatrixXd &jacobianRight, const Eigen::MatrixXd &bimanualGraspMatrix) {
+ // Jacobians dimensions
+ int nbVarsLeft = jacobianLeft.rows(); // task space dim
+ int nbDofsLeft = jacobianLeft.cols(); // joint space dim
+ int nbVarsRight = jacobianRight.rows(); // task space dim
+ int nbDofsRight = jacobianRight.cols(); // joint space dim
+ int nbVars = nbVarsLeft + nbVarsRight;
+ int nbDofs = nbDofsLeft + nbDofsRight;
+
+ // Compute bimanual jacobian
+ Eigen::MatrixXd bimanualJacobian = manipulability->computeBimanualJacobian(jacobianLeft, jacobianRight);
+
+ // Calculate derivative of Jacobian, see "Symbolic differentiation of the velocity mapping for a serial kinematic chain", Bruyninck et al., Mechanism and Machine Theory. 1996
+ Eigen::Tensor<double, 3> dJdq(nbVars, nbDofs, nbDofs);
+ dJdq = computeBimanualJacobianDerivative(jacobianLeft, jacobianRight);
+
+ // Compute pseudo-inverse of grasp matrix
+ Eigen::MatrixXd pinverseBimanualGraspMatrix = bimanualGraspMatrix.transpose() * (bimanualGraspMatrix*bimanualGraspMatrix.transpose()).inverse();
+
+ // Permutation of first two dimensions
+ Eigen::array<int, 3> shuffling({1, 0, 2}); // Permutation of first two dimensions
+ Eigen::Tensor<double, 3> permdJdq = dJdq.shuffle(shuffling);
+
+ // Compute manipulability jacobian
+ Eigen::Tensor<double, 3> manipJ(nbVars/2, nbVars/2, nbDofs);
+ manipJ = tensorMatrixProductPermutate(tensorMatrixProduct(dJdq, pinverseBimanualGraspMatrix.transpose()), pinverseBimanualGraspMatrix.transpose() * bimanualJacobian)
+ + tensorMatrixProductPermutate(tensorMatrixProduct(permdJdq, pinverseBimanualGraspMatrix.transpose() * bimanualJacobian), pinverseBimanualGraspMatrix.transpose());
+
+ switch (manipulability->getType()) {
+ case AbstractManipulability::Velocity:
+ return manipJ;
+ case AbstractManipulability::Force:
+ {
+ Eigen::MatrixXd kinematicManipulability = manipulability->computeBimanualManipulability(bimanualJacobian, bimanualGraspMatrix, AbstractManipulability::Velocity);
+ return tensorMatrixProductPermutate(tensorMatrixProduct(-manipJ, kinematicManipulability), kinematicManipulability);
+ }
+ default:
+ throw std::runtime_error("Unknown manipulability type");
+ }
+}
+
+Eigen::Tensor<double, 3> BimanualManipulabilityTracking::computeBimanualJacobianDerivative(const Eigen::MatrixXd &jacobianLeft, const Eigen::MatrixXd &jacobianRight) {
+ // Jacobians dimensions
+ int nbVarsLeft = jacobianLeft.rows(); // task space dim
+ int nbDofsLeft = jacobianLeft.cols(); // joint space dim
+ int nbVarsRight = jacobianRight.rows(); // task space dim
+ int nbDofsRight = jacobianRight.cols(); // joint space dim
+
+ // Compute the derivative of each Jacobian
+ Eigen::Tensor<double, 3> dJdqLeft = computeJacobianDerivative(jacobianLeft);
+ Eigen::Tensor<double, 3> dJdqRight = computeJacobianDerivative(jacobianRight);
+
+ // Build the a block-diagonal tensor out of the two Jacobian derivatives
+ Eigen::Tensor<double, 3> dJdqBimanual(nbVarsLeft + nbVarsRight, nbDofsLeft + nbDofsRight, nbDofsLeft + nbDofsRight);
+ dJdqBimanual.setZero();
+ Eigen::array<long,3> offset = {0,0,0};
+ Eigen::array<long,3> extent = {nbVarsLeft,nbDofsLeft, nbDofsLeft};
+ dJdqBimanual.slice(offset, extent) = dJdqLeft;
+ offset = {nbVarsLeft,nbDofsLeft, nbDofsLeft};
+ extent = {nbVarsRight,nbDofsRight, nbDofsRight};
+ dJdqBimanual.slice(offset, extent) = dJdqRight;
+
+ return dJdqBimanual;
+}
+
+int BimanualManipulabilityTracking::getTaskVars() {
+ return manipulability->getTaskVars();
+}
+
+AbstractManipulability::Mode BimanualManipulabilityTracking::getMode() {
+ return manipulability->getMode();
+}
+
+BimanualManipulabilityPtr BimanualManipulabilityTracking::getManipulability() {
+ return manipulability;
+}
+
+Eigen::MatrixXd BimanualManipulabilityTracking::computeCurrentManipulability() {
+ return manipulability->computeManipulability();
+}
+
+std::vector<std::string> BimanualManipulabilityTracking::getJointNames() {
+ return manipulability->getJointNames();
+}
+
+VisualizationNodePtr BimanualManipulabilityTracking::getManipulabilityVis(const Eigen::MatrixXd &manipulability, const std::string &visualizationType, double scaling) {
+ return this->manipulability->getManipulabilityVis(manipulability, visualizationType, scaling);
+}
+
+void BimanualManipulabilityTracking::setConvertMMtoM(bool value) {
+ manipulability->setConvertMMtoM(value);
+}
+
+}
diff --git a/VirtualRobot/Manipulability/BimanualManipulabilityTracking.h b/VirtualRobot/Manipulability/BimanualManipulabilityTracking.h
new file mode 100644
index 000000000..43977395e
--- /dev/null
+++ b/VirtualRobot/Manipulability/BimanualManipulabilityTracking.h
@@ -0,0 +1,71 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Andre Meixner
+* @author Noémie Jaquier
+* @copyright 2021 Andre Meixner
+* GNU Lesser General Public License
+*
+*/
+#pragma once
+
+#include "AbstractManipulabilityTracking.h"
+
+namespace VirtualRobot
+{
+
+class BimanualManipulability;
+typedef std::shared_ptr<BimanualManipulability> BimanualManipulabilityPtr;
+
+/**
+ * Implementation of manipulability tracking, see
+ * N. Jaquier, L. Rozo, D. G. Caldwell and S. Calinon. Geometry-aware Tracking of Manipulability Ellipsoids, in Robotics: Science and Systems (R:SS), 2018. (http://www.roboticsproceedings.org/rss14/p27.pdf)
+ * N. Jaquier, L. Rozo, D. G. Caldwell and S. Calinon. Geometry-aware Manipulability Learning, Tracking and Transfer, International Journal of Robotics Research (IJRR), 2020.
+ * @brief The ManipulabilityTracking class
+ */
+class BimanualManipulabilityTracking : public AbstractManipulabilityTracking
+{
+public:
+ BimanualManipulabilityTracking(BimanualManipulabilityPtr manipulability);
+
+ Eigen::VectorXf calculateVelocity(const Eigen::MatrixXd &manipulabilityDesired, Eigen::MatrixXd gainMatrix=Eigen::MatrixXd()) override;
+
+ Eigen::Tensor<double, 3> computeBimanualManipulabilityJacobian(const Eigen::MatrixXd &jacobianLeft, const Eigen::MatrixXd &jacobianRight, const Eigen::MatrixXd &bimanualGraspMatrix);
+
+ Eigen::Tensor<double, 3> computeBimanualJacobianDerivative(const Eigen::MatrixXd &jacobianLeft, const Eigen::MatrixXd &jacobianRight);
+
+ virtual int getTaskVars() override;
+
+ virtual AbstractManipulability::Mode getMode() override;
+
+ BimanualManipulabilityPtr getManipulability();
+
+ Eigen::MatrixXd computeCurrentManipulability() override;
+
+ virtual std::vector<std::string> getJointNames() override;
+
+ VisualizationNodePtr getManipulabilityVis(const Eigen::MatrixXd &manipulability, const std::string &visualizationType = "", double scaling = 1000.0) override;
+
+ void setConvertMMtoM(bool value) override;
+
+protected:
+ BimanualManipulabilityPtr manipulability;
+};
+
+typedef std::shared_ptr<BimanualManipulabilityTracking> BimanualManipulabilityTrackingPtr;
+
+}
diff --git a/VirtualRobot/Manipulability/SingleChainManipulability.cpp b/VirtualRobot/Manipulability/SingleChainManipulability.cpp
new file mode 100644
index 000000000..e2e93fe45
--- /dev/null
+++ b/VirtualRobot/Manipulability/SingleChainManipulability.cpp
@@ -0,0 +1,140 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Andre Meixner
+* @author Noémie Jaquier
+* @copyright 2021 Andre Meixner
+* GNU Lesser General Public License
+*
+*/
+#include "SingleChainManipulability.h"
+
+#include <Visualization/VisualizationFactory.h>
+#include <Visualization/VisualizationNode.h>
+#include <Eigen/Dense>
+#include "../IK/DifferentialIK.h"
+#include "VirtualRobot/RobotNodeSet.h"
+#include "VirtualRobot/Robot.h"
+#include <SimoxUtility/math/convert/pos_quat_to_mat4f.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+
+
+namespace VirtualRobot
+{
+
+Eigen::MatrixXd AbstractSingleChainManipulability::computeManipulability(const Eigen::MatrixXd &jacobian, AbstractManipulability::Type type) {
+ Eigen::MatrixXd velocityManipulability = jacobian * jacobian.transpose();
+
+ switch (type) {
+ case Velocity:
+ return velocityManipulability;
+ case Force:
+ return velocityManipulability.inverse();
+ default:
+ throw std::runtime_error("Unkown manipulability type");
+ }
+}
+
+
+
+
+SingleRobotNodeSetManipulability::SingleRobotNodeSetManipulability(const RobotNodeSetPtr &rns, Mode mode, Type type, bool convertMMtoM)
+ : SingleRobotNodeSetManipulability(rns, rns->getTCP(), mode, type, RobotNodePtr(), convertMMtoM)
+{
+}
+
+SingleRobotNodeSetManipulability::SingleRobotNodeSetManipulability(const RobotNodeSetPtr &rns, const RobotNodePtr &node, Mode mode, Type type, const RobotNodePtr &coordSystem, bool convertMMtoM)
+ : AbstractSingleChainManipulability(mode, type), rns(rns), node(node), coordSystem(coordSystem)
+{
+ ik.reset(new DifferentialIK(rns, coordSystem, JacobiProvider::eSVDDamped));
+ ik->convertModelScalingtoM(convertMMtoM);
+}
+
+RobotNodeSetPtr SingleRobotNodeSetManipulability::getRobotNodeSet() {
+ return rns;
+}
+
+RobotPtr SingleRobotNodeSetManipulability::getRobot() {
+ return rns->getRobot();
+}
+
+void SingleRobotNodeSetManipulability::setConvertMMtoM(bool value) {
+ ik->convertModelScalingtoM(value);
+}
+
+Eigen::Vector3f SingleRobotNodeSetManipulability::getLocalPosition() {
+ return rns->getTCP()->getPositionInRootFrame();
+}
+
+Eigen::Vector3f SingleRobotNodeSetManipulability::getGlobalPosition() {
+ return rns->getTCP()->getGlobalPosition();
+}
+
+std::vector<std::string> SingleRobotNodeSetManipulability::getJointNames() {
+ return rns->getNodeNames();
+}
+
+Eigen::MatrixXd SingleRobotNodeSetManipulability::computeJacobian(IKSolver::CartesianSelection mode) {
+ return ik->getJacobianMatrix(node, mode).cast<double>();
+}
+
+
+
+
+SingleChainManipulability::SingleChainManipulability(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian,
+ AbstractManipulability::Mode mode, AbstractManipulability::Type type,
+ const std::vector<std::string> &jointNames,
+ const Eigen::Vector3f &globalPosition, const Eigen::Vector3f &localPosition) :
+ AbstractSingleChainManipulability(mode, type),
+ jacobian(jacobian),
+ jointNames(jointNames),
+ globalPosition(globalPosition),
+ localPosition(localPosition)
+{
+}
+
+Eigen::Vector3f SingleChainManipulability::getLocalPosition() {
+ return globalPosition;
+}
+
+Eigen::Vector3f SingleChainManipulability::getGlobalPosition() {
+ return localPosition;
+}
+
+std::vector<std::string> SingleChainManipulability::getJointNames() {
+ return jointNames;
+}
+
+void SingleChainManipulability::setJacobian(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian) {
+ this->jacobian = jacobian;
+}
+
+void SingleChainManipulability::setLocalPosition(const Eigen::Vector3f &localPosition) {
+ this->localPosition = localPosition;
+}
+
+void SingleChainManipulability::setGlobalPosition(const Eigen::Vector3f &globalPosition) {
+ this->globalPosition = globalPosition;
+}
+
+Eigen::MatrixXd SingleChainManipulability::computeJacobian(IKSolver::CartesianSelection mode) {
+ return GetJacobianSubMatrix(jacobian, mode);
+}
+
+
+
+}
diff --git a/VirtualRobot/Manipulability/SingleChainManipulability.h b/VirtualRobot/Manipulability/SingleChainManipulability.h
new file mode 100644
index 000000000..871e61d64
--- /dev/null
+++ b/VirtualRobot/Manipulability/SingleChainManipulability.h
@@ -0,0 +1,120 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Andre Meixner
+* @author Noémie Jaquier
+* @copyright 2021 Andre Meixner
+* GNU Lesser General Public License
+*
+*/
+#pragma once
+
+#include "AbstractManipulability.h"
+
+namespace VirtualRobot
+{
+
+
+class AbstractSingleChainManipulability : public AbstractManipulability
+{
+public:
+ using AbstractManipulability::AbstractManipulability;
+
+ using AbstractManipulability::computeManipulability;
+
+ virtual Eigen::MatrixXd computeManipulability(const Eigen::MatrixXd &jacobian, Type type) override;
+};
+
+typedef std::shared_ptr<AbstractSingleChainManipulability> AbstractSingleChainManipulabilityPtr;
+
+
+
+class SingleRobotNodeSetManipulability : public AbstractSingleChainManipulability
+{
+public:
+ SingleRobotNodeSetManipulability(const RobotNodeSetPtr& rns, Mode mode, Type type, bool convertMMtoM = true);
+
+ /**
+ * @param rns The robotNodes (i.e., joints) for which the Jacobians should be calculated.
+ * @param node
+ * @param coordSystem The coordinate system in which the Jacobians are defined. By default the global coordinate system is used.
+ */
+ SingleRobotNodeSetManipulability(const RobotNodeSetPtr& rns, const RobotNodePtr& node, Mode mode, Type type, const RobotNodePtr& coordSystem = RobotNodePtr(), bool convertMMtoM = true);
+
+ RobotNodeSetPtr getRobotNodeSet();
+
+ virtual RobotPtr getRobot();
+
+ virtual void setConvertMMtoM(bool value) override;
+
+ virtual Eigen::Vector3f getLocalPosition() override;
+
+ virtual Eigen::Vector3f getGlobalPosition() override;
+
+ virtual std::vector<std::string> getJointNames() override;
+
+protected:
+ virtual Eigen::MatrixXd computeJacobian(IKSolver::CartesianSelection mode) override;
+
+private:
+ RobotNodeSetPtr rns;
+ RobotNodePtr node;
+ RobotNodePtr coordSystem;
+ DifferentialIKPtr ik;
+};
+
+typedef std::shared_ptr<SingleRobotNodeSetManipulability> SingleRobotNodeSetManipulabilityPtr;
+
+
+
+class SingleChainManipulability : public AbstractSingleChainManipulability
+{
+public:
+ SingleChainManipulability(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian, Mode mode, Type type,
+ const std::vector<std::string> &jointNames = std::vector<std::string>(),
+ const Eigen::Vector3f &globalPosition = Eigen::Vector3f::Zero(), const Eigen::Vector3f &localPosition = Eigen::Vector3f::Zero());
+
+ virtual Eigen::Vector3f getLocalPosition() override;
+
+ virtual Eigen::Vector3f getGlobalPosition() override;
+
+ virtual std::vector<std::string> getJointNames() override;
+
+ void setJacobian(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian);
+
+ void setLocalPosition(const Eigen::Vector3f &localPosition);
+
+ void setGlobalPosition(const Eigen::Vector3f &globalPosition);
+
+ virtual void setConvertMMtoM(bool /*value*/) override {
+ // do nothing
+ }
+
+protected:
+ virtual Eigen::MatrixXd computeJacobian(IKSolver::CartesianSelection mode) override;
+
+private:
+ Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian;
+ std::vector<std::string> jointNames;
+ Eigen::Vector3f globalPosition;
+ Eigen::Vector3f localPosition;
+};
+
+typedef std::shared_ptr<SingleChainManipulability> SingleChainManipulabilityPtr;
+
+
+}
diff --git a/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp b/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp
new file mode 100644
index 000000000..9edd01a32
--- /dev/null
+++ b/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp
@@ -0,0 +1,119 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Andre Meixner
+* @author Noémie Jaquier
+* @copyright 2021 Andre Meixner
+* GNU Lesser General Public License
+*
+*/
+#include "SingleChainManipulabilityTracking.h"
+
+#include "SingleChainManipulability.h"
+
+namespace VirtualRobot
+{
+
+SingleChainManipulabilityTracking::SingleChainManipulabilityTracking(AbstractSingleChainManipulabilityPtr manipulability) : manipulability(manipulability)
+{
+}
+
+Eigen::Tensor<double, 3> SingleChainManipulabilityTracking::computeManipulabilityJacobian(const Eigen::MatrixXd &jacobian) {
+ Eigen::Tensor<double, 3> derivJ = computeJacobianDerivative(jacobian);
+
+ Eigen::array<int, 3> shuffling({1, 0, 2}); // Permutation of first two dimensions
+ Eigen::Tensor<double, 3> permDerivJ = derivJ.shuffle(shuffling);
+
+ // Compute dJ/dq x_2 J
+ Eigen::Tensor<double, 3> dJtdq_J = tensorMatrixProduct(permDerivJ, jacobian);
+
+ // Compute dJ'/dq x_1 J + dJ/dq x_2 J
+ int rows = jacobian.rows(); // task space dim
+ int columns = jacobian.cols(); // joint space dim
+ Eigen::array<int, 2> shufflingTranpose({1, 0}); // for transposing
+ Eigen::Tensor<double, 3> manipJ(rows,rows,columns);
+ for(int s = 0; s < columns; ++s) {
+ Eigen::Tensor<double, 2> a = dJtdq_J.chip(s, 2);
+ Eigen::Tensor<double, 2> b = dJtdq_J.chip(s, 2);
+ Eigen::Tensor<double, 2> b_trans = b.shuffle(shufflingTranpose);
+ manipJ.chip(s, 2) = a + b_trans;
+ }
+
+ switch (manipulability->getType()) {
+ case AbstractManipulability::Velocity:
+ return manipJ;
+ case AbstractManipulability::Force:
+ {
+ Eigen::MatrixXd kinematicManipulability = manipulability->computeManipulability(jacobian, AbstractManipulability::Velocity);
+ return tensorMatrixProductPermutate(tensorMatrixProduct(-manipJ, kinematicManipulability), kinematicManipulability);
+ }
+ default:
+ throw std::runtime_error("Unknown manipulability type");
+ }
+}
+
+Eigen::VectorXf SingleChainManipulabilityTracking::calculateVelocity(const Eigen::MatrixXd &manipulabilityDesired, Eigen::MatrixXd gainMatrix) {
+ Eigen::MatrixXd jacobian = manipulability->computeFullJacobian(); // full jacobian is required for derivative
+ Eigen::MatrixXd jacobian_sub = manipulability->getJacobianSubMatrix(jacobian);
+ Eigen::MatrixXd manipulabilityCurrent = manipulability->computeManipulability(jacobian_sub);
+ Eigen::MatrixXd manipulability_velocity = logMap(manipulabilityDesired, manipulabilityCurrent);
+ Eigen::Tensor<double, 3> manipJ = computeManipulabilityJacobian(jacobian);
+ Eigen::Tensor<double, 3> manipJ_sub = subCube(manipJ);
+
+ Eigen::MatrixXd matManipJ = computeManipulabilityJacobianMandelNotation(manipJ_sub);
+ double dampingFactor = getDamping(matManipJ);
+ Eigen::MatrixXd dampedLSI = dampedLeastSquaresInverse(matManipJ, dampingFactor);
+ Eigen::MatrixXd manipulability_mandel = symMatrixToVector(manipulability_velocity);
+
+ if (gainMatrix.rows() == 0)
+ {
+ gainMatrix = getDefaultGainMatrix();
+ }
+
+ Eigen::VectorXd dq = dampedLSI * gainMatrix * manipulability_mandel;
+ return dq.cast<float>();
+}
+
+int SingleChainManipulabilityTracking::getTaskVars() {
+ return manipulability->getTaskVars();
+}
+
+AbstractManipulability::Mode SingleChainManipulabilityTracking::getMode() {
+ return manipulability->getMode();
+}
+
+AbstractSingleChainManipulabilityPtr SingleChainManipulabilityTracking::getManipulability() {
+ return manipulability;
+}
+
+Eigen::MatrixXd SingleChainManipulabilityTracking::computeCurrentManipulability() {
+ return manipulability->computeManipulability();
+}
+
+std::vector<std::string> SingleChainManipulabilityTracking::getJointNames() {
+ return manipulability->getJointNames();
+}
+
+VisualizationNodePtr SingleChainManipulabilityTracking::getManipulabilityVis(const Eigen::MatrixXd &manipulability, const std::string &visualizationType, double scaling) {
+ return this->manipulability->getManipulabilityVis(manipulability, visualizationType, scaling);
+}
+
+void SingleChainManipulabilityTracking::setConvertMMtoM(bool value) {
+ manipulability->setConvertMMtoM(value);
+}
+
+}
diff --git a/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h b/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h
new file mode 100644
index 000000000..dedb6647d
--- /dev/null
+++ b/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h
@@ -0,0 +1,68 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Andre Meixner
+* @author Noémie Jaquier
+* @copyright 2021 Andre Meixner
+* GNU Lesser General Public License
+*
+*/
+#pragma once
+
+#include "AbstractManipulabilityTracking.h"
+
+namespace VirtualRobot
+{
+
+typedef std::shared_ptr<class AbstractSingleChainManipulability> AbstractSingleChainManipulabilityPtr;
+
+/**
+ * Implementation of manipulability tracking, see
+ * N. Jaquier, L. Rozo, D. G. Caldwell and S. Calinon. Geometry-aware Tracking of Manipulability Ellipsoids, in Robotics: Science and Systems (R:SS), 2018. (http://www.roboticsproceedings.org/rss14/p27.pdf)
+ * N. Jaquier, L. Rozo, D. G. Caldwell and S. Calinon. Geometry-aware Manipulability Learning, Tracking and Transfer, International Journal of Robotics Research (IJRR), 2020.
+ * @brief The ManipulabilityTracking class
+ */
+class SingleChainManipulabilityTracking : public AbstractManipulabilityTracking
+{
+public:
+ SingleChainManipulabilityTracking(AbstractSingleChainManipulabilityPtr manipulability);
+
+ Eigen::Tensor<double, 3> computeManipulabilityJacobian(const Eigen::MatrixXd &jacobian);
+
+ Eigen::VectorXf calculateVelocity(const Eigen::MatrixXd &manipulabilityDesired, Eigen::MatrixXd gainMatrix=Eigen::MatrixXd()) override;
+
+ virtual int getTaskVars() override;
+
+ virtual AbstractManipulability::Mode getMode() override;
+
+ AbstractSingleChainManipulabilityPtr getManipulability();
+
+ Eigen::MatrixXd computeCurrentManipulability() override;
+
+ virtual std::vector<std::string> getJointNames() override;
+
+ VisualizationNodePtr getManipulabilityVis(const Eigen::MatrixXd &manipulability, const std::string &visualizationType = "", double scaling = 1000) override;
+
+ void setConvertMMtoM(bool value) override;
+
+protected:
+ AbstractSingleChainManipulabilityPtr manipulability;
+};
+
+typedef std::shared_ptr<SingleChainManipulabilityTracking> SingleChainManipulabilityTrackingPtr;
+
+}
diff --git a/VirtualRobot/Workspace/Manipulability.h b/VirtualRobot/Workspace/Manipulability.h
index dcf3d6a85..1212867c0 100644
--- a/VirtualRobot/Workspace/Manipulability.h
+++ b/VirtualRobot/Workspace/Manipulability.h
@@ -1,4 +1,4 @@
-/**
+/*
* This file is part of Simox.
*
* Simox is free software; you can redistribute it and/or modify
diff --git a/VirtualRobot/examples/RobotViewer/CMakeLists.txt b/VirtualRobot/examples/RobotViewer/CMakeLists.txt
index 39c04c6be..54a3035aa 100644
--- a/VirtualRobot/examples/RobotViewer/CMakeLists.txt
+++ b/VirtualRobot/examples/RobotViewer/CMakeLists.txt
@@ -3,17 +3,17 @@ PROJECT ( RobotViewer )
IF(Simox_VISUALIZATION AND Simox_USE_COIN_VISUALIZATION)
# the variable "demo_SRCS" contains all .cpp files of this project
- FILE(GLOB demo_SRCS ${PROJECT_SOURCE_DIR}/RobotViewer.cpp ${PROJECT_SOURCE_DIR}/showRobotWindow.cpp)
- FILE(GLOB demo_INCS ${PROJECT_SOURCE_DIR}/showRobotWindow.h)
+ FILE(GLOB demo_SRCS ${PROJECT_SOURCE_DIR}/RobotViewer.cpp ${PROJECT_SOURCE_DIR}/showRobotWindow.cpp DiffIKWidget.cpp)
+ FILE(GLOB demo_INCS ${PROJECT_SOURCE_DIR}/showRobotWindow.h DiffIKWidget.h)
################################## moc'ing ##############################
set(GUI_MOC_HDRS
- ${PROJECT_SOURCE_DIR}/showRobotWindow.h
+ ${PROJECT_SOURCE_DIR}/showRobotWindow.h DiffIKWidget.h
)
set(GUI_UIS
- ${PROJECT_SOURCE_DIR}/RobotViewer.ui
+ ${PROJECT_SOURCE_DIR}/RobotViewer.ui DiffIKWidget.ui
)
# create the executable
diff --git a/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp b/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp
new file mode 100644
index 000000000..b70f20aaf
--- /dev/null
+++ b/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp
@@ -0,0 +1,664 @@
+#include "DiffIKWidget.h"
+#include "ui_DiffIKWidget.h"
+
+#include <QDialog>
+#include <QVBoxLayout>
+#include <QRegExp>
+#include <Inventor/nodes/SoMatrixTransform.h>
+#include <Inventor/nodes/SoUnits.h>
+#include <SimoxUtility/algorithm/string/string_conversion.h>
+#include <SimoxUtility/math/convert.h>
+#include <VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h>
+#include <VirtualRobot/Manipulability/SingleChainManipulability.h>
+#include <VirtualRobot/Manipulability/BimanualManipulability.h>
+#include <VirtualRobot/Manipulability/BimanualManipulabilityTracking.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <QThread>
+
+DiffIKWidget* DiffIKWidget::diffIKWidget = nullptr;
+
+DiffIKWidget::DiffIKWidget(SoSeparator *sceneSep, QDialog *parent) :
+ QWidget(parent),
+ dialog(parent),
+ ui(new Ui::DiffIKWidget),
+ manipSep(new SoSeparator()),
+ followManipSep(new SoSeparator()),
+ endeffectorSep(new SoSeparator()),
+ currentRobotNodeSet(nullptr),
+ newRNS(nullptr),
+ clonedRobot(nullptr)
+
+{
+ ui->setupUi(this);
+ sceneSep->addChild(manipSep);
+ sceneSep->addChild(followManipSep);
+ sceneSep->addChild(endeffectorSep);
+
+ connect(ui->comboBoxRNS, SIGNAL(currentTextChanged(QString)), this, SLOT(setRobotNodeSet(QString)));
+ connect(ui->comboBoxRNS2, SIGNAL(currentTextChanged(QString)), this, SLOT(setRobotNodeSet2(QString)));
+ connect(ui->printJacobianButton, SIGNAL(clicked()), this, SLOT(printJacobian()));
+ connect(ui->printJacobian2Button, SIGNAL(clicked()), this, SLOT(printJacobian2()));
+
+ connect(ui->checkBoxVisManip, SIGNAL(toggled(bool)), this, SLOT(updateCurrentManipulabilityEllipsoidVis()));
+ connect(ui->checkBoxBimanual, SIGNAL(toggled(bool)), this, SLOT(updateCurrentManipulabilityEllipsoidVis()));
+ connect(ui->comboBoxManip, SIGNAL(currentTextChanged(QString)), this, SLOT(updateCurrentManipulabilityEllipsoidVis()));
+ connect(ui->comboBoxManipType, SIGNAL(currentTextChanged(QString)), this, SLOT(updateCurrentManipulabilityEllipsoidVis()));
+ connect(ui->elliosoidScaling, SIGNAL(valueChanged(int)), this, SLOT(updateCurrentManipulabilityEllipsoidVis()));
+ connect(ui->ellipsoidTransparency, SIGNAL(valueChanged(double)), this, SLOT(updateCurrentManipulabilityEllipsoidVis()));
+ connect(this, SIGNAL(currentManipUpdated()), this, SLOT(updateCurrentManipulabilityEllipsoidVis()));
+
+ connect(ui->followBox, SIGNAL(toggled(bool)), this, SLOT(updateFollowManipulabilityEllipsoidVis()));
+ connect(ui->comboBoxManip, SIGNAL(currentTextChanged(QString)), this, SLOT(updateFollowManipulabilityEllipsoidVis()));
+ connect(ui->elliosoidScaling, SIGNAL(valueChanged(int)), this, SLOT(updateFollowManipulabilityEllipsoidVis()));
+ connect(ui->ellipsoidTransparency, SIGNAL(valueChanged(double)), this, SLOT(updateFollowManipulabilityEllipsoidVis()));
+ connect(this, SIGNAL(currentManipUpdated()), this, SLOT(updateFollowManipulabilityEllipsoidVis()));
+ connect(ui->followManipulability, SIGNAL(textChanged()), this, SLOT(updateFollowManipulabilityEllipsoidVis()));
+ connect(ui->checkBoxBimanual, SIGNAL(toggled(bool)), this, SLOT(updateFollowManipulabilityEllipsoidVis()));
+
+ connect(ui->stepButton, SIGNAL(clicked()), this, SLOT(stepFollowManip()));
+ connect(ui->solveButton, SIGNAL(clicked()), this, SLOT(followManip()));
+ connect(this, SIGNAL(distanceUpdated(double)), this, SLOT(updateDistance(double)));
+
+ connect(ui->xTarget, SIGNAL(valueChanged(double)), this, SLOT(updateEndeffectorPoseVis()));
+ connect(ui->yTarget, SIGNAL(valueChanged(double)), this, SLOT(updateEndeffectorPoseVis()));
+ connect(ui->zTarget, SIGNAL(valueChanged(double)), this, SLOT(updateEndeffectorPoseVis()));
+ connect(ui->rollTarget, SIGNAL(valueChanged(double)), this, SLOT(updateEndeffectorPoseVis()));
+ connect(ui->pitchTarget, SIGNAL(valueChanged(double)), this, SLOT(updateEndeffectorPoseVis()));
+ connect(ui->yawTarget, SIGNAL(valueChanged(double)), this, SLOT(updateEndeffectorPoseVis()));
+ connect(ui->checkBoxVisTarget, SIGNAL(toggled(bool)), this, SLOT(updateEndeffectorPoseVis()));
+ connect(ui->checkBoxOriIK, SIGNAL(toggled(bool)), this, SLOT(updateEndeffectorPoseVis()));
+ connect(ui->currentPoseButton, SIGNAL(clicked()), this, SLOT(setEndEffectorPose()));
+ connect(ui->solveIKButton, SIGNAL(clicked()), this, SLOT(solveIK()));
+
+ connect(ui->resetJointValues, SIGNAL(clicked()), this, SLOT(resetJointValues()));
+ connect(ui->setAverageJointValues, SIGNAL(clicked()), this, SLOT(setAverageJointValues()));
+ connect(ui->setRandomJointValues, SIGNAL(clicked()), this, SLOT(setRandomJointValues()));
+
+ qRegisterMetaType<VirtualRobot::AbstractManipulabilityTrackingPtr>("VirtualRobot::AbstractManipulabilityTrackingPtr");
+ qRegisterMetaType<Eigen::MatrixXd>("Eigen::MatrixXd");
+ qRegisterMetaType<Eigen::VectorXf>("Eigen::VectorXf");
+ qRegisterMetaType<std::map<std::string, float>>("std::map<std::string, float>");
+ qRegisterMetaType<VirtualRobot::CompositeDiffIKPtr>("VirtualRobot::CompositeDiffIKPtr");
+ qRegisterMetaType<VirtualRobot::NullspaceManipulabilityPtr>("VirtualRobot::NullspaceManipulabilityPtr");
+ qRegisterMetaType<VirtualRobot::RobotNodeSetPtr>("VirtualRobot::RobotNodeSetPtr");
+
+ Worker *worker = new Worker;
+ worker->moveToThread(&workerThread);
+ connect(&workerThread, &QThread::finished, worker, &QObject::deleteLater);
+ connect(this, &DiffIKWidget::followManipAsync, worker, &Worker::followManip);
+ connect(this, &DiffIKWidget::solveIKAsync, worker, &Worker::solveIK);
+ connect(worker, &Worker::distanceUpdated, this, &DiffIKWidget::updateDistance);
+ connect(worker, &Worker::currentManipUpdated, this, &DiffIKWidget::updateCurrentManipulabilityEllipsoidVis);
+ connect(worker, &Worker::jointValuesUpdated, this, &DiffIKWidget::updateJointValues);
+ connect(worker, &Worker::finished, this, &DiffIKWidget::workerFinished);
+ workerThread.start();
+}
+
+DiffIKWidget::~DiffIKWidget()
+{
+ manipSep->unref();
+ followManipSep->unref();
+ endeffectorSep->unref();
+ workerThread.quit();
+ workerThread.wait();
+ delete ui;
+}
+
+QDialog *DiffIKWidget::getDialog() {
+ return dialog;
+}
+
+void DiffIKWidget::open(QWidget *parent, SoSeparator *sceneSep) {
+ if (!diffIKWidget) {
+ auto diffIKDialog = new QDialog(parent);
+ QVBoxLayout* layout = new QVBoxLayout();
+ diffIKWidget = new DiffIKWidget(sceneSep, diffIKDialog);
+ layout->addWidget(diffIKWidget);
+ diffIKDialog->setLayout(layout);
+ }
+ diffIKWidget->getDialog()->show();
+}
+
+void DiffIKWidget::update(VirtualRobot::RobotPtr robot) {
+ if (diffIKWidget && diffIKWidget->getDialog()->isVisible()) {
+ if (diffIKWidget->robot != robot) {
+ diffIKWidget->robot = robot;
+ diffIKWidget->addRobotNodeSets();
+ }
+ diffIKWidget->updateCurrentManipulabilityEllipsoidVis();
+ diffIKWidget->updateFollowManipulabilityEllipsoidVis();
+ diffIKWidget->updateEndeffectorPoseVis();
+ }
+}
+
+void DiffIKWidget::close() {
+ delete diffIKWidget;
+ diffIKWidget = nullptr;
+}
+
+
+Eigen::MatrixXd DiffIKWidget::readFollowManipulability() {
+ try {
+ Eigen::MatrixXd matrix;
+ QString data = ui->followManipulability->toPlainText();
+ // QTextEdit content
+ QStringList strList = data.split(QRegExp("[\n]"), QString::SkipEmptyParts);
+
+ if (strList.size() == 6) {
+ matrix = Eigen::Matrix<double, 6, 6>();
+ }
+ else if (strList.size() == 3) {
+ matrix = Eigen::Matrix3d();
+ }
+ else {
+ return Eigen::Matrix<double, 0, 0>();
+ }
+ matrix.setZero();
+
+ for (int i = 0; i < strList.size(); i++) {
+ QStringList s = strList[i].split(QRegExp(" "), QString::SkipEmptyParts);
+ for (int j = 0; j < s.size(); j++) {
+ float value = simox::alg::to_<double>(s[j].toStdString());
+ matrix(i,j) = value;
+ if (j > matrix.cols()) break;
+ }
+ if (i > matrix.rows()) break;
+ }
+ return matrix;
+ }
+ catch (...) {
+ return Eigen::Matrix<double, 0, 0>();
+ }
+}
+
+VirtualRobot::AbstractManipulability::Mode DiffIKWidget::getMode(QComboBox *comboBox) {
+ if (comboBox->currentText() == "Whole") {
+ return VirtualRobot::AbstractManipulability::Whole;
+ }
+ else if (comboBox->currentText() == "Position") {
+ return VirtualRobot::AbstractManipulability::Position;
+ }
+ else if (comboBox->currentText() == "Rotation") {
+ return VirtualRobot::AbstractManipulability::Orientation;
+ }
+ else {
+ throw std::runtime_error("Wrong!");
+ }
+}
+
+VirtualRobot::AbstractManipulability::Type DiffIKWidget::getManipulabilityType(QComboBox *comboBox) {
+ if (comboBox->currentText() == "Velocity") {
+ return VirtualRobot::AbstractManipulability::Velocity;
+ }
+ else if (comboBox->currentText() == "Force") {
+ return VirtualRobot::AbstractManipulability::Force;
+ }
+ else {
+ throw std::runtime_error("Wrong!");
+ }
+}
+
+Eigen::Matrix4f DiffIKWidget::getEndEffectorPos() {
+ return simox::math::pos_rpy_to_mat4f(ui->xTarget->value(), ui->yTarget->value(), ui->zTarget->value(),
+ ui->rollTarget->value(), ui->pitchTarget->value(), ui->yawTarget->value());
+}
+
+Eigen::Matrix4f DiffIKWidget::getEndEffectorPos2() {
+ return simox::math::pos_rpy_to_mat4f(ui->xTarget2->value(), ui->yTarget2->value(), ui->zTarget2->value(),
+ ui->rollTarget2->value(), ui->pitchTarget2->value(), ui->yawTarget2->value());
+}
+
+void DiffIKWidget::addRobotNodeSets() {
+ ui->comboBoxRNS->clear();
+ for (auto& robotNodeSet : robot->getRobotNodeSets())
+ {
+ if (robotNodeSet->getTCP())
+ ui->comboBoxRNS->addItem(QString(robotNodeSet->getName().c_str()));
+ }
+ ui->comboBoxRNS->setCurrentIndex(0);
+ ui->comboBoxRNS2->clear();
+ for (auto& robotNodeSet : robot->getRobotNodeSets())
+ {
+ if (robotNodeSet->getTCP())
+ ui->comboBoxRNS2->addItem(QString(robotNodeSet->getName().c_str()));
+ }
+ ui->comboBoxRNS2->setCurrentIndex(0);
+}
+
+void DiffIKWidget::updateCurrentManipulabilityEllipsoidVis() {
+ manipSep->removeAllChildren();
+ ui->currentManipulability->clear();
+ if (ui->checkBoxVisManip->isChecked()) {
+ auto manipTracking = getManipulabilityTracking();
+ if (!manipTracking) return;
+ auto manipulability = manipTracking->computeCurrentManipulability();
+ VirtualRobot::VisualizationNodePtr visNode = manipTracking->getManipulabilityVis(manipulability, "", ui->elliosoidScaling->value());
+ auto coinVisNode = std::dynamic_pointer_cast<VirtualRobot::CoinVisualizationNode>(visNode);
+ if (coinVisNode) {
+ auto coinVis = coinVisNode->getCoinVisualization();
+ auto mat = new SoMaterial();
+ mat->transparency.setValue(ui->ellipsoidTransparency->value());
+ manipSep->addChild(mat);
+ manipSep->addChild(coinVis);
+ }
+ std::stringstream ss;
+ ss << manipulability;
+ ui->currentManipulability->setText(QString::fromStdString(ss.str()));
+ }
+}
+
+void DiffIKWidget::updateFollowManipulabilityEllipsoidVis() {
+ followManipSep->removeAllChildren();
+ if (ui->followBox->isChecked() && currentRobotNodeSet) {
+ Eigen::MatrixXd followManip = readFollowManipulability();
+ VirtualRobot::RobotNodeSetPtr rns = nullptr;
+ VirtualRobot::AbstractManipulabilityTrackingPtr tracking = getManipulabilityTracking();
+ if (tracking) {
+ if (followManip.rows() != tracking->getTaskVars()) return;
+
+ double distance = tracking->computeDistance(followManip);
+ emit distanceUpdated(distance);
+
+ VirtualRobot::VisualizationNodePtr visNode = tracking->getManipulabilityVis(followManip, "", ui->elliosoidScaling->value());
+ auto coinVisNode = std::dynamic_pointer_cast<VirtualRobot::CoinVisualizationNode>(visNode);
+ if (coinVisNode) {
+ auto coinVis = coinVisNode->getCoinVisualization();
+ auto mat = new SoMaterial();
+ mat->transparency.setValue(ui->ellipsoidTransparency->value());
+ followManipSep->addChild(mat);
+ followManipSep->addChild(coinVis);
+ }
+ }
+ }
+}
+
+void DiffIKWidget::updateEndeffectorPoseVis(VirtualRobot::RobotNodeSetPtr robotNodeSet, const Eigen::Matrix4f &pose) {
+ if (robotNodeSet) {
+ auto tcp = robotNodeSet->getTCP();
+ if (tcp) {
+ SoSeparator* sep = new SoSeparator();
+ // Set the visualization stuff to milimeters
+ SoUnits *u = new SoUnits();
+ u->units = SoUnits::MILLIMETERS;
+ sep->addChild(u);
+
+ // set a transformation matrix for the visualization
+ Eigen::Matrix4f transformation = robotNodeSet->getRobot()->getGlobalPose(pose);
+ SoMatrixTransform* mt = new SoMatrixTransform();
+ SbMatrix m_(reinterpret_cast<SbMat*>(transformation.data()));
+ mt->matrix.setValue(m_);
+ sep->addChild(mt);
+
+ auto mat = new SoMaterial();
+ mat->transparency.setValue(0.5);
+ sep->addChild(mat);
+
+ if (ui->checkBoxOriIK->isChecked()) {
+ auto visNode = tcp->getVisualization(VirtualRobot::SceneObject::Full);
+ auto coinVisNode = std::dynamic_pointer_cast<VirtualRobot::CoinVisualizationNode>(visNode);
+ if (coinVisNode) {
+ auto coinVis = coinVisNode->getCoinVisualization();
+ sep->addChild(coinVis);
+ }
+ else {
+ sep->addChild(VirtualRobot::CoinVisualizationFactory::CreateSphere(50, 1, 0, 0));
+ }
+ }
+ else {
+ sep->addChild(VirtualRobot::CoinVisualizationFactory::CreateSphere(50, 1, 0, 0));
+ }
+ endeffectorSep->addChild(sep);
+ }
+ }
+}
+
+VirtualRobot::AbstractManipulabilityTrackingPtr DiffIKWidget::getManipulabilityTracking() {
+ return getManipulabilityTracking(currentRobotNodeSet, currentRobotNodeSet2);
+}
+
+VirtualRobot::AbstractManipulabilityTrackingPtr DiffIKWidget::getManipulabilityTracking(VirtualRobot::RobotNodeSetPtr r1, VirtualRobot::RobotNodeSetPtr r2, bool setNewRNS) {
+ VirtualRobot::AbstractManipulabilityTrackingPtr tracking = nullptr;
+ if (r1 && r1->getTCP()) {
+ auto mode = getMode(ui->comboBoxManip);
+ if (!ui->checkBoxBimanual->isChecked()) {
+ VirtualRobot::SingleRobotNodeSetManipulabilityPtr manip(new VirtualRobot::SingleRobotNodeSetManipulability(r1, mode, getManipulabilityType(ui->comboBoxManipType)));
+ if (setNewRNS) newRNS = manip->getRobotNodeSet();
+ tracking = VirtualRobot::SingleChainManipulabilityTrackingPtr(new VirtualRobot::SingleChainManipulabilityTracking(manip));
+ }
+ else if (r2 && r2->getTCP() && r1 != r2) {
+ VirtualRobot::BimanualManipulabilityPtr manip(new VirtualRobot::BimanualManipulability(r1, r2, mode, getManipulabilityType(ui->comboBoxManipType)));
+ if (setNewRNS) newRNS = manip->createRobotNodeSet();
+ tracking = VirtualRobot::BimanualManipulabilityTrackingPtr(new VirtualRobot::BimanualManipulabilityTracking(manip));
+ }
+ }
+ return tracking;
+}
+
+void DiffIKWidget::updateEndeffectorPoseVis() {
+ endeffectorSep->removeAllChildren();
+ if (ui->checkBoxVisTarget->isChecked()) {
+ updateEndeffectorPoseVis(currentRobotNodeSet, getEndEffectorPos());
+ updateEndeffectorPoseVis(currentRobotNodeSet2, getEndEffectorPos2());
+ }
+ if (ui->checkBoxSolveContinuous->isChecked()) {
+ solveIK(true);
+ }
+}
+
+void DiffIKWidget::stepFollowManip() {
+ auto manipTracking = getManipulabilityTracking(currentRobotNodeSet, currentRobotNodeSet2, true);
+ if (!manipTracking) return;
+ Eigen::MatrixXd followManip = readFollowManipulability();
+ if (followManip.rows() != manipTracking->getTaskVars()) {
+ std::cout << "Wrong manipulability matrix!" << std::endl;
+ return;
+ }
+
+ Eigen::VectorXf velocity = manipTracking->calculateVelocity(followManip);
+ std::cout << "Nullspace velocities without gain:\n" << velocity << "\n" << std::endl;
+ Eigen::VectorXf jointValues = newRNS->getJointValuesEigen() + velocity * ui->kGain->value();;
+ newRNS->setJointValues(jointValues);
+ double distance = manipTracking->computeDistance(followManip);
+ emit distanceUpdated(distance);
+ emit currentManipUpdated();
+}
+
+void DiffIKWidget::followManip() {
+ if (!currentRobotNodeSet) {
+ std::cout << "RobotNodeSet is null" << std::endl;
+ return;
+ }
+
+ clonedRobot = currentRobotNodeSet->getRobot()->clone();
+ auto clonedRobotNodeSet = clonedRobot->getRobotNodeSet(currentRobotNodeSet->getName());
+ auto manipTracking = getManipulabilityTracking(clonedRobot->getRobotNodeSet(currentRobotNodeSet->getName()),
+ currentRobotNodeSet2 ? clonedRobot->getRobotNodeSet(currentRobotNodeSet2->getName()) : nullptr,
+ true);
+ if (!manipTracking) return;
+ Eigen::MatrixXd followManip = readFollowManipulability();
+ if (followManip.rows() != manipTracking->getTaskVars()) {
+ std::cout << "Wrong manipulability matrix!" << std::endl;
+ return;
+ }
+ float maxDistance = ui->maxDistance->value();
+ float kGain = ui->kGain->value();
+
+ ui->solveButton->setEnabled(false);
+ ui->solveIKButton->setEnabled(false);
+ emit followManipAsync(manipTracking, newRNS, followManip, kGain, maxDistance);
+}
+
+void DiffIKWidget::updateDistance(double distance) {
+ ui->currentDistance->setText(QString::fromStdString(std::to_string(distance)));
+}
+
+void DiffIKWidget::updateJointValues(const std::map<std::string, float> &jointValues) {
+ currentRobotNodeSet->getRobot()->setJointValues(jointValues);
+ updateFollowManipulabilityEllipsoidVis();
+}
+
+void DiffIKWidget::solveIK(bool untilReached) {
+ if (!currentRobotNodeSet || !currentRobotNodeSet->getTCP()) {
+ std::cout << "RobotNodeSet or TCP is null" << std::endl;
+ return;
+ }
+
+ std::cout << "Solving IK ..." << std::endl;
+
+ clonedRobot = currentRobotNodeSet->getRobot()->clone();
+ VirtualRobot::RobotNodeSetPtr clonedRobotNodeSet = nullptr;
+ if (ui->checkBoxBimanual->isChecked()) {
+ if (currentRobotNodeSet == currentRobotNodeSet2) return;
+ std::vector<std::string> robotNodeNames = currentRobotNodeSet->getNodeNames();
+ std::vector<std::string> robotNodeNames2 = currentRobotNodeSet2->getNodeNames();
+ robotNodeNames.insert(robotNodeNames.end(), robotNodeNames2.begin(), robotNodeNames2.end());
+ clonedRobotNodeSet = VirtualRobot::RobotNodeSet::createRobotNodeSet(clonedRobot, "New", robotNodeNames, clonedRobot->getRootNode()->getName());
+ }
+ else clonedRobotNodeSet = clonedRobot->getRobotNodeSet(currentRobotNodeSet->getName());
+ auto tcp = currentRobotNodeSet->getTCP();
+ if (!tcp) return;
+
+ VirtualRobot::CompositeDiffIKPtr ik(new VirtualRobot::CompositeDiffIK(clonedRobotNodeSet));
+ Eigen::Matrix4f pose = getEndEffectorPos();
+ auto target1 = ik->addTarget(clonedRobot->getRobotNode(tcp->getName()), pose, ui->checkBoxOriIK->isChecked() ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position);
+
+ if (ui->checkBoxBimanual->isChecked()) {
+ auto tcp = currentRobotNodeSet2->getTCP();
+ if (tcp) {
+ auto node = clonedRobot->getRobotNode(tcp->getName());
+ Eigen::Matrix4f pose = getEndEffectorPos2();
+ ik->addTarget(node, pose, ui->checkBoxOriIK2->isChecked() ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position);
+ }
+ else return;
+ }
+
+ float jointLimitAvoidance = ui->jointLimitAvoidance->value();
+ if (ui->checkBoxJointLimitAvoidance->isChecked() && jointLimitAvoidance > 0) {
+ VirtualRobot::CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(new VirtualRobot::CompositeDiffIK::NullspaceJointLimitAvoidance(clonedRobotNodeSet));
+ nsjla->kP = jointLimitAvoidance;
+ for (auto node : clonedRobotNodeSet->getAllRobotNodes()) {
+ if (node->isLimitless()) {
+ nsjla->setWeight(node->getName(), 0);
+ }
+ }
+ ik->addNullspaceGradient(nsjla);
+ }
+
+ VirtualRobot::NullspaceManipulabilityPtr nsman = nullptr;
+ float kGain = ui->kGainNullspace->value();
+ if (ui->checkBoxManipulabilityNullspace->isChecked() && kGain > 0) {
+ std::cout << "Adding manipulability as nullspace target" << std::endl;
+ auto manipTracking = getManipulabilityTracking(clonedRobot->getRobotNodeSet(currentRobotNodeSet->getName()),
+ currentRobotNodeSet2 ? clonedRobot->getRobotNodeSet(currentRobotNodeSet2->getName()) : nullptr);
+ if (!manipTracking) {
+ std::cout << "Manip tracking zero!" << std::endl;
+ return;
+ }
+ Eigen::MatrixXd followManip = readFollowManipulability();
+ if (followManip.rows() != manipTracking->getTaskVars()) {
+ std::cout << "Wrong manipulability matrix!" << std::endl;
+ return;
+ }
+ nsman = VirtualRobot::NullspaceManipulabilityPtr(new VirtualRobot::NullspaceManipulability(manipTracking, kGain, followManip));
+ ik->addNullspaceGradient(nsman);
+ }
+
+ float kSoechting = ui->kSoechting->value();
+ if (ui->checkBoxSoechtingNullspace->isChecked() && kSoechting > 0) {
+ if (robot->getName() == "Armar6" && currentRobotNodeSet->getName() == "RightArm") {
+ std::cout << "Adding soechting nullspace" << std::endl;
+ VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
+ armjoints.clavicula = clonedRobot->getRobotNode("ArmR1_Cla1");
+ armjoints.shoulder1 = clonedRobot->getRobotNode("ArmR2_Sho1");
+ armjoints.shoulder2 = clonedRobot->getRobotNode("ArmR3_Sho2");
+ armjoints.shoulder3 = clonedRobot->getRobotNode("ArmR4_Sho3");
+ armjoints.elbow = clonedRobot->getRobotNode("ArmR5_Elb1");
+
+ VirtualRobot::SoechtingNullspaceGradientPtr gradient(new VirtualRobot::SoechtingNullspaceGradient(target1, "ArmR2_Sho1", VirtualRobot::Soechting::ArmType::Right, armjoints));
+ gradient->kP = kSoechting;
+ ik->addNullspaceGradient(gradient);
+ }
+ else if (robot->getName() == "Armar6" && currentRobotNodeSet->getName() == "LeftArm") {
+ std::cout << "Adding soechting nullspace" << std::endl;
+ VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
+ armjoints.clavicula = clonedRobot->getRobotNode("ArmL1_Cla1");
+ armjoints.shoulder1 = clonedRobot->getRobotNode("ArmL2_Sho1");
+ armjoints.shoulder2 = clonedRobot->getRobotNode("ArmL3_Sho2");
+ armjoints.shoulder3 = clonedRobot->getRobotNode("ArmL4_Sho3");
+ armjoints.elbow = clonedRobot->getRobotNode("ArmL5_Elb1");
+
+ VirtualRobot::SoechtingNullspaceGradientPtr gradient(new VirtualRobot::SoechtingNullspaceGradient(target1, "ArmL2_Sho1", VirtualRobot::Soechting::ArmType::Left, armjoints));
+ gradient->kP = kSoechting;
+ ik->addNullspaceGradient(gradient);
+ }
+ else std::cout << "Soechting currently supports only Armar6 and RightArm/LeftArm robot node set for first robot node set for demonstration" << std::endl;
+ }
+
+ ui->solveButton->setEnabled(false);
+ ui->solveIKButton->setEnabled(false);
+ emit solveIKAsync(ik, untilReached ? -1 : ui->ikSteps->value(), nsman);
+
+}
+
+void DiffIKWidget::setEndEffectorPose() {
+ if (currentRobotNodeSet && currentRobotNodeSet->getTCP()) {
+ auto tcp = currentRobotNodeSet->getTCP();
+ auto pose = tcp->getPoseInRootFrame();
+ auto position = simox::math::mat4f_to_pos(pose);
+ ui->xTarget->setValue(position(0));
+ ui->yTarget->setValue(position(1));
+ ui->zTarget->setValue(position(2));
+ auto rpy = simox::math::mat4f_to_rpy(pose);
+ ui->rollTarget->setValue(rpy(0));
+ ui->pitchTarget->setValue(rpy(1));
+ ui->yawTarget->setValue(rpy(2));
+ }
+ if (currentRobotNodeSet2 && currentRobotNodeSet2->getTCP()) {
+ auto tcp = currentRobotNodeSet2->getTCP();
+ auto pose = tcp->getPoseInRootFrame();
+ auto position = simox::math::mat4f_to_pos(pose);
+ ui->xTarget2->setValue(position(0));
+ ui->yTarget2->setValue(position(1));
+ ui->zTarget2->setValue(position(2));
+ auto rpy = simox::math::mat4f_to_rpy(pose);
+ ui->rollTarget2->setValue(rpy(0));
+ ui->pitchTarget2->setValue(rpy(1));
+ ui->yawTarget2->setValue(rpy(2));
+ }
+}
+
+void DiffIKWidget::workerFinished() {
+ ui->solveButton->setEnabled(true);
+ ui->solveIKButton->setEnabled(true);
+ emit currentManipUpdated();
+}
+
+void DiffIKWidget::resetJointValues() {
+ auto values = robot->getJointValues();
+ for (auto &value : values) {
+ value.second = 0;
+ }
+ robot->setJointValues(values);
+
+ emit currentManipUpdated();
+}
+
+void DiffIKWidget::setAverageJointValues() {
+ robot->setPropagatingJointValuesEnabled(false);
+ for (auto node : robot->getRobotNodes()) {
+ if (node->isRotationalJoint())
+ robot->setJointValue(node, (node->getJointLimitLo() + node->getJointLimitHi()) / 2.0f);
+ }
+ robot->setPropagatingJointValuesEnabled(true);
+ robot->updatePose();
+
+ emit currentManipUpdated();
+}
+
+float randomFloat(float a, float b) {
+ float random = ((float) rand()) / (float) RAND_MAX;
+ float diff = b - a;
+ float r = random * diff;
+ return a + r;
+}
+
+void DiffIKWidget::setRandomJointValues() {
+ robot->setPropagatingJointValuesEnabled(false);
+ for (auto node : robot->getRobotNodes()) {
+ if (node->isRotationalJoint()) {
+ float jointValue = randomFloat(node->getJointLimitLo(), node->getJointLimitHi());
+ if (!std::isnan(jointValue))
+ robot->setJointValue(node, jointValue);
+ }
+ }
+ robot->setPropagatingJointValuesEnabled(true);
+ robot->updatePose();
+
+ emit currentManipUpdated();
+}
+
+void DiffIKWidget::setRobotNodeSet(QString name) {
+ currentRobotNodeSet = robot->getRobotNodeSet(name.toStdString());
+}
+
+void DiffIKWidget::setRobotNodeSet2(QString name) {
+ currentRobotNodeSet2 = robot->getRobotNodeSet(name.toStdString());
+}
+
+void printJacobi(VirtualRobot::RobotNodeSetPtr rns, bool scale) {
+ if (!rns) return;
+ VirtualRobot::DifferentialIKPtr diffIK(new VirtualRobot::DifferentialIK(rns, VirtualRobot::RobotNodePtr(), VirtualRobot::JacobiProvider::eSVDDamped));
+ diffIK->convertModelScalingtoM(scale);
+ Eigen::MatrixXd jacobian = diffIK->getJacobianMatrix(VirtualRobot::IKSolver::All).cast<double>();
+ Eigen::IOFormat CommaInitFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", " << ", ";");
+ std::cout << "Jacobian matrix for " << rns->getName() << ":\n"
+ << jacobian << "\n\n" << jacobian.format(CommaInitFmt) << "\n\n" << std::endl;
+}
+
+void DiffIKWidget::printJacobian() {
+ printJacobi(currentRobotNodeSet, true);
+}
+
+void DiffIKWidget::printJacobian2() {
+ printJacobi(currentRobotNodeSet, true);
+}
+
+void Worker::followManip(VirtualRobot::AbstractManipulabilityTrackingPtr manipTracking, VirtualRobot::RobotNodeSetPtr rns,
+ const Eigen::MatrixXd& followManip, float kGain, float maxDistance) {
+ double distance = 1000;
+ double lastDistance = 1000;
+ int count = 0;
+ while (distance > maxDistance) {
+ Eigen::VectorXf velocity = manipTracking->calculateVelocity(followManip);
+ Eigen::VectorXf jointValues = rns->getJointValuesEigen() + velocity * kGain;
+ rns->setJointValues(jointValues);
+ distance = manipTracking->computeDistance(followManip);
+ emit distanceUpdated(distance);
+ if (distance / lastDistance < 0.99) {
+ emit jointValuesUpdated(rns->getJointValueMap());
+ emit currentManipUpdated();
+
+ lastDistance = distance;
+ count = 0;
+ }
+ else count++;
+ if (count > 1000) break; // stop after 1000 iterations without large change
+ }
+ if (distance < maxDistance) {
+ emit jointValuesUpdated(rns->getJointValueMap());
+ emit distanceUpdated(distance);
+ emit currentManipUpdated();
+ }
+ else emit distanceUpdated(lastDistance);
+ emit finished();
+}
+
+void Worker::solveIK(VirtualRobot::CompositeDiffIKPtr ik, int steps, VirtualRobot::NullspaceManipulabilityPtr nsman) {
+ VirtualRobot::CompositeDiffIK::Parameters cp;
+ cp.resetRnsValues = false;
+ cp.returnIKSteps = true;
+ cp.steps = 1;
+ VirtualRobot::CompositeDiffIK::SolveState state;
+ ik->solve(cp, state);
+
+ int i = 0;
+ while (i < steps || (steps < 0 && !ik->getLastResult().reached && i < 1000)) {
+ ik->step(cp, state, i);
+ emit jointValuesUpdated(ik->getRobotNodeSet()->getJointValueMap());
+ if (nsman) {
+ emit distanceUpdated(nsman->computeDistance());
+ emit currentManipUpdated();
+ }
+ i++;
+ }
+ emit jointValuesUpdated(ik->getRobotNodeSet()->getJointValueMap());
+ if (nsman) {
+ emit distanceUpdated(nsman->computeDistance());
+ emit currentManipUpdated();
+ }
+ emit finished();
+}
diff --git a/VirtualRobot/examples/RobotViewer/DiffIKWidget.h b/VirtualRobot/examples/RobotViewer/DiffIKWidget.h
new file mode 100644
index 000000000..159d3e673
--- /dev/null
+++ b/VirtualRobot/examples/RobotViewer/DiffIKWidget.h
@@ -0,0 +1,108 @@
+#pragma once
+#include <QWidget>
+#include <QDialog>
+#include <VirtualRobot/VirtualRobot.h>
+#include <Inventor/nodes/SoSeparator.h>
+#include <VirtualRobot/IK/IKSolver.h>
+#include <QComboBox>
+#include <QThread>
+#include <VirtualRobot/Manipulability/AbstractManipulabilityTracking.h>
+#include <VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h>
+#include <VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.h>
+
+
+namespace Ui {
+class DiffIKWidget;
+}
+
+Q_DECLARE_METATYPE(Eigen::MatrixXd)
+Q_DECLARE_METATYPE(Eigen::VectorXf)
+Q_DECLARE_METATYPE(VirtualRobot::AbstractManipulabilityTrackingPtr)
+Q_DECLARE_METATYPE(VirtualRobot::CompositeDiffIKPtr)
+Q_DECLARE_METATYPE(VirtualRobot::NullspaceManipulabilityPtr)
+Q_DECLARE_METATYPE(VirtualRobot::RobotNodeSetPtr)
+
+
+class Worker : public QObject
+{
+ Q_OBJECT
+
+public slots:
+ void followManip(VirtualRobot::AbstractManipulabilityTrackingPtr manip, VirtualRobot::RobotNodeSetPtr rns, const Eigen::MatrixXd& followManip, float kGain, float maxDistance);
+ void solveIK(VirtualRobot::CompositeDiffIKPtr ik, int steps, VirtualRobot::NullspaceManipulabilityPtr = nullptr);
+
+signals:
+ void distanceUpdated(double distance);
+ void currentManipUpdated();
+ void jointValuesUpdated(const std::map<std::string, float> &jointValues);
+ void finished();
+};
+
+class DiffIKWidget : public QWidget
+{
+ Q_OBJECT
+
+public:
+ explicit DiffIKWidget(SoSeparator *sceneSep, QDialog *parent = nullptr);
+ ~DiffIKWidget();
+
+ QDialog* getDialog();
+
+ static void open(QWidget *parent, SoSeparator *sceneSep);
+
+ static void update(VirtualRobot::RobotPtr robot);
+
+ static void close();
+
+ static DiffIKWidget *diffIKWidget;
+
+private slots:
+ void updateCurrentManipulabilityEllipsoidVis();
+ void updateFollowManipulabilityEllipsoidVis();
+ void updateEndeffectorPoseVis();
+ void stepFollowManip();
+ void followManip();
+ void updateDistance(double distance);
+ void updateJointValues(const std::map<std::string, float> &jointValues);
+ void solveIK(bool untilReached = false);
+ void setEndEffectorPose();
+ void workerFinished();
+ void resetJointValues();
+ void setAverageJointValues();
+ void setRandomJointValues();
+ void setRobotNodeSet(QString name);
+ void setRobotNodeSet2(QString name);
+ void printJacobian();
+ void printJacobian2();
+
+ Eigen::MatrixXd readFollowManipulability();
+
+signals:
+ void distanceUpdated(double distance);
+ void currentManipUpdated();
+ void followManipAsync(VirtualRobot::AbstractManipulabilityTrackingPtr manip, VirtualRobot::RobotNodeSetPtr rns, const Eigen::MatrixXd& followManip, float kGain, float maxDistance);
+ void solveIKAsync(VirtualRobot::CompositeDiffIKPtr ik, int steps, VirtualRobot::NullspaceManipulabilityPtr nsman);
+
+private:
+ VirtualRobot::AbstractManipulability::Mode getMode(QComboBox* comboBox);
+ VirtualRobot::AbstractManipulability::Type getManipulabilityType(QComboBox* comboBox);
+ Eigen::Matrix4f getEndEffectorPos();
+ Eigen::Matrix4f getEndEffectorPos2();
+
+ void addRobotNodeSets();
+ void updateEndeffectorPoseVis(VirtualRobot::RobotNodeSetPtr robotNodeSet, const Eigen::Matrix4f &pose);
+ VirtualRobot::AbstractManipulabilityTrackingPtr getManipulabilityTracking();
+ VirtualRobot::AbstractManipulabilityTrackingPtr getManipulabilityTracking(VirtualRobot::RobotNodeSetPtr r1, VirtualRobot::RobotNodeSetPtr r2, bool setNewRNS = false);
+
+ QDialog *dialog;
+ Ui::DiffIKWidget *ui;
+ SoSeparator *manipSep;
+ SoSeparator *followManipSep;
+ SoSeparator *endeffectorSep;
+ VirtualRobot::RobotPtr robot;
+ VirtualRobot::RobotNodeSetPtr currentRobotNodeSet;
+ VirtualRobot::RobotNodeSetPtr currentRobotNodeSet2;
+ VirtualRobot::RobotNodeSetPtr newRNS;
+ VirtualRobot::RobotPtr clonedRobot;
+ QThread workerThread;
+};
diff --git a/VirtualRobot/examples/RobotViewer/DiffIKWidget.ui b/VirtualRobot/examples/RobotViewer/DiffIKWidget.ui
new file mode 100644
index 000000000..91a7d19fa
--- /dev/null
+++ b/VirtualRobot/examples/RobotViewer/DiffIKWidget.ui
@@ -0,0 +1,871 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>DiffIKWidget</class>
+ <widget class="QWidget" name="DiffIKWidget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>679</width>
+ <height>937</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QGroupBox" name="groupBox">
+ <property name="title">
+ <string>Robot node sets</string>
+ </property>
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QComboBox" name="comboBoxRNS"/>
+ </item>
+ <item>
+ <widget class="QPushButton" name="printJacobianButton">
+ <property name="text">
+ <string>Print Jacobian</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QComboBox" name="comboBoxRNS2"/>
+ </item>
+ <item>
+ <widget class="QPushButton" name="printJacobian2Button">
+ <property name="text">
+ <string>Print Jacobian</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <widget class="QGroupBox" name="manipulabilityBox">
+ <property name="title">
+ <string>Manipulability</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_4">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QCheckBox" name="checkBoxVisManip">
+ <property name="text">
+ <string>Visualize</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QComboBox" name="comboBoxManip">
+ <item>
+ <property name="text">
+ <string>Position</string>
+ </property>
+ </item>
+ <item>
+ <property name="text">
+ <string>Rotation</string>
+ </property>
+ </item>
+ <item>
+ <property name="text">
+ <string>Whole</string>
+ </property>
+ </item>
+ </widget>
+ </item>
+ <item>
+ <widget class="QComboBox" name="comboBoxManipType">
+ <item>
+ <property name="text">
+ <string>Velocity</string>
+ </property>
+ </item>
+ <item>
+ <property name="text">
+ <string>Force</string>
+ </property>
+ </item>
+ </widget>
+ </item>
+ <item>
+ <widget class="QCheckBox" name="checkBoxBimanual">
+ <property name="text">
+ <string>Bimanual</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>Scaling</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QSpinBox" name="elliosoidScaling">
+ <property name="minimum">
+ <number>1</number>
+ </property>
+ <property name="maximum">
+ <number>10000</number>
+ </property>
+ <property name="value">
+ <number>1000</number>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Transparency</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QDoubleSpinBox" name="ellipsoidTransparency">
+ <property name="maximum">
+ <double>1.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.500000000000000</double>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <widget class="QGroupBox" name="groupBox_3">
+ <property name="title">
+ <string>Current</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_3">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QTextBrowser" name="currentManipulability"/>
+ </item>
+ <item>
+ <widget class="QGroupBox" name="followBox">
+ <property name="title">
+ <string>Follow</string>
+ </property>
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <property name="checked">
+ <bool>false</bool>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QTextEdit" name="followManipulability"/>
+ </item>
+ <item>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="1" column="0">
+ <widget class="QPushButton" name="stepButton">
+ <property name="text">
+ <string>Step</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="6">
+ <widget class="QLabel" name="label_9">
+ <property name="text">
+ <string>Max Distance</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="5">
+ <widget class="QPushButton" name="solveButton">
+ <property name="text">
+ <string>Solve</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="7">
+ <widget class="QLabel" name="currentDistance">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>0.0</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLabel" name="label_8">
+ <property name="text">
+ <string>k_gain</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="7">
+ <widget class="QDoubleSpinBox" name="maxDistance">
+ <property name="decimals">
+ <number>4</number>
+ </property>
+ <property name="maximum">
+ <double>1.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.100000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="6">
+ <widget class="QLabel" name="label_3">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Current Distance</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QDoubleSpinBox" name="kGain">
+ <property name="decimals">
+ <number>11</number>
+ </property>
+ <property name="minimum">
+ <double>0.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.001000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.001000000000000</double>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <widget class="QGroupBox" name="groupBox_4">
+ <property name="title">
+ <string>Endeffector targets</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item row="2" column="3">
+ <widget class="QDoubleSpinBox" name="rollTarget">
+ <property name="minimum">
+ <double>-10.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="6">
+ <widget class="QDoubleSpinBox" name="pitchTarget">
+ <property name="minimum">
+ <double>-10.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="2">
+ <widget class="QLabel" name="label_11">
+ <property name="text">
+ <string>roll</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLabel" name="label_4">
+ <property name="text">
+ <string>x</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QCheckBox" name="checkBoxOriIK">
+ <property name="text">
+ <string>Orientation</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="2">
+ <widget class="QLabel" name="label_15">
+ <property name="text">
+ <string>x</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="5">
+ <widget class="QLabel" name="label_12">
+ <property name="text">
+ <string>pitch</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="7">
+ <widget class="QLabel" name="label_20">
+ <property name="text">
+ <string>yaw</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QCheckBox" name="checkBoxOriIK2">
+ <property name="text">
+ <string>Orientation (2)</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="5">
+ <widget class="QLabel" name="label_5">
+ <property name="text">
+ <string>y</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="8">
+ <widget class="QDoubleSpinBox" name="zTarget">
+ <property name="decimals">
+ <number>0</number>
+ </property>
+ <property name="minimum">
+ <double>-2000.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>2000.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>25.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="3">
+ <widget class="QDoubleSpinBox" name="xTarget">
+ <property name="decimals">
+ <number>0</number>
+ </property>
+ <property name="minimum">
+ <double>-2000.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>2000.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>25.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="2">
+ <widget class="QLabel" name="label_16">
+ <property name="text">
+ <string>roll</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="5">
+ <widget class="QLabel" name="label_17">
+ <property name="text">
+ <string>y</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="8">
+ <widget class="QDoubleSpinBox" name="yawTarget">
+ <property name="minimum">
+ <double>-10.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="5">
+ <widget class="QLabel" name="label_18">
+ <property name="text">
+ <string>pitch</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="7">
+ <widget class="QLabel" name="label_6">
+ <property name="text">
+ <string>z</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="6">
+ <widget class="QDoubleSpinBox" name="yTarget">
+ <property name="decimals">
+ <number>0</number>
+ </property>
+ <property name="minimum">
+ <double>-2000.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>2000.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>25.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLabel" name="label_7">
+ <property name="text">
+ <string>Position</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="7">
+ <widget class="QLabel" name="label_13">
+ <property name="text">
+ <string>yaw</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="7">
+ <widget class="QLabel" name="label_19">
+ <property name="text">
+ <string>z</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QCheckBox" name="checkBoxVisTarget">
+ <property name="text">
+ <string>Visualize</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="7" colspan="2">
+ <widget class="QPushButton" name="currentPoseButton">
+ <property name="text">
+ <string>Set to current pose</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1" colspan="8">
+ <widget class="Line" name="line">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="3">
+ <widget class="QDoubleSpinBox" name="xTarget2">
+ <property name="decimals">
+ <number>0</number>
+ </property>
+ <property name="minimum">
+ <double>-2000.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>2000.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>25.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="3">
+ <widget class="QDoubleSpinBox" name="rollTarget2">
+ <property name="minimum">
+ <double>-10.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="8">
+ <widget class="QDoubleSpinBox" name="zTarget2">
+ <property name="decimals">
+ <number>0</number>
+ </property>
+ <property name="minimum">
+ <double>-2000.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>2000.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>25.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="6">
+ <widget class="QDoubleSpinBox" name="yTarget2">
+ <property name="decimals">
+ <number>0</number>
+ </property>
+ <property name="minimum">
+ <double>-2000.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>2000.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>25.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="6">
+ <widget class="QDoubleSpinBox" name="pitchTarget2">
+ <property name="minimum">
+ <double>-10.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="8">
+ <widget class="QDoubleSpinBox" name="yawTarget2">
+ <property name="minimum">
+ <double>-10.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QLabel" name="label_21">
+ <property name="text">
+ <string>Position (2)</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <widget class="QGroupBox" name="groupBox_2">
+ <property name="title">
+ <string>IK</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_3">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <property name="horizontalSpacing">
+ <number>6</number>
+ </property>
+ <item row="0" column="1">
+ <widget class="QLabel" name="label_10">
+ <property name="text">
+ <string>steps</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="5">
+ <widget class="QDoubleSpinBox" name="jointLimitAvoidance">
+ <property name="decimals">
+ <number>8</number>
+ </property>
+ <property name="minimum">
+ <double>0.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>1.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QSpinBox" name="ikSteps">
+ <property name="toolTip">
+ <string>Set -1 to solve until reached</string>
+ </property>
+ <property name="minimum">
+ <number>-1</number>
+ </property>
+ <property name="maximum">
+ <number>5000</number>
+ </property>
+ <property name="value">
+ <number>100</number>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QPushButton" name="solveIKButton">
+ <property name="text">
+ <string>Solve IK</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="4">
+ <widget class="QCheckBox" name="checkBoxManipulabilityNullspace">
+ <property name="text">
+ <string>Manipulability as nullspace</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="5">
+ <widget class="QDoubleSpinBox" name="kGainNullspace">
+ <property name="decimals">
+ <number>8</number>
+ </property>
+ <property name="maximum">
+ <double>1.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.010000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="3" rowspan="4">
+ <widget class="Line" name="line_2">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="4">
+ <widget class="QCheckBox" name="checkBoxSoechtingNullspace">
+ <property name="toolTip">
+ <string/>
+ </property>
+ <property name="text">
+ <string>Soechting as nullspace</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="5">
+ <widget class="QDoubleSpinBox" name="kSoechting">
+ <property name="decimals">
+ <number>8</number>
+ </property>
+ <property name="minimum">
+ <double>0.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>1.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="4">
+ <widget class="QCheckBox" name="checkBoxJointLimitAvoidance">
+ <property name="text">
+ <string>Joint limit avoidance</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QCheckBox" name="checkBoxSolveContinuous">
+ <property name="text">
+ <string>solve continuously</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout_3">
+ <item>
+ <widget class="QPushButton" name="resetJointValues">
+ <property name="text">
+ <string>Zero joints</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="setAverageJointValues">
+ <property name="text">
+ <string>Average joints</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="setRandomJointValues">
+ <property name="text">
+ <string>Random joints</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/VirtualRobot/examples/RobotViewer/RobotViewer.cpp b/VirtualRobot/examples/RobotViewer/RobotViewer.cpp
index 38e77a4ad..17822371c 100644
--- a/VirtualRobot/examples/RobotViewer/RobotViewer.cpp
+++ b/VirtualRobot/examples/RobotViewer/RobotViewer.cpp
@@ -1,4 +1,4 @@
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobotException.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/XML/RobotIO.h>
diff --git a/VirtualRobot/examples/RobotViewer/RobotViewer.ui b/VirtualRobot/examples/RobotViewer/RobotViewer.ui
index 2bcffb59b..1b391d13b 100644
--- a/VirtualRobot/examples/RobotViewer/RobotViewer.ui
+++ b/VirtualRobot/examples/RobotViewer/RobotViewer.ui
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1117</width>
- <height>1011</height>
+ <width>1121</width>
+ <height>1046</height>
</rect>
</property>
<property name="windowTitle">
@@ -21,6 +21,12 @@
<enum>Qt::Horizontal</enum>
</property>
<widget class="QFrame" name="frameViewer">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
<property name="frameShape">
<enum>QFrame::StyledPanel</enum>
</property>
@@ -30,7 +36,7 @@
</widget>
<widget class="QScrollArea" name="scrollArea">
<property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Expanding">
+ <sizepolicy hsizetype="Preferred" vsizetype="Expanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
@@ -44,7 +50,7 @@
<x>0</x>
<y>0</y>
<width>841</width>
- <height>1061</height>
+ <height>1092</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_3">
@@ -485,6 +491,13 @@
</layout>
</widget>
</item>
+ <item>
+ <widget class="QPushButton" name="openDiffIKButton">
+ <property name="text">
+ <string>Open Diff IK</string>
+ </property>
+ </widget>
+ </item>
<item>
<widget class="QGroupBox" name="groupBoxDistance">
<property name="title">
@@ -509,7 +522,7 @@
<property name="spacing">
<number>0</number>
</property>
- <item row="0" column="0">
+ <item row="1" column="0">
<widget class="QWidget" name="widget_5" native="true">
<layout class="QGridLayout" name="gridLayout_10">
<item row="2" column="0">
@@ -607,7 +620,7 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1117</width>
+ <width>1121</width>
<height>22</height>
</rect>
</property>
diff --git a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
index b90c08b49..e663db1bd 100644
--- a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
+++ b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
@@ -1,5 +1,6 @@
#include "showRobotWindow.h"
+#include "DiffIKWidget.h"
#include "VirtualRobot/EndEffector/EndEffector.h"
#include "VirtualRobot/Workspace/Reachability.h"
#include <VirtualRobot/RuntimeEnvironment.h>
@@ -70,6 +71,7 @@ showRobotWindow::~showRobotWindow()
{
robot.reset();
sceneSep->unref();
+ DiffIKWidget::close();
}
/*
@@ -105,7 +107,7 @@ void showRobotWindow::setupUI()
viewer->setGLRenderAction(new SoLineHighlightRenderAction);
- viewer->setTransparencyType(SoGLRenderAction::BLEND);
+ viewer->setTransparencyType(SoGLRenderAction::SORTED_OBJECT_BLEND);
viewer->setFeedbackVisibility(true);
viewer->setSceneGraph(sceneSep);
viewer->viewAll();
@@ -144,6 +146,8 @@ void showRobotWindow::setupUI()
connect(UI.doubleSpinBoxDistancePtX, qOverload<double>(&QDoubleSpinBox::valueChanged), this, &showRobotWindow::updatePointDistanceVisu);
connect(UI.doubleSpinBoxDistancePtY, qOverload<double>(&QDoubleSpinBox::valueChanged), this, &showRobotWindow::updatePointDistanceVisu);
connect(UI.doubleSpinBoxDistancePtZ, qOverload<double>(&QDoubleSpinBox::valueChanged), this, &showRobotWindow::updatePointDistanceVisu);
+
+ connect(UI.openDiffIKButton, SIGNAL(clicked()), this, SLOT(openDiffIK()));
}
QString showRobotWindow::formatString(const char* s, float f)
@@ -428,6 +432,11 @@ void showRobotWindow::updatePointDistanceVisu()
UI.labelDistancePtDist->setText(QString::number(distance));
}
+void showRobotWindow::openDiffIK() {
+ DiffIKWidget::open(this, sceneSep);
+ DiffIKWidget::update(robot);
+}
+
void showRobotWindow::showRobot()
{
//m_pGraspScenery->showRobot(m_pShowRobot->state() == QCheckBox::On);
@@ -507,6 +516,7 @@ void showRobotWindow::selectRNS(int nr)
}
updateJointBox();
+ DiffIKWidget::update(robot);
selectJoint(0);
displayTriangles();
}
@@ -583,6 +593,7 @@ void showRobotWindow::jointValueChanged(int pos)
robot->setJointValue(currentRobotNodes[nr], fPos);
UI.lcdNumberJointValue->display((double)fPos);
+ DiffIKWidget::update(robot);
updatePointDistanceVisu();
}
@@ -792,6 +803,7 @@ void showRobotWindow::updatRobotInfo()
robot->getRobotNodeSets(robotNodeSets);
robot->getEndEffectors(eefs);
updateEEFBox();
+ DiffIKWidget::update(robot);
updateRNSBox();
selectRNS(0);
diff --git a/VirtualRobot/examples/RobotViewer/showRobotWindow.h b/VirtualRobot/examples/RobotViewer/showRobotWindow.h
index 95e4f0d20..3b62ad930 100644
--- a/VirtualRobot/examples/RobotViewer/showRobotWindow.h
+++ b/VirtualRobot/examples/RobotViewer/showRobotWindow.h
@@ -67,6 +67,7 @@ public slots:
void exportVRML();
void exportXML();
void updatePointDistanceVisu();
+ void openDiffIK();
SoQtExaminerViewer* getExaminerViewer()
{
diff --git a/VirtualRobot/math/Helpers.cpp b/VirtualRobot/math/Helpers.cpp
index fe1418636..03e2b53fb 100644
--- a/VirtualRobot/math/Helpers.cpp
+++ b/VirtualRobot/math/Helpers.cpp
@@ -256,6 +256,16 @@ namespace math
std::swap(a, b);
}
+ Eigen::Vector3f Helpers::LimitTo(const Eigen::Vector3f& val, float maxNorm)
+ {
+ float norm = val.norm();
+ if (norm > maxNorm)
+ {
+ return val / norm * maxNorm;
+ }
+ return val;
+ }
+
Eigen::Matrix4f Helpers::CreateTranslationPose(const Eigen::Vector3f& pos)
{
return Pose(pos, Eigen::Matrix3f::Identity());
diff --git a/VirtualRobot/math/Helpers.h b/VirtualRobot/math/Helpers.h
index 97f54b5c0..a69cf281f 100644
--- a/VirtualRobot/math/Helpers.h
+++ b/VirtualRobot/math/Helpers.h
@@ -63,6 +63,8 @@ namespace math
static Eigen::Vector3f Average(const std::vector<Eigen::Vector3f>& vectors);
static void Swap(float& a,float& b);
+ static Eigen::Vector3f LimitTo(const Eigen::Vector3f& val, float maxNorm);
+
// POSE UTILITY
// These methods have been moved to <SimoxUtility/math/pose.h>, but remain here for
diff --git a/VirtualRobot/tests/CMakeLists.txt b/VirtualRobot/tests/CMakeLists.txt
index bcbe315b3..d102bca0f 100644
--- a/VirtualRobot/tests/CMakeLists.txt
+++ b/VirtualRobot/tests/CMakeLists.txt
@@ -35,6 +35,8 @@ ADD_VR_TEST( VirtualRobotRuntimeEnvironmentTest )
ADD_VR_TEST( VirtualRobotMjcfTest )
+ADD_VR_TEST( VirtualRobotManipulabilityTest )
+
ADD_VR_TEST( VirtualRobotMujocoMeshTest )
diff --git a/VirtualRobot/tests/VirtualRobotManipulabilityTest.cpp b/VirtualRobot/tests/VirtualRobotManipulabilityTest.cpp
new file mode 100644
index 000000000..8cfed15f1
--- /dev/null
+++ b/VirtualRobot/tests/VirtualRobotManipulabilityTest.cpp
@@ -0,0 +1,223 @@
+/**
+* @package VirtualRobot
+* @author Andre Meixner
+* @copyright 2021 Andre Meixner
+*/
+
+#define BOOST_TEST_MODULE VirtualRobot_VirtualRobotManipulabilityTest
+
+#include <VirtualRobot/VirtualRobotTest.h>
+#include <VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h>
+#include <VirtualRobot/Manipulability/SingleChainManipulability.h>
+#include <SimoxUtility/algorithm/string/string_conversion_eigen.h>
+
+#define CHECK_SMALL_VELOCITY(a, b, tolerance) { \
+ BOOST_REQUIRE_EQUAL(a.rows(), b.rows()); \
+ for (unsigned int i = 0; i < a.rows(); i++) { \
+ BOOST_CHECK_SMALL(std::abs(a(i) - b(i)), tolerance); \
+ } \
+}
+
+#define FLOAT_CLOSE_TO_DIFF 1e-4f
+
+#define CHECK_SMALL_DIFF_VELOCITY(a, b) { \
+ CHECK_SMALL_VELOCITY(a, b, FLOAT_CLOSE_TO_DIFF) \
+}
+
+
+BOOST_AUTO_TEST_SUITE(ManipulabilityTest)
+
+BOOST_AUTO_TEST_CASE(testPositionalVelocityManipulabilityTracking)
+{
+ // Armar6 average joints
+ Eigen::MatrixXd jacobian(6, 8);
+ jacobian << -0.656626, -0.169947, 0.0867239, 0.098642, -0.594799, 0.0045068, -0.0390586, -0.253288, 0.860518, 0.231487, 1.13725e-08, -5.96046e-08, 0.168292, -1.30385e-08, 2.37487e-08, 0.0535143, 0, 0.634252, 0.570741, 0.649175, 0.0903796, 0.0296596, -0.25705, 0.0384871, 0, 0.965926, 1.31134e-07, 0.988652, 0.150226, 0.311744, -0.938216, 0.150226, 0, 1.26666e-07, -1, 1.29646e-07, -6.77231e-08, 0.948985, 0.315322, -2.62416e-08, 1, 0.258819, 0, -0.150226, 0.988652, -0.0473694, 0.142562, 0.988652;
+
+ Eigen::MatrixXd desiredManipulability(3, 3);
+ desiredManipulability << 1.15186, -0.735976, -0.118876,
+ -0.735976, 0.653656, -0.0866033,
+ -0.118876, -0.0866033, 1.14306;
+
+ Eigen::VectorXf expectedVelocity(8);
+ expectedVelocity << 0.1729,
+ -0.7210,
+ -3.4009,
+ 1.9029,
+ 0.5024,
+ 0.2881,
+ -2.7081,
+ -0.2294;
+
+
+ VirtualRobot::SingleChainManipulabilityPtr manipulability(
+ new VirtualRobot::SingleChainManipulability(jacobian, VirtualRobot::AbstractManipulability::Position,
+ VirtualRobot::AbstractManipulability::Velocity));
+ VirtualRobot::SingleChainManipulabilityTrackingPtr tracking(new VirtualRobot::SingleChainManipulabilityTracking(manipulability));
+ Eigen::VectorXf velocity = tracking->calculateVelocity(desiredManipulability);
+
+ CHECK_SMALL_DIFF_VELOCITY(velocity, expectedVelocity)
+}
+
+//BOOST_AUTO_TEST_CASE(testOrientationalVelocityManipulabilityTracking)
+//{
+// // Armar6 average joints
+// Eigen::MatrixXd jacobian(6, 8);
+// jacobian << -0.656626, -0.169947, 0.0867239, 0.098642, -0.594799, 0.0045068, -0.0390586, -0.253288, 0.860518, 0.231487, 1.13725e-08, -5.96046e-08, 0.168292, -1.30385e-08, 2.37487e-08, 0.0535143, 0, 0.634252, 0.570741, 0.649175, 0.0903796, 0.0296596, -0.25705, 0.0384871, 0, 0.965926, 1.31134e-07, 0.988652, 0.150226, 0.311744, -0.938216, 0.150226, 0, 1.26666e-07, -1, 1.29646e-07, -6.77231e-08, 0.948985, 0.315322, -2.62416e-08, 1, 0.258819, 0, -0.150226, 0.988652, -0.0473694, 0.142562, 0.988652;
+
+// Eigen::MatrixXd desiredManipulability(3, 3);
+// desiredManipulability << 2.85461, -0.0611938, 0.161456,
+// -0.0611938, 2.34351, -0.424717,
+// 0.161456, -0.424717, 2.80188;
+
+// Eigen::VectorXf expectedVelocity(8);
+// expectedVelocity << 1.0036,
+// 4.3729,
+// 10.4689,
+// -7.2922,
+// -15.6157,
+// -0.8079,
+// 7.3426,
+// 27.5755;
+
+
+// VirtualRobot::SingleChainManipulabilityPtr manipulability(
+// new VirtualRobot::SingleChainManipulability(jacobian, VirtualRobot::AbstractManipulability::Orientation,
+// VirtualRobot::AbstractManipulability::Velocity));
+// VirtualRobot::SingleChainManipulabilityTrackingPtr tracking(new VirtualRobot::SingleChainManipulabilityTracking(manipulability));
+// Eigen::VectorXf velocity = tracking->calculateVelocity(desiredManipulability);
+
+// CHECK_SMALL_DIFF_VELOCITY(velocity, expectedVelocity)
+//}
+
+BOOST_AUTO_TEST_CASE(testWholeVelocityManipulabilityTracking)
+{
+ // Armar6 average joints
+ Eigen::MatrixXd jacobian(6, 8);
+ jacobian << -0.656626, -0.169947, 0.0867239, 0.098642, -0.594799, 0.0045068, -0.0390586, -0.253288, 0.860518, 0.231487, 1.13725e-08, -5.96046e-08, 0.168292, -1.30385e-08, 2.37487e-08, 0.0535143, 0, 0.634252, 0.570741, 0.649175, 0.0903796, 0.0296596, -0.25705, 0.0384871, 0, 0.965926, 1.31134e-07, 0.988652, 0.150226, 0.311744, -0.938216, 0.150226, 0, 1.26666e-07, -1, 1.29646e-07, -6.77231e-08, 0.948985, 0.315322, -2.62416e-08, 1, 0.258819, 0, -0.150226, 0.988652, -0.0473694, 0.142562, 0.988652;
+
+ Eigen::MatrixXd desiredManipulability(6, 6);
+ desiredManipulability << 1.15186, -0.735976, -0.118876, -0.140627, 0.372079, -1.64893,
+ -0.735976, 0.653656, -0.0866033, -0.220524, -0.0254303, 0.970515,
+ -0.118876, -0.0866033, 1.14306, 1.49837, -0.560595, 0.166057,
+ -0.140627, -0.220524, 1.49837, 2.85461, -0.0611938, 0.161456,
+ 0.372079, -0.0254303, -0.560595, -0.0611938, 2.34351, -0.424717,
+ -1.64893, 0.970515, 0.166057, 0.161456, -0.424717, 2.80188;
+
+ Eigen::VectorXf expectedVelocity(8);
+ expectedVelocity << 0.0083,
+ -0.2695,
+ 0.0478,
+ -2.2594,
+ -1.4446,
+ 34.0653,
+ -2.2005,
+ 3.7379;
+
+
+ VirtualRobot::SingleChainManipulabilityPtr manipulability(
+ new VirtualRobot::SingleChainManipulability(jacobian, VirtualRobot::AbstractManipulability::Whole,
+ VirtualRobot::AbstractManipulability::Velocity));
+ VirtualRobot::SingleChainManipulabilityTrackingPtr tracking(new VirtualRobot::SingleChainManipulabilityTracking(manipulability));
+ Eigen::VectorXf velocity = tracking->calculateVelocity(desiredManipulability);
+
+ CHECK_SMALL_DIFF_VELOCITY(velocity, expectedVelocity)
+}
+
+BOOST_AUTO_TEST_CASE(testPositionalForceManipulabilityTracking)
+{
+ // Armar6 average joints
+ Eigen::MatrixXd jacobian(6, 8);
+ jacobian << -0.656626, -0.169947, 0.0867239, 0.098642, -0.594799, 0.0045068, -0.0390586, -0.253288, 0.860518, 0.231487, 1.13725e-08, -5.96046e-08, 0.168292, -1.30385e-08, 2.37487e-08, 0.0535143, 0, 0.634252, 0.570741, 0.649175, 0.0903796, 0.0296596, -0.25705, 0.0384871, 0, 0.965926, 1.31134e-07, 0.988652, 0.150226, 0.311744, -0.938216, 0.150226, 0, 1.26666e-07, -1, 1.29646e-07, -6.77231e-08, 0.948985, 0.315322, -2.62416e-08, 1, 0.258819, 0, -0.150226, 0.988652, -0.0473694, 0.142562, 0.988652;
+
+ Eigen::MatrixXd desiredManipulability(3, 3);
+ desiredManipulability << 3.54843, 4.08521, 0.678543,
+ 4.08521, 6.24856, 0.898272,
+ 0.678543, 0.898272, 1.01347;
+
+ Eigen::VectorXf expectedVelocity(8);
+ expectedVelocity << 1.1371,
+ 0.5298,
+ -0.2561,
+ 2.5805,
+ 0.3481,
+ -0.2669,
+ 2.4973,
+ -1.7452;
+
+
+ VirtualRobot::SingleChainManipulabilityPtr manipulability(
+ new VirtualRobot::SingleChainManipulability(jacobian, VirtualRobot::AbstractManipulability::Position,
+ VirtualRobot::AbstractManipulability::Force));
+ VirtualRobot::SingleChainManipulabilityTrackingPtr tracking(new VirtualRobot::SingleChainManipulabilityTracking(manipulability));
+ Eigen::VectorXf velocity = tracking->calculateVelocity(desiredManipulability);
+
+ CHECK_SMALL_DIFF_VELOCITY(velocity, expectedVelocity)
+}
+
+//BOOST_AUTO_TEST_CASE(testOrientationalForceManipulabilityTracking)
+//{
+// // Armar6 average joints
+// Eigen::MatrixXd jacobian(6, 8);
+// jacobian << -0.656626, -0.169947, 0.0867239, 0.098642, -0.594799, 0.0045068, -0.0390586, -0.253288, 0.860518, 0.231487, 1.13725e-08, -5.96046e-08, 0.168292, -1.30385e-08, 2.37487e-08, 0.0535143, 0, 0.634252, 0.570741, 0.649175, 0.0903796, 0.0296596, -0.25705, 0.0384871, 0, 0.965926, 1.31134e-07, 0.988652, 0.150226, 0.311744, -0.938216, 0.150226, 0, 1.26666e-07, -1, 1.29646e-07, -6.77231e-08, 0.948985, 0.315322, -2.62416e-08, 1, 0.258819, 0, -0.150226, 0.988652, -0.0473694, 0.142562, 0.988652;
+
+// Eigen::MatrixXd desiredManipulability(3, 3);
+// desiredManipulability << 0.351529, 0.00566362, -0.019398,
+// 0.00566362, 0.438855, 0.0661965,
+// -0.019398, 0.0661965, 0.368055;
+
+// Eigen::VectorXf expectedVelocity(8);
+// expectedVelocity << 1.0036,
+// 4.3729,
+// 10.4689,
+// -7.2922,
+// -15.6157,
+// -0.8079,
+// 7.3426,
+// 27.5755;
+
+
+// VirtualRobot::SingleChainManipulabilityPtr manipulability(
+// new VirtualRobot::SingleChainManipulability(jacobian, VirtualRobot::AbstractManipulability::Orientation,
+// VirtualRobot::AbstractManipulability::Force));
+// VirtualRobot::SingleChainManipulabilityTrackingPtr tracking(new VirtualRobot::SingleChainManipulabilityTracking(manipulability));
+// Eigen::VectorXf velocity = tracking->calculateVelocity(desiredManipulability);
+
+// CHECK_SMALL_DIFF_VELOCITY(velocity, expectedVelocity)
+//}
+
+BOOST_AUTO_TEST_CASE(testWholeForceManipulabilityTracking)
+{
+ // Armar6 average joints
+ Eigen::MatrixXd jacobian(6, 8);
+ jacobian << -0.656626, -0.169947, 0.0867239, 0.098642, -0.594799, 0.0045068, -0.0390586, -0.253288, 0.860518, 0.231487, 1.13725e-08, -5.96046e-08, 0.168292, -1.30385e-08, 2.37487e-08, 0.0535143, 0, 0.634252, 0.570741, 0.649175, 0.0903796, 0.0296596, -0.25705, 0.0384871, 0, 0.965926, 1.31134e-07, 0.988652, 0.150226, 0.311744, -0.938216, 0.150226, 0, 1.26666e-07, -1, 1.29646e-07, -6.77231e-08, 0.948985, 0.315322, -2.62416e-08, 1, 0.258819, 0, -0.150226, 0.988652, -0.0473694, 0.142562, 0.988652;
+
+ Eigen::MatrixXd desiredManipulability(6, 6);
+ desiredManipulability << 17.7117, 11.2696, -3.55426, 3.20849, -2.33407, 6.19188,
+ 11.2696, 10.7247, -2.67026, 2.59712, -1.76195, 2.65897,
+ -3.55426, -2.67026, 4.97508, -2.90084, 1.45506, -1.07393,
+ 3.20849, 2.59712, -2.90084, 2.16073, -0.957258, 0.890947,
+ -2.33407, -1.76195, 1.45506, -0.957258, 0.984315, -0.645189,
+ 6.19188, 2.65897, -1.07393, 0.890947, -0.645189, 2.99438;
+
+ Eigen::VectorXf expectedVelocity(8);
+ expectedVelocity << 0.1131,
+ 0.1082,
+ 4.6499,
+ -0.7927,
+ -0.6206,
+ -25.2698,
+ -0.4688,
+ 0.4812;
+
+
+ VirtualRobot::SingleChainManipulabilityPtr manipulability(
+ new VirtualRobot::SingleChainManipulability(jacobian, VirtualRobot::AbstractManipulability::Whole,
+ VirtualRobot::AbstractManipulability::Force));
+ VirtualRobot::SingleChainManipulabilityTrackingPtr tracking(new VirtualRobot::SingleChainManipulabilityTracking(manipulability));
+ Eigen::VectorXf velocity = tracking->calculateVelocity(desiredManipulability);
+
+ CHECK_SMALL_DIFF_VELOCITY(velocity, expectedVelocity)
+}
+
+
+BOOST_AUTO_TEST_SUITE_END()
--
GitLab