From 72439aa8e86f155bf2c67708c243de1efd2da68c Mon Sep 17 00:00:00 2001
From: Christian Dreher <c.dreher@kit.edu>
Date: Thu, 13 Mar 2025 21:27:45 +0100
Subject: [PATCH] fix: Max PWM values, clamping, PID initialization, shape
control targets
---
.../hand/armar7/njoint_controller/Shape.cpp | 14 ++++++--
.../armar7/sub_device/absolute/Device.cpp | 2 ++
.../armar7/sub_device/relative/Device.cpp | 2 ++
.../PositionVelocityPWMCascadedController.h | 34 ++++++++++---------
.../joint_controller/RelativePosition.cpp | 2 +-
.../joint_controller/RelativePosition.h | 10 +++++-
.../armar7de/joint_controller/Velocity.cpp | 2 +-
.../hand/armar7de/joint_controller/Velocity.h | 10 +++++-
8 files changed, 53 insertions(+), 23 deletions(-)
diff --git a/source/devices/ethercat/hand/armar7/njoint_controller/Shape.cpp b/source/devices/ethercat/hand/armar7/njoint_controller/Shape.cpp
index cd993bfa..46a87ec9 100644
--- a/source/devices/ethercat/hand/armar7/njoint_controller/Shape.cpp
+++ b/source/devices/ethercat/hand/armar7/njoint_controller/Shape.cpp
@@ -18,6 +18,7 @@
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
#include <ArmarXCore/observers/variant/Variant.h>
+
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/interface/WidgetDescription.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
@@ -26,6 +27,7 @@
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder/StaticWidgets.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder/StringWidgets.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
+
#include <RobotAPI/components/units/RobotUnit/ControlModes.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
@@ -57,7 +59,7 @@ namespace devices::ethercat::hand::armar7
ARMARX_CHECK_EXPRESSION(prov);
auto thumbDeviceName = config->deviceName + armar7de::constants::THUMB_FLEXION;
armarx::ControlTargetBase* ct =
- useControlTarget(thumbDeviceName, armar7de::ctrl_modes::Armar7deHandPwm);
+ useControlTarget(thumbDeviceName, armarx::ControlModes::Position1DoF);
ARMARX_CHECK_EXPRESSION(ct) << "control target: " << thumbDeviceName;
armarx::ConstSensorDevicePtr thumbSensor = peekSensorDevice(thumbDeviceName);
ARMARX_CHECK_EXPRESSION(thumbSensor) << "device name: " << thumbDeviceName;
@@ -69,7 +71,7 @@ namespace devices::ethercat::hand::armar7
thumbFlexionControlTarget = ct->asA<armar7de::RelativePositionControlTarget>();
auto fingersDeviceName = config->deviceName + armar7de::constants::FINGERS;
- ct = useControlTarget(fingersDeviceName, armar7de::ctrl_modes::Armar7deHandPwm);
+ ct = useControlTarget(fingersDeviceName, armarx::ControlModes::Position1DoF);
ARMARX_CHECK_EXPRESSION(ct) << "control target: " << fingersDeviceName;
armarx::ConstSensorDevicePtr fingersSensor = peekSensorDevice(fingersDeviceName);
ARMARX_CHECK_EXPRESSION(fingersSensor) << "device name: " << fingersDeviceName;
@@ -81,7 +83,7 @@ namespace devices::ethercat::hand::armar7
fingersFlexionControlTarget = ct->asA<armar7de::RelativePositionControlTarget>();
auto indexDeviceName = config->deviceName + armar7de::constants::INDEX;
- ct = useControlTarget(indexDeviceName, armar7de::ctrl_modes::Armar7deHandPwm);
+ ct = useControlTarget(indexDeviceName, armarx::ControlModes::Position1DoF);
ARMARX_CHECK_EXPRESSION(ct) << "control target: " << indexDeviceName;
armarx::ConstSensorDevicePtr indexSensor = peekSensorDevice(indexDeviceName);
ARMARX_CHECK_EXPRESSION(indexSensor) << "device name: " << indexDeviceName;
@@ -253,6 +255,8 @@ namespace devices::ethercat::hand::armar7
// "Stop motion" case where PWM is set to 0.
case State::zero_pwm:
{
+ // NOT WORKING RIGHT NOW.
+
fingersPwm = 0;
indexPwm = 0;
thumbPwm = 0;
@@ -262,6 +266,8 @@ namespace devices::ethercat::hand::armar7
// Special taring case.
case State::tare:
{
+ // NOT WORKING RIGHT NOW.
+
// Magic numbers to trigger taring the hand.
fingersPwm = 32'765;
indexPwm = 32'766;
@@ -272,6 +278,8 @@ namespace devices::ethercat::hand::armar7
// Special firmware reset case.
case State::reset_firmware:
{
+ // NOT WORKING RIGHT NOW.
+
// Magic numbers to trigger a firmware reset and reboot.
fingersPwm = 32'765;
indexPwm = 32'766;
diff --git a/source/devices/ethercat/hand/armar7/sub_device/absolute/Device.cpp b/source/devices/ethercat/hand/armar7/sub_device/absolute/Device.cpp
index e2275503..086f117b 100644
--- a/source/devices/ethercat/hand/armar7/sub_device/absolute/Device.cpp
+++ b/source/devices/ethercat/hand/armar7/sub_device/absolute/Device.cpp
@@ -7,6 +7,7 @@
#include <RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h>
#include <RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h>
#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
+
#include <armarx/control/ethercat/DeviceInterface.h>
#include <armarx/control/ethercat/ErrorReporting.h>
@@ -43,6 +44,7 @@ namespace devices::ethercat::hand::armar7::sub_device::absolute
addJointController(emergencyController.get());
addJointController(stopMovementController.get());
addJointController(pwmController.get());
+ addJointController(velocityController.get());
addJointController(positionController.get());
}
diff --git a/source/devices/ethercat/hand/armar7/sub_device/relative/Device.cpp b/source/devices/ethercat/hand/armar7/sub_device/relative/Device.cpp
index 71a61b89..d5025dfa 100644
--- a/source/devices/ethercat/hand/armar7/sub_device/relative/Device.cpp
+++ b/source/devices/ethercat/hand/armar7/sub_device/relative/Device.cpp
@@ -7,6 +7,7 @@
#include <RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h>
#include <RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h>
#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
+
#include <armarx/control/ethercat/DeviceInterface.h>
#include <armarx/control/ethercat/ErrorReporting.h>
@@ -48,6 +49,7 @@ namespace devices::ethercat::hand::armar7::sub_device::relative
addJointController(emergencyController.get());
addJointController(stopMovementController.get());
addJointController(pwmController.get());
+ addJointController(velocityController.get());
addJointController(positionController.get());
}
diff --git a/source/devices/ethercat/hand/armar7de/joint_controller/PositionVelocityPWMCascadedController.h b/source/devices/ethercat/hand/armar7de/joint_controller/PositionVelocityPWMCascadedController.h
index 488b5bdd..a99f8872 100644
--- a/source/devices/ethercat/hand/armar7de/joint_controller/PositionVelocityPWMCascadedController.h
+++ b/source/devices/ethercat/hand/armar7de/joint_controller/PositionVelocityPWMCascadedController.h
@@ -40,9 +40,9 @@ namespace devices::ethercat::hand::armar7de
};
PIDConfiguration position_pid_configuration;
- double max_velocity;
+ double max_velocity = 0;
PIDConfiguration velocity_pid_configuration;
- std::uint16_t max_pwm;
+ std::uint16_t max_pwm = 0;
};
PositionVelocityPWMCascadedController() = default;
@@ -56,27 +56,29 @@ namespace devices::ethercat::hand::armar7de
void
update_configuration(Configuration const& configuration)
{
- _velocity_based_position_controller.Kp =
- configuration.position_pid_configuration.proportional_gain;
- _velocity_based_position_controller.Ki =
- configuration.position_pid_configuration.integral_gain;
- _velocity_based_position_controller.Kd =
- configuration.position_pid_configuration.derivative_gain;
-
- _velocity_based_position_controller.setMaxControlValue(configuration.max_velocity);
+ _velocity_based_position_controller =
+ armarx::PIDController(configuration.position_pid_configuration.proportional_gain,
+ configuration.position_pid_configuration.integral_gain,
+ configuration.position_pid_configuration.derivative_gain);
+
+ set_max_velocity(configuration.max_velocity);
_velocity_based_position_controller.threadSafe = false;
- _pwm_based_velocity_controller.Kp =
- configuration.velocity_pid_configuration.proportional_gain;
- _pwm_based_velocity_controller.Ki =
- configuration.velocity_pid_configuration.integral_gain;
- _pwm_based_velocity_controller.Kd =
- configuration.velocity_pid_configuration.derivative_gain;
+ _pwm_based_velocity_controller =
+ armarx::PIDController(configuration.velocity_pid_configuration.proportional_gain,
+ configuration.velocity_pid_configuration.integral_gain,
+ configuration.velocity_pid_configuration.derivative_gain);
set_max_pwm(configuration.max_pwm);
_pwm_based_velocity_controller.threadSafe = false;
}
+ void
+ set_max_velocity(double new_max_velocity)
+ {
+ _velocity_based_position_controller.setMaxControlValue(new_max_velocity);
+ }
+
void
set_max_pwm(std::uint16_t new_max_pwm)
{
diff --git a/source/devices/ethercat/hand/armar7de/joint_controller/RelativePosition.cpp b/source/devices/ethercat/hand/armar7de/joint_controller/RelativePosition.cpp
index 0f8413a0..f85b9fb7 100644
--- a/source/devices/ethercat/hand/armar7de/joint_controller/RelativePosition.cpp
+++ b/source/devices/ethercat/hand/armar7de/joint_controller/RelativePosition.cpp
@@ -29,7 +29,7 @@ namespace devices::ethercat::hand::armar7de
.max_pwm = data->getMaxPWM(),
})
{
- ;
+ target.init(data->getMaxPWM());
}
armarx::ControlTargetBase*
diff --git a/source/devices/ethercat/hand/armar7de/joint_controller/RelativePosition.h b/source/devices/ethercat/hand/armar7de/joint_controller/RelativePosition.h
index 6ce3e7f7..6aad7b16 100644
--- a/source/devices/ethercat/hand/armar7de/joint_controller/RelativePosition.h
+++ b/source/devices/ethercat/hand/armar7de/joint_controller/RelativePosition.h
@@ -28,11 +28,19 @@ namespace devices::ethercat::hand::armar7de
public:
std::uint16_t maxPWM = std::numeric_limits<std::uint16_t>::max();
+ std::uint16_t configuredMaxPWM = maxPWM; // From hardware config as limit.
+
+ void
+ init(std::uint16_t maxPWM)
+ {
+ this->maxPWM = maxPWM;
+ configuredMaxPWM = maxPWM;
+ }
void
reset() override
{
- maxPWM = std::numeric_limits<std::uint16_t>::max();
+ maxPWM = configuredMaxPWM;
ControlTarget1DoFActuatorPosition::reset();
}
diff --git a/source/devices/ethercat/hand/armar7de/joint_controller/Velocity.cpp b/source/devices/ethercat/hand/armar7de/joint_controller/Velocity.cpp
index b26c3c3a..300bfc84 100644
--- a/source/devices/ethercat/hand/armar7de/joint_controller/Velocity.cpp
+++ b/source/devices/ethercat/hand/armar7de/joint_controller/Velocity.cpp
@@ -30,7 +30,7 @@ namespace devices::ethercat::hand::armar7de
})
{
- ;
+ target.init(data->getMaxPWM());
}
armarx::ControlTargetBase*
diff --git a/source/devices/ethercat/hand/armar7de/joint_controller/Velocity.h b/source/devices/ethercat/hand/armar7de/joint_controller/Velocity.h
index 22ae3a73..6a5b24e1 100644
--- a/source/devices/ethercat/hand/armar7de/joint_controller/Velocity.h
+++ b/source/devices/ethercat/hand/armar7de/joint_controller/Velocity.h
@@ -28,11 +28,19 @@ namespace devices::ethercat::hand::armar7de
public:
std::uint16_t maxPWM = std::numeric_limits<std::uint16_t>::max();
+ std::uint16_t configuredMaxPWM = maxPWM; // From hardware config as limit.
+
+ void
+ init(std::uint16_t maxPWM)
+ {
+ this->maxPWM = maxPWM;
+ configuredMaxPWM = maxPWM;
+ }
void
reset() override
{
- maxPWM = std::numeric_limits<std::uint16_t>::max();
+ maxPWM = configuredMaxPWM;
ControlTarget1DoFActuatorVelocity::reset();
}
--
GitLab