Merge branch 'master' of gitlab.com:ArmarX/RobotAPI

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp

@@ -86,14 +86,13 @@ namespace armarx
 
     void NJointCartesianVelocityControllerWithRamp::rtPreActivateController()
     {
-        ARMARX_IMPORTANT << "rtPreActivateController start";
         Eigen::VectorXf currentJointVelocities(velocitySensors.size());
         for (size_t i = 0; i < velocitySensors.size(); i++)
         {
             currentJointVelocities(i) = *velocitySensors.at(i);
         }
         controller->setCurrentJointVelocity(currentJointVelocities);
-        ARMARX_IMPORTANT << "rtPreActivateController end";
+        ARMARX_IMPORTANT << "initial joint velocities: " << currentJointVelocities.transpose();
     }
 
     void NJointCartesianVelocityControllerWithRamp::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp

@@ -61,14 +61,14 @@ namespace armarx
         {
             TrajectoryController& contr = *rtGetControlStruct().trajectoryCtrl;
 
-            auto dimNames = contr.getTraj()->getDimensionNames();
+            const auto& dimNames = contr.getTraj()->getDimensionNames();
             for (size_t i = 0; i < dimNames.size(); i++)
             {
                 const auto& jointName = dimNames.at(i);
-                currentPos(i) = (sensors.count(jointName) == 1) ? sensors[jointName]->position : 0.0f;
+                currentPos(i) = (sensors.count(jointName) == 1) ? sensors.at(jointName)->position : 0.0f;
             }
 
-            auto newVelocities = contr.update(timeSinceLastIteration.toSecondsDouble() * direction, currentPos);
+            auto& newVelocities = contr.update(timeSinceLastIteration.toSecondsDouble() * direction, currentPos);
             currentTimestamp = contr.getCurrentTimestamp();
             finished = (currentTimestamp >= contr.getTraj()->rbegin()->getTimestamp() && direction == 1.0)
                        || (currentTimestamp <= contr.getTraj()->begin()->getTimestamp() && direction == -1.0);

source/RobotAPI/libraries/CMakeLists.txt

@@ -6,3 +6,5 @@ add_subdirectory(RobotAPIVariantWidget)
 add_subdirectory(RobotAPINJointControllers)
 
 add_subdirectory(DMPController)
+
+add_subdirectory(RobotStatechartHelpers)

source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt

+set(LIB_NAME       RobotStatechartHelpers)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+find_package(Eigen3 QUIET)
+find_package(Simox QUIET)
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+if (Eigen3_FOUND AND Simox_FOUND)
+    include_directories(
+        ${Eigen3_INCLUDE_DIR}
+        ${Simox_INCLUDE_DIRS}
+    )
+endif()
+
+set(COMPONENT_LIBS
+    ArmarXCoreInterfaces RobotAPIInterfaces RobotAPICore ${Simox_LIBRARIES}
+    ArmarXCore ArmarXCoreObservers
+)
+
+set(LIB_FILES
+#./RobotStatechartHelpers.cpp
+VelocityControllerHelper.cpp
+PositionControllerHelper.cpp
+ForceTorqueHelper.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(LIB_HEADERS
+#./RobotStatechartHelpers.h
+VelocityControllerHelper.h
+PositionControllerHelper.h
+ForceTorqueHelper.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${COMPONENT_LIBS}")

source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp

+/*
+ * This file is part of ArmarX.
+ * 
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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/>.
+ *
+ * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "ForceTorqueHelper.h"
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <ArmarXCore/observers/variant/DatafieldRef.h>
+
+using namespace armarx;
+
+ForceTorqueHelper::ForceTorqueHelper(const ForceTorqueUnitObserverInterfacePrx &forceTorqueObserver, const std::string &FTDatefieldName)
+{
+    forceDf = DatafieldRefPtr::dynamicCast(forceTorqueObserver->getForceDatafield(FTDatefieldName));
+    torqueDf = DatafieldRefPtr::dynamicCast(forceTorqueObserver->getTorqueDatafield(FTDatefieldName));
+    setZero();
+}
+
+Eigen::Vector3f ForceTorqueHelper::getForce()
+{
+    return forceDf->getDataField()->get<FramedDirection>()->toEigen() - initialForce;
+}
+
+Eigen::Vector3f ForceTorqueHelper::getTorque()
+{
+    return torqueDf->getDataField()->get<FramedDirection>()->toEigen() - initialTorque;
+}
+
+void ForceTorqueHelper::setZero()
+{
+    initialForce = forceDf->getDataField()->get<FramedDirection>()->toEigen();
+    initialTorque = torqueDf->getDataField()->get<FramedDirection>()->toEigen();
+}

source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h

+/*
+ * This file is part of ArmarX.
+ * 
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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/>.
+ *
+ * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+
+#include <RobotAPI/interface/units/ForceTorqueUnit.h>
+#include <Eigen/Dense>
+
+namespace armarx
+{
+    class DatafieldRef;
+    typedef IceInternal::Handle<DatafieldRef> DatafieldRefPtr;
+
+    class ForceTorqueHelper;
+    typedef boost::shared_ptr<ForceTorqueHelper> ForceTorqueHelperPtr;
+
+    class ForceTorqueHelper
+    {
+    public:
+        ForceTorqueHelper(const ForceTorqueUnitObserverInterfacePrx&  forceTorqueObserver, const std::string& FTDatefieldName);
+
+        Eigen::Vector3f getForce();
+        Eigen::Vector3f getTorque();
+        void setZero();
+
+        DatafieldRefPtr forceDf;
+        DatafieldRefPtr torqueDf;
+        Eigen::Vector3f initialForce;
+        Eigen::Vector3f initialTorque;
+    };
+}

source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp

+/*
+ * This file is part of ArmarX.
+ * 
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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/>.
+ *
+ * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "PositionControllerHelper.h"
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+using namespace armarx;
+
+PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr &tcp, const VelocityControllerHelperPtr &velocityControllerHelper, const Eigen::Matrix4f &target)
+    : posController(tcp), velocityControllerHelper(velocityControllerHelper)
+{
+    waypoints.push_back(target);
+    currentWaypointIndex = 0;
+}
+
+void PositionControllerHelper::update()
+{
+    if(!isLastWaypoint() && isTargetNear())
+    {
+        currentWaypointIndex++;
+    }
+    Eigen::VectorXf cv = posController.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All);
+    velocityControllerHelper->setTargetVelocity(cv);
+}
+
+void PositionControllerHelper::setNewWaypoints(const std::vector<Eigen::Matrix4f> &waypoints)
+{
+    this->waypoints = waypoints;
+    currentWaypointIndex = 0;
+}
+
+void PositionControllerHelper::setNewWaypoints(const std::vector<PosePtr> &waypoints)
+{
+    this->waypoints.clear();
+    for(const PosePtr& pose : waypoints)
+    {
+        this->waypoints.push_back(pose->toEigen());
+    }
+    currentWaypointIndex = 0;
+}
+
+void PositionControllerHelper::addWaypoint(const Eigen::Matrix4f &waypoint)
+{
+    this->waypoints.push_back(waypoint);
+}
+
+void PositionControllerHelper::setTarget(const Eigen::Matrix4f &target)
+{
+    waypoints.clear();
+    waypoints.push_back(target);
+    currentWaypointIndex = 0;
+}
+
+float PositionControllerHelper::getPositionError() const
+{
+    return posController.getPositionError(getCurrentTarget());
+}
+
+float PositionControllerHelper::getOrientationError() const
+{
+    return posController.getOrientationError(getCurrentTarget());
+}
+
+bool PositionControllerHelper::isTargetReached() const
+{
+    return getPositionError() < thresholdPositionReached && getOrientationError() < thresholdOrientationReached;
+}
+
+bool PositionControllerHelper::isTargetNear() const
+{
+    return getPositionError() < thresholdPositionNear && getOrientationError() < thresholdOrientationNear;
+}
+
+bool PositionControllerHelper::isFinalTargetReached() const
+{
+    return isLastWaypoint() && isTargetReached();
+}
+
+bool PositionControllerHelper::isFinalTargetNear() const
+{
+    return isLastWaypoint() && isTargetNear();
+}
+
+bool PositionControllerHelper::isLastWaypoint() const
+{
+    return currentWaypointIndex + 1 >= waypoints.size();
+}
+
+const Eigen::Matrix4f &PositionControllerHelper::getCurrentTarget() const
+{
+    return waypoints.at(currentWaypointIndex);
+}

source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h

+/*
+ * This file is part of ArmarX.
+ * 
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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/>.
+ *
+ * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+
+#include <Eigen/Dense>
+
+#include "VelocityControllerHelper.h"
+
+#include <VirtualRobot/Robot.h>
+
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
+namespace armarx
+{
+    class PositionControllerHelper;
+    typedef boost::shared_ptr<PositionControllerHelper> PositionControllerHelperPtr;
+
+    class PositionControllerHelper
+    {
+    public:
+        PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const Eigen::Matrix4f& target);
+
+        void update();
+
+        void setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints);
+        void setNewWaypoints(const std::vector<PosePtr>& waypoints);
+        void addWaypoint(const Eigen::Matrix4f& waypoint);
+        void setTarget(const Eigen::Matrix4f& target);
+
+        float getPositionError() const;
+
+        float getOrientationError() const;
+
+        bool isTargetReached() const;
+        bool isTargetNear() const;
+        bool isFinalTargetReached() const;
+        bool isFinalTargetNear() const;
+
+        bool isLastWaypoint() const;
+
+        const Eigen::Matrix4f& getCurrentTarget() const;
+
+        CartesianPositionController posController;
+        VelocityControllerHelperPtr velocityControllerHelper;
+
+        std::vector<Eigen::Matrix4f> waypoints;
+        size_t currentWaypointIndex = 0;
+
+        float thresholdPositionReached = 0;
+        float thresholdOrientationReached = 0;
+        float thresholdPositionNear = 0;
+        float thresholdOrientationNear = 0;
+    };
+}

source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.cpp

+/*
+ * 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::ArmarXObjects::RobotStatechartHelpers
+ * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "RobotStatechartHelpers.h"
+
+
+using namespace armarx;
+
+

source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.h

+/*
+ * 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::ArmarXObjects::RobotStatechartHelpers
+ * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+
+namespace armarx
+{
+    /**
+    * @defgroup Library-RobotStatechartHelpers RobotStatechartHelpers
+    * @ingroup RobotAPI
+    * A description of the library RobotStatechartHelpers.
+    *
+    * @class RobotStatechartHelpers
+    * @ingroup Library-RobotStatechartHelpers
+    * @brief Brief description of class RobotStatechartHelpers.
+    *
+    * Detailed description of class RobotStatechartHelpers.
+    */
+    class RobotStatechartHelpers
+    {
+    public:
+
+    };
+
+}

source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp

+/*
+ * This file is part of ArmarX.
+ * 
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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/>.
+ *
+ * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "VelocityControllerHelper.h"
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+using namespace armarx;
+
+
+VelocityControllerHelper::VelocityControllerHelper(const RobotUnitInterfacePrx &robotUnit, const std::string &nodeSetName, const std::string &controllerName)
+    : robotUnit(robotUnit), controllerName(controllerName)
+{
+    config = new NJointCartesianVelocityControllerWithRampConfig(nodeSetName, "", CartesianSelectionMode::eAll, 500, 1, 2, 1, 2);
+    init();
+}
+
+VelocityControllerHelper::VelocityControllerHelper(const RobotUnitInterfacePrx &robotUnit, NJointCartesianVelocityControllerWithRampConfigPtr config, const std::string &controllerName)
+    : robotUnit(robotUnit), controllerName(controllerName)
+{
+    this->config = config;
+    init();
+}
+
+void VelocityControllerHelper::init()
+{
+    NJointControllerInterfacePrx ctrl = robotUnit->getNJointController(controllerName);
+    if (ctrl)
+    {
+        controllerCreated = false;
+    }
+    else
+    {
+        ctrl = robotUnit->createNJointController("NJointCartesianVelocityControllerWithRamp", controllerName, config);
+        controllerCreated = true;
+    }
+    controller = NJointCartesianVelocityControllerWithRampInterfacePrx::checkedCast(ctrl);
+    controller->activateController();
+}
+
+void VelocityControllerHelper::setTargetVelocity(const Eigen::VectorXf &cv)
+{
+    controller->setTargetVelocity(cv(0), cv(1), cv(2), cv(3), cv(4), cv(5));
+}
+
+void VelocityControllerHelper::cleanup()
+{
+    controller->deactivateController();
+    if (controllerCreated)
+    {
+        while (controller->isControllerActive())
+        {
+            TimeUtil::SleepMS(1);
+        }
+        controller->deleteController();
+    }
+}

source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h

+/*
+ * This file is part of ArmarX.
+ * 
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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/>.
+ *
+ * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+
+#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+#include <Eigen/Dense>
+
+namespace armarx
+{
+    class VelocityControllerHelper;
+    typedef boost::shared_ptr<VelocityControllerHelper> VelocityControllerHelperPtr;
+
+    class VelocityControllerHelper
+    {
+    public:
+        VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, const std::string& nodeSetName, const std::string& controllerName);
+        VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, NJointCartesianVelocityControllerWithRampConfigPtr config, const std::string& controllerName);
+
+        void init();
+
+        void setTargetVelocity(const Eigen::VectorXf& cv);
+
+        void cleanup();
+
+        NJointCartesianVelocityControllerWithRampConfigPtr config;
+        NJointCartesianVelocityControllerWithRampInterfacePrx controller;
+        RobotUnitInterfacePrx robotUnit;
+        std::string controllerName;
+        bool controllerCreated = false;
+    };
+}

source/RobotAPI/libraries/core/CMakeLists.txt

@@ -51,6 +51,7 @@ set(LIB_FILES
 set(LIB_HEADERS
     EigenStl.h
     PIDController.h
+    MultiDimPIDController.h
     Trajectory.h
     TrajectoryController.h
     VectorHelpers.h

source/RobotAPI/libraries/core/CartesianPositionController.cpp

@@ -32,7 +32,7 @@ CartesianPositionController::CartesianPositionController(const VirtualRobot::Rob
 {
 }
 
-Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode)
+Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const
 {
     int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
     int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
@@ -68,14 +68,14 @@ Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& ta
     return cartesianVel;
 }
 
-float CartesianPositionController::getPositionError(const Eigen::Matrix4f& targetPose)
+float CartesianPositionController::getPositionError(const Eigen::Matrix4f& targetPose) const
 {
     Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
     Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
     return errPos.norm();
 }
 
-float CartesianPositionController::getOrientationError(const Eigen::Matrix4f& targetPose)
+float CartesianPositionController::getOrientationError(const Eigen::Matrix4f& targetPose) const
 {
     Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
     Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
@@ -84,14 +84,14 @@ float CartesianPositionController::getOrientationError(const Eigen::Matrix4f& ta
     return aa.angle();
 }
 
-Eigen::Vector3f CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose)
+Eigen::Vector3f CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose) const
 {
     Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
     return targetPos - tcp->getPositionInRootFrame();
 
 }
 
-Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Matrix4f& targetPose)
+Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Matrix4f& targetPose) const
 {
     Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
     Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);

source/RobotAPI/libraries/core/CartesianPositionController.h

@@ -40,12 +40,12 @@ namespace armarx
     public:
         CartesianPositionController(const VirtualRobot::RobotNodePtr& tcp);
 
-        Eigen::VectorXf calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode);
+        Eigen::VectorXf calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const;
 
-        float getPositionError(const Eigen::Matrix4f& targetPose);
-        float getOrientationError(const Eigen::Matrix4f& targetPose);
-        Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose);
-        Eigen::Vector3f getOrientationDiff(const Eigen::Matrix4f& targetPose);
+        float getPositionError(const Eigen::Matrix4f& targetPose) const;
+        float getOrientationError(const Eigen::Matrix4f& targetPose) const;
+        Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose) const;
+        Eigen::Vector3f getOrientationDiff(const Eigen::Matrix4f& targetPose) const;
 
         //CartesianVelocityController velocityController;
         float KpPos = 1;

source/RobotAPI/libraries/core/CartesianVelocityController.cpp

@@ -57,19 +57,28 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
     //ARMARX_IMPORTANT << jacobi;
     inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
 
-    Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
+    //    ARMARX_IMPORTANT << "nullspaceVel " << nullspaceVel.transpose();
+    //    Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
     //ARMARX_IMPORTANT << "The rank of the jacobi is " << lu_decomp.rank();
     //ARMARX_IMPORTANT << "Null space: " << lu_decomp.kernel();
-    Eigen::MatrixXf nullspace = lu_decomp.kernel();
-    Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
-    for (int i = 0; i < nullspace.cols(); i++)
-    {
-        nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
-    }
+    //    Eigen::MatrixXf nullspace = lu_decomp.kernel();
+
+
+    Eigen::MatrixXf nullspace = Eigen::MatrixXf::Identity(jacobi.cols(), jacobi.cols()) - inv * jacobi;
+
+    //    Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
+    //    for (int i = 0; i < nullspace.cols(); i++)
+    //    {
+    //        nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
+    //    }
+
+    Eigen::VectorXf nsv = nullspace * nullspaceVel;
 
     //nsv /= kernel.cols();
 
+
     Eigen::VectorXf jointVel = inv * cartesianVel;
+
     jointVel += nsv;
 
     return jointVel;
@@ -98,7 +107,9 @@ Eigen::VectorXf CartesianVelocityController::calculateNullspaceVelocity(const Ei
 {
     Eigen::VectorXf regularization = calculateRegularization(mode);
     Eigen::VectorXf vel = cartesianVel * regularization;
+
     return KpScale * vel.norm() * calculateJointLimitAvoidance();
+
 }
 
 void CartesianVelocityController::setCartesianRegularization(float cartesianMMRegularization, float cartesianRadianRegularization)

source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp

@@ -23,6 +23,8 @@
 
 #include "CartesianVelocityControllerWithRamp.h"
 
+#include <ArmarXCore/core/logging/Logging.h>
+
 using namespace armarx;
 
 CartesianVelocityControllerWithRamp::CartesianVelocityControllerWithRamp(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode,
@@ -36,17 +38,29 @@ CartesianVelocityControllerWithRamp::CartesianVelocityControllerWithRamp(const V
 void CartesianVelocityControllerWithRamp::setCurrentJointVelocity(const Eigen::VectorXf& currentJointVelocity)
 {
     Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller.tcp, mode);
+    Eigen::MatrixXf inv = controller.ik->computePseudoInverseJacobianMatrix(jacobi, controller.ik->getJacobiRegularization(mode));
+
 
     Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
     Eigen::MatrixXf nullspace = lu_decomp.kernel();
     Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
-    for (int i = 0; i < nullspace.cols(); i++)
-    {
-        nsv += nullspace.col(i) * nullspace.col(i).dot(currentJointVelocity) / nullspace.col(i).squaredNorm();
-    }
+
+
+    //        int r = lu_decomp.rank();
+
+    //        Eigen::MatrixXf nullspace = Eigen::MatrixXf::Zero(r, V.cols());
+    //        Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
+
+
+    //    for (int i = 0; i < nullspace.cols(); i++)
+    //    {
+    //        nsv += nullspace.col(i) * nullspace.col(i).dot(currentJointVelocity) / nullspace.col(i).squaredNorm();
+    //    }
 
     cartesianVelocityRamp.setState(jacobi * currentJointVelocity, mode);
-    nullSpaceVelocityRamp.setState(nsv);
+    //    nullSpaceVelocityRamp.setState(nsv);
+    nullSpaceVelocityRamp.setState(currentJointVelocity - inv * jacobi * currentJointVelocity);
+    //    ARMARX_IMPORTANT << "nsv " << currentJointVelocity - inv* jacobi* currentJointVelocity;
 }
 
 Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt)

source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp

@@ -23,6 +23,8 @@
 
 #include "CartesianVelocityRamp.h"
 
+#include <ArmarXCore/core/logging/Logging.h>
+
 using namespace armarx;
 
 CartesianVelocityRamp::CartesianVelocityRamp()
@@ -60,6 +62,7 @@ Eigen::VectorXf CartesianVelocityRamp::update(const Eigen::VectorXf& target, flo
     }
 
     state += delta / factor;
+    //    ARMARX_IMPORTANT << "CartRamp state " << state;
     return state;
 }
 

source/RobotAPI/libraries/core/JointVelocityRamp.cpp

@@ -22,6 +22,7 @@
  */
 
 #include "JointVelocityRamp.h"
+#include <ArmarXCore/core/logging/Logging.h>
 
 using namespace armarx;
 
@@ -36,13 +37,18 @@ void JointVelocityRamp::setState(const Eigen::VectorXf& state)
 
 Eigen::VectorXf JointVelocityRamp::update(const Eigen::VectorXf& target, float dt)
 {
+    if (dt <= 0)
+    {
+        return state;
+    }
 
     Eigen::VectorXf delta = target - state;
 
-    float factor = delta.norm() / maxAcceleration;
+    float factor = delta.norm() / maxAcceleration / dt;
     factor = std::max(factor, 1.f);
 
-    state += delta / factor * dt;
+    state += delta / factor;
+    //    ARMARX_IMPORTANT << "JointRamp state " << state;
     return state;
 
 }

source/RobotAPI/libraries/core/MultiDimPIDController.h

+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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    ArmarX
+ * @author     Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+#pragma once
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <Eigen/Core>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
+
+namespace armarx
+{
+    template <int dimensions = Eigen::Dynamic>
+    class MultiDimPIDControllerTemplate :
+        public Logging
+    {
+    public:
+        typedef Eigen::Matrix<float, dimensions, 1> PIDVectorX;
+
+        MultiDimPIDControllerTemplate(float Kp,
+                                      float Ki,
+                                      float Kd,
+                                      double maxControlValue = std::numeric_limits<double>::max(),
+                                      double maxDerivation = std::numeric_limits<double>::max(),
+                                      bool threadSafe = true,
+                                      std::vector<bool> limitless = {}) :
+            Kp(Kp),
+            Ki(Ki),
+            Kd(Kd),
+            integral(0),
+            derivative(0),
+            previousError(0),
+            maxControlValue(maxControlValue),
+            maxDerivation(maxDerivation),
+            threadSafe(threadSafe),
+            limitless(limitless)
+        {
+            reset();
+        }
+
+        void preallocate(size_t size)
+        {
+            stackAllocations.zeroVec = PIDVectorX::Zero(size);
+            stackAllocations.errorVec = stackAllocations.zeroVec;
+            stackAllocations.direction = stackAllocations.zeroVec;
+            stackAllocations.oldControlValue = stackAllocations.zeroVec;
+        }
+
+        ~MultiDimPIDControllerTemplate() {}
+        void update(const double deltaSec, const PIDVectorX& measuredValue, const PIDVectorX& targetValue)
+        {
+            ScopedRecursiveLockPtr lock = getLock();
+            processValue = measuredValue;
+            target = targetValue;
+
+            stackAllocations.errorVec = target - processValue;
+
+            if (limitless.size() != 0)
+            {
+                ARMARX_CHECK_EQUAL(limitless.size(), (size_t)stackAllocations.errorVec.rows());
+                for (size_t i = 0; i < limitless.size(); i++)
+                {
+                    if (limitless.at(i))
+                    {
+                        stackAllocations.errorVec(i) = math::MathUtils::angleModPI(stackAllocations.errorVec(i));
+                    }
+                }
+            }
+
+
+            double error = stackAllocations.errorVec.norm();
+
+            //double dt = (now - lastUpdateTime).toSecondsDouble();
+            //    ARMARX_INFO << deactivateSpam() << VAROUT(dt);
+            if (!firstRun)
+            {
+                integral += error * deltaSec;
+
+                if (deltaSec > 0.0)
+                {
+                    derivative = (error - previousError) / deltaSec;
+                }
+            }
+
+            firstRun = false;
+            stackAllocations.direction = targetValue; // copy size
+
+            if (error > 0)
+            {
+                stackAllocations.direction = stackAllocations.errorVec.normalized();
+            }
+            else
+            {
+                stackAllocations.direction.setZero();
+            }
+
+            if (controlValue.rows() > 0)
+            {
+                stackAllocations.oldControlValue = controlValue;
+            }
+            else
+            {
+                stackAllocations.oldControlValue = stackAllocations.zeroVec;
+            }
+            controlValue = stackAllocations.direction * (Kp * error + Ki * integral + Kd * derivative);
+
+            if (deltaSec > 0.0)
+            {
+                PIDVectorX accVec = (controlValue - stackAllocations.oldControlValue) / deltaSec;
+                float maxNewJointAcc = accVec.maxCoeff();
+                float minNewJointAcc = accVec.minCoeff();
+                maxNewJointAcc = std::max<float>(fabs(minNewJointAcc), fabs(maxNewJointAcc));
+                if (maxNewJointAcc > maxDerivation)
+                {
+                    auto newValue = stackAllocations.oldControlValue + accVec * maxDerivation / maxNewJointAcc * deltaSec;
+                    ARMARX_DEBUG << deactivateSpam(0.5) << VAROUT(maxDerivation) << VAROUT(maxNewJointAcc) << VAROUT(controlValue)  << VAROUT(stackAllocations.oldControlValue) << VAROUT(newValue);
+                    controlValue = newValue;
+                }
+            }
+
+
+            float max = controlValue.maxCoeff();
+            float min = controlValue.minCoeff();
+            max = std::max<float>(fabs(min), fabs(max));
+
+
+
+            if (max > maxControlValue)
+            {
+                auto newValue = controlValue  * maxControlValue / max;
+                ARMARX_DEBUG << deactivateSpam(0.5) << " Control value to big: " << controlValue << " max value: " << maxControlValue << " new value: " << newValue;
+                controlValue = newValue;
+            }
+            ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) <<  " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
+
+            previousError = error;
+            lastUpdateTime += IceUtil::Time::seconds(deltaSec);
+
+        }
+        void update(const PIDVectorX& measuredValue, const PIDVectorX& targetValue)
+        {
+            ScopedRecursiveLockPtr lock = getLock();
+            IceUtil::Time now = TimeUtil::GetTime();
+
+            if (firstRun)
+            {
+                lastUpdateTime = TimeUtil::GetTime();
+            }
+
+            double dt = (now - lastUpdateTime).toSecondsDouble();
+            update(dt, measuredValue, targetValue);
+            lastUpdateTime = now;
+        }
+        const PIDVectorX&
+        getControlValue() const
+        {
+            return controlValue;
+        }
+        void setMaxControlValue(double value)
+        {
+            ScopedRecursiveLockPtr lock = getLock();
+            maxControlValue = value;
+        }
+
+        void reset()
+        {
+            ScopedRecursiveLockPtr lock = getLock();
+            firstRun = true;
+            previousError = 0;
+            integral = 0;
+            lastUpdateTime = TimeUtil::GetTime();
+            //    controlValue.setZero();
+            //    processValue.setZero();
+            //    target.setZero();
+
+
+        }
+        //    protected:
+        float Kp, Ki, Kd;
+        double integral;
+        double derivative;
+        double previousError;
+        PIDVectorX processValue;
+        PIDVectorX target;
+        IceUtil::Time lastUpdateTime;
+        PIDVectorX controlValue;
+        double maxControlValue;
+        double maxDerivation;
+        bool firstRun;
+        mutable  RecursiveMutex mutex;
+        bool threadSafe = true;
+        std::vector<bool> limitless;
+    private:
+
+        struct StackAllocationHelper
+        {
+            PIDVectorX errorVec;
+            PIDVectorX direction;
+            PIDVectorX oldControlValue;
+            PIDVectorX zeroVec;
+        } stackAllocations;
+
+        ScopedRecursiveLockPtr getLock() const
+        {
+            if (threadSafe)
+            {
+                return ScopedRecursiveLockPtr(new ScopedRecursiveLock(mutex));
+            }
+            else
+            {
+                return ScopedRecursiveLockPtr();
+            }
+        }
+    };
+    typedef MultiDimPIDControllerTemplate<> MultiDimPIDController;
+    typedef boost::shared_ptr<MultiDimPIDControllerTemplate<>> MultiDimPIDControllerPtr;
+}