diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp index 7b5add1afe52bee590a546fee4ceb52f77e6ba51..b23a1466b7b30517f1f5ded46465cd497bb4ca13 100644 --- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp +++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp @@ -300,14 +300,14 @@ namespace armarx filteredForceInRoot = forceFrameInRoot.block<3, 3>(0, 0) * filteredForce; float targetForce = user2rtData.getUpToDateReadBuffer().targetForce; - Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0); - Eigen::Matrix3f currentToolOri = toolTransform * currentHandOri; + // Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0); + // Eigen::Matrix3f currentToolOri = toolTransform * currentHandOri; float desiredZVel = kpf * (targetForce - filteredForceInRoot(2)); targetVel(2) -= desiredZVel; // targetVel.block<3, 1>(0, 0) = currentToolOri * targetVel.block<3, 1>(0, 0); - Eigen::Vector3f currentToolDir = currentToolOri * oriToolDir; + // Eigen::Vector3f currentToolDir = currentToolOri * oriToolDir; for (int i = 3; i < 6; ++i) {