diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp index ca62981433b1a07e729846848137806bb4282fe9..e9cd2d29048c4b59da2edc7e9f66e1e61ab7e4fd 100644 --- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp +++ b/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(); diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp index aa0fe7399f985b4cf08bd321a6cc48dcb15a20c5..6b40bb6e449bc3342195ca0f705bd1c11ebe2a1d 100644 --- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp +++ b/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; diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h index f0d9ae529195dd29d1bb8e8266487513dd40fb2c..6abebdc691fb1bb8f0f6e53b6c03c1b2504181db 100644 --- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h +++ b/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;