From a6d03d1b9a957caac4a83851c687b6854fe9f37b Mon Sep 17 00:00:00 2001
From: ArmarX User <armarx@kit.edu>
Date: Thu, 10 Oct 2019 10:16:47 +0200
Subject: [PATCH] fix joint position dmp when rt starts without setting a goal
---
.../DMPController/NJointJSPositionDMPController.cpp | 9 ++++++++-
1 file changed, 8 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSPositionDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSPositionDMPController.cpp
index 586c4a6b7..cb97cb2e8 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSPositionDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSPositionDMPController.cpp
@@ -219,7 +219,14 @@ namespace armarx
// {
// targets[dimNames[i]]->velocity = targetJointVels[i];
// } std::map<std::string, ControlTarget1DoFActuatorVelocity*> targets;
- targets[dimNames[i]]->position = targetJointAngles[i];
+ if (started)
+ {
+ targets[dimNames[i]]->position = targetJointAngles[i];
+ }
+ else
+ {
+ targets[dimNames[i]]->position = qpos[i];
+ }
}
--
GitLab