From 9e2949d27842490508a442bfb76c540cf70feb02 Mon Sep 17 00:00:00 2001
From: Meixner <andre.meixner@kit.edu>
Date: Sun, 7 Mar 2021 01:21:46 +0100
Subject: [PATCH] Refactored joint limit avoidance for
SingleChainManipulability
---
.../ManipulabilityNullspaceGradient.cpp | 12 +++---
.../ManipulabilityNullspaceGradient.h | 6 ++-
.../AbstractManipulabilityTracking.cpp | 41 +++++++++++-------
.../AbstractManipulabilityTracking.h | 6 +--
.../Manipulability/BimanualManipulability.cpp | 32 +++++++-------
.../BimanualManipulabilityTracking.cpp | 16 +++----
.../BimanualManipulabilityTracking.h | 2 +-
.../SingleChainManipulability.cpp | 16 +++----
.../SingleChainManipulabilityTracking.cpp | 43 ++++---------------
.../SingleChainManipulabilityTracking.h | 4 +-
.../examples/RobotViewer/DiffIKWidget.cpp | 15 +++++--
11 files changed, 93 insertions(+), 100 deletions(-)
diff --git a/VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.cpp b/VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.cpp
index f032925f4..46efb8641 100644
--- a/VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.cpp
+++ b/VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.cpp
@@ -26,11 +26,13 @@
namespace VirtualRobot
{
-NullspaceManipulability::NullspaceManipulability(AbstractManipulabilityTrackingPtr manipulabilityTracking, float k_gain, const Eigen::MatrixXd& manipulabilityDesired) :
+NullspaceManipulability::NullspaceManipulability(AbstractManipulabilityTrackingPtr manipulabilityTracking, const Eigen::MatrixXd& manipulabilityDesired,
+ const Eigen::MatrixXd &gainMatrix, bool jointLimitAvoidance) :
CompositeDiffIK::NullspaceGradient(manipulabilityTracking->getJointNames()),
manipulabilityTracking(manipulabilityTracking),
- k_gain(k_gain),
- manipulabilityDesired(manipulabilityDesired)
+ manipulabilityDesired(manipulabilityDesired),
+ gainMatrix(gainMatrix),
+ jointLimitAvoidance(jointLimitAvoidance)
{
}
@@ -45,7 +47,7 @@ void NullspaceManipulability::init(CompositeDiffIK::Parameters&)
Eigen::VectorXf NullspaceManipulability::getGradient(CompositeDiffIK::Parameters& /*params*/, int /*stepNr*/)
{
- Eigen::VectorXf velocities = manipulabilityTracking->calculateVelocity(manipulabilityDesired);
+ Eigen::VectorXf velocities = manipulabilityTracking->calculateVelocity(manipulabilityDesired, gainMatrix, jointLimitAvoidance);
// check if nan
unsigned int nan = 0;
for (unsigned int i = 0; i < velocities.rows(); i++) {
@@ -56,7 +58,7 @@ Eigen::VectorXf NullspaceManipulability::getGradient(CompositeDiffIK::Parameters
}
if (nan > 0)
std::cout << "Nan in nullspace manipulability velocities: " << nan << "/" << velocities.rows() << " \n";
- return k_gain * velocities;
+ return velocities;
}
}
diff --git a/VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.h b/VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.h
index 3a1b9c6fe..b6509b6ff 100644
--- a/VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.h
+++ b/VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.h
@@ -30,11 +30,13 @@ namespace VirtualRobot
{
public:
NullspaceManipulability(AbstractManipulabilityTrackingPtr manipulabilityTracking,
- float k_gain, const Eigen::MatrixXd& manipulabilityDesired);
+ const Eigen::MatrixXd& manipulabilityDesired,
+ const Eigen::MatrixXd &gainMatrix = Eigen::MatrixXd(), bool jointLimitAvoidance = false);
AbstractManipulabilityTrackingPtr manipulabilityTracking;
- float k_gain;
Eigen::MatrixXd manipulabilityDesired;
+ Eigen::MatrixXd gainMatrix;
+ bool jointLimitAvoidance;
double computeDistance();
diff --git a/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp
index 7f9ba6d0c..96fd7b81a 100644
--- a/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp
+++ b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp
@@ -31,8 +31,8 @@
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> AbstractManipulabilityTracking::calculateVelocityMap(const Eigen::MatrixXd &manipulabilityDesired, const std::vector<std::string> &jointNames, const Eigen::MatrixXd &gainMatrix, bool jointLimitAvoidance) {
+ Eigen::VectorXf velocities = calculateVelocity(manipulabilityDesired, gainMatrix, jointLimitAvoidance);
std::map<std::string, float> velocityMap;
if ((unsigned int)velocities.rows() == jointNames.size()) {
for (unsigned int index = 0; index < jointNames.size(); index++) {
@@ -164,28 +164,39 @@ double AbstractManipulabilityTracking::getDamping(const Eigen::MatrixXd &matrix)
return damping;
}
-Eigen::MatrixXd AbstractManipulabilityTracking::getJointsLimitsWeightMatrix(const Eigen::VectorXd jointAngles, const Eigen::VectorXd jointLimitsLow, const Eigen::VectorXd jointLimitsHigh) {
+Eigen::MatrixXd AbstractManipulabilityTracking::getJointsLimitsWeightMatrix(const Eigen::VectorXd &jointAngles, const Eigen::VectorXd &jointLimitsLow, const Eigen::VectorXd &jointLimitsHigh) {
int nbJoints = jointAngles.size();
Eigen::VectorXd weights(nbJoints);
- Eigen::VectorXd gradient(nbJoints);
+ Eigen::VectorXd gradient = Eigen::VectorXd::Zero(nbJoints);
+
+ if (jointAngleLimitGradient.rows() == 0)
+ {
+ jointAngleLimitGradient = Eigen::VectorXd::Zero(nbJoints);
+ }
//std::cout << "Saved gradient:\n" << jointAngleLimitGradient << std::endl;
for (int i = 0; i < nbJoints; i++) {
- // TODO add condition on velocities
- gradient(i) = std::pow(jointLimitsHigh(i) - jointLimitsLow(i), 2) * (2*jointAngles(i) - jointLimitsHigh(i) - jointLimitsLow(i)) /
- ( 4 * std::pow(jointLimitsHigh(i) - jointAngles(i), 2) * std::pow(jointAngles(i) - jointLimitsLow(i), 2) );
-
- if((std::abs(gradient(i)) - std::abs(jointAngleLimitGradient(i))) >= 0.) {
- // Joint going towards limits
- weights(i) = 1. / std::sqrt((1. + std::abs(gradient(i))));
- }
- else {
- // Joint going towards the center
+ if (jointLimitsLow(i) == 0 && jointLimitsHigh(i) == 0) {
+ // Joint is limitless
weights(i) = 1.;
}
+ else {
+ // TODO add condition on velocities
+ gradient(i) = std::pow(jointLimitsHigh(i) - jointLimitsLow(i), 2) * (2*jointAngles(i) - jointLimitsHigh(i) - jointLimitsLow(i)) /
+ ( 4 * std::pow(jointLimitsHigh(i) - jointAngles(i), 2) * std::pow(jointAngles(i) - jointLimitsLow(i), 2) );
+
+ if((std::abs(gradient(i)) - std::abs(jointAngleLimitGradient(i))) >= 0.) {
+ // Joint going towards limits
+ weights(i) = 1. / std::sqrt((1. + std::abs(gradient(i))));
+ }
+ else {
+ // Joint going towards the center
+ weights(i) = 1.;
+ }
+ }
}
- //std::cout << "Gradient:\n" << gradient << std::endl;
+ //std::cout << "Gradient:\n" << gradient << "\n\n" << std::endl;
setjointAngleLimitGradient(gradient);
Eigen::MatrixXd weightsMatrix = weights.asDiagonal();
diff --git a/VirtualRobot/Manipulability/AbstractManipulabilityTracking.h b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.h
index 4fe51f982..1e0ac7628 100644
--- a/VirtualRobot/Manipulability/AbstractManipulabilityTracking.h
+++ b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.h
@@ -54,9 +54,9 @@ auto Matrix_to_Tensor(const MatrixType<Scalar> &matrix, Dims... dims)
class AbstractManipulabilityTracking
{
public:
- virtual Eigen::VectorXf calculateVelocity(const Eigen::MatrixXd &manipulabilityDesired, Eigen::MatrixXd gainMatrix=Eigen::MatrixXd()) = 0;
+ virtual Eigen::VectorXf calculateVelocity(const Eigen::MatrixXd &manipulabilityDesired, const Eigen::MatrixXd &gainMatrix = Eigen::MatrixXd(), bool jointLimitAvoidance = false) = 0;
- std::map<std::string, float> calculateVelocityMap(const Eigen::MatrixXd &manipulabilityDesired, const std::vector<std::string> &jointNames);
+ std::map<std::string, float> calculateVelocityMap(const Eigen::MatrixXd &manipulabilityDesired, const std::vector<std::string> &jointNames, const Eigen::MatrixXd &gainMatrix = Eigen::MatrixXd(), bool jointLimitAvoidance = false);
Eigen::Tensor<double, 3> computeJacobianDerivative(const Eigen::MatrixXd &jacobian);
@@ -82,7 +82,7 @@ public:
double getDamping(const Eigen::MatrixXd &matrix);
/* Calculate weight matrix for joint limits avoidance */
- Eigen::MatrixXd getJointsLimitsWeightMatrix(const Eigen::VectorXd jointAngles, const Eigen::VectorXd jointLimitsLow, const Eigen::VectorXd jointLimitsHigh);
+ Eigen::MatrixXd getJointsLimitsWeightMatrix(const Eigen::VectorXd &jointAngles, const Eigen::VectorXd &jointLimitsLow, const Eigen::VectorXd &jointLimitsHigh);
Eigen::MatrixXd computeManipulabilityJacobianMandelNotation(const Eigen::Tensor<double, 3> &manipulabilityJacobian);
diff --git a/VirtualRobot/Manipulability/BimanualManipulability.cpp b/VirtualRobot/Manipulability/BimanualManipulability.cpp
index 9c7290f23..ede081a12 100644
--- a/VirtualRobot/Manipulability/BimanualManipulability.cpp
+++ b/VirtualRobot/Manipulability/BimanualManipulability.cpp
@@ -131,22 +131,22 @@ Eigen::VectorXd BimanualManipulability::getJointLimitsHigh() {
for (size_t i = 0; i < rnsLeft->getSize(); i++)
{
RobotNodePtr rn = rnsLeft->getNode(i);
- /*if (rn->isLimitless())
+ if (rn->isLimitless())
{
- jointLimitHi(i) = ?infinity;
+ jointLimitHi(i) = 0;
}
- else */
- jointLimitHi(i) = rn->getJointLimitHi();
+ else
+ jointLimitHi(i) = rn->getJointLimitHi();
}
for (size_t i = 0; i < rnsRight->getSize(); i++)
{
RobotNodePtr rn = rnsRight->getNode(i);
- /*if (rn->isLimitless())
+ if (rn->isLimitless())
{
- jointLimitHi(i) = ?infinity;
+ jointLimitHi(i + rnsLeft->getSize()) = 0;
}
- else */
- jointLimitHi(i + rnsLeft->getSize()) = rn->getJointLimitHi();
+ else
+ jointLimitHi(i + rnsLeft->getSize()) = rn->getJointLimitHi();
}
return jointLimitHi;
}
@@ -156,22 +156,22 @@ Eigen::VectorXd BimanualManipulability::getJointLimitsLow() {
for (size_t i = 0; i < rnsLeft->getSize(); i++)
{
RobotNodePtr rn = rnsLeft->getNode(i);
- /*if (rn->isLimitless())
+ if (rn->isLimitless())
{
- jointLimitLo(i) = ?infinity;
+ jointLimitLo(i) = 0;
}
- else */
- jointLimitLo(i) = rn->getJointLimitLo();
+ else
+ jointLimitLo(i) = rn->getJointLimitLo();
}
for (size_t i = 0; i < rnsRight->getSize(); i++)
{
RobotNodePtr rn = rnsRight->getNode(i);
- /*if (rn->isLimitless())
+ if (rn->isLimitless())
{
- jointLimitLo(i) = ?infinity;
+ jointLimitLo(i + rnsLeft->getSize()) = 0;
}
- else */
- jointLimitLo(i + rnsLeft->getSize()) = rn->getJointLimitLo();
+ else
+ jointLimitLo(i + rnsLeft->getSize()) = rn->getJointLimitLo();
}
return jointLimitLo;
}
diff --git a/VirtualRobot/Manipulability/BimanualManipulabilityTracking.cpp b/VirtualRobot/Manipulability/BimanualManipulabilityTracking.cpp
index 86616c667..0e7166263 100644
--- a/VirtualRobot/Manipulability/BimanualManipulabilityTracking.cpp
+++ b/VirtualRobot/Manipulability/BimanualManipulabilityTracking.cpp
@@ -33,7 +33,7 @@ BimanualManipulabilityTracking::BimanualManipulabilityTracking(BimanualManipulab
{
}
-Eigen::VectorXf BimanualManipulabilityTracking::calculateVelocity(const Eigen::MatrixXd &manipulabilityDesired, Eigen::MatrixXd gainMatrix) {
+Eigen::VectorXf BimanualManipulabilityTracking::calculateVelocity(const Eigen::MatrixXd &manipulabilityDesired, const Eigen::MatrixXd &gainMatrix, bool jointLimitAvoidance) {
// Compute left and right Jacobians
Eigen::MatrixXd jacobianLeft = manipulability->computeFullJacobianLeft();
Eigen::MatrixXd jacobianRight = manipulability->computeFullJacobianRight();
@@ -55,18 +55,18 @@ Eigen::VectorXf BimanualManipulabilityTracking::calculateVelocity(const Eigen::M
Eigen::Tensor<double, 3> manipJ_sub = subCube(manipJ);
Eigen::MatrixXd matManipJ = computeManipulabilityJacobianMandelNotation(manipJ_sub);
+ if (jointLimitAvoidance)
+ {
+ // TODO
+ std::cout << "Joint limit avoidance for bimanual manipulability is not yet implemented!" << std::endl;
+ }
+
// 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;
+ Eigen::VectorXd dq = dampedLSI * (gainMatrix.rows() == 0 ? getDefaultGainMatrix() : gainMatrix) * manipulability_mandel;
return dq.cast<float>();
}
diff --git a/VirtualRobot/Manipulability/BimanualManipulabilityTracking.h b/VirtualRobot/Manipulability/BimanualManipulabilityTracking.h
index 43977395e..68cfe23c0 100644
--- a/VirtualRobot/Manipulability/BimanualManipulabilityTracking.h
+++ b/VirtualRobot/Manipulability/BimanualManipulabilityTracking.h
@@ -42,7 +42,7 @@ class BimanualManipulabilityTracking : public AbstractManipulabilityTracking
public:
BimanualManipulabilityTracking(BimanualManipulabilityPtr manipulability);
- Eigen::VectorXf calculateVelocity(const Eigen::MatrixXd &manipulabilityDesired, Eigen::MatrixXd gainMatrix=Eigen::MatrixXd()) override;
+ Eigen::VectorXf calculateVelocity(const Eigen::MatrixXd &manipulabilityDesired, const Eigen::MatrixXd &gainMatrix=Eigen::MatrixXd(), bool jointLimitAvoidance = false) override;
Eigen::Tensor<double, 3> computeBimanualManipulabilityJacobian(const Eigen::MatrixXd &jacobianLeft, const Eigen::MatrixXd &jacobianRight, const Eigen::MatrixXd &bimanualGraspMatrix);
diff --git a/VirtualRobot/Manipulability/SingleChainManipulability.cpp b/VirtualRobot/Manipulability/SingleChainManipulability.cpp
index e50b897a8..adefea6d0 100644
--- a/VirtualRobot/Manipulability/SingleChainManipulability.cpp
+++ b/VirtualRobot/Manipulability/SingleChainManipulability.cpp
@@ -97,12 +97,12 @@ Eigen::VectorXd SingleRobotNodeSetManipulability::getJointLimitsHigh() {
for (size_t i = 0; i < rns->getSize(); i++)
{
RobotNodePtr rn = rns->getNode(i);
- /*if (rn->isLimitless())
+ if (rn->isLimitless())
{
- jointLimitHi(i) = ?infinity;
+ jointLimitHi(i) = 0;
}
- else */
- jointLimitHi(i) = rn->getJointLimitHi();
+ else
+ jointLimitHi(i) = rn->getJointLimitHi();
}
return jointLimitHi;
}
@@ -112,12 +112,12 @@ Eigen::VectorXd SingleRobotNodeSetManipulability::getJointLimitsLow() {
for (size_t i = 0; i < rns->getSize(); i++)
{
RobotNodePtr rn = rns->getNode(i);
- /*if (rn->isLimitless())
+ if (rn->isLimitless())
{
- jointLimitLo(i) = ?infinity;
+ jointLimitLo(i) = 0;
}
- else */
- jointLimitLo(i) = rn->getJointLimitLo();
+ else
+ jointLimitLo(i) = rn->getJointLimitLo();
}
return jointLimitLo;
}
diff --git a/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp b/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp
index cecf95ee3..468f87822 100644
--- a/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp
+++ b/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp
@@ -72,7 +72,7 @@ Eigen::Tensor<double, 3> SingleChainManipulabilityTracking::computeManipulabilit
}
}
-Eigen::VectorXf SingleChainManipulabilityTracking::calculateVelocity(const Eigen::MatrixXd &manipulabilityDesired, Eigen::MatrixXd gainMatrix) {
+Eigen::VectorXf SingleChainManipulabilityTracking::calculateVelocity(const Eigen::MatrixXd &manipulabilityDesired, const Eigen::MatrixXd &gainMatrix, bool jointLimitAvoidance) {
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);
@@ -81,44 +81,17 @@ Eigen::VectorXf SingleChainManipulabilityTracking::calculateVelocity(const Eigen
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)
+ if (jointLimitAvoidance)
{
- gainMatrix = getDefaultGainMatrix();
+ // Compute weighted manipulability Jacobian
+ Eigen::MatrixXd jointsWeightMatrix = getJointsLimitsWeightMatrix(manipulability->getJointAngles(), manipulability->getJointLimitsLow(), manipulability->getJointLimitsHigh());
+ matManipJ = matManipJ * jointsWeightMatrix;
}
-
- Eigen::VectorXd dq = dampedLSI * gainMatrix * manipulability_mandel;
- return dq.cast<float>();
-}
-
-Eigen::VectorXf SingleChainManipulabilityTracking::calculateVelocityWithJointLimitsAvoidance(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::MatrixXd manipulability_mandel = symMatrixToVector(manipulability_velocity);
-
- // Compute manipulability Jacobian
- Eigen::Tensor<double, 3> manipJ = computeManipulabilityJacobian(jacobian);
- Eigen::Tensor<double, 3> manipJ_sub = subCube(manipJ);
- Eigen::MatrixXd matManipJ = computeManipulabilityJacobianMandelNotation(manipJ_sub);
-
- // Compute weighted manipulability Jacobian
- Eigen::MatrixXd jointsWeightMatrix = getJointsLimitsWeightMatrix(manipulability->getJointAngles(), manipulability->getJointLimitsHigh(), manipulability->getJointLimitsLow());
- matManipJ = matManipJ * jointsWeightMatrix;
- // Damping factor, and pseudo inverse
double dampingFactor = getDamping(matManipJ);
- Eigen::MatrixXd dampedLSI = jointsWeightMatrix * dampedLeastSquaresInverse(matManipJ, dampingFactor);
-
- if (gainMatrix.rows() == 0)
- {
- gainMatrix = getDefaultGainMatrix();
- }
+ Eigen::MatrixXd dampedLSI = dampedLeastSquaresInverse(matManipJ, dampingFactor);
+ Eigen::MatrixXd manipulability_mandel = symMatrixToVector(manipulability_velocity);
- Eigen::VectorXd dq = dampedLSI * gainMatrix * manipulability_mandel;
+ Eigen::VectorXd dq = dampedLSI * (gainMatrix.rows() == 0 ? getDefaultGainMatrix() : gainMatrix) * manipulability_mandel;
return dq.cast<float>();
}
diff --git a/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h b/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h
index d655f2c67..eaea394ee 100644
--- a/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h
+++ b/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h
@@ -43,9 +43,7 @@ public:
Eigen::Tensor<double, 3> computeManipulabilityJacobian(const Eigen::MatrixXd &jacobian);
- Eigen::VectorXf calculateVelocity(const Eigen::MatrixXd &manipulabilityDesired, Eigen::MatrixXd gainMatrix=Eigen::MatrixXd()) override;
-
- Eigen::VectorXf calculateVelocityWithJointLimitsAvoidance(const Eigen::MatrixXd &manipulabilityDesired, Eigen::MatrixXd gainMatrix=Eigen::MatrixXd());
+ Eigen::VectorXf calculateVelocity(const Eigen::MatrixXd &manipulabilityDesired, const Eigen::MatrixXd &gainMatrix = Eigen::MatrixXd(), bool jointLimitAvoidance = false) override;
virtual int getTaskVars() override;
diff --git a/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp b/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp
index b70f20aaf..2fa4a4316 100644
--- a/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp
+++ b/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp
@@ -358,7 +358,7 @@ void DiffIKWidget::stepFollowManip() {
return;
}
- Eigen::VectorXf velocity = manipTracking->calculateVelocity(followManip);
+ Eigen::VectorXf velocity = manipTracking->calculateVelocity(followManip, Eigen::MatrixXd(), true);
std::cout << "Nullspace velocities without gain:\n" << velocity << "\n" << std::endl;
Eigen::VectorXf jointValues = newRNS->getJointValuesEigen() + velocity * ui->kGain->value();;
newRNS->setJointValues(jointValues);
@@ -463,7 +463,8 @@ void DiffIKWidget::solveIK(bool untilReached) {
std::cout << "Wrong manipulability matrix!" << std::endl;
return;
}
- nsman = VirtualRobot::NullspaceManipulabilityPtr(new VirtualRobot::NullspaceManipulability(manipTracking, kGain, followManip));
+ nsman = VirtualRobot::NullspaceManipulabilityPtr(new VirtualRobot::NullspaceManipulability(manipTracking, followManip, Eigen::MatrixXd(), true));
+ nsman->kP = kGain;
ik->addNullspaceGradient(nsman);
}
@@ -568,7 +569,13 @@ float randomFloat(float a, float b) {
void DiffIKWidget::setRandomJointValues() {
robot->setPropagatingJointValuesEnabled(false);
- for (auto node : robot->getRobotNodes()) {
+ std::vector<std::string> robotNodeNames = currentRobotNodeSet->getNodeNames();
+ if (ui->checkBoxBimanual && currentRobotNodeSet2) {
+ auto robotNodeNames2 = currentRobotNodeSet2->getNodeNames();
+ robotNodeNames.insert(robotNodeNames.end(), robotNodeNames2.begin(), robotNodeNames2.end());
+ }
+ for (const auto &name : robotNodeNames) {
+ auto node = robot->getRobotNode(name);
if (node->isRotationalJoint()) {
float jointValue = randomFloat(node->getJointLimitLo(), node->getJointLimitHi());
if (!std::isnan(jointValue))
@@ -613,7 +620,7 @@ void Worker::followManip(VirtualRobot::AbstractManipulabilityTrackingPtr manipTr
double lastDistance = 1000;
int count = 0;
while (distance > maxDistance) {
- Eigen::VectorXf velocity = manipTracking->calculateVelocity(followManip);
+ Eigen::VectorXf velocity = manipTracking->calculateVelocity(followManip, Eigen::MatrixXd(), true);
Eigen::VectorXf jointValues = rns->getJointValuesEigen() + velocity * kGain;
rns->setJointValues(jointValues);
distance = manipTracking->computeDistance(followManip);
--
GitLab