diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt index 8e696dce3f8e6bc68e44dc1ac948e57e710bcdab..3615120f6c99c496a64c9a7ee17c7bbb2b70dc31 100644 --- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt +++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt @@ -3,6 +3,8 @@ set(LIB_NAME RobotUnit) armarx_component_set_name("${LIB_NAME}") armarx_set_target("Library: ${LIB_NAME}") +find_package(DMP QUIET) + find_package(Simox ${ArmarX_Simox_VERSION} QUIET) armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available") if (Simox_FOUND) @@ -118,6 +120,22 @@ set(LIB_HEADERS Devices/RTThreadTimingsSensorDevice.h ) +if (DMP_FOUND) + message(STATUS "DMP libraries is ${DMP_LIBRARIES}") + + include_directories(${DMP_INCLUDE_DIRS}) + link_directories(${DMP_LIB_DIRS}) + + list(APPEND LIB_HEADERS + NJointControllers/NJointJointSpaceDMPController.h + ) + list(APPEND LIB_FILES + NJointControllers/NJointJointSpaceDMPController.cpp) + + list(APPEND LIBS ${DMP_LIBRARIES}) +endif () + + add_subdirectory(test) armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}") diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.cpp new file mode 100644 index 0000000000000000000000000000000000000000..299888c1b3020199403bd17a42d8d256de9f21e3 --- /dev/null +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.cpp @@ -0,0 +1,158 @@ +#include "NJointJointSpaceDMPController.h" + + + +namespace armarx +{ + + NJointControllerRegistration<NJointJointSpaceDMPController> registrationControllerNJointJointSpaceDMPController("NJointJointSpaceDMPController"); + + std::string NJointJointSpaceDMPController::getClassName(const Ice::Current&) const + { + return "NJointJointSpaceDMPController"; + } + + NJointJointSpaceDMPController::NJointJointSpaceDMPController(armarx::NJointControllerDescriptionProviderInterfacePtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&) + { + NJointJointSpaceDMPControllerConfigPtr cfg = NJointJointSpaceDMPControllerConfigPtr::dynamicCast(config); + ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "Needed type: NJointJointSpaceDMPControllerConfigPtr"); + + for (std::string jointName : cfg->jointNames) + { + ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF); + const SensorValueBase* sv = prov->getSensorValue(jointName); + targets.insert(std::make_pair(jointName, ct->asA<ControlTarget1DoFActuatorVelocity>())); + positionSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorPosition>())); + torqueSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorTorque>())); + gravityTorqueSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFGravityTorque>())); + } + if (cfg->jointNames.size() == 0) + { + ARMARX_ERROR << "cfg->jointNames.size() == 0"; + } + + DMPAsForwardControl = cfg->DMPAsForwardControl; + dmpPtr.reset(new DMP::UMIDMP(cfg->kernelSize, cfg->Kd, cfg->baseMode, cfg->tau)); + timeDuration = cfg->timeDuration; + canVal = timeDuration; + finished = false; + + NJointJointSpaceDMPControllerControlData initData; + initData.tau = 1.0; + initData.isStart = false; + reinitTripleBuffer(initData); + } + + void NJointJointSpaceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) + { + if (rtGetControlStruct().isStart) + { + currentState.clear(); + for (size_t i = 0; i < dimNames.size(); i++) + { + const auto& jointName = dimNames.at(i); + DMP::DMPState currentPos; + currentPos.pos = (positionSensors.count(jointName) == 1) ? positionSensors[jointName]->position : 0.0f; + currentPos.vel = (velocitySensors.count(jointName) == 1) ? velocitySensors[jointName]->velocity : 0.0f; + currentState.push_back(currentPos); + } + + double tau = rtGetControlStruct().tau; + double deltaT = timeSinceLastIteration.toSecondsDouble(); + canVal -= 1 / tau * deltaT * 1 ; + double dmpDeltaT = deltaT / timeDuration; + dmpPtr->setTemporalFactor(tau); + + DMP::DVec targetState; + currentState = dmpPtr->calculateNextPointWithEulerMethod(currentState, canVal / timeDuration, dmpDeltaT, targetState, 1.0); + if (canVal < 1e-8) + { + finished = true; + } + + for (size_t i = 0; i < dimNames.size(); ++i) + { + const auto& jointName = dimNames.at(i); + if (targets.count(jointName) == 1) + { + targets[jointName]->velocity = finished ? 0.0f : currentState[i].vel; + } + } + } + } + + void NJointJointSpaceDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) + { + DMP::Vec<DMP::SampledTrajectoryV2 > trajs; + + DMP::DVec ratios; + for (size_t i = 0; i < fileNames.size(); ++i) + { + DMP::SampledTrajectoryV2 traj; + traj.readFromCSVFile(fileNames.at(i)); + dimNames = traj.getDimensionNames(); + traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1); + trajs.push_back(traj); + + if (i == 0) + { + ratios.push_back(1.0); + } + else + { + ratios.push_back(0.0); + } + } + dmpPtr->learnFromTrajectories(trajs); + dmpPtr->setOneStepMPC(true); + + + dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios); + + ARMARX_INFO << "Learned DMP ... "; + } + + void NJointJointSpaceDMPController::runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&) + { + currentState.clear(); + for (size_t i = 0; i < dimNames.size(); i++) + { + const auto& jointName = dimNames.at(i); + DMP::DMPState currentPos; + currentPos.pos = (positionSensors.count(jointName) == 1) ? positionSensors[jointName]->position : 0.0f; + currentPos.vel = (velocitySensors.count(jointName) == 1) ? velocitySensors[jointName]->velocity : 0.0f; + currentState.push_back(currentPos); + } + dmpPtr->prepareExecution(goals, currentState, 1, tau); + finished = false; + + this->goals = goals; + this->tau = tau; + + LockGuardType guard {controlDataMutex}; + getWriterControlStruct().tau = tau; + getWriterControlStruct().isStart = true; + writeControlStruct(); + + } + + void NJointJointSpaceDMPController::setTemporalFactor(double tau, const Ice::Current&) + { + this->tau = tau; + LockGuardType guard {controlDataMutex}; + getWriterControlStruct().tau = tau; + getWriterControlStruct().isStart = true; + writeControlStruct(); + } + + void NJointJointSpaceDMPController::rtPreActivateController() + { + } + + void NJointJointSpaceDMPController::rtPostDeactivateController() + { + + } + + +} diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.h new file mode 100644 index 0000000000000000000000000000000000000000..89d1395dcb27ca170b25d9631e1a9cfc8da07d48 --- /dev/null +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.h @@ -0,0 +1,92 @@ + +#pragma once + + +#include "NJointController.h" +#include <VirtualRobot/Robot.h> +#include "../RobotUnit.h" +#include "../ControlTargets/ControlTarget1DoFActuator.h" +#include "../SensorValues/SensorValue1DoFActuator.h" +#include <dmp/representation/dmp/umidmp.h> +#include <RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.h> + +namespace armarx +{ + + TYPEDEF_PTRS_HANDLE(NJointJointSpaceDMPController); + + TYPEDEF_PTRS_HANDLE(NJointJointSpaceDMPControllerControlData); + class NJointJointSpaceDMPControllerControlData + { + public: + double tau; + bool isStart; + }; + + + // class SimplePID + // { + // public: + // float Kp = 0, Kd = 0; + // float lastError = 0; + // float update(float dt, float error); + // }; + + class NJointJointSpaceDMPController : + public NJointControllerWithTripleBuffer<NJointJointSpaceDMPControllerControlData>, + public NJointJointSpaceDMPControllerInterface + { + public: + using ConfigPtrT = NJointJointSpaceDMPControllerConfigPtr; + NJointJointSpaceDMPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&); + + // NJointControllerInterface interface + std::string getClassName(const Ice::Current&) const override; + + // NJointController interface + void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override; + + // static NJointCartesianVelocityControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values); + // NJointCartesianVelocityController( + // NJointControllerDescriptionProviderInterfacePtr prov, + // NJointCartesianVelocityControllerConfigPtr config, + // const boost::shared_ptr<VirtualRobot::Robot>& r); + + + // + bool isFinished(const Ice::Current&) + { + return finished; + } + + void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&); + void setTemporalFactor(double tau, const Ice::Current&); + + void runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&); + + protected: + void rtPreActivateController() override; + void rtPostDeactivateController() override; + private: + std::map<std::string, const SensorValue1DoFActuatorTorque*> torqueSensors; + std::map<std::string, const SensorValue1DoFGravityTorque*> gravityTorqueSensors; + std::map<std::string, const SensorValue1DoFActuatorPosition*> positionSensors; + std::map<std::string, const SensorValue1DoFActuatorVelocity*> velocitySensors; + std::map<std::string, ControlTarget1DoFActuatorVelocity*> targets; + + + std::vector<double> goals; + DMP::UMIDMPPtr dmpPtr; + bool DMPAsForwardControl; + double timeDuration; + + double canVal; + + double tau; + double finished; + std::vector<std::string> dimNames; + DMP::Vec<DMP::DMPState> currentState; + }; + +} // namespace armarx + diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt index 3ac499d6b68e99f31f99da4678ae5091422825e1..8bc308172ea96dae58ca1f38d680cdb57b75c441 100644 --- a/source/RobotAPI/interface/CMakeLists.txt +++ b/source/RobotAPI/interface/CMakeLists.txt @@ -47,6 +47,8 @@ set(SLICE_FILES units/RobotUnit/NJointCartesianActiveImpedanceController.ice units/RobotUnit/RobotUnitInterface.ice + units/RobotUnit/NJointJointSpaceDMPController.ice + components/ViewSelectionInterface.ice components/TrajectoryPlayerInterface.ice components/RobotNameServiceInterface.ice diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice new file mode 100644 index 0000000000000000000000000000000000000000..4faa50476deb917d8321f2fbf9e8423aeb7fc50d --- /dev/null +++ b/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice @@ -0,0 +1,52 @@ +/* + * 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 + */ + +#ifndef _ARMARX_ROBOTAPI_NJointTrajectoryControllerInterface_SLICE_ +#define _ARMARX_ROBOTAPI_NJointTrajectoryControllerInterface_SLICE_ + +#include <RobotAPI/interface/units/RobotUnit/NJointController.ice> +#include <RobotAPI/interface/core/Trajectory.ice> + +module armarx +{ + class NJointJointSpaceDMPControllerConfig extends NJointControllerConfig + { + Ice::StringSeq jointNames; + float Kd = 20; + int kernelSize = 100; + double tau = 1; + int baseMode = 1; + + double timeDuration = 10; + bool DMPAsForwardControl = true; + }; + + + interface NJointJointSpaceDMPControllerInterface extends NJointControllerInterface + { + void learnDMPFromFiles(Ice::StringSeq trajfiles); + bool isFinished(); + void runDMP(Ice::DoubleSeq goals, double tau); + void setTemporalFactor(double tau); + }; +}; +#endif diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt index 0e93b7eddc1a18bd7a2ed0ffa088fe4500e2afeb..dc91c6a504edfc82e05c44ed64af3367e00ade53 100644 --- a/source/RobotAPI/libraries/core/CMakeLists.txt +++ b/source/RobotAPI/libraries/core/CMakeLists.txt @@ -2,6 +2,7 @@ armarx_set_target("RobotAPI Core Library: RobotAPICore") find_package(Eigen3 QUIET) find_package(Simox ${ArmarX_Simox_VERSION} QUIET) +#find_package(DMP QUIET) armarx_build_if(Eigen3_FOUND "Eigen3 not available") armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available") @@ -78,6 +79,24 @@ set(LIB_HEADERS CartesianPositionController.h ) + +#if (DMP_FOUND) +# message(STATUS "DMP libraries is ${DMP_LIBRARIES}") + +# include_directories(${DMP_INCLUDE_DIRS}) +# link_directories(${DMP_LIB_DIRS}) + +# list(APPEND LIB_HEADERS +# DMPController.h +# ) +# list(APPEND LIB_FILES +# DMPController.cpp +# ) + +# list(APPEND LIBS ${DMP_LIBRARIES}) +#endif () + + add_subdirectory(test) armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")