From fc356fd52dfba143586a96ee3800a285fa781c50 Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Thu, 18 Jun 2020 16:03:03 +0200
Subject: [PATCH] CompositeDiffIK: added step
---
.../NaturalIKTest/NaturalIKTest.cpp | 126 ++++++---
.../libraries/diffik/CompositeDiffIK.cpp | 249 +++++++++++-------
.../libraries/diffik/CompositeDiffIK.h | 19 ++
3 files changed, 259 insertions(+), 135 deletions(-)
diff --git a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
index feb67dc48..3c90676a5 100644
--- a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
+++ b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
@@ -565,6 +565,12 @@ namespace armarx
};
+ struct ShoulderAngles
+ {
+ float SE, SR;
+ NaturalIK::SoechtingForwardPositions fwd;
+ };
+
void NaturalIKTest::vizTaskRun()
{
//ARMARX_IMPORTANT << "vizTask starts";
@@ -593,6 +599,9 @@ namespace armarx
VirtualRobot::RobotNodePtr elb_R = rns_R->getNode(4);
VirtualRobot::RobotNodePtr wri1_R = rns_R->getNode(6);
+ VirtualRobot::RobotNodePtr elb_L = rns_L->getNode(4);
+
+
//float value = 0.5f * (1.0f + std::sin(timeInSeconds));
//robot.joint("ArmR2_Sho1", value);
//robot.joint("ArmR3_Sho2", value);
@@ -630,6 +639,11 @@ namespace armarx
arm_R.elbow = elb_R;
arm_R.tcp = rns_R->getTCP();
+ NaturalIK::ArmJoints arm_L;
+ arm_L.rns = rns_L;
+ arm_L.elbow = elb_L;
+ arm_L.tcp = rns_L->getTCP();
+
auto makeVizSide = [&](viz::Layer & layer, NaturalIK & ik, Eigen::Vector3f target, Eigen::Vector3f & err)
{
@@ -731,57 +745,101 @@ namespace armarx
// ArmR7_Wri1
// ArmR8_Wri2
- VirtualRobot::RobotNodeSetPtr rnsP_R = robot->getRobotNodeSet("PlatformPlanningRightArm");
- VirtualRobot::RobotNodeSetPtr rnsP = robot->getRobotNodeSet("PlatformPlanningBothArms");
- if (p.useCompositeIK)
+ auto calcShoulderAngles = [&](NaturalIK & natik, Eigen::Matrix4f target)
{
-
-
- VirtualRobot::IKSolver::CartesianSelection mode_R = p.p_R.setOri ? VirtualRobot::IKSolver::CartesianSelection::All : VirtualRobot::IKSolver::CartesianSelection::Position;
- CompositeDiffIK cik_R(rnsP_R);
- CompositeDiffIK::TargetPtr tgt = cik_R.addTarget(arm_R.tcp, target_R, mode_R);
- CompositeDiffIK::NullspaceJointTargetPtr nsjt(new CompositeDiffIK::NullspaceJointTarget(rnsP_R));
- NaturalIK::SoechtingForwardPositions fwd = ik_R.solveSoechtingIK(math::Helpers::Position(target_R));
+ NaturalIK::SoechtingForwardPositions fwd = natik.solveSoechtingIK(math::Helpers::Position(target));
Eigen::Vector3f elb = fwd.elbow - fwd.shoulder;
float SE = fwd.soechtingAngles.SE;
- //ARMARX_IMPORTANT << VAROUT(elb);
elb = Eigen::AngleAxisf(-SE, Eigen::Vector3f::UnitX()) * elb;
- //ARMARX_IMPORTANT << VAROUT(elb);
float SR = std::atan2(elb(0), -elb(2));
SR = std::max(-0.1f, SR);
- nsjt->set(1 + 3, SE, 1);
- nsjt->set(2 + 3, SR, 2);
- nsjt->set(3 + 3, -M_PI / 4, 0.5f);
- //ARMARX_IMPORTANT << VAROUT(SE) << VAROUT(SR);
- nsjt->kP = 1;
- cik_R.addNullspaceGradient(nsjt);
+ ShoulderAngles sa;
+ sa.SE = SE;
+ sa.SR = SR;
+ sa.fwd = fwd;
+ return sa;
+ };
+
+ //VirtualRobot::RobotNodeSetPtr rnsP_R = robot->getRobotNodeSet("PlatformPlanningRightArm");
+ VirtualRobot::RobotNodeSetPtr rnsP = robot->getRobotNodeSet("PlatformPlanningBothArms");
+ if (p.useCompositeIK)
+ {
+ CompositeDiffIK cik(rnsP);
+
- CompositeDiffIK::NullspaceTargetPtr nst(new CompositeDiffIK::NullspaceTarget(rnsP_R, arm_R.elbow,
- math::Helpers::CreatePose(fwd.elbow, Eigen::Matrix3f::Identity()),
- VirtualRobot::IKSolver::CartesianSelection::Position));
- nst->kP = 0;
- cik_R.addNullspaceGradient(nst);
+ VirtualRobot::IKSolver::CartesianSelection mode_R = p.p_R.setOri ? VirtualRobot::IKSolver::CartesianSelection::All : VirtualRobot::IKSolver::CartesianSelection::Position;
+ VirtualRobot::IKSolver::CartesianSelection mode_L = p.p_L.setOri ? VirtualRobot::IKSolver::CartesianSelection::All : VirtualRobot::IKSolver::CartesianSelection::Position;
+ CompositeDiffIK::TargetPtr tgt_R = cik.addTarget(arm_R.tcp, target_R, mode_R);
+ CompositeDiffIK::TargetPtr tgt_L = cik.addTarget(arm_L.tcp, target_L, mode_L);
+
+ CompositeDiffIK::NullspaceJointTargetPtr nsjt(new CompositeDiffIK::NullspaceJointTarget(rnsP));
+ //NaturalIK::SoechtingForwardPositions fwd_R = ik_R.solveSoechtingIK(math::Helpers::Position(target_R));
+ //Eigen::Vector3f elb_R = fwd_R.elbow - fwd_R.shoulder;
+ //float SE = fwd_R.soechtingAngles.SE;
+ //elb_R = Eigen::AngleAxisf(-SE, Eigen::Vector3f::UnitX()) * elb;
+ //float SR = std::atan2(elb_R(0), -elb_R(2));
+ //SR = std::max(-0.1f, SR);
+
+ ShoulderAngles sa_R = calcShoulderAngles(ik_R, target_R);
+ ShoulderAngles sa_L = calcShoulderAngles(ik_L, target_L);
+
+ nsjt->set("ArmR2_Sho1", +sa_R.SE, 1);
+ nsjt->set("ArmR3_Sho2", +sa_R.SR, 2);
+ nsjt->set("ArmR4_Sho3", -M_PI / 4, 0.5f);
+
+ nsjt->set("ArmL2_Sho1", -sa_L.SE, 1);
+ nsjt->set("ArmL3_Sho2", +sa_L.SR, 2);
+ nsjt->set("ArmL4_Sho3", +M_PI / 4, 0.5f);
- CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(new CompositeDiffIK::NullspaceJointLimitAvoidance(rnsP_R));
+ nsjt->set("ArmR1_Cla1", 0, 0.5f);
+ nsjt->set("ArmL1_Cla1", 0, 0.5f);
+
+
+ //ARMARX_IMPORTANT << VAROUT(SE) << VAROUT(SR);
+ nsjt->kP = 1;
+ cik.addNullspaceGradient(nsjt);
+
+ //CompositeDiffIK::NullspaceTargetPtr nst_R(new CompositeDiffIK::NullspaceTarget(rnsP, arm_R.elbow,
+ // math::Helpers::CreatePose(fwd_R.elbow, Eigen::Matrix3f::Identity()),
+ // VirtualRobot::IKSolver::CartesianSelection::Position));
+ //nst_R->kP = 0;
+ //cik.addNullspaceGradient(nst_R);
+ CompositeDiffIK::NullspaceTargetPtr nst_R = cik.addNullspacePositionTarget(arm_R.elbow, sa_R.fwd.elbow);
+ nst_R->kP = 0;
+ CompositeDiffIK::NullspaceTargetPtr nst_L = cik.addNullspacePositionTarget(arm_L.elbow, sa_L.fwd.elbow);
+ nst_L->kP = 0;
+
+ CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(new CompositeDiffIK::NullspaceJointLimitAvoidance(rnsP));
nsjla->setWeight(0, 0);
nsjla->setWeight(1, 0);
nsjla->setWeight(2, 0);
- nsjla->setWeight(2 + 3, 0);
+ nsjla->setWeight("ArmR4_Sho3", 0);
+ nsjla->setWeight("ArmL4_Sho3", 0);
nsjla->kP = 0.1;
- cik_R.addNullspaceGradient(nsjla);
+ cik.addNullspaceGradient(nsjla);
CompositeDiffIK::Parameters cp;
cp.resetRnsValues = true;
cp.returnIKSteps = true;
- CompositeDiffIK::Result cikResult = cik_R.solve(cp);
+ CompositeDiffIK::Result cikResult = cik.solve(cp);
layer_iksteps.clear();
int i = 0;
- for (const CompositeDiffIK::TargetStep& s : tgt->ikSteps)
+ for (const CompositeDiffIK::TargetStep& s : tgt_R->ikSteps)
{
layer_iksteps.add(viz::Box("tcp_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
i++;
}
- for (const CompositeDiffIK::NullspaceTargetStep& s : nst->ikSteps)
+ for (const CompositeDiffIK::TargetStep& s : tgt_L->ikSteps)
+ {
+ layer_iksteps.add(viz::Box("tcp_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
+ i++;
+ }
+ for (const CompositeDiffIK::NullspaceTargetStep& s : nst_R->ikSteps)
+ {
+ layer_iksteps.add(viz::Box("elb_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
+ i++;
+ }
+ for (const CompositeDiffIK::NullspaceTargetStep& s : nst_L->ikSteps)
{
layer_iksteps.add(viz::Box("elb_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
i++;
@@ -790,9 +848,9 @@ namespace armarx
}
else
{
- rnsP_R->getNode(0)->setJointValue(0);
- rnsP_R->getNode(1)->setJointValue(0);
- rnsP_R->getNode(2)->setJointValue(0);
+ rnsP->getNode(0)->setJointValue(0);
+ rnsP->getNode(1)->setJointValue(0);
+ rnsP->getNode(2)->setJointValue(0);
NaturalDiffIK::Result ikResult;
if (p.p_R.setOri)
{
@@ -818,7 +876,7 @@ namespace armarx
- vizrobot.joints(rnsP_R->getJointValueMap());
+ vizrobot.joints(rnsP->getJointValueMap());
arviz.commit({layer_R, layer_L, layer_robot, layer_iksteps});
@@ -857,7 +915,7 @@ namespace armarx
ori.y() = oriOld.x() * -1;
ori.z() = oriOld.w();
- return ori;
+ return (ori * Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ()));
}
diff --git a/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp b/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp
index 68e627eac..63214d324 100644
--- a/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp
@@ -51,6 +51,15 @@ void CompositeDiffIK::addNullspaceGradient(const CompositeDiffIK::NullspaceGradi
nullspaceGradients.emplace_back(gradient);
}
+CompositeDiffIK::NullspaceTargetPtr CompositeDiffIK::addNullspacePositionTarget(const VirtualRobot::RobotNodePtr& tcp, const Eigen::Vector3f& target)
+{
+ CompositeDiffIK::NullspaceTargetPtr nst(new CompositeDiffIK::NullspaceTarget(rns, tcp,
+ math::Helpers::CreatePose(target, Eigen::Matrix3f::Identity()),
+ VirtualRobot::IKSolver::CartesianSelection::Position));
+ addNullspaceGradient(nst);
+ return nst;
+}
+
CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params)
{
@@ -72,131 +81,42 @@ CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params)
}
//ARMARX_IMPORTANT << VAROUT(rns->getJointValuesEigen().transpose());
- int rows = 0;
- int cols = rns->getSize();
+ SolveState s;
+
+ s.rows = 0;
+ s.cols = rns->getSize();
for (const TargetPtr& target : targets)
{
- rows += CartesianSelectionToSize(target->mode);
+ s.rows += CartesianSelectionToSize(target->mode);
}
- Eigen::VectorXf jointRegularization = Eigen::VectorXf::Zero(cols);
+ s.jointRegularization = Eigen::VectorXf::Zero(s.cols);
for (size_t i = 0; i < rns->getSize(); i++)
{
- jointRegularization(i) = rns->getNode(i)->isTranslationalJoint() ? params.jointRegularizationTranslation : params.jointRegularizationRotation;
+ s.jointRegularization(i) = rns->getNode(i)->isTranslationalJoint() ? params.jointRegularizationTranslation : params.jointRegularizationRotation;
}
- Eigen::VectorXf cartesianRegularization = Eigen::VectorXf::Zero(rows);
+ 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, cols);
- cartesianRegularization.block(row, 0, h, 1) = tmpVC.calculateRegularization(target->mode);
+ target->jacobi = Eigen::MatrixXf::Zero(h, s.cols);
+ s.cartesianRegularization.block(row, 0, h, 1) = tmpVC.calculateRegularization(target->mode);
row += h;
}
}
//ARMARX_IMPORTANT << "START";
//ARMARX_INFO << VAROUT(regularization.transpose());
- Eigen::VectorXf currentJointValues = rns->getJointValuesEigen();
+ s.jointValues = rns->getJointValuesEigen();
for (size_t i = 0; i <= params.steps; i++)
{
- //ARMARX_IMPORTANT << VAROUT(i);
- Eigen::MatrixXf jacobi = Eigen::MatrixXf::Zero(rows, cols);
- Eigen::MatrixXf invJac = Eigen::MatrixXf::Zero(cols, rows);
-
- Eigen::VectorXf cartesianVel = Eigen::VectorXf::Zero(rows);
- {
- int row = 0;
- for (const TargetPtr& target : targets)
- {
- ik->updateJacobianMatrix(target->jacobi, target->tcp, target->mode);
- int h = CartesianSelectionToSize(target->mode);
- jacobi.block(row, 0, h, cols) = target->jacobi;
- Eigen::VectorXf cv = target->pCtrl.calculate(target->target, target->mode);
- //ARMARX_INFO << VAROUT(cv.transpose());
- 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(target->target);
- step.oriDiff = target->pCtrl.getOrientationDiff(target->target);
- target->ikSteps.emplace_back(step);
- }
- }
- }
- //ARMARX_INFO << VAROUT(cartesianVel.transpose());
- if (i == 0)
- {
- //ARMARX_IMPORTANT << VAROUT(jacobi);
- }
-
- for (int row = 0; row < rows; row++)
- {
- jacobi.block(row, 0, 1, cols) = jacobi.block(row, 0, 1, cols).cwiseProduct(jointRegularization.transpose());
- }
- if (i == 0)
- {
- //ARMARX_IMPORTANT << VAROUT(jacobi);
- }
-
- ik->updatePseudoInverseJacobianMatrix(invJac, jacobi, 0, cartesianRegularization);
-
- Eigen::VectorXf nullspaceVel = Eigen::VectorXf::Zero(cols);
-
- for (const NullspaceGradientPtr& nsGradient : nullspaceGradients)
- {
- Eigen::VectorXf nsgrad = nsGradient->kP * nsGradient->getGradient(params);
- //ARMARX_INFO << VAROUT(nsgrad.transpose());
- nullspaceVel += nsgrad;
- if (i == 0)
- {
- //ARMARX_IMPORTANT << VAROUT(nsgrad.transpose());
- }
- }
- //LimitInfNormTo(nullspaceVel, params.maxJointAngleStep);
-
- Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
- Eigen::MatrixXf nullspace = lu_decomp.kernel();
-
- Eigen::VectorXf nsv = Eigen::VectorXf::Zero(cols);
- 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();
- }
- }
- Eigen::VectorXf jv = invJac * cartesianVel;
- //ARMARX_INFO << VAROUT(jv.transpose());
- //ARMARX_INFO << VAROUT(nsv.transpose());
- jv = jv + nsv;
- jv = jv * params.stepSize;
- jv = LimitInfNormTo(jv, params.maxJointAngleStep);
- jv = jv.cwiseProduct(jointRegularization);
-
- //ARMARX_INFO << VAROUT(jv.transpose());
-
- Eigen::VectorXf newJointValues = currentJointValues + jv;
- if (i == 0)
- {
- //ARMARX_IMPORTANT << VAROUT(cartesianVel.transpose());
- //ARMARX_IMPORTANT << VAROUT(nullspaceVel.transpose());
-
- //ARMARX_IMPORTANT << VAROUT(currentJointValues.transpose());
- //ARMARX_IMPORTANT << VAROUT(newJointValues.transpose());
- }
- rns->setJointValues(newJointValues);
- currentJointValues = newJointValues;
+ step(params, s, i);
}
bool allReached = true;
@@ -232,6 +152,132 @@ CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params)
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)
+{
+ //ARMARX_IMPORTANT << VAROUT(i);
+ 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::VectorXf cv = target->pCtrl.calculate(target->target, target->mode);
+ //ARMARX_INFO << VAROUT(cv.transpose());
+ 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(target->target);
+ step.oriDiff = target->pCtrl.getOrientationDiff(target->target);
+ target->ikSteps.emplace_back(step);
+ }
+ }
+ }
+ //ARMARX_INFO << VAROUT(cartesianVel.transpose());
+ if (stepNr == 0)
+ {
+ //ARMARX_IMPORTANT << VAROUT(s.jacobi);
+ }
+
+ 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());
+ }
+ if (stepNr == 0)
+ {
+ //ARMARX_IMPORTANT << VAROUT(s.jacobi);
+ }
+
+ 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->getGradient(params);
+ //ARMARX_INFO << VAROUT(nsgrad.transpose());
+ nullspaceVel += nsgrad;
+ if (stepNr == 0)
+ {
+ //ARMARX_IMPORTANT << VAROUT(nsgrad.transpose());
+ }
+ }
+ //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;
+ //ARMARX_INFO << VAROUT(jv.transpose());
+ //ARMARX_INFO << VAROUT(nsv.transpose());
+ jv = jv + nsv;
+ jv = jv * params.stepSize;
+ jv = LimitInfNormTo(jv, params.maxJointAngleStep);
+ jv = jv.cwiseProduct(s.jointRegularization);
+
+ //ARMARX_INFO << VAROUT(jv.transpose());
+
+ Eigen::VectorXf newJointValues = s.jointValues + jv;
+ if (stepNr == 0)
+ {
+ //ARMARX_IMPORTANT << VAROUT(cartesianVel.transpose());
+ //ARMARX_IMPORTANT << VAROUT(nullspaceVel.transpose());
+
+ //ARMARX_IMPORTANT << VAROUT(currentJointValues.transpose());
+ //ARMARX_IMPORTANT << VAROUT(newJointValues.transpose());
+ }
+ rns->setJointValues(newJointValues);
+ s.jointValues = newJointValues;
+}
+
int CompositeDiffIK::CartesianSelectionToSize(VirtualRobot::IKSolver::CartesianSelection mode)
{
switch (mode)
@@ -247,6 +293,7 @@ int CompositeDiffIK::CartesianSelectionToSize(VirtualRobot::IKSolver::CartesianS
}
}
+
CompositeDiffIK::Target::Target(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const Eigen::Matrix4f& target, VirtualRobot::IKSolver::CartesianSelection mode)
: tcp(tcp), target(target), mode(mode), pCtrl(tcp)
{
diff --git a/source/RobotAPI/libraries/diffik/CompositeDiffIK.h b/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
index 9f6576269..5fe297233 100644
--- a/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
@@ -167,19 +167,38 @@ namespace armarx
Eigen::Vector3f elbowPosDiff;
};
+ 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);
CompositeDiffIK(const VirtualRobot::RobotNodeSetPtr& rns);
void addTarget(const TargetPtr& target);
TargetPtr addTarget(const VirtualRobot::RobotNodePtr& tcp, const Eigen::Matrix4f& target, VirtualRobot::IKSolver::CartesianSelection mode);
+
void addNullspaceGradient(const NullspaceGradientPtr& gradient);
+ NullspaceTargetPtr addNullspacePositionTarget(const VirtualRobot::RobotNodePtr& tcp, const Eigen::Vector3f& target);
Result solve(Parameters params);
+ static Eigen::MatrixXf CalculateNullspaceSVD(const Eigen::Matrix4f& jacobi);
+ static Eigen::MatrixXf CalculateNullspaceLU(const Eigen::Matrix4f& jacobi);
+ void step(Parameters& params, SolveState& s, int stepNr);
int CartesianSelectionToSize(VirtualRobot::IKSolver::CartesianSelection mode);
+
private:
VirtualRobot::RobotNodeSetPtr rns;
std::vector<TargetPtr> targets;
--
GitLab