diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 978fd19bcd408f00974daaa0060fc2b3ff8853d4..32308c8a90c3ed3bb0a5f85e6de9df50c2354586 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -52,6 +52,9 @@ set(SLICE_FILES
units/RobotUnit/NJointCartesianTorqueController.ice
units/RobotUnit/NJointCartesianActiveImpedanceController.ice
units/RobotUnit/NJointCartesianVelocityControllerWithRamp.ice
+
+ units/RobotUnit/NJointGripperController.ice
+
units/RobotUnit/RobotUnitInterface.ice
units/RobotUnit/NJointJointSpaceDMPController.ice
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointGripperController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointGripperController.ice
new file mode 100644
index 0000000000000000000000000000000000000000..1b79c59ba77f1987ad3c9fc2e912132431741f25
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointGripperController.ice
@@ -0,0 +1,39 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::NJointControllerInterface
+ * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
+#include <RobotAPI/interface/core/Trajectory.ice>
+
+module armarx
+{
+
+ class NJointGripperZeroTorqueControllerConfig extends NJointControllerConfig
+ {
+ string nodeSetName;
+ };
+
+ interface NJointGripperZeroTorqueControllerInterface extends NJointControllerInterface
+ {
+ };
+};
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index d3d62e88d8e4bb28a2b3da115ed570abaa308c71..e4af46f9e7fb9115bf9389e76c2bb877d4f61f0e 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -65,13 +65,16 @@ module armarx
double maxLinearVel;
double maxAngularVel;
-
+ double maxJointVelocity;
string debugName;
float Kp_LinearVel;
float Kd_LinearVel;
float Kp_AngularVel;
float Kd_AngularVel;
+
+ float pos_filter;
+ float vel_filter;
};
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
index de73c55fc27451cb15d1ecec4d80d06f4a9c23d1..7cc4f276201013cbcdc6296863ae2e1800b8979f 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
@@ -103,7 +103,7 @@ namespace armarx
if (d.getSiblingControlActorIndex() >= 0)
{
auto& d2 = actorData.at(d.getSiblingControlActorIndex());
- ARMARX_IMPORTANT << "decoupling factor: " << d.parallelGripperDecouplingFactor;
+ // ARMARX_IMPORTANT << "decoupling factor: " << d.parallelGripperDecouplingFactor;
d.adjustedRelativePosition = d.relativePosition.value +
d2->relativePosition.value * d.parallelGripperDecouplingFactor;
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
index 74ef54267526a33c4082dc468ffe885985253d94..c2da980f612073bbf9db3f885b26edca57b16367 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
@@ -27,9 +27,11 @@ set(LIBS RobotAPIInterfaces ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreE
)
set(LIB_FILES
+ GripperController/NJointGripperZeroTorqueController.cpp
)
set(LIB_HEADERS
+ GripperController/NJointGripperZeroTorqueController.h
)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index b291966f1f168da4b59f32611f81af9dcd5d8934..a8fc9987885cae9cd0d5c2a89a61eb7a25889794 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -19,21 +19,14 @@ namespace armarx
const SensorValueBase* sv = useSensorValue(jointName);
targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
- const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
+ // const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
+ // const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
+
const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
- if (!torqueSensor)
- {
- ARMARX_WARNING << "No Torque sensor available for " << jointName;
- }
- if (!gravityTorqueSensor)
- {
- ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
- }
- torqueSensors.push_back(torqueSensor);
- gravityTorqueSensors.push_back(gravityTorqueSensor);
+ // torqueSensors.push_back(torqueSensor);
+ // gravityTorqueSensors.push_back(gravityTorqueSensor);
velocitySensors.push_back(velocitySensor);
positionSensors.push_back(positionSensor);
};
@@ -99,6 +92,13 @@ namespace armarx
DpF = cfg->Kd_LinearVel;
DoF = cfg->Kd_AngularVel;
+ filtered_qvel.setZero(targets.size());
+ vel_filter_factor = cfg->vel_filter;
+
+ filtered_position.setZero();
+ pos_filter_factor = cfg->pos_filter;
+
+ firstRun = true;
}
std::string NJointTSDMPController::getClassName(const Ice::Current&) const
@@ -118,15 +118,19 @@ namespace armarx
taskSpaceDMPController->flow(deltaT, currentPose, currentTwist);
+
if (taskSpaceDMPController->canVal < 1e-8)
{
finished = true;
}
targetVels = taskSpaceDMPController->getTargetVelocity();
-
-
targetPose = taskSpaceDMPController->getTargetPoseMat();
std::vector<double> targetState = taskSpaceDMPController->getTargetPose();
+
+ // ARMARX_IMPORTANT << "CanVal: " << taskSpaceDMPController->canVal;
+ // ARMARX_IMPORTANT << "targetVels: " << targetVels;
+ // ARMARX_IMPORTANT << "targetPose: " << targetPose;
+
debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
@@ -172,6 +176,8 @@ namespace armarx
void NJointTSDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
+
+
double deltaT = timeSinceLastIteration.toSecondsDouble();
Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
@@ -183,7 +189,8 @@ namespace armarx
qvel(i) = velocitySensors[i]->velocity;
}
- Eigen::VectorXf tcptwist = jacobi * qvel;
+ filtered_qvel = (1 - vel_filter_factor) * filtered_qvel + vel_filter_factor * qvel;
+ Eigen::VectorXf tcptwist = jacobi * filtered_qvel;
controllerSensorData.getWriteBuffer().currentPose = currentPose;
controllerSensorData.getWriteBuffer().currentTwist = tcptwist;
@@ -201,14 +208,23 @@ namespace armarx
Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
+ if (firstRun)
+ {
+ filtered_position = currentPose.block<3, 1>(0, 3);
+ firstRun = false;
+ }
+ else
+ {
+ filtered_position = (1 - pos_filter_factor) * filtered_position + pos_filter_factor * currentPose.block<3, 1>(0, 3);
+ }
Eigen::VectorXf rtTargetVel;
rtTargetVel.resize(6);
- rtTargetVel.block<3, 1>(0, 0) = KpF * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) + DpF * (targetVel.block<3, 1>(0, 0) - tcptwist.block<3, 1>(0, 0));
+ rtTargetVel.block<3, 1>(0, 0) = KpF * (targetPose.block<3, 1>(0, 3) - filtered_position) + DpF * (targetVel.block<3, 1>(0, 0) - tcptwist.block<3, 1>(0, 0));
rtTargetVel.block<3, 1>(3, 0) = KoF * errorRPY + DoF * (targetVel.block<3, 1>(3, 0) - tcptwist.block<3, 1>(3, 0));
// rtTargetVel = targetVel;
+
float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
if (normLinearVelocity > cfg->maxLinearVel)
{
@@ -267,16 +283,16 @@ namespace armarx
for (size_t i = 0; i < tcpController->rns->getSize(); i++)
{
jnv(i) += rtGetControlStruct().nullspaceJointVelocities.at(i);
- if (torqueSensors.at(i) && gravityTorqueSensors.at(i) && rtGetControlStruct().torqueKp.at(i) != 0)
- {
- torquePIDs.at(i).Kp = rtGetControlStruct().torqueKp.at(i);
- torquePIDs.at(i).Kd = rtGetControlStruct().torqueKd.at(i);
- jnv(i) += torquePIDs.at(i).update(deltaT, torqueSensors.at(i)->torque - gravityTorqueSensors.at(i)->gravityTorque);
- }
- else
- {
- torquePIDs.at(i).lastError = 0;
- }
+ // if (torqueSensors.at(i) && gravityTorqueSensors.at(i) && rtGetControlStruct().torqueKp.at(i) != 0)
+ // {
+ // torquePIDs.at(i).Kp = rtGetControlStruct().torqueKp.at(i);
+ // torquePIDs.at(i).Kd = rtGetControlStruct().torqueKd.at(i);
+ // jnv(i) += torquePIDs.at(i).update(deltaT, torqueSensors.at(i)->torque - gravityTorqueSensors.at(i)->gravityTorque);
+ // }
+ // else
+ // {
+ // torquePIDs.at(i).lastError = 0;
+ // }
}
Eigen::VectorXf jointTargetVelocities = tcpController->calculate(x, jnv, mode);
@@ -285,10 +301,11 @@ namespace armarx
for (size_t i = 0; i < targets.size(); ++i)
{
targets.at(i)->velocity = jointTargetVelocities(i);
- if (!targets.at(i)->isValid())
+
+ if (!targets.at(i)->isValid() || fabs(targets.at(i)->velocity) > cfg->maxJointVelocity)
{
ARMARX_IMPORTANT << deactivateSpam(1, std::to_string(i))
- << "Torque controller target is invalid - setting to zero! set value: " << targets.at(i)->velocity;
+ << "Velocity controller target is invalid - setting to zero! set value: " << targets.at(i)->velocity;
targets.at(i)->velocity = 0.0f;
}
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index 68a4f41d1bb1c5bbbb34c28d803ba85f26afd1cc..91fd0b4b6f48df45846e6ca120a86ba207771223 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -163,6 +163,12 @@ namespace armarx
std::string debugName;
+ Eigen::VectorXf filtered_qvel;
+ Eigen::Vector3f filtered_position;
+ float vel_filter_factor;
+ float pos_filter_factor;
+ bool firstRun;
+
float KpF;
float DpF;
float KoF;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/GripperController/NJointGripperZeroTorqueController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/GripperController/NJointGripperZeroTorqueController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4173f0942a1e188e148d18c6ead9b0a0ab32b4f7
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/GripperController/NJointGripperZeroTorqueController.cpp
@@ -0,0 +1,57 @@
+#include "NJointGripperZeroTorqueController.h"
+#include <VirtualRobot/RobotNodeSet.h>
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointGripperZeroTorqueController> registrationControllerNJointGripperZeroTorqueController("NJointGripperZeroTorqueController");
+
+ NJointGripperZeroTorqueController::NJointGripperZeroTorqueController(const RobotUnitPtr&, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ useSynchronizedRtRobot();
+ NJointGripperZeroTorqueControllerConfigPtr cfg = NJointGripperZeroTorqueControllerConfigPtr::dynamicCast(config);
+ VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
+
+ for (size_t i = 0; i < rns->getSize(); ++i)
+ {
+ std::string jointName = rns->getNode(i)->getName();
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::ZeroTorque1DoF);
+ targets.push_back(ct->asA<ControlTarget1DoFActuatorZeroTorque>());
+ };
+ }
+
+ std::string NJointGripperZeroTorqueController::getClassName(const Ice::Current&) const
+ {
+ return "NJointGripperZeroTorqueController";
+ }
+
+ void NJointGripperZeroTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ targets.at(i)->torque = 0;
+ }
+ }
+
+ WidgetDescription::WidgetPtr NJointGripperZeroTorqueController::GenerateConfigDescription(const VirtualRobot::RobotPtr & robot, const std::map<std::string, ConstControlDevicePtr> & controlDevices, const std::map<std::string, ConstSensorDevicePtr> &)
+ {
+ using namespace armarx::WidgetDescription;
+ HBoxLayoutPtr layout = new HBoxLayout;
+
+ LabelPtr label = new Label;
+ label->text = "nodeset name";
+ layout->children.emplace_back(label);
+
+ StringComboBoxPtr box = new StringComboBox;
+ box->defaultIndex = 0;
+ box->options = robot->getRobotNodeSetNames();
+ box->name = "nodeSetName";
+ layout->children.emplace_back(box);
+
+ return layout;
+ }
+
+ NJointGripperZeroTorqueControllerConfigPtr NJointGripperZeroTorqueController::GenerateConfigFromVariants(const StringVariantBaseMap &values)
+ {
+ return new NJointGripperZeroTorqueControllerConfig(values.at("nodeSetName")->getString());
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/GripperController/NJointGripperZeroTorqueController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/GripperController/NJointGripperZeroTorqueController.h
new file mode 100644
index 0000000000000000000000000000000000000000..f5c6accfc21d19e4bd3854e4c600bbb843d7e5f4
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/GripperController/NJointGripperZeroTorqueController.h
@@ -0,0 +1,53 @@
+
+#pragma once
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointGripperController.h>
+
+
+namespace armarx
+{
+
+
+ TYPEDEF_PTRS_HANDLE(NJointGripperZeroTorqueController);
+ TYPEDEF_PTRS_HANDLE(NJointGripperZeroTorqueControllerControlData);
+
+
+ class NJointGripperZeroTorqueControllerControlData
+ {
+ };
+
+ /**
+ * @brief The NJointGripperZeroTorqueController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
+ class NJointGripperZeroTorqueController :
+ public NJointControllerWithTripleBuffer<NJointGripperZeroTorqueControllerControlData>,
+ public NJointGripperZeroTorqueControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointGripperZeroTorqueControllerConfigPtr;
+ NJointGripperZeroTorqueController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current&) const override;
+
+ // NJointController interface
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ static WidgetDescription::WidgetPtr GenerateConfigDescription(
+ const VirtualRobot::RobotPtr&,
+ const std::map<std::string, ConstControlDevicePtr>&,
+ const std::map<std::string, ConstSensorDevicePtr>&);
+
+ static NJointGripperZeroTorqueControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
+
+ private:
+ std::vector<ControlTarget1DoFActuatorZeroTorque*> targets;
+ };
+
+} // namespace armarx
+