added KinematicUnitHelper

source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt

@@ -28,6 +28,7 @@ set(LIB_FILES
 VelocityControllerHelper.cpp
 PositionControllerHelper.cpp
 ForceTorqueHelper.cpp
+KinematicUnitHelper.cpp
 RobotNameHelper.cpp
 #@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
 )
@@ -36,6 +37,7 @@ set(LIB_HEADERS
 VelocityControllerHelper.h
 PositionControllerHelper.h
 ForceTorqueHelper.h
+KinematicUnitHelper.h
 RobotNameHelper.h
 #@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
 )

source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.cpp

+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "KinematicUnitHelper.h"
+
+using namespace armarx;
+
+KinematicUnitHelper::KinematicUnitHelper(const KinematicUnitInterfacePrx& kinUnit)
+    : kinUnit(kinUnit)
+{
+}
+
+void KinematicUnitHelper::setJointAngles(const NameValueMap& jointAngles)
+{
+    kinUnit->switchControlMode(MakeControlModes(jointAngles, ePositionControl));
+    kinUnit->setJointAngles(jointAngles);
+}
+
+void KinematicUnitHelper::setJointVelocities(const NameValueMap& jointVelocities)
+{
+    kinUnit->switchControlMode(MakeControlModes(jointVelocities, eVelocityControl));
+    kinUnit->setJointVelocities(jointVelocities);
+}
+
+void KinematicUnitHelper::setJointTorques(const NameValueMap& jointTorques)
+{
+    kinUnit->switchControlMode(MakeControlModes(jointTorques, eTorqueControl));
+    kinUnit->setJointTorques(jointTorques);
+}
+
+NameControlModeMap KinematicUnitHelper::MakeControlModes(const NameValueMap& jointValues, ControlMode controlMode)
+{
+    NameControlModeMap cm;
+    for (const auto& pair : jointValues)
+    {
+        cm[pair.first] = controlMode;
+    }
+    return cm;
+}

source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h

+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+
+namespace armarx
+{
+    class KinematicUnitHelper;
+    typedef boost::shared_ptr<KinematicUnitHelper> KinematicUnitHelperPtr;
+
+    class KinematicUnitHelper
+    {
+    public:
+        KinematicUnitHelper(const KinematicUnitInterfacePrx& kinUnit);
+
+        void setJointAngles(const NameValueMap& jointAngles);
+        void setJointVelocities(const NameValueMap& jointVelocities);
+        void setJointTorques(const NameValueMap& jointTorques);
+        static NameControlModeMap MakeControlModes(const NameValueMap& jointValues, ControlMode controlMode);
+
+    private:
+        KinematicUnitInterfacePrx kinUnit;
+    };
+}