From c5c163472f80d205edb6cfc1c83399597adfad3c Mon Sep 17 00:00:00 2001
From: JeffGao <jianfenggaobit@gmail.com>
Date: Thu, 25 Jul 2019 18:59:32 +0200
Subject: [PATCH] adaptive wiping works, first draft
---
build/.gitkeep | 0
.../NJointAdaptiveWipingController.cpp | 39 +++++++++++++++++--
.../NJointAdaptiveWipingController.h | 1 +
3 files changed, 36 insertions(+), 4 deletions(-)
delete mode 100644 build/.gitkeep
diff --git a/build/.gitkeep b/build/.gitkeep
deleted file mode 100644
index e69de29bb..000000000
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp
index d19f4e585..a58e3ae3d 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp
@@ -267,6 +267,8 @@ namespace armarx
Eigen::VectorXf currentTwist = jacobi * qvel_filtered;
Eigen::VectorXf targetVel(6);
+ Eigen::Vector3f axis;
+ axis.setZero();
targetVel.setZero();
if (firstRun || !dmpRunning)
{
@@ -340,20 +342,40 @@ namespace armarx
if (filteredForceInRoot.norm() > fabs(cfg->minimumReactForce))
{
- Eigen::Vector3f desiredToolDir = filteredForceInRoot.normalized();// / filteredForceInRoot.norm();
+ Eigen::Vector3f toolYDir;
+ toolYDir << 0, 1.0, 0;
+ Eigen::Vector3f toolYDirInRoot = currentToolOri * toolYDir;
+ Eigen::Vector3f projectedFilteredForceInRoot = filteredForceInRoot - filteredForceInRoot.dot(toolYDirInRoot) * toolYDirInRoot;
+ Eigen::Vector3f desiredToolDir = projectedFilteredForceInRoot.normalized();// / projectedFilteredForceInRoot.norm();
currentToolDir.normalize();
- Eigen::Vector3f axis = currentToolDir.cross(desiredToolDir);
+ axis = currentToolDir.cross(desiredToolDir);
+ axis = axis.normalized();
float angle = acosf(currentToolDir.dot(desiredToolDir));
+
if (fabs(angle) < M_PI / 2)
{
- Eigen::AngleAxisf desiredToolRot(angle, axis);
+ // sigmoid function
+ float adaptedAngularKp = 1 / (1 + exp(0.2 * (angle - M_PI / 4)));
+ float angularKp = fmin(adaptedAngularKp, cfg->angularKp);
+
+ // test axis
+ Eigen::Vector3f fixedAxis;
+ if (axis(1) > 0)
+ {
+ fixedAxis << 0.0, 1.0, 0.0;
+ }
+ else
+ {
+ fixedAxis << 0.0, -1.0, 0.0;
+ }
+ Eigen::AngleAxisf desiredToolRot(angle, fixedAxis);
Eigen::Matrix3f desiredRotMat = desiredToolRot * Eigen::Matrix3f::Identity();
Eigen::Vector3f angularDiff = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
- targetVel.tail(3) = cfg->angularKp * angularDiff;
+ targetVel.tail(3) = angularKp * angularDiff;
Eigen::Vector3f desiredRPY = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
Eigen::Vector3f checkedToolDir = desiredRotMat * currentToolDir;
@@ -443,6 +465,7 @@ namespace armarx
debugRT.getWriteBuffer().globalPose = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystem(targetPose);
debugRT.getWriteBuffer().currentPose = currentPose;
debugRT.getWriteBuffer().filteredForceInRoot = filteredForceInRoot;
+ debugRT.getWriteBuffer().rotationAxis = axis;
debugRT.getWriteBuffer().filteredForce = filteredForce;
debugRT.getWriteBuffer().globalFilteredForce = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(filteredForceInRoot);
debugRT.getWriteBuffer().targetVel = targetVel;
@@ -623,6 +646,11 @@ namespace armarx
datafields["filteredForceInRoot_y"] = new Variant(filteredForceInRoot(1));
datafields["filteredForceInRoot_z"] = new Variant(filteredForceInRoot(2));
+ Eigen::Vector3f rotationAxis = debugRT.getUpToDateReadBuffer().rotationAxis;
+ datafields["rotationAxis_x"] = new Variant(rotationAxis(0));
+ datafields["rotationAxis_y"] = new Variant(rotationAxis(1));
+ datafields["rotationAxis_z"] = new Variant(rotationAxis(2));
+
Eigen::Vector3f reactForce = debugRT.getUpToDateReadBuffer().reactForce;
datafields["reactForce_x"] = new Variant(reactForce(0));
datafields["reactForce_y"] = new Variant(reactForce(1));
@@ -632,6 +660,9 @@ namespace armarx
datafields["targetVel_x"] = new Variant(targetVel(0));
datafields["targetVel_y"] = new Variant(targetVel(1));
datafields["targetVel_z"] = new Variant(targetVel(2));
+ datafields["targetVel_ro"] = new Variant(targetVel(3));
+ datafields["targetVel_pi"] = new Variant(targetVel(4));
+ datafields["targetVel_ya"] = new Variant(targetVel(5));
Eigen::Vector3f adaptK = debugRT.getUpToDateReadBuffer().adaptK;
datafields["adaptK_x"] = new Variant(adaptK(0));
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h
index 9bfd26e64..9c2b8d01d 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h
@@ -109,6 +109,7 @@ namespace armarx
Eigen::Matrix4f targetPose;
Eigen::Vector3f filteredForce;
Eigen::Vector3f filteredForceInRoot;
+ Eigen::Vector3f rotationAxis;
Eigen::Vector3f reactForce;
Eigen::Vector3f adaptK;
--
GitLab