added accelration to kit gripper board

source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp

@@ -267,6 +267,7 @@ namespace armarx
         sensorValue.position = actorDataPtr->getPosition();
         sensorValue.relativePosition = actorDataPtr->getRelativePosition();
         sensorValue.velocity = actorDataPtr->getVelocity();
+        sensorValue.acceleration = actorDataPtr->getAcceleration();
         sensorValue.absoluteEncoderVelocity = actorDataPtr->getAbsoluteEncoderVelocity();
         sensorValue.targetPWM = actorDataPtr->getTargetPWM();
         sensorValue.motorCurrent = actorDataPtr->getTargetPWM();

source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp

@@ -86,6 +86,7 @@ namespace armarx
     void KITGripperBasisBoardData::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
     {
         int i = 0;
+        double dt = timeSinceLastIteration.toSecondsDouble();
         for (auto& ptr : actorData)
         {
             if (!ptr)
@@ -144,11 +145,13 @@ namespace armarx
                 d.absoluteEncoderVelocity = math::MathUtils::AngleDelta(d.lastAbsolutePosition, d.sanitizedAbsolutePosition) / timeSinceLastIteration.toSecondsDouble();
             }
             d.lastAbsolutePosition = d.sanitizedAbsolutePosition;
+            float oldVelocity = d.velocity.value;
             d.velocity.read();
             d.velocity.value = d.velocityFilter.update(sensorValuesTimestamp, d.velocity.value);
             d.torque.read();
             d.currentMaxPWM = std::round(*d.velocityTicks * d.currentPWMBoundGradient + d.currentPWMBoundOffset);
             d.currentMinPWM = std::round(*d.velocityTicks * d.currentPWMBoundGradient - d.currentPWMBoundOffset);
+            d.acceleration = (d.velocity.value - oldVelocity) / dt;
             //            d.acceleration.read();
             //            d.gravityTorque.read();
             //            d.motorCurrent.read();
@@ -217,6 +220,11 @@ namespace armarx
         return velocity.value;
     }
 
+    float ActorData::getAcceleration() const
+    {
+        return acceleration;
+    }
+
     float ActorData::getTorque() const
     {
         return torque.value;

source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h

@@ -33,7 +33,7 @@
 namespace armarx
 {
     class   SensorValueKITGripperBasisBoard :
-    //            virtual public SensorValue1DoFActuatorMotorTemperature,
+        //            virtual public SensorValue1DoFActuatorMotorTemperature,
         virtual public SensorValueIMU
     {
     public:
@@ -85,6 +85,7 @@ namespace armarx
         float getRelativePosition() const;
         float getCompensatedRelativePosition() const;
         float getVelocity() const;
+        float getAcceleration() const;
         float getTorque() const;
         float getSoftLimitHi() const;
         float getSoftLimitLo() const;
@@ -121,6 +122,7 @@ namespace armarx
         LinearConvertedValue<int32_t> velocity;
         rtfilters::AverageFilter velocityFilter;
         float absoluteEncoderVelocity = 0.0f;
+        float acceleration;
         float lastAbsolutePosition = std::nanf("");
         LinearConvertedValue<int32_t> torque;
         LinearConvertedValue<int32_t> targetPWM;