diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
index 61115c4e124cf52f73255721f53f199df8bd52f0..45b8ff4727ae36de3b168ed02fe5c649163d1d1f 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
@@ -372,12 +372,13 @@ namespace armarx
//handle case 1
const double clampedTargetV = math::MathUtils::LimitTo(targetV, maxV);
const double curverror = clampedTargetV - currentV;
+ // ARMARX_INFO << deactivateSpam(0.01) << VAROUT(clampedTargetV) << VAROUT(currentV) << VAROUT(curverror) << VAROUT(maxV);
if (std::abs(curverror) < directSetVLimit)
{
double nextAcc = (clampedTargetV - currentV) / useddt;
double nextJerk = (nextAcc - currentAcc) / useddt;
Output result {clampedTargetV, nextAcc, nextJerk};
-
+ // ARMARX_INFO << deactivateSpam(0.01) << "vel error lower than direct set vel limit.";
return result;
}
@@ -1313,6 +1314,7 @@ namespace armarx
{
softLimitViolation = 1;
}
+ // ARMARX_INFO << deactivateSpam(0.01) << VAROUT(softLimitViolation);
const double upperDt = std::max(std::abs(dt), std::abs(maxDt));
@@ -1321,7 +1323,7 @@ namespace armarx
// 2. we directly set v and ignore acc/dec (if |curr - target| <= limit)
// 3. we need to accelerate (if curr and target v have same sign and |curr| < |target|)
// 4. we need to decelerate (other cases)
- double nextv;
+ double nextv = 0.0;
//handle case 1
const double vsquared = currentV * currentV;
const double brakingDist = sign(currentV) * vsquared / 2.f / std::abs(deceleration); //the braking distance points in the direction of the velocity
@@ -1339,25 +1341,24 @@ namespace armarx
const double dec = std::abs(vsquared / 2.0 / wayToGo);
const double vel = currentV - sign(currentV) * dec * upperDt;
nextv = boost::algorithm::clamp(vel, -maxV, maxV);
- // ARMARX_INFO /*<< deactivateSpam(0.1) */ << "clamped new Vel: " << VAROUT(nextv) << VAROUT(currentV);
+ // ARMARX_INFO << deactivateSpam(0.1) << "clamped new Vel: " << VAROUT(nextv) << VAROUT(currentV);
if (sign(currentV) != sign(nextv))
{
- // ARMARX_INFO << deactivateSpam(1) << "wrong sign: stopping"; //stop now
+ // ARMARX_INFO << deactivateSpam(0.01) << "wrong sign: stopping"; //stop now
nextv = 0;
}
}
else
{
//handle 2-3
+ // ARMARX_INFO << deactivateSpam(0.01) << "Desired target Vel: " << targetV << " " << VAROUT(nextv);
return VelocityControllerWithRampedAcceleration::run();
- // ARMARX_INFO << deactivateSpam(1) << "Desired target Vel: " << targetV << " " << VAROUT(nextv);
-
}
if (softLimitViolation == sign(nextv) && nextv != 0)
{
//the area between soft and hard limits is sticky
//the controller can only move out of it (not further in)
- // ARMARX_INFO << deactivateSpam(1) << "Soft limit violation. " << softLimitViolation << VAROUT(nextv);
+ // ARMARX_INFO << deactivateSpam(0.01) << "Soft limit violation. " << softLimitViolation << VAROUT(nextv);
nextv = 0;
}
double nextAcc = (nextv - currentV) / useddt; // doesnt work with calculated acc
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.cpp
index 5e5441eaaf750affbce122d8f65a24fe94a7262f..1dad32dfa11df759934664503e6d2134182445ee 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.cpp
@@ -17,7 +17,7 @@ namespace armarx
{
if (target.isValid())
{
- jointData->setTargetPWM(target.current * 1000);
+ jointData->setTargetPWM(target.current);
}
}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
index cf2a124f59843bbd356da7b9fce97388acc105b1..00f90e0ee73dd5363081bb33b78e5fc9f46b4e32 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
@@ -95,7 +95,6 @@ void JointPWMPositionController::rtRun(const IceUtil::Time& sensorValuesTimestam
limitlessController.targetPosition = target.position;
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;
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
index 019649a886079be243f94e58a9d7e9fe74b13ef9..d7f09e47bce2c1fcb60c09a87a7abbf4571e8e61 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
@@ -59,49 +59,48 @@ void JointPWMVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestam
{
if (target.isValid())
{
-
+ auto currentPosition = dataPtr->getPosition();
+ if (isLimitless)
{
- auto currentPosition = dataPtr->getPosition();
- if (isLimitless)
- {
- velController.currentPosition = velController.positionLimitHiSoft - (velController.positionLimitHiSoft - velController.positionLimitLoSoft) * 0.5;
- ARMARX_DEBUG << deactivateSpam(1) << "currentPosition: " << velController.currentPosition;
- }
- else
- {
- velController.currentPosition = currentPosition;
- }
- velController.currentV = lastTargetVelocity;
- velController.currentAcc = lastTargetAcceleration;
- velController.dt = timeSinceLastIteration.toSecondsDouble();
- velController.targetV = target.velocity;
- auto r = velController.run();
- double newVel = r.velocity;
- double newAcc = r.acceleration;
-
- if (std::isnan(newVel))
- {
- newVel = 0;
- newAcc = 0;
- }
- if ((currentPosition > velController.positionLimitHiSoft && target.velocity > 0)
- || (currentPosition < velController.positionLimitLoSoft && target.velocity < 0))
- {
- newVel = 0;
- newAcc = 0;
- ARMARX_INFO << deactivateSpam(1) << "Breaking now at " << dataPtr->getPosition() << " pwm: " << dataPtr->getTargetPWM();
- }
+ velController.currentPosition = velController.positionLimitHiSoft - (velController.positionLimitHiSoft - velController.positionLimitLoSoft) * 0.5;
+ // ARMARX_DEBUG << deactivateSpam(1) << "currentPosition: " << velController.currentPosition;
+ }
+ else
+ {
+ velController.currentPosition = currentPosition;
+ }
+ velController.currentV = lastTargetVelocity;
+ velController.currentAcc = lastTargetAcceleration;
+ velController.dt = timeSinceLastIteration.toSecondsDouble();
+ velController.targetV = target.velocity;
+ // ARMARX_INFO << deactivateSpam(0.01) << VAROUT(target.velocity);
+ auto r = velController.run();
+ double newVel = r.velocity;
+ double newAcc = r.acceleration;
+
+ if (std::isnan(newVel))
+ {
+ newVel = 0;
+ newAcc = 0;
+ }
+ if ((currentPosition > velController.positionLimitHiSoft && target.velocity > 0)
+ || (currentPosition < velController.positionLimitLoSoft && target.velocity < 0))
+ {
+ newVel = 0;
+ newAcc = 0;
+ // ARMARX_INFO << deactivateSpam(0.01) << "Breaking now at " << dataPtr->getPosition() << " pwm: " << dataPtr->getTargetPWM();
+ }
- auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel, sensorValue->gravityTorque));
- dataPtr->setTargetPWM(targetPWM);
+ auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel, sensorValue->gravityTorque));
+ // ARMARX_INFO << deactivateSpam(0.01) << "before setting: " << VAROUT(targetPWM);
+ dataPtr->setTargetPWM(targetPWM);
- lastTargetVelocity = newVel;
- lastTargetAcceleration = newAcc;
+ lastTargetVelocity = newVel;
+ lastTargetAcceleration = newAcc;
- // ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
- // target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
+ // ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
+ // target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
- }
}
else
{
@@ -243,7 +242,13 @@ StringVariantBaseMap JointPWMVelocityController::publish(const DebugDrawerInterf
{"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
{"pidIntegralCV", new Variant(controller.pid->integral * controller.pid->Ki)},
{"pidPropCV", new Variant(controller.pid->previousError * controller.pid->Kp)},
- {"pidDiffCV", new Variant(controller.pid->derivative * controller.pid->Kd)}
+ {"pidDiffCV", new Variant(controller.pid->derivative * controller.pid->Kd)},
+ {"pidError", new Variant(controller.pid->previousError)},
+ {"pidIntegral", new Variant(controller.pid->integral)},
+ {"velControllerTargetV", new Variant(velController.targetV)},
+ {"velControllerCurrentV", new Variant(velController.currentV)},
+ {"currentMaxPWM", new Variant(dataPtr->getCurrentMaxPWM())},
+ {"currentMinPWM", new Variant(dataPtr->getCurrentMinPWM())}
};
}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp
index 2b83bac93f9102bce75baec4b05850ee8c91da4f..95cd867b41709602a12b325127667b225d48b5c8 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp
@@ -53,10 +53,11 @@ namespace armarx
pid->update(deltaT.toSecondsDouble(), lastActualVelocity, targetVelocity);
targetPWM = pid->getControlValue();
}
+ // ARMARX_INFO << deactivateSpam(0.01) << "a: " << VAROUT(targetPWM);
float torqueFF = config->feedforwardTorqueToPWMFactor * -gravityTorque;
// ARMARX_INFO << deactivateSpam(1) << VAROUT(torqueFF);
targetPWM += torqueFF;
-
+ // ARMARX_INFO << deactivateSpam(0.01) << "b: " << VAROUT(targetPWM);
//feed forward
if (std::abs(targetVelocity) > 0.001 && std::abs(currentVelocity) < 0.0001f)
@@ -64,15 +65,9 @@ namespace armarx
targetPWM += config->PWMDeadzone * math::MathUtils::Sign(targetVelocity); // deadzone
}
targetPWM += config->feedforwardVelocityToPWMFactor * targetVelocity; // approx. feedforward vel
-
-
-
-
+ // ARMARX_INFO << deactivateSpam(0.01) << "c: " << VAROUT(targetPWM);
// ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
// target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
-
-
-
return targetPWM;
}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
index 4f29f113f2ccc8d800ba43a515d4baed907857fe..b85763d2f9388ed27eb258737b79dcc0d259862e 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
@@ -51,7 +51,7 @@ namespace armarx
ARMARX_IMPORTANT << "Creating actor data class for " << name << " at index " << connectorIndex;
auto initActorData = [&](int* position, int* velocity, int* targetPWM)
{
- ARMARX_INFO << name << " " << connectorIndex << " filter window size: " << configNode.first_node("VelocityFilterWindowSize").value_as_int32();
+ // ARMARX_INFO << name << " " << connectorIndex << " filter window size: " << configNode.first_node("VelocityFilterWindowSize").value_as_int32();
actorData.at(connectorIndex).reset(new ActorData(configNode.first_node("VelocityFilterWindowSize").value_as_int32()));
actorData.at(connectorIndex)->targetPWMPtr.init(targetPWM, conversionNode.first_node("pwm"));
actorData.at(connectorIndex)->maxPWM = configNode.first_node("maxPWM").value_as_uint32();
@@ -126,7 +126,8 @@ namespace armarx
ActorData& d = *ptr;
d.relativePosition.read();
-
+ // ARMARX_IMPORTANT << deactivateSpam(0.01) << d.robotNode->getName() << "1: " << sensorOUT->motor1_current_pos << "2: " << sensorOUT->motor2_current_pos << "3: " << sensorOUT->motor3_current_pos;
+ // ARMARX_IMPORTANT << deactivateSpam(0.01) << d.robotNode->getName() << "1: " << sensorOUT->motor1_current_pos << "2: " << sensorOUT->motor2_current_pos << "3: " << sensorOUT->motor3_current_pos;
if (d.getSiblingControlActorIndex() >= 0) // if >=0 (0, 1, 2), device is coupled with another motor
{
auto& d2 = actorData.at(d.getSiblingControlActorIndex()); // get the index of the coupled motor
@@ -151,7 +152,7 @@ namespace armarx
(d2->position.value + jumpingOffset) * d.parallelGripperDecouplingFactor;
lastAbsoluteAngle = d2->position.value;
- // ARMARX_INFO << deactivateSpam(0.01) << "ddd adj rel pos: " << d.adjustedRelativePosition << " rel pos: " << d.relativePosition.value << " dec factor: " << d.parallelGripperDecouplingFactor;
+ // ARMARX_INFO << deactivateSpam(0.01) << "dec factor: " << d.parallelGripperDecouplingFactor;
}
else
{
@@ -166,10 +167,12 @@ namespace armarx
if (i == 0)
{
d.rawABSEncoderTicks = (((uint32_t)sensorOUT->RawABSEncoderValueBytes[0] << 24 | (uint32_t)sensorOUT->RawABSEncoderValueBytes[1] << 16 | (uint32_t)sensorOUT->RawABSEncoderValueBytes[2] << 8 | (uint32_t)sensorOUT->RawABSEncoderValueBytes[3]) & 0xFFFFF000) >> 12;
+ // ARMARX_IMPORTANT << deactivateSpam(0.01) << d.robotNode->getName() << "abs: " << d.rawABSEncoderTicks;
}
else if (i == 1)
{
d.rawABSEncoderTicks = (((uint32_t)sensorOUT->RawABSEncoder2ValueBytes[0] << 24 | (uint32_t)sensorOUT->RawABSEncoder2ValueBytes[1] << 16 | (uint32_t)sensorOUT->RawABSEncoder2ValueBytes[2] << 8 | (uint32_t)sensorOUT->RawABSEncoder2ValueBytes[3]) & 0xFFFFF000) >> 12;
+ // ARMARX_IMPORTANT << deactivateSpam(0.01) << d.robotNode->getName() << "abs: " << d.rawABSEncoderTicks;
}
else
{
@@ -230,8 +233,9 @@ namespace armarx
}
// d.torque.read();
- d.currentMaxPWM = std::round(*d.velocityTicks * d.currentPWMBoundGradient + d.currentPWMBoundOffset);
- d.currentMinPWM = std::round(*d.velocityTicks * d.currentPWMBoundGradient - d.currentPWMBoundOffset);
+ d.currentMinPWM = -std::round(*d.velocityTicks * d.currentPWMBoundGradient + d.currentPWMBoundOffset);
+ d.currentMaxPWM = -std::round(*d.velocityTicks * d.currentPWMBoundGradient - d.currentPWMBoundOffset);
+ // ARMARX_INFO << deactivateSpam(0.01) << d.robotNode->getName() << "1: " << VAROUT(*d.velocityTicks) << VAROUT(d.currentMaxPWM) << VAROUT(d.currentMinPWM);
d.acceleration = (d.velocity.value - oldVelocity) / dt;
// d.acceleration.read();
// d.gravityTorque.read();
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h
index d96244105fdd4f7533859e7e435f588465bcded9..c37d71d560bf44ecb1b11a0fa742a33ee85fb206 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h
@@ -57,7 +57,7 @@ namespace armarx
int16_t pad6;
- int32_t motor1_current_pos;
+ int32_t motor1_current_pos; // integration of the relative encoder ticks
int32_t motor1_current_speed; // ticks pro milliseconds
int32_t motor2_current_pos;
int32_t motor2_current_speed;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
index a31bc7d02144f5b1f39730cadf15cc4431492162..22b4817229a461fcd5960c98d7c977975e2b0348 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
@@ -26,7 +26,7 @@ set(LIB_HEADERS
list(APPEND LIB_HEADERS
- DMPController/NJointJointSpaceDMPController.h
+# DMPController/NJointJointSpaceDMPController.h
DMPController/NJointJSDMPController.h
DMPController/NJointTSDMPController.h
DMPController/NJointCCDMPController.h
@@ -39,7 +39,7 @@ set(LIB_HEADERS
)
list(APPEND LIB_FILES
- DMPController/NJointJointSpaceDMPController.cpp
+# DMPController/NJointJointSpaceDMPController.cpp
DMPController/NJointJSDMPController.cpp
DMPController/NJointTSDMPController.cpp
DMPController/NJointCCDMPController.cpp
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index 5a4700dc5ee7b1a618227e03f97b0260b213bd00..2e56fb7a76865a6d87b6cae924ef0f10fd040d23 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -170,7 +170,7 @@ void PIDController::update(double deltaSec, double measuredValue, double targetV
}
}
controlValue = std::min(fabs(controlValue), maxControlValue) * math::MathUtils::Sign(controlValue);
- ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) << " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
+ // ARMARX_INFO << deactivateSpam(0.01) << " error: " << error << " cV: " << (controlValue) << " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
previousError = error;
lastUpdateTime += IceUtil::Time::secondsDouble(deltaSec);