Added Current Control

source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp

@@ -125,6 +125,11 @@ armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc,
                 ARMARX_CHECK_EXPRESSION(eUnknown == c);
                 c = eTorqueControl;
             }
+            if (actuatorData.ctrlCur && nJointCtrls.count(actuatorData.ctrlCur.get()))
+            {
+                ARMARX_CHECK_EXPRESSION(eUnknown == c);
+                c = eCurrentControl;
+            }
             ctrlModesAValueChanged =
                 ctrlModesAValueChanged || !ctrlModes.count(name) || (ctrlModes.at(name) != c);
             ctrlModes[name] = c;
@@ -282,6 +287,22 @@ armarx::KinematicSubUnit::setJointTorques(const armarx::NameValueMap& torques, c
     }
 }
 
+void
+armarx::KinematicSubUnit::setJointCurrents(const armarx::NameValueMap& currents, const Ice::Current&)
+{
+    std::lock_guard<std::mutex> guard{dataMutex};
+    for (const auto& n2v : currents)
+    {
+        if (devs.count(n2v.first) > 0)
+        {
+            if (devs.at(n2v.first).ctrlCur)
+            {
+                devs.at(n2v.first).ctrlCur->set(n2v.second);
+            }
+        }
+    }
+}
+
 void
 armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMap& ncm,
                                             const Ice::Current&)
@@ -490,6 +511,8 @@ armarx::KinematicSubUnit::ActuatorData::getController(armarx::ControlMode c) con
             return ctrlVel;
         case eTorqueControl:
             return ctrlTor;
+        case eCurrentControl:
+            return ctrlCur;
         default:
             return nullptr;
     }
@@ -510,5 +533,9 @@ armarx::KinematicSubUnit::ActuatorData::getActiveController() const
     {
         return ctrlTor;
     }
+    if (ctrlCur->isControllerActive())
+    {
+        return ctrlCur;
+    }
     return nullptr;
 }

source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h

@@ -50,6 +50,7 @@ namespace armarx
             NJointKinematicUnitPassThroughControllerPtr ctrlPos;
             NJointKinematicUnitPassThroughControllerPtr ctrlVel;
             NJointKinematicUnitPassThroughControllerPtr ctrlTor;
+            NJointKinematicUnitPassThroughControllerPtr ctrlCur;
 
             NJointControllerPtr getController(ControlMode c) const;
             NJointControllerPtr getActiveController() const;
@@ -72,6 +73,8 @@ namespace armarx
         void setJointAngles(const NameValueMap& angles, const Ice::Current&) override;
         void setJointVelocities(const NameValueMap& velocities, const Ice::Current&) override;
         void setJointTorques(const NameValueMap& torques, const Ice::Current&) override;
+        void setJointCurrents(const NameValueMap& currents, const Ice::Current&); //override?
+        
         void switchControlMode(const NameControlModeMap& ncm, const Ice::Current&) override;
 
         void setJointAccelerations(const NameValueMap&, const Ice::Current&) override;

source/RobotAPI/interface/units/KinematicUnitInterface.ice

@@ -69,7 +69,8 @@ module armarx
         ePositionControl,
         eVelocityControl,
         eTorqueControl,
-        ePositionVelocityControl
+        ePositionVelocityControl,
+        eCurrentControl
     };
 	/**
 	* [OperationStatus] defines whether a KinematicUnit is online, offline, or initialized.
@@ -169,7 +170,7 @@ module armarx
          */
         void requestJoints(Ice::StringSeq joints) throws KinematicUnitUnavailable;
         /**
-         * After usage joints should be released so that another component can get access to these joints.
+         * After usage joints should be released so that another component can get access to these joints./common/homes/students/ulxvc_koch/Workspaces/armarx_integration/robots/armar7
          */
         void releaseJoints(Ice::StringSeq joints) throws KinematicUnitNotOwnedException;
         
@@ -189,11 +190,13 @@ module armarx
         void setJointAngles(NameValueMap targetJointAngles) throws OutOfRangeException;
         void setJointVelocities(NameValueMap targetJointVelocities) throws OutOfRangeException;
         void setJointTorques(NameValueMap targetJointTorques) throws OutOfRangeException;
+        void setJointCurrents(NameValueMap targetJointCurrents) throws OutOfRangeException;
         void setJointAccelerations(NameValueMap targetJointAccelerations) throws OutOfRangeException;
         void setJointDecelerations(NameValueMap targetJointDecelerations) throws OutOfRangeException;
         
         ["cpp:const"] NameValueMap   getJointAngles();
         ["cpp:const"] NameValueMap   getJointVelocities();
+        ["cpp:const"] NameValueMap   getJointCurrents();
         ["cpp:const"] Ice::StringSeq getJoints();
         
         /*!