diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index fc0818d1964a1fabf84a6591d75e623f4be82a67..954fd93541db100ecf455544a6c62ac8169d7a02 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -56,7 +56,9 @@ set(SLICE_FILES
units/RobotUnit/NJointJointSpaceDMPController.ice
units/RobotUnit/NJointTaskSpaceDMPController.ice
- units/RobotUnit/NJointBimanualForceMPController.ice
+ units/RobotUnit/NJointBimanualForceMPController.ice
+
+ units/RobotUnit/DSControllerBase.ice
units/RobotUnit/TaskSpaceActiveImpedanceControl.ice
components/ViewSelectionInterface.ice
diff --git a/source/RobotAPI/interface/units/RobotUnit/DSControllerBase.ice b/source/RobotAPI/interface/units/RobotUnit/DSControllerBase.ice
new file mode 100644
index 0000000000000000000000000000000000000000..1ae23199e5281780ec263c0ec62b0dbf3e6faabf
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotUnit/DSControllerBase.ice
@@ -0,0 +1,137 @@
+/*
+ * 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 DSControllerConfig extends NJointControllerConfig
+ {
+ float posiKp = 5;
+ float v_max = 0.15;
+ float posiDamping = 10;
+
+ float oriDamping;
+ float oriKp;
+
+ float filterTimeConstant = 0.01;
+ float torqueLimit = 1;
+
+
+ string nodeSetName = "";
+ string tcpName = "";
+
+ Ice::FloatSeq desiredPosition;
+ Ice::FloatSeq desiredQuaternion;
+
+ Ice::FloatSeq qnullspaceVec;
+
+ float nullspaceKp;
+ float nullspaceDamping;
+
+
+ Ice::StringSeq gmmParamsFiles;
+ float positionErrorTolerance;
+
+ float dsAdaptorEpsilon;
+
+ };
+
+ interface DSControllerInterface extends NJointControllerInterface
+ {
+
+ };
+
+ class DSRTBimanualControllerConfig extends NJointControllerConfig
+ {
+ float posiKp = 5;
+ float v_max = 0.15;
+ Ice::FloatSeq posiDamping;
+ float couplingStiffness = 10;
+ float couplingForceLimit = 50;
+
+ float forwardGain = 1;
+
+ float oriDamping;
+ float oriKp;
+
+ float filterTimeConstant = 0.01;
+ float torqueLimit = 1;
+ float NullTorqueLimit = 0.2;
+
+// string tcpName = "";
+
+// Ice::FloatSeq desiredPosition;
+
+ Ice::FloatSeq left_desiredQuaternion;
+ Ice::FloatSeq right_desiredQuaternion;
+
+ Ice::FloatSeq leftarm_qnullspaceVec;
+ Ice::FloatSeq rightarm_qnullspaceVec;
+
+ float nullspaceKp;
+ float nullspaceDamping;
+
+
+
+ string gmmParamsFile = "";
+ float positionErrorTolerance;
+
+ float contactForce;
+ float guardTargetZUp;
+ float guardTargetZDown;
+ float loweringForce;
+ float liftingForce;
+ bool guardDesiredDirection;
+ float highPassFilterFactor;
+
+
+ Ice::FloatSeq left_oriUp;
+ Ice::FloatSeq left_oriDown;
+ Ice::FloatSeq right_oriUp;
+ Ice::FloatSeq right_oriDown;
+ float forceFilterCoeff;
+ float forceErrorZDisturbance;
+ float forceErrorZThreshold;
+ float forceErrorZGain;
+ float desiredTorqueDisturbance;
+ float TorqueFilterConstant;
+ float leftForceOffsetZ;
+ float rightForceOffsetZ;
+ float contactDistanceTolerance;
+ float mountingDistanceTolerance;
+ float mountingCorrectionFilterFactor;
+
+ Ice::FloatSeq forceLeftOffset;
+ Ice::FloatSeq forceRightOffset;
+ };
+
+
+ interface DSBimanualControllerInterface extends NJointControllerInterface
+ {
+ void setToDefaultTarget();
+
+ };
+
+};
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index d5d62998ce906e8e70412f60e13afb01478f16c3..fcf59654d38bcc0273e36a0145391753d43bee91 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -8,5 +8,6 @@ add_subdirectory(RobotAPIVariantWidget)
add_subdirectory(RobotAPINJointControllers)
add_subdirectory(DMPController)
+add_subdirectory(DSControllers)
add_subdirectory(RobotStatechartHelpers)
diff --git a/source/RobotAPI/libraries/DSControllers/CMakeLists.txt b/source/RobotAPI/libraries/DSControllers/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..cd6450365d5d569b5b64c95850220060389824a3
--- /dev/null
+++ b/source/RobotAPI/libraries/DSControllers/CMakeLists.txt
@@ -0,0 +1,53 @@
+set(LIB_NAME DSControllers)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+find_package(Eigen3 QUIET)
+find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
+
+find_package(MATHLIB QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+if (Eigen3_FOUND AND Simox_FOUND)
+ include_directories(${Simox_INCLUDE_DIRS})
+ include_directories(SYSTEM ${Eigen3_INCLUDE_DIR})
+endif()
+
+include_directories(${MATHLIB_INCLUDE_DIRS})
+
+
+message(STATUS "mathlib is ${MATHLIB_LIBS}")
+set(LIBS ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreEigen3Variants
+ VirtualRobot
+ Saba
+ SimDynamics
+ RobotUnit
+ RobotAPIUnits
+ RobotAPICore
+ RobotAPIInterfaces
+ ${MATHLIB_LIB}
+)
+
+
+
+set(LIB_FILES
+./DSRTBimanualController.cpp
+./DSRTController.cpp
+./GMRDynamics.cpp
+./Gaussians.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(LIB_HEADERS
+./DSRTBimanualController.h
+./DSRTController.h
+./GMRDynamics.h
+./Gaussians.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+# add unit tests
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d338e827cdf26e1a8dea9ea4bdaa82fa75743a18
--- /dev/null
+++ b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
@@ -0,0 +1,1004 @@
+/*
+ * 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 DSController::ArmarXObjects::DSRTBimanualController
+ * @author Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "DSRTBimanualController.h"
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+using namespace armarx;
+
+NJointControllerRegistration<DSRTBimanualController> registrationControllerDSRTBimanualController("DSRTBimanualController");
+
+void DSRTBimanualController::onInitNJointController()
+{
+ ARMARX_INFO << "init ...";
+ controllerStopRequested = false;
+ controllerRunFinished = false;
+ runTask("DSRTBimanualControllerTask", [&]
+ {
+ CycleUtil c(1);
+ getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+ while (getState() == eManagedIceObjectStarted && !controllerStopRequested)
+ {
+ ARMARX_INFO << deactivateSpam(1) << "control fct";
+ if (isControllerActive())
+ {
+ controllerRun();
+ }
+ c.waitForCycleDuration();
+ }
+ controllerRunFinished = true;
+ ARMARX_INFO << "Control Fct finished";
+ });
+
+
+}
+
+void DSRTBimanualController::onDisconnectNJointController()
+{
+ ARMARX_INFO << "disconnect";
+ controllerStopRequested = true;
+ while (!controllerRunFinished)
+ {
+ ARMARX_INFO << deactivateSpam(1) << "waiting for run function";
+ usleep(10000);
+ }
+}
+
+
+DSRTBimanualController::DSRTBimanualController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+{
+ cfg = DSRTBimanualControllerConfigPtr::dynamicCast(config);
+ useSynchronizedRtRobot();
+
+ VirtualRobot::RobotNodeSetPtr left_ns = rtGetRobot()->getRobotNodeSet("LeftArm");
+ VirtualRobot::RobotNodeSetPtr right_ns = rtGetRobot()->getRobotNodeSet("RightArm");
+
+ left_jointNames.clear();
+ right_jointNames.clear();
+
+ ARMARX_CHECK_EXPRESSION_W_HINT(left_ns, "LeftArm");
+ ARMARX_CHECK_EXPRESSION_W_HINT(right_ns, "RightArm");
+
+ // for left arm
+ for (size_t i = 0; i < left_ns->getSize(); ++i)
+ {
+ std::string jointName = left_ns->getNode(i)->getName();
+ left_jointNames.push_back(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ ARMARX_CHECK_EXPRESSION(ct);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ ARMARX_CHECK_EXPRESSION(sv);
+ auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
+ ARMARX_CHECK_EXPRESSION(casted_ct);
+ left_torque_targets.push_back(casted_ct);
+
+ const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ if (!torqueSensor)
+ {
+ ARMARX_WARNING << "No Torque sensor available for " << jointName;
+ }
+ if (!gravityTorqueSensor)
+ {
+ ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
+ }
+
+ left_torqueSensors.push_back(torqueSensor);
+ left_gravityTorqueSensors.push_back(gravityTorqueSensor);
+ left_velocitySensors.push_back(velocitySensor);
+ left_positionSensors.push_back(positionSensor);
+ };
+
+ // for right arm
+ for (size_t i = 0; i < right_ns->getSize(); ++i)
+ {
+ std::string jointName = right_ns->getNode(i)->getName();
+ right_jointNames.push_back(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ ARMARX_CHECK_EXPRESSION(ct);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ ARMARX_CHECK_EXPRESSION(sv);
+ auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
+ ARMARX_CHECK_EXPRESSION(casted_ct);
+ right_torque_targets.push_back(casted_ct);
+
+ const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ if (!torqueSensor)
+ {
+ ARMARX_WARNING << "No Torque sensor available for " << jointName;
+ }
+ if (!gravityTorqueSensor)
+ {
+ ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
+ }
+
+ right_torqueSensors.push_back(torqueSensor);
+ right_gravityTorqueSensors.push_back(gravityTorqueSensor);
+ right_velocitySensors.push_back(velocitySensor);
+ right_positionSensors.push_back(positionSensor);
+ };
+
+
+ const SensorValueBase* svlf = useSensorValue("FT L");
+ leftForceTorque = svlf->asA<SensorValueForceTorque>();
+ const SensorValueBase* svrf = useSensorValue("FT R");
+ rightForceTorque = svrf->asA<SensorValueForceTorque>();
+
+ ARMARX_INFO << "Initialized " << left_torque_targets.size() << " targets for the left arm";
+ ARMARX_INFO << "Initialized " << right_torque_targets.size() << " targets for the right arm";
+
+ left_arm_tcp = left_ns->getTCP();
+ right_arm_tcp = right_ns->getTCP();
+
+ left_sensor_frame = left_ns->getRobot()->getRobotNode("ArmL8_Wri2");
+ right_sensor_frame = right_ns->getRobot()->getRobotNode("ArmR8_Wri2");
+
+ left_ik.reset(new VirtualRobot::DifferentialIK(left_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ right_ik.reset(new VirtualRobot::DifferentialIK(right_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+
+
+ // ?? not sure about this
+ DSRTBimanualControllerSensorData initSensorData;
+
+ initSensorData.left_tcpPose = left_arm_tcp->getPoseInRootFrame();
+ initSensorData.currentTime = 0;
+ controllerSensorData.reinitAllBuffers(initSensorData);
+
+ initSensorData.right_tcpPose = right_arm_tcp->getPoseInRootFrame();
+ initSensorData.currentTime = 0;
+ controllerSensorData.reinitAllBuffers(initSensorData);
+
+
+ left_oldPosition = left_arm_tcp->getPositionInRootFrame();
+ left_oldOrientation = left_arm_tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+
+ right_oldPosition = right_arm_tcp->getPositionInRootFrame();
+ right_oldOrientation = right_arm_tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+
+
+ std::vector<float> left_desiredQuaternionVec = cfg->left_desiredQuaternion;
+ ARMARX_CHECK_EQUAL(left_desiredQuaternionVec.size(), 4);
+
+ std::vector<float> right_desiredQuaternionVec = cfg->right_desiredQuaternion;
+ ARMARX_CHECK_EQUAL(right_desiredQuaternionVec.size(), 4);
+
+ left_desiredQuaternion.w() = left_desiredQuaternionVec.at(0);
+ left_desiredQuaternion.x() = left_desiredQuaternionVec.at(1);
+ left_desiredQuaternion.y() = left_desiredQuaternionVec.at(2);
+ left_desiredQuaternion.z() = left_desiredQuaternionVec.at(3);
+
+ right_desiredQuaternion.w() = right_desiredQuaternionVec.at(0);
+ right_desiredQuaternion.x() = right_desiredQuaternionVec.at(1);
+ right_desiredQuaternion.y() = right_desiredQuaternionVec.at(2);
+ right_desiredQuaternion.z() = right_desiredQuaternionVec.at(3);
+
+
+ // set initial joint velocities filter
+
+ left_jointVelocity_filtered.resize(left_torque_targets.size());
+ left_jointVelocity_filtered.setZero();
+ right_jointVelocity_filtered.resize(left_torque_targets.size());
+ right_jointVelocity_filtered.setZero();
+
+ // do we need to duplicate this?
+ DSRTBimanualControllerControlData initData;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ initData.left_tcpDesiredLinearVelocity(i) = 0;
+ initData.right_tcpDesiredLinearVelocity(i) = 0;
+
+ }
+
+ for (size_t i = 0; i < 3; ++i)
+ {
+ initData.left_tcpDesiredAngularError(i) = 0;
+ initData.right_tcpDesiredAngularError(i) = 0;
+
+ }
+ reinitTripleBuffer(initData);
+
+
+ // initial filter
+ left_desiredTorques_filtered.resize(left_torque_targets.size());
+ left_desiredTorques_filtered.setZero();
+ right_desiredTorques_filtered.resize(right_torque_targets.size());
+ right_desiredTorques_filtered.setZero();
+
+
+ left_currentTCPLinearVelocity_filtered.setZero();
+ right_currentTCPLinearVelocity_filtered.setZero();
+
+ left_currentTCPAngularVelocity_filtered.setZero();
+ right_currentTCPAngularVelocity_filtered.setZero();
+
+ left_tcpDesiredTorque_filtered.setZero();
+ right_tcpDesiredTorque_filtered.setZero();
+
+
+ smooth_startup = 0;
+
+
+ filterTimeConstant = cfg->filterTimeConstant;
+ posiKp = cfg->posiKp;
+ v_max = cfg->v_max;
+ Damping = cfg->posiDamping;
+ Coupling_stiffness = cfg->couplingStiffness;
+ Coupling_force_limit = cfg->couplingForceLimit;
+ forward_gain = cfg->forwardGain;
+ torqueLimit = cfg->torqueLimit;
+ null_torqueLimit = cfg->NullTorqueLimit;
+ oriKp = cfg->oriKp;
+ oriDamping = cfg->oriDamping;
+ contactForce = cfg->contactForce;
+
+ left_oriUp.w() = cfg->left_oriUp[0];
+ left_oriUp.x() = cfg->left_oriUp[1];
+ left_oriUp.y() = cfg->left_oriUp[2];
+ left_oriUp.z() = cfg->left_oriUp[3];
+
+ left_oriDown.w() = cfg->left_oriDown[0];
+ left_oriDown.x() = cfg->left_oriDown[1];
+ left_oriDown.y() = cfg->left_oriDown[2];
+ left_oriDown.z() = cfg->left_oriDown[3];
+
+ right_oriUp.w() = cfg->right_oriUp[0];
+ right_oriUp.x() = cfg->right_oriUp[1];
+ right_oriUp.y() = cfg->right_oriUp[2];
+ right_oriUp.z() = cfg->right_oriUp[3];
+
+ right_oriDown.w() = cfg->right_oriDown[0];
+ right_oriDown.x() = cfg->right_oriDown[1];
+ right_oriDown.y() = cfg->right_oriDown[2];
+ right_oriDown.z() = cfg->right_oriDown[3];
+
+
+ guardTargetZUp = cfg->guardTargetZUp;
+ guardTargetZDown = cfg->guardTargetZDown;
+ guardDesiredZ = guardTargetZUp;
+ guard_mounting_correction_z = 0;
+
+ guardXYHighFrequency = 0;
+ left_force_old.setZero();
+ right_force_old.setZero();
+
+ left_contactForceZ_correction = 0;
+ right_contactForceZ_correction = 0;
+
+
+ std::vector<float> leftarm_qnullspaceVec = cfg->leftarm_qnullspaceVec;
+ std::vector<float> rightarm_qnullspaceVec = cfg->rightarm_qnullspaceVec;
+
+ left_qnullspace.resize(leftarm_qnullspaceVec.size());
+ right_qnullspace.resize(rightarm_qnullspaceVec.size());
+
+ for (size_t i = 0; i < leftarm_qnullspaceVec.size(); ++i)
+ {
+ left_qnullspace(i) = leftarm_qnullspaceVec[i];
+ right_qnullspace(i) = rightarm_qnullspaceVec[i];
+ }
+
+ nullspaceKp = cfg->nullspaceKp;
+ nullspaceDamping = cfg->nullspaceDamping;
+
+
+ //set GMM parameters ...
+ if (cfg->gmmParamsFile == "")
+ {
+ ARMARX_ERROR << "gmm params file cannot be empty string ...";
+ }
+ gmmMotionGenerator.reset(new BimanualGMMMotionGen(cfg->gmmParamsFile));
+ positionErrorTolerance = cfg->positionErrorTolerance;
+ forceFilterCoeff = cfg->forceFilterCoeff;
+ for (size_t i = 0 ; i < 3; ++i)
+ {
+ leftForceOffset[i] = cfg->forceLeftOffset.at(i);
+ rightForceOffset[i] = cfg->forceRightOffset.at(i);
+ }
+ isDefaultTarget = false;
+ ARMARX_INFO << "Initialization done";
+}
+
+void DSRTBimanualController::controllerRun()
+{
+ if (!controllerSensorData.updateReadBuffer())
+ {
+ return;
+ }
+
+
+ // receive the measurements
+ Eigen::Matrix4f left_currentTCPPose = controllerSensorData.getReadBuffer().left_tcpPose;
+ Eigen::Matrix4f right_currentTCPPose = controllerSensorData.getReadBuffer().right_tcpPose;
+
+ Eigen::Vector3f left_force = controllerSensorData.getReadBuffer().left_force;
+ Eigen::Vector3f right_force = controllerSensorData.getReadBuffer().right_force;
+ Eigen::Vector3f both_arm_force_ave = - 0.5 * (left_force + right_force); // negative sign for the direction
+
+
+ float left_force_z = left_force(2);
+ float right_force_z = right_force(2);
+
+ Eigen::Vector3f left_currentTCPPositionInMeter;
+ Eigen::Vector3f right_currentTCPPositionInMeter;
+
+ left_currentTCPPositionInMeter << left_currentTCPPose(0, 3), left_currentTCPPose(1, 3), left_currentTCPPose(2, 3);
+ right_currentTCPPositionInMeter << right_currentTCPPose(0, 3), right_currentTCPPose(1, 3), right_currentTCPPose(2, 3);
+
+ left_currentTCPPositionInMeter = 0.001 * left_currentTCPPositionInMeter;
+ right_currentTCPPositionInMeter = 0.001 * right_currentTCPPositionInMeter;
+
+
+ // updating the desired height for the guard based on the interaction forces
+ float both_arm_height_z_ave = 0.5 * (left_currentTCPPositionInMeter(2) + right_currentTCPPositionInMeter(2));
+
+
+ float adaptive_ratio = 1;
+ float force_error_z = 0;
+ float guard_mounting_correction_x = 0;
+ float guard_mounting_correction_y = 0;
+
+
+ if (cfg->guardDesiredDirection)
+ {
+ adaptive_ratio = 1;
+ force_error_z = both_arm_force_ave(2) - adaptive_ratio * cfg->liftingForce;
+
+ // meausures the high-frequency components of the interaction forces
+ float diff_norm = (left_force - left_force_old).squaredNorm() + (right_force - right_force_old).squaredNorm();
+ // diff_norm = deadzone(diff_norm,0.1,2);
+ guardXYHighFrequency = cfg->highPassFilterFactor * guardXYHighFrequency + 0.1 * diff_norm;
+
+ guardXYHighFrequency = (guardXYHighFrequency > 10) ? 10 : guardXYHighFrequency;
+
+ left_force_old = left_force;
+ right_force_old = right_force;
+
+ if (fabs(both_arm_height_z_ave - guardTargetZUp) < cfg->mountingDistanceTolerance)
+ {
+ guard_mounting_correction_z = (1 - cfg->mountingCorrectionFilterFactor) * guard_mounting_correction_z + cfg->mountingCorrectionFilterFactor * (- 0.1 * (guardXYHighFrequency - 8));
+ guard_mounting_correction_z = deadzone(guard_mounting_correction_z, 0, 0.1);
+
+
+ guard_mounting_correction_x = guard_mounting_correction_x - cfg->forceErrorZGain * deadzone(both_arm_force_ave(0), 1, 3);
+ guard_mounting_correction_y = guard_mounting_correction_y - cfg->forceErrorZGain * deadzone(both_arm_force_ave(1), 1, 3);
+
+ guard_mounting_correction_x = deadzone(guard_mounting_correction_x, 0, 0.1);
+ guard_mounting_correction_y = deadzone(guard_mounting_correction_y, 0, 0.1);
+
+
+ }
+
+ }
+ else
+ {
+ adaptive_ratio = (0.5 * (both_arm_height_z_ave - guardTargetZDown) / (guardTargetZUp - guardTargetZDown) + 0.5);
+ force_error_z = both_arm_force_ave(2) - adaptive_ratio * cfg->loweringForce;
+ }
+
+
+
+ force_error_z = deadzone(force_error_z, cfg->forceErrorZDisturbance, cfg->forceErrorZThreshold);
+ guardDesiredZ = guardDesiredZ - cfg->forceErrorZGain * (force_error_z);
+
+
+ guardDesiredZ = (guardDesiredZ > guardTargetZUp) ? guardTargetZUp : guardDesiredZ;
+ guardDesiredZ = (guardDesiredZ < guardTargetZDown) ? guardTargetZDown : guardDesiredZ;
+
+ if(isDefaultTarget)
+ {
+ guardDesiredZ = guardTargetZDown;
+ }
+
+ // update the desired velocity
+ gmmMotionGenerator->updateDesiredVelocity(
+ left_currentTCPPositionInMeter,
+ right_currentTCPPositionInMeter,
+ positionErrorTolerance ,
+ guardDesiredZ,
+ guard_mounting_correction_x,
+ guard_mounting_correction_y,
+ guard_mounting_correction_z);
+
+ // ARMARX_INFO << "tcpDesiredLinearVelocity: " << gmmMotionGenerator->left_DS_DesiredVelocity;
+ // ARMARX_INFO << "tcpDesiredLinearVelocity: " << gmmMotionGenerator->right_DS_DesiredVelocity;
+
+
+ Eigen::Vector3f left_tcpDesiredAngularError;
+ Eigen::Vector3f right_tcpDesiredAngularError;
+
+ left_tcpDesiredAngularError << 0, 0, 0;
+ right_tcpDesiredAngularError << 0, 0, 0;
+
+
+
+ Eigen::Matrix3f left_currentRotMat = left_currentTCPPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f right_currentRotMat = right_currentTCPPose.block<3, 3>(0, 0);
+
+
+ float lqratio = (left_currentTCPPositionInMeter(2) - guardTargetZDown) / (guardTargetZUp - guardTargetZDown);
+ float rqratio = (right_currentTCPPositionInMeter(2) - guardTargetZDown) / (guardTargetZUp - guardTargetZDown);
+
+ lqratio = (lqratio > 1) ? 1 : lqratio;
+ lqratio = (lqratio < 0) ? 0 : lqratio;
+ rqratio = (rqratio > 1) ? 1 : rqratio;
+ rqratio = (rqratio < 0) ? 0 : rqratio;
+
+
+ Eigen::Quaternionf left_desiredQuaternion = quatSlerp(lqratio, left_oriDown, left_oriUp);
+ Eigen::Quaternionf right_desiredQuaternion = quatSlerp(rqratio, right_oriDown, right_oriUp);
+
+
+ Eigen::Matrix3f left_desiredRotMat = left_desiredQuaternion.normalized().toRotationMatrix();
+ Eigen::Matrix3f right_desiredRotMat = right_desiredQuaternion.normalized().toRotationMatrix();
+
+ Eigen::Matrix3f left_orientationError = left_currentRotMat * left_desiredRotMat.inverse();
+ Eigen::Matrix3f right_orientationError = right_currentRotMat * right_desiredRotMat.inverse();
+
+ Eigen::Quaternionf left_diffQuaternion(left_orientationError);
+ Eigen::Quaternionf right_diffQuaternion(right_orientationError);
+
+ Eigen::AngleAxisf left_angleAxis(left_diffQuaternion);
+ Eigen::AngleAxisf right_angleAxis(right_diffQuaternion);
+
+ left_tcpDesiredAngularError = left_angleAxis.angle() * left_angleAxis.axis();
+ right_tcpDesiredAngularError = right_angleAxis.angle() * right_angleAxis.axis();
+
+
+ // writing to the buffer
+ getWriterControlStruct().left_tcpDesiredLinearVelocity = gmmMotionGenerator->left_DS_DesiredVelocity;
+ getWriterControlStruct().right_tcpDesiredLinearVelocity = gmmMotionGenerator->right_DS_DesiredVelocity;
+ getWriterControlStruct().left_right_distanceInMeter = gmmMotionGenerator->left_right_position_errorInMeter;
+
+ getWriterControlStruct().left_tcpDesiredAngularError = left_tcpDesiredAngularError;
+ getWriterControlStruct().right_tcpDesiredAngularError = right_tcpDesiredAngularError;
+
+ writeControlStruct();
+ debugCtrlDataInfo.getWriteBuffer().desredZ = guardDesiredZ;
+ debugCtrlDataInfo.getWriteBuffer().force_error_z = force_error_z;
+ debugCtrlDataInfo.getWriteBuffer().guardXYHighFrequency = guardXYHighFrequency;
+ debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_x = guard_mounting_correction_x;
+ debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_y = guard_mounting_correction_y;
+ debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_z = guard_mounting_correction_z;
+ debugCtrlDataInfo.commitWrite();
+
+}
+
+
+void DSRTBimanualController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+{
+ // reading the control objectives from the other threads (desired velocities)
+ Eigen::Vector3f left_tcpDesiredLinearVelocity = rtGetControlStruct().left_tcpDesiredLinearVelocity;
+ Eigen::Vector3f right_tcpDesiredLinearVelocity = rtGetControlStruct().right_tcpDesiredLinearVelocity;
+ Eigen::Vector3f left_tcpDesiredAngularError = rtGetControlStruct().left_tcpDesiredAngularError;
+ Eigen::Vector3f right_tcpDesiredAngularError = rtGetControlStruct().right_tcpDesiredAngularError;
+ Eigen::Vector3f left_right_distanceInMeter = rtGetControlStruct().left_right_distanceInMeter;
+
+ double deltaT = timeSinceLastIteration.toSecondsDouble();
+
+
+ // measure the sesor data for the other threads
+
+ Eigen::Matrix4f leftSensorFrame = left_sensor_frame->getPoseInRootFrame();
+ Eigen::Matrix4f rightSensorFrame = right_sensor_frame->getPoseInRootFrame();
+
+ // ARMARX_IMPORTANT << "left force offset: " << leftForceOffset;
+ // ARMARX_IMPORTANT << "right force offset: " << rightForceOffset;
+
+ Eigen::Vector3f left_forceInRoot = leftSensorFrame.block<3, 3>(0, 0) * (leftForceTorque->force - leftForceOffset);
+ Eigen::Vector3f right_forceInRoot = rightSensorFrame.block<3, 3>(0, 0) * (rightForceTorque->force - rightForceOffset);
+// Eigen::Vector3f left_torqueInRoot = leftSensorFrame.block<3, 3>(0, 0) * leftForceTorque->torque;
+// Eigen::Vector3f right_torqueInRoot = rightSensorFrame.block<3, 3>(0, 0) * rightForceTorque->torque;
+ left_forceInRoot(2) = left_forceInRoot(2) + cfg->leftForceOffsetZ; // - 4 + 8.5;
+ right_forceInRoot(2) = right_forceInRoot(2) + cfg->rightForceOffsetZ; // + 30 - 5.2;
+
+
+ Eigen::Matrix4f left_currentTCPPose = left_arm_tcp->getPoseInRootFrame();
+ Eigen::Matrix4f right_currentTCPPose = right_arm_tcp->getPoseInRootFrame();
+
+
+ Eigen::MatrixXf left_jacobi = left_ik->getJacobianMatrix(left_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
+ Eigen::MatrixXf right_jacobi = right_ik->getJacobianMatrix(right_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
+
+ Eigen::VectorXf left_qpos;
+ Eigen::VectorXf left_qvel;
+
+ Eigen::VectorXf right_qpos;
+ Eigen::VectorXf right_qvel;
+
+ left_qpos.resize(left_positionSensors.size());
+ left_qvel.resize(left_velocitySensors.size());
+
+ right_qpos.resize(right_positionSensors.size());
+ right_qvel.resize(right_velocitySensors.size());
+
+ int jointDim = left_positionSensors.size();
+
+ for (size_t i = 0; i < left_velocitySensors.size(); ++i)
+ {
+ left_qpos(i) = left_positionSensors[i]->position;
+ left_qvel(i) = left_velocitySensors[i]->velocity;
+
+ right_qpos(i) = right_positionSensors[i]->position;
+ right_qvel(i) = right_velocitySensors[i]->velocity;
+ }
+
+ Eigen::VectorXf left_tcptwist = left_jacobi * left_qvel;
+ Eigen::VectorXf right_tcptwist = right_jacobi * right_qvel;
+
+ Eigen::Vector3f left_currentTCPLinearVelocity;
+ Eigen::Vector3f right_currentTCPLinearVelocity;
+ Eigen::Vector3f left_currentTCPAngularVelocity;
+ Eigen::Vector3f right_currentTCPAngularVelocity;
+ left_currentTCPLinearVelocity << 0.001 * left_tcptwist(0), 0.001 * left_tcptwist(1), 0.001 * left_tcptwist(2);
+ right_currentTCPLinearVelocity << 0.001 * right_tcptwist(0), 0.001 * right_tcptwist(1), 0.001 * right_tcptwist(2);
+ left_currentTCPAngularVelocity << left_tcptwist(3), left_tcptwist(4), left_tcptwist(5);
+ right_currentTCPAngularVelocity << right_tcptwist(3), right_tcptwist(4), right_tcptwist(5);
+ double filterFactor = deltaT / (filterTimeConstant + deltaT);
+ left_currentTCPLinearVelocity_filtered = (1 - filterFactor) * left_currentTCPLinearVelocity_filtered + filterFactor * left_currentTCPLinearVelocity;
+ right_currentTCPLinearVelocity_filtered = (1 - filterFactor) * right_currentTCPLinearVelocity_filtered + filterFactor * right_currentTCPLinearVelocity;
+ left_currentTCPAngularVelocity_filtered = (1 - filterFactor) * left_currentTCPAngularVelocity_filtered + left_currentTCPAngularVelocity;
+ right_currentTCPAngularVelocity_filtered = (1 - filterFactor) * right_currentTCPAngularVelocity_filtered + right_currentTCPAngularVelocity;
+ left_jointVelocity_filtered = (1 - filterFactor) * left_jointVelocity_filtered + filterFactor * left_qvel;
+ right_jointVelocity_filtered = (1 - filterFactor) * right_jointVelocity_filtered + filterFactor * right_qvel;
+
+ // writing into the bufffer for other threads
+ controllerSensorData.getWriteBuffer().left_tcpPose = left_currentTCPPose;
+ controllerSensorData.getWriteBuffer().right_tcpPose = right_currentTCPPose;
+ controllerSensorData.getWriteBuffer().left_force = left_forceInRoot;
+ controllerSensorData.getWriteBuffer().right_force = right_forceInRoot;
+ controllerSensorData.getWriteBuffer().currentTime += deltaT;
+ controllerSensorData.commitWrite();
+
+
+
+
+ // inverse dynamics:
+
+
+ // reading the tcp orientation
+
+
+
+
+ // computing the task-specific forces
+ Eigen::Vector3f left_DS_force = -(1.2 * left_currentTCPLinearVelocity_filtered - forward_gain * left_tcpDesiredLinearVelocity);
+ Eigen::Vector3f right_DS_force = -(1.2 * right_currentTCPLinearVelocity_filtered - forward_gain * right_tcpDesiredLinearVelocity);
+ for (int i = 0; i < 3; ++i)
+ {
+ left_DS_force(i) = left_DS_force(i) * Damping[i];
+ right_DS_force(i) = right_DS_force(i) * Damping[i];
+
+ left_DS_force(i) = deadzone(left_DS_force(i), 0.1, 100);
+ right_DS_force(i) = deadzone(right_DS_force(i), 0.1, 100);
+
+ }
+
+ // computing coupling forces
+ Eigen::Vector3f coupling_force = - Coupling_stiffness * left_right_distanceInMeter;
+ float coupling_force_norm = coupling_force.norm();
+
+ if (coupling_force_norm > Coupling_force_limit)
+ {
+ coupling_force = (Coupling_force_limit / coupling_force_norm) * coupling_force;
+ }
+
+ coupling_force(0) = deadzone(coupling_force(0), 0.1, 100);
+ coupling_force(1) = deadzone(coupling_force(1), 0.1, 100);
+ coupling_force(2) = deadzone(coupling_force(2), 0.1, 100);
+
+
+
+
+ double v_both = left_currentTCPLinearVelocity_filtered.norm() + right_currentTCPLinearVelocity_filtered.norm();
+ float force_contact_switch = 0;
+ float left_height = left_currentTCPPose(2, 3) * 0.001;
+ float right_height = right_currentTCPPose(2, 3) * 0.001;
+
+ float disTolerance = cfg->contactDistanceTolerance;
+ bool isUp = fabs(left_height - guardTargetZUp) < disTolerance && fabs(right_height - guardTargetZUp) < disTolerance;
+ if (v_both < disTolerance && isUp)
+ {
+ force_contact_switch = 1;
+ }
+ else if (v_both < 0.05 && isUp)
+ {
+ force_contact_switch = (0.05 - v_both) / (0.05 - disTolerance);
+ }
+ else if (v_both >= 0.05)
+ {
+ force_contact_switch = 0;
+ }
+
+ // computing for contact forces
+ float left_force_error = force_contact_switch * (-left_forceInRoot(2) - contactForce);
+ float right_force_error = force_contact_switch * (-right_forceInRoot(2) - contactForce);
+
+ // float left_force_error_limited = left_force_error;
+ // float right_force_error_limited = right_force_error;
+
+ // left_force_error_limited = (left_force_error_limited > 2) ? 2 : left_force_error_limited;
+ // left_force_error_limited = (left_force_error_limited < -2) ? -2 : left_force_error_limited;
+
+ // right_force_error_limited = (right_force_error_limited > 2) ? 2 : right_force_error_limited;
+ // right_force_error_limited = (right_force_error_limited < -2) ? -2 : right_force_error_limited;
+
+
+ left_force_error = deadzone(left_force_error, 0.5, 2);
+ right_force_error = deadzone(right_force_error, 0.5, 2);
+
+ left_contactForceZ_correction = left_contactForceZ_correction - forceFilterCoeff * left_force_error;
+ right_contactForceZ_correction = right_contactForceZ_correction - forceFilterCoeff * right_force_error;
+
+ left_contactForceZ_correction = (left_contactForceZ_correction > 30) ? 30 : left_contactForceZ_correction;
+ right_contactForceZ_correction = (right_contactForceZ_correction > 30) ? 30 : right_contactForceZ_correction;
+
+ left_contactForceZ_correction = (left_contactForceZ_correction < -30) ? -contactForce : left_contactForceZ_correction;
+ right_contactForceZ_correction = (right_contactForceZ_correction < -30) ? -contactForce : right_contactForceZ_correction;
+
+ Eigen::Vector3f left_tcpDesiredForce = left_DS_force + coupling_force;
+ Eigen::Vector3f right_tcpDesiredForce = right_DS_force - coupling_force;
+
+ left_tcpDesiredForce(2) += force_contact_switch * (contactForce + left_contactForceZ_correction);
+ right_tcpDesiredForce(2) += force_contact_switch * (contactForce + right_contactForceZ_correction);
+
+
+
+ Eigen::Vector3f left_tcpDesiredTorque = - oriKp * left_tcpDesiredAngularError - oriDamping * left_currentTCPAngularVelocity_filtered;
+ Eigen::Vector3f right_tcpDesiredTorque = - oriKp * right_tcpDesiredAngularError - oriDamping * right_currentTCPAngularVelocity_filtered;
+
+ // for (int i = 0; i < left_tcpDesiredTorque.size(); ++i)
+ // {
+ // left_tcpDesiredTorque(i) = deadzone(left_tcpDesiredTorque(i), )
+
+ // }
+
+
+ left_tcpDesiredTorque_filtered = (1 - filterFactor) * left_tcpDesiredTorque_filtered + filterFactor * left_tcpDesiredTorque;
+ right_tcpDesiredTorque_filtered = (1 - filterFactor) * right_tcpDesiredTorque_filtered + filterFactor * right_tcpDesiredTorque;
+
+
+ Eigen::Vector6f left_tcpDesiredWrench;
+ Eigen::Vector6f right_tcpDesiredWrench;
+
+ left_tcpDesiredWrench << 0.001 * left_tcpDesiredForce, left_tcpDesiredTorque_filtered;
+ right_tcpDesiredWrench << 0.001 * right_tcpDesiredForce, right_tcpDesiredTorque_filtered;
+
+
+ // calculate the null-spcae torque
+ Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
+
+ float lambda = 0.2;
+ Eigen::MatrixXf left_jtpinv = left_ik->computePseudoInverseJacobianMatrix(left_jacobi.transpose(), lambda);
+ Eigen::MatrixXf right_jtpinv = right_ik->computePseudoInverseJacobianMatrix(right_jacobi.transpose(), lambda);
+
+
+ Eigen::VectorXf left_nullqerror = left_qpos - left_qnullspace;
+ Eigen::VectorXf right_nullqerror = right_qpos - right_qnullspace;
+
+ for (int i = 0; i < left_nullqerror.size(); ++i)
+ {
+ if (fabs(left_nullqerror(i)) < 0.09)
+ {
+ left_nullqerror(i) = 0;
+ }
+
+ if (fabs(right_nullqerror(i)) < 0.09)
+ {
+ right_nullqerror(i) = 0;
+ }
+ }
+
+
+ Eigen::VectorXf left_nullspaceTorque = - nullspaceKp * left_nullqerror - nullspaceDamping * left_jointVelocity_filtered;
+ Eigen::VectorXf right_nullspaceTorque = - nullspaceKp * right_nullqerror - nullspaceDamping * right_jointVelocity_filtered;
+
+ Eigen::VectorXf left_projectedNullTorque = (I - left_jacobi.transpose() * left_jtpinv) * left_nullspaceTorque;
+ Eigen::VectorXf right_projectedNullTorque = (I - right_jacobi.transpose() * right_jtpinv) * right_nullspaceTorque;
+
+ float left_nulltorque_norm = left_projectedNullTorque.norm();
+ float right_nulltorque_norm = right_projectedNullTorque.norm();
+
+ if (left_nulltorque_norm > null_torqueLimit)
+ {
+ left_projectedNullTorque = (null_torqueLimit / left_nulltorque_norm) * left_projectedNullTorque;
+ }
+
+ if (right_nulltorque_norm > null_torqueLimit)
+ {
+ right_projectedNullTorque = (null_torqueLimit / right_nulltorque_norm) * right_projectedNullTorque;
+ }
+
+ Eigen::VectorXf left_jointDesiredTorques = left_jacobi.transpose() * left_tcpDesiredWrench + left_projectedNullTorque;
+ Eigen::VectorXf right_jointDesiredTorques = right_jacobi.transpose() * right_tcpDesiredWrench + right_projectedNullTorque;
+
+
+ right_desiredTorques_filtered = (1 - cfg->TorqueFilterConstant) * right_desiredTorques_filtered + cfg->TorqueFilterConstant * right_jointDesiredTorques;
+
+
+ for (size_t i = 0; i < left_torque_targets.size(); ++i)
+ {
+ float desiredTorque = smooth_startup * left_jointDesiredTorques(i);
+ desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
+ left_desiredTorques_filtered(i) = (1 - cfg->TorqueFilterConstant) * left_desiredTorques_filtered(i) + cfg->TorqueFilterConstant * desiredTorque;
+
+ std::string names = left_jointNames[i] + "_desiredTorques";
+ debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
+ names = names + "_filtered";
+ debugDataInfo.getWriteBuffer().desired_torques[names] = left_desiredTorques_filtered(i);
+
+ if (fabs(left_desiredTorques_filtered(i)) > torqueLimit)
+ {
+ left_torque_targets.at(i)->torque = 0;
+ }
+ else
+ {
+ left_torque_targets.at(i)->torque = left_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
+ }
+ }
+
+ for (size_t i = 0; i < right_torque_targets.size(); ++i)
+ {
+ float desiredTorque = smooth_startup * right_jointDesiredTorques(i);
+ desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
+ right_desiredTorques_filtered(i) = (1 - cfg->TorqueFilterConstant) * right_desiredTorques_filtered(i) + cfg->TorqueFilterConstant * desiredTorque;
+
+ std::string names = right_jointNames[i] + "_desiredTorques";
+ debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
+ names = names + "_filtered";
+ debugDataInfo.getWriteBuffer().desired_torques[names] = right_desiredTorques_filtered(i);
+
+ if (fabs(right_desiredTorques_filtered(i)) > torqueLimit)
+ {
+ right_torque_targets.at(i)->torque = 0;
+ }
+ else
+ {
+ right_torque_targets.at(i)->torque = right_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
+ }
+ }
+
+ smooth_startup = smooth_startup + 0.001 * (1.1 - smooth_startup);
+ smooth_startup = (smooth_startup > 1) ? 1 : smooth_startup;
+ smooth_startup = (smooth_startup < 0) ? 0 : smooth_startup;
+
+
+
+ debugDataInfo.getWriteBuffer().left_realForce_x = left_forceInRoot(0);
+ debugDataInfo.getWriteBuffer().left_realForce_y = left_forceInRoot(1);
+ debugDataInfo.getWriteBuffer().left_realForce_z = left_forceInRoot(2);
+ debugDataInfo.getWriteBuffer().right_realForce_x = right_forceInRoot(0);
+ debugDataInfo.getWriteBuffer().right_realForce_y = right_forceInRoot(1);
+ debugDataInfo.getWriteBuffer().right_realForce_z = right_forceInRoot(2);
+
+ debugDataInfo.getWriteBuffer().left_force_error = left_force_error;
+ debugDataInfo.getWriteBuffer().right_force_error = right_force_error;
+
+
+ debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_x = left_tcpDesiredTorque_filtered(0);
+ debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_y = left_tcpDesiredTorque_filtered(1);
+ debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_z = left_tcpDesiredTorque_filtered(2);
+ debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_x = right_tcpDesiredTorque_filtered(0);
+ debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_y = right_tcpDesiredTorque_filtered(1);
+ debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_z = right_tcpDesiredTorque_filtered(2);
+ // 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().tcpDesiredAngularError_x = tcpDesiredAngularError(0);
+ // debugDataInfo.getWriteBuffer().tcpDesiredAngularError_y = tcpDesiredAngularError(1);
+ // debugDataInfo.getWriteBuffer().tcpDesiredAngularError_z = tcpDesiredAngularError(2);
+
+ // debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_x = currentTCPAngularVelocity(0);
+ // debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_y = currentTCPAngularVelocity(1);
+ // debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_z = currentTCPAngularVelocity(2);
+
+ // debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_x = currentTCPLinearVelocity_filtered(0);
+ // debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_y = currentTCPLinearVelocity_filtered(1);
+ // debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_z = currentTCPLinearVelocity_filtered(2);
+
+ debugDataInfo.commitWrite();
+
+ // }
+ // else
+ // {
+ // for (size_t i = 0; i < targets.size(); ++i)
+ // {
+ // targets.at(i)->torque = 0;
+
+ // }
+ // }
+
+
+
+}
+
+float DSRTBimanualController::deadzone(float input, float disturbance, float threshold)
+{
+ if (input > 0)
+ {
+ input = input - disturbance;
+ input = (input < 0) ? 0 : input;
+ input = (input > threshold) ? threshold : input;
+ }
+ else if (input < 0)
+ {
+ input = input + disturbance;
+ input = (input > 0) ? 0 : input;
+ input = (input < -threshold) ? -threshold : input;
+ }
+
+
+ return input;
+}
+
+Eigen::Quaternionf DSRTBimanualController::quatSlerp(double t, const Eigen::Quaternionf& q0, const Eigen::Quaternionf& q1)
+{
+ double cosHalfTheta = q0.w() * q1.w() + q0.x() * q1.x() + q0.y() * q1.y() + q0.z() * q1.z();
+
+
+ Eigen::Quaternionf q1x = q1;
+ if (cosHalfTheta < 0)
+ {
+ q1x.w() = -q1.w();
+ q1x.x() = -q1.x();
+ q1x.y() = -q1.y();
+ q1x.z() = -q1.z();
+ }
+
+
+ if (fabs(cosHalfTheta) >= 1.0)
+ {
+ return q0;
+ }
+
+ double halfTheta = acos(cosHalfTheta);
+ double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
+
+
+ Eigen::Quaternionf result;
+ if (fabs(sinHalfTheta) < 0.001)
+ {
+ result.w() = (1 - t) * q0.w() + t * q1x.w();
+ result.x() = (1 - t) * q0.x() + t * q1x.x();
+ result.y() = (1 - t) * q0.y() + t * q1x.y();
+ result.z() = (1 - t) * q0.z() + t * q1x.z();
+
+ }
+ else
+ {
+ double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
+ double ratioB = sin(t * halfTheta) / sinHalfTheta;
+
+ result.w() = ratioA * q0.w() + ratioB * q1x.w();
+ result.x() = ratioA * q0.x() + ratioB * q1x.x();
+ result.y() = ratioA * q0.y() + ratioB * q1x.y();
+ result.z() = ratioA * q0.z() + ratioB * q1x.z();
+ }
+
+ return result;
+}
+
+void DSRTBimanualController::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);
+ }
+
+ // std::string nameJacobi = "jacobiori";
+ // std::string nameJacobipos = "jacobipos";
+
+ // std::vector<float> jacobiVec = debugDataInfo.getUpToDateReadBuffer().jacobiVec;
+ // std::vector<float> jacobiPos = debugDataInfo.getUpToDateReadBuffer().jacobiPos;
+
+ // for (size_t i = 0; i < jacobiVec.size(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string name = nameJacobi + ss.str();
+ // datafields[name] = new Variant(jacobiVec[i]);
+ // std::string namepos = nameJacobipos + ss.str();
+ // datafields[namepos] = new Variant(jacobiPos[i]);
+
+ // }
+
+
+ datafields["left_tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_x);
+ datafields["left_tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_y);
+ datafields["left_tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_z);
+ datafields["right_tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_x);
+ datafields["right_tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_y);
+ datafields["right_tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_z);
+
+ datafields["left_realForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_x);
+ datafields["left_realForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_y);
+ datafields["left_realForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_z);
+ datafields["right_realForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_x);
+ datafields["right_realForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_y);
+ datafields["right_realForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_z);
+
+ datafields["left_force_error"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_force_error);
+ datafields["right_force_error"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_force_error);
+
+
+ datafields["Desired_Guard_Z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().desredZ);
+ datafields["Force_Error_Z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().force_error_z);
+ datafields["guardXYHighFrequency"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guardXYHighFrequency);
+ datafields["guard_mounting_correction_x"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_x);
+ datafields["guard_mounting_correction_y"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_y);
+ datafields["guard_mounting_correction_z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_z);
+
+
+
+ // 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["tcpDesiredAngularError_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_x);
+ // datafields["tcpDesiredAngularError_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_y);
+ // datafields["tcpDesiredAngularError_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_z);
+
+ // datafields["currentTCPAngularVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_x);
+ // datafields["currentTCPAngularVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_y);
+ // datafields["currentTCPAngularVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_z);
+
+
+ // datafields["currentTCPLinearVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_x);
+ // datafields["currentTCPLinearVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_y);
+ // datafields["currentTCPLinearVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_z);
+
+
+ debugObs->setDebugChannel("DSBimanualControllerOutput", datafields);
+
+}
+
+void DSRTBimanualController::rtPreActivateController()
+{
+
+}
+
+void DSRTBimanualController::rtPostDeactivateController()
+{
+
+}
+
+void armarx::DSRTBimanualController::setToDefaultTarget(const Ice::Current &)
+{
+ isDefaultTarget = true;
+}
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.h b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.h
new file mode 100644
index 0000000000000000000000000000000000000000..e4158acca2bd3f51ea9c5c6c0414ab73a7816c09
--- /dev/null
+++ b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.h
@@ -0,0 +1,539 @@
+/*
+ * 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 DSController::ArmarXObjects::DSRTBimanualController
+ * @author Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_DSController_DSRTBimanualController_H
+#define _ARMARX_LIB_DSController_DSRTBimanualController_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 <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Tools/Gravity.h>
+
+#include <RobotAPI/interface/units/RobotUnit/DSControllerBase.h>
+#include "GMRDynamics.h"
+#include <ArmarXCore/util/json/JSONObject.h>
+
+#include "MathLib.h"
+//#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
+//#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
+
+
+namespace armarx
+{
+ class DSRTBimanualControllerControlData
+ {
+ public:
+ Eigen::Vector3f left_tcpDesiredLinearVelocity;
+ Eigen::Vector3f left_tcpDesiredAngularError;
+
+ Eigen::Vector3f right_tcpDesiredLinearVelocity;
+ Eigen::Vector3f right_tcpDesiredAngularError;
+
+ Eigen::Vector3f left_right_distanceInMeter;
+
+ };
+
+ typedef boost::shared_ptr<GMRDynamics> BimanualGMMPtr;
+
+ struct BimanualGMRParameters
+ {
+ int K_gmm_;
+ int dim_;
+ std::vector<double> Priors_;
+ std::vector<double> Mu_;
+ std::vector<double> Sigma_;
+ std::vector<double> attractor_;
+ double dt_;
+ };
+
+
+ class BimanualGMMMotionGen
+ {
+ public:
+ BimanualGMMMotionGen() {}
+
+ BimanualGMMMotionGen(const std::string& fileName)
+ {
+ getGMMParamsFromJsonFile(fileName);
+ }
+
+ BimanualGMMPtr leftarm_gmm;
+ BimanualGMMPtr rightarm_gmm;
+
+ BimanualGMRParameters leftarm_gmmParas;
+ BimanualGMRParameters rightarm_gmmParas;
+
+ Eigen::Vector3f leftarm_Target;
+ Eigen::Vector3f rightarm_Target;
+
+ Eigen::Vector3f left_DS_DesiredVelocity;
+ Eigen::Vector3f right_DS_DesiredVelocity;
+ Eigen::Vector3f left_right_position_errorInMeter;
+
+
+
+ float scaling;
+ float v_max;
+
+ void getGMMParamsFromJsonFile(const std::string& fileName)
+ {
+ std::ifstream infile { fileName };
+ std::string objDefs = { std::istreambuf_iterator<char>(infile), std::istreambuf_iterator<char>() };
+ // StructuralJsonParser jsonParser(objDefs);
+ // jsonParser.parse();
+ // JPathNavigator jpnav(jsonParser.parsedJson);
+ // float k = jpnav.selectSingleNode("left/K").asFloat();
+ // ARMARX_INFO << "k: " << k ;
+ // jpnav.selectSingleNode("left")
+ JSONObjectPtr json = new JSONObject();
+ json->fromString(objDefs);
+
+ // leftarm_gmmParas.K_gmm_ = AbstractObjectSerializerPtr::dynamicCast<JSONObjectPtr>(json->getElement("left"))->getInt("K");
+ // leftarm_gmmParas.dim_ = json->getElement("left")->getInt("dim");
+ // boost::dynamic_pointer_cast<JSONObjectPtr>(json->getElement("left"))->getArray<double>("Priors", leftarm_gmmParas.Priors_);
+
+ // json->getElement("left")->getArray<double>("Mu", leftarm_gmmParas.Mu_);
+ // json->getElement("left")->getArray<double>("attractor", leftarm_gmmParas.attractor_);
+ // json->getElement("left")->getArray<double>("Sigma", leftarm_gmmParas.Sigma_);
+
+ // rightarm_gmmParas.K_gmm_ = json->getElement("right")->getInt("K");
+ // rightarm_gmmParas.dim_ = json->getElement("right")->getInt("dim");
+ // json->getElement("right")->getArray<double>("Priors", rightarm_gmmParas.Priors_);
+ // json->getElement("right")->getArray<double>("Mu", rightarm_gmmParas.Mu_);
+ // json->getElement("right")->getArray<double>("attractor", rightarm_gmmParas.attractor_);
+ // json->getElement("right")->getArray<double>("Sigma", rightarm_gmmParas.Sigma_);
+
+
+ leftarm_gmmParas.K_gmm_ = json->getInt("leftK");
+ leftarm_gmmParas.dim_ = json->getInt("leftDim");
+ json->getArray<double>("leftPriors", leftarm_gmmParas.Priors_);
+ json->getArray<double>("leftMu", leftarm_gmmParas.Mu_);
+ json->getArray<double>("leftAttractor", leftarm_gmmParas.attractor_);
+ json->getArray<double>("leftSigma", leftarm_gmmParas.Sigma_);
+
+
+ rightarm_gmmParas.K_gmm_ = json->getInt("rightK");
+ rightarm_gmmParas.dim_ = json->getInt("rightDim");
+ json->getArray<double>("rightPriors", rightarm_gmmParas.Priors_);
+ json->getArray<double>("rightMu", rightarm_gmmParas.Mu_);
+ json->getArray<double>("rightAttractor", rightarm_gmmParas.attractor_);
+ json->getArray<double>("rightSigma", rightarm_gmmParas.Sigma_);
+
+
+ scaling = json->getDouble("Scaling");
+ v_max = json->getDouble("MaxVelocity");
+
+ leftarm_gmm.reset(new GMRDynamics(leftarm_gmmParas.K_gmm_, leftarm_gmmParas.dim_, leftarm_gmmParas.dt_, leftarm_gmmParas.Priors_, leftarm_gmmParas.Mu_, leftarm_gmmParas.Sigma_));
+ leftarm_gmm->initGMR(0, 2, 3, 5);
+
+ rightarm_gmm.reset(new GMRDynamics(rightarm_gmmParas.K_gmm_, rightarm_gmmParas.dim_, rightarm_gmmParas.dt_, rightarm_gmmParas.Priors_, rightarm_gmmParas.Mu_, rightarm_gmmParas.Sigma_));
+ rightarm_gmm->initGMR(0, 2, 3, 5);
+
+
+ // if (leftarm_gmmParas.attractor_.size() < 3)
+ // {
+ // ARMARX_ERROR << "attractor in json file for the left arm should be 6 dimension vector ... ";
+ // }
+
+ // if (rightarm_gmmParas.attractor_.size() < 3)
+ // {
+ // ARMARX_ERROR << "attractor in json file for the left arm should be 6 dimension vector ... ";
+ // }
+
+ std::cout << "line 162." << std::endl;
+
+
+ for (int i = 0; i < 3; ++i)
+ {
+ leftarm_Target(i) = leftarm_gmmParas.attractor_.at(i);
+ rightarm_Target(i) = rightarm_gmmParas.attractor_.at(i);
+ }
+
+ std::cout << "Finished GMM." << std::endl;
+
+ }
+
+
+ void updateDesiredVelocity(
+ const Eigen::Vector3f& leftarm_PositionInMeter,
+ const Eigen::Vector3f& rightarm_PositionInMeter,
+ float positionErrorToleranceInMeter,
+ float desiredZ,
+ float correction_x,
+ float correction_y,
+ float correction_z)
+ {
+ MathLib::Vector position_error;
+ position_error.Resize(3);
+
+ MathLib::Vector desired_vel;
+ desired_vel.Resize(3);
+
+
+
+ Eigen::Vector3f leftarm_Target_corrected = leftarm_Target;
+ Eigen::Vector3f rightarm_Target_corrected = rightarm_Target;
+
+ leftarm_Target_corrected(0) += correction_x;
+ rightarm_Target_corrected(0) += correction_x;
+
+ leftarm_Target_corrected(1) += correction_y;
+ rightarm_Target_corrected(1) += correction_y;
+
+ leftarm_Target_corrected(2) = desiredZ + correction_z;
+ rightarm_Target_corrected(2) = desiredZ + correction_z;
+
+
+
+ // for the left arm
+ Eigen::Vector3f leftarm_PositionError = leftarm_PositionInMeter - leftarm_Target_corrected;
+ if (leftarm_PositionError.norm() < positionErrorToleranceInMeter)
+ {
+ leftarm_PositionError.setZero();
+ }
+
+ for (int i = 0; i < 3; ++i)
+ {
+ position_error(i) = leftarm_PositionError(i);
+ }
+
+ desired_vel = leftarm_gmm->getVelocity(position_error);
+
+ Eigen::Vector3f leftarm_desired_vel;
+ leftarm_desired_vel << desired_vel(0), desired_vel(1), desired_vel(2);
+
+ leftarm_desired_vel = scaling * leftarm_desired_vel;
+
+ float lenVec = leftarm_desired_vel.norm();
+
+ if (std::isnan(lenVec))
+ {
+ leftarm_desired_vel.setZero();
+ }
+
+ if (lenVec > v_max)
+ {
+ leftarm_desired_vel = (v_max / lenVec) * leftarm_desired_vel;
+ }
+
+ left_DS_DesiredVelocity = leftarm_desired_vel;
+
+
+ // for the right arm
+ Eigen::Vector3f rightarm_PositionError = rightarm_PositionInMeter - rightarm_Target_corrected;
+ if (rightarm_PositionError.norm() < positionErrorToleranceInMeter)
+ {
+ rightarm_PositionError.setZero();
+ }
+
+ for (int i = 0; i < 3; ++i)
+ {
+ position_error(i) = rightarm_PositionError(i);
+ }
+
+ desired_vel = rightarm_gmm->getVelocity(position_error);
+
+ Eigen::Vector3f rightarm_desired_vel;
+ rightarm_desired_vel << desired_vel(0), desired_vel(1), desired_vel(2);
+
+ rightarm_desired_vel = scaling * rightarm_desired_vel;
+
+ lenVec = rightarm_desired_vel.norm();
+ if (std::isnan(lenVec))
+ {
+ rightarm_desired_vel.setZero();
+ }
+
+ if (lenVec > v_max)
+ {
+ rightarm_desired_vel = (v_max / lenVec) * rightarm_desired_vel;
+ }
+
+ right_DS_DesiredVelocity = rightarm_desired_vel;
+
+ left_right_position_errorInMeter = leftarm_PositionError - rightarm_PositionError;
+
+ }
+
+
+
+ };
+
+ typedef boost::shared_ptr<BimanualGMMMotionGen> BimanualGMMMotionGenPtr;
+
+
+
+
+ /**
+ * @defgroup Library-DSRTBimanualController DSRTBimanualController
+ * @ingroup DSController
+ * A description of the library DSRTBimanualController.
+ *
+ * @class DSRTBimanualController
+ * @ingroup Library-DSRTBimanualController
+ * @brief Brief description of class DSRTBimanualController.
+ *
+ * Detailed description of class DSRTBimanualController.
+ */
+
+ class DSRTBimanualController : public NJointControllerWithTripleBuffer<DSRTBimanualControllerControlData>, public DSBimanualControllerInterface
+ {
+
+ // ManagedIceObject interface
+ protected:
+ void onInitNJointController();
+ void onDisconnectNJointController();
+
+
+ void controllerRun();
+
+
+
+ // NJointControllerInterface interface
+ public:
+ using ConfigPtrT = DSRTBimanualControllerConfigPtr;
+
+ DSRTBimanualController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+
+ std::string getClassName(const Ice::Current&) const
+ {
+ return "DSRTBimanualController";
+ }
+
+ // NJointController interface
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ void setToDefaultTarget(const Ice::Current&);
+ private:
+
+ float deadzone(float currentValue, float targetValue, float threshold);
+ Eigen::Quaternionf quatSlerp(double t, const Eigen::Quaternionf& q0, const Eigen::Quaternionf& q1);
+
+ // PeriodicTask<DSRTBimanualController>::pointer_type controllerTask;
+ BimanualGMMMotionGenPtr gmmMotionGenerator;
+ struct DSRTBimanualControllerSensorData
+ {
+ Eigen::Matrix4f left_tcpPose;
+ Eigen::Matrix4f right_tcpPose;
+
+ // Eigen::Vector3f left_linearVelocity;
+ // Eigen::Vector3f right_linearVelocity;
+
+ Eigen::Vector3f left_force;
+ Eigen::Vector3f right_force;
+
+
+ double currentTime;
+
+
+ };
+ TripleBuffer<DSRTBimanualControllerSensorData> controllerSensorData;
+
+ struct DSCtrlDebugInfo
+ {
+ float desredZ;
+ float force_error_z;
+ float guardXYHighFrequency;
+ float guard_mounting_correction_x;
+ float guard_mounting_correction_y;
+ float guard_mounting_correction_z;
+ };
+ TripleBuffer<DSCtrlDebugInfo> debugCtrlDataInfo;
+
+ struct DSRTDebugInfo
+ {
+ 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;
+
+ // force torque sensor in root
+ float left_realForce_x;
+ float left_realForce_y;
+ float left_realForce_z;
+ float right_realForce_x;
+ float right_realForce_y;
+ float right_realForce_z;
+ float left_force_error;
+ float right_force_error;
+
+ float left_tcpDesiredTorque_x;
+ float left_tcpDesiredTorque_y;
+ float left_tcpDesiredTorque_z;
+ float right_tcpDesiredTorque_x;
+ float right_tcpDesiredTorque_y;
+ float right_tcpDesiredTorque_z;
+
+ };
+ TripleBuffer<DSRTDebugInfo> debugDataInfo;
+
+
+ std::vector<const SensorValue1DoFActuatorTorque*> left_torqueSensors;
+ std::vector<const SensorValue1DoFGravityTorque*> left_gravityTorqueSensors;
+ std::vector<const SensorValue1DoFActuatorVelocity*> left_velocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> left_positionSensors;
+
+ std::vector<const SensorValue1DoFActuatorTorque*> right_torqueSensors;
+ std::vector<const SensorValue1DoFGravityTorque*> right_gravityTorqueSensors;
+ std::vector<const SensorValue1DoFActuatorVelocity*> right_velocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> right_positionSensors;
+
+ const SensorValueForceTorque* leftForceTorque;
+ const SensorValueForceTorque* rightForceTorque;
+
+ std::vector<ControlTarget1DoFActuatorTorque*> left_torque_targets;
+ std::vector<ControlTarget1DoFActuatorTorque*> right_torque_targets;
+
+
+ VirtualRobot::RobotNodePtr left_arm_tcp;
+ VirtualRobot::RobotNodePtr right_arm_tcp;
+
+ VirtualRobot::RobotNodePtr left_sensor_frame;
+ VirtualRobot::RobotNodePtr right_sensor_frame;
+
+ VirtualRobot::DifferentialIKPtr left_ik;
+ Eigen::MatrixXf left_jacobip;
+ Eigen::MatrixXf left_jacobio;
+
+ VirtualRobot::DifferentialIKPtr right_ik;
+ Eigen::MatrixXf right_jacobip;
+ Eigen::MatrixXf right_jacobio;
+
+ Eigen::Quaternionf left_desiredQuaternion;
+ Eigen::Vector3f left_oldPosition;
+ Eigen::Matrix3f left_oldOrientation;
+
+ Eigen::Quaternionf right_desiredQuaternion;
+ Eigen::Vector3f right_oldPosition;
+ Eigen::Matrix3f right_oldOrientation;
+
+ Eigen::Vector3f left_currentTCPLinearVelocity_filtered;
+ Eigen::Vector3f left_currentTCPAngularVelocity_filtered;
+
+ Eigen::VectorXf left_jointVelocity_filtered;
+ Eigen::VectorXf right_jointVelocity_filtered;
+
+ Eigen::VectorXf left_desiredTorques_filtered;
+ Eigen::VectorXf right_desiredTorques_filtered;
+
+
+ Eigen::Vector3f left_tcpDesiredTorque_filtered;
+ Eigen::Vector3f right_tcpDesiredTorque_filtered;
+
+ Eigen::Vector3f leftForceOffset;
+ Eigen::Vector3f rightForceOffset;
+
+ float left_contactForceZ_correction;
+ float right_contactForceZ_correction;
+
+ float smooth_startup;
+
+ DSRTBimanualControllerConfigPtr cfg;
+
+ Eigen::Vector3f right_currentTCPLinearVelocity_filtered;
+ Eigen::Vector3f right_currentTCPAngularVelocity_filtered;
+
+ float filterTimeConstant;
+
+ std::vector<std::string> left_jointNames;
+ std::vector<std::string> right_jointNames;
+
+ float posiKp;
+ float v_max;
+ std::vector<float> Damping;
+ float torqueLimit;
+ float null_torqueLimit;
+
+ float Coupling_stiffness;
+ float Coupling_force_limit;
+
+ float forward_gain;
+
+ float oriKp;
+ float oriDamping;
+
+ float nullspaceKp;
+ float nullspaceDamping;
+
+ float contactForce;
+
+ float guardTargetZUp;
+ float guardTargetZDown;
+ float guardDesiredZ;
+ float guard_mounting_correction_z;
+
+ float guardXYHighFrequency;
+ Eigen::Vector3f left_force_old;
+ Eigen::Vector3f right_force_old;
+
+ Eigen::VectorXf left_qnullspace;
+ Eigen::VectorXf right_qnullspace;
+
+ Eigen::Quaternionf left_oriUp;
+ Eigen::Quaternionf left_oriDown;
+ Eigen::Quaternionf right_oriUp;
+ Eigen::Quaternionf right_oriDown;
+
+ // std::vector<BimanualGMMMotionGenPtr> BimanualGMMMotionGenList;
+
+
+ float forceFilterCoeff;
+
+ float positionErrorTolerance;
+ bool controllerStopRequested = false;
+ bool controllerRunFinished = false;
+
+ bool isDefaultTarget = true;
+
+ // NJointController interface
+ protected:
+ void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+ // NJointController interface
+ protected:
+ void rtPreActivateController();
+ void rtPostDeactivateController();
+ };
+
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTController.cpp b/source/RobotAPI/libraries/DSControllers/DSRTController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..efe9bf5231456b0b743ac46c8b830d3958e79812
--- /dev/null
+++ b/source/RobotAPI/libraries/DSControllers/DSRTController.cpp
@@ -0,0 +1,442 @@
+/*
+ * 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 DSController::ArmarXObjects::DSRTController
+ * @author Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "DSRTController.h"
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+using namespace armarx;
+
+NJointControllerRegistration<DSRTController> registrationControllerDSRTController("DSRTController");
+
+void DSRTController::onInitNJointController()
+{
+ ARMARX_INFO << "init ...";
+
+ runTask("DSRTControllerTask", [&]
+ {
+ CycleUtil c(1);
+ getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+ while (getState() == eManagedIceObjectStarted)
+ {
+ if (isControllerActive())
+ {
+ controllerRun();
+ }
+ c.waitForCycleDuration();
+ }
+ });
+
+
+}
+
+void DSRTController::onDisconnectNJointController()
+{
+
+}
+
+
+DSRTController::DSRTController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+{
+ DSControllerConfigPtr cfg = DSControllerConfigPtr::dynamicCast(config);
+ useSynchronizedRtRobot();
+ VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->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 = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ ARMARX_CHECK_EXPRESSION(ct);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ ARMARX_CHECK_EXPRESSION(sv);
+ auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
+ ARMARX_CHECK_EXPRESSION(casted_ct);
+ targets.push_back(casted_ct);
+
+ const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ if (!torqueSensor)
+ {
+ ARMARX_WARNING << "No Torque sensor available for " << jointName;
+ }
+ if (!gravityTorqueSensor)
+ {
+ ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
+ }
+
+ torqueSensors.push_back(torqueSensor);
+ gravityTorqueSensors.push_back(gravityTorqueSensor);
+ velocitySensors.push_back(velocitySensor);
+ positionSensors.push_back(positionSensor);
+ };
+ ARMARX_INFO << "Initialized " << targets.size() << " targets";
+ tcp = rns->getTCP();
+ ARMARX_CHECK_EXPRESSION_W_HINT(tcp, cfg->tcpName);
+
+
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+
+ DSRTControllerSensorData initSensorData;
+ initSensorData.tcpPose = tcp->getPoseInRootFrame();
+ initSensorData.currentTime = 0;
+ controllerSensorData.reinitAllBuffers(initSensorData);
+
+
+ oldPosition = tcp->getPositionInRootFrame();
+ oldOrientation = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+
+ std::vector<float> desiredPositionVec = cfg->desiredPosition;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ desiredPosition(i) = desiredPositionVec[i];
+ }
+ ARMARX_INFO << "ik reseted ";
+
+ std::vector<float> desiredQuaternionVec = cfg->desiredQuaternion;
+ ARMARX_CHECK_EQUAL(desiredQuaternionVec.size(), 4);
+
+ desiredQuaternion.x() = desiredQuaternionVec.at(0);
+ desiredQuaternion.y() = desiredQuaternionVec.at(1);
+ desiredQuaternion.z() = desiredQuaternionVec.at(2);
+ desiredQuaternion.w() = desiredQuaternionVec.at(3);
+
+
+
+ DSRTControllerControlData initData;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ initData.tcpDesiredLinearVelocity(i) = 0;
+ }
+
+ for (size_t i = 0; i < 3; ++i)
+ {
+ initData.tcpDesiredAngularError(i) = 0;
+ }
+ reinitTripleBuffer(initData);
+
+ // initial filter
+ currentTCPLinearVelocity_filtered.setZero();
+ currentTCPAngularVelocity_filtered.setZero();
+ filterTimeConstant = cfg->filterTimeConstant;
+ posiKp = cfg->posiKp;
+ v_max = cfg->v_max;
+ posiDamping = cfg->posiDamping;
+ torqueLimit = cfg->torqueLimit;
+ oriKp = cfg->oriKp;
+ oriDamping = cfg->oriDamping;
+
+
+ std::vector<float> qnullspaceVec = cfg->qnullspaceVec;
+
+ qnullspace.resize(qnullspaceVec.size());
+
+ for (size_t i = 0; i < qnullspaceVec.size(); ++i)
+ {
+ qnullspace(i) = qnullspaceVec[i];
+ }
+
+ nullspaceKp = cfg->nullspaceKp;
+ nullspaceDamping = cfg->nullspaceDamping;
+
+
+ //set GMM parameters ...
+ std::vector<std::string> gmmParamsFiles = cfg->gmmParamsFiles;
+ if (gmmParamsFiles.size() == 0)
+ {
+ ARMARX_ERROR << "no gmm found ... ";
+ }
+
+ gmmMotionGenList.clear();
+ float sumBelief = 0;
+ for (size_t i = 0; i < gmmParamsFiles.size(); ++i)
+ {
+ gmmMotionGenList.push_back(GMMMotionGenPtr(new GMMMotionGen(gmmParamsFiles.at(i))));
+ sumBelief += gmmMotionGenList[i]->belief;
+ }
+ ARMARX_CHECK_EQUAL(gmmMotionGenList.size(), 2);
+
+ dsAdaptorPtr.reset(new DSAdaptor(gmmMotionGenList, cfg->dsAdaptorEpsilon));
+ positionErrorTolerance = cfg->positionErrorTolerance;
+
+
+ ARMARX_INFO << "Initialization done";
+}
+
+
+void DSRTController::controllerRun()
+{
+ if (!controllerSensorData.updateReadBuffer())
+ {
+ return;
+ }
+
+
+ Eigen::Matrix4f currentTCPPose = controllerSensorData.getReadBuffer().tcpPose;
+ Eigen::Vector3f realVelocity = controllerSensorData.getReadBuffer().linearVelocity;
+
+ Eigen::Vector3f currentTCPPositionInMeter;
+ currentTCPPositionInMeter << currentTCPPose(0, 3), currentTCPPose(1, 3), currentTCPPose(2, 3);
+ currentTCPPositionInMeter = 0.001 * currentTCPPositionInMeter;
+
+ dsAdaptorPtr->updateDesiredVelocity(currentTCPPositionInMeter, positionErrorTolerance);
+ Eigen::Vector3f tcpDesiredLinearVelocity = dsAdaptorPtr->totalDesiredVelocity;
+ dsAdaptorPtr->updateBelief(realVelocity);
+
+
+
+ float vecLen = tcpDesiredLinearVelocity.norm();
+ if (vecLen > v_max)
+ {
+
+ tcpDesiredLinearVelocity = tcpDesiredLinearVelocity / vecLen * v_max;
+ }
+
+ ARMARX_INFO << "tcpDesiredLinearVelocity: " << tcpDesiredLinearVelocity;
+
+ debugDataInfo.getWriteBuffer().belief0 = dsAdaptorPtr->task0_belief;
+ debugDataInfo.getWriteBuffer().belief1 = gmmMotionGenList[0]->belief;
+ debugDataInfo.getWriteBuffer().belief2 = gmmMotionGenList[1]->belief;
+ debugDataInfo.commitWrite();
+
+ Eigen::Vector3f tcpDesiredAngularError;
+ tcpDesiredAngularError << 0, 0, 0;
+
+ Eigen::Matrix3f currentOrientation = currentTCPPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f desiredOrientation = desiredQuaternion.normalized().toRotationMatrix();
+ Eigen::Matrix3f orientationError = currentOrientation * desiredOrientation.inverse();
+ Eigen::Quaternionf diffQuaternion(orientationError);
+ Eigen::AngleAxisf angleAxis(diffQuaternion);
+ tcpDesiredAngularError = angleAxis.angle() * angleAxis.axis();
+
+ getWriterControlStruct().tcpDesiredLinearVelocity = tcpDesiredLinearVelocity;
+ getWriterControlStruct().tcpDesiredAngularError = tcpDesiredAngularError;
+
+ writeControlStruct();
+}
+
+
+void DSRTController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+{
+ double deltaT = timeSinceLastIteration.toSecondsDouble();
+ if (deltaT != 0)
+ {
+
+ Eigen::Matrix4f currentTCPPose = tcp->getPoseInRootFrame();
+
+
+ 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);
+ double filterFactor = deltaT / (filterTimeConstant + deltaT);
+ currentTCPLinearVelocity_filtered = (1 - filterFactor) * currentTCPLinearVelocity_filtered + filterFactor * currentTCPLinearVelocity;
+
+ controllerSensorData.getWriteBuffer().tcpPose = currentTCPPose;
+ controllerSensorData.getWriteBuffer().currentTime += deltaT;
+ controllerSensorData.getWriteBuffer().linearVelocity = currentTCPLinearVelocity_filtered;
+ controllerSensorData.commitWrite();
+
+
+ Eigen::Vector3f currentTCPAngularVelocity;
+ currentTCPAngularVelocity << tcptwist(3), tcptwist(4), tcptwist(5);
+ // // calculate linear velocity
+ // Eigen::Vector3f currentTCPPosition;
+ // currentTCPPosition << currentTCPPose(0, 3), currentTCPPose(1, 3), currentTCPPose(2, 3);
+ // Eigen::Vector3f currentTCPLinearVelocity_raw = (currentTCPPosition - oldPosition) / deltaT;
+ // oldPosition = currentTCPPosition;
+
+ // // calculate angular velocity
+ // Eigen::Matrix3f currentTCPOrientation = currentTCPPose.block<3, 3>(0, 0);
+ // Eigen::Matrix3f currentTCPDiff = currentTCPOrientation * oldOrientation.inverse();
+ // Eigen::AngleAxisf currentAngleAxisDiff(currentTCPDiff);
+ // Eigen::Vector3f currentTCPAngularVelocity_raw = currentAngleAxisDiff.angle() * currentAngleAxisDiff.axis();
+ // oldOrientation = currentTCPOrientation;
+ // currentTCPAngularVelocity_filtered = (1 - filterFactor) * currentTCPAngularVelocity_filtered + filterFactor * currentTCPAngularVelocity_raw;
+
+
+ Eigen::Vector3f tcpDesiredLinearVelocity = rtGetControlStruct().tcpDesiredLinearVelocity;
+ Eigen::Vector3f tcpDesiredAngularError = rtGetControlStruct().tcpDesiredAngularError;
+
+ // calculate desired tcp force
+ Eigen::Vector3f tcpDesiredForce = -posiDamping * (currentTCPLinearVelocity_filtered - tcpDesiredLinearVelocity);
+
+ // calculate desired tcp torque
+ Eigen::Vector3f tcpDesiredTorque = - oriKp * tcpDesiredAngularError - oriDamping * 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 = qpos - qnullspace;
+
+ for (int i = 0; i < nullqerror.rows(); ++i)
+ {
+ if (fabs(nullqerror(i)) < 0.09)
+ {
+ nullqerror(i) = 0;
+ }
+ }
+ Eigen::VectorXf nullspaceTorque = - nullspaceKp * nullqerror - nullspaceDamping * 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; // targets.at(i)->velocity = desiredVelocity;
+ }
+
+
+ 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().tcpDesiredAngularError_x = tcpDesiredAngularError(0);
+ debugDataInfo.getWriteBuffer().tcpDesiredAngularError_y = tcpDesiredAngularError(1);
+ debugDataInfo.getWriteBuffer().tcpDesiredAngularError_z = tcpDesiredAngularError(2);
+
+ debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_x = currentTCPAngularVelocity(0);
+ debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_y = currentTCPAngularVelocity(1);
+ debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_z = currentTCPAngularVelocity(2);
+
+ debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_x = currentTCPLinearVelocity_filtered(0);
+ debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_y = currentTCPLinearVelocity_filtered(1);
+ debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_z = currentTCPLinearVelocity_filtered(2);
+
+ debugDataInfo.commitWrite();
+
+ }
+ else
+ {
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ targets.at(i)->torque = 0;
+
+ }
+ }
+
+
+
+}
+
+void DSRTController::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);
+ }
+
+ // std::string nameJacobi = "jacobiori";
+ // std::string nameJacobipos = "jacobipos";
+
+ // std::vector<float> jacobiVec = debugDataInfo.getUpToDateReadBuffer().jacobiVec;
+ // std::vector<float> jacobiPos = debugDataInfo.getUpToDateReadBuffer().jacobiPos;
+
+ // for (size_t i = 0; i < jacobiVec.size(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string name = nameJacobi + ss.str();
+ // datafields[name] = new Variant(jacobiVec[i]);
+ // std::string namepos = nameJacobipos + ss.str();
+ // datafields[namepos] = new Variant(jacobiPos[i]);
+
+ // }
+
+
+
+ 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["tcpDesiredAngularError_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_x);
+ datafields["tcpDesiredAngularError_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_y);
+ datafields["tcpDesiredAngularError_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_z);
+
+ datafields["currentTCPAngularVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_x);
+ datafields["currentTCPAngularVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_y);
+ datafields["currentTCPAngularVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_z);
+
+
+ datafields["currentTCPLinearVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_x);
+ datafields["currentTCPLinearVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_y);
+ datafields["currentTCPLinearVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_z);
+
+ datafields["belief_0"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief0);
+ datafields["belief_1"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief1);
+ datafields["belief_2"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief2);
+
+ debugObs->setDebugChannel("DSControllerOutput", datafields);
+
+}
+
+void DSRTController::rtPreActivateController()
+{
+
+}
+
+void DSRTController::rtPostDeactivateController()
+{
+
+}
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTController.h b/source/RobotAPI/libraries/DSControllers/DSRTController.h
new file mode 100644
index 0000000000000000000000000000000000000000..d60552cef0a5bb6bf563b3d161a93185c0560220
--- /dev/null
+++ b/source/RobotAPI/libraries/DSControllers/DSRTController.h
@@ -0,0 +1,491 @@
+/*
+ * 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 DSController::ArmarXObjects::DSRTController
+ * @author Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_DSController_DSRTController_H
+#define _ARMARX_LIB_DSController_DSRTController_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 <VirtualRobot/Tools/Gravity.h>
+
+#include <RobotAPI/interface/units/RobotUnit/DSControllerBase.h>
+#include "GMRDynamics.h"
+#include <ArmarXCore/util/json/JSONObject.h>
+
+#include "MathLib.h"
+
+namespace armarx
+{
+ class DSRTControllerControlData
+ {
+ public:
+ Eigen::Vector3f tcpDesiredLinearVelocity;
+ Eigen::Vector3f tcpDesiredAngularError;
+ };
+
+ typedef boost::shared_ptr<GMRDynamics> GMMPtr;
+
+ struct GMRParameters
+ {
+ int K_gmm_;
+ int dim_;
+ std::vector<double> Priors_;
+ std::vector<double> Mu_;
+ std::vector<double> Sigma_;
+ std::vector<double> attractor_;
+ double dt_;
+ };
+
+
+ class GMMMotionGen
+ {
+ public:
+ GMMMotionGen() {}
+
+ GMMMotionGen(const std::string& fileName)
+ {
+ getGMMParamsFromJsonFile(fileName);
+ }
+
+ GMMPtr gmm;
+ GMRParameters gmmParas;
+ Eigen::Vector3f desiredDefaultTarget;
+ float scaling;
+
+ float belief;
+ float v_max;
+
+ void getGMMParamsFromJsonFile(const std::string& fileName)
+ {
+ std::ifstream infile { fileName };
+ std::string objDefs = { std::istreambuf_iterator<char>(infile), std::istreambuf_iterator<char>() };
+ JSONObjectPtr json = new JSONObject();
+ json->fromString(objDefs);
+
+
+ gmmParas.K_gmm_ = json->getInt("K");
+ gmmParas.dim_ = json->getInt("dim");
+ json->getArray<double>("Priors", gmmParas.Priors_);
+ json->getArray<double>("Mu", gmmParas.Mu_);
+ json->getArray<double>("attractor", gmmParas.attractor_);
+ json->getArray<double>("Sigma", gmmParas.Sigma_);
+
+ scaling = json->getDouble("Scaling");
+ belief = json->getDouble("InitBelief");
+ belief = 0;
+ v_max = json->getDouble("MaxVelocity");
+
+ gmm.reset(new GMRDynamics(gmmParas.K_gmm_, gmmParas.dim_, gmmParas.dt_, gmmParas.Priors_, gmmParas.Mu_, gmmParas.Sigma_));
+ gmm->initGMR(0, 2, 3, 5);
+
+ if (gmmParas.attractor_.size() < 3)
+ {
+ ARMARX_ERROR << "attractor in json file should be 6 dimension vector ... ";
+ }
+
+ for (int i = 0; i < 3; ++i)
+ {
+ desiredDefaultTarget(i) = gmmParas.attractor_.at(i);
+ }
+ }
+
+ void updateDesiredVelocity(const Eigen::Vector3f& currentPositionInMeter, float positionErrorToleranceInMeter)
+ {
+ Eigen::Vector3f PositionError = currentPositionInMeter - desiredDefaultTarget;
+ if (PositionError.norm() < positionErrorToleranceInMeter)
+ {
+ PositionError.setZero();
+ }
+
+ MathLib::Vector position_error;
+ position_error.Resize(3);
+
+ for (int i = 0; i < 3; ++i)
+ {
+ position_error(i) = PositionError(i);
+ }
+
+ MathLib::Vector desired_vel;
+ desired_vel.Resize(3);
+ desired_vel = gmm->getVelocity(position_error);
+
+ Eigen::Vector3f tcpDesiredLinearVelocity;
+ tcpDesiredLinearVelocity << desired_vel(0), desired_vel(1), desired_vel(2);
+
+ currentDesiredVelocity = scaling * tcpDesiredLinearVelocity;
+
+
+ float lenVec = tcpDesiredLinearVelocity.norm();
+ if (std::isnan(lenVec))
+ {
+ tcpDesiredLinearVelocity.setZero();
+ }
+
+ if (lenVec > v_max)
+ {
+ tcpDesiredLinearVelocity = (v_max / lenVec) * tcpDesiredLinearVelocity;
+ }
+ }
+
+
+
+ Eigen::Vector3f currentDesiredVelocity;
+ };
+
+ typedef boost::shared_ptr<GMMMotionGen> GMMMotionGenPtr;
+
+ class DSAdaptor
+ {
+ public:
+ float task0_belief;
+ float epsilon;
+ DSAdaptor() {}
+
+ DSAdaptor(std::vector<GMMMotionGenPtr> gmmMotionGenList, float epsilon)
+ {
+ task0_belief = 1;
+ this->gmmMotionGenList = gmmMotionGenList;
+
+ ARMARX_INFO << "epsilon: " << epsilon;
+ this->epsilon = epsilon;
+
+ totalDesiredVelocity.setZero();
+ }
+
+ Eigen::Vector3f totalDesiredVelocity;
+ std::vector<GMMMotionGenPtr> gmmMotionGenList;
+
+
+ void updateDesiredVelocity(const Eigen::Vector3f& currentPositionInMeter, float positionErrorToleranceInMeter)
+ {
+ totalDesiredVelocity.setZero();
+ for (size_t i = 0; i < gmmMotionGenList.size(); ++i)
+ {
+ gmmMotionGenList[i]->updateDesiredVelocity(currentPositionInMeter, positionErrorToleranceInMeter);
+ totalDesiredVelocity += gmmMotionGenList[i]->belief * gmmMotionGenList[i]->currentDesiredVelocity;
+ }
+ }
+
+ void updateBelief(const Eigen::Vector3f& realVelocity)
+ {
+ std::vector<float> beliefUpdate;
+ beliefUpdate.resize(gmmMotionGenList.size());
+
+ float nullInnerSimilarity = 0;
+ for (size_t i = 0; i < gmmMotionGenList.size(); ++i)
+ {
+
+ GMMMotionGenPtr currentGMM = gmmMotionGenList[i];
+
+ float belief = currentGMM->belief;
+ Eigen::Vector3f OtherTasks = totalDesiredVelocity - belief * currentGMM->currentDesiredVelocity;
+ float innerSimilarity = 2 * OtherTasks.dot(currentGMM->currentDesiredVelocity);
+ float outerDisSimilarity = (realVelocity - currentGMM->currentDesiredVelocity).squaredNorm();
+
+ if (innerSimilarity > nullInnerSimilarity)
+ {
+ nullInnerSimilarity = innerSimilarity;
+ }
+
+ beliefUpdate[i] = - outerDisSimilarity - innerSimilarity;
+
+
+ }
+
+
+
+
+ float nullOuterSimilarity = realVelocity.squaredNorm();
+ float zeroTaskRawBeliefUpdate = - nullInnerSimilarity - nullOuterSimilarity;
+
+ if (zeroTaskRawBeliefUpdate < 0.2)
+ {
+ zeroTaskRawBeliefUpdate -= 1000;
+ }
+
+
+ beliefUpdate.insert(beliefUpdate.begin(), zeroTaskRawBeliefUpdate);
+
+ WinnerTakeAll(beliefUpdate);
+ task0_belief += epsilon * beliefUpdate[0];
+ if (task0_belief > 1)
+ {
+ task0_belief = 1;
+ }
+ else if (task0_belief < 0)
+ {
+ task0_belief = 0;
+ }
+
+ float beliefSum = task0_belief;
+
+ for (size_t i = 0; i < gmmMotionGenList.size(); ++i)
+ {
+ gmmMotionGenList[i]->belief += epsilon * beliefUpdate[i + 1];
+
+
+ if (gmmMotionGenList[i]->belief > 1)
+ {
+ gmmMotionGenList[i]->belief = 1;
+ }
+ else if (gmmMotionGenList[i]->belief < 0)
+ {
+ gmmMotionGenList[i]->belief = 0;
+ }
+
+ beliefSum += gmmMotionGenList[i]->belief;
+
+ }
+
+ for (size_t i = 0; i < gmmMotionGenList.size(); ++i)
+ {
+ gmmMotionGenList[i]->belief /= beliefSum;
+ }
+
+ task0_belief /= beliefSum;
+ }
+
+ private:
+
+ void WinnerTakeAll(std::vector<float>& UpdateBeliefs_)
+ {
+ // std::fill(UpdateBeliefs_.begin(), UpdateBeliefs_.end(), 0);
+
+ size_t winner_index = 0;
+
+ for (size_t i = 1; i < UpdateBeliefs_.size(); i++)
+ {
+ if (UpdateBeliefs_[i] > UpdateBeliefs_[winner_index])
+ {
+ winner_index = i;
+ }
+ }
+
+ float winner_belief = task0_belief;
+
+ if (winner_index != 0)
+ {
+ winner_belief = gmmMotionGenList[winner_index - 1]->belief;
+ }
+
+ if (winner_belief == 1)
+ {
+ return;
+ }
+
+ int runnerUp_index = 0;
+
+ if (winner_index == 0)
+ {
+ runnerUp_index = 1;
+ }
+
+ for (size_t i = 0; i < UpdateBeliefs_.size(); i++)
+ {
+ if (i == winner_index)
+ {
+ continue;
+ }
+
+ if (UpdateBeliefs_[i] > UpdateBeliefs_[runnerUp_index])
+ {
+ runnerUp_index = i;
+ }
+ }
+
+ float offset = 0.5 * (UpdateBeliefs_[winner_index] + UpdateBeliefs_[runnerUp_index]);
+
+ for (size_t i = 0; i < UpdateBeliefs_.size(); i++)
+ {
+ UpdateBeliefs_[i] -= offset;
+ }
+
+ float UpdateSum = 0;
+
+ for (size_t i = 0; i < UpdateBeliefs_.size(); i++)
+ {
+ float belief = task0_belief;
+ if (i != 0)
+ {
+ belief = gmmMotionGenList[i - 1]->belief;
+ }
+
+ if (belief != 0 || UpdateBeliefs_[i] > 0)
+ {
+ UpdateSum += UpdateBeliefs_[i];
+ }
+ }
+
+ UpdateBeliefs_[winner_index] -= UpdateSum;
+ }
+ };
+
+ typedef boost::shared_ptr<DSAdaptor> DSAdaptorPtr;
+
+
+
+ /**
+ * @defgroup Library-DSRTController DSRTController
+ * @ingroup DSController
+ * A description of the library DSRTController.
+ *
+ * @class DSRTController
+ * @ingroup Library-DSRTController
+ * @brief Brief description of class DSRTController.
+ *
+ * Detailed description of class DSRTController.
+ */
+
+ class DSRTController : public NJointControllerWithTripleBuffer<DSRTControllerControlData>, public DSControllerInterface
+ {
+
+ // ManagedIceObject interface
+ protected:
+ void onInitNJointController();
+ void onDisconnectNJointController();
+
+
+ void controllerRun();
+
+
+
+ // NJointControllerInterface interface
+ public:
+ using ConfigPtrT = DSControllerConfigPtr;
+
+ DSRTController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+
+ std::string getClassName(const Ice::Current&) const
+ {
+ return "DSRTController";
+ }
+
+ // NJointController interface
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ private:
+ // PeriodicTask<DSRTController>::pointer_type controllerTask;
+
+ struct DSRTControllerSensorData
+ {
+ Eigen::Matrix4f tcpPose;
+ double currentTime;
+
+ Eigen::Vector3f linearVelocity;
+
+ };
+ TripleBuffer<DSRTControllerSensorData> controllerSensorData;
+
+ struct DSRTDebugInfo
+ {
+ 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 belief0;
+ float belief1;
+ float belief2;
+ };
+ TripleBuffer<DSRTDebugInfo> debugDataInfo;
+
+ std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
+ std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
+ std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
+
+ std::vector<ControlTarget1DoFActuatorTorque*> targets;
+
+
+ VirtualRobot::RobotNodePtr tcp;
+
+ VirtualRobot::DifferentialIKPtr ik;
+ Eigen::MatrixXf jacobip;
+ Eigen::MatrixXf jacobio;
+
+ Eigen::Vector3f desiredPosition;
+
+ Eigen::Quaternionf desiredQuaternion;
+ Eigen::Vector3f oldPosition;
+
+ Eigen::Matrix3f oldOrientation;
+
+ Eigen::Vector3f currentTCPLinearVelocity_filtered;
+ Eigen::Vector3f currentTCPAngularVelocity_filtered;
+
+ float filterTimeConstant;
+
+ std::vector<std::string> jointNames;
+
+ float posiKp;
+ float v_max;
+ float posiDamping;
+ float torqueLimit;
+
+ float oriKp;
+ float oriDamping;
+
+ float nullspaceKp;
+ float nullspaceDamping;
+
+ Eigen::VectorXf qnullspace;
+
+ std::vector<GMMMotionGenPtr> gmmMotionGenList;
+
+ DSAdaptorPtr dsAdaptorPtr;
+
+ float positionErrorTolerance;
+
+
+ // NJointController interface
+ protected:
+ void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+ // NJointController interface
+ protected:
+ void rtPreActivateController();
+ void rtPostDeactivateController();
+ };
+
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/DSControllers/GMRDynamics.cpp b/source/RobotAPI/libraries/DSControllers/GMRDynamics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0eba736752d546b1c3f5891025515314029526c8
--- /dev/null
+++ b/source/RobotAPI/libraries/DSControllers/GMRDynamics.cpp
@@ -0,0 +1,128 @@
+/*
+ * GMRDynamics.cpp
+ *
+ * Created on: Nov 20, 2011
+ * Author: Seungsu KIM
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include "GMRDynamics.h"
+
+
+GMRDynamics::GMRDynamics(int nStates, int nVar, double delta_t, const char *f_mu, const char *f_sigma, const char *f_prio )
+{
+ this->delta_t = delta_t;
+ GMM = new Gaussians(nStates, nVar, f_mu, f_sigma, f_prio);
+}
+
+GMRDynamics::GMRDynamics(int nStates, int nVar, double delta_t, const vector<double> pri_vec, const vector<double> mu_vec, const vector<double> sig_vec)
+{
+ this->delta_t = delta_t;
+ GMM = new Gaussians(nStates, nVar, pri_vec, mu_vec, sig_vec);
+}
+
+
+void GMRDynamics::initGMR(int first_inindex, int last_inindex, int first_outindex, int last_outindex)
+{
+ GMM->InitFastGMR(first_inindex, last_inindex, first_outindex, last_outindex);
+
+ gDim = last_inindex - first_inindex + 1;
+ if ( gDim != ( last_outindex - first_outindex + 1 ) ) {
+ cout << "dynamics dimension is not matching" << endl;
+ }
+
+ gXi.Resize(gDim);
+ target.Resize(gDim);
+
+ gXi.Zero();
+ target.Zero();
+}
+
+void GMRDynamics::setStateTarget(Vector state, Vector target)
+{
+ setTarget(target);
+ setState(state);
+}
+
+void GMRDynamics::setTarget(Vector target, double target_t)
+{
+ this->target_t = target_t;
+
+ //gXi += (this->target - target);
+ this->target = target;
+}
+
+Vector GMRDynamics::getTarget(void)
+{
+ return target;
+}
+
+double GMRDynamics::getTargetT(void)
+{
+ return target_t;
+}
+
+void GMRDynamics::setState(Vector state)
+{
+ gXi = state;
+}
+
+Vector GMRDynamics::getState(void)
+{
+ return gXi;
+}
+
+void GMRDynamics::setCurrentTime(double current_t)
+{
+ this->current_t = current_t;
+}
+
+double GMRDynamics::getCurrentTime(void)
+{
+ return current_t;
+}
+
+Vector GMRDynamics::getVelocity(Vector x)
+{
+ return GMM->Regression(x);
+
+
+}
+
+
+Vector GMRDynamics::getNextState(void)
+{
+ return getNextState(1.0);
+}
+
+Vector GMRDynamics::getNextState(double lamda)
+{
+ // target time
+ target_t -= (delta_t*lamda);
+
+ gXi += (getVelocity(gXi - target) * (delta_t*lamda));
+
+ return gXi;
+}
+
+double GMRDynamics::getReachingTime(double lamda)
+{
+ unsigned int frame = 0;
+ unsigned int li = 0;
+ Vector xi(3);
+ xi.Set(gXi);
+
+ for (frame = 0; frame < REACHING_ITERATION_MAX; frame++) {
+ for (li = 0; li < INTEGRATION_L; li++) {
+ xi += getVelocity(xi - target) * delta_t / (double)INTEGRATION_L * lamda;
+
+ if ( (xi - target).Norm() < GMR_ERROR_TOLERANCE ) {
+ return (double)(frame * INTEGRATION_L + li) * delta_t / (double)INTEGRATION_L;
+ }
+ }
+ }
+ return (double)(frame * INTEGRATION_L + li) * delta_t / (double)INTEGRATION_L;
+}
+
diff --git a/source/RobotAPI/libraries/DSControllers/GMRDynamics.h b/source/RobotAPI/libraries/DSControllers/GMRDynamics.h
new file mode 100644
index 0000000000000000000000000000000000000000..de826ca48963846616753dd86912f5c28d4c962b
--- /dev/null
+++ b/source/RobotAPI/libraries/DSControllers/GMRDynamics.h
@@ -0,0 +1,56 @@
+/*
+ * GMRDynamics.h
+ *
+ * Created on: Nov 20, 2011
+ * Author: Seungsu KIM
+ */
+
+#ifndef __GMRDYNAMICS_H__
+#define __GMRDYNAMICS_H__
+
+#include "Gaussians.h"
+
+#define GMR_ERROR_TOLERANCE 0.001
+#define INTEGRATION_L 5
+#define REACHING_ITERATION_MAX 500
+#define REAL_MIN (1e-30)
+
+// GMR Dynamics
+class GMRDynamics
+{
+private:
+ Gaussians* GMM;
+
+ double delta_t;
+ double target_t;
+ double current_t;
+
+ Vector gXi;
+ Vector target;
+ unsigned int gDim;
+
+public:
+ GMRDynamics(int nStates, int nVar, double delta_t, const char* f_mu, const char* f_sigma, const char* f_prio);
+ GMRDynamics(int nStates, int nVar, double delta_t, const vector<double> pri_vec, const vector<double> mu_vec, const vector<double> sig_vec);
+
+ void initGMR(int first_inindex, int last_inindex, int first_outindex, int last_outindex);
+
+ void setStateTarget(Vector state, Vector target);
+ void setTarget(Vector target, double target_t = -1.0);
+ Vector getTarget(void);
+ double getTargetT(void);
+ void setState(Vector state);
+ Vector getState(void);
+ void setCurrentTime(double current_t);
+ double getCurrentTime(void);
+
+ Vector getVelocity(Vector x);
+
+ Vector getNextState(void);
+ Vector getNextState(double lamda);
+ double getReachingTime(double lamda);
+};
+
+
+
+#endif //__GMRDYNAMICS_H__
diff --git a/source/RobotAPI/libraries/DSControllers/Gaussians.cpp b/source/RobotAPI/libraries/DSControllers/Gaussians.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..200605424145ced271fc45acb4bea5044e2b3d38
--- /dev/null
+++ b/source/RobotAPI/libraries/DSControllers/Gaussians.cpp
@@ -0,0 +1,518 @@
+/*
+ * Gaussians.cpp
+ *
+ * Created on: Nov 19, 2011
+ * Author: Seungsu KIM
+ */
+
+#include <math.h>
+#include <iostream>
+#include <fstream>
+
+#include "Gaussians.h"
+
+using namespace std;
+/*
+Gaussians::Gaussians(GMMs *model)
+{
+ this->model.nbStates = model->nbStates;
+ this->model.nbDim = model->nbDim;
+
+ this->model.States = (GMMState *)malloc(model->nbStates*sizeof(GMMState) );
+
+ for(int s=0; s<model->nbStates; s++ ){
+ this->model.States[s].Mu = model->GMMState[s].Mu;
+ this->model.States[s].Sigma = model->GMMState[s].Sigma;
+ this->model.States[s].Prio = model->GMMState[s].Prio;
+ }
+}
+*/
+
+Gaussians::Gaussians(const char *f_mu, const char *f_sigma, const char *f_prio)
+{
+ int s, i, j;
+ int nbStates;
+ int nbDim;
+
+ Matrix fMatrix;
+ fMatrix.Load(f_prio);
+ if ( fMatrix(0, fMatrix.ColumnSize() - 1) > 0.0 )
+ {
+ nbStates = fMatrix.ColumnSize();
+ }
+ else
+ {
+ nbStates = fMatrix.ColumnSize() - 1;
+ }
+
+ for ( s = 0; s < nbStates; s++ ) {
+ model.States[s].Prio = fMatrix(0, s);
+ }
+
+ fMatrix.Load(f_mu);
+ nbDim = fMatrix.RowSize();
+ model.nbStates = nbStates;
+ model.nbDim = nbDim;
+
+
+ for ( s = 0; s < nbStates; s++ ) {
+ model.States[s].Mu.Resize(nbDim);
+ model.States[s].Sigma.Resize(nbDim, nbDim );
+ }
+
+ for ( s = 0; s < nbStates; s++ ) {
+ model.States[s].Mu = fMatrix.GetColumn(s);
+ }
+
+ fMatrix.Load(f_sigma);
+ j = 0;
+ for ( s = 0; s < nbStates; s++ ) {
+ for ( i = 0; i < nbDim; i++ ) {
+ model.States[s].Sigma.SetRow(fMatrix.GetRow(j), i);
+ j++;
+ }
+ }
+}
+
+Gaussians::Gaussians(int nbStates, int nbDim, const char *f_mu, const char *f_sigma, const char *f_prio)
+{
+
+ int s, i, j;
+
+ model.nbStates = nbStates;
+ model.nbDim = nbDim;
+
+ for ( s = 0; s < nbStates; s++ ) {
+ model.States[s].Mu.Resize(nbDim);
+ model.States[s].Sigma.Resize(nbDim, nbDim );
+ }
+
+ Matrix fMatrix(nbDim, nbStates);
+ fMatrix.Load(f_mu);
+ for ( s = 0; s < nbStates; s++ ) {
+ model.States[s].Mu = fMatrix.GetColumn(s);
+ }
+
+ fMatrix.Resize(nbStates * nbDim, nbDim);
+ fMatrix.Load(f_sigma);
+ j = 0;
+ for ( s = 0; s < nbStates; s++ ) {
+ for ( i = 0; i < nbDim; i++ ) {
+ model.States[s].Sigma.SetRow(fMatrix.GetRow(j), i);
+ j++;
+ }
+ }
+
+ fMatrix.Resize(1, nbStates);
+ fMatrix.Load(f_prio);
+ Vector fVector(nbStates);
+ for ( s = 0; s < nbStates; s++ ) {
+ model.States[s].Prio = fMatrix(0, s);
+ }
+
+}
+
+Gaussians::Gaussians(const int nbStates, const int nbDim, const vector<double> pri_vec, const vector<double> mu_vec, const vector<double> sig_vec) {
+
+ model.nbStates = nbStates;
+ model.nbDim = nbDim;
+
+ for ( int s = 0; s < nbStates; s++ ) {
+ model.States[s].Mu.Resize(nbDim);
+ model.States[s].Sigma.Resize(nbDim, nbDim );
+ }
+
+ for ( int s = 0; s < nbStates; s++ ) {
+ model.States[s].Prio = pri_vec[s];
+ }
+ // cout << endl << "Printing the constructed Priors" << endl;
+ // for ( int s = 0; s < nbStates; s++ ) {
+ // cout << model.States[s].Prio << "\t";
+ // }
+ // cout << endl;
+
+
+ for ( int s = 0; s < nbStates; s++ ) {
+ for (int d = 0; d < nbDim; d++) {
+ model.States[s].Mu[d] = mu_vec[s * nbDim + d];
+ }
+ }
+
+
+ // cout << endl << "Printing the constructed Mu" << endl;
+ // for ( int s = 0; s < nbStates; s++ ) {
+ // for (int d = 0; d < nbDim; d++) {
+ // cout << model.States[s].Mu[d] << "\t";
+ // }
+ // cout << endl;
+ // }
+
+ for ( int s = 0; s < nbStates; s++ ) {
+ for (int row = 0; row < nbDim; row++) {
+ for (int col = 0; col < nbDim; col++) {
+ int ind = s * nbDim * nbDim + row * nbDim + col;
+ model.States[s].Sigma(row, col) = sig_vec[ind];
+ }
+ }
+ }
+
+
+ // cout << endl << "Printing the constructed Sigma" << endl;
+ // for ( int s = 0; s < nbStates; s++ ) {
+ // for (int row = 0; row < nbDim; row++) {
+ // for (int col = 0; col < nbDim; col++) {
+ // cout << model.States[s].Sigma(row, col) << "\t";
+ // }
+ // cout <<endl;
+ // }
+ // cout << endl;
+ // }
+
+
+
+
+}
+
+
+
+
+void Gaussians::setGMMs(GMMs *model)
+{
+ for (unsigned int s = 0; s < model->nbStates; s++ ) {
+ this->model.States[s].Mu = model->States[s].Mu;
+ this->model.States[s].Sigma = model->States[s].Sigma;
+ this->model.States[s].Prio = model->States[s].Prio;
+ }
+}
+
+
+void Gaussians::InitFastGaussians(int first_inindex, int last_inindex)
+{
+ double det;
+ int nbIN = last_inindex - first_inindex + 1;
+
+ for (unsigned int s = 0; s < model.nbStates; s++ ) {
+ gmmpinv[s].MuI.Resize(nbIN);
+ gmmpinv[s].SigmaII.Resize(nbIN, nbIN);
+ gmmpinv[s].SigmaIIInv.Resize(nbIN, nbIN);
+ }
+
+ for (unsigned int s = 0; s < model.nbStates; s++ ) {
+ for ( int i = first_inindex; i <= last_inindex; i++ ) gmmpinv[s].MuI(i - first_inindex) = model.States[s].Mu(i);
+ for ( int i = first_inindex; i <= last_inindex; i++ )
+ for ( int j = first_inindex; j <= last_inindex; j++ )
+ gmmpinv[s].SigmaII(i - first_inindex, j - first_inindex) = model.States[s].Sigma(i, j);
+
+ gmmpinv[s].SigmaII.Inverse(gmmpinv[s].SigmaIIInv, &det);
+ //gmmpinv[s].SigmaIIInv = gmmpinv[s].SigmaII.Inverse();
+ //gmmpinv[s].SigmaII.Inverse(&det);
+ if (det < 0) det = 1e-30;
+ gmmpinv[s].detSigmaII = det;
+
+ }
+
+ nbDimI = last_inindex - first_inindex + 1;
+ gfDiff.Resize(nbDimI);
+ gfDiffp.Resize(nbDimI);
+ gDer.Resize(nbDimI);
+
+}
+
+double Gaussians::GaussianPDFFast(int state, Vector x)
+{
+ double p;
+ gfDiff = x - gmmpinv[state].MuI;
+ gfDiffp = gmmpinv[state].SigmaIIInv * gfDiff;
+
+ p = exp(-0.5 * gfDiff.Dot(gfDiffp)) / sqrt(pow(2.0 * PI, nbDimI) * ( gmmpinv[state].detSigmaII + 1e-30));
+
+ return p;
+}
+
+double Gaussians::GaussianProbFast(Vector x)
+{
+ double totalP = 0;
+ for (unsigned int s = 0; s < model.nbStates; s++ ) {
+ totalP += model.States[s].Prio * GaussianPDFFast(s, x);
+ }
+ return totalP;
+}
+
+Vector Gaussians::GaussianDerProbFast(Vector x)
+{
+ gDer.Zero();
+ for (unsigned int s = 0; s < model.nbStates; s++ ) {
+ gDer += (gmmpinv[s].SigmaIIInv * (x - gmmpinv[s].MuI)) * model.States[s].Prio * GaussianPDFFast(s, x);
+ }
+ return -gDer;
+}
+
+void Gaussians::InitFastGMR(int first_inindex, int last_inindex, int first_outindex, int last_outindex)
+{
+ double det;
+ int nbIN = last_inindex - first_inindex + 1;
+ int nbOUT = last_outindex - first_outindex + 1;
+
+ gPdf.Resize(model.nbStates);
+
+ for (unsigned int s = 0; s < model.nbStates; s++ ) {
+ gmmpinv[s].MuI.Resize(nbIN);
+ gmmpinv[s].SigmaII.Resize(nbIN, nbIN);
+ gmmpinv[s].SigmaIIInv.Resize(nbIN, nbIN);
+
+ gmmpinv[s].muO.Resize(nbOUT);
+ gmmpinv[s].SigmaIO.Resize(nbIN, nbOUT);
+ gmmpinv[s].SigmaIOInv.Resize(nbOUT, nbOUT);
+ }
+
+ for (unsigned int s = 0; s < model.nbStates; s++ ) {
+ for ( int i = first_inindex; i <= last_inindex; i++ ) {
+ gmmpinv[s].MuI(i - first_inindex) = model.States[s].Mu(i);
+
+ for ( int j = first_inindex; j <= last_inindex; j++ ) {
+ gmmpinv[s].SigmaII(i - first_inindex, j - first_inindex) = model.States[s].Sigma(i, j);
+ }
+ for ( int j = first_outindex; j <= last_outindex; j++ ) {
+ gmmpinv[s].SigmaIO(i - first_inindex, j - first_outindex) = model.States[s].Sigma(i, j);
+ }
+ }
+
+ for ( int i = first_outindex; i <= last_outindex; i++ ) {
+ gmmpinv[s].muO(i - first_outindex) = model.States[s].Mu(i);
+ }
+
+ gmmpinv[s].SigmaII.Inverse(gmmpinv[s].SigmaIIInv, &det);
+ if (det < 0) det = 1e-30;
+ gmmpinv[s].detSigmaII = det;
+ (gmmpinv[s].SigmaIO).Transpose().Inverse(gmmpinv[s].SigmaIOInv, &det);
+ }
+
+ nbDimI = last_inindex - first_inindex + 1;
+ gfDiff.Resize(nbDimI);
+ gfDiffp.Resize(nbDimI);
+ gDer.Resize(nbDimI);
+
+}
+
+void Gaussians::Regression(const Vector & indata, Vector & outdata, Matrix & derGMR)
+{
+ Regression(indata, outdata);
+ cout << "derivative is not implemented yet " << endl;
+}
+
+void Gaussians::Regression(const Vector & indata, Vector & outdata)
+{
+ double pdfall;
+ Vector h(model.nbStates);
+ Vector r_diff(outdata.Size());
+
+ for (unsigned int s = 0; s < model.nbStates; s++) {
+ gPdf(s) = model.States[s].Prio * GaussianPDFFast(s, indata);
+ }
+ pdfall = gPdf.Sum();
+
+ outdata.Zero();
+ for (unsigned int s = 0; s < model.nbStates; s++) {
+ //h(s) = gPdf(s)/(pdfall + 1e-30 );
+ h(s) = gPdf(s) / (pdfall );
+ r_diff = gmmpinv[s].SigmaIO.Transpose() * gmmpinv[s].SigmaIIInv * (indata - gmmpinv[s].MuI);
+
+ for (unsigned int i = 0; i < r_diff.Size(); i++ ) {
+ outdata(i) += h(s) * (r_diff(i) + gmmpinv[s].muO(i));
+ }
+ }
+}
+
+Vector Gaussians::Regression(const Vector & indata)
+{
+ Vector outdata(indata.Size());
+ Regression(indata, outdata);
+ return outdata;
+}
+
+
+/*
+#include <math.h>
+#include "Gaussians.h"
+
+#include "armadillo"
+
+using namespace arma;
+using namespace std;
+
+Gaussians::Gaussians(GMMs *model)
+{
+ this->model.nbStates = model->nbStates;
+ this->model.nbDim = model->nbDim;
+
+ this->model.States = (GMMState *)malloc(model->nbStates*sizeof(GMMState) );
+
+ for(int s=0; s<model->nbStates; s++ ){
+ this->model.States[s].Mu = model->GMMState[s].Mu;
+ this->model.States[s].Sigma = model->GMMState[s].Sigma;
+ this->model.States[s].Prio = model->GMMState[s].Prio;
+ }
+}
+
+Gaussians::Gaussians(int nbStates, int nbDim, char *f_mu, char *f_sigma, char *f_prio)
+{
+
+ int s, i, j;
+
+ model.nbStates = nbStates;
+ model.nbDim = nbDim;
+ model.States = (GMMState *)malloc(nbStates*sizeof(GMMState) );
+
+ for( s=0; s<nbStates; s++ ){
+ model.States[s].Mu = zeros<vec>(nbDim);
+ model.States[s].Sigma = zeros<mat>(nbDim, nbDim );
+ }
+
+ // f_mu
+ ifstream fin;
+ fin.open(f_mu);
+ for( i=0; i<nbDim; i++ ){
+ for( s=0; s<nbStates; s++ ){
+ fin >> model.States[s].Mu(i);
+ }
+ }
+ fin.close();
+
+ // f_sigma
+ fin.open(f_sigma);
+ for( s=0; s<nbStates; s++ ){
+ for( i=0; i<nbDim; i++ ){
+ for( j=0; j<nbDim; j++ ){
+ fin >> model.States[s].Sigma(i,j);
+ }
+ }
+ }
+ fin.close();
+
+ // f_prio
+ fin.open(f_prio);
+ for( s=0; s<nbStates; s++ ){
+ fin >>model.States[s].Prio;
+ }
+ fin.close();
+}
+
+void Gaussians::setGMMs(GMMs *model)
+{
+ for(int s=0; s<model->nbStates; s++ ){
+ this->model.States[s].Mu = model->GMMState[s].Mu;
+ this->model.States[s].Sigma = model->GMMState[s].Sigma;
+ this->model.States[s].Prio = model->GMMState[s].Prio;
+ }
+}
+
+
+void Gaussians::InitFastGaussians(int first_inindex, int last_inindex)
+{
+ gmmpinv = (GMMStateP *)malloc(model.nbStates*sizeof(GMMStateP) );
+
+ for(int s=0; s<model.nbStates; s++ ){
+ gmmpinv[s].MuI = model.States[s].Mu.subvec(first_inindex, last_inindex);
+ gmmpinv[s].SigmaII = model.States[s].Sigma.submat(first_inindex, first_inindex, last_inindex, last_inindex);
+ gmmpinv[s].SigmaIIInv = pinv(gmmpinv[s].SigmaII);
+ gmmpinv[s].detSigmaII = det(gmmpinv[s].SigmaII);
+ }
+
+ nbDimI = last_inindex - first_inindex +1;
+ gfDiff = zeros<vec>(nbDimI);
+ gfDiffp = zeros<vec>(nbDimI);
+ gDer = zeros<vec>(nbDimI);
+}
+
+double Gaussians::GaussianPDFFast(int state, vec x)
+{
+ double p;
+ gfDiff = x - gmmpinv[state].MuI;
+ gfDiffp = gmmpinv[state].SigmaIIInv * gfDiff;
+
+ p = exp(-0.5*dot(gfDiff, gfDiffp)) / sqrt(pow(2.0*math::pi(), nbDimI)*( gmmpinv[state].detSigmaII +1e-30));
+
+ return p;
+}
+
+double Gaussians::GaussianProbFast(vec x)
+{
+ double totalP=0;
+ for(int s=0; s<model.nbStates; s++ ){
+ totalP += model.States[s].Prio*GaussianPDFFast(s,x);
+ }
+ return totalP;
+}
+
+vec Gaussians::GaussianDerProbFast(vec x)
+{
+ gDer.zeros();
+ for(int s=0; s<model.nbStates; s++ ){
+ gDer += model.States[s].Prio * gmmpinv[s].SigmaIIInv *(x-gmmpinv[s].MuI)*GaussianPDFFast(s,x);
+ }
+ return -gDer;
+}
+
+//-------------------------------------------------------------------------------------------------------
+void AllocateGMMs(GMMs *model, int nbDim, int nbStates)
+{
+ model->nbDim = nbDim;
+ model->nbStates = nbStates;
+ model->GMMState = (GMMState *)malloc(nbStates*sizeof(GMMState) );
+
+ for(int s=0; s<nbStates; s++ ){
+ model->GMMState[s].Mu = zeros<vec>(nbDim);
+ model->GMMState[s].Sigma = zeros<mat>(nbDim, nbDim );
+ }
+}
+
+
+double GaussianPDF(vec x, vec Mu, mat Sigma)
+{
+ double p;
+ vec diff = x - Mu;
+ vec diffp = pinv( Sigma ) * diff;
+ int nbDim = x.size();
+
+ p = exp(-0.5*dot(diff, diffp)) / sqrt(pow(2.0*math::pi(), nbDim)*( abs(det(Sigma)) +1e-30));
+
+ if(p < 1e-30){
+ return 1e-30;
+ }
+ else{
+ return p;
+ }
+}
+
+void GaussianMux(GMMs *modelK, GMMs *modelL, GMMs *modelOut)
+{
+ int k,l,j;
+ int K = modelK->nbStates;
+ int L = modelL->nbStates;
+ int J = K*L;
+
+ //modelOut->nbDim = modelK->nbDim;
+ //modelOut->nbStates = J;
+ //modelOut->GMMState = (GMMState *)malloc(J*sizeof(GMMState) );
+
+ j=0;
+ for(k=0; k<K; k++){
+ for(l=0; l<L; l++){
+ modelOut->GMMState[j].Sigma = pinv( pinv(modelK->GMMState[k].Sigma) + pinv( modelL->GMMState[l].Sigma) );
+ modelOut->GMMState[j].Mu = modelOut->GMMState[j].Sigma *( pinv(modelK->GMMState[k].Sigma) * modelK->GMMState[k].Mu + pinv(modelL->GMMState[l].Sigma) * modelL->GMMState[l].Mu );
+ modelOut->GMMState[j].Prio = modelK->GMMState[k].Prio* modelL->GMMState[l].Prio * GaussianPDF( modelK->GMMState[k].Mu, modelL->GMMState[l].Mu, modelK->GMMState[k].Sigma + modelL->GMMState[l].Sigma);
+ j++;
+ }
+ }
+}
+
+void GaussianRotate(GMMs *model, arma::vec P, arma::mat R, GMMs *modelOut)
+{
+ for(int s=0; s<model->nbStates; s++){
+ modelOut->GMMState[s].Mu = R*model->GMMState[s].Mu + P;
+ modelOut->GMMState[s].Sigma = R*model->GMMState[s].Sigma*trans(R);
+ }
+ //modelOut->nbDim = model->nbDim;
+ //modelOut->nbStates = model->nbStates;
+}
+*/
diff --git a/source/RobotAPI/libraries/DSControllers/Gaussians.h b/source/RobotAPI/libraries/DSControllers/Gaussians.h
new file mode 100644
index 0000000000000000000000000000000000000000..b72890a832d7f81b619a24655b236daa869acbad
--- /dev/null
+++ b/source/RobotAPI/libraries/DSControllers/Gaussians.h
@@ -0,0 +1,83 @@
+/*
+ * Gaussians.h
+ *
+ * Created on: Nov 19, 2011
+ * Author: Seungsu KIM
+ */
+
+#ifndef __GAUSSIANSM_H__
+#define __GAUSSIANSM_H__
+
+#include "MathLib.h"
+
+#define GAUSSIAN_MAXIMUM_NUMBER 50
+
+using namespace MathLib;
+
+struct GMMState
+{
+ Vector Mu;
+ Matrix Sigma;
+ double Prio;
+};
+
+struct GMMStateP
+{
+ Vector MuI;
+ Matrix SigmaII;
+ Matrix SigmaIIInv;
+ double detSigmaII;
+
+ // for GMR
+ Vector muO;
+ Matrix SigmaIO;
+ Matrix SigmaIOInv;
+};
+
+struct GMMs
+{
+ unsigned int nbStates;
+ unsigned int nbDim;
+
+ GMMState States[GAUSSIAN_MAXIMUM_NUMBER];
+};
+
+class Gaussians
+{
+private:
+ GMMStateP gmmpinv[GAUSSIAN_MAXIMUM_NUMBER];
+
+public:
+ GMMs model;
+
+ Gaussians(const char* f_mu, const char* f_sigma, const char* f_prio);
+ Gaussians(int nbStates, int nbDim, const char* f_mu, const char* f_sigma, const char* f_prio);
+ Gaussians(const int nbStates, const int nbDim, const vector<double> pri_vec, const vector<double> mu_vec, const vector<double> sig_vec);
+ Gaussians(GMMs* model);
+
+ void setGMMs(GMMs* model);
+
+ // For fast computation of GaussianPDF
+ Vector gfDiff, gfDiffp;
+ Vector gDer;
+ Vector gPdf;
+ int nbDimI;
+
+
+ void InitFastGaussians(int first_inindex, int last_inindex);
+ double GaussianPDFFast(int state, Vector x);
+ double GaussianProbFast(Vector x);
+ Vector GaussianDerProbFast(Vector x);
+
+ void InitFastGMR(int first_inindex, int last_inindex, int first_outindex, int last_outindex);
+ void Regression(const Vector& indata, Vector& outdata, Matrix& derGMR);
+ void Regression(const Vector& indata, Vector& outdata);
+ Vector Regression(const Vector& indata);
+
+};
+/*
+void GaussianMux(GMMs *modelK, GMMs *modelL, GMMs *modelOut);
+void GaussianRotate(GMMs *model, Vector P, Matrix R, GMMs *modelOut);
+*/
+
+#endif //__GAUSSIANS_H__