diff --git a/etc/cmake/FindNLOPT.cmake b/etc/cmake/FindNLOPT.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..c73967496e4bbb330c7a2f6e17b84c668e261833
--- /dev/null
+++ b/etc/cmake/FindNLOPT.cmake
@@ -0,0 +1,41 @@
+# Copyright (c) 2015, Georgia Tech Graphics Lab and Humanoid Robotics Lab
+# This file is provided under the "BSD-style" License
+
+# Find NLOPT
+#
+# This sets the following variables:
+# NLOPT_FOUND
+# NLOPT_INCLUDE_DIRS
+# NLOPT_LIBRARIES
+# NLOPT_DEFINITIONS
+# NLOPT_VERSION
+
+find_package(PkgConfig QUIET)
+
+# Check to see if pkgconfig is installed.
+pkg_check_modules(PC_NLOPT nlopt QUIET)
+
+# Definitions
+set(NLOPT_DEFINITIONS ${PC_NLOPT_CFLAGS_OTHER})
+
+# Include directories
+find_path(NLOPT_INCLUDE_DIRS
+ NAMES nlopt.h
+ HINTS ${PC_NLOPT_INCLUDEDIR}
+ PATHS "${CMAKE_INSTALL_PREFIX}/include")
+
+# Libraries
+find_library(NLOPT_LIBRARIES
+ NAMES nlopt nlopt_cxx
+ HINTS ${PC_NLOPT_LIBDIR})
+
+# Version
+set(NLOPT_VERSION ${PC_NLOPT_VERSION})
+
+# Set (NAME)_FOUND if all the variables and the version are satisfied.
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(NLOPT
+ FAIL_MESSAGE DEFAULT_MSG
+ REQUIRED_VARS NLOPT_INCLUDE_DIRS NLOPT_LIBRARIES
+ VERSION_VAR NLOPT_VERSION)
+
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 6b32bcb8a4a0302482b3014f5c213abf9a9d3ca4..5678e8e939f29de9edd16e075d0a3cbfee967d38 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -52,6 +52,7 @@ set(SLICE_FILES
units/RobotUnit/NJointJointSpaceDMPController.ice
units/RobotUnit/NJointTaskSpaceDMPController.ice
+ units/RobotUnit/NJointBimanualForceMPController.ice
units/RobotUnit/TaskSpaceActiveImpedanceControl.ice
components/ViewSelectionInterface.ice
diff --git a/source/RobotAPI/interface/components/RobotNameServiceInterface.ice b/source/RobotAPI/interface/components/RobotNameServiceInterface.ice
index ee0d94b95bb6f23b3e99cc9bb175dab644bbdb3c..3478d40669895361a857ba972167bb85c131d0bc 100644
--- a/source/RobotAPI/interface/components/RobotNameServiceInterface.ice
+++ b/source/RobotAPI/interface/components/RobotNameServiceInterface.ice
@@ -21,8 +21,7 @@
#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.ice>
-#ifndef _ARMARX_ROBOTAPI_COMPONENTS_ROBOT_NAME_SERVICE_SLICE_
-#define _ARMARX_ROBOTAPI_COMPONENTS_ROBOT_NAME_SERVICE_SLICE_
+#pragma once
module armarx
{
@@ -30,5 +29,3 @@ module armarx
{
};
};
-
-#endif
diff --git a/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice b/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice
index 57f0dcfd505c981c073dcf421bcb817b8dda8fc7..3e0f1eded9f05946c493584146891e8bc01632d0 100644
--- a/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice
+++ b/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice
@@ -20,12 +20,11 @@
* GNU General Public License
*/
+#pragma once
+
#include <RobotAPI/interface/core/Trajectory.ice>
#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.ice>
-#ifndef _ARMARX_ROBOTAPI_SUBUNITS_TRAJECTORYCONTROLLER_SLICE_
-#define _ARMARX_ROBOTAPI_SUBUNITS_TRAJECTORYCONTROLLER_SLICE_
-
module armarx
{
interface TrajectoryPlayerInterface extends KinematicUnitListener
@@ -53,5 +52,3 @@ module armarx
void considerConstraints(bool consider);
};
};
-
-#endif
diff --git a/source/RobotAPI/interface/core/moved_to_robotcomponents_RobotIK.ice b/source/RobotAPI/interface/core/moved_to_robotcomponents_RobotIK.ice
deleted file mode 100644
index b2b7f6c407436e1df168b86abe738b409e083153..0000000000000000000000000000000000000000
--- a/source/RobotAPI/interface/core/moved_to_robotcomponents_RobotIK.ice
+++ /dev/null
@@ -1,291 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package
- * @author
- * @date
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#pragma once
-
-#include <RobotAPI/interface/core/FramedPoseBase.ice>
-#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
-
-module armarx
-{
- /**
- * Available Cartesian selection options for target poses.
- * See VirtualRobot::IKSolver for more information.
- */
- enum CartesianSelection
- {
- eX,
- eY,
- eZ,
- ePosition,
- eOrientation,
- eAll
- };
-
- sequence<string> NodeNameList;
-
- struct WorkspaceBounds {
- float x;
- float y;
- float z;
- float ro;
- float pi;
- float ya;
- };
-
- enum InverseJacobiMethod
- {
- eSVD,
- eSVDDamped,
- eTranspose,
- };
-
- struct CoMIKDescriptor
- {
- // REQUIRED:
- int priority; // priority for hierarchical ik
- float gx; // desired center of mass x
- float gy; // desired center of mass y
- string robotNodeSetBodies; // body set for which the center of mass is computed
- float tolerance; // error tolerance
- // OPTIONAL:
- string coordSysName; // coordinate system name in which the center of mass is defined; empty string for global system
- };
-
- struct DifferentialIKDescriptor
- {
- // REQUIRED:
- int priority;
- string tcpName;
- PoseBase tcpGoal;
- CartesianSelection csel;
- InverseJacobiMethod ijm;
- // OPTIONAL
- string coordSysName;
- };
-
- struct ExtendedIKResult
- {
- bool isReachable;
- float error;
- NameValueMap jointAngles;
- };
-
- sequence<DifferentialIKDescriptor> IKTasks;
-
- interface RobotIKInterface
- {
-
-
- // TODO: throw exceptions
- /**
- * @brief Computes a single IK solution for the given robot node set and desired TCP pose.
- * @details The TCP pose can be defined in any frame and is converted internally to a global pose
- * according to the current robot state. The CartesianSelection
- * parameter defines which part of the target pose should be reached.
- *
- * @param robotNodeSetName The name of a robot node set (robot node set) that is either stored
- * within the robot model or has been defined via \ref defineRobotNodeSet.
- * @param tcpPose The framed target pose for the TCP.
- * @param cs Specifies which part of the pose needs to be reached by the IK, e.g. the position only,
- * the orientation only or all.
- * @return A map that maps each joint name to its value in the found IK solution.
- */
- ["cpp:const"] idempotent NameValueMap computeIKFramedPose(string robotNodeSetName,
- FramedPoseBase tcpPose,
- CartesianSelection cs);
-
- /**
- * @brief Computes a single IK solution for the given robot node set and desired global TCP pose.
- * @details The TCP pose is assumed to be in the global frame. The CartesianSelection
- * parameter defines which part of the target pose should be reached.
- *
- * @param robotNodeSetName The name of a robot node set (robot node set) that is either stored
- * within the robot model or has been defined via \ref defineRobotNodeSet.
- * @param tcpPose The global target pose for the TCP.
- * @param cs Specifies which part of the pose needs to be reached by the IK, e.g. the position only,
- * the orientation only or all.
- * @return A map that maps each joint name to its value in the found IK solution.
- */
- ["cpp:const"] idempotent NameValueMap computeIKGlobalPose(string robotNodeSetName,
- PoseBase tcpPose,
- CartesianSelection cs);
-
- /**
- * @brief Computes a single IK solution, error and reachability for the given robot node set and desired global TCP pose.
- * @details The TCP pose is assumed to be in the global frame. The CartesianSelection
- * parameter defines which part of the target pose should be reached.
- *
- * @param robotNodeSetName The name of a robot node set (robot node set) that is either stored
- * within the robot model or has been defined via \ref defineRobotNodeSet.
- * @param tcpPose The global target pose for the TCP.
- * @param cs Specifies which part of the pose needs to be reached by the IK, e.g. the position only,
- * the orientation only or all.
- * @return A map that maps each joint name to its value in the found IK solution, the reachability and computational error.
- */
- ["cpp:const"] idempotent ExtendedIKResult computeExtendedIKGlobalPose(string robotNodeSetName,
- PoseBase tcpPose,
- CartesianSelection cs);
-
- /**
- * @brief Computes an IK solution for the given robot joints such that the center of mass lies above the
- * given point.
- * @details
- *
- * @param robotNodeSetJoints Name of the robot node set that contains the joints you wish to compute the IK for.
- * @param comIK A center of mass description. Note that the priority field is relevant for this function as there
- * is only a single CoM of descriptor.
- *
- * @return The ik-solution. Returns an empty vector if there is no solution.
- */
- ["cpp:const"] idempotent NameValueMap computeCoMIK(string robotNodeSetJoints,
- CoMIKDescriptor comIK);
-
- /**
- * @brief Computes a configuration gradient in order to solve several tasks/constraints simultaneously.
- * @details This function allows you to use the HierarchicalIK solver provided by Simox.
- * It computes a configuration gradient for the given robot node set that minimizes the workspace errors
- * for multiple tasks simultaneously. You can specify IK tasks, a center of mass task and a joint limit avoidance task.
- * For details for the different type of tasks, see the parameter description. You must define a priority for each task.
- * The task with maximal priority is the first task to be solved. Each subsequent task is then solved
- * within the null space of the previous task. <b> Note that this function returns a gradient and NOT
- * an absolute IK solution. The gradient is computed on the current configuration of the robot. <\b>.
- * The gradient is computed from the current robot configuration.
- *
- * See @url http://simox.sourceforge.net/documentation/class_virtual_robot_1_1_hierarchical_i_k.html
- * and @url http://sourceforge.net/p/simox/wiki/VirtualRobot/#hierarchical-ik-solving for more information.
- *
- * @param robotNodeSet The robot node set (e.g. kinematic tree) you wish to compute the gradient for.
- * @param diffIKs A list of IK tasks. Each IK task specifies a TCP and a desired pose for this TCP.
- * @param comIK A center of mass tasks. Defines where the center should be and its priority. Is only used if the
- CoM-task is enabled.
- * @param stepSize
- * @param avoidJointLimits Set whether or not to avoid joint limits.
- * @param enableCenterOfMass Set whether or not to adjust the center of mass.
- * @return A configuration gradient...
- */
- ["cpp:const"] idempotent NameValueMap computeHierarchicalDeltaIK(string robotNodeSet,
- IKTasks diffIKs,
- CoMIKDescriptor comIKs,
- float stepSize,
- bool avoidJointLimits,
- bool enableCenterOfMass);
- /**
- * @brief Defines a new robot node set.
- * @details Define a new robot node set with the given name that consists out of the given list of nodes with given
- * TCP and root frame. Iff the chain is successfully added or already exists, <it>true</it> is returned.
- * @param name The name of the robot node set.
- * @param nodes The list of robot nodes that make up the robot node set.
- * @param tcpName The name of the TCP node.
- * @param rootNode The name of the kinematic root.
- *
- * @return True, iff chain was added or already exists. False, iff a different chain with similar name already exists.
- */
- bool defineRobotNodeSet(string name, NodeNameList nodes, string tcpName, string rootNode);
-
- /**
- * @brief Creates a new reachability space for the given robot node set.
- * @details If there is no reachability space for the given robot node set yet, a new one is created. This may take
- * some time. The function returns true iff a reachability space for the given robot node set exists after
- * execution of this function.
- *
- * @param chainName The name of a defined robot node set. This can be either a robot node set that is defined in the
- * robot model or a robot node set that has been manually defined calling \ref defineRobotNodeSet.
- * @param coordinateSystem The name of the robot node in whose coordinate system the reachability space shall be defined in. If you wish to choose the global coordinate system, pass an empty string. Note, however, that any reachability space defined in the global coordinate system gets invalidated once the robot moves.
- * @param stepTranslation The extend of a voxel dimension in translational dimensions (x,y,z) [mm]
- * @param stepRotation The extend of a voxel dimension in rotational dimensions (roll,pitch,yaw) [rad]
- * @param minBounds The minimum workspace poses (x,y,z,ro,pi,ya) given in the base node's coordinate system [mm and rad]
- * @param maxBounds The maximum workspace poses (x,y,z,ro,pi,ya) given in base node's coordinate system [mm and rad]
- * @param numSamples The number of tcp samples to take to create the reachability space (e.g 1000000)
- * @return True iff the a reachability space for the given robot node set is available after execution of this function.
- * False in case of a failure, e.g. there is no chain with the given name.
- */
- bool createReachabilitySpace(string chainName, string coordinateSystem, float stepTranslation, float stepRotation,
- WorkspaceBounds minBounds, WorkspaceBounds maxBounds, int numSamples);
-
- /**
- * @brief Returns whether this component has a reachability space for the given robot node set.
- * @details True if there is a reachability space available, else false.
- *
- * @param chainName Name of the robot node set.
- * @return True if there is a reachability space available, else false.
- */
- ["cpp:const"] idempotent bool hasReachabilitySpace(string chainName);
-
- /**
- * @brief Loads the reachability space from the given file.
- * @details If loading the reachability space succeeds, the reachability space is ready to be used after this function
- * terminates. A reachability space can only be loaded if there is no reachability space for the respective
- * robot node set yet.
- *
- * @param filename Binary file containing a reachability space. Ensure that the path is valid and accessible from
- * where this component is running.
- * @return True iff loading the reachability space was successful.
- */
- bool loadReachabilitySpace(string filename);
-
- /**
- * @brief Saves a previously created reachability space of the given robot node set.
- * @details A reachability space for a robot node set can only be saved, if actually is one.
- * You can check whether a reachability space is available by calling \ref hasReachabilitySpace(robotNodeSet).
- *
- * @param robotNodeSet The robot node set for which you wish to save the reachability space.
- * @param filename The filename if the file(must be an accessible path for this component) you wish to save the space in.
- * @return True iff saving was successful.
- */
- ["cpp:const"] idempotent bool saveReachabilitySpace(string robotNodeSet, string filename);
-
- /**
- * @brief Returns whether a given framed pose is currently reachable by the TCP of the given robot node set.
- * @details To determine whether a pose is reachable a reachability space for the given robot node set is
- * required. If no such space exists, the function returns <it>false<\it>.
- * Call \ref createReachabilitySpace first to ensure there is such a space.
- *
- * @param chainName A name of a robot node set either defined in the robot model or previously defined via
- * \ref defineRobotNodeSet.
- * @param framedPose A framed pose to check for reachability. The pose is transformed into a global pose using the
- * current robot state.
- *
- * @return True, if the pose is reachable by the TCP of the given chain. False, if it is not reachable or
- * there is no reachability space for the given chain available.
- */
- ["cpp:const"] idempotent bool isFramedPoseReachable(string chainName, FramedPoseBase framedPose);
-
- /**
- * @brief Returns whether a given global pose is currently reachable by the TCP of the given robot node set.
- * @details To determine whether a pose is reachable a reachability space for the given robot node set is
- * required. If no such space exists, the function returns <it>false<\it>.
- * Call \ref createReachabilitySpace first to ensure there is such a space.
- *
- * @param chainName A name of a robot node set either defined in the robot model or previously defined via
- * \ref defineRobotNodeSet.
- * @param pose A global pose to check for reachability.
- *
- * @return True, if the pose is reachable by the TCP of the given chain. False, if it is not reachable or
- * there is no reachability space for the given chain available.
- */
- ["cpp:const"] idempotent bool isPoseReachable(string chainName, PoseBase framedPose);
-
-
- };
-};
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.ice
new file mode 100644
index 0000000000000000000000000000000000000000..49acfeefb9fec34ea497c6209d8a0b1c3c846c9b
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.ice
@@ -0,0 +1,84 @@
+/*
+ * 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>
+#include <RobotAPI/interface/core/Trajectory.ice>
+
+module armarx
+{
+ class NJointBimanualForceMPControllerConfig extends NJointControllerConfig
+ {
+
+ // dmp configuration
+ int kernelSize = 100;
+ string dmpMode = "MinimumJerk";
+ string dmpStyle = "Discrete";
+ double dmpAmplitude = 1;
+
+ double phaseL = 10;
+ double phaseK = 10;
+ double phaseDist0 = 50;
+ double phaseDist1 = 10;
+ double phaseKpPos = 1;
+ double phaseKpOri = 0.1;
+ double timeDuration = 10;
+ double posToOriRatio = 100;
+
+ double maxLinearVel;
+ double maxAngularVel;
+
+ // velocity control
+ float Kp_LinearVel;
+ float Kd_LinearVel;
+ float Kp_AngularVel;
+ float Kd_AngularVel;
+
+ // force control
+ float forceP;
+ float forceI;
+ float forceD;
+
+ float filterCoeff;
+ float KpJointLimitAvoidanceScale;
+
+ float targetSupportForce;
+ Ice::FloatSeq leftForceOffset;
+ Ice::FloatSeq rightForceOffset;
+
+ string debugName;
+
+ };
+
+
+ interface NJointBimanualForceMPControllerInterface extends NJointControllerInterface
+ {
+ void learnDMPFromFiles(string whichMP, Ice::StringSeq trajfiles);
+ bool isFinished();
+ void runDMP(Ice::DoubleSeq leftGoals, Ice::DoubleSeq rightGoals);
+ double getCanVal();
+
+ void setViaPoints(string whichDMP, double canVal, Ice::DoubleSeq viaPoint);
+ };
+};
+
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
index 73fa0080373f5152050fd873f7aa337f8851aab2..74ef54267526a33c4082dc468ffe885985253d94 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
@@ -46,6 +46,7 @@ if (DMP_FOUND )
DMPController/NJointCCDMPController.h
DMPController/NJointBimanualCCDMPController.h
DMPController/NJointTaskSpaceImpedanceDMPController.h
+ DMPController/NJointBimanualForceMPController.h
)
list(APPEND LIB_FILES
@@ -55,6 +56,8 @@ if (DMP_FOUND )
DMPController/NJointCCDMPController.cpp
DMPController/NJointBimanualCCDMPController.cpp
DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+ DMPController/NJointBimanualForceMPController.cpp
+
)
list(APPEND LIBS ${DMP_LIBRARIES} DMPController)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eeb9ebe8f80df2f0775ad22d8b1156e913dc9d3e
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp
@@ -0,0 +1,496 @@
+#include "NJointBimanualForceMPController.h"
+#include <random>
+
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointBimanualForceMPController> registrationControllerNJointBimanualForceMPController("NJointBimanualForceMPController");
+
+ NJointBimanualForceMPController::NJointBimanualForceMPController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ useSynchronizedRtRobot();
+ cfg = NJointBimanualForceMPControllerConfigPtr::dynamicCast(config);
+
+ VirtualRobot::RobotNodeSetPtr lrns = rtGetRobot()->getRobotNodeSet("LeftArm");
+ for (size_t i = 0; i < lrns->getSize(); ++i)
+ {
+ std::string jointName = lrns->getNode(i)->getName();
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+
+ if (!velocitySensor)
+ {
+ ARMARX_WARNING << "No velocity sensor available for " << jointName;
+ }
+ if (!positionSensor)
+ {
+ ARMARX_WARNING << "No position sensor available for " << jointName;
+ }
+
+ leftVelocitySensors.push_back(velocitySensor);
+ leftPositionSensors.push_back(positionSensor);
+ };
+
+ VirtualRobot::RobotNodeSetPtr rrns = rtGetRobot()->getRobotNodeSet("RightArm");
+ for (size_t i = 0; i < rrns->getSize(); ++i)
+ {
+ std::string jointName = rrns->getNode(i)->getName();
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+
+ if (!velocitySensor)
+ {
+ ARMARX_WARNING << "No velocity sensor available for " << jointName;
+ }
+ if (!positionSensor)
+ {
+ ARMARX_WARNING << "No position sensor available for " << jointName;
+ }
+
+ rightVelocitySensors.push_back(velocitySensor);
+ rightPositionSensors.push_back(positionSensor);
+ };
+ const SensorValueBase* svlf = robUnit->getSensorDevice("FT L")->getSensorValue();
+ leftForceTorque = svlf->asA<SensorValueForceTorque>();
+ const SensorValueBase* svrf = robUnit->getSensorDevice("FT R")->getSensorValue();
+ rightForceTorque = svrf->asA<SensorValueForceTorque>();
+
+ leftIK.reset(new VirtualRobot::DifferentialIK(lrns, lrns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ rightIK.reset(new VirtualRobot::DifferentialIK(rrns, rrns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+
+ leftTCP = lrns->getTCP();
+ rightTCP = rrns->getTCP();
+
+ leftLearned = false;
+ rightLearned = false;
+
+ // set tcp controller
+ leftTCPController.reset(new CartesianVelocityController(lrns, leftTCP));
+ rightTCPController.reset(new CartesianVelocityController(rrns, rightTCP));
+
+
+ finished = false;
+ TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
+ taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
+ taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
+ taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
+ taskSpaceDMPConfig.DMPStyle = cfg->dmpStyle;
+ taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
+ taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
+ taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
+ taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
+ taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
+ taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
+ taskSpaceDMPConfig.phaseStopParas.Dori = 0;
+ taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
+ taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
+ taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
+ leftDMPController.reset(new TaskSpaceDMPController("Left", taskSpaceDMPConfig, false));
+ rightDMPController.reset(new TaskSpaceDMPController("Right", taskSpaceDMPConfig, false));
+
+
+ // initialize tcp position and orientation
+ NJointBimanualForceMPControllerSensorData initSensorData;
+ initSensorData.leftPoseInRootFrame = leftTCP->getPoseInRootFrame();
+ initSensorData.leftTwistInRootFrame.setZero();
+ initSensorData.rightPoseInRootFrame = rightTCP->getPoseInRootFrame();
+ initSensorData.rightTwistInRootFrame.setZero();
+ initSensorData.leftForceInRootFrame.setZero();
+ initSensorData.rightForceInRootFrame.setZero();
+ controllerSensorData.reinitAllBuffers(initSensorData);
+
+ NJointBimanualForceMPControllerControlData initData;
+ initData.leftTargetPose = leftTCP->getPoseInRootFrame();
+ initData.rightTargetPose = rightTCP->getPoseInRootFrame();
+ reinitTripleBuffer(initData);
+
+
+ Kp_LinearVel = cfg->Kp_LinearVel;
+ Kp_AngularVel = cfg->Kp_AngularVel;
+ Kd_LinearVel = cfg->Kd_LinearVel;
+ Kd_AngularVel = cfg->Kd_AngularVel;
+
+ forceIterm = 0;
+ I_decay = 0.9;
+ xvel = 0;
+
+ leftFilteredValue.setZero();
+ rightFilteredValue.setZero();
+
+ for (size_t i = 0; i < 3; ++i)
+ {
+ leftForceOffset(i) = cfg->leftForceOffset.at(i);
+ }
+ for (size_t i = 0; i < 3; ++i)
+ {
+ rightForceOffset(i) = cfg->rightForceOffset.at(i);
+ }
+
+
+ targetSupportForce = cfg->targetSupportForce;
+
+ NJointBimanualForceMPControllerInterfaceData initInterfaceData;
+ initInterfaceData.leftTcpPoseInRootFrame = leftTCP->getPoseInRootFrame();
+ initInterfaceData.rightTcpPoseInRootFrame = rightTCP->getPoseInRootFrame();
+ interfaceData.reinitAllBuffers(initInterfaceData);
+ }
+
+ std::string NJointBimanualForceMPController::getClassName(const Ice::Current&) const
+ {
+ return "NJointBimanualForceMPController";
+ }
+
+ void NJointBimanualForceMPController::controllerRun()
+ {
+ if (!controllerSensorData.updateReadBuffer() || !leftDMPController || !rightDMPController)
+ {
+ return;
+ }
+ double deltaT = controllerSensorData.getReadBuffer().deltaT;
+ Eigen::Matrix4f leftPose = controllerSensorData.getReadBuffer().leftPoseInRootFrame;
+ Eigen::Matrix4f rightPose = controllerSensorData.getReadBuffer().rightPoseInRootFrame;
+ Eigen::Vector6f leftTwist = controllerSensorData.getReadBuffer().leftTwistInRootFrame;
+ Eigen::Vector6f rightTwist = controllerSensorData.getReadBuffer().rightTwistInRootFrame;
+ Eigen::Vector3f leftForce = controllerSensorData.getReadBuffer().leftForceInRootFrame;
+ Eigen::Vector3f rightForce = controllerSensorData.getReadBuffer().rightForceInRootFrame;
+
+ float forceOnHands = (leftForce + rightForce)(2) ;
+
+ xvel = cfg->forceP * (targetSupportForce + forceOnHands) + I_decay * cfg->forceI * forceIterm;
+
+ forceIterm += targetSupportForce - forceOnHands;
+
+ canVal = canVal + forceSign * xvel * deltaT;
+ // canVal = cfg->timeDuration + forceSign * xvel;
+
+
+ if (canVal > cfg->timeDuration)
+ {
+ canVal = cfg->timeDuration;
+ }
+
+
+ leftDMPController->canVal = canVal;
+ rightDMPController->canVal = canVal;
+
+ leftDMPController->flow(deltaT, leftPose, leftTwist);
+ rightDMPController->flow(deltaT, rightPose, rightTwist);
+
+ if (canVal < 1e-8)
+ {
+ finished = true;
+ }
+
+
+ std::vector<double> leftTargetState = leftDMPController->getTargetPose();
+ debugOutputData.getWriteBuffer().dmpTargets["leftTarget_x"] = leftTargetState[0];
+ debugOutputData.getWriteBuffer().dmpTargets["leftTarget_y"] = leftTargetState[1];
+ debugOutputData.getWriteBuffer().dmpTargets["leftTarget_z"] = leftTargetState[2];
+ debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qw"] = leftTargetState[3];
+ debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qx"] = leftTargetState[4];
+ debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qy"] = leftTargetState[5];
+ debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qz"] = leftTargetState[6];
+ std::vector<double> rightTargetState = rightDMPController->getTargetPose();
+ debugOutputData.getWriteBuffer().dmpTargets["rightTarget_x"] = rightTargetState[0];
+ debugOutputData.getWriteBuffer().dmpTargets["rightTarget_y"] = rightTargetState[1];
+ debugOutputData.getWriteBuffer().dmpTargets["rightTarget_z"] = rightTargetState[2];
+ debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qw"] = rightTargetState[3];
+ debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qx"] = rightTargetState[4];
+ debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qy"] = rightTargetState[5];
+ debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qz"] = rightTargetState[6];
+
+
+ {
+ debugOutputData.getWriteBuffer().currentPose["leftPose_x"] = leftPose(0, 3);
+ debugOutputData.getWriteBuffer().currentPose["leftPose_y"] = leftPose(1, 3);
+ debugOutputData.getWriteBuffer().currentPose["leftPose_z"] = leftPose(2, 3);
+ VirtualRobot::MathTools::Quaternion leftQuat = VirtualRobot::MathTools::eigen4f2quat(leftPose);
+ debugOutputData.getWriteBuffer().currentPose["leftPose_qw"] = leftQuat.w;
+ debugOutputData.getWriteBuffer().currentPose["leftPose_qx"] = leftQuat.x;
+ debugOutputData.getWriteBuffer().currentPose["leftPose_qy"] = leftQuat.y;
+ debugOutputData.getWriteBuffer().currentPose["leftPose_qz"] = leftQuat.z;
+ }
+ {
+ debugOutputData.getWriteBuffer().currentPose["rightPose_x"] = rightPose(0, 3);
+ debugOutputData.getWriteBuffer().currentPose["rightPose_y"] = rightPose(1, 3);
+ debugOutputData.getWriteBuffer().currentPose["rightPose_z"] = rightPose(2, 3);
+ VirtualRobot::MathTools::Quaternion rightQuat = VirtualRobot::MathTools::eigen4f2quat(rightPose);
+ debugOutputData.getWriteBuffer().currentPose["rightPose_qw"] = rightQuat.w;
+ debugOutputData.getWriteBuffer().currentPose["rightPose_qx"] = rightQuat.x;
+ debugOutputData.getWriteBuffer().currentPose["rightPose_qy"] = rightQuat.y;
+ debugOutputData.getWriteBuffer().currentPose["rightPose_qz"] = rightQuat.z;
+ }
+
+ debugOutputData.getWriteBuffer().canVal = canVal;
+ debugOutputData.getWriteBuffer().forceOnHands = forceOnHands;
+ debugOutputData.commitWrite();
+
+
+ getWriterControlStruct().leftTargetPose = leftDMPController->getTargetPoseMat();
+ getWriterControlStruct().rightTargetPose = rightDMPController->getTargetPoseMat();
+ writeControlStruct();
+
+
+ }
+
+
+ void NJointBimanualForceMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ Eigen::Matrix4f leftPose = leftTCP->getPoseInRootFrame();
+ Eigen::Matrix4f rightPose = rightTCP->getPoseInRootFrame();
+ Eigen::MatrixXf leftJacobi = leftIK->getJacobianMatrix(leftTCP, VirtualRobot::IKSolver::CartesianSelection::All);
+ Eigen::MatrixXf rightJacobi = leftIK->getJacobianMatrix(leftTCP, VirtualRobot::IKSolver::CartesianSelection::All);
+
+ Eigen::VectorXf qvel;
+ qvel.resize(leftVelocitySensors.size());
+ for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
+ {
+ qvel(i) = leftVelocitySensors[i]->velocity;
+ }
+ Eigen::Vector6f leftTwist = leftJacobi * qvel;
+ for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
+ {
+ qvel(i) = rightVelocitySensors[i]->velocity;
+ }
+ Eigen::Vector6f rightTwist = rightJacobi * qvel;
+
+ leftFilteredValue = (1 - cfg->filterCoeff) * (leftForceTorque->force - leftForceOffset) + cfg->filterCoeff * leftFilteredValue;
+ rightFilteredValue = (1 - cfg->filterCoeff) * (rightForceTorque->force - rightForceOffset) + cfg->filterCoeff * rightFilteredValue;
+
+ Eigen::Matrix4f leftSensorFrame = rtGetRobot()->getRobotNode("ArmL8_Wri2")->getPoseInRootFrame();
+ Eigen::Matrix4f rightSensorFrame = rtGetRobot()->getRobotNode("ArmR8_Wri2")->getPoseInRootFrame();
+
+ Eigen::Vector3f leftForceInRootFrame = leftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0) * leftFilteredValue;
+ Eigen::Vector3f rightForceInRootFrame = rightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0) * rightFilteredValue;
+
+ controllerSensorData.getWriteBuffer().leftPoseInRootFrame = leftPose;
+ controllerSensorData.getWriteBuffer().rightPoseInRootFrame = rightPose;
+ controllerSensorData.getWriteBuffer().leftTwistInRootFrame = leftTwist;
+ controllerSensorData.getWriteBuffer().rightTwistInRootFrame = rightTwist;
+ controllerSensorData.getWriteBuffer().leftForceInRootFrame = leftForceInRootFrame;
+ controllerSensorData.getWriteBuffer().rightForceInRootFrame = rightForceInRootFrame;
+ controllerSensorData.getWriteBuffer().deltaT = timeSinceLastIteration.toSecondsDouble();
+ controllerSensorData.commitWrite();
+
+ interfaceData.getWriteBuffer().leftTcpPoseInRootFrame = leftPose;
+ interfaceData.getWriteBuffer().rightTcpPoseInRootFrame = rightPose;
+ interfaceData.commitWrite();
+
+
+
+
+ Eigen::Matrix4f targetPose = rtGetControlStruct().leftTargetPose;
+ Eigen::Matrix4f currentPose = leftPose;
+ Eigen::Vector6f leftVel;
+
+ {
+ Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
+ Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+
+ Eigen::Vector6f rtTargetVel;
+ rtTargetVel.block<3, 1>(0, 0) = Kp_LinearVel * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3))
+ + Kd_LinearVel * (- leftTwist.block<3, 1>(0, 0));
+ rtTargetVel.block<3, 1>(3, 0) = Kp_AngularVel * errorRPY + Kd_AngularVel * (- leftTwist.block<3, 1>(3, 0));
+
+ float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
+ if (normLinearVelocity > cfg->maxLinearVel)
+ {
+ rtTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rtTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
+ }
+
+ float normAngularVelocity = rtTargetVel.block<3, 1>(3, 0).norm();
+ if (normAngularVelocity > cfg->maxAngularVel)
+ {
+ rtTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rtTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
+ }
+
+ for (size_t i = 0; i < 6; i++)
+ {
+ leftVel(i) = rtTargetVel(i);
+ }
+
+ }
+
+ // cartesian vel controller
+
+
+ targetPose = rtGetControlStruct().rightTargetPose;
+ currentPose = rightPose;
+ Eigen::Vector6f rightVel;
+
+ {
+ Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
+ Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+
+ Eigen::Vector6f rtTargetVel;
+ rtTargetVel.block<3, 1>(0, 0) = Kp_LinearVel * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3))
+ + Kd_LinearVel * (- rightTwist.block<3, 1>(0, 0));
+ rtTargetVel.block<3, 1>(3, 0) = Kp_AngularVel * errorRPY + Kd_AngularVel * (- rightTwist.block<3, 1>(3, 0));
+
+ float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
+ if (normLinearVelocity > cfg->maxLinearVel)
+ {
+ rtTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rtTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
+ }
+
+ float normAngularVelocity = rtTargetVel.block<3, 1>(3, 0).norm();
+ if (normAngularVelocity > cfg->maxAngularVel)
+ {
+ rtTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rtTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
+ }
+
+ for (size_t i = 0; i < 6; i++)
+ {
+ rightVel(i) = rtTargetVel(i);
+ }
+ }
+
+
+ Eigen::VectorXf leftJTV = leftTCPController->calculate(leftVel, cfg->KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection::All);
+ for (size_t i = 0; i < leftTargets.size(); ++i)
+ {
+ leftTargets.at(i)->velocity = leftJTV(i);
+ if (!leftTargets.at(i)->isValid())
+ {
+ ARMARX_IMPORTANT << deactivateSpam(1, std::to_string(i))
+ << "Velocity controller target is invalid - setting to zero! set value: " << leftTargets.at(i)->velocity;
+ leftTargets.at(i)->velocity = 0.0f;
+ }
+ }
+
+ Eigen::VectorXf rightJTV = rightTCPController->calculate(rightVel, cfg->KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection::All);
+ for (size_t i = 0; i < rightTargets.size(); ++i)
+ {
+ rightTargets.at(i)->velocity = rightJTV(i);
+ if (!rightTargets.at(i)->isValid())
+ {
+ ARMARX_IMPORTANT << deactivateSpam(1, std::to_string(i))
+ << "Velocity controller target is invalid - setting to zero! set value: " << rightTargets.at(i)->velocity;
+ rightTargets.at(i)->velocity = 0.0f;
+ }
+ }
+
+
+ }
+
+
+ void NJointBimanualForceMPController::learnDMPFromFiles(const string& whichDMP, const Ice::StringSeq& fileNames, const Ice::Current&)
+ {
+ ARMARX_IMPORTANT << "Learn DMP " << whichDMP;
+ if (whichDMP == "Left")
+ {
+ leftDMPController->learnDMPFromFiles(fileNames);
+ leftLearned = true;
+ ARMARX_INFO << "Left Learned";
+ }
+ if (whichDMP == "Right")
+ {
+ rightDMPController->learnDMPFromFiles(fileNames);
+ rightLearned = true;
+ ARMARX_INFO << "Right Learned";
+ }
+
+ }
+
+
+ void NJointBimanualForceMPController::runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&)
+ {
+ if (!leftLearned)
+ {
+ ARMARX_ERROR << "Left DMP is not learned, aborted";
+ return;
+ }
+
+ if (!rightLearned)
+ {
+ ARMARX_ERROR << "Right DMP is not learned, aborted";
+ return;
+ }
+
+ while (!interfaceData.updateReadBuffer())
+ {
+ usleep(100);
+ }
+
+ Eigen::Matrix4f leftPose = interfaceData.getReadBuffer().leftTcpPoseInRootFrame;
+ Eigen::Matrix4f rightPose = interfaceData.getReadBuffer().rightTcpPoseInRootFrame;
+
+ forceSign = leftGoals.at(2) - leftPose(2, 3) > 0 ? -1 : 1;
+ // Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
+ ARMARX_IMPORTANT << "leftGoals dim: " << leftGoals.size();
+ ARMARX_IMPORTANT << "rightGoals dim: " << rightGoals.size();
+
+ leftDMPController->prepareExecution(leftDMPController->eigen4f2vec(leftPose), leftGoals);
+ rightDMPController->prepareExecution(rightDMPController->eigen4f2vec(rightPose), rightGoals);
+
+ canVal = cfg->timeDuration;
+ finished = false;
+
+ ARMARX_INFO << "run DMP";
+ controllerTask->start();
+
+ }
+
+ void NJointBimanualForceMPController::setViaPoints(const std::string& whichDMP, double u, const Ice::DoubleSeq& viaPoint, const Ice::Current&)
+ {
+ if (whichDMP == "Left")
+ {
+ leftDMPController->setViaPose(u, viaPoint);
+ }
+ if (whichDMP == "Right")
+ {
+ rightDMPController->setViaPose(u, viaPoint);
+ }
+ }
+
+ void NJointBimanualForceMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+ {
+ std::string debugName = cfg->debugName;
+ std::string datafieldName = debugName;
+ StringVariantBaseMap datafields;
+
+ auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
+ for (auto& pair : dmpTargets)
+ {
+ datafieldName = pair.first + "_" + debugName;
+ datafields[datafieldName] = new Variant(pair.second);
+ }
+
+ auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
+ for (auto& pair : currentPose)
+ {
+ datafieldName = pair.first + "_" + debugName;
+ datafields[datafieldName] = new Variant(pair.second);
+ }
+
+ datafieldName = "canVal_" + debugName;
+ datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().canVal);
+
+ datafieldName = "forceOnHands_" + debugName;
+ datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().forceOnHands);
+ datafieldName = "DMPController_" + debugName;
+
+ debugObs->setDebugChannel(datafieldName, datafields);
+ }
+
+ void NJointBimanualForceMPController::onInitComponent()
+ {
+ ARMARX_INFO << "init ...";
+ controllerTask = new PeriodicTask<NJointBimanualForceMPController>(this, &NJointBimanualForceMPController::controllerRun, 0.3);
+ }
+
+ void NJointBimanualForceMPController::onDisconnectComponent()
+ {
+ ARMARX_INFO << "stopped ...";
+ controllerTask->stop();
+ }
+
+
+
+}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h
new file mode 100644
index 0000000000000000000000000000000000000000..c59c12e2f0993ff22cd250e9f6a4ffa4294eabc5
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h
@@ -0,0 +1,173 @@
+
+#pragma once
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <dmp/representation/dmp/umitsmp.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
+#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+
+using namespace DMP;
+namespace armarx
+{
+ TYPEDEF_PTRS_HANDLE(NJointBimanualForceMPController);
+ TYPEDEF_PTRS_HANDLE(NJointBimanualForceMPControllerControlData);
+
+ typedef std::pair<double, DVec > ViaPoint;
+ typedef std::vector<ViaPoint > ViaPointsSet;
+ class NJointBimanualForceMPControllerControlData
+ {
+ public:
+ Eigen::Matrix4f leftTargetPose;
+ Eigen::Matrix4f rightTargetPose;
+ };
+
+ /**
+ * @brief The NJointBimanualForceMPController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
+ class NJointBimanualForceMPController :
+ public NJointControllerWithTripleBuffer<NJointBimanualForceMPControllerControlData>,
+ public NJointBimanualForceMPControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointBimanualForceMPControllerConfigPtr;
+ NJointBimanualForceMPController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current&) const;
+
+ // NJointController interface
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ // NJointBimanualForceMPControllerInterface interface
+ void learnDMPFromFiles(const string& whichMP, const Ice::StringSeq& fileNames, const Ice::Current&);
+ bool isFinished(const Ice::Current&)
+ {
+ return finished;
+ }
+
+ void runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&);
+
+ double getCanVal(const Ice::Current&)
+ {
+ return canVal;
+ }
+
+ void setViaPoints(const string& whichDMP, double canVal, const Ice::DoubleSeq& viaPoint, const Ice::Current&);
+
+ protected:
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+ void onInitComponent();
+ void onDisconnectComponent();
+
+ void controllerRun();
+ private:
+ struct DebugBufferData
+ {
+ StringFloatDictionary dmpTargets;
+ StringFloatDictionary currentPose;
+ double canVal;
+ float forceOnHands;
+ };
+ TripleBuffer<DebugBufferData> debugOutputData;
+
+ struct NJointBimanualForceMPControllerSensorData
+ {
+ Eigen::Matrix4f leftPoseInRootFrame;
+ Eigen::Vector6f leftTwistInRootFrame;
+
+ Eigen::Matrix4f rightPoseInRootFrame;
+ Eigen::Vector6f rightTwistInRootFrame;
+
+ Eigen::Vector3f leftForceInRootFrame;
+ Eigen::Vector3f rightForceInRootFrame;
+ double deltaT;
+ };
+ TripleBuffer<NJointBimanualForceMPControllerSensorData> controllerSensorData;
+
+ struct NJointBimanualForceMPControllerInterfaceData
+ {
+ Eigen::Matrix4f leftTcpPoseInRootFrame;
+ Eigen::Matrix4f rightTcpPoseInRootFrame;
+ };
+ TripleBuffer<NJointBimanualForceMPControllerInterfaceData> interfaceData;
+
+ std::vector<ControlTarget1DoFActuatorVelocity*> leftTargets;
+ std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
+
+ std::vector<ControlTarget1DoFActuatorVelocity*> rightTargets;
+ std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
+
+ const SensorValueForceTorque* rightForceTorque;
+ const SensorValueForceTorque* leftForceTorque;
+
+ VirtualRobot::DifferentialIKPtr leftIK;
+ VirtualRobot::DifferentialIKPtr rightIK;
+
+
+ TaskSpaceDMPControllerPtr leftDMPController;
+ TaskSpaceDMPControllerPtr rightDMPController;
+
+ // velocity ik controller parameters
+ CartesianVelocityControllerPtr leftTCPController;
+ CartesianVelocityControllerPtr rightTCPController;
+
+ // dmp parameters
+ bool finished;
+ VirtualRobot::RobotNodePtr leftTCP;
+ VirtualRobot::RobotNodePtr rightTCP;
+
+
+ Eigen::VectorXf targetVels;
+ Eigen::Matrix4f targetPose;
+
+ NJointBimanualForceMPControllerConfigPtr cfg;
+ mutable MutexType controllerMutex;
+ PeriodicTask<NJointBimanualForceMPController>::pointer_type controllerTask;
+
+ // velocity control
+ float Kp_LinearVel;
+ float Kd_LinearVel;
+ float Kp_AngularVel;
+ float Kd_AngularVel;
+
+ // force control
+ float Kp_f;
+ float Kd_f;
+ float Ki_f;
+
+ double canVal;
+ double xvel;
+
+ float forceIterm;
+ bool leftLearned;
+ bool rightLearned;
+
+ Eigen::Vector3f leftFilteredValue;
+ Eigen::Vector3f rightFilteredValue;
+
+ Eigen::Vector3f leftForceOffset;
+ Eigen::Vector3f rightForceOffset;
+ float targetSupportForce;
+
+ double I_decay;
+
+ float forceSign;
+
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
index a288078a9178d1259b2283affad03c754d0b3310..fde6f96c95f289bbc8d80fd2f4f5be5763726e9e 100644
--- a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
@@ -71,6 +71,11 @@ void SimpleJsonLoggerEntry::Add(const std::string& key, const std::vector<float>
obj->add(key, ToArr(value));
}
+void SimpleJsonLoggerEntry::Add(const std::string& key, const std::map<std::string, float>& value)
+{
+ obj->add(key, ToObj(value));
+}
+
JsonArrayPtr SimpleJsonLoggerEntry::ToArr(const Eigen::VectorXf& vec)
{
@@ -101,6 +106,16 @@ JsonObjectPtr SimpleJsonLoggerEntry::ToObj(Eigen::Vector3f vec)
return obj;
}
+JsonObjectPtr SimpleJsonLoggerEntry::ToObj(const std::map<std::string, float>& value)
+{
+ JsonObjectPtr obj(new JsonObject);
+ for (const std::pair<std::string, float>& pair : value)
+ {
+ obj->add(pair.first, JsonValue::Create(pair.second));
+ }
+ return obj;
+}
+
JsonArrayPtr SimpleJsonLoggerEntry::ToArr(Eigen::Matrix4f mat)
{
JsonArrayPtr arr(new JsonArray);
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h
index 44034658024bfb8cb498b7e1f6d50cb4a1c726f9..9565419e5d15017c8a2bf1e937ec1615a740186f 100644
--- a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h
@@ -27,6 +27,7 @@
#include <ArmarXGui/libraries/StructuralJson/JsonObject.h>
#include <Eigen/Dense>
+#include <map>
namespace armarx
{
@@ -46,10 +47,12 @@ namespace armarx
void Add(const std::string& key, const std::string& value);
void Add(const std::string& key, float value);
void Add(const std::string& key, const std::vector<float>& value);
+ void Add(const std::string& key, const std::map<std::string, float>& value);
static JsonArrayPtr ToArr(const Eigen::VectorXf& vec);
static JsonArrayPtr ToArr(const std::vector<float>& vec);
static JsonObjectPtr ToObj(Eigen::Vector3f vec);
+ static JsonObjectPtr ToObj(const std::map<std::string, float>& value);
static JsonArrayPtr ToArr(Eigen::Matrix4f mat);
static JsonArrayPtr MatrixToArr(Eigen::MatrixXf mat);