realized imagine gripper teachin

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

source/RobotAPI/interface/units/RobotUnit/NJointGripperController.ice

+/*
+ * 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
+    {
+    };
+};

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;
     };
 
 

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;
             }

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
 )
 
 

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;
             }
         }

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;

source/RobotAPI/libraries/RobotAPINJointControllers/GripperController/NJointGripperZeroTorqueController.cpp

+#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());
+    }
+}

source/RobotAPI/libraries/RobotAPINJointControllers/GripperController/NJointGripperZeroTorqueController.h

+
+#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
+