diff --git a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
index f230c8305809e4d0657b5fea04570c82fd462eec..feb67dc48078e393f420fe922e8e872e0683a769 100644
--- a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
+++ b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
@@ -101,78 +101,120 @@ namespace armarx
RemoteGui::WidgetPtr NaturalIKTest::buildGui()
{
- return RemoteGui::makeVBoxLayout().addChild(
- RemoteGui::makeSimpleGridLayout().cols(3)
- .addTextLabel("X")
- .addChild(RemoteGui::makeFloatSlider("X").min(-500).max(500).value(0))
- .addChild(new RemoteGui::HSpacer)
+ RemoteGui::detail::GroupBoxBuilder leftBox = RemoteGui::makeGroupBox().label("Left").addChild(
+ RemoteGui::makeSimpleGridLayout().cols(2)
+ .addTextLabel("L.X")
+ .addChild(RemoteGui::makeFloatSlider("L.X").min(-600).max(600).value(0))
- .addTextLabel("Y")
- .addChild(RemoteGui::makeFloatSlider("Y").min(200).max(1000).value(400))
- .addChild(new RemoteGui::HSpacer)
+ .addTextLabel("L.Y")
+ .addChild(RemoteGui::makeFloatSlider("L.Y").min(200).max(1200).value(600))
- .addTextLabel("Z")
- .addChild(RemoteGui::makeFloatSlider("Z").min(-600).max(600).value(0))
- .addChild(new RemoteGui::HSpacer)
+ .addTextLabel("L.Z")
+ .addChild(RemoteGui::makeFloatSlider("L.Z").min(-600).max(600).value(-200))
- .addTextLabel("scale")
- .addChild(RemoteGui::makeFloatSlider("scale").min(0.5).max(2).value(1.3f))
- .addChild(new RemoteGui::HSpacer)
+ .addTextLabel("L.scale")
+ .addChild(RemoteGui::makeFloatSlider("L.scale").min(0.5).max(2).value(1.3f))
- .addTextLabel("minElbZ")
- .addChild(RemoteGui::makeFloatSlider("minElbZ").min(500).max(2000).value(500))
- .addChild(new RemoteGui::HSpacer)
+ .addTextLabel("L.minElbZ")
+ .addChild(RemoteGui::makeFloatSlider("L.minElbZ").min(500).max(2000).value(500))
- .addChild(RemoteGui::makeCheckBox("setOri").value(true))
- .addChild(new RemoteGui::HSpacer)
- .addChild(new RemoteGui::HSpacer)
+ .addChild(RemoteGui::makeCheckBox("L.setOri").value(true))
+ .addChild(new RemoteGui::HSpacer)
- .addTextLabel("rX")
- .addChild(RemoteGui::makeFloatSlider("rX").min(-180).max(180).value(0))
- .addChild(new RemoteGui::HSpacer)
+ .addTextLabel("L.rX")
+ .addChild(RemoteGui::makeFloatSlider("L.rX").min(-180).max(180).value(0))
- .addTextLabel("rY")
- .addChild(RemoteGui::makeFloatSlider("rY").min(-180).max(180).value(0))
- .addChild(new RemoteGui::HSpacer)
+ .addTextLabel("L.rY")
+ .addChild(RemoteGui::makeFloatSlider("L.rY").min(-180).max(180).value(0))
- .addTextLabel("rZ")
- .addChild(RemoteGui::makeFloatSlider("rZ").min(-180).max(180).value(0))
- .addChild(new RemoteGui::HSpacer)
+ .addTextLabel("L.rZ")
+ .addChild(RemoteGui::makeFloatSlider("L.rZ").min(-180).max(180).value(0))
+ );
+ RemoteGui::detail::GroupBoxBuilder rightBox = RemoteGui::makeGroupBox().label("Right").addChild(
+ RemoteGui::makeSimpleGridLayout().cols(2)
+ .addTextLabel("R.X")
+ .addChild(RemoteGui::makeFloatSlider("R.X").min(-600).max(600).value(0))
- .addTextLabel("elbowKp")
- .addChild(RemoteGui::makeFloatSlider("elbowKp").min(0).max(1).value(1))
- .addChild(new RemoteGui::HSpacer)
+ .addTextLabel("R.Y")
+ .addChild(RemoteGui::makeFloatSlider("R.Y").min(200).max(1200).value(600))
- .addTextLabel("jlaKp")
- .addChild(RemoteGui::makeFloatSlider("jlaKp").min(0).max(1).value(0))
- .addChild(new RemoteGui::HSpacer)
+ .addTextLabel("R.Z")
+ .addChild(RemoteGui::makeFloatSlider("R.Z").min(-600).max(600).value(-200))
- .addChild(RemoteGui::makeButton("runTest").label("run test"))
+ .addTextLabel("R.scale")
+ .addChild(RemoteGui::makeFloatSlider("R.scale").min(0.5).max(2).value(1.3f))
- )
- .addChild(RemoteGui::makeLabel("errors").value("<errors>"));
+ .addTextLabel("R.minElbZ")
+ .addChild(RemoteGui::makeFloatSlider("R.minElbZ").min(500).max(2000).value(500))
+
+ .addChild(RemoteGui::makeCheckBox("R.setOri").value(true))
+ .addChild(new RemoteGui::HSpacer)
+
+ .addTextLabel("R.rX")
+ .addChild(RemoteGui::makeFloatSlider("R.rX").min(-180).max(180).value(0))
+
+ .addTextLabel("R.rY")
+ .addChild(RemoteGui::makeFloatSlider("R.rY").min(-180).max(180).value(0))
+
+ .addTextLabel("R.rZ")
+ .addChild(RemoteGui::makeFloatSlider("R.rZ").min(-180).max(180).value(0))
+ );
+
+ return RemoteGui::makeVBoxLayout()
+ .addChild(RemoteGui::makeCheckBox("useCompositeIK").value(false))
+ .addChild(RemoteGui::makeSimpleGridLayout().cols(2).addChild(leftBox).addChild(rightBox))
+ .addChild(RemoteGui::makeSimpleGridLayout().cols(2)
+ .addTextLabel("elbowKp")
+ .addChild(RemoteGui::makeFloatSlider("elbowKp").min(0).max(1).value(1))
+
+ .addTextLabel("jlaKp")
+ .addChild(RemoteGui::makeFloatSlider("jlaKp").min(0).max(1).value(0))
+ )
+ .addChild(RemoteGui::makeLabel("errors").value("<errors>"))
+ .addChild(RemoteGui::makeButton("runTest").label("run test"));
}
void NaturalIKTest::processGui(RemoteGui::TabProxy& prx)
{
prx.receiveUpdates();
- const float x = prx.getValue<float>("X").get();
- const float y = prx.getValue<float>("Y").get();
- const float z = prx.getValue<float>("Z").get();
- p.target = Eigen::Vector3f(x, y, z);
- p.scale = prx.getValue<float>("scale").get();
- p.ikparams.minimumElbowHeight = prx.getValue<float>("minElbZ").get();
- const float rx = prx.getValue<float>("rX").get();
- const float ry = prx.getValue<float>("rY").get();
- const float rz = prx.getValue<float>("rZ").get();
- p.targetRotation = Eigen::Vector3f(rx, ry, rz);
+ p.useCompositeIK = prx.getValue<bool>("useCompositeIK").get();
+
+ float x, y, z, rx, ry, rz;
+
+ x = prx.getValue<float>("R.X").get();
+ y = prx.getValue<float>("R.Y").get();
+ z = prx.getValue<float>("R.Z").get();
+ p.p_R.target = Eigen::Vector3f(x, y, z);
+ p.p_R.scale = prx.getValue<float>("R.scale").get();
+ p.p_R.ikparams.minimumElbowHeight = prx.getValue<float>("R.minElbZ").get();
+ p.p_R.setOri = prx.getValue<bool>("R.setOri").get();
+ rx = prx.getValue<float>("R.rX").get();
+ ry = prx.getValue<float>("R.rY").get();
+ rz = prx.getValue<float>("R.rZ").get();
+ p.p_R.targetRotation = Eigen::Vector3f(rx, ry, rz);
+
+ x = prx.getValue<float>("L.X").get();
+ y = prx.getValue<float>("L.Y").get();
+ z = prx.getValue<float>("L.Z").get();
+ p.p_L.target = Eigen::Vector3f(x, y, z);
+ p.p_L.scale = prx.getValue<float>("L.scale").get();
+ p.p_L.ikparams.minimumElbowHeight = prx.getValue<float>("L.minElbZ").get();
+ p.p_L.setOri = prx.getValue<bool>("L.setOri").get();
+ rx = prx.getValue<float>("L.rX").get();
+ ry = prx.getValue<float>("L.rY").get();
+ rz = prx.getValue<float>("L.rZ").get();
+ p.p_L.targetRotation = Eigen::Vector3f(rx, ry, rz);
+
+ p.p_R.ikparams.diffIKparams.elbowKp = prx.getValue<float>("elbowKp").get();
+ p.p_R.ikparams.diffIKparams.jointLimitAvoidanceKp = prx.getValue<float>("jlaKp").get();
+ p.p_R.ikparams.diffIKparams.returnIKSteps = true;
+
+ p.p_L.ikparams.diffIKparams.elbowKp = prx.getValue<float>("elbowKp").get();
+ p.p_L.ikparams.diffIKparams.jointLimitAvoidanceKp = prx.getValue<float>("jlaKp").get();
+ p.p_L.ikparams.diffIKparams.returnIKSteps = true;
- p.ikparams.diffIKparams.elbowKp = prx.getValue<float>("elbowKp").get();
- p.ikparams.diffIKparams.jointLimitAvoidanceKp = prx.getValue<float>("jlaKp").get();
- p.ikparams.diffIKparams.returnIKSteps = true;
- p.setOri = prx.getValue<bool>("setOri").get();
p.targetValid = true;
std::stringstream ss;
@@ -487,7 +529,7 @@ namespace armarx
nsjt->set(2, 0.2, 1);
ik.addNullspaceGradient(nsjt);
CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(new CompositeDiffIK::NullspaceJointLimitAvoidance(rns_R));
- nsjla->setScale(2, 0);
+ nsjla->setWeight(2, 0);
ik.addNullspaceGradient(nsjla);
CompositeDiffIK::Result result = ik.solve(pc);
if (result.reached)
@@ -591,7 +633,7 @@ namespace armarx
auto makeVizSide = [&](viz::Layer & layer, NaturalIK & ik, Eigen::Vector3f target, Eigen::Vector3f & err)
{
- ik.setScale(p.scale);
+ ik.setScale(p.p_R.scale);
Eigen::Vector3f vtgt = target;
NaturalIK::SoechtingAngles soechtingAngles = ik.CalculateSoechtingAngles(vtgt);
@@ -641,8 +683,8 @@ namespace armarx
continue;
}
- makeVizSide(layer_R, ik_R, p.target + offset, p.err_R);
- makeVizSide(layer_L, ik_L, p.target + offset, p.err_L);
+ makeVizSide(layer_R, ik_R, p.p_R.target + offset, p.err_R);
+ makeVizSide(layer_L, ik_L, p.p_L.target + offset, p.err_L);
//Eigen::Quaternionf targetOri(0.70029222965240479,
@@ -655,51 +697,128 @@ namespace armarx
// 0.70710688,
// -0.70710682,
// 0);
- Eigen::Quaternionf referenceOri(0.70029222965240479,
- -0.6757887601852417,
- 0.036805182695388794,
- 0.22703713178634644);
- Eigen::AngleAxisf aa(p.targetRotation.norm() / 180 * M_PI, p.targetRotation.normalized());
- Eigen::Matrix3f targetOri = referenceOri.toRotationMatrix() * aa.toRotationMatrix();
-
- Eigen::Matrix4f target_R = math::Helpers::Pose(p.target + offset, targetOri);
+ Eigen::Quaternionf referenceOri_R(0.70029222965240479,
+ -0.6757887601852417,
+ 0.036805182695388794,
+ 0.22703713178634644);
+ Eigen::Quaternionf referenceOri_L = mirrorOri(referenceOri_R);
+
+ Eigen::AngleAxisf aa_R(p.p_R.targetRotation.norm() / 180 * M_PI, p.p_R.targetRotation.normalized());
+ Eigen::Matrix3f targetOri_R = referenceOri_R.toRotationMatrix() * aa_R.toRotationMatrix();
+ Eigen::Matrix4f target_R = math::Helpers::Pose(p.p_R.target + offset, targetOri_R);
+
+ Eigen::AngleAxisf aa_L(p.p_L.targetRotation.norm() / 180 * M_PI, p.p_L.targetRotation.normalized());
+ Eigen::Matrix3f targetOri_L = referenceOri_L.toRotationMatrix() * aa_L.toRotationMatrix();
+ Eigen::Matrix4f target_L = math::Helpers::Pose(p.p_L.target + offset, targetOri_L);
+
+ // X_Platform
+ // Y_Platform
+ // Yaw_Platform
+ // ArmL1_Cla1
+ // ArmL2_Sho1
+ // ArmL3_Sho2
+ // ArmL4_Sho3
+ // ArmL5_Elb1
+ // ArmL6_Elb2
+ // ArmL7_Wri1
+ // ArmL8_Wri2
+ // ArmR1_Cla1
+ // ArmR2_Sho1
+ // ArmR3_Sho2
+ // ArmR4_Sho3
+ // ArmR5_Elb1
+ // ArmR6_Elb2
+ // ArmR7_Wri1
+ // ArmR8_Wri2
+
+ VirtualRobot::RobotNodeSetPtr rnsP_R = robot->getRobotNodeSet("PlatformPlanningRightArm");
+ VirtualRobot::RobotNodeSetPtr rnsP = robot->getRobotNodeSet("PlatformPlanningBothArms");
+ if (p.useCompositeIK)
+ {
+ 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));
+ 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);
+
+ 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);
+
+ CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(new CompositeDiffIK::NullspaceJointLimitAvoidance(rnsP_R));
+ nsjla->setWeight(0, 0);
+ nsjla->setWeight(1, 0);
+ nsjla->setWeight(2, 0);
+ nsjla->setWeight(2 + 3, 0);
+ nsjla->kP = 0.1;
+ cik_R.addNullspaceGradient(nsjla);
+ CompositeDiffIK::Parameters cp;
+ cp.resetRnsValues = true;
+ cp.returnIKSteps = true;
+ CompositeDiffIK::Result cikResult = cik_R.solve(cp);
+
+ layer_iksteps.clear();
+ int i = 0;
+ for (const CompositeDiffIK::TargetStep& s : tgt->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)
+ {
+ layer_iksteps.add(viz::Box("elb_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
+ i++;
+ }
- NaturalDiffIK::Result ikResult;
- if (p.setOri)
- {
- ikResult = ik_R.calculateIK(target_R, arm_R, p.ikparams);
}
else
{
- ikResult = ik_R.calculateIKpos(math::Helpers::Position(target_R), arm_R, p.ikparams);
- }
+ rnsP_R->getNode(0)->setJointValue(0);
+ rnsP_R->getNode(1)->setJointValue(0);
+ rnsP_R->getNode(2)->setJointValue(0);
+ NaturalDiffIK::Result ikResult;
+ if (p.p_R.setOri)
+ {
+ ikResult = ik_R.calculateIK(target_R, arm_R, p.p_R.ikparams);
+ }
+ else
+ {
+ ikResult = ik_R.calculateIKpos(math::Helpers::Position(target_R), arm_R, p.p_R.ikparams);
+ }
- layer_iksteps.clear();
- std::stringstream ss;
- int i = 0;
- for (const NaturalDiffIK::IKStep& s : ikResult.ikSteps)
- {
- ss << s.pdTcp.norm() << "; " << s.odTcp.norm() << "; " << s.pdElb.norm() << "\n";
- layer_iksteps.add(viz::Box("tcp_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
- layer_iksteps.add(viz::Box("elb_" + std::to_string(i)).size(20).pose(s.elbPose).color(viz::Color::blue()));
- i++;
+ layer_iksteps.clear();
+ std::stringstream ss;
+ int i = 0;
+ for (const NaturalDiffIK::IKStep& s : ikResult.ikSteps)
+ {
+ ss << s.pdTcp.norm() << "; " << s.odTcp.norm() << "; " << s.pdElb.norm() << "\n";
+ layer_iksteps.add(viz::Box("tcp_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
+ layer_iksteps.add(viz::Box("elb_" + std::to_string(i)).size(20).pose(s.elbPose).color(viz::Color::blue()));
+ i++;
+ }
+ //ARMARX_IMPORTANT << ss.str();
}
- //ARMARX_IMPORTANT << ss.str();
- CompositeDiffIK cik_R(rns_R);
- cik_R.addTarget(arm_R.tcp, target_R, VirtualRobot::IKSolver::CartesianSelection::All);
- CompositeDiffIK::NullspaceJointTargetPtr nsjt(new CompositeDiffIK::NullspaceJointTarget(rns_R));
- nsjt->set(2, 0.2, 1);
- cik_R.addNullspaceGradient(nsjt);
- CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(new CompositeDiffIK::NullspaceJointLimitAvoidance(rns_R));
- nsjla->set(2, 0);
- cik_R.addNullspaceGradient(nsjla);
- CompositeDiffIK::Paramaters cp;
- CompositeDiffIK cikResult = cik_R.solve(cp);
- vizrobot.joints(arm_R.rns->getJointValueMap());
+
+ vizrobot.joints(rnsP_R->getJointValueMap());
arviz.commit({layer_R, layer_L, layer_robot, layer_iksteps});
@@ -711,6 +830,35 @@ namespace armarx
}
+ Eigen::Matrix4f NaturalIKTest::mirrorPose(Eigen::Matrix4f oldPose)
+ {
+ Eigen::Quaternionf oriOld(oldPose.block<3, 3>(0, 0));
+ Eigen::Quaternionf ori;
+
+ ori.w() = oriOld.z();
+ ori.x() = oriOld.y() * -1;
+ ori.y() = oriOld.x() * -1;
+ ori.z() = oriOld.w();
+
+ Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
+ pose.block<3, 3>(0, 0) = (ori * Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())).toRotationMatrix();
+ pose(0, 3) = -oldPose(0, 3);
+ pose(1, 3) = oldPose(1, 3);
+ pose(2, 3) = oldPose(2, 3);
+
+ return pose;
+ }
+ Eigen::Quaternionf NaturalIKTest::mirrorOri(Eigen::Quaternionf oriOld)
+ {
+ Eigen::Quaternionf ori;
+
+ ori.w() = oriOld.z();
+ ori.x() = oriOld.y() * -1;
+ ori.y() = oriOld.x() * -1;
+ ori.z() = oriOld.w();
+
+ return ori;
+ }
diff --git a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.h b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.h
index 9f9d09cc5ca987330c552d8a508ccebaffb79eee..ca8a91ddf88ef9ce10c92027f10e2f1f69cd8b56 100644
--- a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.h
+++ b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.h
@@ -68,17 +68,24 @@ namespace armarx
{
public:
- struct GuiParams
+ struct GuiSideParams
{
- std::atomic_bool targetValid = false;
Eigen::Vector3f target;
Eigen::Vector3f targetRotation;
- Eigen::Vector3f err_R = Eigen::Vector3f::Zero(), err_L = Eigen::Vector3f::Zero();
float scale = 1.3f;
float minElbZ;
-
- NaturalIK::Parameters ikparams;
std::atomic_bool setOri;
+ NaturalIK::Parameters ikparams;
+ };
+
+ struct GuiParams
+ {
+ GuiSideParams p_R;
+ GuiSideParams p_L;
+ std::atomic_bool targetValid = false;
+ Eigen::Vector3f err_R = Eigen::Vector3f::Zero(), err_L = Eigen::Vector3f::Zero();
+
+ std::atomic_bool useCompositeIK;
};
@@ -107,6 +114,8 @@ namespace armarx
void vizTaskRun();
void testTaskRun();
+ Eigen::Matrix4f mirrorPose(Eigen::Matrix4f oldPose);
+ Eigen::Quaternionf mirrorOri(Eigen::Quaternionf oriOld);
private:
diff --git a/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp b/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp
index 0e72540bd51656cea2b5695669a1cf6d6ca1c50b..68e627eac22e1b18f4e96023e3dc0fd2f29bd6b7 100644
--- a/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp
@@ -39,9 +39,11 @@ void CompositeDiffIK::addTarget(const TargetPtr& target)
targets.emplace_back(target);
}
-void CompositeDiffIK::addTarget(const VirtualRobot::RobotNodePtr& tcp, const Eigen::Matrix4f& target, VirtualRobot::IKSolver::CartesianSelection mode)
+CompositeDiffIK::TargetPtr CompositeDiffIK::addTarget(const VirtualRobot::RobotNodePtr& tcp, const Eigen::Matrix4f& target, VirtualRobot::IKSolver::CartesianSelection mode)
{
- addTarget(std::make_shared<Target>(rns, tcp, target, mode));
+ TargetPtr t = std::make_shared<Target>(rns, tcp, target, mode);
+ addTarget(t);
+ return t;
}
void CompositeDiffIK::addNullspaceGradient(const CompositeDiffIK::NullspaceGradientPtr& gradient)
@@ -52,6 +54,8 @@ void CompositeDiffIK::addNullspaceGradient(const CompositeDiffIK::NullspaceGradi
CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params)
{
+ //ARMARX_IMPORTANT << "###";
+ //ARMARX_IMPORTANT << VAROUT(rns->getJointValuesEigen().transpose());
if (params.resetRnsValues)
{
for (VirtualRobot::RobotNodePtr rn : rns->getAllRobotNodes())
@@ -66,6 +70,7 @@ CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params)
}
}
}
+ //ARMARX_IMPORTANT << VAROUT(rns->getJointValuesEigen().transpose());
int rows = 0;
int cols = rns->getSize();
@@ -75,7 +80,13 @@ CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params)
rows += CartesianSelectionToSize(target->mode);
}
- Eigen::VectorXf regularization = Eigen::VectorXf::Zero(rows);
+ Eigen::VectorXf jointRegularization = Eigen::VectorXf::Zero(cols);
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ jointRegularization(i) = rns->getNode(i)->isTranslationalJoint() ? params.jointRegularizationTranslation : params.jointRegularizationRotation;
+ }
+
+ Eigen::VectorXf cartesianRegularization = Eigen::VectorXf::Zero(rows);
{
CartesianVelocityController tmpVC(rns);
int row = 0;
@@ -83,15 +94,18 @@ CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params)
{
int h = CartesianSelectionToSize(target->mode);
target->jacobi = Eigen::MatrixXf::Zero(h, cols);
- regularization.block(row, 0, h, 1) = tmpVC.calculateRegularization(target->mode);
+ 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();
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);
@@ -103,20 +117,51 @@ CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params)
ik->updateJacobianMatrix(target->jacobi, target->tcp, target->mode);
int h = CartesianSelectionToSize(target->mode);
jacobi.block(row, 0, h, cols) = target->jacobi;
- cartesianVel.block(row, 0, h, 1) = target->pCtrl.calculate(target->target, target->mode);
+ 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);
+ }
- ik->updatePseudoInverseJacobianMatrix(invJac, jacobi, 0, regularization);
+ 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)
{
- nullspaceVel += nsGradient->kP * nsGradient->getGradient();
+ 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);
+ //LimitInfNormTo(nullspaceVel, params.maxJointAngleStep);
Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
Eigen::MatrixXf nullspace = lu_decomp.kernel();
@@ -131,10 +176,25 @@ CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params)
nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
}
}
- Eigen::VectorXf jv = invJac * cartesianVel + nsv;
- LimitInfNormTo(jv, params.maxJointAngleStep);
+ 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;
}
@@ -145,6 +205,8 @@ CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params)
allReached = allReached && target->pCtrl.reached(target->target, target->mode, target->maxPosError, target->maxOriError);
}
+ //ARMARX_IMPORTANT << "END";
+
//ARMARX_IMPORTANT << ss.str();
@@ -197,10 +259,26 @@ CompositeDiffIK::NullspaceTarget::NullspaceTarget(const VirtualRobot::RobotNodeS
}
-Eigen::VectorXf CompositeDiffIK::NullspaceTarget::getGradient()
+void CompositeDiffIK::NullspaceTarget::init(Parameters&)
{
- Eigen::VectorXf cartesianVel = pCtrl.calculate(target, mode);
- return vCtrl.calculate(cartesianVel, mode);
+ ikSteps.clear();
+}
+
+Eigen::VectorXf CompositeDiffIK::NullspaceTarget::getGradient(Parameters& params)
+{
+ 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;
}
Eigen::VectorXf CompositeDiffIK::LimitInfNormTo(Eigen::VectorXf vec, float maxValue)
@@ -228,10 +306,35 @@ CompositeDiffIK::NullspaceJointTarget::NullspaceJointTarget(const VirtualRobot::
void CompositeDiffIK::NullspaceJointTarget::set(int index, float target, float weight)
{
this->target(index) = target;
- this->weight(weight) = weight;
+ 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 LocalException("RobotNodeSet has no joint ") << jointName;
+ }
+ set(index, target, weight);
}
-Eigen::VectorXf CompositeDiffIK::NullspaceJointTarget::getGradient()
+void CompositeDiffIK::NullspaceJointTarget::set(const VirtualRobot::RobotNodePtr& rn, float target, float weight)
+{
+ int index = rns->getRobotNodeIndex(rn);
+ if (index < 0)
+ {
+ throw LocalException("RobotNodeSet has no joint ") << rn->getName();
+ }
+ set(index, target, weight);
+}
+
+void CompositeDiffIK::NullspaceJointTarget::init(CompositeDiffIK::Parameters&)
+{
+
+}
+
+Eigen::VectorXf CompositeDiffIK::NullspaceJointTarget::getGradient(Parameters& params)
{
return (target - rns->getJointValuesEigen()).cwiseProduct(weight);
}
@@ -248,12 +351,45 @@ CompositeDiffIK::NullspaceJointLimitAvoidance::NullspaceJointLimitAvoidance(cons
}
-void CompositeDiffIK::NullspaceJointLimitAvoidance::setScale(int index, float weight)
+void CompositeDiffIK::NullspaceJointLimitAvoidance::setWeight(int index, float weight)
{
this->weight(index) = weight;
}
-Eigen::VectorXf CompositeDiffIK::NullspaceJointLimitAvoidance::getGradient()
+void CompositeDiffIK::NullspaceJointLimitAvoidance::setWeight(const std::string& jointName, float weight)
+{
+ int index = rns->getRobotNodeIndex(jointName);
+ if (index < 0)
+ {
+ throw LocalException("RobotNodeSet has no joint ") << jointName;
+ }
+ setWeight(index, weight);
+}
+
+void CompositeDiffIK::NullspaceJointLimitAvoidance::setWeight(const VirtualRobot::RobotNodePtr& rn, float weight)
+{
+ int index = rns->getRobotNodeIndex(rn);
+ if (index < 0)
+ {
+ throw LocalException("RobotNodeSet has no joint ") << rn->getName();
+ }
+ setWeight(index, weight);
+}
+
+void CompositeDiffIK::NullspaceJointLimitAvoidance::setWeights(const VirtualRobot::RobotNodeSetPtr& rns, float weight)
+{
+ for (const VirtualRobot::RobotNodePtr& rn : rns->getAllRobotNodes())
+ {
+ setWeight(rn, weight);
+ }
+}
+
+void CompositeDiffIK::NullspaceJointLimitAvoidance::init(CompositeDiffIK::Parameters&)
+{
+
+}
+
+Eigen::VectorXf CompositeDiffIK::NullspaceJointLimitAvoidance::getGradient(Parameters& params)
{
Eigen::VectorXf r(rns->getSize());
for (size_t i = 0; i < rns->getSize(); i++)
diff --git a/source/RobotAPI/libraries/diffik/CompositeDiffIK.h b/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
index 5bf5f383b440df5e719d7c93ca6031c415d3beed..9f657626983dc69d8959ed69f9beb3f077a7ce44 100644
--- a/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
@@ -39,14 +39,37 @@ namespace armarx
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;
+ float jointRegularizationRotation = 1;
+ };
+
class NullspaceGradient
{
public:
- virtual Eigen::VectorXf getGradient() = 0;
+ virtual void init(Parameters& params) = 0;
+ virtual Eigen::VectorXf getGradient(Parameters& params) = 0;
float kP = 1;
};
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:
@@ -58,7 +81,10 @@ namespace armarx
CartesianPositionController pCtrl;
CartesianVelocityController vCtrl;
- Eigen::VectorXf getGradient() override;
+ void init(Parameters&) override;
+ Eigen::VectorXf getGradient(Parameters& params) override;
+
+ std::vector<NullspaceTargetStep> ikSteps;
};
typedef std::shared_ptr<class NullspaceTarget> NullspaceTargetPtr;
@@ -68,12 +94,15 @@ namespace armarx
NullspaceJointTarget(const VirtualRobot::RobotNodeSetPtr& rns);
NullspaceJointTarget(const VirtualRobot::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 VirtualRobot::RobotNodePtr& rn, float target, float weight);
VirtualRobot::RobotNodeSetPtr rns;
Eigen::VectorXf target;
Eigen::VectorXf weight;
- Eigen::VectorXf getGradient() override;
+ void init(Parameters&) override;
+ Eigen::VectorXf getGradient(Parameters& params) override;
};
typedef std::shared_ptr<class NullspaceJointTarget> NullspaceJointTargetPtr;
@@ -82,15 +111,27 @@ namespace armarx
public:
NullspaceJointLimitAvoidance(const VirtualRobot::RobotNodeSetPtr& rns);
NullspaceJointLimitAvoidance(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& weight);
- void setScale(int index, float weight);
+ void setWeight(int index, float weight);
+ void setWeight(const std::string& jointName, float weight);
+ void setWeight(const VirtualRobot::RobotNodePtr& rn, float weight);
+ void setWeights(const VirtualRobot::RobotNodeSetPtr& rns, float weight);
+
VirtualRobot::RobotNodeSetPtr rns;
Eigen::VectorXf weight;
- Eigen::VectorXf getGradient() override;
+ void init(Parameters&) override;
+ Eigen::VectorXf getGradient(Parameters& params) override;
};
typedef std::shared_ptr<class NullspaceJointLimitAvoidance> NullspaceJointLimitAvoidancePtr;
+ struct TargetStep
+ {
+ Eigen::Vector3f posDiff;
+ Eigen::Vector3f oriDiff;
+ Eigen::Matrix4f tcpPose;
+ Eigen::VectorXf cartesianVel;
+ };
class Target
{
@@ -103,34 +144,13 @@ namespace armarx
CartesianPositionController pCtrl;
float maxPosError = 10.f;
float maxOriError = 0.05f;
+
+ std::vector<TargetStep> ikSteps;
};
typedef std::shared_ptr<Target> TargetPtr;
- struct Parameters
- {
- Parameters() {}
- // IK params
- size_t steps = 40;
- float maxJointAngleStep = 0.1f;
- bool resetRnsValues = true;
- bool returnIKSteps = false;
- };
- struct IKStep
- {
- Eigen::VectorXf jointValues;
- Eigen::Vector3f pdTcp;
- Eigen::Vector3f odTcp;
- Eigen::Vector3f pdElb;
- Eigen::Matrix4f tcpPose;
- Eigen::Matrix4f elbPose;
- Eigen::VectorXf cartesianVel;
- Eigen::VectorXf cartesianVelElb;
- Eigen::VectorXf jvElb;
- Eigen::VectorXf jvLA;
- Eigen::VectorXf jv;
- Eigen::VectorXf jvClamped;
- };
+
struct Result
{
@@ -145,7 +165,6 @@ namespace armarx
Eigen::VectorXf jointLimitMargins;
float minimumJointLimitMargin;
Eigen::Vector3f elbowPosDiff;
- std::vector<IKStep> ikSteps;
};
static Eigen::VectorXf LimitInfNormTo(Eigen::VectorXf vec, float maxValue);
@@ -153,7 +172,7 @@ namespace armarx
CompositeDiffIK(const VirtualRobot::RobotNodeSetPtr& rns);
void addTarget(const TargetPtr& target);
- void addTarget(const VirtualRobot::RobotNodePtr& tcp, const Eigen::Matrix4f& target, VirtualRobot::IKSolver::CartesianSelection mode);
+ TargetPtr addTarget(const VirtualRobot::RobotNodePtr& tcp, const Eigen::Matrix4f& target, VirtualRobot::IKSolver::CartesianSelection mode);
void addNullspaceGradient(const NullspaceGradientPtr& gradient);
Result solve(Parameters params);
diff --git a/source/RobotAPI/libraries/natik/NaturalIK.cpp b/source/RobotAPI/libraries/natik/NaturalIK.cpp
index f0ad1b7309e7f33c84764363ca5cd150a370b18b..76bd6e75e6301cb29a80b608d8fd6fb2b7303b46 100644
--- a/source/RobotAPI/libraries/natik/NaturalIK.cpp
+++ b/source/RobotAPI/libraries/natik/NaturalIK.cpp
@@ -178,8 +178,10 @@ NaturalIK::SoechtingForwardPositions NaturalIK::forwardKinematics(SoechtingAngle
//ARMARX_IMPORTANT << VAROUT(elb.transpose()) << VAROUT(wri.transpose());
NaturalIK::SoechtingForwardPositions res;
+ res.shoulder = shoulderPos;
res.elbow = shoulderPos + elb;
res.wrist = shoulderPos + elb + wri;
+ res.soechtingAngles = sa;
return res;
}
diff --git a/source/RobotAPI/libraries/natik/NaturalIK.h b/source/RobotAPI/libraries/natik/NaturalIK.h
index d6315096a58084776797e6ce5551b65651ff4026..7e1ecb940cc7adb84c0091e4e1f3427923559c74 100644
--- a/source/RobotAPI/libraries/natik/NaturalIK.h
+++ b/source/RobotAPI/libraries/natik/NaturalIK.h
@@ -64,11 +64,6 @@ namespace armarx
VirtualRobot::RobotNodePtr tcp;
};
- struct SoechtingForwardPositions
- {
- Eigen::Vector3f elbow;
- Eigen::Vector3f wrist;
- };
struct SoechtingAngles
{
@@ -91,6 +86,14 @@ namespace armarx
static constexpr float MMM_SHOULDER_POS = (MMM_LSC2LS + MMM_BT2LSC) * MMM_L;
};
+ struct SoechtingForwardPositions
+ {
+ Eigen::Vector3f shoulder;
+ Eigen::Vector3f elbow;
+ Eigen::Vector3f wrist;
+ SoechtingAngles soechtingAngles;
+ };
+
public:
NaturalIK(std::string side, Eigen::Vector3f shoulderPos = Eigen::Vector3f::Zero(), float scale = 1);