diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index fc18eebbd7f004305f67c6f2e2f1462eb69bead0..8babcb823e858f3b255b1d6c3715cd4847cfb363 100644
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -53,6 +53,8 @@ set(LIB_FILES
NJointControllers/NJointCartesianVelocityController.cpp
NJointControllers/NJointCartesianTorqueController.cpp
NJointControllers/NJointTaskSpaceImpedanceController.cpp
+ NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
+ #NJointControllers/NJointCartesianActiveImpedanceController.cpp
Devices/SensorDevice.cpp
Devices/ControlDevice.cpp
@@ -109,6 +111,7 @@ set(LIB_HEADERS
NJointControllers/NJointTCPController.h
NJointControllers/NJointCartesianVelocityController.h
NJointControllers/NJointCartesianTorqueController.h
+ NJointControllers/NJointCartesianVelocityControllerWithRamp.h
#NJointControllers/NJointCartesianActiveImpedanceController.h
Devices/DeviceBase.h
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..63ea69e0d6780409e3d2d9e7b875320df55097b2
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
@@ -0,0 +1,359 @@
+/*
+ * 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 Sonja Marahrens( sonja.marahrens at kit dot edu)
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "NJointCartesianVelocityControllerWithRamp.h"
+#include <VirtualRobot/RobotNodeSet.h>
+#include "../RobotUnit.h"
+
+#define DEFAULT_TCP_STRING "default TCP"
+
+namespace armarx
+{
+
+ NJointControllerRegistration<NJointCartesianVelocityControllerWithRamp> registrationControllerNJointCartesianVelocityControllerWithRamp("NJointCartesianVelocityControllerWithRamp");
+
+ std::string NJointCartesianVelocityControllerWithRamp::getClassName(const Ice::Current&) const
+ {
+ return "NJointCartesianVelocityControllerWithRamp";
+ }
+
+ NJointCartesianVelocityControllerWithRamp::NJointCartesianVelocityControllerWithRamp(
+ NJointControllerDescriptionProviderInterfacePtr prov,
+ const NJointCartesianVelocityControllerWithRampConfigPtr& config,
+ const boost::shared_ptr<VirtualRobot::Robot>& r)
+ {
+ ARMARX_CHECK_EXPRESSION(prov);
+ RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
+ ARMARX_CHECK_EXPRESSION(robotUnit);
+ ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty());
+ VirtualRobot::RobotNodeSetPtr rns = robotUnit->getRtRobot()->getRobotNodeSet(config->nodeSetName);
+ ARMARX_CHECK_EXPRESSION_W_HINT(rns, config->nodeSetName);
+ for (size_t i = 0; i < rns->getSize(); ++i)
+ {
+ std::string jointName = rns->getNode(i)->getName();
+ ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = prov->getSensorValue(jointName);
+ targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
+
+ const SensorValue1DoFActuatorFilteredVelocity* filteredVelocitySensor = sv->asA<SensorValue1DoFActuatorFilteredVelocity>();
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ ARMARX_CHECK_EXPRESSION(filteredVelocitySensor || velocitySensor);
+ if (filteredVelocitySensor)
+ {
+ velocitySensors.push_back(&filteredVelocitySensor->filteredvelocity);
+ }
+ else if (velocitySensor)
+ {
+ velocitySensors.push_back(&velocitySensor->velocity);
+ }
+
+ };
+
+ VirtualRobot::RobotNodePtr tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? rns->getTCP() : robotUnit->getRtRobot()->getRobotNode(config->tcpName);
+ ARMARX_CHECK_EXPRESSION_W_HINT(tcp, config->tcpName);
+
+ nodeSetName = config->nodeSetName;
+ jointLimitAvoidanceScale = config->jointLimitAvoidanceScale;
+ KpJointLimitAvoidance = config->KpJointLimitAvoidance;
+ mode = ModeFromIce(config->mode);
+
+ controller.reset(new CartesianVelocityControllerWithRamp(rns, Eigen::VectorXf::Zero(rns->getSize()), mode,
+ config->maxPositionAcceleration, config->maxOrientationAcceleration, config->maxNullspaceAcceleration));
+ }
+
+
+ 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";
+ }
+
+ void NJointCartesianVelocityControllerWithRamp::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ Eigen::VectorXf x;
+ if (mode == VirtualRobot::IKSolver::All)
+ {
+ x.resize(6);
+ x << rtGetControlStruct().xVel, rtGetControlStruct().yVel, rtGetControlStruct().zVel, rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel, rtGetControlStruct().yawVel;
+ }
+ else if (mode == VirtualRobot::IKSolver::Position)
+ {
+ x.resize(3);
+ x << rtGetControlStruct().xVel, rtGetControlStruct().yVel, rtGetControlStruct().zVel;
+ }
+ else if (mode == VirtualRobot::IKSolver::Orientation)
+ {
+ x.resize(3);
+ x << rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel, rtGetControlStruct().yawVel;
+ }
+ else
+ {
+ // No mode has been set
+ return;
+ }
+
+ Eigen::VectorXf jnv = KpJointLimitAvoidance * controller->controller.calculateJointLimitAvoidance();
+ jnv += controller->controller.calculateNullspaceVelocity(x, jointLimitAvoidanceScale, mode);
+ Eigen::VectorXf jointTargetVelocities = controller->calculate(x, jnv, timeSinceLastIteration.toSecondsDouble());
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ targets.at(i)->velocity = jointTargetVelocities(i);
+ }
+ }
+
+
+
+ void NJointCartesianVelocityControllerWithRamp::setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration, const Ice::Current&)
+ {
+ controller->setMaxAccelerations(maxPositionAcceleration, maxOrientationAcceleration, maxNullspaceAcceleration);
+ }
+
+ 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;
+ getWriterControlStruct().zVel = vz;
+ getWriterControlStruct().rollVel = vrx;
+ getWriterControlStruct().pitchVel = vry;
+ getWriterControlStruct().yawVel = vrz;
+ writeControlStruct();
+ }
+
+ void NJointCartesianVelocityControllerWithRamp::setJointLimitAvoidanceScale(float jointLimitAvoidanceScale, const Ice::Current&)
+ {
+ this->jointLimitAvoidanceScale = jointLimitAvoidanceScale;
+ }
+
+ void NJointCartesianVelocityControllerWithRamp::setKpJointLimitAvoidance(float KpJointLimitAvoidance, const Ice::Current&)
+ {
+ this->KpJointLimitAvoidance = KpJointLimitAvoidance;
+ }
+
+
+
+ void NJointCartesianVelocityControllerWithRamp::rtPostDeactivateController()
+ {
+
+ }
+
+ WidgetDescription::StringWidgetDictionary NJointCartesianVelocityControllerWithRamp::getFunctionDescriptions(const Ice::Current&) const
+ {
+ return
+ {
+ {"ControllerTarget", createTargetLayout()},
+ {"ControllerParameters", createParameterLayout()}
+ };
+ }
+
+ void NJointCartesianVelocityControllerWithRamp::callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&)
+ {
+ if (name == "ControllerTarget")
+ {
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().xVel = valueMap.at("x")->getFloat();
+ getWriterControlStruct().yVel = valueMap.at("y")->getFloat();
+ getWriterControlStruct().zVel = valueMap.at("z")->getFloat();
+ getWriterControlStruct().rollVel = valueMap.at("roll")->getFloat();
+ getWriterControlStruct().pitchVel = valueMap.at("pitch")->getFloat();
+ getWriterControlStruct().yawVel = valueMap.at("yaw")->getFloat();
+ KpJointLimitAvoidance = valueMap.at("KpJointLimitAvoidance")->getFloat();
+ jointLimitAvoidanceScale = valueMap.at("jointLimitAvoidanceScale")->getFloat();
+ writeControlStruct();
+ }
+ else if (name == "ControllerParameters")
+ {
+ LockGuardType guard {controlDataMutex};
+ setMaxAccelerations(valueMap.at("maxPositionAcceleration")->getFloat(),
+ valueMap.at("maxOrientationAcceleration")->getFloat(),
+ valueMap.at("maxNullspaceAcceleration")->getFloat());
+ }
+ else
+ {
+ ARMARX_WARNING << "Unknown function name called: " << name;
+ }
+ }
+
+ WidgetDescription::VBoxLayoutPtr NJointCartesianVelocityControllerWithRamp::createTargetLayout() const
+ {
+ LayoutBuilder layout;
+ layout.addSlider("x", -100, 100, 0);
+ layout.addSlider("y", -100, 100, 0);
+ layout.addSlider("z", -100, 100, 0);
+ layout.addSlider("roll", -0.5, 0.5, 0);
+ layout.addSlider("pitch", -0.5, 0.5, 0);
+ layout.addSlider("yaw", -0.5, 0.5, 0);
+ layout.newLine();
+ layout.addSlider("KpJointLimitAvoidance", -10, 10, KpJointLimitAvoidance);
+ layout.addSlider("jointLimitAvoidanceScale", -10, 10, jointLimitAvoidanceScale);
+ return layout.getLayout();
+ }
+ WidgetDescription::VBoxLayoutPtr NJointCartesianVelocityControllerWithRamp::createParameterLayout() const
+ {
+ LayoutBuilder layout;
+ layout.addSlider("maxPositionAcceleration", 0, 1000, controller->getMaxPositionAcceleration());
+ layout.addSlider("maxOrientationAcceleration", 0, 100, controller->getMaxOrientationAcceleration());
+ layout.addSlider("maxNullspaceAcceleration", 0, 100, controller->getMaxNullspaceAcceleration());
+ return layout.getLayout();
+ }
+
+
+ WidgetDescription::WidgetPtr NJointCartesianVelocityControllerWithRamp::GenerateConfigDescription(const boost::shared_ptr<VirtualRobot::Robot>& 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);
+
+ LabelPtr labelTCP = new Label;
+ labelTCP->text = "tcp name";
+ layout->children.emplace_back(labelTCP);
+ StringComboBoxPtr boxTCP = new StringComboBox;
+ boxTCP->defaultIndex = 0;
+
+ boxTCP->options = robot->getRobotNodeNames();
+ boxTCP->options.insert(boxTCP->options.begin(), DEFAULT_TCP_STRING);
+ boxTCP->name = "tcpName";
+ layout->children.emplace_back(boxTCP);
+
+ LabelPtr labelMode = new Label;
+ labelMode->text = "mode";
+ layout->children.emplace_back(labelMode);
+ StringComboBoxPtr boxMode = new StringComboBox;
+
+ boxMode->options = {"Position", "Orientation", "Both"};
+ boxMode->name = "mode";
+ layout->children.emplace_back(boxMode);
+ layout->children.emplace_back(new HSpacer);
+ boxMode->defaultIndex = 2;
+
+
+ auto addSlider = [&](const std::string & label, float max, float defaultValue)
+ {
+ layout->children.emplace_back(new Label(false, label));
+ FloatSliderPtr floatSlider = new FloatSlider;
+ floatSlider->name = label;
+ floatSlider->min = 0;
+ floatSlider->defaultValue = defaultValue;
+ floatSlider->max = max;
+ layout->children.emplace_back(floatSlider);
+ };
+ addSlider("maxPositionAcceleration", 1000, 100);
+ addSlider("maxOrientationAcceleration", 10, 1);
+ addSlider("maxNullspaceAcceleration", 10, 2);
+ addSlider("KpJointLimitAvoidance", 10, 2);
+ addSlider("jointLimitAvoidanceScale", 10, 2);
+
+ return layout;
+ }
+
+ NJointCartesianVelocityControllerWithRampConfigPtr NJointCartesianVelocityControllerWithRamp::GenerateConfigFromVariants(const StringVariantBaseMap& values)
+ {
+ return new NJointCartesianVelocityControllerWithRampConfig(values.at("nodeSetName")->getString(), values.at("tcpName")->getString(),
+ IceModeFromString(values.at("mode")->getString()),
+ values.at("maxPositionAcceleration")->getFloat(),
+ values.at("maxOrientationAcceleration")->getFloat(),
+ values.at("maxNullspaceAcceleration")->getFloat(),
+ values.at("KpJointLimitAvoidance")->getFloat(),
+ values.at("jointLimitAvoidanceScale")->getFloat());
+ }
+
+ VirtualRobot::IKSolver::CartesianSelection NJointCartesianVelocityControllerWithRamp::ModeFromString(const std::string mode)
+ {
+ //ARMARX_IMPORTANT_S << "the mode is " << mode;
+ if (mode == "Position")
+ {
+ return VirtualRobot::IKSolver::CartesianSelection::Position;
+ }
+ if (mode == "Orientation")
+ {
+ return VirtualRobot::IKSolver::CartesianSelection::Orientation;
+ }
+ if (mode == "Both")
+ {
+ return VirtualRobot::IKSolver::CartesianSelection::All;
+ }
+ ARMARX_ERROR_S << "invalid mode " << mode;
+ return (VirtualRobot::IKSolver::CartesianSelection)0;
+ }
+
+ CartesianSelectionMode::CartesianSelection NJointCartesianVelocityControllerWithRamp::IceModeFromString(const std::string mode)
+ {
+ if (mode == "Position")
+ {
+ return CartesianSelectionMode::CartesianSelection::ePosition;
+ }
+ if (mode == "Orientation")
+ {
+ return CartesianSelectionMode::CartesianSelection::eOrientation;
+ }
+ if (mode == "Both")
+ {
+ return CartesianSelectionMode::CartesianSelection::eAll;
+ }
+ ARMARX_ERROR_S << "invalid mode " << mode;
+ return (CartesianSelectionMode::CartesianSelection)0;
+ }
+
+ VirtualRobot::IKSolver::CartesianSelection NJointCartesianVelocityControllerWithRamp::ModeFromIce(const CartesianSelectionMode::CartesianSelection mode)
+ {
+ if (mode == CartesianSelectionMode::CartesianSelection::ePosition)
+ {
+ return VirtualRobot::IKSolver::CartesianSelection::Position;
+ }
+ if (mode == CartesianSelectionMode::CartesianSelection::eOrientation)
+ {
+ return VirtualRobot::IKSolver::CartesianSelection::Orientation;
+ }
+ if (mode == CartesianSelectionMode::CartesianSelection::eAll)
+ {
+ return VirtualRobot::IKSolver::CartesianSelection::All;
+ }
+ ARMARX_ERROR_S << "invalid mode " << mode;
+ return (VirtualRobot::IKSolver::CartesianSelection)0;
+ }
+
+
+
+
+} // namespace armarx
+
+
+
+
+
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
new file mode 100644
index 0000000000000000000000000000000000000000..f93dc8e184220fcef3496722eeffdcec16e46c35
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
@@ -0,0 +1,144 @@
+/*
+ * 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 Sonja Marahrens( sonja.marahrens at kit dot edu)
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+
+#include "NJointController.h"
+#include <VirtualRobot/Robot.h>
+#include "../RobotUnit.h"
+#include "../ControlTargets/ControlTarget1DoFActuator.h"
+#include "../SensorValues/SensorValue1DoFActuator.h"
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
+
+namespace armarx
+{
+
+ TYPEDEF_PTRS_HANDLE(NJointCartesianVelocityControllerWithRamp);
+
+
+ class NJointCartesianVelocityControllerWithRampControlData
+ {
+ public:
+ float xVel = 0;
+ float yVel = 0;
+ float zVel = 0;
+ float rollVel = 0;
+ float pitchVel = 0;
+ float yawVel = 0;
+ };
+
+ class LayoutBuilder
+ {
+ WidgetDescription::VBoxLayoutPtr vlayout = new WidgetDescription::VBoxLayout;
+ WidgetDescription::HBoxLayoutPtr hlayout;
+ public:
+ LayoutBuilder()
+ {
+ newLine();
+ }
+
+ void newLine()
+ {
+ using namespace WidgetDescription;
+ hlayout = new HBoxLayout;
+ vlayout->children.emplace_back(hlayout);
+ }
+
+ void addSlider(const std::string& label, float min, float max, float value)
+ {
+ using namespace WidgetDescription;
+ hlayout->children.emplace_back(new Label(false, label));
+ FloatSliderPtr slider = new FloatSlider;
+ slider->name = label;
+ slider->min = min;
+ slider->defaultValue = value;
+ slider->max = max;
+ hlayout->children.emplace_back(slider);
+ }
+
+ WidgetDescription::VBoxLayoutPtr getLayout() const
+ {
+ return vlayout;
+ }
+
+ };
+
+
+
+ class NJointCartesianVelocityControllerWithRamp :
+ public NJointControllerWithTripleBuffer<NJointCartesianVelocityControllerWithRampControlData>,
+ public NJointCartesianVelocityControllerWithRampInterface
+ {
+ public:
+ using ConfigPtrT = NJointCartesianVelocityControllerWithRampConfigPtr;
+ NJointCartesianVelocityControllerWithRamp(NJointControllerDescriptionProviderInterfacePtr prov, const NJointCartesianVelocityControllerWithRampConfigPtr& 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 boost::shared_ptr<VirtualRobot::Robot>&,
+ const std::map<std::string, ConstControlDevicePtr>&,
+ const std::map<std::string, ConstSensorDevicePtr>&);
+ static NJointCartesianVelocityControllerWithRampConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
+ WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
+ void callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&) override;
+ WidgetDescription::VBoxLayoutPtr createTargetLayout() const;
+ WidgetDescription::VBoxLayoutPtr createParameterLayout() const;
+
+ static VirtualRobot::IKSolver::CartesianSelection ModeFromString(const std::string mode);
+ static CartesianSelectionMode::CartesianSelection IceModeFromString(const std::string mode);
+ static VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const CartesianSelectionMode::CartesianSelection mode);
+
+ protected:
+ void rtPreActivateController() override;
+ void rtPostDeactivateController() override;
+ private:
+ std::vector<ControlTarget1DoFActuatorVelocity*> targets;
+ //std::vector<const SensorValue1DoFActuatorPosition*> sensors;
+ std::vector<const float*> velocitySensors;
+
+ std::string nodeSetName;
+ float jointLimitAvoidanceScale;
+ float KpJointLimitAvoidance;
+ VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
+
+ CartesianVelocityControllerWithRampPtr controller;
+
+
+ // NJointCartesianVelocityControllerWithRampInterface interface
+ public:
+ void setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration, const Ice::Current& = Ice::Current()) override;
+ 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;
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
index 23e427b8d18915720554512347b11bade7b5a734..fc8b362bcd2c705f163387fa4f41884d9d92ac92 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
@@ -53,6 +53,18 @@ namespace armarx
return svi;
}
};
+ class SensorValue1DoFActuatorFilteredVelocity : virtual public SensorValueBase
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ float filteredvelocity = 0.0f;
+ static SensorValueInfo<SensorValue1DoFActuatorFilteredVelocity> GetClassMemberInfo()
+ {
+ SensorValueInfo<SensorValue1DoFActuatorFilteredVelocity> svi;
+ svi.addMemberVariable(&SensorValue1DoFActuatorFilteredVelocity::filteredvelocity, "filteredvelocity");
+ return svi;
+ }
+ };
class SensorValue1DoFActuatorAcceleration : virtual public SensorValueBase
{
public:
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index d7d212371e7dff8f1c97ae921d325cebe09eb25e..f291a755554ae599ae14a0b484ee4a99335d066e 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -16,6 +16,7 @@ set(SLICE_FILES
core/RobotState.ice
core/RobotStateObserverInterface.ice
core/Trajectory.ice
+ core/CartesianSelection.ice
selflocalisation/SelfLocalisationProcess.ice
@@ -45,6 +46,7 @@ set(SLICE_FILES
units/RobotUnit/NJointCartesianVelocityController.ice
units/RobotUnit/NJointCartesianTorqueController.ice
units/RobotUnit/NJointCartesianActiveImpedanceController.ice
+ units/RobotUnit/NJointCartesianVelocityControllerWithRamp.ice
units/RobotUnit/RobotUnitInterface.ice
units/RobotUnit/NJointJointSpaceDMPController.ice
diff --git a/source/RobotAPI/interface/core/CartesianSelection.ice b/source/RobotAPI/interface/core/CartesianSelection.ice
new file mode 100644
index 0000000000000000000000000000000000000000..6fc3d9b0897a8195766ba58aca46fb2b781b6daa
--- /dev/null
+++ b/source/RobotAPI/interface/core/CartesianSelection.ice
@@ -0,0 +1,38 @@
+/*
+* 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 ArmarX::RobotAPI
+* @author Mirko Waechter
+* @copyright 2016 Humanoids Group, HIS, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+module armarx
+{
+ module CartesianSelectionMode
+ {
+ enum CartesianSelection
+ {
+ ePosition = 7,
+ eOrientation = 8,
+ eAll = 15
+ };
+ };
+
+};
+
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.ice
new file mode 100644
index 0000000000000000000000000000000000000000..da13b86d37334d4f63fdd28b4a4056cb44d89dee
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.ice
@@ -0,0 +1,50 @@
+/*
+ * 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 Sonja Marahrens ( sonja dot marahrens at kit dot edu )
+ * @date 2018
+ * @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/CartesianSelection.ice>
+
+module armarx
+{
+
+ class NJointCartesianVelocityControllerWithRampConfig extends NJointControllerConfig
+ {
+ string nodeSetName = "";
+ string tcpName = "";
+ CartesianSelectionMode::CartesianSelection mode = CartesianSelectionMode::eAll;
+ float maxPositionAcceleration = 0;
+ float maxOrientationAcceleration = 0;
+ float maxNullspaceAcceleration = 0;
+ float KpJointLimitAvoidance = 0;
+ float jointLimitAvoidanceScale = 0;
+ };
+
+ interface NJointCartesianVelocityControllerWithRampInterface extends NJointControllerInterface
+ {
+ void setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration);
+ void setTargetVelocity(float vx, float vy, float vz, float vrx, float vry, float vrz);
+ void setJointLimitAvoidanceScale(float scale);
+ void setKpJointLimitAvoidance(float KpJointLimitAvoidance);
+ };
+};
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamps.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamps.ice
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.h b/source/RobotAPI/libraries/core/CartesianVelocityController.h
index fa1fb482f6c8b6c1e4b6169f1b721ae6def9ab4f..66fbcf4bb8ddc7462e2cc087a811cbb5971ff076 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.h
@@ -52,10 +52,10 @@ namespace armarx
Eigen::MatrixXf inv;
VirtualRobot::RobotNodeSetPtr rns;
VirtualRobot::DifferentialIKPtr ik;
+ VirtualRobot::RobotNodePtr tcp;
private:
Eigen::VectorXf calculateRegularization(VirtualRobot::IKSolver::CartesianSelection mode);
- VirtualRobot::RobotNodePtr tcp;
float cartesianMMRegularization;
float cartesianRadianRegularization;
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
index cbeb7e53a742703565114b2ecbbae6162845d78d..264db7d88a2101edcf3d674e3ccfa4abbfae8734 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
@@ -29,7 +29,13 @@ CartesianVelocityControllerWithRamp::CartesianVelocityControllerWithRamp(const V
float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration, const VirtualRobot::RobotNodePtr& tcp)
: controller(rns, tcp), mode(mode)
{
- Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(tcp, mode);
+ setMaxAccelerations(maxPositionAcceleration, maxOrientationAcceleration, maxNullspaceAcceleration);
+ setCurrentJointVelocity(currentJointVelocity);
+}
+
+void CartesianVelocityControllerWithRamp::setCurrentJointVelocity(const Eigen::VectorXf& currentJointVelocity)
+{
+ Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller.tcp, mode);
Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
Eigen::MatrixXf nullspace = lu_decomp.kernel();
@@ -39,11 +45,39 @@ CartesianVelocityControllerWithRamp::CartesianVelocityControllerWithRamp(const V
nsv += nullspace.col(i) * nullspace.col(i).dot(currentJointVelocity) / nullspace.col(i).squaredNorm();
}
- cartesianVelocity.reset(new CartesianVelocityRamp(jacobi * currentJointVelocity, maxPositionAcceleration, maxOrientationAcceleration, mode));
- nullSpaceVelocity.reset(new JointVelocityRamp(nsv, maxNullspaceAcceleration));
+ cartesianVelocityRamp.setState(jacobi * currentJointVelocity, mode);
+ nullSpaceVelocityRamp.setState(nsv);
+}
+
+Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt)
+{
+ Eigen::VectorXf nullspaceVel = controller.calculateNullspaceVelocity(cartesianVel, jointLimitAvoidanceScale, mode);
+ return calculate(cartesianVel, nullspaceVel, dt);
}
Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, float dt)
{
- return controller.calculate(cartesianVelocity->update(cartesianVel, dt), nullSpaceVelocity->update(nullspaceVel, dt), mode);
+ return controller.calculate(cartesianVelocityRamp.update(cartesianVel, dt), nullSpaceVelocityRamp.update(nullspaceVel, dt), mode);
+}
+
+void CartesianVelocityControllerWithRamp::setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration)
+{
+ cartesianVelocityRamp.setMaxOrientationAcceleration(maxOrientationAcceleration);
+ cartesianVelocityRamp.setMaxPositionAcceleration(maxPositionAcceleration);
+ nullSpaceVelocityRamp.setMaxAccelaration(maxNullspaceAcceleration);
+}
+
+float CartesianVelocityControllerWithRamp::getMaxOrientationAcceleration()
+{
+ return cartesianVelocityRamp.getMaxOrientationAcceleration();
+}
+
+float CartesianVelocityControllerWithRamp::getMaxPositionAcceleration()
+{
+ return cartesianVelocityRamp.getMaxPositionAcceleration();
+}
+
+float CartesianVelocityControllerWithRamp::getMaxNullspaceAcceleration()
+{
+ return nullSpaceVelocityRamp.getMaxAccelaration();
}
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
index 55f9ad770278cfdab2831177511a2ee4dd75e614..a32b443a67476363b1456cfa583cc99eb7deb549 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
@@ -48,13 +48,22 @@ namespace armarx
float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration,
const VirtualRobot::RobotNodePtr& tcp = nullptr);
+ void setCurrentJointVelocity(const Eigen::VectorXf& currentJointVelocity);
+
+ Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt);
Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, float dt);
- private:
+ void setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration);
+
+ float getMaxOrientationAcceleration();
+ float getMaxPositionAcceleration();
+ float getMaxNullspaceAcceleration();
+
+
CartesianVelocityController controller;
+ private:
VirtualRobot::IKSolver::CartesianSelection mode;
- CartesianVelocityRampPtr cartesianVelocity;
- JointVelocityRampPtr nullSpaceVelocity;
-
+ CartesianVelocityRamp cartesianVelocityRamp;
+ JointVelocityRamp nullSpaceVelocityRamp;
};
}
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
index d7fe3d7a19dd405bccb264da83bd4b722925134f..b5ed838bd322bd78581b68c52328454d3556dcb9 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
@@ -25,21 +25,29 @@
using namespace armarx;
-CartesianVelocityRamp::CartesianVelocityRamp(const Eigen::VectorXf& state, float maxPositiopnAcceleration, float maxOrientationAcceleration, VirtualRobot::IKSolver::CartesianSelection mode)
- : state(state), maxPositionAcceleration(maxPositiopnAcceleration), maxOrientationAcceleration(maxOrientationAcceleration), mode(mode)
+CartesianVelocityRamp::CartesianVelocityRamp()
+{ }
+
+void CartesianVelocityRamp::setState(const Eigen::VectorXf& state, VirtualRobot::IKSolver::CartesianSelection mode)
{
+ this->state = state;
+ this->mode = mode;
}
Eigen::VectorXf CartesianVelocityRamp::update(const Eigen::VectorXf& target, float dt)
{
+ if (dt <= 0)
+ {
+ return state;
+ }
Eigen::VectorXf delta = target - state;
int i = 0;
- float factor = 1;
+ float factor = 1 ;
if (mode & VirtualRobot::IKSolver::Position)
{
Eigen::Vector3f posDelta = delta.block<3, 1>(i, 0);
- float posFactor = posDelta.norm() / maxPositionAcceleration;
+ float posFactor = posDelta.norm() / maxPositionAcceleration / dt;
factor = std::max(factor, posFactor);
i += 3;
}
@@ -47,10 +55,32 @@ Eigen::VectorXf CartesianVelocityRamp::update(const Eigen::VectorXf& target, flo
if (mode & VirtualRobot::IKSolver::Orientation)
{
Eigen::Vector3f oriDelta = delta.block<3, 1>(i, 0);
- float oriFactor = oriDelta.norm() / maxOrientationAcceleration;
+ float oriFactor = oriDelta.norm() / maxOrientationAcceleration / dt;
factor = std::max(factor, oriFactor);
}
- state += delta / factor * dt;
+ state += delta / factor;
return state;
}
+
+void CartesianVelocityRamp::setMaxPositionAcceleration(float maxPositionAcceleration)
+{
+ this->maxPositionAcceleration = maxPositionAcceleration;
+
+}
+
+void CartesianVelocityRamp::setMaxOrientationAcceleration(float maxOrientationAcceleration)
+{
+ this->maxOrientationAcceleration = maxOrientationAcceleration;
+
+}
+
+float CartesianVelocityRamp::getMaxPositionAcceleration()
+{
+ return maxPositionAcceleration;
+}
+
+float CartesianVelocityRamp::getMaxOrientationAcceleration()
+{
+ return maxOrientationAcceleration;
+}
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityRamp.h b/source/RobotAPI/libraries/core/CartesianVelocityRamp.h
index 006b11e21f60a3619fdf542779776492a912cfc9..51abe44bc22a8647062397e9ee2fb123129ae7c4 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityRamp.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityRamp.h
@@ -36,16 +36,23 @@ namespace armarx
class CartesianVelocityRamp
{
public:
- CartesianVelocityRamp(const Eigen::VectorXf& state, float maxPositionAcceleration, float maxOrientationAcceleration, VirtualRobot::IKSolver::CartesianSelection mode);
+ CartesianVelocityRamp();
+ void setState(const Eigen::VectorXf& state, VirtualRobot::IKSolver::CartesianSelection mode);
Eigen::VectorXf update(const Eigen::VectorXf& target, float dt);
+ void setMaxPositionAcceleration(float maxPositionAcceleration);
+ void setMaxOrientationAcceleration(float maxOrientationAcceleration);
+
+ float getMaxPositionAcceleration();
+ float getMaxOrientationAcceleration();
+
private:
- float maxPositionAcceleration;
- float maxOrientationAcceleration;
+ float maxPositionAcceleration = 0;
+ float maxOrientationAcceleration = 0;
- Eigen::VectorXf state;
- VirtualRobot::IKSolver::CartesianSelection mode;
+ Eigen::VectorXf state = Eigen::VectorXf::Zero(6);
+ VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::CartesianSelection::All;
};
diff --git a/source/RobotAPI/libraries/core/JointVelocityRamp.cpp b/source/RobotAPI/libraries/core/JointVelocityRamp.cpp
index 94382e88be5af9492ec91cf5665a9330e266ee21..f0488c9e3e4c743b1add06c1e9bbe13cf7a92ebc 100644
--- a/source/RobotAPI/libraries/core/JointVelocityRamp.cpp
+++ b/source/RobotAPI/libraries/core/JointVelocityRamp.cpp
@@ -26,10 +26,12 @@
using namespace armarx;
-JointVelocityRamp::JointVelocityRamp(const Eigen::VectorXf& state, float maxAcceleration)
- : state(state), maxAcceleration(maxAcceleration)
-{
+JointVelocityRamp::JointVelocityRamp()
+{ }
+void JointVelocityRamp::setState(const Eigen::VectorXf& state)
+{
+ this->state = state;
}
Eigen::VectorXf JointVelocityRamp::update(const Eigen::VectorXf& target, float dt)
@@ -45,3 +47,14 @@ Eigen::VectorXf JointVelocityRamp::update(const Eigen::VectorXf& target, float d
}
+void JointVelocityRamp::setMaxAccelaration(float maxAcceleration)
+{
+ this->maxAcceleration = maxAcceleration;
+
+}
+
+float JointVelocityRamp::getMaxAccelaration()
+{
+ return maxAcceleration;
+}
+
diff --git a/source/RobotAPI/libraries/core/JointVelocityRamp.h b/source/RobotAPI/libraries/core/JointVelocityRamp.h
index 4ad10d51e054aa20cc131010425e1f524bcdd2e4..c31b4a4c2a5448653c7a7f7bbe2dd095f9573b2a 100644
--- a/source/RobotAPI/libraries/core/JointVelocityRamp.h
+++ b/source/RobotAPI/libraries/core/JointVelocityRamp.h
@@ -34,10 +34,14 @@ namespace armarx
class JointVelocityRamp
{
public:
- JointVelocityRamp(const Eigen::VectorXf& state, float maxAcceleration);
+ JointVelocityRamp();
+ void setState(const Eigen::VectorXf& state);
Eigen::VectorXf update(const Eigen::VectorXf& target, float dt);
+ void setMaxAccelaration(float maxAcceleration);
+ float getMaxAccelaration();
+
private:
Eigen::VectorXf state;
diff --git a/source/RobotAPI/libraries/core/test/CMakeLists.txt b/source/RobotAPI/libraries/core/test/CMakeLists.txt
index de4e6dee8c5c1e738dd78d555f61e2a9b0abcc34..a78cfe8edfe93fb1a6c2c22258f8e44c77f7a6d7 100644
--- a/source/RobotAPI/libraries/core/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/test/CMakeLists.txt
@@ -7,3 +7,4 @@ armarx_add_test(MathUtilsTest MathUtilsTest.cpp "${LIBS}")
armarx_add_test(CartesianVelocityControllerTest CartesianVelocityControllerTest.cpp "${LIBS}")
armarx_add_test(CartesianVelocityRampTest CartesianVelocityRampTest.cpp "${LIBS}")
+armarx_add_test(CartesianVelocityControllerWithRampTest CartesianVelocityControllerWithRampTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..487018abde075ae58f1eca8696b84a76685e002a
--- /dev/null
+++ b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp
@@ -0,0 +1,111 @@
+/*
+* 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 ArmarX::RobotAPI::Test
+* @author Nils Adermann (naderman at naderman dot de)
+* @date 2010
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#define BOOST_TEST_MODULE RobotAPI::CartesianVelocityController::Test
+#define ARMARX_BOOST_TEST
+#include <RobotAPI/Test.h>
+#include <ArmarXCore/core/test/IceTestHelper.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include "../CartesianVelocityControllerWithRamp.h"
+
+using namespace armarx;
+
+BOOST_AUTO_TEST_CASE(CartesianVelocityRampTest)
+{
+ Eigen::VectorXf state(6);
+ state << 0, 0, 0, 0, 0, 0;
+ float dt = 0.1f;
+
+ CartesianVelocityRamp c;
+ c.setState(state, VirtualRobot::IKSolver::CartesianSelection::All);
+ c.setMaxPositionAcceleration(100);
+ c.setMaxOrientationAcceleration(1);
+
+ Eigen::VectorXf target(6);
+ target << 200, 0, 0, 3, 0, 0;
+
+
+
+ for (size_t i = 0; i < 30; i++)
+ {
+ state = c.update(target, dt);
+ ARMARX_IMPORTANT << "state:" << state;
+ }
+
+}
+
+
+BOOST_AUTO_TEST_CASE(test1)
+{
+ const std::string project = "RobotAPI";
+ armarx::CMakePackageFinder finder(project);
+
+ if (!finder.packageFound())
+ {
+ ARMARX_ERROR_S << "ArmarX Package " << project << " has not been found!";
+ }
+ else
+ {
+ armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
+ }
+ std::string fn = "RobotAPI/robots/Armar3/ArmarIII.xml";
+ ArmarXDataPath::getAbsolutePath(fn, fn);
+ std::string robotFilename = fn;
+ VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFilename, VirtualRobot::RobotIO::eStructure);
+ VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet("HipYawRightArm");
+
+
+ Eigen::VectorXf currentjointVel(8);
+ currentjointVel << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
+
+
+ Eigen::VectorXf targetTcpVel(6);
+ targetTcpVel << 20, 0, 0, 0, 0, 0;
+
+ CartesianVelocityControllerWithRampPtr h(new CartesianVelocityControllerWithRamp(rns, currentjointVel, VirtualRobot::IKSolver::CartesianSelection::All, 100, 1, 1));
+ Eigen::MatrixXf jacobi = h->controller.ik->getJacobianMatrix(rns->getTCP(), VirtualRobot::IKSolver::CartesianSelection::All);
+ ARMARX_IMPORTANT << "Initial TCP Vel: " << (jacobi * currentjointVel).transpose();
+
+
+ float dt = 0.1f;
+
+ for (size_t n = 0; n < 10; n++)
+ {
+ //ARMARX_IMPORTANT << rns->getJointValues();
+ Eigen::VectorXf vel = h->calculate(targetTcpVel, 2, dt);
+ ARMARX_IMPORTANT << "jv: " << vel.transpose();
+ Eigen::VectorXf tcpVel = (h->controller.jacobi * vel);
+ ARMARX_IMPORTANT << "cv: " << tcpVel.transpose();
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ rns->getNode(i)->setJointValue(rns->getNode(i)->getJointValue() + vel(i) * dt);
+ }
+ }
+ /*
+ Eigen::VectorXf tcpVel = (h->jacobi * vel).transpose();
+ ARMARX_IMPORTANT << tcpVel;
+ BOOST_CHECK_LE((targetTcpVel - tcpVel).norm(), 0.01f);
+ */
+
+}
diff --git a/source/RobotAPI/libraries/core/test/CartesianVelocityRampTest.cpp b/source/RobotAPI/libraries/core/test/CartesianVelocityRampTest.cpp
index 90298759c02d03588c590205edafdfdaba39c364..5bfd4e36b8216ae4367ea582e18f382e1d3cb06f 100644
--- a/source/RobotAPI/libraries/core/test/CartesianVelocityRampTest.cpp
+++ b/source/RobotAPI/libraries/core/test/CartesianVelocityRampTest.cpp
@@ -39,10 +39,13 @@ BOOST_AUTO_TEST_CASE(testBoth)
Eigen::VectorXf state = MakeVectorXf(0, 0, 0, 0, 0, 0);
float dt = 0.1f;
- CartesianVelocityRampPtr c(new CartesianVelocityRamp(state, 100, 1, VirtualRobot::IKSolver::CartesianSelection::All));
+ CartesianVelocityRamp c;
+ c.setState(state, VirtualRobot::IKSolver::CartesianSelection::All);
+ c.setMaxPositionAcceleration(100);
+ c.setMaxOrientationAcceleration(1);
Eigen::VectorXf target = MakeVectorXf(100, 0, 0, 0, 0, 0);
- state = c->update(target, dt);
+ state = c.update(target, dt);
Eigen::VectorXf expected = MakeVectorXf(10, 0, 0, 0, 0, 0);
BOOST_CHECK_LE((state - expected).norm(), 0.01f);