controller sanity checks

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp

@@ -120,6 +120,7 @@ void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& sensorValues
 {
     Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
 
+    ARMARX_CHECK_EQUAL(positionSensors.size(), velocitySensors.size());
     Eigen::VectorXf qpos;
     Eigen::VectorXf qvel;
     qpos.resize(positionSensors.size());
@@ -134,6 +135,7 @@ void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& sensorValues
     }
 
     Eigen::VectorXf tcptwist = jacobi * qvel;
+    ARMARX_CHECK_EQUAL(tcptwist.rows(), 6);
 
     Eigen::Vector3f currentTCPLinearVelocity;
     currentTCPLinearVelocity << 0.001 * tcptwist(0),   0.001 * tcptwist(1), 0.001 * tcptwist(2);
@@ -167,6 +169,8 @@ void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& sensorValues
 
     Eigen::VectorXf nullqerror = desiredJointPosition - qpos;
 
+    ARMARX_CHECK_EQUAL(nullqerror.rows(), targets.size());
+
     for (int i = 0; i < nullqerror.rows(); ++i)
     {
         if (fabs(nullqerror(i)) < 0.09)
@@ -177,6 +181,7 @@ void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& sensorValues
     Eigen::VectorXf nullspaceTorque = knull * nullqerror - dnull * qvel;
 
     Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * tcpDesiredWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
+    ARMARX_CHECK_EQUAL(jointDesiredTorques.rows(), targets.size());
 
     for (size_t i = 0; i < targets.size(); ++i)
     {
@@ -245,5 +250,7 @@ void NJointTaskSpaceImpedanceController::setTarget(const Ice::FloatSeq& target,
     writeControlStruct();
 }
 
+void armarx::NJointTaskSpaceImpedanceController::rtPreActivateController()
+{
 
-
+}

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h

@@ -71,6 +71,7 @@ namespace armarx
         void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
 
         void setTarget(const Ice::FloatSeq&, const Ice::Current&);
+
     private:
         Eigen::Vector3f desiredPosition;
         Eigen::Quaternionf desiredQuaternion;
@@ -123,6 +124,10 @@ namespace armarx
         float torqueLimit;
         std::vector<std::string> jointNames;
 
+
+        // NJointController interface
+    protected:
+        virtual void rtPreActivateController() override;
     };
 
 }