diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index 63d1b4d00926aed6ab6ae1eeea46d7f6281c8214..f2d1ec328e91bd315d5f74bf0ab8879b85e112c0 100644
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -54,6 +54,8 @@ set(LIB_FILES
NJointControllers/NJointTCPController.cpp
NJointControllers/NJointCartesianVelocityController.cpp
NJointControllers/NJointCartesianTorqueController.cpp
+ NJointControllers/NJointTaskSpaceImpedanceController.cpp
+ NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
#NJointControllers/NJointCartesianActiveImpedanceController.cpp
Devices/SensorDevice.cpp
@@ -116,6 +118,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/NJointControllers/NJointTaskSpaceImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f73d4af4150497a0eabfd70200a03340ed578945
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
@@ -0,0 +1,236 @@
+/*
+ * 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 TaskSpaceActiveImpedanceControl::ArmarXObjects::NJointTaskSpaceImpedanceController
+ * @author zhou ( you dot zhou at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "NJointTaskSpaceImpedanceController.h"
+
+
+using namespace armarx;
+
+NJointControllerRegistration<NJointTaskSpaceImpedanceController> registrationControllerDSRTController("NJointTaskSpaceImpedanceController");
+
+
+NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+{
+
+ NJointTaskSpaceImpedanceControlConfigPtr cfg = NJointTaskSpaceImpedanceControlConfigPtr::dynamicCast(config);
+
+ ARMARX_CHECK_EXPRESSION(cfg);
+ ARMARX_CHECK_EXPRESSION(prov);
+ RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
+ ARMARX_CHECK_EXPRESSION(robotUnit);
+ ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
+ VirtualRobot::RobotNodeSetPtr rns = robotUnit->getRtRobot()->getRobotNodeSet(cfg->nodeSetName);
+
+ jointNames.clear();
+ ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName);
+ for (size_t i = 0; i < rns->getSize(); ++i)
+ {
+ std::string jointName = rns->getNode(i)->getName();
+ jointNames.push_back(jointName);
+ ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Torque1DoF);
+ ARMARX_CHECK_EXPRESSION(ct);
+ const SensorValueBase* sv = prov->getSensorValue(jointName);
+ ARMARX_CHECK_EXPRESSION(sv);
+ auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>();
+ ARMARX_CHECK_EXPRESSION(casted_ct);
+ targets.push_back(casted_ct);
+
+ const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ if (!torqueSensor)
+ {
+ ARMARX_WARNING << "No Torque sensor available for " << jointName;
+ }
+
+
+ torqueSensors.push_back(torqueSensor);
+ velocitySensors.push_back(velocitySensor);
+ positionSensors.push_back(positionSensor);
+ };
+ tcp = rns->getTCP();
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+
+ ARMARX_CHECK_EQUAL(cfg->desiredPose.size(), 7);
+ desiredPosition(0) = cfg->desiredPose[0];
+ desiredPosition(1) = cfg->desiredPose[1];
+ desiredPosition(2) = cfg->desiredPose[2];
+
+ desiredQuaternion.w() = cfg->desiredPose[3];
+ desiredQuaternion.x() = cfg->desiredPose[4];
+ desiredQuaternion.y() = cfg->desiredPose[5];
+ desiredQuaternion.z() = cfg->desiredPose[6];
+ // desiredTwist = cfg->desiredTwist;
+ // ARMARX_CHECK_EQUAL(desiredPose.rows(), 6);
+
+
+ ARMARX_CHECK_EQUAL(cfg->Kpos.size(), 3);
+ ARMARX_CHECK_EQUAL(cfg->Dpos.size(), 3);
+ ARMARX_CHECK_EQUAL(cfg->Kori.size(), 3);
+ ARMARX_CHECK_EQUAL(cfg->Dori.size(), 3);
+
+ kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
+ dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
+ kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
+ dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
+
+ torqueLimit = cfg->torqueLimit;
+
+ desiredJointPosition.resize(targets.size());
+
+ ARMARX_CHECK_EQUAL(cfg->desiredJointPositions.size(), targets.size());
+
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ desiredJointPosition(i) = cfg->desiredJointPositions.at(i);
+ }
+
+
+ knull = cfg->Knull;
+ dnull = cfg->Dnull;
+
+ NJointTaskSpaceImpedanceControllerControlData initData;
+ initData.desiredPose.resize(7);
+ initData.desiredPose.setZero();
+ initData.desiredTwist.resize(6);
+ initData.desiredTwist.setZero();
+
+ reinitTripleBuffer(initData);
+}
+
+void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+{
+ Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
+
+ Eigen::VectorXf qpos;
+ Eigen::VectorXf qvel;
+ qpos.resize(positionSensors.size());
+ qvel.resize(velocitySensors.size());
+
+ int jointDim = positionSensors.size();
+
+ for (size_t i = 0; i < velocitySensors.size(); ++i)
+ {
+ qpos(i) = positionSensors[i]->position;
+ qvel(i) = velocitySensors[i]->velocity;
+ }
+
+ Eigen::VectorXf tcptwist = jacobi * qvel;
+
+ Eigen::Vector3f currentTCPLinearVelocity;
+ currentTCPLinearVelocity << 0.001 * tcptwist(0), 0.001 * tcptwist(1), 0.001 * tcptwist(2);
+ Eigen::Vector3f currentTCPAngularVelocity;
+ currentTCPAngularVelocity << tcptwist(3), tcptwist(4), tcptwist(5);
+
+ Eigen::Vector3f currentTCPPosition = tcp->getPositionInRootFrame();
+ Eigen::Vector3f tcpForces = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition);
+ Eigen::Vector3f tcpDesiredForce = tcpForces - dpos.cwiseProduct(currentTCPLinearVelocity);
+
+ Eigen::Matrix4f currentTCPPose = tcp->getPoseInRootFrame();
+ Eigen::Matrix3f currentRotMat = currentTCPPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f desiredMat(desiredQuaternion);
+ Eigen::Matrix3f diffMat = desiredMat * currentRotMat.inverse();
+ Eigen::Quaternionf cquat(currentRotMat);
+ // Eigen::Quaternionf diffQuaternion = desiredQuaternion * cquat.conjugate();
+ // Eigen::AngleAxisf angleAxis(diffQuaternion);
+
+ Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+
+ Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
+ Eigen::Vector6f tcpDesiredWrench;
+ tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
+
+ // calculate desired joint torque
+ Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
+
+ float lambda = 2;
+
+ Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), lambda);
+
+ Eigen::VectorXf nullqerror = desiredJointPosition - qpos;
+
+ for (int i = 0; i < nullqerror.rows(); ++i)
+ {
+ if (fabs(nullqerror(i)) < 0.09)
+ {
+ nullqerror(i) = 0;
+ }
+ }
+ Eigen::VectorXf nullspaceTorque = knull * nullqerror - dnull * qvel;
+
+ Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * tcpDesiredWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
+
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ float desiredTorque = jointDesiredTorques(i);
+
+ desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
+ desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
+
+ debugDataInfo.getWriteBuffer().desired_torques[jointNames[i]] = jointDesiredTorques(i);
+
+ targets.at(i)->torque = desiredTorque;
+ }
+
+
+ float errorAngle = cquat.angularDistance(desiredQuaternion);
+ float posiError = (desiredPosition - currentTCPPosition).norm();
+
+ debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
+ debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
+ debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
+ debugDataInfo.getWriteBuffer().tcpDesiredTorque_x = tcpDesiredTorque(0);
+ debugDataInfo.getWriteBuffer().tcpDesiredTorque_y = tcpDesiredTorque(1);
+ debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
+ debugDataInfo.getWriteBuffer().quatError = errorAngle;
+ debugDataInfo.getWriteBuffer().posiError = posiError;
+ debugDataInfo.commitWrite();
+
+}
+
+void NJointTaskSpaceImpedanceController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+{
+ StringVariantBaseMap datafields;
+ auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
+ for (auto& pair : values)
+ {
+ datafields[pair.first] = new Variant(pair.second);
+ }
+
+ datafields["desiredForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
+ datafields["desiredForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
+ datafields["desiredForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
+
+ datafields["tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
+ datafields["tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
+ datafields["tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
+
+ datafields["quatError"] = new Variant(debugDataInfo.getUpToDateReadBuffer().quatError);
+ datafields["posiError"] = new Variant(debugDataInfo.getUpToDateReadBuffer().posiError);
+
+ debugObs->setDebugChannel("DSControllerOutput", datafields);
+
+
+}
+
+
+
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
new file mode 100644
index 0000000000000000000000000000000000000000..bc0d19d0972ec7dc889dd710ed99d2cd5365add1
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
@@ -0,0 +1,130 @@
+/*
+ * 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 TaskSpaceActiveImpedanceControl::ArmarXObjects::NJointTaskSpaceImpedanceController
+ * @author zhou ( you dot zhou at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_TaskSpaceActiveImpedanceControl_NJointTaskSpaceImpedanceController_H
+#define _ARMARX_LIB_TaskSpaceActiveImpedanceControl_NJointTaskSpaceImpedanceController_H
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+
+#include <RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.h>
+
+namespace armarx
+{
+ class NJointTaskSpaceImpedanceControllerControlData
+ {
+ public:
+ Eigen::VectorXf desiredPose;
+ Eigen::VectorXf desiredTwist;
+ };
+ /**
+ * @defgroup Library-NJointTaskSpaceImpedanceController NJointTaskSpaceImpedanceController
+ * @ingroup TaskSpaceActiveImpedanceControl
+ * A description of the library NJointTaskSpaceImpedanceController.
+ *
+ * @class NJointTaskSpaceImpedanceController
+ * @ingroup Library-NJointTaskSpaceImpedanceController
+ * @brief Brief description of class NJointTaskSpaceImpedanceController.
+ *
+ * Detailed description of class NJointTaskSpaceImpedanceController.
+ */
+ class NJointTaskSpaceImpedanceController : public NJointControllerWithTripleBuffer<NJointTaskSpaceImpedanceControllerControlData>, public NJointTaskSpaceImpedanceControlInterface
+ {
+ public:
+ using ConfigPtrT = NJointTaskSpaceImpedanceControlConfigPtr;
+
+ NJointTaskSpaceImpedanceController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+
+ std::string getClassName(const Ice::Current&) const
+ {
+ return "TaskSpaceImpedanceController";
+ }
+
+ // NJointController interface
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ protected:
+ void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+
+ private:
+ Eigen::Vector3f desiredPosition;
+ Eigen::Quaternionf desiredQuaternion;
+ Eigen::VectorXf desiredTwist;
+ VirtualRobot::DifferentialIKPtr ik;
+ Eigen::Vector3f kpos;
+ Eigen::Vector3f kori;
+ Eigen::Vector3f dpos;
+ Eigen::Vector3f dori;
+
+ Eigen::VectorXf desiredJointPosition;
+
+ float knull, dnull;
+
+ VirtualRobot::RobotNodePtr tcp;
+
+ std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
+ std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
+
+ std::vector<ControlTarget1DoFActuatorTorque*> targets;
+
+ struct NJointTaskSpaceImpedanceControllerDebugInfo
+ {
+ StringFloatDictionary desired_torques;
+ float desiredForce_x;
+ float desiredForce_y;
+ float desiredForce_z;
+ float tcpDesiredTorque_x;
+ float tcpDesiredTorque_y;
+ float tcpDesiredTorque_z;
+
+ float tcpDesiredAngularError_x;
+ float tcpDesiredAngularError_y;
+ float tcpDesiredAngularError_z;
+
+ float currentTCPAngularVelocity_x;
+ float currentTCPAngularVelocity_y;
+ float currentTCPAngularVelocity_z;
+
+ float currentTCPLinearVelocity_x;
+ float currentTCPLinearVelocity_y;
+ float currentTCPLinearVelocity_z;
+
+ float quatError;
+ float posiError;
+ };
+ TripleBuffer<NJointTaskSpaceImpedanceControllerDebugInfo> debugDataInfo;
+
+ float torqueLimit;
+ std::vector<std::string> jointNames;
+
+ };
+
+}
+
+#endif
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 2ea2965e5fc0a6c23787684003fdaf66677d335a..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,11 +46,13 @@ 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
units/RobotUnit/NJointTaskSpaceDMPController.ice
+ units/RobotUnit/TaskSpaceActiveImpedanceControl.ice
components/ViewSelectionInterface.ice
components/TrajectoryPlayerInterface.ice
components/RobotNameServiceInterface.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/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index baf640bdab8bcdb696c8b589995cf3e4fe7b5090..ac949e0e7eb4aa0898ad8a0b3c5613dfcca9b962 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -66,6 +66,12 @@ module armarx
double maxLinearVel;
double maxAngularVel;
+ string debugName;
+
+ float Kp_LinearVel;
+ float Kd_LinearVel;
+ float Kp_AngularVel;
+ float Kd_AngularVel;
};
@@ -110,6 +116,7 @@ module armarx
Ice::StringSeq dmpTypes;
Ice::DoubleSeq amplitudes;
+
double posToOriRatio = 100;
// velocity controller configuration
@@ -157,29 +164,34 @@ module armarx
Ice::FloatSeq leftDesiredJointValues;
Ice::FloatSeq rightDesiredJointValues;
- float KoriFollower = 1;
- float KposFollower = 1;
+// float KoriFollower = 1;
+// float KposFollower = 1;
double maxLinearVel;
double maxAngularVel;
- Ice::FloatSeq Kpos;
- Ice::FloatSeq Dpos;
- Ice::FloatSeq Kori;
- Ice::FloatSeq Dori;
+ Ice::FloatSeq leftKpos;
+ Ice::FloatSeq leftDpos;
+ Ice::FloatSeq leftKori;
+ Ice::FloatSeq leftDori;
+
+ Ice::FloatSeq rightKpos;
+ Ice::FloatSeq rightDpos;
+ Ice::FloatSeq rightKori;
+ Ice::FloatSeq rightDori;
float knull;
float dnull;
float torqueLimit;
- Ice::FloatSeq Kpf;
- Ice::FloatSeq Kif;
- Ice::FloatSeq DesiredForce;
+// Ice::FloatSeq Kpf;
+// Ice::FloatSeq Kif;
+// Ice::FloatSeq DesiredForce;
- float BoxWidth;
+// float BoxWidth;
- float FilterTimeConstant;
+// float FilterTimeConstant;
};
diff --git a/source/RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.ice b/source/RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.ice
new file mode 100644
index 0000000000000000000000000000000000000000..b736e4f1bfd78a226d50c19ad7f4ccef351516b5
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.ice
@@ -0,0 +1,53 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::NJointControllerInterface
+ * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
+
+module armarx
+{
+ class NJointTaskSpaceImpedanceControlConfig extends NJointControllerConfig
+ {
+ Ice::FloatSeq desiredPose;
+ Ice::FloatSeq desiredTwist;
+
+ Ice::FloatSeq Kpos;
+ Ice::FloatSeq Kori;
+ Ice::FloatSeq Dpos;
+ Ice::FloatSeq Dori;
+
+ string nodeSetName;
+ float torqueLimit;
+
+ float Knull;
+ float Dnull;
+
+ Ice::FloatSeq desiredJointPositions;
+ };
+
+ interface NJointTaskSpaceImpedanceControlInterface extends NJointControllerInterface
+ {
+
+ };
+
+};
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
index b712d6491dfee79e774e22748a6c16733af466b3..462493dfb858a6b15d643f44da4ddf34a5da3ed8 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
@@ -88,22 +88,32 @@ void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentP
currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, deltaT / timeDuration, targetPoseVec);
- for (size_t i = 0; i < 7; ++i)
- {
- targetPoseVec[i] = currentState[i].pos;
- }
+// for (size_t i = 0; i < 7; ++i)
+// {
+// targetPoseVec[i] = currentState[i].pos;
+// }
- float vel0, vel1;
- Eigen::Vector3f linearVel;
- linearVel << twist(0), twist(1), twist(2);
- for (size_t i = 0; i < 3; i++)
+ if (isPhaseStopControl)
{
- vel0 = currentState[i].vel / timeDuration;
- vel1 = config.phaseStopParas.Kpos * (targetPoseVec[i] - currentPosition(i)) - config.phaseStopParas.Dpos * linearVel(i);
- targetVel(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
- }
+ float vel0, vel1;
+ Eigen::Vector3f linearVel;
+ linearVel << twist(0), twist(1), twist(2);
+ for (size_t i = 0; i < 3; i++)
+ {
+ vel0 = currentState[i].vel / timeDuration;
+ vel1 = config.phaseStopParas.Kpos * (targetPoseVec[i] - currentPosition(i)) - config.phaseStopParas.Dpos * linearVel(i);
+ targetVel(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
+ }
+ }
+ else
+ {
+ for (size_t i = 0; i < 3; i++)
+ {
+ targetVel(i) = currentState[i].vel / timeDuration;
+ }
+ }
Eigen::Quaterniond dmpQuaternionVel;
dmpQuaternionVel.w() = currentState[3].vel;
@@ -150,9 +160,19 @@ void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentP
Eigen::Vector3f diffRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
Eigen::Vector3f angularVel1 = config.phaseStopParas.Kori * diffRPY - config.phaseStopParas.Dori * currentAngularVel;
- targetVel(3) = mpcFactor * angularVel0.x() / timeDuration + (1 - mpcFactor) * angularVel1(0);
- targetVel(4) = mpcFactor * angularVel0.y() / timeDuration + (1 - mpcFactor) * angularVel1(1);
- targetVel(5) = mpcFactor * angularVel0.z() / timeDuration + (1 - mpcFactor) * angularVel1(2);
+
+ if (isPhaseStopControl)
+ {
+ targetVel(3) = angularVel0.x() / timeDuration;
+ targetVel(4) = angularVel0.y() / timeDuration;
+ targetVel(5) = angularVel0.z() / timeDuration;
+ }
+ else
+ {
+ targetVel(3) = mpcFactor * angularVel0.x() / timeDuration + (1 - mpcFactor) * angularVel1(0);
+ targetVel(4) = mpcFactor * angularVel0.y() / timeDuration + (1 - mpcFactor) * angularVel1(1);
+ targetVel(5) = mpcFactor * angularVel0.z() / timeDuration + (1 - mpcFactor) * angularVel1(2);
+ }
debugData.canVal = canVal;
debugData.oriError = oriError;
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
index c62fef3fb25c3bbf43fd8093db3308110b68d195..c62ad28e2a932a29032642d0230591b23f23fd29 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
@@ -81,7 +81,7 @@ namespace armarx
class TaskSpaceDMPController
{
public:
- TaskSpaceDMPController(std::string name, const TaskSpaceDMPControllerConfig& config, bool isPhaseStop = true)
+ TaskSpaceDMPController(std::string name, const TaskSpaceDMPControllerConfig& config, bool isPhaseStopControl = true)
{
this->config = config;
@@ -104,7 +104,7 @@ namespace armarx
targetVel.setZero();
currentState.resize(7);
- this->isPhaseStop = isPhaseStop;
+ this->isPhaseStopControl = isPhaseStopControl;
dmpName = name;
}
@@ -166,7 +166,7 @@ namespace armarx
}
double canVal;
- bool isPhaseStop;
+ bool isPhaseStopControl;
std::string dmpName;
private:
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
index 1aefa06bbcd8af31e1e96509f0e273afe63edb8a..2563140cbc25077ea7a71edf91f193adf40e5e54 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
@@ -13,7 +13,6 @@ namespace armarx
useSynchronizedRtRobot();
leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
- rnsLeftBody = rtGetRobot()->getRobotNodeSet("LeftArmCol");
for (size_t i = 0; i < leftRNS->getSize(); ++i)
{
std::string jointName = leftRNS->getNode(i)->getName();
@@ -46,11 +45,6 @@ namespace armarx
leftAccelerationSensors.push_back(accelerationSensor);
};
-
-
-
- rnsRightBody = rtGetRobot()->getRobotNodeSet("RightArmCol");
-
rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
for (size_t i = 0; i < rightRNS->getSize(); ++i)
{
@@ -110,10 +104,10 @@ namespace armarx
taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
- TaskSpaceDMPControllerPtr leftLeader(new TaskSpaceDMPController("leftLeader", taskSpaceDMPConfig));
- TaskSpaceDMPControllerPtr leftFollower(new TaskSpaceDMPController("leftFollower", taskSpaceDMPConfig));
- TaskSpaceDMPControllerPtr rightLeader(new TaskSpaceDMPController("rightLeader", taskSpaceDMPConfig));
- TaskSpaceDMPControllerPtr rightFollower(new TaskSpaceDMPController("rightFollower", taskSpaceDMPConfig));
+ TaskSpaceDMPControllerPtr leftLeader(new TaskSpaceDMPController("leftLeader", taskSpaceDMPConfig, false));
+ TaskSpaceDMPControllerPtr leftFollower(new TaskSpaceDMPController("leftFollower", taskSpaceDMPConfig, false));
+ TaskSpaceDMPControllerPtr rightLeader(new TaskSpaceDMPController("rightLeader", taskSpaceDMPConfig, false));
+ TaskSpaceDMPControllerPtr rightFollower(new TaskSpaceDMPController("rightFollower", taskSpaceDMPConfig, false));
leftGroup.push_back(leftLeader);
leftGroup.push_back(rightFollower);
@@ -134,7 +128,7 @@ namespace armarx
initSensorData.currentRightPose = rightRNS->getTCP()->getPoseInRootFrame();
controllerSensorData.reinitAllBuffers(initSensorData);
- leaderName = "both";
+ leaderName = cfg->defautLeader;
NJointBimanualCCDMPControllerControlData initData;
initData.leftTargetVel.resize(6);
@@ -159,50 +153,24 @@ namespace armarx
rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
}
- KoriFollower = cfg->KoriFollower;
- KposFollower = cfg->KposFollower;
- kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
- dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
- kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
- dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
+ leftKpos << cfg->leftKpos[0], cfg->leftKpos[1], cfg->leftKpos[2];
+ leftDpos << cfg->leftDpos[0], cfg->leftDpos[1], cfg->leftDpos[2];
+ leftKori << cfg->leftKori[0], cfg->leftKori[1], cfg->leftKori[2];
+ leftDori << cfg->leftDori[0], cfg->leftDori[1], cfg->leftDori[2];
knull = cfg->knull;
dnull = cfg->dnull;
- torqueLimit = cfg->torqueLimit;
- kpf.resize(12);
- kif.resize(12);
- forceC_des.resize(12);
-
- ARMARX_CHECK_EQUAL(cfg->Kpf.size(), kpf.rows());
- ARMARX_CHECK_EQUAL(cfg->Kif.size(), kif.rows());
- ARMARX_CHECK_EQUAL(cfg->DesiredForce.size(), forceC_des.rows());
-
-
- for (size_t i = 0; i < 12; i++)
- {
- kpf(i) = cfg->Kpf.at(i);
- kif(i) = cfg->Kif.at(i);
- forceC_des(i) = cfg->DesiredForce.at(i);
- }
+ torqueLimit = cfg->torqueLimit;
- boxWidth = cfg->BoxWidth;
- filtered_leftForce.setZero();
- filtered_leftTorque.setZero();
- filtered_rightForce.setZero();
- filtered_rightTorque.setZero();
+ maxLinearVel = cfg->maxLinearVel;
+ maxAngularVel = cfg->maxAngularVel;
- filterTimeConstant = cfg->FilterTimeConstant;
- ftPIDController.resize(12);
- for (size_t i = 0; i < ftPIDController.size(); i++)
- {
- ftPIDController[i].reset(new PIDController(kpf(i), kif(i), 0.0, 10, 10));
- }
}
std::string NJointBimanualCCDMPController::getClassName(const Ice::Current&) const
@@ -275,14 +243,20 @@ namespace armarx
TaskSpaceDMPControllerPtr leaderDMP = currentControlGroup[0];
TaskSpaceDMPControllerPtr followerDMP = currentControlGroup[1];
+ virtualtimer = leaderDMP->canVal;
+
+ if(virtualtimer < 1e-8)
+ {
+ finished = true;
+ }
leaderDMP->flow(deltaT, currentLeaderPose, currentLeaderTwist);
Eigen::Matrix4f currentFollowerLocalPose;
- currentFollowerLocalPose.block<3, 3>(0, 0) = currentFollowerPose.block<3, 3>(0, 0) * currentLeaderPose.block<3, 3>(0, 0).inverse();
- currentFollowerLocalPose.block<3, 1>(0, 3) = currentFollowerLocalPose.block<3, 3>(0, 0) * (currentFollowerPose.block<3, 1>(0, 3) - currentLeaderPose.block<3, 1>(0, 3));
- followerDMP->flow(deltaT, currentFollowerPose, currentFollowerTwist);
+ currentFollowerLocalPose.block<3, 3>(0, 0) = currentLeaderPose.block<3, 3>(0, 0).inverse() * currentFollowerPose.block<3, 3>(0, 0);
+ currentFollowerLocalPose.block<3, 1>(0, 3) = currentLeaderPose.block<3, 3>(0, 0).inverse() * (currentFollowerPose.block<3, 1>(0, 3) - currentLeaderPose.block<3, 1>(0, 3));
+ followerDMP->flow(deltaT, currentFollowerLocalPose, currentFollowerTwist);
Eigen::VectorXf leaderTargetVel = leaderDMP->getTargetVelocity();
Eigen::Matrix4f leaderTargetPose = leaderDMP->getTargetPoseMat();
@@ -290,23 +264,24 @@ namespace armarx
std::vector<double> followerLocalTargetPoseVec = followerDMP->getTargetPose();
Eigen::Matrix4f followerTargetPose;
- followerTargetPose.block<3, 3>(0, 0) = followerLocalTargetPose.block<3, 3>(0, 0) * currentLeaderPose.block<3, 3>(0, 0);
+ followerTargetPose.block<3, 3>(0, 0) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 3>(0, 0);
followerTargetPose.block<3, 1>(0, 3) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 1>(0, 3) + currentLeaderPose.block<3, 1>(0, 3);
Eigen::VectorXf followerLocalTargetVel = followerDMP->getTargetVelocity();
Eigen::VectorXf followerTargetVel = followerLocalTargetVel;
+ followerTargetVel.setZero();
+
// followerTargetVel.block<3, 1>(0, 0) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetVel.block<3, 1>(0, 0) + leaderTargetVel.block<3, 1>(0, 0);
- followerTargetVel.block<3, 1>(0, 0) = KposFollower * (followerTargetPose.block<3, 1>(0, 3) - currentFollowerPose.block<3, 1>(0, 3));
+ // followerTargetVel.block<3, 1>(0, 0) = KposFollower * (followerTargetPose.block<3, 1>(0, 3) - currentFollowerPose.block<3, 1>(0, 3));
+ // Eigen::Matrix3f followerDiffMat = followerTargetPose.block<3, 3>(0, 0) * currentFollowerPose.block<3, 3>(0, 0).inverse();
+ // Eigen::Vector3f followerDiffRPY = KoriFollower * VirtualRobot::MathTools::eigen3f2rpy(followerDiffMat);
+ // followerTargetVel(3) = followerDiffRPY(0);
+ // followerTargetVel(4) = followerDiffRPY(1);
+ // followerTargetVel(5) = followerDiffRPY(2);
- Eigen::Matrix3f followerDiffMat = followerTargetPose.block<3, 3>(0, 0) * currentFollowerPose.block<3, 3>(0, 0).inverse();
- Eigen::Vector3f followerDiffRPY = KoriFollower * VirtualRobot::MathTools::eigen3f2rpy(followerDiffMat);
- followerTargetVel(3) = followerDiffRPY(0);
- followerTargetVel(4) = followerDiffRPY(1);
- followerTargetVel(5) = followerDiffRPY(2);
- virtualtimer = leaderDMP->canVal;
std::vector<double> leftDMPTarget;
@@ -315,6 +290,9 @@ namespace armarx
Eigen::Matrix4f leftTargetPose;
Eigen::Matrix4f rightTargetPose;
+ float leftKratio = 1.0;
+ float rightKratio = 1.0;
+
if (leaderName == "Left")
{
leftTargetVel = leaderTargetVel;
@@ -325,7 +303,9 @@ namespace armarx
leftTargetPose = leaderTargetPose;
- rightTargetPose = followerLocalTargetPose;
+ rightTargetPose = followerTargetPose;
+
+
}
else if (leaderName == "right")
{
@@ -336,90 +316,17 @@ namespace armarx
leftDMPTarget = followerLocalTargetPoseVec;
rightTargetPose = leaderTargetPose;
- leftTargetPose = followerLocalTargetPose;
+ leftTargetPose = followerTargetPose;
}
-
-
-
- // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_xvel"] = leftTargetVel(0);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_yvel"] = leftTargetVel(1);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_zvel"] = leftTargetVel(2);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_rollvel"] = leftTargetVel(3);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_pitchvel"] = leftTargetVel(4);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_yawvel"] = leftTargetVel(5);
-
- // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_xvel"] = rightTargetVel(0);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_yvel"] = rightTargetVel(1);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_zvel"] = rightTargetVel(2);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_rollvel"] = rightTargetVel(3);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_pitchvel"] = rightTargetVel(4);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_yawvel"] = rightTargetVel(5);
-
-
- // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_x"] = leftDMPTarget[0];
- // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_y"] = leftDMPTarget[1];
- // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_z"] = leftDMPTarget[2];
- // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qw"] = leftDMPTarget[3];
- // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qx"] = leftDMPTarget[4];
- // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qy"] = leftDMPTarget[5];
- // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qz"] = leftDMPTarget[6];
-
- // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_x"] = rightDMPTarget[0];
- // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_y"] = rightDMPTarget[1];
- // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_z"] = rightDMPTarget[2];
- // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qw"] = rightDMPTarget[3];
- // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qx"] = rightDMPTarget[4];
- // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qy"] = rightDMPTarget[5];
- // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qz"] = rightDMPTarget[6];
-
- // std::vector<double> currentLeftPoseVec = leaderDMP->eigen4f2vec(controllerSensorData.getReadBuffer().currentLeftPose);
- // std::vector<double> currentRightPoseVec = leaderDMP->eigen4f2vec(controllerSensorData.getReadBuffer().currentRightPose);
-
-
- // debugOutputData.getWriteBuffer().currentPose["leftPose_x"] = currentLeftPoseVec[0];
- // debugOutputData.getWriteBuffer().currentPose["leftPose_y"] = currentLeftPoseVec[1];
- // debugOutputData.getWriteBuffer().currentPose["leftPose_z"] = currentLeftPoseVec[2];
- // debugOutputData.getWriteBuffer().currentPose["leftPose_qw"] = currentLeftPoseVec[3];
- // debugOutputData.getWriteBuffer().currentPose["leftPose_qx"] = currentLeftPoseVec[4];
- // debugOutputData.getWriteBuffer().currentPose["leftPose_qy"] = currentLeftPoseVec[5];
- // debugOutputData.getWriteBuffer().currentPose["leftPose_qz"] = currentLeftPoseVec[6];
-
- // debugOutputData.getWriteBuffer().currentPose["rightPose_x"] = currentRightPoseVec[0];
- // debugOutputData.getWriteBuffer().currentPose["rightPose_y"] = currentRightPoseVec[1];
- // debugOutputData.getWriteBuffer().currentPose["rightPose_z"] = currentRightPoseVec[2];
- // debugOutputData.getWriteBuffer().currentPose["rightPose_qw"] = currentRightPoseVec[3];
- // debugOutputData.getWriteBuffer().currentPose["rightPose_qx"] = currentRightPoseVec[4];
- // debugOutputData.getWriteBuffer().currentPose["rightPose_qy"] = currentRightPoseVec[5];
- // debugOutputData.getWriteBuffer().currentPose["rightPose_qz"] = currentRightPoseVec[6];
-
-
- // debugOutputData.getWriteBuffer().leaderCanVal = leaderDMP->debugData.canVal;
- // debugOutputData.getWriteBuffer().leadermpcFactor = leaderDMP->debugData.mpcFactor;
- // debugOutputData.getWriteBuffer().leadererror = leaderDMP->debugData.poseError;
- // debugOutputData.getWriteBuffer().leaderposError = leaderDMP->debugData.posiError;
- // debugOutputData.getWriteBuffer().leaderoriError = leaderDMP->debugData.oriError;
-
- // debugOutputData.getWriteBuffer().followerCanVal = followerDMP->debugData.canVal;
- // debugOutputData.getWriteBuffer().followermpcFactor = followerDMP->debugData.mpcFactor;
- // debugOutputData.getWriteBuffer().followererror = followerDMP->debugData.poseError;
- // debugOutputData.getWriteBuffer().followerposError = followerDMP->debugData.posiError;
- // debugOutputData.getWriteBuffer().followeroriError = followerDMP->debugData.oriError;
-
- // debugOutputData.commitWrite();
-
-
-
-
-
-
-
LockGuardType guard {controlDataMutex};
getWriterControlStruct().leftTargetVel = leftTargetVel;
getWriterControlStruct().rightTargetVel = rightTargetVel;
getWriterControlStruct().leftTargetPose = leftTargetPose;
getWriterControlStruct().rightTargetPose = rightTargetPose;
+ getWriterControlStruct().virtualTime = virtualtimer;
+
writeControlStruct();
}
@@ -456,11 +363,11 @@ namespace armarx
controllerSensorData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
controllerSensorData.getWriteBuffer().deltaT = deltaT;
controllerSensorData.getWriteBuffer().currentTime += deltaT;
+
// cartesian vel controller
Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
- jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
Eigen::VectorXf leftqpos;
Eigen::VectorXf leftqvel;
@@ -473,7 +380,6 @@ namespace armarx
}
Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
- jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
Eigen::VectorXf rightqpos;
@@ -490,227 +396,133 @@ namespace armarx
Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
-
controllerSensorData.getWriteBuffer().currentLeftTwist = currentLeftTwist;
controllerSensorData.getWriteBuffer().currentRightTwist = currentRightTwist;
controllerSensorData.commitWrite();
+ jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
+ jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
+
Eigen::Matrix4f leftTargetPose = rtGetControlStruct().leftTargetPose;
Eigen::Matrix4f rightTargetPose = rtGetControlStruct().rightTargetPose;
- Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
- Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
+ double virtualtime = rtGetControlStruct().virtualTime;
Eigen::Matrix4f leftCurrentPose = tcpLeft->getPoseInRootFrame();
Eigen::Matrix4f rightCurrentPose = tcpRight->getPoseInRootFrame();
- // Todo: force control
- int nl = leftRNS->getSize();
- int nr = rightRNS->getSize();
- int n = nl + nr;
- // GraspMatrix
-
-// Eigen::Matrix4f poseLeftInRightCoordinate = tcpRight->toLocalCoordinateSystem(tcpLeft->getGlobalPose());
-// Eigen::Vector3f positionLeftInRightCoordinate;
-// positionLeftInRightCoordinate << poseLeftInRightCoordinate(0,3), poseLeftInRightCoordinate(1,3), poseLeftInRightCoordinate(2,3);
-// boxWidth = 0.001 * 0.5 * positionLeftInRightCoordinate.norm();
-
-
- Eigen::Vector3f localDistanceCoM;
- localDistanceCoM << 0.5 * boxWidth, 0, 0;
-
-
- Eigen::Vector3f rl = -leftCurrentPose.block<3, 3>(0, 0) * localDistanceCoM; //tcpLeft->getRobot()->getRootNode()->toLocalCoordinateSystem(tcpLeft->toGlobalCoordinateSystemVec(localDistanceCoM));
- Eigen::Vector3f rr = rightCurrentPose.block<3, 3>(0, 0) * localDistanceCoM;
- Eigen::MatrixXf leftGraspMatrix = Eigen::MatrixXf::Identity(6, 6);
- Eigen::MatrixXf rightGraspMatrix = Eigen::MatrixXf::Identity(6, 6);
- leftGraspMatrix.block<3, 3>(3, 0) << 0, -rl(2), rl(1),
- rl(2), 0, -rl(0),
- -rl(1), rl(0), 0;
- rightGraspMatrix.block<3, 3>(3, 0) << 0, -rr(2), rr(1),
- rr(2), 0, -rr(0),
- -rr(1), rr(0), 0;
- Eigen::MatrixXf graspMatrix;
- graspMatrix.resize(6, 12);
- graspMatrix << leftGraspMatrix, rightGraspMatrix;
-
- // constraint Jacobain and projection matrix
- Eigen::MatrixXf jacobi;
- jacobi.resize(12, n);
- jacobi.setZero(12, n);
- jacobi.block < 6, 8 > (0, 0) = jacobiL;
- jacobi.block < 6, 8 > (6, 8) = jacobiR;
-
- Eigen::MatrixXf pinvGraspMatrix = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 1);
- Eigen::MatrixXf proj2GraspNullspace = Eigen::MatrixXf::Identity(12, 12) - pinvGraspMatrix * graspMatrix;
- Eigen::MatrixXf jacobiConstrained = proj2GraspNullspace * jacobi;
-
- Eigen::MatrixXf pinvJacobiC = leftIK->computePseudoInverseJacobianMatrix(jacobiConstrained, 1);
- Eigen::MatrixXf projection = Eigen::MatrixXf::Identity(n, n) - pinvJacobiC * jacobiConstrained;
-
-
-
- // calculate inertia matrix
- Eigen::MatrixXf leftInertialMatrix;
- Eigen::MatrixXf rightInertialMatrix;
- leftInertialMatrix.setZero(nl, nl);
- rightInertialMatrix.setZero(nr, nr);
-
- for (size_t i = 0; i < rnsLeftBody->getNodeNames().size(); ++i)
+ Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
+ float normLinearVelocity = leftTargetVel.block<3, 1>(0, 0).norm();
+ if (normLinearVelocity > maxLinearVel)
{
- VirtualRobot::RobotNodePtr rnbody = rnsLeftBody->getNode(i);
- VirtualRobot::RobotNodePtr rnjoint = leftRNS->getNode(i + 1);
-
- Eigen::MatrixXf jacobipos_rn = 0.001 * leftIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Position);
- Eigen::MatrixXf jacobiori_rn = leftIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Orientation);
-
-
- float m = rnbody->getMass();
- Eigen::Matrix3f InertiaMatrix = rnbody->getInertiaMatrix();
- leftInertialMatrix += m * jacobipos_rn.transpose() * jacobipos_rn + jacobiori_rn.transpose() * InertiaMatrix * jacobiori_rn;
+ leftTargetVel.block<3, 1>(0, 0) = maxLinearVel * leftTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
}
-
- for (size_t i = 0; i < rnsRightBody->getNodeNames().size(); ++i)
+ float normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
+ if (normAngularVelocity > maxAngularVel)
{
- VirtualRobot::RobotNodePtr rnbody = rnsRightBody->getNode(i);
- VirtualRobot::RobotNodePtr rnjoint = rightRNS->getNode(i + 1);
-
- Eigen::MatrixXf jacobipos_rn = 0.001 * rightIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Position);
- Eigen::MatrixXf jacobiori_rn = rightIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Orientation);
-
-
- float m = rnbody->getMass();
- Eigen::Matrix3f InertiaMatrix = rnbody->getInertiaMatrix();
-
- rightInertialMatrix += m * jacobipos_rn.transpose() * jacobipos_rn + jacobiori_rn.transpose() * InertiaMatrix * jacobiori_rn;
+ leftTargetVel.block<3, 1>(3, 0) = maxAngularVel * leftTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
}
- Eigen::MatrixXf M = Eigen::MatrixXf::Zero(n, n);
- M.topLeftCorner(nl, nl) = leftInertialMatrix;
- M.bottomRightCorner(nr, nr) = rightInertialMatrix;
- Eigen::MatrixXf Mc = M + projection * M - M * projection;
+ Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
+ normLinearVelocity = rightTargetVel.block<3, 1>(0, 0).norm();
+ if (normLinearVelocity > maxLinearVel)
+ {
+ rightTargetVel.block<3, 1>(0, 0) = maxLinearVel * rightTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
+ }
+ normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
+ if (normAngularVelocity > maxAngularVel)
+ {
+ rightTargetVel.block<3, 1>(3, 0) = maxAngularVel * rightTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
+ }
// unconstrained space controller
Eigen::Vector6f leftJointControlWrench;
{
+
+
Eigen::Vector3f targetTCPLinearVelocity;
targetTCPLinearVelocity << 0.001 * leftTargetVel(0), 0.001 * leftTargetVel(1), 0.001 * leftTargetVel(2);
Eigen::Vector3f currentTCPLinearVelocity;
- currentTCPLinearVelocity << currentLeftTwist(0), currentLeftTwist(1), currentLeftTwist(2);
+ currentTCPLinearVelocity << 0.001 * currentLeftTwist(0), 0.001 * currentLeftTwist(1), 0.001 * currentLeftTwist(2);
Eigen::Vector3f currentTCPAngularVelocity;
currentTCPAngularVelocity << currentLeftTwist(3), currentLeftTwist(4), currentLeftTwist(5);
Eigen::Vector3f currentTCPPosition = leftCurrentPose.block<3, 1>(0, 3);
Eigen::Vector3f desiredPosition = leftTargetPose.block<3, 1>(0, 3);
- Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) + dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
+ Eigen::Vector3f tcpDesiredForce = 0.001 * leftKpos.cwiseProduct(desiredPosition - currentTCPPosition) + leftDpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
Eigen::Matrix3f currentRotMat = leftCurrentPose.block<3, 3>(0, 0);
Eigen::Matrix3f diffMat = leftTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
- Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
+ Eigen::Vector3f tcpDesiredTorque = leftKori.cwiseProduct(rpy) - leftDori.cwiseProduct(currentTCPAngularVelocity);
leftJointControlWrench << tcpDesiredForce, tcpDesiredTorque;
}
-
Eigen::Vector6f rightJointControlWrench;
{
+
+ Eigen::Vector3f targetTCPLinearVelocity;
+ targetTCPLinearVelocity << 0.001 * rightTargetVel(0), 0.001 * rightTargetVel(1), 0.001 * rightTargetVel(2);
+
Eigen::Vector3f currentTCPLinearVelocity;
- currentTCPLinearVelocity << currentRightTwist(0), currentRightTwist(1), currentRightTwist(2);
+ currentTCPLinearVelocity << 0.001 * currentRightTwist(0), 0.001 * currentRightTwist(1), 0.001 * currentRightTwist(2);
Eigen::Vector3f currentTCPAngularVelocity;
currentTCPAngularVelocity << currentRightTwist(3), currentRightTwist(4), currentRightTwist(5);
Eigen::Vector3f currentTCPPosition = rightCurrentPose.block<3, 1>(0, 3);
Eigen::Vector3f desiredPosition = rightTargetPose.block<3, 1>(0, 3);
- Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) - dpos.cwiseProduct(currentTCPLinearVelocity);
+ Eigen::Vector3f tcpDesiredForce = 0.001 * rightKpos.cwiseProduct(desiredPosition - currentTCPPosition) + rightDpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
Eigen::Matrix3f currentRotMat = rightCurrentPose.block<3, 3>(0, 0);
Eigen::Matrix3f diffMat = rightTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
- Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
+ Eigen::Vector3f tcpDesiredTorque = rightKori.cwiseProduct(rpy) - rightDori.cwiseProduct(currentTCPAngularVelocity);
rightJointControlWrench << tcpDesiredForce, tcpDesiredTorque;
}
- Eigen::VectorXf forceUnconstrained(12);
- forceUnconstrained << leftJointControlWrench, rightJointControlWrench;
- forceUnconstrained = proj2GraspNullspace * forceUnconstrained;
+// Eigen::VectorXf leftJointLimitAvoidance(leftRNS->getSize());
+// for (size_t i = 0; i < leftRNS->getSize(); i++)
+// {
+// VirtualRobot::RobotNodePtr rn = leftRNS->getNode(i);
+// if (rn->isLimitless())
+// {
+// leftJointLimitAvoidance(i) = 0;
+// }
+// else
+// {
+// float f = math::MathUtils::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
+// leftJointLimitAvoidance(i) = cos(f * M_PI);
+// }
+// }
+// Eigen::VectorXf leftNullspaceTorque = knull * leftJointLimitAvoidance - dnull * leftqvel;
+// Eigen::VectorXf rightJointLimitAvoidance(rightRNS->getSize());
+// for (size_t i = 0; i < rightRNS->getSize(); i++)
+// {
+// VirtualRobot::RobotNodePtr rn = rightRNS->getNode(i);
+// if (rn->isLimitless())
+// {
+// rightJointLimitAvoidance(i) = 0;
+// }
+// else
+// {
+// float f = math::MathUtils::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
+// rightJointLimitAvoidance(i) = cos(f * M_PI);
+// }
+// }
+// Eigen::VectorXf rightNullspaceTorque = knull * rightJointLimitAvoidance - dnull * rightqvel;
+
Eigen::VectorXf leftNullspaceTorque = knull * (leftDesiredJointValues - leftqpos) - dnull * leftqvel;
Eigen::VectorXf rightNullspaceTorque = knull * (rightDesiredJointValues - rightqpos) - dnull * rightqvel;
float lambda = 2;
- Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
-// Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * leftJointControlWrench + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
- Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * forceUnconstrained.head(6) + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
-
- Eigen::MatrixXf jtpinvR = leftIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
-// Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * rightJointControlWrench + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
- Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * forceUnconstrained.tail<6>() + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
-
- Eigen::VectorXf torqueUnconstrained(16);
- torqueUnconstrained << leftJointDesiredTorques, rightJointDesiredTorques;
-
- // constrained space control
-
- Eigen::MatrixXf mu = (Eigen::MatrixXf::Identity(n, n) - projection) * M * Mc.inverse();
- Eigen::VectorXf forceConstrained = Eigen::VectorXf::Zero(12);
-
-
- double filterFactor = 1 / ((filterTimeConstant / deltaT) + 1);
-
- filtered_leftForce = (1 - filterFactor) * filtered_leftForce + filterFactor * leftForceTorque->force;
- filtered_leftTorque = (1 - filterFactor) * filtered_leftTorque + filterFactor * leftForceTorque->torque;
- filtered_rightForce = (1 - filterFactor) * filtered_rightForce + filterFactor * rightForceTorque->force;
- filtered_rightTorque = (1 - filterFactor) * filtered_rightForce + filterFactor * rightForceTorque->torque;
-
- Eigen::VectorXf filteredForce;
- filteredForce.resize(12);
- filteredForce << filtered_leftForce, filtered_leftTorque, filtered_rightForce, filtered_rightTorque;
-
- for (size_t i = 0; i < 12; i++)
- {
- ftPIDController[i]->update(filteredForce(i), forceC_des(i));
- forceConstrained(i) = ftPIDController[i]->getControlValue();
-// forceConstrained(i) = forceC_des(i) + kpf(i) * (forceC_des(i) - filtered_leftForce(i)); // TODO: add PID controller
-// forceConstrained(i + 3) = forceC_des(i + 3) + kpf(i + 3) * (forceC_des(i + 3) - filtered_leftTorque(i)); // TODO: add PID controller
-// forceConstrained(i + 6) = forceC_des(i + 6) + kpf(i + 6) * (forceC_des(i + 6) - filtered_rightForce(i)); // TODO: add PID controller
-// forceConstrained(i + 9) = forceC_des(i + 9) + kpf(i + 9) * (forceC_des(i + 9) - filtered_rightTorque(i)); // TODO: add PID controller
- }
-
- forceConstrained.block<3,1>(0,0) = leftCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3,1>(0,0);
- forceConstrained.block<3,1>(3,0) = leftCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3,1>(3,0);
- forceConstrained.block<3,1>(6,0) = rightCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3,1>(6,0);
- forceConstrained.block<3,1>(9,0) = rightCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3,1>(9,0);
-
-
- Eigen::VectorXf qacc;
- qacc.resize(n);
- for (size_t i = 0; i < leftAccelerationSensors.size(); ++i)
- {
- qacc(i) = 0.001 * leftAccelerationSensors[i]->acceleration;
- }
-
- for (size_t i = 0; i < rightAccelerationSensors.size(); ++i)
- {
- qacc(i + leftAccelerationSensors.size()) = 0.001 * rightAccelerationSensors[i]->acceleration;
- }
-
-
- Eigen::VectorXf torqueConstrained = mu * (torqueUnconstrained + (Eigen::MatrixXf::Identity(n, n) - projection) * qacc) - jacobiConstrained.transpose() * forceConstrained;
-
-
- // all torques
-
-
-// Eigen::VectorXf torqueInput = projection * torqueUnconstrained + projection * torqueConstrained;
-// Eigen::VectorXf torqueInput = torqueUnconstrained + jacobi.transpose() * (Eigen::MatrixXf::Identity(12,12) - proj2GraspNullspace) * forceConstrained;
-// Eigen::VectorXf torqueInput = torqueUnconstrained + jacobi.transpose() * forceConstrained;
- Eigen::VectorXf torqueInput = torqueUnconstrained + projection * jacobi.transpose() * forceConstrained;
-
- leftJointDesiredTorques = torqueInput.head(nl);
- rightJointDesiredTorques = torqueInput.block<8, 1>(8, 0);
+ Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
+ Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
+ Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * leftJointControlWrench + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
+ Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * rightJointControlWrench + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
// torque limit
for (size_t i = 0; i < leftTargets.size(); ++i)
@@ -748,10 +560,21 @@ namespace armarx
rightTargets.at(i)->torque = desiredTorque;
}
-
- // debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
- // debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
- // debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
+ debugDataInfo.getWriteBuffer().leftControlSignal_x = leftJointControlWrench(0);
+
+ debugDataInfo.getWriteBuffer().leftControlSignal_x = leftJointControlWrench(0);
+ debugDataInfo.getWriteBuffer().leftControlSignal_y = leftJointControlWrench(1);
+ debugDataInfo.getWriteBuffer().leftControlSignal_z = leftJointControlWrench(2);
+ debugDataInfo.getWriteBuffer().leftControlSignal_ro = leftJointControlWrench(3);
+ debugDataInfo.getWriteBuffer().leftControlSignal_pi = leftJointControlWrench(4);
+ debugDataInfo.getWriteBuffer().leftControlSignal_ya = leftJointControlWrench(5);
+
+ debugDataInfo.getWriteBuffer().rightControlSignal_x = rightJointControlWrench(0);
+ debugDataInfo.getWriteBuffer().rightControlSignal_y = rightJointControlWrench(1);
+ debugDataInfo.getWriteBuffer().rightControlSignal_z = rightJointControlWrench(2);
+ debugDataInfo.getWriteBuffer().rightControlSignal_ro = rightJointControlWrench(3);
+ debugDataInfo.getWriteBuffer().rightControlSignal_pi = rightJointControlWrench(4);
+ debugDataInfo.getWriteBuffer().rightControlSignal_ya = rightJointControlWrench(5);
// debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_x = tcpDesiredTorque(0);
// debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_y = tcpDesiredTorque(1);
// debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
@@ -772,32 +595,12 @@ namespace armarx
debugDataInfo.getWriteBuffer().rightCurrentPose_x = rightCurrentPose(0, 3);
debugDataInfo.getWriteBuffer().rightCurrentPose_y = rightCurrentPose(1, 3);
debugDataInfo.getWriteBuffer().rightCurrentPose_z = rightCurrentPose(2, 3);
-
-
- for(size_t i = 0 ; i < 12; ++i)
- {
- std::stringstream ss;
- ss << i;
- std::string name = "forceConstrained_" + ss.str();
- debugDataInfo.getWriteBuffer().constrained_force[name] = forceConstrained(i);
- }
-
+ debugDataInfo.getWriteBuffer().virtualTime = virtualtime;
debugDataInfo.commitWrite();
- // Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
- // float normLinearVelocity = leftTargetVel.block<3, 1>(0, 0).norm();
- // if (normLinearVelocity > cfg->maxLinearVel)
- // {
- // leftTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * leftTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
- // }
- // float normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
- // if (normAngularVelocity > cfg->maxAngularVel)
- // {
- // leftTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * leftTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
- // }
// Eigen::VectorXf leftNullSpaceJointVelocity = cfg->knull * (leftDesiredJointValues - leftqpos);
// Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL, leftIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
@@ -807,18 +610,6 @@ namespace armarx
// {
// leftTargets.at(i)->velocity = jnvL(i);
// }
-
- // Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
- // normLinearVelocity = rightTargetVel.block<3, 1>(0, 0).norm();
- // if (normLinearVelocity > cfg->maxLinearVel)
- // {
- // rightTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rightTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
- // }
- // normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
- // if (normAngularVelocity > cfg->maxAngularVel)
- // {
- // rightTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rightTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
- // }
// Eigen::VectorXf rightNullSpaceJointVelocity = cfg->knull * (rightDesiredJointValues - rightqpos);
// Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR, rightIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
// Eigen::VectorXf jnvR = jtpinvR * rightTargetVel + (I - jtpinvR * jacobiR) * rightNullSpaceJointVelocity;
@@ -916,14 +707,15 @@ namespace armarx
leftGroup.at(1)->prepareExecution(getLocalPose(leftPose, rightPose), getLocalPose(leftGoals, rightGoals));
rightGroup.at(1)->prepareExecution(getLocalPose(rightPose, leftPose), getLocalPose(rightGoals, leftGoals));
+ finished = false;
controllerTask->start();
}
Eigen::Matrix4f NJointBimanualCCDMPController::getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose)
{
Eigen::Matrix4f localPose;
- localPose.block<3, 3>(0, 0) = globalTargetPose.block<3, 3>(0, 0) * newCoordinate.block<3, 3>(0, 0).inverse();
- localPose.block<3, 1>(0, 3) = localPose.block<3, 3>(0, 0) * (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
+ localPose.block<3, 3>(0, 0) = newCoordinate.block<3, 3>(0, 0).inverse() * globalTargetPose.block<3, 3>(0, 0);
+ localPose.block<3, 1>(0, 3) = newCoordinate.block<3, 3>(0, 0).inverse() * (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
return localPose;
}
@@ -961,36 +753,23 @@ namespace armarx
datafields["rightCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_y);
datafields["rightCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_z);
- // StringVariantBaseMap datafields;
- // auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
- // for (auto& pair : values)
- // {
- // datafields[pair.first] = new Variant(pair.second);
- // }
+ datafields["leftControlSignal_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_x);
+ datafields["leftControlSignal_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_y);
+ datafields["leftControlSignal_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_z);
+ datafields["leftControlSignal_ro"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_ro);
+ datafields["leftControlSignal_pi"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_pi);
+ datafields["leftControlSignal_ya"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_ya);
- // auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
- // for (auto& pair : dmpTargets)
- // {
- // datafields[pair.first] = new Variant(pair.second);
- // }
- // auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
- // for (auto& pair : currentPose)
- // {
- // datafields[pair.first] = new Variant(pair.second);
- // }
+ datafields["rightControlSignal_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_x);
+ datafields["rightControlSignal_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_y);
+ datafields["rightControlSignal_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_z);
+ datafields["rightControlSignal_ro"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_ro);
+ datafields["rightControlSignal_pi"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_pi);
+ datafields["rightControlSignal_ya"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_ya);
+
+ datafields["virtual_timer"] = new Variant(debugDataInfo.getUpToDateReadBuffer().virtualTime);
- // datafields["leaderCanVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderCanVal);
- // datafields["leadermpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().leadermpcFactor);
- // datafields["leaderError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leadererror);
- // datafields["leaderposError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderposError);
- // datafields["leaderoriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderoriError);
-
- // datafields["followerCanVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().followerCanVal);
- // datafields["followermpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().followermpcFactor);
- // datafields["followerError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followererror);
- // datafields["followerposError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followerposError);
- // datafields["followeroriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followeroriError);
debugObs->setDebugChannel("DMPController", datafields);
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
index ad39aac384d72d7345e38d4c35cebaa64d8d2887..9b62d80f6a10a21d917143993c881df08b658c70 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
@@ -15,6 +15,7 @@
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
#include <RobotAPI/libraries/core/PIDController.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
using namespace DMP;
@@ -35,6 +36,9 @@ namespace armarx
Eigen::Matrix4f leftTargetPose;
Eigen::Matrix4f rightTargetPose;
+
+ double virtualTime;
+
};
class NJointBimanualCCDMPController :
@@ -56,7 +60,7 @@ namespace armarx
void learnDMPFromFiles(const std::string&, const Ice::StringSeq&, const Ice::Current&);
bool isFinished(const Ice::Current&)
{
- return false;
+ return finished;
}
void runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&);
@@ -121,6 +125,21 @@ namespace armarx
float rightCurrentPose_y;
float rightCurrentPose_z;
+ float leftControlSignal_x;
+ float leftControlSignal_y;
+ float leftControlSignal_z;
+ float leftControlSignal_ro;
+ float leftControlSignal_pi;
+ float leftControlSignal_ya;
+
+
+ float rightControlSignal_x;
+ float rightControlSignal_y;
+ float rightControlSignal_z;
+ float rightControlSignal_ro;
+ float rightControlSignal_pi;
+ float rightControlSignal_ya;
+
// StringFloatDictionary latestTargetVelocities;
// StringFloatDictionary dmpTargets;
// StringFloatDictionary currentPose;
@@ -137,9 +156,11 @@ namespace armarx
// double followeroriError;
// double followerCanVal;
+ double virtualTime;
};
+ bool finished;
TripleBuffer<DebugBufferData> debugDataInfo;
struct NJointBimanualCCDMPControllerSensorData
@@ -152,6 +173,8 @@ namespace armarx
Eigen::VectorXf currentRightTwist;
+
+
};
TripleBuffer<NJointBimanualCCDMPControllerSensorData> controllerSensorData;
@@ -199,12 +222,16 @@ namespace armarx
Eigen::VectorXf forceC_des;
float boxWidth;
- Eigen::Vector3f kpos;
- Eigen::Vector3f kori;
- Eigen::Vector3f dpos;
- Eigen::Vector3f dori;
- Eigen::VectorXf kpf;
- Eigen::VectorXf kif;
+ Eigen::Vector3f leftKpos;
+ Eigen::Vector3f leftKori;
+ Eigen::Vector3f leftDpos;
+ Eigen::Vector3f leftDori;
+
+ Eigen::Vector3f rightKpos;
+ Eigen::Vector3f rightKori;
+ Eigen::Vector3f rightDpos;
+ Eigen::Vector3f rightDori;
+
float knull;
float dnull;
@@ -215,17 +242,10 @@ namespace armarx
float torqueLimit;
VirtualRobot::RobotNodeSetPtr leftRNS;
VirtualRobot::RobotNodeSetPtr rightRNS;
- VirtualRobot::RobotNodeSetPtr rnsLeftBody;
- VirtualRobot::RobotNodeSetPtr rnsRightBody;
-
- Eigen::Vector3f filtered_leftForce;
- Eigen::Vector3f filtered_leftTorque;
- Eigen::Vector3f filtered_rightForce;
- Eigen::Vector3f filtered_rightTorque;
- float filterTimeConstant;
- std::vector<PIDControllerPtr> ftPIDController;
+ float maxLinearVel;
+ float maxAngularVel;
// NJointBimanualCCDMPControllerInterface interface
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7112666794b57434aeee754772966f7e437bfb7d
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.cpp
@@ -0,0 +1,1098 @@
+#include "NJointBimanualCCDMPController.h"
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointBimanualCCDMPController> registrationControllerNJointBimanualCCDMPController("NJointBimanualCCDMPController");
+
+ NJointBimanualCCDMPController::NJointBimanualCCDMPController(armarx::NJointControllerDescriptionProviderInterfacePtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ ARMARX_INFO << "Preparing ... ";
+ cfg = NJointBimanualCCDMPControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION(prov);
+ RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
+ ARMARX_CHECK_EXPRESSION(robotUnit);
+
+ leftRNS = robotUnit->getRtRobot()->getRobotNodeSet("LeftArm");
+ rnsLeftBody = robotUnit->getRtRobot()->getRobotNodeSet("LeftArmCol");
+ for (size_t i = 0; i < leftRNS->getSize(); ++i)
+ {
+ std::string jointName = leftRNS->getNode(i)->getName();
+
+ leftJointNames.push_back(jointName);
+ ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Torque1DoF);
+ const SensorValueBase* sv = prov->getSensorValue(jointName);
+ leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ const SensorValue1DoFActuatorAcceleration* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
+
+ if (!velocitySensor)
+ {
+ ARMARX_WARNING << "No velocitySensor available for " << jointName;
+ }
+ if (!positionSensor)
+ {
+ ARMARX_WARNING << "No positionSensor available for " << jointName;
+ }
+
+ if (!accelerationSensor)
+ {
+ ARMARX_WARNING << "No accelerationSensor available for " << jointName;
+ }
+
+
+ leftVelocitySensors.push_back(velocitySensor);
+ leftPositionSensors.push_back(positionSensor);
+ leftAccelerationSensors.push_back(accelerationSensor);
+
+ };
+
+
+
+ rnsRightBody = robotUnit->getRtRobot()->getRobotNodeSet("RightArmCol");
+
+ rightRNS = robotUnit->getRtRobot()->getRobotNodeSet("RightArm");
+ for (size_t i = 0; i < rightRNS->getSize(); ++i)
+ {
+ std::string jointName = rightRNS->getNode(i)->getName();
+ rightJointNames.push_back(jointName);
+ ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Torque1DoF);
+ const SensorValueBase* sv = prov->getSensorValue(jointName);
+ rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ const SensorValue1DoFActuatorAcceleration* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
+
+
+ if (!velocitySensor)
+ {
+ ARMARX_WARNING << "No velocitySensor available for " << jointName;
+ }
+ if (!positionSensor)
+ {
+ ARMARX_WARNING << "No positionSensor available for " << jointName;
+ }
+ if (!accelerationSensor)
+ {
+ ARMARX_WARNING << "No accelerationSensor available for " << jointName;
+ }
+
+ rightVelocitySensors.push_back(velocitySensor);
+ rightPositionSensors.push_back(positionSensor);
+ rightAccelerationSensors.push_back(accelerationSensor);
+
+ };
+
+
+ const SensorValueBase* svlf = prov->getSensorValue("FT L");
+ leftForceTorque = svlf->asA<SensorValueForceTorque>();
+ const SensorValueBase* svrf = prov->getSensorValue("FT R");
+ rightForceTorque = svrf->asA<SensorValueForceTorque>();
+
+ leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rightRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+
+
+ TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
+ taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
+ taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
+ taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
+ taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
+ taskSpaceDMPConfig.DMPAmplitude = 1.0;
+ taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
+ taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
+ taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
+ taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
+ taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
+ taskSpaceDMPConfig.phaseStopParas.Dori = 0;
+ taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
+ taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
+ taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
+
+
+ TaskSpaceDMPControllerPtr leftLeader(new TaskSpaceDMPController("leftLeader", taskSpaceDMPConfig));
+ TaskSpaceDMPControllerPtr leftFollower(new TaskSpaceDMPController("leftFollower", taskSpaceDMPConfig));
+ TaskSpaceDMPControllerPtr rightLeader(new TaskSpaceDMPController("rightLeader", taskSpaceDMPConfig));
+ TaskSpaceDMPControllerPtr rightFollower(new TaskSpaceDMPController("rightFollower", taskSpaceDMPConfig));
+
+ leftGroup.push_back(leftLeader);
+ leftGroup.push_back(rightFollower);
+
+ rightGroup.push_back(rightLeader);
+ rightGroup.push_back(leftFollower);
+
+ bothLeaderGroup.push_back(leftLeader);
+ bothLeaderGroup.push_back(rightLeader);
+
+
+ tcpLeft = leftRNS->getTCP();
+ tcpRight = rightRNS->getTCP();
+ NJointBimanualCCDMPControllerSensorData initSensorData;
+ initSensorData.deltaT = 0;
+ initSensorData.currentTime = 0;
+ initSensorData.currentLeftPose = leftRNS->getTCP()->getPoseInRootFrame();
+ initSensorData.currentRightPose = rightRNS->getTCP()->getPoseInRootFrame();
+ controllerSensorData.reinitAllBuffers(initSensorData);
+
+ leaderName = "both";
+
+ NJointBimanualCCDMPControllerControlData initData;
+ initData.leftTargetVel.resize(6);
+ initData.leftTargetVel.setZero();
+ initData.rightTargetVel.resize(6);
+ initData.rightTargetVel.setZero();
+ reinitTripleBuffer(initData);
+
+ leftDesiredJointValues.resize(leftTargets.size());
+ ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
+
+ for (size_t i = 0; i < leftTargets.size(); ++i)
+ {
+ leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
+ }
+
+ rightDesiredJointValues.resize(rightTargets.size());
+ ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
+
+ for (size_t i = 0; i < rightTargets.size(); ++i)
+ {
+ rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
+ }
+
+ KoriFollower = cfg->KoriFollower;
+ KposFollower = cfg->KposFollower;
+
+ kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
+ dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
+ kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
+ dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
+
+
+ knull = cfg->knull;
+ dnull = cfg->dnull;
+
+ torqueLimit = cfg->torqueLimit;
+
+ kpf.resize(12);
+ kif.resize(12);
+ forceC_des.resize(12);
+
+ ARMARX_CHECK_EQUAL(cfg->Kpf.size(), kpf.rows());
+ ARMARX_CHECK_EQUAL(cfg->Kif.size(), kif.rows());
+ ARMARX_CHECK_EQUAL(cfg->DesiredForce.size(), forceC_des.rows());
+
+
+ for (size_t i = 0; i < 12; i++)
+ {
+ kpf(i) = cfg->Kpf.at(i);
+ kif(i) = cfg->Kif.at(i);
+ forceC_des(i) = cfg->DesiredForce.at(i);
+ }
+
+ boxWidth = cfg->BoxWidth;
+
+ filtered_leftForce.setZero();
+ filtered_leftTorque.setZero();
+ filtered_rightForce.setZero();
+ filtered_rightTorque.setZero();
+
+ // offset_leftForce.setZero();
+ // offset_leftTorque.setZero();
+ // offset_rightForce.setZero();
+ // offset_rightTorque.setZero();
+
+ offset_leftForce(0) = -4.34;
+ offset_leftForce(1) = -8.21;
+ offset_leftForce(2) = 15.59;
+
+ offset_leftTorque(0) = 1.42;
+ offset_leftTorque(1) = 0.29;
+ offset_leftTorque(2) = 0.09;
+
+ offset_rightForce(0) = 2.88;
+ offset_rightForce(1) = 11.15;
+ offset_rightForce(2) = -20.18;
+
+ offset_rightTorque(0) = -1.83;
+ offset_rightTorque(1) = 0.34;
+ offset_rightTorque(2) = -0.01;
+
+
+ filterTimeConstant = cfg->FilterTimeConstant;
+
+ ftPIDController.resize(12);
+ for (size_t i = 0; i < ftPIDController.size(); i++)
+ {
+ ftPIDController[i].reset(new PIDController(kpf(i), kif(i), 0.0, 10, 10));
+ }
+
+ isForceCompensateDone = false;
+
+ }
+
+ std::string NJointBimanualCCDMPController::getClassName(const Ice::Current&) const
+ {
+ return "NJointBimanualCCDMPController";
+ }
+
+ void NJointBimanualCCDMPController::controllerRun()
+ {
+ if (!controllerSensorData.updateReadBuffer())
+ {
+ return;
+ }
+
+ double deltaT = controllerSensorData.getReadBuffer().deltaT;
+
+
+ Eigen::VectorXf leftTargetVel;
+ leftTargetVel.resize(6);
+ leftTargetVel.setZero();
+ Eigen::VectorXf rightTargetVel;
+ rightTargetVel.resize(6);
+ rightTargetVel.setZero();
+
+ std::vector<TaskSpaceDMPControllerPtr > currentControlGroup;
+ Eigen::Matrix4f currentLeaderPose;
+ Eigen::Matrix4f currentFollowerPose;
+ Eigen::VectorXf currentLeaderTwist;
+ Eigen::VectorXf currentFollowerTwist;
+ if (leaderName == "Left")
+ {
+ currentControlGroup = leftGroup;
+ currentLeaderPose = controllerSensorData.getReadBuffer().currentLeftPose;
+ currentFollowerPose = controllerSensorData.getReadBuffer().currentRightPose;
+ currentLeaderTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
+ currentFollowerTwist = controllerSensorData.getReadBuffer().currentRightTwist;
+ }
+ else if (leaderName == "right")
+ {
+ currentControlGroup = rightGroup;
+ currentLeaderPose = controllerSensorData.getReadBuffer().currentRightPose;
+ currentFollowerPose = controllerSensorData.getReadBuffer().currentLeftPose;
+ currentLeaderTwist = controllerSensorData.getReadBuffer().currentRightTwist;
+ currentFollowerTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
+ }
+ else
+ {
+ currentControlGroup = bothLeaderGroup;
+
+ TaskSpaceDMPControllerPtr leaderDMPleft = currentControlGroup[0];
+ TaskSpaceDMPControllerPtr leaderDMPright = currentControlGroup[1];
+ leaderDMPleft->flow(deltaT, controllerSensorData.getReadBuffer().currentLeftPose, controllerSensorData.getReadBuffer().currentLeftTwist);
+ leaderDMPright->flow(deltaT, controllerSensorData.getReadBuffer().currentRightPose, controllerSensorData.getReadBuffer().currentRightTwist);
+
+ Eigen::VectorXf leftTargetVel = leaderDMPleft->getTargetVelocity();
+ Eigen::Matrix4f leftTargetPose = leaderDMPleft->getTargetPoseMat();
+ Eigen::VectorXf rightTargetVel = leaderDMPright->getTargetVelocity();
+ Eigen::Matrix4f rightTargetPose = leaderDMPright->getTargetPoseMat();
+
+ virtualtimer = leaderDMPleft->canVal;
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().leftTargetVel = leftTargetVel;
+ getWriterControlStruct().rightTargetVel = rightTargetVel;
+ getWriterControlStruct().leftTargetPose = leftTargetPose;
+ getWriterControlStruct().rightTargetPose = rightTargetPose;
+ writeControlStruct();
+
+ return;
+ }
+
+ TaskSpaceDMPControllerPtr leaderDMP = currentControlGroup[0];
+ TaskSpaceDMPControllerPtr followerDMP = currentControlGroup[1];
+
+
+ leaderDMP->flow(deltaT, currentLeaderPose, currentLeaderTwist);
+
+ Eigen::Matrix4f currentFollowerLocalPose;
+ currentFollowerLocalPose.block<3, 3>(0, 0) = currentFollowerPose.block<3, 3>(0, 0) * currentLeaderPose.block<3, 3>(0, 0).inverse();
+ currentFollowerLocalPose.block<3, 1>(0, 3) = currentFollowerLocalPose.block<3, 3>(0, 0) * (currentFollowerPose.block<3, 1>(0, 3) - currentLeaderPose.block<3, 1>(0, 3));
+ followerDMP->flow(deltaT, currentFollowerPose, currentFollowerTwist);
+
+ Eigen::VectorXf leaderTargetVel = leaderDMP->getTargetVelocity();
+ Eigen::Matrix4f leaderTargetPose = leaderDMP->getTargetPoseMat();
+ Eigen::Matrix4f followerLocalTargetPose = followerDMP->getTargetPoseMat();
+ std::vector<double> followerLocalTargetPoseVec = followerDMP->getTargetPose();
+
+ Eigen::Matrix4f followerTargetPose;
+ followerTargetPose.block<3, 3>(0, 0) = followerLocalTargetPose.block<3, 3>(0, 0) * currentLeaderPose.block<3, 3>(0, 0);
+ followerTargetPose.block<3, 1>(0, 3) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 1>(0, 3) + currentLeaderPose.block<3, 1>(0, 3);
+
+
+ Eigen::VectorXf followerLocalTargetVel = followerDMP->getTargetVelocity();
+ Eigen::VectorXf followerTargetVel = followerLocalTargetVel;
+ // followerTargetVel.block<3, 1>(0, 0) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetVel.block<3, 1>(0, 0) + leaderTargetVel.block<3, 1>(0, 0);
+ followerTargetVel.block<3, 1>(0, 0) = KposFollower * (followerTargetPose.block<3, 1>(0, 3) - currentFollowerPose.block<3, 1>(0, 3));
+
+
+ Eigen::Matrix3f followerDiffMat = followerTargetPose.block<3, 3>(0, 0) * currentFollowerPose.block<3, 3>(0, 0).inverse();
+ Eigen::Vector3f followerDiffRPY = KoriFollower * VirtualRobot::MathTools::eigen3f2rpy(followerDiffMat);
+ followerTargetVel(3) = followerDiffRPY(0);
+ followerTargetVel(4) = followerDiffRPY(1);
+ followerTargetVel(5) = followerDiffRPY(2);
+
+ virtualtimer = leaderDMP->canVal;
+
+
+ std::vector<double> leftDMPTarget;
+ std::vector<double> rightDMPTarget;
+
+ Eigen::Matrix4f leftTargetPose;
+ Eigen::Matrix4f rightTargetPose;
+
+ if (leaderName == "Left")
+ {
+ leftTargetVel = leaderTargetVel;
+ rightTargetVel = followerTargetVel;
+
+ leftDMPTarget = leaderDMP->getTargetPose();
+ rightDMPTarget = followerLocalTargetPoseVec;
+
+
+ leftTargetPose = leaderTargetPose;
+ rightTargetPose = followerLocalTargetPose;
+ }
+ else if (leaderName == "right")
+ {
+ rightTargetVel = leaderTargetVel;
+ leftTargetVel = followerTargetVel;
+
+ rightDMPTarget = leaderDMP->getTargetPose();
+ leftDMPTarget = followerLocalTargetPoseVec;
+
+ rightTargetPose = leaderTargetPose;
+ leftTargetPose = followerLocalTargetPose;
+
+ }
+
+
+
+
+
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_xvel"] = leftTargetVel(0);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_yvel"] = leftTargetVel(1);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_zvel"] = leftTargetVel(2);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_rollvel"] = leftTargetVel(3);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_pitchvel"] = leftTargetVel(4);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_yawvel"] = leftTargetVel(5);
+
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_xvel"] = rightTargetVel(0);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_yvel"] = rightTargetVel(1);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_zvel"] = rightTargetVel(2);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_rollvel"] = rightTargetVel(3);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_pitchvel"] = rightTargetVel(4);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_yawvel"] = rightTargetVel(5);
+
+
+ // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_x"] = leftDMPTarget[0];
+ // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_y"] = leftDMPTarget[1];
+ // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_z"] = leftDMPTarget[2];
+ // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qw"] = leftDMPTarget[3];
+ // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qx"] = leftDMPTarget[4];
+ // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qy"] = leftDMPTarget[5];
+ // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qz"] = leftDMPTarget[6];
+
+ // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_x"] = rightDMPTarget[0];
+ // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_y"] = rightDMPTarget[1];
+ // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_z"] = rightDMPTarget[2];
+ // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qw"] = rightDMPTarget[3];
+ // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qx"] = rightDMPTarget[4];
+ // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qy"] = rightDMPTarget[5];
+ // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qz"] = rightDMPTarget[6];
+
+ // std::vector<double> currentLeftPoseVec = leaderDMP->eigen4f2vec(controllerSensorData.getReadBuffer().currentLeftPose);
+ // std::vector<double> currentRightPoseVec = leaderDMP->eigen4f2vec(controllerSensorData.getReadBuffer().currentRightPose);
+
+
+ // debugOutputData.getWriteBuffer().currentPose["leftPose_x"] = currentLeftPoseVec[0];
+ // debugOutputData.getWriteBuffer().currentPose["leftPose_y"] = currentLeftPoseVec[1];
+ // debugOutputData.getWriteBuffer().currentPose["leftPose_z"] = currentLeftPoseVec[2];
+ // debugOutputData.getWriteBuffer().currentPose["leftPose_qw"] = currentLeftPoseVec[3];
+ // debugOutputData.getWriteBuffer().currentPose["leftPose_qx"] = currentLeftPoseVec[4];
+ // debugOutputData.getWriteBuffer().currentPose["leftPose_qy"] = currentLeftPoseVec[5];
+ // debugOutputData.getWriteBuffer().currentPose["leftPose_qz"] = currentLeftPoseVec[6];
+
+ // debugOutputData.getWriteBuffer().currentPose["rightPose_x"] = currentRightPoseVec[0];
+ // debugOutputData.getWriteBuffer().currentPose["rightPose_y"] = currentRightPoseVec[1];
+ // debugOutputData.getWriteBuffer().currentPose["rightPose_z"] = currentRightPoseVec[2];
+ // debugOutputData.getWriteBuffer().currentPose["rightPose_qw"] = currentRightPoseVec[3];
+ // debugOutputData.getWriteBuffer().currentPose["rightPose_qx"] = currentRightPoseVec[4];
+ // debugOutputData.getWriteBuffer().currentPose["rightPose_qy"] = currentRightPoseVec[5];
+ // debugOutputData.getWriteBuffer().currentPose["rightPose_qz"] = currentRightPoseVec[6];
+
+
+ // debugOutputData.getWriteBuffer().leaderCanVal = leaderDMP->debugData.canVal;
+ // debugOutputData.getWriteBuffer().leadermpcFactor = leaderDMP->debugData.mpcFactor;
+ // debugOutputData.getWriteBuffer().leadererror = leaderDMP->debugData.poseError;
+ // debugOutputData.getWriteBuffer().leaderposError = leaderDMP->debugData.posiError;
+ // debugOutputData.getWriteBuffer().leaderoriError = leaderDMP->debugData.oriError;
+
+ // debugOutputData.getWriteBuffer().followerCanVal = followerDMP->debugData.canVal;
+ // debugOutputData.getWriteBuffer().followermpcFactor = followerDMP->debugData.mpcFactor;
+ // debugOutputData.getWriteBuffer().followererror = followerDMP->debugData.poseError;
+ // debugOutputData.getWriteBuffer().followerposError = followerDMP->debugData.posiError;
+ // debugOutputData.getWriteBuffer().followeroriError = followerDMP->debugData.oriError;
+
+ // debugOutputData.commitWrite();
+
+
+
+
+
+
+
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().leftTargetVel = leftTargetVel;
+ getWriterControlStruct().rightTargetVel = rightTargetVel;
+ getWriterControlStruct().leftTargetPose = leftTargetPose;
+ getWriterControlStruct().rightTargetPose = rightTargetPose;
+ writeControlStruct();
+ }
+
+ Eigen::VectorXf NJointBimanualCCDMPController::getControlWrench(const Eigen::VectorXf& tcptwist, const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& targetPose)
+ {
+ // Eigen::Vector3f currentTCPLinearVelocity;
+ // currentTCPLinearVelocity << 0.001 * tcptwist(0), 0.001 * tcptwist(1), 0.001 * tcptwist(2);
+ // Eigen::Vector3f currentTCPAngularVelocity;
+ // currentTCPAngularVelocity << tcptwist(3), tcptwist(4), tcptwist(5);
+
+ // Eigen::Vector3f currentTCPPosition = currentPose.block<3,1>(0,3);
+ // Eigen::Vector3f desiredPosition = targetPose.block<3,1>(0,3);
+ // Eigen::Vector3f tcpForces = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition);
+ // Eigen::Vector3f tcpDesiredForce = tcpForces - dpos.cwiseProduct(currentTCPLinearVelocity);
+
+ // Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
+ // Eigen::Matrix3f diffMat = targetPose.block<3,3>(0,0) * currentRotMat.inverse();
+ // Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+
+ // Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
+ // Eigen::Vector6f tcpDesiredWrench;
+ // tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
+
+ // return tcpDesiredWrench;
+
+ }
+
+
+
+ void NJointBimanualCCDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ double deltaT = timeSinceLastIteration.toSecondsDouble();
+ controllerSensorData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
+ controllerSensorData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
+ controllerSensorData.getWriteBuffer().deltaT = deltaT;
+ controllerSensorData.getWriteBuffer().currentTime += deltaT;
+
+
+ // if(controllerSensorData.getWriteBuffer().currentTime > 0.3 && !isForceCompensateDone)
+ // {
+ // offset_leftForce = filtered_leftForce;
+ // offset_leftTorque = filtered_leftTorque;
+ // offset_rightForce = filtered_rightForce;
+ // offset_rightTorque = filtered_rightTorque;
+ // isForceCompensateDone = true;
+
+
+ // filtered_leftForce.setZero();
+ // filtered_leftTorque.setZero();
+ // filtered_rightForce.setZero();
+ // filtered_rightTorque.setZero();
+ // }
+
+ // if(controllerSensorData.getWriteBuffer().currentTime <= 0.3)
+ // {
+ // // torque limit
+ // for (size_t i = 0; i < leftTargets.size(); ++i)
+ // {
+ // leftTargets.at(i)->torque = 0;
+ // }
+
+ // for (size_t i = 0; i < rightTargets.size(); ++i)
+ // {
+ // rightTargets.at(i)->torque = 0;
+ // }
+
+ // return;
+ // }
+
+ // cartesian vel controller
+ Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
+
+ Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
+ jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
+
+ Eigen::VectorXf leftqpos;
+ Eigen::VectorXf leftqvel;
+ leftqpos.resize(leftPositionSensors.size());
+ leftqvel.resize(leftVelocitySensors.size());
+ for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
+ {
+ leftqpos(i) = leftPositionSensors[i]->position;
+ leftqvel(i) = leftVelocitySensors[i]->velocity;
+ }
+
+ Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
+ jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
+
+
+ Eigen::VectorXf rightqpos;
+ Eigen::VectorXf rightqvel;
+ rightqpos.resize(rightPositionSensors.size());
+ rightqvel.resize(rightVelocitySensors.size());
+
+ for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
+ {
+ rightqpos(i) = rightPositionSensors[i]->position;
+ rightqvel(i) = rightVelocitySensors[i]->velocity;
+ }
+
+ Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
+ Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
+
+
+ controllerSensorData.getWriteBuffer().currentLeftTwist = currentLeftTwist;
+ controllerSensorData.getWriteBuffer().currentRightTwist = currentRightTwist;
+ controllerSensorData.commitWrite();
+
+
+ Eigen::Matrix4f leftTargetPose = rtGetControlStruct().leftTargetPose;
+ Eigen::Matrix4f rightTargetPose = rtGetControlStruct().rightTargetPose;
+ Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
+ Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
+ Eigen::Matrix4f leftCurrentPose = tcpLeft->getPoseInRootFrame();
+ Eigen::Matrix4f rightCurrentPose = tcpRight->getPoseInRootFrame();
+
+ // Todo: force control
+ int nl = leftRNS->getSize();
+ int nr = rightRNS->getSize();
+ int n = nl + nr;
+ // GraspMatrix
+
+ // Eigen::Matrix4f poseLeftInRightCoordinate = tcpRight->toLocalCoordinateSystem(tcpLeft->getGlobalPose());
+ // Eigen::Vector3f positionLeftInRightCoordinate;
+ // positionLeftInRightCoordinate << poseLeftInRightCoordinate(0,3), poseLeftInRightCoordinate(1,3), poseLeftInRightCoordinate(2,3);
+ // boxWidth = 0.001 * 0.5 * positionLeftInRightCoordinate.norm();
+
+
+ Eigen::Vector3f localDistanceCoM;
+ localDistanceCoM << 0.5 * boxWidth, 0, 0;
+
+
+ Eigen::Vector3f rl = -leftCurrentPose.block<3, 3>(0, 0) * localDistanceCoM; //tcpLeft->getRobot()->getRootNode()->toLocalCoordinateSystem(tcpLeft->toGlobalCoordinateSystemVec(localDistanceCoM));
+ Eigen::Vector3f rr = rightCurrentPose.block<3, 3>(0, 0) * localDistanceCoM;
+ Eigen::MatrixXf leftGraspMatrix = Eigen::MatrixXf::Identity(6, 6);
+ Eigen::MatrixXf rightGraspMatrix = Eigen::MatrixXf::Identity(6, 6);
+ leftGraspMatrix.block<3, 3>(3, 0) << 0, -rl(2), rl(1),
+ rl(2), 0, -rl(0),
+ -rl(1), rl(0), 0;
+ rightGraspMatrix.block<3, 3>(3, 0) << 0, -rr(2), rr(1),
+ rr(2), 0, -rr(0),
+ -rr(1), rr(0), 0;
+ Eigen::MatrixXf graspMatrix;
+ graspMatrix.resize(6, 12);
+ graspMatrix << leftGraspMatrix, rightGraspMatrix;
+
+ // constraint Jacobain and projection matrix
+ Eigen::MatrixXf jacobi;
+ jacobi.resize(12, n);
+ jacobi.setZero(12, n);
+ jacobi.block < 6, 8 > (0, 0) = jacobiL;
+ jacobi.block < 6, 8 > (6, 8) = jacobiR;
+
+ Eigen::MatrixXf pinvGraspMatrix = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 1);
+ Eigen::MatrixXf proj2GraspNullspace = Eigen::MatrixXf::Identity(12, 12) - pinvGraspMatrix * graspMatrix;
+ Eigen::MatrixXf jacobiConstrained = proj2GraspNullspace * jacobi;
+
+ Eigen::MatrixXf pinvJacobiC = leftIK->computePseudoInverseJacobianMatrix(jacobiConstrained, 1);
+ Eigen::MatrixXf projection = Eigen::MatrixXf::Identity(n, n) - pinvJacobiC * jacobiConstrained;
+
+
+
+ // calculate inertia matrix
+ Eigen::MatrixXf leftInertialMatrix;
+ Eigen::MatrixXf rightInertialMatrix;
+ leftInertialMatrix.setZero(nl, nl);
+ rightInertialMatrix.setZero(nr, nr);
+
+ for (size_t i = 0; i < rnsLeftBody->getNodeNames().size(); ++i)
+ {
+ VirtualRobot::RobotNodePtr rnbody = rnsLeftBody->getNode(i);
+ VirtualRobot::RobotNodePtr rnjoint = leftRNS->getNode(i + 1);
+
+ Eigen::MatrixXf jacobipos_rn = 0.001 * leftIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Position);
+ Eigen::MatrixXf jacobiori_rn = leftIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Orientation);
+
+
+ float m = rnbody->getMass();
+ Eigen::Matrix3f InertiaMatrix = rnbody->getInertiaMatrix();
+ leftInertialMatrix += m * jacobipos_rn.transpose() * jacobipos_rn + jacobiori_rn.transpose() * InertiaMatrix * jacobiori_rn;
+ }
+
+ for (size_t i = 0; i < rnsRightBody->getNodeNames().size(); ++i)
+ {
+ VirtualRobot::RobotNodePtr rnbody = rnsRightBody->getNode(i);
+ VirtualRobot::RobotNodePtr rnjoint = rightRNS->getNode(i + 1);
+
+ Eigen::MatrixXf jacobipos_rn = 0.001 * rightIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Position);
+ Eigen::MatrixXf jacobiori_rn = rightIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Orientation);
+
+
+ float m = rnbody->getMass();
+ Eigen::Matrix3f InertiaMatrix = rnbody->getInertiaMatrix();
+
+ rightInertialMatrix += m * jacobipos_rn.transpose() * jacobipos_rn + jacobiori_rn.transpose() * InertiaMatrix * jacobiori_rn;
+ }
+ Eigen::MatrixXf M = Eigen::MatrixXf::Zero(n, n);
+ M.topLeftCorner(nl, nl) = leftInertialMatrix;
+ M.bottomRightCorner(nr, nr) = rightInertialMatrix;
+
+ Eigen::MatrixXf Mc = M + projection * M - M * projection;
+
+
+ // force filter and measure
+ double filterFactor = 1 / ((filterTimeConstant / deltaT) + 1);
+
+ filtered_leftForce = (1 - filterFactor) * filtered_leftForce + filterFactor * (leftForceTorque->force - offset_leftForce);
+ filtered_leftTorque = (1 - filterFactor) * filtered_leftTorque + filterFactor * (leftForceTorque->torque - offset_leftTorque);
+ filtered_rightForce = (1 - filterFactor) * filtered_rightForce + filterFactor * (rightForceTorque->force - offset_rightForce);
+ filtered_rightTorque = (1 - filterFactor) * filtered_rightForce + filterFactor * (rightForceTorque->torque - offset_rightTorque);
+
+ Eigen::VectorXf filteredForce;
+ filteredForce.resize(12);
+ filteredForce << filtered_leftForce, filtered_leftTorque, filtered_rightForce, filtered_rightTorque;
+ filteredForce.block<3, 1>(0, 0) = leftCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(0, 0);
+ filteredForce.block<3, 1>(3, 0) = leftCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(3, 0);
+ filteredForce.block<3, 1>(6, 0) = rightCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(6, 0);
+ filteredForce.block<3, 1>(9, 0) = rightCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(9, 0);
+
+
+ // calculate force control part
+ Eigen::VectorXf constrainedForceMeasure = proj2GraspNullspace * filteredForce;
+
+ constrainedForceMeasure.block<3, 1>(0, 0) = leftCurrentPose.block<3, 3>(0, 0).transpose() * constrainedForceMeasure.block<3, 1>(0, 0);
+ constrainedForceMeasure.block<3, 1>(3, 0) = leftCurrentPose.block<3, 3>(0, 0).transpose() * constrainedForceMeasure.block<3, 1>(3, 0);
+ constrainedForceMeasure.block<3, 1>(6, 0) = rightCurrentPose.block<3, 3>(0, 0).transpose() * constrainedForceMeasure.block<3, 1>(6, 0);
+ constrainedForceMeasure.block<3, 1>(9, 0) = rightCurrentPose.block<3, 3>(0, 0).transpose() * constrainedForceMeasure.block<3, 1>(9, 0);
+ Eigen::VectorXf forceConstrained = Eigen::VectorXf::Zero(12);
+ for (size_t i = 0; i < 12; i++)
+ {
+ ftPIDController[i]->update(constrainedForceMeasure(i), forceC_des(i));
+ forceConstrained(i) = ftPIDController[i]->getControlValue() + forceC_des(i);
+ // forceConstrained(i) = forceC_des(i) + kpf(i) * (forceC_des(i) - filtered_leftForce(i)); // TODO: add PID controller
+ // forceConstrained(i + 3) = forceC_des(i + 3) + kpf(i + 3) * (forceC_des(i + 3) - filtered_leftTorque(i)); // TODO: add PID controller
+ // forceConstrained(i + 6) = forceC_des(i + 6) + kpf(i + 6) * (forceC_des(i + 6) - filtered_rightForce(i)); // TODO: add PID controller
+ // forceConstrained(i + 9) = forceC_des(i + 9) + kpf(i + 9) * (forceC_des(i + 9) - filtered_rightTorque(i)); // TODO: add PID controller
+ }
+ forceConstrained.block<3, 1>(0, 0) = leftCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(0, 0);
+ forceConstrained.block<3, 1>(3, 0) = leftCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(3, 0);
+ forceConstrained.block<3, 1>(6, 0) = rightCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(6, 0);
+ forceConstrained.block<3, 1>(9, 0) = rightCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(9, 0);
+
+
+
+
+
+
+ // unconstrained space controller
+ Eigen::Vector6f leftJointControlWrench;
+ {
+ Eigen::Vector3f targetTCPLinearVelocity;
+ targetTCPLinearVelocity << 0.001 * leftTargetVel(0), 0.001 * leftTargetVel(1), 0.001 * leftTargetVel(2);
+
+ Eigen::Vector3f currentTCPLinearVelocity;
+ currentTCPLinearVelocity << currentLeftTwist(0), currentLeftTwist(1), currentLeftTwist(2);
+ Eigen::Vector3f currentTCPAngularVelocity;
+ currentTCPAngularVelocity << currentLeftTwist(3), currentLeftTwist(4), currentLeftTwist(5);
+ Eigen::Vector3f currentTCPPosition = leftCurrentPose.block<3, 1>(0, 3);
+ Eigen::Vector3f desiredPosition = leftTargetPose.block<3, 1>(0, 3);
+
+ Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) + dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
+ Eigen::Matrix3f currentRotMat = leftCurrentPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f diffMat = leftTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
+ Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+ Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
+ leftJointControlWrench << tcpDesiredForce, tcpDesiredTorque;
+ }
+
+
+ Eigen::Vector6f rightJointControlWrench;
+ {
+ Eigen::Vector3f targetTCPLinearVelocity;
+ targetTCPLinearVelocity << 0.001 * rightTargetVel(0), 0.001 * rightTargetVel(1), 0.001 * rightTargetVel(2);
+
+ Eigen::Vector3f currentTCPLinearVelocity;
+ currentTCPLinearVelocity << currentRightTwist(0), currentRightTwist(1), currentRightTwist(2);
+ Eigen::Vector3f currentTCPAngularVelocity;
+ currentTCPAngularVelocity << currentRightTwist(3), currentRightTwist(4), currentRightTwist(5);
+ Eigen::Vector3f currentTCPPosition = rightCurrentPose.block<3, 1>(0, 3);
+ Eigen::Vector3f desiredPosition = rightTargetPose.block<3, 1>(0, 3);
+
+ Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) - dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
+ Eigen::Matrix3f currentRotMat = rightCurrentPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f diffMat = rightTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
+ Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+ Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
+ rightJointControlWrench << tcpDesiredForce, tcpDesiredTorque;
+ }
+
+ Eigen::VectorXf forceUnconstrained(12);
+ forceUnconstrained << leftJointControlWrench, rightJointControlWrench;
+
+ Eigen::VectorXf unConstrainedForceMeasure = pinvGraspMatrix * graspMatrix * filteredForce;
+
+ Eigen::MatrixXf taskSpaceInertia = (jacobi * M.inverse() * jacobi.transpose()).inverse();
+
+
+
+ forceUnconstrained = taskSpaceInertia * forceUnconstrained ;//+ unConstrainedForceMeasure) - unConstrainedForceMeasure;
+
+ Eigen::VectorXf leftNullspaceTorque = knull * (leftDesiredJointValues - leftqpos) - dnull * leftqvel;
+ Eigen::VectorXf rightNullspaceTorque = knull * (rightDesiredJointValues - rightqpos) - dnull * rightqvel;
+ float lambda = 2;
+
+ Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
+ Eigen::MatrixXf jtpinvR = leftIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
+ forceUnconstrained.head(8) = forceUnconstrained.head(8) + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
+ forceUnconstrained.tail<8>() = forceUnconstrained.tail<8>() + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
+
+
+ //
+ //// Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * leftJointControlWrench + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
+ // Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * forceUnconstrained.head(6) + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
+
+ //
+ //// Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * rightJointControlWrench + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
+ // Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * forceUnconstrained.tail<6>() + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
+
+ // Eigen::VectorXf torqueUnconstrained(16);
+ // torqueUnconstrained << leftJointDesiredTorques, rightJointDesiredTorques;
+
+ // constrained space control
+
+ // Eigen::MatrixXf mu = (Eigen::MatrixXf::Identity(n, n) - projection) * M * Mc.inverse();
+ // Eigen::VectorXf qacc;
+ // qacc.resize(n);
+ // for (size_t i = 0; i < leftAccelerationSensors.size(); ++i)
+ // {
+ // qacc(i) = 0.001 * leftAccelerationSensors[i]->acceleration;
+ // }
+
+ // for (size_t i = 0; i < rightAccelerationSensors.size(); ++i)
+ // {
+ // qacc(i + leftAccelerationSensors.size()) = 0.001 * rightAccelerationSensors[i]->acceleration;
+ // }
+
+
+ // Eigen::VectorXf torqueConstrained = mu * (torqueUnconstrained + (Eigen::MatrixXf::Identity(n, n) - projection) * qacc) - jacobiConstrained.transpose() * forceConstrained;
+
+
+ // all torques
+
+
+
+ // Eigen::VectorXf torqueInput = projection * torqueUnconstrained + projection * torqueConstrained;
+ // Eigen::VectorXf torqueInput = torqueUnconstrained + jacobi.transpose() * (Eigen::MatrixXf::Identity(12,12) - proj2GraspNullspace) * forceConstrained;
+ // Eigen::VectorXf torqueInput = torqueUnconstrained + jacobi.transpose() * forceConstrained;
+ Eigen::VectorXf torqueInput = jacobi.transpose() * (forceUnconstrained + forceConstrained);
+
+ Eigen::VectorXf leftJointDesiredTorques = torqueInput.head(nl);
+ Eigen::VectorXf rightJointDesiredTorques = torqueInput.block<8, 1>(8, 0);
+
+ // torque limit
+ for (size_t i = 0; i < leftTargets.size(); ++i)
+ {
+ float desiredTorque = leftJointDesiredTorques(i);
+
+ if (isnan(desiredTorque))
+ {
+ desiredTorque = 0;
+ }
+
+ desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
+ desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
+
+ debugDataInfo.getWriteBuffer().desired_torques[leftJointNames[i]] = leftJointDesiredTorques(i);
+
+ leftTargets.at(i)->torque = desiredTorque;
+ }
+
+
+
+ for (size_t i = 0; i < rightTargets.size(); ++i)
+ {
+ float desiredTorque = rightJointDesiredTorques(i);
+
+ if (isnan(desiredTorque))
+ {
+ desiredTorque = 0;
+ }
+
+ desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
+ desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
+
+ debugDataInfo.getWriteBuffer().desired_torques[rightJointNames[i]] = rightJointDesiredTorques(i);
+
+ rightTargets.at(i)->torque = desiredTorque;
+ }
+
+ // debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
+ // debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
+ // debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
+ // debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_x = tcpDesiredTorque(0);
+ // debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_y = tcpDesiredTorque(1);
+ // debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
+ // debugDataInfo.getWriteBuffer().quatError = errorAngle;
+ // debugDataInfo.getWriteBuffer().posiError = posiError;
+ debugDataInfo.getWriteBuffer().leftTargetPose_x = leftTargetPose(0, 3);
+ debugDataInfo.getWriteBuffer().leftTargetPose_y = leftTargetPose(1, 3);
+ debugDataInfo.getWriteBuffer().leftTargetPose_z = leftTargetPose(2, 3);
+ debugDataInfo.getWriteBuffer().leftCurrentPose_x = leftCurrentPose(0, 3);
+ debugDataInfo.getWriteBuffer().leftCurrentPose_y = leftCurrentPose(1, 3);
+ debugDataInfo.getWriteBuffer().leftCurrentPose_z = leftCurrentPose(2, 3);
+
+
+ debugDataInfo.getWriteBuffer().rightTargetPose_x = rightTargetPose(0, 3);
+ debugDataInfo.getWriteBuffer().rightTargetPose_y = rightTargetPose(1, 3);
+ debugDataInfo.getWriteBuffer().rightTargetPose_z = rightTargetPose(2, 3);
+
+ debugDataInfo.getWriteBuffer().rightCurrentPose_x = rightCurrentPose(0, 3);
+ debugDataInfo.getWriteBuffer().rightCurrentPose_y = rightCurrentPose(1, 3);
+ debugDataInfo.getWriteBuffer().rightCurrentPose_z = rightCurrentPose(2, 3);
+
+
+ for (size_t i = 0 ; i < 12; ++i)
+ {
+ std::stringstream ss;
+ ss << i;
+ std::string name = "forceConstrained_" + ss.str();
+ debugDataInfo.getWriteBuffer().constrained_force[name] = filteredForce(i);
+ }
+
+
+ debugDataInfo.commitWrite();
+
+
+
+ // Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
+ // float normLinearVelocity = leftTargetVel.block<3, 1>(0, 0).norm();
+ // if (normLinearVelocity > cfg->maxLinearVel)
+ // {
+ // leftTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * leftTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
+ // }
+ // float normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
+ // if (normAngularVelocity > cfg->maxAngularVel)
+ // {
+ // leftTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * leftTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
+ // }
+
+ // Eigen::VectorXf leftNullSpaceJointVelocity = cfg->knull * (leftDesiredJointValues - leftqpos);
+ // Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL, leftIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
+ // Eigen::VectorXf jnvL = jtpinvL * leftTargetVel + (I - jtpinvL * jacobiL) * leftNullSpaceJointVelocity;
+
+ // for (size_t i = 0; i < leftTargets.size(); ++i)
+ // {
+ // leftTargets.at(i)->velocity = jnvL(i);
+ // }
+
+ // Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
+ // normLinearVelocity = rightTargetVel.block<3, 1>(0, 0).norm();
+ // if (normLinearVelocity > cfg->maxLinearVel)
+ // {
+ // rightTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rightTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
+ // }
+ // normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
+ // if (normAngularVelocity > cfg->maxAngularVel)
+ // {
+ // rightTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rightTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
+ // }
+ // Eigen::VectorXf rightNullSpaceJointVelocity = cfg->knull * (rightDesiredJointValues - rightqpos);
+ // Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR, rightIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
+ // Eigen::VectorXf jnvR = jtpinvR * rightTargetVel + (I - jtpinvR * jacobiR) * rightNullSpaceJointVelocity;
+
+ // for (size_t i = 0; i < rightTargets.size(); ++i)
+ // {
+ // rightTargets.at(i)->velocity = jnvR(i);
+ // }
+ }
+
+ void NJointBimanualCCDMPController::learnDMPFromFiles(const std::string& name, const Ice::StringSeq& fileNames, const Ice::Current&)
+ {
+ if (name == "LeftLeader")
+ {
+ leftGroup.at(0)->learnDMPFromFiles(fileNames);
+ }
+ else if (name == "LeftFollower")
+ {
+ rightGroup.at(1)->learnDMPFromFiles(fileNames);
+ }
+ else if (name == "RightLeader")
+ {
+ rightGroup.at(0)->learnDMPFromFiles(fileNames);
+ }
+ else
+ {
+ leftGroup.at(1)->learnDMPFromFiles(fileNames);
+ }
+ }
+
+
+ void NJointBimanualCCDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
+ {
+ LockGuardType guard(controllerMutex);
+ if (leaderName == "Left")
+ {
+ leftGroup.at(0)->setViaPose(u, viapoint);
+ }
+ else
+ {
+ rightGroup.at(0)->setViaPose(u, viapoint);
+ }
+ }
+
+ void NJointBimanualCCDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
+ {
+ LockGuardType guard(controllerMutex);
+ if (leaderName == "Left")
+ {
+ leftGroup.at(0)->setGoalPoseVec(goals);
+ }
+ else
+ {
+ rightGroup.at(0)->setGoalPoseVec(goals);
+ }
+
+ }
+
+ void NJointBimanualCCDMPController::changeLeader(const Ice::Current&)
+ {
+ LockGuardType guard(controllerMutex);
+
+ if (leaderName == "Left")
+ {
+ leaderName = "Right";
+ rightGroup.at(0)->canVal = virtualtimer;
+ rightGroup.at(1)->canVal = virtualtimer;
+ }
+ else
+ {
+ leaderName = "Left";
+ leftGroup.at(0)->canVal = virtualtimer;
+ leftGroup.at(1)->canVal = virtualtimer;
+ }
+
+ }
+
+
+
+
+
+ void NJointBimanualCCDMPController::runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&)
+ {
+ virtualtimer = cfg->timeDuration;
+
+ Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
+ Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
+
+ leftGroup.at(0)->prepareExecution(leftGroup.at(0)->eigen4f2vec(leftPose), leftGoals);
+ rightGroup.at(0)->prepareExecution(rightGroup.at(0)->eigen4f2vec(rightPose), rightGoals);
+
+
+ ARMARX_INFO << "leftgroup goal local pose: " << getLocalPose(leftGoals, rightGoals);
+
+ leftGroup.at(1)->prepareExecution(getLocalPose(leftPose, rightPose), getLocalPose(leftGoals, rightGoals));
+ rightGroup.at(1)->prepareExecution(getLocalPose(rightPose, leftPose), getLocalPose(rightGoals, leftGoals));
+
+ controllerTask->start();
+ }
+
+ Eigen::Matrix4f NJointBimanualCCDMPController::getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose)
+ {
+ Eigen::Matrix4f localPose;
+ localPose.block<3, 3>(0, 0) = globalTargetPose.block<3, 3>(0, 0) * newCoordinate.block<3, 3>(0, 0).inverse();
+ localPose.block<3, 1>(0, 3) = localPose.block<3, 3>(0, 0) * (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
+
+ return localPose;
+ }
+
+
+ void NJointBimanualCCDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+ {
+
+ StringVariantBaseMap datafields;
+ auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
+ for (auto& pair : values)
+ {
+ datafields[pair.first] = new Variant(pair.second);
+ }
+
+ auto constrained_force = debugDataInfo.getUpToDateReadBuffer().constrained_force;
+ for (auto& pair : constrained_force)
+ {
+ datafields[pair.first] = new Variant(pair.second);
+ }
+
+
+ datafields["leftTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_x);
+ datafields["leftTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_y);
+ datafields["leftTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_z);
+ datafields["rightTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_x);
+ datafields["rightTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_y);
+ datafields["rightTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_z);
+
+
+ datafields["leftCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_x);
+ datafields["leftCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_y);
+ datafields["leftCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_z);
+ datafields["rightCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_x);
+ datafields["rightCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_y);
+ datafields["rightCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_z);
+
+ // StringVariantBaseMap datafields;
+ // auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
+ // for (auto& pair : values)
+ // {
+ // datafields[pair.first] = new Variant(pair.second);
+ // }
+
+ // auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
+ // for (auto& pair : dmpTargets)
+ // {
+ // datafields[pair.first] = new Variant(pair.second);
+ // }
+
+ // auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
+ // for (auto& pair : currentPose)
+ // {
+ // datafields[pair.first] = new Variant(pair.second);
+ // }
+
+ // datafields["leaderCanVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderCanVal);
+ // datafields["leadermpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().leadermpcFactor);
+ // datafields["leaderError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leadererror);
+ // datafields["leaderposError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderposError);
+ // datafields["leaderoriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderoriError);
+
+ // datafields["followerCanVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().followerCanVal);
+ // datafields["followermpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().followermpcFactor);
+ // datafields["followerError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followererror);
+ // datafields["followerposError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followerposError);
+ // datafields["followeroriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followeroriError);
+
+ debugObs->setDebugChannel("DMPController", datafields);
+ }
+
+ void NJointBimanualCCDMPController::onInitComponent()
+ {
+ ARMARX_INFO << "init ...";
+ controllerTask = new PeriodicTask<NJointBimanualCCDMPController>(this, &NJointBimanualCCDMPController::controllerRun, 0.3);
+ }
+
+ void NJointBimanualCCDMPController::onDisconnectComponent()
+ {
+ controllerTask->stop();
+ ARMARX_INFO << "stopped ...";
+ }
+
+
+
+}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.h
new file mode 100644
index 0000000000000000000000000000000000000000..7b1e430c6f2de6615f4dce83e07027c19455a98a
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.h
@@ -0,0 +1,240 @@
+
+#pragma once
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <dmp/representation/dmp/umitsmp.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
+
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+
+#include <RobotAPI/libraries/core/PIDController.h>
+
+
+using namespace DMP;
+namespace armarx
+{
+
+
+ TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPController);
+ TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPControllerControlData);
+
+ typedef std::pair<double, DVec > ViaPoint;
+ typedef std::vector<ViaPoint > ViaPointsSet;
+ class NJointBimanualCCDMPControllerControlData
+ {
+ public:
+ Eigen::VectorXf leftTargetVel;
+ Eigen::VectorXf rightTargetVel;
+
+ Eigen::Matrix4f leftTargetPose;
+ Eigen::Matrix4f rightTargetPose;
+ };
+
+ class NJointBimanualCCDMPController :
+ public NJointControllerWithTripleBuffer<NJointBimanualCCDMPControllerControlData>,
+ public NJointBimanualCCDMPControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointBimanualCCDMPControllerConfigPtr;
+ NJointBimanualCCDMPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current&) const;
+
+ // NJointController interface
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ // NJointBimanualCCDMPControllerInterface interface
+ void learnDMPFromFiles(const std::string&, const Ice::StringSeq&, const Ice::Current&);
+ bool isFinished(const Ice::Current&)
+ {
+ return false;
+ }
+
+ void runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&);
+ void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
+ void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
+
+ void changeLeader(const Ice::Current&);
+
+ double getVirtualTime(const Ice::Current&)
+ {
+ return virtualtimer;
+ }
+
+ std::string getLeaderName(const Ice::Current&)
+ {
+ return leaderName;
+ }
+
+ protected:
+
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+ void onInitComponent();
+ void onDisconnectComponent();
+ void controllerRun();
+ private:
+
+ Eigen::VectorXf getControlWrench(const Eigen::VectorXf& twist, const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& targetPose);
+
+ Eigen::Matrix4f getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose);
+ Eigen::Matrix4f getLocalPose(const std::vector<double>& newCoordinateVec, const std::vector<double>& globalTargetPoseVec)
+ {
+ Eigen::Matrix4f newCoordinate = VirtualRobot::MathTools::quat2eigen4f(newCoordinateVec.at(4), newCoordinateVec.at(5), newCoordinateVec.at(6), newCoordinateVec.at(3));
+ newCoordinate(0, 3) = newCoordinateVec.at(0);
+ newCoordinate(1, 3) = newCoordinateVec.at(1);
+ newCoordinate(2, 3) = newCoordinateVec.at(2);
+
+ Eigen::Matrix4f globalTargetPose = VirtualRobot::MathTools::quat2eigen4f(globalTargetPoseVec.at(4), globalTargetPoseVec.at(5), globalTargetPoseVec.at(6), globalTargetPoseVec.at(3));
+ globalTargetPose(0, 3) = globalTargetPoseVec.at(0);
+ globalTargetPose(1, 3) = globalTargetPoseVec.at(1);
+ globalTargetPose(2, 3) = globalTargetPoseVec.at(2);
+
+ return getLocalPose(newCoordinate, globalTargetPose);
+
+ }
+
+ struct DebugBufferData
+ {
+ StringFloatDictionary desired_torques;
+ StringFloatDictionary constrained_force;
+ float leftTargetPose_x;
+ float leftTargetPose_y;
+ float leftTargetPose_z;
+ float rightTargetPose_x;
+ float rightTargetPose_y;
+ float rightTargetPose_z;
+
+ float leftCurrentPose_x;
+ float leftCurrentPose_y;
+ float leftCurrentPose_z;
+ float rightCurrentPose_x;
+ float rightCurrentPose_y;
+ float rightCurrentPose_z;
+
+ // StringFloatDictionary latestTargetVelocities;
+ // StringFloatDictionary dmpTargets;
+ // StringFloatDictionary currentPose;
+
+ // double leadermpcFactor;
+ // double leadererror;
+ // double leaderposError;
+ // double leaderoriError;
+ // double leaderCanVal;
+
+ // double followermpcFactor;
+ // double followererror;
+ // double followerposError;
+ // double followeroriError;
+ // double followerCanVal;
+
+
+ };
+
+ TripleBuffer<DebugBufferData> debugDataInfo;
+
+ struct NJointBimanualCCDMPControllerSensorData
+ {
+ double currentTime;
+ double deltaT;
+ Eigen::Matrix4f currentLeftPose;
+ Eigen::Matrix4f currentRightPose;
+ Eigen::VectorXf currentLeftTwist;
+ Eigen::VectorXf currentRightTwist;
+
+
+ };
+ TripleBuffer<NJointBimanualCCDMPControllerSensorData> controllerSensorData;
+
+
+ std::vector<ControlTarget1DoFActuatorTorque*> leftTargets;
+ std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
+ std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
+
+ std::vector<ControlTarget1DoFActuatorTorque*> rightTargets;
+ std::vector<const SensorValue1DoFActuatorAcceleration*> rightAccelerationSensors;
+ std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
+
+ const SensorValueForceTorque* rightForceTorque;
+ const SensorValueForceTorque* leftForceTorque;
+
+ NJointBimanualCCDMPControllerConfigPtr cfg;
+ VirtualRobot::DifferentialIKPtr leftIK;
+ VirtualRobot::DifferentialIKPtr rightIK;
+
+ std::vector<TaskSpaceDMPControllerPtr > leftGroup;
+ std::vector<TaskSpaceDMPControllerPtr > rightGroup;
+ std::vector<TaskSpaceDMPControllerPtr > bothLeaderGroup;
+
+
+ std::string leaderName;
+
+ VirtualRobot::RobotNodePtr tcpLeft;
+ VirtualRobot::RobotNodePtr tcpRight;
+
+ double virtualtimer;
+
+ mutable MutexType controllerMutex;
+ PeriodicTask<NJointBimanualCCDMPController>::pointer_type controllerTask;
+
+ Eigen::VectorXf leftDesiredJointValues;
+ Eigen::VectorXf rightDesiredJointValues;
+
+ float KoriFollower;
+ float KposFollower;
+ float DposFollower;
+ float DoriFollower;
+
+ Eigen::VectorXf forceC_des;
+ float boxWidth;
+
+ Eigen::Vector3f kpos;
+ Eigen::Vector3f kori;
+ Eigen::Vector3f dpos;
+ Eigen::Vector3f dori;
+ Eigen::VectorXf kpf;
+ Eigen::VectorXf kif;
+
+ float knull;
+ float dnull;
+
+ std::vector<std::string> leftJointNames;
+ std::vector<std::string> rightJointNames;
+
+ float torqueLimit;
+ VirtualRobot::RobotNodeSetPtr leftRNS;
+ VirtualRobot::RobotNodeSetPtr rightRNS;
+ VirtualRobot::RobotNodeSetPtr rnsLeftBody;
+ VirtualRobot::RobotNodeSetPtr rnsRightBody;
+
+ Eigen::Vector3f filtered_leftForce;
+ Eigen::Vector3f filtered_leftTorque;
+ Eigen::Vector3f filtered_rightForce;
+ Eigen::Vector3f filtered_rightTorque;
+ float filterTimeConstant;
+
+ std::vector<PIDControllerPtr> ftPIDController;
+
+ Eigen::Vector3f offset_leftForce;
+ Eigen::Vector3f offset_leftTorque;
+ Eigen::Vector3f offset_rightForce;
+ Eigen::Vector3f offset_rightTorque;
+
+ bool isForceCompensateDone;
+
+ // NJointBimanualCCDMPControllerInterface interface
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index 1a62a87186637a75fb9269e2b159aaf3f6b681c8..4ce687e3eb4680d31dcdcda379534ade1020e833 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -92,6 +92,12 @@ namespace armarx
initData.mode = ModeFromIce(cfg->mode);
reinitTripleBuffer(initData);
+ debugName = cfg->debugName;
+
+ KpF = cfg->Kp_LinearVel;
+ KoF = cfg->Kp_AngularVel;
+ DpF = cfg->Kd_LinearVel;
+ DoF = cfg->Kd_AngularVel;
}
@@ -111,49 +117,57 @@ namespace armarx
Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
taskSpaceDMPController->flow(deltaT, currentPose, currentTwist);
+
+ if (taskSpaceDMPController->canVal < 1e-8)
+ {
+ finished = true;
+ }
targetVels = taskSpaceDMPController->getTargetVelocity();
- std::vector<double> targetState = taskSpaceDMPController->getTargetPose();
-
-
- debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels[0];
- debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels[1];
- debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels[2];
- debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels[3];
- debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels[4];
- debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels[5];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_x"] = targetState[0];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_y"] = targetState[1];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_z"] = targetState[2];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_qw"] = targetState[3];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_qx"] = targetState[4];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_qy"] = targetState[5];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_qz"] = targetState[6];
- debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
- debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
- debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
-
- VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
- debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
- debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
- debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
- debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
+ targetPose = taskSpaceDMPController->getTargetPoseMat();
+ // std::vector<double> targetState = taskSpaceDMPController->getTargetPose();
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
+ // debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
+ // debugOutputData.getWriteBuffer().dmpTargets["dmp_x"] = targetState[0];
+ // debugOutputData.getWriteBuffer().dmpTargets["dmp_y"] = targetState[1];
+ // debugOutputData.getWriteBuffer().dmpTargets["dmp_z"] = targetState[2];
+ // debugOutputData.getWriteBuffer().dmpTargets["dmp_qw"] = targetState[3];
+ // debugOutputData.getWriteBuffer().dmpTargets["dmp_qx"] = targetState[4];
+ // debugOutputData.getWriteBuffer().dmpTargets["dmp_qy"] = targetState[5];
+ // debugOutputData.getWriteBuffer().dmpTargets["dmp_qz"] = targetState[6];
+ // debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
+ // debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
+ // debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
+
+ // VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
+ // debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
+ // debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
+ // debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
+ // debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
debugOutputData.getWriteBuffer().currentCanVal = taskSpaceDMPController->debugData.canVal;
- debugOutputData.getWriteBuffer().mpcFactor = taskSpaceDMPController->debugData.mpcFactor;
- debugOutputData.getWriteBuffer().error = taskSpaceDMPController->debugData.poseError;
- debugOutputData.getWriteBuffer().posError = taskSpaceDMPController->debugData.posiError;
- debugOutputData.getWriteBuffer().oriError = taskSpaceDMPController->debugData.oriError;
+ // debugOutputData.getWriteBuffer().mpcFactor = taskSpaceDMPController->debugData.mpcFactor;
+ // debugOutputData.getWriteBuffer().error = taskSpaceDMPController->debugData.poseError;
+ // debugOutputData.getWriteBuffer().posError = taskSpaceDMPController->debugData.posiError;
+ // debugOutputData.getWriteBuffer().oriError = taskSpaceDMPController->debugData.oriError;
+ // debugOutputData.getWriteBuffer().deltaT = deltaT;
+
debugOutputData.commitWrite();
getWriterControlStruct().targetTSVel = targetVels;
+ getWriterControlStruct().targetPose = targetPose;
+
writeControlStruct();
}
void NJointTSDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
- Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
+ Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
double deltaT = timeSinceLastIteration.toSecondsDouble();
Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
@@ -167,7 +181,7 @@ namespace armarx
Eigen::VectorXf tcptwist = jacobi * qvel;
- controllerSensorData.getWriteBuffer().currentPose = pose;
+ controllerSensorData.getWriteBuffer().currentPose = currentPose;
controllerSensorData.getWriteBuffer().currentTwist = tcptwist;
controllerSensorData.getWriteBuffer().deltaT = deltaT;
controllerSensorData.getWriteBuffer().currentTime += deltaT;
@@ -175,17 +189,29 @@ namespace armarx
Eigen::VectorXf targetVel = rtGetControlStruct().targetTSVel;
+ Eigen::Matrix4f targetPose = rtGetControlStruct().targetPose;
- float normLinearVelocity = targetVel.block<3, 1>(0, 0).norm();
+
+ Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
+ Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+
+
+ Eigen::VectorXf rtTargetVel;
+ rtTargetVel.resize(6);
+ rtTargetVel.block<3, 1>(0, 0) = KpF * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) + DpF * (targetVel.block<3, 1>(0, 0) - tcptwist.block<3, 1>(0, 0));
+ rtTargetVel.block<3, 1>(3, 0) = KoF * errorRPY + DoF * (targetVel.block<3, 1>(3, 0) - tcptwist.block<3, 1>(3, 0));
+
+
+ float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
if (normLinearVelocity > cfg->maxLinearVel)
{
- targetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * targetVel.block<3, 1>(0, 0) / normLinearVelocity;
+ rtTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rtTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
}
- float normAngularVelocity = targetVel.block<3, 1>(3, 0).norm();
+ float normAngularVelocity = rtTargetVel.block<3, 1>(3, 0).norm();
if (normAngularVelocity > cfg->maxAngularVel)
{
- targetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * targetVel.block<3, 1>(3, 0) / normAngularVelocity;
+ rtTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rtTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
}
@@ -198,7 +224,7 @@ namespace armarx
x.resize(6);
for (size_t i = 0; i < 6; i++)
{
- x(i) = targetVel(i);
+ x(i) = rtTargetVel(i);
}
}
else if (mode == VirtualRobot::IKSolver::Position)
@@ -206,7 +232,7 @@ namespace armarx
x.resize(3);
for (size_t i = 0; i < 3; i++)
{
- x(i) = targetVel(i);
+ x(i) = rtTargetVel(i);
}
}
@@ -215,11 +241,12 @@ namespace armarx
x.resize(3);
for (size_t i = 0; i < 3; i++)
{
- x(i) = targetVel(i + 3);
+ x(i) = rtTargetVel(i + 3);
}
}
else
{
+
// No mode has been set
return;
}
@@ -250,6 +277,8 @@ namespace armarx
{
targets.at(i)->velocity = jointTargetVelocities(i);
}
+
+
}
@@ -349,34 +378,44 @@ namespace armarx
void NJointTSDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
{
+ std::string datafieldName = debugName;
StringVariantBaseMap datafields;
- auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
- for (auto& pair : values)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
- for (auto& pair : dmpTargets)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
- for (auto& pair : currentPose)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
- datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
- datafields["poseError"] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
- datafields["phaseStop"] = new Variant(debugOutputData.getUpToDateReadBuffer().phaseStop);
- datafields["posError"] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
- datafields["oriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
- datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
- debugObs->setDebugChannel("DMPController", datafields);
+ // auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
+ // for (auto& pair : values)
+ // {
+ // datafieldName = pair.first + "_" + debugName;
+ // datafields[datafieldName] = new Variant(pair.second);
+ // }
+
+ // auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
+ // for (auto& pair : dmpTargets)
+ // {
+ // datafieldName = pair.first + "_" + debugName;
+ // datafields[datafieldName] = new Variant(pair.second);
+ // }
+
+ // auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
+ // for (auto& pair : currentPose)
+ // {
+ // datafieldName = pair.first + "_" + debugName;
+ // datafields[datafieldName] = new Variant(pair.second);
+ // }
+
+ datafieldName = "canVal_" + debugName;
+ datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
+ // datafieldName = "mpcFactor_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
+ // datafieldName = "error_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
+ // datafieldName = "posError_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
+ // datafieldName = "oriError_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
+ // datafieldName = "deltaT_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
+ // datafieldName = "DMPController_" + debugName;
+
+ debugObs->setDebugChannel(datafieldName, datafields);
}
void NJointTSDMPController::onInitComponent()
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index a70f5a2962b929399e8a1a0f4e24d63a0a425687..c3473908f7d29783ea091533c8e86525eb0b7a6f 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -27,6 +27,7 @@ namespace armarx
{
public:
Eigen::VectorXf targetTSVel;
+ Eigen::Matrix4f targetPose;
// cartesian velocity control data
std::vector<float> nullspaceJointVelocities;
float avoidJointLimitsKp = 0;
@@ -81,7 +82,7 @@ namespace armarx
void setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&);
void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&);
- double getCanVal(const Ice::Current &)
+ double getCanVal(const Ice::Current&)
{
return taskSpaceDMPController->canVal;
}
@@ -143,13 +144,19 @@ namespace armarx
VirtualRobot::DifferentialIKPtr ik;
VirtualRobot::RobotNodePtr tcp;
Eigen::VectorXf targetVels;
+ Eigen::Matrix4f targetPose;
NJointTaskSpaceDMPControllerConfigPtr cfg;
mutable MutexType controllerMutex;
PeriodicTask<NJointTSDMPController>::pointer_type controllerTask;
- // NJointTaskSpaceDMPControllerInterface interface
+ std::string debugName;
+
+ float KpF;
+ float DpF;
+ float KoF;
+ float DoF;
};
} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index dc91c6a504edfc82e05c44ed64af3367e00ade53..76beb836204971595ff2b84c4b2f973ea488bb09 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -41,6 +41,9 @@ set(LIB_FILES
math/TimeSeriesUtils.cpp
CartesianVelocityController.cpp
CartesianPositionController.cpp
+ CartesianVelocityRamp.cpp
+ JointVelocityRamp.cpp
+ CartesianVelocityControllerWithRamp.cpp
)
set(LIB_HEADERS
@@ -77,6 +80,10 @@ set(LIB_HEADERS
math/TimeSeriesUtils.h
CartesianVelocityController.h
CartesianPositionController.h
+ CartesianVelocityRamp.h
+ JointVelocityRamp.h
+ CartesianVelocityControllerWithRamp.h
+ EigenHelpers.h
)
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index 3e7765206e1dfd0375da9bc64c2a3f1fc326fdfd..d873e26c9078764965ba7f49f3f95de7ee63bcb9 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -37,9 +37,17 @@ CartesianVelocityController::CartesianVelocityController(const VirtualRobot::Rob
ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
this->tcp = tcp ? tcp : rns->getTCP();
+ this->cartesianMMRegularization = 100;
+ this->cartesianRadianRegularization = 1;
}
-Eigen::VectorXf CartesianVelocityController::calculate(Eigen::VectorXf cartesianVel, Eigen::VectorXf nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode)
+Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, float KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection mode)
+{
+ Eigen::VectorXf nullspaceVel = calculateNullspaceVelocity(cartesianVel, KpJointLimitAvoidanceScale, mode);
+ return calculate(cartesianVel, nullspaceVel, mode);
+}
+
+Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode)
{
//ARMARX_IMPORTANT << VAROUT(rns.get());
//ARMARX_IMPORTANT << VAROUT(tcp.get());
@@ -52,11 +60,11 @@ Eigen::VectorXf CartesianVelocityController::calculate(Eigen::VectorXf cartesian
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 kernel = lu_decomp.kernel();
- Eigen::VectorXf nsv = Eigen::VectorXf::Zero(kernel.rows());
- for (int i = 0; i < kernel.cols(); i++)
+ Eigen::MatrixXf nullspace = lu_decomp.kernel();
+ Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
+ for (int i = 0; i < nullspace.cols(); i++)
{
- nsv += kernel.col(i) * kernel.col(i).dot(nullspaceVel) / kernel.col(i).squaredNorm();
+ nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
}
//nsv /= kernel.cols();
@@ -85,3 +93,46 @@ Eigen::VectorXf CartesianVelocityController::calculateJointLimitAvoidance()
}
return r;
}
+
+Eigen::VectorXf CartesianVelocityController::calculateNullspaceVelocity(const Eigen::VectorXf& cartesianVel, float KpScale, VirtualRobot::IKSolver::CartesianSelection mode)
+{
+ Eigen::VectorXf regularization = calculateRegularization(mode);
+ Eigen::VectorXf vel = cartesianVel * regularization;
+ return KpScale * vel.norm() * calculateJointLimitAvoidance();
+}
+
+void CartesianVelocityController::setCartesianRegularization(float cartesianMMRegularization, float cartesianRadianRegularization)
+{
+ this->cartesianMMRegularization = cartesianMMRegularization;
+ this->cartesianRadianRegularization = cartesianRadianRegularization;
+}
+
+Eigen::VectorXf CartesianVelocityController::calculateRegularization(VirtualRobot::IKSolver::CartesianSelection mode)
+{
+ Eigen::VectorXf regularization(6);
+
+ int i = 0;
+
+ if (mode & VirtualRobot::IKSolver::X)
+ {
+ regularization(i++) = 1 / cartesianMMRegularization;
+ }
+
+ if (mode & VirtualRobot::IKSolver::Y)
+ {
+ regularization(i++) = 1 / cartesianMMRegularization;
+ }
+
+ if (mode & VirtualRobot::IKSolver::Z)
+ {
+ regularization(i++) = 1 / cartesianMMRegularization;
+ }
+
+ if (mode & VirtualRobot::IKSolver::Orientation)
+ {
+ regularization(i++) = 1 / cartesianRadianRegularization;
+ regularization(i++) = 1 / cartesianRadianRegularization;
+ regularization(i++) = 1 / cartesianRadianRegularization;
+ }
+ return regularization.topRows(i);
+}
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.h b/source/RobotAPI/libraries/core/CartesianVelocityController.h
index e55ec62c33b48b83cba6c1131750b5eb9e6e7b61..66fbcf4bb8ddc7462e2cc087a811cbb5971ff076 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.h
@@ -41,16 +41,24 @@ namespace armarx
public:
CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp = nullptr);
- Eigen::VectorXf calculate(Eigen::VectorXf cartesianVel, Eigen::VectorXf nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode);
+ Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, float KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection mode);
+ Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode);
Eigen::VectorXf calculateJointLimitAvoidance();
+ Eigen::VectorXf calculateNullspaceVelocity(const Eigen::VectorXf& cartesianVel, float KpScale, VirtualRobot::IKSolver::CartesianSelection mode);
+
+ void setCartesianRegularization(float cartesianMMRegularization, float cartesianRadianRegularization);
Eigen::MatrixXf jacobi;
Eigen::MatrixXf inv;
VirtualRobot::RobotNodeSetPtr rns;
-
- private:
VirtualRobot::DifferentialIKPtr ik;
VirtualRobot::RobotNodePtr tcp;
+
+ private:
+ Eigen::VectorXf calculateRegularization(VirtualRobot::IKSolver::CartesianSelection mode);
+
+ float cartesianMMRegularization;
+ float cartesianRadianRegularization;
};
}
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..264db7d88a2101edcf3d674e3ccfa4abbfae8734
--- /dev/null
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
@@ -0,0 +1,83 @@
+/*
+ * 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 Sonja Marahrens (sonja dot marahrens)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "CartesianVelocityControllerWithRamp.h"
+
+using namespace armarx;
+
+CartesianVelocityControllerWithRamp::CartesianVelocityControllerWithRamp(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode,
+ float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration, const VirtualRobot::RobotNodePtr& tcp)
+ : controller(rns, tcp), mode(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();
+ 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);
+}
+
+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(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
new file mode 100644
index 0000000000000000000000000000000000000000..a32b443a67476363b1456cfa583cc99eb7deb549
--- /dev/null
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
@@ -0,0 +1,69 @@
+/*
+ * 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 Sonja Marahrens (sonja dot marahrens)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "CartesianVelocityController.h"
+#include "CartesianVelocityRamp.h"
+#include "JointVelocityRamp.h"
+
+#include <boost/shared_ptr.hpp>
+
+#include <VirtualRobot/RobotNodeSet.h>
+
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Robot.h>
+#include <Eigen/Dense>
+
+
+namespace armarx
+{
+ class CartesianVelocityControllerWithRamp;
+ typedef boost::shared_ptr<CartesianVelocityControllerWithRamp> CartesianVelocityControllerWithRampPtr;
+
+ class CartesianVelocityControllerWithRamp
+ {
+ public:
+ CartesianVelocityControllerWithRamp(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode,
+ 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);
+
+ void setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration);
+
+ float getMaxOrientationAcceleration();
+ float getMaxPositionAcceleration();
+ float getMaxNullspaceAcceleration();
+
+
+ CartesianVelocityController controller;
+ private:
+ VirtualRobot::IKSolver::CartesianSelection mode;
+ CartesianVelocityRamp cartesianVelocityRamp;
+ JointVelocityRamp nullSpaceVelocityRamp;
+ };
+}
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b5ed838bd322bd78581b68c52328454d3556dcb9
--- /dev/null
+++ b/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
@@ -0,0 +1,86 @@
+/*
+ * 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 Sonja Marahrens (sonja dot marahrens)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "CartesianVelocityRamp.h"
+
+using namespace armarx;
+
+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 ;
+
+ if (mode & VirtualRobot::IKSolver::Position)
+ {
+ Eigen::Vector3f posDelta = delta.block<3, 1>(i, 0);
+ float posFactor = posDelta.norm() / maxPositionAcceleration / dt;
+ factor = std::max(factor, posFactor);
+ i += 3;
+ }
+
+ if (mode & VirtualRobot::IKSolver::Orientation)
+ {
+ Eigen::Vector3f oriDelta = delta.block<3, 1>(i, 0);
+ float oriFactor = oriDelta.norm() / maxOrientationAcceleration / dt;
+ factor = std::max(factor, oriFactor);
+ }
+
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..51abe44bc22a8647062397e9ee2fb123129ae7c4
--- /dev/null
+++ b/source/RobotAPI/libraries/core/CartesianVelocityRamp.h
@@ -0,0 +1,59 @@
+/*
+ * 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 Sonja Marahrens (sonja dot marahrens)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Robot.h>
+#include <Eigen/Dense>
+
+namespace armarx
+{
+ class CartesianVelocityRamp;
+ typedef boost::shared_ptr<CartesianVelocityRamp> CartesianVelocityRampPtr;
+
+ class CartesianVelocityRamp
+ {
+ public:
+ 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 = 0;
+ float maxOrientationAcceleration = 0;
+
+ Eigen::VectorXf state = Eigen::VectorXf::Zero(6);
+ VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::CartesianSelection::All;
+
+
+ };
+}
diff --git a/source/RobotAPI/libraries/core/EigenHelpers.h b/source/RobotAPI/libraries/core/EigenHelpers.h
new file mode 100644
index 0000000000000000000000000000000000000000..742bcca37d57df9541eebf9eb508197a46bed251
--- /dev/null
+++ b/source/RobotAPI/libraries/core/EigenHelpers.h
@@ -0,0 +1,117 @@
+/*
+ * 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 <eigen3/Eigen/Core>
+
+namespace armarx
+{
+
+ template<class ScalarType>
+ inline Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> MakeVectorX(std::initializer_list<ScalarType> ilist)
+ {
+ Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> r(ilist.size());
+ std::size_t i = 0;
+ for (const auto e : ilist)
+ {
+ r(i++) = e;
+ }
+ return r;
+ }
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(std::initializer_list<float> ilist)
+ {
+ return MakeVectorX<float>(ilist);
+ }
+
+ template<class ScalarType, class...Ts>
+ inline Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> MakeVectorXWarnNarrowing(Ts&& ...ts)
+ {
+ return MakeVectorX<ScalarType>(std::initializer_list<ScalarType> {std::forward<Ts>(ts)...});
+ }
+ template<class ScalarType, class...Ts>
+ inline Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> MakeVectorXIgnoreNarrowing(Ts&& ...ts)
+ {
+ return MakeVectorX<ScalarType>(std::initializer_list<ScalarType> {static_cast<ScalarType>(std::forward<Ts>(ts))...});
+ }
+ template<class ScalarType, class...Ts>
+ inline Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> MakeVectorX(Ts&& ...ts)
+ {
+ return MakeVectorX<ScalarType>(std::initializer_list<ScalarType> {static_cast<ScalarType>(std::forward<Ts>(ts))...});
+ }
+
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(std::vector<float> vec)
+ {
+ Eigen::Matrix<float, Eigen::Dynamic, 1> r(vec.size());
+ std::size_t i = 0;
+ for (const auto e : vec)
+ {
+ r(i++) = e;
+ }
+ return r;
+ }
+
+ /* Qt-Creator does not support variable amount of parameters. The following methods are only for Qt-Creator. */
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1)
+ {
+ return MakeVectorX<float>(f1);
+ }
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1, float f2)
+ {
+ return MakeVectorX<float>(f1, f2);
+ }
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1, float f2, float f3)
+ {
+ return MakeVectorX<float>(f1, f2, f3);
+ }
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1, float f2, float f3, float f4)
+ {
+ return MakeVectorX<float>(f1, f2, f3, f4);
+ }
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1, float f2, float f3, float f4, float f5)
+ {
+ return MakeVectorX<float>(f1, f2, f3, f4, f5);
+ }
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1, float f2, float f3, float f4, float f5, float f6)
+ {
+ return MakeVectorX<float>(f1, f2, f3, f4, f5, f6);
+ }
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1, float f2, float f3, float f4, float f5, float f6, float f7)
+ {
+ return MakeVectorX<float>(f1, f2, f3, f4, f5, f6, f7);
+ }
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1, float f2, float f3, float f4, float f5, float f6, float f7, float f8)
+ {
+ return MakeVectorX<float>(f1, f2, f3, f4, f5, f6, f7, f8);
+ }
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1, float f2, float f3, float f4, float f5, float f6, float f7, float f8, float f9)
+ {
+ return MakeVectorX<float>(f1, f2, f3, f4, f5, f6, f7, f8, f9);
+ }
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1, float f2, float f3, float f4, float f5, float f6, float f7, float f8, float f9, float f10)
+ {
+ return MakeVectorX<float>(f1, f2, f3, f4, f5, f6, f7, f8, f9, f10);
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/core/JointVelocityRamp.cpp b/source/RobotAPI/libraries/core/JointVelocityRamp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f0488c9e3e4c743b1add06c1e9bbe13cf7a92ebc
--- /dev/null
+++ b/source/RobotAPI/libraries/core/JointVelocityRamp.cpp
@@ -0,0 +1,60 @@
+/*
+ * 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 Sonja Marahrens (sonja dot marahrens)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "JointVelocityRamp.h"
+
+using namespace armarx;
+
+
+JointVelocityRamp::JointVelocityRamp()
+{ }
+
+void JointVelocityRamp::setState(const Eigen::VectorXf& state)
+{
+ this->state = state;
+}
+
+Eigen::VectorXf JointVelocityRamp::update(const Eigen::VectorXf& target, float dt)
+{
+
+ Eigen::VectorXf delta = target - state;
+
+ float factor = delta.norm() / maxAcceleration;
+ factor = std::max(factor, 1.f);
+
+ state += delta / factor * dt;
+ return state;
+
+}
+
+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
new file mode 100644
index 0000000000000000000000000000000000000000..c31b4a4c2a5448653c7a7f7bbe2dd095f9573b2a
--- /dev/null
+++ b/source/RobotAPI/libraries/core/JointVelocityRamp.h
@@ -0,0 +1,51 @@
+/*
+ * 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 Sonja Marahrens (sonja dot marahrens)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+#include <Eigen/Dense>
+
+namespace armarx
+{
+ class JointVelocityRamp;
+ typedef boost::shared_ptr<JointVelocityRamp> JointVelocityRampPtr;
+
+ class JointVelocityRamp
+ {
+ public:
+ 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;
+ float maxAcceleration;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/core/math/ColorUtils.h b/source/RobotAPI/libraries/core/math/ColorUtils.h
index fb79987b2e9ba827e23cdcef58696b259628d984..6bbb60e30da1f2576192e6427f6e9a08d99de926 100644
--- a/source/RobotAPI/libraries/core/math/ColorUtils.h
+++ b/source/RobotAPI/libraries/core/math/ColorUtils.h
@@ -95,9 +95,9 @@ namespace armarx
HsvColor RgbToHsv(const DrawColor24Bit& in)
{
- double r = in.r * 0.00392156862;
- double g = in.g * 0.00392156862;
- double b = in.b * 0.00392156862;
+ double r = 0.00392156862 * in.r;
+ double g = 0.00392156862 * in.g;
+ double b = 0.00392156862 * in.b;
HsvColor out;
double min, max, delta;
diff --git a/source/RobotAPI/libraries/core/test/CMakeLists.txt b/source/RobotAPI/libraries/core/test/CMakeLists.txt
index 38dff717efe933335499c980fc31a8efd8efd9cc..a78cfe8edfe93fb1a6c2c22258f8e44c77f7a6d7 100644
--- a/source/RobotAPI/libraries/core/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/test/CMakeLists.txt
@@ -5,3 +5,6 @@ armarx_add_test(PIDControllerTest PIDControllerTest.cpp "${LIBS}")
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
new file mode 100644
index 0000000000000000000000000000000000000000..5bfd4e36b8216ae4367ea582e18f382e1d3cb06f
--- /dev/null
+++ b/source/RobotAPI/libraries/core/test/CartesianVelocityRampTest.cpp
@@ -0,0 +1,53 @@
+/*
+* 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::CartesianVelocityRamp::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 "../CartesianVelocityRamp.h"
+#include <RobotAPI/libraries/core/EigenHelpers.h>
+
+using namespace armarx;
+
+
+
+BOOST_AUTO_TEST_CASE(testBoth)
+{
+ Eigen::VectorXf state = MakeVectorXf(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 = MakeVectorXf(100, 0, 0, 0, 0, 0);
+ state = c.update(target, dt);
+
+ Eigen::VectorXf expected = MakeVectorXf(10, 0, 0, 0, 0, 0);
+ BOOST_CHECK_LE((state - expected).norm(), 0.01f);
+
+}