From a037115091b2a23262ba5a261fb6ee3a672fa828 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 1 Feb 2023 08:49:10 +0100
Subject: [PATCH] Add placeholder for regularization in CompositeDiffIk
---
.../IK/CompositeDiffIK/CompositeDiffIK.cpp | 22 ++++++++++++++++++-
1 file changed, 21 insertions(+), 1 deletion(-)
diff --git a/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp b/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp
index 455f4cb80..de9c51167 100644
--- a/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp
+++ b/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp
@@ -91,7 +91,27 @@ CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params, SolveState &s)
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;
+ float regularization = 1;
+ if (rns->getNode(i)->isTranslationalJoint())
+ {
+ regularization = params.jointRegularizationTranslation;
+ }
+ else if (rns->getNode(i)->isRotationalJoint())
+ {
+ regularization = params.jointRegularizationRotation;
+ }
+ else if (rns->getNode(i)->isHemisphereJoint())
+ {
+ // FIXME: ToDo?
+ regularization = params.jointRegularizationRotation;
+ }
+ else if (rns->getNode(i)->isFourBarJoint())
+ {
+ // FIXME: ToDo?
+ regularization = params.jointRegularizationRotation;
+ }
+
+ s.jointRegularization(i) = regularization;
}
s.cartesianRegularization = Eigen::VectorXf::Zero(s.rows);
--
GitLab