From 1df18ff589b37131769de68bb649befedede78c5 Mon Sep 17 00:00:00 2001
From: ArmarX User <armarx@kit.edu>
Date: Wed, 28 Aug 2019 17:25:32 +0200
Subject: [PATCH] fix kitgripper joint controllers after merge
---
.../KITGripperEtherCAT/CMakeLists.txt | 2 +
...JointKITGripperEmergencyStopController.cpp | 2 +-
.../JointPWMPositionController.cpp | 64 ++++++++++---------
.../JointPWMPositionController.h | 1 +
.../JointPWMVelocityController.cpp | 2 +-
.../KITGripperBasisBoard.cpp | 1 +
.../KITGripperBasisBoardData.cpp | 5 +-
.../KITGripperBasisBoardData.h | 11 +---
8 files changed, 45 insertions(+), 43 deletions(-)
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/CMakeLists.txt b/source/RobotAPI/libraries/KITGripperEtherCAT/CMakeLists.txt
index ce5236543..425e679db 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/CMakeLists.txt
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/CMakeLists.txt
@@ -58,6 +58,7 @@ set(LIB_FILES
KITGripperBasisBoard/JointController/JointPWMTorqueController.cpp
KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp
KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.cpp
+ KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.cpp
KITGripperBasisBoard/JointController/PWMVelocityController.cpp
KITGripperBasisBoard/JointController/JointZeroTorqueController.cpp
KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
@@ -74,6 +75,7 @@ set(LIB_HEADERS
KITGripperBasisBoard/JointController/JointPWMVelocityController.h
KITGripperBasisBoard/JointController/JointPWMPositionController.h
KITGripperBasisBoard/JointController/JointPWMTorqueController.h
+ KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.h
KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h
KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.h
KITGripperBasisBoard/JointController/PWMVelocityController.h
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp
index 3bbf673c0..83a7a9855 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp
@@ -18,7 +18,7 @@ namespace armarx
targetPwm = 0.0f;
}
// dataPtr->setTargetPWM(targetPwm);
- lastPWM *= 0.9997;
+ lastPWM *= 0.0;
// ARMARX_RT_LOGF_INFO("old pwm %d, new pwm: %.2f", dataPtr->getTargetPWM(), lastPWM);
dataPtr->setTargetPWM(lastPWM);
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
index e4bf73b8c..cf2a124f5 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
@@ -34,14 +34,14 @@ JointPWMPositionController::JointPWMPositionController(const std::string deviceN
// add limitless joint controller to solve the flipping sensor value problem
limitlessController.positionPeriodLo = dataPtr->getSoftLimitLo();
limitlessController.positionPeriodHi = dataPtr->getSoftLimitHi();
- limitlessController.maxDt = velocityControllerConfigDataPtr->maxDt;
- limitlessController.maxV = velocityControllerConfigDataPtr->maxVelocityRad;
- limitlessController.p = velocityControllerConfigDataPtr->posControlP;
+ limitlessController.maxDt = positionControllerConfigDataPtr->maxDt;
+ limitlessController.maxV = positionControllerConfigDataPtr->maxVelocityRad;
+ limitlessController.pid->Kp = positionControllerConfigDataPtr->posControlP;
// ARMARX_INFO << deviceName << " pos P: " << limitlessController.p;
// limitlessController.p = 10.0f; // for bit joint
// limitlessController.p = 3.0f; // for base joint
- limitlessController.acceleration = velocityControllerConfigDataPtr->maxAccelerationRad;
- limitlessController.deceleration = velocityControllerConfigDataPtr->maxDecelerationRad;
+ limitlessController.acceleration = positionControllerConfigDataPtr->maxAccelerationRad;
+ limitlessController.deceleration = positionControllerConfigDataPtr->maxDecelerationRad;
limitlessController.accuracy = 0.001f;
// ARMARX_INFO << "period low: " << limitlessController.positionPeriodLo << ", period high: " << limitlessController.positionPeriodHi;
// ARMARX_INFO << "maxDt: " << limitlessController.maxDt << ", maxV: " << limitlessController.maxV;
@@ -96,30 +96,32 @@ void JointPWMPositionController::rtRun(const IceUtil::Time& sensorValuesTimestam
newVel = limitlessController.run();
// ARMARX_IMPORTANT << "using limitless position controller";
// ARMARX_RT_LOGF_WARNING("Position controller not implemented for limitless joints").deactivateSpam(10);
+ if (std::isnan(newVel))
+ {
+ newVel = 0;
+ }
+ newVel = math::MathUtils::LimitTo(newVel, posController.maxV);
+ pidPosController->update(timeSinceLastIteration.toSecondsDouble(), dataPtr->getVelocity(), newVel);
}
else
{
// posController.currentPosition = currentPosition;
- posController.currentV = lastTargetVelocity;
- posController.dt = timeSinceLastIteration.toSecondsDouble();
- posController.setTargetPosition(target.position);
- // ARMARX_CHECK_EXPRESSION(posController.validParameters());
- auto r = posController.run();
- posController.currentPosition = r.position;
- posController.currentAcc = r.acceleration;
- newVel = r.velocity;
- }
+ posController.currentV = lastTargetVelocity;
+ posController.dt = timeSinceLastIteration.toSecondsDouble();
+ posController.setTargetPosition(target.position);
+ // ARMARX_CHECK_EXPRESSION(posController.validParameters());
+ auto r = posController.run();
+ posController.currentPosition = r.position;
+ posController.currentAcc = r.acceleration;
+ newVel = r.velocity;
+ pidPosController->update(timeSinceLastIteration.toSecondsDouble(), currentPosition, r.position);
+ }
// double newVel = posController.p * (posController.targetPosition - posController.currentPosition);
// newVel = math::MathUtils::LimitTo(newVel, posController.maxV);
// ARMARX_INFO << deactivateSpam(1) << VAROUT(newVel);
- if (std::isnan(newVel))
- {
- newVel = 0;
- }
- pidPosController->update(timeSinceLastIteration.toSecondsDouble(), currentPosition, r.position);
+
auto targetPWM = pidPosController->getControlValue();
// auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel));
- newVel = math::MathUtils::LimitTo(newVel, posController.maxV);
float torqueFF = config->feedforwardTorqueToPWMFactor * -sensorValue->gravityTorque;
targetPWM += torqueFF;
@@ -134,18 +136,19 @@ void JointPWMPositionController::rtRun(const IceUtil::Time& sensorValuesTimestam
ARMARX_ERROR << deactivateSpam(1) << "Target PWM of " << getParent().getDeviceName() << " is NaN!";
targetPWM = 0.0f;
}
- float updateRatio = 0.3;
- this->targetPWM = (1.0 - updateRatio) * this->targetPWM + updateRatio * targetPWM;
- float pwmDiff = std::abs(dataPtr->getTargetPWM() - targetPWM);
- // ARMARX_INFO << deactivateSpam(0.3) << VAROUT(pwmDiff) << VAROUT(dataPtr->getTargetPWM()) << VAROUT(targetPWM) << VAROUT(dataPtr->getVelocity());
- if (pwmDiff > 5 || dataPtr->getVelocity() > 0.01) // avoid jittering when standing still
- {
- // ARMARX_INFO << deactivateSpam(0.0, std::to_string(targetPWM)) << "Setting new targetPWM to" << targetPWM << " diff: " << pwmDiff << " vel: " << dataPtr->getVelocity();
- dataPtr->setTargetPWM(targetPWM);
- }
+ // float updateRatio = 0.3;
+ // this->targetPWM = (1.0 - updateRatio) * this->targetPWM + updateRatio * targetPWM;
+ // float pwmDiff = std::abs(dataPtr->getTargetPWM() - targetPWM);
+ // // ARMARX_INFO << deactivateSpam(0.3) << VAROUT(pwmDiff) << VAROUT(dataPtr->getTargetPWM()) << VAROUT(targetPWM) << VAROUT(dataPtr->getVelocity());
+ // if (pwmDiff > 5 || dataPtr->getVelocity() > 0.01) // avoid jittering when standing still
+ // {
+ // // ARMARX_INFO << deactivateSpam(0.0, std::to_string(targetPWM)) << "Setting new targetPWM to" << targetPWM << " diff: " << pwmDiff << " vel: " << dataPtr->getVelocity();
+ // dataPtr->setTargetPWM(targetPWM);
+ // }
this->targetPWM = targetPWM;
lastTargetVelocity = newVel;
+ dataPtr->setTargetPWM(targetPWM);
// auto name = getParent().getDeviceName().c_str();
// ARMARX_RT_LOGF_INFO("%s: position: %.f, target position: %.f, targetvelocity: %.f, target PWM: %d", name,
// currentPosition, targetPosition, newVel, targetPWM).deactivateSpam(1);
@@ -168,10 +171,12 @@ void JointPWMPositionController::rtPreActivateController()
{
targetPWM = 0.0f;
lastTargetVelocity = dataPtr->getVelocity();
+ posController.reset();
posController.currentAcc = dataPtr->getAcceleration();
posController.currentPosition = dataPtr->getPosition();
posController.currentV = dataPtr->getVelocity();
pidPosController->reset();
+
// controller.reset(dataPtr->getVelocity());
}
@@ -328,6 +333,7 @@ PWMPositionControllerConfigurationCPtr PWMPositionControllerConfiguration::Creat
configData.maxAccelerationRad = node.first_node("maxAccelerationRad").value_as_float();
configData.maxDecelerationRad = node.first_node("maxDecelerationRad").value_as_float();
configData.maxDt = node.first_node("maxDt").value_as_float();
+ configData.posControlP = node.first_node("posControlP").value_as_float();
configData.p = node.first_node("p").value_as_float();
configData.i = node.first_node("i").value_as_float();
configData.d = node.first_node("d").value_as_float();
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h
index d409d5769..cf20557db 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h
@@ -32,6 +32,7 @@ namespace armarx
float maxAccelerationRad;
float maxDecelerationRad;
float maxDt;
+ float posControlP;
float p;
float i;
float d;
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
index 67da10653..019649a88 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
@@ -30,7 +30,7 @@ JointPWMVelocityController::JointPWMVelocityController(const std::string deviceN
// velController.acceleration = velocityControllerConfigDataPtr->maxAccelerationRad;
velController.deceleration = velocityControllerConfigDataPtr->maxDecelerationRad;
- velController.jerk = 30;
+ velController.jerk = 10;
velController.maxDt = velocityControllerConfigDataPtr->maxDt;
velController.maxV = velocityControllerConfigDataPtr->maxVelocityRad;
velController.directSetVLimit = velocityControllerConfigDataPtr->directSetVLimit;
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp
index 834674e21..4da369637 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp
@@ -323,3 +323,4 @@ namespace armarx
+
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
index 8f85a09ff..4f29f113f 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
@@ -125,7 +125,6 @@ namespace armarx
}
ActorData& d = *ptr;
-
d.relativePosition.read();
if (d.getSiblingControlActorIndex() >= 0) // if >=0 (0, 1, 2), device is coupled with another motor
@@ -209,6 +208,8 @@ namespace armarx
d.absoluteEncoderVelocity = math::MathUtils::AngleDelta(d.lastAbsolutePosition, d.sanitizedAbsolutePosition) / timeSinceLastIteration.toSecondsDouble();
}
d.lastAbsolutePosition = d.sanitizedAbsolutePosition;
+ float oldVelocity = d.velocity.value;
+
if (d.getUseABSEncoderForVelocity() >= 0)
{
d.velocity.value = d.absoluteEncoderVelocity;
@@ -281,7 +282,7 @@ namespace armarx
}
ActorData::ActorData(int32_t filterWindowSize):
- velocityFilter(filterWindowSize){}
+ velocityFilter(filterWindowSize) {}
int8_t KITGripperBasisBoardData::getIMUTemperature() const
{
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
index 31dd7c18f..c308bf68c 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
@@ -38,10 +38,7 @@ namespace armarx
{
public:
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
-
int IMUTemperature;
-
-
static SensorValueInfo<SensorValueKITGripperBasisBoard> GetClassMemberInfo()
{
SensorValueInfo<SensorValueKITGripperBasisBoard> svi;
@@ -72,7 +69,6 @@ namespace armarx
float absoluteEncoderVelocity;
int32_t maxPWM;
int32_t minPWM;
- float torque;
float torqueWithInertia;
float rawVelocity;
float filteredVelocity;
@@ -88,7 +84,6 @@ namespace armarx
svi.addMemberVariable(&KITGripperActorSensorData::minPWM, "minPWM");
svi.addMemberVariable(&KITGripperActorSensorData::velocityTicksPerMs, "velocityTicksPerMs");
svi.addMemberVariable(&KITGripperActorSensorData::absoluteEncoderVelocity, "absoluteEncoderVelocity");
- svi.addMemberVariable(&KITGripperActorSensorData::torque, "torque");
svi.addMemberVariable(&KITGripperActorSensorData::torqueWithInertia, "torqueWithInertia");
svi.addMemberVariable(&KITGripperActorSensorData::rawVelocity, "rawVelocity");
svi.addMemberVariable(&KITGripperActorSensorData::filteredVelocity, "filteredVelocity");
@@ -102,11 +97,10 @@ namespace armarx
{
public:
ActorData();
- void setTargetPWM(int32_t targetPWM);
+ ActorData(int32_t filterWindowSize);
float getPosition() const;
float getRelativePosition() const;
float getCompensatedRelativePosition() const;
- float getVelocity() const;
float getAcceleration() const;
int32_t getRelativePositionSensorOnly() const;
bool getPositionControlEnabled() const;
@@ -142,9 +136,7 @@ namespace armarx
int32_t getSiblingControlActorIndex() const;
float getParallelGripperDecouplingFactor() const;
- float getParallelGripperDecouplingFactor() const;
- float getAbsoluteEncoderVelocity() const;
private:
// parallel gripper controller configuration
@@ -157,7 +149,6 @@ namespace armarx
LinearConvertedValue<int32_t> relativePosition;
float adjustedRelativePosition;
float relativePositionOffset = std::nan("");
- float parallelGripperDecouplingFactor = std::nanf("");
LinearConvertedValue<u_int32_t> position;
float sanitizedAbsolutePosition;
float lastAbsolutePosition = std::nanf("");
--
GitLab