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

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

@@ -138,7 +138,6 @@ namespace armarx
 
     void NJointCartesianVelocityControllerWithRamp::setTargetVelocity(float vx, float vy, float vz, float vrx, float vry, float vrz, const Ice::Current&)
     {
-        //Template --> anpassen
         LockGuardType guard {controlDataMutex};
         getWriterControlStruct().xVel = vx;
         getWriterControlStruct().yVel = vy;
@@ -159,6 +158,19 @@ namespace armarx
         this->KpJointLimitAvoidance = KpJointLimitAvoidance;
     }
 
+    void NJointCartesianVelocityControllerWithRamp::immediateHardStop(const Ice::Current&)
+    {
+        LockGuardType guard {controlDataMutex};
+        controller->setCurrentJointVelocity(Eigen::VectorXf::Zero(velocitySensors.size()));
+        getWriterControlStruct().xVel = 0;
+        getWriterControlStruct().yVel = 0;
+        getWriterControlStruct().zVel = 0;
+        getWriterControlStruct().rollVel = 0;
+        getWriterControlStruct().pitchVel = 0;
+        getWriterControlStruct().yawVel = 0;
+        writeControlStruct();
+    }
+
 
 
     void NJointCartesianVelocityControllerWithRamp::rtPostDeactivateController()

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

@@ -138,6 +138,7 @@ namespace armarx
         void setTargetVelocity(float vx, float vy, float vz, float vrx, float vry, float vrz, const Ice::Current&) override;
         void setJointLimitAvoidanceScale(float jointLimitAvoidanceScale, const Ice::Current&) override;
         void setKpJointLimitAvoidance(float KpJointLimitAvoidance, const Ice::Current&) override;
+        void immediateHardStop(const Ice::Current&) override;
     };
 
 } // namespace armarx

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

@@ -94,8 +94,8 @@ namespace armarx
         //        }
         //        ARMARX_INFO << deactivateSpam(0.1) << VAROUT(oriCtrl.currentPosition) << VAROUT(orientationError);
         //        ARMARX_INFO << deactivateSpam(0.1) << VAROUT(target->velocityRotation) << VAROUT(sv->velocityRotation);
-        ARMARX_RT_LOGF_INFO("current pose x: %.2f y: %2.f, error x: %.2f y: %2.f error: %2.f ori error: %2.f", currentPose[0], currentPose[1], posError[0], posError[1], posError.norm(), orientationError).deactivateSpam(0.1);
-        ARMARX_RT_LOGF_INFO("new target vel: %2.f, %2.f current vel: %2.f, %2.f", target->velocityX, target->velocityY, sv->velocityX, sv->velocityY).deactivateSpam(0.1);
+        //ARMARX_RT_LOGF_INFO("current pose x: %.2f y: %2.f, error x: %.2f y: %2.f error: %2.f ori error: %2.f", currentPose[0], currentPose[1], posError[0], posError[1], posError.norm(), orientationError).deactivateSpam(0.1);
+        //ARMARX_RT_LOGF_INFO("new target vel: %2.f, %2.f current vel: %2.f, %2.f", target->velocityX, target->velocityY, sv->velocityX, sv->velocityY).deactivateSpam(0.1);
     }
 
     void NJointHolonomicPlatformRelativePositionController::rtPreActivateController()

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h

@@ -48,13 +48,13 @@ namespace armarx
     {
     public:
         std::string platformName;
-        float p = 0.5f;
+        float p = 1.0f;
         float i = 0.0f;
         float d = 0.0f;
         float maxVelocity = 300;
         float maxAcceleration = 100;
-        float maxRotationVelocity = 0.2;
-        float maxRotationAcceleration = 0.1;
+        float maxRotationVelocity = 0.5;
+        float maxRotationAcceleration = 0.2;
         //        float rad2MMFactor = 50.0f;
     };
 

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

@@ -44,6 +44,7 @@ module armarx
     {
         void setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration);
         void setTargetVelocity(float vx, float vy, float vz, float vrx, float vry, float vrz);
+        void immediateHardStop();
         void setJointLimitAvoidanceScale(float scale);
         void setKpJointLimitAvoidance(float KpJointLimitAvoidance);
     };

source/RobotAPI/libraries/core/CMakeLists.txt

@@ -42,6 +42,7 @@ set(LIB_FILES
     math/TimeSeriesUtils.cpp
     CartesianVelocityController.cpp
     CartesianPositionController.cpp
+    CartesianFeedForwardPositionController.cpp
     CartesianVelocityRamp.cpp
     JointVelocityRamp.cpp
     CartesianVelocityControllerWithRamp.cpp
@@ -82,6 +83,7 @@ set(LIB_HEADERS
     math/TimeSeriesUtils.h
     CartesianVelocityController.h
     CartesianPositionController.h
+    CartesianFeedForwardPositionController.h
     CartesianVelocityRamp.h
     JointVelocityRamp.h
     CartesianVelocityControllerWithRamp.h

source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.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 "CartesianFeedForwardPositionController.h"
+#include <RobotAPI/libraries/core/math/MathUtils.h>
+
+using namespace armarx;
+
+CartesianFeedForwardPositionController::CartesianFeedForwardPositionController(const VirtualRobot::RobotNodePtr& tcp)
+    : tcp(tcp)
+{
+
+}
+
+Eigen::VectorXf CartesianFeedForwardPositionController::calculate(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t, VirtualRobot::IKSolver::CartesianSelection mode)
+{
+    int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
+    int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
+    Eigen::VectorXf cartesianVel(posLen + oriLen);
+
+    if (posLen)
+    {
+        Eigen::Vector3f targetPos = trajectory->GetPosition(t);
+        Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
+        Eigen::Vector3f posVel =  errPos * KpPos;
+        if (enableFeedForward)
+        {
+            posVel += trajectory->GetPositionDerivative(t);
+        }
+
+        if (maxPosVel > 0)
+        {
+            posVel = math::MathUtils::LimitTo(posVel, maxPosVel);
+        }
+        cartesianVel.block<3, 1>(0, 0) = posVel;
+    }
+
+    if (oriLen)
+    {
+        Eigen::Matrix3f targetOri = trajectory->GetOrientation(t).toRotationMatrix();
+        Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+        Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
+        Eigen::AngleAxisf aa(oriDir);
+        Eigen::Vector3f errOri = aa.axis() * aa.angle();
+        Eigen::Vector3f oriVel = errOri * KpOri;
+        if (enableFeedForward)
+        {
+            oriVel += trajectory->GetOrientationDerivative(t);
+        }
+
+        if (maxOriVel > 0)
+        {
+            oriVel = math::MathUtils::LimitTo(oriVel, maxOriVel);
+        }
+        cartesianVel.block<3, 1>(posLen, 0) = oriVel;
+    }
+    return cartesianVel;
+}
+
+float CartesianFeedForwardPositionController::getPositionError(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t)
+{
+    Eigen::Vector3f targetPos = trajectory->GetPosition(t);
+    Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
+    return errPos.norm();
+}
+
+float CartesianFeedForwardPositionController::getOrientationError(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t)
+{
+    Eigen::Matrix3f targetOri = trajectory->GetOrientation(t).toRotationMatrix();
+    Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+    Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
+    Eigen::AngleAxisf aa(oriDir);
+    return aa.angle();
+}
+
+Eigen::Vector3f CartesianFeedForwardPositionController::getPositionDiff(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t)
+{
+    Eigen::Vector3f targetPos = trajectory->GetPosition(t);
+    return targetPos - tcp->getPositionInRootFrame();
+
+}
+
+Eigen::Vector3f CartesianFeedForwardPositionController::getOrientationDiff(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t)
+{
+    Eigen::Matrix3f targetOri = trajectory->GetOrientation(t).toRotationMatrix();
+    Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+    Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
+    Eigen::AngleAxisf aa(oriDir);
+    return aa.axis() * aa.angle();
+}

source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.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 <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Robot.h>
+#include <Eigen/Dense>
+#include <VirtualRobot/math/AbstractFunctionR1R6.h>
+
+namespace armarx
+{
+    class CartesianFeedForwardPositionController;
+    typedef boost::shared_ptr<CartesianFeedForwardPositionController> CartesianFeedForwardPositionControllerPtr;
+
+    class CartesianFeedForwardPositionController
+    {
+    public:
+        CartesianFeedForwardPositionController(const VirtualRobot::RobotNodePtr& tcp);
+        Eigen::VectorXf calculate(const math::AbstractFunctionR1R6Ptr& trajectory, float t, VirtualRobot::IKSolver::CartesianSelection mode);
+
+        float getPositionError(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t);
+        float getOrientationError(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t);
+        Eigen::Vector3f getPositionDiff(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t);
+        Eigen::Vector3f getOrientationDiff(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t);
+
+        float KpPos = 1;
+        float KpOri = 1;
+        float maxPosVel = -1;
+        float maxOriVel = -1;
+        bool enableFeedForward = true;
+
+    private:
+        VirtualRobot::RobotNodePtr tcp;
+    };
+}