From 188a9af89a8a13c58e8754431054963e0110b937 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 16 Dec 2024 21:17:57 +0100
Subject: [PATCH] running clang-format-17 on codebase
---
.../applications/check_hwcfg/RobotMock.h | 79 +-
.../applications/check_hwcfg/RobotNodeMock.h | 23 +-
.../applications/check_hwcfg/SensorMock.h | 14 +-
.../applications/check_hwcfg/TestDeviceList.h | 29 +-
.../applications/check_hwcfg/main.cpp | 72 +-
.../ethercat/common/elmo/gold/Mappings.h | 5 -
source/devices/ethercat/common/h2t_devices.h | 2 -
.../ethercat/common/imagine_board/Config.h | 36 +-
.../ethercat/common/imagine_board/Data.cpp | 53 +-
.../ethercat/common/imagine_board/Data.h | 7 +-
.../common/imagine_board/SensorValue.h | 1 -
.../ethercat/common/imagine_board/Slave.cpp | 23 +-
.../ethercat/common/imagine_board/Slave.h | 1 -
.../ethercat/common/imagine_board/SlaveIO.h | 4 +-
.../ethercat/common/sanity_checking/Config.h | 27 +-
.../sensor_board/armar6/ADS1263CRCHelper.cpp | 55 +-
.../sensor_board/armar6/ADS1263CRCHelper.h | 5 +-
.../armar6/SensorboardSDOHelper.cpp | 109 +--
.../armar6/SensorboardSDOHelper.h | 124 +--
.../common/sensor_board/armar6/Slave.cpp | 26 -
.../common/sensor_board/armar6/Slave.h | 4 -
.../common/sensor_board/armar6/SlaveIO.h | 4 +-
.../common/sensor_board/armar7de/Config.cpp | 1 -
.../common/sensor_board/armar7de/Config.h | 3 +-
.../common/sensor_board/armar7de/Data.cpp | 8 +-
.../common/sensor_board/armar7de/Data.h | 5 +-
.../common/sensor_board/armar7de/SlaveIO.h | 4 +-
.../devices/ethercat/ethercat_hub/Slave.cpp | 5 +-
source/devices/ethercat/ethercat_hub/Slave.h | 37 +-
.../ethercat/ft_sensor_board/armar6/Config.h | 16 +-
.../ethercat/ft_sensor_board/armar6/Data.cpp | 70 +-
.../ethercat/ft_sensor_board/armar6/Data.h | 70 +-
.../ft_sensor_board/armar6/Device.cpp | 21 +-
.../ethercat/ft_sensor_board/armar6/Device.h | 4 +-
.../ethercat/ft_sensor_board/armar6/Slave.cpp | 12 -
.../ethercat/ft_sensor_board/armar6/Slave.h | 17 +-
.../ethercat/ft_sensor_board/armar6/SlaveIO.h | 20 +-
.../ethercat/ft_sensor_board/armarde/Config.h | 16 +-
.../ethercat/ft_sensor_board/armarde/Data.cpp | 24 +-
.../ethercat/ft_sensor_board/armarde/Data.h | 34 +-
.../ft_sensor_board/armarde/Device.cpp | 13 +-
.../ethercat/ft_sensor_board/armarde/Device.h | 3 +-
.../ft_sensor_board/armarde/ErrorDecoder.cpp | 3 +-
.../ft_sensor_board/armarde/Slave.cpp | 10 -
.../ft_sensor_board/armarde/SlaveIO.h | 1 -
.../devices/ethercat/hand/armar6_v1/Config.h | 42 +-
.../devices/ethercat/hand/armar6_v1/Data.cpp | 77 +-
source/devices/ethercat/hand/armar6_v1/Data.h | 62 +-
.../devices/ethercat/hand/armar6_v1/Device.h | 2 -
.../devices/ethercat/hand/armar6_v1/Slave.cpp | 8 -
.../devices/ethercat/hand/armar6_v1/Slave.h | 7 +-
.../devices/ethercat/hand/armar6_v1/SlaveIO.h | 4 +-
.../joint_controller/EmergencyStop.cpp | 4 -
.../joint_controller/EmergencyStop.h | 1 -
.../hand/armar6_v1/joint_controller/PWM.cpp | 5 -
.../hand/armar6_v1/joint_controller/PWM.h | 3 -
.../joint_controller/StopMovement.cpp | 4 -
.../armar6_v1/joint_controller/StopMovement.h | 1 -
.../armar6_v1/njoint_controller/Shape.cpp | 184 +++--
.../hand/armar6_v1/njoint_controller/Shape.h | 119 ++-
.../devices/ethercat/hand/armar6_v2/Config.h | 18 +-
.../devices/ethercat/hand/armar6_v2/Data.cpp | 65 +-
source/devices/ethercat/hand/armar6_v2/Data.h | 37 +-
.../devices/ethercat/hand/armar6_v2/Device.h | 2 +-
.../devices/ethercat/hand/armar6_v2/Slave.cpp | 12 -
.../devices/ethercat/hand/armar6_v2/Slave.h | 1 -
.../devices/ethercat/hand/armar6_v2/SlaveIO.h | 4 +-
.../joint_controller/EmergencyStop.cpp | 21 +-
.../joint_controller/EmergencyStop.h | 11 +-
.../hand/armar6_v2/joint_controller/PWM.cpp | 25 +-
.../hand/armar6_v2/joint_controller/PWM.h | 38 +-
.../armar6_v2/joint_controller/Position.cpp | 31 +-
.../armar6_v2/joint_controller/Position.h | 34 +-
.../joint_controller/StopMovement.cpp | 21 +-
.../armar6_v2/joint_controller/StopMovement.h | 11 +-
.../armar6_v2/njoint_controller/Shape.cpp | 90 ++-
.../hand/armar6_v2/njoint_controller/Shape.h | 53 +-
.../njoint_controller/ShapeInterface.ice | 40 +-
source/devices/ethercat/hand/armar7/Data.cpp | 7 -
source/devices/ethercat/hand/armar7/Data.h | 2 -
source/devices/ethercat/hand/armar7/Device.h | 2 -
.../ethercat/hand/armar7/ErrorDecoder.h | 2 -
source/devices/ethercat/hand/armar7/Slave.h | 1 -
.../hand/armar7/njoint_controller/Shape.cpp | 3 +-
.../hand/armar7/sub_device/absolute/Data.cpp | 16 +-
.../hand/armar7/sub_device/absolute/Data.h | 1 -
.../armar7/sub_device/absolute/Device.cpp | 4 -
.../hand/armar7/sub_device/absolute/Device.h | 2 -
.../hand/armar7/sub_device/relative/Data.cpp | 10 -
.../hand/armar7/sub_device/relative/Data.h | 2 -
.../armar7/sub_device/relative/Device.cpp | 8 +-
.../hand/armar7/sub_device/relative/Device.h | 3 +-
.../joint_controller/AbsolutePosition.cpp | 5 -
.../joint_controller/AbsolutePosition.h | 3 -
.../joint_controller/EmergencyStop.cpp | 18 +-
.../armar7de/joint_controller/EmergencyStop.h | 2 -
.../hand/armar7de/joint_controller/PWM.cpp | 5 -
.../hand/armar7de/joint_controller/PWM.h | 4 -
.../joint_controller/RelativePosition.cpp | 9 +-
.../joint_controller/RelativePosition.h | 3 -
.../joint_controller/StopMovement.cpp | 4 -
.../armar7de/joint_controller/StopMovement.h | 2 -
.../njoint_controller/ShapeInterface.ice | 65 +-
.../armar7de/sub_device/absolute/Config.h | 1 -
.../hand/armar7de/sub_device/absolute/Data.h | 1 -
.../armar7de/sub_device/relative/Config.h | 1 -
.../hand/armar7de/sub_device/relative/Data.h | 1 -
source/devices/ethercat/hand/armarde/Data.cpp | 25 +-
source/devices/ethercat/hand/armarde/Data.h | 3 +-
.../ethercat/hand/armarde/ErrorDecoder.cpp | 4 -
.../ethercat/hand/armarde/ErrorDecoder.h | 2 -
.../devices/ethercat/hand/armarde/Slave.cpp | 14 -
source/devices/ethercat/hand/armarde/Slave.h | 1 -
.../devices/ethercat/hand/armarde/SlaveIO.h | 2 -
.../hand/armarde/njoint_controller/Shape.cpp | 5 +-
.../hand/armarde/sub_device/absolute/Data.cpp | 17 +-
.../hand/armarde/sub_device/absolute/Data.h | 1 -
.../armarde/sub_device/absolute/Device.cpp | 4 -
.../hand/armarde/sub_device/absolute/Device.h | 2 -
.../hand/armarde/sub_device/relative/Data.cpp | 10 -
.../hand/armarde/sub_device/relative/Data.h | 2 -
.../armarde/sub_device/relative/Device.cpp | 8 +-
.../hand/armarde/sub_device/relative/Device.h | 3 +-
.../ethercat/hand/common/AbstractHand.h | 2 +-
.../common/AbstractHandControllerWrapper.cpp | 10 +-
.../hand/soft_finger_vision/BoardIn.h | 5 +-
.../hand/soft_finger_vision/BoardOut.h | 7 +-
.../soft_finger_vision/FunctionalDevice.cpp | 226 +++---
.../soft_finger_vision/FunctionalDevice.h | 41 +-
.../hand/soft_finger_vision/Slave.cpp | 9 +-
.../ethercat/hand/soft_finger_vision/Slave.h | 28 +-
.../hand/soft_finger_vision/bus_data/CNN.cpp | 32 +-
.../hand/soft_finger_vision/bus_data/CNN.h | 17 +-
.../hand/soft_finger_vision/bus_data/Hand.cpp | 59 +-
.../hand/soft_finger_vision/bus_data/Hand.h | 41 +-
.../soft_finger_vision/bus_data/Motor.cpp | 81 +-
.../hand/soft_finger_vision/bus_data/Motor.h | 79 +-
.../control_target/CNNObjectId.h | 37 +-
.../controller/PositionToPWM.cpp | 18 +-
.../controller/PositionToPWM.h | 2 +-
.../controller/VelocityToPWM.cpp | 24 +-
.../controller/VelocityToPWM.h | 4 +-
.../joint_controller/CNNEmergencyStop.cpp | 16 +-
.../joint_controller/CNNEmergencyStop.h | 12 +-
.../joint_controller/CNNStopMovement.cpp | 15 +-
.../joint_controller/CNNStopMovement.h | 4 +-
.../joint_controller/CNNTargetObject.cpp | 21 +-
.../joint_controller/CNNTargetObject.h | 11 +-
.../joint_controller/EmergencyStop.cpp | 19 +-
.../joint_controller/EmergencyStop.h | 14 +-
.../joint_controller/Position.cpp | 30 +-
.../joint_controller/Position.h | 23 +-
.../joint_controller/Pwm.cpp | 25 +-
.../soft_finger_vision/joint_controller/Pwm.h | 17 +-
.../joint_controller/StopMovement.cpp | 18 +-
.../joint_controller/StopMovement.h | 7 +-
.../joint_controller/Velocity.cpp | 30 +-
.../joint_controller/Velocity.h | 26 +-
.../njoint_controller/GraspOnDetection.cpp | 267 +++---
.../njoint_controller/GraspOnDetection.h | 60 +-
.../GraspOnDetectionInterface.ice | 31 +-
.../njoint_controller/Shape.cpp | 166 ++--
.../njoint_controller/Shape.h | 76 +-
.../njoint_controller/ShapeInterface.ice | 11 +-
.../soft_finger_vision/sensor_value/Hand.h | 12 +-
.../soft_finger_vision/sensor_value/Motor.h | 6 +-
.../soft_finger_vision/utility/Identities.h | 154 +++-
.../hand/soft_finger_vision/utility/Sensors.h | 20 +-
.../hand/soft_sensorized_finger/BoardIn.h | 5 +-
.../hand/soft_sensorized_finger/BoardOut.h | 42 +-
.../FunctionalDevice.cpp | 272 ++++---
.../soft_sensorized_finger/FunctionalDevice.h | 34 +-
.../hand/soft_sensorized_finger/Slave.cpp | 6 +
.../hand/soft_sensorized_finger/Slave.h | 12 +-
.../bus_data/Finger.cpp | 261 +++---
.../soft_sensorized_finger/bus_data/Finger.h | 69 +-
.../soft_sensorized_finger/bus_data/Hand.cpp | 67 +-
.../soft_sensorized_finger/bus_data/Hand.h | 40 +-
.../soft_sensorized_finger/bus_data/Motor.cpp | 81 +-
.../soft_sensorized_finger/bus_data/Motor.h | 92 ++-
.../controller/CloseAndHold.cpp | 34 +-
.../controller/CloseAndHold.h | 22 +-
.../controller/PositionToPWM.cpp | 18 +-
.../controller/PositionToPWM.h | 2 +-
.../controller/VelocityToPWM.cpp | 24 +-
.../controller/VelocityToPWM.h | 4 +-
.../joint_controller/EmergencyStop.cpp | 19 +-
.../joint_controller/EmergencyStop.h | 13 +-
.../joint_controller/Position.cpp | 30 +-
.../joint_controller/Position.h | 24 +-
.../joint_controller/Pwm.cpp | 25 +-
.../joint_controller/Pwm.h | 12 +-
.../joint_controller/StopMovement.cpp | 18 +-
.../joint_controller/StopMovement.h | 9 +-
.../joint_controller/Velocity.cpp | 30 +-
.../joint_controller/Velocity.h | 29 +-
.../njoint_controller/CloseUntilContact.cpp | 186 +++--
.../njoint_controller/CloseUntilContact.h | 120 +--
.../MinimalGraspingForceV1.cpp | 761 ++++++++++--------
.../MinimalGraspingForceV1.h | 95 ++-
.../MinimalGraspingForceV1Interface.ice | 207 ++---
.../njoint_controller/Shape.cpp | 171 ++--
.../njoint_controller/Shape.h | 80 +-
.../njoint_controller/ShapeInterface.ice | 39 +-
.../minimal_grasping_force_v1/Accelerometer.h | 27 +-
.../minimal_grasping_force_v1/Config.h | 71 +-
.../minimal_grasping_force_v1/FingerSensor.h | 23 +-
.../GraspPhaseControllerData.h | 27 +-
.../MeanFreeSensor.h | 80 +-
.../MotorGraspPhaseControllerData.h | 194 ++---
.../ShearForceSensor.h | 8 +-
.../sensor_value/Finger.h | 197 ++---
.../sensor_value/Hand.h | 17 +-
.../sensor_value/Motor.h | 19 +-
.../utility/Identities.h | 154 +++-
.../utility/SensorFit.cpp | 303 +++----
.../utility/SensorFit.h | 117 ++-
.../soft_sensorized_finger/utility/Sensors.h | 47 +-
source/devices/ethercat/head/armar6/Config.h | 17 +-
.../ethercat/head/armar6/ControlTargets.h | 30 +-
.../devices/ethercat/head/armar6/Device.cpp | 47 +-
source/devices/ethercat/head/armar6/Device.h | 13 +-
.../ethercat/head/armar6/PWMVelocity.cpp | 42 +-
.../ethercat/head/armar6/PWMVelocity.h | 33 +-
.../armar6/joint_controller/EmergencyStop.cpp | 23 +-
.../armar6/joint_controller/EmergencyStop.h | 16 +-
.../head/armar6/joint_controller/PWM.cpp | 19 +-
.../head/armar6/joint_controller/PWM.h | 17 +-
.../armar6/joint_controller/PWMPosition.cpp | 288 ++++---
.../armar6/joint_controller/PWMPosition.h | 43 +-
.../armar6/joint_controller/PWMVelocity.cpp | 211 ++---
.../armar6/joint_controller/PWMVelocity.h | 36 +-
.../armar6/joint_controller/StopMovement.cpp | 19 +-
.../armar6/joint_controller/StopMovement.h | 15 +-
.../armar6/joint_controller/ZeroTorque.cpp | 44 +-
.../head/armar6/joint_controller/ZeroTorque.h | 34 +-
.../njoint_controller/PWMPassThrough.cpp | 41 +-
.../armar6/njoint_controller/PWMPassThrough.h | 57 +-
.../sub_device/DeviceControlModeSetter.h | 5 +-
.../rotation/joint_controller/Position.cpp | 1 -
.../rotation/joint_controller/Position.h | 1 -
.../joint_controller/PositionWithVelocity.h | 1 -
.../ethercat/head_board/armar7de/Config.h | 29 +-
.../head_board/armar7de/ControlTargets.h | 13 +-
.../ethercat/head_board/armar7de/Data.cpp | 18 +-
.../ethercat/head_board/armar7de/Data.h | 15 +-
.../ethercat/head_board/armar7de/Device.cpp | 26 +-
.../ethercat/head_board/armar7de/Device.h | 13 +-
.../head_board/armar7de/ErrorDecoder.cpp | 2 +-
.../head_board/armar7de/LedControlData.cpp | 20 +-
.../head_board/armar7de/LedControlData.h | 6 +-
.../head_board/armar7de/PWMVelocity.cpp | 6 +-
.../head_board/armar7de/PWMVelocity.h | 3 -
.../head_board/armar7de/SensorValue.h | 1 -
.../ethercat/head_board/armar7de/Slave.cpp | 8 -
.../ethercat/head_board/armar7de/Slave.h | 5 +-
.../ethercat/head_board/armar7de/SlaveIO.h | 2 -
.../joint_controller/EmergencyStop.cpp | 10 +-
.../armar7de/joint_controller/EmergencyStop.h | 4 -
.../armar7de/joint_controller/LedColor.cpp | 16 +-
.../armar7de/joint_controller/LedColor.h | 4 +-
.../joint_controller/LedEmergencyStop.cpp | 14 +-
.../joint_controller/LedEmergencyStop.h | 10 +-
.../joint_controller/LedStopMovement.cpp | 13 +-
.../joint_controller/LedStopMovement.h | 2 +
.../armar7de/joint_controller/PWM.cpp | 4 +-
.../armar7de/joint_controller/PWM.h | 1 -
.../armar7de/joint_controller/PWMPosition.cpp | 14 +-
.../armar7de/joint_controller/PWMPosition.h | 3 +-
.../armar7de/joint_controller/PWMVelocity.cpp | 18 +-
.../armar7de/joint_controller/PWMVelocity.h | 3 -
.../joint_controller/StopMovement.cpp | 8 +-
.../armar7de/joint_controller/StopMovement.h | 3 -
.../armar7de/joint_controller/ZeroTorque.cpp | 8 +-
.../armar7de/joint_controller/ZeroTorque.h | 5 +-
.../armar7de/njoint_controller/EyeColor.cpp | 71 +-
.../armar7de/njoint_controller/EyeColor.h | 55 +-
.../njoint_controller/EyeColorInterface.ice | 18 +-
.../armar6_mecanum/Armar6MecanumPlatform.cpp | 45 +-
.../armar6_mecanum/Armar6MecanumPlatform.h | 7 +-
.../ethercat/platform/armar6_mecanum/Data.cpp | 165 ++--
.../ethercat/platform/armar6_mecanum/Data.h | 30 +-
.../platform/armar6_mecanum/Device.cpp | 22 +-
.../ethercat/platform/armar6_mecanum/Device.h | 15 +-
.../joint_controller/Emergency.cpp | 9 +-
.../joint_controller/Emergency.h | 9 +-
.../joint_controller/StopMovement.cpp | 15 +-
.../joint_controller/StopMovement.h | 9 +-
.../joint_controller/Velocity.cpp | 54 +-
.../joint_controller/Velocity.h | 44 +-
.../armar7_omni/joint_controller/Emergency.h | 1 -
.../armar7_omni/joint_controller/Velocity.h | 1 -
source/devices/ethercat/power_board/Config.h | 36 +-
.../ethercat/power_board/ControlTargets.h | 3 +-
source/devices/ethercat/power_board/Data.cpp | 4 +-
.../ethercat/power_board/LedControlData.h | 6 +-
source/devices/ethercat/power_board/Slave.cpp | 2 -
source/devices/ethercat/power_board/Slave.h | 2 +-
source/devices/ethercat/power_board/SlaveIO.h | 2 -
.../power_board/joint_controller/LedColor.h | 1 +
.../joint_controller/LedEmergencyStop.cpp | 12 +-
.../joint_controller/LedEmergencyStop.h | 11 +-
.../joint_controller/LedStopMovement.cpp | 14 +-
.../joint_controller/LedStopMovement.h | 2 +
.../led_control/LedColorController.cpp | 3 -
.../njoint_controller/LedStripsColors.cpp | 6 -
.../njoint_controller/LedStripsColors.h | 2 -
.../test/LedColorControllerTest.cpp | 13 +-
.../sensor_actor_unit/armar6/Data.cpp | 21 +-
.../ethercat/sensor_actor_unit/armar6/Data.h | 38 +-
.../sensor_actor_unit/armar6/Device.cpp | 44 +-
.../sensor_actor_unit/armar6/Device.h | 17 +-
.../joint_controller/ActiveImpedance.cpp | 111 +--
.../armar6/joint_controller/ActiveImpedance.h | 34 +-
.../armar6/joint_controller/Current.cpp | 22 +-
.../armar6/joint_controller/Current.h | 16 +-
.../joint_controller/CurrentPassThrough.cpp | 24 +-
.../joint_controller/CurrentPassThrough.h | 16 +-
.../armar6/joint_controller/ElmoEmergency.h | 2 +-
.../armar6/joint_controller/Position.cpp | 35 +-
.../armar6/joint_controller/Position.h | 28 +-
.../armar6/joint_controller/StopMovement.h | 2 +-
.../armar6/joint_controller/Torque.h | 26 +-
.../armar6/joint_controller/Velocity.cpp | 49 +-
.../armar6/joint_controller/Velocity.h | 30 +-
.../joint_controller/VelocityTorque.cpp | 137 ++--
.../armar6/joint_controller/VelocityTorque.h | 34 +-
.../armar6/joint_controller/ZeroTorque.cpp | 486 +++++------
.../armar6/joint_controller/ZeroTorque.h | 38 +-
.../armar6/joint_controller/ZeroVelocity.cpp | 50 +-
.../armar6/joint_controller/ZeroVelocity.h | 36 +-
.../armar7_wrist/ControlTargets.cpp | 1 -
.../armar7_wrist/ControlTargets.h | 1 -
.../sensor_actor_unit/armar7_wrist/Data.cpp | 18 +-
.../sensor_actor_unit/armar7_wrist/Device.cpp | 1 -
.../sensor_actor_unit/armar7_wrist/Device.h | 20 +-
.../armar7_wrist/PerActuator.h | 11 +-
.../armar7_wrist/SensorValue.cpp | 55 +-
.../armar7_wrist/SensorValue.h | 1 +
.../joint_controller/ElmoEmergency.h | 2 +-
.../armar7_wrist/joint_controller/Position.h | 1 -
.../joint_controller/StopMovement.cpp | 9 +-
.../joint_controller/StopMovement.h | 6 +-
.../armar7_wrist/joint_controller/Torque.cpp | 13 +-
.../armar7_wrist/joint_controller/Torque.h | 4 +-
.../joint_controller/Velocity copy.h | 1 -
.../armar7_wrist/joint_controller/Velocity.h | 1 -
.../joint_controller/ZeroTorque.cpp | 394 ++++-----
.../joint_controller/ZeroTorque.h | 46 +-
.../joint_controller/ZeroVelocity.cpp | 2 -
.../joint_controller/ZeroVelocity.h | 2 -
.../armar7_wrist/joint_controller/common.cpp | 1 -
.../armar7_wrist/joint_controller/common.h | 8 +-
.../joint_controller/forward_declarations.h | 1 -
.../joint_controller/interfaces.h | 3 +-
.../armar7_wrist/sensor_board/Config.cpp | 9 +-
.../armar7_wrist/sensor_board/Data.cpp | 1 -
.../armar7_wrist/sensor_board/Data.h | 25 +-
.../armar7_wrist/sensor_board/Slave.h | 3 +-
.../armar7_wrist/sensor_board/SlaveIO.cpp | 1 -
.../sub_device/DeviceControlModeSetter.h | 2 +-
.../sub_device/hemisphere/ControllerData.h | 1 -
.../sub_device/hemisphere/Device.h | 9 +-
.../sub_device/rotation/ControllerData.h | 8 +-
.../armar7_wrist/sub_device/rotation/Device.h | 14 +-
.../sensor_actor_unit/armar7de/Data.h | 2 -
.../sensor_actor_unit/armar7de/Device.cpp | 1 -
.../sensor_actor_unit/armar7de/Device.h | 2 +-
.../joint_controller/ActiveImpedance.cpp | 111 +--
.../joint_controller/ActiveImpedance.h | 34 +-
.../armar7de/joint_controller/Current.cpp | 22 +-
.../armar7de/joint_controller/Current.h | 16 +-
.../joint_controller/CurrentPassThrough.cpp | 25 +-
.../joint_controller/CurrentPassThrough.h | 16 +-
.../armar7de/joint_controller/Position.cpp | 1 -
.../armar7de/joint_controller/Torque.cpp | 2 -
.../joint_controller/VelocityTorque.cpp | 137 ++--
.../joint_controller/VelocityTorque.h | 34 +-
.../armar7de/joint_controller/ZeroTorque.cpp | 392 ++++-----
.../armar7de/joint_controller/ZeroTorque.h | 43 +-
.../devices/ethercat/template_device/Config.h | 15 +-
.../ethercat/template_device/ControlTargets.h | 3 +-
.../devices/ethercat/template_device/Data.cpp | 10 +-
.../devices/ethercat/template_device/Data.h | 5 +-
.../ethercat/template_device/Device.cpp | 13 +-
.../devices/ethercat/template_device/Device.h | 3 +-
.../ethercat/template_device/Slave.cpp | 10 -
.../ethercat/template_device/SlaveIO.h | 2 -
.../joint_controller/LedColor.h | 1 +
.../joint_controller/LedEmergencyStop.cpp | 1 +
.../joint_controller/LedStopMovement.h | 1 +
.../ethercat/torso/armar6_prismatic/Data.h | 9 +-
.../torso/armar6_prismatic/Device.cpp | 15 +-
.../ethercat/torso/armar6_prismatic/Device.h | 1 -
.../joint_controller/Emergency.cpp | 6 -
.../joint_controller/Emergency.h | 13 +-
.../joint_controller/Position.cpp | 35 +-
.../joint_controller/Position.h | 22 +-
.../joint_controller/Velocity.cpp | 28 +-
.../joint_controller/Velocity.h | 22 +-
400 files changed, 6988 insertions(+), 6562 deletions(-)
mode change 100755 => 100644 source/devices/ethercat/platform/armar6_mecanum/Armar6MecanumPlatform.cpp
mode change 100755 => 100644 source/devices/ethercat/platform/armar6_mecanum/Armar6MecanumPlatform.h
diff --git a/source/devices/ethercat/applications/check_hwcfg/RobotMock.h b/source/devices/ethercat/applications/check_hwcfg/RobotMock.h
index b6524d72..9f6d32dc 100644
--- a/source/devices/ethercat/applications/check_hwcfg/RobotMock.h
+++ b/source/devices/ethercat/applications/check_hwcfg/RobotMock.h
@@ -1,9 +1,11 @@
#pragma once
#include <VirtualRobot/Robot.h>
+
#include "SensorMock.h"
-namespace devices::ethercat {
+namespace devices::ethercat
+{
class RobotMock : public VirtualRobot::Robot
{
public:
@@ -11,42 +13,48 @@ namespace devices::ethercat {
{
}
- void setRootNode(VirtualRobot::RobotNodePtr node) override
+ void
+ setRootNode(VirtualRobot::RobotNodePtr node) override
{
}
- VirtualRobot::RobotNodePtr getRootNode() const override
+ VirtualRobot::RobotNodePtr
+ getRootNode() const override
{
return {};
}
- void registerRobotNode(VirtualRobot::RobotNodePtr node) override
+ void
+ registerRobotNode(VirtualRobot::RobotNodePtr node) override
{
-
}
- void deregisterRobotNode(VirtualRobot::RobotNodePtr node) override
+ void
+ deregisterRobotNode(VirtualRobot::RobotNodePtr node) override
{
-
}
- bool hasRobotNode(VirtualRobot::RobotNodePtr node) const override
+ bool
+ hasRobotNode(VirtualRobot::RobotNodePtr node) const override
{
return robotNode == node;
}
- bool hasRobotNode(const std::string& robotNodeName) const override
+ bool
+ hasRobotNode(const std::string& robotNodeName) const override
{
return robotNodeName == rnName;
}
- void setRobotNode(std::string name, VirtualRobot::RobotNodePtr robotNode)
+ void
+ setRobotNode(std::string name, VirtualRobot::RobotNodePtr robotNode)
{
this->rnName = name;
this->robotNode = robotNode;
}
- VirtualRobot::RobotNodePtr getRobotNode(const std::string& robotNodeName) const override
+ VirtualRobot::RobotNodePtr
+ getRobotNode(const std::string& robotNodeName) const override
{
if (robotNodeName == rnName)
{
@@ -58,67 +66,74 @@ namespace devices::ethercat {
}
}
- void getRobotNodes(std::vector< VirtualRobot::RobotNodePtr >& storeNodes, bool clearVector = true) const override
+ void
+ getRobotNodes(std::vector<VirtualRobot::RobotNodePtr>& storeNodes,
+ bool clearVector = true) const override
{
-
}
- void registerRobotNodeSet(VirtualRobot::RobotNodeSetPtr nodeSet) override
+ void
+ registerRobotNodeSet(VirtualRobot::RobotNodeSetPtr nodeSet) override
{
-
}
- void deregisterRobotNodeSet(VirtualRobot::RobotNodeSetPtr nodeSet) override
+ void
+ deregisterRobotNodeSet(VirtualRobot::RobotNodeSetPtr nodeSet) override
{
-
}
- VirtualRobot::RobotNodeSetPtr getRobotNodeSet(const std::string& nodeSetName) const override
+ VirtualRobot::RobotNodeSetPtr
+ getRobotNodeSet(const std::string& nodeSetName) const override
{
return {};
}
- void getRobotNodeSets(std::vector<VirtualRobot::RobotNodeSetPtr>& storeNodeSet) const override
+ void
+ getRobotNodeSets(std::vector<VirtualRobot::RobotNodeSetPtr>& storeNodeSet) const override
{
-
}
- void registerEndEffector(VirtualRobot::EndEffectorPtr endEffector) override
+ void
+ registerEndEffector(VirtualRobot::EndEffectorPtr endEffector) override
{
-
}
- bool hasEndEffector(const std::string& endEffectorName) const override
+ bool
+ hasEndEffector(const std::string& endEffectorName) const override
{
return false;
}
- VirtualRobot::EndEffectorPtr getEndEffector(const std::string& endEffectorName) const override
+ VirtualRobot::EndEffectorPtr
+ getEndEffector(const std::string& endEffectorName) const override
{
return {};
}
- void getEndEffectors(std::vector<VirtualRobot::EndEffectorPtr>& storeEEF) const override
+ void
+ getEndEffectors(std::vector<VirtualRobot::EndEffectorPtr>& storeEEF) const override
{
-
}
- virtual bool hasRobotNodeSet(const std::string& name) const override
+ virtual bool
+ hasRobotNodeSet(const std::string& name) const override
{
return false;
}
- virtual void setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyValues) override
+ virtual void
+ setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyValues) override
{
-
}
- virtual std::shared_ptr<VirtualRobot::Sensor> getSensor(const std::string& name) const override
+ virtual std::shared_ptr<VirtualRobot::Sensor>
+ getSensor(const std::string& name) const override
{
return std::make_shared<SensorMock>(std::weak_ptr(robotNode), "name");
}
- void init(VirtualRobot::RobotPtr ptr)
+ void
+ init(VirtualRobot::RobotPtr ptr)
{
self = ptr;
}
@@ -127,4 +142,4 @@ namespace devices::ethercat {
VirtualRobot::RobotNodePtr robotNode;
std::string rnName = "";
};
-}
+} // namespace devices::ethercat
diff --git a/source/devices/ethercat/applications/check_hwcfg/RobotNodeMock.h b/source/devices/ethercat/applications/check_hwcfg/RobotNodeMock.h
index a7ff397d..746860a9 100644
--- a/source/devices/ethercat/applications/check_hwcfg/RobotNodeMock.h
+++ b/source/devices/ethercat/applications/check_hwcfg/RobotNodeMock.h
@@ -3,26 +3,33 @@
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/VirtualRobot.h>
-namespace devices::ethercat {
+namespace devices::ethercat
+{
class RobotNodeMock : public VirtualRobot::RobotNode
{
public:
RobotNodeMock(VirtualRobot::RobotWeakPtr rob,
- const std::string& name,
- float jointLimitLo,
- float jointLimitHi)
- : VirtualRobot::RobotNode(rob, name, jointLimitLo, jointLimitHi)
+ const std::string& name,
+ float jointLimitLo,
+ float jointLimitHi) :
+ VirtualRobot::RobotNode(rob, name, jointLimitLo, jointLimitHi)
{
}
- VirtualRobot::RobotNodePtr _clone(const VirtualRobot::RobotPtr newRobot, const VirtualRobot::VisualizationNodePtr visualizationModel, const VirtualRobot::CollisionModelPtr collisionModel, VirtualRobot::CollisionCheckerPtr colChecker, float scaling) override
+ VirtualRobot::RobotNodePtr
+ _clone(const VirtualRobot::RobotPtr newRobot,
+ const VirtualRobot::VisualizationNodePtr visualizationModel,
+ const VirtualRobot::CollisionModelPtr collisionModel,
+ VirtualRobot::CollisionCheckerPtr colChecker,
+ float scaling) override
{
return {};
}
- std::string _toXML(const std::string& modelPath) override
+ std::string
+ _toXML(const std::string& modelPath) override
{
return "";
}
};
-}
+} // namespace devices::ethercat
diff --git a/source/devices/ethercat/applications/check_hwcfg/SensorMock.h b/source/devices/ethercat/applications/check_hwcfg/SensorMock.h
index f6350dfa..f6f455a5 100644
--- a/source/devices/ethercat/applications/check_hwcfg/SensorMock.h
+++ b/source/devices/ethercat/applications/check_hwcfg/SensorMock.h
@@ -2,19 +2,23 @@
#include <VirtualRobot/Nodes/Sensor.h>
-namespace devices::ethercat {
+namespace devices::ethercat
+{
class SensorMock : public VirtualRobot::Sensor
{
public:
SensorMock(std::weak_ptr<VirtualRobot::GraspableSensorizedObject> node,
- const std::string& name) : VirtualRobot::Sensor(node, name)
+ const std::string& name) :
+ VirtualRobot::Sensor(node, name)
{
-
}
- std::shared_ptr<VirtualRobot::Sensor> _clone(const std::shared_ptr<VirtualRobot::GraspableSensorizedObject> node, const std::shared_ptr<VirtualRobot::VisualizationNode> visualizationModel, float scaling) override
+ std::shared_ptr<VirtualRobot::Sensor>
+ _clone(const std::shared_ptr<VirtualRobot::GraspableSensorizedObject> node,
+ const std::shared_ptr<VirtualRobot::VisualizationNode> visualizationModel,
+ float scaling) override
{
return {};
}
};
-}
+} // namespace devices::ethercat
diff --git a/source/devices/ethercat/applications/check_hwcfg/TestDeviceList.h b/source/devices/ethercat/applications/check_hwcfg/TestDeviceList.h
index 5af710d7..f8eb1958 100644
--- a/source/devices/ethercat/applications/check_hwcfg/TestDeviceList.h
+++ b/source/devices/ethercat/applications/check_hwcfg/TestDeviceList.h
@@ -1,31 +1,44 @@
#pragma once
-#include <map>
#include <functional>
-#include <armarx/control/hardware_config/DeviceConfig.h>
+#include <map>
+
#include <VirtualRobot/Robot.h>
-namespace devices::ethercat {
+#include <armarx/control/hardware_config/DeviceConfig.h>
+
+namespace devices::ethercat
+{
class TestDeviceList
{
public:
- void registerDevice(std::string typeName, std::function<void(std::shared_ptr<armarx::control::hardware_config::DeviceConfig>&)> testFunc)
+ void
+ registerDevice(
+ std::string typeName,
+ std::function<void(std::shared_ptr<armarx::control::hardware_config::DeviceConfig>&)>
+ testFunc)
{
m[typeName] = testFunc;
}
- void testDevice(std::shared_ptr<armarx::control::hardware_config::DeviceConfig>& conf)
+ void
+ testDevice(std::shared_ptr<armarx::control::hardware_config::DeviceConfig>& conf)
{
auto it = m.find(conf->getType());
if (it == m.end())
{
- std::cout << "Device with name '" << conf->getName() << "' and type '" << conf->getType() << "' not tested" << std::endl << std::endl;
+ std::cout << "Device with name '" << conf->getName() << "' and type '"
+ << conf->getType() << "' not tested" << std::endl
+ << std::endl;
return;
}
(it->second)(conf);
}
private:
- std::map<std::string, std::function<void(std::shared_ptr<armarx::control::hardware_config::DeviceConfig>&)>> m;
+ std::map<
+ std::string,
+ std::function<void(std::shared_ptr<armarx::control::hardware_config::DeviceConfig>&)>>
+ m;
};
-}
+} // namespace devices::ethercat
diff --git a/source/devices/ethercat/applications/check_hwcfg/main.cpp b/source/devices/ethercat/applications/check_hwcfg/main.cpp
index a4a989c3..c1fc23d7 100644
--- a/source/devices/ethercat/applications/check_hwcfg/main.cpp
+++ b/source/devices/ethercat/applications/check_hwcfg/main.cpp
@@ -2,22 +2,21 @@
#include <armarx/control/hardware_config/ConfigParser.h>
// Devices armarde
-#include <devices/ethercat/sensor_actor_unit/armar7de/Device.h>
#include <devices/ethercat/head_board/armar7de/Device.h>
#include <devices/ethercat/platform/armar6_mecanum/Device.h>
-#include <devices/ethercat/torso/armar6_prismatic/Device.h>
#include <devices/ethercat/power_board/Device.h>
-
-// Devices armar6
-#include <devices/ethercat/sensor_actor_unit/armar6/Device.h>
-#include <devices/ethercat/platform/armar6_mecanum/Device.h>
+#include <devices/ethercat/sensor_actor_unit/armar7de/Device.h>
#include <devices/ethercat/torso/armar6_prismatic/Device.h>
-#include <devices/ethercat/hand/armar6_v2/Device.h>
-#include <devices/ethercat/ft_sensor_board/armar6/Device.h>
+// Devices armar6
#include "RobotMock.h"
#include "RobotNodeMock.h"
#include "TestDeviceList.h"
+#include <devices/ethercat/ft_sensor_board/armar6/Device.h>
+#include <devices/ethercat/hand/armar6_v2/Device.h>
+#include <devices/ethercat/platform/armar6_mecanum/Device.h>
+#include <devices/ethercat/sensor_actor_unit/armar6/Device.h>
+#include <devices/ethercat/torso/armar6_prismatic/Device.h>
using namespace armarx::control::hardware_config;
using namespace armarx;
@@ -77,11 +76,14 @@ readHardwareConfigFile(std::string filePath)
return multiNode;
}
-template<typename T>
-inline void testDevice(std::shared_ptr<DeviceConfig>& conf, VirtualRobot::RobotPtr const& robot)
+template <typename T>
+inline void
+testDevice(std::shared_ptr<DeviceConfig>& conf, VirtualRobot::RobotPtr const& robot)
{
- std::cout << "Testing device with name '" << conf->getName() << "' and type '" << conf->getType() << "': ";
- try {
+ std::cout << "Testing device with name '" << conf->getName() << "' and type '"
+ << conf->getType() << "': ";
+ try
+ {
T dev(*conf, robot);
std::vector<std::string> itemsNotReadErrors;
@@ -90,15 +92,15 @@ inline void testDevice(std::shared_ptr<DeviceConfig>& conf, VirtualRobot::RobotP
if (itemsNotReadErrors.size() == 0)
{
std::cout << "No issues found" << std::endl;
- } else {
+ }
+ else
+ {
std::cout << "Errors from items defined only in XML:" << std::endl;
for (auto& s : itemsNotReadErrors)
{
std::cout << s << std::endl;
}
}
-
-
}
catch (const std::exception& ex)
{
@@ -108,12 +110,14 @@ inline void testDevice(std::shared_ptr<DeviceConfig>& conf, VirtualRobot::RobotP
std::cout << std::endl;
}
-template<typename T>
-inline void testDeviceWithDefaultMock(std::shared_ptr<DeviceConfig>& conf)
+template <typename T>
+inline void
+testDeviceWithDefaultMock(std::shared_ptr<DeviceConfig>& conf)
{
std::shared_ptr<RobotMock> robot = std::make_shared<RobotMock>();
robot->init(std::dynamic_pointer_cast<VirtualRobot::Robot>(robot));
- robot->setRobotNode(conf->getName(), std::make_shared<RobotNodeMock>(robot, conf->getName(), 3, 10));
+ robot->setRobotNode(conf->getName(),
+ std::make_shared<RobotNodeMock>(robot, conf->getName(), 3, 10));
testDevice<T>(conf, robot);
}
@@ -158,7 +162,8 @@ main(int argc, char* argv[])
}
else
{
- std::cout << "Testing devices for robot '" << devicesStr << "' not supported" << std::endl;
+ std::cout << "Testing devices for robot '" << devicesStr << "' not supported"
+ << std::endl;
return 1;
}
}
@@ -179,7 +184,8 @@ main(int argc, char* argv[])
std::cout << "Input file is missing. Usage: check_hwcfg <HardwareConfigFile>" << std::endl;
std::cout << "Options:" << std::endl;
std::cout << "--print-structure -p Print the parsed data" << std::endl;
- std::cout << "--test-devices=<robot> -t=<r> Test devices by specifying a robot" << std::endl;
+ std::cout << "--test-devices=<robot> -t=<r> Test devices by specifying a robot"
+ << std::endl;
}
filePath = argv[1];
@@ -198,7 +204,7 @@ main(int argc, char* argv[])
char* posCStr = err.where<char>();
size_t len = strlen(posCStr);
- (void) len;
+ (void)len;
char shortenedStr[1000] = {0};
strncpy(shortenedStr, posCStr, 999);
std::cout << shortenedStr << std::endl;
@@ -256,18 +262,26 @@ main(int argc, char* argv[])
// Register Devices according to selected robot
if (testDevices == TestDevices::ArmarDE)
{
- devs.registerDevice("Joint", testDeviceWithDefaultMock<sensor_actor_unit::armar7de::Device>);
- devs.registerDevice("HeadBoardArmar7de", testDeviceWithDefaultMock<head_board::armar7de::Device>);
- devs.registerDevice("HolonomicPlatform", testDeviceWithDefaultMock<platform::armar6_mecanum::Device>);
- devs.registerDevice("PrismaticJoint", testDeviceWithDefaultMock<torso::armar6_prismatic::Device>);
+ devs.registerDevice("Joint",
+ testDeviceWithDefaultMock<sensor_actor_unit::armar7de::Device>);
+ devs.registerDevice("HeadBoardArmar7de",
+ testDeviceWithDefaultMock<head_board::armar7de::Device>);
+ devs.registerDevice("HolonomicPlatform",
+ testDeviceWithDefaultMock<platform::armar6_mecanum::Device>);
+ devs.registerDevice("PrismaticJoint",
+ testDeviceWithDefaultMock<torso::armar6_prismatic::Device>);
devs.registerDevice("PowerBoard", testDeviceWithDefaultMock<power_board::Device>);
}
else if (testDevices == TestDevices::Armar6)
{
- devs.registerDevice("Joint", testDeviceWithDefaultMock<sensor_actor_unit::armar6::Device>);
- devs.registerDevice("HeadBoardArmar7de", testDeviceWithDefaultMock<head_board::armar7de::Device>);
- devs.registerDevice("HolonomicPlatform", testDeviceWithDefaultMock<platform::armar6_mecanum::Device>);
- devs.registerDevice("PrismaticJoint", testDeviceWithDefaultMock<torso::armar6_prismatic::Device>);
+ devs.registerDevice("Joint",
+ testDeviceWithDefaultMock<sensor_actor_unit::armar6::Device>);
+ devs.registerDevice("HeadBoardArmar7de",
+ testDeviceWithDefaultMock<head_board::armar7de::Device>);
+ devs.registerDevice("HolonomicPlatform",
+ testDeviceWithDefaultMock<platform::armar6_mecanum::Device>);
+ devs.registerDevice("PrismaticJoint",
+ testDeviceWithDefaultMock<torso::armar6_prismatic::Device>);
devs.registerDevice("KITHandV2", testDeviceWithDefaultMock<hand::armar6_v2::Device>);
// Disabled, because not yet testable - produces segfault
diff --git a/source/devices/ethercat/common/elmo/gold/Mappings.h b/source/devices/ethercat/common/elmo/gold/Mappings.h
index 91f647ec..11648202 100644
--- a/source/devices/ethercat/common/elmo/gold/Mappings.h
+++ b/source/devices/ethercat/common/elmo/gold/Mappings.h
@@ -20,7 +20,6 @@ namespace devices::ethercat::common::elmo::gold
std::uint8_t upperIndexByte = 0;
};
-
/**
* This struct holds the complete buffer for one of the two mapping objects that can be configured manually.
* These are the objects 0x1607and 1608 for master->slave for slave->master 0x1A07 and 0x1A08 are the right objects.
@@ -44,7 +43,6 @@ namespace devices::ethercat::common::elmo::gold
mapping_entry mapping_entry8;
};
-
//RxPDO written to index 0x1C12
/** this is the buffer to map rx entries that is written to index 0x1C12
* ATTENTION if you change this buffer you have to change the struct below as well!
@@ -63,7 +61,6 @@ namespace devices::ethercat::common::elmo::gold
0x08,
0x16}};
-
/**
* This creates the buffer for the mapping object. The buffer can be written into the 0x1607 or 0x1608 Object.
* If you want to CHANGE THE MAPPING READ THIS:
@@ -156,7 +153,6 @@ namespace devices::ethercat::common::elmo::gold
return mapping;
}
-
//TX elmo -> master mapping written to 0x1C13
/** This is the buffer to map tx entries that is written to index 0x1C13
* ATTENTION if you change this buffer you have to change the struct below also!!!!
@@ -177,7 +173,6 @@ namespace devices::ethercat::common::elmo::gold
}};
-
/**
* This creates the buffer for the mapping object. The buffer can be written into the 0x1A07 or 0x1A08 Object.
* If you want to CHANGE THE MAPPING READ THIS:
diff --git a/source/devices/ethercat/common/h2t_devices.h b/source/devices/ethercat/common/h2t_devices.h
index 2a2fbd81..a957a024 100644
--- a/source/devices/ethercat/common/h2t_devices.h
+++ b/source/devices/ethercat/common/h2t_devices.h
@@ -3,13 +3,11 @@
#include <armarx/control/ethercat/DeviceInterface.h>
-
namespace devices::ethercat::common
{
static constexpr std::uint32_t H2TVendorId = 0x7bc;
-
/**
* Older H2T devices were flashed with the serial number being encoded as product code
* due to limitations at that time.
diff --git a/source/devices/ethercat/common/imagine_board/Config.h b/source/devices/ethercat/common/imagine_board/Config.h
index 190d3c80..62100ba5 100644
--- a/source/devices/ethercat/common/imagine_board/Config.h
+++ b/source/devices/ethercat/common/imagine_board/Config.h
@@ -5,24 +5,26 @@
namespace devices::ethercat::common::imagine_board
{
namespace hwconfig = armarx::control::hardware_config;
+
struct ActorConfig
{
- ActorConfig(hwconfig::DeviceConfigBase& actorConfig)
- : name(actorConfig.getName()),
- connector(actorConfig.getUint("connector")),
- enabled(actorConfig.getBool("enabled")),
- positionControlEnabled(actorConfig.getBool("PositionControlEnabled")),
- velocityControlEnabled(actorConfig.getBool("VelocityControlEnabled")),
- pwm(actorConfig.getLinearConfig("pwm")),
- maxPwm(actorConfig.getUint("maxPWM")),
- position(actorConfig.getLinearConfig("position")),
- relativePosition(actorConfig.getLinearConfig("relativePosition")),
- velocity(actorConfig.getLinearConfig("velocity")),
- torque(actorConfig.getLinearConfig("torque")),
- parallelGripperDecouplingFactor(actorConfig.getFloat("ParallelGripperDecouplingFactor")),
- parallelControlSiblingIndex(actorConfig.getInt("ParallelControlSiblingIndex")),
- currentPWMBoundGradient(actorConfig.getFloat("CurrentPWMBoundGradient")),
- currentPWMBoundOffset(actorConfig.getInt("CurrentPWMBoundOffset"))
+ ActorConfig(hwconfig::DeviceConfigBase& actorConfig) :
+ name(actorConfig.getName()),
+ connector(actorConfig.getUint("connector")),
+ enabled(actorConfig.getBool("enabled")),
+ positionControlEnabled(actorConfig.getBool("PositionControlEnabled")),
+ velocityControlEnabled(actorConfig.getBool("VelocityControlEnabled")),
+ pwm(actorConfig.getLinearConfig("pwm")),
+ maxPwm(actorConfig.getUint("maxPWM")),
+ position(actorConfig.getLinearConfig("position")),
+ relativePosition(actorConfig.getLinearConfig("relativePosition")),
+ velocity(actorConfig.getLinearConfig("velocity")),
+ torque(actorConfig.getLinearConfig("torque")),
+ parallelGripperDecouplingFactor(
+ actorConfig.getFloat("ParallelGripperDecouplingFactor")),
+ parallelControlSiblingIndex(actorConfig.getInt("ParallelControlSiblingIndex")),
+ currentPWMBoundGradient(actorConfig.getFloat("CurrentPWMBoundGradient")),
+ currentPWMBoundOffset(actorConfig.getInt("CurrentPWMBoundOffset"))
{
}
@@ -45,4 +47,4 @@ namespace devices::ethercat::common::imagine_board
float currentPWMBoundGradient;
std::int32_t currentPWMBoundOffset;
};
-}
+} // namespace devices::ethercat::common::imagine_board
diff --git a/source/devices/ethercat/common/imagine_board/Data.cpp b/source/devices/ethercat/common/imagine_board/Data.cpp
index 3b532734..52e0b9c1 100644
--- a/source/devices/ethercat/common/imagine_board/Data.cpp
+++ b/source/devices/ethercat/common/imagine_board/Data.cpp
@@ -34,7 +34,6 @@
#include <RobotAPI/components/units/RobotUnit/util/RtLogging.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
-
namespace devices::ethercat::common::imagine_board
{
Data::Data(std::list<ActorConfig> config,
@@ -55,11 +54,8 @@ namespace devices::ethercat::common::imagine_board
{
headActorData.at(motorConfig.connector).reset(new SensorActorData);
headActorData.at(motorConfig.connector)
- ->targetPWMPtr.init(targetPWM,
- motorConfig.pwm,
- std::nanf("1"),
- true,
- "targetPWMPtr");
+ ->targetPWMPtr.init(
+ targetPWM, motorConfig.pwm, std::nanf("1"), true, "targetPWMPtr");
headActorData.at(motorConfig.connector)->maxPWM = motorConfig.maxPwm;
headActorData.at(motorConfig.connector)
->position.init(&headActorData.at(motorConfig.connector)->rawABSEncoderTicks,
@@ -74,26 +70,26 @@ namespace devices::ethercat::common::imagine_board
true,
"relativePosition");
headActorData.at(motorConfig.connector)
- ->velocity.init(velocity,
- motorConfig.velocity,
- std::nanf("1"),
- true,
- "velocity");
+ ->velocity.init(
+ velocity, motorConfig.velocity, std::nanf("1"), true, "velocity");
headActorData.at(motorConfig.connector)
- ->torque.init(torque,
- motorConfig.torque,
- std::nanf("1"),
- true,
- "torque");
- headActorData.at(motorConfig.connector)->robotNode = robot->getRobotNode(motorConfig.name);
+ ->torque.init(torque, motorConfig.torque, std::nanf("1"), true, "torque");
+ headActorData.at(motorConfig.connector)->robotNode =
+ robot->getRobotNode(motorConfig.name);
headActorData.at(motorConfig.connector)->velocityTicks = velocity;
- headActorData.at(motorConfig.connector)->positionControlEnabled = motorConfig.positionControlEnabled;
- headActorData.at(motorConfig.connector)->velocityControlEnabled = motorConfig.velocityControlEnabled;
-
- headActorData.at(motorConfig.connector)->parallelGripperDecouplingFactor = motorConfig.parallelGripperDecouplingFactor;
- headActorData.at(motorConfig.connector)->parallelControlEnabled = motorConfig.parallelControlSiblingIndex;
- headActorData.at(motorConfig.connector)->currentPWMBoundGradient = motorConfig.currentPWMBoundGradient;
- headActorData.at(motorConfig.connector)->currentPWMBoundOffset = motorConfig.currentPWMBoundOffset;
+ headActorData.at(motorConfig.connector)->positionControlEnabled =
+ motorConfig.positionControlEnabled;
+ headActorData.at(motorConfig.connector)->velocityControlEnabled =
+ motorConfig.velocityControlEnabled;
+
+ headActorData.at(motorConfig.connector)->parallelGripperDecouplingFactor =
+ motorConfig.parallelGripperDecouplingFactor;
+ headActorData.at(motorConfig.connector)->parallelControlEnabled =
+ motorConfig.parallelControlSiblingIndex;
+ headActorData.at(motorConfig.connector)->currentPWMBoundGradient =
+ motorConfig.currentPWMBoundGradient;
+ headActorData.at(motorConfig.connector)->currentPWMBoundOffset =
+ motorConfig.currentPWMBoundOffset;
};
switch (motorConfig.connector)
{
@@ -116,7 +112,8 @@ namespace devices::ethercat::common::imagine_board
&sensorIN->motor3_target_pwm);
break;
default:
- throw armarx::LocalException("Motor index out of range: ") << motorConfig.connector;
+ throw armarx::LocalException("Motor index out of range: ")
+ << motorConfig.connector;
}
}
}
@@ -184,7 +181,8 @@ namespace devices::ethercat::common::imagine_board
2u);
ARMARX_RT_LOGF_ERROR("%s: Absolute Encoder Error and Warning bits: %b",
d.robotNode->getName().c_str(),
- error_and_warnings).deactivateSpam (1);
+ error_and_warnings)
+ .deactivateSpam(1);
}
}
@@ -226,7 +224,6 @@ namespace devices::ethercat::common::imagine_board
}
}
-
SensorActorDataPtr&
Data::getSensorActorData(size_t actorIndex)
{
@@ -383,4 +380,4 @@ namespace devices::ethercat::common::imagine_board
return sensorOUT->IMUTemperature;
}
-} // namespace robot_devices::ethercat::common::imagine_board
+} // namespace devices::ethercat::common::imagine_board
diff --git a/source/devices/ethercat/common/imagine_board/Data.h b/source/devices/ethercat/common/imagine_board/Data.h
index 6da84328..70e60649 100644
--- a/source/devices/ethercat/common/imagine_board/Data.h
+++ b/source/devices/ethercat/common/imagine_board/Data.h
@@ -34,14 +34,12 @@
#include "Config.h"
#include "Slave.h"
-
namespace VirtualRobot
{
using RobotPtr = std::shared_ptr<class Robot>;
using RobotNodePtr = std::shared_ptr<class RobotNode>;
} // namespace VirtualRobot
-
namespace devices::ethercat::common::imagine_board
{
@@ -76,7 +74,6 @@ namespace devices::ethercat::common::imagine_board
}
};
-
class SensorActorData
{
public:
@@ -142,8 +139,8 @@ namespace devices::ethercat::common::imagine_board
friend class Data;
};
- using SensorActorDataPtr = std::shared_ptr<SensorActorData>;
+ using SensorActorDataPtr = std::shared_ptr<SensorActorData>;
class Data : public armarx::control::ethercat::DataInterface
{
@@ -159,12 +156,14 @@ namespace devices::ethercat::common::imagine_board
/// @brief This function is never called!
void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration) override;
+
/// @brief This function is never called!
void
rtWriteTargetValues(const IceUtil::Time&, const IceUtil::Time&) override
{
std::terminate();
}
+
SensorActorDataPtr& getSensorActorData(size_t actorIndex);
std::int8_t getIMUTemperature() const;
diff --git a/source/devices/ethercat/common/imagine_board/SensorValue.h b/source/devices/ethercat/common/imagine_board/SensorValue.h
index 5efe2518..7c8fd303 100644
--- a/source/devices/ethercat/common/imagine_board/SensorValue.h
+++ b/source/devices/ethercat/common/imagine_board/SensorValue.h
@@ -28,7 +28,6 @@
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueIMU.h>
-
namespace devices::ethercat::common::imagine_board
{
diff --git a/source/devices/ethercat/common/imagine_board/Slave.cpp b/source/devices/ethercat/common/imagine_board/Slave.cpp
index c9ab6d1e..7ca3bba6 100644
--- a/source/devices/ethercat/common/imagine_board/Slave.cpp
+++ b/source/devices/ethercat/common/imagine_board/Slave.cpp
@@ -32,7 +32,6 @@
#include <devices/ethercat/common/h2t_devices.h>
#include <devices/ethercat/head/armar6/Device.h>
-
namespace devices::ethercat::common::imagine_board
{
@@ -42,53 +41,45 @@ namespace devices::ethercat::common::imagine_board
setTag(this->slaveIdentifier.getName());
}
-
void
Slave::doMappings()
{
}
-
bool
Slave::prepareForRun()
{
return true;
}
-
void
Slave::execute()
{
}
-
bool
Slave::shutdown()
{
return true;
}
-
void
Slave::prepareForOp()
{
}
-
bool
Slave::hasError()
{
return false;
}
-
std::string
Slave::getDefaultName()
{
return "ImagineBoard_SensorActorAddon";
}
-
bool
Slave::isSlaveIdentifierAccepted(const armarx::control::ethercat::SlaveIdentifier& sid)
{
@@ -96,22 +87,14 @@ namespace devices::ethercat::common::imagine_board
// The product code was used instead.
const std::vector<std::uint32_t> validProductCodes{
- 0x1000,
- 0x1001,
- 0x1002,
- 0x1003,
- 0x1004,
- 0x1005,
- 0x1006,
- 0x1007
- };
+ 0x1000, 0x1001, 0x1002, 0x1003, 0x1004, 0x1005, 0x1006, 0x1007};
const bool productCodeMatches =
std::find(validProductCodes.begin(), validProductCodes.end(), sid.productCode) !=
validProductCodes.end();
- return sid.vendorID == devices::ethercat::common::H2TVendorId
- and sid.serialNumber == 0 and productCodeMatches;
+ return sid.vendorID == devices::ethercat::common::H2TVendorId and sid.serialNumber == 0 and
+ productCodeMatches;
}
} // namespace devices::ethercat::common::imagine_board
diff --git a/source/devices/ethercat/common/imagine_board/Slave.h b/source/devices/ethercat/common/imagine_board/Slave.h
index ab4cc277..16a4f0e7 100644
--- a/source/devices/ethercat/common/imagine_board/Slave.h
+++ b/source/devices/ethercat/common/imagine_board/Slave.h
@@ -30,7 +30,6 @@
#include <devices/ethercat/common/imagine_board/SlaveIO.h>
-
namespace devices::ethercat::common::imagine_board
{
diff --git a/source/devices/ethercat/common/imagine_board/SlaveIO.h b/source/devices/ethercat/common/imagine_board/SlaveIO.h
index 09e4af99..38a0048f 100644
--- a/source/devices/ethercat/common/imagine_board/SlaveIO.h
+++ b/source/devices/ethercat/common/imagine_board/SlaveIO.h
@@ -4,7 +4,6 @@
// STD/STL
#include <cstdint>
-
namespace devices::ethercat::common::imagine_board
{
@@ -46,7 +45,6 @@ namespace devices::ethercat::common::imagine_board
} __attribute__((__packed__));
-
/**
* @brief PDO mapping master->sensorB
*/
@@ -69,4 +67,4 @@ namespace devices::ethercat::common::imagine_board
std::int32_t motor4_target_pwm;
} __attribute__((__packed__));
-}
+} // namespace devices::ethercat::common::imagine_board
diff --git a/source/devices/ethercat/common/sanity_checking/Config.h b/source/devices/ethercat/common/sanity_checking/Config.h
index 670ba1b0..92540b9e 100644
--- a/source/devices/ethercat/common/sanity_checking/Config.h
+++ b/source/devices/ethercat/common/sanity_checking/Config.h
@@ -18,27 +18,26 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-
- #pragma once
+
+#pragma once
#include <armarx/control/hardware_config/Config.h>
#include <armarx/control/hardware_config/DeviceConfig.h>
#include <armarx/control/hardware_config/Types.h>
+
#include <devices/ethercat/common/sanity_checking/AbsRelEncoderChecker.h>
-
- namespace devices::ethercat::common::sanity_checking
- {
+namespace devices::ethercat::common::sanity_checking
+{
namespace hwconfig = armarx::control::hardware_config;
-
- class Config
- {
- public:
- Config(hwconfig::Config& hwConfig);
+ class Config
+ {
+ public:
+ Config(hwconfig::Config& hwConfig);
+
+ AbsRelEncoderCheckerParams absRelEncoderCheckerParams;
+ };
- AbsRelEncoderCheckerParams absRelEncoderCheckerParams;
-};
-
- } // namespace devices::ethercat::common::sanity_checking
+} // namespace devices::ethercat::common::sanity_checking
diff --git a/source/devices/ethercat/common/sensor_board/armar6/ADS1263CRCHelper.cpp b/source/devices/ethercat/common/sensor_board/armar6/ADS1263CRCHelper.cpp
index ce514f36..218156eb 100644
--- a/source/devices/ethercat/common/sensor_board/armar6/ADS1263CRCHelper.cpp
+++ b/source/devices/ethercat/common/sensor_board/armar6/ADS1263CRCHelper.cpp
@@ -3,31 +3,32 @@
namespace devices::ethercat::common::sensor_board::armar6
{
- ADS1263CRCHelper::ADS1263CRCHelper()
- = default;
+ ADS1263CRCHelper::ADS1263CRCHelper() = default;
-
- std::uint8_t ADS1263CRCHelper::CalculateCRC8_ADS1263(std::uint8_t* bytes, std::size_t numBytes)
+ std::uint8_t
+ ADS1263CRCHelper::CalculateCRC8_ADS1263(std::uint8_t* bytes, std::size_t numBytes)
{
// Data sheet: http://www.ti.com/lit/ds/symlink/ads1263.pdf
- static const std::uint8_t CRC8_TABLE_FOR_ADS1263[] =
- {
- 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d,
- 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d,
- 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
- 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd,
- 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea,
- 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
- 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a,
- 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a,
- 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
- 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4,
- 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44,
- 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
- 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63,
- 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13,
- 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
- 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3,
+ static const std::uint8_t CRC8_TABLE_FOR_ADS1263[] = {
+ 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23,
+ 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41,
+ 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf,
+ 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85,
+ 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc,
+ 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe,
+ 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, 0x27, 0x20,
+ 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a,
+ 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74,
+ 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8,
+ 0xad, 0xaa, 0xa3, 0xa4, 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6,
+ 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c,
+ 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02,
+ 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, 0x4e, 0x49, 0x40, 0x47,
+ 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39,
+ 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13,
+ 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d,
+ 0x84, 0x83, 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef,
+ 0xfa, 0xfd, 0xf4, 0xf3,
};
std::uint8_t crc = 0;
for (int i = numBytes - 1; i >= 0; --i)
@@ -37,16 +38,16 @@ namespace devices::ethercat::common::sensor_board::armar6
return crc;
}
-
- std::uint8_t ADS1263CRCHelper::CalculateCRC8_Torque(std::int32_t value)
+ std::uint8_t
+ ADS1263CRCHelper::CalculateCRC8_Torque(std::int32_t value)
{
return CalculateCRC8_ADS1263((std::uint8_t*)&value, 4);
}
-
- std::uint8_t ADS1263CRCHelper::CalculateCRC8_Temperature(std::int32_t value)
+ std::uint8_t
+ ADS1263CRCHelper::CalculateCRC8_Temperature(std::int32_t value)
{
return CalculateCRC8_ADS1263((std::uint8_t*)&value, 3);
}
-} // namespace devices::ethercat::common::sensor_board::v1
+} // namespace devices::ethercat::common::sensor_board::armar6
diff --git a/source/devices/ethercat/common/sensor_board/armar6/ADS1263CRCHelper.h b/source/devices/ethercat/common/sensor_board/armar6/ADS1263CRCHelper.h
index 6a5647fc..d20c7116 100644
--- a/source/devices/ethercat/common/sensor_board/armar6/ADS1263CRCHelper.h
+++ b/source/devices/ethercat/common/sensor_board/armar6/ADS1263CRCHelper.h
@@ -4,7 +4,6 @@
// STD/STL
#include <cstdint>
-
namespace devices::ethercat::common::sensor_board::armar6
{
@@ -12,14 +11,12 @@ namespace devices::ethercat::common::sensor_board::armar6
{
public:
-
ADS1263CRCHelper();
static std::uint8_t CalculateCRC8_ADS1263(std::uint8_t* bytes, std::size_t numBytes);
static std::uint8_t CalculateCRC8_Torque(std::int32_t value);
static std::uint8_t CalculateCRC8_Temperature(std::int32_t value);
-
};
-} // namespace devices::ethercat::common::sensor_board::v1
+} // namespace devices::ethercat::common::sensor_board::armar6
diff --git a/source/devices/ethercat/common/sensor_board/armar6/SensorboardSDOHelper.cpp b/source/devices/ethercat/common/sensor_board/armar6/SensorboardSDOHelper.cpp
index 04a32fa9..03612fe4 100644
--- a/source/devices/ethercat/common/sensor_board/armar6/SensorboardSDOHelper.cpp
+++ b/source/devices/ethercat/common/sensor_board/armar6/SensorboardSDOHelper.cpp
@@ -8,25 +8,24 @@
#define IMU_CALIBRATION_DATA_SIZE 11
-
namespace devices::ethercat::common::sensor_board::armar6
{
- SensorboardSDOHelper::SensorboardSDOHelper(armarx::control::ethercat::Bus* bus, std::uint16_t slaveNumber, std::string humanName) :
- imu(bus, slaveNumber, humanName),
- humanName(humanName)
+ SensorboardSDOHelper::SensorboardSDOHelper(armarx::control::ethercat::Bus* bus,
+ std::uint16_t slaveNumber,
+ std::string humanName) :
+ imu(bus, slaveNumber, humanName), humanName(humanName)
{
-
}
-
- IMUSDOHelper& SensorboardSDOHelper::getImu()
+ IMUSDOHelper&
+ SensorboardSDOHelper::getImu()
{
return imu;
}
-
- std::string IMUSDOHelper::SensorBoardCommandToString(IMUSDOHelper::sensorboard_command_t command)
+ std::string
+ IMUSDOHelper::SensorBoardCommandToString(IMUSDOHelper::sensorboard_command_t command)
{
std::string stringCommand;
switch (command)
@@ -85,8 +84,9 @@ namespace devices::ethercat::common::sensor_board::armar6
return stringCommand;
}
-
- std::string IMUSDOHelper::SensorBoardCommandResultToString(IMUSDOHelper::sensorboard_command_result_t result)
+ std::string
+ IMUSDOHelper::SensorBoardCommandResultToString(
+ IMUSDOHelper::sensorboard_command_result_t result)
{
std::string stringResult;
switch (result)
@@ -130,17 +130,20 @@ namespace devices::ethercat::common::sensor_board::armar6
return stringResult;
}
-
- IMUSDOHelper::IMUSDOHelper(armarx::control::ethercat::Bus* bus, std::uint16_t slaveNumber, std::string humanName) :
- bus(bus),
- slaveNumber(slaveNumber),
- humanName(humanName)
+ IMUSDOHelper::IMUSDOHelper(armarx::control::ethercat::Bus* bus,
+ std::uint16_t slaveNumber,
+ std::string humanName) :
+ bus(bus), slaveNumber(slaveNumber), humanName(humanName)
{
setTag("IMUSDOHelper_" + humanName);
}
- IMUSDOHelper::sensorboard_command_result_t IMUSDOHelper::execCommand(sensorboard_command_t command, bool reportError, IceUtil::Time sleepTime, int retries) const
+ IMUSDOHelper::sensorboard_command_result_t
+ IMUSDOHelper::execCommand(sensorboard_command_t command,
+ bool reportError,
+ IceUtil::Time sleepTime,
+ int retries) const
{
ARMARX_DEBUG << "Executing IMU command: " << SensorBoardCommandToString(command);
bus->writeSDOEntry(slaveNumber, 0x8000, 1, static_cast<std::uint8_t>(command));
@@ -159,47 +162,56 @@ namespace devices::ethercat::common::sensor_board::armar6
i++;
if (i > retries)
{
- throw armarx::LocalException("Could not execute command with ") << retries << " retries: " << SensorBoardCommandToString(command);
+ throw armarx::LocalException("Could not execute command with ")
+ << retries << " retries: " << SensorBoardCommandToString(command);
}
}
if (value != static_cast<std::uint8_t>(command))
{
break;
}
- ARMARX_DEBUG << deactivateSpam(1) << "Waiting for command completion - current value: 0x" << std::hex << (int)value;
+ ARMARX_DEBUG << deactivateSpam(1)
+ << "Waiting for command completion - current value: 0x" << std::hex
+ << (int)value;
}
if (value != COMMAND_NONE)
{
- throw armarx::LocalException("Command value is not COMMAND_NONE after executing command - did someone write commands at the same time? current value: 0x") << std::hex << SensorBoardCommandResultToString(sensorboard_command_result_t(value));
+ throw armarx::LocalException(
+ "Command value is not COMMAND_NONE after executing command - did someone write "
+ "commands at the same time? current value: 0x")
+ << std::hex
+ << SensorBoardCommandResultToString(sensorboard_command_result_t(value));
}
bus->readSDOEntry(slaveNumber, 0x8000, 2, value);
if (value != COMMAND_SUCCESS && reportError)
{
- ARMARX_WARNING << std::hex << "command failed - command result: " << SensorBoardCommandResultToString(sensorboard_command_result_t(value));
+ ARMARX_WARNING << std::hex << "command failed - command result: "
+ << SensorBoardCommandResultToString(sensorboard_command_result_t(value));
}
return static_cast<sensorboard_command_result_t>(value);
}
-
- void IMUSDOHelper::reset() const
+ void
+ IMUSDOHelper::reset() const
{
auto cmd_result = execCommand(COMMAND_STOP_ALL_CYCLIC_REPORTING, false);
if (cmd_result != COMMAND_ERROR_NOT_REPORTING && cmd_result != COMMAND_SUCCESS)
{
- throw armarx::LocalException("resetting of IMU failed: result: 0x") << SensorBoardCommandResultToString(cmd_result);
+ throw armarx::LocalException("resetting of IMU failed: result: 0x")
+ << SensorBoardCommandResultToString(cmd_result);
}
}
-
- void IMUSDOHelper::init() const
+ void
+ IMUSDOHelper::init() const
{
execCommand(COMMAND_INIT_BNO055, true, IceUtil::Time::secondsDouble(3));
}
-
- void IMUSDOHelper::start() const
+ void
+ IMUSDOHelper::start() const
{
bus->writeSDOEntry(slaveNumber, 0x8000, 3, static_cast<std::uint8_t>(OPERATION_MODE_NDOF));
@@ -211,35 +223,34 @@ namespace devices::ethercat::common::sensor_board::armar6
execCommand(COMMAND_START_BNO055_CYCLIC_REPORTING);
}
-
- bool IMUSDOHelper::pingBNO055() const
+ bool
+ IMUSDOHelper::pingBNO055() const
{
auto value = execCommand(COMMAND_PING_BNO055, true);
return (value == COMMAND_SUCCESS);
-
}
-
- bool IMUSDOHelper::pingTMP007() const
+ bool
+ IMUSDOHelper::pingTMP007() const
{
auto value = execCommand(COMMAND_PING_TMP007, false);
return (value == COMMAND_SUCCESS);
}
-
- bool IMUSDOHelper::getUseTMP007() const
+ bool
+ IMUSDOHelper::getUseTMP007() const
{
return useTMP007;
}
-
- void IMUSDOHelper::setUseTMP007(bool value)
+ void
+ IMUSDOHelper::setUseTMP007(bool value)
{
useTMP007 = value;
}
-
- std::vector<std::uint16_t> IMUSDOHelper::calibrateSensor() const
+ std::vector<std::uint16_t>
+ IMUSDOHelper::calibrateSensor() const
{
execCommand(COMMAND_STOP_ALL_CYCLIC_REPORTING);
@@ -248,7 +259,8 @@ namespace devices::ethercat::common::sensor_board::armar6
bus->writeSDOEntry(slaveNumber, 0x8000, 3, static_cast<std::uint8_t>(OPERATION_MODE_NDOF));
execCommand(COMMAND_SWITCH_MODE, true);
- execCommand(COMMAND_START_CALIBRATION_STATUS_PRINTING, true, IceUtil::Time::secondsDouble(1.1));
+ execCommand(
+ COMMAND_START_CALIBRATION_STATUS_PRINTING, true, IceUtil::Time::secondsDouble(1.1));
while (true)
{
bus->readSDOEntry(slaveNumber, 0x8000, 5, value);
@@ -265,10 +277,11 @@ namespace devices::ethercat::common::sensor_board::armar6
return getCalibration();
}
-
- std::vector<std::uint16_t> IMUSDOHelper::getCalibration() const
+ std::vector<std::uint16_t>
+ IMUSDOHelper::getCalibration() const
{
- bus->writeSDOEntry(slaveNumber, 0x8000, 3, static_cast<std::uint8_t>(OPERATION_MODE_CONFIG));
+ bus->writeSDOEntry(
+ slaveNumber, 0x8000, 3, static_cast<std::uint8_t>(OPERATION_MODE_CONFIG));
execCommand(COMMAND_SWITCH_MODE, true);
execCommand(COMMAND_READ_CALIBRATION);
std::uint16_t value16;
@@ -281,12 +294,13 @@ namespace devices::ethercat::common::sensor_board::armar6
return result;
}
-
- void IMUSDOHelper::setCalibration(std::vector<std::uint16_t> calibrationData)
+ void
+ IMUSDOHelper::setCalibration(std::vector<std::uint16_t> calibrationData)
{
ARMARX_CHECK_EQUAL(calibrationData.size(), IMU_CALIBRATION_DATA_SIZE);
// ARMARX_INFO << "Setting IMU calibration";
- bus->writeSDOEntry(slaveNumber, 0x8000, 3, static_cast<std::uint8_t>(OPERATION_MODE_CONFIG));
+ bus->writeSDOEntry(
+ slaveNumber, 0x8000, 3, static_cast<std::uint8_t>(OPERATION_MODE_CONFIG));
execCommand(COMMAND_SWITCH_MODE, true);
for (std::uint8_t n = 1; n <= IMU_CALIBRATION_DATA_SIZE; n++)
{
@@ -317,7 +331,6 @@ namespace devices::ethercat::common::sensor_board::armar6
// usleep(100000);
// }
// execCommand(COMMAND_STOP_CALIBRATION_STATUS_PRINTING);
-
}
-} // namespace devices::ethercat::common::sensor_board::v1
+} // namespace devices::ethercat::common::sensor_board::armar6
diff --git a/source/devices/ethercat/common/sensor_board/armar6/SensorboardSDOHelper.h b/source/devices/ethercat/common/sensor_board/armar6/SensorboardSDOHelper.h
index 5229462e..6c41bad5 100644
--- a/source/devices/ethercat/common/sensor_board/armar6/SensorboardSDOHelper.h
+++ b/source/devices/ethercat/common/sensor_board/armar6/SensorboardSDOHelper.h
@@ -11,84 +11,86 @@
// armarx
#include <ArmarXCore/core/logging/Logging.h>
-
namespace armarx::control::ethercat
{
class Bus;
}
-
namespace devices::ethercat::common::sensor_board::armar6
{
- class IMUSDOHelper :
- public armarx::Logging
+ class IMUSDOHelper : public armarx::Logging
{
public:
-
- enum bno055_opmode_t :
- std::uint8_t
+ enum bno055_opmode_t : std::uint8_t
{
/* Operation mode settings*/
- OPERATION_MODE_CONFIG = 0X00,
- OPERATION_MODE_ACCONLY = 0X01,
- OPERATION_MODE_MAGONLY = 0X02,
- OPERATION_MODE_GYRONLY = 0X03,
- OPERATION_MODE_ACCMAG = 0X04,
- OPERATION_MODE_ACCGYRO = 0X05,
- OPERATION_MODE_MAGGYRO = 0X06,
- OPERATION_MODE_AMG = 0X07,
- OPERATION_MODE_IMUPLUS = 0X08,
- OPERATION_MODE_COMPASS = 0X09,
- OPERATION_MODE_M4G = 0X0A,
- OPERATION_MODE_NDOF_FMC_OFF = 0X0B,
- OPERATION_MODE_NDOF = 0X0C
+ OPERATION_MODE_CONFIG = 0X00,
+ OPERATION_MODE_ACCONLY = 0X01,
+ OPERATION_MODE_MAGONLY = 0X02,
+ OPERATION_MODE_GYRONLY = 0X03,
+ OPERATION_MODE_ACCMAG = 0X04,
+ OPERATION_MODE_ACCGYRO = 0X05,
+ OPERATION_MODE_MAGGYRO = 0X06,
+ OPERATION_MODE_AMG = 0X07,
+ OPERATION_MODE_IMUPLUS = 0X08,
+ OPERATION_MODE_COMPASS = 0X09,
+ OPERATION_MODE_M4G = 0X0A,
+ OPERATION_MODE_NDOF_FMC_OFF = 0X0B,
+ OPERATION_MODE_NDOF = 0X0C
};
- enum sensorboard_command_t :
- std::uint8_t
+ enum sensorboard_command_t : std::uint8_t
{
- COMMAND_NONE = 0x00,
- COMMAND_READ_STATUS = 0x01,
- COMMAND_SWITCH_MODE = 0x02,
- COMMAND_READ_CALIBRATION = 0x03,
- COMMAND_WRITE_CALIBRATION = 0x04,
- COMMAND_APPLY_REPORT_VECTOR_REGISTER = 0x05,
- COMMAND_START_BNO055_CYCLIC_REPORTING = 0x10,
- COMMAND_START_TMP007_CYCLIC_REPORTING = 0x11,
- COMMAND_STOP_ALL_CYCLIC_REPORTING = 0x12,
+ COMMAND_NONE = 0x00,
+ COMMAND_READ_STATUS = 0x01,
+ COMMAND_SWITCH_MODE = 0x02,
+ COMMAND_READ_CALIBRATION = 0x03,
+ COMMAND_WRITE_CALIBRATION = 0x04,
+ COMMAND_APPLY_REPORT_VECTOR_REGISTER = 0x05,
+ COMMAND_START_BNO055_CYCLIC_REPORTING = 0x10,
+ COMMAND_START_TMP007_CYCLIC_REPORTING = 0x11,
+ COMMAND_STOP_ALL_CYCLIC_REPORTING = 0x12,
COMMAND_START_CALIBRATION_STATUS_PRINTING = 0x20,
- COMMAND_STOP_CALIBRATION_STATUS_PRINTING = 0x21,
- COMMAND_ENABLE_I2C_ISR = 0x30,
- COMMAND_DISABLE_I2C_ISR = 0x31,
- COMMAND_PING_BNO055 = 0x40,
- COMMAND_PING_TMP007 = 0x41,
- COMMAND_INIT_BNO055 = 0x48
+ COMMAND_STOP_CALIBRATION_STATUS_PRINTING = 0x21,
+ COMMAND_ENABLE_I2C_ISR = 0x30,
+ COMMAND_DISABLE_I2C_ISR = 0x31,
+ COMMAND_PING_BNO055 = 0x40,
+ COMMAND_PING_TMP007 = 0x41,
+ COMMAND_INIT_BNO055 = 0x48
};
- enum sensorboard_command_result_t :
- std::uint8_t
+ enum sensorboard_command_result_t : std::uint8_t
{
- COMMAND_SUCCESS = 0x00,
- COMMAND_EXECUTING = 0x01,
- COMMAND_ERROR_ALREADY_REPORTING = 0x10,
- COMMAND_ERROR_NOT_REPORTING = 0x11,
- COMMAND_ERROR_INVALID_MODE = 0x12,
- COMMAND_ERROR_BUS_BUSY = 0x13,
- COMMAND_ERROR_ONLY_ALLOWED_IN_CONFIG_MODE = 0x14,
- COMMAND_ERROR_CALIBRATION_PRINTING_ACTIVE = 0x15,
- COMMAND_ERROR_NO_RESPONSE = 0x16,
- COMMAND_ERROR_INIT_FAILED = 0x17,
- COMMAND_ERROR_UNKNOWN_COMMAND = 0xFF
+ COMMAND_SUCCESS = 0x00,
+ COMMAND_EXECUTING = 0x01,
+ COMMAND_ERROR_ALREADY_REPORTING = 0x10,
+ COMMAND_ERROR_NOT_REPORTING = 0x11,
+ COMMAND_ERROR_INVALID_MODE = 0x12,
+ COMMAND_ERROR_BUS_BUSY = 0x13,
+ COMMAND_ERROR_ONLY_ALLOWED_IN_CONFIG_MODE = 0x14,
+ COMMAND_ERROR_CALIBRATION_PRINTING_ACTIVE = 0x15,
+ COMMAND_ERROR_NO_RESPONSE = 0x16,
+ COMMAND_ERROR_INIT_FAILED = 0x17,
+ COMMAND_ERROR_UNKNOWN_COMMAND = 0xFF
};
- IMUSDOHelper(armarx::control::ethercat::Bus* bus, std::uint16_t slaveNumber, std::string humanName);
+ IMUSDOHelper(armarx::control::ethercat::Bus* bus,
+ std::uint16_t slaveNumber,
+ std::string humanName);
static std::string SensorBoardCommandToString(sensorboard_command_t command);
static std::string SensorBoardCommandResultToString(sensorboard_command_result_t result);
- ~IMUSDOHelper() override {}
- sensorboard_command_result_t execCommand(sensorboard_command_t command, bool reportError = true, IceUtil::Time sleepTime = IceUtil::Time::seconds(0), int retries = 3) const;
+ ~IMUSDOHelper() override
+ {
+ }
+
+ sensorboard_command_result_t
+ execCommand(sensorboard_command_t command,
+ bool reportError = true,
+ IceUtil::Time sleepTime = IceUtil::Time::seconds(0),
+ int retries = 3) const;
void reset() const;
void init() const;
@@ -103,31 +105,29 @@ namespace devices::ethercat::common::sensor_board::armar6
void setUseTMP007(bool value);
private:
-
armarx::control::ethercat::Bus* bus;
std::uint16_t slaveNumber;
std::string humanName;
bool useTMP007 = true;
-
};
-
class SensorboardSDOHelper
{
public:
+ SensorboardSDOHelper(armarx::control::ethercat::Bus* bus,
+ std::uint16_t slaveNumber,
+ std::string humanName);
- SensorboardSDOHelper(armarx::control::ethercat::Bus* bus, std::uint16_t slaveNumber, std::string humanName);
-
- ~SensorboardSDOHelper() {}
+ ~SensorboardSDOHelper()
+ {
+ }
IMUSDOHelper& getImu();
private:
-
IMUSDOHelper imu;
std::string humanName;
-
};
-} // namespace devices::ethercat::common::sensor_board::v1
+} // namespace devices::ethercat::common::sensor_board::armar6
diff --git a/source/devices/ethercat/common/sensor_board/armar6/Slave.cpp b/source/devices/ethercat/common/sensor_board/armar6/Slave.cpp
index 6ae90a0c..3a187d80 100644
--- a/source/devices/ethercat/common/sensor_board/armar6/Slave.cpp
+++ b/source/devices/ethercat/common/sensor_board/armar6/Slave.cpp
@@ -11,7 +11,6 @@
#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof(*(arr)))
-
namespace devices::ethercat::common::sensor_board::armar6
{
@@ -37,7 +36,6 @@ namespace devices::ethercat::common::sensor_board::armar6
0xAA, 0x3D, 0x13, 0x84, 0x4F, 0xD8, 0xF6, 0x61, 0xF7, 0x60, 0x4E, 0xD9, 0x12, 0x85, 0xAB,
0x3C};
-
// use this function to calculate CRC from 32-bit number
static std::uint8_t
crc8_4B(std::uint32_t bb)
@@ -62,7 +60,6 @@ namespace devices::ethercat::common::sensor_board::armar6
return crc;
}
-
Slave::Slave(const armarx::control::ethercat::SlaveIdentifier slaveIdentifier) :
SlaveInterfaceWithIO(slaveIdentifier),
sdoHelper(&armarx::control::ethercat::Bus::getBus(),
@@ -72,7 +69,6 @@ namespace devices::ethercat::common::sensor_board::armar6
setTag(this->slaveIdentifier.getName());
}
-
void
Slave::execute()
{
@@ -155,7 +151,6 @@ namespace devices::ethercat::common::sensor_board::armar6
}
}
-
bool
Slave::prepareForRun()
{
@@ -163,7 +158,6 @@ namespace devices::ethercat::common::sensor_board::armar6
return true;
}
-
bool
Slave::shutdown()
{
@@ -211,13 +205,11 @@ namespace devices::ethercat::common::sensor_board::armar6
return true;
}
-
void
Slave::doMappings()
{
}
-
std::uint32_t
Slave::getRawAbsValue()
{
@@ -230,42 +222,36 @@ namespace devices::ethercat::common::sensor_board::armar6
return absEncoderWithStatus >> 12;
}
-
const std::int16_t*
Slave::getGearTemperatur()
{
return &outputs->GearboxTemperature;
}
-
const std::int16_t*
Slave::getMotorTemperatur()
{
return &outputs->MotorTemperature;
}
-
const std::int32_t*
Slave::getTorqueValue()
{
return &outputs->TorqueValue;
}
-
bool
Slave::hasError()
{
return false;
}
-
bool
Slave::getCheckAbsEncoderReading() const
{
return checkAbsEncoderReading;
}
-
void
Slave::setCheckAbsEncoderReading(bool value)
{
@@ -273,21 +259,18 @@ namespace devices::ethercat::common::sensor_board::armar6
checkAbsEncoderReading = value;
}
-
bool
Slave::getUseTMP007() const
{
return useTMP007;
}
-
void
Slave::setUseTMP007(bool value)
{
useTMP007 = value;
}
-
void
Slave::prepareForSafeOp()
{
@@ -332,7 +315,6 @@ namespace devices::ethercat::common::sensor_board::armar6
}
}
-
void
Slave::finishPreparingForSafeOp()
{
@@ -342,21 +324,18 @@ namespace devices::ethercat::common::sensor_board::armar6
}
}
-
bool
Slave::getUseIMU() const
{
return useIMU;
}
-
void
Slave::setUseIMU(bool value)
{
useIMU = value;
}
-
void
Slave::prepareForOp()
{
@@ -369,34 +348,29 @@ namespace devices::ethercat::common::sensor_board::armar6
// ARMARX_IMPORTANT << "IMU Calibration: " << stream.str();
}
-
void
Slave::finishPreparingForOp()
{
}
-
std::vector<std::uint16_t>
Slave::getIMUCalibrationData() const
{
return IMUCalibrationData;
}
-
void
Slave::setIMUCalibrationData(const std::vector<std::uint16_t>& value)
{
IMUCalibrationData = value;
}
-
std::string
Slave::getDefaultName()
{
return "Armar6SensorBoard";
}
-
bool
Slave::isSlaveIdentifierAccepted(const armarx::control::ethercat::SlaveIdentifier& sid)
{
diff --git a/source/devices/ethercat/common/sensor_board/armar6/Slave.h b/source/devices/ethercat/common/sensor_board/armar6/Slave.h
index f584e8e9..bd9757a9 100644
--- a/source/devices/ethercat/common/sensor_board/armar6/Slave.h
+++ b/source/devices/ethercat/common/sensor_board/armar6/Slave.h
@@ -11,7 +11,6 @@
#include <devices/ethercat/common/sensor_board/armar6/SensorboardSDOHelper.h>
#include <devices/ethercat/common/sensor_board/armar6/SlaveIO.h>
-
namespace devices::ethercat::common::sensor_board::armar6
{
@@ -33,7 +32,6 @@ namespace devices::ethercat::common::sensor_board::armar6
AbsoluteEncoderErrorBit_Count
};
-
const char* const AbsoluteEncoderErrorBitNames[AbsoluteEncoderErrorBit_Count] = {
"b0_Reserved",
"b1_Reserved",
@@ -48,7 +46,6 @@ namespace devices::ethercat::common::sensor_board::armar6
"b10_Warning",
"b11_Error"};
-
struct SensorBoardStatistics
{
std::uint64_t bitCount[AbsoluteEncoderErrorBit_Count] = {};
@@ -56,7 +53,6 @@ namespace devices::ethercat::common::sensor_board::armar6
std::uint64_t totalCount = 0;
};
-
class Slave : public armarx::control::ethercat::SlaveInterfaceWithIO<SlaveIn, SlaveOut>
{
diff --git a/source/devices/ethercat/common/sensor_board/armar6/SlaveIO.h b/source/devices/ethercat/common/sensor_board/armar6/SlaveIO.h
index fa7c69a6..83b44d2c 100644
--- a/source/devices/ethercat/common/sensor_board/armar6/SlaveIO.h
+++ b/source/devices/ethercat/common/sensor_board/armar6/SlaveIO.h
@@ -3,7 +3,6 @@
#include <cstdint>
-
namespace devices::ethercat::common::sensor_board::armar6
{
@@ -46,14 +45,13 @@ namespace devices::ethercat::common::sensor_board::armar6
std::int16_t IMUQuaternionZ;
// Die Temperature (Infrared Sensor for gearbox)
- std::int16_t DieTemperature; // T_DIE
+ std::int16_t DieTemperature; // T_DIE
std::int16_t GearboxTemperature; // T_OBJ
std::uint16_t SensorboardUpdateRate;
std::uint16_t I2CUpdateRate;
} __attribute__((__packed__));
-
/**
* @brief PDO mapping master->sensorB
*/
diff --git a/source/devices/ethercat/common/sensor_board/armar7de/Config.cpp b/source/devices/ethercat/common/sensor_board/armar7de/Config.cpp
index a1aded9a..752e782b 100644
--- a/source/devices/ethercat/common/sensor_board/armar7de/Config.cpp
+++ b/source/devices/ethercat/common/sensor_board/armar7de/Config.cpp
@@ -1,6 +1,5 @@
#include "Config.h"
-
namespace devices::ethercat::common::sensor_board::armar7de
{
diff --git a/source/devices/ethercat/common/sensor_board/armar7de/Config.h b/source/devices/ethercat/common/sensor_board/armar7de/Config.h
index e6bb0137..ec9ec2e7 100644
--- a/source/devices/ethercat/common/sensor_board/armar7de/Config.h
+++ b/source/devices/ethercat/common/sensor_board/armar7de/Config.h
@@ -1,6 +1,7 @@
#pragma once
#include <cstdint>
+
#include <armarx/control/hardware_config/SlaveConfig.h>
namespace devices::ethercat::common::sensor_board::armar7de
@@ -24,4 +25,4 @@ namespace devices::ethercat::common::sensor_board::armar7de
float torqueFactor;
float torqueOffset;
};
-}
+} // namespace devices::ethercat::common::sensor_board::armar7de
diff --git a/source/devices/ethercat/common/sensor_board/armar7de/Data.cpp b/source/devices/ethercat/common/sensor_board/armar7de/Data.cpp
index 91f6c70f..de5fbc75 100644
--- a/source/devices/ethercat/common/sensor_board/armar7de/Data.cpp
+++ b/source/devices/ethercat/common/sensor_board/armar7de/Data.cpp
@@ -4,17 +4,14 @@
namespace devices::ethercat::common::sensor_board::armar7de
{
- Data::Data(SensorBoardConfig config,
- SlaveOut* outputs,
- SlaveIn* inputs) :
+ Data::Data(SensorBoardConfig config, SlaveOut* outputs, SlaveIn* inputs) :
outputs(outputs), inputs(inputs), errorDecoder(outputs, inputs)
{
ARMARX_CHECK_EXPRESSION(outputs);
ARMARX_CHECK_EXPRESSION(inputs);
voltage.init(&(outputs->VoltageMonitor), config.voltage);
- absoluteEncoder.init(&(outputs->AbsoluteEncoderValue),
- config.absoluteEncoder);
+ absoluteEncoder.init(&(outputs->AbsoluteEncoderValue), config.absoluteEncoder);
torque.init(&(outputs->TorqueADCValue),
config.torqueFactor,
config.torqueOffset,
@@ -197,7 +194,6 @@ namespace devices::ethercat::common::sensor_board::armar7de
return false;
}
-
void
Data::setRedLedIntensity(std::uint8_t redIntensity)
{
diff --git a/source/devices/ethercat/common/sensor_board/armar7de/Data.h b/source/devices/ethercat/common/sensor_board/armar7de/Data.h
index a591ac8a..e39cad85 100644
--- a/source/devices/ethercat/common/sensor_board/armar7de/Data.h
+++ b/source/devices/ethercat/common/sensor_board/armar7de/Data.h
@@ -55,8 +55,9 @@ namespace devices::ethercat::common::sensor_board::armar7de
read()
{
const double torqueAdcValueNormalized =
- static_cast<double>((*raw) / (1 << ((torqueAdcPgaAndSps & 0b01110000) >> 4)));
- const double torqueInuV = torqueAdcValueNormalized * 1'000'000 * v_ref / (1 << 23) - offset;
+ static_cast<double>((*raw) / (1 << ((torqueAdcPgaAndSps & 0b01110000) >> 4)));
+ const double torqueInuV =
+ torqueAdcValueNormalized * 1'000'000 * v_ref / (1 << 23) - offset;
value = torqueInuV * factor;
}
diff --git a/source/devices/ethercat/common/sensor_board/armar7de/SlaveIO.h b/source/devices/ethercat/common/sensor_board/armar7de/SlaveIO.h
index d3cc46c1..f1fd00bb 100644
--- a/source/devices/ethercat/common/sensor_board/armar7de/SlaveIO.h
+++ b/source/devices/ethercat/common/sensor_board/armar7de/SlaveIO.h
@@ -3,7 +3,6 @@
#include <cstdint>
-
namespace devices::ethercat::common::sensor_board::armar7de
{
@@ -49,7 +48,6 @@ namespace devices::ethercat::common::sensor_board::armar7de
std::uint8_t pad2;
} __attribute__((__packed__));
-
/**
* @brief PDO mapping master->sensorB
*/
@@ -68,4 +66,4 @@ namespace devices::ethercat::common::sensor_board::armar7de
std::uint8_t flags;
} __attribute__((__packed__));
-} // namespace robot_devices::ethercat::common::sensor_board::armar7de
+} // namespace devices::ethercat::common::sensor_board::armar7de
diff --git a/source/devices/ethercat/ethercat_hub/Slave.cpp b/source/devices/ethercat/ethercat_hub/Slave.cpp
index d9bd9bb2..ef8365ac 100644
--- a/source/devices/ethercat/ethercat_hub/Slave.cpp
+++ b/source/devices/ethercat/ethercat_hub/Slave.cpp
@@ -7,8 +7,8 @@
// armarx
#include <armarx/control/ethercat/Bus.h>
-#include <devices/ethercat/common/h2t_devices.h>
+#include <devices/ethercat/common/h2t_devices.h>
namespace devices::ethercat::ethercat_hub
{
@@ -17,10 +17,9 @@ namespace devices::ethercat::ethercat_hub
Slave::Slave(const armarx::control::ethercat::SlaveIdentifier slaveIdentifier) :
SlaveInterface(slaveIdentifier)
{
- setTag(this->slaveIdentifier.getName ());
+ setTag(this->slaveIdentifier.getName());
}
-
std::string
Slave::getDefaultName()
{
diff --git a/source/devices/ethercat/ethercat_hub/Slave.h b/source/devices/ethercat/ethercat_hub/Slave.h
index eb6117bf..2bb3f24a 100644
--- a/source/devices/ethercat/ethercat_hub/Slave.h
+++ b/source/devices/ethercat/ethercat_hub/Slave.h
@@ -4,63 +4,66 @@
// armarx
#include <armarx/control/ethercat/SlaveInterface.h>
-
namespace devices::ethercat::ethercat_hub
{
- class Slave :
- public armarx::control::ethercat::SlaveInterface
+ class Slave : public armarx::control::ethercat::SlaveInterface
{
public:
-
Slave(const armarx::control::ethercat::SlaveIdentifier slaveIdentifier);
// AbstractSlave interface
public:
-
- void doMappings() override
+ void
+ doMappings() override
{
}
- bool prepareForRun() override
+ bool
+ prepareForRun() override
{
return true;
}
- void execute() override
+ void
+ execute() override
{
}
- bool shutdown() override
+ bool
+ shutdown() override
{
return true;
}
- void setInputPDO(void* ptr) override
+ void
+ setInputPDO(void* ptr) override
{
}
- void setOutputPDO(void* ptr) override
+ void
+ setOutputPDO(void* ptr) override
{
}
- void prepareForOp() override
+ void
+ prepareForOp() override
{
}
- bool hasError() override
+ bool
+ hasError() override
{
return false;
}
// AbstractSlave interface
public:
-
static std::string getDefaultName();
- static bool isSlaveIdentifierAccepted(const armarx::control::ethercat::SlaveIdentifier& sid);
-
+ static bool
+ isSlaveIdentifierAccepted(const armarx::control::ethercat::SlaveIdentifier& sid);
};
-} // namespace armarx
+} // namespace devices::ethercat::ethercat_hub
diff --git a/source/devices/ethercat/ft_sensor_board/armar6/Config.h b/source/devices/ethercat/ft_sensor_board/armar6/Config.h
index 70099307..fc18b299 100644
--- a/source/devices/ethercat/ft_sensor_board/armar6/Config.h
+++ b/source/devices/ethercat/ft_sensor_board/armar6/Config.h
@@ -5,16 +5,16 @@
namespace devices::ethercat::ft_sensor_board::armar6
{
namespace hwconfig = armarx::control::hardware_config;
+
struct Config
{
- Config(hwconfig::DeviceConfig& hwConfig)
- : adcTemperature(hwConfig.getLinearConfig("adcTemperature")),
- voltageFactor(hwConfig.getFloat("voltageFactor")),
- ftCalibrationMatrix(hwConfig.getMatrix<float, 6, 6>("FTCalibrationMatrix")),
- channelOrder(hwConfig.getMatrix<std::int32_t, 6, 1>("ChannelOrder")),
- offsetList(hwConfig.getIntList("Offset"))
+ Config(hwconfig::DeviceConfig& hwConfig) :
+ adcTemperature(hwConfig.getLinearConfig("adcTemperature")),
+ voltageFactor(hwConfig.getFloat("voltageFactor")),
+ ftCalibrationMatrix(hwConfig.getMatrix<float, 6, 6>("FTCalibrationMatrix")),
+ channelOrder(hwConfig.getMatrix<std::int32_t, 6, 1>("ChannelOrder")),
+ offsetList(hwConfig.getIntList("Offset"))
{
-
}
hwconfig::types::LinearConfig adcTemperature;
@@ -23,4 +23,4 @@ namespace devices::ethercat::ft_sensor_board::armar6
Eigen::Matrix<std::int32_t, 6, 1> channelOrder;
std::list<std::int32_t> offsetList;
};
-}
+} // namespace devices::ethercat::ft_sensor_board::armar6
diff --git a/source/devices/ethercat/ft_sensor_board/armar6/Data.cpp b/source/devices/ethercat/ft_sensor_board/armar6/Data.cpp
index 33d7fa8f..984f4a0d 100644
--- a/source/devices/ethercat/ft_sensor_board/armar6/Data.cpp
+++ b/source/devices/ethercat/ft_sensor_board/armar6/Data.cpp
@@ -26,22 +26,27 @@
// armarx
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelperTemplates.h>
-
namespace devices::ethercat::ft_sensor_board::armar6
{
- Data::Data(Config& config,
- SlaveOut* sensorOUT, SlaveIn* sensorIN) :
- sensorOUT(sensorOUT),
- sensorIN(sensorIN)
+ Data::Data(Config& config, SlaveOut* sensorOUT, SlaveIn* sensorIN) :
+ sensorOUT(sensorOUT), sensorIN(sensorIN)
{
ARMARX_CHECK_EXPRESSION(sensorOUT);
- adc_1_to_3_temperature.init(&sensorOUT->adc_1_to_3_temperature, config.adcTemperature, 0, false, "adc_1_to_3_temperature");
- adc_4_to_6_temperature.init(&sensorOUT->adc_4_to_6_temperature, config.adcTemperature, 0, false, "adc_4_to_6_temperature");
+ adc_1_to_3_temperature.init(&sensorOUT->adc_1_to_3_temperature,
+ config.adcTemperature,
+ 0,
+ false,
+ "adc_1_to_3_temperature");
+ adc_4_to_6_temperature.init(&sensorOUT->adc_4_to_6_temperature,
+ config.adcTemperature,
+ 0,
+ false,
+ "adc_4_to_6_temperature");
ticksToVoltFactor = config.voltageFactor;
@@ -56,14 +61,16 @@ namespace devices::ethercat::ft_sensor_board::armar6
std::vector<int> offsetList{config.offsetList.begin(), config.offsetList.end()};
ARMARX_DEBUG << VAROUT(offsetList);
ARMARX_CHECK_EQUAL(offsetList.size(), 6);
- offset << offsetList[0], offsetList[1], offsetList[2], offsetList[3], offsetList[4], offsetList[5];
+ offset << offsetList[0], offsetList[1], offsetList[2], offsetList[3], offsetList[4],
+ offsetList[5];
ARMARX_DEBUG << VAROUT(offset);
filters.resize(6, armarx::control::rt_filters::RtMedianFilter(10));
}
-
- void Data::rtReadSensorValues(const IceUtil::Time& /*sensorValuesTimestamp*/, const IceUtil::Time& /*timeSinceLastIteration*/)
+ void
+ Data::rtReadSensorValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
{
adc_1_to_3_temperature.read();
adc_4_to_6_temperature.read();
@@ -106,13 +113,16 @@ namespace devices::ethercat::ft_sensor_board::armar6
//ARMARX_INFO << VAROUT(rawForceTorque);
// ARMARX_IMPORTANT << VAROUT(rawForceTorque);
- forceTorqueValues = conversionMatrix * (rawForceTorque.cast<float>() * ticksToVoltFactor);
+ forceTorqueValues =
+ conversionMatrix * (rawForceTorque.cast<float>() * ticksToVoltFactor);
#ifdef FT_CALIB
forces = forceTorqueValues.block<3, 1>(0, 0);
torques = forceTorqueValues.block<3, 1>(3, 0);
#else
- forces << filters.at(0).update(forceTorqueValues(0)), filters.at(1).update(forceTorqueValues(1)), filters.at(2).update(forceTorqueValues(2));
- torques << filters.at(3).update(forceTorqueValues(3)), filters.at(4).update(forceTorqueValues(4)), filters.at(5).update(forceTorqueValues(5));
+ forces << filters.at(0).update(forceTorqueValues(0)),
+ filters.at(1).update(forceTorqueValues(1)), filters.at(2).update(forceTorqueValues(2));
+ torques << filters.at(3).update(forceTorqueValues(3)),
+ filters.at(4).update(forceTorqueValues(4)), filters.at(5).update(forceTorqueValues(5));
#endif
#ifdef FT_CALIB
@@ -125,48 +135,48 @@ namespace devices::ethercat::ft_sensor_board::armar6
bestForceVec = forces;
}
permutationsTried++;
- }
- while (boost::range::next_permutation(permutation));
- ARMARX_INFO << deactivateSpam(1) << VAROUT(min_score) << VAROUT(bestPermutation) << VAROUT(permutationsTried);
+ } while (boost::range::next_permutation(permutation));
+ ARMARX_INFO << deactivateSpam(1) << VAROUT(min_score) << VAROUT(bestPermutation)
+ << VAROUT(permutationsTried);
forces = bestForceVec;
#endif
// ARMARX_IMPORTANT << "filters applied";
}
-
- const Eigen::Vector3f& Data::getForces() const
+ const Eigen::Vector3f&
+ Data::getForces() const
{
return forces;
}
-
- const Eigen::Vector3f& Data::getTorques() const
+ const Eigen::Vector3f&
+ Data::getTorques() const
{
return torques;
}
-
- float Data::getADC1to3Temperature() const
+ float
+ Data::getADC1to3Temperature() const
{
return adc_1_to_3_temperature.value;
}
-
- float Data::getADC4to6Temperature() const
+ float
+ Data::getADC4to6Temperature() const
{
return adc_4_to_6_temperature.value;
}
-
- float Data::getUpdateRate() const
+ float
+ Data::getUpdateRate() const
{
return sensorOUT->update_rate / 3;
}
-
- const Eigen::Vector6i& Data::getRawValues() const
+ const Eigen::Vector6i&
+ Data::getRawValues() const
{
return rawForceTorque;
}
-}
+} // namespace devices::ethercat::ft_sensor_board::armar6
diff --git a/source/devices/ethercat/ft_sensor_board/armar6/Data.h b/source/devices/ethercat/ft_sensor_board/armar6/Data.h
index e9d1886a..7206ab28 100644
--- a/source/devices/ethercat/ft_sensor_board/armar6/Data.h
+++ b/source/devices/ethercat/ft_sensor_board/armar6/Data.h
@@ -32,69 +32,71 @@
#include <armarx/control/rt_filters/RtMedianFilter.h>
// robot_devices
-#include <devices/ethercat/ft_sensor_board/armar6/Slave.h>
#include <devices/ethercat/ft_sensor_board/armar6/Config.h>
-
+#include <devices/ethercat/ft_sensor_board/armar6/Slave.h>
namespace Eigen
{
using Matrix6f = Matrix<float, 6, 6>;
using Vector6f = Matrix<float, 6, 1>;
using Vector6i = Matrix<int, 6, 1>;
-}
-
+} // namespace Eigen
namespace devices::ethercat::ft_sensor_board::armar6
{
- class FTSensorDataValue :
- public armarx::SensorValueForceTorque
+ class FTSensorDataValue : public armarx::SensorValueForceTorque
{
public:
-
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- float adc_1_to_3_temperature;
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION float adc_1_to_3_temperature;
float adc_4_to_6_temperature;
Eigen::Vector6f rawForceTorque;
int updateRate;
- static SensorValueInfo<FTSensorDataValue> GetClassMemberInfo()
+
+ static SensorValueInfo<FTSensorDataValue>
+ GetClassMemberInfo()
{
SensorValueInfo<FTSensorDataValue> svi;
svi.addBaseClass<SensorValueForceTorque>();
- svi.addMemberVariable(&FTSensorDataValue::adc_1_to_3_temperature, "adc_1_to_3_temperature");
- svi.addMemberVariable(&FTSensorDataValue::adc_4_to_6_temperature, "adc_4_to_6_temperature");
+ svi.addMemberVariable(&FTSensorDataValue::adc_1_to_3_temperature,
+ "adc_1_to_3_temperature");
+ svi.addMemberVariable(&FTSensorDataValue::adc_4_to_6_temperature,
+ "adc_4_to_6_temperature");
svi.addMemberVariable(&FTSensorDataValue::rawForceTorque, "rawForceTorque")
- .setVariantReportFunction(
- [](const IceUtil::Time & timestamp, const FTSensorDataValue * ptr)
- {
- return std::map<std::string, VariantBasePtr>
- {
- {"adc_raw_1_to_3", {new armarx::TimedVariant {armarx::Vector3((Eigen::Vector3f) ptr->rawForceTorque.block<3, 1>(0, 0)), timestamp}}},
- {"adc_raw_4_to_6", {new armarx::TimedVariant {armarx::Vector3((Eigen::Vector3f) ptr->rawForceTorque.block<3, 1>(3, 0)), timestamp}}}
- };
- }
- );
+ .setVariantReportFunction(
+ [](const IceUtil::Time& timestamp, const FTSensorDataValue* ptr)
+ {
+ return std::map<std::string, VariantBasePtr>{
+ {"adc_raw_1_to_3",
+ {new armarx::TimedVariant{
+ armarx::Vector3(
+ (Eigen::Vector3f)ptr->rawForceTorque.block<3, 1>(0, 0)),
+ timestamp}}},
+ {"adc_raw_4_to_6",
+ {new armarx::TimedVariant{
+ armarx::Vector3(
+ (Eigen::Vector3f)ptr->rawForceTorque.block<3, 1>(3, 0)),
+ timestamp}}}};
+ });
return svi;
}
-
};
-
-
- class Data :
- public armarx::control::ethercat::DataInterface
+ class Data : public armarx::control::ethercat::DataInterface
{
public:
+ Data(Config& config, SlaveOut* sensorOUT, SlaveIn* sensorIN);
- Data(Config& config,
- SlaveOut* sensorOUT, SlaveIn* sensorIN);
+ void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
- void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
- void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
+ void
+ rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override
{
- (void) sensorValuesTimestamp, (void) timeSinceLastIteration;
+ (void)sensorValuesTimestamp, (void)timeSinceLastIteration;
}
const Eigen::Vector3f& getForces() const;
@@ -105,7 +107,6 @@ namespace devices::ethercat::ft_sensor_board::armar6
const Eigen::Vector6i& getRawValues() const;
private:
-
SlaveOut* sensorOUT;
SlaveIn* sensorIN;
@@ -123,7 +124,6 @@ namespace devices::ethercat::ft_sensor_board::armar6
std::vector<armarx::control::rt_filters::RtMedianFilter> filters;
bool firstRun = true;
Eigen::Vector6f gravityOffset;
-
};
-}
+} // namespace devices::ethercat::ft_sensor_board::armar6
diff --git a/source/devices/ethercat/ft_sensor_board/armar6/Device.cpp b/source/devices/ethercat/ft_sensor_board/armar6/Device.cpp
index cb509503..a3e29763 100644
--- a/source/devices/ethercat/ft_sensor_board/armar6/Device.cpp
+++ b/source/devices/ethercat/ft_sensor_board/armar6/Device.cpp
@@ -34,7 +34,6 @@
#include <devices/ethercat/common/h2t_devices.h>
-
namespace devices::ethercat::ft_sensor_board::armar6
{
@@ -51,8 +50,7 @@ namespace devices::ethercat::ft_sensor_board::armar6
this->sensorNode = robot->getSensor(getDeviceName());
ARMARX_CHECK_EXPRESSION(this->sensorNode) << getDeviceName();
this->robot = sensorNode->getRobotNode()->getRobot();
- auto dynamicRoot =
- robot->getRobotNode(hwConfig.getString("FirstRobotNodeNameAfterSensor"));
+ auto dynamicRoot = robot->getRobotNode(hwConfig.getString("FirstRobotNodeNameAfterSensor"));
auto parent = sensorNode->getParent();
massSum = 0;
Eigen::Vector3f averageCoM;
@@ -109,10 +107,8 @@ namespace devices::ethercat::ft_sensor_board::armar6
gravityOffsetVecForce = Eigen::Vector3f::Zero();
}
-
Device::~Device() = default;
-
armarx::control::ethercat::DeviceInterface::TryAssignResult
Device::tryAssign(armarx::control::ethercat::SlaveInterface& slave)
{
@@ -128,7 +124,6 @@ namespace devices::ethercat::ft_sensor_board::armar6
return result;
}
-
armarx::control::ethercat::DeviceInterface::AllAssignedResult
Device::onAllAssigned()
{
@@ -148,7 +143,6 @@ namespace devices::ethercat::ft_sensor_board::armar6
return "FTSensorBoard_Armar6";
}
-
void
Device::postSwitchToSafeOp()
{
@@ -158,13 +152,12 @@ namespace devices::ethercat::ft_sensor_board::armar6
<< " of slave with id " << ftSlave->getSlaveNumber()
<< "\nOutputPtrs = " << ftSlave->getOutputsPtr()
<< "\nOutputPtrs = " << ftSlave->getInputsPtr();
- dataPtr = std::make_unique<Data>(
- config, ftSlave->getOutputsPtr(), ftSlave->getInputsPtr());
+ dataPtr =
+ std::make_unique<Data>(config, ftSlave->getOutputsPtr(), ftSlave->getInputsPtr());
}
ARMARX_CHECK_NOT_NULL(dataPtr);
}
-
Eigen::Vector3f
changeDirectionFrame(const Eigen::Vector3f direction,
const VirtualRobot::SceneObjectPtr& currentFrame,
@@ -178,7 +171,6 @@ namespace devices::ethercat::ft_sensor_board::armar6
return toNewFrame.block(0, 0, 3, 3).inverse() * direction;
}
-
Eigen::Vector3f
Device::changeDirectionFrameToGlobal(const Eigen::Vector3f direction,
const VirtualRobot::SceneObjectPtr& currentFrame)
@@ -193,7 +185,6 @@ namespace devices::ethercat::ft_sensor_board::armar6
return toNewFrame.block(0, 0, 3, 3).inverse() * direction;
}
-
Eigen::Vector3f
Device::changeDirectionFrameFromGlobal(const Eigen::Vector3f& globalDirection,
const VirtualRobot::SceneObjectPtr& newFrame)
@@ -208,7 +199,6 @@ namespace devices::ethercat::ft_sensor_board::armar6
return toNewFrame.block(0, 0, 3, 3).inverse() * globalDirection;
}
-
void
Device::updateSensorValueStruct()
{
@@ -321,21 +311,18 @@ namespace devices::ethercat::ft_sensor_board::armar6
// sensorValue.gravityCompensatedTorque = parentPoseGlobalInv * sensorValue.gravityCompensatedTorque;
}
-
std::string
Device::getReportingFrame() const
{
return sensorNode->getParent()->getName();
}
-
armarx::control::ethercat::SlaveIdentifier
Device::getSlaveIdentifier() const
{
return slaveIdentifier;
}
-
Data*
Device::getData() const
{
@@ -348,7 +335,6 @@ namespace devices::ethercat::ft_sensor_board::armar6
return dataPtr.get();
}
-
void
Device::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -364,7 +350,6 @@ namespace devices::ethercat::ft_sensor_board::armar6
updateSensorValueStruct();
}
-
const armarx::SensorValueBase*
Device::getSensorValue() const
{
diff --git a/source/devices/ethercat/ft_sensor_board/armar6/Device.h b/source/devices/ethercat/ft_sensor_board/armar6/Device.h
index ea04030d..1c771d02 100644
--- a/source/devices/ethercat/ft_sensor_board/armar6/Device.h
+++ b/source/devices/ethercat/ft_sensor_board/armar6/Device.h
@@ -30,17 +30,15 @@
#include <armarx/control/ethercat/DeviceInterface.h>
#include <armarx/control/ethercat/SlaveIdentifier.h>
+#include <devices/ethercat/ft_sensor_board/armar6/Config.h>
#include <devices/ethercat/ft_sensor_board/armar6/Data.h>
#include <devices/ethercat/ft_sensor_board/armar6/Slave.h>
-#include <devices/ethercat/ft_sensor_board/armar6/Config.h>
-
namespace devices::ethercat::ft_sensor_board::armar6
{
using JointFTSensorShapeControllerPtr = std::shared_ptr<class JointFTSensorShapeController>;
-
class Device : public armarx::SensorDevice, public armarx::control::ethercat::DeviceInterface
{
diff --git a/source/devices/ethercat/ft_sensor_board/armar6/Slave.cpp b/source/devices/ethercat/ft_sensor_board/armar6/Slave.cpp
index d1ece8bf..269d3f94 100644
--- a/source/devices/ethercat/ft_sensor_board/armar6/Slave.cpp
+++ b/source/devices/ethercat/ft_sensor_board/armar6/Slave.cpp
@@ -5,7 +5,6 @@
#include <devices/ethercat/common/h2t_devices.h>
#include <devices/ethercat/common/sensor_board/armar6/ADS1263CRCHelper.h>
-
namespace devices::ethercat::ft_sensor_board::armar6
{
@@ -15,7 +14,6 @@ namespace devices::ethercat::ft_sensor_board::armar6
setTag(this->slaveIdentifier.getName());
}
-
void
Slave::execute()
{
@@ -44,65 +42,55 @@ namespace devices::ethercat::ft_sensor_board::armar6
"adc_4_to_6_temperature");
}
-
bool
Slave::prepareForRun()
{
return true;
}
-
bool
Slave::shutdown()
{
return true;
}
-
void
Slave::doMappings()
{
}
-
bool
Slave::hasError()
{
return false;
}
-
void
Slave::prepareForSafeOp()
{
}
-
void
Slave::finishPreparingForSafeOp()
{
}
-
void
Slave::prepareForOp()
{
}
-
void
Slave::finishPreparingForOp()
{
}
-
std::string
Slave::getDefaultName()
{
return "Armar6FTSensorBoard";
}
-
bool
Slave::isSlaveIdentifierAccepted(const armarx::control::ethercat::SlaveIdentifier& sid)
{
diff --git a/source/devices/ethercat/ft_sensor_board/armar6/Slave.h b/source/devices/ethercat/ft_sensor_board/armar6/Slave.h
index eab96f10..03649854 100644
--- a/source/devices/ethercat/ft_sensor_board/armar6/Slave.h
+++ b/source/devices/ethercat/ft_sensor_board/armar6/Slave.h
@@ -6,19 +6,16 @@
#include <devices/ethercat/ft_sensor_board/armar6/SlaveIO.h>
-
namespace devices::ethercat::ft_sensor_board::armar6
{
- static constexpr std::uint32_t H2T_FTSENSOR_PRODUCT_CODE_LEFT = 0x501;
- static constexpr std::uint32_t H2T_FTSENSOR_PRODUCT_CODE_RIGHT = 0x502;
- static constexpr std::uint32_t H2T_FTSENSOR_PRODUCT_CODE_RIGHT_2 = 0x503;
+ static constexpr std::uint32_t H2T_FTSENSOR_PRODUCT_CODE_LEFT = 0x501;
+ static constexpr std::uint32_t H2T_FTSENSOR_PRODUCT_CODE_RIGHT = 0x502;
+ static constexpr std::uint32_t H2T_FTSENSOR_PRODUCT_CODE_RIGHT_2 = 0x503;
- class Slave :
- public armarx::control::ethercat::SlaveInterfaceWithIO<SlaveIn, SlaveOut>
+ class Slave : public armarx::control::ethercat::SlaveInterfaceWithIO<SlaveIn, SlaveOut>
{
public:
-
Slave(const armarx::control::ethercat::SlaveIdentifier slaveIdentifier);
/**
@@ -57,8 +54,8 @@ namespace devices::ethercat::ft_sensor_board::armar6
static std::string getDefaultName();
- static bool isSlaveIdentifierAccepted(const armarx::control::ethercat::SlaveIdentifier& sid);
-
+ static bool
+ isSlaveIdentifierAccepted(const armarx::control::ethercat::SlaveIdentifier& sid);
};
-}
+} // namespace devices::ethercat::ft_sensor_board::armar6
diff --git a/source/devices/ethercat/ft_sensor_board/armar6/SlaveIO.h b/source/devices/ethercat/ft_sensor_board/armar6/SlaveIO.h
index cbe456f4..11b38735 100644
--- a/source/devices/ethercat/ft_sensor_board/armar6/SlaveIO.h
+++ b/source/devices/ethercat/ft_sensor_board/armar6/SlaveIO.h
@@ -4,7 +4,6 @@
// STD/STL
#include <cstdint>
-
namespace devices::ethercat::ft_sensor_board::armar6
{
@@ -22,22 +21,21 @@ namespace devices::ethercat::ft_sensor_board::armar6
std::int32_t adc_1_to_3_temperature;
std::int32_t adc_4_to_6_temperature;
- std::uint8_t dms_adc_crc_1;
- std::uint8_t dms_adc_crc_2;
- std::uint8_t dms_adc_crc_3;
- std::uint8_t dms_adc_crc_4;
- std::uint8_t dms_adc_crc_5;
- std::uint8_t dms_adc_crc_6;
+ std::uint8_t dms_adc_crc_1;
+ std::uint8_t dms_adc_crc_2;
+ std::uint8_t dms_adc_crc_3;
+ std::uint8_t dms_adc_crc_4;
+ std::uint8_t dms_adc_crc_5;
+ std::uint8_t dms_adc_crc_6;
- std::uint8_t adc_temperature_crc_1_to_3;
- std::uint8_t adc_temperature_crc_4_to_6;
+ std::uint8_t adc_temperature_crc_1_to_3;
+ std::uint8_t adc_temperature_crc_4_to_6;
std::uint16_t update_rate;
std::int16_t pad16;
} __attribute__((__packed__));
-
/**
* @brief PDO mapping master->sensorB
*/
@@ -50,4 +48,4 @@ namespace devices::ethercat::ft_sensor_board::armar6
std::uint8_t pad2;
} __attribute__((__packed__));
-}
+} // namespace devices::ethercat::ft_sensor_board::armar6
diff --git a/source/devices/ethercat/ft_sensor_board/armarde/Config.h b/source/devices/ethercat/ft_sensor_board/armarde/Config.h
index 9d8b0d86..3c2656ba 100644
--- a/source/devices/ethercat/ft_sensor_board/armarde/Config.h
+++ b/source/devices/ethercat/ft_sensor_board/armarde/Config.h
@@ -6,13 +6,13 @@ namespace devices::ethercat::ft_sensor_board::armarde
{
struct DataConfig
{
- DataConfig(armarx::control::hardware_config::DeviceConfig& hwConfig)
- : firstRobotNodeNameAfterSensor(hwConfig.getString("FirstRobotNodeNameAfterSensor")),
- ticksToVoltFactor(hwConfig.getFloat("ticksToVoltFactor")),
- conversionMatrix(hwConfig.getMatrix<float, 6, 6>("FTCalibrationMatrix")),
- channelOrder(hwConfig.getMatrix<int, 6, 1>("ChannelOrder")),
- offsetList(hwConfig.getIntList("Offset")),
- temperature(hwConfig.getLinearConfig("temperature"))
+ DataConfig(armarx::control::hardware_config::DeviceConfig& hwConfig) :
+ firstRobotNodeNameAfterSensor(hwConfig.getString("FirstRobotNodeNameAfterSensor")),
+ ticksToVoltFactor(hwConfig.getFloat("ticksToVoltFactor")),
+ conversionMatrix(hwConfig.getMatrix<float, 6, 6>("FTCalibrationMatrix")),
+ channelOrder(hwConfig.getMatrix<int, 6, 1>("ChannelOrder")),
+ offsetList(hwConfig.getIntList("Offset")),
+ temperature(hwConfig.getLinearConfig("temperature"))
{
}
@@ -23,4 +23,4 @@ namespace devices::ethercat::ft_sensor_board::armarde
std::list<int> offsetList;
armarx::control::hardware_config::types::LinearConfig temperature;
};
-}
+} // namespace devices::ethercat::ft_sensor_board::armarde
diff --git a/source/devices/ethercat/ft_sensor_board/armarde/Data.cpp b/source/devices/ethercat/ft_sensor_board/armarde/Data.cpp
index f00ecf86..004e11ae 100644
--- a/source/devices/ethercat/ft_sensor_board/armarde/Data.cpp
+++ b/source/devices/ethercat/ft_sensor_board/armarde/Data.cpp
@@ -26,14 +26,13 @@
#include "Data.h"
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelperTemplates.h>
namespace devices::ethercat::ft_sensor_board::armarde
{
- Data::Data(const DataConfig& config, SlaveOut* outputs) :
- outputs(outputs)
+ Data::Data(const DataConfig& config, SlaveOut* outputs) : outputs(outputs)
{
ARMARX_CHECK_EXPRESSION(outputs);
@@ -50,7 +49,8 @@ namespace devices::ethercat::ft_sensor_board::armarde
std::vector<int> offsetList(config.offsetList.begin(), config.offsetList.end());
ARMARX_DEBUG << VAROUT(offsetList);
ARMARX_CHECK_EQUAL(offsetList.size(), 6);
- offset << offsetList[0], offsetList[1], offsetList[2], offsetList[3], offsetList[4], offsetList[5];
+ offset << offsetList[0], offsetList[1], offsetList[2], offsetList[3], offsetList[4],
+ offsetList[5];
ARMARX_DEBUG << VAROUT(offset);
filters.resize(6, armarx::control::rt_filters::RtMedianFilter(10));
@@ -58,7 +58,8 @@ namespace devices::ethercat::ft_sensor_board::armarde
temperature.init(&(outputs->supplyCurrentShunt), config.temperature);
}
- std::vector<int> Data::readIntList(std::string str)
+ std::vector<int>
+ Data::readIntList(std::string str)
{
std::vector<std::string> parts = armarx::Split(str, "\t \n", true, true);
std::vector<int> result;
@@ -109,8 +110,10 @@ namespace devices::ethercat::ft_sensor_board::armarde
//ARMARX_INFO << VAROUT(rawForceTorque);
// ARMARX_IMPORTANT << VAROUT(rawForceTorque);
forceTorqueValues = conversionMatrix * (rawForceTorque.cast<float>() * ticksToVoltFactor);
- forces << filters.at(0).update(forceTorqueValues(0)), filters.at(1).update(forceTorqueValues(1)), filters.at(2).update(forceTorqueValues(2));
- torques << filters.at(3).update(forceTorqueValues(3)), filters.at(4).update(forceTorqueValues(4)), filters.at(5).update(forceTorqueValues(5));
+ forces << filters.at(0).update(forceTorqueValues(0)),
+ filters.at(1).update(forceTorqueValues(1)), filters.at(2).update(forceTorqueValues(2));
+ torques << filters.at(3).update(forceTorqueValues(3)),
+ filters.at(4).update(forceTorqueValues(4)), filters.at(5).update(forceTorqueValues(5));
// ARMARX_IMPORTANT << "filters applied";
@@ -140,6 +143,7 @@ namespace devices::ethercat::ft_sensor_board::armarde
{
return adc1UpdateRate;
}
+
std::int32_t
Data::getAdc2UpdateRate() const
{
@@ -176,7 +180,6 @@ namespace devices::ethercat::ft_sensor_board::armarde
return forces;
}
-
const Eigen::Vector3f&
Data::getTorques() const
{
@@ -202,7 +205,10 @@ namespace devices::ethercat::ft_sensor_board::armarde
valueSigned = valueSigned / 256;
//From Datasheet: Temperature (°C) = [(Temperature Reading (μV) – 122,400) / 420 μV/°C] + 25°C
- float tempUv = static_cast<float>(valueSigned) * 5000000.0f / static_cast<float>(0x1000000); //multiplied with 2*ref voltage (full-scale ADC-range = +-Vref, in µV) and divided by ADC-resolution (24 bit)
+ float tempUv =
+ static_cast<float>(valueSigned) * 5000000.0f /
+ static_cast<float>(
+ 0x1000000); //multiplied with 2*ref voltage (full-scale ADC-range = +-Vref, in µV) and divided by ADC-resolution (24 bit)
float tempDegC = (tempUv - 122400.0f) / 420.0f + 25.0f;
return tempDegC;
diff --git a/source/devices/ethercat/ft_sensor_board/armarde/Data.h b/source/devices/ethercat/ft_sensor_board/armarde/Data.h
index 9d4c50f1..0dd9dbdf 100644
--- a/source/devices/ethercat/ft_sensor_board/armarde/Data.h
+++ b/source/devices/ethercat/ft_sensor_board/armarde/Data.h
@@ -32,15 +32,15 @@
#include <armarx/control/ethercat/DataInterface.h>
#include <armarx/control/rt_filters/RtMedianFilter.h>
-#include <devices/ethercat/ft_sensor_board/armarde/SlaveIO.h>
#include <devices/ethercat/ft_sensor_board/armarde/Config.h>
+#include <devices/ethercat/ft_sensor_board/armarde/SlaveIO.h>
namespace Eigen
{
using Matrix6f = Matrix<float, 6, 6>;
using Vector6f = Matrix<float, 6, 1>;
using Vector6i = Matrix<int, 6, 1>;
-}
+} // namespace Eigen
namespace devices::ethercat::ft_sensor_board::armarde
{
@@ -77,7 +77,7 @@ namespace devices::ethercat::ft_sensor_board::armarde
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- std::int32_t mainCounter;
+ std::int32_t mainCounter;
std::int32_t mainUpdateRate;
std::int32_t adc1UpdateRate;
std::int32_t adc2UpdateRate;
@@ -90,7 +90,6 @@ namespace devices::ethercat::ft_sensor_board::armarde
Eigen::Vector6f rawForceTorque;
float temperature;
-
static SensorValueInfo<FTSensorDataValue>
GetClassMemberInfo()
{
@@ -109,23 +108,27 @@ namespace devices::ethercat::ft_sensor_board::armarde
svi.addMemberVariable(&FTSensorDataValue::temperatureAdc2, "temperatureAdc2");
svi.addMemberVariable(&FTSensorDataValue::rawForceTorque, "rawForceTorque")
- .setVariantReportFunction(
- [](const IceUtil::Time & timestamp, const FTSensorDataValue * ptr)
- {
- return std::map<std::string, VariantBasePtr>
- {
- {"adc_raw_1_to_3", {new armarx::TimedVariant {armarx::Vector3((Eigen::Vector3f) ptr->rawForceTorque.block<3, 1>(0, 0)), timestamp}}},
- {"adc_raw_4_to_6", {new armarx::TimedVariant {armarx::Vector3((Eigen::Vector3f) ptr->rawForceTorque.block<3, 1>(3, 0)), timestamp}}}
- };
- }
- );
+ .setVariantReportFunction(
+ [](const IceUtil::Time& timestamp, const FTSensorDataValue* ptr)
+ {
+ return std::map<std::string, VariantBasePtr>{
+ {"adc_raw_1_to_3",
+ {new armarx::TimedVariant{
+ armarx::Vector3(
+ (Eigen::Vector3f)ptr->rawForceTorque.block<3, 1>(0, 0)),
+ timestamp}}},
+ {"adc_raw_4_to_6",
+ {new armarx::TimedVariant{
+ armarx::Vector3(
+ (Eigen::Vector3f)ptr->rawForceTorque.block<3, 1>(3, 0)),
+ timestamp}}}};
+ });
svi.addMemberVariable(&FTSensorDataValue::temperature, "temperature");
return svi;
}
};
-
class Data : public armarx::control::ethercat::DataInterface
{
public:
@@ -179,7 +182,6 @@ namespace devices::ethercat::ft_sensor_board::armarde
std::vector<int> readIntList(std::string str);
float getTemperatureAdcInDeg(std::int32_t rawValue) const;
-
};
using DataPtr = std::shared_ptr<Data>;
diff --git a/source/devices/ethercat/ft_sensor_board/armarde/Device.cpp b/source/devices/ethercat/ft_sensor_board/armarde/Device.cpp
index 500e04f3..0ee15399 100644
--- a/source/devices/ethercat/ft_sensor_board/armarde/Device.cpp
+++ b/source/devices/ethercat/ft_sensor_board/armarde/Device.cpp
@@ -108,7 +108,6 @@ namespace devices::ethercat::ft_sensor_board::armarde
gravityOffsetVecForce = Eigen::Vector3f::Zero();
}
-
armarx::control::ethercat::DeviceInterface::TryAssignResult
Device::tryAssign(armarx::control::ethercat::SlaveInterface& slave)
{
@@ -120,7 +119,6 @@ namespace devices::ethercat::ft_sensor_board::armarde
return result;
}
-
armarx::control::ethercat::DeviceInterface::AllAssignedResult
Device::onAllAssigned()
{
@@ -158,7 +156,6 @@ namespace devices::ethercat::ft_sensor_board::armarde
{
}
-
Eigen::Vector3f
changeDirectionFrame(const Eigen::Vector3f direction,
const VirtualRobot::SceneObjectPtr& currentFrame,
@@ -172,7 +169,6 @@ namespace devices::ethercat::ft_sensor_board::armarde
return toNewFrame.block(0, 0, 3, 3).inverse() * direction;
}
-
Eigen::Vector3f
Device::changeDirectionFrameToGlobal(const Eigen::Vector3f direction,
const VirtualRobot::SceneObjectPtr& currentFrame)
@@ -187,7 +183,6 @@ namespace devices::ethercat::ft_sensor_board::armarde
return toNewFrame.block(0, 0, 3, 3).inverse() * direction;
}
-
Eigen::Vector3f
Device::changeDirectionFrameFromGlobal(const Eigen::Vector3f& globalDirection,
const VirtualRobot::SceneObjectPtr& newFrame)
@@ -202,10 +197,9 @@ namespace devices::ethercat::ft_sensor_board::armarde
return toNewFrame.block(0, 0, 3, 3).inverse() * globalDirection;
}
-
void
Device::updateSensorValueStruct()
- {
+ {
sensorValue.force = dataPtr->getForces();
sensorValue.torque = dataPtr->getTorques();
@@ -254,14 +248,12 @@ namespace devices::ethercat::ft_sensor_board::armarde
sensorValue.temperature = dataPtr->getTemperature();
}
-
std::string
Device::getReportingFrame() const
{
return sensorNode->getParent()->getName();
}
-
std::shared_ptr<Data>
Device::getData() const
{
@@ -273,14 +265,12 @@ namespace devices::ethercat::ft_sensor_board::armarde
return dataPtr;
}
-
const std::string&
Device::getJointName() const
{
return getDeviceName();
}
-
void
Device::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -294,7 +284,6 @@ namespace devices::ethercat::ft_sensor_board::armarde
updateSensorValueStruct();
}
-
const armarx::SensorValueBase*
Device::getSensorValue() const
{
diff --git a/source/devices/ethercat/ft_sensor_board/armarde/Device.h b/source/devices/ethercat/ft_sensor_board/armarde/Device.h
index bee0293f..bcd0e2bf 100644
--- a/source/devices/ethercat/ft_sensor_board/armarde/Device.h
+++ b/source/devices/ethercat/ft_sensor_board/armarde/Device.h
@@ -33,19 +33,18 @@
#include <devices/ethercat/ft_sensor_board/armarde/Data.h>
#include <devices/ethercat/ft_sensor_board/armarde/Slave.h>
-
namespace devices::ethercat::ft_sensor_board::armarde
{
using JointFTSensorShapeControllerPtr = std::shared_ptr<class JointFTSensorShapeController>;
-
class Device : public armarx::SensorDevice, public armarx::control::ethercat::DeviceInterface
{
public:
Device(armarx::control::hardware_config::DeviceConfig& hwConfig,
VirtualRobot::RobotPtr const& robot);
+
~Device() override
{
}
diff --git a/source/devices/ethercat/ft_sensor_board/armarde/ErrorDecoder.cpp b/source/devices/ethercat/ft_sensor_board/armarde/ErrorDecoder.cpp
index 584f799c..bb21f676 100644
--- a/source/devices/ethercat/ft_sensor_board/armarde/ErrorDecoder.cpp
+++ b/source/devices/ethercat/ft_sensor_board/armarde/ErrorDecoder.cpp
@@ -28,8 +28,7 @@
namespace devices::ethercat::ft_sensor_board::armarde
{
- ErrorDecoder::ErrorDecoder(SlaveOut* outputs) :
- outputs(outputs)
+ ErrorDecoder::ErrorDecoder(SlaveOut* outputs) : outputs(outputs)
{
}
diff --git a/source/devices/ethercat/ft_sensor_board/armarde/Slave.cpp b/source/devices/ethercat/ft_sensor_board/armarde/Slave.cpp
index f4ea7784..b5e9bfcf 100644
--- a/source/devices/ethercat/ft_sensor_board/armarde/Slave.cpp
+++ b/source/devices/ethercat/ft_sensor_board/armarde/Slave.cpp
@@ -31,7 +31,6 @@
#include <devices/ethercat/common/h2t_devices.h>
-
namespace devices::ethercat::ft_sensor_board::armarde
{
Slave::Slave(const armarx::control::ethercat::SlaveIdentifier slaveIdentifier_) :
@@ -42,33 +41,28 @@ namespace devices::ethercat::ft_sensor_board::armarde
setTag(this->slaveIdentifier.getName());
}
-
void
Slave::execute()
{
}
-
bool
Slave::prepareForRun()
{
return true;
}
-
bool
Slave::shutdown()
{
return true;
}
-
void
Slave::doMappings()
{
}
-
bool
Slave::hasError()
{
@@ -188,19 +182,16 @@ namespace devices::ethercat::ft_sensor_board::armarde
return hasError;
}
-
void
Slave::prepareForSafeOp()
{
}
-
void
Slave::finishPreparingForSafeOp()
{
}
-
void
Slave::prepareForOp()
{
@@ -208,7 +199,6 @@ namespace devices::ethercat::ft_sensor_board::armarde
errorDecoder = ErrorDecoder(getOutputsPtr());
}
-
void
Slave::finishPreparingForOp()
{
diff --git a/source/devices/ethercat/ft_sensor_board/armarde/SlaveIO.h b/source/devices/ethercat/ft_sensor_board/armarde/SlaveIO.h
index 91b9172a..68e3233a 100644
--- a/source/devices/ethercat/ft_sensor_board/armarde/SlaveIO.h
+++ b/source/devices/ethercat/ft_sensor_board/armarde/SlaveIO.h
@@ -29,7 +29,6 @@
#include <cstdint>
-
namespace devices::ethercat::ft_sensor_board::armarde
{
/*
diff --git a/source/devices/ethercat/hand/armar6_v1/Config.h b/source/devices/ethercat/hand/armar6_v1/Config.h
index 7d5d0be1..7f8f3d1a 100644
--- a/source/devices/ethercat/hand/armar6_v1/Config.h
+++ b/source/devices/ethercat/hand/armar6_v1/Config.h
@@ -5,15 +5,16 @@
namespace devices::ethercat::hand::armar6_v1
{
namespace hwconfig = armarx::control::hardware_config;
+
struct DataConfig
{
- DataConfig(hwconfig::DeviceConfig& hwConfig)
- : motorTemperature(hwConfig.getLinearConfig("motorTemperature")),
- imuQuaternion(hwConfig.getLinearConfig("imuQuaternion")),
- accelerometer(hwConfig.getLinearConfig("accelerometer"))
+ DataConfig(hwconfig::DeviceConfig& hwConfig) :
+ motorTemperature(hwConfig.getLinearConfig("motorTemperature")),
+ imuQuaternion(hwConfig.getLinearConfig("imuQuaternion")),
+ accelerometer(hwConfig.getLinearConfig("accelerometer"))
{
-
}
+
hwconfig::types::LinearConfig motorTemperature;
hwconfig::types::LinearConfig imuQuaternion;
hwconfig::types::LinearConfig accelerometer;
@@ -21,13 +22,14 @@ namespace devices::ethercat::hand::armar6_v1
struct CommandSequenceConfig
{
- CommandSequenceConfig(std::string name, hwconfig::Config& cfg)
- : delay(cfg.getInt(name + "Delay")),
- activePwm(cfg.getInt(name + "ActivePwm")),
- duration(cfg.getInt(name + "Duration")),
- holdPwm(cfg.getInt(name + "HoldPwm"))
+ CommandSequenceConfig(std::string name, hwconfig::Config& cfg) :
+ delay(cfg.getInt(name + "Delay")),
+ activePwm(cfg.getInt(name + "ActivePwm")),
+ duration(cfg.getInt(name + "Duration")),
+ holdPwm(cfg.getInt(name + "HoldPwm"))
{
}
+
std::int16_t delay;
std::int16_t activePwm;
std::int16_t duration;
@@ -36,17 +38,17 @@ namespace devices::ethercat::hand::armar6_v1
struct ShapeConfig
{
- ShapeConfig(hwconfig::Config& controllerConfig)
- : fullClose("FullClose", controllerConfig),
- halfClose("HalfClose", controllerConfig),
- none("None", controllerConfig),
- halfOpen("HalfOpen", controllerConfig),
- fullOpen("FullOpen", controllerConfig),
- deltaOpen("DeltaOpen", controllerConfig),
- deltaClose("DeltaClose", controllerConfig)
+ ShapeConfig(hwconfig::Config& controllerConfig) :
+ fullClose("FullClose", controllerConfig),
+ halfClose("HalfClose", controllerConfig),
+ none("None", controllerConfig),
+ halfOpen("HalfOpen", controllerConfig),
+ fullOpen("FullOpen", controllerConfig),
+ deltaOpen("DeltaOpen", controllerConfig),
+ deltaClose("DeltaClose", controllerConfig)
{
-
}
+
CommandSequenceConfig fullClose;
CommandSequenceConfig halfClose;
CommandSequenceConfig none;
@@ -55,4 +57,4 @@ namespace devices::ethercat::hand::armar6_v1
CommandSequenceConfig deltaOpen;
CommandSequenceConfig deltaClose;
};
-}
+} // namespace devices::ethercat::hand::armar6_v1
diff --git a/source/devices/ethercat/hand/armar6_v1/Data.cpp b/source/devices/ethercat/hand/armar6_v1/Data.cpp
index df112a1d..44a0e5da 100644
--- a/source/devices/ethercat/hand/armar6_v1/Data.cpp
+++ b/source/devices/ethercat/hand/armar6_v1/Data.cpp
@@ -29,15 +29,14 @@
namespace devices::ethercat::hand::armar6_v1
{
- Data::Data(DataConfig& config,
- SlaveOut* sensorOUT, SlaveIn* sensorIN) :
- sensorOUT(sensorOUT),
- sensorIN(sensorIN)
+ Data::Data(DataConfig& config, SlaveOut* sensorOUT, SlaveIn* sensorIN) :
+ sensorOUT(sensorOUT), sensorIN(sensorIN)
{
command = 0;
ARMARX_CHECK_EXPRESSION(sensorOUT);
- motorTemperatureFingers.init(&sensorOUT->motor_fingers_temperature, config.motorTemperature);
+ motorTemperatureFingers.init(&sensorOUT->motor_fingers_temperature,
+ config.motorTemperature);
motorTemperatureThumb.init(&sensorOUT->motor_thumb_temperature, config.motorTemperature);
/*target_pwm_finger_motor.init(&sensorOUT->pwm_finger_motor, conversionNode.first_node("pwmMotor"));
@@ -57,8 +56,9 @@ namespace devices::ethercat::hand::armar6_v1
accelerometerTemperature.init(&sensorOUT->accelerometerTemperature, config.accelerometer);
}
-
- void Data::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ Data::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
motorTemperatureFingers.read();
motorTemperatureThumb.read();
@@ -82,8 +82,9 @@ namespace devices::ethercat::hand::armar6_v1
accelerometerTemperature.read();
}
-
- void Data::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ Data::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
sensorIN->command = command;
sensorIN->direction_finger_motor = direction_finger_motor;
@@ -92,8 +93,8 @@ namespace devices::ethercat::hand::armar6_v1
sensorIN->pwm_thumb_state = target_pwm_thumb_motor;
}
-
- void Data::setCommand(Command command, std::int16_t fingerPwm, std::int16_t thumbPwm)
+ void
+ Data::setCommand(Command command, std::int16_t fingerPwm, std::int16_t thumbPwm)
{
this->command = static_cast<std::uint8_t>(command);
this->direction_finger_motor = fingerPwm > 0 ? 1 : 0;
@@ -102,82 +103,82 @@ namespace devices::ethercat::hand::armar6_v1
this->target_pwm_thumb_motor = std::abs(thumbPwm);
}
-
- float Data::getMotorTemperatureFingers() const
+ float
+ Data::getMotorTemperatureFingers() const
{
return motorTemperatureFingers.value;
}
-
- float Data::getMotorTemperatureThumb() const
+ float
+ Data::getMotorTemperatureThumb() const
{
return motorTemperatureThumb.value;
}
-
- float Data::getTargetPWMFingerMotor() const
+ float
+ Data::getTargetPWMFingerMotor() const
{
return target_pwm_finger_motor;
}
-
- float Data::getTargetPWMThumbMotor() const
+ float
+ Data::getTargetPWMThumbMotor() const
{
return target_pwm_thumb_motor;
}
-
- float Data::getFingerMotorDirection() const
+ float
+ Data::getFingerMotorDirection() const
{
return direction_finger_motor;
}
-
- float Data::getThumbMotorDirection() const
+ float
+ Data::getThumbMotorDirection() const
{
return direction_thumb_motor;
}
-
- const Eigen::Quaternionf& Data::getImutQuaternion() const
+ const Eigen::Quaternionf&
+ Data::getImutQuaternion() const
{
return imuQuaternion;
}
-
- const Eigen::Vector3f& Data::getLinearAcceleration() const
+ const Eigen::Vector3f&
+ Data::getLinearAcceleration() const
{
return linearAcceleration;
}
-
- float Data::getAccelerometerTemperature() const
+ float
+ Data::getAccelerometerTemperature() const
{
return accelerometerTemperature.value;
}
-
- std::uint8_t Data::getActualFingerPwm() const
+ std::uint8_t
+ Data::getActualFingerPwm() const
{
return sensorOUT->pwm_finger_motor;
}
-
- std::uint8_t Data::getActualFingerDirection() const
+ std::uint8_t
+ Data::getActualFingerDirection() const
{
return sensorOUT->direction_finger_motor;
}
-
- std::uint8_t Data::getActualThumbPwm() const
+ std::uint8_t
+ Data::getActualThumbPwm() const
{
return sensorOUT->pwm_thumb_motor;
}
-
- std::uint8_t Data::getActualThumbDirection() const
+ std::uint8_t
+ Data::getActualThumbDirection() const
{
return sensorOUT->direction_thumb_motor;
}
-}
+} // namespace devices::ethercat::hand::armar6_v1
diff --git a/source/devices/ethercat/hand/armar6_v1/Data.h b/source/devices/ethercat/hand/armar6_v1/Data.h
index 869ea51a..69af7e31 100644
--- a/source/devices/ethercat/hand/armar6_v1/Data.h
+++ b/source/devices/ethercat/hand/armar6_v1/Data.h
@@ -29,20 +29,19 @@
#include <memory>
// armarx
-#include <armarx/control/ethercat/DataInterface.h>
-#include <armarx/control/rt_filters/AverageFilter.h>
-#include <armarx/control/rt_filters/FirFilter.h>
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h>
#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h>
#include <RobotAPI/libraries/core/Pose.h>
+#include <armarx/control/ethercat/DataInterface.h>
+#include <armarx/control/rt_filters/AverageFilter.h>
+#include <armarx/control/rt_filters/FirFilter.h>
// robot_devices
-#include <devices/ethercat/hand/armar6_v1/Slave.h>
#include <devices/ethercat/hand/armar6_v1/Config.h>
-
+#include <devices/ethercat/hand/armar6_v1/Slave.h>
namespace devices::ethercat::hand::armar6_v1
{
@@ -51,9 +50,7 @@ namespace devices::ethercat::hand::armar6_v1
{
public:
-
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- float motorTemperatureFingers;
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION float motorTemperatureFingers;
float motorTemperatureThumb;
float target_pwm_finger_motor;
float target_pwm_thumb_motor;
@@ -71,25 +68,32 @@ namespace devices::ethercat::hand::armar6_v1
float accelerometerTemperature;
- static SensorValueInfo<SensorDataValue> GetClassMemberInfo()
+ static SensorValueInfo<SensorDataValue>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorDataValue> svi;
- svi.addMemberVariable(&SensorDataValue::motorTemperatureFingers, "motorTemperatureFingers");
+ svi.addMemberVariable(&SensorDataValue::motorTemperatureFingers,
+ "motorTemperatureFingers");
svi.addMemberVariable(&SensorDataValue::motorTemperatureThumb, "motorTemperatureThumb");
- svi.addMemberVariable(&SensorDataValue::target_pwm_finger_motor, "target_pwm_finger_motor");
- svi.addMemberVariable(&SensorDataValue::target_pwm_thumb_motor, "target_pwm_thumb_motor");
- svi.addMemberVariable(&SensorDataValue::direction_finger_motor, "direction_finger_motor");
+ svi.addMemberVariable(&SensorDataValue::target_pwm_finger_motor,
+ "target_pwm_finger_motor");
+ svi.addMemberVariable(&SensorDataValue::target_pwm_thumb_motor,
+ "target_pwm_thumb_motor");
+ svi.addMemberVariable(&SensorDataValue::direction_finger_motor,
+ "direction_finger_motor");
svi.addMemberVariable(&SensorDataValue::direction_thumb_motor, "direction_thumb_motor");
svi.addMemberVariable(&SensorDataValue::imuQuaternion, "imuQuaternion");
svi.addMemberVariable(&SensorDataValue::linearAcceleration, "linearAcceleration");
- svi.addMemberVariable(&SensorDataValue::accelerometerTemperature, "accelerometerTemperature");
+ svi.addMemberVariable(&SensorDataValue::accelerometerTemperature,
+ "accelerometerTemperature");
svi.addMemberVariable(&SensorDataValue::actual_finger_pwm, "actual_finger_pwm");
- svi.addMemberVariable(&SensorDataValue::actual_finger_direction, "actual_finger_direction");
+ svi.addMemberVariable(&SensorDataValue::actual_finger_direction,
+ "actual_finger_direction");
svi.addMemberVariable(&SensorDataValue::actual_thumb_pwm, "actual_thumb_pwm");
- svi.addMemberVariable(&SensorDataValue::actual_thumb_direction, "actual_thumb_direction");
+ svi.addMemberVariable(&SensorDataValue::actual_thumb_direction,
+ "actual_thumb_direction");
return svi;
}
-
};
@@ -108,8 +112,8 @@ namespace devices::ethercat::hand::armar6_v1
PWM = 0xFF
};
-
- inline std::string KITHandCommandToString(Command command)
+ inline std::string
+ KITHandCommandToString(Command command)
{
switch (command)
{
@@ -140,18 +144,16 @@ namespace devices::ethercat::hand::armar6_v1
};
}
-
- class Data :
- public armarx::control::ethercat::DataInterface
+ class Data : public armarx::control::ethercat::DataInterface
{
public:
+ Data(DataConfig& config, SlaveOut* sensorOUT, SlaveIn* sensorIN);
- Data(DataConfig& config,
- SlaveOut* sensorOUT, SlaveIn* sensorIN);
-
- void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
- void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+ void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
void setCommand(Command command, std::int16_t fingerPwm, std::int16_t thumbPwm);
float getMotorTemperatureFingers() const;
@@ -171,7 +173,6 @@ namespace devices::ethercat::hand::armar6_v1
std::uint8_t getActualThumbDirection() const;
private:
-
SlaveOut* sensorOUT;
SlaveIn* sensorIN;
std::atomic<std::uint8_t> command;
@@ -194,7 +195,6 @@ namespace devices::ethercat::hand::armar6_v1
armarx::control::ethercat::LinearConvertedValue<std::int16_t> linearAccY;
armarx::control::ethercat::LinearConvertedValue<std::int16_t> linearAccZ;
armarx::control::ethercat::LinearConvertedValue<std::uint8_t> accelerometerTemperature;
-
};
-}
+} // namespace devices::ethercat::hand::armar6_v1
diff --git a/source/devices/ethercat/hand/armar6_v1/Device.h b/source/devices/ethercat/hand/armar6_v1/Device.h
index 4a0ba560..7c9388c5 100644
--- a/source/devices/ethercat/hand/armar6_v1/Device.h
+++ b/source/devices/ethercat/hand/armar6_v1/Device.h
@@ -37,7 +37,6 @@
#include <devices/ethercat/hand/armar6_v1/Slave.h>
#include <devices/ethercat/hand/common/AbstractHand.h>
-
namespace devices::ethercat::hand::armar6_v1
{
@@ -48,7 +47,6 @@ namespace devices::ethercat::hand::armar6_v1
class StopMovement;
} // namespace joint_controller
-
class Device :
public armarx::ControlDevice,
public armarx::SensorDevice,
diff --git a/source/devices/ethercat/hand/armar6_v1/Slave.cpp b/source/devices/ethercat/hand/armar6_v1/Slave.cpp
index 1ea199af..0fb4ec26 100644
--- a/source/devices/ethercat/hand/armar6_v1/Slave.cpp
+++ b/source/devices/ethercat/hand/armar6_v1/Slave.cpp
@@ -19,13 +19,11 @@ namespace devices::ethercat::hand::armar6_v1
setTag(this->slaveIdentifier.getName());
}
-
void
Slave::execute()
{
}
-
bool
Slave::prepareForRun()
{
@@ -33,33 +31,28 @@ namespace devices::ethercat::hand::armar6_v1
return true;
}
-
bool
Slave::shutdown()
{
return true;
}
-
void
Slave::doMappings()
{
}
-
bool
Slave::hasError()
{
return false;
}
-
void
Slave::prepareForSafeOp()
{
}
-
void
Slave::finishPreparingForSafeOp()
{
@@ -70,7 +63,6 @@ namespace devices::ethercat::hand::armar6_v1
{
}
-
void
Slave::finishPreparingForOp()
{
diff --git a/source/devices/ethercat/hand/armar6_v1/Slave.h b/source/devices/ethercat/hand/armar6_v1/Slave.h
index e452bde2..9486bb47 100644
--- a/source/devices/ethercat/hand/armar6_v1/Slave.h
+++ b/source/devices/ethercat/hand/armar6_v1/Slave.h
@@ -9,16 +9,13 @@
#define H2T_KITHAND_LEFT_PRODUCT_CODE 0x401
#define H2T_KITHAND_RIGHT_PRODUCT_CODE 0x402
-
namespace devices::ethercat::hand::armar6_v1
{
- class Slave :
- public armarx::control::ethercat::SlaveInterfaceWithIO<SlaveIn, SlaveOut>
+ class Slave : public armarx::control::ethercat::SlaveInterfaceWithIO<SlaveIn, SlaveOut>
{
public:
-
Slave(const armarx::control::ethercat::SlaveIdentifier& slaveIdentifier);
/**
@@ -60,4 +57,4 @@ namespace devices::ethercat::hand::armar6_v1
isSlaveIdentifierAccepted(const armarx::control::ethercat::SlaveIdentifier& sid);
};
-}
+} // namespace devices::ethercat::hand::armar6_v1
diff --git a/source/devices/ethercat/hand/armar6_v1/SlaveIO.h b/source/devices/ethercat/hand/armar6_v1/SlaveIO.h
index 1374a43a..b2ec4334 100644
--- a/source/devices/ethercat/hand/armar6_v1/SlaveIO.h
+++ b/source/devices/ethercat/hand/armar6_v1/SlaveIO.h
@@ -4,7 +4,6 @@
// STD/STL
#include <cstdint>
-
namespace devices::ethercat::hand::armar6_v1
{
@@ -44,7 +43,6 @@ namespace devices::ethercat::hand::armar6_v1
} __attribute__((__packed__));
-
/**
* @brief PDO mapping master->sensorB
*/
@@ -67,4 +65,4 @@ namespace devices::ethercat::hand::armar6_v1
} __attribute__((__packed__));
-}
+} // namespace devices::ethercat::hand::armar6_v1
diff --git a/source/devices/ethercat/hand/armar6_v1/joint_controller/EmergencyStop.cpp b/source/devices/ethercat/hand/armar6_v1/joint_controller/EmergencyStop.cpp
index 7fb506e2..3b3ecc9d 100644
--- a/source/devices/ethercat/hand/armar6_v1/joint_controller/EmergencyStop.cpp
+++ b/source/devices/ethercat/hand/armar6_v1/joint_controller/EmergencyStop.cpp
@@ -1,6 +1,5 @@
#include "EmergencyStop.h"
-
namespace devices::ethercat::hand::armar6_v1::joint_controller
{
@@ -8,21 +7,18 @@ namespace devices::ethercat::hand::armar6_v1::joint_controller
{
}
-
void
EmergencyStop::rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
{
}
-
armarx::ControlTargetBase*
EmergencyStop::getControlTarget()
{
return ⌖
}
-
void
EmergencyStop::rtPreActivateController()
{
diff --git a/source/devices/ethercat/hand/armar6_v1/joint_controller/EmergencyStop.h b/source/devices/ethercat/hand/armar6_v1/joint_controller/EmergencyStop.h
index 1879d111..96292fcf 100644
--- a/source/devices/ethercat/hand/armar6_v1/joint_controller/EmergencyStop.h
+++ b/source/devices/ethercat/hand/armar6_v1/joint_controller/EmergencyStop.h
@@ -5,7 +5,6 @@
#include <devices/ethercat/hand/armar6_v1/Data.h>
-
namespace devices::ethercat::hand::armar6_v1::joint_controller
{
diff --git a/source/devices/ethercat/hand/armar6_v1/joint_controller/PWM.cpp b/source/devices/ethercat/hand/armar6_v1/joint_controller/PWM.cpp
index fa806d49..3f330400 100644
--- a/source/devices/ethercat/hand/armar6_v1/joint_controller/PWM.cpp
+++ b/source/devices/ethercat/hand/armar6_v1/joint_controller/PWM.cpp
@@ -1,6 +1,5 @@
#include "PWM.h"
-
namespace devices::ethercat::hand::armar6_v1::joint_controller
{
@@ -8,14 +7,12 @@ namespace devices::ethercat::hand::armar6_v1::joint_controller
{
}
-
armarx::ControlTargetBase*
PWM::getControlTarget()
{
return ⌖
}
-
void
PWM::rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -29,13 +26,11 @@ namespace devices::ethercat::hand::armar6_v1::joint_controller
}
}
-
void
PWM::rtPreActivateController()
{
}
-
void
PWM::rtPostDeactivateController()
{
diff --git a/source/devices/ethercat/hand/armar6_v1/joint_controller/PWM.h b/source/devices/ethercat/hand/armar6_v1/joint_controller/PWM.h
index 91f59c3b..76775a5c 100644
--- a/source/devices/ethercat/hand/armar6_v1/joint_controller/PWM.h
+++ b/source/devices/ethercat/hand/armar6_v1/joint_controller/PWM.h
@@ -6,7 +6,6 @@
#include <devices/ethercat/hand/armar6_v1/Data.h>
#include <devices/ethercat/hand/armar6_v1/Device.h>
-
namespace devices::ethercat::hand::armar6_v1::joint_controller
{
@@ -15,7 +14,6 @@ namespace devices::ethercat::hand::armar6_v1::joint_controller
static const std::string HandPWM = "ControlMode_HandPwm";
}
-
class HandPWMControlTarget : public armarx::ControlTargetBase
{
@@ -53,7 +51,6 @@ namespace devices::ethercat::hand::armar6_v1::joint_controller
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
};
-
class PWM : public armarx::JointController
{
diff --git a/source/devices/ethercat/hand/armar6_v1/joint_controller/StopMovement.cpp b/source/devices/ethercat/hand/armar6_v1/joint_controller/StopMovement.cpp
index d5be6659..744fa264 100644
--- a/source/devices/ethercat/hand/armar6_v1/joint_controller/StopMovement.cpp
+++ b/source/devices/ethercat/hand/armar6_v1/joint_controller/StopMovement.cpp
@@ -1,6 +1,5 @@
#include "StopMovement.h"
-
namespace devices::ethercat::hand::armar6_v1::joint_controller
{
@@ -8,21 +7,18 @@ namespace devices::ethercat::hand::armar6_v1::joint_controller
{
}
-
void
StopMovement::rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
{
}
-
armarx::ControlTargetBase*
StopMovement::getControlTarget()
{
return ⌖
}
-
void
StopMovement::rtPreActivateController()
{
diff --git a/source/devices/ethercat/hand/armar6_v1/joint_controller/StopMovement.h b/source/devices/ethercat/hand/armar6_v1/joint_controller/StopMovement.h
index 48de8943..c6136467 100644
--- a/source/devices/ethercat/hand/armar6_v1/joint_controller/StopMovement.h
+++ b/source/devices/ethercat/hand/armar6_v1/joint_controller/StopMovement.h
@@ -5,7 +5,6 @@
#include <devices/ethercat/hand/armar6_v1/Data.h>
-
namespace devices::ethercat::hand::armar6_v1::joint_controller
{
diff --git a/source/devices/ethercat/hand/armar6_v1/njoint_controller/Shape.cpp b/source/devices/ethercat/hand/armar6_v1/njoint_controller/Shape.cpp
index 3eab9844..6aa7656f 100644
--- a/source/devices/ethercat/hand/armar6_v1/njoint_controller/Shape.cpp
+++ b/source/devices/ethercat/hand/armar6_v1/njoint_controller/Shape.cpp
@@ -1,15 +1,12 @@
-#include <devices/ethercat/hand/armar6_v1/njoint_controller/Shape.h>
-
-
#include <devices/ethercat/hand/armar6_v1/Data.h>
#include <devices/ethercat/hand/armar6_v1/joint_controller/PWM.h>
-
+#include <devices/ethercat/hand/armar6_v1/njoint_controller/Shape.h>
namespace devices::ethercat::hand::armar6_v1::njoint_controller
{
- armarx::NJointControllerRegistration<ShapeController> registrationControllerNJointKITHandShapeController("NJointKITHandShapeController");
-
+ armarx::NJointControllerRegistration<ShapeController>
+ registrationControllerNJointKITHandShapeController("NJointKITHandShapeController");
ShapeController::ShapeController(armarx::RobotUnitPtr prov,
ShapeControllerConfigPtr config,
@@ -17,7 +14,8 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
{
ARMARX_CHECK_EXPRESSION(prov);
- armarx::ControlTargetBase* ct = useControlTarget(config->deviceName, joint_controller::ctrl_modes::HandPWM);
+ armarx::ControlTargetBase* ct =
+ useControlTarget(config->deviceName, joint_controller::ctrl_modes::HandPWM);
ARMARX_CHECK_EXPRESSION(ct);
sensorValues = useSensorValue<SensorDataValue>(config->deviceName);
ARMARX_CHECK_EXPRESSION(sensorValues) << "device name: " << config->deviceName;
@@ -56,32 +54,36 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
handBusy = false;
}
-
- void ShapeController::setShapeCommand(Command command)
+ void
+ ShapeController::setShapeCommand(Command command)
{
this->command = command;
- ARMARX_INFO << deactivateSpam(1, KITHandCommandToString(command)) << "new shape command: " << KITHandCommandToString(command);
+ ARMARX_INFO << deactivateSpam(1, KITHandCommandToString(command))
+ << "new shape command: " << KITHandCommandToString(command);
}
-
- void ShapeController::queueTargetShape(ShapeController::HandShapeState fingerState, ShapeController::HandShapeState thumbState)
+ void
+ ShapeController::queueTargetShape(ShapeController::HandShapeState fingerState,
+ ShapeController::HandShapeState thumbState)
{
- ARMARX_IMPORTANT << "queueTargetShape: " << VAROUT(fingerState) << " " << VAROUT(thumbState);
+ ARMARX_IMPORTANT << "queueTargetShape: " << VAROUT(fingerState) << " "
+ << VAROUT(thumbState);
this->queuedFingerState = fingerState;
this->queuedThumbState = thumbState;
this->hasQueuedTarget = true;
}
-
- void ShapeController::queueDeltaShapeCommand(ShapeController::DeltaShape fingerDelta, ShapeController::DeltaShape thumbDelta)
+ void
+ ShapeController::queueDeltaShapeCommand(ShapeController::DeltaShape fingerDelta,
+ ShapeController::DeltaShape thumbDelta)
{
this->queuedFingerDelta = fingerDelta;
this->queuedThumbDelta = thumbDelta;
this->hasQueuedDelta = true;
}
-
- void ShapeController::stopMotion()
+ void
+ ShapeController::stopMotion()
{
this->hasQueuedDelta = false;
this->hasQueuedTarget = false;
@@ -90,14 +92,14 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
handBusy = false;
}
-
/*bool NJointKITHandShapeController::isShaping()
{
return actualFingerState != targetFingerState || actualThumbState != targetThumbState;
}*/
- void ShapeController::setConfigs(const ShapeConfig& fingersConfig, const ShapeConfig& thumbConfig)
+ void
+ ShapeController::setConfigs(const ShapeConfig& fingersConfig, const ShapeConfig& thumbConfig)
{
fingerCommandSequences.fromShapeConfig(fingersConfig);
thumbCommandSequences.fromShapeConfig(thumbConfig);
@@ -106,34 +108,36 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
thumbCommandSeq = thumbCommandSequences.None;
}
-
/*bool NJointKITHandShapeController::hasQueuedTarget()
{
return targetFingerState != queuedFingerState || targetThumbState != queuedThumbState;
}*/
- ShapeController::HandShapeSubCommand ShapeController::getShapeSubCommand(ShapeController::HandShapeState currentState, ShapeController::HandShapeState targetState)
+ ShapeController::HandShapeSubCommand
+ ShapeController::getShapeSubCommand(ShapeController::HandShapeState currentState,
+ ShapeController::HandShapeState targetState)
{
// ARMARX_IMPORTANT << VAROUT(currentState) << " " << VAROUT(targetState);
- HandShapeSubCommand command = (HandShapeSubCommand)transitionMatrix(currentState, targetState);
+ HandShapeSubCommand command =
+ (HandShapeSubCommand)transitionMatrix(currentState, targetState);
return command;
}
-
- ShapeController::HandShapeState ShapeController::getActualThumbState()
+ ShapeController::HandShapeState
+ ShapeController::getActualThumbState()
{
return actualThumbState;
}
-
- ShapeController::HandShapeState ShapeController::getActualFingerState()
+ ShapeController::HandShapeState
+ ShapeController::getActualFingerState()
{
return actualFingerState;
}
-
- std::string ShapeController::HandShapeStateToString(ShapeController::HandShapeState state)
+ std::string
+ ShapeController::HandShapeStateToString(ShapeController::HandShapeState state)
{
switch (state)
{
@@ -148,20 +152,21 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
}
}
-
- std::int16_t ShapeController::getFingerPwm()
+ std::int16_t
+ ShapeController::getFingerPwm()
{
return fingerPwm;
}
-
- std::int16_t ShapeController::getThumbPwm()
+ std::int16_t
+ ShapeController::getThumbPwm()
{
return thumbPwm;
}
-
- void ShapeController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ ShapeController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
//controlTarget->shape = static_cast<u_int8_t>(command.operator armarx::KITHandCommand());
@@ -175,8 +180,10 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
lastStart = sensorValuesTimestamp;
handBusy = true;
- fingerCommandSeq = fingerCommandSequences.getCommandSequence(getShapeSubCommand(actualFingerState, targetFingerState));
- thumbCommandSeq = thumbCommandSequences.getCommandSequence(getShapeSubCommand(actualThumbState, targetThumbState));
+ fingerCommandSeq = fingerCommandSequences.getCommandSequence(
+ getShapeSubCommand(actualFingerState, targetFingerState));
+ thumbCommandSeq = thumbCommandSequences.getCommandSequence(
+ getShapeSubCommand(actualThumbState, targetThumbState));
//ARMARX_IMPORTANT << VAROUT(fingerCommandSeq.activePwm) << " " << VAROUT(thumbCommandSeq.activePwm);
}
@@ -185,8 +192,10 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
hasQueuedDelta = false;
lastStart = sensorValuesTimestamp;
handBusy = true;
- fingerCommandSeq = fingerCommandSequences.getDeltaSequence(actualFingerState, queuedFingerDelta);
- thumbCommandSeq = thumbCommandSequences.getDeltaSequence(actualThumbState, queuedThumbDelta);
+ fingerCommandSeq =
+ fingerCommandSequences.getDeltaSequence(actualFingerState, queuedFingerDelta);
+ thumbCommandSeq =
+ thumbCommandSequences.getDeltaSequence(actualThumbState, queuedThumbDelta);
}
if (handBusy)
@@ -200,7 +209,8 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
handBusy = false;
actualFingerState = targetFingerState;
actualThumbState = targetThumbState;
- ARMARX_DEBUG_S << "shape completed. fingerPwm=" << fingerPwm << " thumbPwm=" << thumbPwm;
+ ARMARX_DEBUG_S << "shape completed. fingerPwm=" << fingerPwm
+ << " thumbPwm=" << thumbPwm;
}
}
else
@@ -213,8 +223,8 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
controlTarget->pwm_thumb_motor = thumbPwm;
}
-
- void ShapeController::CommandSequence::setPwm(int deltaMs, std::int16_t& pwm)
+ void
+ ShapeController::CommandSequence::setPwm(int deltaMs, std::int16_t& pwm)
{
if (deltaMs > duration + delay)
{
@@ -226,14 +236,19 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
}
}
-
- bool ShapeController::CommandSequence::isDone(int deltaMs)
+ bool
+ ShapeController::CommandSequence::isDone(int deltaMs)
{
return deltaMs > duration + delay;
}
-
- void ShapeController::addSlider(std::vector<armarx::WidgetDescription::WidgetPtr>& children, std::string prefix, std::string name, std::int16_t defaultValue, std::int16_t min, std::int16_t max) const
+ void
+ ShapeController::addSlider(std::vector<armarx::WidgetDescription::WidgetPtr>& children,
+ std::string prefix,
+ std::string name,
+ std::int16_t defaultValue,
+ std::int16_t min,
+ std::int16_t max) const
{
using namespace armarx::WidgetDescription;
VBoxLayoutPtr vbox = new VBoxLayout;
@@ -247,8 +262,12 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
children.emplace_back(vbox);
}
-
- armarx::WidgetDescription::WidgetPtr ShapeController::createSequenceSliders(const std::string& name, std::int16_t min, std::int16_t max, CommandSequence fingerCommandSeq, CommandSequence thumbCommandSeq) const
+ armarx::WidgetDescription::WidgetPtr
+ ShapeController::createSequenceSliders(const std::string& name,
+ std::int16_t min,
+ std::int16_t max,
+ CommandSequence fingerCommandSeq,
+ CommandSequence thumbCommandSeq) const
{
using namespace armarx::WidgetDescription;
//VBoxLayoutPtr vbox = new VBoxLayout;
@@ -276,8 +295,11 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
return groupBox;
}
-
- void ShapeController::readSequence(const armarx::StringVariantBaseMap& valueMap, const std::string& name, CommandSequence& fingerCommandSeq, CommandSequence& thumbCommandSeq) const
+ void
+ ShapeController::readSequence(const armarx::StringVariantBaseMap& valueMap,
+ const std::string& name,
+ CommandSequence& fingerCommandSeq,
+ CommandSequence& thumbCommandSeq) const
{
fingerCommandSeq.activePwm = valueMap.at(name + "_" + "fingerActivePwm")->getFloat();
fingerCommandSeq.duration = valueMap.at(name + "_" + "fingerDuration")->getFloat();
@@ -295,27 +317,51 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
}
}
-
- armarx::WidgetDescription::StringWidgetDictionary ShapeController::getFunctionDescriptions(const Ice::Current&) const
+ armarx::WidgetDescription::StringWidgetDictionary
+ ShapeController::getFunctionDescriptions(const Ice::Current&) const
{
using namespace armarx::WidgetDescription;
VBoxLayoutPtr vboxLayout = new VBoxLayout;
- vboxLayout->children.emplace_back(createSequenceSliders("FullClose", 0, 255, fingerCommandSequences.FullClose, thumbCommandSequences.FullClose));
- vboxLayout->children.emplace_back(createSequenceSliders("HalfClose", 0, 255, fingerCommandSequences.HalfClose, thumbCommandSequences.HalfClose));
- vboxLayout->children.emplace_back(createSequenceSliders("HalfOpen", -255, 0, fingerCommandSequences.HalfOpen, thumbCommandSequences.HalfOpen));
- vboxLayout->children.emplace_back(createSequenceSliders("FullOpen", -255, 0, fingerCommandSequences.FullOpen, thumbCommandSequences.FullOpen));
+ vboxLayout->children.emplace_back(createSequenceSliders("FullClose",
+ 0,
+ 255,
+ fingerCommandSequences.FullClose,
+ thumbCommandSequences.FullClose));
+ vboxLayout->children.emplace_back(createSequenceSliders("HalfClose",
+ 0,
+ 255,
+ fingerCommandSequences.HalfClose,
+ thumbCommandSequences.HalfClose));
+ vboxLayout->children.emplace_back(createSequenceSliders(
+ "HalfOpen", -255, 0, fingerCommandSequences.HalfOpen, thumbCommandSequences.HalfOpen));
+ vboxLayout->children.emplace_back(createSequenceSliders(
+ "FullOpen", -255, 0, fingerCommandSequences.FullOpen, thumbCommandSequences.FullOpen));
return {{"HandControllerConfig", vboxLayout}};
}
-
- void ShapeController::callDescribedFunction(const std::string& name, const armarx::StringVariantBaseMap& valueMap, const Ice::Current&)
+ void
+ ShapeController::callDescribedFunction(const std::string& name,
+ const armarx::StringVariantBaseMap& valueMap,
+ const Ice::Current&)
{
if (name == "HandControllerConfig")
{
- readSequence(valueMap, "FullClose", fingerCommandSequences.FullClose, thumbCommandSequences.FullClose);
- readSequence(valueMap, "HalfClose", fingerCommandSequences.HalfClose, thumbCommandSequences.HalfClose);
- readSequence(valueMap, "HalfOpen", fingerCommandSequences.HalfOpen, thumbCommandSequences.HalfOpen);
- readSequence(valueMap, "FullOpen", fingerCommandSequences.FullOpen, thumbCommandSequences.FullOpen);
+ readSequence(valueMap,
+ "FullClose",
+ fingerCommandSequences.FullClose,
+ thumbCommandSequences.FullClose);
+ readSequence(valueMap,
+ "HalfClose",
+ fingerCommandSequences.HalfClose,
+ thumbCommandSequences.HalfClose);
+ readSequence(valueMap,
+ "HalfOpen",
+ fingerCommandSequences.HalfOpen,
+ thumbCommandSequences.HalfOpen);
+ readSequence(valueMap,
+ "FullOpen",
+ fingerCommandSequences.FullOpen,
+ thumbCommandSequences.FullOpen);
ARMARX_IMPORTANT << "fingerHoldPwm: " << fingerCommandSequences.FullClose.holdPwm;
}
@@ -325,8 +371,8 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
}
}
-
- void ShapeController::CommandSequenceConfig::fromShapeConfig(const ShapeConfig& shapeConfig)
+ void
+ ShapeController::CommandSequenceConfig::fromShapeConfig(const ShapeConfig& shapeConfig)
{
FullClose = shapeConfig.fullClose;
@@ -338,8 +384,9 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
DeltaClose = shapeConfig.deltaClose;
}
-
- ShapeController::CommandSequence ShapeController::CommandSequenceConfig::getCommandSequence(ShapeController::HandShapeSubCommand cmd)
+ ShapeController::CommandSequence
+ ShapeController::CommandSequenceConfig::getCommandSequence(
+ ShapeController::HandShapeSubCommand cmd)
{
switch (cmd)
{
@@ -358,8 +405,9 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
}
}
-
- ShapeController::CommandSequence ShapeController::CommandSequenceConfig::getDeltaSequence(HandShapeState currentState, ShapeController::DeltaShape cmd)
+ ShapeController::CommandSequence
+ ShapeController::CommandSequenceConfig::getDeltaSequence(HandShapeState currentState,
+ ShapeController::DeltaShape cmd)
{
switch (cmd)
{
@@ -374,4 +422,4 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
}
}
-} // namespace armarx
+} // namespace devices::ethercat::hand::armar6_v1::njoint_controller
diff --git a/source/devices/ethercat/hand/armar6_v1/njoint_controller/Shape.h b/source/devices/ethercat/hand/armar6_v1/njoint_controller/Shape.h
index 7bc620ad..9787b938 100644
--- a/source/devices/ethercat/hand/armar6_v1/njoint_controller/Shape.h
+++ b/source/devices/ethercat/hand/armar6_v1/njoint_controller/Shape.h
@@ -8,41 +8,55 @@
#include <devices/ethercat/hand/armar6_v1/Data.h>
#include <devices/ethercat/hand/armar6_v1/joint_controller/PWM.h>
-
namespace devices::ethercat::hand::armar6_v1::njoint_controller
{
using ShapeControllerConfigPtr = IceUtil::Handle<class ShapeControllerConfig>;
using ShapeControllerPtr = IceUtil::Handle<class ShapeController>;
-
- class ShapeControllerConfig :
- virtual public armarx::NJointControllerConfig
+ class ShapeControllerConfig : virtual public armarx::NJointControllerConfig
{
public:
-
- ShapeControllerConfig(std::string const& deviceName):
- deviceName(deviceName)
- {}
+ ShapeControllerConfig(std::string const& deviceName) : deviceName(deviceName)
+ {
+ }
const std::string deviceName;
-
};
-
- class ShapeController :
- public armarx::NJointController
+ class ShapeController : public armarx::NJointController
{
public:
-
using ConfigPtrT = ShapeControllerConfigPtr;
- ShapeController(armarx::RobotUnitPtr prov, ShapeControllerConfigPtr config, const VirtualRobot::RobotPtr& r);
+ ShapeController(armarx::RobotUnitPtr prov,
+ ShapeControllerConfigPtr config,
+ const VirtualRobot::RobotPtr& r);
+
+ enum HandShapeState : int
+ {
+ ShapeClosed = 0,
+ ShapeHalfOpen = 1,
+ ShapeOpen = 2,
+ ShapeDelta = 3
+ };
- enum HandShapeState : int { ShapeClosed = 0, ShapeHalfOpen = 1, ShapeOpen = 2, ShapeDelta = 3 };
- enum HandShapeSubCommand : int { CommandFullClose = -2, CommandHalfClose = -1, CommandNone = 0, CommandHalfOpen = 1, CommandFullOpen = 2 };
- enum DeltaShape : int {DeltaClose = -1, DeltaNone = 0, DeltaOpen = 1};
+ enum HandShapeSubCommand : int
+ {
+ CommandFullClose = -2,
+ CommandHalfClose = -1,
+ CommandNone = 0,
+ CommandHalfOpen = 1,
+ CommandFullOpen = 2
+ };
+
+ enum DeltaShape : int
+ {
+ DeltaClose = -1,
+ DeltaNone = 0,
+ DeltaOpen = 1
+ };
struct CommandSequence
{
@@ -51,17 +65,25 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
std::int16_t duration;
std::int16_t holdPwm;
- CommandSequence(std::int16_t delay, std::int16_t activePwm, std::int16_t duration, std::int16_t holdPwm)
- : delay(delay), activePwm(activePwm), duration(duration), holdPwm(holdPwm)
- {}
-
- CommandSequence(hand::armar6_v1::CommandSequenceConfig sequence)
- : delay(sequence.delay), activePwm(sequence.activePwm), duration(sequence.duration), holdPwm(sequence.holdPwm)
- {}
-
- CommandSequence()
- : CommandSequence(0, 0, 0, 0)
- {}
+ CommandSequence(std::int16_t delay,
+ std::int16_t activePwm,
+ std::int16_t duration,
+ std::int16_t holdPwm) :
+ delay(delay), activePwm(activePwm), duration(duration), holdPwm(holdPwm)
+ {
+ }
+
+ CommandSequence(hand::armar6_v1::CommandSequenceConfig sequence) :
+ delay(sequence.delay),
+ activePwm(sequence.activePwm),
+ duration(sequence.duration),
+ holdPwm(sequence.holdPwm)
+ {
+ }
+
+ CommandSequence() : CommandSequence(0, 0, 0, 0)
+ {
+ }
void setPwm(int deltaMs, std::int16_t& pwm);
bool isDone(int deltaMs);
@@ -86,8 +108,8 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
// NJointControllerInterface interface
public:
-
- std::string getClassName(const Ice::Current&) const override
+ std::string
+ getClassName(const Ice::Current&) const override
{
return "NJointKITHandShapeController";
}
@@ -102,7 +124,8 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
void setConfigs(const ShapeConfig& fingersConfig, const ShapeConfig& thumbConfig);
- HandShapeSubCommand getShapeSubCommand(HandShapeState currentState, HandShapeState targetState);
+ HandShapeSubCommand getShapeSubCommand(HandShapeState currentState,
+ HandShapeState targetState);
HandShapeState getActualThumbState();
HandShapeState getActualFingerState();
@@ -110,10 +133,10 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
std::int16_t getFingerPwm();
std::int16_t getThumbPwm();
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
private:
-
std::atomic<Command> command;
const SensorDataValue* sensorValues;
joint_controller::HandPWMControlTarget* controlTarget;
@@ -141,15 +164,29 @@ namespace devices::ethercat::hand::armar6_v1::njoint_controller
CommandSequenceConfig fingerCommandSequences;
CommandSequenceConfig thumbCommandSequences;
- armarx::WidgetDescription::WidgetPtr createSequenceSliders(const std::string& name, std::int16_t min, std::int16_t max, CommandSequence fingerCommandSeq, CommandSequence thumbCommandSeq) const;
- void readSequence(const armarx::StringVariantBaseMap& valueMap, const std::string& name, CommandSequence& fingerCommandSeq, CommandSequence& thumbCommandSeq) const;
+ armarx::WidgetDescription::WidgetPtr
+ createSequenceSliders(const std::string& name,
+ std::int16_t min,
+ std::int16_t max,
+ CommandSequence fingerCommandSeq,
+ CommandSequence thumbCommandSeq) const;
+ void readSequence(const armarx::StringVariantBaseMap& valueMap,
+ const std::string& name,
+ CommandSequence& fingerCommandSeq,
+ CommandSequence& thumbCommandSeq) const;
public:
-
- void addSlider(std::vector<armarx::WidgetDescription::WidgetPtr>& children, std::string prefix, std::string name, std::int16_t defaultValue, std::int16_t min, std::int16_t max) const;
- armarx::WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
- void callDescribedFunction(const std::string& name, const armarx::StringVariantBaseMap& valueMap, const Ice::Current&) override;
-
+ void addSlider(std::vector<armarx::WidgetDescription::WidgetPtr>& children,
+ std::string prefix,
+ std::string name,
+ std::int16_t defaultValue,
+ std::int16_t min,
+ std::int16_t max) const;
+ armarx::WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current&) const override;
+ void callDescribedFunction(const std::string& name,
+ const armarx::StringVariantBaseMap& valueMap,
+ const Ice::Current&) override;
};
-}
+} // namespace devices::ethercat::hand::armar6_v1::njoint_controller
diff --git a/source/devices/ethercat/hand/armar6_v2/Config.h b/source/devices/ethercat/hand/armar6_v2/Config.h
index b1d7d743..88216f7a 100644
--- a/source/devices/ethercat/hand/armar6_v2/Config.h
+++ b/source/devices/ethercat/hand/armar6_v2/Config.h
@@ -5,21 +5,23 @@
namespace devices::ethercat::hand::armar6_v2
{
namespace hwconfig = armarx::control::hardware_config;
+
struct DataConfig
{
- DataConfig(hwconfig::DeviceConfig& deviceConfig, hwconfig::DeviceConfigBase& subDeviceConfig)
- : motorTemperature(deviceConfig.getLinearConfig("motorTemperature")),
- relativeEncoder(subDeviceConfig.getLinearConfig("relativeEncoder")),
- motorCurrent(subDeviceConfig.getLinearConfig("motorCurrent")),
- maxPwm(subDeviceConfig.getUint("maxPwm")),
- Kp(subDeviceConfig.getFloat("Kp"))
+ DataConfig(hwconfig::DeviceConfig& deviceConfig,
+ hwconfig::DeviceConfigBase& subDeviceConfig) :
+ motorTemperature(deviceConfig.getLinearConfig("motorTemperature")),
+ relativeEncoder(subDeviceConfig.getLinearConfig("relativeEncoder")),
+ motorCurrent(subDeviceConfig.getLinearConfig("motorCurrent")),
+ maxPwm(subDeviceConfig.getUint("maxPwm")),
+ Kp(subDeviceConfig.getFloat("Kp"))
{
-
}
+
hwconfig::types::LinearConfig motorTemperature;
hwconfig::types::LinearConfig relativeEncoder;
hwconfig::types::LinearConfig motorCurrent;
std::uint32_t maxPwm;
float Kp;
};
-}
+} // namespace devices::ethercat::hand::armar6_v2
diff --git a/source/devices/ethercat/hand/armar6_v2/Data.cpp b/source/devices/ethercat/hand/armar6_v2/Data.cpp
index aff78864..955e735a 100644
--- a/source/devices/ethercat/hand/armar6_v2/Data.cpp
+++ b/source/devices/ethercat/hand/armar6_v2/Data.cpp
@@ -26,40 +26,44 @@
// armarx
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
+#include <ArmarXCore/core/logging/Logging.h>
namespace devices::ethercat::hand::armar6_v2
{
- Data::Data(const DataConfig& config,
- SlaveOut* sensorOUT, SlaveIn* sensorIN,
- bool isThumb) :
- isThumb(isThumb),
- sensorOUT(sensorOUT),
- sensorIN(sensorIN)
+ Data::Data(const DataConfig& config, SlaveOut* sensorOUT, SlaveIn* sensorIN, bool isThumb) :
+ isThumb(isThumb), sensorOUT(sensorOUT), sensorIN(sensorIN)
{
maxPWM = config.maxPwm;
ARMARX_INFO << "maxPwmTag : " << maxPWM;
kp = config.Kp;
ARMARX_CHECK_EXPRESSION(sensorOUT);
- motorTemperature.init(isThumb ? &sensorOUT->motor_thumb_temperature : &sensorOUT->motor_fingers_temperature, config.motorTemperature);
+ motorTemperature.init(isThumb ? &sensorOUT->motor_thumb_temperature
+ : &sensorOUT->motor_fingers_temperature,
+ config.motorTemperature);
- relativeEncoder.init(isThumb ? &sensorOUT->relative_encoder_value_thumb : &sensorOUT->relative_encoder_value_fingers, config.relativeEncoder);
+ relativeEncoder.init(isThumb ? &sensorOUT->relative_encoder_value_thumb
+ : &sensorOUT->relative_encoder_value_fingers,
+ config.relativeEncoder);
- motorCurrent.init(isThumb ? &sensorOUT->motor_current_value_thumb : &sensorOUT->motor_current_value_fingers, config.motorCurrent);
+ motorCurrent.init(isThumb ? &sensorOUT->motor_current_value_thumb
+ : &sensorOUT->motor_current_value_fingers,
+ config.motorCurrent);
actual_pwm_ptr = isThumb ? &sensorOUT->pwm_thumb_motor : &sensorOUT->pwm_finger_motor;
- actual_direction_ptr = isThumb ? &sensorOUT->direction_thumb_motor : &sensorOUT->direction_finger_motor;
+ actual_direction_ptr =
+ isThumb ? &sensorOUT->direction_thumb_motor : &sensorOUT->direction_finger_motor;
- direction_motor_ptr = isThumb ? &sensorIN->direction_thumb_motor : &sensorIN->direction_finger_motor;
+ direction_motor_ptr =
+ isThumb ? &sensorIN->direction_thumb_motor : &sensorIN->direction_finger_motor;
target_pwm_ptr = isThumb ? &sensorIN->pwm_thumb_state : &sensorIN->pwm_finger_state;
}
-
- void Data::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ Data::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
motorTemperature.read();
relativeEncoder.read();
@@ -71,8 +75,9 @@ namespace devices::ethercat::hand::armar6_v2
}
}
-
- void Data::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ Data::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
*direction_motor_ptr = direction_motor;
*target_pwm_ptr = target_pwm_motor;
@@ -83,15 +88,15 @@ namespace devices::ethercat::hand::armar6_v2
}
}
-
- void Data::setCommand(std::int16_t pwm)
+ void
+ Data::setCommand(std::int16_t pwm)
{
direction_motor = pwm > 0 ? 1 : 0;
target_pwm_motor = std::abs(pwm);
}
-
- void Data::updateSensorValueStruct(SensorDataValue& data)
+ void
+ Data::updateSensorValueStruct(SensorDataValue& data)
{
ARMARX_CHECK_EXPRESSION(actual_pwm_ptr);
ARMARX_CHECK_EXPRESSION(actual_direction_ptr);
@@ -104,28 +109,28 @@ namespace devices::ethercat::hand::armar6_v2
data.motorCurrent = motorCurrent.value;
}
-
- float Data::getMotorTemperature() const
+ float
+ Data::getMotorTemperature() const
{
return motorTemperature.value;
}
-
- float Data::getRelativeEncoderValue() const
+ float
+ Data::getRelativeEncoderValue() const
{
return relativeEncoder.value;
}
-
- unsigned int Data::getMaxPWM() const
+ unsigned int
+ Data::getMaxPWM() const
{
return maxPWM;
}
-
- float Data::getKp() const
+ float
+ Data::getKp() const
{
return kp;
}
-}
+} // namespace devices::ethercat::hand::armar6_v2
diff --git a/source/devices/ethercat/hand/armar6_v2/Data.h b/source/devices/ethercat/hand/armar6_v2/Data.h
index ea051442..05727d90 100644
--- a/source/devices/ethercat/hand/armar6_v2/Data.h
+++ b/source/devices/ethercat/hand/armar6_v2/Data.h
@@ -29,22 +29,20 @@
#include <memory>
// armarx
-#include <armarx/control/ethercat/DataInterface.h>
-#include <armarx/control/rt_filters/AverageFilter.h>
-#include <armarx/control/rt_filters/FirFilter.h>
-
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h>
#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h>
#include <RobotAPI/libraries/core/Pose.h>
+#include <armarx/control/ethercat/DataInterface.h>
+#include <armarx/control/rt_filters/AverageFilter.h>
+#include <armarx/control/rt_filters/FirFilter.h>
// robot_devices
#include <devices/ethercat/common/elmo/gold/Data.h>
-#include <devices/ethercat/hand/armar6_v2/SlaveIO.h>
#include <devices/ethercat/hand/armar6_v2/Config.h>
-
+#include <devices/ethercat/hand/armar6_v2/SlaveIO.h>
namespace devices::ethercat::hand::armar6_v2
{
@@ -55,10 +53,9 @@ namespace devices::ethercat::hand::armar6_v2
{
public:
-
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- float target_pwm_motor;
+ float target_pwm_motor;
float direction_motor;
float actual_pwm;
@@ -66,7 +63,8 @@ namespace devices::ethercat::hand::armar6_v2
float motorCurrent;
- static SensorValueInfo<SensorDataValue> GetClassMemberInfo()
+ static SensorValueInfo<SensorDataValue>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorDataValue> svi;
svi.addBaseClass<SensorValue1DoFActuatorPosition>();
@@ -79,21 +77,18 @@ namespace devices::ethercat::hand::armar6_v2
return svi;
}
-
};
-
- class Data :
- public armarx::control::ethercat::DataInterface
+ class Data : public armarx::control::ethercat::DataInterface
{
public:
+ Data(const DataConfig& config, SlaveOut* sensorOUT, SlaveIn* sensorIN, bool isThumb);
- Data(const DataConfig& config,
- SlaveOut* sensorOUT, SlaveIn* sensorIN, bool isThumb);
-
- void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
- void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+ void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
void setCommand(std::int16_t pwm);
void updateSensorValueStruct(SensorDataValue& data);
@@ -106,7 +101,6 @@ namespace devices::ethercat::hand::armar6_v2
float getKp() const;
private:
-
bool isThumb;
unsigned int maxPWM;
float kp;
@@ -129,7 +123,6 @@ namespace devices::ethercat::hand::armar6_v2
// sensor values in etherCAT buffer
std::uint8_t* actual_pwm_ptr = NULL;
std::uint8_t* actual_direction_ptr = NULL;
-
};
-}
+} // namespace devices::ethercat::hand::armar6_v2
diff --git a/source/devices/ethercat/hand/armar6_v2/Device.h b/source/devices/ethercat/hand/armar6_v2/Device.h
index 54741d97..ba350043 100644
--- a/source/devices/ethercat/hand/armar6_v2/Device.h
+++ b/source/devices/ethercat/hand/armar6_v2/Device.h
@@ -36,7 +36,6 @@
#include <devices/ethercat/hand/armar6_v2/joint_controller/StopMovement.h>
#include <devices/ethercat/hand/common/AbstractHand.h>
-
namespace devices::ethercat::hand::armar6_v2
{
@@ -63,6 +62,7 @@ namespace devices::ethercat::hand::armar6_v2
public:
RobotUnitDevice(const std::string& deviceName);
+
const armarx::SensorValueBase*
getSensorValue() const override
{
diff --git a/source/devices/ethercat/hand/armar6_v2/Slave.cpp b/source/devices/ethercat/hand/armar6_v2/Slave.cpp
index d5f48735..02e4c782 100644
--- a/source/devices/ethercat/hand/armar6_v2/Slave.cpp
+++ b/source/devices/ethercat/hand/armar6_v2/Slave.cpp
@@ -7,7 +7,6 @@
#include <devices/ethercat/common/h2t_devices.h>
-
namespace devices::ethercat::hand::armar6_v2
{
@@ -20,13 +19,11 @@ namespace devices::ethercat::hand::armar6_v2
setTag(this->slaveIdentifier.getName());
}
-
void
Slave::execute()
{
}
-
bool
Slave::prepareForRun()
{
@@ -35,58 +32,49 @@ namespace devices::ethercat::hand::armar6_v2
return true;
}
-
bool
Slave::shutdown()
{
return true;
}
-
void
Slave::doMappings()
{
}
-
bool
Slave::hasError()
{
return false;
}
-
void
Slave::prepareForSafeOp()
{
}
-
void
Slave::finishPreparingForSafeOp()
{
}
-
void
Slave::prepareForOp()
{
}
-
void
Slave::finishPreparingForOp()
{
}
-
std::string
Slave::getDefaultName()
{
return "Armar6HandV2";
}
-
bool
Slave::isSlaveIdentifierAccepted(const armarx::control::ethercat::SlaveIdentifier& sid)
{
diff --git a/source/devices/ethercat/hand/armar6_v2/Slave.h b/source/devices/ethercat/hand/armar6_v2/Slave.h
index 887ca321..3cb98513 100644
--- a/source/devices/ethercat/hand/armar6_v2/Slave.h
+++ b/source/devices/ethercat/hand/armar6_v2/Slave.h
@@ -10,7 +10,6 @@
#include <devices/ethercat/hand/armar6_v2/SlaveIO.h>
-
namespace devices::ethercat::hand::armar6_v2
{
diff --git a/source/devices/ethercat/hand/armar6_v2/SlaveIO.h b/source/devices/ethercat/hand/armar6_v2/SlaveIO.h
index d1eba01f..ea33beae 100644
--- a/source/devices/ethercat/hand/armar6_v2/SlaveIO.h
+++ b/source/devices/ethercat/hand/armar6_v2/SlaveIO.h
@@ -4,7 +4,6 @@
// SDT/STL
#include <cstdint>
-
namespace devices::ethercat::hand::armar6_v2
{
@@ -34,7 +33,6 @@ namespace devices::ethercat::hand::armar6_v2
} __attribute__((__packed__));
-
/**
* @brief PDO mapping master->sensorB
*/
@@ -48,4 +46,4 @@ namespace devices::ethercat::hand::armar6_v2
} __attribute__((__packed__));
-}
+} // namespace devices::ethercat::hand::armar6_v2
diff --git a/source/devices/ethercat/hand/armar6_v2/joint_controller/EmergencyStop.cpp b/source/devices/ethercat/hand/armar6_v2/joint_controller/EmergencyStop.cpp
index 3f3670b9..8f3831da 100644
--- a/source/devices/ethercat/hand/armar6_v2/joint_controller/EmergencyStop.cpp
+++ b/source/devices/ethercat/hand/armar6_v2/joint_controller/EmergencyStop.cpp
@@ -1,32 +1,29 @@
#include <devices/ethercat/hand/armar6_v2/joint_controller/EmergencyStop.h>
-
namespace devices::ethercat::hand::armar6_v2
{
- EmergencyStopController::EmergencyStopController(Data* dataPtr) :
- dataPtr(dataPtr)
+ EmergencyStopController::EmergencyStopController(Data* dataPtr) : dataPtr(dataPtr)
{
-
}
-
- void EmergencyStopController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ EmergencyStopController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
-
}
-
- armarx::ControlTargetBase* EmergencyStopController::getControlTarget()
+ armarx::ControlTargetBase*
+ EmergencyStopController::getControlTarget()
{
return ⌖
}
-
- void EmergencyStopController::rtPreActivateController()
+ void
+ EmergencyStopController::rtPreActivateController()
{
ARMARX_INFO << "Stopping hand!";
dataPtr->setCommand(0);
}
-} // namespace armarx
+} // namespace devices::ethercat::hand::armar6_v2
diff --git a/source/devices/ethercat/hand/armar6_v2/joint_controller/EmergencyStop.h b/source/devices/ethercat/hand/armar6_v2/joint_controller/EmergencyStop.h
index 788fc39d..32bfdd34 100644
--- a/source/devices/ethercat/hand/armar6_v2/joint_controller/EmergencyStop.h
+++ b/source/devices/ethercat/hand/armar6_v2/joint_controller/EmergencyStop.h
@@ -7,23 +7,20 @@
// robot_devices
#include <devices/ethercat/hand/armar6_v2/Data.h>
-
namespace devices::ethercat::hand::armar6_v2
{
using EmergencyStopControllerPtr = std::shared_ptr<class EmergencyStopController>;
-
class EmergencyStopController : public armarx::JointController
{
public:
-
EmergencyStopController(Data* dataPtr);
private:
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
/**
* Returns the Target for this controller, but as this is the Emergency controller it will ignored.
@@ -35,10 +32,8 @@ namespace devices::ethercat::hand::armar6_v2
void rtPreActivateController() override;
private:
-
armarx::DummyControlTargetEmergencyStop target;
Data* dataPtr;
-
};
-} // namespace
+} // namespace devices::ethercat::hand::armar6_v2
diff --git a/source/devices/ethercat/hand/armar6_v2/joint_controller/PWM.cpp b/source/devices/ethercat/hand/armar6_v2/joint_controller/PWM.cpp
index 9992499f..028bb76f 100644
--- a/source/devices/ethercat/hand/armar6_v2/joint_controller/PWM.cpp
+++ b/source/devices/ethercat/hand/armar6_v2/joint_controller/PWM.cpp
@@ -1,24 +1,21 @@
#include <devices/ethercat/hand/armar6_v2/joint_controller/PWM.h>
-
namespace devices::ethercat::hand::armar6_v2
{
- PWMController::PWMController(Data* data, Device* hand) :
- data(data),
- hand(hand)
+ PWMController::PWMController(Data* data, Device* hand) : data(data), hand(hand)
{
-
}
-
- armarx::ControlTargetBase* PWMController::getControlTarget()
+ armarx::ControlTargetBase*
+ PWMController::getControlTarget()
{
return ⌖
}
-
- void PWMController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ PWMController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (target.isValid())
{
@@ -28,15 +25,15 @@ namespace devices::ethercat::hand::armar6_v2
}
}
-
- void PWMController::rtPreActivateController()
+ void
+ PWMController::rtPreActivateController()
{
}
-
- void PWMController::rtPostDeactivateController()
+ void
+ PWMController::rtPostDeactivateController()
{
data->setCommand(0);
}
-} // namespace armarx
+} // namespace devices::ethercat::hand::armar6_v2
diff --git a/source/devices/ethercat/hand/armar6_v2/joint_controller/PWM.h b/source/devices/ethercat/hand/armar6_v2/joint_controller/PWM.h
index 583278d9..ff7367bf 100644
--- a/source/devices/ethercat/hand/armar6_v2/joint_controller/PWM.h
+++ b/source/devices/ethercat/hand/armar6_v2/joint_controller/PWM.h
@@ -5,9 +5,8 @@
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
// robot_devices
-#include <devices/ethercat/hand/armar6_v2/Device.h>
#include <devices/ethercat/hand/armar6_v2/Data.h>
-
+#include <devices/ethercat/hand/armar6_v2/Device.h>
namespace devices::ethercat::hand::armar6_v2
{
@@ -17,31 +16,32 @@ namespace devices::ethercat::hand::armar6_v2
static const std::string HandV2Pwm = "ControlMode_HandPwm";
}
-
- class PWMControlTarget :
- public armarx::ControlTargetBase
+ class PWMControlTarget : public armarx::ControlTargetBase
{
public:
-
std::int16_t pwm_motor;
- const std::string& getControlMode() const override
+ const std::string&
+ getControlMode() const override
{
return ctrl_modes::HandV2Pwm;
}
- void reset() override
+ void
+ reset() override
{
pwm_motor = std::numeric_limits<int16_t>::max();
}
- bool isValid() const override
+ bool
+ isValid() const override
{
- return std::abs(pwm_motor) < 256 ;
+ return std::abs(pwm_motor) < 256;
}
- static ControlTargetInfo<PWMControlTarget> GetClassMemberInfo()
+ static ControlTargetInfo<PWMControlTarget>
+ GetClassMemberInfo()
{
ControlTargetInfo<PWMControlTarget> cti;
cti.addMemberVariable(&PWMControlTarget::pwm_motor, "pwm_motor");
@@ -49,40 +49,32 @@ namespace devices::ethercat::hand::armar6_v2
}
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
-
};
-
- class PWMController :
- public armarx::JointController
+ class PWMController : public armarx::JointController
{
public:
-
PWMController(Data* data, Device* hand);
// JointController interface
public:
-
armarx::ControlTargetBase* getControlTarget() override;
protected:
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
void rtPreActivateController() override;
void rtPostDeactivateController() override;
private:
-
Data* data;
Device* hand;
PWMControlTarget target;
-
};
-
using PWMControllerPtr = std::shared_ptr<PWMController>;
-} // namespace armarx
+} // namespace devices::ethercat::hand::armar6_v2
diff --git a/source/devices/ethercat/hand/armar6_v2/joint_controller/Position.cpp b/source/devices/ethercat/hand/armar6_v2/joint_controller/Position.cpp
index 06dfe7ab..2cc8349a 100644
--- a/source/devices/ethercat/hand/armar6_v2/joint_controller/Position.cpp
+++ b/source/devices/ethercat/hand/armar6_v2/joint_controller/Position.cpp
@@ -4,52 +4,53 @@
// armarx
#include <RobotAPI/libraries/core/math/MathUtils.h>
-
namespace devices::ethercat::hand::armar6_v2
{
PositionController::PositionController(Data* data, Device* hand) :
- data(data),
- hand(hand),
- PID(data->getKp(), 0.0f, 0.0f)
+ data(data), hand(hand), PID(data->getKp(), 0.0f, 0.0f)
{
// thumbPID.threadSafe = false;
PID.threadSafe = false;
}
-
- armarx::ControlTargetBase* PositionController::getControlTarget()
+ armarx::ControlTargetBase*
+ PositionController::getControlTarget()
{
return ⌖
}
-
- void PositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ PositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (target.isValid())
{
int maxPWM = std::min<int>(data->getMaxPWM(), target.maxPWM);
// thumbPID.update(timeSinceLastIteration.toSecondsDouble(), data->getRelativeEncoderValue(), target.TargetPosition);
- PID.update(timeSinceLastIteration.toSecondsDouble(), data->getRelativeEncoderValue(), target.position);
+ PID.update(timeSinceLastIteration.toSecondsDouble(),
+ data->getRelativeEncoderValue(),
+ target.position);
// ARMARX_IMPORTANT << "rtRun: " << VAROUT(target.pwm_finger_motor) << " " << VAROUT(target.pwm_thumb_motor);
- std::int16_t TargetPWM = armarx::math::MathUtils::LimitMinMax(-maxPWM, maxPWM, int(PID.getControlValue() * maxPWM));
+ std::int16_t TargetPWM = armarx::math::MathUtils::LimitMinMax(
+ -maxPWM, maxPWM, int(PID.getControlValue() * maxPWM));
// int16 thumbTargetPWM = math::MathUtils::LimitMinMax(-target.maxThumbPWM, target.maxThumbPWM, int(thumbPID.getControlValue() * target.maxThumbPWM));
// ARMARX_RT_LOGF_INFO("target pwm: %d, target.maxPWM: %d, target.position: %.2f, current position: %.2f kp: %.2f", TargetPWM, maxPWM, target.position, data->getRelativeEncoderValue(), PID.Kp).deactivateSpam(0.5);
data->setCommand(TargetPWM);
}
}
-
- void PositionController::rtPreActivateController()
+ void
+ PositionController::rtPreActivateController()
{
}
-
- void PositionController::rtPostDeactivateController()
+ void
+ PositionController::rtPostDeactivateController()
{
data->setCommand(0);
}
-} // namespace armarx
+} // namespace devices::ethercat::hand::armar6_v2
diff --git a/source/devices/ethercat/hand/armar6_v2/joint_controller/Position.h b/source/devices/ethercat/hand/armar6_v2/joint_controller/Position.h
index 1835db1a..48d4f48f 100644
--- a/source/devices/ethercat/hand/armar6_v2/joint_controller/Position.h
+++ b/source/devices/ethercat/hand/armar6_v2/joint_controller/Position.h
@@ -2,14 +2,13 @@
// armarx
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/libraries/core/PIDController.h>
// robot_devices
-#include <devices/ethercat/hand/armar6_v2/Device.h>
#include <devices/ethercat/hand/armar6_v2/Data.h>
-
+#include <devices/ethercat/hand/armar6_v2/Device.h>
namespace devices::ethercat::hand::armar6_v2
{
@@ -19,26 +18,27 @@ namespace devices::ethercat::hand::armar6_v2
// static const std::string HandV2PositionControl = "ControlMode_HandPositionControl";
}
- class PositionControlTarget :
- public armarx::ControlTarget1DoFActuatorPosition
+ class PositionControlTarget : public armarx::ControlTarget1DoFActuatorPosition
{
public:
-
std::int16_t maxPWM;
- void reset() override
+ void
+ reset() override
{
maxPWM = 255;
ControlTarget1DoFActuatorPosition::reset();
}
- bool isValid() const override
+ bool
+ isValid() const override
{
return std::abs(maxPWM) < 256 && ControlTarget1DoFActuatorPosition::isValid();
}
- static ControlTargetInfo<PositionControlTarget> GetClassMemberInfo()
+ static ControlTargetInfo<PositionControlTarget>
+ GetClassMemberInfo()
{
ControlTargetInfo<PositionControlTarget> cti;
cti.addBaseClass<ControlTarget1DoFActuatorPosition>();
@@ -47,40 +47,32 @@ namespace devices::ethercat::hand::armar6_v2
}
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
-
};
-
- class PositionController :
- public armarx::JointController
+ class PositionController : public armarx::JointController
{
public:
-
PositionController(Data* data, Device* hand);
// JointController interface
public:
-
armarx::ControlTargetBase* getControlTarget() override;
protected:
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
void rtPreActivateController() override;
void rtPostDeactivateController() override;
private:
-
Data* data;
Device* hand;
armarx::PIDController PID;
PositionControlTarget target;
-
};
-
using PositionControllerPtr = std::shared_ptr<PositionController>;
-} // namespace armarx
+} // namespace devices::ethercat::hand::armar6_v2
diff --git a/source/devices/ethercat/hand/armar6_v2/joint_controller/StopMovement.cpp b/source/devices/ethercat/hand/armar6_v2/joint_controller/StopMovement.cpp
index 286e7514..971c8bbf 100644
--- a/source/devices/ethercat/hand/armar6_v2/joint_controller/StopMovement.cpp
+++ b/source/devices/ethercat/hand/armar6_v2/joint_controller/StopMovement.cpp
@@ -1,32 +1,29 @@
#include <devices/ethercat/hand/armar6_v2/joint_controller/StopMovement.h>
-
namespace devices::ethercat::hand::armar6_v2
{
- StopMovementController::StopMovementController(Data* dataPtr) :
- dataPtr(dataPtr)
+ StopMovementController::StopMovementController(Data* dataPtr) : dataPtr(dataPtr)
{
-
}
-
- void StopMovementController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ StopMovementController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
-
}
-
- armarx::ControlTargetBase* StopMovementController::getControlTarget()
+ armarx::ControlTargetBase*
+ StopMovementController::getControlTarget()
{
return ⌖
}
-
- void StopMovementController::rtPreActivateController()
+ void
+ StopMovementController::rtPreActivateController()
{
ARMARX_RT_LOGF_INFO("Stopping hand!");
dataPtr->setCommand(0);
}
-} // namespace armarx
+} // namespace devices::ethercat::hand::armar6_v2
diff --git a/source/devices/ethercat/hand/armar6_v2/joint_controller/StopMovement.h b/source/devices/ethercat/hand/armar6_v2/joint_controller/StopMovement.h
index ab57c9bd..2dfdc158 100644
--- a/source/devices/ethercat/hand/armar6_v2/joint_controller/StopMovement.h
+++ b/source/devices/ethercat/hand/armar6_v2/joint_controller/StopMovement.h
@@ -7,23 +7,20 @@
// robot_devices
#include <devices/ethercat/hand/armar6_v2/Data.h>
-
namespace devices::ethercat::hand::armar6_v2
{
using StopMovementControllerPtr = std::shared_ptr<class StopMovementController>;
-
class StopMovementController : public armarx::JointController
{
public:
-
StopMovementController(Data* dataPtr);
private:
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
/**
* Returns the Target for this controller, but as this is the Emergency controller it will ignored.
@@ -35,10 +32,8 @@ namespace devices::ethercat::hand::armar6_v2
void rtPreActivateController() override;
private:
-
armarx::DummyControlTargetStopMovement target;
Data* dataPtr;
-
};
-} // namespace armarx
+} // namespace devices::ethercat::hand::armar6_v2
diff --git a/source/devices/ethercat/hand/armar6_v2/njoint_controller/Shape.cpp b/source/devices/ethercat/hand/armar6_v2/njoint_controller/Shape.cpp
index 751bba65..4f63c813 100644
--- a/source/devices/ethercat/hand/armar6_v2/njoint_controller/Shape.cpp
+++ b/source/devices/ethercat/hand/armar6_v2/njoint_controller/Shape.cpp
@@ -8,12 +8,11 @@
#include <devices/ethercat/hand/armar6_v2/Data.h>
#include <devices/ethercat/hand/armar6_v2/joint_controller/PWM.h>
-
namespace devices::ethercat::hand::armar6_v2
{
- armarx::NJointControllerRegistration<ShapeController> registrationControllerNJointKITHandV2ShapeController("NJointKITHandV2ShapeController");
-
+ armarx::NJointControllerRegistration<ShapeController>
+ registrationControllerNJointKITHandV2ShapeController("NJointKITHandV2ShapeController");
ShapeController::ShapeController(armarx::RobotUnitPtr prov,
ShapeControllerConfigPtr config,
@@ -43,21 +42,21 @@ namespace devices::ethercat::hand::armar6_v2
fingersControlTarget = ct->asA<PWMControlTarget>();
}
-
- void ShapeController::rtPreActivateController()
+ void
+ ShapeController::rtPreActivateController()
{
this->fingersTarget = fingersSensorValues->position;
this->thumbTarget = thumbSensorValues->position;
}
-
- void ShapeController::stopMotion()
+ void
+ ShapeController::stopMotion()
{
enableControl = false;
}
-
- void ShapeController::setConfigs(const DataConfig& fingersConfig, const DataConfig& thumbConfig)
+ void
+ ShapeController::setConfigs(const DataConfig& fingersConfig, const DataConfig& thumbConfig)
{
fingersMaxPwm = fingersConfig.maxPwm;
thumbMaxPwm = thumbConfig.maxPwm;
@@ -67,56 +66,57 @@ namespace devices::ethercat::hand::armar6_v2
thumbPwmLimit = thumbMaxPwm;
}
-
/*bool NJointKITHandV2ShapeController::hasQueuedTarget()
{
return targetFingerState != queuedFingerState || targetThumbState != queuedThumbState;
}*/
- std::int16_t ShapeController::getFingersPwm()
+ std::int16_t
+ ShapeController::getFingersPwm()
{
return fingersPwm;
}
-
- std::int16_t ShapeController::getThumbPwm()
+ std::int16_t
+ ShapeController::getThumbPwm()
{
return thumbPwm;
}
-
- float ShapeController::getFingersTarget()
+ float
+ ShapeController::getFingersTarget()
{
return fingersTarget;
}
-
- float ShapeController::getThumbTarget()
+ float
+ ShapeController::getThumbTarget()
{
return thumbTarget;
}
-
- float ShapeController::getFingersJointValue() const
+ float
+ ShapeController::getFingersJointValue() const
{
return actualFingersJointValue;
}
-
- float ShapeController::getThumbJointValue() const
+ float
+ ShapeController::getThumbJointValue() const
{
return actualThumbJointValue;
}
-
- bool ShapeController::isControlEnabled()
+ bool
+ ShapeController::isControlEnabled()
{
return enableControl;
}
-
- void ShapeController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ ShapeController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
actualFingersJointValue = fingersSensorValues->position;
@@ -127,8 +127,10 @@ namespace devices::ethercat::hand::armar6_v2
float fingersError = fingersTarget - actualFingersJointValue;
float thumbError = thumbTarget - actualThumbJointValue;
- fingersPwm = armarx::math::MathUtils::LimitMinMax(-fingersMaxPwm, fingersMaxPwm, (int)(KpFingers * fingersMaxPwm * fingersError));
- thumbPwm = armarx::math::MathUtils::LimitMinMax(-thumbMaxPwm, thumbMaxPwm, (int)(KpThumb * thumbMaxPwm * thumbError));
+ fingersPwm = armarx::math::MathUtils::LimitMinMax(
+ -fingersMaxPwm, fingersMaxPwm, (int)(KpFingers * fingersMaxPwm * fingersError));
+ thumbPwm = armarx::math::MathUtils::LimitMinMax(
+ -thumbMaxPwm, thumbMaxPwm, (int)(KpThumb * thumbMaxPwm * thumbError));
}
else
{
@@ -141,8 +143,8 @@ namespace devices::ethercat::hand::armar6_v2
thumbControlTarget->pwm_motor = thumbPwm;
}
-
- armarx::WidgetDescription::StringWidgetDictionary ShapeController::getFunctionDescriptions(const Ice::Current&) const
+ armarx::WidgetDescription::StringWidgetDictionary
+ ShapeController::getFunctionDescriptions(const Ice::Current&) const
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr hbox = new HBoxLayout;
@@ -186,8 +188,10 @@ namespace devices::ethercat::hand::armar6_v2
return {{"Targets", hbox}};
}
-
- void ShapeController::callDescribedFunction(const std::string& name, const armarx::StringVariantBaseMap& valueMap, const Ice::Current&)
+ void
+ ShapeController::callDescribedFunction(const std::string& name,
+ const armarx::StringVariantBaseMap& valueMap,
+ const Ice::Current&)
{
if (name == "Targets")
{
@@ -203,24 +207,30 @@ namespace devices::ethercat::hand::armar6_v2
}
}
-
- void ShapeController::setTargets(float fingers, float thumb, const Ice::Current&)
+ void
+ ShapeController::setTargets(float fingers, float thumb, const Ice::Current&)
{
setTargetsWithPwm(fingers, thumb, 1, 1);
}
-
- void ShapeController::setTargetsWithPwm(float fingers, float thumb, float fingersRelativeMaxPwm, float thumbRelativeMaxPwm, const Ice::Current&)
+ void
+ ShapeController::setTargetsWithPwm(float fingers,
+ float thumb,
+ float fingersRelativeMaxPwm,
+ float thumbRelativeMaxPwm,
+ const Ice::Current&)
{
this->fingersTarget = armarx::math::MathUtils::LimitMinMax(0.f, 2.f, fingers);
this->thumbTarget = armarx::math::MathUtils::LimitMinMax(0.f, 2.f, thumb);
- this->fingersMaxPwm = armarx::math::MathUtils::LimitMinMax(0, fingersPwmLimit, (int)(fingersPwmLimit * fingersRelativeMaxPwm));
- this->thumbMaxPwm = armarx::math::MathUtils::LimitMinMax(0, thumbPwmLimit, (int)(thumbPwmLimit * thumbRelativeMaxPwm));
+ this->fingersMaxPwm = armarx::math::MathUtils::LimitMinMax(
+ 0, fingersPwmLimit, (int)(fingersPwmLimit * fingersRelativeMaxPwm));
+ this->thumbMaxPwm = armarx::math::MathUtils::LimitMinMax(
+ 0, thumbPwmLimit, (int)(thumbPwmLimit * thumbRelativeMaxPwm));
enableControl = true;
}
-
- JointValues ShapeController::getJointValues(const Ice::Current&)
+ JointValues
+ ShapeController::getJointValues(const Ice::Current&)
{
JointValues values;
values.fingersJointValue = actualFingersJointValue;
@@ -228,4 +238,4 @@ namespace devices::ethercat::hand::armar6_v2
return values;
}
-} // namespace armarx
+} // namespace devices::ethercat::hand::armar6_v2
diff --git a/source/devices/ethercat/hand/armar6_v2/njoint_controller/Shape.h b/source/devices/ethercat/hand/armar6_v2/njoint_controller/Shape.h
index 312a8e87..65991755 100644
--- a/source/devices/ethercat/hand/armar6_v2/njoint_controller/Shape.h
+++ b/source/devices/ethercat/hand/armar6_v2/njoint_controller/Shape.h
@@ -10,47 +10,40 @@
#include <devices/ethercat/hand/armar6_v2/njoint_controller/Shape.h>
#include <devices/ethercat/hand/armar6_v2/njoint_controller/ShapeInterface.h>
-
namespace devices::ethercat::hand::armar6_v2
{
using ShapeControllerConfigPtr = IceUtil::Handle<class ShapeControllerConfig>;
-
- class ShapeControllerConfig :
- virtual public armarx::NJointControllerConfig
+ class ShapeControllerConfig : virtual public armarx::NJointControllerConfig
{
public:
-
- ShapeControllerConfig(std::string const& deviceName):
- deviceName(deviceName)
- {}
+ ShapeControllerConfig(std::string const& deviceName) : deviceName(deviceName)
+ {
+ }
const std::string deviceName;
-
};
using ShapeControllerPtr = IceInternal::Handle<class ShapeController>;
-
- class ShapeController :
- public armarx::NJointController,
- public ShapeControllerInterface
+ class ShapeController : public armarx::NJointController, public ShapeControllerInterface
{
public:
-
using ConfigPtrT = ShapeControllerConfigPtr;
- ShapeController(armarx::RobotUnitPtr prov, ShapeControllerConfigPtr config, const VirtualRobot::RobotPtr& r);
+ ShapeController(armarx::RobotUnitPtr prov,
+ ShapeControllerConfigPtr config,
+ const VirtualRobot::RobotPtr& r);
// NJointControllerInterface interface
public:
-
- std::string getClassName(const Ice::Current&) const override
+ std::string
+ getClassName(const Ice::Current&) const override
{
return "NJointKITHandV2ShapeController";
}
@@ -70,10 +63,10 @@ namespace devices::ethercat::hand::armar6_v2
bool isControlEnabled();
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
private:
-
const SensorDataValue* thumbSensorValues;
const SensorDataValue* fingersSensorValues;
PWMControlTarget* thumbControlTarget;
@@ -99,23 +92,27 @@ namespace devices::ethercat::hand::armar6_v2
bool enableControl = false;
public:
-
- armarx::WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
- void callDescribedFunction(const std::string& name, const armarx::StringVariantBaseMap& valueMap, const Ice::Current&) override;
+ armarx::WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current&) const override;
+ void callDescribedFunction(const std::string& name,
+ const armarx::StringVariantBaseMap& valueMap,
+ const Ice::Current&) override;
// NJointKITHandV2ShapeControllerInterface interface
public:
-
- void setTargets(float fingers, float thumb, const Ice::Current& = Ice::emptyCurrent) override;
- void setTargetsWithPwm(float fingers, float thumb, float fingersRelativeMaxPwm, float thumbRelativeMaxPwm, const Ice::Current& = Ice::emptyCurrent) override;
+ void
+ setTargets(float fingers, float thumb, const Ice::Current& = Ice::emptyCurrent) override;
+ void setTargetsWithPwm(float fingers,
+ float thumb,
+ float fingersRelativeMaxPwm,
+ float thumbRelativeMaxPwm,
+ const Ice::Current& = Ice::emptyCurrent) override;
JointValues getJointValues(const Ice::Current&) override;
// NJointControllerBase interface
protected:
-
void rtPreActivateController() override;
-
};
-}
+} // namespace devices::ethercat::hand::armar6_v2
diff --git a/source/devices/ethercat/hand/armar6_v2/njoint_controller/ShapeInterface.ice b/source/devices/ethercat/hand/armar6_v2/njoint_controller/ShapeInterface.ice
index 685956ac..3c5440c2 100644
--- a/source/devices/ethercat/hand/armar6_v2/njoint_controller/ShapeInterface.ice
+++ b/source/devices/ethercat/hand/armar6_v2/njoint_controller/ShapeInterface.ice
@@ -28,21 +28,31 @@
#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
-module devices { module ethercat { module hand { module armar6_v2
+module devices
{
-
- struct JointValues
- {
- float thumbJointValue;
- float fingersJointValue;
- };
-
-
- interface ShapeControllerInterface extends armarx::NJointControllerInterface
+ module ethercat
{
- void setTargets(float fingers, float thumb);
- void setTargetsWithPwm(float fingers, float thumb, float fingersRelativeMaxPwm, float thumbRelativeMaxPwm);
- JointValues getJointValues();
+ module hand
+ {
+ module armar6_v2
+ {
+
+ struct JointValues
+ {
+ float thumbJointValue;
+ float fingersJointValue;
+ };
+
+ interface ShapeControllerInterface extends armarx::NJointControllerInterface
+ {
+ void setTargets(float fingers, float thumb);
+ void setTargetsWithPwm(float fingers,
+ float thumb,
+ float fingersRelativeMaxPwm,
+ float thumbRelativeMaxPwm);
+ JointValues getJointValues();
+ };
+ };
+ };
};
-
-};};};};
+};
diff --git a/source/devices/ethercat/hand/armar7/Data.cpp b/source/devices/ethercat/hand/armar7/Data.cpp
index f0941db9..22a56571 100644
--- a/source/devices/ethercat/hand/armar7/Data.cpp
+++ b/source/devices/ethercat/hand/armar7/Data.cpp
@@ -6,7 +6,6 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
-
namespace devices::ethercat::hand::armar7
{
@@ -16,7 +15,6 @@ namespace devices::ethercat::hand::armar7
ARMARX_CHECK_EXPRESSION(sensorOUT);
}
-
void
Data::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -31,7 +29,6 @@ namespace devices::ethercat::hand::armar7
N_TOF_CELLS * sizeof(std::uint16_t)); // TODO: Constant
}
-
void
Data::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -39,7 +36,6 @@ namespace devices::ethercat::hand::armar7
;
}
-
void
Data::updateSensorValueStruct(SensorDataValue& data)
{
@@ -60,21 +56,18 @@ namespace devices::ethercat::hand::armar7
}
}
-
std::uint16_t
Data::getMainUpdateRate() const
{
return mainUpdateRate;
}
-
bool
Data::getEmergencyStopStatus() const
{
return emergencyStopStatus;
}
-
std::array<std::uint16_t, 16> // TODO: Constant
Data::getTofDepth() const
{
diff --git a/source/devices/ethercat/hand/armar7/Data.h b/source/devices/ethercat/hand/armar7/Data.h
index 982e9b6f..8f811d5a 100644
--- a/source/devices/ethercat/hand/armar7/Data.h
+++ b/source/devices/ethercat/hand/armar7/Data.h
@@ -18,7 +18,6 @@
#include <devices/ethercat/hand/armar7/ErrorDecoder.h>
#include <devices/ethercat/hand/armar7/SlaveIO.h>
-
namespace devices::ethercat::hand::armar7
{
@@ -53,7 +52,6 @@ namespace devices::ethercat::hand::armar7
}
};
-
class Data : public armarx::control::ethercat::DataInterface
{
diff --git a/source/devices/ethercat/hand/armar7/Device.h b/source/devices/ethercat/hand/armar7/Device.h
index 41e5e557..302f6478 100644
--- a/source/devices/ethercat/hand/armar7/Device.h
+++ b/source/devices/ethercat/hand/armar7/Device.h
@@ -15,7 +15,6 @@
#include <devices/ethercat/hand/armar7de/joint_controller/StopMovement.h>
#include <devices/ethercat/hand/common/AbstractHand.h>
-
namespace devices::ethercat::hand::armar7::sub_device
{
using DeviceInterfacePtr = std::shared_ptr<class DeviceInterface>;
@@ -83,7 +82,6 @@ namespace devices::ethercat::hand::armar7
return motorTemperatureShutdownThreshold_;
}
-
const armarx::SensorValueBase*
getSensorValue() const override
{
diff --git a/source/devices/ethercat/hand/armar7/ErrorDecoder.h b/source/devices/ethercat/hand/armar7/ErrorDecoder.h
index 4ce3f3ab..7955e7a9 100644
--- a/source/devices/ethercat/hand/armar7/ErrorDecoder.h
+++ b/source/devices/ethercat/hand/armar7/ErrorDecoder.h
@@ -5,13 +5,11 @@
#include <devices/ethercat/hand/armar7/SlaveIO.h>
-
namespace armarx::control::ethercat
{
class SlaveInterface;
}
-
namespace devices::ethercat::hand::armar7
{
diff --git a/source/devices/ethercat/hand/armar7/Slave.h b/source/devices/ethercat/hand/armar7/Slave.h
index 781437e9..6cafcf97 100644
--- a/source/devices/ethercat/hand/armar7/Slave.h
+++ b/source/devices/ethercat/hand/armar7/Slave.h
@@ -11,7 +11,6 @@
#include <devices/ethercat/hand/armar7/ErrorDecoder.h>
#include <devices/ethercat/hand/armar7/SlaveIO.h>
-
namespace devices::ethercat::hand::armar7
{
diff --git a/source/devices/ethercat/hand/armar7/njoint_controller/Shape.cpp b/source/devices/ethercat/hand/armar7/njoint_controller/Shape.cpp
index 62035e1a..cc891f0e 100644
--- a/source/devices/ethercat/hand/armar7/njoint_controller/Shape.cpp
+++ b/source/devices/ethercat/hand/armar7/njoint_controller/Shape.cpp
@@ -3,13 +3,12 @@
#include <cmath>
#include "RobotAPI/components/units/RobotUnit/ControlModes.h"
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h> // FIXME avoid this
#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <devices/ethercat/hand/armar7de/joint_controller/AbsolutePosition.h>
#include <devices/ethercat/hand/armar7de/joint_controller/PWM.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h> // FIXME avoid this
-
namespace devices::ethercat::hand::armar7
{
diff --git a/source/devices/ethercat/hand/armar7/sub_device/absolute/Data.cpp b/source/devices/ethercat/hand/armar7/sub_device/absolute/Data.cpp
index 139bfb9d..a888735c 100644
--- a/source/devices/ethercat/hand/armar7/sub_device/absolute/Data.cpp
+++ b/source/devices/ethercat/hand/armar7/sub_device/absolute/Data.cpp
@@ -25,7 +25,6 @@ namespace devices::ethercat::hand::armar7::sub_device::absolute
config.absoluteEncoderMagnetometerZ);
}
-
void
Data::rtReadSensorValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
const IceUtil::Time& /*timeSinceLastIteration*/)
@@ -41,16 +40,16 @@ namespace devices::ethercat::hand::armar7::sub_device::absolute
std::atan2(absoluteEncoderMagnetometerY.value, absoluteEncoderMagnetometerX.value);
const float sign = config.motorPositionIsInverted ? -1.0 : 1.0;
- motorPosition = (absoluteEncoderAngle + config.motorPositionZeroOffset) * sign * config.motorPositionScaling;
+ motorPosition = (absoluteEncoderAngle + config.motorPositionZeroOffset) * sign *
+ config.motorPositionScaling;
motorPosition = simox::math::periodic_clamp<float>(motorPosition, -M_PI, M_PI);
}
-
void
Data::rtWriteTargetValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
const IceUtil::Time& /*timeSinceLastIteration*/)
{
- if(config.pwmIsInverted)
+ if (config.pwmIsInverted)
{
*motorPwmPtr = -motorPwmValue;
}
@@ -60,7 +59,6 @@ namespace devices::ethercat::hand::armar7::sub_device::absolute
}
}
-
void
Data::updateSensorValueStruct(armar7de::sub_device::absolute::SensorDataValue& data)
{
@@ -76,56 +74,48 @@ namespace devices::ethercat::hand::armar7::sub_device::absolute
data.motorPWM = motorPwmValue;
}
-
float
Data::getKp() const
{
return config.Kp;
}
-
std::uint16_t
Data::getMaxPWM() const
{
return config.maxPwm;
}
-
float
Data::getJointLimitLow() const
{
return config.jointLimitLow;
}
-
float
Data::getJointLimitHigh() const
{
return config.jointLimitHigh;
}
-
bool
Data::getMotorError() const
{
return motorError;
}
-
std::uint8_t
Data::getAbsoluteEncoderStatusBits() const
{
return absoluteEncoderStatusBits;
}
-
float
Data::getMotorPosition() const
{
return motorPosition;
}
-
void
Data::setMotorPwmValue(std::int16_t pwm)
{
diff --git a/source/devices/ethercat/hand/armar7/sub_device/absolute/Data.h b/source/devices/ethercat/hand/armar7/sub_device/absolute/Data.h
index d162e56c..07142107 100644
--- a/source/devices/ethercat/hand/armar7/sub_device/absolute/Data.h
+++ b/source/devices/ethercat/hand/armar7/sub_device/absolute/Data.h
@@ -21,7 +21,6 @@
#include <devices/ethercat/hand/armar7de/sub_device/absolute/Config.h>
#include <devices/ethercat/hand/armar7de/sub_device/absolute/Data.h>
-
namespace devices::ethercat::hand::armar7::sub_device::absolute
{
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 aeaa8f18..c4531a24 100644
--- a/source/devices/ethercat/hand/armar7/sub_device/absolute/Device.cpp
+++ b/source/devices/ethercat/hand/armar7/sub_device/absolute/Device.cpp
@@ -5,7 +5,6 @@
#include <devices/ethercat/hand/armar7de/joint_controller/PWM.h>
#include <devices/ethercat/hand/armar7de/joint_controller/StopMovement.h>
-
namespace devices::ethercat::hand::armar7::sub_device::absolute
{
@@ -18,7 +17,6 @@ namespace devices::ethercat::hand::armar7::sub_device::absolute
;
}
-
void
Device::init(devices::ethercat::hand::armar7::Device* hand, Data* dataPtr)
{
@@ -35,7 +33,6 @@ namespace devices::ethercat::hand::armar7::sub_device::absolute
addJointController(positionController.get());
}
-
void
Device::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -50,7 +47,6 @@ namespace devices::ethercat::hand::armar7::sub_device::absolute
dataPtr->updateSensorValueStruct(sensorValue);
}
-
void
Device::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
diff --git a/source/devices/ethercat/hand/armar7/sub_device/absolute/Device.h b/source/devices/ethercat/hand/armar7/sub_device/absolute/Device.h
index 89702dc2..4d312f7b 100644
--- a/source/devices/ethercat/hand/armar7/sub_device/absolute/Device.h
+++ b/source/devices/ethercat/hand/armar7/sub_device/absolute/Device.h
@@ -33,7 +33,6 @@
#include <devices/ethercat/hand/armar7/Slave.h>
#include <devices/ethercat/hand/common/AbstractHand.h>
-
namespace devices::ethercat::hand::armar7de
{
using EmergencyStopControllerPtr = std::shared_ptr<class EmergencyStopController>;
@@ -48,7 +47,6 @@ namespace devices::ethercat::hand::armar7
class Device;
} // namespace devices::ethercat::hand::armar7
-
namespace devices::ethercat::hand::armar7::sub_device::absolute
{
class Device :
diff --git a/source/devices/ethercat/hand/armar7/sub_device/relative/Data.cpp b/source/devices/ethercat/hand/armar7/sub_device/relative/Data.cpp
index 23fa0e22..533cece5 100644
--- a/source/devices/ethercat/hand/armar7/sub_device/relative/Data.cpp
+++ b/source/devices/ethercat/hand/armar7/sub_device/relative/Data.cpp
@@ -8,7 +8,6 @@
#include <devices/ethercat/hand/armar7/Data.h>
-
namespace devices::ethercat::hand::armar7::sub_device::relative
{
@@ -49,7 +48,6 @@ namespace devices::ethercat::hand::armar7::sub_device::relative
}
}
-
void
Data::rtReadSensorValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
const IceUtil::Time& /*timeSinceLastIteration*/)
@@ -60,7 +58,6 @@ namespace devices::ethercat::hand::armar7::sub_device::relative
motorPosition.read();
}
-
void
Data::rtWriteTargetValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
const IceUtil::Time& /*timeSinceLastIteration*/)
@@ -68,7 +65,6 @@ namespace devices::ethercat::hand::armar7::sub_device::relative
*motorPwmPtr = motorPwmValue;
}
-
void
Data::updateSensorValueStruct(armar7de::sub_device::relative::SensorDataValue& data)
{
@@ -80,42 +76,36 @@ namespace devices::ethercat::hand::armar7::sub_device::relative
data.relativeEncoderTicks = motorPosition.getRaw();
}
-
float
Data::getKp() const
{
return config.Kp;
}
-
std::uint16_t
Data::getMaxPWM() const
{
return config.maxPwm;
}
-
bool
Data::getMotorError() const
{
return motorError;
}
-
float
Data::getMotorTemperature() const
{
return motorTemperature.value;
}
-
float
Data::getMotorPosition() const
{
return motorPosition.value;
}
-
void
Data::setMotorPwmValue(std::int16_t pwm)
{
diff --git a/source/devices/ethercat/hand/armar7/sub_device/relative/Data.h b/source/devices/ethercat/hand/armar7/sub_device/relative/Data.h
index 28c76b1a..1889f5d6 100644
--- a/source/devices/ethercat/hand/armar7/sub_device/relative/Data.h
+++ b/source/devices/ethercat/hand/armar7/sub_device/relative/Data.h
@@ -21,7 +21,6 @@
#include <devices/ethercat/hand/armar7de/sub_device/relative/Config.h>
#include <devices/ethercat/hand/armar7de/sub_device/relative/Data.h>
-
namespace devices::ethercat::hand::armar7::sub_device::relative
{
@@ -33,7 +32,6 @@ namespace devices::ethercat::hand::armar7::sub_device::relative
// ThumbCircumduction = 3
};
-
class Data : public armar7de::sub_device::DataInterface
{
using DataConfig = armar7de::sub_device::relative::DataConfig;
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 7b329946..6face24a 100644
--- a/source/devices/ethercat/hand/armar7/sub_device/relative/Device.cpp
+++ b/source/devices/ethercat/hand/armar7/sub_device/relative/Device.cpp
@@ -1,12 +1,11 @@
#include "Device.h"
-#include <devices/ethercat/hand/armar7de/joint_controller/RelativePosition.h>
#include <devices/ethercat/hand/armar7/Data.h>
+#include <devices/ethercat/hand/armar7/Device.h>
#include <devices/ethercat/hand/armar7de/joint_controller/EmergencyStop.h>
#include <devices/ethercat/hand/armar7de/joint_controller/PWM.h>
+#include <devices/ethercat/hand/armar7de/joint_controller/RelativePosition.h>
#include <devices/ethercat/hand/armar7de/joint_controller/StopMovement.h>
-#include <devices/ethercat/hand/armar7/Device.h>
-
namespace devices::ethercat::hand::armar7::sub_device::relative
{
@@ -21,7 +20,6 @@ namespace devices::ethercat::hand::armar7::sub_device::relative
;
}
-
void
Device::init(devices::ethercat::hand::armar7::Device* hand, Data* dataPtr)
{
@@ -39,7 +37,6 @@ namespace devices::ethercat::hand::armar7::sub_device::relative
addJointController(positionController.get());
}
-
void
Device::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -54,7 +51,6 @@ namespace devices::ethercat::hand::armar7::sub_device::relative
dataPtr->updateSensorValueStruct(sensorValue);
}
-
void
Device::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
diff --git a/source/devices/ethercat/hand/armar7/sub_device/relative/Device.h b/source/devices/ethercat/hand/armar7/sub_device/relative/Device.h
index 73c17a42..982b556e 100644
--- a/source/devices/ethercat/hand/armar7/sub_device/relative/Device.h
+++ b/source/devices/ethercat/hand/armar7/sub_device/relative/Device.h
@@ -12,7 +12,6 @@
#include <devices/ethercat/hand/armar7/Slave.h>
#include <devices/ethercat/hand/common/AbstractHand.h>
-
namespace devices::ethercat::hand::armar7de
{
using EmergencyStopControllerPtr = std::shared_ptr<class EmergencyStopController>;
@@ -27,7 +26,6 @@ namespace devices::ethercat::hand::armar7
class Device;
} // namespace devices::ethercat::hand::armar7
-
namespace devices::ethercat::hand::armar7::sub_device::relative
{
class Device :
@@ -42,6 +40,7 @@ namespace devices::ethercat::hand::armar7::sub_device::relative
public:
Device(const std::string& deviceName);
+
const armarx::SensorValueBase*
getSensorValue() const override
{
diff --git a/source/devices/ethercat/hand/armar7de/joint_controller/AbsolutePosition.cpp b/source/devices/ethercat/hand/armar7de/joint_controller/AbsolutePosition.cpp
index 444ec682..89834bb2 100644
--- a/source/devices/ethercat/hand/armar7de/joint_controller/AbsolutePosition.cpp
+++ b/source/devices/ethercat/hand/armar7de/joint_controller/AbsolutePosition.cpp
@@ -2,7 +2,6 @@
#include <RobotAPI/libraries/core/math/MathUtils.h>
-
namespace devices::ethercat::hand::armar7de
{
@@ -14,14 +13,12 @@ namespace devices::ethercat::hand::armar7de
PID.threadSafe = false;
}
-
armarx::ControlTargetBase*
AbsolutePositionController::getControlTarget()
{
return ⌖
}
-
void
AbsolutePositionController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
const IceUtil::Time& timeSinceLastIteration)
@@ -39,14 +36,12 @@ namespace devices::ethercat::hand::armar7de
}
}
-
void
AbsolutePositionController::rtPreActivateController()
{
;
}
-
void
AbsolutePositionController::rtPostDeactivateController()
{
diff --git a/source/devices/ethercat/hand/armar7de/joint_controller/AbsolutePosition.h b/source/devices/ethercat/hand/armar7de/joint_controller/AbsolutePosition.h
index fa1c59f5..18b40662 100644
--- a/source/devices/ethercat/hand/armar7de/joint_controller/AbsolutePosition.h
+++ b/source/devices/ethercat/hand/armar7de/joint_controller/AbsolutePosition.h
@@ -7,7 +7,6 @@
#include <devices/ethercat/hand/armar7de/sub_device/Data.h>
-
namespace devices::ethercat::hand::armar7de
{
@@ -47,7 +46,6 @@ namespace devices::ethercat::hand::armar7de
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
};
-
class AbsolutePositionController : public armarx::JointController
{
@@ -71,7 +69,6 @@ namespace devices::ethercat::hand::armar7de
AbsolutePositionControlTarget target;
};
-
using AbsolutePositionControllerPtr = std::shared_ptr<AbsolutePositionController>;
} // namespace devices::ethercat::hand::armar7de
diff --git a/source/devices/ethercat/hand/armar7de/joint_controller/EmergencyStop.cpp b/source/devices/ethercat/hand/armar7de/joint_controller/EmergencyStop.cpp
index 04cf56cf..d5327934 100644
--- a/source/devices/ethercat/hand/armar7de/joint_controller/EmergencyStop.cpp
+++ b/source/devices/ethercat/hand/armar7de/joint_controller/EmergencyStop.cpp
@@ -1,32 +1,30 @@
#include <devices/ethercat/hand/armar7de/joint_controller/EmergencyStop.h>
-
namespace devices::ethercat::hand::armar7de
{
EmergencyStopController::EmergencyStopController(sub_device::DataInterface* dataPtr) :
dataPtr(dataPtr)
{
-
}
-
- void EmergencyStopController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ EmergencyStopController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
-
}
-
- armarx::ControlTargetBase* EmergencyStopController::getControlTarget()
+ armarx::ControlTargetBase*
+ EmergencyStopController::getControlTarget()
{
return ⌖
}
-
- void EmergencyStopController::rtPreActivateController()
+ void
+ EmergencyStopController::rtPreActivateController()
{
ARMARX_INFO << "Stopping hand!";
dataPtr->setMotorPwmValue(0);
}
-} // namespace armarx
+} // namespace devices::ethercat::hand::armar7de
diff --git a/source/devices/ethercat/hand/armar7de/joint_controller/EmergencyStop.h b/source/devices/ethercat/hand/armar7de/joint_controller/EmergencyStop.h
index fd149410..ffa50bee 100644
--- a/source/devices/ethercat/hand/armar7de/joint_controller/EmergencyStop.h
+++ b/source/devices/ethercat/hand/armar7de/joint_controller/EmergencyStop.h
@@ -5,13 +5,11 @@
#include <devices/ethercat/hand/armar7de/sub_device/Data.h>
-
namespace devices::ethercat::hand::armar7de
{
using EmergencyStopControllerPtr = std::shared_ptr<class EmergencyStopController>;
-
class EmergencyStopController : public armarx::JointController
{
diff --git a/source/devices/ethercat/hand/armar7de/joint_controller/PWM.cpp b/source/devices/ethercat/hand/armar7de/joint_controller/PWM.cpp
index e0a966cb..1aa9e679 100644
--- a/source/devices/ethercat/hand/armar7de/joint_controller/PWM.cpp
+++ b/source/devices/ethercat/hand/armar7de/joint_controller/PWM.cpp
@@ -1,6 +1,5 @@
#include <devices/ethercat/hand/armar7de/joint_controller/PWM.h>
-
namespace devices::ethercat::hand::armar7de
{
@@ -9,14 +8,12 @@ namespace devices::ethercat::hand::armar7de
;
}
-
armarx::ControlTargetBase*
PWMController::getControlTarget()
{
return ⌖
}
-
void
PWMController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
const IceUtil::Time& /*timeSinceLastIteration*/)
@@ -27,14 +24,12 @@ namespace devices::ethercat::hand::armar7de
}
}
-
void
PWMController::rtPreActivateController()
{
;
}
-
void
PWMController::rtPostDeactivateController()
{
diff --git a/source/devices/ethercat/hand/armar7de/joint_controller/PWM.h b/source/devices/ethercat/hand/armar7de/joint_controller/PWM.h
index f40ee9ed..125c2b16 100644
--- a/source/devices/ethercat/hand/armar7de/joint_controller/PWM.h
+++ b/source/devices/ethercat/hand/armar7de/joint_controller/PWM.h
@@ -5,7 +5,6 @@
#include <devices/ethercat/hand/armar7de/sub_device/Data.h>
-
namespace devices::ethercat::hand::armar7de
{
@@ -14,7 +13,6 @@ namespace devices::ethercat::hand::armar7de
static const std::string Armar7deHandPwm = "ControlMode_HandPwm";
}
-
class PWMControlTarget : public armarx::ControlTargetBase
{
@@ -51,7 +49,6 @@ namespace devices::ethercat::hand::armar7de
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
};
-
class PWMController : public armarx::JointController
{
@@ -74,7 +71,6 @@ namespace devices::ethercat::hand::armar7de
PWMControlTarget target;
};
-
using PWMControllerPtr = std::shared_ptr<PWMController>;
} // namespace devices::ethercat::hand::armar7de
diff --git a/source/devices/ethercat/hand/armar7de/joint_controller/RelativePosition.cpp b/source/devices/ethercat/hand/armar7de/joint_controller/RelativePosition.cpp
index 25d7632b..3d25b5f3 100644
--- a/source/devices/ethercat/hand/armar7de/joint_controller/RelativePosition.cpp
+++ b/source/devices/ethercat/hand/armar7de/joint_controller/RelativePosition.cpp
@@ -1,10 +1,10 @@
#include "RelativePosition.h"
+
#include <cstdint>
#include <limits>
#include <RobotAPI/libraries/core/math/MathUtils.h>
-
namespace devices::ethercat::hand::armar7de
{
@@ -16,14 +16,12 @@ namespace devices::ethercat::hand::armar7de
PID.threadSafe = false;
}
-
armarx::ControlTargetBase*
RelativePositionController::getControlTarget()
{
return ⌖
}
-
void
RelativePositionController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
const IceUtil::Time& timeSinceLastIteration)
@@ -37,7 +35,7 @@ namespace devices::ethercat::hand::armar7de
target.position);
const double controlValuePWM = PID.getControlValue() * maxPWM;
-
+
// overflow check
// TODO fix this. causes the unit to crash
// ARMARX_CHECK(std::abs(controlValuePWM) < std::numeric_limits<std::int16_t>::max());
@@ -48,20 +46,17 @@ namespace devices::ethercat::hand::armar7de
// overflow check
//ARMARX_CHECK(std::abs(targetPWM) < std::numeric_limits<std::int16_t>::max());
-
data->setMotorPwmValue(static_cast<std::int16_t>(targetPWM));
}
}
-
void
RelativePositionController::rtPreActivateController()
{
;
}
-
void
RelativePositionController::rtPostDeactivateController()
{
diff --git a/source/devices/ethercat/hand/armar7de/joint_controller/RelativePosition.h b/source/devices/ethercat/hand/armar7de/joint_controller/RelativePosition.h
index 6e0fdba3..6ed3baee 100644
--- a/source/devices/ethercat/hand/armar7de/joint_controller/RelativePosition.h
+++ b/source/devices/ethercat/hand/armar7de/joint_controller/RelativePosition.h
@@ -9,7 +9,6 @@
#include <devices/ethercat/hand/armar7de/sub_device/Data.h>
-
namespace devices::ethercat::hand::armar7de
{
@@ -49,7 +48,6 @@ namespace devices::ethercat::hand::armar7de
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
};
-
class RelativePositionController : public armarx::JointController
{
@@ -73,7 +71,6 @@ namespace devices::ethercat::hand::armar7de
RelativePositionControlTarget target;
};
-
using PositionControllerPtr = std::shared_ptr<RelativePositionController>;
} // namespace devices::ethercat::hand::armar7de
diff --git a/source/devices/ethercat/hand/armar7de/joint_controller/StopMovement.cpp b/source/devices/ethercat/hand/armar7de/joint_controller/StopMovement.cpp
index 979248a3..22dee3e2 100644
--- a/source/devices/ethercat/hand/armar7de/joint_controller/StopMovement.cpp
+++ b/source/devices/ethercat/hand/armar7de/joint_controller/StopMovement.cpp
@@ -1,6 +1,5 @@
#include <devices/ethercat/hand/armar7de/joint_controller/StopMovement.h>
-
namespace devices::ethercat::hand::armar7de
{
@@ -10,7 +9,6 @@ namespace devices::ethercat::hand::armar7de
;
}
-
void
StopMovementController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -18,14 +16,12 @@ namespace devices::ethercat::hand::armar7de
;
}
-
armarx::ControlTargetBase*
StopMovementController::getControlTarget()
{
return ⌖
}
-
void
StopMovementController::rtPreActivateController()
{
diff --git a/source/devices/ethercat/hand/armar7de/joint_controller/StopMovement.h b/source/devices/ethercat/hand/armar7de/joint_controller/StopMovement.h
index f2f4775e..82700ff2 100644
--- a/source/devices/ethercat/hand/armar7de/joint_controller/StopMovement.h
+++ b/source/devices/ethercat/hand/armar7de/joint_controller/StopMovement.h
@@ -5,13 +5,11 @@
#include <devices/ethercat/hand/armar7de/sub_device/Data.h>
-
namespace devices::ethercat::hand::armar7de
{
using StopMovementControllerPtr = std::shared_ptr<class StopMovementController>;
-
class StopMovementController : public armarx::JointController
{
diff --git a/source/devices/ethercat/hand/armar7de/njoint_controller/ShapeInterface.ice b/source/devices/ethercat/hand/armar7de/njoint_controller/ShapeInterface.ice
index 89e0d190..7b97cd91 100644
--- a/source/devices/ethercat/hand/armar7de/njoint_controller/ShapeInterface.ice
+++ b/source/devices/ethercat/hand/armar7de/njoint_controller/ShapeInterface.ice
@@ -25,40 +25,55 @@
// armarx
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
-
#include <RobotAPI/interface/core/NameValueMap.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
-module devices { module ethercat { module hand { module armar7de
+module devices
{
+ module ethercat
+ {
+ module hand
+ {
+ module armar7de
+ {
- // legacy implementation, remove when no longer needed
- // struct JointValues
- // {
- // float thumbJointValue;
- // float fingersJointValue;
- // float indexJointValue;
- // float thumbRotationJointValue;
- // };
+ // legacy implementation, remove when no longer needed
+ // struct JointValues
+ // {
+ // float thumbJointValue;
+ // float fingersJointValue;
+ // float indexJointValue;
+ // float thumbRotationJointValue;
+ // };
- interface ShapeControllerInterface extends armarx::NJointControllerInterface
- {
- void setTargetsMap(armarx::NameValueMap targets);
- void setTargetsWithPwmMap(armarx::NameValueMap targets, armarx::NameValueMap maxPWM);
+ interface ShapeControllerInterface extends armarx::NJointControllerInterface
+ {
+ void setTargetsMap(armarx::NameValueMap targets);
+ void setTargetsWithPwmMap(armarx::NameValueMap targets,
+ armarx::NameValueMap maxPWM);
- ["deprecate:setTargets() has been deprecated, use setTargetsMap() instead."]
- void setTargets(float fingers, float index, float thumb, float thumbRotation);
- ["deprecate:setTargetsWithPwm() has been deprecated, use setTargetsWithPwmMap() instead."]
- void setTargetsWithPwm(float fingers, float index, float thumb, float thumbRotation,
- float fingersRelativeMaxPwm, float indexRelativeMaxPwm, float thumbRelativeMaxPwm,
- float thumbRotationRelativeMaxPwm);
+ ["deprecate:setTargets() has been deprecated, use setTargetsMap() "
+ "instead."] void
+ setTargets(float fingers, float index, float thumb, float thumbRotation);
+ ["deprecate:setTargetsWithPwm() has been deprecated, use "
+ "setTargetsWithPwmMap() instead."] void
+ setTargetsWithPwm(float fingers,
+ float index,
+ float thumb,
+ float thumbRotation,
+ float fingersRelativeMaxPwm,
+ float indexRelativeMaxPwm,
+ float thumbRelativeMaxPwm,
+ float thumbRotationRelativeMaxPwm);
- armarx::NameValueMap getJointValues();
+ armarx::NameValueMap getJointValues();
- // JointValues getJointValues();
+ // JointValues getJointValues();
+ };
+ };
+ };
};
-
-};};};};
+};
diff --git a/source/devices/ethercat/hand/armar7de/sub_device/absolute/Config.h b/source/devices/ethercat/hand/armar7de/sub_device/absolute/Config.h
index 6c7f815d..59800098 100644
--- a/source/devices/ethercat/hand/armar7de/sub_device/absolute/Config.h
+++ b/source/devices/ethercat/hand/armar7de/sub_device/absolute/Config.h
@@ -4,7 +4,6 @@
#include <armarx/control/hardware_config/DeviceConfig.h>
#include <armarx/control/hardware_config/Types.h>
-
namespace devices::ethercat::hand::armar7de::sub_device::absolute
{
diff --git a/source/devices/ethercat/hand/armar7de/sub_device/absolute/Data.h b/source/devices/ethercat/hand/armar7de/sub_device/absolute/Data.h
index e1bc3921..0926a381 100644
--- a/source/devices/ethercat/hand/armar7de/sub_device/absolute/Data.h
+++ b/source/devices/ethercat/hand/armar7de/sub_device/absolute/Data.h
@@ -14,7 +14,6 @@
#include <armarx/control/rt_filters/FirFilter.h>
#include <armarx/control/rt_filters/RtMedianFilter.h>
-
namespace devices::ethercat::hand::armar7de::sub_device::absolute
{
diff --git a/source/devices/ethercat/hand/armar7de/sub_device/relative/Config.h b/source/devices/ethercat/hand/armar7de/sub_device/relative/Config.h
index 83b69017..55427a9c 100644
--- a/source/devices/ethercat/hand/armar7de/sub_device/relative/Config.h
+++ b/source/devices/ethercat/hand/armar7de/sub_device/relative/Config.h
@@ -2,7 +2,6 @@
#include <armarx/control/hardware_config/DeviceConfig.h>
-
namespace devices::ethercat::hand::armar7de::sub_device::relative
{
diff --git a/source/devices/ethercat/hand/armar7de/sub_device/relative/Data.h b/source/devices/ethercat/hand/armar7de/sub_device/relative/Data.h
index 0050b7cf..0580836b 100644
--- a/source/devices/ethercat/hand/armar7de/sub_device/relative/Data.h
+++ b/source/devices/ethercat/hand/armar7de/sub_device/relative/Data.h
@@ -14,7 +14,6 @@
#include <armarx/control/rt_filters/FirFilter.h>
#include <armarx/control/rt_filters/RtMedianFilter.h>
-
namespace devices::ethercat::hand::armar7de::sub_device::relative
{
diff --git a/source/devices/ethercat/hand/armarde/Data.cpp b/source/devices/ethercat/hand/armarde/Data.cpp
index a11a6b33..179f06ff 100644
--- a/source/devices/ethercat/hand/armarde/Data.cpp
+++ b/source/devices/ethercat/hand/armarde/Data.cpp
@@ -7,7 +7,6 @@
#include <devices/ethercat/hand/armarde/Data.h>
-
namespace devices::ethercat::hand::armarde
{
@@ -17,7 +16,6 @@ namespace devices::ethercat::hand::armarde
ARMARX_CHECK_EXPRESSION(sensorOUT);
}
-
void
Data::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -28,11 +26,12 @@ namespace devices::ethercat::hand::armarde
// ARMARX_INFO << "Data::rtReadSensorValues";
memcpy(tofDepth.data(), sensorOUT->tofDepth, N_TOF_CELLS * sizeof(std::uint16_t));
- memcpy(tofTargetDetected.data(), sensorOUT->tofTargetDetected, N_TOF_CELLS * sizeof(std::uint8_t));
+ memcpy(tofTargetDetected.data(),
+ sensorOUT->tofTargetDetected,
+ N_TOF_CELLS * sizeof(std::uint8_t));
memcpy(tofSigma.data(), sensorOUT->tofSigma, N_TOF_CELLS * sizeof(std::uint8_t));
}
-
void
Data::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -40,7 +39,6 @@ namespace devices::ethercat::hand::armarde
;
}
-
void
Data::updateSensorValueStruct(SensorDataValue& data)
{
@@ -64,11 +62,14 @@ namespace devices::ethercat::hand::armarde
// auto newtofSigma = tofSigma;
- for (std::size_t i = 0; i < N_COLS; i++) {
- for(std::size_t j = 0; j < N_ROWS; j++) {
- data.tofDepth.at(j*N_COLS + i) = tofDepth.at((((N_ROWS-1)-j)*N_COLS) +i);
- data.tofTargetDetected.at(j*N_COLS + i) = tofTargetDetected.at((((N_ROWS-1)-j)*N_COLS) +i);
- data.tofSigma.at(j*N_COLS + i) = tofSigma.at((((N_ROWS-1)-j)*N_COLS) +i);
+ for (std::size_t i = 0; i < N_COLS; i++)
+ {
+ for (std::size_t j = 0; j < N_ROWS; j++)
+ {
+ data.tofDepth.at(j * N_COLS + i) = tofDepth.at((((N_ROWS - 1) - j) * N_COLS) + i);
+ data.tofTargetDetected.at(j * N_COLS + i) =
+ tofTargetDetected.at((((N_ROWS - 1) - j) * N_COLS) + i);
+ data.tofSigma.at(j * N_COLS + i) = tofSigma.at((((N_ROWS - 1) - j) * N_COLS) + i);
}
}
@@ -79,22 +80,18 @@ namespace devices::ethercat::hand::armarde
}
}
-
-
std::uint16_t
Data::getMainUpdateRate() const
{
return mainUpdateRate;
}
-
bool
Data::getEmergencyStopStatus() const
{
return emergencyStopStatus;
}
-
std::array<std::uint16_t, 64> // TODO: Constant
Data::getToFDepth() const
{
diff --git a/source/devices/ethercat/hand/armarde/Data.h b/source/devices/ethercat/hand/armarde/Data.h
index 463e77f2..5060f9f8 100644
--- a/source/devices/ethercat/hand/armarde/Data.h
+++ b/source/devices/ethercat/hand/armarde/Data.h
@@ -26,8 +26,6 @@ namespace devices::ethercat::hand::armarde
constexpr std::size_t N_ROWS = 8;
constexpr std::size_t N_COLS = 8;
-
-
class SensorDataValue : public ::armarx::SensorValueBase
{
@@ -48,6 +46,7 @@ namespace devices::ethercat::hand::armarde
// std::array<uint8_t, N_TOF_CELLS> tofSigma;
float tofDepthMedian;
+
static SensorValueInfo<SensorDataValue>
GetClassMemberInfo()
{
diff --git a/source/devices/ethercat/hand/armarde/ErrorDecoder.cpp b/source/devices/ethercat/hand/armarde/ErrorDecoder.cpp
index 87cc5d4b..384b80ab 100644
--- a/source/devices/ethercat/hand/armarde/ErrorDecoder.cpp
+++ b/source/devices/ethercat/hand/armarde/ErrorDecoder.cpp
@@ -1,6 +1,5 @@
#include "ErrorDecoder.h"
-
namespace devices::ethercat::hand::armarde
{
@@ -10,7 +9,6 @@ namespace devices::ethercat::hand::armarde
;
}
-
bool
ErrorDecoder::getEmergencyStopStatus() const
{
@@ -18,7 +16,6 @@ namespace devices::ethercat::hand::armarde
return (0x80 & outputs->statusBits) ? true : false;
}
-
bool
ErrorDecoder::motorErrorDetected(armarx::control::ethercat::SlaveInterface* slave,
bool printError) const
@@ -51,7 +48,6 @@ namespace devices::ethercat::hand::armarde
MotorStatus::ErrorThumb | MotorStatus::ErrorThumbRotation)) != 0;
}
-
bool
ErrorDecoder::absoluteEncoderErrorDetected(armarx::control::ethercat::SlaveInterface* slave,
bool printError) const
diff --git a/source/devices/ethercat/hand/armarde/ErrorDecoder.h b/source/devices/ethercat/hand/armarde/ErrorDecoder.h
index c5e0b53f..91e1c943 100644
--- a/source/devices/ethercat/hand/armarde/ErrorDecoder.h
+++ b/source/devices/ethercat/hand/armarde/ErrorDecoder.h
@@ -5,13 +5,11 @@
#include <devices/ethercat/hand/armarde/SlaveIO.h>
-
namespace armarx::control::ethercat
{
class SlaveInterface;
}
-
namespace devices::ethercat::hand::armarde
{
diff --git a/source/devices/ethercat/hand/armarde/Slave.cpp b/source/devices/ethercat/hand/armarde/Slave.cpp
index bdb69408..99a0d595 100644
--- a/source/devices/ethercat/hand/armarde/Slave.cpp
+++ b/source/devices/ethercat/hand/armarde/Slave.cpp
@@ -7,7 +7,6 @@
#include <devices/ethercat/common/h2t_devices.h>
-
namespace devices::ethercat::hand::armarde
{
@@ -20,14 +19,12 @@ namespace devices::ethercat::hand::armarde
setTag(this->slaveIdentifier.getName());
}
-
void
Slave::execute()
{
;
}
-
bool
Slave::prepareForRun()
{
@@ -36,21 +33,18 @@ namespace devices::ethercat::hand::armarde
return true;
}
-
bool
Slave::shutdown()
{
return true;
}
-
void
Slave::doMappings()
{
;
}
-
bool
Slave::hasError()
{
@@ -58,56 +52,48 @@ namespace devices::ethercat::hand::armarde
errorDecoder.motorErrorDetected(this, true);
}
-
void
Slave::prepareForSafeOp()
{
;
}
-
void
Slave::finishPreparingForSafeOp()
{
;
}
-
void
Slave::prepareForOp()
{
errorDecoder = ErrorDecoder(getOutputsPtr(), getInputsPtr());
}
-
void
Slave::finishPreparingForOp()
{
;
}
-
bool
Slave::isEmergencyStopActive() const
{
return errorDecoder.getEmergencyStopStatus();
}
-
bool
Slave::recoverFromEmergencyStop()
{
return true;
}
-
std::string
Slave::getDefaultName()
{
return "ArmardeHand";
}
-
bool
Slave::isSlaveIdentifierAccepted(const armarx::control::ethercat::SlaveIdentifier& sid)
{
diff --git a/source/devices/ethercat/hand/armarde/Slave.h b/source/devices/ethercat/hand/armarde/Slave.h
index b9ec421b..afdccfb2 100644
--- a/source/devices/ethercat/hand/armarde/Slave.h
+++ b/source/devices/ethercat/hand/armarde/Slave.h
@@ -11,7 +11,6 @@
#include <devices/ethercat/hand/armarde/ErrorDecoder.h>
#include <devices/ethercat/hand/armarde/SlaveIO.h>
-
namespace devices::ethercat::hand::armarde
{
diff --git a/source/devices/ethercat/hand/armarde/SlaveIO.h b/source/devices/ethercat/hand/armarde/SlaveIO.h
index a72dfb87..6b8e702a 100644
--- a/source/devices/ethercat/hand/armarde/SlaveIO.h
+++ b/source/devices/ethercat/hand/armarde/SlaveIO.h
@@ -5,7 +5,6 @@
#include <array>
#include <cstdint>
-
namespace devices::ethercat::hand::armarde
{
@@ -43,7 +42,6 @@ namespace devices::ethercat::hand::armarde
} __attribute__((__packed__));
-
/**
* @brief PDO mapping master->sensorB
*/
diff --git a/source/devices/ethercat/hand/armarde/njoint_controller/Shape.cpp b/source/devices/ethercat/hand/armarde/njoint_controller/Shape.cpp
index 04d50224..55000284 100644
--- a/source/devices/ethercat/hand/armarde/njoint_controller/Shape.cpp
+++ b/source/devices/ethercat/hand/armarde/njoint_controller/Shape.cpp
@@ -3,13 +3,12 @@
#include <cmath>
#include "RobotAPI/components/units/RobotUnit/ControlModes.h"
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h> // FIXME avoid this
#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <devices/ethercat/hand/armar7de/joint_controller/AbsolutePosition.h>
#include <devices/ethercat/hand/armar7de/joint_controller/PWM.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h> // FIXME avoid this
-
namespace devices::ethercat::hand::armar7de
{
@@ -459,4 +458,4 @@ namespace devices::ethercat::hand::armar7de
return values;
}
-} // namespace devices::ethercat::hand::armarde
+} // namespace devices::ethercat::hand::armar7de
diff --git a/source/devices/ethercat/hand/armarde/sub_device/absolute/Data.cpp b/source/devices/ethercat/hand/armarde/sub_device/absolute/Data.cpp
index 4e4b49c1..015c75e5 100644
--- a/source/devices/ethercat/hand/armarde/sub_device/absolute/Data.cpp
+++ b/source/devices/ethercat/hand/armarde/sub_device/absolute/Data.cpp
@@ -6,7 +6,6 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
-
namespace devices::ethercat::hand::armarde::sub_device::absolute
{
@@ -26,7 +25,6 @@ namespace devices::ethercat::hand::armarde::sub_device::absolute
config.absoluteEncoderMagnetometerZ);
}
-
void
Data::rtReadSensorValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
const IceUtil::Time& /*timeSinceLastIteration*/)
@@ -42,16 +40,16 @@ namespace devices::ethercat::hand::armarde::sub_device::absolute
std::atan2(absoluteEncoderMagnetometerY.value, absoluteEncoderMagnetometerX.value);
const float sign = config.motorPositionIsInverted ? -1.0 : 1.0;
- motorPosition = (absoluteEncoderAngle + config.motorPositionZeroOffset) * sign * config.motorPositionScaling;
+ motorPosition = (absoluteEncoderAngle + config.motorPositionZeroOffset) * sign *
+ config.motorPositionScaling;
motorPosition = simox::math::periodic_clamp<float>(motorPosition, -M_PI, M_PI);
}
-
void
Data::rtWriteTargetValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
const IceUtil::Time& /*timeSinceLastIteration*/)
{
- if(config.pwmIsInverted)
+ if (config.pwmIsInverted)
{
*motorPwmPtr = -motorPwmValue;
}
@@ -61,7 +59,6 @@ namespace devices::ethercat::hand::armarde::sub_device::absolute
}
}
-
void
Data::updateSensorValueStruct(armar7de::sub_device::absolute::SensorDataValue& data)
{
@@ -77,56 +74,48 @@ namespace devices::ethercat::hand::armarde::sub_device::absolute
data.motorPWM = motorPwmValue;
}
-
float
Data::getKp() const
{
return config.Kp;
}
-
std::uint16_t
Data::getMaxPWM() const
{
return config.maxPwm;
}
-
float
Data::getJointLimitLow() const
{
return config.jointLimitLow;
}
-
float
Data::getJointLimitHigh() const
{
return config.jointLimitHigh;
}
-
bool
Data::getMotorError() const
{
return motorError;
}
-
std::uint8_t
Data::getAbsoluteEncoderStatusBits() const
{
return absoluteEncoderStatusBits;
}
-
float
Data::getMotorPosition() const
{
return motorPosition;
}
-
void
Data::setMotorPwmValue(std::int16_t pwm)
{
diff --git a/source/devices/ethercat/hand/armarde/sub_device/absolute/Data.h b/source/devices/ethercat/hand/armarde/sub_device/absolute/Data.h
index 0a510076..0ee90f11 100644
--- a/source/devices/ethercat/hand/armarde/sub_device/absolute/Data.h
+++ b/source/devices/ethercat/hand/armarde/sub_device/absolute/Data.h
@@ -21,7 +21,6 @@
#include <devices/ethercat/hand/armarde/ErrorDecoder.h>
#include <devices/ethercat/hand/armarde/SlaveIO.h>
-
namespace devices::ethercat::hand::armarde::sub_device::absolute
{
diff --git a/source/devices/ethercat/hand/armarde/sub_device/absolute/Device.cpp b/source/devices/ethercat/hand/armarde/sub_device/absolute/Device.cpp
index 9cfd7e97..f6703bf6 100644
--- a/source/devices/ethercat/hand/armarde/sub_device/absolute/Device.cpp
+++ b/source/devices/ethercat/hand/armarde/sub_device/absolute/Device.cpp
@@ -5,7 +5,6 @@
#include <devices/ethercat/hand/armar7de/joint_controller/PWM.h>
#include <devices/ethercat/hand/armar7de/joint_controller/StopMovement.h>
-
namespace devices::ethercat::hand::armarde::sub_device::absolute
{
@@ -18,7 +17,6 @@ namespace devices::ethercat::hand::armarde::sub_device::absolute
;
}
-
void
Device::init(devices::ethercat::hand::armarde::Device* hand, Data* dataPtr)
{
@@ -35,7 +33,6 @@ namespace devices::ethercat::hand::armarde::sub_device::absolute
addJointController(positionController.get());
}
-
void
Device::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -50,7 +47,6 @@ namespace devices::ethercat::hand::armarde::sub_device::absolute
dataPtr->updateSensorValueStruct(sensorValue);
}
-
void
Device::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
diff --git a/source/devices/ethercat/hand/armarde/sub_device/absolute/Device.h b/source/devices/ethercat/hand/armarde/sub_device/absolute/Device.h
index 87fcb573..f25b2bb5 100644
--- a/source/devices/ethercat/hand/armarde/sub_device/absolute/Device.h
+++ b/source/devices/ethercat/hand/armarde/sub_device/absolute/Device.h
@@ -33,7 +33,6 @@
#include <devices/ethercat/hand/armarde/Slave.h>
#include <devices/ethercat/hand/common/AbstractHand.h>
-
namespace devices::ethercat::hand::armar7de
{
using EmergencyStopControllerPtr = std::shared_ptr<class EmergencyStopController>;
@@ -48,7 +47,6 @@ namespace devices::ethercat::hand::armarde
class Device;
} // namespace devices::ethercat::hand::armarde
-
namespace devices::ethercat::hand::armarde::sub_device::absolute
{
class Device :
diff --git a/source/devices/ethercat/hand/armarde/sub_device/relative/Data.cpp b/source/devices/ethercat/hand/armarde/sub_device/relative/Data.cpp
index 01e15850..19b636c7 100644
--- a/source/devices/ethercat/hand/armarde/sub_device/relative/Data.cpp
+++ b/source/devices/ethercat/hand/armarde/sub_device/relative/Data.cpp
@@ -8,7 +8,6 @@
#include <devices/ethercat/hand/armarde/Data.h>
-
namespace devices::ethercat::hand::armarde::sub_device::relative
{
@@ -49,7 +48,6 @@ namespace devices::ethercat::hand::armarde::sub_device::relative
}
}
-
void
Data::rtReadSensorValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
const IceUtil::Time& /*timeSinceLastIteration*/)
@@ -60,7 +58,6 @@ namespace devices::ethercat::hand::armarde::sub_device::relative
motorPosition.read();
}
-
void
Data::rtWriteTargetValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
const IceUtil::Time& /*timeSinceLastIteration*/)
@@ -68,7 +65,6 @@ namespace devices::ethercat::hand::armarde::sub_device::relative
*motorPwmPtr = motorPwmValue;
}
-
void
Data::updateSensorValueStruct(armar7de::sub_device::relative::SensorDataValue& data)
{
@@ -80,42 +76,36 @@ namespace devices::ethercat::hand::armarde::sub_device::relative
data.relativeEncoderTicks = motorPosition.getRaw();
}
-
float
Data::getKp() const
{
return config.Kp;
}
-
std::uint16_t
Data::getMaxPWM() const
{
return config.maxPwm;
}
-
bool
Data::getMotorError() const
{
return motorError;
}
-
float
Data::getMotorTemperature() const
{
return motorTemperature.value;
}
-
float
Data::getMotorPosition() const
{
return motorPosition.value;
}
-
void
Data::setMotorPwmValue(std::int16_t pwm)
{
diff --git a/source/devices/ethercat/hand/armarde/sub_device/relative/Data.h b/source/devices/ethercat/hand/armarde/sub_device/relative/Data.h
index 38cc6152..5e8a6d21 100644
--- a/source/devices/ethercat/hand/armarde/sub_device/relative/Data.h
+++ b/source/devices/ethercat/hand/armarde/sub_device/relative/Data.h
@@ -21,7 +21,6 @@
#include <devices/ethercat/hand/armarde/ErrorDecoder.h>
#include <devices/ethercat/hand/armarde/SlaveIO.h>
-
namespace devices::ethercat::hand::armarde::sub_device::relative
{
@@ -33,7 +32,6 @@ namespace devices::ethercat::hand::armarde::sub_device::relative
// ThumbCircumduction = 3
};
-
class Data : public armar7de::sub_device::DataInterface
{
using DataConfig = armar7de::sub_device::relative::DataConfig;
diff --git a/source/devices/ethercat/hand/armarde/sub_device/relative/Device.cpp b/source/devices/ethercat/hand/armarde/sub_device/relative/Device.cpp
index d4135705..e59843ec 100644
--- a/source/devices/ethercat/hand/armarde/sub_device/relative/Device.cpp
+++ b/source/devices/ethercat/hand/armarde/sub_device/relative/Device.cpp
@@ -1,13 +1,12 @@
#include "Device.h"
-#include <devices/ethercat/hand/armar7de/joint_controller/RelativePosition.h>
-#include <devices/ethercat/hand/armarde/Data.h>
#include <devices/ethercat/hand/armar7de/joint_controller/EmergencyStop.h>
#include <devices/ethercat/hand/armar7de/joint_controller/PWM.h>
+#include <devices/ethercat/hand/armar7de/joint_controller/RelativePosition.h>
#include <devices/ethercat/hand/armar7de/joint_controller/StopMovement.h>
+#include <devices/ethercat/hand/armarde/Data.h>
#include <devices/ethercat/hand/armarde/Device.h>
-
namespace devices::ethercat::hand::armarde::sub_device::relative
{
@@ -21,7 +20,6 @@ namespace devices::ethercat::hand::armarde::sub_device::relative
;
}
-
void
Device::init(devices::ethercat::hand::armarde::Device* hand, Data* dataPtr)
{
@@ -39,7 +37,6 @@ namespace devices::ethercat::hand::armarde::sub_device::relative
addJointController(positionController.get());
}
-
void
Device::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -54,7 +51,6 @@ namespace devices::ethercat::hand::armarde::sub_device::relative
dataPtr->updateSensorValueStruct(sensorValue);
}
-
void
Device::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
diff --git a/source/devices/ethercat/hand/armarde/sub_device/relative/Device.h b/source/devices/ethercat/hand/armarde/sub_device/relative/Device.h
index 1da802e2..3b3609b0 100644
--- a/source/devices/ethercat/hand/armarde/sub_device/relative/Device.h
+++ b/source/devices/ethercat/hand/armarde/sub_device/relative/Device.h
@@ -12,7 +12,6 @@
#include <devices/ethercat/hand/armarde/Slave.h>
#include <devices/ethercat/hand/common/AbstractHand.h>
-
namespace devices::ethercat::hand::armar7de
{
using EmergencyStopControllerPtr = std::shared_ptr<class EmergencyStopController>;
@@ -27,7 +26,6 @@ namespace devices::ethercat::hand::armarde
class Device;
} // namespace devices::ethercat::hand::armarde
-
namespace devices::ethercat::hand::armarde::sub_device::relative
{
class Device :
@@ -42,6 +40,7 @@ namespace devices::ethercat::hand::armarde::sub_device::relative
public:
Device(const std::string& deviceName);
+
const armarx::SensorValueBase*
getSensorValue() const override
{
diff --git a/source/devices/ethercat/hand/common/AbstractHand.h b/source/devices/ethercat/hand/common/AbstractHand.h
index b06eb363..8e64adac 100644
--- a/source/devices/ethercat/hand/common/AbstractHand.h
+++ b/source/devices/ethercat/hand/common/AbstractHand.h
@@ -28,11 +28,11 @@
#include <devices/ethercat/hand/common/AbstractHandUnitControllerWrapper.h>
-
namespace devices::ethercat::hand::common
{
TYPEDEF_PTRS_SHARED(AbstractHand);
+
class AbstractHand
{
diff --git a/source/devices/ethercat/hand/common/AbstractHandControllerWrapper.cpp b/source/devices/ethercat/hand/common/AbstractHandControllerWrapper.cpp
index 014ba711..15974cb3 100644
--- a/source/devices/ethercat/hand/common/AbstractHandControllerWrapper.cpp
+++ b/source/devices/ethercat/hand/common/AbstractHandControllerWrapper.cpp
@@ -1,13 +1,12 @@
#include "AbstractHandControllerWrapper.h"
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/EndEffector/EndEffector.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/XML/BaseIO.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-
#include <ArmarXCore/core/time.h>
@@ -22,10 +21,11 @@ namespace devices::ethercat::hand::common
std::map<std::string, std::map<std::string, float>> preshapesByName;
auto robotToUse = robot;
- if(forceRobotFileReload)
+ if (forceRobotFileReload)
{
ARMARX_VERBOSE << "Reloading robot model `" << robot->getFilename() << "`";
- robotToUse = VirtualRobot::RobotIO::loadRobot(robot->getFilename(), VirtualRobot::BaseIO::eStructure);
+ robotToUse = VirtualRobot::RobotIO::loadRobot(robot->getFilename(),
+ VirtualRobot::BaseIO::eStructure);
}
const auto eef = robotToUse->getEndEffector(eefName);
@@ -54,7 +54,7 @@ namespace devices::ethercat::hand::common
return preshapesByName;
}
- void
+ void
AbstractHandControllerWrapper::reloadPreshapes()
{
ARMARX_INFO << "Reloading preshapes ...";
diff --git a/source/devices/ethercat/hand/soft_finger_vision/BoardIn.h b/source/devices/ethercat/hand/soft_finger_vision/BoardIn.h
index 7a816474..6412c4ab 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/BoardIn.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/BoardIn.h
@@ -21,7 +21,8 @@ namespace devices::ethercat::hand::finger_vision_soft
BoardIn_motor_target thumb;
BoardIn_motor_target index;
BoardIn_motor_target other;
- BoardIn_cnn_target cnn;
+ BoardIn_cnn_target cnn;
} __attribute__((__packed__));
+
static_assert(sizeof(BoardIn) == (64) / 8);
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/BoardOut.h b/source/devices/ethercat/hand/soft_finger_vision/BoardOut.h
index 2323b2ef..30a51f9f 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/BoardOut.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/BoardOut.h
@@ -30,9 +30,10 @@ namespace devices::ethercat::hand::finger_vision_soft
BoardOut_motor_value motor_thumb;
BoardOut_motor_value motor_index;
BoardOut_motor_value motor_other;
- std::uint32_t temperature;
+ std::uint32_t temperature;
BoardOut_diagnostics diagnostics;
- BoardOut_cnn_data cnn_data;
+ BoardOut_cnn_data cnn_data;
} __attribute__((__packed__));
+
static_assert(sizeof(BoardOut) == 836);
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/FunctionalDevice.cpp b/source/devices/ethercat/hand/soft_finger_vision/FunctionalDevice.cpp
index bca864fb..fadc7a9e 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/FunctionalDevice.cpp
+++ b/source/devices/ethercat/hand/soft_finger_vision/FunctionalDevice.cpp
@@ -1,17 +1,16 @@
-#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelperTemplates.h>
-#include <ArmarXCore/core/ManagedIceObject.h>
#include "joint_controller/CNNEmergencyStop.h"
#include "joint_controller/CNNStopMovement.h"
#include "joint_controller/CNNTargetObject.h"
#include "joint_controller/EmergencyStop.h"
#include "joint_controller/Position.h"
-#include "joint_controller/Velocity.h"
#include "joint_controller/Pwm.h"
#include "joint_controller/StopMovement.h"
-
+#include "joint_controller/Velocity.h"
#include "njoint_controller/Shape.h"
//HandSensorDevice
@@ -20,6 +19,7 @@ namespace devices::ethercat::hand::finger_vision_soft::detail
class HandSensorDevice : public SensorDevice
{
friend class FunctionalDevice;
+
public:
HandSensorDevice(const std::string& deviceName) :
DeviceBase(deviceName), SensorDevice(deviceName)
@@ -27,13 +27,14 @@ namespace devices::ethercat::hand::finger_vision_soft::detail
ARMARX_DEBUG << " ctor HandSensorDevice " << deviceName;
}
- const HandSensorValue* getSensorValue() const override
+ const HandSensorValue*
+ getSensorValue() const override
{
return &sensorValue;
}
- void init(const FunctionalDevicePtr& handPtr,
- const HandSensorDataPtr& dataPtr)
+ void
+ init(const FunctionalDevicePtr& handPtr, const HandSensorDataPtr& dataPtr)
{
ARMARX_DEBUG << " init HandSensorDevice " << getDeviceName();
ARMARX_CHECK_NOT_NULL(handPtr);
@@ -41,26 +42,29 @@ namespace devices::ethercat::hand::finger_vision_soft::detail
hand = handPtr;
data = dataPtr;
}
- void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration) override
+
+ void
+ rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override
{
data->rtReadSensorValues(sensorValuesTimestamp, timeSinceLastIteration);
data->updateSensorValueStruct(sensorValue);
}
+
private:
FunctionalDevicePtr hand;
HandSensorDataPtr data;
HandSensorValue sensorValue;
};
-}
+} // namespace devices::ethercat::hand::finger_vision_soft::detail
//MotorControlDevice
namespace devices::ethercat::hand::finger_vision_soft::detail
{
- class MotorControlDevice :
- public SensorDevice, public ControlDevice
+ class MotorControlDevice : public SensorDevice, public ControlDevice
{
friend class FunctionalDevice;
+
public:
MotorControlDevice(const std::string& deviceName) :
DeviceBase(deviceName), SensorDevice(deviceName), ControlDevice(deviceName)
@@ -68,15 +72,17 @@ namespace devices::ethercat::hand::finger_vision_soft::detail
ARMARX_DEBUG << " ctor MotorControlDevice " << deviceName;
}
- const SensorValueBase* getSensorValue() const override
+ const SensorValueBase*
+ getSensorValue() const override
{
return &sensorValue;
}
- void init(const FunctionalDevicePtr& handPtr,
- const HandSensorDevicePtr& devHandPtr,
- const HandSensorDataPtr& dataHandPtr,
- const MotorControlDataPtr& dataMotorPtr)
+ void
+ init(const FunctionalDevicePtr& handPtr,
+ const HandSensorDevicePtr& devHandPtr,
+ const HandSensorDataPtr& dataHandPtr,
+ const MotorControlDataPtr& dataMotorPtr)
{
ARMARX_TRACE;
ARMARX_DEBUG << " init MotorControlDevice " << getDeviceName();
@@ -92,20 +98,25 @@ namespace devices::ethercat::hand::finger_vision_soft::detail
ctrls.at(0).reset(new JointStopMovementController(dataMotor));
ctrls.at(1).reset(new JointEmergencyStopController(dataMotor));
ctrls.at(2).reset(new JointPwmController(dataMotor, hand));
- ctrls.at(3).reset(new JointPositionController(dataMotor, hand, getControlTargetAccessToken()));
- ctrls.at(4).reset(new JointVelocityController(dataMotor, hand, getControlTargetAccessToken()));
+ ctrls.at(3).reset(
+ new JointPositionController(dataMotor, hand, getControlTargetAccessToken()));
+ ctrls.at(4).reset(
+ new JointVelocityController(dataMotor, hand, getControlTargetAccessToken()));
for (const auto& ctrl : ctrls)
{
addJointController(ctrl.get());
}
}
- void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration) override
+
+ void
+ rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override
{
if (motorIsShutDown)
{
- motorIsShutDown = *handSensorTemp < dataHand->rtGetMotorTemperatureRestartThreshold();
+ motorIsShutDown =
+ *handSensorTemp < dataHand->rtGetMotorTemperatureRestartThreshold();
}
motorIsShutDown = *handSensorTemp >= dataHand->rtGetMotorTemperatureShutdownThreshold();
@@ -118,20 +129,23 @@ namespace devices::ethercat::hand::finger_vision_soft::detail
<< dataHand->rtGetMotorTemperatureShutdownThreshold()
<< ", threshold restart: "
<< dataHand->rtGetMotorTemperatureRestartThreshold()
- << ") - stopping motor - motor temperature: "
- << *handSensorTemp << " °C";
+ << ") - stopping motor - motor temperature: " << *handSensorTemp
+ << " °C";
}
dataMotor->setCommand(0);
}
dataMotor->rtWriteTargetValues(sensorValuesTimestamp, timeSinceLastIteration);
firstRun = false;
}
- void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration) override
+
+ void
+ rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override
{
dataMotor->rtReadSensorValues(sensorValuesTimestamp, timeSinceLastIteration);
dataMotor->updateSensorValueStruct(sensorValue);
}
+
private:
FunctionalDevicePtr hand;
@@ -145,15 +159,15 @@ namespace devices::ethercat::hand::finger_vision_soft::detail
bool motorIsShutDown = false;
bool firstRun = true;
};
-}
+} // namespace devices::ethercat::hand::finger_vision_soft::detail
//CNNControlDevice
namespace devices::ethercat::hand::finger_vision_soft::detail
{
- class CNNControlDevice :
- public ControlDevice
+ class CNNControlDevice : public ControlDevice
{
friend class FunctionalDevice;
+
public:
CNNControlDevice(const std::string& deviceName) :
DeviceBase(deviceName), ControlDevice(deviceName)
@@ -161,7 +175,8 @@ namespace devices::ethercat::hand::finger_vision_soft::detail
ARMARX_DEBUG << " ctor CNNControlDevice " << deviceName;
}
- void init(const CNNControlDataPtr& dataPtr)
+ void
+ init(const CNNControlDataPtr& dataPtr)
{
ARMARX_TRACE;
ARMARX_DEBUG << " init CNNControlDevice " << getDeviceName();
@@ -177,29 +192,33 @@ namespace devices::ethercat::hand::finger_vision_soft::detail
addJointController(ctrl.get());
}
}
- void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration) override
+
+ void
+ rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override
{
data->rtWriteTargetValues(sensorValuesTimestamp, timeSinceLastIteration);
}
+
private:
CNNControlDataPtr data;
std::array<std::unique_ptr<JointController>, 3> ctrls;
};
-}
-
+} // namespace devices::ethercat::hand::finger_vision_soft::detail
//ControllerWrapper
namespace devices::ethercat::hand::finger_vision_soft
{
TYPEDEF_PTRS_SHARED(ControllerWrapper);
+
class ControllerWrapper : public AbstractHandUnitControllerWrapper
{
public:
NJointShapeControllerPtr controller;
- void setShape(const std::string& shape) override
+ void
+ setShape(const std::string& shape) override
{
ARMARX_CHECK_EXPRESSION(controller);
if (!controller->isControllerActive())
@@ -228,19 +247,25 @@ namespace devices::ethercat::hand::finger_vision_soft
ARMARX_ERROR << "shape '" << shape << "' not supported.";
}
}
- std::string describeHandState() const override
+
+ std::string
+ describeHandState() const override
{
std::stringstream ss;
ss.precision(3);
ss << std::fixed;
- ss << controller->getIndexJointValue() << " " << controller->getIndexTarget() << " " << controller->getIndexPwm() << " / ";
- ss << controller->getOtherJointValue() << " " << controller->getOtherTarget() << " " << controller->getOtherPwm() << " / ";
- ss << controller->getThumbJointValue() << " " << controller->getThumbTarget() << " " << controller->getThumbPwm();
+ ss << controller->getIndexJointValue() << " " << controller->getIndexTarget() << " "
+ << controller->getIndexPwm() << " / ";
+ ss << controller->getOtherJointValue() << " " << controller->getOtherTarget() << " "
+ << controller->getOtherPwm() << " / ";
+ ss << controller->getThumbJointValue() << " " << controller->getThumbTarget() << " "
+ << controller->getThumbPwm();
ss << " " << (controller->isControlEnabled() ? "active" : "disabled");
return ss.str();
}
- void setJointAngles(const NameValueMap& targetJointAngles) override
+ void
+ setJointAngles(const NameValueMap& targetJointAngles) override
{
ARMARX_CHECK_EXPRESSION(controller);
if (!controller->isControllerActive())
@@ -249,7 +274,7 @@ namespace devices::ethercat::hand::finger_vision_soft
}
float indexTarget = controller->getIndexTarget();
- float lmfTarget = controller->getOtherTarget();
+ float lmfTarget = controller->getOtherTarget();
float thumbTarget = controller->getThumbTarget();
for (const auto& [name, value] : targetJointAngles)
{
@@ -273,7 +298,8 @@ namespace devices::ethercat::hand::finger_vision_soft
controller->setTargets(indexTarget, lmfTarget, thumbTarget);
}
- std::map<std::string, float> getActualJointValues() override
+ std::map<std::string, float>
+ getActualJointValues() override
{
NameValueMap jointValues;
jointValues["index"] = controller->getIndexJointValue();
@@ -282,34 +308,30 @@ namespace devices::ethercat::hand::finger_vision_soft
return jointValues;
}
};
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
//FunctionalDevice
namespace devices::ethercat::hand::finger_vision_soft
{
FunctionalDeviceFactory::SubClassRegistry
- FunctionalDevice::registry(
- "KITFingerVisionSoftHandV1",
- &FunctionalDeviceFactory::createInstance<FunctionalDevice>);
-
- FunctionalDevice::FunctionalDevice(
- RapidXmlReaderNode jointNode,
- DefaultRapidXmlReaderNode defaultConfigurationNode,
- VirtualRobot::RobotPtr const& robot
- ):
+ FunctionalDevice::registry("KITFingerVisionSoftHandV1",
+ &FunctionalDeviceFactory::createInstance<FunctionalDevice>);
+
+ FunctionalDevice::FunctionalDevice(RapidXmlReaderNode jointNode,
+ DefaultRapidXmlReaderNode defaultConfigurationNode,
+ VirtualRobot::RobotPtr const& robot) :
AbstractFunctionalDevice(
- defaultConfigurationNode
- .first_node("KITFingerVisionSoftHandV1DefaultConfiguration")
- .add_node_at_end(jointNode)),
+ defaultConfigurationNode.first_node("KITFingerVisionSoftHandV1DefaultConfiguration")
+ .add_node_at_end(jointNode)),
deviceName(jointNode.attribute_value("name")),
configNode(jointNode),
robot(robot),
slaveIdentifier(jointNode)
{
-
}
- void FunctionalDevice::init(SlavePtr slave)
+ void
+ FunctionalDevice::init(SlavePtr slave)
{
ARMARX_TRACE;
ARMARX_DEBUG << "init " << deviceName;
@@ -318,8 +340,8 @@ namespace devices::ethercat::hand::finger_vision_soft
if (slave->getSlaveIdentifier().ProductID != slaveIdentifier.ProductID)
{
std::stringstream reason;
- reason << "Invalid Serial Number for SensorBoard for arm joint : "
- << deviceName << "!\n"
+ reason << "Invalid Serial Number for SensorBoard for arm joint : " << deviceName
+ << "!\n"
<< "product ID in config: " << slaveIdentifier.ProductID << " "
<< "product ID from bus: " << slave->getSlaveIdentifier().ProductID;
throw LocalException(reason.str());
@@ -332,15 +354,16 @@ namespace devices::ethercat::hand::finger_vision_soft
using HDevT = detail::HandSensorDevice;
using CNNDevT = detail::CNNControlDevice;
- otherControlDevicePtr = armarx::make_shared<MDevT >(deviceName + ".motor.other");
- indexControlDevicePtr = armarx::make_shared<MDevT >(deviceName + ".motor.index");
- thumbControlDevicePtr = armarx::make_shared<MDevT >(deviceName + ".motor.thumb");
- handSensorDevicePtr = armarx::make_shared<HDevT >(deviceName);
- cnnControlDevicePtr = armarx::make_shared<CNNDevT>(deviceName + ".cnn");
+ otherControlDevicePtr = armarx::make_shared<MDevT>(deviceName + ".motor.other");
+ indexControlDevicePtr = armarx::make_shared<MDevT>(deviceName + ".motor.index");
+ thumbControlDevicePtr = armarx::make_shared<MDevT>(deviceName + ".motor.thumb");
+ handSensorDevicePtr = armarx::make_shared<HDevT>(deviceName);
+ cnnControlDevicePtr = armarx::make_shared<CNNDevT>(deviceName + ".cnn");
}
}
- void FunctionalDevice::initData()
+ void
+ FunctionalDevice::initData()
{
ARMARX_TRACE;
ARMARX_DEBUG << "initData " << deviceName;
@@ -352,13 +375,11 @@ namespace devices::ethercat::hand::finger_vision_soft
{
auto* ctrlAll = handSlave->getInputsPtr();
auto* snsAll = handSlave->getOutputsPtr();
- auto init = [&](auto & sd, auto * cf, auto * sf, auto name)
+ auto init = [&](auto& sd, auto* cf, auto* sf, auto name)
{
ARMARX_TRACE_LITE;
sd = std::make_shared<MotorControlData>(
- configNode, getNode(),
- cf, snsAll, sf,
- name);
+ configNode, getNode(), cf, snsAll, sf, name);
};
init(otherControlDataPtr, &ctrlAll->other, &snsAll->motor_other, "other");
init(indexControlDataPtr, &ctrlAll->index, &snsAll->motor_index, "index");
@@ -367,73 +388,80 @@ namespace devices::ethercat::hand::finger_vision_soft
//hand
{
handSensorDataPtr = std::make_shared<HandSensorData>(
- configNode, getNode(), handSlave->getOutputsPtr(), "Hand");
+ configNode, getNode(), handSlave->getOutputsPtr(), "Hand");
}
//cnn
{
- cnnControlDataPtr = std::make_shared<CNNControlData>(&(handSlave->getInputsPtr()->cnn));
+ cnnControlDataPtr =
+ std::make_shared<CNNControlData>(&(handSlave->getInputsPtr()->cnn));
}
}
//init devices
{
- handSensorDevicePtr ->init(hand, handSensorDataPtr);
+ handSensorDevicePtr->init(hand, handSensorDataPtr);
- otherControlDevicePtr->init(hand, handSensorDevicePtr, handSensorDataPtr, otherControlDataPtr);
- indexControlDevicePtr->init(hand, handSensorDevicePtr, handSensorDataPtr, indexControlDataPtr);
- thumbControlDevicePtr->init(hand, handSensorDevicePtr, handSensorDataPtr, thumbControlDataPtr);
+ otherControlDevicePtr->init(
+ hand, handSensorDevicePtr, handSensorDataPtr, otherControlDataPtr);
+ indexControlDevicePtr->init(
+ hand, handSensorDevicePtr, handSensorDataPtr, indexControlDataPtr);
+ thumbControlDevicePtr->init(
+ hand, handSensorDevicePtr, handSensorDataPtr, thumbControlDataPtr);
cnnControlDevicePtr->init(cnnControlDataPtr);
}
}
- SlaveIdentifier FunctionalDevice::getSlaveIdentifier() const
+ SlaveIdentifier
+ FunctionalDevice::getSlaveIdentifier() const
{
return slaveIdentifier;
}
- AbstractHandUnitControllerWrapperPtr FunctionalDevice::createHandUnitControllerWrapper(
- RobotUnit* robotUnit,
- const std::string& handName,
- RapidXmlReaderNode configNode) const
+ AbstractHandUnitControllerWrapperPtr
+ FunctionalDevice::createHandUnitControllerWrapper(RobotUnit* robotUnit,
+ const std::string& handName,
+ RapidXmlReaderNode configNode) const
{
ARMARX_TRACE;
ControllerWrapperPtr wrapper(new ControllerWrapper());
- NJointShapeControllerConfigPtr cfg =
- new NJointShapeControllerConfig(deviceName);
+ NJointShapeControllerConfigPtr cfg = new NJointShapeControllerConfig(deviceName);
ARMARX_INFO << "createHandUnitControllerWrapper " << VAROUT(deviceName);
- wrapper->controller = NJointShapeControllerPtr::dynamicCast(
- robotUnit->createNJointController("NJointKITFingerVisionSoftHandV1ShapeController",
- handName + "_NJointKITFingerVisionSoftHandV1ShapeController",
- cfg, false, true));
+ wrapper->controller =
+ NJointShapeControllerPtr::dynamicCast(robotUnit->createNJointController(
+ "NJointKITFingerVisionSoftHandV1ShapeController",
+ handName + "_NJointKITFingerVisionSoftHandV1ShapeController",
+ cfg,
+ false,
+ true));
ARMARX_TRACE;
- DefaultRapidXmlReaderNode defaultConfigurationNode
- {
- configNode
- .parent_node()
- .first_node("DefaultConfiguration")
- .first_node("KITFingerVisionSoftHandV1DefaultConfiguration")
- };
+ DefaultRapidXmlReaderNode defaultConfigurationNode{
+ configNode.parent_node()
+ .first_node("DefaultConfiguration")
+ .first_node("KITFingerVisionSoftHandV1DefaultConfiguration")};
wrapper->controller->readConfig(configNode, defaultConfigurationNode);
return wrapper;
}
- std::string FunctionalDevice::getHandConfigNodeName() const
+ std::string
+ FunctionalDevice::getHandConfigNodeName() const
{
return "KITFingerVisionSoftHandV1";
}
- std::string FunctionalDevice::getDeviceName() const
+ std::string
+ FunctionalDevice::getDeviceName() const
{
return deviceName;
}
- std::string FunctionalDevice::getHandDeviceName() const
+ std::string
+ FunctionalDevice::getHandDeviceName() const
{
return deviceName;
}
- void FunctionalDevice::addDevicesTo(
- const KITFingerVisionSoftHandV1FactoryPtr& f)
+ void
+ FunctionalDevice::addDevicesTo(const KITFingerVisionSoftHandV1FactoryPtr& f)
{
ARMARX_TRACE;
@@ -450,4 +478,4 @@ namespace devices::ethercat::hand::finger_vision_soft
f->addControlDevice(cnnControlDevicePtr);
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/FunctionalDevice.h b/source/devices/ethercat/hand/soft_finger_vision/FunctionalDevice.h
index cb61d961..14ad7d3f 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/FunctionalDevice.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/FunctionalDevice.h
@@ -5,34 +5,27 @@
#include <armarx/control/ethercat/AbstractDevice.h>
#include <armarx/control/ethercat/SlaveIdentifier.h>
+#include "Slave.h"
+#include "bus_data/CNN.h"
#include "bus_data/Hand.h"
#include "bus_data/Motor.h"
-#include "bus_data/CNN.h"
-#include "Slave.h"
-
#include <devices/ethercat/hand/common/AbstractHand.h>
namespace devices::ethercat::hand::finger_vision_soft::detail
{
- using MotorControlDevicePtr =
- std::shared_ptr<class MotorControlDevice>;
+ using MotorControlDevicePtr = std::shared_ptr<class MotorControlDevice>;
- using HandSensorDevicePtr =
- std::shared_ptr<class HandSensorDevice>;
+ using HandSensorDevicePtr = std::shared_ptr<class HandSensorDevice>;
- using CNNControlDevicePtr =
- std::shared_ptr<class CNNControlDevice>;
-}
+ using CNNControlDevicePtr = std::shared_ptr<class CNNControlDevice>;
+} // namespace devices::ethercat::hand::finger_vision_soft::detail
namespace devices::ethercat::hand::finger_vision_soft
{
- using FunctionalDevicePtr =
- std::shared_ptr<class FunctionalDevice>;
+ using FunctionalDevicePtr = std::shared_ptr<class FunctionalDevice>;
- class FunctionalDevice :
- public AbstractDevice,
- public AbstractHand
+ class FunctionalDevice : public AbstractDevice, public AbstractHand
{
public:
FunctionalDevice(RapidXmlReaderNode jointNode,
@@ -44,10 +37,12 @@ namespace devices::ethercat::hand::finger_vision_soft
SlaveIdentifier getSlaveIdentifier() const;
std::string getDeviceName() const;
+
public:
- AbstractHandUnitControllerWrapperPtr createHandUnitControllerWrapper(
- RobotUnit* robotUnit, const std::string& handName,
- RapidXmlReaderNode configNode) const override;
+ AbstractHandUnitControllerWrapperPtr
+ createHandUnitControllerWrapper(RobotUnit* robotUnit,
+ const std::string& handName,
+ RapidXmlReaderNode configNode) const override;
std::string getHandConfigNodeName() const override;
std::string getHandDeviceName() const override;
@@ -63,18 +58,18 @@ namespace devices::ethercat::hand::finger_vision_soft
MotorControlDataPtr indexControlDataPtr;
MotorControlDataPtr thumbControlDataPtr;
- CNNControlDataPtr cnnControlDataPtr;
+ CNNControlDataPtr cnnControlDataPtr;
- HandSensorDataPtr handSensorDataPtr;
+ HandSensorDataPtr handSensorDataPtr;
///device objects for sensor and control
detail::MotorControlDevicePtr otherControlDevicePtr;
detail::MotorControlDevicePtr indexControlDevicePtr;
detail::MotorControlDevicePtr thumbControlDevicePtr;
- detail::HandSensorDevicePtr handSensorDevicePtr;
+ detail::HandSensorDevicePtr handSensorDevicePtr;
- detail::CNNControlDevicePtr cnnControlDevicePtr;
+ detail::CNNControlDevicePtr cnnControlDevicePtr;
///bus devices - serial form config and pointer to actual bus slave
const SlaveIdentifier slaveIdentifier;
@@ -83,4 +78,4 @@ namespace devices::ethercat::hand::finger_vision_soft
IceUtil::Time lastReadUpdate, lastWriteUpdate;
};
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/Slave.cpp b/source/devices/ethercat/hand/soft_finger_vision/Slave.cpp
index 2ecbd674..25521803 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/Slave.cpp
+++ b/source/devices/ethercat/hand/soft_finger_vision/Slave.cpp
@@ -18,8 +18,7 @@ namespace devices::ethercat::hand::finger_vision_soft
{
inputs = nullptr;
outputs = nullptr;
- setTag("KITFingerVisionSoftHandV1Slave_" +
- slaveIdentifier.name); // TODO: in getDefaultName
+ setTag("KITFingerVisionSoftHandV1Slave_" + slaveIdentifier.name); // TODO: in getDefaultName
}
bool
@@ -46,29 +45,33 @@ namespace devices::ethercat::hand::finger_vision_soft
Slave::execute()
{
}
+
void
Slave::doMappings()
{
}
+
void
Slave::prepareForSafeOp()
{
}
+
void
Slave::finishPreparingForSafeOp()
{
}
+
void
Slave::prepareForOp()
{
}
+
void
Slave::finishPreparingForOp()
{
}
} // namespace devices::ethercat::hand::finger_vision_soft
-
namespace devices::ethercat::hand::finger_vision_soft
{
/**
diff --git a/source/devices/ethercat/hand/soft_finger_vision/Slave.h b/source/devices/ethercat/hand/soft_finger_vision/Slave.h
index 6f7a45cb..346bcf27 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/Slave.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/Slave.h
@@ -1,11 +1,11 @@
#pragma once
-#include <armarx/control/ethercat/AbstractSlave.h>
-#include <armarx/control/ethercat/DeviceFactory.h>
-
+#include <atomic>
#include <memory>
#include <thread>
-#include <atomic>
+
+#include <armarx/control/ethercat/AbstractSlave.h>
+#include <armarx/control/ethercat/DeviceFactory.h>
#include "BoardIn.h"
#include "BoardOut.h"
@@ -13,16 +13,16 @@
namespace devices::ethercat::hand::finger_vision_soft
{
const std::uint32_t H2T_RIGHT_PRODUCT_CODE = 0x42424243;
- const std::uint32_t H2T_LEFT_PRODUCT_CODE = 0x43434344;
+ const std::uint32_t H2T_LEFT_PRODUCT_CODE = 0x43434344;
using SlavePtr = std::shared_ptr<class Slave>;
- class Slave :
- public armarx::control::ethercat::AbstractSlaveWithInputOutput<BoardIn, BoardOut>
+ class Slave : public armarx::control::ethercat::AbstractSlaveWithInputOutput<BoardIn, BoardOut>
{
public:
- Slave(const armarx::control::ethercat::SlaveIdentifier slaveIdentifier, u_int16_t slaveNumber);
+ Slave(const armarx::control::ethercat::SlaveIdentifier slaveIdentifier,
+ u_int16_t slaveNumber);
void doMappings() override;
void prepareForOp() override;
@@ -44,15 +44,21 @@ namespace devices::ethercat::hand::finger_vision_soft
class KITFingerVisionSoftHandV1Factory : public armarx::control::ethercat::DeviceFactory
{
public:
- KITFingerVisionSoftHandV1Factory() {}
- static std::string getName()
+ KITFingerVisionSoftHandV1Factory()
+ {
+ }
+
+ static std::string
+ getName()
{
return "KITFingerVisionSoftHandV1Factory";
}
+
private:
static SubClassRegistry registry;
static SharedPointerType createInstance(FactoryArgs args);
};
+
using KITFingerVisionSoftHandV1FactoryPtr = std::shared_ptr<KITFingerVisionSoftHandV1Factory>;
-} // namespace devices::ethercat::hand::finger_vision_soft
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/bus_data/CNN.cpp b/source/devices/ethercat/hand/soft_finger_vision/bus_data/CNN.cpp
index 6b737a65..68eb8a6e 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/bus_data/CNN.cpp
+++ b/source/devices/ethercat/hand/soft_finger_vision/bus_data/CNN.cpp
@@ -1,29 +1,31 @@
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
#include "CNN.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
namespace devices::ethercat::hand::finger_vision_soft
{
- CNNControlData::CNNControlData(BoardIn_cnn_target* target) :
- target{target}
+ CNNControlData::CNNControlData(BoardIn_cnn_target* target) : target{target}
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(target);
}
- void CNNControlData::rtReadSensorValues(
- const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& /*timeSinceLastIteration*/)
- {}
+ void
+ CNNControlData::rtReadSensorValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
+ {
+ }
- void CNNControlData::rtWriteTargetValues(
- const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& /*timeSinceLastIteration*/)
- {}
+ void
+ CNNControlData::rtWriteTargetValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
+ {
+ }
- void CNNControlData::setId(int16 id)
+ void
+ CNNControlData::setId(int16 id)
{
target->object_id = id;
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/bus_data/CNN.h b/source/devices/ethercat/hand/soft_finger_vision/bus_data/CNN.h
index 6f1d4d1f..99f505f6 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/bus_data/CNN.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/bus_data/CNN.h
@@ -2,22 +2,18 @@
#include <memory>
-
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h>
#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h>
#include <RobotAPI/libraries/core/Pose.h>
-
#include <armarx/control/ethercat/AbstractData.h>
-
#include <armarx/control/rt_filters/MedianFilteredLinearConvertedValue.h>
-#include <devices/ethercat/common/Elmo/ElmoData.h>
-
-#include "../Slave.h"
#include "../SensorValue/Motor.h"
+#include "../Slave.h"
+#include <devices/ethercat/common/Elmo/ElmoData.h>
namespace devices::ethercat::hand::finger_vision_soft
{
@@ -33,9 +29,10 @@ namespace devices::ethercat::hand::finger_vision_soft
const IceUtil::Time& timeSinceLastIteration) override;
void setId(int16 id);
+
private:
BoardIn_cnn_target* target;
};
- using CNNControlDataPtr =
- std::shared_ptr<CNNControlData>;
-}
+
+ using CNNControlDataPtr = std::shared_ptr<CNNControlData>;
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/bus_data/Hand.cpp b/source/devices/ethercat/hand/soft_finger_vision/bus_data/Hand.cpp
index fa9c6915..9b242c34 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/bus_data/Hand.cpp
+++ b/source/devices/ethercat/hand/soft_finger_vision/bus_data/Hand.cpp
@@ -1,33 +1,29 @@
+#include "Hand.h"
+
#include <boost/algorithm/clamp.hpp>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "Hand.h"
+#include <ArmarXCore/core/logging/Logging.h>
namespace devices::ethercat::hand::finger_vision_soft
{
- HandSensorData::HandSensorData(
- const RapidXmlReaderNode& node,
- DefaultRapidXmlReaderNode defaultConfigurationNode,
- BoardOut* sensorAll,
- const std::string& configName
- ):
+ HandSensorData::HandSensorData(const RapidXmlReaderNode& node,
+ DefaultRapidXmlReaderNode defaultConfigurationNode,
+ BoardOut* sensorAll,
+ const std::string& configName) :
sensor{sensorAll}
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(sensorAll);
- auto cfg =
- defaultConfigurationNode
- .add_node_at_end(node)
- .first_node("Hand");
- ARMARX_DEBUG << "configName " << configName << "\nall config paths:\n" << cfg.getChildPaths();
+ auto cfg = defaultConfigurationNode.add_node_at_end(node).first_node("Hand");
+ ARMARX_DEBUG << "configName " << configName << "\nall config paths:\n"
+ << cfg.getChildPaths();
//hand_shutdown_threshold
{
const auto node = cfg.first_node("hand_shutdown_threshold");
node.attribute_as("shutdown_temp", motor_temp_threshold_shutdown);
- node.attribute_as("restart_temp", motor_temp_threshold_restart);
+ node.attribute_as("restart_temp", motor_temp_threshold_restart);
}
//sensor_hand_temp
{
@@ -40,28 +36,29 @@ namespace devices::ethercat::hand::finger_vision_soft
}
}
- void HandSensorData::rtReadSensorValues(
- const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& /*timeSinceLastIteration*/)
- {}
+ void
+ HandSensorData::rtReadSensorValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
+ {
+ }
- void HandSensorData::rtWriteTargetValues(
- const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& /*timeSinceLastIteration*/)
- {}
+ void
+ HandSensorData::rtWriteTargetValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
+ {
+ }
- void HandSensorData::updateSensorValueStruct(
- HandSensorValue& data)
+ void
+ HandSensorData::updateSensorValueStruct(HandSensorValue& data)
{
- data.frame_counter = sensor->diagnostics.sensor_frame_counter;
+ data.frame_counter = sensor->diagnostics.sensor_frame_counter;
data.update_rate_main = sensor->diagnostics.main_update_rate;
data.update_rate_fpga = sensor->diagnostics.fpga_update_rate;
- data.raw_temperature = sensor->temperature;
+ data.raw_temperature = sensor->temperature;
// a*exp(b*x)+c
data.temperature = temperature.update(
- temp_correction_a *
- std::exp(data.raw_temperature * temp_correction_b) +
- temp_correction_c);
+ temp_correction_a * std::exp(data.raw_temperature * temp_correction_b) +
+ temp_correction_c);
data.cnn_data = sensor->cnn_data.data;
//update popcount
std::size_t pops = 0;
@@ -130,4 +127,4 @@ namespace devices::ethercat::hand::finger_vision_soft
<< sensor->cnn_data.finger_number;
}
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/bus_data/Hand.h b/source/devices/ethercat/hand/soft_finger_vision/bus_data/Hand.h
index 5d4266ba..484c5170 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/bus_data/Hand.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/bus_data/Hand.h
@@ -2,39 +2,32 @@
#include <memory>
-
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h>
#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <armarx/control/ethercat/AbstractData.h>
-
#include <armarx/control/rt_filters/AverageFilter.h>
#include <armarx/control/rt_filters/FirFilter.h>
#include <armarx/control/rt_filters/RtMedianFilter.h>
-
-
-
-#include "../Slave.h"
#include "../SensorValue/Hand.h"
+#include "../Slave.h"
namespace devices::ethercat::hand::finger_vision_soft
{
class HandSensorData;
- using HandSensorDataPtr =
- std::shared_ptr<HandSensorData>;
+ using HandSensorDataPtr = std::shared_ptr<HandSensorData>;
class HandSensorData : public AbstractData
{
public:
- HandSensorData(
- const RapidXmlReaderNode& node,
- DefaultRapidXmlReaderNode defaultConfigurationNode,
- BoardOut* sensorAll,
- const std::string& configName);
+ HandSensorData(const RapidXmlReaderNode& node,
+ DefaultRapidXmlReaderNode defaultConfigurationNode,
+ BoardOut* sensorAll,
+ const std::string& configName);
void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration) override;
@@ -43,24 +36,26 @@ namespace devices::ethercat::hand::finger_vision_soft
void updateSensorValueStruct(HandSensorValue& data);
- float rtGetMotorTemperatureShutdownThreshold() const
+ float
+ rtGetMotorTemperatureShutdownThreshold() const
{
return motor_temp_threshold_shutdown;
}
- float rtGetMotorTemperatureRestartThreshold() const
+ float
+ rtGetMotorTemperatureRestartThreshold() const
{
return motor_temp_threshold_restart;
}
private:
const BoardOut* sensor;
- float motor_temp_threshold_shutdown;
- float motor_temp_threshold_restart;
+ float motor_temp_threshold_shutdown;
+ float motor_temp_threshold_restart;
// a*exp(b*x)+c
- float temp_correction_a;
- float temp_correction_b;
- float temp_correction_c;
- RtMedianFilter temperature;
+ float temp_correction_a;
+ float temp_correction_b;
+ float temp_correction_c;
+ RtMedianFilter temperature;
};
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/bus_data/Motor.cpp b/source/devices/ethercat/hand/soft_finger_vision/bus_data/Motor.cpp
index 72b0c103..01ff598a 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/bus_data/Motor.cpp
+++ b/source/devices/ethercat/hand/soft_finger_vision/bus_data/Motor.cpp
@@ -1,22 +1,19 @@
+#include "Motor.h"
+
#include <boost/algorithm/clamp.hpp>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "Motor.h"
+#include <ArmarXCore/core/logging/Logging.h>
namespace devices::ethercat::hand::finger_vision_soft
{
- MotorControlData::MotorControlData(
- const RapidXmlReaderNode& node,
- DefaultRapidXmlReaderNode defaultConfigurationNode,
- BoardIn_motor_target* target,
- BoardOut* sensorAll,
- BoardOut_motor_value* sensorMotor,
- const std::string& configName
- ) :
- target{target},
- sensor_motor{sensorMotor}
+ MotorControlData::MotorControlData(const RapidXmlReaderNode& node,
+ DefaultRapidXmlReaderNode defaultConfigurationNode,
+ BoardIn_motor_target* target,
+ BoardOut* sensorAll,
+ BoardOut_motor_value* sensorMotor,
+ const std::string& configName) :
+ target{target}, sensor_motor{sensorMotor}
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(target);
@@ -25,19 +22,15 @@ namespace devices::ethercat::hand::finger_vision_soft
{
ARMARX_TRACE;
- auto configNodeAll =
- defaultConfigurationNode
- .add_node_at_end(node)
- .first_node("Motor");
+ auto configNodeAll = defaultConfigurationNode.add_node_at_end(node).first_node("Motor");
- auto cfg = configNodeAll
- .first_node("default")
- .add_node_at_end(configNodeAll.first_node(configName));
+ auto cfg = configNodeAll.first_node("default").add_node_at_end(
+ configNodeAll.first_node(configName));
//low level pwm
{
auto cfg_pwm = cfg.first_node("ctrl_low_level_pwm");
- cfg_pwm.attribute_as("max", pwm_max);
+ cfg_pwm.attribute_as("max", pwm_max);
ARMARX_CHECK_GREATER_EQUAL(pwm_max, 0);
}
//ctrl_position
@@ -65,46 +58,48 @@ namespace devices::ethercat::hand::finger_vision_soft
ARMARX_CHECK_GREATER_EQUAL(vel_ctrl_default_max_pwm, 0);
}
- velocity .init(&sensorMotor->vel, cfg.first_node("sensor_motor_velocity"));
- relative_position.init(&sensorMotor->pos, cfg.first_node("sensor_motor_relative_position"));
+ velocity.init(&sensorMotor->vel, cfg.first_node("sensor_motor_velocity"));
+ relative_position.init(&sensorMotor->pos,
+ cfg.first_node("sensor_motor_relative_position"));
}
}
- void MotorControlData::rtReadSensorValues(
- const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& /*timeSinceLastIteration*/)
+ void
+ MotorControlData::rtReadSensorValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
{
- velocity .read();
+ velocity.read();
relative_position.read();
}
- void MotorControlData::rtWriteTargetValues(
- const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& /*timeSinceLastIteration*/)
+ void
+ MotorControlData::rtWriteTargetValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
{
target->pwm = target_pwm_motor;
}
- void MotorControlData::setCommand(int16 pwm)
+ void
+ MotorControlData::setCommand(int16 pwm)
{
- const auto pwm_clamped = boost::algorithm::clamp<std::int64_t>(
- pwm, -pwm_max, pwm_max);
+ const auto pwm_clamped = boost::algorithm::clamp<std::int64_t>(pwm, -pwm_max, pwm_max);
target_pwm_motor = pwm_clamped;
}
- std::int16_t MotorControlData::getMaxPWM() const
+ std::int16_t
+ MotorControlData::getMaxPWM() const
{
return pwm_max;
}
- void MotorControlData::updateSensorValueStruct(
- MotorSensorValue& data)
+ void
+ MotorControlData::updateSensorValueStruct(MotorSensorValue& data)
{
- data.velocity = velocity.value;
- data.position = relative_position.value;
- data.raw_velocity = sensor_motor->vel;
- data.raw_position = sensor_motor->pos;
- data.motorPWM = target_pwm_motor;
- data.pwm_max = pwm_max;
+ data.velocity = velocity.value;
+ data.position = relative_position.value;
+ data.raw_velocity = sensor_motor->vel;
+ data.raw_position = sensor_motor->pos;
+ data.motorPWM = target_pwm_motor;
+ data.pwm_max = pwm_max;
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/bus_data/Motor.h b/source/devices/ethercat/hand/soft_finger_vision/bus_data/Motor.h
index 08e18725..da45da3e 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/bus_data/Motor.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/bus_data/Motor.h
@@ -2,21 +2,18 @@
#include <memory>
-
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h>
#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h>
#include <RobotAPI/libraries/core/Pose.h>
-
#include <armarx/control/ethercat/AbstractData.h>
#include <armarx/control/rt_filters/MedianFilteredLinearConvertedValue.h>
-#include <devices/ethercat/common/Elmo/ElmoData.h>
-
-#include "../Slave.h"
#include "../SensorValue/Motor.h"
+#include "../Slave.h"
+#include <devices/ethercat/common/Elmo/ElmoData.h>
namespace devices::ethercat::hand::finger_vision_soft
{
@@ -24,14 +21,12 @@ namespace devices::ethercat::hand::finger_vision_soft
{
public:
- MotorControlData(
- const RapidXmlReaderNode& node,
- DefaultRapidXmlReaderNode defaultConfigurationNode,
- BoardIn_motor_target* target,
- BoardOut* sensorAll,
- BoardOut_motor_value* sensorMotor,
- const std::string& configName
- );
+ MotorControlData(const RapidXmlReaderNode& node,
+ DefaultRapidXmlReaderNode defaultConfigurationNode,
+ BoardIn_motor_target* target,
+ BoardOut* sensorAll,
+ BoardOut_motor_value* sensorMotor,
+ const std::string& configName);
void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration) override;
@@ -41,66 +36,82 @@ namespace devices::ethercat::hand::finger_vision_soft
void setCommand(int16 pwm);
int16_t getMaxPWM() const;
- float getPosCtrlKp() const
+ float
+ getPosCtrlKp() const
{
return pos_ctrl_Kp;
}
- float getPosCtrlKi() const
+
+ float
+ getPosCtrlKi() const
{
return pos_ctrl_Ki;
}
- float getPosCtrlKd() const
+
+ float
+ getPosCtrlKd() const
{
return pos_ctrl_Kd;
}
- std::int16_t getPosCtrlDefaultMaxPWM() const
+
+ std::int16_t
+ getPosCtrlDefaultMaxPWM() const
{
return pos_ctrl_default_max_pwm;
}
- float getVelCtrlKp() const
+ float
+ getVelCtrlKp() const
{
return vel_ctrl_Kp;
}
- float getVelCtrlKi() const
+
+ float
+ getVelCtrlKi() const
{
return vel_ctrl_Ki;
}
- float getVelCtrlKd() const
+
+ float
+ getVelCtrlKd() const
{
return vel_ctrl_Kd;
}
- std::int16_t getVelCtrlDefaultMaxPWM() const
+
+ std::int16_t
+ getVelCtrlDefaultMaxPWM() const
{
return vel_ctrl_default_max_pwm;
}
- float getRelativeEncoderValue() const
+ float
+ getRelativeEncoderValue() const
{
return relative_position.value;
}
void updateSensorValueStruct(MotorSensorValue& data);
+
private:
- BoardIn_motor_target* target;
- const BoardOut_motor_value* sensor_motor;
+ BoardIn_motor_target* target;
+ const BoardOut_motor_value* sensor_motor;
MedianFilteredLinearConvertedValue<int32_t> velocity;
MedianFilteredLinearConvertedValue<int32_t> relative_position;
- float pos_ctrl_Kp = 10;
- float pos_ctrl_Ki = 1;
- float pos_ctrl_Kd = 0;
+ float pos_ctrl_Kp = 10;
+ float pos_ctrl_Ki = 1;
+ float pos_ctrl_Kd = 0;
std::int16_t pos_ctrl_default_max_pwm;
- float vel_ctrl_Kp = 10;
- float vel_ctrl_Ki = 1;
- float vel_ctrl_Kd = 0;
+ float vel_ctrl_Kp = 10;
+ float vel_ctrl_Ki = 1;
+ float vel_ctrl_Kd = 0;
std::int16_t vel_ctrl_default_max_pwm;
std::int16_t pwm_max;
// target values set from outside
int16_t target_pwm_motor;
};
- using MotorControlDataPtr =
- std::shared_ptr<MotorControlData>;
-}
+
+ using MotorControlDataPtr = std::shared_ptr<MotorControlData>;
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/control_target/CNNObjectId.h b/source/devices/ethercat/hand/soft_finger_vision/control_target/CNNObjectId.h
index 515df11f..ef80ec7c 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/control_target/CNNObjectId.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/control_target/CNNObjectId.h
@@ -22,49 +22,64 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h>
#include <ArmarXCore/observers/variant/Variant.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h>
namespace devices::ethercat::hand::finger_vision_soft::ControlModes
{
//'normal' actor modes
static const std::string CNNObjectId = "ControlMode_CNNObjectId";
-}
+} // namespace devices::ethercat::hand::finger_vision_soft::ControlModes
+
namespace devices::ethercat::hand::finger_vision_soft
{
- class ControlTargetCNNObjectId: public ControlTargetBase
+ class ControlTargetCNNObjectId : public ControlTargetBase
{
public:
std::int16_t object_id = 0;
ControlTargetCNNObjectId() = default;
ControlTargetCNNObjectId(const ControlTargetCNNObjectId&) = default;
ControlTargetCNNObjectId(ControlTargetCNNObjectId&&) = default;
- ControlTargetCNNObjectId(std::int16_t val) : object_id{val} {}
+
+ ControlTargetCNNObjectId(std::int16_t val) : object_id{val}
+ {
+ }
+
ControlTargetCNNObjectId& operator=(const ControlTargetCNNObjectId&) = default;
ControlTargetCNNObjectId& operator=(ControlTargetCNNObjectId&&) = default;
- ControlTargetCNNObjectId& operator=(std::int16_t val)
+
+ ControlTargetCNNObjectId&
+ operator=(std::int16_t val)
{
object_id = val;
return *this;
}
- virtual const std::string& getControlMode() const override
+
+ virtual const std::string&
+ getControlMode() const override
{
return ControlModes::CNNObjectId;
}
- virtual void reset() override
+
+ virtual void
+ reset() override
{
object_id = 0;
}
- virtual bool isValid() const override
+
+ virtual bool
+ isValid() const override
{
return true;
}
- DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
- static ControlTargetInfo<ControlTargetCNNObjectId> GetClassMemberInfo()
+
+ DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION static ControlTargetInfo<
+ ControlTargetCNNObjectId>
+ GetClassMemberInfo()
{
ControlTargetInfo<ControlTargetCNNObjectId> cti;
cti.addMemberVariable(&ControlTargetCNNObjectId::object_id, "object_id");
return cti;
}
};
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/controller/PositionToPWM.cpp b/source/devices/ethercat/hand/soft_finger_vision/controller/PositionToPWM.cpp
index 8e900527..3c2267d7 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/controller/PositionToPWM.cpp
+++ b/source/devices/ethercat/hand/soft_finger_vision/controller/PositionToPWM.cpp
@@ -1,25 +1,25 @@
-#include <boost/algorithm/clamp.hpp>
-
#include "PositionToPWM.h"
+#include <boost/algorithm/clamp.hpp>
+
namespace devices::ethercat::hand::finger_vision_soft::Controllers
{
- PositionToPWM::PositionToPWM(float p, float i, float d) :
- PID{p, i, d}
+ PositionToPWM::PositionToPWM(float p, float i, float d) : PID{p, i, d}
{
PID.threadSafe = false;
}
- void PositionToPWM::reset()
+ void
+ PositionToPWM::reset()
{
PID.reset();
}
- int16_t PositionToPWM::calculate(float curr_pos, float targ_pos, float dt, int16_t maxPWM)
+ int16_t
+ PositionToPWM::calculate(float curr_pos, float targ_pos, float dt, int16_t maxPWM)
{
PID.update(dt, curr_pos, targ_pos);
return boost::algorithm::clamp(
- static_cast<std::int16_t>(PID.getControlValue() * maxPWM),
- -maxPWM, maxPWM);
+ static_cast<std::int16_t>(PID.getControlValue() * maxPWM), -maxPWM, maxPWM);
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft::Controllers
diff --git a/source/devices/ethercat/hand/soft_finger_vision/controller/PositionToPWM.h b/source/devices/ethercat/hand/soft_finger_vision/controller/PositionToPWM.h
index 8e780d5a..b8d838fc 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/controller/PositionToPWM.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/controller/PositionToPWM.h
@@ -18,4 +18,4 @@ namespace devices::ethercat::hand::finger_vision_soft::Controllers
std::int16_t calculate(float curr_pos, float targ_pos, float dt, std::int16_t maxPWM);
PIDController PID;
};
-}
+} // namespace devices::ethercat::hand::finger_vision_soft::Controllers
diff --git a/source/devices/ethercat/hand/soft_finger_vision/controller/VelocityToPWM.cpp b/source/devices/ethercat/hand/soft_finger_vision/controller/VelocityToPWM.cpp
index 02e5e10a..99561536 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/controller/VelocityToPWM.cpp
+++ b/source/devices/ethercat/hand/soft_finger_vision/controller/VelocityToPWM.cpp
@@ -1,24 +1,24 @@
-#include <boost/algorithm/clamp.hpp>
-
#include "VelocityToPWM.h"
+#include <boost/algorithm/clamp.hpp>
+
namespace devices::ethercat::hand::finger_vision_soft::Controllers
{
- VelocityToPWM::VelocityToPWM(float p, float i, float d) :
- pos_ctrl{p, i, d}
- {}
+ VelocityToPWM::VelocityToPWM(float p, float i, float d) : pos_ctrl{p, i, d}
+ {
+ }
- void VelocityToPWM::reset(float pos)
+ void
+ VelocityToPWM::reset(float pos)
{
pos_ctrl.reset();
- virtual_pos_target = pos;
+ virtual_pos_target = pos;
}
- int16_t VelocityToPWM::calculate(float curr_pos, float velocity, float dt, int16_t maxPWM)
+ int16_t
+ VelocityToPWM::calculate(float curr_pos, float velocity, float dt, int16_t maxPWM)
{
- virtual_pos_target = boost::algorithm::clamp(
- virtual_pos_target + velocity * dt,
- 0, 1);
+ virtual_pos_target = boost::algorithm::clamp(virtual_pos_target + velocity * dt, 0, 1);
return pos_ctrl.calculate(curr_pos, virtual_pos_target, dt, maxPWM);
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft::Controllers
diff --git a/source/devices/ethercat/hand/soft_finger_vision/controller/VelocityToPWM.h b/source/devices/ethercat/hand/soft_finger_vision/controller/VelocityToPWM.h
index fb0abbfb..bd4d1192 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/controller/VelocityToPWM.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/controller/VelocityToPWM.h
@@ -17,6 +17,6 @@ namespace devices::ethercat::hand::finger_vision_soft::Controllers
void reset(float pos);
std::int16_t calculate(float curr_pos, float velocity, float dt, std::int16_t maxPWM);
PositionToPWM pos_ctrl;
- float virtual_pos_target = 0;
+ float virtual_pos_target = 0;
};
-}
+} // namespace devices::ethercat::hand::finger_vision_soft::Controllers
diff --git a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNEmergencyStop.cpp b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNEmergencyStop.cpp
index d1010270..91fdc2a1 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNEmergencyStop.cpp
+++ b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNEmergencyStop.cpp
@@ -3,18 +3,22 @@
namespace devices::ethercat::hand::finger_vision_soft
{
JointCNNEmergencyStopController::JointCNNEmergencyStopController()
- {}
- void JointCNNEmergencyStopController::rtRun(
- const IceUtil::Time&, const IceUtil::Time&)
{
}
- ControlTargetBase* JointCNNEmergencyStopController::getControlTarget()
+ void
+ JointCNNEmergencyStopController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
+ {
+ }
+
+ ControlTargetBase*
+ JointCNNEmergencyStopController::getControlTarget()
{
return ⌖
}
- void JointCNNEmergencyStopController::rtPreActivateController()
+ void
+ JointCNNEmergencyStopController::rtPreActivateController()
{
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNEmergencyStop.h b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNEmergencyStop.h
index 02d6eea4..15bec0a3 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNEmergencyStop.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNEmergencyStop.h
@@ -8,16 +8,18 @@ namespace devices::ethercat::hand::finger_vision_soft
{
public:
JointCNNEmergencyStopController();
+
private:
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
ControlTargetBase* getControlTarget() override;
void rtPreActivateController() override;
+
private:
DummyControlTargetEmergencyStop target;
};
-
- using JointCNNEmergencyStopControllerPtr =
- std::shared_ptr<JointCNNEmergencyStopController>;
-}
+ using JointCNNEmergencyStopControllerPtr = std::shared_ptr<JointCNNEmergencyStopController>;
+
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNStopMovement.cpp b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNStopMovement.cpp
index a43e586c..5de52ad4 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNStopMovement.cpp
+++ b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNStopMovement.cpp
@@ -3,19 +3,22 @@
namespace devices::ethercat::hand::finger_vision_soft
{
JointCNNStopMovementController::JointCNNStopMovementController()
- {}
+ {
+ }
- void JointCNNStopMovementController::rtRun(
- const IceUtil::Time&, const IceUtil::Time&)
+ void
+ JointCNNStopMovementController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
{
}
- ControlTargetBase* JointCNNStopMovementController::getControlTarget()
+ ControlTargetBase*
+ JointCNNStopMovementController::getControlTarget()
{
return ⌖
}
- void JointCNNStopMovementController::rtPreActivateController()
+ void
+ JointCNNStopMovementController::rtPreActivateController()
{
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNStopMovement.h b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNStopMovement.h
index 328ca464..a2ab1205 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNStopMovement.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNStopMovement.h
@@ -12,8 +12,10 @@ namespace devices::ethercat::hand::finger_vision_soft
const IceUtil::Time& timeSinceLastIteration) override;
ControlTargetBase* getControlTarget() override;
void rtPreActivateController() override;
+
private:
DummyControlTargetStopMovement target;
};
+
using JointCNNStopMovementControllerPtr = std::shared_ptr<JointCNNStopMovementController>;
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNTargetObject.cpp b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNTargetObject.cpp
index 3780421b..02aa6f8d 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNTargetObject.cpp
+++ b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNTargetObject.cpp
@@ -2,29 +2,32 @@
namespace devices::ethercat::hand::finger_vision_soft
{
- JointCNNTargetObjectController::JointCNNTargetObjectController(
- CNNControlDataPtr data) :
+ JointCNNTargetObjectController::JointCNNTargetObjectController(CNNControlDataPtr data) :
data(data)
{
ARMARX_CHECK_NOT_NULL(data);
}
- ControlTargetBase* JointCNNTargetObjectController::getControlTarget()
+ ControlTargetBase*
+ JointCNNTargetObjectController::getControlTarget()
{
return ⌖
}
- void JointCNNTargetObjectController::rtRun(
- const IceUtil::Time&, const IceUtil::Time&)
+ void
+ JointCNNTargetObjectController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
{
data->setId(target.object_id);
}
- void JointCNNTargetObjectController::rtPreActivateController()
- {}
+ void
+ JointCNNTargetObjectController::rtPreActivateController()
+ {
+ }
- void JointCNNTargetObjectController::rtPostDeactivateController()
+ void
+ JointCNNTargetObjectController::rtPostDeactivateController()
{
data->setId(0);
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNTargetObject.h b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNTargetObject.h
index f76c091e..1abfb27c 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNTargetObject.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/CNNTargetObject.h
@@ -1,15 +1,16 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include "../FunctionalDevice.h"
#include "../bus_data/CNN.h"
#include "../control_target/CNNObjectId.h"
-#include "../FunctionalDevice.h"
namespace devices::ethercat::hand::finger_vision_soft
{
using FunctionalDevicePtr = std::shared_ptr<class FunctionalDevice>;
+
class JointCNNTargetObjectController : public JointController
{
public:
@@ -19,9 +20,11 @@ namespace devices::ethercat::hand::finger_vision_soft
const IceUtil::Time& timeSinceLastIteration) override;
void rtPreActivateController() override;
void rtPostDeactivateController() override;
+
private:
- CNNControlDataPtr data;
+ CNNControlDataPtr data;
ControlTargetCNNObjectId target;
};
+
using JointCNNTargetObjectControllerPtr = std::shared_ptr<JointCNNTargetObjectController>;
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/EmergencyStop.cpp b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/EmergencyStop.cpp
index 5c0268ae..f1c93d29 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/EmergencyStop.cpp
+++ b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/EmergencyStop.cpp
@@ -2,23 +2,26 @@
namespace devices::ethercat::hand::finger_vision_soft
{
- JointEmergencyStopController::JointEmergencyStopController(
- MotorControlDataPtr dataPtr) :
+ JointEmergencyStopController::JointEmergencyStopController(MotorControlDataPtr dataPtr) :
dataPtr(dataPtr)
- {}
- void JointEmergencyStopController::rtRun(
- const IceUtil::Time&, const IceUtil::Time&)
+ {
+ }
+
+ void
+ JointEmergencyStopController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
{
dataPtr->setCommand(0);
}
- ControlTargetBase* JointEmergencyStopController::getControlTarget()
+ ControlTargetBase*
+ JointEmergencyStopController::getControlTarget()
{
return ⌖
}
- void JointEmergencyStopController::rtPreActivateController()
+ void
+ JointEmergencyStopController::rtPreActivateController()
{
dataPtr->setCommand(0);
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/EmergencyStop.h b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/EmergencyStop.h
index e0e47fea..7ec3f3bf 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/EmergencyStop.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/EmergencyStop.h
@@ -9,17 +9,19 @@ namespace devices::ethercat::hand::finger_vision_soft
class JointEmergencyStopController : public JointController
{
public:
- JointEmergencyStopController(
- MotorControlDataPtr dataPtr);
+ JointEmergencyStopController(MotorControlDataPtr dataPtr);
+
private:
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
ControlTargetBase* getControlTarget() override;
void rtPreActivateController() override;
+
private:
DummyControlTargetEmergencyStop target;
MotorControlDataPtr dataPtr;
};
- using JointEmergencyStopControllerPtr =
- std::shared_ptr<JointEmergencyStopController>;
-}
+
+ using JointEmergencyStopControllerPtr = std::shared_ptr<JointEmergencyStopController>;
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Position.cpp b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Position.cpp
index 7d155b6e..6453040c 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Position.cpp
+++ b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Position.cpp
@@ -1,7 +1,8 @@
+#include "Position.h"
+
#include <RobotAPI/libraries/core/math/MathUtils.h>
#include "../FunctionalDevice.h"
-#include "Position.h"
namespace devices::ethercat::hand::finger_vision_soft
{
@@ -18,32 +19,33 @@ namespace devices::ethercat::hand::finger_vision_soft
target.setPWMLimits(data->getMaxPWM(), data->getPosCtrlDefaultMaxPWM(), token);
}
- ControlTargetBase* JointPositionController::getControlTarget()
+ ControlTargetBase*
+ JointPositionController::getControlTarget()
{
return ⌖
}
- void JointPositionController::rtRun(
- const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& timeSinceLastIteration)
+ void
+ JointPositionController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& timeSinceLastIteration)
{
ARMARX_CHECK_EXPRESSION(target.isValid());
- last_pwm = ctrl.calculate(
- data->getRelativeEncoderValue(),
- target.position,
- timeSinceLastIteration.toSecondsDouble(),
- std::min<std::int16_t>(data->getMaxPWM(), target.maxPWM)
- );
+ last_pwm = ctrl.calculate(data->getRelativeEncoderValue(),
+ target.position,
+ timeSinceLastIteration.toSecondsDouble(),
+ std::min<std::int16_t>(data->getMaxPWM(), target.maxPWM));
data->setCommand(last_pwm);
}
- void JointPositionController::rtPreActivateController()
+ void
+ JointPositionController::rtPreActivateController()
{
ctrl.reset();
}
- void JointPositionController::rtPostDeactivateController()
+ void
+ JointPositionController::rtPostDeactivateController()
{
data->setCommand(last_pwm);
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Position.h b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Position.h
index 4d70dba7..65cf1eb3 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Position.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Position.h
@@ -1,10 +1,10 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include "../bus_data/Motor.h"
#include "../FunctionalDevice.h"
+#include "../bus_data/Motor.h"
#include "../controller/PositionToPWM.h"
#include "../joint_controller/Pwm.h"
@@ -13,21 +13,22 @@ namespace devices::ethercat::hand::finger_vision_soft
class JointPositionController : public JointController
{
public:
- JointPositionController(
- MotorControlDataPtr data,
- FunctionalDevicePtr hand,
- ControlTargetBase::ControlDeviceAccessToken token);
+ JointPositionController(MotorControlDataPtr data,
+ FunctionalDevicePtr hand,
+ ControlTargetBase::ControlDeviceAccessToken token);
ControlTargetBase* getControlTarget() override;
void rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration) override;
void rtPreActivateController() override;
void rtPostDeactivateController() override;
+
private:
- MotorControlDataPtr data;
- FunctionalDevicePtr hand;
+ MotorControlDataPtr data;
+ FunctionalDevicePtr hand;
ControlTarget1DoFActuatorPositionWithPWMLimit target;
- Controllers::PositionToPWM ctrl;
- std::int16_t last_pwm;
+ Controllers::PositionToPWM ctrl;
+ std::int16_t last_pwm;
};
+
using JointPositionControllerPtr = std::shared_ptr<JointPositionController>;
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Pwm.cpp b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Pwm.cpp
index b35b5f20..92e3e222 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Pwm.cpp
+++ b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Pwm.cpp
@@ -2,32 +2,33 @@
namespace devices::ethercat::hand::finger_vision_soft
{
- JointPwmController::JointPwmController(
- MotorControlDataPtr data,
- FunctionalDevicePtr hand) :
- data(data),
- hand(hand)
+ JointPwmController::JointPwmController(MotorControlDataPtr data, FunctionalDevicePtr hand) :
+ data(data), hand(hand)
{
ARMARX_CHECK_NOT_NULL(data);
ARMARX_CHECK_NOT_NULL(hand);
}
- ControlTargetBase* JointPwmController::getControlTarget()
+ ControlTargetBase*
+ JointPwmController::getControlTarget()
{
return ⌖
}
- void JointPwmController::rtRun(
- const IceUtil::Time&, const IceUtil::Time&)
+ void
+ JointPwmController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
{
data->setCommand(target.pwm);
}
- void JointPwmController::rtPreActivateController()
- {}
+ void
+ JointPwmController::rtPreActivateController()
+ {
+ }
- void JointPwmController::rtPostDeactivateController()
+ void
+ JointPwmController::rtPostDeactivateController()
{
data->setCommand(0);
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Pwm.h b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Pwm.h
index 2e6470e6..a51aa231 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Pwm.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Pwm.h
@@ -1,29 +1,30 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include "../bus_data/Motor.h"
#include "../FunctionalDevice.h"
+#include "../bus_data/Motor.h"
namespace devices::ethercat::hand::finger_vision_soft
{
using FunctionalDevicePtr = std::shared_ptr<class FunctionalDevice>;
+
class JointPwmController : public JointController
{
public:
- JointPwmController(
- MotorControlDataPtr data,
- FunctionalDevicePtr hand);
+ JointPwmController(MotorControlDataPtr data, FunctionalDevicePtr hand);
ControlTargetBase* getControlTarget() override;
void rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration) override;
void rtPreActivateController() override;
void rtPostDeactivateController() override;
+
private:
- MotorControlDataPtr data;
- FunctionalDevicePtr hand;
+ MotorControlDataPtr data;
+ FunctionalDevicePtr hand;
ControlTarget1DoFActuatorPWM target;
};
+
using JointPwmControllerPtr = std::shared_ptr<JointPwmController>;
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/StopMovement.cpp b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/StopMovement.cpp
index f0df0383..6b0b1526 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/StopMovement.cpp
+++ b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/StopMovement.cpp
@@ -2,24 +2,26 @@
namespace devices::ethercat::hand::finger_vision_soft
{
- JointStopMovementController::JointStopMovementController(
- MotorControlDataPtr dataPtr) :
+ JointStopMovementController::JointStopMovementController(MotorControlDataPtr dataPtr) :
dataPtr(dataPtr)
- {}
+ {
+ }
- void JointStopMovementController::rtRun(
- const IceUtil::Time&, const IceUtil::Time&)
+ void
+ JointStopMovementController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
{
dataPtr->setCommand(0);
}
- ControlTargetBase* JointStopMovementController::getControlTarget()
+ ControlTargetBase*
+ JointStopMovementController::getControlTarget()
{
return ⌖
}
- void JointStopMovementController::rtPreActivateController()
+ void
+ JointStopMovementController::rtPreActivateController()
{
dataPtr->setCommand(0);
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/StopMovement.h b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/StopMovement.h
index c25b06bf..8f8971a6 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/StopMovement.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/StopMovement.h
@@ -9,15 +9,16 @@ namespace devices::ethercat::hand::finger_vision_soft
class JointStopMovementController : public JointController
{
public:
- JointStopMovementController(
- MotorControlDataPtr dataPtr);
+ JointStopMovementController(MotorControlDataPtr dataPtr);
void rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration) override;
ControlTargetBase* getControlTarget() override;
void rtPreActivateController() override;
+
private:
DummyControlTargetStopMovement target;
MotorControlDataPtr dataPtr;
};
+
using JointStopMovementControllerPtr = std::shared_ptr<JointStopMovementController>;
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Velocity.cpp b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Velocity.cpp
index c0985103..6ae38adc 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Velocity.cpp
+++ b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Velocity.cpp
@@ -1,9 +1,10 @@
+#include "Velocity.h"
+
#include <boost/algorithm/clamp.hpp>
#include <RobotAPI/libraries/core/math/MathUtils.h>
#include "../FunctionalDevice.h"
-#include "Velocity.h"
namespace devices::ethercat::hand::finger_vision_soft
{
@@ -20,33 +21,34 @@ namespace devices::ethercat::hand::finger_vision_soft
target.setPWMLimits(data->getMaxPWM(), data->getVelCtrlDefaultMaxPWM(), token);
}
- ControlTargetBase* JointVelocityController::getControlTarget()
+ ControlTargetBase*
+ JointVelocityController::getControlTarget()
{
return ⌖
}
- void JointVelocityController::rtRun(
- const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& timeSinceLastIteration)
+ void
+ JointVelocityController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& timeSinceLastIteration)
{
ARMARX_CHECK_EXPRESSION(target.isValid());
- last_pwm = ctrl.calculate(
- data->getRelativeEncoderValue(),
- target.velocity,
- timeSinceLastIteration.toSecondsDouble(),
- std::min<std::int16_t>(data->getMaxPWM(), target.maxPWM)
- );
+ last_pwm = ctrl.calculate(data->getRelativeEncoderValue(),
+ target.velocity,
+ timeSinceLastIteration.toSecondsDouble(),
+ std::min<std::int16_t>(data->getMaxPWM(), target.maxPWM));
data->setCommand(last_pwm);
}
- void JointVelocityController::rtPreActivateController()
+ void
+ JointVelocityController::rtPreActivateController()
{
ctrl.reset(data->getRelativeEncoderValue());
}
- void JointVelocityController::rtPostDeactivateController()
+ void
+ JointVelocityController::rtPostDeactivateController()
{
data->setCommand(last_pwm);
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Velocity.h b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Velocity.h
index 6d755d6d..531b6e2f 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Velocity.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/joint_controller/Velocity.h
@@ -1,11 +1,11 @@
#pragma once
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/libraries/core/PIDController.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include "../bus_data/Motor.h"
#include "../FunctionalDevice.h"
+#include "../bus_data/Motor.h"
#include "../controller/VelocityToPWM.h"
#include "../joint_controller/Pwm.h"
@@ -14,22 +14,22 @@ namespace devices::ethercat::hand::finger_vision_soft
class JointVelocityController : public JointController
{
public:
- JointVelocityController(
- MotorControlDataPtr data,
- FunctionalDevicePtr hand,
- ControlTargetBase::ControlDeviceAccessToken token);
+ JointVelocityController(MotorControlDataPtr data,
+ FunctionalDevicePtr hand,
+ ControlTargetBase::ControlDeviceAccessToken token);
ControlTargetBase* getControlTarget() override;
void rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration) override;
void rtPreActivateController() override;
void rtPostDeactivateController() override;
+
private:
- MotorControlDataPtr data;
- FunctionalDevicePtr hand;
+ MotorControlDataPtr data;
+ FunctionalDevicePtr hand;
ControlTarget1DoFActuatorVelocityWithPWMLimit target;
- Controllers::VelocityToPWM ctrl;
- std::int16_t last_pwm;
+ Controllers::VelocityToPWM ctrl;
+ std::int16_t last_pwm;
};
- using JointVelocityControllerPtr =
- std::shared_ptr<JointVelocityController>;
-}
+
+ using JointVelocityControllerPtr = std::shared_ptr<JointVelocityController>;
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/GraspOnDetection.cpp b/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/GraspOnDetection.cpp
index a20c2ed7..ef9d9dfa 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/GraspOnDetection.cpp
+++ b/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/GraspOnDetection.cpp
@@ -1,34 +1,45 @@
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-
#include "GraspOnDetection.h"
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+
//register
namespace devices::ethercat::hand::finger_vision_soft
{
NJointControllerRegistration<GraspOnDetection_NJointController>
- registrationControllerGraspOnDetection_NJointControllers("KITFingerVisionSoftHandV1_GraspOnDetection_NJointController");
+ registrationControllerGraspOnDetection_NJointControllers(
+ "KITFingerVisionSoftHandV1_GraspOnDetection_NJointController");
- GraspOnDetection_NJointController::ControlData::MotorData::MotorData(float p, float i, float d) :
+ GraspOnDetection_NJointController::ControlData::MotorData::MotorData(float p,
+ float i,
+ float d) :
pos_ctrl(p, i, d)
- {}
+ {
+ }
- void GraspOnDetection_NJointController::ControlData::MotorData::run(float curr, float targ, float dt, const auto& cfg)
+ void
+ GraspOnDetection_NJointController::ControlData::MotorData::run(float curr,
+ float targ,
+ float dt,
+ const auto& cfg)
{
target_pwm->pwm = pos_ctrl.calculate(curr, targ, dt, cfg.pwm_max);
}
- const GraspOnDetection_NJointController::ControlData::MotorData& GraspOnDetection_NJointController::ControlData::motor_data(Sensors::MotorIdentity id) const
+ const GraspOnDetection_NJointController::ControlData::MotorData&
+ GraspOnDetection_NJointController::ControlData::motor_data(Sensors::MotorIdentity id) const
{
- return KITFingerVisionSoftHand::V1::identity::Select(
- id, other, index, thumb);
+ return KITFingerVisionSoftHand::V1::identity::Select(id, other, index, thumb);
}
- GraspOnDetection_NJointController::ControlData::ControlData(const NJointGraspOnDetectionControllerConfig& cfg) :
+ GraspOnDetection_NJointController::ControlData::ControlData(
+ const NJointGraspOnDetectionControllerConfig& cfg) :
other(cfg.motor_other.pos_pid.p, cfg.motor_other.pos_pid.i, cfg.motor_other.pos_pid.d),
index(cfg.motor_index.pos_pid.p, cfg.motor_index.pos_pid.i, cfg.motor_index.pos_pid.d),
thumb(cfg.motor_thumb.pos_pid.p, cfg.motor_thumb.pos_pid.i, cfg.motor_thumb.pos_pid.d)
- {}
-}
+ {
+ }
+} // namespace devices::ethercat::hand::finger_vision_soft
+
//create
namespace devices::ethercat::hand::finger_vision_soft
{
@@ -52,7 +63,7 @@ namespace devices::ethercat::hand::finger_vision_soft
e->defaultValue = "RightHand";
line->children.emplace_back(e);
}
- const auto add_line = [&](const std::string & name, float val)
+ const auto add_line = [&](const std::string& name, float val)
{
HBoxLayoutPtr line = new HBoxLayout;
set_params->children.emplace_back(line);
@@ -62,11 +73,11 @@ namespace devices::ethercat::hand::finger_vision_soft
e->name = name;
e->min = -200000;
e->defaultValue = val;
- e->max = 200000;
+ e->max = 200000;
e->steps = 4000001;
line->children.emplace_back(e);
};
- const auto add_check_line = [&](const std::string & name, bool val)
+ const auto add_check_line = [&](const std::string& name, bool val)
{
HBoxLayoutPtr line = new HBoxLayout;
set_params->children.emplace_back(line);
@@ -83,28 +94,28 @@ namespace devices::ethercat::hand::finger_vision_soft
add_check_line("close_full_force", true);
add_check_line("detect_on_little", true);
- add_check_line("detect_on_ring", true);
+ add_check_line("detect_on_ring", true);
add_check_line("detect_on_middle", true);
- add_check_line("detect_on_index", true);
- add_check_line("detect_on_thumb", true);
+ add_check_line("detect_on_index", true);
+ add_check_line("detect_on_thumb", true);
- add_line("motor_index.pos_pid.p", 4.0f);
- add_line("motor_index.pos_pid.i", 0.4);
- add_line("motor_index.pos_pid.d", 1);
+ add_line("motor_index.pos_pid.p", 4.0f);
+ add_line("motor_index.pos_pid.i", 0.4);
+ add_line("motor_index.pos_pid.d", 1);
add_line("motor_index.pwm_max", 1000.f);
add_line("motor_index.pos_open", 0.48f);
add_line("motor_index.pos_close", 0.63f);
- add_line("motor_other.pos_pid.p", 12.0f);
- add_line("motor_other.pos_pid.i", 1.2);
- add_line("motor_other.pos_pid.d", 1);
+ add_line("motor_other.pos_pid.p", 12.0f);
+ add_line("motor_other.pos_pid.i", 1.2);
+ add_line("motor_other.pos_pid.d", 1);
add_line("motor_other.pwm_max", 1000.f);
add_line("motor_other.pos_open", 0.f);
add_line("motor_other.pos_close", 0.f);
- add_line("motor_thumb.pos_pid.p", 4.0f);
- add_line("motor_thumb.pos_pid.i", 0.4);
- add_line("motor_thumb.pos_pid.d", 1);
+ add_line("motor_thumb.pos_pid.p", 4.0f);
+ add_line("motor_thumb.pos_pid.i", 0.4);
+ add_line("motor_thumb.pos_pid.d", 1);
add_line("motor_thumb.pwm_max", 1000.f);
add_line("motor_thumb.pos_open", 0.34f);
add_line("motor_thumb.pos_close", 0.51f);
@@ -112,49 +123,53 @@ namespace devices::ethercat::hand::finger_vision_soft
return set_params;
}
+
NJointGraspOnDetectionControllerConfigPtr
- GraspOnDetection_NJointController::GenerateConfigFromVariants(const StringVariantBaseMap& valueMap)
+ GraspOnDetection_NJointController::GenerateConfigFromVariants(
+ const StringVariantBaseMap& valueMap)
{
NJointGraspOnDetectionControllerConfigPtr cfg = new NJointGraspOnDetectionControllerConfig;
cfg->hand_name = valueMap.at("hand")->getString();
- cfg->detection_threshold_per_finger = valueMap.at("detection_threshold_per_finger")->getFloat();
- cfg->detection_threshold_total = valueMap.at("detection_threshold_total")->getFloat();
- cfg->finger_count_threahold = valueMap.at("finger_count_threahold")->getFloat();
-
- cfg->close_full_force = valueMap.at("close_full_force")->getBool();
- cfg->detect_on_little = valueMap.at("detect_on_little")->getBool();
- cfg->detect_on_ring = valueMap.at("detect_on_ring")->getBool();
- cfg->detect_on_middle = valueMap.at("detect_on_middle")->getBool();
- cfg->detect_on_index = valueMap.at("detect_on_index")->getBool();
- cfg->detect_on_thumb = valueMap.at("detect_on_thumb")->getBool();
-
- cfg->motor_index.pos_pid.p = valueMap.at("motor_index.pos_pid.p")->getFloat();
- cfg->motor_index.pos_pid.i = valueMap.at("motor_index.pos_pid.i")->getFloat();
- cfg->motor_index.pos_pid.d = valueMap.at("motor_index.pos_pid.d")->getFloat();
- cfg->motor_index.pwm_max = valueMap.at("motor_index.pwm_max")->getFloat();
- cfg->motor_index.pos_open = valueMap.at("motor_index.pos_open")->getFloat();
- cfg->motor_index.pos_close = valueMap.at("motor_index.pos_close")->getFloat();
-
- cfg->motor_other.pos_pid.p = valueMap.at("motor_other.pos_pid.p")->getFloat();
- cfg->motor_other.pos_pid.i = valueMap.at("motor_other.pos_pid.i")->getFloat();
- cfg->motor_other.pos_pid.d = valueMap.at("motor_other.pos_pid.d")->getFloat();
- cfg->motor_other.pwm_max = valueMap.at("motor_other.pwm_max")->getFloat();
- cfg->motor_other.pos_open = valueMap.at("motor_other.pos_open")->getFloat();
- cfg->motor_other.pos_close = valueMap.at("motor_other.pos_close")->getFloat();
-
- cfg->motor_thumb.pos_pid.p = valueMap.at("motor_thumb.pos_pid.p")->getFloat();
- cfg->motor_thumb.pos_pid.i = valueMap.at("motor_thumb.pos_pid.i")->getFloat();
- cfg->motor_thumb.pos_pid.d = valueMap.at("motor_thumb.pos_pid.d")->getFloat();
- cfg->motor_thumb.pwm_max = valueMap.at("motor_thumb.pwm_max")->getFloat();
- cfg->motor_thumb.pos_open = valueMap.at("motor_thumb.pos_open")->getFloat();
- cfg->motor_thumb.pos_close = valueMap.at("motor_thumb.pos_close")->getFloat();
+ cfg->detection_threshold_per_finger =
+ valueMap.at("detection_threshold_per_finger")->getFloat();
+ cfg->detection_threshold_total = valueMap.at("detection_threshold_total")->getFloat();
+ cfg->finger_count_threahold = valueMap.at("finger_count_threahold")->getFloat();
+
+ cfg->close_full_force = valueMap.at("close_full_force")->getBool();
+ cfg->detect_on_little = valueMap.at("detect_on_little")->getBool();
+ cfg->detect_on_ring = valueMap.at("detect_on_ring")->getBool();
+ cfg->detect_on_middle = valueMap.at("detect_on_middle")->getBool();
+ cfg->detect_on_index = valueMap.at("detect_on_index")->getBool();
+ cfg->detect_on_thumb = valueMap.at("detect_on_thumb")->getBool();
+
+ cfg->motor_index.pos_pid.p = valueMap.at("motor_index.pos_pid.p")->getFloat();
+ cfg->motor_index.pos_pid.i = valueMap.at("motor_index.pos_pid.i")->getFloat();
+ cfg->motor_index.pos_pid.d = valueMap.at("motor_index.pos_pid.d")->getFloat();
+ cfg->motor_index.pwm_max = valueMap.at("motor_index.pwm_max")->getFloat();
+ cfg->motor_index.pos_open = valueMap.at("motor_index.pos_open")->getFloat();
+ cfg->motor_index.pos_close = valueMap.at("motor_index.pos_close")->getFloat();
+
+ cfg->motor_other.pos_pid.p = valueMap.at("motor_other.pos_pid.p")->getFloat();
+ cfg->motor_other.pos_pid.i = valueMap.at("motor_other.pos_pid.i")->getFloat();
+ cfg->motor_other.pos_pid.d = valueMap.at("motor_other.pos_pid.d")->getFloat();
+ cfg->motor_other.pwm_max = valueMap.at("motor_other.pwm_max")->getFloat();
+ cfg->motor_other.pos_open = valueMap.at("motor_other.pos_open")->getFloat();
+ cfg->motor_other.pos_close = valueMap.at("motor_other.pos_close")->getFloat();
+
+ cfg->motor_thumb.pos_pid.p = valueMap.at("motor_thumb.pos_pid.p")->getFloat();
+ cfg->motor_thumb.pos_pid.i = valueMap.at("motor_thumb.pos_pid.i")->getFloat();
+ cfg->motor_thumb.pos_pid.d = valueMap.at("motor_thumb.pos_pid.d")->getFloat();
+ cfg->motor_thumb.pwm_max = valueMap.at("motor_thumb.pwm_max")->getFloat();
+ cfg->motor_thumb.pos_open = valueMap.at("motor_thumb.pos_open")->getFloat();
+ cfg->motor_thumb.pos_close = valueMap.at("motor_thumb.pos_close")->getFloat();
return cfg;
}
- NJointGraspOnDetectionControllerConfig GraspOnDetection_NJointController::make_default_cfg()
+ NJointGraspOnDetectionControllerConfig
+ GraspOnDetection_NJointController::make_default_cfg()
{
NJointGraspOnDetectionControllerConfig c;
@@ -164,30 +179,30 @@ namespace devices::ethercat::hand::finger_vision_soft
c.close_full_force = true;
c.detect_on_little = true;
- c.detect_on_ring = true;
+ c.detect_on_ring = true;
c.detect_on_middle = true;
- c.detect_on_index = true;
- c.detect_on_thumb = true;
+ c.detect_on_index = true;
+ c.detect_on_thumb = true;
c.motor_index.pos_pid.p = 4.0f;
c.motor_index.pos_pid.i = 0.4;
c.motor_index.pos_pid.d = 1;
- c.motor_index.pwm_max = 1000.f;
- c.motor_index.pos_open = 0.48f;
+ c.motor_index.pwm_max = 1000.f;
+ c.motor_index.pos_open = 0.48f;
c.motor_index.pos_close = 0.63f;
c.motor_other.pos_pid.p = 12.0f;
c.motor_other.pos_pid.i = 1.2;
c.motor_other.pos_pid.d = 1;
- c.motor_other.pwm_max = 1000.f;
- c.motor_other.pos_open = 0.f;
+ c.motor_other.pwm_max = 1000.f;
+ c.motor_other.pos_open = 0.f;
c.motor_other.pos_close = 0.f;
c.motor_thumb.pos_pid.p = 4.0f;
c.motor_thumb.pos_pid.i = 0.4;
c.motor_thumb.pos_pid.d = 1;
- c.motor_thumb.pwm_max = 1000.f;
- c.motor_thumb.pos_open = 0.34f;
+ c.motor_thumb.pwm_max = 1000.f;
+ c.motor_thumb.pos_open = 0.34f;
c.motor_thumb.pos_close = 0.51f;
return c;
}
@@ -195,22 +210,26 @@ namespace devices::ethercat::hand::finger_vision_soft
GraspOnDetection_NJointController::GraspOnDetection_NJointController(
RobotUnitPtr robotUnit,
const NJointGraspOnDetectionControllerConfigPtr& config,
- const VirtualRobot::RobotPtr&
- ) :
- _ctrl_data([ & ]
- {
- ARMARX_CHECK_NOT_NULL(config);
- return * config;
- }())
+ const VirtualRobot::RobotPtr&) :
+ _ctrl_data(
+ [&]
+ {
+ ARMARX_CHECK_NOT_NULL(config);
+ return *config;
+ }())
{
ARMARX_CHECK_NOT_NULL(config);
configure(config);
//get ctrl
{
- ControlTargetBase* ct_thumb = useControlTarget(config->hand_name + ".motor.thumb", armarx::ControlModes::PWM1DoF);
- ControlTargetBase* ct_index = useControlTarget(config->hand_name + ".motor.index", armarx::ControlModes::PWM1DoF);
- ControlTargetBase* ct_other = useControlTarget(config->hand_name + ".motor.other", armarx::ControlModes::PWM1DoF);
- ControlTargetBase* ct_cnn = useControlTarget(config->hand_name + ".cnn", ControlModes::CNNObjectId);
+ ControlTargetBase* ct_thumb =
+ useControlTarget(config->hand_name + ".motor.thumb", armarx::ControlModes::PWM1DoF);
+ ControlTargetBase* ct_index =
+ useControlTarget(config->hand_name + ".motor.index", armarx::ControlModes::PWM1DoF);
+ ControlTargetBase* ct_other =
+ useControlTarget(config->hand_name + ".motor.other", armarx::ControlModes::PWM1DoF);
+ ControlTargetBase* ct_cnn =
+ useControlTarget(config->hand_name + ".cnn", ControlModes::CNNObjectId);
ARMARX_CHECK_EXPRESSION(ct_thumb->isA<ControlTarget1DoFActuatorPWM>());
ARMARX_CHECK_EXPRESSION(ct_index->isA<ControlTarget1DoFActuatorPWM>());
ARMARX_CHECK_EXPRESSION(ct_other->isA<ControlTarget1DoFActuatorPWM>());
@@ -225,26 +244,33 @@ namespace devices::ethercat::hand::finger_vision_soft
//get sens
_sensors = KITFingerVisionSoftHand::V1::Sensors(config->hand_name, *this);
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
+
//name
namespace devices::ethercat::hand::finger_vision_soft
{
- std::string GraspOnDetection_NJointController::getClassName(const Ice::Current&) const
+ std::string
+ GraspOnDetection_NJointController::getClassName(const Ice::Current&) const
{
return "GraspOnDetection_NJointController";
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
+
//remote function calls
namespace devices::ethercat::hand::finger_vision_soft
{
- WidgetDescription::StringWidgetDictionary GraspOnDetection_NJointController::getFunctionDescriptions(const Ice::Current&) const
+ WidgetDescription::StringWidgetDictionary
+ GraspOnDetection_NJointController::getFunctionDescriptions(const Ice::Current&) const
{
using namespace armarx::WidgetDescription;
return {{"Start", nullptr}, {"Release", nullptr}};
}
- void GraspOnDetection_NJointController::callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&)
+ void
+ GraspOnDetection_NJointController::callDescribedFunction(const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&)
{
if (name == "Start")
{
@@ -259,40 +285,51 @@ namespace devices::ethercat::hand::finger_vision_soft
ARMARX_WARNING << "Unknown function name called: " << name;
}
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
+
//iface
namespace devices::ethercat::hand::finger_vision_soft
{
- void GraspOnDetection_NJointController::start(const Ice::Current&)
+ void
+ GraspOnDetection_NJointController::start(const Ice::Current&)
{
_clicked_start = true;
}
- void GraspOnDetection_NJointController::reset(const Ice::Current&)
+
+ void
+ GraspOnDetection_NJointController::reset(const Ice::Current&)
{
_clicked_reset = true;
}
- void GraspOnDetection_NJointController::configure(const ConfigPtrT& cfg, const Ice::Current&)
+
+ void
+ GraspOnDetection_NJointController::configure(const ConfigPtrT& cfg, const Ice::Current&)
{
ARMARX_CHECK_NOT_NULL(cfg);
std::lock_guard g{_ice_to_rt_write_mutex};
_ice_to_rt.getWriteBuffer().cfg = *cfg;
_ice_to_rt.commitWrite();
}
- void GraspOnDetection_NJointController::setAutofire(bool fire, const Ice::Current&)
+
+ void
+ GraspOnDetection_NJointController::setAutofire(bool fire, const Ice::Current&)
{
_autofire = fire;
}
- bool GraspOnDetection_NJointController::detected(const Ice::Current&)
+
+ bool
+ GraspOnDetection_NJointController::detected(const Ice::Current&)
{
return _detected;
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
+
//rt control
namespace devices::ethercat::hand::finger_vision_soft
{
- void GraspOnDetection_NJointController::rtRun(
- const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration)
+ void
+ GraspOnDetection_NJointController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
const auto& buf = _ice_to_rt.getUpToDateReadBuffer();
const auto dt = timeSinceLastIteration.toSecondsDouble();
@@ -303,7 +340,7 @@ namespace devices::ethercat::hand::finger_vision_soft
{
if (_clicked_reset.exchange(false))
{
- _ctrl_data.close = false;
+ _ctrl_data.close = false;
_ctrl_data.running = false;
_ctrl_data.other.pos_ctrl.reset();
_ctrl_data.index.pos_ctrl.reset();
@@ -316,7 +353,7 @@ namespace devices::ethercat::hand::finger_vision_soft
}
//config
{
- auto cfg_pos_pid = [&](auto & ctrl, const auto & c)
+ auto cfg_pos_pid = [&](auto& ctrl, const auto& c)
{
ctrl.PID.Kp = c.p;
ctrl.PID.Ki = c.i;
@@ -338,8 +375,9 @@ namespace devices::ethercat::hand::finger_vision_soft
if (!_ctrl_data.close)
{
long n_over_thresh = 0;
- long n_detections = 0;
- const auto thresh = static_cast<std::uint32_t>(buf.cfg.detection_threshold_per_finger);
+ long n_detections = 0;
+ const auto thresh =
+ static_cast<std::uint32_t>(buf.cfg.detection_threshold_per_finger);
const auto test = [&](bool active, auto pop)
{
if (active && (pop >= thresh))
@@ -349,11 +387,11 @@ namespace devices::ethercat::hand::finger_vision_soft
}
};
test(buf.cfg.detect_on_little, _sensors.hand->cnn_data_popcount_little);
- test(buf.cfg.detect_on_ring, _sensors.hand->cnn_data_popcount_ring);
+ test(buf.cfg.detect_on_ring, _sensors.hand->cnn_data_popcount_ring);
test(buf.cfg.detect_on_middle, _sensors.hand->cnn_data_popcount_middle);
- test(buf.cfg.detect_on_index, _sensors.hand->cnn_data_popcount_index);
- test(buf.cfg.detect_on_thumb, _sensors.hand->cnn_data_popcount_thumb);
- _detected = (n_detections >= buf.cfg.detection_threshold_total) &&
+ test(buf.cfg.detect_on_index, _sensors.hand->cnn_data_popcount_index);
+ test(buf.cfg.detect_on_thumb, _sensors.hand->cnn_data_popcount_thumb);
+ _detected = (n_detections >= buf.cfg.detection_threshold_total) &&
(n_over_thresh >= buf.cfg.finger_count_threahold);
if (_autofire)
{
@@ -369,18 +407,25 @@ namespace devices::ethercat::hand::finger_vision_soft
target_index = buf.cfg.motor_index.pos_close;
target_thumb = buf.cfg.motor_thumb.pos_close;
}
- _ctrl_data.other.run(_sensors.motor.other()->position, target_other, dt, buf.cfg.motor_other);
- _ctrl_data.index.run(_sensors.motor.index()->position, target_index, dt, buf.cfg.motor_index);
- _ctrl_data.thumb.run(_sensors.motor.thumb()->position, target_thumb, dt, buf.cfg.motor_thumb);
+ _ctrl_data.other.run(
+ _sensors.motor.other()->position, target_other, dt, buf.cfg.motor_other);
+ _ctrl_data.index.run(
+ _sensors.motor.index()->position, target_index, dt, buf.cfg.motor_index);
+ _ctrl_data.thumb.run(
+ _sensors.motor.thumb()->position, target_thumb, dt, buf.cfg.motor_thumb);
}
}
- void GraspOnDetection_NJointController::rtPreActivateController()
+ void
+ GraspOnDetection_NJointController::rtPreActivateController()
{
_clicked_reset = true;
_clicked_start = false;
- _detected = false;
+ _detected = false;
+ }
+
+ void
+ GraspOnDetection_NJointController::rtPostDeactivateController()
+ {
}
- void GraspOnDetection_NJointController::rtPostDeactivateController()
- {}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/GraspOnDetection.h b/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/GraspOnDetection.h
index 4efb1324..0af40269 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/GraspOnDetection.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/GraspOnDetection.h
@@ -2,15 +2,13 @@
#include <mutex>
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-
-#include <Armar6RT/units/Armar6Unit/Devices/KITFingerVisionSoftHand/V1/utility/Sensors.h>
-#include <Armar6RT/units/Armar6Unit/Devices/KITFingerVisionSoftHand/V1/Controllers/PositionToPWM.h>
-#include <Armar6RT/units/Armar6Unit/Devices/KITFingerVisionSoftHand/V1/ControlTarget/CNNObjectId.h>
-
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
#include <Armar6RT/interface/units/KITFingerVisionSoftHand/V1/NJointGraspOnDetectionController.h>
+#include <Armar6RT/units/Armar6Unit/Devices/KITFingerVisionSoftHand/V1/ControlTarget/CNNObjectId.h>
+#include <Armar6RT/units/Armar6Unit/Devices/KITFingerVisionSoftHand/V1/Controllers/PositionToPWM.h>
+#include <Armar6RT/units/Armar6Unit/Devices/KITFingerVisionSoftHand/V1/utility/Sensors.h>
namespace devices::ethercat::hand::finger_vision_soft
{
@@ -31,50 +29,55 @@ namespace devices::ethercat::hand::finger_vision_soft
{
public:
static NJointGraspOnDetectionControllerConfig make_default_cfg();
+
public:
using ConfigPtrT = NJointGraspOnDetectionControllerConfigPtr;
- GraspOnDetection_NJointController(
- RobotUnitPtr robotUnit,
- const ConfigPtrT& config,
- const VirtualRobot::RobotPtr&);
+ GraspOnDetection_NJointController(RobotUnitPtr robotUnit,
+ const ConfigPtrT& config,
+ const VirtualRobot::RobotPtr&);
std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override;
- WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
- void callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current& = Ice::emptyCurrent) override;
+ WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
+ void callDescribedFunction(const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current& = Ice::emptyCurrent) override;
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
- static WidgetDescription::WidgetPtr GenerateConfigDescription(
- const VirtualRobot::RobotPtr&,
- const std::map<std::string, ConstControlDevicePtr>&,
- const std::map<std::string, ConstSensorDevicePtr>&);
+ static WidgetDescription::WidgetPtr
+ GenerateConfigDescription(const VirtualRobot::RobotPtr&,
+ const std::map<std::string, ConstControlDevicePtr>&,
+ const std::map<std::string, ConstSensorDevicePtr>&);
+
+ static ConfigPtrT GenerateConfigFromVariants(const StringVariantBaseMap& values);
- static ConfigPtrT
- GenerateConfigFromVariants(const StringVariantBaseMap& values);
private:
void start(const Ice::Current& = Ice::emptyCurrent) override;
void reset(const Ice::Current& = Ice::emptyCurrent) override;
void configure(const ConfigPtrT& cfg, const Ice::Current& = Ice::emptyCurrent) override;
void setAutofire(bool fire, const Ice::Current& = Ice::emptyCurrent) override;
bool detected(const Ice::Current& = Ice::emptyCurrent) override;
+
protected:
void rtPreActivateController() override;
void rtPostDeactivateController() override;
- std::atomic_bool _clicked_reset = false;
- std::atomic_bool _clicked_start = false;
- std::atomic_bool _autofire = true;
- std::atomic_bool _detected = false;
+ std::atomic_bool _clicked_reset = false;
+ std::atomic_bool _clicked_start = false;
+ std::atomic_bool _autofire = true;
+ std::atomic_bool _detected = false;
KITFingerVisionSoftHand::V1::Sensors _sensors;
struct IceToRT
{
NJointGraspOnDetectionControllerConfig cfg;
-
};
+
WriteBufferedTripleBuffer<IceToRT> _ice_to_rt;
- mutable std::recursive_mutex _ice_to_rt_write_mutex;
+ mutable std::recursive_mutex _ice_to_rt_write_mutex;
struct ControlData
{
@@ -85,15 +88,18 @@ namespace devices::ethercat::hand::finger_vision_soft
ControlTarget1DoFActuatorPWM* target_pwm = nullptr;
void run(float curr, float targ, float dt, const auto& cfg);
};
+
MotorData other;
MotorData index;
MotorData thumb;
- const MotorData& motor_data(KITFingerVisionSoftHand::V1::Sensors::MotorIdentity id) const;
+ const MotorData&
+ motor_data(KITFingerVisionSoftHand::V1::Sensors::MotorIdentity id) const;
ControlData(const NJointGraspOnDetectionControllerConfig& cfg);
bool close = false;
bool running = false;
};
+
ControlData _ctrl_data;
ControlTargetCNNObjectId* _object;
};
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/GraspOnDetectionInterface.ice b/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/GraspOnDetectionInterface.ice
index 16878b4c..85f109b4 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/GraspOnDetectionInterface.ice
+++ b/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/GraspOnDetectionInterface.ice
@@ -16,38 +16,40 @@ module armarx
float i = 0;
float d = 0;
};
+
struct MotorCfg
{
- PID pos_pid;
+ PID pos_pid;
float pos_close = 0.5;
- float pos_open = 0.1;
- short pwm_max = 1000;
- short pwm_open = -300;
+ float pos_open = 0.1;
+ short pwm_max = 1000;
+ short pwm_open = -300;
};
};
class NJointGraspOnDetectionControllerConfig extends NJointControllerConfig
{
- string hand_name;
+ string hand_name;
NJointGraspOnDetectionControllerConfigElements::MotorCfg motor_other;
NJointGraspOnDetectionControllerConfigElements::MotorCfg motor_index;
NJointGraspOnDetectionControllerConfigElements::MotorCfg motor_thumb;
- bool close_full_force = false;
+ bool close_full_force = false;
- bool detect_on_little = false;
- bool detect_on_ring = false;
- bool detect_on_middle = false;
- bool detect_on_index = true;
- bool detect_on_thumb = true;
+ bool detect_on_little = false;
+ bool detect_on_ring = false;
+ bool detect_on_middle = false;
+ bool detect_on_index = true;
+ bool detect_on_thumb = true;
- long detection_threshold_per_finger = 50;
- long detection_threshold_total = 200;
- short finger_count_threahold = 1;
+ long detection_threshold_per_finger = 50;
+ long detection_threshold_total = 200;
+ short finger_count_threahold = 1;
long object_id = 0;
};
+
interface NJointGraspOnDetectionControllerInterface extends NJointControllerInterface
{
void setAutofire(bool fire);
@@ -59,4 +61,3 @@ module armarx
};
};
};
-
diff --git a/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/Shape.cpp b/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/Shape.cpp
index 49d53ca2..572980b6 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/Shape.cpp
+++ b/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/Shape.cpp
@@ -10,22 +10,21 @@
namespace devices::ethercat::hand::finger_vision_soft
{
armarx::NJointControllerRegistration<NJointShapeController>
- registrationControllerNJointShapeController(
- "NJointKITFingerVisionSoftHandV1ShapeController");
+ registrationControllerNJointShapeController(
+ "NJointKITFingerVisionSoftHandV1ShapeController");
NJointShapeController::NJointShapeController(armarx::RobotUnitPtr prov,
- NJointShapeControllerConfigPtr config,
- const VirtualRobot::RobotPtr&)
+ NJointShapeControllerConfigPtr config,
+ const VirtualRobot::RobotPtr&)
{
ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(prov);
- const auto init = [&](auto & data, const std::string & suffix)
+ const auto init = [&](auto& data, const std::string& suffix)
{
const auto name = config->deviceName + ".motor." + suffix;
ControlTargetBase* ct = useControlTarget(name, ControlModes::PWM1DoF);
- ARMARX_CHECK_NOT_NULL(ct) << " control target " << name
- << " control mode " << ControlModes::PWM1DoF
- << ARMARX_STREAM_PRINTER
+ ARMARX_CHECK_NOT_NULL(ct) << " control target " << name << " control mode "
+ << ControlModes::PWM1DoF << ARMARX_STREAM_PRINTER
{
const auto ptr = peekControlDevice(name);
if (ptr)
@@ -44,32 +43,31 @@ namespace devices::ethercat::hand::finger_vision_soft
data.motor_sensor = sensor->getSensorValue()->asA<MotorSensorValue>();
ARMARX_CHECK_NOT_NULL(data.motor_sensor) << "device name: " << name;
- data.pwm_target = ct->asA<armarx::ControlTarget1DoFActuatorPWM>();
+ data.pwm_target = ct->asA<armarx::ControlTarget1DoFActuatorPWM>();
ARMARX_CHECK_NOT_NULL(data.pwm_target) << "device name: " << name;
};
init(otherData, "other");
init(indexData, "index");
init(thumbData, "thumb");
}
- void NJointShapeController::readConfig(
- const armarx::RapidXmlReaderNode& configNode,
- armarx::DefaultRapidXmlReaderNode defaultConfigurationNode)
+
+ void
+ NJointShapeController::readConfig(const armarx::RapidXmlReaderNode& configNode,
+ armarx::DefaultRapidXmlReaderNode defaultConfigurationNode)
{
ARMARX_TRACE;
- ARMARX_DEBUG << "readConfig got cfg:\n" << VAROUT(configNode.getPath())
- << "\n" << VAROUT(defaultConfigurationNode.getPathsString());
+ ARMARX_DEBUG << "readConfig got cfg:\n"
+ << VAROUT(configNode.getPath()) << "\n"
+ << VAROUT(defaultConfigurationNode.getPathsString());
auto configNodeAll =
- defaultConfigurationNode
- .add_node_at_end(configNode)
- .first_node("Motor");
+ defaultConfigurationNode.add_node_at_end(configNode).first_node("Motor");
- const auto init = [&](auto & data, auto name)
+ const auto init = [&](auto& data, auto name)
{
- auto cfg = configNodeAll
- .first_node("default")
- .add_node_at_end(configNodeAll.first_node(name))
- .first_node("ctrl_shape");
+ auto cfg = configNodeAll.first_node("default")
+ .add_node_at_end(configNodeAll.first_node(name))
+ .first_node("ctrl_shape");
cfg.attribute_as("Kp", data.Kp);
cfg.attribute_as("Ki", data.Ki);
cfg.attribute_as("Kd", data.Kd);
@@ -79,60 +77,77 @@ namespace devices::ethercat::hand::finger_vision_soft
init(indexData, "index");
init(otherData, "other");
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
//getter
namespace devices::ethercat::hand::finger_vision_soft
{
- int16_t NJointShapeController::getOtherPwm()
+ int16_t
+ NJointShapeController::getOtherPwm()
{
return otherData.last_pwm;
}
- int16_t NJointShapeController::getIndexPwm()
+
+ int16_t
+ NJointShapeController::getIndexPwm()
{
return indexData.last_pwm;
}
- int16_t NJointShapeController::getThumbPwm()
+
+ int16_t
+ NJointShapeController::getThumbPwm()
{
return thumbData.last_pwm;
}
- float NJointShapeController::getOtherTarget() const
+ float
+ NJointShapeController::getOtherTarget() const
{
std::lock_guard g{lastTargetBufReadMutex};
return lastTargetBuf.getUpToDateReadBuffer().other.target;
}
- float NJointShapeController::getIndexTarget() const
+
+ float
+ NJointShapeController::getIndexTarget() const
{
std::lock_guard g{lastTargetBufReadMutex};
return lastTargetBuf.getUpToDateReadBuffer().index.target;
}
- float NJointShapeController::getThumbTarget() const
+
+ float
+ NJointShapeController::getThumbTarget() const
{
std::lock_guard g{lastTargetBufReadMutex};
return lastTargetBuf.getUpToDateReadBuffer().thumb.target;
}
- float NJointShapeController::getOtherJointValue() const
+ float
+ NJointShapeController::getOtherJointValue() const
{
return otherData.joint_value;
}
- float NJointShapeController::getIndexJointValue() const
+
+ float
+ NJointShapeController::getIndexJointValue() const
{
return indexData.joint_value;
}
- float NJointShapeController::getThumbJointValue() const
+
+ float
+ NJointShapeController::getThumbJointValue() const
{
return thumbData.joint_value;
}
- bool NJointShapeController::isControlEnabled() const
+ bool
+ NJointShapeController::isControlEnabled() const
{
std::lock_guard g{lastTargetBufReadMutex};
return lastTargetBuf.getUpToDateReadBuffer().enable_control;
}
- KITFingerVisionSoftHandV1JointValues NJointShapeController::getJointValues(const Ice::Current&) const
+ KITFingerVisionSoftHandV1JointValues
+ NJointShapeController::getJointValues(const Ice::Current&) const
{
KITFingerVisionSoftHandV1JointValues values;
values.otherJointValue = getOtherJointValue();
@@ -140,48 +155,57 @@ namespace devices::ethercat::hand::finger_vision_soft
values.thumbJointValue = getThumbJointValue();
return values;
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
//setter
namespace devices::ethercat::hand::finger_vision_soft
{
- void NJointShapeController::stopMotion()
+ void
+ NJointShapeController::stopMotion()
{
std::lock_guard g{targetBufWriteMutex};
targetBuf.getWriteBuffer().enable_control = false;
targetBuf.commitWrite();
}
- void NJointShapeController::setTargets(
- float lrm, float index, float thumb, const Ice::Current&)
+
+ void
+ NJointShapeController::setTargets(float lrm, float index, float thumb, const Ice::Current&)
{
setTargetsWithPwm(lrm, index, thumb, 1, 1, 1);
}
- void NJointShapeController::setTargetsWithPwm(
- float lrm, float index, float thumb,
- float lrmRelativePWM, float indexRelativePWM, float thumbRelativePWM,
- const Ice::Current&)
+ void
+ NJointShapeController::setTargetsWithPwm(float lrm,
+ float index,
+ float thumb,
+ float lrmRelativePWM,
+ float indexRelativePWM,
+ float thumbRelativePWM,
+ const Ice::Current&)
{
std::lock_guard g{targetBufWriteMutex};
auto& buf = targetBuf.getWriteBuffer();
buf.enable_control = true;
- buf.index.target = armarx::math::MathUtils::LimitMinMax(0.f, 2.f, thumb);
- buf.thumb.target = armarx::math::MathUtils::LimitMinMax(0.f, 2.f, index);
- buf.other.target = armarx::math::MathUtils::LimitMinMax(0.f, 2.f, lrm);
- buf.index.max_pwm = static_cast<std::int16_t>(indexData.pwm_limit * boost::algorithm::clamp(indexRelativePWM, 0, 1));
- buf.thumb.max_pwm = static_cast<std::int16_t>(thumbData.pwm_limit * boost::algorithm::clamp(thumbRelativePWM, 0, 1));
- buf.other.max_pwm = static_cast<std::int16_t>(otherData.pwm_limit * boost::algorithm::clamp(lrmRelativePWM, 0, 1));
+ buf.index.target = armarx::math::MathUtils::LimitMinMax(0.f, 2.f, thumb);
+ buf.thumb.target = armarx::math::MathUtils::LimitMinMax(0.f, 2.f, index);
+ buf.other.target = armarx::math::MathUtils::LimitMinMax(0.f, 2.f, lrm);
+ buf.index.max_pwm = static_cast<std::int16_t>(
+ indexData.pwm_limit * boost::algorithm::clamp(indexRelativePWM, 0, 1));
+ buf.thumb.max_pwm = static_cast<std::int16_t>(
+ thumbData.pwm_limit * boost::algorithm::clamp(thumbRelativePWM, 0, 1));
+ buf.other.max_pwm = static_cast<std::int16_t>(
+ otherData.pwm_limit * boost::algorithm::clamp(lrmRelativePWM, 0, 1));
targetBuf.commitWrite();
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
//control
namespace devices::ethercat::hand::finger_vision_soft
{
- void NJointShapeController::rtRun(
- const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointShapeController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
otherData.joint_value = otherData.motor_sensor->position;
indexData.joint_value = indexData.motor_sensor->position;
@@ -194,12 +218,13 @@ namespace devices::ethercat::hand::finger_vision_soft
if (buf.enable_control)
{
- auto calc = [](auto & buff, auto & data)
+ auto calc = [](auto& buff, auto& data)
{
const float err = buff.target - data.motor_sensor->position;
- data.pwm_target->pwm = boost::algorithm::clamp(
- static_cast<std::int16_t>(data.Kp * buff.max_pwm * err),
- -buff.max_pwm, buff.max_pwm);
+ data.pwm_target->pwm =
+ boost::algorithm::clamp(static_cast<std::int16_t>(data.Kp * buff.max_pwm * err),
+ -buff.max_pwm,
+ buff.max_pwm);
};
calc(buf.other, otherData);
calc(buf.index, indexData);
@@ -215,16 +240,17 @@ namespace devices::ethercat::hand::finger_vision_soft
indexData.last_pwm = indexData.pwm_target->pwm;
thumbData.last_pwm = thumbData.pwm_target->pwm;
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
//gui
namespace devices::ethercat::hand::finger_vision_soft
{
- armarx::WidgetDescription::StringWidgetDictionary NJointShapeController::getFunctionDescriptions(const Ice::Current&) const
+ armarx::WidgetDescription::StringWidgetDictionary
+ NJointShapeController::getFunctionDescriptions(const Ice::Current&) const
{
using namespace armarx::WidgetDescription;
VBoxLayoutPtr vbox = new VBoxLayout;
- const auto add = [&](const std::string & name, const auto value)
+ const auto add = [&](const std::string& name, const auto value)
{
HBoxLayoutPtr hbox = new HBoxLayout;
vbox->children.emplace_back(hbox);
@@ -255,22 +281,24 @@ namespace devices::ethercat::hand::finger_vision_soft
return {{"Targets", vbox}};
}
- void NJointShapeController::callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&)
+ void
+ NJointShapeController::callDescribedFunction(const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&)
{
if (name == "Targets")
{
- setTargetsWithPwm(
- valueMap.at("other_val")->getFloat(),
- valueMap.at("index_val")->getFloat(),
- valueMap.at("thumb_val")->getFloat(),
-
- valueMap.at("other_pwm")->getFloat(),
- valueMap.at("index_pwm")->getFloat(),
- valueMap.at("thumb_pwm")->getFloat());
+ setTargetsWithPwm(valueMap.at("other_val")->getFloat(),
+ valueMap.at("index_val")->getFloat(),
+ valueMap.at("thumb_val")->getFloat(),
+
+ valueMap.at("other_pwm")->getFloat(),
+ valueMap.at("index_pwm")->getFloat(),
+ valueMap.at("thumb_pwm")->getFloat());
}
else
{
ARMARX_WARNING << "Unknown function name called: " << name;
}
}
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/Shape.h b/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/Shape.h
index d9d40db8..9a94616a 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/Shape.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/Shape.h
@@ -5,9 +5,9 @@
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
#include "robot_devices/ethercat/hand/finger_vision_soft/sensor_value/Motor.h"
-#include <devices/ethercat/hand/finger_vision_soft/njoint_controller/Shape.h>
#include "../joint_controller/Pwm.h"
+#include <devices/ethercat/hand/finger_vision_soft/njoint_controller/Shape.h>
namespace devices::ethercat::hand::finger_vision_soft
{
@@ -17,9 +17,9 @@ namespace devices::ethercat::hand::finger_vision_soft
class NJointShapeControllerConfig : virtual public armarx::NJointControllerConfig
{
public:
- NJointShapeControllerConfig(std::string const& deviceName):
- deviceName(deviceName)
- {}
+ NJointShapeControllerConfig(std::string const& deviceName) : deviceName(deviceName)
+ {
+ }
const std::string deviceName;
};
@@ -34,22 +34,21 @@ namespace devices::ethercat::hand::finger_vision_soft
{
public:
using ConfigPtrT = NJointShapeControllerConfigPtr;
- NJointShapeController(
- armarx::RobotUnitPtr prov,
- NJointShapeControllerConfigPtr config,
- const VirtualRobot::RobotPtr& r);
+ NJointShapeController(armarx::RobotUnitPtr prov,
+ NJointShapeControllerConfigPtr config,
+ const VirtualRobot::RobotPtr& r);
public:
- std::string getClassName(const Ice::Current&) const override
+ std::string
+ getClassName(const Ice::Current&) const override
{
return "NJointKITFingerVisionSoftHandV1ShapeController";
}
void stopMotion();
- void readConfig(
- const armarx::RapidXmlReaderNode& configNode,
- armarx::DefaultRapidXmlReaderNode defaultConfigurationNod);
+ void readConfig(const armarx::RapidXmlReaderNode& configNode,
+ armarx::DefaultRapidXmlReaderNode defaultConfigurationNod);
int16_t getOtherPwm();
int16_t getIndexPwm();
@@ -65,30 +64,42 @@ namespace devices::ethercat::hand::finger_vision_soft
bool isControlEnabled() const;
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
public:
- armarx::WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
- void callDescribedFunction(const std::string& name, const armarx::StringVariantBaseMap& valueMap, const Ice::Current&) override;
-
- void setTargets(float lrm, float index, float thumb,
+ armarx::WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current&) const override;
+ void callDescribedFunction(const std::string& name,
+ const armarx::StringVariantBaseMap& valueMap,
+ const Ice::Current&) override;
+
+ void setTargets(float lrm,
+ float index,
+ float thumb,
const Ice::Current& = Ice::emptyCurrent) override;
- void setTargetsWithPwm(float lrm, float index, float thumb,
- float lrmRelativePWM, float indexRelativePWM, float thumbRelativePWM,
+ void setTargetsWithPwm(float lrm,
+ float index,
+ float thumb,
+ float lrmRelativePWM,
+ float indexRelativePWM,
+ float thumbRelativePWM,
const Ice::Current& = Ice::emptyCurrent) override;
KITFingerVisionSoftHandV1JointValues getJointValues(const Ice::Current&) const override;
+
private:
struct FingerData
{
- const MotorSensorValue* motorSensor;
+ const MotorSensorValue* motorSensor;
armarx::ControlTarget1DoFActuatorPWM* pwmTarget;
- std::atomic<float> jointValue;
- std::atomic<int16_t> lastPwm;
- int16_t pwmLimit = 0;
- float Kp = 0;
- float Ki = 0;
- float Kd = 0;
+ std::atomic<float> jointValue;
+ std::atomic<int16_t> lastPwm;
+ int16_t pwmLimit = 0;
+ float Kp = 0;
+ float Ki = 0;
+ float Kd = 0;
};
+
FingerData otherData;
FingerData indexData;
FingerData thumbData;
@@ -98,17 +109,18 @@ namespace devices::ethercat::hand::finger_vision_soft
struct TargetData
{
int16_t maxPwm = 0;
- float target = 0;
+ float target = 0;
};
+
TargetData index;
TargetData thumb;
TargetData other;
bool enableControl = false;
};
- mutable std::recursive_mutex targetBufWriteMutex;
- armarx::WriteBufferedTripleBuffer<FingersTargetData> targetBuf;
- mutable std::recursive_mutex lastTargetBufReadMutex;
- armarx::TripleBuffer<FingersTargetData> lastTargetBuf;
+ mutable std::recursive_mutex targetBufWriteMutex;
+ armarx::WriteBufferedTripleBuffer<FingersTargetData> targetBuf;
+ mutable std::recursive_mutex lastTargetBufReadMutex;
+ armarx::TripleBuffer<FingersTargetData> lastTargetBuf;
};
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/ShapeInterface.ice b/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/ShapeInterface.ice
index 90b66d01..c36e6ee1 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/ShapeInterface.ice
+++ b/source/devices/ethercat/hand/soft_finger_vision/njoint_controller/ShapeInterface.ice
@@ -11,11 +11,16 @@ module armarx
float otherJointValue;
};
- interface NJointKITFingerVisionSoftHandV1ShapeControllerInterface extends NJointControllerInterface
+ interface NJointKITFingerVisionSoftHandV1ShapeControllerInterface extends
+ NJointControllerInterface
{
void setTargets(float lrm, float index, float thumb);
- void setTargetsWithPwm(float lrm, float index, float thumb, float lrmRelativePWM, float indexRelativePWM, float thumbRelativePWM);
+ void setTargetsWithPwm(float lrm,
+ float index,
+ float thumb,
+ float lrmRelativePWM,
+ float indexRelativePWM,
+ float thumbRelativePWM);
["cpp:const"] KITFingerVisionSoftHandV1JointValues getJointValues();
};
};
-
diff --git a/source/devices/ethercat/hand/soft_finger_vision/sensor_value/Hand.h b/source/devices/ethercat/hand/soft_finger_vision/sensor_value/Hand.h
index eff39d98..074131b6 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/sensor_value/Hand.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/sensor_value/Hand.h
@@ -9,7 +9,7 @@ namespace devices::ethercat::hand::finger_vision_soft
public:
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- float temperature;
+ float temperature;
std::uint32_t raw_temperature;
std::uint32_t frame_counter;
@@ -32,13 +32,13 @@ namespace devices::ethercat::hand::finger_vision_soft
std::uint32_t cnn_data_popcount_thumb_frame_counter;
std::uint32_t cnn_data_popcount_little_number_of_images = 0;
- std::uint32_t cnn_data_popcount_ring_number_of_images = 0;
+ std::uint32_t cnn_data_popcount_ring_number_of_images = 0;
std::uint32_t cnn_data_popcount_middle_number_of_images = 0;
- std::uint32_t cnn_data_popcount_index_number_of_images = 0;
- std::uint32_t cnn_data_popcount_thumb_number_of_images = 0;
- std::uint32_t cnn_data_popcount_error_number_of_images = 0;
+ std::uint32_t cnn_data_popcount_index_number_of_images = 0;
+ std::uint32_t cnn_data_popcount_thumb_number_of_images = 0;
+ std::uint32_t cnn_data_popcount_error_number_of_images = 0;
static SensorValueInfo<HandSensorValue> GetClassMemberInfo();
};
-} // namespace devices::ethercat::hand::finger_vision_soft
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/sensor_value/Motor.h b/source/devices/ethercat/hand/soft_finger_vision/sensor_value/Motor.h
index 7cbffffa..7b2a8020 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/sensor_value/Motor.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/sensor_value/Motor.h
@@ -12,11 +12,11 @@ namespace devices::ethercat::hand::finger_vision_soft
public:
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- std::int32_t raw_position;
- std::int32_t raw_velocity;
+ std::int32_t raw_position;
+ std::int32_t raw_velocity;
std::uint32_t pwm_max;
static SensorValueInfo<MotorSensorValue> GetClassMemberInfo();
};
-} // namespace devices::ethercat::hand::finger_vision_soft
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_finger_vision/utility/Identities.h b/source/devices/ethercat/hand/soft_finger_vision/utility/Identities.h
index 3d9b5761..281809cd 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/utility/Identities.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/utility/Identities.h
@@ -30,11 +30,10 @@ namespace devices::ethercat::hand::finger_vision_soft::identity
other = 1,
thumb = 2
};
- template<class T>
- static decltype(auto) Select(MotorIdentity id,
- T& other,
- T& index,
- T& thumb)
+
+ template <class T>
+ static decltype(auto)
+ Select(MotorIdentity id, T& other, T& index, T& thumb)
{
switch (id)
{
@@ -50,18 +49,15 @@ namespace devices::ethercat::hand::finger_vision_soft::identity
enum class FingerIdentity : std::size_t
{
little = 0,
- ring = 1,
+ ring = 1,
middle = 2,
- index = 3,
- thumb = 4
+ index = 3,
+ thumb = 4
};
- template<class T>
- static decltype(auto) Select(FingerIdentity id,
- T& little,
- T& ring,
- T& middle,
- T& index,
- T& thumb)
+
+ template <class T>
+ static decltype(auto)
+ Select(FingerIdentity id, T& little, T& ring, T& middle, T& index, T& thumb)
{
switch (id)
{
@@ -79,7 +75,7 @@ namespace devices::ethercat::hand::finger_vision_soft::identity
ARMARX_CHECK_EXPRESSION(!"unreachable code!");
}
- template<class ET, class VT, std::size_t N>
+ template <class ET, class VT, std::size_t N>
class EnumIndexedArray : public std::array<VT, N>
{
public:
@@ -88,60 +84,140 @@ namespace devices::ethercat::hand::finger_vision_soft::identity
using std::array<VT, N>::at;
- value_t& at(enum_t id)
+ value_t&
+ at(enum_t id)
{
return at(static_cast<std::size_t>(id));
}
- const value_t& at(enum_t id) const
+
+ const value_t&
+ at(enum_t id) const
{
return at(static_cast<std::size_t>(id));
}
- const value_t& operator()(enum_t id) const
+ const value_t&
+ operator()(enum_t id) const
{
return at(id);
}
- value_t& operator()(enum_t id)
+
+ value_t&
+ operator()(enum_t id)
{
return at(id);
}
};
- template<class T>
+ template <class T>
class FingerArray : public EnumIndexedArray<FingerIdentity, T, 5>
{
public:
// *INDENT-OFF*
- T& little() { return this->at(FingerIdentity::little); }
- const T& little() const { return this->at(FingerIdentity::little); }
+ T&
+ little()
+ {
+ return this->at(FingerIdentity::little);
+ }
+
+ const T&
+ little() const
+ {
+ return this->at(FingerIdentity::little);
+ }
+
+ T&
+ ring()
+ {
+ return this->at(FingerIdentity::ring);
+ }
+
+ const T&
+ ring() const
+ {
+ return this->at(FingerIdentity::ring);
+ }
+
+ T&
+ middle()
+ {
+ return this->at(FingerIdentity::middle);
+ }
+
+ const T&
+ middle() const
+ {
+ return this->at(FingerIdentity::middle);
+ }
+
+ T&
+ index()
+ {
+ return this->at(FingerIdentity::index);
+ }
- T& ring () { return this->at(FingerIdentity::ring ); }
- const T& ring () const { return this->at(FingerIdentity::ring ); }
+ const T&
+ index() const
+ {
+ return this->at(FingerIdentity::index);
+ }
- T& middle() { return this->at(FingerIdentity::middle); }
- const T& middle() const { return this->at(FingerIdentity::middle); }
+ T&
+ thumb()
+ {
+ return this->at(FingerIdentity::thumb);
+ }
- T& index () { return this->at(FingerIdentity::index ); }
- const T& index () const { return this->at(FingerIdentity::index ); }
+ const T&
+ thumb() const
+ {
+ return this->at(FingerIdentity::thumb);
+ }
- T& thumb () { return this->at(FingerIdentity::thumb ); }
- const T& thumb () const { return this->at(FingerIdentity::thumb ); }
// *INDENT-ON*
};
- template<class T>
+ template <class T>
class MotorArray : public EnumIndexedArray<MotorIdentity, T, 3>
{
public:
// *INDENT-OFF*
- T& other() { return this->at(MotorIdentity::other); }
- const T& other() const { return this->at(MotorIdentity::other); }
+ T&
+ other()
+ {
+ return this->at(MotorIdentity::other);
+ }
+
+ const T&
+ other() const
+ {
+ return this->at(MotorIdentity::other);
+ }
- T& index() { return this->at(MotorIdentity::index); }
- const T& index() const { return this->at(MotorIdentity::index); }
+ T&
+ index()
+ {
+ return this->at(MotorIdentity::index);
+ }
+
+ const T&
+ index() const
+ {
+ return this->at(MotorIdentity::index);
+ }
+
+ T&
+ thumb()
+ {
+ return this->at(MotorIdentity::thumb);
+ }
+
+ const T&
+ thumb() const
+ {
+ return this->at(MotorIdentity::thumb);
+ }
- T& thumb() { return this->at(MotorIdentity::thumb); }
- const T& thumb() const { return this->at(MotorIdentity::thumb); }
// *INDENT-ON*
};
-}
+} // namespace devices::ethercat::hand::finger_vision_soft::identity
diff --git a/source/devices/ethercat/hand/soft_finger_vision/utility/Sensors.h b/source/devices/ethercat/hand/soft_finger_vision/utility/Sensors.h
index 23cc3e32..89c203e0 100644
--- a/source/devices/ethercat/hand/soft_finger_vision/utility/Sensors.h
+++ b/source/devices/ethercat/hand/soft_finger_vision/utility/Sensors.h
@@ -24,16 +24,15 @@
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <devices/ethercat/hand/finger_vision_soft/sensor_value/Motor.h>
-#include <devices/ethercat/hand/finger_vision_soft/sensor_value/Hand.h>
-
#include "Identities.h"
+#include <devices/ethercat/hand/finger_vision_soft/sensor_value/Hand.h>
+#include <devices/ethercat/hand/finger_vision_soft/sensor_value/Motor.h>
namespace devices::ethercat::hand::finger_vision_soft
{
struct Sensors
{
- using MotorIdentity = identity::MotorIdentity;
+ using MotorIdentity = identity::MotorIdentity;
using FingerIdentity = identity::FingerIdentity;
identity::MotorArray<const MotorSensorValue*> motor;
@@ -49,14 +48,15 @@ namespace devices::ethercat::hand::finger_vision_soft
Sensors(const std::string& hand_name, NJointController& parent)
{
init(parent, hand_name, hand);
- init(parent, hand_name + ".motor.thumb", motor.thumb());
- init(parent, hand_name + ".motor.index", motor.index());
- init(parent, hand_name + ".motor.other", motor.other());
+ init(parent, hand_name + ".motor.thumb", motor.thumb());
+ init(parent, hand_name + ".motor.index", motor.index());
+ init(parent, hand_name + ".motor.other", motor.other());
}
private:
- template<class T>
- void init(NJointController& parent, const std::string& name, T const*& ptr)
+ template <class T>
+ void
+ init(NJointController& parent, const std::string& name, T const*& ptr)
{
const SensorValueBase* sv = parent.useSensorValue(name);
ARMARX_CHECK_NOT_NULL(sv) << VAROUT(name);
@@ -65,4 +65,4 @@ namespace devices::ethercat::hand::finger_vision_soft
}
};
-}
+} // namespace devices::ethercat::hand::finger_vision_soft
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/BoardIn.h b/source/devices/ethercat/hand/soft_sensorized_finger/BoardIn.h
index c7f46ca9..d91d167d 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/BoardIn.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/BoardIn.h
@@ -1,6 +1,5 @@
#pragma once
-
namespace devices::ethercat::hand::soft_sensorized_finger
{
struct BoardIn_motor_target
@@ -14,9 +13,9 @@ namespace devices::ethercat::hand::soft_sensorized_finger
BoardIn_motor_target thumb;
BoardIn_motor_target index;
BoardIn_motor_target other;
- uint16_t padding;
+ uint16_t padding;
} __attribute__((__packed__));
static_assert(sizeof(BoardIn) == (64) / 8);
-} // namespace devices::ethercat::hand::soft_sensorized_finger
+} // namespace devices::ethercat::hand::soft_sensorized_finger
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/BoardOut.h b/source/devices/ethercat/hand/soft_sensorized_finger/BoardOut.h
index e8c73f6c..838c712b 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/BoardOut.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/BoardOut.h
@@ -7,26 +7,26 @@ namespace devices::ethercat::hand::soft_sensorized_finger
struct BoardOut_joint_encoder_value
{
std::array<std::int16_t, 3> xyz;
- std::uint16_t temperature;
+ std::uint16_t temperature;
} __attribute__((__packed__));
struct BoardOut_pressure_value
{
std::uint32_t value;
std::int16_t temperature;
- std::int16_t padding;
+ std::int16_t padding;
} __attribute__((__packed__));
struct BoardOut_shear_force_value
{
std::array<std::int16_t, 3> xyz;
- std::uint16_t temperature;
+ std::uint16_t temperature;
} __attribute__((__packed__));
struct BoardOut_proximity_value
{
std::uint16_t value;
- std::int16_t padding;
+ std::int16_t padding;
} __attribute__((__packed__));
struct BoardOut_accelerometer_value
@@ -40,37 +40,37 @@ namespace devices::ethercat::hand::soft_sensorized_finger
BoardOut_joint_encoder_value joint_encoder_prox;
BoardOut_joint_encoder_value joint_encoder_dist;
- BoardOut_pressure_value pressure_prox_l;
- BoardOut_pressure_value pressure_prox_r;
- BoardOut_pressure_value pressure_dist_joint;
- BoardOut_pressure_value pressure_dist_tip;
+ BoardOut_pressure_value pressure_prox_l;
+ BoardOut_pressure_value pressure_prox_r;
+ BoardOut_pressure_value pressure_dist_joint;
+ BoardOut_pressure_value pressure_dist_tip;
- BoardOut_shear_force_value shear_force_dist_joint;
- BoardOut_shear_force_value shear_force_dist_tip;
+ BoardOut_shear_force_value shear_force_dist_joint;
+ BoardOut_shear_force_value shear_force_dist_tip;
- BoardOut_proximity_value proximity;
+ BoardOut_proximity_value proximity;
BoardOut_accelerometer_value accelerometer;
} __attribute__((__packed__));
struct BoardOut_motor_value
{
- std::int32_t relative_position;
- std::int32_t velocity;
+ std::int32_t relative_position;
+ std::int32_t velocity;
} __attribute__((__packed__));
/// @brief PDO mapping sensorB->master
struct BoardOut
{
- BoardOut_motor_value motor_thumb;
- BoardOut_motor_value motor_index;
- BoardOut_motor_value motor_other;
+ BoardOut_motor_value motor_thumb;
+ BoardOut_motor_value motor_index;
+ BoardOut_motor_value motor_other;
- std::uint32_t temperature;
+ std::uint32_t temperature;
- std::uint32_t sensor_frame_counter;
- std::uint16_t update_rate_main;
- std::uint16_t update_rate_fpga;
+ std::uint32_t sensor_frame_counter;
+ std::uint16_t update_rate_main;
+ std::uint16_t update_rate_fpga;
BoardOut_finger_value little;
BoardOut_finger_value ring;
@@ -82,4 +82,4 @@ namespace devices::ethercat::hand::soft_sensorized_finger
static_assert(sizeof(BoardOut) == (7168) / 8);
-} // namespace devices::ethercat::hand::soft_sensorized_finger
+} // namespace devices::ethercat::hand::soft_sensorized_finger
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/FunctionalDevice.cpp b/source/devices/ethercat/hand/soft_sensorized_finger/FunctionalDevice.cpp
index 68196b1b..e1e958fa 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/FunctionalDevice.cpp
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/FunctionalDevice.cpp
@@ -2,26 +2,26 @@
// armarx
-#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelperTemplates.h>
-#include <ArmarXCore/core/ManagedIceObject.h>
#include "joint_controller/EmergencyStop.h"
#include "joint_controller/Position.h"
-#include "joint_controller/Velocity.h"
#include "joint_controller/Pwm.h"
#include "joint_controller/StopMovement.h"
+#include "joint_controller/Velocity.h"
#include "njoint_controller/Shape.h"
#include "sensor_value/Motor.h"
-
//FingerSensorDevice
namespace devices::ethercat::hand::soft_sensorized_finger::detail
{
class FingerSensorDevice : public armarx::SensorDevice
{
friend class FunctionalDevice;
+
public:
FingerSensorDevice(const std::string& deviceName) :
DeviceBase(deviceName), SensorDevice(deviceName)
@@ -29,13 +29,15 @@ namespace devices::ethercat::hand::soft_sensorized_finger::detail
ARMARX_DEBUG << " ctor FingerSensorDevice " << deviceName;
}
- const armarx::SensorValueBase* getSensorValue() const override
+ const armarx::SensorValueBase*
+ getSensorValue() const override
{
return &sensorValue;
}
- void init(const joint_controller::FunctionalDevicePtr& handPtr,
- const bus_data::FingerSensorDataPtr& dataPtr)
+ void
+ init(const joint_controller::FunctionalDevicePtr& handPtr,
+ const bus_data::FingerSensorDataPtr& dataPtr)
{
ARMARX_TRACE;
@@ -45,18 +47,21 @@ namespace devices::ethercat::hand::soft_sensorized_finger::detail
hand = handPtr;
data = dataPtr;
}
- void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration) override
+
+ void
+ rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override
{
data->rtReadSensorValues(sensorValuesTimestamp, timeSinceLastIteration);
data->updateSensorValueStruct(sensorValue);
}
+
private:
joint_controller::FunctionalDevicePtr hand;
bus_data::FingerSensorDataPtr data;
FingerSensorValue sensorValue;
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::detail
//HandSensorDevice
namespace devices::ethercat::hand::soft_sensorized_finger::detail
@@ -64,6 +69,7 @@ namespace devices::ethercat::hand::soft_sensorized_finger::detail
class HandSensorDevice : public armarx::SensorDevice
{
friend class FunctionalDevice;
+
public:
HandSensorDevice(const std::string& deviceName) :
DeviceBase(deviceName), SensorDevice(deviceName)
@@ -71,13 +77,15 @@ namespace devices::ethercat::hand::soft_sensorized_finger::detail
ARMARX_DEBUG << " ctor HandSensorDevice " << deviceName;
}
- const HandSensorValue* getSensorValue() const override
+ const HandSensorValue*
+ getSensorValue() const override
{
return &sensorValue;
}
- void init(const joint_controller::FunctionalDevicePtr& handPtr,
- const bus_data::HandSensorDataPtr& dataPtr)
+ void
+ init(const joint_controller::FunctionalDevicePtr& handPtr,
+ const bus_data::HandSensorDataPtr& dataPtr)
{
ARMARX_DEBUG << " init HandSensorDevice " << getDeviceName();
ARMARX_CHECK_NOT_NULL(handPtr);
@@ -85,26 +93,29 @@ namespace devices::ethercat::hand::soft_sensorized_finger::detail
hand = handPtr;
data = dataPtr;
}
- void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration) override
+
+ void
+ rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override
{
data->rtReadSensorValues(sensorValuesTimestamp, timeSinceLastIteration);
data->updateSensorValueStruct(sensorValue);
}
+
private:
joint_controller::FunctionalDevicePtr hand;
bus_data::HandSensorDataPtr data;
HandSensorValue sensorValue;
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::detail
//MotorControlDevice
namespace devices::ethercat::hand::soft_sensorized_finger::detail
{
- class MotorControlDevice :
- public armarx::SensorDevice, public armarx::ControlDevice
+ class MotorControlDevice : public armarx::SensorDevice, public armarx::ControlDevice
{
friend class FunctionalDevice;
+
public:
MotorControlDevice(const std::string& deviceName) :
DeviceBase(deviceName), SensorDevice(deviceName), ControlDevice(deviceName)
@@ -112,15 +123,17 @@ namespace devices::ethercat::hand::soft_sensorized_finger::detail
ARMARX_DEBUG << " ctor MotorControlDevice " << deviceName;
}
- const armarx::SensorValueBase* getSensorValue() const override
+ const armarx::SensorValueBase*
+ getSensorValue() const override
{
return &sensorValue;
}
- void init(const joint_controller::FunctionalDevicePtr& handPtr,
- const HandSensorDevicePtr& devHandPtr,
- const bus_data::HandSensorDataPtr& dataHandPtr,
- const bus_data::MotorControlDataPtr& dataMotorPtr)
+ void
+ init(const joint_controller::FunctionalDevicePtr& handPtr,
+ const HandSensorDevicePtr& devHandPtr,
+ const bus_data::HandSensorDataPtr& dataHandPtr,
+ const bus_data::MotorControlDataPtr& dataMotorPtr)
{
ARMARX_TRACE;
ARMARX_DEBUG << " init MotorControlDevice " << getDeviceName();
@@ -136,20 +149,25 @@ namespace devices::ethercat::hand::soft_sensorized_finger::detail
ctrls.at(0).reset(new joint_controller::StopMovementController(dataMotor));
ctrls.at(1).reset(new joint_controller::EmergencyStopController(dataMotor));
ctrls.at(2).reset(new joint_controller::PwmController(dataMotor, hand));
- ctrls.at(3).reset(new joint_controller::PositionController(dataMotor, hand, getControlTargetAccessToken()));
- ctrls.at(4).reset(new joint_controller::VelocityController(dataMotor, hand, getControlTargetAccessToken()));
+ ctrls.at(3).reset(new joint_controller::PositionController(
+ dataMotor, hand, getControlTargetAccessToken()));
+ ctrls.at(4).reset(new joint_controller::VelocityController(
+ dataMotor, hand, getControlTargetAccessToken()));
for (const auto& ctrl : ctrls)
{
addJointController(ctrl.get());
}
}
- void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration) override
+
+ void
+ rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override
{
if (motorIsShutDown)
{
- motorIsShutDown = *handSensorTemp < dataHand->rtGetMotorTemperatureRestartThreshold();
+ motorIsShutDown =
+ *handSensorTemp < dataHand->rtGetMotorTemperatureRestartThreshold();
}
motorIsShutDown = *handSensorTemp >= dataHand->rtGetMotorTemperatureShutdownThreshold();
@@ -162,20 +180,23 @@ namespace devices::ethercat::hand::soft_sensorized_finger::detail
<< dataHand->rtGetMotorTemperatureShutdownThreshold()
<< ", threshold restart: "
<< dataHand->rtGetMotorTemperatureRestartThreshold()
- << ") - stopping motor - motor temperature: "
- << *handSensorTemp << " °C";
+ << ") - stopping motor - motor temperature: " << *handSensorTemp
+ << " °C";
}
dataMotor->setCommand(0);
}
dataMotor->rtWriteTargetValues(sensorValuesTimestamp, timeSinceLastIteration);
firstRun = false;
}
- void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration) override
+
+ void
+ rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override
{
dataMotor->rtReadSensorValues(sensorValuesTimestamp, timeSinceLastIteration);
dataMotor->updateSensorValueStruct(sensorValue);
}
+
private:
joint_controller::FunctionalDevicePtr hand;
@@ -189,18 +210,20 @@ namespace devices::ethercat::hand::soft_sensorized_finger::detail
bool motorIsShutDown = false;
bool firstRun = true;
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::detail
//ControllerWrapper
namespace devices::ethercat::hand::soft_sensorized_finger
{
TYPEDEF_PTRS_SHARED(ControllerWrapper);
+
class ControllerWrapper : public hands::common::AbstractHandUnitControllerWrapper
{
public:
joint_controller::JointPwmControllerPtr controller;
- void setShape(const std::string& shape) override
+ void
+ setShape(const std::string& shape) override
{
ARMARX_CHECK_EXPRESSION(controller);
if (!controller->isControllerActive())
@@ -229,21 +252,27 @@ namespace devices::ethercat::hand::soft_sensorized_finger
ARMARX_ERROR << "shape '" << shape << "' not supported.";
}
}
- std::string describeHandState() const override
+
+ std::string
+ describeHandState() const override
{
std::stringstream ss;
ss.precision(3);
ss << std::fixed;
- ss << controller->getIndexJointValue() << " " << controller->getIndexTarget() << " " << controller->getIndexPwm();
+ ss << controller->getIndexJointValue() << " " << controller->getIndexTarget() << " "
+ << controller->getIndexPwm();
ss << " / ";
- ss << controller->getOtherJointValue() << " " << controller->getOtherTarget() << " " << controller->getOtherPwm();
+ ss << controller->getOtherJointValue() << " " << controller->getOtherTarget() << " "
+ << controller->getOtherPwm();
ss << " / ";
- ss << controller->getThumbJointValue() << " " << controller->getThumbTarget() << " " << controller->getThumbPwm();
+ ss << controller->getThumbJointValue() << " " << controller->getThumbTarget() << " "
+ << controller->getThumbPwm();
ss << " " << (controller->isControlEnabled() ? "active" : "disabled");
return ss.str();
}
- void setJointAngles(const armarx::NameValueMap& targetJointAngles) override
+ void
+ setJointAngles(const armarx::NameValueMap& targetJointAngles) override
{
ARMARX_CHECK_EXPRESSION(controller);
if (!controller->isControllerActive())
@@ -252,7 +281,7 @@ namespace devices::ethercat::hand::soft_sensorized_finger
}
float indexTarget = controller->getIndexTarget();
- float lmfTarget = controller->getOtherTarget();
+ float lmfTarget = controller->getOtherTarget();
float thumbTarget = controller->getThumbTarget();
for (const auto& [name, value] : targetJointAngles)
{
@@ -276,43 +305,40 @@ namespace devices::ethercat::hand::soft_sensorized_finger
controller->setTargets(indexTarget, lmfTarget, thumbTarget);
}
- std::map<std::string, float> getActualJointValues() override
+ std::map<std::string, float>
+ getActualJointValues() override
{
NameValueMap jointValues;
- jointValues["index"] = controller->getIndexJointValue();
+ jointValues["index"] = controller->getIndexJointValue();
jointValues["other"] = controller->getOtherJointValue();
- jointValues["thumb"] = controller->getThumbJointValue();
+ jointValues["thumb"] = controller->getThumbJointValue();
return jointValues;
}
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger
//FunctionalDevice
namespace devices::ethercat::hand::soft_sensorized_finger
{
FunctionalDeviceFactory::SubClassRegistry
- FunctionalDevice::registry(
- "KITSensorizedSoftFingerHandV1",
- &FunctionalDeviceFactory::createInstance<FunctionalDevice>);
-
- FunctionalDevice::FunctionalDevice(
- RapidXmlReaderNode jointNode,
- DefaultRapidXmlReaderNode defaultConfigurationNode,
- VirtualRobot::RobotPtr const& robot
- ):
+ FunctionalDevice::registry("KITSensorizedSoftFingerHandV1",
+ &FunctionalDeviceFactory::createInstance<FunctionalDevice>);
+
+ FunctionalDevice::FunctionalDevice(RapidXmlReaderNode jointNode,
+ DefaultRapidXmlReaderNode defaultConfigurationNode,
+ VirtualRobot::RobotPtr const& robot) :
AbstractFunctionalDevice(
- defaultConfigurationNode
- .first_node("KITSensorizedSoftFingerHandV1DefaultConfiguration")
- .add_node_at_end(jointNode)),
+ defaultConfigurationNode.first_node("KITSensorizedSoftFingerHandV1DefaultConfiguration")
+ .add_node_at_end(jointNode)),
deviceName(jointNode.attribute_value("name")),
configNode(jointNode),
robot(robot),
slaveIdentifier(jointNode)
{
-
}
- void FunctionalDevice::init(SlavePtr slave)
+ void
+ FunctionalDevice::init(SlavePtr slave)
{
ARMARX_TRACE;
ARMARX_DEBUG << "init " << deviceName;
@@ -321,8 +347,8 @@ namespace devices::ethercat::hand::soft_sensorized_finger
if (slave->getSlaveIdentifier().ProductID != slaveIdentifier.ProductID)
{
std::stringstream reason;
- reason << "Invalid Serial Number for SensorBoard for arm joint : "
- << deviceName << "!\n"
+ reason << "Invalid Serial Number for SensorBoard for arm joint : " << deviceName
+ << "!\n"
<< "product ID in config: " << slaveIdentifier.ProductID << " "
<< "product ID from bus: " << slave->getSlaveIdentifier().ProductID;
throw LocalException(reason.str());
@@ -332,23 +358,24 @@ namespace devices::ethercat::hand::soft_sensorized_finger
//create devices
{
using SDevT = detail::FingerSensorDevice;
- littleSensorDevicePtr = armarx::make_shared<SDevT>(deviceName + ".finger.little");
- ringSensorDevicePtr = armarx::make_shared<SDevT>(deviceName + ".finger.ring");
- middleSensorDevicePtr = armarx::make_shared<SDevT>(deviceName + ".finger.middle");
- indexSensorDevicePtr = armarx::make_shared<SDevT>(deviceName + ".finger.index");
- thumbSensorDevicePtr = armarx::make_shared<SDevT>(deviceName + ".finger.thumb");
+ littleSensorDevicePtr = armarx::make_shared<SDevT>(deviceName + ".finger.little");
+ ringSensorDevicePtr = armarx::make_shared<SDevT>(deviceName + ".finger.ring");
+ middleSensorDevicePtr = armarx::make_shared<SDevT>(deviceName + ".finger.middle");
+ indexSensorDevicePtr = armarx::make_shared<SDevT>(deviceName + ".finger.index");
+ thumbSensorDevicePtr = armarx::make_shared<SDevT>(deviceName + ".finger.thumb");
using MDevT = detail::MotorControlDevice;
otherControlDevicePtr = armarx::make_shared<MDevT>(deviceName + ".motor.other");
- indexControlDevicePtr = armarx::make_shared<MDevT>(deviceName + ".motor.index");
- thumbControlDevicePtr = armarx::make_shared<MDevT>(deviceName + ".motor.thumb");
+ indexControlDevicePtr = armarx::make_shared<MDevT>(deviceName + ".motor.index");
+ thumbControlDevicePtr = armarx::make_shared<MDevT>(deviceName + ".motor.thumb");
using HDevT = detail::HandSensorDevice;
- handSensorDevicePtr = armarx::make_shared<HDevT>(deviceName);
+ handSensorDevicePtr = armarx::make_shared<HDevT>(deviceName);
}
}
- void FunctionalDevice::initData()
+ void
+ FunctionalDevice::initData()
{
ARMARX_TRACE;
ARMARX_DEBUG << "initData " << deviceName;
@@ -359,108 +386,99 @@ namespace devices::ethercat::hand::soft_sensorized_finger
//init sensor
{
auto* snsAll = handSlave->getOutputsPtr();
- auto init = [&](auto & sd, auto * sf, auto name)
- {
- sd = std::make_shared<FingerSensorData>(
- configNode, getNode(),
- snsAll, sf,
- name);
+ auto init = [&](auto& sd, auto* sf, auto name) {
+ sd =
+ std::make_shared<FingerSensorData>(configNode, getNode(), snsAll, sf, name);
};
init(littleSensorDataPtr, &snsAll->little, "little");
- init(ringSensorDataPtr, &snsAll->ring, "ring");
+ init(ringSensorDataPtr, &snsAll->ring, "ring");
init(middleSensorDataPtr, &snsAll->middle, "middle");
- init(indexSensorDataPtr, &snsAll->index, "index");
- init(thumbSensorDataPtr, &snsAll->thumb, "thumb");
+ init(indexSensorDataPtr, &snsAll->index, "index");
+ init(thumbSensorDataPtr, &snsAll->thumb, "thumb");
}
//init ctrl
{
auto* ctrlAll = handSlave->getInputsPtr();
auto* snsAll = handSlave->getOutputsPtr();
- auto init = [&](auto & sd, auto * cf, auto * sf, auto name)
+ auto init = [&](auto& sd, auto* cf, auto* sf, auto name)
{
ARMARX_TRACE_LITE;
sd = std::make_shared<MotorControlData>(
- configNode, getNode(),
- cf, snsAll, sf,
- name);
+ configNode, getNode(), cf, snsAll, sf, name);
};
- init(otherControlDataPtr,
- &ctrlAll->other,
- &snsAll->motor_other,
- "other");
- init(indexControlDataPtr,
- &ctrlAll->index,
- &snsAll->motor_index,
- "index");
- init(thumbControlDataPtr,
- &ctrlAll->thumb,
- &snsAll->motor_thumb,
- "thumb");
+ init(otherControlDataPtr, &ctrlAll->other, &snsAll->motor_other, "other");
+ init(indexControlDataPtr, &ctrlAll->index, &snsAll->motor_index, "index");
+ init(thumbControlDataPtr, &ctrlAll->thumb, &snsAll->motor_thumb, "thumb");
}
//hand
{
handSensorDataPtr = std::make_shared<HandSensorData>(
- configNode, getNode(), handSlave->getOutputsPtr(), "Hand");
+ configNode, getNode(), handSlave->getOutputsPtr(), "Hand");
}
}
//init devices
{
- littleSensorDevicePtr ->init(hand, littleSensorDataPtr);
- ringSensorDevicePtr ->init(hand, ringSensorDataPtr);
- middleSensorDevicePtr ->init(hand, middleSensorDataPtr);
- indexSensorDevicePtr ->init(hand, indexSensorDataPtr);
- thumbSensorDevicePtr ->init(hand, thumbSensorDataPtr);
-
- handSensorDevicePtr ->init(hand, handSensorDataPtr);
-
- otherControlDevicePtr->init(hand, handSensorDevicePtr, handSensorDataPtr, otherControlDataPtr);
- indexControlDevicePtr ->init(hand, handSensorDevicePtr, handSensorDataPtr, indexControlDataPtr);
- thumbControlDevicePtr ->init(hand, handSensorDevicePtr, handSensorDataPtr, thumbControlDataPtr);
+ littleSensorDevicePtr->init(hand, littleSensorDataPtr);
+ ringSensorDevicePtr->init(hand, ringSensorDataPtr);
+ middleSensorDevicePtr->init(hand, middleSensorDataPtr);
+ indexSensorDevicePtr->init(hand, indexSensorDataPtr);
+ thumbSensorDevicePtr->init(hand, thumbSensorDataPtr);
+
+ handSensorDevicePtr->init(hand, handSensorDataPtr);
+
+ otherControlDevicePtr->init(
+ hand, handSensorDevicePtr, handSensorDataPtr, otherControlDataPtr);
+ indexControlDevicePtr->init(
+ hand, handSensorDevicePtr, handSensorDataPtr, indexControlDataPtr);
+ thumbControlDevicePtr->init(
+ hand, handSensorDevicePtr, handSensorDataPtr, thumbControlDataPtr);
}
}
- SlaveIdentifier FunctionalDevice::getSlaveIdentifier() const
+ SlaveIdentifier
+ FunctionalDevice::getSlaveIdentifier() const
{
return slaveIdentifier;
}
- AbstractHandUnitControllerWrapperPtr FunctionalDevice::createHandUnitControllerWrapper(
- RobotUnit* robotUnit,
- const std::string& handName,
- RapidXmlReaderNode configNode) const
+ AbstractHandUnitControllerWrapperPtr
+ FunctionalDevice::createHandUnitControllerWrapper(RobotUnit* robotUnit,
+ const std::string& handName,
+ RapidXmlReaderNode configNode) const
{
ARMARX_TRACE;
ControllerWrapperPtr wrapper(new ControllerWrapper());
- NJointShapeControllerConfigPtr cfg =
- new NJointShapeControllerConfig(deviceName);
+ NJointShapeControllerConfigPtr cfg = new NJointShapeControllerConfig(deviceName);
ARMARX_INFO << "createHandUnitControllerWrapper " << VAROUT(deviceName);
wrapper->controller = NJointShapeControllerPtr::dynamicCast(
- robotUnit->createNJointController("NJointKITSensorizedSoftFingerHandV1ShapeController",
- handName + "_NJointShapeController",
- cfg, false, true));
+ robotUnit->createNJointController("NJointKITSensorizedSoftFingerHandV1ShapeController",
+ handName + "_NJointShapeController",
+ cfg,
+ false,
+ true));
ARMARX_TRACE;
- DefaultRapidXmlReaderNode defaultConfigurationNode
- {
- configNode
- .parent_node()
- .first_node("DefaultConfiguration")
- .first_node("KITSensorizedSoftFingerHandV1DefaultConfiguration")
- };
+ DefaultRapidXmlReaderNode defaultConfigurationNode{
+ configNode.parent_node()
+ .first_node("DefaultConfiguration")
+ .first_node("KITSensorizedSoftFingerHandV1DefaultConfiguration")};
wrapper->controller->readConfig(configNode, defaultConfigurationNode);
return wrapper;
}
- std::string FunctionalDevice::getHandConfigNodeName() const
+ std::string
+ FunctionalDevice::getHandConfigNodeName() const
{
return "KITSensorizedSoftFingerHandV1";
}
- std::string FunctionalDevice::getDeviceName() const
+ std::string
+ FunctionalDevice::getDeviceName() const
{
return deviceName;
}
- std::string FunctionalDevice::getHandDeviceName() const
+ std::string
+ FunctionalDevice::getHandDeviceName() const
{
return deviceName;
}
@@ -486,4 +504,4 @@ namespace devices::ethercat::hand::soft_sensorized_finger
// f->addSensorDevice(handSensorDevicePtr);
// }
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/FunctionalDevice.h b/source/devices/ethercat/hand/soft_sensorized_finger/FunctionalDevice.h
index d0690809..8d5f8794 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/FunctionalDevice.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/FunctionalDevice.h
@@ -8,25 +8,21 @@
#include <armarx/control/ethercat/AbstractDevice.h>
#include <armarx/control/ethercat/SlaveIdentifier.h>
-#include <devices/ethercat/hand/common/AbstractHand.h>
-#include <devices/ethercat/hand/common/AbstractHandUnitControllerWrapper.h>
-#include "bus_data/Hand.h"
+#include "Slave.h"
#include "bus_data/Finger.h"
+#include "bus_data/Hand.h"
#include "bus_data/Motor.h"
-#include "Slave.h"
-
+#include <devices/ethercat/hand/common/AbstractHand.h>
+#include <devices/ethercat/hand/common/AbstractHandUnitControllerWrapper.h>
namespace devices::ethercat::hand::soft_sensorized_finger::detail
{
- using FingerSensorDevicePtr =
- std::shared_ptr<class FingerSensorDevice>;
+ using FingerSensorDevicePtr = std::shared_ptr<class FingerSensorDevice>;
- using MotorControlDevicePtr =
- std::shared_ptr<class MotorControlDevice>;
+ using MotorControlDevicePtr = std::shared_ptr<class MotorControlDevice>;
- using HandSensorDevicePtr =
- std::shared_ptr<class HandSensorDevice>;
-}
+ using HandSensorDevicePtr = std::shared_ptr<class HandSensorDevice>;
+} // namespace devices::ethercat::hand::soft_sensorized_finger::detail
namespace devices::ethercat::hand::soft_sensorized_finger
{
@@ -45,10 +41,12 @@ namespace devices::ethercat::hand::soft_sensorized_finger
armarx::control::ethercat::SlaveIdentifier getSlaveIdentifier() const;
std::string getDeviceName() const;
+
public:
- common::AbstractHandUnitControllerWrapperPtr createHandUnitControllerWrapper(
- armarx::RobotUnit* robotUnit, const std::string& handName,
- armarx::RapidXmlReaderPtr configNode) const override;
+ common::AbstractHandUnitControllerWrapperPtr
+ createHandUnitControllerWrapper(armarx::RobotUnit* robotUnit,
+ const std::string& handName,
+ armarx::RapidXmlReaderPtr configNode) const override;
std::string getHandConfigNodeName() const override;
std::string getHandDeviceName() const override;
@@ -70,7 +68,7 @@ namespace devices::ethercat::hand::soft_sensorized_finger
bus_data::MotorControlDataPtr indexControlDataPtr;
bus_data::MotorControlDataPtr thumbControlDataPtr;
- bus_data::HandSensorDataPtr handSensorDataPtr;
+ bus_data::HandSensorDataPtr handSensorDataPtr;
///device objects for sensor and control
detail::FingerSensorDevicePtr littleSensorDevicePtr;
@@ -83,7 +81,7 @@ namespace devices::ethercat::hand::soft_sensorized_finger
detail::MotorControlDevicePtr indexControlDevicePtr;
detail::MotorControlDevicePtr thumbControlDevicePtr;
- detail::HandSensorDevicePtr handSensorDevicePtr;
+ detail::HandSensorDevicePtr handSensorDevicePtr;
///bus devices - serial form config and pointer to actual bus slave
const armarx::control::ethercat::SlaveIdentifier slaveIdentifier;
@@ -92,4 +90,4 @@ namespace devices::ethercat::hand::soft_sensorized_finger
IceUtil::Time lastReadUpdate, lastWriteUpdate;
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/Slave.cpp b/source/devices/ethercat/hand/soft_sensorized_finger/Slave.cpp
index e4034bd2..5705c7b1 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/Slave.cpp
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/Slave.cpp
@@ -44,27 +44,33 @@ namespace devices::ethercat::hand::soft_sensorized_finger
Slave::execute()
{
}
+
void
Slave::doMappings()
{
}
+
void
Slave::prepareForSafeOp()
{
}
+
void
Slave::finishPreparingForSafeOp()
{
}
+
void
Slave::prepareForOp()
{
}
+
void
Slave::finishPreparingForOp()
{
}
} // namespace devices::ethercat::hand::soft_sensorized_finger
+
namespace devices::ethercat::hand::soft_sensorized_finger
{
/**
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/Slave.h b/source/devices/ethercat/hand/soft_sensorized_finger/Slave.h
index 9aac2abc..168ebbc8 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/Slave.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/Slave.h
@@ -1,8 +1,8 @@
#pragma once
+#include <atomic>
#include <memory>
#include <thread>
-#include <atomic>
#include <armarx/control/ethercat/AbstractSlave.h>
// #include <armarx/control/ethercat/DeviceFactory.h>
@@ -13,17 +13,17 @@
namespace devices::ethercat::hand::soft_sensorized_finger
{
const std::uint32_t H2T_RIGHT_PRODUCT_CODE = 0x42424242;
- const std::uint32_t H2T_LEFT_PRODUCT_CODE = 0x43434343;
+ const std::uint32_t H2T_LEFT_PRODUCT_CODE = 0x43434343;
class Slave;
using SlavePtr = std::shared_ptr<Slave>;
- class Slave :
- public armarx::control::ethercat::AbstractSlaveWithInputOutput<BoardIn, BoardOut>
+ class Slave : public armarx::control::ethercat::AbstractSlaveWithInputOutput<BoardIn, BoardOut>
{
public:
- Slave(const armarx::control::ethercat::SlaveIdentifier slaveIdentifier, u_int16_t slaveNumber);
+ Slave(const armarx::control::ethercat::SlaveIdentifier slaveIdentifier,
+ u_int16_t slaveNumber);
void doMappings() override;
void prepareForOp() override;
@@ -56,4 +56,4 @@ namespace devices::ethercat::hand::soft_sensorized_finger
// };
// using KITSensorizedSoftFingerHandV1FactoryPtr = std::shared_ptr<KITSensorizedSoftFingerHandV1Factory>;
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Finger.cpp b/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Finger.cpp
index af428024..52eae43f 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Finger.cpp
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Finger.cpp
@@ -1,80 +1,106 @@
+#include "Finger.h"
+
#include <boost/algorithm/clamp.hpp>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
-#include "Finger.h"
#include "robot_devices/ethercat/hand/soft_sensorized_finger/sensor_value/Finger.h"
namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
{
- FingerSensorData::FingerSensorData(
- const armarx::RapidXmlReaderNode& node,
- armarx::DefaultRapidXmlReaderNode defaultConfigurationNode,
- BoardOut* sensorAll,
- BoardOut_finger_value* sensorFinger,
- const std::string& configName
- ):
+ FingerSensorData::FingerSensorData(const armarx::RapidXmlReaderNode& node,
+ armarx::DefaultRapidXmlReaderNode defaultConfigurationNode,
+ BoardOut* sensorAll,
+ BoardOut_finger_value* sensorFinger,
+ const std::string& configName) :
sensor_finger{sensorFinger}
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(sensorAll);
ARMARX_CHECK_NOT_NULL(sensorFinger);
- auto configNodeAll =
- defaultConfigurationNode
- .add_node_at_end(node)
- .first_node("Finger");
- auto cfg = configNodeAll
- .first_node("default")
- .add_node_at_end(configNodeAll.first_node(configName));
- ARMARX_DEBUG << "configName " << configName << "\nall config paths:\n" << cfg.getChildPaths();
-
- sensor_frame_counter = &sensorAll->sensor_frame_counter;
- joint_encoder_prox_temp .init(&sensorFinger->joint_encoder_prox .temperature, cfg.first_node("sensor_joint_encoder_prox_temp"));
- joint_encoder_dist_temp .init(&sensorFinger->joint_encoder_dist .temperature, cfg.first_node("sensor_joint_encoder_dist_temp"));
- pressure_prox_l_temp .init(&sensorFinger->pressure_prox_l .temperature, cfg.first_node("sensor_pressure_prox_l_temp"));
- pressure_prox_r_temp .init(&sensorFinger->pressure_prox_r .temperature, cfg.first_node("sensor_pressure_prox_r_temp"));
- pressure_dist_joint_temp .init(&sensorFinger->pressure_dist_joint .temperature, cfg.first_node("sensor_pressure_dist_joint_temp"));
- pressure_dist_tip_temp .init(&sensorFinger->pressure_dist_tip .temperature, cfg.first_node("sensor_pressure_dist_tip_temp"));
- shear_force_dist_joint_temp .init(&sensorFinger->shear_force_dist_joint.temperature, cfg.first_node("sensor_shear_force_dist_joint_temp"));
- shear_force_dist_tip_temp .init(&sensorFinger->shear_force_dist_tip .temperature, cfg.first_node("sensor_shear_force_dist_tip_temp"));
-
- joint_encoder_prox.at(0) .init(&sensorFinger->joint_encoder_prox.xyz.at(0), cfg.first_node("sensor_joint_encoder_prox_x"));
- joint_encoder_prox.at(1) .init(&sensorFinger->joint_encoder_prox.xyz.at(1), cfg.first_node("sensor_joint_encoder_prox_y"));
- joint_encoder_prox.at(2) .init(&sensorFinger->joint_encoder_prox.xyz.at(2), cfg.first_node("sensor_joint_encoder_prox_z"));
-
- joint_encoder_dist.at(0) .init(&sensorFinger->joint_encoder_dist.xyz.at(0), cfg.first_node("sensor_joint_encoder_dist_x"));
- joint_encoder_dist.at(1) .init(&sensorFinger->joint_encoder_dist.xyz.at(1), cfg.first_node("sensor_joint_encoder_dist_y"));
- joint_encoder_dist.at(2) .init(&sensorFinger->joint_encoder_dist.xyz.at(2), cfg.first_node("sensor_joint_encoder_dist_z"));
-
- pressure_prox_l .init(&sensorFinger->pressure_prox_l .value, cfg.first_node("sensor_pressure_prox_l"));
- pressure_prox_r .init(&sensorFinger->pressure_prox_r .value, cfg.first_node("sensor_pressure_prox_r"));
- pressure_dist_joint .init(&sensorFinger->pressure_dist_joint.value, cfg.first_node("sensor_pressure_dist_joint"));
- pressure_dist_tip .init(&sensorFinger->pressure_dist_tip .value, cfg.first_node("sensor_pressure_dist_tip"));
-
- shear_force_dist_joint.at(0) .init(&sensorFinger->shear_force_dist_joint.xyz.at(0), cfg.first_node("sensor_shear_force_dist_joint_x"));
- shear_force_dist_joint.at(1) .init(&sensorFinger->shear_force_dist_joint.xyz.at(1), cfg.first_node("sensor_shear_force_dist_joint_y"));
- shear_force_dist_joint.at(2) .init(&sensorFinger->shear_force_dist_joint.xyz.at(2), cfg.first_node("sensor_shear_force_dist_joint_z"));
-
- shear_force_dist_tip.at(0) .init(&sensorFinger->shear_force_dist_tip.xyz.at(0), cfg.first_node("sensor_shear_force_dist_tip_x"));
- shear_force_dist_tip.at(1) .init(&sensorFinger->shear_force_dist_tip.xyz.at(1), cfg.first_node("sensor_shear_force_dist_tip_y"));
- shear_force_dist_tip.at(2) .init(&sensorFinger->shear_force_dist_tip.xyz.at(2), cfg.first_node("sensor_shear_force_dist_tip_z"));
-
- proximity .init(&sensorFinger->proximity.value, cfg.first_node("sensor_proximity"));
+ auto configNodeAll = defaultConfigurationNode.add_node_at_end(node).first_node("Finger");
+ auto cfg = configNodeAll.first_node("default").add_node_at_end(
+ configNodeAll.first_node(configName));
+ ARMARX_DEBUG << "configName " << configName << "\nall config paths:\n"
+ << cfg.getChildPaths();
+
+ sensor_frame_counter = &sensorAll->sensor_frame_counter;
+ joint_encoder_prox_temp.init(&sensorFinger->joint_encoder_prox.temperature,
+ cfg.first_node("sensor_joint_encoder_prox_temp"));
+ joint_encoder_dist_temp.init(&sensorFinger->joint_encoder_dist.temperature,
+ cfg.first_node("sensor_joint_encoder_dist_temp"));
+ pressure_prox_l_temp.init(&sensorFinger->pressure_prox_l.temperature,
+ cfg.first_node("sensor_pressure_prox_l_temp"));
+ pressure_prox_r_temp.init(&sensorFinger->pressure_prox_r.temperature,
+ cfg.first_node("sensor_pressure_prox_r_temp"));
+ pressure_dist_joint_temp.init(&sensorFinger->pressure_dist_joint.temperature,
+ cfg.first_node("sensor_pressure_dist_joint_temp"));
+ pressure_dist_tip_temp.init(&sensorFinger->pressure_dist_tip.temperature,
+ cfg.first_node("sensor_pressure_dist_tip_temp"));
+ shear_force_dist_joint_temp.init(&sensorFinger->shear_force_dist_joint.temperature,
+ cfg.first_node("sensor_shear_force_dist_joint_temp"));
+ shear_force_dist_tip_temp.init(&sensorFinger->shear_force_dist_tip.temperature,
+ cfg.first_node("sensor_shear_force_dist_tip_temp"));
+
+ joint_encoder_prox.at(0).init(&sensorFinger->joint_encoder_prox.xyz.at(0),
+ cfg.first_node("sensor_joint_encoder_prox_x"));
+ joint_encoder_prox.at(1).init(&sensorFinger->joint_encoder_prox.xyz.at(1),
+ cfg.first_node("sensor_joint_encoder_prox_y"));
+ joint_encoder_prox.at(2).init(&sensorFinger->joint_encoder_prox.xyz.at(2),
+ cfg.first_node("sensor_joint_encoder_prox_z"));
+
+ joint_encoder_dist.at(0).init(&sensorFinger->joint_encoder_dist.xyz.at(0),
+ cfg.first_node("sensor_joint_encoder_dist_x"));
+ joint_encoder_dist.at(1).init(&sensorFinger->joint_encoder_dist.xyz.at(1),
+ cfg.first_node("sensor_joint_encoder_dist_y"));
+ joint_encoder_dist.at(2).init(&sensorFinger->joint_encoder_dist.xyz.at(2),
+ cfg.first_node("sensor_joint_encoder_dist_z"));
+
+ pressure_prox_l.init(&sensorFinger->pressure_prox_l.value,
+ cfg.first_node("sensor_pressure_prox_l"));
+ pressure_prox_r.init(&sensorFinger->pressure_prox_r.value,
+ cfg.first_node("sensor_pressure_prox_r"));
+ pressure_dist_joint.init(&sensorFinger->pressure_dist_joint.value,
+ cfg.first_node("sensor_pressure_dist_joint"));
+ pressure_dist_tip.init(&sensorFinger->pressure_dist_tip.value,
+ cfg.first_node("sensor_pressure_dist_tip"));
+
+ shear_force_dist_joint.at(0).init(&sensorFinger->shear_force_dist_joint.xyz.at(0),
+ cfg.first_node("sensor_shear_force_dist_joint_x"));
+ shear_force_dist_joint.at(1).init(&sensorFinger->shear_force_dist_joint.xyz.at(1),
+ cfg.first_node("sensor_shear_force_dist_joint_y"));
+ shear_force_dist_joint.at(2).init(&sensorFinger->shear_force_dist_joint.xyz.at(2),
+ cfg.first_node("sensor_shear_force_dist_joint_z"));
+
+ shear_force_dist_tip.at(0).init(&sensorFinger->shear_force_dist_tip.xyz.at(0),
+ cfg.first_node("sensor_shear_force_dist_tip_x"));
+ shear_force_dist_tip.at(1).init(&sensorFinger->shear_force_dist_tip.xyz.at(1),
+ cfg.first_node("sensor_shear_force_dist_tip_y"));
+ shear_force_dist_tip.at(2).init(&sensorFinger->shear_force_dist_tip.xyz.at(2),
+ cfg.first_node("sensor_shear_force_dist_tip_z"));
+
+ proximity.init(&sensorFinger->proximity.value, cfg.first_node("sensor_proximity"));
for (std::size_t i = 0; i < 17; ++i)
{
- accelerometer_fifo.at(i).at(0).init(&sensorFinger->accelerometer.fifo.at(i).at(0), cfg.first_node("sensor_accelerometer_x"));
- accelerometer_fifo.at(i).at(1).init(&sensorFinger->accelerometer.fifo.at(i).at(1), cfg.first_node("sensor_accelerometer_y"));
- accelerometer_fifo.at(i).at(2).init(&sensorFinger->accelerometer.fifo.at(i).at(2), cfg.first_node("sensor_accelerometer_z"));
+ accelerometer_fifo.at(i).at(0).init(&sensorFinger->accelerometer.fifo.at(i).at(0),
+ cfg.first_node("sensor_accelerometer_x"));
+ accelerometer_fifo.at(i).at(1).init(&sensorFinger->accelerometer.fifo.at(i).at(1),
+ cfg.first_node("sensor_accelerometer_y"));
+ accelerometer_fifo.at(i).at(2).init(&sensorFinger->accelerometer.fifo.at(i).at(2),
+ cfg.first_node("sensor_accelerometer_z"));
}
}
- void read_converter(auto& c)
+ void
+ read_converter(auto& c)
{
c.read();
}
- template<class T, std::size_t N>
- void read_converter(std::array<T, N>& cs)
+
+ template <class T, std::size_t N>
+ void
+ read_converter(std::array<T, N>& cs)
{
for (auto& c : cs)
{
@@ -82,9 +108,9 @@ namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
}
}
- void FingerSensorData::rtReadSensorValues(
- const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& /*timeSinceLastIteration*/)
+ void
+ FingerSensorData::rtReadSensorValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
{
read_converter(joint_encoder_prox_temp);
read_converter(joint_encoder_dist_temp);
@@ -106,69 +132,70 @@ namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
read_converter(accelerometer_fifo);
}
- void FingerSensorData::rtWriteTargetValues(
- const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& /*timeSinceLastIteration*/)
- {}
+ void
+ FingerSensorData::rtWriteTargetValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
+ {
+ }
- void FingerSensorData::updateSensorValueStruct(
- FingerSensorValue& data)
+ void
+ FingerSensorData::updateSensorValueStruct(FingerSensorValue& data)
{
- data.frame_counter = *sensor_frame_counter;
-
- data.joint_encoder_prox.x() = joint_encoder_prox.at(0) .value;
- data.joint_encoder_prox.y() = joint_encoder_prox.at(1) .value;
- data.joint_encoder_prox.z() = joint_encoder_prox.at(2) .value;
- data.joint_encoder_prox_temp = joint_encoder_prox_temp .value;
- data.raw_joint_encoder_prox = sensor_finger->joint_encoder_prox.xyz;
- data.raw_joint_encoder_prox_temp = sensor_finger->joint_encoder_prox.temperature;
-
- data.joint_encoder_dist.x() = joint_encoder_dist.at(0) .value;
- data.joint_encoder_dist.y() = joint_encoder_dist.at(1) .value;
- data.joint_encoder_dist.z() = joint_encoder_dist.at(2) .value;
- data.joint_encoder_dist_temp = joint_encoder_dist_temp .value;
- data.raw_joint_encoder_dist = sensor_finger->joint_encoder_dist.xyz;
- data.raw_joint_encoder_dist_temp = sensor_finger->joint_encoder_dist.temperature;
-
- data.pressure_prox_l = pressure_prox_l .value;
- data.pressure_prox_l_temp = pressure_prox_l_temp .value;
- data.raw_pressure_prox_l = sensor_finger->pressure_prox_l.value;
- data.raw_pressure_prox_l_temp = sensor_finger->pressure_prox_l.temperature;
-
- data.pressure_prox_r = pressure_prox_r .value;
- data.pressure_prox_r_temp = pressure_prox_r_temp .value;
- data.raw_pressure_prox_r = sensor_finger->pressure_prox_r.value;
- data.raw_pressure_prox_r_temp = sensor_finger->pressure_prox_r.temperature;
-
- data.pressure_dist_joint = pressure_dist_joint .value;
- data.pressure_dist_joint_temp = pressure_dist_joint_temp .value;
- data.raw_pressure_dist_joint = sensor_finger->pressure_dist_joint.value;
- data.raw_pressure_dist_joint_temp = sensor_finger->pressure_dist_joint.temperature;
-
- data.pressure_dist_tip = pressure_dist_tip .value;
- data.pressure_dist_tip_temp = pressure_dist_tip_temp .value;
- data.raw_pressure_dist_tip = sensor_finger->pressure_dist_tip.value;
- data.raw_pressure_dist_tip_temp = sensor_finger->pressure_dist_tip.temperature;
-
- data.shear_force_dist_joint.x() = shear_force_dist_joint.at(0).value;
- data.shear_force_dist_joint.y() = shear_force_dist_joint.at(1).value;
- data.shear_force_dist_joint.z() = shear_force_dist_joint.at(2).value;
- data.shear_force_dist_joint_temp = shear_force_dist_joint_temp .value;
- data.raw_shear_force_dist_joint = sensor_finger->shear_force_dist_joint.xyz;
+ data.frame_counter = *sensor_frame_counter;
+
+ data.joint_encoder_prox.x() = joint_encoder_prox.at(0).value;
+ data.joint_encoder_prox.y() = joint_encoder_prox.at(1).value;
+ data.joint_encoder_prox.z() = joint_encoder_prox.at(2).value;
+ data.joint_encoder_prox_temp = joint_encoder_prox_temp.value;
+ data.raw_joint_encoder_prox = sensor_finger->joint_encoder_prox.xyz;
+ data.raw_joint_encoder_prox_temp = sensor_finger->joint_encoder_prox.temperature;
+
+ data.joint_encoder_dist.x() = joint_encoder_dist.at(0).value;
+ data.joint_encoder_dist.y() = joint_encoder_dist.at(1).value;
+ data.joint_encoder_dist.z() = joint_encoder_dist.at(2).value;
+ data.joint_encoder_dist_temp = joint_encoder_dist_temp.value;
+ data.raw_joint_encoder_dist = sensor_finger->joint_encoder_dist.xyz;
+ data.raw_joint_encoder_dist_temp = sensor_finger->joint_encoder_dist.temperature;
+
+ data.pressure_prox_l = pressure_prox_l.value;
+ data.pressure_prox_l_temp = pressure_prox_l_temp.value;
+ data.raw_pressure_prox_l = sensor_finger->pressure_prox_l.value;
+ data.raw_pressure_prox_l_temp = sensor_finger->pressure_prox_l.temperature;
+
+ data.pressure_prox_r = pressure_prox_r.value;
+ data.pressure_prox_r_temp = pressure_prox_r_temp.value;
+ data.raw_pressure_prox_r = sensor_finger->pressure_prox_r.value;
+ data.raw_pressure_prox_r_temp = sensor_finger->pressure_prox_r.temperature;
+
+ data.pressure_dist_joint = pressure_dist_joint.value;
+ data.pressure_dist_joint_temp = pressure_dist_joint_temp.value;
+ data.raw_pressure_dist_joint = sensor_finger->pressure_dist_joint.value;
+ data.raw_pressure_dist_joint_temp = sensor_finger->pressure_dist_joint.temperature;
+
+ data.pressure_dist_tip = pressure_dist_tip.value;
+ data.pressure_dist_tip_temp = pressure_dist_tip_temp.value;
+ data.raw_pressure_dist_tip = sensor_finger->pressure_dist_tip.value;
+ data.raw_pressure_dist_tip_temp = sensor_finger->pressure_dist_tip.temperature;
+
+ data.shear_force_dist_joint.x() = shear_force_dist_joint.at(0).value;
+ data.shear_force_dist_joint.y() = shear_force_dist_joint.at(1).value;
+ data.shear_force_dist_joint.z() = shear_force_dist_joint.at(2).value;
+ data.shear_force_dist_joint_temp = shear_force_dist_joint_temp.value;
+ data.raw_shear_force_dist_joint = sensor_finger->shear_force_dist_joint.xyz;
data.raw_shear_force_dist_joint_temp = sensor_finger->shear_force_dist_joint.temperature;
- data.shear_force_dist_tip.x() = shear_force_dist_tip.at(0) .value;
- data.shear_force_dist_tip.y() = shear_force_dist_tip.at(1) .value;
- data.shear_force_dist_tip.z() = shear_force_dist_tip.at(2) .value;
- data.shear_force_dist_tip_temp = shear_force_dist_tip_temp .value;
- data.raw_shear_force_dist_tip = sensor_finger->shear_force_dist_tip.xyz;
- data.raw_shear_force_dist_tip_temp = sensor_finger->shear_force_dist_tip.temperature;
+ data.shear_force_dist_tip.x() = shear_force_dist_tip.at(0).value;
+ data.shear_force_dist_tip.y() = shear_force_dist_tip.at(1).value;
+ data.shear_force_dist_tip.z() = shear_force_dist_tip.at(2).value;
+ data.shear_force_dist_tip_temp = shear_force_dist_tip_temp.value;
+ data.raw_shear_force_dist_tip = sensor_finger->shear_force_dist_tip.xyz;
+ data.raw_shear_force_dist_tip_temp = sensor_finger->shear_force_dist_tip.temperature;
- data.proximity = proximity .value;
- data.raw_proximity = sensor_finger->proximity.value;
+ data.proximity = proximity.value;
+ data.raw_proximity = sensor_finger->proximity.value;
- data.raw_accelerometer_fifo = sensor_finger->accelerometer.fifo;
- data.accelerometer_fifo_length = sensor_finger->accelerometer.fifo_length;
+ data.raw_accelerometer_fifo = sensor_finger->accelerometer.fifo;
+ data.accelerometer_fifo_length = sensor_finger->accelerometer.fifo_length;
for (std::size_t i = 0; i < data.accelerometer_fifo.size(); ++i)
{
data.accelerometer_fifo.at(i).x() = accelerometer_fifo.at(i).at(0).value;
@@ -176,4 +203,4 @@ namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
data.accelerometer_fifo.at(i).z() = accelerometer_fifo.at(i).at(2).value;
}
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Finger.h b/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Finger.h
index e1eff128..9f8a652b 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Finger.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Finger.h
@@ -3,15 +3,13 @@
#include <memory>
-
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h>
#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <armarx/control/ethercat/AbstractData.h>
-
#include <armarx/control/rt_filters/MedianFilteredLinearConvertedValue.h>
#include "../Slave.h"
@@ -20,53 +18,54 @@
namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
{
class FingerSensorData;
- using FingerSensorDataPtr =
- std::shared_ptr<FingerSensorData>;
+ using FingerSensorDataPtr = std::shared_ptr<FingerSensorData>;
class FingerSensorData : public armarx::control::ethercat::AbstractData
{
public:
- FingerSensorData(
- const armarx::RapidXmlReaderNode& node,
- armarx::DefaultRapidXmlReaderNode defaultConfigurationNode,
- BoardOut* sensorAll,
- BoardOut_finger_value* sensorFinger,
- const std::string& configName);
+ FingerSensorData(const armarx::RapidXmlReaderNode& node,
+ armarx::DefaultRapidXmlReaderNode defaultConfigurationNode,
+ BoardOut* sensorAll,
+ BoardOut_finger_value* sensorFinger,
+ const std::string& configName);
- void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
- void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+ void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
void updateSensorValueStruct(FingerSensorValue& data);
+
private:
- template<class T>
+ template <class T>
using Conv = armarx::control::ethercat::LinearConvertedValue<T>;
- template<class T>
+ template <class T>
using ConvFiltered = armarx::control::rt_filters::MedianFilteredLinearConvertedValue<T>;
- BoardOut_finger_value* sensor_finger;
- std::uint32_t* sensor_frame_counter;
- ConvFiltered<std::uint16_t> joint_encoder_prox_temp;
- ConvFiltered<std::uint16_t> joint_encoder_dist_temp;
- ConvFiltered<std::int16_t> pressure_prox_l_temp;
- ConvFiltered<std::int16_t> pressure_prox_r_temp;
- ConvFiltered<std::int16_t> pressure_dist_joint_temp;
- ConvFiltered<std::int16_t> pressure_dist_tip_temp;
- ConvFiltered<std::uint16_t> shear_force_dist_joint_temp;
- ConvFiltered<std::uint16_t> shear_force_dist_tip_temp;
+ BoardOut_finger_value* sensor_finger;
+ std::uint32_t* sensor_frame_counter;
+ ConvFiltered<std::uint16_t> joint_encoder_prox_temp;
+ ConvFiltered<std::uint16_t> joint_encoder_dist_temp;
+ ConvFiltered<std::int16_t> pressure_prox_l_temp;
+ ConvFiltered<std::int16_t> pressure_prox_r_temp;
+ ConvFiltered<std::int16_t> pressure_dist_joint_temp;
+ ConvFiltered<std::int16_t> pressure_dist_tip_temp;
+ ConvFiltered<std::uint16_t> shear_force_dist_joint_temp;
+ ConvFiltered<std::uint16_t> shear_force_dist_tip_temp;
- std::array<ConvFiltered<std::int16_t>, 3> joint_encoder_prox;
- std::array<ConvFiltered<std::int16_t>, 3> joint_encoder_dist;
+ std::array<ConvFiltered<std::int16_t>, 3> joint_encoder_prox;
+ std::array<ConvFiltered<std::int16_t>, 3> joint_encoder_dist;
- ConvFiltered<u_int32_t> pressure_prox_l;
- ConvFiltered<u_int32_t> pressure_prox_r;
- ConvFiltered<u_int32_t> pressure_dist_joint;
- ConvFiltered<u_int32_t> pressure_dist_tip;
+ ConvFiltered<u_int32_t> pressure_prox_l;
+ ConvFiltered<u_int32_t> pressure_prox_r;
+ ConvFiltered<u_int32_t> pressure_dist_joint;
+ ConvFiltered<u_int32_t> pressure_dist_tip;
- std::array<ConvFiltered<std::int16_t>, 3> shear_force_dist_joint;
- std::array<ConvFiltered<std::int16_t>, 3> shear_force_dist_tip;
+ std::array<ConvFiltered<std::int16_t>, 3> shear_force_dist_joint;
+ std::array<ConvFiltered<std::int16_t>, 3> shear_force_dist_tip;
- ConvFiltered<std::uint16_t> proximity;
+ ConvFiltered<std::uint16_t> proximity;
std::array<std::array<Conv<std::int16_t>, 3>, 17> accelerometer_fifo;
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Hand.cpp b/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Hand.cpp
index f92275b8..7fd67598 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Hand.cpp
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Hand.cpp
@@ -1,35 +1,32 @@
+#include "Hand.h"
+
#include <boost/algorithm/clamp.hpp>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "Hand.h"
+#include <ArmarXCore/core/logging/Logging.h>
namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
{
- HandSensorData::HandSensorData(
- const armarx::RapidXmlReaderNode& node,
- armarx::DefaultRapidXmlReaderNode defaultConfigurationNode,
- BoardOut* sensorAll,
- const std::string& configName)
+ HandSensorData::HandSensorData(const armarx::RapidXmlReaderNode& node,
+ armarx::DefaultRapidXmlReaderNode defaultConfigurationNode,
+ BoardOut* sensorAll,
+ const std::string& configName)
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(sensorAll);
- auto cfg =
- defaultConfigurationNode
- .add_node_at_end(node)
- .first_node("Hand");
- ARMARX_DEBUG << "configName " << configName << "\nall config paths:\n" << cfg.getChildPaths();
+ auto cfg = defaultConfigurationNode.add_node_at_end(node).first_node("Hand");
+ ARMARX_DEBUG << "configName " << configName << "\nall config paths:\n"
+ << cfg.getChildPaths();
sensor_frame_counter = &sensorAll->sensor_frame_counter;
- update_rate_main = &sensorAll->update_rate_main;
- update_rate_fpga = &sensorAll->update_rate_fpga;
- raw_temperature = &sensorAll->temperature;
+ update_rate_main = &sensorAll->update_rate_main;
+ update_rate_fpga = &sensorAll->update_rate_fpga;
+ raw_temperature = &sensorAll->temperature;
//hand_shutdown_threshold
{
const auto node = cfg.first_node("hand_shutdown_threshold");
node.attribute_as("shutdown_temp", motor_temp_threshold_shutdown);
- node.attribute_as("restart_temp", motor_temp_threshold_restart);
+ node.attribute_as("restart_temp", motor_temp_threshold_restart);
}
//sensor_hand_temp
{
@@ -38,31 +35,33 @@ namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
node.attribute_as("b", temp_correction_b);
node.attribute_as("c", temp_correction_c);
- temperature = armarx::control::rt_filters::RtMedianFilter{node.attribute_as<std::size_t>("median_filter_width")};
+ temperature = armarx::control::rt_filters::RtMedianFilter{
+ node.attribute_as<std::size_t>("median_filter_width")};
}
}
- void HandSensorData::rtReadSensorValues(
- const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& /*timeSinceLastIteration*/)
- {}
+ void
+ HandSensorData::rtReadSensorValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
+ {
+ }
- void HandSensorData::rtWriteTargetValues(
- const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& /*timeSinceLastIteration*/)
- {}
+ void
+ HandSensorData::rtWriteTargetValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
+ {
+ }
- void HandSensorData::updateSensorValueStruct(
- HandSensorValue& data)
+ void
+ HandSensorData::updateSensorValueStruct(HandSensorValue& data)
{
- data.frame_counter = *sensor_frame_counter;
+ data.frame_counter = *sensor_frame_counter;
data.update_rate_main = *update_rate_main;
data.update_rate_fpga = *update_rate_fpga;
- data.raw_temperature = *raw_temperature;
+ data.raw_temperature = *raw_temperature;
// a*exp(b*x)+c
data.temperature = temperature.update(
- temp_correction_a *
- std::exp(data.raw_temperature * temp_correction_b) +
- temp_correction_c);
+ temp_correction_a * std::exp(data.raw_temperature * temp_correction_b) +
+ temp_correction_c);
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Hand.h b/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Hand.h
index 63ef51bc..b0f9b797 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Hand.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Hand.h
@@ -3,15 +3,13 @@
#include <memory>
-
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h>
#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <armarx/control/ethercat/AbstractData.h>
-
#include <armarx/control/rt_filters/AverageFilter.h>
#include <armarx/control/rt_filters/FirFilter.h>
#include <armarx/control/rt_filters/RtMedianFilter.h>
@@ -24,29 +22,31 @@
namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
{
class HandSensorData;
- using HandSensorDataPtr =
- std::shared_ptr<HandSensorData>;
+ using HandSensorDataPtr = std::shared_ptr<HandSensorData>;
class HandSensorData : public armarx::control::ethercat::AbstractData
{
public:
- HandSensorData(
- const armarx::RapidXmlReaderNode& node,
- armarx::DefaultRapidXmlReaderNode defaultConfigurationNode,
- BoardOut* sensorAll,
- const std::string& configName);
+ HandSensorData(const armarx::RapidXmlReaderNode& node,
+ armarx::DefaultRapidXmlReaderNode defaultConfigurationNode,
+ BoardOut* sensorAll,
+ const std::string& configName);
- void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
- void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+ void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
void updateSensorValueStruct(HandSensorValue& data);
- float rtGetMotorTemperatureShutdownThreshold() const
+ float
+ rtGetMotorTemperatureShutdownThreshold() const
{
return motor_temp_threshold_shutdown;
}
- float rtGetMotorTemperatureRestartThreshold() const
+ float
+ rtGetMotorTemperatureRestartThreshold() const
{
return motor_temp_threshold_restart;
}
@@ -56,12 +56,12 @@ namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
std::uint32_t* raw_temperature;
std::uint16_t* update_rate_main;
std::uint16_t* update_rate_fpga;
- float motor_temp_threshold_shutdown;
- float motor_temp_threshold_restart;
+ float motor_temp_threshold_shutdown;
+ float motor_temp_threshold_restart;
// a*exp(b*x)+c
- float temp_correction_a;
- float temp_correction_b;
- float temp_correction_c;
+ float temp_correction_a;
+ float temp_correction_b;
+ float temp_correction_c;
armarx::control::rt_filters::RtMedianFilter temperature;
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Motor.cpp b/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Motor.cpp
index 846a607b..2cfb7f42 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Motor.cpp
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Motor.cpp
@@ -1,25 +1,20 @@
#include "Motor.h"
-
#include <boost/algorithm/clamp.hpp>
// armarx
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
+#include <ArmarXCore/core/logging/Logging.h>
namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
{
- MotorControlData::MotorControlData(
- const armarx::RapidXmlReaderNode& node,
- armarx::DefaultRapidXmlReaderNode defaultConfigurationNode,
- BoardIn_motor_target* target,
- BoardOut* sensorAll,
- BoardOut_motor_value* sensorMotor,
- const std::string& configName
- ) :
- target{target},
- sensor_motor{sensorMotor}
+ MotorControlData::MotorControlData(const armarx::RapidXmlReaderNode& node,
+ armarx::DefaultRapidXmlReaderNode defaultConfigurationNode,
+ BoardIn_motor_target* target,
+ BoardOut* sensorAll,
+ BoardOut_motor_value* sensorMotor,
+ const std::string& configName) :
+ target{target}, sensor_motor{sensorMotor}
{
ARMARX_TRACE;
@@ -29,19 +24,15 @@ namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
{
ARMARX_TRACE;
- auto configNodeAll =
- defaultConfigurationNode
- .add_node_at_end(node)
- .first_node("Motor");
+ auto configNodeAll = defaultConfigurationNode.add_node_at_end(node).first_node("Motor");
- auto cfg = configNodeAll
- .first_node("default")
- .add_node_at_end(configNodeAll.first_node(configName));
+ auto cfg = configNodeAll.first_node("default").add_node_at_end(
+ configNodeAll.first_node(configName));
//low level pwm
{
auto cfg_pwm = cfg.first_node("ctrl_low_level_pwm");
- cfg_pwm.attribute_as("max", pwm_max);
+ cfg_pwm.attribute_as("max", pwm_max);
cfg_pwm.attribute_as("factor", pwm_factor);
ARMARX_CHECK_GREATER_EQUAL(pwm_max, 0);
}
@@ -71,47 +62,49 @@ namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
}
sensor_frame_counter = &sensorAll->sensor_frame_counter;
- velocity .init(&sensorMotor->velocity, cfg.first_node("sensor_motor_velocity"));
- relative_position.init(&sensorMotor->relative_position, cfg.first_node("sensor_motor_relative_position"));
+ velocity.init(&sensorMotor->velocity, cfg.first_node("sensor_motor_velocity"));
+ relative_position.init(&sensorMotor->relative_position,
+ cfg.first_node("sensor_motor_relative_position"));
}
}
- void MotorControlData::rtReadSensorValues(
- const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& /*timeSinceLastIteration*/)
+ void
+ MotorControlData::rtReadSensorValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
{
- velocity .read();
+ velocity.read();
relative_position.read();
}
- void MotorControlData::rtWriteTargetValues(
- const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& /*timeSinceLastIteration*/)
+ void
+ MotorControlData::rtWriteTargetValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
{
target->pwm = target_pwm_motor;
}
- void MotorControlData::setCommand(int16 pwm)
+ void
+ MotorControlData::setCommand(int16 pwm)
{
- const auto pwm_clamped = boost::algorithm::clamp<std::int64_t>(
- pwm, -pwm_max, pwm_max);
+ const auto pwm_clamped = boost::algorithm::clamp<std::int64_t>(pwm, -pwm_max, pwm_max);
target_pwm_motor = pwm_clamped;
}
- std::int16_t MotorControlData::getMaxPWM() const
+ std::int16_t
+ MotorControlData::getMaxPWM() const
{
return pwm_max;
}
- void MotorControlData::updateSensorValueStruct(
- MotorSensorValue& data)
+ void
+ MotorControlData::updateSensorValueStruct(MotorSensorValue& data)
{
- data.frame_counter = *sensor_frame_counter;
- data.velocity = velocity.value;
- data.position = relative_position.value;
- data.raw_velocity = sensor_motor->velocity;
- data.raw_position = sensor_motor->relative_position;
- data.motorPWM = target_pwm_motor;
- data.pwm_max = pwm_max;
+ data.frame_counter = *sensor_frame_counter;
+ data.velocity = velocity.value;
+ data.position = relative_position.value;
+ data.raw_velocity = sensor_motor->velocity;
+ data.raw_position = sensor_motor->relative_position;
+ data.motorPWM = target_pwm_motor;
+ data.pwm_max = pwm_max;
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Motor.h b/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Motor.h
index 378661cb..c600a591 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Motor.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/bus_data/Motor.h
@@ -3,22 +3,22 @@
#include <cstdint>
#include <memory>
-#include <osal.h>
+#include <osal.h>
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h>
#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <armarx/control/ethercat/AbstractData.h>
-
#include <armarx/control/rt_filters/MedianFilteredLinearConvertedValue.h>
+#include "robot_devices/ethercat/hand/soft_sensorized_finger/sensor_value/Motor.h"
+
#include "../Slave.h"
#include "../sensor_value/Motor.h"
-#include "robot_devices/ethercat/hand/soft_sensorized_finger/sensor_value/Motor.h"
namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
{
@@ -26,83 +26,99 @@ namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
{
public:
- MotorControlData(
- const armarx::RapidXmlReaderNode& node,
- armarx::DefaultRapidXmlReaderNode defaultConfigurationNode,
- BoardIn_motor_target* target,
- BoardOut* sensorAll,
- BoardOut_motor_value* sensorMotor,
- const std::string& configName
- );
-
- void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
- void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ MotorControlData(const armarx::RapidXmlReaderNode& node,
+ armarx::DefaultRapidXmlReaderNode defaultConfigurationNode,
+ BoardIn_motor_target* target,
+ BoardOut* sensorAll,
+ BoardOut_motor_value* sensorMotor,
+ const std::string& configName);
+
+ void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+ void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
void setCommand(std::int16_t pwm);
int16_t getMaxPWM() const;
- float getPosCtrlKp() const
+ float
+ getPosCtrlKp() const
{
return pos_ctrl_Kp;
}
- float getPosCtrlKi() const
+
+ float
+ getPosCtrlKi() const
{
return pos_ctrl_Ki;
}
- float getPosCtrlKd() const
+
+ float
+ getPosCtrlKd() const
{
return pos_ctrl_Kd;
}
- std::int16_t getPosCtrlDefaultMaxPWM() const
+
+ std::int16_t
+ getPosCtrlDefaultMaxPWM() const
{
return pos_ctrl_default_max_pwm;
}
- float getVelCtrlKp() const
+ float
+ getVelCtrlKp() const
{
return vel_ctrl_Kp;
}
- float getVelCtrlKi() const
+
+ float
+ getVelCtrlKi() const
{
return vel_ctrl_Ki;
}
- float getVelCtrlKd() const
+
+ float
+ getVelCtrlKd() const
{
return vel_ctrl_Kd;
}
- std::int16_t getVelCtrlDefaultMaxPWM() const
+
+ std::int16_t
+ getVelCtrlDefaultMaxPWM() const
{
return vel_ctrl_default_max_pwm;
}
- float getRelativeEncoderValue() const
+ float
+ getRelativeEncoderValue() const
{
return relative_position.value;
}
void updateSensorValueStruct(MotorSensorValue& data);
+
private:
- BoardIn_motor_target* target;
+ BoardIn_motor_target* target;
const BoardOut_motor_value* sensor_motor;
- std::uint32_t* sensor_frame_counter;
- armarx::control::rt_filters::MedianFilteredLinearConvertedValue<int32_t> velocity;
- armarx::control::rt_filters::MedianFilteredLinearConvertedValue<int32_t> relative_position;
+ std::uint32_t* sensor_frame_counter;
+ armarx::control::rt_filters::MedianFilteredLinearConvertedValue<int32_t> velocity;
+ armarx::control::rt_filters::MedianFilteredLinearConvertedValue<int32_t> relative_position;
- float pos_ctrl_Kp = 10;
- float pos_ctrl_Ki = 1;
- float pos_ctrl_Kd = 0;
+ float pos_ctrl_Kp = 10;
+ float pos_ctrl_Ki = 1;
+ float pos_ctrl_Kd = 0;
std::int16_t pos_ctrl_default_max_pwm;
- float vel_ctrl_Kp = 10;
- float vel_ctrl_Ki = 1;
- float vel_ctrl_Kd = 0;
+ float vel_ctrl_Kp = 10;
+ float vel_ctrl_Ki = 1;
+ float vel_ctrl_Kd = 0;
std::int16_t vel_ctrl_default_max_pwm;
std::int16_t pwm_max;
- int32_t pwm_factor;
+ int32_t pwm_factor;
// target values set from outside
int16_t target_pwm_motor;
};
- using MotorControlDataPtr =
- std::shared_ptr<MotorControlData>;
-}
+
+ using MotorControlDataPtr = std::shared_ptr<MotorControlData>;
+} // namespace devices::ethercat::hand::soft_sensorized_finger::bus_data
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/controller/CloseAndHold.cpp b/source/devices/ethercat/hand/soft_sensorized_finger/controller/CloseAndHold.cpp
index ceee8417..573ba67b 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/controller/CloseAndHold.cpp
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/controller/CloseAndHold.cpp
@@ -1,28 +1,29 @@
-#include <boost/algorithm/clamp.hpp>
-
#include "CloseAndHold.h"
+#include <boost/algorithm/clamp.hpp>
+
namespace devices::ethercat::hand::soft_sensorized_finger::controller
{
CloseAndHold::CloseAndHold(const CloseAndHoldParams& p)
{
cfg.holding_start_force_threshold = p.holding_start_force_threshold;
- cfg.holding_stop_force_threshold = p.holding_stop_force_threshold;
+ cfg.holding_stop_force_threshold = p.holding_stop_force_threshold;
state.vel_ctrl = VelocityToPWM{p.p, p.i, p.d};
}
- void CloseAndHold::reset(float pos)
+ void
+ CloseAndHold::reset(float pos)
{
state.vel_ctrl.reset(pos);
}
- int16_t CloseAndHold::calculate(
- float force,
- float curr_pos,
- float velocity,
- float dt,
- std::int16_t maxPWM)
+ int16_t
+ CloseAndHold::calculate(float force,
+ float curr_pos,
+ float velocity,
+ float dt,
+ std::int16_t maxPWM)
{
if (force <= cfg.holding_stop_force_threshold)
{
@@ -45,21 +46,18 @@ namespace devices::ethercat::hand::soft_sensorized_finger::controller
{
case HoldingMode::pwm:
{
- out_pwm = (state.holding && velocity >= 0) ?
- cfg.holding_pwm :
- state.vel_ctrl.calculate(curr_pos, velocity, dt, maxPWM);
+ out_pwm = (state.holding && velocity >= 0)
+ ? cfg.holding_pwm
+ : state.vel_ctrl.calculate(curr_pos, velocity, dt, maxPWM);
}
break;
case HoldingMode::velocity:
{
out_pwm = state.vel_ctrl.calculate(
- curr_pos,
- state.holding ? cfg.holding_velocity : velocity,
- dt,
- maxPWM);
+ curr_pos, state.holding ? cfg.holding_velocity : velocity, dt, maxPWM);
}
break;
};
return out_pwm;
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/controller/CloseAndHold.h b/source/devices/ethercat/hand/soft_sensorized_finger/controller/CloseAndHold.h
index b17ac459..9a32ae58 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/controller/CloseAndHold.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/controller/CloseAndHold.h
@@ -10,8 +10,9 @@ namespace devices::ethercat::hand::soft_sensorized_finger::controller
float i = 0;
float d = 0;
float holding_start_force_threshold = 0;
- float holding_stop_force_threshold = 0;
+ float holding_stop_force_threshold = 0;
};
+
struct CloseAndHold
{
CloseAndHold(const CloseAndHoldParams& p);
@@ -23,12 +24,8 @@ namespace devices::ethercat::hand::soft_sensorized_finger::controller
CloseAndHold& operator=(const CloseAndHold&) = default;
void reset(float pos);
- std::int16_t calculate(
- float force,
- float curr_pos,
- float velocity,
- float dt,
- std::int16_t maxPWM);
+ std::int16_t
+ calculate(float force, float curr_pos, float velocity, float dt, std::int16_t maxPWM);
enum class HoldingMode
{
@@ -39,20 +36,23 @@ namespace devices::ethercat::hand::soft_sensorized_finger::controller
struct Configuration
{
float holding_start_force_threshold = 0;
- float holding_stop_force_threshold = 0;
+ float holding_stop_force_threshold = 0;
HoldingMode holding_mode = HoldingMode::velocity;
float holding_velocity = 0;
std::int16_t holding_pwm = 100;
};
+
Configuration cfg;
+
struct State
{
VelocityToPWM vel_ctrl;
- bool holding = false;
- std::int16_t last_pwm = 0;
+ bool holding = false;
+ std::int16_t last_pwm = 0;
};
+
State state;
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/controller/PositionToPWM.cpp b/source/devices/ethercat/hand/soft_sensorized_finger/controller/PositionToPWM.cpp
index 04a9dde6..fb36b183 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/controller/PositionToPWM.cpp
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/controller/PositionToPWM.cpp
@@ -1,25 +1,25 @@
-#include <boost/algorithm/clamp.hpp>
-
#include "PositionToPWM.h"
+#include <boost/algorithm/clamp.hpp>
+
namespace devices::ethercat::hand::soft_sensorized_finger::controller
{
- PositionToPWM::PositionToPWM(float p, float i, float d) :
- PID{p, i, d}
+ PositionToPWM::PositionToPWM(float p, float i, float d) : PID{p, i, d}
{
PID.threadSafe = false;
}
- void PositionToPWM::reset()
+ void
+ PositionToPWM::reset()
{
PID.reset();
}
- int16_t PositionToPWM::calculate(float curr_pos, float targ_pos, float dt, int16_t maxPWM)
+ int16_t
+ PositionToPWM::calculate(float curr_pos, float targ_pos, float dt, int16_t maxPWM)
{
PID.update(dt, curr_pos, boost::algorithm::clamp(targ_pos, 0.0f, 1.0f));
return boost::algorithm::clamp(
- static_cast<std::int16_t>(PID.getControlValue() * maxPWM),
- -maxPWM, maxPWM);
+ static_cast<std::int16_t>(PID.getControlValue() * maxPWM), -maxPWM, maxPWM);
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/controller/PositionToPWM.h b/source/devices/ethercat/hand/soft_sensorized_finger/controller/PositionToPWM.h
index 14d9682f..8c3e0cdc 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/controller/PositionToPWM.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/controller/PositionToPWM.h
@@ -18,4 +18,4 @@ namespace devices::ethercat::hand::soft_sensorized_finger::controller
std::int16_t calculate(float curr_pos, float targ_pos, float dt, std::int16_t maxPWM);
armarx::PIDController PID;
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/controller/VelocityToPWM.cpp b/source/devices/ethercat/hand/soft_sensorized_finger/controller/VelocityToPWM.cpp
index f521aa47..5a066607 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/controller/VelocityToPWM.cpp
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/controller/VelocityToPWM.cpp
@@ -1,24 +1,24 @@
-#include <boost/algorithm/clamp.hpp>
-
#include "VelocityToPWM.h"
+#include <boost/algorithm/clamp.hpp>
+
namespace devices::ethercat::hand::soft_sensorized_finger::controller
{
- VelocityToPWM::VelocityToPWM(float p, float i, float d) :
- pos_ctrl{p, i, d}
- {}
+ VelocityToPWM::VelocityToPWM(float p, float i, float d) : pos_ctrl{p, i, d}
+ {
+ }
- void VelocityToPWM::reset(float pos)
+ void
+ VelocityToPWM::reset(float pos)
{
pos_ctrl.reset();
- virtual_pos_target = pos;
+ virtual_pos_target = pos;
}
- int16_t VelocityToPWM::calculate(float curr_pos, float velocity, float dt, int16_t maxPWM)
+ int16_t
+ VelocityToPWM::calculate(float curr_pos, float velocity, float dt, int16_t maxPWM)
{
- virtual_pos_target = boost::algorithm::clamp(
- virtual_pos_target + velocity * dt,
- 0, 1);
+ virtual_pos_target = boost::algorithm::clamp(virtual_pos_target + velocity * dt, 0, 1);
return pos_ctrl.calculate(curr_pos, virtual_pos_target, dt, maxPWM);
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/controller/VelocityToPWM.h b/source/devices/ethercat/hand/soft_sensorized_finger/controller/VelocityToPWM.h
index 4fee9f4c..0829afe4 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/controller/VelocityToPWM.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/controller/VelocityToPWM.h
@@ -17,6 +17,6 @@ namespace devices::ethercat::hand::soft_sensorized_finger::controller
void reset(float pos);
std::int16_t calculate(float curr_pos, float velocity, float dt, std::int16_t maxPWM);
PositionToPWM pos_ctrl;
- float virtual_pos_target = 0;
+ float virtual_pos_target = 0;
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/EmergencyStop.cpp b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/EmergencyStop.cpp
index f131f7f2..490459ac 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/EmergencyStop.cpp
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/EmergencyStop.cpp
@@ -2,23 +2,26 @@
namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
{
- EmergencyStopController::EmergencyStopController(
- bus_data::MotorControlDataPtr dataPtr) :
+ EmergencyStopController::EmergencyStopController(bus_data::MotorControlDataPtr dataPtr) :
dataPtr(dataPtr)
- {}
- void EmergencyStopController::rtRun(
- const IceUtil::Time&, const IceUtil::Time&)
+ {
+ }
+
+ void
+ EmergencyStopController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
{
dataPtr->setCommand(0);
}
- armarx::ControlTargetBase* EmergencyStopController::getControlTarget()
+ armarx::ControlTargetBase*
+ EmergencyStopController::getControlTarget()
{
return ⌖
}
- void EmergencyStopController::rtPreActivateController()
+ void
+ EmergencyStopController::rtPreActivateController()
{
dataPtr->setCommand(0);
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/EmergencyStop.h b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/EmergencyStop.h
index eca5b74e..49fcd764 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/EmergencyStop.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/EmergencyStop.h
@@ -9,19 +9,20 @@ namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
class EmergencyStopController : public armarx::JointController
{
public:
- EmergencyStopController(
- bus_data::MotorControlDataPtr dataPtr);
+ EmergencyStopController(bus_data::MotorControlDataPtr dataPtr);
+
private:
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
void rtPreActivateController() override;
+
private:
armarx::DummyControlTargetEmergencyStop target;
bus_data::MotorControlDataPtr dataPtr;
};
- using JointEmergencyStopControllerPtr =
- std::shared_ptr<EmergencyStopController>;
+ using JointEmergencyStopControllerPtr = std::shared_ptr<EmergencyStopController>;
-} // namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
+} // namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Position.cpp b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Position.cpp
index aa893c8b..7f6b1aab 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Position.cpp
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Position.cpp
@@ -1,7 +1,8 @@
+#include "Position.h"
+
#include <RobotAPI/libraries/core/math/MathUtils.h>
#include "../FunctionalDevice.h"
-#include "Position.h"
namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
{
@@ -18,32 +19,33 @@ namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
target.setPWMLimits(data->getMaxPWM(), data->getPosCtrlDefaultMaxPWM(), token);
}
- armarx::ControlTargetBase* PositionController::getControlTarget()
+ armarx::ControlTargetBase*
+ PositionController::getControlTarget()
{
return ⌖
}
- void PositionController::rtRun(
- const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& timeSinceLastIteration)
+ void
+ PositionController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& timeSinceLastIteration)
{
ARMARX_CHECK_EXPRESSION(target.isValid());
- last_pwm = ctrl.calculate(
- data->getRelativeEncoderValue(),
- target.position,
- timeSinceLastIteration.toSecondsDouble(),
- std::min<std::int16_t>(data->getMaxPWM(), target.maxPWM)
- );
+ last_pwm = ctrl.calculate(data->getRelativeEncoderValue(),
+ target.position,
+ timeSinceLastIteration.toSecondsDouble(),
+ std::min<std::int16_t>(data->getMaxPWM(), target.maxPWM));
data->setCommand(last_pwm);
}
- void PositionController::rtPreActivateController()
+ void
+ PositionController::rtPreActivateController()
{
ctrl.reset();
}
- void PositionController::rtPostDeactivateController()
+ void
+ PositionController::rtPostDeactivateController()
{
data->setCommand(last_pwm);
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Position.h b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Position.h
index 1da45d46..d9ad916e 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Position.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Position.h
@@ -1,10 +1,10 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include "../bus_data/Motor.h"
#include "../FunctionalDevice.h"
+#include "../bus_data/Motor.h"
#include "../controller/PositionToPWM.h"
#include "../joint_controller/Pwm.h"
@@ -13,10 +13,9 @@ namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
class PositionController : public armarx::JointController
{
public:
- PositionController(
- bus_data::MotorControlDataPtr data,
- FunctionalDevicePtr hand,
- armarx::ControlTargetBase::ControlDeviceAccessToken token);
+ PositionController(bus_data::MotorControlDataPtr data,
+ FunctionalDevicePtr hand,
+ armarx::ControlTargetBase::ControlDeviceAccessToken token);
armarx::ControlTargetBase* getControlTarget() override;
void rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration) override;
@@ -24,13 +23,12 @@ namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
void rtPostDeactivateController() override;
private:
- bus_data::MotorControlDataPtr data;
- FunctionalDevicePtr hand;
- armarx::ControlTarget1DoFActuatorPositionWithPWMLimit target;
+ bus_data::MotorControlDataPtr data;
+ FunctionalDevicePtr hand;
+ armarx::ControlTarget1DoFActuatorPositionWithPWMLimit target;
controller::PositionToPWM ctrl;
- std::int16_t last_pwm;
+ std::int16_t last_pwm;
};
- using PositionControllerPtr =
- std::shared_ptr<PositionController>;
-} // namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
+ using PositionControllerPtr = std::shared_ptr<PositionController>;
+} // namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Pwm.cpp b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Pwm.cpp
index 43aa0aa9..5a6b9e0e 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Pwm.cpp
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Pwm.cpp
@@ -2,32 +2,33 @@
namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
{
- PwmController::PwmController(
- bus_data::MotorControlDataPtr data,
- FunctionalDevicePtr hand) :
- data(data),
- hand(hand)
+ PwmController::PwmController(bus_data::MotorControlDataPtr data, FunctionalDevicePtr hand) :
+ data(data), hand(hand)
{
ARMARX_CHECK_NOT_NULL(data);
ARMARX_CHECK_NOT_NULL(hand);
}
- armarx::ControlTargetBase* PwmController::getControlTarget()
+ armarx::ControlTargetBase*
+ PwmController::getControlTarget()
{
return ⌖
}
- void PwmController::rtRun(
- const IceUtil::Time&, const IceUtil::Time&)
+ void
+ PwmController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
{
data->setCommand(target.pwm);
}
- void PwmController::rtPreActivateController()
- {}
+ void
+ PwmController::rtPreActivateController()
+ {
+ }
- void PwmController::rtPostDeactivateController()
+ void
+ PwmController::rtPostDeactivateController()
{
data->setCommand(0);
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Pwm.h b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Pwm.h
index 93c759db..5bb15853 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Pwm.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Pwm.h
@@ -1,28 +1,28 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include "../bus_data/Motor.h"
#include "../FunctionalDevice.h"
+#include "../bus_data/Motor.h"
namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
{
class PwmController : public armarx::JointController
{
public:
- PwmController(
- bus_data::MotorControlDataPtr data,
- FunctionalDevicePtr hand);
+ PwmController(bus_data::MotorControlDataPtr data, FunctionalDevicePtr hand);
armarx::ControlTargetBase* getControlTarget() override;
void rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration) override;
void rtPreActivateController() override;
void rtPostDeactivateController() override;
+
private:
bus_data::MotorControlDataPtr data;
FunctionalDevicePtr hand;
armarx::ControlTarget1DoFActuatorPWM target;
};
+
using JointPwmControllerPtr = std::shared_ptr<PwmController>;
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/StopMovement.cpp b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/StopMovement.cpp
index 213a4339..822f7432 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/StopMovement.cpp
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/StopMovement.cpp
@@ -2,24 +2,26 @@
namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
{
- StopMovementController::StopMovementController(
- bus_data::MotorControlDataPtr dataPtr) :
+ StopMovementController::StopMovementController(bus_data::MotorControlDataPtr dataPtr) :
dataPtr(dataPtr)
- {}
+ {
+ }
- void StopMovementController::rtRun(
- const IceUtil::Time&, const IceUtil::Time&)
+ void
+ StopMovementController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
{
dataPtr->setCommand(0);
}
- armarx::ControlTargetBase* StopMovementController::getControlTarget()
+ armarx::ControlTargetBase*
+ StopMovementController::getControlTarget()
{
return ⌖
}
- void StopMovementController::rtPreActivateController()
+ void
+ StopMovementController::rtPreActivateController()
{
dataPtr->setCommand(0);
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/StopMovement.h b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/StopMovement.h
index 2ae74a90..79d5c5c2 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/StopMovement.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/StopMovement.h
@@ -9,17 +9,16 @@ namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
class StopMovementController : public armarx::JointController
{
public:
- StopMovementController(
- bus_data::MotorControlDataPtr dataPtr);
+ StopMovementController(bus_data::MotorControlDataPtr dataPtr);
void rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
void rtPreActivateController() override;
+
private:
armarx::DummyControlTargetStopMovement target;
bus_data::MotorControlDataPtr dataPtr;
};
- using StopMovementControllerPtr =
- std::shared_ptr<StopMovementController>;
-}
+ using StopMovementControllerPtr = std::shared_ptr<StopMovementController>;
+} // namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Velocity.cpp b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Velocity.cpp
index 16cd243d..2a991f49 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Velocity.cpp
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Velocity.cpp
@@ -1,9 +1,10 @@
+#include "Velocity.h"
+
#include <boost/algorithm/clamp.hpp>
#include <RobotAPI/libraries/core/math/MathUtils.h>
#include "../FunctionalDevice.h"
-#include "Velocity.h"
namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
{
@@ -20,33 +21,34 @@ namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
target.setPWMLimits(data->getMaxPWM(), data->getVelCtrlDefaultMaxPWM(), token);
}
- armarx::ControlTargetBase* VelocityController::getControlTarget()
+ armarx::ControlTargetBase*
+ VelocityController::getControlTarget()
{
return ⌖
}
- void VelocityController::rtRun(
- const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& timeSinceLastIteration)
+ void
+ VelocityController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& timeSinceLastIteration)
{
ARMARX_CHECK_EXPRESSION(target.isValid());
- last_pwm = ctrl.calculate(
- data->getRelativeEncoderValue(),
- target.velocity,
- timeSinceLastIteration.toSecondsDouble(),
- std::min<std::int16_t>(data->getMaxPWM(), target.maxPWM)
- );
+ last_pwm = ctrl.calculate(data->getRelativeEncoderValue(),
+ target.velocity,
+ timeSinceLastIteration.toSecondsDouble(),
+ std::min<std::int16_t>(data->getMaxPWM(), target.maxPWM));
data->setCommand(last_pwm);
}
- void VelocityController::rtPreActivateController()
+ void
+ VelocityController::rtPreActivateController()
{
ctrl.reset(data->getRelativeEncoderValue());
}
- void VelocityController::rtPostDeactivateController()
+ void
+ VelocityController::rtPostDeactivateController()
{
data->setCommand(last_pwm);
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Velocity.h b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Velocity.h
index 2712cc61..1e9a8ec4 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Velocity.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/joint_controller/Velocity.h
@@ -1,36 +1,37 @@
#pragma once
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/libraries/core/PIDController.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include "../bus_data/Motor.h"
+#include "robot_devices/ethercat/hand/soft_sensorized_finger/joint_controller/Pwm.h"
+
#include "../FunctionalDevice.h"
+#include "../bus_data/Motor.h"
#include "../controller/VelocityToPWM.h"
#include "../joint_controller/Pwm.h"
-#include "robot_devices/ethercat/hand/soft_sensorized_finger/joint_controller/Pwm.h"
namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
{
class VelocityController : public armarx::JointController
{
public:
- VelocityController(
- bus_data::MotorControlDataPtr data,
- FunctionalDevicePtr hand,
- armarx::ControlTargetBase::ControlDeviceAccessToken token);
+ VelocityController(bus_data::MotorControlDataPtr data,
+ FunctionalDevicePtr hand,
+ armarx::ControlTargetBase::ControlDeviceAccessToken token);
armarx::ControlTargetBase* getControlTarget() override;
void rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration) override;
void rtPreActivateController() override;
void rtPostDeactivateController() override;
+
private:
- bus_data::MotorControlDataPtr data;
- FunctionalDevicePtr hand;
+ bus_data::MotorControlDataPtr data;
+ FunctionalDevicePtr hand;
armarx::ControlTarget1DoFActuatorVelocityWithPWMLimit target;
- controller::VelocityToPWM ctrl;
- std::int16_t last_pwm;
+ controller::VelocityToPWM ctrl;
+ std::int16_t last_pwm;
};
- using VelocityControllerPtr =
- std::shared_ptr<VelocityController>;
-}
+
+ using VelocityControllerPtr = std::shared_ptr<VelocityController>;
+} // namespace devices::ethercat::hand::soft_sensorized_finger::joint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/CloseUntilContact.cpp b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/CloseUntilContact.cpp
index bbbbb01a..ec90f717 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/CloseUntilContact.cpp
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/CloseUntilContact.cpp
@@ -20,15 +20,16 @@
* GNU General Public License
*/
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-
#include "CloseUntilContact.h"
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+
//bouler
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
armarx::NJointControllerRegistration<KITSensorizedSoftFingerHandV1NJointController>
- registrationControllerKITSensorizedSoftFingerHandV1NJointControllers("KITSensorizedSoftFingerHandV1NJointController");
+ registrationControllerKITSensorizedSoftFingerHandV1NJointControllers(
+ "KITSensorizedSoftFingerHandV1NJointController");
KITSensorizedSoftFingerHandV1NJointController::KITSensorizedSoftFingerHandV1NJointController(
armarx::RobotUnitPtr robotUnit,
@@ -78,9 +79,12 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
params_index.d = config->index_d;
params_other.d = config->other_d;
- params_thumb.holding_start_force_threshold = config->thumb_holding_start_force_threshold;
- params_index.holding_start_force_threshold = config->index_holding_start_force_threshold;
- params_other.holding_start_force_threshold = config->other_holding_start_force_threshold;
+ params_thumb.holding_start_force_threshold =
+ config->thumb_holding_start_force_threshold;
+ params_index.holding_start_force_threshold =
+ config->index_holding_start_force_threshold;
+ params_other.holding_start_force_threshold =
+ config->other_holding_start_force_threshold;
params_thumb.holding_stop_force_threshold = config->thumb_holding_stop_force_threshold;
params_index.holding_stop_force_threshold = config->index_holding_stop_force_threshold;
@@ -92,9 +96,12 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
}
//get ctrl
{
- armarx::ControlTargetBase* ct_thumb = useControlTarget(config->hand_name + ".motor.thumb", armarx::ControlModes::PWM1DoF);
- armarx::ControlTargetBase* ct_index = useControlTarget(config->hand_name + ".motor.index", armarx::ControlModes::PWM1DoF);
- armarx::ControlTargetBase* ct_other = useControlTarget(config->hand_name + ".motor.other", armarx::ControlModes::PWM1DoF);
+ armarx::ControlTargetBase* ct_thumb =
+ useControlTarget(config->hand_name + ".motor.thumb", armarx::ControlModes::PWM1DoF);
+ armarx::ControlTargetBase* ct_index =
+ useControlTarget(config->hand_name + ".motor.index", armarx::ControlModes::PWM1DoF);
+ armarx::ControlTargetBase* ct_other =
+ useControlTarget(config->hand_name + ".motor.other", armarx::ControlModes::PWM1DoF);
ARMARX_CHECK_EXPRESSION(ct_thumb->isA<armarx::ControlTarget1DoFActuatorPWM>());
ARMARX_CHECK_EXPRESSION(ct_index->isA<armarx::ControlTarget1DoFActuatorPWM>());
ARMARX_CHECK_EXPRESSION(ct_other->isA<armarx::ControlTarget1DoFActuatorPWM>());
@@ -107,17 +114,25 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
_sensors = Sensors(config->hand_name, *this);
}
- std::string KITSensorizedSoftFingerHandV1NJointController::getClassName(const Ice::Current&) const
+ std::string
+ KITSensorizedSoftFingerHandV1NJointController::getClassName(const Ice::Current&) const
{
return "KITSensorizedSoftFingerHandV1NJointController";
}
- armarx::WidgetDescription::StringWidgetDictionary KITSensorizedSoftFingerHandV1NJointController::getFunctionDescriptions(const Ice::Current&) const
+ armarx::WidgetDescription::StringWidgetDictionary
+ KITSensorizedSoftFingerHandV1NJointController::getFunctionDescriptions(
+ const Ice::Current&) const
{
using namespace armarx::WidgetDescription;
VBoxLayoutPtr set_params = new VBoxLayout;
{
- const auto line = [&](const std::string & name, auto & start, auto & stop, auto & vel, auto & pwm, auto & mode)
+ const auto line = [&](const std::string& name,
+ auto& start,
+ auto& stop,
+ auto& vel,
+ auto& pwm,
+ auto& mode)
{
HBoxLayoutPtr line = new HBoxLayout;
set_params->children.emplace_back(line);
@@ -170,29 +185,70 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
line->children.emplace_back(e);
}
};
- line("thumb", _force_thresh_hold_start_thumb, _force_thresh_hold_stop_thumb, _finger_vel_thumb_trg, _max_pwm_thumb, _holding_mode_thumb);
- line("index", _force_thresh_hold_start_index, _force_thresh_hold_stop_index, _finger_vel_index_trg, _max_pwm_index, _holding_mode_index);
- line("other", _force_thresh_hold_start_other, _force_thresh_hold_stop_other, _finger_vel_other_trg, _max_pwm_other, _holding_mode_other);
+ line("thumb",
+ _force_thresh_hold_start_thumb,
+ _force_thresh_hold_stop_thumb,
+ _finger_vel_thumb_trg,
+ _max_pwm_thumb,
+ _holding_mode_thumb);
+ line("index",
+ _force_thresh_hold_start_index,
+ _force_thresh_hold_stop_index,
+ _finger_vel_index_trg,
+ _max_pwm_index,
+ _holding_mode_index);
+ line("other",
+ _force_thresh_hold_start_other,
+ _force_thresh_hold_stop_other,
+ _finger_vel_other_trg,
+ _max_pwm_other,
+ _holding_mode_other);
}
- return {{"SetParams", set_params}, {"Open", nullptr}, {"Close", nullptr}, {"Relax", nullptr}};
+ return {
+ {"SetParams", set_params}, {"Open", nullptr}, {"Close", nullptr}, {"Relax", nullptr}};
}
- void KITSensorizedSoftFingerHandV1NJointController::callDescribedFunction(const std::string& name, const armarx::StringVariantBaseMap& valueMap, const Ice::Current&)
+ void
+ KITSensorizedSoftFingerHandV1NJointController::callDescribedFunction(
+ const std::string& name,
+ const armarx::StringVariantBaseMap& valueMap,
+ const Ice::Current&)
{
if (name == "SetParams")
{
- const auto read = [&](const std::string & name, auto & start, auto & stop, auto & vel, auto & pwm, auto & mode)
+ const auto read = [&](const std::string& name,
+ auto& start,
+ auto& stop,
+ auto& vel,
+ auto& pwm,
+ auto& mode)
{
start = valueMap.at(name + "start")->getFloat();
stop = valueMap.at(name + "stop")->getFloat();
vel = valueMap.at(name + "vel")->getFloat();
pwm = valueMap.at(name + "pwm")->getInt();
- mode = (valueMap.at(name + "hold") ->getString() == "vel") ? CAHCtrl::HoldingMode::velocity : CAHCtrl::HoldingMode::pwm;
+ mode = (valueMap.at(name + "hold")->getString() == "vel")
+ ? CAHCtrl::HoldingMode::velocity
+ : CAHCtrl::HoldingMode::pwm;
};
- read("thumb", _force_thresh_hold_start_thumb, _force_thresh_hold_stop_thumb, _finger_vel_thumb_trg, _max_pwm_thumb, _holding_mode_thumb);
- read("index", _force_thresh_hold_start_index, _force_thresh_hold_stop_index, _finger_vel_index_trg, _max_pwm_index, _holding_mode_index);
- read("other", _force_thresh_hold_start_other, _force_thresh_hold_stop_other, _finger_vel_other_trg, _max_pwm_other, _holding_mode_other);
-
+ read("thumb",
+ _force_thresh_hold_start_thumb,
+ _force_thresh_hold_stop_thumb,
+ _finger_vel_thumb_trg,
+ _max_pwm_thumb,
+ _holding_mode_thumb);
+ read("index",
+ _force_thresh_hold_start_index,
+ _force_thresh_hold_stop_index,
+ _finger_vel_index_trg,
+ _max_pwm_index,
+ _holding_mode_index);
+ read("other",
+ _force_thresh_hold_start_other,
+ _force_thresh_hold_stop_other,
+ _finger_vel_other_trg,
+ _max_pwm_other,
+ _holding_mode_other);
}
else if (name == "Open")
{
@@ -237,7 +293,7 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
e->defaultValue = "RightHand";
line->children.emplace_back(e);
}
- const auto line = [&](const std::string & name)
+ const auto line = [&](const std::string& name)
{
HBoxLayoutPtr line = new HBoxLayout;
set_params->children.emplace_back(line);
@@ -324,47 +380,72 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
}
KITSensorizedSoftFingerHandV1NJointControllerConfigPtr
- KITSensorizedSoftFingerHandV1NJointController::GenerateConfigFromVariants(const armarx::StringVariantBaseMap& valueMap)
+ KITSensorizedSoftFingerHandV1NJointController::GenerateConfigFromVariants(
+ const armarx::StringVariantBaseMap& valueMap)
{
- KITSensorizedSoftFingerHandV1NJointControllerConfigPtr cfg = new KITSensorizedSoftFingerHandV1NJointControllerConfig;
+ KITSensorizedSoftFingerHandV1NJointControllerConfigPtr cfg =
+ new KITSensorizedSoftFingerHandV1NJointControllerConfig;
cfg->hand_name = valueMap.at("hand")->getString();
- const auto read = [&](const std::string & name, auto & p, auto & i, auto & d, auto & start, auto & stop, auto & pwm, auto & vel, auto & mode)
+ const auto read = [&](const std::string& name,
+ auto& p,
+ auto& i,
+ auto& d,
+ auto& start,
+ auto& stop,
+ auto& pwm,
+ auto& vel,
+ auto& mode)
{
- p = valueMap.at(name + "p") ->getFloat();
- i = valueMap.at(name + "i") ->getFloat();
- d = valueMap.at(name + "d") ->getFloat();
+ p = valueMap.at(name + "p")->getFloat();
+ i = valueMap.at(name + "i")->getFloat();
+ d = valueMap.at(name + "d")->getFloat();
start = valueMap.at(name + "start")->getFloat();
- stop = valueMap.at(name + "stop") ->getFloat();
- vel = valueMap.at(name + "vel") ->getFloat();
- pwm = valueMap.at(name + "pwm") ->getInt();
- mode = (valueMap.at(name + "hold") ->getString() == "vel") ? CAHCtrl::HoldingMode::velocity : CAHCtrl::HoldingMode::pwm;
+ stop = valueMap.at(name + "stop")->getFloat();
+ vel = valueMap.at(name + "vel")->getFloat();
+ pwm = valueMap.at(name + "pwm")->getInt();
+ mode = (valueMap.at(name + "hold")->getString() == "vel")
+ ? CAHCtrl::HoldingMode::velocity
+ : CAHCtrl::HoldingMode::pwm;
};
- read("thumb", cfg->thumb_p, cfg->thumb_i, cfg->thumb_d,
+ read("thumb",
+ cfg->thumb_p,
+ cfg->thumb_i,
+ cfg->thumb_d,
cfg->thumb_holding_start_force_threshold,
cfg->thumb_holding_stop_force_threshold,
- cfg->thumb_max_pwm, cfg->thumb_velocity,
+ cfg->thumb_max_pwm,
+ cfg->thumb_velocity,
cfg->thumb_holding_mode);
- read("index", cfg->index_p, cfg->index_i, cfg->index_d,
+ read("index",
+ cfg->index_p,
+ cfg->index_i,
+ cfg->index_d,
cfg->index_holding_start_force_threshold,
cfg->index_holding_stop_force_threshold,
- cfg->index_max_pwm, cfg->index_velocity,
+ cfg->index_max_pwm,
+ cfg->index_velocity,
cfg->index_holding_mode);
- read("other", cfg->other_p, cfg->other_i, cfg->other_d,
+ read("other",
+ cfg->other_p,
+ cfg->other_i,
+ cfg->other_d,
cfg->other_holding_start_force_threshold,
cfg->other_holding_stop_force_threshold,
- cfg->other_max_pwm, cfg->other_velocity,
+ cfg->other_max_pwm,
+ cfg->other_velocity,
cfg->other_holding_mode);
return cfg;
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
- void KITSensorizedSoftFingerHandV1NJointController::rtRun(
+ void
+ KITSensorizedSoftFingerHandV1NJointController::rtRun(
const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
{
@@ -386,17 +467,24 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
_ctrl_index.cfg.holding_stop_force_threshold = _force_thresh_hold_stop_index.load();
_ctrl_other.cfg.holding_stop_force_threshold = _force_thresh_hold_stop_other.load();
- _target_pwm_thumb->pwm = _ctrl_thumb.calculate(thumb_force, thumb_pos, _finger_vel_thumb, dt, _max_pwm_thumb);
- _target_pwm_index->pwm = _ctrl_index.calculate(index_force, index_pos, _finger_vel_index, dt, _max_pwm_index);
- _target_pwm_other->pwm = _ctrl_other.calculate(other_force, other_pos, _finger_vel_other, dt, _max_pwm_other);
+ _target_pwm_thumb->pwm =
+ _ctrl_thumb.calculate(thumb_force, thumb_pos, _finger_vel_thumb, dt, _max_pwm_thumb);
+ _target_pwm_index->pwm =
+ _ctrl_index.calculate(index_force, index_pos, _finger_vel_index, dt, _max_pwm_index);
+ _target_pwm_other->pwm =
+ _ctrl_other.calculate(other_force, other_pos, _finger_vel_other, dt, _max_pwm_other);
}
- void KITSensorizedSoftFingerHandV1NJointController::rtPreActivateController()
+ void
+ KITSensorizedSoftFingerHandV1NJointController::rtPreActivateController()
{
_ctrl_thumb.reset(_sensors.motor.thumb()->position);
_ctrl_index.reset(_sensors.motor.index()->position);
_ctrl_other.reset(_sensors.motor.other()->position);
}
- void KITSensorizedSoftFingerHandV1NJointController::rtPostDeactivateController()
- {}
-}
+
+ void
+ KITSensorizedSoftFingerHandV1NJointController::rtPostDeactivateController()
+ {
+ }
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/CloseUntilContact.h b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/CloseUntilContact.h
index 307c7599..0b58ade6 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/CloseUntilContact.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/CloseUntilContact.h
@@ -31,38 +31,40 @@
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
TYPEDEF_PTRS_HANDLE(KITSensorizedSoftFingerHandV1NJointControllerConfig);
- class KITSensorizedSoftFingerHandV1NJointControllerConfig : public armarx::NJointControllerConfig
+
+ class KITSensorizedSoftFingerHandV1NJointControllerConfig :
+ public armarx::NJointControllerConfig
{
public:
using CAHCtrl = Controllers::CloseAndHold;
- std::string hand_name;
+ std::string hand_name;
- float thumb_p = 0;
- float thumb_i = 0;
- float thumb_d = 0;
- float thumb_holding_start_force_threshold = 0;
- float thumb_holding_stop_force_threshold = 0;
- std::int16_t thumb_max_pwm = 512;
- float thumb_velocity = 0;
+ float thumb_p = 0;
+ float thumb_i = 0;
+ float thumb_d = 0;
+ float thumb_holding_start_force_threshold = 0;
+ float thumb_holding_stop_force_threshold = 0;
+ std::int16_t thumb_max_pwm = 512;
+ float thumb_velocity = 0;
CAHCtrl::HoldingMode thumb_holding_mode = CAHCtrl::HoldingMode::velocity;
- float index_p = 0;
- float index_i = 0;
- float index_d = 0;
- float index_holding_start_force_threshold = 0;
- float index_holding_stop_force_threshold = 0;
- std::int16_t index_max_pwm = 512;
- float index_velocity = 0;
+ float index_p = 0;
+ float index_i = 0;
+ float index_d = 0;
+ float index_holding_start_force_threshold = 0;
+ float index_holding_stop_force_threshold = 0;
+ std::int16_t index_max_pwm = 512;
+ float index_velocity = 0;
CAHCtrl::HoldingMode index_holding_mode = CAHCtrl::HoldingMode::velocity;
- float other_p = 0;
- float other_i = 0;
- float other_d = 0;
- float other_holding_start_force_threshold = 0;
- float other_holding_stop_force_threshold = 0;
- std::int16_t other_max_pwm = 512;
- float other_velocity = 0;
+ float other_p = 0;
+ float other_i = 0;
+ float other_d = 0;
+ float other_holding_start_force_threshold = 0;
+ float other_holding_stop_force_threshold = 0;
+ std::int16_t other_max_pwm = 512;
+ float other_velocity = 0;
CAHCtrl::HoldingMode other_holding_mode = CAHCtrl::HoldingMode::velocity;
};
@@ -77,8 +79,7 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
*
* Detailed description of class KITSensorizedSoftFingerHandV1NJointController.
*/
- class KITSensorizedSoftFingerHandV1NJointController :
- public armarx::NJointController
+ class KITSensorizedSoftFingerHandV1NJointController : public armarx::NJointController
{
public:
using ConfigPtrT = KITSensorizedSoftFingerHandV1NJointControllerConfigPtr;
@@ -88,56 +89,61 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
const VirtualRobot::RobotPtr&);
std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override;
- armarx::WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
- void callDescribedFunction(const std::string& name, const armarx::StringVariantBaseMap& valueMap, const Ice::Current& = Ice::emptyCurrent) override;
+ armarx::WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
+ void callDescribedFunction(const std::string& name,
+ const armarx::StringVariantBaseMap& valueMap,
+ const Ice::Current& = Ice::emptyCurrent) override;
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
- static armarx::WidgetDescription::WidgetPtr GenerateConfigDescription(
- const VirtualRobot::RobotPtr&,
- const std::map<std::string, armarx::ConstControlDevicePtr>&,
- const std::map<std::string, armarx::ConstSensorDevicePtr>&);
+ static armarx::WidgetDescription::WidgetPtr
+ GenerateConfigDescription(const VirtualRobot::RobotPtr&,
+ const std::map<std::string, armarx::ConstControlDevicePtr>&,
+ const std::map<std::string, armarx::ConstSensorDevicePtr>&);
static KITSensorizedSoftFingerHandV1NJointControllerConfigPtr
GenerateConfigFromVariants(const armarx::StringVariantBaseMap& values);
+
protected:
void rtPreActivateController() override;
void rtPostDeactivateController() override;
using CAHCtrl = Controllers::CloseAndHold;
- std::atomic<float> _force_thresh_hold_start_thumb{0};
- std::atomic<float> _force_thresh_hold_start_index{0};
- std::atomic<float> _force_thresh_hold_start_other{0};
+ std::atomic<float> _force_thresh_hold_start_thumb{0};
+ std::atomic<float> _force_thresh_hold_start_index{0};
+ std::atomic<float> _force_thresh_hold_start_other{0};
- std::atomic<float> _force_thresh_hold_stop_thumb{0};
- std::atomic<float> _force_thresh_hold_stop_index{0};
- std::atomic<float> _force_thresh_hold_stop_other{0};
+ std::atomic<float> _force_thresh_hold_stop_thumb{0};
+ std::atomic<float> _force_thresh_hold_stop_index{0};
+ std::atomic<float> _force_thresh_hold_stop_other{0};
- float _finger_vel_thumb_trg{0};
- float _finger_vel_index_trg{0};
- float _finger_vel_other_trg{0};
+ float _finger_vel_thumb_trg{0};
+ float _finger_vel_index_trg{0};
+ float _finger_vel_other_trg{0};
- std::atomic<float> _finger_vel_thumb{0};
- std::atomic<float> _finger_vel_index{0};
- std::atomic<float> _finger_vel_other{0};
+ std::atomic<float> _finger_vel_thumb{0};
+ std::atomic<float> _finger_vel_index{0};
+ std::atomic<float> _finger_vel_other{0};
- std::atomic_int16_t _max_pwm_thumb{0};
- std::atomic_int16_t _max_pwm_index{0};
- std::atomic_int16_t _max_pwm_other{0};
+ std::atomic_int16_t _max_pwm_thumb{0};
+ std::atomic_int16_t _max_pwm_index{0};
+ std::atomic_int16_t _max_pwm_other{0};
- std::atomic<CAHCtrl::HoldingMode> _holding_mode_thumb{CAHCtrl::HoldingMode::velocity};
- std::atomic<CAHCtrl::HoldingMode> _holding_mode_index{CAHCtrl::HoldingMode::velocity};
- std::atomic<CAHCtrl::HoldingMode> _holding_mode_other{CAHCtrl::HoldingMode::velocity};
+ std::atomic<CAHCtrl::HoldingMode> _holding_mode_thumb{CAHCtrl::HoldingMode::velocity};
+ std::atomic<CAHCtrl::HoldingMode> _holding_mode_index{CAHCtrl::HoldingMode::velocity};
+ std::atomic<CAHCtrl::HoldingMode> _holding_mode_other{CAHCtrl::HoldingMode::velocity};
- CAHCtrl _ctrl_thumb;
- CAHCtrl _ctrl_index;
- CAHCtrl _ctrl_other;
+ CAHCtrl _ctrl_thumb;
+ CAHCtrl _ctrl_index;
+ CAHCtrl _ctrl_other;
- armarx::ControlTarget1DoFActuatorPWM* _target_pwm_thumb = nullptr;
- armarx::ControlTarget1DoFActuatorPWM* _target_pwm_index = nullptr;
- armarx::ControlTarget1DoFActuatorPWM* _target_pwm_other = nullptr;
+ armarx::ControlTarget1DoFActuatorPWM* _target_pwm_thumb = nullptr;
+ armarx::ControlTarget1DoFActuatorPWM* _target_pwm_index = nullptr;
+ armarx::ControlTarget1DoFActuatorPWM* _target_pwm_other = nullptr;
Sensors _sensors;
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/MinimalGraspingForceV1.cpp b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/MinimalGraspingForceV1.cpp
index 7062b1fb..6e84a1da 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/MinimalGraspingForceV1.cpp
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/MinimalGraspingForceV1.cpp
@@ -1,15 +1,18 @@
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include "MinimalGraspingForceV1.h"
+
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
-#include <devices/ethercat/hand/soft_sensorized_finger/utility/SensorFit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include "MinimalGraspingForceV1.h"
+#include <devices/ethercat/hand/soft_sensorized_finger/utility/SensorFit.h>
//register
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
armarx::NJointControllerRegistration<MinimalGraspingForceV1_NJointController>
- registrationControllerMinimalGraspingForceV1_NJointController("KITSensorizedSoftFingerHandV1_GraspPhaseControllerV1_NJointController");
+ registrationControllerMinimalGraspingForceV1_NJointController(
+ "KITSensorizedSoftFingerHandV1_GraspPhaseControllerV1_NJointController");
}
+
//create
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
@@ -19,7 +22,8 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
const std::map<std::string, armarx::ConstControlDevicePtr>&,
const std::map<std::string, armarx::ConstSensorDevicePtr>&)
{
- using T = KITSensorizedSoftFingerHand::V1::NJointMinimalGraspingForceV1ControllerConfigElements::MotorCfg;
+ using T = KITSensorizedSoftFingerHand::V1::
+ NJointMinimalGraspingForceV1ControllerConfigElements::MotorCfg;
NJointMinimalGraspingForceV1ControllerConfig c;
make_default_cfg(c);
ARMARX_TRACE;
@@ -45,25 +49,25 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
FloatSpinBoxPtr e = new FloatSpinBox;
e->name = "place_fft_energy_threshold";
e->min = -2'000'000;
- e->max = 2'000'000;
+ e->max = 2'000'000;
e->defaultValue = c.place_fft_energy_threshold;
e->steps = 4'001;
line->children.emplace_back(e);
}
- const auto add_table_line_f = [&](const std::string & suff, float o, float i, float t)
+ const auto add_table_line_f = [&](const std::string& suff, float o, float i, float t)
{
HBoxLayoutPtr line = new HBoxLayout;
set_params->children.emplace_back(line);
line->children.emplace_back(new Label(false, suff));
- const auto fbox = [&line](const std::string & name, float val)
+ const auto fbox = [&line](const std::string& name, float val)
{
FloatSpinBoxPtr e = new FloatSpinBox;
e->name = name;
e->min = -5'000'000;
e->defaultValue = val;
- e->max = 5'000'000;
- e->steps = e->max - e->min + 1;
+ e->max = 5'000'000;
+ e->steps = e->max - e->min + 1;
e->decimals = 8;
line->children.emplace_back(e);
};
@@ -71,15 +75,16 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
fbox("motor_index." + suff, i);
fbox("motor_thumb." + suff, t);
};
- const auto add_table_line = [&](const std::string & suff, auto m)
- {
- add_table_line_f(suff, c.motor_other.*m, c.motor_index.*m, c.motor_thumb.*m);
- };
- const auto add_pid_table_line = [&](const std::string & pre, auto m)
+ const auto add_table_line = [&](const std::string& suff, auto m)
+ { add_table_line_f(suff, c.motor_other.*m, c.motor_index.*m, c.motor_thumb.*m); };
+ const auto add_pid_table_line = [&](const std::string& pre, auto m)
{
- add_table_line_f(pre + ".p", (c.motor_other.*m).p, (c.motor_index.*m).p, (c.motor_thumb.*m).p);
- add_table_line_f(pre + ".i", (c.motor_other.*m).i, (c.motor_index.*m).i, (c.motor_thumb.*m).i);
- add_table_line_f(pre + ".d", (c.motor_other.*m).d, (c.motor_index.*m).d, (c.motor_thumb.*m).d);
+ add_table_line_f(
+ pre + ".p", (c.motor_other.*m).p, (c.motor_index.*m).p, (c.motor_thumb.*m).p);
+ add_table_line_f(
+ pre + ".i", (c.motor_other.*m).i, (c.motor_index.*m).i, (c.motor_thumb.*m).i);
+ add_table_line_f(
+ pre + ".d", (c.motor_other.*m).d, (c.motor_index.*m).d, (c.motor_thumb.*m).d);
};
add_pid_table_line("pid_pos", &T::pid_pos);
@@ -95,60 +100,67 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
set_params->children.emplace_back(new HLine);
add_pid_table_line("pid_force", &T::pid_force);
- add_table_line("pid_force_limit", &T::pid_force_limit);
- add_table_line("pid_force_forward_p", &T::pid_force_forward_p);
+ add_table_line("pid_force_limit", &T::pid_force_limit);
+ add_table_line("pid_force_forward_p", &T::pid_force_forward_p);
set_params->children.emplace_back(new HLine);
- add_table_line("max_pwm", &T::max_pwm);
- add_table_line("pwm_open", &T::pwm_open);
- add_table_line("pwm_freely_moving", &T::pwm_freely_moving);
- add_table_line("min_pwm_close_to_contact", &T::min_pwm_close_to_contact);
- add_table_line("target_pressure", &T::target_pressure);
- add_table_line("target_shear", &T::target_shear);
- add_table_line("force_ctrl_pressure_factor", &T::force_ctrl_pressure_factor);
+ add_table_line("max_pwm", &T::max_pwm);
+ add_table_line("pwm_open", &T::pwm_open);
+ add_table_line("pwm_freely_moving", &T::pwm_freely_moving);
+ add_table_line("min_pwm_close_to_contact", &T::min_pwm_close_to_contact);
+ add_table_line("target_pressure", &T::target_pressure);
+ add_table_line("target_shear", &T::target_shear);
+ add_table_line("force_ctrl_pressure_factor", &T::force_ctrl_pressure_factor);
return set_params;
}
+
typename MinimalGraspingForceV1_NJointController::ConfigPtrT
- MinimalGraspingForceV1_NJointController::GenerateConfigFromVariants(const StringVariantBaseMap& valueMap)
+ MinimalGraspingForceV1_NJointController::GenerateConfigFromVariants(
+ const StringVariantBaseMap& valueMap)
{
ARMARX_TRACE;
- KITSensorizedSoftFingerHand::V1::NJointMinimalGraspingForceV1ControllerConfigPtr cfg = new KITSensorizedSoftFingerHand::V1::NJointMinimalGraspingForceV1ControllerConfig;
+ KITSensorizedSoftFingerHand::V1::NJointMinimalGraspingForceV1ControllerConfigPtr cfg =
+ new KITSensorizedSoftFingerHand::V1::NJointMinimalGraspingForceV1ControllerConfig;
cfg->hand_name = valueMap.at("hand")->getString();
- cfg->place_fft_energy_threshold = valueMap.at("place_fft_energy_threshold")->getFloat();
+ cfg->place_fft_energy_threshold = valueMap.at("place_fft_energy_threshold")->getFloat();
- const auto read_motor_cfg = [&](const std::string & name, auto & targ)
+ const auto read_motor_cfg = [&](const std::string& name, auto& targ)
{
- targ.pid_pos.p = valueMap.at("motor_" + name + ".pid_pos.p")->getFloat();
- targ.pid_pos.i = valueMap.at("motor_" + name + ".pid_pos.i")->getFloat();
- targ.pid_pos.d = valueMap.at("motor_" + name + ".pid_pos.d")->getFloat();
-
- targ.pid_pressure.p = valueMap.at("motor_" + name + ".pid_pressure.p")->getFloat();
- targ.pid_pressure.i = valueMap.at("motor_" + name + ".pid_pressure.i")->getFloat();
- targ.pid_pressure.d = valueMap.at("motor_" + name + ".pid_pressure.d")->getFloat();
-
- targ.pid_shear.p = valueMap.at("motor_" + name + ".pid_shear.p")->getFloat();
- targ.pid_shear.i = valueMap.at("motor_" + name + ".pid_shear.i")->getFloat();
- targ.pid_shear.d = valueMap.at("motor_" + name + ".pid_shear.d")->getFloat();
-
- targ.pid_unload.p = valueMap.at("motor_" + name + ".pid_unload.p")->getFloat();
- targ.pid_unload.i = valueMap.at("motor_" + name + ".pid_unload.i")->getFloat();
- targ.pid_unload.d = valueMap.at("motor_" + name + ".pid_unload.d")->getFloat();
-
- targ.pid_force.p = valueMap.at("motor_" + name + ".pid_force.p")->getFloat();
- targ.pid_force.i = valueMap.at("motor_" + name + ".pid_force.i")->getFloat();
- targ.pid_force.d = valueMap.at("motor_" + name + ".pid_force.d")->getFloat();
- targ.pid_force_limit = valueMap.at("motor_" + name + ".pid_force_limit")->getFloat();
- targ.pid_force_forward_p = valueMap.at("motor_" + name + ".pid_force_forward_p")->getFloat();
-
- targ.max_pwm = valueMap.at("motor_" + name + ".max_pwm")->getFloat();
- targ.pwm_open = valueMap.at("motor_" + name + ".pwm_open")->getFloat();
- targ.pwm_freely_moving = valueMap.at("motor_" + name + ".pwm_freely_moving")->getFloat();
- targ.min_pwm_close_to_contact = valueMap.at("motor_" + name + ".min_pwm_close_to_contact")->getFloat();
- targ.target_pressure = valueMap.at("motor_" + name + ".target_pressure")->getFloat();
- targ.target_shear = valueMap.at("motor_" + name + ".target_shear")->getFloat();
-
- targ.force_ctrl_pressure_factor = valueMap.at("motor_" + name + ".force_ctrl_pressure_factor")->getFloat();
+ targ.pid_pos.p = valueMap.at("motor_" + name + ".pid_pos.p")->getFloat();
+ targ.pid_pos.i = valueMap.at("motor_" + name + ".pid_pos.i")->getFloat();
+ targ.pid_pos.d = valueMap.at("motor_" + name + ".pid_pos.d")->getFloat();
+
+ targ.pid_pressure.p = valueMap.at("motor_" + name + ".pid_pressure.p")->getFloat();
+ targ.pid_pressure.i = valueMap.at("motor_" + name + ".pid_pressure.i")->getFloat();
+ targ.pid_pressure.d = valueMap.at("motor_" + name + ".pid_pressure.d")->getFloat();
+
+ targ.pid_shear.p = valueMap.at("motor_" + name + ".pid_shear.p")->getFloat();
+ targ.pid_shear.i = valueMap.at("motor_" + name + ".pid_shear.i")->getFloat();
+ targ.pid_shear.d = valueMap.at("motor_" + name + ".pid_shear.d")->getFloat();
+
+ targ.pid_unload.p = valueMap.at("motor_" + name + ".pid_unload.p")->getFloat();
+ targ.pid_unload.i = valueMap.at("motor_" + name + ".pid_unload.i")->getFloat();
+ targ.pid_unload.d = valueMap.at("motor_" + name + ".pid_unload.d")->getFloat();
+
+ targ.pid_force.p = valueMap.at("motor_" + name + ".pid_force.p")->getFloat();
+ targ.pid_force.i = valueMap.at("motor_" + name + ".pid_force.i")->getFloat();
+ targ.pid_force.d = valueMap.at("motor_" + name + ".pid_force.d")->getFloat();
+ targ.pid_force_limit = valueMap.at("motor_" + name + ".pid_force_limit")->getFloat();
+ targ.pid_force_forward_p =
+ valueMap.at("motor_" + name + ".pid_force_forward_p")->getFloat();
+
+ targ.max_pwm = valueMap.at("motor_" + name + ".max_pwm")->getFloat();
+ targ.pwm_open = valueMap.at("motor_" + name + ".pwm_open")->getFloat();
+ targ.pwm_freely_moving =
+ valueMap.at("motor_" + name + ".pwm_freely_moving")->getFloat();
+ targ.min_pwm_close_to_contact =
+ valueMap.at("motor_" + name + ".min_pwm_close_to_contact")->getFloat();
+ targ.target_pressure = valueMap.at("motor_" + name + ".target_pressure")->getFloat();
+ targ.target_shear = valueMap.at("motor_" + name + ".target_shear")->getFloat();
+
+ targ.force_ctrl_pressure_factor =
+ valueMap.at("motor_" + name + ".force_ctrl_pressure_factor")->getFloat();
};
read_motor_cfg("index", cfg->motor_index);
@@ -158,10 +170,16 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
return cfg;
}
- MinimalGraspingForceV1_NJointController::MinimalGraspingForceV1_NJointController(RobotUnitPtr robotUnit,
- const ConfigPtrT& config,
- const VirtualRobot::RobotPtr&) :
- _grasp_phase_controller_data{_finger_sensors, [ & ]{ ARMARX_CHECK_NOT_NULL(config); return * config; }() }
+ MinimalGraspingForceV1_NJointController::MinimalGraspingForceV1_NJointController(
+ RobotUnitPtr robotUnit,
+ const ConfigPtrT& config,
+ const VirtualRobot::RobotPtr&) :
+ _grasp_phase_controller_data{_finger_sensors,
+ [&]
+ {
+ ARMARX_CHECK_NOT_NULL(config);
+ return *config;
+ }()}
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(config);
@@ -175,7 +193,7 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
}
ARMARX_INFO << "get ctrl";
{
- auto get = [this](auto & targ, auto name)
+ auto get = [this](auto& targ, auto name)
{
ARMARX_INFO << " get ctrl for " << name;
ControlTargetBase* ct = useControlTarget(name, ControlModes::PWM1DoF);
@@ -190,19 +208,23 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
ARMARX_INFO << "get sens";
_sensors = KITSensorizedSoftFingerHand::V1::Sensors(config->hand_name, *this);
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
+
//name
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
- std::string MinimalGraspingForceV1_NJointController::getClassName(const Ice::Current&) const
+ std::string
+ MinimalGraspingForceV1_NJointController::getClassName(const Ice::Current&) const
{
return "MinimalGraspingForceV1_NJointController";
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
+
//remote function calls
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
- WidgetDescription::StringWidgetDictionary MinimalGraspingForceV1_NJointController::getFunctionDescriptions(const Ice::Current&) const
+ WidgetDescription::StringWidgetDictionary
+ MinimalGraspingForceV1_NJointController::getFunctionDescriptions(const Ice::Current&) const
{
using namespace armarx::WidgetDescription;
const auto make_ctrl_seetings = [](auto default_val, auto min, auto max)
@@ -247,7 +269,7 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
const auto make_pid_seetings = []
{
VBoxLayoutPtr settings = new VBoxLayout;
- const auto add_line = [&](const std::string & k, float val)
+ const auto add_line = [&](const std::string& k, float val)
{
HBoxLayoutPtr line = new HBoxLayout;
settings->children.emplace_back(line);
@@ -290,25 +312,25 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
add_line("d", 0);
return settings;
};
- return
- {
- {"pos_control", make_ctrl_seetings(0, 0, 1)},
- {"pwm_control", make_ctrl_seetings(0, -1000, 1000)},
- {"pos_control_pid", make_pid_seetings()},
- {"pressure_control_pid", make_pid_seetings()},
- {"shear_control_pid", make_pid_seetings()},
- {"unload_control_pid", make_pid_seetings()},
- {"Open", nullptr},
- {"Close", nullptr},
- {"Recalibrate", nullptr},
- {"StartPlacing", nullptr}
- };
+ return {{"pos_control", make_ctrl_seetings(0, 0, 1)},
+ {"pwm_control", make_ctrl_seetings(0, -1000, 1000)},
+ {"pos_control_pid", make_pid_seetings()},
+ {"pressure_control_pid", make_pid_seetings()},
+ {"shear_control_pid", make_pid_seetings()},
+ {"unload_control_pid", make_pid_seetings()},
+ {"Open", nullptr},
+ {"Close", nullptr},
+ {"Recalibrate", nullptr},
+ {"StartPlacing", nullptr}};
}
- void MinimalGraspingForceV1_NJointController::callDescribedFunction(
- const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&)
+ void
+ MinimalGraspingForceV1_NJointController::callDescribedFunction(
+ const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&)
{
- const auto read_pid_settings = [&](auto & o, auto & i, auto & t)
+ const auto read_pid_settings = [&](auto& o, auto& i, auto& t)
{
o.p = valueMap.at("op")->getFloat();
i.p = valueMap.at("ip")->getFloat();
@@ -337,7 +359,6 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
valueMap.at("o")->getFloat(),
valueMap.at("i")->getFloat(),
valueMap.at("t")->getFloat());
-
}
else if (name == "pos_control_pid")
{
@@ -406,11 +427,13 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
_clicked_close = true;
}
+
void
MinimalGraspingForceV1_NJointController::recalibrate(const Ice::Current&)
{
_clicked_recalibrate = true;
}
+
NJointMinimalGraspingForceV1ControllerDebugData
MinimalGraspingForceV1_NJointController::getDebugData(const Ice::Current&)
{
@@ -422,118 +445,134 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
buf.motor_controller_data.index().grasp_phase,
buf.motor_controller_data.thumb().grasp_phase});
- r.updating_sensors = buf.updating_sensors ;
- r.resetting_controller = buf.resetting_controller ;
- r.run_controller = buf.run_controller ;
+ r.updating_sensors = buf.updating_sensors;
+ r.resetting_controller = buf.resetting_controller;
+ r.run_controller = buf.run_controller;
- r.last_fpga_dt = buf.last_fpga_dt ;
+ r.last_fpga_dt = buf.last_fpga_dt;
- r.active_shear_avg_len_xy = buf.active_shear_avg_len_xy ;
- r.active_shear_avg_len_z = buf.active_shear_avg_len_z ;
- r.active_shear_avg_ratio = buf.active_shear_avg_ratio ;
- r.active_shear_num = buf.active_shear_num ;
- r.used_shear_avg_ratio = buf.used_shear_avg_ratio ;
+ r.active_shear_avg_len_xy = buf.active_shear_avg_len_xy;
+ r.active_shear_avg_len_z = buf.active_shear_avg_len_z;
+ r.active_shear_avg_ratio = buf.active_shear_avg_ratio;
+ r.active_shear_num = buf.active_shear_num;
+ r.used_shear_avg_ratio = buf.used_shear_avg_ratio;
- r.open_signal_finger_count = buf.open_signal_finger_count ;
- r.open_signal = buf.open_signal ;
+ r.open_signal_finger_count = buf.open_signal_finger_count;
+ r.open_signal = buf.open_signal;
- const auto motor = [](auto & trg, const auto & src)
+ const auto motor = [](auto& trg, const auto& src)
{
trg.grasp_phase = src.grasp_phase;
- trg.contact_detection_flag = src.contact_detection_flag ;
-
- trg.shear_force_used_ratio = src.shear_force_used_ratio ;
- trg.shear_force_used_ratio_num = src.shear_force_used_ratio_num ;
- trg.maximum_pressure = src.maximum_pressure ;
- trg.pressure_on_start_unload = src.pressure_on_start_unload ;
-
- trg.target_shear = src.target_shear ;
- trg.target_pressure = src.target_pressure ;
-
- trg.contact_detection_flag = src.contact_detection_flag ;
- trg.finger_did_close = src.finger_did_close ;
-
- trg.monotonic_shear_pressure_control_last_pwm = src.monotonic_shear_pressure_control_last_pwm ;
- trg.monotonic_shear_pressure_control_last_pwm_pressure = src.monotonic_shear_pressure_control_last_pwm_pressure ;
- trg.monotonic_shear_pressure_control_last_pwm_shear = src.monotonic_shear_pressure_control_last_pwm_shear ;
- trg.monotonic_shear_pressure_control_last_delta_pressure = src.monotonic_shear_pressure_control_last_delta_pressure ;
- trg.monotonic_shear_pressure_control_last_delta_shear = src.monotonic_shear_pressure_control_last_delta_shear ;
-
- trg.target_pressure_rate_of_change = src.target_pressure_rate_of_change ;
- trg.pressure_rate_of_change = src.pressure_rate_of_change ;
- trg.unload_phase_controller_normal_force_via_pwm_last_pwm = src.unload_phase_controller_normal_force_via_pwm_last_pwm ;
+ trg.contact_detection_flag = src.contact_detection_flag;
+
+ trg.shear_force_used_ratio = src.shear_force_used_ratio;
+ trg.shear_force_used_ratio_num = src.shear_force_used_ratio_num;
+ trg.maximum_pressure = src.maximum_pressure;
+ trg.pressure_on_start_unload = src.pressure_on_start_unload;
+
+ trg.target_shear = src.target_shear;
+ trg.target_pressure = src.target_pressure;
+
+ trg.contact_detection_flag = src.contact_detection_flag;
+ trg.finger_did_close = src.finger_did_close;
+
+ trg.monotonic_shear_pressure_control_last_pwm =
+ src.monotonic_shear_pressure_control_last_pwm;
+ trg.monotonic_shear_pressure_control_last_pwm_pressure =
+ src.monotonic_shear_pressure_control_last_pwm_pressure;
+ trg.monotonic_shear_pressure_control_last_pwm_shear =
+ src.monotonic_shear_pressure_control_last_pwm_shear;
+ trg.monotonic_shear_pressure_control_last_delta_pressure =
+ src.monotonic_shear_pressure_control_last_delta_pressure;
+ trg.monotonic_shear_pressure_control_last_delta_shear =
+ src.monotonic_shear_pressure_control_last_delta_shear;
+
+ trg.target_pressure_rate_of_change = src.target_pressure_rate_of_change;
+ trg.pressure_rate_of_change = src.pressure_rate_of_change;
+ trg.unload_phase_controller_normal_force_via_pwm_last_pwm =
+ src.unload_phase_controller_normal_force_via_pwm_last_pwm;
};
motor(r.motor_other, buf.motor_controller_data.other());
motor(r.motor_index, buf.motor_controller_data.index());
motor(r.motor_thumb, buf.motor_controller_data.thumb());
- const auto finger = [](auto & trg, const auto & sens)
+ const auto finger = [](auto& trg, const auto& sens)
{
- const auto f_shear = [](auto & out, const auto & in)
+ const auto f_shear = [](auto& out, const auto& in)
{
out.len_xy = in.len_xy;
- out.len_z = in.len_z ;
-
- out.x_current = in.sensors.at(0).current ;
- out.x_continuous_mean = in.sensors.at(0).continuous_mean ;
- out.x_current_mean_free_scaled = in.sensors.at(0).current_mean_free_scaled ;
- out.x_current_continuous_mean_free_scaled = in.sensors.at(0).current_continuous_mean_free_scaled;
-
- out.y_current = in.sensors.at(1).current ;
- out.y_continuous_mean = in.sensors.at(1).continuous_mean ;
- out.y_current_mean_free_scaled = in.sensors.at(1).current_mean_free_scaled ;
- out.y_current_continuous_mean_free_scaled = in.sensors.at(1).current_continuous_mean_free_scaled;
-
- out.z_current = in.sensors.at(2).current ;
- out.z_continuous_mean = in.sensors.at(2).continuous_mean ;
- out.z_current_mean_free_scaled = in.sensors.at(2).current_mean_free_scaled ;
- out.z_current_continuous_mean_free_scaled = in.sensors.at(2).current_continuous_mean_free_scaled;
+ out.len_z = in.len_z;
+
+ out.x_current = in.sensors.at(0).current;
+ out.x_continuous_mean = in.sensors.at(0).continuous_mean;
+ out.x_current_mean_free_scaled = in.sensors.at(0).current_mean_free_scaled;
+ out.x_current_continuous_mean_free_scaled =
+ in.sensors.at(0).current_continuous_mean_free_scaled;
+
+ out.y_current = in.sensors.at(1).current;
+ out.y_continuous_mean = in.sensors.at(1).continuous_mean;
+ out.y_current_mean_free_scaled = in.sensors.at(1).current_mean_free_scaled;
+ out.y_current_continuous_mean_free_scaled =
+ in.sensors.at(1).current_continuous_mean_free_scaled;
+
+ out.z_current = in.sensors.at(2).current;
+ out.z_continuous_mean = in.sensors.at(2).continuous_mean;
+ out.z_current_mean_free_scaled = in.sensors.at(2).current_mean_free_scaled;
+ out.z_current_continuous_mean_free_scaled =
+ in.sensors.at(2).current_continuous_mean_free_scaled;
};
f_shear(trg.shear_force_dist_joint, sens.shear_force_dist_joint);
- f_shear(trg.shear_force_dist_tip, sens.shear_force_dist_tip);
+ f_shear(trg.shear_force_dist_tip, sens.shear_force_dist_tip);
- trg.accelerometer.shear_force_criterium = sens.accelerometer.shear_force_criterium ;
- trg.accelerometer.fft_contact_detect_energy = sens.accelerometer.fft_contact_detect_energy ;
+ trg.accelerometer.shear_force_criterium = sens.accelerometer.shear_force_criterium;
+ trg.accelerometer.fft_contact_detect_energy =
+ sens.accelerometer.fft_contact_detect_energy;
- const auto f_nf = [](auto & out, const auto & in)
+ const auto f_nf = [](auto& out, const auto& in)
{
- out.current = in.current ;
- out.continuous_mean = in.continuous_mean ;
- out.current_mean_free_scaled = in.current_mean_free_scaled ;
+ out.current = in.current;
+ out.continuous_mean = in.continuous_mean;
+ out.current_mean_free_scaled = in.current_mean_free_scaled;
out.current_continuous_mean_free_scaled = in.current_continuous_mean_free_scaled;
};
- f_nf(trg.nf_prox_l, sens.normal_force_sensors.at(0));
- f_nf(trg.nf_prox_r, sens.normal_force_sensors.at(1));
+ f_nf(trg.nf_prox_l, sens.normal_force_sensors.at(0));
+ f_nf(trg.nf_prox_r, sens.normal_force_sensors.at(1));
f_nf(trg.nf_dist_joint, sens.normal_force_sensors.at(2));
- f_nf(trg.nf_dist_tip, sens.normal_force_sensors.at(3));
+ f_nf(trg.nf_dist_tip, sens.normal_force_sensors.at(3));
};
finger(r.finger_little, buf.normalized_finger_sensors.little());
- finger(r.finger_ring, buf.normalized_finger_sensors.ring());
+ finger(r.finger_ring, buf.normalized_finger_sensors.ring());
finger(r.finger_middle, buf.normalized_finger_sensors.middle());
- finger(r.finger_index, buf.normalized_finger_sensors.index());
- finger(r.finger_thumb, buf.normalized_finger_sensors.thumb());
+ finger(r.finger_index, buf.normalized_finger_sensors.index());
+ finger(r.finger_thumb, buf.normalized_finger_sensors.thumb());
return r;
}
+
void
MinimalGraspingForceV1_NJointController::openMGF(const Ice::Current&)
{
_clicked_open = true;
}
+
void
MinimalGraspingForceV1_NJointController::startPlacing(const Ice::Current&)
{
_clicked_start_placing = true;
}
+
void
- MinimalGraspingForceV1_NJointController::overridePWM(
- bool active, Ice::Float o, Ice::Float i, Ice::Float t, const Ice::Current&)
+ MinimalGraspingForceV1_NJointController::overridePWM(bool active,
+ Ice::Float o,
+ Ice::Float i,
+ Ice::Float t,
+ const Ice::Current&)
{
std::lock_guard g{_ice_to_rt_buffer_write_mutex};
auto& buf = _ice_to_rt_buffer.getWriteBuffer();
buf.pwm_control.active = active;
- buf.pwm_control.o = o ;
- buf.pwm_control.i = i ;
- buf.pwm_control.t = t ;
+ buf.pwm_control.o = o;
+ buf.pwm_control.i = i;
+ buf.pwm_control.t = t;
_ice_to_rt_buffer.commitWrite();
}
@@ -546,22 +585,33 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
buf.motor_controller_data.index().grasp_phase,
buf.motor_controller_data.thumb().grasp_phase});
}
- void MinimalGraspingForceV1_NJointController::setConfig(const NJointMinimalGraspingForceV1ControllerConfigPtr& cfg, const Ice::Current&)
+
+ void
+ MinimalGraspingForceV1_NJointController::setConfig(
+ const NJointMinimalGraspingForceV1ControllerConfigPtr& cfg,
+ const Ice::Current&)
{
ARMARX_CHECK_NOT_NULL(cfg);
std::lock_guard g{_ice_to_rt_buffer_write_mutex};
_ice_to_rt_buffer.getWriteBuffer().cfg = *cfg;
_ice_to_rt_buffer.commitWrite();
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
+
//rt control
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
- void MinimalGraspingForceV1_NJointController::rtRun(
- const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration)
+ void
+ MinimalGraspingForceV1_NJointController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
- static const auto now_sec = []()->float {return std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now().time_since_epoch()).count() * 1e-9f;};
+ static const auto now_sec = []() -> float
+ {
+ return std::chrono::duration_cast<std::chrono::nanoseconds>(
+ std::chrono::steady_clock::now().time_since_epoch())
+ .count() *
+ 1e-9f;
+ };
ARMARX_TRACE;
auto& obuf = _rt_to_ice.getWriteBuffer();
@@ -599,21 +649,21 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
}
//configure motors
{
- const auto recfg_motor = [&](auto & m, auto & mcfg)
+ const auto recfg_motor = [&](auto& m, auto& mcfg)
{
- const auto set_pid = [&](auto & pid, const auto & params)
+ const auto set_pid = [&](auto& pid, const auto& params)
{
pid.Kp = params.p;
pid.Ki = params.i;
pid.Kd = params.d;
};
set_pid(m.pos_pwm_controller.PID, mcfg.pid_pos);
- set_pid(m.pid_shear, mcfg.pid_shear);
- set_pid(m.pid_unload, mcfg.pid_unload);
+ set_pid(m.pid_shear, mcfg.pid_shear);
+ set_pid(m.pid_unload, mcfg.pid_unload);
- set_pid(m.pid_force, mcfg.pid_force);
- m.pid_force_forward_p = mcfg.pid_force_forward_p;
- m.pid_force_limit = std::abs(mcfg.pid_force_limit);
+ set_pid(m.pid_force, mcfg.pid_force);
+ m.pid_force_forward_p = mcfg.pid_force_forward_p;
+ m.pid_force_limit = std::abs(mcfg.pid_force_limit);
m.force_ctrl_pressure_factor = mcfg.force_ctrl_pressure_factor;
m.controller_state.target_shear = mcfg.target_shear;
@@ -621,39 +671,46 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
m.max_pwm = mcfg.max_pwm;
m.encoder_delta_for_movent = mcfg.encoder_delta_for_movent;
};
- recfg_motor(_grasp_phase_controller_data.motor_controller_data.other(), ibuf.cfg.motor_other);
- recfg_motor(_grasp_phase_controller_data.motor_controller_data.index(), ibuf.cfg.motor_index);
- recfg_motor(_grasp_phase_controller_data.motor_controller_data.thumb(), ibuf.cfg.motor_thumb);
+ recfg_motor(_grasp_phase_controller_data.motor_controller_data.other(),
+ ibuf.cfg.motor_other);
+ recfg_motor(_grasp_phase_controller_data.motor_controller_data.index(),
+ ibuf.cfg.motor_index);
+ recfg_motor(_grasp_phase_controller_data.motor_controller_data.thumb(),
+ ibuf.cfg.motor_thumb);
}
// calc stuff
{
obuf.run_controller = _grasp_phase_controller_data.run_controller;
- obuf.updating_sensors = _sensors.hand->frame_counter > _grasp_phase_controller_data.fpga_iteration;
+ obuf.updating_sensors =
+ _sensors.hand->frame_counter > _grasp_phase_controller_data.fpga_iteration;
if (obuf.updating_sensors)
{
- _finger_sensors.little().update(_sensors.finger.little(), !_grasp_phase_controller_data.run_controller);
- _finger_sensors.ring() .update(_sensors.finger.ring(), !_grasp_phase_controller_data.run_controller);
- _finger_sensors.middle().update(_sensors.finger.middle(), !_grasp_phase_controller_data.run_controller);
- _finger_sensors.index() .update(_sensors.finger.index(), !_grasp_phase_controller_data.run_controller);
- _finger_sensors.thumb() .update(_sensors.finger.thumb(), !_grasp_phase_controller_data.run_controller);
+ _finger_sensors.little().update(_sensors.finger.little(),
+ !_grasp_phase_controller_data.run_controller);
+ _finger_sensors.ring().update(_sensors.finger.ring(),
+ !_grasp_phase_controller_data.run_controller);
+ _finger_sensors.middle().update(_sensors.finger.middle(),
+ !_grasp_phase_controller_data.run_controller);
+ _finger_sensors.index().update(_sensors.finger.index(),
+ !_grasp_phase_controller_data.run_controller);
+ _finger_sensors.thumb().update(_sensors.finger.thumb(),
+ !_grasp_phase_controller_data.run_controller);
_grasp_phase_controller_data.fpga_iteration = _sensors.hand->frame_counter;
- _grasp_phase_controller_data.last_fpga_dt = time - _grasp_phase_controller_data.time_last_fpga_update;
+ _grasp_phase_controller_data.last_fpga_dt =
+ time - _grasp_phase_controller_data.time_last_fpga_update;
obuf.last_fpga_dt = _grasp_phase_controller_data.last_fpga_dt;
_grasp_phase_controller_data.fpga_update = true;
_grasp_phase_controller_data.time_last_fpga_update = time;
}
}
- controller_run_data ctrl_run_data
- {
- _sensors,
- timeSinceLastIteration.toSecondsDouble(),
- _grasp_phase_controller_data.fpga_update,
- _grasp_phase_controller_data.last_fpga_dt
- };
+ controller_run_data ctrl_run_data{_sensors,
+ timeSinceLastIteration.toSecondsDouble(),
+ _grasp_phase_controller_data.fpga_update,
+ _grasp_phase_controller_data.last_fpga_dt};
{
// update criteria for controller state changes
for (auto& m : _grasp_phase_controller_data.motor_controller_data)
@@ -663,22 +720,23 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
//shear over hand
{
obuf.active_shear_avg_len_xy = 0;
- obuf.active_shear_avg_len_z = 0;
- obuf.active_shear_num = 0;
+ obuf.active_shear_avg_len_z = 0;
+ obuf.active_shear_num = 0;
for (const auto& f : _finger_sensors)
{
if (f.test_for_object_touched())
{
}
- if (f.normal_force_sensors.at(2).test_current_over_one_percent_plus_peak_noise())//dist joint
+ if (f.normal_force_sensors.at(2)
+ .test_current_over_one_percent_plus_peak_noise()) //dist joint
{
obuf.active_shear_avg_len_xy += f.shear_force_dist_joint.len_xy;
- obuf.active_shear_avg_len_z += f.shear_force_dist_joint.len_z;
+ obuf.active_shear_avg_len_z += f.shear_force_dist_joint.len_z;
++obuf.active_shear_num;
obuf.active_shear_avg_len_xy += f.shear_force_dist_tip.len_xy;
- obuf.active_shear_avg_len_z += f.shear_force_dist_tip.len_z;
+ obuf.active_shear_avg_len_z += f.shear_force_dist_tip.len_z;
++obuf.active_shear_num;
}
}
@@ -686,8 +744,9 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
if (obuf.active_shear_num)
{
obuf.active_shear_avg_len_xy /= obuf.active_shear_num;
- obuf.active_shear_avg_len_z /= obuf.active_shear_num;
- obuf.active_shear_avg_ratio = obuf.active_shear_avg_len_z / obuf.active_shear_avg_len_xy;
+ obuf.active_shear_avg_len_z /= obuf.active_shear_num;
+ obuf.active_shear_avg_ratio =
+ obuf.active_shear_avg_len_z / obuf.active_shear_avg_len_xy;
}
else
{
@@ -699,7 +758,8 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
if (std::isfinite(obuf.active_shear_avg_ratio))
{
- obuf.used_shear_avg_ratio = m.shear_avg_filter.update(obuf.active_shear_avg_ratio);
+ obuf.used_shear_avg_ratio =
+ m.shear_avg_filter.update(obuf.active_shear_avg_ratio);
}
else
{
@@ -715,12 +775,14 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
obuf.open_signal_finger_count = 0;
for (const auto& f : _finger_sensors)
{
- if (f.accelerometer.fft_contact_detect_energy >= ibuf.cfg.place_fft_energy_threshold)
+ if (f.accelerometer.fft_contact_detect_energy >=
+ ibuf.cfg.place_fft_energy_threshold)
{
++obuf.open_signal_finger_count;
}
}
- obuf.open_signal = (obuf.open_signal_finger_count >= static_cast<std::size_t>(ibuf.cfg.place_finger_trigger_count));
+ obuf.open_signal = (obuf.open_signal_finger_count >=
+ static_cast<std::size_t>(ibuf.cfg.place_finger_trigger_count));
}
}
@@ -728,13 +790,17 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
//execute controller
if (ibuf.pos_control.active)
{
- const auto ctrl = [&](auto & targ, auto & ctrl, auto ptarg)
- {
- targ->pwm = ctrl.pos_control_pwm(ctrl_run_data, ptarg);
- };
- ctrl(_target_pwm_other, _grasp_phase_controller_data.motor_controller_data.other(), ibuf.pos_control.o);
- ctrl(_target_pwm_index, _grasp_phase_controller_data.motor_controller_data.index(), ibuf.pos_control.i);
- ctrl(_target_pwm_thumb, _grasp_phase_controller_data.motor_controller_data.thumb(), ibuf.pos_control.t);
+ const auto ctrl = [&](auto& targ, auto& ctrl, auto ptarg)
+ { targ->pwm = ctrl.pos_control_pwm(ctrl_run_data, ptarg); };
+ ctrl(_target_pwm_other,
+ _grasp_phase_controller_data.motor_controller_data.other(),
+ ibuf.pos_control.o);
+ ctrl(_target_pwm_index,
+ _grasp_phase_controller_data.motor_controller_data.index(),
+ ibuf.pos_control.i);
+ ctrl(_target_pwm_thumb,
+ _grasp_phase_controller_data.motor_controller_data.thumb(),
+ ibuf.pos_control.t);
}
else if (ibuf.pwm_control.active)
{
@@ -761,59 +827,68 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
const float minimum_distance = motor_ctrl_data.minimum_distance(_sensors);
//close with maximum velocity, until the distance sensors sense something
- pwm_target = std::max(static_cast<float>(subcfg.min_pwm_close_to_contact),
- static_cast<float>(subcfg.pwm_freely_moving * minimum_distance));
- std::cout << static_cast<int>(motor_id) << " " << VAROUT(pwm_target) << "\n";
+ pwm_target = std::max(
+ static_cast<float>(subcfg.min_pwm_close_to_contact),
+ static_cast<float>(subcfg.pwm_freely_moving * minimum_distance));
+ std::cout << static_cast<int>(motor_id) << " " << VAROUT(pwm_target)
+ << "\n";
//check if we have reached the object
if (motor_ctrl_data.controller_state.contact_detection_flag
// || motor_ctrl_data.controller_state.finger_did_close
- )
+ )
{
- motor_ctrl_data.controller_state.grasp_phase = GraspPhase::BARRIER_PRE_LLH;
- motor_ctrl_data.position_of_initial_contact = _sensors.motor(motor_id)->position;
+ motor_ctrl_data.controller_state.grasp_phase =
+ GraspPhase::BARRIER_PRE_LLH;
+ motor_ctrl_data.position_of_initial_contact =
+ _sensors.motor(motor_id)->position;
}
}
break;
case GraspPhase::BARRIER_PRE_LLH:
{
//targ.at(i)->pwm = pwm_cont.at(i);
- pwm_target = motor_ctrl_data.pos_control_pwm(ctrl_run_data, motor_ctrl_data.position_of_initial_contact);
+ pwm_target = motor_ctrl_data.pos_control_pwm(
+ ctrl_run_data, motor_ctrl_data.position_of_initial_contact);
}
break;
case GraspPhase::LOAD_LIFT_HOLD:
{
pwm_target = motor_ctrl_data.monotonic_shear_pressure_control_pwm_force(
- ctrl_run_data,
- subcfg.target_pressure,
- subcfg.target_shear,
- (motor_ctrl_data.controller_state.shear_force_used_ratio_num != 0)
- );
+ ctrl_run_data,
+ subcfg.target_pressure,
+ subcfg.target_shear,
+ (motor_ctrl_data.controller_state.shear_force_used_ratio_num != 0));
if (obuf.open_signal && _clicked_start_placing)
{
motor_ctrl_data.controller_state.grasp_phase = GraspPhase::UNLOAD;
motor_ctrl_data.controller_state.pressure_on_start_unload =
motor_ctrl_data.controller_state.maximum_pressure;
motor_ctrl_data.controller_state.target_pressure_rate_of_change =
- static_cast<float>(motor_ctrl_data.controller_state.pressure_on_start_unload) * ctrl_run_data.dt_sec_fpga;
+ static_cast<float>(
+ motor_ctrl_data.controller_state.pressure_on_start_unload) *
+ ctrl_run_data.dt_sec_fpga;
}
}
break;
case GraspPhase::UNLOAD:
{
- pwm_target = motor_ctrl_data.unload_phase_controller_normal_force_via_pwm(ctrl_run_data);
+ pwm_target = motor_ctrl_data.unload_phase_controller_normal_force_via_pwm(
+ ctrl_run_data);
//check if we have lost contact to the object
- if (!motor_ctrl_data.controller_state.contact_detection_flag || (_sensors.motor(motor_id)->position < 0.15))
+ if (!motor_ctrl_data.controller_state.contact_detection_flag ||
+ (_sensors.motor(motor_id)->position < 0.15))
{
- motor_ctrl_data.controller_state.grasp_phase = GraspPhase::BARRIER_PRE_OPEN;
- motor_ctrl_data.position_of_unload_complete = motor_ctrl_data.last_encoder_pos;
+ motor_ctrl_data.controller_state.grasp_phase =
+ GraspPhase::BARRIER_PRE_OPEN;
+ motor_ctrl_data.position_of_unload_complete =
+ motor_ctrl_data.last_encoder_pos;
}
}
break;
case GraspPhase::BARRIER_PRE_OPEN:
{
pwm_target = motor_ctrl_data.pos_control_pwm(
- ctrl_run_data,
- motor_ctrl_data.position_of_unload_complete);
+ ctrl_run_data, motor_ctrl_data.position_of_unload_complete);
}
break;
case GraspPhase::OPEN:
@@ -827,20 +902,29 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
}
}
//transition from freely_closing to load via barrier
- if (_grasp_phase_controller_data.motor_controller_data.other().controller_state.grasp_phase == GraspPhase::BARRIER_PRE_LLH &&
- _grasp_phase_controller_data.motor_controller_data.index().controller_state.grasp_phase == GraspPhase::BARRIER_PRE_LLH &&
- _grasp_phase_controller_data.motor_controller_data.thumb().controller_state.grasp_phase == GraspPhase::BARRIER_PRE_LLH)
+ if (_grasp_phase_controller_data.motor_controller_data.other()
+ .controller_state.grasp_phase == GraspPhase::BARRIER_PRE_LLH &&
+ _grasp_phase_controller_data.motor_controller_data.index()
+ .controller_state.grasp_phase == GraspPhase::BARRIER_PRE_LLH &&
+ _grasp_phase_controller_data.motor_controller_data.thumb()
+ .controller_state.grasp_phase == GraspPhase::BARRIER_PRE_LLH)
{
- _grasp_phase_controller_data.motor_controller_data.other().controller_state.grasp_phase = GraspPhase::LOAD_LIFT_HOLD;
- _grasp_phase_controller_data.motor_controller_data.index().controller_state.grasp_phase = GraspPhase::LOAD_LIFT_HOLD;
- _grasp_phase_controller_data.motor_controller_data.thumb().controller_state.grasp_phase = GraspPhase::LOAD_LIFT_HOLD;
+ _grasp_phase_controller_data.motor_controller_data.other()
+ .controller_state.grasp_phase = GraspPhase::LOAD_LIFT_HOLD;
+ _grasp_phase_controller_data.motor_controller_data.index()
+ .controller_state.grasp_phase = GraspPhase::LOAD_LIFT_HOLD;
+ _grasp_phase_controller_data.motor_controller_data.thumb()
+ .controller_state.grasp_phase = GraspPhase::LOAD_LIFT_HOLD;
}
//transition from unload to freely opening via barrier
- if (_grasp_phase_controller_data.motor_controller_data.other().controller_state.grasp_phase == GraspPhase::BARRIER_PRE_OPEN &&
- _grasp_phase_controller_data.motor_controller_data.index().controller_state.grasp_phase == GraspPhase::BARRIER_PRE_OPEN &&
- _grasp_phase_controller_data.motor_controller_data.thumb().controller_state.grasp_phase == GraspPhase::BARRIER_PRE_OPEN)
+ if (_grasp_phase_controller_data.motor_controller_data.other()
+ .controller_state.grasp_phase == GraspPhase::BARRIER_PRE_OPEN &&
+ _grasp_phase_controller_data.motor_controller_data.index()
+ .controller_state.grasp_phase == GraspPhase::BARRIER_PRE_OPEN &&
+ _grasp_phase_controller_data.motor_controller_data.thumb()
+ .controller_state.grasp_phase == GraspPhase::BARRIER_PRE_OPEN)
{
- auto set = [&](auto & data)
+ auto set = [&](auto& data)
{
data.controller_state.grasp_phase = GraspPhase::OPEN;
data.position_of_unload_complete = data.last_encoder_pos;
@@ -854,7 +938,8 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
//write results
for (int i = 0; i < 3; i++)
{
- obuf.motor_controller_data.at(i) = _grasp_phase_controller_data.motor_controller_data.at(i).controller_state;
+ obuf.motor_controller_data.at(i) =
+ _grasp_phase_controller_data.motor_controller_data.at(i).controller_state;
}
obuf.normalized_finger_sensors = _finger_sensors;
@@ -862,38 +947,47 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
_rt_to_ice.commitWrite();
}
- void MinimalGraspingForceV1_NJointController::rtPreActivateController()
+ void
+ MinimalGraspingForceV1_NJointController::rtPreActivateController()
{
ARMARX_TRACE;
_clicked_open = false;
_clicked_close = false;
_clicked_stop = false;
- const auto make_negative = [](auto & v)
- {
- v = -std::abs(v);
- };
+ const auto make_negative = [](auto& v) { v = -std::abs(v); };
make_negative(_finger_sensors.index().shear_force_dist_joint.sensors.at(0).scaling);
make_negative(_finger_sensors.index().shear_force_dist_joint.sensors.at(1).scaling);
make_negative(_finger_sensors.index().shear_force_dist_joint.sensors.at(2).scaling);
}
- void MinimalGraspingForceV1_NJointController::rtPostDeactivateController()
+ void
+ MinimalGraspingForceV1_NJointController::rtPostDeactivateController()
{
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
+
//publish
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
- void MinimalGraspingForceV1_NJointController::onPublishActivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&)
+ void
+ MinimalGraspingForceV1_NJointController::onPublishActivation(const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx&)
{
//maybe do some setup when the controller starts running (for visu / logging)
}
- void MinimalGraspingForceV1_NJointController::onPublishDeactivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&)
+
+ void
+ MinimalGraspingForceV1_NJointController::onPublishDeactivation(const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx&)
{
//maybe do some cleanup when the controller starts running (for visu / logging)
}
- void MinimalGraspingForceV1_NJointController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& obs)
+
+ void
+ MinimalGraspingForceV1_NJointController::onPublish(const SensorAndControl&,
+ const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx& obs)
{
ARMARX_TRACE;
std::lock_guard g{_rt_to_ice_read_mutex};
@@ -904,23 +998,45 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
auto publish_controller = [&](const std::string name, int idx)
{
const auto& mcdata = buf.motor_controller_data.at(idx);
- oh.setDebugObserverDatafield("Contact Flag " + name, 1e-5f + mcdata.contact_detection_flag);
- oh.setDebugObserverDatafield("Closing Flag " + name, 1e-5f + mcdata.finger_did_close);
- oh.setDebugObserverDatafield("Controller Grasp Phase " + name, 1e-5f + static_cast<int>(mcdata.grasp_phase));
- oh.setDebugObserverDatafield("Controller current maximum normal force (phase load) " + name, 1e-5f + static_cast<int>(mcdata.maximum_pressure));
-
- oh.setDebugObserverDatafield("Controller current monotonic_shear_pressure_control_last_pwm " + name, 1e-5f + mcdata.monotonic_shear_pressure_control_last_pwm);
- oh.setDebugObserverDatafield("Controller current monotonic_shear_pressure_control_last_pwm_pressure " + name, 1e-5f + mcdata.monotonic_shear_pressure_control_last_pwm_pressure);
- oh.setDebugObserverDatafield("Controller current monotonic_shear_pressure_control_last_pwm_shear " + name, 1e-5f + mcdata.monotonic_shear_pressure_control_last_pwm_shear);
- oh.setDebugObserverDatafield("Controller current monotonic_shear_pressure_control_last_delta_pressure " + name, 1e-5f + mcdata.monotonic_shear_pressure_control_last_delta_pressure);
- oh.setDebugObserverDatafield("Controller current monotonic_shear_pressure_control_last_delta_shear " + name, 1e-5f + mcdata.monotonic_shear_pressure_control_last_delta_shear);
-
- oh.setDebugObserverDatafield("Controller current target_shear " + name, 1e-5f + mcdata.target_shear);
- oh.setDebugObserverDatafield("Controller current target_pressure " + name, 1e-5f + mcdata.target_pressure);
-
- oh.setDebugObserverDatafield("uload target_pressure_rate_of_change " + name, 1e-5f + mcdata.target_pressure_rate_of_change);
- oh.setDebugObserverDatafield("uload pressure_rate_of_change " + name, 1e-5f + mcdata.pressure_rate_of_change);
- oh.setDebugObserverDatafield("uload unload_phase_controller_normal_force_via_pwm_last_pwm " + name, 1e-5f + mcdata.unload_phase_controller_normal_force_via_pwm_last_pwm);
+ oh.setDebugObserverDatafield("Contact Flag " + name,
+ 1e-5f + mcdata.contact_detection_flag);
+ oh.setDebugObserverDatafield("Closing Flag " + name,
+ 1e-5f + mcdata.finger_did_close);
+ oh.setDebugObserverDatafield("Controller Grasp Phase " + name,
+ 1e-5f + static_cast<int>(mcdata.grasp_phase));
+ oh.setDebugObserverDatafield(
+ "Controller current maximum normal force (phase load) " + name,
+ 1e-5f + static_cast<int>(mcdata.maximum_pressure));
+
+ oh.setDebugObserverDatafield(
+ "Controller current monotonic_shear_pressure_control_last_pwm " + name,
+ 1e-5f + mcdata.monotonic_shear_pressure_control_last_pwm);
+ oh.setDebugObserverDatafield(
+ "Controller current monotonic_shear_pressure_control_last_pwm_pressure " + name,
+ 1e-5f + mcdata.monotonic_shear_pressure_control_last_pwm_pressure);
+ oh.setDebugObserverDatafield(
+ "Controller current monotonic_shear_pressure_control_last_pwm_shear " + name,
+ 1e-5f + mcdata.monotonic_shear_pressure_control_last_pwm_shear);
+ oh.setDebugObserverDatafield(
+ "Controller current monotonic_shear_pressure_control_last_delta_pressure " +
+ name,
+ 1e-5f + mcdata.monotonic_shear_pressure_control_last_delta_pressure);
+ oh.setDebugObserverDatafield(
+ "Controller current monotonic_shear_pressure_control_last_delta_shear " + name,
+ 1e-5f + mcdata.monotonic_shear_pressure_control_last_delta_shear);
+
+ oh.setDebugObserverDatafield("Controller current target_shear " + name,
+ 1e-5f + mcdata.target_shear);
+ oh.setDebugObserverDatafield("Controller current target_pressure " + name,
+ 1e-5f + mcdata.target_pressure);
+
+ oh.setDebugObserverDatafield("uload target_pressure_rate_of_change " + name,
+ 1e-5f + mcdata.target_pressure_rate_of_change);
+ oh.setDebugObserverDatafield("uload pressure_rate_of_change " + name,
+ 1e-5f + mcdata.pressure_rate_of_change);
+ oh.setDebugObserverDatafield(
+ "uload unload_phase_controller_normal_force_via_pwm_last_pwm " + name,
+ 1e-5f + mcdata.unload_phase_controller_normal_force_via_pwm_last_pwm);
};
publish_controller("other", 0);
publish_controller("index", 1);
@@ -935,14 +1051,12 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
//normal_force
{
- const auto mk_plot_nfor = [&](const std::string & pos)
+ const auto mk_plot_nfor = [&](const std::string& pos)
{
- return [&, pos](const std::string & valn, auto val)
+ return [&, pos](const std::string& valn, auto val)
{
- oh.setDebugObserverDatafield("Normal Force " +
- valn + " " +
- pos + " " +
- name,
+ oh.setDebugObserverDatafield("Normal Force " + valn + " " + pos + " " +
+ name,
1e-5f + static_cast<float>(val));
};
};
@@ -955,47 +1069,49 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
//shear
{
- const auto plot_shear = [&](const std::string & pos, const auto & sens)
+ const auto plot_shear = [&](const std::string& pos, const auto& sens)
{
- const auto mk_plot_sfor = [&](const std::string & xyz)
+ const auto mk_plot_sfor = [&](const std::string& xyz)
{
- return [&, xyz](const std::string & valn, auto val)
+ return [&, xyz](const std::string& valn, auto val)
{
- oh.setDebugObserverDatafield("Shear Force " +
- pos + " " +
- valn + " " +
- xyz + " " +
- name, 1e-5f + static_cast<float>(val));
+ oh.setDebugObserverDatafield("Shear Force " + pos + " " + valn +
+ " " + xyz + " " + name,
+ 1e-5f + static_cast<float>(val));
};
};
sens.sensors.at(0).visit(mk_plot_sfor("x"));
sens.sensors.at(1).visit(mk_plot_sfor("y"));
sens.sensors.at(2).visit(mk_plot_sfor("z"));
- oh.setDebugObserverDatafield("Shear Force len_xy " + pos + " " + name, 1e-5f + sens.len_xy);
- oh.setDebugObserverDatafield("Shear Force len_z " + pos + " " + name, 1e-5f + sens.len_z);
+ oh.setDebugObserverDatafield("Shear Force len_xy " + pos + " " + name,
+ 1e-5f + sens.len_xy);
+ oh.setDebugObserverDatafield("Shear Force len_z " + pos + " " + name,
+ 1e-5f + sens.len_z);
};
plot_shear("joint", fdata.shear_force_dist_joint);
plot_shear("tip", fdata.shear_force_dist_tip);
}
//accelerometer fft
{
- const auto plot_fft = [&](const std::string & coords, const auto & fft)
+ const auto plot_fft = [&](const std::string& coords, const auto& fft)
{
for (std::size_t i = 0; i < fft.size(); ++i)
{
- oh.setDebugObserverDatafield("accelerometer fft " +
- coords + " magnitude " +
- std::to_string(i) + " " +
- name, 1e-5f + fft.at(i));
+ oh.setDebugObserverDatafield("accelerometer fft " + coords +
+ " magnitude " + std::to_string(i) +
+ " " + name,
+ 1e-5f + fft.at(i));
}
};
plot_fft("x", fdata.accelerometer.fft_absolute_x);
plot_fft("y", fdata.accelerometer.fft_absolute_y);
plot_fft("xy", fdata.accelerometer.fft_absolute_xy);
- oh.setDebugObserverDatafield("accelerometer fft energy " + name, 1e-5f + fdata.accelerometer.fft_contact_detect_energy);
+ oh.setDebugObserverDatafield("accelerometer fft energy " + name,
+ 1e-5f +
+ fdata.accelerometer.fft_contact_detect_energy);
}
};
publish_finger("little", 0);
@@ -1003,40 +1119,57 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
publish_finger("middle", 2);
publish_finger("index", 3);
publish_finger("thumb", 4);
- oh.setDebugObserverDatafield("accelerometer fft energy min",
- 1e-5f + std::min({buf.normalized_finger_sensors.at(0).accelerometer.fft_contact_detect_energy,
- buf.normalized_finger_sensors.at(1).accelerometer.fft_contact_detect_energy,
- buf.normalized_finger_sensors.at(2).accelerometer.fft_contact_detect_energy,
- buf.normalized_finger_sensors.at(3).accelerometer.fft_contact_detect_energy,
- buf.normalized_finger_sensors.at(4).accelerometer.fft_contact_detect_energy}));
- oh.setDebugObserverDatafield("accelerometer fft energy max",
- 1e-5f + std::max({buf.normalized_finger_sensors.at(0).accelerometer.fft_contact_detect_energy,
- buf.normalized_finger_sensors.at(1).accelerometer.fft_contact_detect_energy,
- buf.normalized_finger_sensors.at(2).accelerometer.fft_contact_detect_energy,
- buf.normalized_finger_sensors.at(3).accelerometer.fft_contact_detect_energy,
- buf.normalized_finger_sensors.at(4).accelerometer.fft_contact_detect_energy}));
- oh.setDebugObserverDatafield("accelerometer fft energy avg",
- 1e-5f + (buf.normalized_finger_sensors.at(0).accelerometer.fft_contact_detect_energy +
- buf.normalized_finger_sensors.at(1).accelerometer.fft_contact_detect_energy +
- buf.normalized_finger_sensors.at(2).accelerometer.fft_contact_detect_energy +
- buf.normalized_finger_sensors.at(3).accelerometer.fft_contact_detect_energy +
- buf.normalized_finger_sensors.at(4).accelerometer.fft_contact_detect_energy) / 5);
+ oh.setDebugObserverDatafield(
+ "accelerometer fft energy min",
+ 1e-5f + std::min({buf.normalized_finger_sensors.at(0)
+ .accelerometer.fft_contact_detect_energy,
+ buf.normalized_finger_sensors.at(1)
+ .accelerometer.fft_contact_detect_energy,
+ buf.normalized_finger_sensors.at(2)
+ .accelerometer.fft_contact_detect_energy,
+ buf.normalized_finger_sensors.at(3)
+ .accelerometer.fft_contact_detect_energy,
+ buf.normalized_finger_sensors.at(4)
+ .accelerometer.fft_contact_detect_energy}));
+ oh.setDebugObserverDatafield(
+ "accelerometer fft energy max",
+ 1e-5f + std::max({buf.normalized_finger_sensors.at(0)
+ .accelerometer.fft_contact_detect_energy,
+ buf.normalized_finger_sensors.at(1)
+ .accelerometer.fft_contact_detect_energy,
+ buf.normalized_finger_sensors.at(2)
+ .accelerometer.fft_contact_detect_energy,
+ buf.normalized_finger_sensors.at(3)
+ .accelerometer.fft_contact_detect_energy,
+ buf.normalized_finger_sensors.at(4)
+ .accelerometer.fft_contact_detect_energy}));
+ oh.setDebugObserverDatafield(
+ "accelerometer fft energy avg",
+ 1e-5f +
+ (buf.normalized_finger_sensors.at(0).accelerometer.fft_contact_detect_energy +
+ buf.normalized_finger_sensors.at(1).accelerometer.fft_contact_detect_energy +
+ buf.normalized_finger_sensors.at(2).accelerometer.fft_contact_detect_energy +
+ buf.normalized_finger_sensors.at(3).accelerometer.fft_contact_detect_energy +
+ buf.normalized_finger_sensors.at(4).accelerometer.fft_contact_detect_energy) /
+ 5);
}
- oh.setDebugObserverDatafield("updating_sensors", 1e-5f + buf.updating_sensors);
- oh.setDebugObserverDatafield("resetting_controller", 1e-5f + buf.resetting_controller);
- oh.setDebugObserverDatafield("run_controller", 1e-5f + buf.run_controller);
+ oh.setDebugObserverDatafield("updating_sensors", 1e-5f + buf.updating_sensors);
+ oh.setDebugObserverDatafield("resetting_controller", 1e-5f + buf.resetting_controller);
+ oh.setDebugObserverDatafield("run_controller", 1e-5f + buf.run_controller);
- oh.setDebugObserverDatafield("active_shear_avg_len_xy", 1e-5f + buf.active_shear_avg_len_xy);
- oh.setDebugObserverDatafield("active_shear_avg_len_z", 1e-5f + buf.active_shear_avg_len_z);
- oh.setDebugObserverDatafield("active_shear_avg_ratio", 1e-5f + buf.active_shear_avg_ratio);
- oh.setDebugObserverDatafield("active_shear_num", 1e-5f + buf.active_shear_num);
- oh.setDebugObserverDatafield("used_shear_avg_ratio", 1e-5f + buf.used_shear_avg_ratio);
+ oh.setDebugObserverDatafield("active_shear_avg_len_xy",
+ 1e-5f + buf.active_shear_avg_len_xy);
+ oh.setDebugObserverDatafield("active_shear_avg_len_z", 1e-5f + buf.active_shear_avg_len_z);
+ oh.setDebugObserverDatafield("active_shear_avg_ratio", 1e-5f + buf.active_shear_avg_ratio);
+ oh.setDebugObserverDatafield("active_shear_num", 1e-5f + buf.active_shear_num);
+ oh.setDebugObserverDatafield("used_shear_avg_ratio", 1e-5f + buf.used_shear_avg_ratio);
- oh.setDebugObserverDatafield("open_signal", 1e-5f + buf.open_signal);
- oh.setDebugObserverDatafield("open_signal_finger_count", 1e-5f + buf.open_signal_finger_count);
+ oh.setDebugObserverDatafield("open_signal", 1e-5f + buf.open_signal);
+ oh.setDebugObserverDatafield("open_signal_finger_count",
+ 1e-5f + buf.open_signal_finger_count);
- oh.setDebugObserverDatafield("last_fpga_dt", buf.last_fpga_dt);
+ oh.setDebugObserverDatafield("last_fpga_dt", buf.last_fpga_dt);
oh.sendDebugObserverBatch();
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/MinimalGraspingForceV1.h b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/MinimalGraspingForceV1.h
index fce6650e..ef917c98 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/MinimalGraspingForceV1.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/MinimalGraspingForceV1.h
@@ -3,10 +3,10 @@
#include "RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h"
-#include <devices/ethercat/hand/soft_sensorized_finger/controller/CloseAndHold.h>
-#include <devices/ethercat/hand/soft_sensorized_finger/utility/Sensors.h>
#include "minimal_grasping_force_v1/GraspPhaseControllerData.h"
+#include <devices/ethercat/hand/soft_sensorized_finger/controller/CloseAndHold.h>
+#include <devices/ethercat/hand/soft_sensorized_finger/utility/Sensors.h>
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
@@ -16,31 +16,31 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
public:
using ConfigPtrT = NJointMinimalGraspingForceV1ControllerConfigPtr;
+
private:
using MotorIdentity = Sensors::MotorIdentity;
using FingerIdentity = Sensors::FingerIdentity;
- template<class T>
+ template <class T>
using FingerArray = identity::FingerArray<T>;
- template<class T>
+ template <class T>
using MotorArray = identity::MotorArray<T>;
- FingerArray<FingerSensor> _finger_sensors; //0 = little, 1 = ring, 2 = middle, 3 = index, 4 = thumb
+ FingerArray<FingerSensor>
+ _finger_sensors; //0 = little, 1 = ring, 2 = middle, 3 = index, 4 = thumb
GraspPhaseControllerData _grasp_phase_controller_data;
//create
public:
- static armarx::WidgetDescription::WidgetPtr GenerateConfigDescription(
- const VirtualRobot::RobotPtr&,
- const std::map<std::string, armarx::ConstControlDevicePtr>&,
- const std::map<std::string, armarx::ConstSensorDevicePtr>&);
+ static armarx::WidgetDescription::WidgetPtr
+ GenerateConfigDescription(const VirtualRobot::RobotPtr&,
+ const std::map<std::string, armarx::ConstControlDevicePtr>&,
+ const std::map<std::string, armarx::ConstSensorDevicePtr>&);
- static ConfigPtrT
- GenerateConfigFromVariants(const armarx::StringVariantBaseMap& values);
+ static ConfigPtrT GenerateConfigFromVariants(const armarx::StringVariantBaseMap& values);
- MinimalGraspingForceV1_NJointController(
- armarx::RobotUnitPtr robotUnit,
- const ConfigPtrT& config,
- const VirtualRobot::RobotPtr&);
+ MinimalGraspingForceV1_NJointController(armarx::RobotUnitPtr robotUnit,
+ const ConfigPtrT& config,
+ const VirtualRobot::RobotPtr&);
//managemant
public:
@@ -48,19 +48,27 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
//remote function calls
public:
- armarx::WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
- void callDescribedFunction(const std::string& name, const armarx::StringVariantBaseMap& valueMap, const Ice::Current& = Ice::emptyCurrent) override;
+ armarx::WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
+ void callDescribedFunction(const std::string& name,
+ const armarx::StringVariantBaseMap& valueMap,
+ const Ice::Current& = Ice::emptyCurrent) override;
//rt control
public:
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
void rtPreActivateController() override;
void rtPostDeactivateController() override;
//publish
public:
- void onPublishActivation(const armarx::DebugDrawerInterfacePrx&, const armarx::DebugObserverInterfacePrx&) override;
- void onPublishDeactivation(const armarx::DebugDrawerInterfacePrx&, const armarx::DebugObserverInterfacePrx&) override;
- void onPublish(const armarx::SensorAndControl&, const armarx::DebugDrawerInterfacePrx&, const armarx::DebugObserverInterfacePrx&) override;
+ void onPublishActivation(const armarx::DebugDrawerInterfacePrx&,
+ const armarx::DebugObserverInterfacePrx&) override;
+ void onPublishDeactivation(const armarx::DebugDrawerInterfacePrx&,
+ const armarx::DebugObserverInterfacePrx&) override;
+ void onPublish(const armarx::SensorAndControl&,
+ const armarx::DebugDrawerInterfacePrx&,
+ const armarx::DebugObserverInterfacePrx&) override;
protected:
//data from ice -> RT
@@ -73,11 +81,13 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
float i = 0;
float t = 0;
};
+
control_data pos_control;
control_data pwm_control;
NJointMinimalGraspingForceV1ControllerConfig cfg;
};
- mutable std::recursive_mutex _ice_to_rt_buffer_write_mutex;
+
+ mutable std::recursive_mutex _ice_to_rt_buffer_write_mutex;
armarx::WriteBufferedTripleBuffer<DataIceToRT> _ice_to_rt_buffer;
std::atomic_bool _clicked_open{false};
@@ -105,42 +115,51 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
float used_shear_avg_ratio = 0;
std::size_t open_signal_finger_count = 0;
- bool open_signal = false;
+ bool open_signal = false;
};
+
armarx::WriteBufferedTripleBuffer<DataRTToPublish> _rt_to_ice;
- mutable std::recursive_mutex _rt_to_ice_read_mutex;
+ mutable std::recursive_mutex _rt_to_ice_read_mutex;
//sensors
Sensors _sensors;
//ctrl target
- armarx::ControlTarget1DoFActuatorPWM* _target_pwm_thumb = nullptr;
- armarx::ControlTarget1DoFActuatorPWM* _target_pwm_index = nullptr;
- armarx::ControlTarget1DoFActuatorPWM* _target_pwm_other = nullptr;
+ armarx::ControlTarget1DoFActuatorPWM* _target_pwm_thumb = nullptr;
+ armarx::ControlTarget1DoFActuatorPWM* _target_pwm_index = nullptr;
+ armarx::ControlTarget1DoFActuatorPWM* _target_pwm_other = nullptr;
armarx::ControlTarget1DoFActuatorPWM*
target_pwm(MotorIdentity id) const
{
- return identity::Select(
- id, _target_pwm_other, _target_pwm_index, _target_pwm_thumb);
+ return identity::Select(id, _target_pwm_other, _target_pwm_index, _target_pwm_thumb);
}
public:
void closeMGF(const Ice::Current& = Ice::emptyCurrent) override;
void openMGF(const Ice::Current& = Ice::emptyCurrent) override;
void startPlacing(const Ice::Current& = Ice::emptyCurrent) override;
- void overridePWM(bool active, Ice::Float o, Ice::Float i, Ice::Float t, const Ice::Current& = Ice::emptyCurrent) override;
+ void overridePWM(bool active,
+ Ice::Float o,
+ Ice::Float i,
+ Ice::Float t,
+ const Ice::Current& = Ice::emptyCurrent) override;
GraspPhase::Phase currentGraspPhase(const Ice::Current& = Ice::emptyCurrent) override;
- void setConfig(const NJointMinimalGraspingForceV1ControllerConfigPtr& cfg, const Ice::Current&) override;
+ void setConfig(const NJointMinimalGraspingForceV1ControllerConfigPtr& cfg,
+ const Ice::Current&) override;
void recalibrate(const Ice::Current& = Ice::emptyCurrent) override;
- NJointMinimalGraspingForceV1ControllerDebugData getDebugData(const Ice::Current& = Ice::emptyCurrent) override;
+ NJointMinimalGraspingForceV1ControllerDebugData
+ getDebugData(const Ice::Current& = Ice::emptyCurrent) override;
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
+
namespace armarx::detail
{
- static_assert(
- hasGenerateConfigDescription< devices::ethercat::hand::soft_sensorized_finger::njoint_controller::MinimalGraspingForceV1_NJointController>::value);
- static_assert(
- hasGenerateConfigFromVariants< devices::ethercat::hand::soft_sensorized_finger::njoint_controller::MinimalGraspingForceV1_NJointController>::value);
-}
+ static_assert(hasGenerateConfigDescription<
+ devices::ethercat::hand::soft_sensorized_finger::njoint_controller::
+ MinimalGraspingForceV1_NJointController>::value);
+ static_assert(hasGenerateConfigFromVariants<
+ devices::ethercat::hand::soft_sensorized_finger::njoint_controller::
+ MinimalGraspingForceV1_NJointController>::value);
+} // namespace armarx::detail
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/MinimalGraspingForceV1Interface.ice b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/MinimalGraspingForceV1Interface.ice
index f7ee6487..0cad22b2 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/MinimalGraspingForceV1Interface.ice
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/MinimalGraspingForceV1Interface.ice
@@ -12,12 +12,12 @@ module armarx
{
enum Phase
{
- FREELY_MOVING = 1,
- BARRIER_PRE_LLH = 2,
- LOAD_LIFT_HOLD = 3,
- UNLOAD = 4,
+ FREELY_MOVING = 1,
+ BARRIER_PRE_LLH = 2,
+ LOAD_LIFT_HOLD = 3,
+ UNLOAD = 4,
BARRIER_PRE_OPEN = 5,
- OPEN = 6
+ OPEN = 6
};
};
@@ -29,27 +29,28 @@ module armarx
float i;
float d;
};
+
struct MotorCfg
{
- PID pid_pos;
- PID pid_pressure;
- PID pid_shear;
- PID pid_unload;
-
- float pid_force_forward_p; //start at 200
- float pid_force_limit;
- PID pid_force;
-
- short max_pwm = 1000;
- short pwm_open = -200;
- short pwm_freely_moving = 1000;
+ PID pid_pos;
+ PID pid_pressure;
+ PID pid_shear;
+ PID pid_unload;
+
+ float pid_force_forward_p; //start at 200
+ float pid_force_limit;
+ PID pid_force;
+
+ short max_pwm = 1000;
+ short pwm_open = -200;
+ short pwm_freely_moving = 1000;
short min_pwm_close_to_contact = 200;
- float target_pressure = 30;
- float target_shear = 1.3;
+ float target_pressure = 30;
+ float target_shear = 1.3;
long free_phase_start_moving_delay_msec = 0;
- float gross_slip_pos_step = 0.1;
+ float gross_slip_pos_step = 0.1;
float gross_slip_threshold = 0.1;
float encoder_delta_for_movent = 0.05;
@@ -60,10 +61,10 @@ module armarx
class NJointMinimalGraspingForceV1ControllerConfig extends NJointControllerConfig
{
- string hand_name;
+ string hand_name;
- float place_fft_energy_threshold = 100000;
- int place_finger_trigger_count = 5;
+ float place_fft_energy_threshold = 100000;
+ int place_finger_trigger_count = 5;
long min_load_lift_hold_duration = 2000000;
NJointMinimalGraspingForceV1ControllerConfigElements::MotorCfg motor_index;
NJointMinimalGraspingForceV1ControllerConfigElements::MotorCfg motor_other;
@@ -81,81 +82,83 @@ module armarx
{
GraspPhase::Phase grasp_phase = GraspPhase::Phase::FREELY_MOVING;
- float squish_distance = 0;
- float last_shear_pressure_position = 0.0f;
- float pos_ctrl_target = 0.0f;
- float normal_force_controller_position_delta = 0.0f;
- float shear_force_controller_position_delta = 0.0f;
- float shear_force_min_xy_len = 0.0f;
- float shear_force_min_z_len = 0.0f;
- float shear_force_min_ratio = 0.0f;
- float shear_force_avg_ratio = 0.0f;
- float shear_force_used_ratio = 0.0f;
- long shear_force_used_ratio_num = 0;
- double slip_detection_value = 0.0f;
- float shear_force_ratio_number_of_active_sensors = 0.0f;
- float maximum_pressure = 0;
- float pressure_on_start_unload = 0;
-
- float target_shear = 0.0f;
- float target_pressure = 0.0f;
-
- bool contact_detection_flag = false;
- long gross_slip_count = 0;
- bool finger_did_close = false;
-
- float monotonic_shear_pressure_control_last_pwm = 0;
- float monotonic_shear_pressure_control_last_pwm_pressure = 0;
- float monotonic_shear_pressure_control_last_pwm_shear = 0;
- float monotonic_shear_pressure_control_last_pwm_slip = 0;
- float monotonic_shear_pressure_control_last_delta_pressure = 0;
- float monotonic_shear_pressure_control_last_delta_shear = 0;
-
- float target_pressure_rate_of_change = 0;
- float pressure_rate_of_change = 0;
- float unload_phase_controller_normal_force_via_pwm_last_pwm = 0;
+ float squish_distance = 0;
+ float last_shear_pressure_position = 0.0f;
+ float pos_ctrl_target = 0.0f;
+ float normal_force_controller_position_delta = 0.0f;
+ float shear_force_controller_position_delta = 0.0f;
+ float shear_force_min_xy_len = 0.0f;
+ float shear_force_min_z_len = 0.0f;
+ float shear_force_min_ratio = 0.0f;
+ float shear_force_avg_ratio = 0.0f;
+ float shear_force_used_ratio = 0.0f;
+ long shear_force_used_ratio_num = 0;
+ double slip_detection_value = 0.0f;
+ float shear_force_ratio_number_of_active_sensors = 0.0f;
+ float maximum_pressure = 0;
+ float pressure_on_start_unload = 0;
+
+ float target_shear = 0.0f;
+ float target_pressure = 0.0f;
+
+ bool contact_detection_flag = false;
+ long gross_slip_count = 0;
+ bool finger_did_close = false;
+
+ float monotonic_shear_pressure_control_last_pwm = 0;
+ float monotonic_shear_pressure_control_last_pwm_pressure = 0;
+ float monotonic_shear_pressure_control_last_pwm_shear = 0;
+ float monotonic_shear_pressure_control_last_pwm_slip = 0;
+ float monotonic_shear_pressure_control_last_delta_pressure = 0;
+ float monotonic_shear_pressure_control_last_delta_shear = 0;
+
+ float target_pressure_rate_of_change = 0;
+ float pressure_rate_of_change = 0;
+ float unload_phase_controller_normal_force_via_pwm_last_pwm = 0;
};
struct NJointMinimalGraspingForceV1ControllerDebugDataFingerNF
{
- long current = 0;
- float continuous_mean = 0;
- float current_mean_free_scaled = 0;
+ long current = 0;
+ float continuous_mean = 0;
+ float current_mean_free_scaled = 0;
float current_continuous_mean_free_scaled = 0;
};
+
struct NJointMinimalGraspingForceV1ControllerDebugDataFingerShear
{
- float len_xy = 0;
- float len_z = 0;
- float ratio = 0;
+ float len_xy = 0;
+ float len_z = 0;
+ float ratio = 0;
- long x_current = 0;
- float x_continuous_mean = 0;
- float x_current_mean_free_scaled = 0;
+ long x_current = 0;
+ float x_continuous_mean = 0;
+ float x_current_mean_free_scaled = 0;
float x_current_continuous_mean_free_scaled = 0;
- long y_current = 0;
- float y_continuous_mean = 0;
- float y_current_mean_free_scaled = 0;
+ long y_current = 0;
+ float y_continuous_mean = 0;
+ float y_current_mean_free_scaled = 0;
float y_current_continuous_mean_free_scaled = 0;
- long z_current = 0;
- float z_continuous_mean = 0;
- float z_current_mean_free_scaled = 0;
+ long z_current = 0;
+ float z_continuous_mean = 0;
+ float z_current_mean_free_scaled = 0;
float z_current_continuous_mean_free_scaled = 0;
};
+
struct NJointMinimalGraspingForceV1ControllerDebugDataFingerAcc
{
- double shear_force_criterium = 0.0f;
+ double shear_force_criterium = 0.0f;
double fft_contact_detect_energy = 0.0f;
};
struct NJointMinimalGraspingForceV1ControllerDebugDataFinger
{
- float fit_prox_x = 0;
- float fit_dist_x = 0;
- float fit_prox_z = 0;
- float fit_dist_z = 0;
+ float fit_prox_x = 0;
+ float fit_dist_x = 0;
+ float fit_prox_z = 0;
+ float fit_dist_z = 0;
float fit_prox_avg = 0;
float fit_dist_avg = 0;
NJointMinimalGraspingForceV1ControllerDebugDataFingerNF nf_prox_l;
@@ -180,43 +183,44 @@ module armarx
NJointMinimalGraspingForceV1ControllerDebugDataFinger finger_thumb;
GraspPhase::Phase grasp_phase = GraspPhase::Phase::FREELY_MOVING;
- long sensor_time_stamp_us_ice = 0;
- long time_stamp_us_std = 0;
+ long sensor_time_stamp_us_ice = 0;
+ long time_stamp_us_std = 0;
- bool updating_sensors = false;
- bool resetting_controller = false;
- bool run_controller = false;
+ bool updating_sensors = false;
+ bool resetting_controller = false;
+ bool run_controller = false;
- float all_shear_avg_len_xy = 0;
- float all_shear_avg_len_z = 0;
- float all_shear_avg_ratio = 0;
+ float all_shear_avg_len_xy = 0;
+ float all_shear_avg_len_z = 0;
+ float all_shear_avg_ratio = 0;
- float last_fpga_dt = 0;
+ float last_fpga_dt = 0;
- float finger_active_shear_avg_len_xy = 0;
- float finger_active_shear_avg_len_z = 0;
- float finger_active_shear_avg_ratio = 0;
- long finger_active_shear_num = 0;
+ float finger_active_shear_avg_len_xy = 0;
+ float finger_active_shear_avg_len_z = 0;
+ float finger_active_shear_avg_ratio = 0;
+ long finger_active_shear_num = 0;
- float active_shear_avg_len_xy = 0;
- float active_shear_avg_len_z = 0;
- float active_shear_avg_ratio = 0;
- long active_shear_num = 0;
- float used_shear_avg_ratio = 0;
+ float active_shear_avg_len_xy = 0;
+ float active_shear_avg_len_z = 0;
+ float active_shear_avg_ratio = 0;
+ long active_shear_num = 0;
+ float used_shear_avg_ratio = 0;
- float negative_z_shear_avg_len_xy = 0;
- float negative_z_shear_avg_len_z = 0;
- float negative_z_shear_avg_ratio = 0;
- long negative_z_shear_num = 0;
+ float negative_z_shear_avg_len_xy = 0;
+ float negative_z_shear_avg_len_z = 0;
+ float negative_z_shear_avg_ratio = 0;
+ long negative_z_shear_num = 0;
float all_shear_avg_len_z_pressure_sensors = 0.0f;
float all_shear_avg_ratio_pressure_sensors = 0.0f;
- long open_signal_finger_count = 0;
- bool open_signal = false;
+ long open_signal_finger_count = 0;
+ bool open_signal = false;
};
- interface NJointMinimalGraspingForceV1ControllerInterface extends NJointControllerInterface
+ interface NJointMinimalGraspingForceV1ControllerInterface extends
+ NJointControllerInterface
{
void closeMGF();
void openMGF();
@@ -230,4 +234,3 @@ module armarx
};
};
};
-
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/Shape.cpp b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/Shape.cpp
index e01abcee..50ecb9fa 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/Shape.cpp
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/Shape.cpp
@@ -1,31 +1,31 @@
+#include "Shape.h"
+
#include <boost/algorithm/clamp.hpp>
#include "RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h"
#include <RobotAPI/libraries/core/math/MathUtils.h>
#include "../joint_controller/Pwm.h"
-#include "Shape.h"
//init
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
armarx::NJointControllerRegistration<NJointShapeController>
- registrationControllerNJointShapeController(
- "NJointKITSensorizedSoftFingerHandV1ShapeController");
+ registrationControllerNJointShapeController(
+ "NJointKITSensorizedSoftFingerHandV1ShapeController");
NJointShapeController::NJointShapeController(armarx::RobotUnitPtr prov,
- NJointShapeControllerConfigPtr config,
- const VirtualRobot::RobotPtr&)
+ NJointShapeControllerConfigPtr config,
+ const VirtualRobot::RobotPtr&)
{
ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(prov);
- const auto init = [&](auto & data, const std::string & suffix)
+ const auto init = [&](auto& data, const std::string& suffix)
{
const auto name = config->deviceName + ".motor." + suffix;
armarx::ControlTargetBase* ct = useControlTarget(name, armarx::ControlModes::PWM1DoF);
- ARMARX_CHECK_NOT_NULL(ct) << " control target " << name
- << " control mode " << armarx::ControlModes::PWM1DoF
- << ARMARX_STREAM_PRINTER
+ ARMARX_CHECK_NOT_NULL(ct) << " control target " << name << " control mode "
+ << armarx::ControlModes::PWM1DoF << ARMARX_STREAM_PRINTER
{
const auto ptr = peekControlDevice(name);
if (ptr)
@@ -44,32 +44,31 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
data.motor_sensor = sensor->getSensorValue()->asA<MotorSensorValue>();
ARMARX_CHECK_NOT_NULL(data.motor_sensor) << "device name: " << name;
- data.pwm_target = ct->asA<armarx::ControlTarget1DoFActuatorPWM>();
+ data.pwm_target = ct->asA<armarx::ControlTarget1DoFActuatorPWM>();
ARMARX_CHECK_NOT_NULL(data.pwm_target) << "device name: " << name;
};
init(_other_data, "other");
init(_index_data, "index");
init(_thumb_data, "thumb");
}
- void NJointShapeController::readConfig(
- const armarx::RapidXmlReaderNode& configNode,
- armarx::DefaultRapidXmlReaderNode defaultConfigurationNode)
+
+ void
+ NJointShapeController::readConfig(const armarx::RapidXmlReaderNode& configNode,
+ armarx::DefaultRapidXmlReaderNode defaultConfigurationNode)
{
ARMARX_TRACE;
- ARMARX_DEBUG << "readConfig got cfg:\n" << VAROUT(configNode.getPath())
- << "\n" << VAROUT(defaultConfigurationNode.getPathsString());
+ ARMARX_DEBUG << "readConfig got cfg:\n"
+ << VAROUT(configNode.getPath()) << "\n"
+ << VAROUT(defaultConfigurationNode.getPathsString());
auto configNodeAll =
- defaultConfigurationNode
- .add_node_at_end(configNode)
- .first_node("Motor");
+ defaultConfigurationNode.add_node_at_end(configNode).first_node("Motor");
- const auto init = [&](auto & data, auto name)
+ const auto init = [&](auto& data, auto name)
{
- auto cfg = configNodeAll
- .first_node("default")
- .add_node_at_end(configNodeAll.first_node(name))
- .first_node("ctrl_shape");
+ auto cfg = configNodeAll.first_node("default")
+ .add_node_at_end(configNodeAll.first_node(name))
+ .first_node("ctrl_shape");
cfg.attribute_as("Kp", data.Kp);
cfg.attribute_as("Ki", data.Ki);
cfg.attribute_as("Kd", data.Kd);
@@ -79,54 +78,70 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
init(_index_data, "index");
init(_other_data, "other");
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
//getter
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
- int16_t NJointShapeController::getOtherPwm()
+ int16_t
+ NJointShapeController::getOtherPwm()
{
return _other_data.last_pwm;
}
- int16_t NJointShapeController::getIndexPwm()
+
+ int16_t
+ NJointShapeController::getIndexPwm()
{
return _index_data.last_pwm;
}
- int16_t NJointShapeController::getThumbPwm()
+
+ int16_t
+ NJointShapeController::getThumbPwm()
{
return _thumb_data.last_pwm;
}
- float NJointShapeController::getOtherTarget() const
+ float
+ NJointShapeController::getOtherTarget() const
{
std::lock_guard g{_last_target_buf_read_mutex};
return _last_target_buf.getUpToDateReadBuffer().other.target;
}
- float NJointShapeController::getIndexTarget() const
+
+ float
+ NJointShapeController::getIndexTarget() const
{
std::lock_guard g{_last_target_buf_read_mutex};
return _last_target_buf.getUpToDateReadBuffer().index.target;
}
- float NJointShapeController::getThumbTarget() const
+
+ float
+ NJointShapeController::getThumbTarget() const
{
std::lock_guard g{_last_target_buf_read_mutex};
return _last_target_buf.getUpToDateReadBuffer().thumb.target;
}
- float NJointShapeController::getOtherJointValue() const
+ float
+ NJointShapeController::getOtherJointValue() const
{
return _other_data.joint_value;
}
- float NJointShapeController::getIndexJointValue() const
+
+ float
+ NJointShapeController::getIndexJointValue() const
{
return _index_data.joint_value;
}
- float NJointShapeController::getThumbJointValue() const
+
+ float
+ NJointShapeController::getThumbJointValue() const
{
return _thumb_data.joint_value;
}
- bool NJointShapeController::isControlEnabled() const
+ bool
+ NJointShapeController::isControlEnabled() const
{
std::lock_guard g{_last_target_buf_read_mutex};
return _last_target_buf.getUpToDateReadBuffer().enable_control;
@@ -136,53 +151,62 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
NJointShapeController::getJointValues(const Ice::Current&) const
{
JointValues values;
- values.otherJointValue = getOtherJointValue();
- values.indexJointValue = getIndexJointValue();
- values.thumbJointValue = getThumbJointValue();
+ values.otherJointValue = getOtherJointValue();
+ values.indexJointValue = getIndexJointValue();
+ values.thumbJointValue = getThumbJointValue();
return values;
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
//setter
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
- void NJointShapeController::stopMotion()
+ void
+ NJointShapeController::stopMotion()
{
std::lock_guard g{_target_buf_write_mutex};
_target_buf.getWriteBuffer().enable_control = false;
_target_buf.commitWrite();
}
- void NJointShapeController::setTargets(
- float lrm, float index, float thumb, const Ice::Current&)
+
+ void
+ NJointShapeController::setTargets(float lrm, float index, float thumb, const Ice::Current&)
{
setTargetsWithPwm(lrm, index, thumb, 1, 1, 1);
}
- void NJointShapeController::setTargetsWithPwm(
- float lrm, float index, float thumb,
- float lrmRelativePWM, float indexRelativePWM, float thumbRelativePWM,
- const Ice::Current&)
+ void
+ NJointShapeController::setTargetsWithPwm(float lrm,
+ float index,
+ float thumb,
+ float lrmRelativePWM,
+ float indexRelativePWM,
+ float thumbRelativePWM,
+ const Ice::Current&)
{
std::lock_guard g{_target_buf_write_mutex};
auto& buf = _target_buf.getWriteBuffer();
buf.enable_control = true;
- buf.index.target = armarx::math::MathUtils::LimitMinMax(0.f, 2.f, thumb);
- buf.thumb.target = armarx::math::MathUtils::LimitMinMax(0.f, 2.f, index);
- buf.other.target = armarx::math::MathUtils::LimitMinMax(0.f, 2.f, lrm);
- buf.index.max_pwm = static_cast<std::int16_t>(_index_data.pwm_limit * boost::algorithm::clamp(indexRelativePWM, 0, 1));
- buf.thumb.max_pwm = static_cast<std::int16_t>(_thumb_data.pwm_limit * boost::algorithm::clamp(thumbRelativePWM, 0, 1));
- buf.other.max_pwm = static_cast<std::int16_t>(_other_data.pwm_limit * boost::algorithm::clamp(lrmRelativePWM, 0, 1));
+ buf.index.target = armarx::math::MathUtils::LimitMinMax(0.f, 2.f, thumb);
+ buf.thumb.target = armarx::math::MathUtils::LimitMinMax(0.f, 2.f, index);
+ buf.other.target = armarx::math::MathUtils::LimitMinMax(0.f, 2.f, lrm);
+ buf.index.max_pwm = static_cast<std::int16_t>(
+ _index_data.pwm_limit * boost::algorithm::clamp(indexRelativePWM, 0, 1));
+ buf.thumb.max_pwm = static_cast<std::int16_t>(
+ _thumb_data.pwm_limit * boost::algorithm::clamp(thumbRelativePWM, 0, 1));
+ buf.other.max_pwm = static_cast<std::int16_t>(
+ _other_data.pwm_limit * boost::algorithm::clamp(lrmRelativePWM, 0, 1));
_target_buf.commitWrite();
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
//control
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
- void NJointShapeController::rtRun(
- const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointShapeController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
_other_data.joint_value = _other_data.motor_sensor->position;
_index_data.joint_value = _index_data.motor_sensor->position;
@@ -194,12 +218,13 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
if (buf.enable_control)
{
- auto calc = [](auto & buff, auto & data)
+ auto calc = [](auto& buff, auto& data)
{
const float err = buff.target - data.motor_sensor->position;
- data.pwm_target->pwm = boost::algorithm::clamp(
- static_cast<std::int16_t>(data.Kp * buff.max_pwm * err),
- -buff.max_pwm, buff.max_pwm);
+ data.pwm_target->pwm =
+ boost::algorithm::clamp(static_cast<std::int16_t>(data.Kp * buff.max_pwm * err),
+ -buff.max_pwm,
+ buff.max_pwm);
};
calc(buf.other, _other_data);
calc(buf.index, _index_data);
@@ -215,16 +240,17 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
_index_data.last_pwm = _index_data.pwm_target->pwm;
_thumb_data.last_pwm = _thumb_data.pwm_target->pwm;
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
//gui
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
- armarx::WidgetDescription::StringWidgetDictionary NJointShapeController::getFunctionDescriptions(const Ice::Current&) const
+ armarx::WidgetDescription::StringWidgetDictionary
+ NJointShapeController::getFunctionDescriptions(const Ice::Current&) const
{
using namespace armarx::WidgetDescription;
VBoxLayoutPtr vbox = new VBoxLayout;
- const auto add = [&](const std::string & name, const auto value)
+ const auto add = [&](const std::string& name, const auto value)
{
HBoxLayoutPtr hbox = new HBoxLayout;
vbox->children.emplace_back(hbox);
@@ -253,25 +279,26 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
add("index", getIndexJointValue());
add("thumb", getThumbJointValue());
return {{"Targets", vbox}};
-
}
- void NJointShapeController::callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&)
+ void
+ NJointShapeController::callDescribedFunction(const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&)
{
if (name == "Targets")
{
- setTargetsWithPwm(
- valueMap.at("other_val")->getFloat(),
- valueMap.at("index_val")->getFloat(),
- valueMap.at("thumb_val")->getFloat(),
+ setTargetsWithPwm(valueMap.at("other_val")->getFloat(),
+ valueMap.at("index_val")->getFloat(),
+ valueMap.at("thumb_val")->getFloat(),
- valueMap.at("other_pwm")->getFloat(),
- valueMap.at("index_pwm")->getFloat(),
- valueMap.at("thumb_pwm")->getFloat());
+ valueMap.at("other_pwm")->getFloat(),
+ valueMap.at("index_pwm")->getFloat(),
+ valueMap.at("thumb_pwm")->getFloat());
}
else
{
ARMARX_WARNING << "Unknown function name called: " << name;
}
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/Shape.h b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/Shape.h
index 76819ef3..5df2ef9e 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/Shape.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/Shape.h
@@ -2,14 +2,14 @@
#include <atomic>
-#include "RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h"
#include "RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h"
+#include "RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h"
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
#include "robot_devices/ethercat/hand/soft_sensorized_finger/sensor_value/Motor.h"
-#include <devices/ethercat/hand/soft_sensorized_finger/njoint_controller/Shape.h>
#include "../joint_controller/Pwm.h"
+#include <devices/ethercat/hand/soft_sensorized_finger/njoint_controller/Shape.h>
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
@@ -18,9 +18,9 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
class NJointShapeControllerConfig : virtual public armarx::NJointControllerConfig
{
public:
- NJointShapeControllerConfig(std::string const& deviceName):
- deviceName(deviceName)
- {}
+ NJointShapeControllerConfig(std::string const& deviceName) : deviceName(deviceName)
+ {
+ }
const std::string deviceName;
};
@@ -34,23 +34,22 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
public:
using ConfigPtrT = NJointShapeControllerConfigPtr;
- NJointShapeController(
- armarx::RobotUnitPtr prov,
- NJointShapeControllerConfigPtr config,
- const VirtualRobot::RobotPtr& r);
+ NJointShapeController(armarx::RobotUnitPtr prov,
+ NJointShapeControllerConfigPtr config,
+ const VirtualRobot::RobotPtr& r);
// NJointControllerInterface interface
public:
- std::string getClassName(const Ice::Current&) const override
+ std::string
+ getClassName(const Ice::Current&) const override
{
return "NJointKITSensorizedSoftFingerHandV1ShapeController";
}
void stopMotion();
- void readConfig(
- const armarx::RapidXmlReaderNode& configNode,
- armarx::DefaultRapidXmlReaderNode defaultConfigurationNod);
+ void readConfig(const armarx::RapidXmlReaderNode& configNode,
+ armarx::DefaultRapidXmlReaderNode defaultConfigurationNod);
int16_t getOtherPwm();
int16_t getIndexPwm();
@@ -66,31 +65,43 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
bool isControlEnabled() const;
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
//interface
public:
- armarx::WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
- void callDescribedFunction(const std::string& name, const armarx::StringVariantBaseMap& valueMap, const Ice::Current&) override;
-
- void setTargets(float lrm, float index, float thumb,
+ armarx::WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current&) const override;
+ void callDescribedFunction(const std::string& name,
+ const armarx::StringVariantBaseMap& valueMap,
+ const Ice::Current&) override;
+
+ void setTargets(float lrm,
+ float index,
+ float thumb,
const Ice::Current& = Ice::emptyCurrent) override;
- void setTargetsWithPwm(float lrm, float index, float thumb,
- float lrmRelativePWM, float indexRelativePWM, float thumbRelativePWM,
+ void setTargetsWithPwm(float lrm,
+ float index,
+ float thumb,
+ float lrmRelativePWM,
+ float indexRelativePWM,
+ float thumbRelativePWM,
const Ice::Current& = Ice::emptyCurrent) override;
JointValues getJointValues(const Ice::Current&) const override;
+
private:
struct finger_data
{
const MotorSensorValue* motor_sensor;
- armarx::ControlTarget1DoFActuatorPWM* pwm_target;
- std::atomic<float> joint_value;
- std::atomic<int16_t> last_pwm;
- int16_t pwm_limit = 0;
- float Kp = 0;
- float Ki = 0;
- float Kd = 0;
+ armarx::ControlTarget1DoFActuatorPWM* pwm_target;
+ std::atomic<float> joint_value;
+ std::atomic<int16_t> last_pwm;
+ int16_t pwm_limit = 0;
+ float Kp = 0;
+ float Ki = 0;
+ float Kd = 0;
};
+
finger_data _other_data;
finger_data _index_data;
finger_data _thumb_data;
@@ -99,19 +110,20 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
struct target_data
{
- int16_t max_pwm = 0;
- float target = 0;
+ int16_t max_pwm = 0;
+ float target = 0;
};
+
target_data index;
target_data thumb;
target_data other;
bool enable_control = false;
};
- mutable std::recursive_mutex _target_buf_write_mutex;
- armarx::WriteBufferedTripleBuffer<fingers_target_data> _target_buf;
- mutable std::recursive_mutex _last_target_buf_read_mutex;
- armarx::TripleBuffer<fingers_target_data> _last_target_buf;
+ mutable std::recursive_mutex _target_buf_write_mutex;
+ armarx::WriteBufferedTripleBuffer<fingers_target_data> _target_buf;
+ mutable std::recursive_mutex _last_target_buf_read_mutex;
+ armarx::TripleBuffer<fingers_target_data> _last_target_buf;
};
-} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/ShapeInterface.ice b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/ShapeInterface.ice
index f1b3eae3..2d5c3f2a 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/ShapeInterface.ice
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/ShapeInterface.ice
@@ -5,22 +5,27 @@
module armarx
{
module KITSensorizedSoftFingerHand
- {
- module V1
- {
- struct JointValues
- {
- float thumbJointValue;
- float indexJointValue;
- float otherJointValue;
- };
+ {
+ module V1
+ {
+ struct JointValues
+ {
+ float thumbJointValue;
+ float indexJointValue;
+ float otherJointValue;
+ };
- interface NJointShapeControllerInterface extends NJointControllerInterface
- {
- void setTargets(float lrm, float index, float thumb);
- void setTargetsWithPwm(float lrm, float index, float thumb, float lrmRelativePWM, float indexRelativePWM, float thumbRelativePWM);
- ["cpp:const"] JointValues getJointValues();
- };
- };
- };
+ interface NJointShapeControllerInterface extends NJointControllerInterface
+ {
+ void setTargets(float lrm, float index, float thumb);
+ void setTargetsWithPwm(float lrm,
+ float index,
+ float thumb,
+ float lrmRelativePWM,
+ float indexRelativePWM,
+ float thumbRelativePWM);
+ ["cpp:const"] JointValues getJointValues();
+ };
+ };
+ };
};
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/Accelerometer.h b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/Accelerometer.h
index ffe5a89e..6c4025d5 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/Accelerometer.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/Accelerometer.h
@@ -4,23 +4,21 @@
#include <cmath>
#include <complex>
-#include <unsupported/Eigen/FFT>
-
#include <devices/ethercat/hand/soft_sensorized_finger/utility/Sensors.h>
-
+#include <unsupported/Eigen/FFT>
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
- template<std::uint16_t N>
+ template <std::uint16_t N>
struct Accelerometer
{
std::array<float, N> x;
std::array<float, N> y;
std::array<float, N> xy;
- std::array < std::complex<float>, N + 1 > fft_tmp;
- std::array < float, N / 2 > fft_absolute_x;
- std::array < float, N / 2 > fft_absolute_y;
- std::array < float, N / 2 > fft_absolute_xy;
+ std::array<std::complex<float>, N + 1> fft_tmp;
+ std::array<float, N / 2> fft_absolute_x;
+ std::array<float, N / 2> fft_absolute_y;
+ std::array<float, N / 2> fft_absolute_xy;
double shear_force_criterium = 0.0f;
double fft_contact_detect_energy = 0.0f;
@@ -46,7 +44,8 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
}
}
- void update(const Sensors::Finger& finger)
+ void
+ update(const Sensors::Finger& finger)
{
ARMARX_TRACE;
static constexpr std::uint16_t buffer_size = N;
@@ -57,7 +56,7 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
std::copy(x.begin() + num_new, x.end(), x.begin());
std::copy(y.begin() + num_new, y.end(), y.begin());
std::copy(xy.begin() + num_new, xy.end(), xy.begin());
- for (std::size_t i = 0 ; i < num_new; ++i)
+ for (std::size_t i = 0; i < num_new; ++i)
{
x.at(num_old + i) = finger->raw_accelerometer_fifo.at(i).at(0);
y.at(num_old + i) = finger->raw_accelerometer_fifo.at(i).at(1);
@@ -66,7 +65,7 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
}
Eigen::FFT<float> fft;
fft.SetFlag(fft.HalfSpectrum);
- const auto update = [&](const auto & in, auto & out)
+ const auto update = [&](const auto& in, auto& out)
{
fft.fwd(fft_tmp.data(), in.data(), N);
for (std::size_t i = 0; i < out.size(); ++i)
@@ -74,8 +73,8 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
out.at(i) = std::abs(fft_tmp.at(i));
}
};
- update(x, fft_absolute_x);
- update(y, fft_absolute_y);
+ update(x, fft_absolute_x);
+ update(y, fft_absolute_y);
update(xy, fft_absolute_xy);
//calculate sum of buckets between 100 and 200hz
@@ -104,4 +103,4 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
}
}
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/Config.h b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/Config.h
index 7ac2d0d5..699676c7 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/Config.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/Config.h
@@ -3,19 +3,19 @@
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <devices/ethercat/hand/soft_sensorized_finger/utility/Identities.h>
#include <Armar6RT/interface/units/KITSensorizedSoftFingerHand/V1/NJointMinimalGraspingForceV1Controller.h>
+#include <devices/ethercat/hand/soft_sensorized_finger/utility/Identities.h>
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
- inline const auto& motor_cfg(const NJointMinimalGraspingForceV1ControllerConfig& cfg,
- identity::MotorIdentity id)
+ inline const auto&
+ motor_cfg(const NJointMinimalGraspingForceV1ControllerConfig& cfg, identity::MotorIdentity id)
{
- return identity::Select(
- id, cfg.motor_other, cfg.motor_index, cfg.motor_thumb);
+ return identity::Select(id, cfg.motor_other, cfg.motor_index, cfg.motor_thumb);
}
- inline void make_default_cfg(NJointMinimalGraspingForceV1ControllerConfig& c)
+ inline void
+ make_default_cfg(NJointMinimalGraspingForceV1ControllerConfig& c)
{
using T = NJointMinimalGraspingForceV1ControllerConfigElements::MotorCfg;
c.place_fft_energy_threshold = 40000;
@@ -28,9 +28,15 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
(c.motor_thumb.*m) = t;
};
const auto set_pid = [&](auto m,
- float op, float ip, float tp,
- float oi, float ii, float ti,
- float od, float id, float td)
+ float op,
+ float ip,
+ float tp,
+ float oi,
+ float ii,
+ float ti,
+ float od,
+ float id,
+ float td)
{
(c.motor_other.*m).p = op;
(c.motor_index.*m).p = ip;
@@ -44,41 +50,26 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
(c.motor_index.*m).d = id;
(c.motor_thumb.*m).d = td;
};
- set_pid(&T::pid_pos,
- 21.0f, 7.0f, 7.0f,
- 0.0f, 0.0f, 0.0f,
- 0.0f, 0.0f, 0.0f);
+ set_pid(&T::pid_pos, 21.0f, 7.0f, 7.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
- set_pid(&T::pid_pressure,
- 0.004f, 0.004f, 0.004f,
- 0.0f, 0.0f, 0.0f,
- 0, 0, 0);
+ set_pid(&T::pid_pressure, 0.004f, 0.004f, 0.004f, 0.0f, 0.0f, 0.0f, 0, 0, 0);
- set_pid(&T::pid_shear,
- 5000, 5000, 5000,
- 0, 0, 0,
- 0, 0, 0);
+ set_pid(&T::pid_shear, 5000, 5000, 5000, 0, 0, 0, 0, 0, 0);
- set_pid(&T::pid_unload,
- -15, -15, -15,
- -2, -2, -2,
- 0, 0, 0);
+ set_pid(&T::pid_unload, -15, -15, -15, -2, -2, -2, 0, 0, 0);
- set(&T::pid_force_forward_p, 0.4, 0.4, 0.4);
- set(&T::pid_force_limit, 75, 75, 75);
- set_pid(&T::pid_force,
- 1, 1, 1,
- 0, 0, 0,
- 0.2, 0.2, 0.2);
+ set(&T::pid_force_forward_p, 0.4, 0.4, 0.4);
+ set(&T::pid_force_limit, 75, 75, 75);
+ set_pid(&T::pid_force, 1, 1, 1, 0, 0, 0, 0.2, 0.2, 0.2);
- set(&T::max_pwm, 1'000, 1'000, 1'000);
- set(&T::pwm_open, -500, -500, -500);
- set(&T::pwm_freely_moving, 1'000, 1'000, 1'000);
- set(&T::min_pwm_close_to_contact, 300, 220, 220);
- set(&T::target_pressure, 120, 120, 120);
- set(&T::target_shear, 0.4, 0.4, 0.4);
- set(&T::encoder_delta_for_movent, 0.05, 0.05, 0.05);
- set(&T::force_ctrl_pressure_factor, 1.0f / 3.f, 1.0f / 3.f, 1);
+ set(&T::max_pwm, 1'000, 1'000, 1'000);
+ set(&T::pwm_open, -500, -500, -500);
+ set(&T::pwm_freely_moving, 1'000, 1'000, 1'000);
+ set(&T::min_pwm_close_to_contact, 300, 220, 220);
+ set(&T::target_pressure, 120, 120, 120);
+ set(&T::target_shear, 0.4, 0.4, 0.4);
+ set(&T::encoder_delta_for_movent, 0.05, 0.05, 0.05);
+ set(&T::force_ctrl_pressure_factor, 1.0f / 3.f, 1.0f / 3.f, 1);
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/FingerSensor.h b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/FingerSensor.h
index 9a01de02..2e986346 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/FingerSensor.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/FingerSensor.h
@@ -2,20 +2,19 @@
#include <array>
-#include <devices/ethercat/hand/soft_sensorized_finger/utility/Sensors.h>
-
+#include "Accelerometer.h"
#include "NormalForceSensor.h"
#include "ShearForceSensor.h"
-#include "Accelerometer.h"
+#include <devices/ethercat/hand/soft_sensorized_finger/utility/Sensors.h>
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
struct FingerSensor
{
std::array<NormalForceSensor, 4> normal_force_sensors;
- ShearForceSensor shear_force_dist_joint;
- ShearForceSensor shear_force_dist_tip;
- Accelerometer<64> accelerometer;
+ ShearForceSensor shear_force_dist_joint;
+ ShearForceSensor shear_force_dist_tip;
+ Accelerometer<64> accelerometer;
FingerSensor()
{
@@ -26,7 +25,8 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
}
}
- void reset_calibration()
+ void
+ reset_calibration()
{
for (auto& s : normal_force_sensors)
{
@@ -42,7 +42,8 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
}
}
- void update(const Sensors::Finger& finger, bool update_continuous)
+ void
+ update(const Sensors::Finger& finger, bool update_continuous)
{
ARMARX_TRACE;
normal_force_sensors.at(0).update(finger->raw_pressure_prox_l, update_continuous);
@@ -54,10 +55,10 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
shear_force_dist_tip.update(finger->raw_shear_force_dist_tip, update_continuous);
accelerometer.update(finger);
-
}
- bool test_for_object_touched() const
+ bool
+ test_for_object_touched() const
{
ARMARX_TRACE;
for (auto& fs : normal_force_sensors)
@@ -70,4 +71,4 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
return false;
}
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/GraspPhaseControllerData.h b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/GraspPhaseControllerData.h
index fb5c5233..7cd2faf9 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/GraspPhaseControllerData.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/GraspPhaseControllerData.h
@@ -11,9 +11,9 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
using config_t = NJointMinimalGraspingForceV1ControllerConfig;
using MotorIdentity = Sensors::MotorIdentity;
- template<class T>
+ template <class T>
using FingerArray = identity::FingerArray<T>;
- template<class T>
+ template <class T>
using MotorArray = identity::MotorArray<T>;
bool run_controller = false;
@@ -23,19 +23,21 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
float last_fpga_dt = 0;
bool fpga_update = false;
- GraspPhaseControllerData(FingerArray<FingerSensor>& fingers,
- const config_t& cfg):
- motor_controller_data
- {
- MotorGraspPhaseControllerData(MotorIdentity::other, cfg, &fingers.at(0), &fingers.at(1), &fingers.at(2)),
- MotorGraspPhaseControllerData(MotorIdentity::index, cfg, &fingers.at(3)),
- MotorGraspPhaseControllerData(MotorIdentity::thumb, cfg, &fingers.at(4))
- }
+ GraspPhaseControllerData(FingerArray<FingerSensor>& fingers, const config_t& cfg) :
+ motor_controller_data{
+ MotorGraspPhaseControllerData(MotorIdentity::other,
+ cfg,
+ &fingers.at(0),
+ &fingers.at(1),
+ &fingers.at(2)),
+ MotorGraspPhaseControllerData(MotorIdentity::index, cfg, &fingers.at(3)),
+ MotorGraspPhaseControllerData(MotorIdentity::thumb, cfg, &fingers.at(4))}
{
ARMARX_TRACE;
}
- void reset()
+ void
+ reset()
{
ARMARX_TRACE;
for (auto& data : motor_controller_data)
@@ -43,6 +45,5 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
data.reset();
}
}
-
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/MeanFreeSensor.h b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/MeanFreeSensor.h
index a03321ed..fd4b4b66 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/MeanFreeSensor.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/MeanFreeSensor.h
@@ -1,11 +1,11 @@
#pragma once
-#include <cstdint>
#include <array>
+#include <cstdint>
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
- template<class ValueT>
+ template <class ValueT>
struct MeanFreeValue
{
MeanFreeValue()
@@ -23,57 +23,60 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
float continuous_peak_to_peak_noise = 0;
float current_continuous_mean_free_scaled = 0;
- int64_t current = 0;
- int64_t mean_offset = 0;
- int64_t peak_to_peak_noise = 0;
- int64_t current_mean_free = 0;
+ int64_t current = 0;
+ int64_t mean_offset = 0;
+ int64_t peak_to_peak_noise = 0;
+ int64_t current_mean_free = 0;
//internal state
- int64_t accumulator = 0;
- int64_t noise_minimum = 0;
- int64_t noise_maximum = 0;
- int64_t number_of_updates = 0;
- float current_scaled = 0;
- float current_mean_free_scaled = 0;
+ int64_t accumulator = 0;
+ int64_t noise_minimum = 0;
+ int64_t noise_maximum = 0;
+ int64_t number_of_updates = 0;
+ float current_scaled = 0;
+ float current_mean_free_scaled = 0;
//do not reset
- int64_t one_percent = 0;
- float scaling = 1;
+ int64_t one_percent = 0;
+ float scaling = 1;
- void reset_calibration()
+ void
+ reset_calibration()
{
- current = 0;
- mean_offset = 0;
- peak_to_peak_noise = 0;
- current_mean_free = 0;
- accumulator = 0;
- noise_minimum = 0;
- noise_maximum = 0;
- number_of_updates = 0;
- current_scaled = 0;
+ current = 0;
+ mean_offset = 0;
+ peak_to_peak_noise = 0;
+ current_mean_free = 0;
+ accumulator = 0;
+ noise_minimum = 0;
+ noise_maximum = 0;
+ number_of_updates = 0;
+ current_scaled = 0;
current_mean_free_scaled = 0;
}
- void visit(auto f) const
+ void
+ visit(auto f) const
{
ARMARX_TRACE;
- f("current", 1e-5 + current);
- f("mean_offset", 1e-5 + mean_offset);
- f("peak_to_peak_noise", 1e-5 + peak_to_peak_noise);
- f("current_mean_free", 1e-5 + current_mean_free);
+ f("current", 1e-5 + current);
+ f("mean_offset", 1e-5 + mean_offset);
+ f("peak_to_peak_noise", 1e-5 + peak_to_peak_noise);
+ f("current_mean_free", 1e-5 + current_mean_free);
- f("continuous_mean", 1e-5 + continuous_mean);
- f("current_continuous_mean_free_scaled", 1e-5 + current_continuous_mean_free_scaled);
+ f("continuous_mean", 1e-5 + continuous_mean);
+ f("current_continuous_mean_free_scaled", 1e-5 + current_continuous_mean_free_scaled);
//f("accumulator", accumulator);
//f("noise_minimum", noise_minimum);
//f("noise_maximum", noise_maximum);
//f("number_of_updates", number_of_updates);
//
- f("scaling", 1e-5 + scaling);
+ f("scaling", 1e-5 + scaling);
//f("current_scaled", current_scaled);
f("current_mean_free_scaled", 1e-5 + current_mean_free_scaled);
}
- bool test_current_over_one_percent_plus_peak_noise() const
+ bool
+ test_current_over_one_percent_plus_peak_noise() const
{
ARMARX_TRACE;
//Offset is 1% of sensor range (effective or data sheet)
@@ -84,7 +87,8 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
return false;
}
- void update(ValueT value, bool update_continuous)
+ void
+ update(ValueT value, bool update_continuous)
{
const int64_t i64val = static_cast<int64_t>(value);
ARMARX_TRACE;
@@ -95,8 +99,10 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
if (update_continuous || number_of_updates <= NUM_AVERAGING_SAMPLES_NORMAL_FORCE_SENSOR)
{
- const auto idx = (++continuous_mean_index) % NUM_AVERAGING_SAMPLES_NORMAL_FORCE_SENSOR;
- continuous_mean += (-continuous_mean_buffer.at(idx) + current) / NUM_AVERAGING_SAMPLES_NORMAL_FORCE_SENSOR;
+ const auto idx =
+ (++continuous_mean_index) % NUM_AVERAGING_SAMPLES_NORMAL_FORCE_SENSOR;
+ continuous_mean += (-continuous_mean_buffer.at(idx) + current) /
+ NUM_AVERAGING_SAMPLES_NORMAL_FORCE_SENSOR;
continuous_mean_buffer.at(idx) = current;
float min = std::numeric_limits<float>::infinity();
float max = -min;
@@ -144,4 +150,4 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
}
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/MotorGraspPhaseControllerData.h b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/MotorGraspPhaseControllerData.h
index 2ab39e95..4e19b5a3 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/MotorGraspPhaseControllerData.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/MotorGraspPhaseControllerData.h
@@ -3,12 +3,11 @@
#include <boost/algorithm/clamp.hpp>
+#include "Config.h"
+#include "FingerSensor.h"
#include <devices/ethercat/hand/soft_sensorized_finger/Controllers/CloseAndHold.h>
-#include <devices/ethercat/hand/soft_sensorized_finger/utility/Sensors.h>
#include <devices/ethercat/hand/soft_sensorized_finger/utility/SensorFit.h>
-
-#include "FingerSensor.h"
-#include "Config.h"
+#include <devices/ethercat/hand/soft_sensorized_finger/utility/Sensors.h>
//declare
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
@@ -40,8 +39,8 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
PIDController pid_unload;
PIDController pid_force;
- float pid_force_limit = 42;
- float pid_force_forward_p = 42;
+ float pid_force_limit = 42;
+ float pid_force_forward_p = 42;
struct controller_state_data
{
@@ -56,18 +55,19 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
bool contact_detection_flag = false;
//bool slip_detection_flag = false;
GraspPhase grasp_phase = GraspPhase::FREELY_MOVING;
- bool finger_did_close = false;
+ bool finger_did_close = false;
- float monotonic_shear_pressure_control_last_pwm = 0;
+ float monotonic_shear_pressure_control_last_pwm = 0;
float monotonic_shear_pressure_control_last_pwm_pressure = 0;
- float monotonic_shear_pressure_control_last_pwm_shear = 0;
+ float monotonic_shear_pressure_control_last_pwm_shear = 0;
float monotonic_shear_pressure_control_last_delta_pressure = 0;
- float monotonic_shear_pressure_control_last_delta_shear = 0;
+ float monotonic_shear_pressure_control_last_delta_shear = 0;
float target_pressure_rate_of_change = 0;
float pressure_rate_of_change = 0;
float unload_phase_controller_normal_force_via_pwm_last_pwm = 0;
};
+
bool contact_detection_flag_noisy_last = false;
controller_state_data controller_state;
float last_encoder_pos = 0;
@@ -75,9 +75,7 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
void update_meassures(const Sensors& sensors, const controller_run_data& ctrl_run_data);
- MotorGraspPhaseControllerData(MotorIdentity m_id,
- const config_t& cfg,
- FingerSensor* ptr);
+ MotorGraspPhaseControllerData(MotorIdentity m_id, const config_t& cfg, FingerSensor* ptr);
MotorGraspPhaseControllerData(MotorIdentity m_id,
const config_t& cfg,
@@ -87,7 +85,7 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
std::int16_t pos_control_pwm(const controller_run_data& ctrl_run_data, float target_pos);
- template<std::size_t N>
+ template <std::size_t N>
struct AvgFilter
{
std::array<float, N> last_values;
@@ -100,7 +98,8 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
reset();
}
- void reset()
+ void
+ reset()
{
for (auto& v : last_values)
{
@@ -110,7 +109,9 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
avg = 0;
avg_previous = 0;
}
- float update(float v)
+
+ float
+ update(float v)
{
if (!std::isfinite(v))
{
@@ -124,29 +125,35 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
return avg;
}
};
+
AvgFilter<10> shear_avg_filter;
std::array<float, 5> last_pressure_values;
std::size_t last_pressure_values_i = 0;
- float unload_phase_controller_normal_force_via_pwm(
- const controller_run_data& ctrl_run_data);
+ float
+ unload_phase_controller_normal_force_via_pwm(const controller_run_data& ctrl_run_data);
+
public:
float force_ctrl_pressure_factor = 1;
- std::int16_t monotonic_shear_pressure_control_pwm_force(const controller_run_data& ctrl_run_data,
- const float target_pressure,
- const float target_shear, bool enable_shear);
+ std::int16_t
+ monotonic_shear_pressure_control_pwm_force(const controller_run_data& ctrl_run_data,
+ const float target_pressure,
+ const float target_shear,
+ bool enable_shear);
void reset();
float minimum_distance(const Sensors& sensors) const;
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
+
//define
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
inline void
- MotorGraspPhaseControllerData::update_meassures(const Sensors& sensors, const controller_run_data& ctrl_run_data)
+ MotorGraspPhaseControllerData::update_meassures(const Sensors& sensors,
+ const controller_run_data& ctrl_run_data)
{
ARMARX_TRACE;
//pressure
@@ -158,8 +165,9 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
for (int i = 2; i < 4; i++)
{
- controller_state.maximum_pressure = std::max(finger->normal_force_sensors.at(i).current_continuous_mean_free_scaled,
- controller_state.maximum_pressure);
+ controller_state.maximum_pressure = std::max(
+ finger->normal_force_sensors.at(i).current_continuous_mean_free_scaled,
+ controller_state.maximum_pressure);
}
}
}
@@ -176,13 +184,15 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
break;
}
}
- controller_state.contact_detection_flag = (contact_detection_flag_noisy_last && contact_detection_flag_noisy_curr);
+ controller_state.contact_detection_flag =
+ (contact_detection_flag_noisy_last && contact_detection_flag_noisy_curr);
contact_detection_flag_noisy_last = contact_detection_flag_noisy_curr;
}
//closing detection
{
const auto current_encoder_pos = sensors.motor(motor_identity)->position;
- controller_state.finger_did_close = (current_encoder_pos - last_encoder_pos) > encoder_delta_for_movent;
+ controller_state.finger_did_close =
+ (current_encoder_pos - last_encoder_pos) > encoder_delta_for_movent;
last_encoder_pos = current_encoder_pos;
}
//pressure rate of change
@@ -195,42 +205,42 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
avg_old += v / last_pressure_values.size();
}
const auto i = (last_pressure_values_i++ % last_pressure_values.size());
- const float avg_new = avg_old - last_pressure_values.at(i) / last_pressure_values.size() + pval / last_pressure_values.size();
+ const float avg_new = avg_old -
+ last_pressure_values.at(i) / last_pressure_values.size() +
+ pval / last_pressure_values.size();
last_pressure_values.at(i) = pval;
- controller_state.pressure_rate_of_change = (avg_new - avg_old) / 0.007f; //ctrl_run_data.dt_sec_fpga;
+ controller_state.pressure_rate_of_change =
+ (avg_new - avg_old) / 0.007f; //ctrl_run_data.dt_sec_fpga;
}
}
- inline
- MotorGraspPhaseControllerData::MotorGraspPhaseControllerData(MotorIdentity m_id,
- const config_t& cfg,
- FingerSensor* ptr) :
+ inline MotorGraspPhaseControllerData::MotorGraspPhaseControllerData(MotorIdentity m_id,
+ const config_t& cfg,
+ FingerSensor* ptr) :
MotorGraspPhaseControllerData(m_id, cfg, ptr, nullptr, nullptr)
- {}
+ {
+ }
- inline
- MotorGraspPhaseControllerData::MotorGraspPhaseControllerData(MotorIdentity m_id,
- const config_t& cfg,
- FingerSensor* ptr1, FingerSensor* ptr2, FingerSensor* ptr3) :
+ inline MotorGraspPhaseControllerData::MotorGraspPhaseControllerData(MotorIdentity m_id,
+ const config_t& cfg,
+ FingerSensor* ptr1,
+ FingerSensor* ptr2,
+ FingerSensor* ptr3) :
motor_identity{m_id},
fingers{ptr1, ptr2, ptr3},
- pos_pwm_controller(
- motor_cfg(cfg, m_id).pid_pos.p,
- motor_cfg(cfg, m_id).pid_pos.i,
- motor_cfg(cfg, m_id).pid_pos.d),
+ pos_pwm_controller(motor_cfg(cfg, m_id).pid_pos.p,
+ motor_cfg(cfg, m_id).pid_pos.i,
+ motor_cfg(cfg, m_id).pid_pos.d),
max_pwm{motor_cfg(cfg, m_id).max_pwm},
- pid_shear(
- motor_cfg(cfg, m_id).pid_shear.p,
- motor_cfg(cfg, m_id).pid_shear.i,
- motor_cfg(cfg, m_id).pid_shear.d),
- pid_unload(
- motor_cfg(cfg, m_id).pid_unload.p,
- motor_cfg(cfg, m_id).pid_unload.i,
- motor_cfg(cfg, m_id).pid_unload.d),
- pid_force(
- motor_cfg(cfg, m_id).pid_force.p,
- motor_cfg(cfg, m_id).pid_force.i,
- motor_cfg(cfg, m_id).pid_force.d),
+ pid_shear(motor_cfg(cfg, m_id).pid_shear.p,
+ motor_cfg(cfg, m_id).pid_shear.i,
+ motor_cfg(cfg, m_id).pid_shear.d),
+ pid_unload(motor_cfg(cfg, m_id).pid_unload.p,
+ motor_cfg(cfg, m_id).pid_unload.i,
+ motor_cfg(cfg, m_id).pid_unload.d),
+ pid_force(motor_cfg(cfg, m_id).pid_force.p,
+ motor_cfg(cfg, m_id).pid_force.i,
+ motor_cfg(cfg, m_id).pid_force.d),
force_ctrl_pressure_factor{motor_cfg(cfg, m_id).force_ctrl_pressure_factor}
{
ARMARX_TRACE;
@@ -240,18 +250,14 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
}
inline std::int16_t
- MotorGraspPhaseControllerData::pos_control_pwm(
- const controller_run_data& ctrl_run_data,
- float target_pos
- )
+ MotorGraspPhaseControllerData::pos_control_pwm(const controller_run_data& ctrl_run_data,
+ float target_pos)
{
ARMARX_TRACE;
- return pos_pwm_controller.calculate(
- ctrl_run_data.sensors.motor(motor_identity)->position,
- target_pos,
- ctrl_run_data.dt_sec,
- max_pwm
- );
+ return pos_pwm_controller.calculate(ctrl_run_data.sensors.motor(motor_identity)->position,
+ target_pos,
+ ctrl_run_data.dt_sec,
+ max_pwm);
}
inline float
@@ -260,14 +266,15 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
if (ctrl_run_data.fpga_updated)
{
- pid_unload.update(
- ctrl_run_data.dt_sec_fpga,
- controller_state.pressure_rate_of_change,
- controller_state.target_pressure_rate_of_change);
+ pid_unload.update(ctrl_run_data.dt_sec_fpga,
+ controller_state.pressure_rate_of_change,
+ controller_state.target_pressure_rate_of_change);
- controller_state.unload_phase_controller_normal_force_via_pwm_last_pwm = pid_unload.getControlValue();
+ controller_state.unload_phase_controller_normal_force_via_pwm_last_pwm =
+ pid_unload.getControlValue();
}
- return std::min(0.f, controller_state.unload_phase_controller_normal_force_via_pwm_last_pwm)/* - 40*/;
+ return std::min(
+ 0.f, controller_state.unload_phase_controller_normal_force_via_pwm_last_pwm) /* - 40*/;
}
inline std::int16_t
@@ -275,30 +282,23 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
const controller_run_data& ctrl_run_data,
const float targ_pressure,
const float targ_shear,
- bool enable_shear
- )
+ bool enable_shear)
{
ARMARX_TRACE;
if (ctrl_run_data.fpga_updated)
{
controller_state.monotonic_shear_pressure_control_last_pwm_pressure = 0;
- controller_state.monotonic_shear_pressure_control_last_pwm_shear = 0;
+ controller_state.monotonic_shear_pressure_control_last_pwm_shear = 0;
controller_state.monotonic_shear_pressure_control_last_delta_pressure = 0;
- controller_state.monotonic_shear_pressure_control_last_delta_shear = 0;
+ controller_state.monotonic_shear_pressure_control_last_delta_shear = 0;
//normal force
{
controller_state.target_pressure = targ_pressure;
static const auto pos_hold_pwm = [](float pos)
{
- piecewise_linear_function<float, float> pwlf
- {
- std::map<float, float>{
- {0.00f, 140.f},
- {0.71f, 175.f},
- {0.94f, 236.f}
- }
- };
+ piecewise_linear_function<float, float> pwlf{
+ std::map<float, float>{{0.00f, 140.f}, {0.71f, 175.f}, {0.94f, 236.f}}};
return pwlf.calc_lin_search(pos);
};
@@ -306,23 +306,24 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
if (std::isfinite(controller_state.shear_force_used_ratio) && enable_shear)
{
controller_state.target_shear = targ_shear;
- pid_shear.update(
- ctrl_run_data.dt_sec_fpga,
- controller_state.shear_force_used_ratio,
- controller_state.target_shear);
- controller_state.monotonic_shear_pressure_control_last_delta_shear = pid_shear.getControlValue();
- controller_state.target_pressure += std::max(0.f, controller_state.monotonic_shear_pressure_control_last_delta_shear);
+ pid_shear.update(ctrl_run_data.dt_sec_fpga,
+ controller_state.shear_force_used_ratio,
+ controller_state.target_shear);
+ controller_state.monotonic_shear_pressure_control_last_delta_shear =
+ pid_shear.getControlValue();
+ controller_state.target_pressure += std::max(
+ 0.f, controller_state.monotonic_shear_pressure_control_last_delta_shear);
}
controller_state.target_pressure *= force_ctrl_pressure_factor;
- pid_force.update(
- ctrl_run_data.dt_sec_fpga,
- static_cast<float>(controller_state.maximum_pressure),
- controller_state.target_pressure);
+ pid_force.update(ctrl_run_data.dt_sec_fpga,
+ static_cast<float>(controller_state.maximum_pressure),
+ controller_state.target_pressure);
controller_state.monotonic_shear_pressure_control_last_delta_pressure =
- boost::algorithm::clamp(pid_force.getControlValue(), -pid_force_limit, pid_force_limit);
+ boost::algorithm::clamp(
+ pid_force.getControlValue(), -pid_force_limit, pid_force_limit);
controller_state.monotonic_shear_pressure_control_last_pwm_pressure =
controller_state.monotonic_shear_pressure_control_last_delta_pressure +
pid_force_forward_p * controller_state.target_pressure +
@@ -334,6 +335,7 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
}
return controller_state.monotonic_shear_pressure_control_last_pwm;
}
+
inline void
MotorGraspPhaseControllerData::reset()
{
@@ -344,9 +346,9 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
pid_unload.reset();
pid_force.reset();
- controller_state.monotonic_shear_pressure_control_last_pwm = 0;
+ controller_state.monotonic_shear_pressure_control_last_pwm = 0;
controller_state.monotonic_shear_pressure_control_last_pwm_pressure = 0;
- controller_state.monotonic_shear_pressure_control_last_pwm_shear = 0;
+ controller_state.monotonic_shear_pressure_control_last_pwm_shear = 0;
for (auto& v : last_pressure_values)
{
v = 0;
@@ -370,4 +372,4 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
ARMARX_CHECK_EXPRESSION(false) << "Unreachable code reached!";
}
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/ShearForceSensor.h b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/ShearForceSensor.h
index e47588a2..c63bd553 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/ShearForceSensor.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/njoint_controller/minimal_grasping_force_v1/ShearForceSensor.h
@@ -3,9 +3,8 @@
#include <array>
#include <cmath>
-#include <devices/ethercat/hand/soft_sensorized_finger/utility/Sensors.h>
-
#include "MeanFreeSensor.h"
+#include <devices/ethercat/hand/soft_sensorized_finger/utility/Sensors.h>
namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
{
@@ -23,7 +22,8 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
sensors.at(2).scaling = 1.21f;
}
- void update(const std::array<std::int16_t, 3>& raw_shear_force, bool update_continuous)
+ void
+ update(const std::array<std::int16_t, 3>& raw_shear_force, bool update_continuous)
{
ARMARX_TRACE;
for (std::size_t i = 0; i < 3; ++i)
@@ -35,4 +35,4 @@ namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
len_z = std::abs(sensors.at(2).current_mean_free_scaled);
}
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::njoint_controller
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/sensor_value/Finger.h b/source/devices/ethercat/hand/soft_sensorized_finger/sensor_value/Finger.h
index 8dcb626f..1a9cbaf1 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/sensor_value/Finger.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/sensor_value/Finger.h
@@ -9,108 +9,111 @@ namespace devices::ethercat::hand::soft_sensorized_finger
public:
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- std::uint32_t frame_counter;
-
- Eigen::Vector3f joint_encoder_prox;
- float joint_encoder_prox_temp;
- std::array<std::int16_t, 3> raw_joint_encoder_prox;
- std::uint16_t raw_joint_encoder_prox_temp;
-
- Eigen::Vector3f joint_encoder_dist;
- float joint_encoder_dist_temp;
- std::array<std::int16_t, 3> raw_joint_encoder_dist;
- std::uint16_t raw_joint_encoder_dist_temp;
-
- float pressure_prox_l;
- float pressure_prox_l_temp;
- std::uint32_t raw_pressure_prox_l;
- std::uint16_t raw_pressure_prox_l_temp;
-
- float pressure_prox_r;
- float pressure_prox_r_temp;
- std::uint32_t raw_pressure_prox_r;
- std::uint16_t raw_pressure_prox_r_temp;
-
- float pressure_dist_joint;
- float pressure_dist_joint_temp;
- std::uint32_t raw_pressure_dist_joint;
- std::uint16_t raw_pressure_dist_joint_temp;
-
- float pressure_dist_tip;
- float pressure_dist_tip_temp;
- std::uint32_t raw_pressure_dist_tip;
- std::uint16_t raw_pressure_dist_tip_temp;
-
- Eigen::Vector3f shear_force_dist_joint;
- float shear_force_dist_joint_temp;
- std::array<std::int16_t, 3> raw_shear_force_dist_joint;
- std::uint16_t raw_shear_force_dist_joint_temp;
-
- Eigen::Vector3f shear_force_dist_tip;
- float shear_force_dist_tip_temp;
- std::array<std::int16_t, 3> raw_shear_force_dist_tip;
- std::uint16_t raw_shear_force_dist_tip_temp;
-
- float proximity;
- std::uint16_t raw_proximity;
-
- std::array<Eigen::Vector3f, 17> accelerometer_fifo;
+ std::uint32_t frame_counter;
+
+ Eigen::Vector3f joint_encoder_prox;
+ float joint_encoder_prox_temp;
+ std::array<std::int16_t, 3> raw_joint_encoder_prox;
+ std::uint16_t raw_joint_encoder_prox_temp;
+
+ Eigen::Vector3f joint_encoder_dist;
+ float joint_encoder_dist_temp;
+ std::array<std::int16_t, 3> raw_joint_encoder_dist;
+ std::uint16_t raw_joint_encoder_dist_temp;
+
+ float pressure_prox_l;
+ float pressure_prox_l_temp;
+ std::uint32_t raw_pressure_prox_l;
+ std::uint16_t raw_pressure_prox_l_temp;
+
+ float pressure_prox_r;
+ float pressure_prox_r_temp;
+ std::uint32_t raw_pressure_prox_r;
+ std::uint16_t raw_pressure_prox_r_temp;
+
+ float pressure_dist_joint;
+ float pressure_dist_joint_temp;
+ std::uint32_t raw_pressure_dist_joint;
+ std::uint16_t raw_pressure_dist_joint_temp;
+
+ float pressure_dist_tip;
+ float pressure_dist_tip_temp;
+ std::uint32_t raw_pressure_dist_tip;
+ std::uint16_t raw_pressure_dist_tip_temp;
+
+ Eigen::Vector3f shear_force_dist_joint;
+ float shear_force_dist_joint_temp;
+ std::array<std::int16_t, 3> raw_shear_force_dist_joint;
+ std::uint16_t raw_shear_force_dist_joint_temp;
+
+ Eigen::Vector3f shear_force_dist_tip;
+ float shear_force_dist_tip_temp;
+ std::array<std::int16_t, 3> raw_shear_force_dist_tip;
+ std::uint16_t raw_shear_force_dist_tip_temp;
+
+ float proximity;
+ std::uint16_t raw_proximity;
+
+ std::array<Eigen::Vector3f, 17> accelerometer_fifo;
std::array<std::array<std::int16_t, 3>, 17> raw_accelerometer_fifo;
- std::uint16_t accelerometer_fifo_length;
+ std::uint16_t accelerometer_fifo_length;
- static SensorValueInfo<FingerSensorValue> GetClassMemberInfo()
+ static SensorValueInfo<FingerSensorValue>
+ GetClassMemberInfo()
{
using T = FingerSensorValue;
SensorValueInfo<T> svi;
- svi.addMemberVariable(&T::frame_counter, "frame_counter");
-
- svi.addMemberVariable(&T::joint_encoder_prox, "joint_encoder_prox");
- svi.addMemberVariable(&T::joint_encoder_prox_temp, "joint_encoder_prox_temp");
- svi.addMemberVariable(&T::raw_joint_encoder_prox, "raw_joint_encoder_prox");
- svi.addMemberVariable(&T::raw_joint_encoder_prox_temp, "raw_joint_encoder_prox_temp");
-
- svi.addMemberVariable(&T::joint_encoder_dist, "joint_encoder_dist");
- svi.addMemberVariable(&T::joint_encoder_dist_temp, "joint_encoder_dist_temp");
- svi.addMemberVariable(&T::raw_joint_encoder_dist, "raw_joint_encoder_dist");
- svi.addMemberVariable(&T::raw_joint_encoder_dist_temp, "raw_joint_encoder_dist_temp");
-
- svi.addMemberVariable(&T::pressure_prox_l, "pressure_prox_l");
- svi.addMemberVariable(&T::pressure_prox_l_temp, "pressure_prox_l_temp");
- svi.addMemberVariable(&T::raw_pressure_prox_l, "raw_pressure_prox_l");
- svi.addMemberVariable(&T::raw_pressure_prox_l_temp, "raw_pressure_prox_l_temp");
-
- svi.addMemberVariable(&T::pressure_prox_r, "pressure_prox_r");
- svi.addMemberVariable(&T::pressure_prox_r_temp, "pressure_prox_r_temp");
- svi.addMemberVariable(&T::raw_pressure_prox_r, "raw_pressure_prox_r");
- svi.addMemberVariable(&T::raw_pressure_prox_r_temp, "raw_pressure_prox_r_temp");
-
- svi.addMemberVariable(&T::pressure_dist_joint, "pressure_dist_joint");
- svi.addMemberVariable(&T::pressure_dist_joint_temp, "pressure_dist_joint_temp");
- svi.addMemberVariable(&T::raw_pressure_dist_joint, "raw_pressure_dist_joint");
- svi.addMemberVariable(&T::raw_pressure_dist_joint_temp, "raw_pressure_dist_joint_temp");
-
- svi.addMemberVariable(&T::pressure_dist_tip, "pressure_dist_tip");
- svi.addMemberVariable(&T::pressure_dist_tip_temp, "pressure_dist_tip_temp");
- svi.addMemberVariable(&T::raw_pressure_dist_tip, "raw_pressure_dist_tip");
- svi.addMemberVariable(&T::raw_pressure_dist_tip_temp, "raw_pressure_dist_tip_temp");
-
- svi.addMemberVariable(&T::shear_force_dist_joint, "shear_force_dist_joint");
- svi.addMemberVariable(&T::shear_force_dist_joint_temp, "shear_force_dist_joint_temp");
- svi.addMemberVariable(&T::raw_shear_force_dist_joint, "raw_shear_force_dist_joint");
- svi.addMemberVariable(&T::raw_shear_force_dist_joint_temp, "raw_shear_force_dist_joint_temp");
-
- svi.addMemberVariable(&T::shear_force_dist_tip, "shear_force_dist_tip");
- svi.addMemberVariable(&T::shear_force_dist_tip_temp, "shear_force_dist_tip_temp");
- svi.addMemberVariable(&T::raw_shear_force_dist_tip, "raw_shear_force_dist_tip");
- svi.addMemberVariable(&T::raw_shear_force_dist_tip_temp, "raw_shear_force_dist_tip_temp");
-
- svi.addMemberVariable(&T::proximity, "proximity");
- svi.addMemberVariable(&T::raw_proximity, "raw_proximity");
-
- svi.addMemberVariable(&T::accelerometer_fifo, "accelerometer_fifo");
- svi.addMemberVariable(&T::raw_accelerometer_fifo, "raw_accelerometer_fifo");
- svi.addMemberVariable(&T::accelerometer_fifo_length, "accelerometer_fifo_length");
+ svi.addMemberVariable(&T::frame_counter, "frame_counter");
+
+ svi.addMemberVariable(&T::joint_encoder_prox, "joint_encoder_prox");
+ svi.addMemberVariable(&T::joint_encoder_prox_temp, "joint_encoder_prox_temp");
+ svi.addMemberVariable(&T::raw_joint_encoder_prox, "raw_joint_encoder_prox");
+ svi.addMemberVariable(&T::raw_joint_encoder_prox_temp, "raw_joint_encoder_prox_temp");
+
+ svi.addMemberVariable(&T::joint_encoder_dist, "joint_encoder_dist");
+ svi.addMemberVariable(&T::joint_encoder_dist_temp, "joint_encoder_dist_temp");
+ svi.addMemberVariable(&T::raw_joint_encoder_dist, "raw_joint_encoder_dist");
+ svi.addMemberVariable(&T::raw_joint_encoder_dist_temp, "raw_joint_encoder_dist_temp");
+
+ svi.addMemberVariable(&T::pressure_prox_l, "pressure_prox_l");
+ svi.addMemberVariable(&T::pressure_prox_l_temp, "pressure_prox_l_temp");
+ svi.addMemberVariable(&T::raw_pressure_prox_l, "raw_pressure_prox_l");
+ svi.addMemberVariable(&T::raw_pressure_prox_l_temp, "raw_pressure_prox_l_temp");
+
+ svi.addMemberVariable(&T::pressure_prox_r, "pressure_prox_r");
+ svi.addMemberVariable(&T::pressure_prox_r_temp, "pressure_prox_r_temp");
+ svi.addMemberVariable(&T::raw_pressure_prox_r, "raw_pressure_prox_r");
+ svi.addMemberVariable(&T::raw_pressure_prox_r_temp, "raw_pressure_prox_r_temp");
+
+ svi.addMemberVariable(&T::pressure_dist_joint, "pressure_dist_joint");
+ svi.addMemberVariable(&T::pressure_dist_joint_temp, "pressure_dist_joint_temp");
+ svi.addMemberVariable(&T::raw_pressure_dist_joint, "raw_pressure_dist_joint");
+ svi.addMemberVariable(&T::raw_pressure_dist_joint_temp, "raw_pressure_dist_joint_temp");
+
+ svi.addMemberVariable(&T::pressure_dist_tip, "pressure_dist_tip");
+ svi.addMemberVariable(&T::pressure_dist_tip_temp, "pressure_dist_tip_temp");
+ svi.addMemberVariable(&T::raw_pressure_dist_tip, "raw_pressure_dist_tip");
+ svi.addMemberVariable(&T::raw_pressure_dist_tip_temp, "raw_pressure_dist_tip_temp");
+
+ svi.addMemberVariable(&T::shear_force_dist_joint, "shear_force_dist_joint");
+ svi.addMemberVariable(&T::shear_force_dist_joint_temp, "shear_force_dist_joint_temp");
+ svi.addMemberVariable(&T::raw_shear_force_dist_joint, "raw_shear_force_dist_joint");
+ svi.addMemberVariable(&T::raw_shear_force_dist_joint_temp,
+ "raw_shear_force_dist_joint_temp");
+
+ svi.addMemberVariable(&T::shear_force_dist_tip, "shear_force_dist_tip");
+ svi.addMemberVariable(&T::shear_force_dist_tip_temp, "shear_force_dist_tip_temp");
+ svi.addMemberVariable(&T::raw_shear_force_dist_tip, "raw_shear_force_dist_tip");
+ svi.addMemberVariable(&T::raw_shear_force_dist_tip_temp,
+ "raw_shear_force_dist_tip_temp");
+
+ svi.addMemberVariable(&T::proximity, "proximity");
+ svi.addMemberVariable(&T::raw_proximity, "raw_proximity");
+
+ svi.addMemberVariable(&T::accelerometer_fifo, "accelerometer_fifo");
+ svi.addMemberVariable(&T::raw_accelerometer_fifo, "raw_accelerometer_fifo");
+ svi.addMemberVariable(&T::accelerometer_fifo_length, "accelerometer_fifo_length");
return svi;
}
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/sensor_value/Hand.h b/source/devices/ethercat/hand/soft_sensorized_finger/sensor_value/Hand.h
index 80830186..392a63a5 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/sensor_value/Hand.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/sensor_value/Hand.h
@@ -9,25 +9,26 @@ namespace devices::ethercat::hand::soft_sensorized_finger
public:
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- float temperature;
+ float temperature;
std::uint32_t raw_temperature;
std::uint32_t frame_counter;
std::uint16_t update_rate_main;
std::uint16_t update_rate_fpga;
- float relative_position;
- float velocity;
+ float relative_position;
+ float velocity;
- static SensorValueInfo<HandSensorValue> GetClassMemberInfo()
+ static SensorValueInfo<HandSensorValue>
+ GetClassMemberInfo()
{
using T = HandSensorValue;
SensorValueInfo<T> svi;
- svi.addMemberVariable(&T::temperature, "temperature");
- svi.addMemberVariable(&T::raw_temperature, "raw_temperature");
- svi.addMemberVariable(&T::frame_counter, "frame_counter");
+ svi.addMemberVariable(&T::temperature, "temperature");
+ svi.addMemberVariable(&T::raw_temperature, "raw_temperature");
+ svi.addMemberVariable(&T::frame_counter, "frame_counter");
svi.addMemberVariable(&T::update_rate_main, "update_rate_main");
svi.addMemberVariable(&T::update_rate_fpga, "update_rate_fpga");
return svi;
}
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/sensor_value/Motor.h b/source/devices/ethercat/hand/soft_sensorized_finger/sensor_value/Motor.h
index b3af7ab7..15c2937e 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/sensor_value/Motor.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/sensor_value/Motor.h
@@ -12,21 +12,22 @@ namespace devices::ethercat::hand::soft_sensorized_finger
public:
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- std::uint32_t frame_counter;
- std::int32_t raw_position;
- std::int32_t raw_velocity;
+ std::uint32_t frame_counter;
+ std::int32_t raw_position;
+ std::int32_t raw_velocity;
std::uint32_t pwm_max;
- static SensorValueInfo<MotorSensorValue> GetClassMemberInfo()
+ static SensorValueInfo<MotorSensorValue>
+ GetClassMemberInfo()
{
using T = MotorSensorValue;
SensorValueInfo<T> svi;
- svi.addMemberVariable(&T::frame_counter, "frame_counter");
+ svi.addMemberVariable(&T::frame_counter, "frame_counter");
- svi.addMemberVariable(&T::raw_position, "raw_position");
- svi.addMemberVariable(&T::raw_velocity, "raw_velocity");
+ svi.addMemberVariable(&T::raw_position, "raw_position");
+ svi.addMemberVariable(&T::raw_velocity, "raw_velocity");
- svi.addMemberVariable(&T::pwm_max, "motor_pwm_max");
+ svi.addMemberVariable(&T::pwm_max, "motor_pwm_max");
svi.addBaseClass<SensorValue1DoFActuatorPosition>();
svi.addBaseClass<SensorValue1DoFActuatorVelocity>();
@@ -34,4 +35,4 @@ namespace devices::ethercat::hand::soft_sensorized_finger
return svi;
}
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/utility/Identities.h b/source/devices/ethercat/hand/soft_sensorized_finger/utility/Identities.h
index c807e0c4..fd874fa2 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/utility/Identities.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/utility/Identities.h
@@ -32,11 +32,10 @@ namespace devices::ethercat::hand::soft_sensorized_finger::identity
other = 1,
thumb = 2
};
- template<class T>
- static decltype(auto) Select(MotorIdentity id,
- T& other,
- T& index,
- T& thumb)
+
+ template <class T>
+ static decltype(auto)
+ Select(MotorIdentity id, T& other, T& index, T& thumb)
{
switch (id)
{
@@ -52,18 +51,15 @@ namespace devices::ethercat::hand::soft_sensorized_finger::identity
enum class FingerIdentity : std::size_t
{
little = 0,
- ring = 1,
+ ring = 1,
middle = 2,
- index = 3,
- thumb = 4
+ index = 3,
+ thumb = 4
};
- template<class T>
- static decltype(auto) Select(FingerIdentity id,
- T& little,
- T& ring,
- T& middle,
- T& index,
- T& thumb)
+
+ template <class T>
+ static decltype(auto)
+ Select(FingerIdentity id, T& little, T& ring, T& middle, T& index, T& thumb)
{
switch (id)
{
@@ -81,7 +77,7 @@ namespace devices::ethercat::hand::soft_sensorized_finger::identity
ARMARX_CHECK_EXPRESSION(!"unreachable code!");
}
- template<class ET, class VT, std::size_t N>
+ template <class ET, class VT, std::size_t N>
class EnumIndexedArray : public std::array<VT, N>
{
public:
@@ -90,60 +86,140 @@ namespace devices::ethercat::hand::soft_sensorized_finger::identity
using std::array<VT, N>::at;
- value_t& at(enum_t id)
+ value_t&
+ at(enum_t id)
{
return at(static_cast<std::size_t>(id));
}
- const value_t& at(enum_t id) const
+
+ const value_t&
+ at(enum_t id) const
{
return at(static_cast<std::size_t>(id));
}
- const value_t& operator()(enum_t id) const
+ const value_t&
+ operator()(enum_t id) const
{
return at(id);
}
- value_t& operator()(enum_t id)
+
+ value_t&
+ operator()(enum_t id)
{
return at(id);
}
};
- template<class T>
+ template <class T>
class FingerArray : public EnumIndexedArray<FingerIdentity, T, 5>
{
public:
// *INDENT-OFF*
- T& little() { return this->at(FingerIdentity::little); }
- const T& little() const { return this->at(FingerIdentity::little); }
+ T&
+ little()
+ {
+ return this->at(FingerIdentity::little);
+ }
+
+ const T&
+ little() const
+ {
+ return this->at(FingerIdentity::little);
+ }
+
+ T&
+ ring()
+ {
+ return this->at(FingerIdentity::ring);
+ }
+
+ const T&
+ ring() const
+ {
+ return this->at(FingerIdentity::ring);
+ }
+
+ T&
+ middle()
+ {
+ return this->at(FingerIdentity::middle);
+ }
+
+ const T&
+ middle() const
+ {
+ return this->at(FingerIdentity::middle);
+ }
+
+ T&
+ index()
+ {
+ return this->at(FingerIdentity::index);
+ }
- T& ring () { return this->at(FingerIdentity::ring ); }
- const T& ring () const { return this->at(FingerIdentity::ring ); }
+ const T&
+ index() const
+ {
+ return this->at(FingerIdentity::index);
+ }
- T& middle() { return this->at(FingerIdentity::middle); }
- const T& middle() const { return this->at(FingerIdentity::middle); }
+ T&
+ thumb()
+ {
+ return this->at(FingerIdentity::thumb);
+ }
- T& index () { return this->at(FingerIdentity::index ); }
- const T& index () const { return this->at(FingerIdentity::index ); }
+ const T&
+ thumb() const
+ {
+ return this->at(FingerIdentity::thumb);
+ }
- T& thumb () { return this->at(FingerIdentity::thumb ); }
- const T& thumb () const { return this->at(FingerIdentity::thumb ); }
// *INDENT-ON*
};
- template<class T>
+ template <class T>
class MotorArray : public EnumIndexedArray<MotorIdentity, T, 3>
{
public:
// *INDENT-OFF*
- T& other() { return this->at(MotorIdentity::other); }
- const T& other() const { return this->at(MotorIdentity::other); }
+ T&
+ other()
+ {
+ return this->at(MotorIdentity::other);
+ }
+
+ const T&
+ other() const
+ {
+ return this->at(MotorIdentity::other);
+ }
- T& index() { return this->at(MotorIdentity::index); }
- const T& index() const { return this->at(MotorIdentity::index); }
+ T&
+ index()
+ {
+ return this->at(MotorIdentity::index);
+ }
+
+ const T&
+ index() const
+ {
+ return this->at(MotorIdentity::index);
+ }
+
+ T&
+ thumb()
+ {
+ return this->at(MotorIdentity::thumb);
+ }
+
+ const T&
+ thumb() const
+ {
+ return this->at(MotorIdentity::thumb);
+ }
- T& thumb() { return this->at(MotorIdentity::thumb); }
- const T& thumb() const { return this->at(MotorIdentity::thumb); }
// *INDENT-ON*
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger::identity
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/utility/SensorFit.cpp b/source/devices/ethercat/hand/soft_sensorized_finger/utility/SensorFit.cpp
index ac383591..6a2fada8 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/utility/SensorFit.cpp
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/utility/SensorFit.cpp
@@ -8,236 +8,142 @@ namespace Eigen
namespace devices::ethercat::hand::soft_sensorized_finger
{
using JENC = SensorFit::JointEncoder;
- Eigen::Vector3f JENC::thumb_prox_fit_joint_angle_data_1_poly(
- float x, float y, float z)
+
+ Eigen::Vector3f
+ JENC::thumb_prox_fit_joint_angle_data_1_poly(float x, float y, float z)
{
- static const Eigen::Vector4d cx
- {
- -3.0149985997869457e+001,
- +3.0128853346157572e-002,
- -3.2319616750818926e-006,
- +1.1794986395837135e-010
- };
- static const Eigen::Vector4d cy
- {
- -3.2882006618854462e+001,
- -3.7483225185537128e-001,
- -4.9745054331382045e-004,
- -2.2910312387766286e-007
- };
- static const Eigen::Vector4d cz
- {
- -1.8987002102600119e+001,
- -3.4080416074681805e-002,
- -4.6260163969121740e-006,
- -2.1003723936766469e-010
- };
- return
- {
- calc_poly(cx, x),
- calc_poly(cy, y),
- calc_poly(cz, z)
- };
+ static const Eigen::Vector4d cx{-3.0149985997869457e+001,
+ +3.0128853346157572e-002,
+ -3.2319616750818926e-006,
+ +1.1794986395837135e-010};
+ static const Eigen::Vector4d cy{-3.2882006618854462e+001,
+ -3.7483225185537128e-001,
+ -4.9745054331382045e-004,
+ -2.2910312387766286e-007};
+ static const Eigen::Vector4d cz{-1.8987002102600119e+001,
+ -3.4080416074681805e-002,
+ -4.6260163969121740e-006,
+ -2.1003723936766469e-010};
+ return {calc_poly(cx, x), calc_poly(cy, y), calc_poly(cz, z)};
}
- Eigen::Vector3f JENC::thumb_prox_fit_joint_angle_data_2_poly(
- float x, float y, float z)
+ Eigen::Vector3f
+ JENC::thumb_prox_fit_joint_angle_data_2_poly(float x, float y, float z)
{
static const Eigen::Vector5d cx = []
{
Eigen::Vector5d v;
- v << -5.5568017768730435e+001,
- +6.8157052557135417e-002,
- -1.7125128969521229e-005,
- +2.1952213287357551e-009,
- -1.3510392732948656e-013,
- +3.1680494194422713e-018;
+ v << -5.5568017768730435e+001, +6.8157052557135417e-002, -1.7125128969521229e-005,
+ +2.1952213287357551e-009, -1.3510392732948656e-013, +3.1680494194422713e-018;
return v;
}();
static const Eigen::Vector5d cy = []
{
Eigen::Vector5d v;
- v << +1.7150983498520592e+001,
- +3.0178399682480750e-001,
- -5.1024537014353864e-004,
- +1.8904540254959379e-006,
- -7.4790300984104864e-009,
- +8.8962636945820215e-012;
+ v << +1.7150983498520592e+001, +3.0178399682480750e-001, -5.1024537014353864e-004,
+ +1.8904540254959379e-006, -7.4790300984104864e-009, +8.8962636945820215e-012;
return v;
}();
static const Eigen::Vector5d cz = []
{
Eigen::Vector5d v;
- v << -3.8214284045175326e+001,
- - 6.8665155192354554e-002,
- - 2.0092443959013517e-005,
- - 2.9100269843583112e-009,
- - 1.9795457264104989e-013,
- - 5.0630697241822937e-018;
+ v << -3.8214284045175326e+001, -6.8665155192354554e-002, -2.0092443959013517e-005,
+ -2.9100269843583112e-009, -1.9795457264104989e-013, -5.0630697241822937e-018;
return v;
}();
- return
- {
- calc_poly(cx, x),
- calc_poly(cy, y),
- calc_poly(cz, z)
- };
+ return {calc_poly(cx, x), calc_poly(cy, y), calc_poly(cz, z)};
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger
namespace devices::ethercat::hand::soft_sensorized_finger
{
using JENC = SensorFit::JointEncoder;
- Eigen::Vector3f JENC::data_3_fit_joint_angle_pwlin_thumb_prox(std::int64_t x, std::int64_t y, std::int64_t z)
+
+ Eigen::Vector3f
+ JENC::data_3_fit_joint_angle_pwlin_thumb_prox(std::int64_t x, std::int64_t y, std::int64_t z)
{
static const std::array<piecewise_linear_function<std::int64_t, float>, 3> fit = []
{
- const std::map<std::int64_t, float> vals_thumb_prox_x
- {
- {966, -4.80f},
- {998, -3.60f},
- {1058, -1.10f},
- {1158, 3.80f},
- {1272, 8.10f},
- {1353, 11.10f},
- {1468, 14.90f},
- {1580, 18.40f},
- {1717, 21.90f},
- {1901, 26.10f},
- {2140, 30.20f},
- {2425, 35.50f},
- {2922, 41.80f},
- {3529, 47.60f},
- {4523, 55.30f},
- {5849, 60.80f},
- {7855, 68.20f},
- {8770, 70.90f},
- {9420, 72.20f},
- {11343, 75.80f},
- {13091, 77.80f},
- {14162, 80.00f}
- };
- const std::map<std::int64_t, float> vals_thumb_prox_y
- {
- {-67, -4.80f},
- {-57, -3.60f},
- {-55, -1.10f},
- {-32, 3.80f},
- {-24, 8.10f},
- {-28, 11.10f},
- {-8, 14.90f},
- {-2, 18.40f},
- {14, 21.90f},
- {44, 26.10f},
- {37, 30.20f},
- {73, 35.50f},
- {89, 41.80f},
- {119, 47.60f},
- {146, 55.30f},
- {195, 60.80f},
- {238, 68.20f},
- {288, 70.90f},
- {323, 72.20f},
- {389, 75.80f},
- {416, 77.80f},
- {308, 80.00f}
- };
- const std::map<std::int64_t, float> vals_thumb_prox_z
- {
- {-627, -4.80f},
- {-665, -3.60f},
- {-687, -1.10f},
- {-749, 3.80f},
- {-840, 8.10f},
- {-903, 11.10f},
- {-993, 14.90f},
- {-1093, 18.40f},
- {-1208, 21.90f},
- {-1366, 26.10f},
- {-1585, 30.20f},
- {-1845, 35.50f},
- {-2316, 41.80f},
- {-2887, 47.60f},
- {-3863, 55.30f},
- {-5210, 60.80f},
- {-7310, 68.20f},
- {-8155, 70.90f},
- {-8909, 72.20f},
- {-10828, 75.80f},
- {-12380, 77.80f},
- {-13366, 80.00f}
- };
+ const std::map<std::int64_t, float> vals_thumb_prox_x{
+ {966, -4.80f}, {998, -3.60f}, {1058, -1.10f}, {1158, 3.80f}, {1272, 8.10f},
+ {1353, 11.10f}, {1468, 14.90f}, {1580, 18.40f}, {1717, 21.90f}, {1901, 26.10f},
+ {2140, 30.20f}, {2425, 35.50f}, {2922, 41.80f}, {3529, 47.60f}, {4523, 55.30f},
+ {5849, 60.80f}, {7855, 68.20f}, {8770, 70.90f}, {9420, 72.20f}, {11343, 75.80f},
+ {13091, 77.80f}, {14162, 80.00f}};
+ const std::map<std::int64_t, float> vals_thumb_prox_y{
+ {-67, -4.80f}, {-57, -3.60f}, {-55, -1.10f}, {-32, 3.80f}, {-24, 8.10f},
+ {-28, 11.10f}, {-8, 14.90f}, {-2, 18.40f}, {14, 21.90f}, {44, 26.10f},
+ {37, 30.20f}, {73, 35.50f}, {89, 41.80f}, {119, 47.60f}, {146, 55.30f},
+ {195, 60.80f}, {238, 68.20f}, {288, 70.90f}, {323, 72.20f}, {389, 75.80f},
+ {416, 77.80f}, {308, 80.00f}};
+ const std::map<std::int64_t, float> vals_thumb_prox_z{
+ {-627, -4.80f}, {-665, -3.60f}, {-687, -1.10f}, {-749, 3.80f},
+ {-840, 8.10f}, {-903, 11.10f}, {-993, 14.90f}, {-1093, 18.40f},
+ {-1208, 21.90f}, {-1366, 26.10f}, {-1585, 30.20f}, {-1845, 35.50f},
+ {-2316, 41.80f}, {-2887, 47.60f}, {-3863, 55.30f}, {-5210, 60.80f},
+ {-7310, 68.20f}, {-8155, 70.90f}, {-8909, 72.20f}, {-10828, 75.80f},
+ {-12380, 77.80f}, {-13366, 80.00f}};
std::array<piecewise_linear_function<std::int64_t, float>, 3> f;
f.at(0).reset(vals_thumb_prox_x);
f.at(1).reset(vals_thumb_prox_y);
f.at(2).reset(vals_thumb_prox_z);
return f;
}();
- return
- {
- fit.at(0).calc_lin_search_with_cache(x),
- fit.at(1).calc_lin_search_with_cache(y),
- fit.at(2).calc_lin_search_with_cache(z)
- };
+ return {fit.at(0).calc_lin_search_with_cache(x),
+ fit.at(1).calc_lin_search_with_cache(y),
+ fit.at(2).calc_lin_search_with_cache(z)};
}
- Eigen::Vector3f JENC::data_3_fit_joint_angle_pwlin_little_dist(std::int64_t x, std::int64_t y, std::int64_t z)
+
+ Eigen::Vector3f
+ JENC::data_3_fit_joint_angle_pwlin_little_dist(std::int64_t x, std::int64_t y, std::int64_t z)
{
static const std::array<piecewise_linear_function<std::int64_t, float>, 3> fit = []
{
- static const std::map<std::int64_t, float> vals_little_dist_x
- {
- {943, 1.20f},
- {1163, 10.20f},
- {1256, 12.20f},
- {1351, 17.70f},
- {1506, 21.50f},
- {1798, 27.70f},
- {2242, 34.50f},
- {2716, 41.20f},
- {3202, 45.30f},
- {4075, 52.10f},
- {5203, 59.00f},
- {6631, 64.90f},
- {8769, 71.80f},
- {10550, 75.40f},
- {13041, 79.20f}
- };
- static const std::map<std::int64_t, float> vals_little_dist_y
- {
- {-188, 1.20},
- {-206, 10.20},
- {-219, 12.20},
- {-219, 17.70},
- {-225, 21.50},
- {-209, 27.70},
- {-194, 34.50},
- {-211, 41.20},
- {-221, 45.30},
- {-304, 52.10},
- {-491, 59.00},
- {-753, 64.90},
- {-1088, 71.80},
- {-1432, 75.40},
- {-1948, 79.20}
- };
- static const std::map<std::int64_t, float> vals_little_dist_z
- {
- { -496, 1.20f},
- { -564, 10.20f},
- { -597, 12.20f},
- { -612, 17.70f},
- { -653, 21.50f},
- { -721, 27.70f},
- { -836, 34.50f},
- { -971, 41.20f},
- { -1106, 45.30f},
- { -1396, 52.10f},
- { -1807, 59.00f},
- { -2406, 64.90f},
- { -3567, 71.80f},
- { -4649, 75.40f},
- { -6236, 79.20f}
- };
+ static const std::map<std::int64_t, float> vals_little_dist_x{{943, 1.20f},
+ {1163, 10.20f},
+ {1256, 12.20f},
+ {1351, 17.70f},
+ {1506, 21.50f},
+ {1798, 27.70f},
+ {2242, 34.50f},
+ {2716, 41.20f},
+ {3202, 45.30f},
+ {4075, 52.10f},
+ {5203, 59.00f},
+ {6631, 64.90f},
+ {8769, 71.80f},
+ {10550, 75.40f},
+ {13041, 79.20f}};
+ static const std::map<std::int64_t, float> vals_little_dist_y{{-188, 1.20},
+ {-206, 10.20},
+ {-219, 12.20},
+ {-219, 17.70},
+ {-225, 21.50},
+ {-209, 27.70},
+ {-194, 34.50},
+ {-211, 41.20},
+ {-221, 45.30},
+ {-304, 52.10},
+ {-491, 59.00},
+ {-753, 64.90},
+ {-1088, 71.80},
+ {-1432, 75.40},
+ {-1948, 79.20}};
+ static const std::map<std::int64_t, float> vals_little_dist_z{{-496, 1.20f},
+ {-564, 10.20f},
+ {-597, 12.20f},
+ {-612, 17.70f},
+ {-653, 21.50f},
+ {-721, 27.70f},
+ {-836, 34.50f},
+ {-971, 41.20f},
+ {-1106, 45.30f},
+ {-1396, 52.10f},
+ {-1807, 59.00f},
+ {-2406, 64.90f},
+ {-3567, 71.80f},
+ {-4649, 75.40f},
+ {-6236, 79.20f}};
std::array<piecewise_linear_function<std::int64_t, float>, 3> f;
f.at(0).reset(vals_little_dist_x);
@@ -245,11 +151,8 @@ namespace devices::ethercat::hand::soft_sensorized_finger
f.at(2).reset(vals_little_dist_z);
return f;
}();
- return
- {
- fit.at(0).calc_lin_search_with_cache(x),
- fit.at(1).calc_lin_search_with_cache(y),
- fit.at(2).calc_lin_search_with_cache(z)
- };
+ return {fit.at(0).calc_lin_search_with_cache(x),
+ fit.at(1).calc_lin_search_with_cache(y),
+ fit.at(2).calc_lin_search_with_cache(z)};
}
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/utility/SensorFit.h b/source/devices/ethercat/hand/soft_sensorized_finger/utility/SensorFit.h
index a9ae6d80..c13aed29 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/utility/SensorFit.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/utility/SensorFit.h
@@ -22,43 +22,51 @@
#pragma once
-#include <Eigen/Core>
-
-#include <vector>
-#include <map>
-#include <limits>
#include <algorithm>
+#include <limits>
+#include <map>
+#include <vector>
+
+#include <Eigen/Core>
namespace devices::ethercat::hand::soft_sensorized_finger
{
- template<class X, class Y>
+ template <class X, class Y>
struct piecewise_linear_function_entry
{
piecewise_linear_function_entry(X lower_bound = 0, Y a = 0, Y b = 0) :
lower_bound{lower_bound}, a{a}, b{b}
- {}
+ {
+ }
+
X lower_bound = 0;
- Y a = 0;
- Y b = 0;
+ Y a = 0;
+ Y b = 0;
};
- template<class X, class Y>
- bool operator<(const X l, const piecewise_linear_function_entry<X, Y>& r)
+ template <class X, class Y>
+ bool
+ operator<(const X l, const piecewise_linear_function_entry<X, Y>& r)
{
return l < r.lower_bound;
}
- template<class X, class Y>
- bool operator<(const piecewise_linear_function_entry<X, Y> l, const piecewise_linear_function_entry<X, Y>& r)
+
+ template <class X, class Y>
+ bool
+ operator<(const piecewise_linear_function_entry<X, Y> l,
+ const piecewise_linear_function_entry<X, Y>& r)
{
return l.lower_bound < r.lower_bound;
}
- template<class X, class Y>
- bool operator<(const piecewise_linear_function_entry<X, Y> l, const X r)
+
+ template <class X, class Y>
+ bool
+ operator<(const piecewise_linear_function_entry<X, Y> l, const X r)
{
return l.lower_bound < r;
}
- template<class X, class Y>
+ template <class X, class Y>
class piecewise_linear_function
{
//without local: lin < 64 -> bin
@@ -66,23 +74,26 @@ namespace devices::ethercat::hand::soft_sensorized_finger
private:
std::vector<piecewise_linear_function_entry<X, Y>> _coefficients;
mutable std::size_t _last;
+
public:
piecewise_linear_function() = default;
+
piecewise_linear_function(const std::map<X, Y>& values)
{
reset(values);
}
+
piecewise_linear_function(std::initializer_list<std::pair<const X, Y>> values) :
piecewise_linear_function(std::map<X, Y>(std::move(values)))
{
}
- void reset(const std::map<X, Y>& points)
+ void
+ reset(const std::map<X, Y>& points)
{
- static constexpr X minimal =
- std::is_floating_point_v<X> ?
- -std::numeric_limits<X>::infinity() :
- std::numeric_limits<X>::lowest();
+ static constexpr X minimal = std::is_floating_point_v<X>
+ ? -std::numeric_limits<X>::infinity()
+ : std::numeric_limits<X>::lowest();
_coefficients.clear();
_last = 0;
switch (points.size())
@@ -102,7 +113,8 @@ namespace devices::ethercat::hand::soft_sensorized_finger
++i1;
for (; i1 != points.end(); i0 = i1, ++i1)
{
- Y a = (i1->second - i0->second) / (static_cast<Y>(i1->first) - static_cast<Y>(i0->first));
+ Y a = (i1->second - i0->second) /
+ (static_cast<Y>(i1->first) - static_cast<Y>(i0->first));
Y b = i0->second - static_cast<Y>(i0->first) * a;
_coefficients.emplace_back(i0->first, a, b);
}
@@ -110,13 +122,16 @@ namespace devices::ethercat::hand::soft_sensorized_finger
}
}
}
- void reset(std::initializer_list<std::pair<const X, Y>> l)
+
+ void
+ reset(std::initializer_list<std::pair<const X, Y>> l)
{
reset(std::map<X, Y>(std::move(l)));
}
private:
- std::size_t bin_search(auto f, auto l, X x) const
+ std::size_t
+ bin_search(auto f, auto l, X x) const
{
auto it = std::upper_bound(f, l, x);
//cant be begin, since begin is min
@@ -126,13 +141,17 @@ namespace devices::ethercat::hand::soft_sensorized_finger
}
return std::distance(_coefficients.begin(), it) - 1;
}
+
public:
- Y calc_at(X x, std::size_t i) const
+ Y
+ calc_at(X x, std::size_t i) const
{
const auto& coeff = _coefficients.at(i);
return coeff.a * static_cast<Y>(x) + coeff.b;
}
- Y calc_lin_search(X x) const
+
+ Y
+ calc_lin_search(X x) const
{
std::size_t i = 0;
for (; i + 1 < _coefficients.size(); ++i)
@@ -144,7 +163,9 @@ namespace devices::ethercat::hand::soft_sensorized_finger
}
return calc_at(x, i);
}
- Y calc_lin_search_with_cache(X x) const
+
+ Y
+ calc_lin_search_with_cache(X x) const
{
if (_last + 1 < _coefficients.size())
{
@@ -152,26 +173,35 @@ namespace devices::ethercat::hand::soft_sensorized_finger
if (x < _coefficients.at(_last).lower_bound)
{
//search down
- for (; x < _coefficients.at(_last).lower_bound; --_last);
+ for (; x < _coefficients.at(_last).lower_bound; --_last)
+ ;
}
else
{
//search up
- for (; _last + 1 < _coefficients.size() && x > _coefficients.at(_last + 1).lower_bound ; ++_last);
+ for (; _last + 1 < _coefficients.size() &&
+ x > _coefficients.at(_last + 1).lower_bound;
+ ++_last)
+ ;
}
}
else
{
//last
- for (; x < _coefficients.at(_last).lower_bound; --_last);
+ for (; x < _coefficients.at(_last).lower_bound; --_last)
+ ;
}
return calc_at(x, _last);
}
- Y calc_bin_search(X x) const
+
+ Y
+ calc_bin_search(X x) const
{
return calc_at(x, bin_search(_coefficients.begin(), _coefficients.end(), x));
}
- Y calc_bin_search_with_cache(X x) const
+
+ Y
+ calc_bin_search_with_cache(X x) const
{
if (_last + 1 < _coefficients.size())
{
@@ -196,28 +226,35 @@ namespace devices::ethercat::hand::soft_sensorized_finger
return calc_at(x, _last);
}
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger
namespace devices::ethercat::hand::soft_sensorized_finger
{
- inline float calc_poly(const auto& coeffs, double v)
+ inline float
+ calc_poly(const auto& coeffs, double v)
{
double acc = 0;
- for (int i = 0; i < coeffs.cols(); ++i)
+ for (int i = 0; i < coeffs.cols(); ++i)
{
acc += std::pow(v, i) * coeffs(i);
}
return static_cast<float>(acc);
}
+
struct SensorFit
{
struct JointEncoder
{
- static Eigen::Vector3f thumb_prox_fit_joint_angle_data_1_poly(float x, float y, float z);
- static Eigen::Vector3f thumb_prox_fit_joint_angle_data_2_poly(float x, float y, float z);
+ static Eigen::Vector3f
+ thumb_prox_fit_joint_angle_data_1_poly(float x, float y, float z);
+ static Eigen::Vector3f
+ thumb_prox_fit_joint_angle_data_2_poly(float x, float y, float z);
- static Eigen::Vector3f data_3_fit_joint_angle_pwlin_thumb_prox(std::int64_t x, std::int64_t y, std::int64_t z);
- static Eigen::Vector3f data_3_fit_joint_angle_pwlin_little_dist(std::int64_t x, std::int64_t y, std::int64_t z);
+ static Eigen::Vector3f
+ data_3_fit_joint_angle_pwlin_thumb_prox(std::int64_t x, std::int64_t y, std::int64_t z);
+ static Eigen::Vector3f data_3_fit_joint_angle_pwlin_little_dist(std::int64_t x,
+ std::int64_t y,
+ std::int64_t z);
};
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger
diff --git a/source/devices/ethercat/hand/soft_sensorized_finger/utility/Sensors.h b/source/devices/ethercat/hand/soft_sensorized_finger/utility/Sensors.h
index ef45b19f..24722322 100644
--- a/source/devices/ethercat/hand/soft_sensorized_finger/utility/Sensors.h
+++ b/source/devices/ethercat/hand/soft_sensorized_finger/utility/Sensors.h
@@ -24,17 +24,16 @@
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <devices/ethercat/hand/soft_sensorized_finger/sensor_value/Motor.h>
+#include "Identities.h"
#include <devices/ethercat/hand/soft_sensorized_finger/sensor_value/Finger.h>
#include <devices/ethercat/hand/soft_sensorized_finger/sensor_value/Hand.h>
-
-#include "Identities.h"
+#include <devices/ethercat/hand/soft_sensorized_finger/sensor_value/Motor.h>
namespace devices::ethercat::hand::soft_sensorized_finger
{
struct Sensors
{
- using MotorIdentity = identity::MotorIdentity;
+ using MotorIdentity = identity::MotorIdentity;
using FingerIdentity = identity::FingerIdentity;
identity::MotorArray<const MotorSensorValue*> motor;
@@ -42,24 +41,32 @@ namespace devices::ethercat::hand::soft_sensorized_finger
struct Finger
{
const FingerSensorValue* sensor = nullptr;
- const FingerSensorValue* operator->() const
+
+ const FingerSensorValue*
+ operator->() const
{
return sensor;
}
- const FingerSensorValue& operator*() const
+
+ const FingerSensorValue&
+ operator*() const
{
return *sensor;
}
- template<class T>
- auto fit_joint_encoder_dist(T fnc, bool invert_values)
+
+ template <class T>
+ auto
+ fit_joint_encoder_dist(T fnc, bool invert_values)
{
std::int16_t factor = invert_values ? -1 : 1;
return fnc(factor * sensor->raw_joint_encoder_dist.at(0),
factor * sensor->raw_joint_encoder_dist.at(1),
factor * sensor->raw_joint_encoder_dist.at(2));
}
- template<class T>
- auto fit_joint_encoder_prox(T fnc, bool invert_values)
+
+ template <class T>
+ auto
+ fit_joint_encoder_prox(T fnc, bool invert_values)
{
std::int16_t factor = invert_values ? -1 : 1;
return fnc(factor * sensor->raw_joint_encoder_prox.at(0),
@@ -81,18 +88,20 @@ namespace devices::ethercat::hand::soft_sensorized_finger
Sensors(const std::string& hand_name, armarx::NJointController& parent)
{
init(parent, hand_name, hand);
- init(parent, hand_name + ".motor.thumb", motor.thumb());
- init(parent, hand_name + ".motor.index", motor.index());
- init(parent, hand_name + ".motor.other", motor.other());
- init(parent, hand_name + ".finger.thumb", finger.thumb().sensor);
- init(parent, hand_name + ".finger.index", finger.index().sensor);
+ init(parent, hand_name + ".motor.thumb", motor.thumb());
+ init(parent, hand_name + ".motor.index", motor.index());
+ init(parent, hand_name + ".motor.other", motor.other());
+ init(parent, hand_name + ".finger.thumb", finger.thumb().sensor);
+ init(parent, hand_name + ".finger.index", finger.index().sensor);
init(parent, hand_name + ".finger.middle", finger.middle().sensor);
- init(parent, hand_name + ".finger.ring", finger.ring().sensor);
+ init(parent, hand_name + ".finger.ring", finger.ring().sensor);
init(parent, hand_name + ".finger.little", finger.little().sensor);
}
+
private:
- template<class T>
- void init(armarx::NJointController& parent, const std::string& name, T const*& ptr)
+ template <class T>
+ void
+ init(armarx::NJointController& parent, const std::string& name, T const*& ptr)
{
const armrax::SensorValueBase* sv = parent.useSensorValue(name);
ARMARX_CHECK_NOT_NULL(sv) << VAROUT(name);
@@ -101,4 +110,4 @@ namespace devices::ethercat::hand::soft_sensorized_finger
}
};
-}
+} // namespace devices::ethercat::hand::soft_sensorized_finger
diff --git a/source/devices/ethercat/head/armar6/Config.h b/source/devices/ethercat/head/armar6/Config.h
index 920a76b4..0886823e 100644
--- a/source/devices/ethercat/head/armar6/Config.h
+++ b/source/devices/ethercat/head/armar6/Config.h
@@ -2,22 +2,21 @@
#include <armarx/control/hardware_config/DeviceConfig.h>
+#include <devices/ethercat/common/imagine_board/Config.h>
#include <devices/ethercat/head/armar6/joint_controller/PWMPosition.h>
#include <devices/ethercat/head/armar6/joint_controller/PWMVelocity.h>
#include <devices/ethercat/head/armar6/joint_controller/ZeroTorque.h>
-#include <devices/ethercat/common/imagine_board/Config.h>
-
namespace devices::ethercat::head::armar6
{
struct ActorConfigWithController : public common::imagine_board::ActorConfig
{
- ActorConfigWithController(armarx::control::hardware_config::DeviceConfigBase& actorConfig)
- : common::imagine_board::ActorConfig(actorConfig)
+ ActorConfigWithController(armarx::control::hardware_config::DeviceConfigBase& actorConfig) :
+ common::imagine_board::ActorConfig(actorConfig)
{
positionControllerCfg = joint_controller::PWMPositionControllerConfiguration::
CreatePWMPositionControllerConfigData(
- actorConfig.getControllerConfig("JointHeadPWMPosition"));
+ actorConfig.getControllerConfig("JointHeadPWMPosition"));
velocityControllerCfg =
PWMVelocityControllerConfiguration::CreatePWMVelocityControllerConfigData(
actorConfig.getControllerConfig("JointHeadPWMVelocity"));
@@ -26,9 +25,11 @@ namespace devices::ethercat::head::armar6
actorConfig.getControllerConfig("JointPWMZeroTorque"));
}
- std::shared_ptr<const joint_controller::PWMPositionControllerConfiguration> positionControllerCfg;
+ std::shared_ptr<const joint_controller::PWMPositionControllerConfiguration>
+ positionControllerCfg;
// TODO: Why not the controller in joint_controllers? Why is there another one?
std::shared_ptr<const PWMVelocityControllerConfiguration> velocityControllerCfg;
- std::shared_ptr<const joint_controller::PWMZeroTorqueControllerConfiguration> zeroTorqueControllerCfg;
+ std::shared_ptr<const joint_controller::PWMZeroTorqueControllerConfiguration>
+ zeroTorqueControllerCfg;
};
-}
+} // namespace devices::ethercat::head::armar6
diff --git a/source/devices/ethercat/head/armar6/ControlTargets.h b/source/devices/ethercat/head/armar6/ControlTargets.h
index 5520374a..6e4b7481 100644
--- a/source/devices/ethercat/head/armar6/ControlTargets.h
+++ b/source/devices/ethercat/head/armar6/ControlTargets.h
@@ -3,7 +3,6 @@
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h>
-
namespace devices::ethercat::head::armar6
{
@@ -12,51 +11,56 @@ namespace devices::ethercat::head::armar6
static const std::string HeadPWM = "ControlMode_HeadPWM";
}
-
- class ControlTargetHeadPWM :
- public armarx::ControlTargetBase
+ class ControlTargetHeadPWM : public armarx::ControlTargetBase
{
public:
-
std::int32_t pwm = 0;
ControlTargetHeadPWM() = default;
ControlTargetHeadPWM(const ControlTargetHeadPWM&) = default;
ControlTargetHeadPWM(ControlTargetHeadPWM&&) = default;
- ControlTargetHeadPWM(std::int32_t val) : pwm{val} {}
+
+ ControlTargetHeadPWM(std::int32_t val) : pwm{val}
+ {
+ }
+
ControlTargetHeadPWM& operator=(const ControlTargetHeadPWM&) = default;
ControlTargetHeadPWM& operator=(ControlTargetHeadPWM&&) = default;
- ControlTargetHeadPWM& operator=(std::int32_t val)
+ ControlTargetHeadPWM&
+ operator=(std::int32_t val)
{
pwm = val;
return *this;
}
- const std::string& getControlMode() const override
+ const std::string&
+ getControlMode() const override
{
return ctrl_modes::HeadPWM;
}
- void reset() override
+ void
+ reset() override
{
pwm = std::numeric_limits<int>::min();
}
- bool isValid() const override
+ bool
+ isValid() const override
{
return pwm != std::numeric_limits<int>::min();
}
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
- static ControlTargetInfo<ControlTargetHeadPWM> GetClassMemberInfo()
+ static ControlTargetInfo<ControlTargetHeadPWM>
+ GetClassMemberInfo()
{
ControlTargetInfo<ControlTargetHeadPWM> cti;
cti.addMemberVariable(&ControlTargetHeadPWM::pwm, "pwm");
return cti;
}
-
};
-}
+} // namespace devices::ethercat::head::armar6
diff --git a/source/devices/ethercat/head/armar6/Device.cpp b/source/devices/ethercat/head/armar6/Device.cpp
index 86f7fad2..ebe274d9 100644
--- a/source/devices/ethercat/head/armar6/Device.cpp
+++ b/source/devices/ethercat/head/armar6/Device.cpp
@@ -23,14 +23,14 @@
*/
-#include <ArmarXCore/core/logging/LoggingUtil.h> // NEEDS TO BE INCLUDED BEFORE ExpressionException.h
-
#include "Device.h"
-#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/logging/LoggingUtil.h> // NEEDS TO BE INCLUDED BEFORE ExpressionException.h
#include "joint_controller/EmergencyStop.h"
#include "joint_controller/PWM.h"
@@ -41,12 +41,10 @@
#include <devices/ethercat/common/h2t_devices.h>
#include <devices/ethercat/common/imagine_board/Slave.h>
-
namespace devices::ethercat::head::armar6
{
- Device::Device(hwconfig::DeviceConfig& config,
- const VirtualRobot::RobotPtr& robot) :
+ Device::Device(hwconfig::DeviceConfig& config, const VirtualRobot::RobotPtr& robot) :
DeviceBase(config.getName()),
SensorDevice(config.getName()),
// ControlDevice(config.getName()),
@@ -84,7 +82,6 @@ namespace devices::ethercat::head::armar6
}
}
-
armarx::control::ethercat::DeviceInterface::TryAssignResult
Device::tryAssign(armarx::control::ethercat::SlaveInterface& slave)
{
@@ -102,7 +99,6 @@ namespace devices::ethercat::head::armar6
return TryAssignResult::unknown;
}
-
armarx::control::ethercat::DeviceInterface::AllAssignedResult
Device::onAllAssigned()
{
@@ -116,24 +112,25 @@ namespace devices::ethercat::head::armar6
return Result::slavesMissing;
}
-
std::string
Device::getClassName() const
{
return "HeadBoard";
}
-
void
Device::postSwitchToSafeOp()
{
if (!dataPtr)
{
- dataPtr.reset(new common::imagine_board::Data(/* copy list and slice the objects to get rid of controller configurations */
- std::list<common::imagine_board::ActorConfig>(actorConfigs.begin(), actorConfigs.end()),
- slave->getOutputsPtr(),
- slave->getInputsPtr(),
- robot));
+ dataPtr.reset(
+ new common::imagine_board::
+ Data(/* copy list and slice the objects to get rid of controller configurations */
+ std::list<common::imagine_board::ActorConfig>(actorConfigs.begin(),
+ actorConfigs.end()),
+ slave->getOutputsPtr(),
+ slave->getInputsPtr(),
+ robot));
for (auto& actorConfig : actorConfigs)
{
@@ -149,21 +146,18 @@ namespace devices::ethercat::head::armar6
ARMARX_CHECK_NOT_NULL(dataPtr);
}
-
const armarx::SensorValueBase*
Device::getSensorValue() const
{
return &sensorValue;
}
-
const armarx::control::ethercat::SlaveIdentifier&
Device::getSlaveIdentifier() const
{
return slaveIdentifier;
}
-
void
Device::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -173,7 +167,6 @@ namespace devices::ethercat::head::armar6
// TODO: read IMU
}
-
// void head_board::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
// {
// // TODO: write LED targets
@@ -214,7 +207,6 @@ namespace devices::ethercat::head::armar6
};
}
-
Device::ActorRobotUnitDevice::ActorRobotUnitDevice(size_t connectorIndex,
const std::string& deviceName,
VirtualRobot::RobotNodePtr robotNode) :
@@ -229,7 +221,6 @@ namespace devices::ethercat::head::armar6
ARMARX_INFO << deviceName << " actor created";
}
-
void
Device::ActorRobotUnitDevice::init(Device* dev,
common::imagine_board::Data* dataPtr,
@@ -273,24 +264,22 @@ namespace devices::ethercat::head::armar6
addJointController(pwmController.get());
}
-
common::imagine_board::SensorActorDataPtr
Device::ActorRobotUnitDevice::getSensorActorDataPtr() const
{
return headActorDataPtr;
}
-
const VirtualRobot::RobotNodePtr&
Device::ActorRobotUnitDevice::getRobotNode() const
{
return robotNode;
}
-
void
- Device::ActorRobotUnitDevice::rtReadSensorValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& /*timeSinceLastIteration*/)
+ Device::ActorRobotUnitDevice::rtReadSensorValues(
+ const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
{
sensorValue.position = headActorDataPtr->getPosition();
sensorValue.relativePosition = headActorDataPtr->getRelativePosition();
@@ -306,10 +295,10 @@ namespace devices::ethercat::head::armar6
0; //estimateTorque(sensorValue.velocityTicksPerMs, sensorValue.targetPWM);
}
-
void
- Device::ActorRobotUnitDevice::rtWriteTargetValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& /*timeSinceLastIteration*/)
+ Device::ActorRobotUnitDevice::rtWriteTargetValues(
+ const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
{
const uint16_t led = std::chrono::duration_cast<std::chrono::seconds>(
std::chrono::high_resolution_clock::now().time_since_epoch())
diff --git a/source/devices/ethercat/head/armar6/Device.h b/source/devices/ethercat/head/armar6/Device.h
index ac86f379..e1ad8a7c 100644
--- a/source/devices/ethercat/head/armar6/Device.h
+++ b/source/devices/ethercat/head/armar6/Device.h
@@ -33,7 +33,6 @@
#include <devices/ethercat/common/imagine_board/Data.h>
#include <devices/ethercat/common/imagine_board/SensorValue.h>
-
#include <devices/ethercat/head/armar6/Config.h>
namespace devices::ethercat::common::imagine_board
@@ -43,7 +42,6 @@ namespace devices::ethercat::common::imagine_board
class Slave;
} // namespace devices::ethercat::common::imagine_board
-
namespace devices::ethercat::head::armar6::joint_controller
{
using StopMovementControllerPtr = std::shared_ptr<class StopMovementController>;
@@ -58,7 +56,6 @@ namespace devices::ethercat::head::armar6::joint_controller
using PWMControllerPtr = std::shared_ptr<class PWMController>;
} // namespace devices::ethercat::head::armar6::joint_controller
-
namespace devices::ethercat::head::armar6
{
@@ -74,9 +71,9 @@ namespace devices::ethercat::head::armar6
public:
class ActorRobotUnitDevice :
- public DeviceInterface::SubDeviceInterface,
- public armarx::ControlDevice,
- public armarx::SensorDevice
+ public DeviceInterface::SubDeviceInterface,
+ public armarx::ControlDevice,
+ public armarx::SensorDevice
{
friend class Device;
@@ -85,6 +82,7 @@ namespace devices::ethercat::head::armar6
ActorRobotUnitDevice(size_t connectorIndex,
const std::string& deviceName,
VirtualRobot::RobotNodePtr robotNode);
+
const armarx::SensorValueBase*
getSensorValue() const override
{
@@ -127,8 +125,7 @@ namespace devices::ethercat::head::armar6
using ActorRobotUnitDevicePtr = std::shared_ptr<ActorRobotUnitDevice>;
- Device(hwconfig::DeviceConfig& config,
- VirtualRobot::RobotPtr const& robot);
+ Device(hwconfig::DeviceConfig& config, VirtualRobot::RobotPtr const& robot);
// DeviceInterface interface
void postSwitchToSafeOp() override;
diff --git a/source/devices/ethercat/head/armar6/PWMVelocity.cpp b/source/devices/ethercat/head/armar6/PWMVelocity.cpp
index fa35a105..57fcb1a1 100644
--- a/source/devices/ethercat/head/armar6/PWMVelocity.cpp
+++ b/source/devices/ethercat/head/armar6/PWMVelocity.cpp
@@ -28,7 +28,8 @@
namespace devices::ethercat::head::armar6
{
- PWMVelocityController::PWMVelocityController(PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) :
+ PWMVelocityController::PWMVelocityController(
+ PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) :
config(velocityControllerConfigDataPtr)
{
pid.reset(new armarx::PIDController(velocityControllerConfigDataPtr->p,
@@ -36,17 +37,22 @@ namespace devices::ethercat::head::armar6
velocityControllerConfigDataPtr->d));
pid->pdOutputFilter.reset(new armarx::rtfilters::AverageFilter(10));
pid->maxIntegral = velocityControllerConfigDataPtr->maxIntegral;
- pid->conditionalIntegralErrorTreshold = velocityControllerConfigDataPtr->conditionalIntegralErrorTreshold;
+ pid->conditionalIntegralErrorTreshold =
+ velocityControllerConfigDataPtr->conditionalIntegralErrorTreshold;
pid->threadSafe = false;
}
-
- double PWMVelocityController::run(IceUtil::Time const& deltaT, double currentVelocity, double targetVelocity, double gravityTorque)
+ double
+ PWMVelocityController::run(IceUtil::Time const& deltaT,
+ double currentVelocity,
+ double targetVelocity,
+ double gravityTorque)
{
double targetPWM = 0;
if (!this->config->feedForwardMode)
{
- lastActualVelocity = lastActualVelocity * (1.0 - config->velocityUpdatePercent) + currentVelocity * config->velocityUpdatePercent;
+ lastActualVelocity = lastActualVelocity * (1.0 - config->velocityUpdatePercent) +
+ currentVelocity * config->velocityUpdatePercent;
pid->update(deltaT.toSecondsDouble(), lastActualVelocity, targetVelocity);
targetPWM = pid->getControlValue();
}
@@ -57,9 +63,11 @@ namespace devices::ethercat::head::armar6
//feed forward
if (std::abs(targetVelocity) > 0.001 && std::abs(currentVelocity) < 0.0001f)
{
- targetPWM += config->PWMDeadzone * armarx::math::MathUtils::Sign(targetVelocity); // deadzone
+ targetPWM +=
+ config->PWMDeadzone * armarx::math::MathUtils::Sign(targetVelocity); // deadzone
}
- targetPWM += config->feedforwardVelocityToPWMFactor * targetVelocity; // approx. feedforward vel
+ targetPWM +=
+ config->feedforwardVelocityToPWMFactor * targetVelocity; // approx. feedforward vel
// 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);
@@ -67,15 +75,16 @@ namespace devices::ethercat::head::armar6
return targetPWM;
}
-
- void PWMVelocityController::reset(double currentVelocity)
+ void
+ PWMVelocityController::reset(double currentVelocity)
{
lastActualVelocity = currentVelocity;
pid->reset();
}
-
- PWMVelocityControllerConfigurationCPtr PWMVelocityControllerConfiguration::CreatePWMVelocityControllerConfigData(hwconfig::Config& hwConfig)
+ PWMVelocityControllerConfigurationCPtr
+ PWMVelocityControllerConfiguration::CreatePWMVelocityControllerConfigData(
+ hwconfig::Config& hwConfig)
{
PWMVelocityControllerConfigurationPtr configData(new PWMVelocityControllerConfiguration());
@@ -91,14 +100,17 @@ namespace devices::ethercat::head::armar6
configData->i = hwConfig.getFloat("i");
configData->d = hwConfig.getFloat("d");
configData->maxIntegral = hwConfig.getFloat("maxIntegral");
- configData->feedforwardVelocityToPWMFactor = hwConfig.getFloat("feedforwardVelocityToPWMFactor");
- configData->feedforwardTorqueToPWMFactor = hwConfig.getFloat("feedforwardTorqueToPWMFactor");
+ configData->feedforwardVelocityToPWMFactor =
+ hwConfig.getFloat("feedforwardVelocityToPWMFactor");
+ configData->feedforwardTorqueToPWMFactor =
+ hwConfig.getFloat("feedforwardTorqueToPWMFactor");
configData->PWMDeadzone = hwConfig.getFloat("PWMDeadzone");
configData->velocityUpdatePercent = hwConfig.getFloat("velocityUpdatePercent");
- configData->conditionalIntegralErrorTreshold = hwConfig.getFloat("conditionalIntegralErrorTreshold");
+ configData->conditionalIntegralErrorTreshold =
+ hwConfig.getFloat("conditionalIntegralErrorTreshold");
configData->feedForwardMode = hwConfig.getBool("FeedForwardMode");
return configData;
}
-} // namespace devices::ethercat::head_board::v1
+} // namespace devices::ethercat::head::armar6
diff --git a/source/devices/ethercat/head/armar6/PWMVelocity.h b/source/devices/ethercat/head/armar6/PWMVelocity.h
index f13d9293..d30b6239 100644
--- a/source/devices/ethercat/head/armar6/PWMVelocity.h
+++ b/source/devices/ethercat/head/armar6/PWMVelocity.h
@@ -25,27 +25,28 @@
#include <atomic>
-#include <armarx/control/hardware_config/Config.h>
-
-#include <RobotAPI/libraries/core/PIDController.h>
#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-
+#include <RobotAPI/libraries/core/PIDController.h>
+#include <armarx/control/hardware_config/Config.h>
namespace devices::ethercat::head::armar6
{
- using PWMVelocityControllerConfigurationPtr = std::shared_ptr<class PWMVelocityControllerConfiguration>;
- using PWMVelocityControllerConfigurationCPtr = std::shared_ptr<const PWMVelocityControllerConfiguration>;
+ using PWMVelocityControllerConfigurationPtr =
+ std::shared_ptr<class PWMVelocityControllerConfiguration>;
+ using PWMVelocityControllerConfigurationCPtr =
+ std::shared_ptr<const PWMVelocityControllerConfiguration>;
namespace hwconfig = armarx::control::hardware_config;
- class PWMVelocityControllerConfiguration : public armarx::RampedAccelerationVelocityControllerConfiguration
+ class PWMVelocityControllerConfiguration :
+ public armarx::RampedAccelerationVelocityControllerConfiguration
{
public:
-
PWMVelocityControllerConfiguration() = default;
- static PWMVelocityControllerConfigurationCPtr CreatePWMVelocityControllerConfigData(hwconfig::Config& hwConfig);
+ static PWMVelocityControllerConfigurationCPtr
+ CreatePWMVelocityControllerConfigData(hwconfig::Config& hwConfig);
float p;
float i;
float d;
@@ -56,24 +57,24 @@ namespace devices::ethercat::head::armar6
float velocityUpdatePercent;
float conditionalIntegralErrorTreshold;
bool feedForwardMode;
-
};
-
class PWMVelocityController
{
public:
-
- PWMVelocityController(PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr);
- double run(IceUtil::Time const& deltaT, double currentVelocity, double targetVelocity, double gravityTorque);
+ PWMVelocityController(
+ PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr);
+ double run(IceUtil::Time const& deltaT,
+ double currentVelocity,
+ double targetVelocity,
+ double gravityTorque);
void reset(double currentVelocity);
PWMVelocityControllerConfigurationCPtr config;
armarx::PIDControllerPtr pid;
std::atomic<double> lastActualVelocity;
-
};
-} // namespace devices::ethercat::head_board::v1
+} // namespace devices::ethercat::head::armar6
diff --git a/source/devices/ethercat/head/armar6/joint_controller/EmergencyStop.cpp b/source/devices/ethercat/head/armar6/joint_controller/EmergencyStop.cpp
index eed73ec6..6159a3bd 100644
--- a/source/devices/ethercat/head/armar6/joint_controller/EmergencyStop.cpp
+++ b/source/devices/ethercat/head/armar6/joint_controller/EmergencyStop.cpp
@@ -7,21 +7,24 @@
// robot_devices
#include <devices/ethercat/common/imagine_board/Data.h>
-
namespace devices::ethercat::head::armar6::joint_controller
{
- EmergencyStopController::EmergencyStopController(common::imagine_board::SensorActorDataPtr dataPtr) :
+ EmergencyStopController::EmergencyStopController(
+ common::imagine_board::SensorActorDataPtr dataPtr) :
dataPtr(dataPtr)
{
pid.reset(new armarx::PIDController(0, 5000, 0));
pid->maxIntegral = 0.1;
}
-
- void EmergencyStopController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/, const IceUtil::Time& timeSinceLastIteration)
+ void
+ EmergencyStopController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& timeSinceLastIteration)
{
- pid->update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(dataPtr->getVelocity()), 0.0);
+ pid->update(timeSinceLastIteration.toSecondsDouble(),
+ static_cast<double>(dataPtr->getVelocity()),
+ 0.0);
double targetPwm = pid->getControlValue();
if (std::isnan(targetPwm))
{
@@ -32,18 +35,18 @@ namespace devices::ethercat::head::armar6::joint_controller
// dataPtr->setTargetPWM(lastPWM);
}
-
- armarx::ControlTargetBase* EmergencyStopController::getControlTarget()
+ armarx::ControlTargetBase*
+ EmergencyStopController::getControlTarget()
{
return ⌖
}
-
- void EmergencyStopController::rtPreActivateController()
+ void
+ EmergencyStopController::rtPreActivateController()
{
pid->reset();
// ARMARX_INFO << "Stopping gripper!";
lastPWM = static_cast<float>(std::clamp(dataPtr->getTargetPWM(), -500, 500));
}
-} // namespace devices::ethercat::head_board::v1::joint_controller
+} // namespace devices::ethercat::head::armar6::joint_controller
diff --git a/source/devices/ethercat/head/armar6/joint_controller/EmergencyStop.h b/source/devices/ethercat/head/armar6/joint_controller/EmergencyStop.h
index ba02bdeb..b01b5bed 100644
--- a/source/devices/ethercat/head/armar6/joint_controller/EmergencyStop.h
+++ b/source/devices/ethercat/head/armar6/joint_controller/EmergencyStop.h
@@ -4,36 +4,30 @@
// armarx
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-
namespace armarx
{
using PIDControllerPtr = std::shared_ptr<class PIDController>;
}
-
namespace devices::ethercat::common::imagine_board
{
using SensorActorDataPtr = std::shared_ptr<class SensorActorData>;
}
-
namespace devices::ethercat::head::armar6::joint_controller
{
using EmergencyStopControllerPtr = std::shared_ptr<class EmergencyStopController>;
-
- class EmergencyStopController :
- public armarx::JointController
+ class EmergencyStopController : public armarx::JointController
{
public:
-
EmergencyStopController(common::imagine_board::SensorActorDataPtr dataPtr);
private:
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
/**
* Returns the Target for this controller, but as this is the Emergency controller it will ignored.
* As this controller will just break
@@ -44,12 +38,10 @@ namespace devices::ethercat::head::armar6::joint_controller
void rtPreActivateController() override;
private:
-
armarx::DummyControlTargetEmergencyStop target;
common::imagine_board::SensorActorDataPtr dataPtr;
armarx::PIDControllerPtr pid;
float lastPWM = 0.0f;
-
};
-} // namespace devices::ethercat::head_board::v1::joint_controller
+} // namespace devices::ethercat::head::armar6::joint_controller
diff --git a/source/devices/ethercat/head/armar6/joint_controller/PWM.cpp b/source/devices/ethercat/head/armar6/joint_controller/PWM.cpp
index 55107957..7217d083 100644
--- a/source/devices/ethercat/head/armar6/joint_controller/PWM.cpp
+++ b/source/devices/ethercat/head/armar6/joint_controller/PWM.cpp
@@ -8,16 +8,15 @@
namespace devices::ethercat::head::armar6::joint_controller
{
- PWMController::PWMController(
- const std::string&,
- Device*,
- common::imagine_board::SensorActorDataPtr jointData
- ) : dataPtr(jointData)
+ PWMController::PWMController(const std::string&,
+ Device*,
+ common::imagine_board::SensorActorDataPtr jointData) :
+ dataPtr(jointData)
{
}
-
- void PWMController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
+ void
+ PWMController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
{
if (target.isValid())
{
@@ -29,10 +28,10 @@ namespace devices::ethercat::head::armar6::joint_controller
}
}
-
- armarx::ControlTargetBase* PWMController::getControlTarget()
+ armarx::ControlTargetBase*
+ PWMController::getControlTarget()
{
return ⌖
}
-} // namespace devices::ethercat::head_board::v1::joint_controller
+} // namespace devices::ethercat::head::armar6::joint_controller
diff --git a/source/devices/ethercat/head/armar6/joint_controller/PWM.h b/source/devices/ethercat/head/armar6/joint_controller/PWM.h
index b9bc55df..23b0d381 100644
--- a/source/devices/ethercat/head/armar6/joint_controller/PWM.h
+++ b/source/devices/ethercat/head/armar6/joint_controller/PWM.h
@@ -2,13 +2,12 @@
// armarx
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
// robot_devices
#include <devices/ethercat/head/armar6/ControlTargets.h>
-
namespace devices::ethercat::common::imagine_board
{
using SensorActorDataPtr = std::shared_ptr<class SensorActorData>;
@@ -21,23 +20,23 @@ namespace devices::ethercat::head::armar6
namespace devices::ethercat::head::armar6::joint_controller
{
- using PWMControllerPtr = std::shared_ptr<class PWMController> ;
+ using PWMControllerPtr = std::shared_ptr<class PWMController>;
class PWMController : public armarx::JointController
{
public:
+ PWMController(const std::string&,
+ Device*,
+ common::imagine_board::SensorActorDataPtr jointData);
- PWMController(const std::string&, Device*, common::imagine_board::SensorActorDataPtr jointData);
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
protected:
-
ControlTargetHeadPWM target;
common::imagine_board::SensorActorDataPtr dataPtr;
-
};
-} // namespace devices::ethercat::head_board::v1::joint_controller
+} // namespace devices::ethercat::head::armar6::joint_controller
diff --git a/source/devices/ethercat/head/armar6/joint_controller/PWMPosition.cpp b/source/devices/ethercat/head/armar6/joint_controller/PWMPosition.cpp
index 45ca64f7..ed8cbe00 100644
--- a/source/devices/ethercat/head/armar6/joint_controller/PWMPosition.cpp
+++ b/source/devices/ethercat/head/armar6/joint_controller/PWMPosition.cpp
@@ -4,12 +4,10 @@
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
-
-#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
-
-#include <RobotAPI/libraries/core/PIDController.h>
+#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
+#include <RobotAPI/libraries/core/PIDController.h>
// robot_devices
#include <devices/ethercat/common/imagine_board/Data.h>
@@ -18,24 +16,33 @@
namespace devices::ethercat::head::armar6::joint_controller
{
- PWMPositionController::PWMPositionController(const std::string& deviceName, Device* board,
- common::imagine_board::SensorActorDataPtr jointData,
- PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr) : JointController(),
+ PWMPositionController::PWMPositionController(
+ const std::string& deviceName,
+ Device* board,
+ common::imagine_board::SensorActorDataPtr jointData,
+ PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr) :
+ JointController(),
config(positionControllerConfigDataPtr),
// controller(positionControllerConfigDataPtr),
- target(), board(board), deviceName(deviceName)
+ target(),
+ board(board),
+ deviceName(deviceName)
{
actorIndex = board->getActorIndex(deviceName);
- sensorValue = std::dynamic_pointer_cast<Device::ActorRobotUnitDevice>(board->getSubDevices().at(actorIndex))->getSensorValue()->asA<armarx::SensorValue1DoFActuator>();
+ sensorValue = std::dynamic_pointer_cast<Device::ActorRobotUnitDevice>(
+ board->getSubDevices().at(actorIndex))
+ ->getSensorValue()
+ ->asA<armarx::SensorValue1DoFActuator>();
ARMARX_CHECK_EXPRESSION(sensorValue) << deviceName;
dataPtr = jointData;
- posController.desiredDeceleration = static_cast<double>(positionControllerConfigDataPtr->maxDecelerationRad);
+ posController.desiredDeceleration =
+ static_cast<double>(positionControllerConfigDataPtr->maxDecelerationRad);
posController.desiredJerk = 100;
posController.maxDt = static_cast<double>(positionControllerConfigDataPtr->maxDt);
posController.maxV = static_cast<double>(positionControllerConfigDataPtr->maxVelocityRad);
posController.p = 4;
- posController.phase2SwitchDistance = 0.1;
+ posController.phase2SwitchDistance = 0.1;
ARMARX_CHECK_GREATER_EQUAL(jointData->getSoftLimitHi(), jointData->getSoftLimitLo());
// controller.positionLimitHiHard = dataPtr->getHardLimitHi();
// posController.positionLimitHi = jointData->getSoftLimitHi();
@@ -46,12 +53,14 @@ namespace devices::ethercat::head::armar6::joint_controller
// ARMARX_IMPORTANT << "position Kp " << posController.pid->Kp;
this->isLimitless = jointData->isLimitless();
- pidPosController.reset(new armarx::PIDController(positionControllerConfigDataPtr->p, positionControllerConfigDataPtr->i, positionControllerConfigDataPtr->d));
- pidPosController->maxIntegral = static_cast<double>(positionControllerConfigDataPtr->maxIntegral);
+ pidPosController.reset(new armarx::PIDController(positionControllerConfigDataPtr->p,
+ positionControllerConfigDataPtr->i,
+ positionControllerConfigDataPtr->d));
+ pidPosController->maxIntegral =
+ static_cast<double>(positionControllerConfigDataPtr->maxIntegral);
pidPosController->differentialFilter.reset(new armarx::rtfilters::AverageFilter(10));
}
-
PWMPositionController::~PWMPositionController() noexcept(true)
{
stopRequested = true;
@@ -61,12 +70,12 @@ namespace devices::ethercat::head::armar6::joint_controller
}
catch (...)
{
-
}
}
-
- void PWMPositionController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/, const IceUtil::Time& timeSinceLastIteration)
+ void
+ PWMPositionController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (target.isValid())
{
@@ -74,7 +83,8 @@ namespace devices::ethercat::head::armar6::joint_controller
if (isLimitless)
{
- ARMARX_RT_LOGF_WARNING("Position controller not implemented for limitless joints").deactivateSpam(10);
+ ARMARX_RT_LOGF_WARNING("Position controller not implemented for limitless joints")
+ .deactivateSpam(10);
return;
}
else
@@ -96,7 +106,9 @@ namespace devices::ethercat::head::armar6::joint_controller
{
newVel = 0;
}
- pidPosController->update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(currentPosition), r.position);
+ pidPosController->update(timeSinceLastIteration.toSecondsDouble(),
+ static_cast<double>(currentPosition),
+ r.position);
auto targetPWM = pidPosController->getControlValue();
// auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel));
newVel = std::clamp(newVel, -posController.maxV, posController.maxV);
@@ -111,7 +123,8 @@ namespace devices::ethercat::head::armar6::joint_controller
if (std::isnan(targetPWM))
{
- ARMARX_ERROR << deactivateSpam(1) << "Target PWM of " << getParent().getDeviceName() << " is NaN!";
+ ARMARX_ERROR << deactivateSpam(1) << "Target PWM of " << getParent().getDeviceName()
+ << " is NaN!";
targetPWM = 0.0f;
}
float updateRatio = 0.3;
@@ -130,23 +143,22 @@ namespace devices::ethercat::head::armar6::joint_controller
// ARMARX_RT_LOGF_INFO("%s: position: %.f, target position: %.f, targetvelocity: %.f, target PWM: %d", name,
// currentPosition, targetPosition, newVel, targetPWM).deactivateSpam(1);
// ARMARX_INFO << deactivateSpam(1) << VAROUT(name) << VAROUT(currentPosition) << VAROUT(targetPosition) << VAROUT(newVel) << VAROUT(targetPWM);
-
-
}
else
{
- ARMARX_ERROR << deactivateSpam(1) << "invalid target set for actor " << getParent().getDeviceName();
+ ARMARX_ERROR << deactivateSpam(1) << "invalid target set for actor "
+ << getParent().getDeviceName();
}
}
-
- armarx::ControlTargetBase* PWMPositionController::getControlTarget()
+ armarx::ControlTargetBase*
+ PWMPositionController::getControlTarget()
{
return ⌖
}
-
- void PWMPositionController::rtPreActivateController()
+ void
+ PWMPositionController::rtPreActivateController()
{
targetPWM = 0.0;
lastTargetVelocity = static_cast<double>(dataPtr->getVelocity());
@@ -158,138 +170,158 @@ namespace devices::ethercat::head::armar6::joint_controller
// controller.reset(dataPtr->getVelocity());
}
-
- void PWMPositionController::rtPostDeactivateController()
+ void
+ PWMPositionController::rtPostDeactivateController()
{
// ARMARX_RT_LOGF_INFO("Setting PWM to 0");
// dataPtr->setTargetPWM(0);
}
-
- armarx::StringVariantBaseMap PWMPositionController::publish(const armarx::DebugDrawerInterfacePrx& /*draw*/, const armarx::DebugObserverInterfacePrx& /*observer*/) const
+ armarx::StringVariantBaseMap
+ PWMPositionController::publish(const armarx::DebugDrawerInterfacePrx& /*draw*/,
+ const armarx::DebugObserverInterfacePrx& /*observer*/) const
{
if (!remoteGui)
{
- threadHandle = armarx::Application::getInstance()->getThreadPool()->runTask([this]
- {
- std::string guiTabName;
- while (!stopRequested)
+ threadHandle = armarx::Application::getInstance()->getThreadPool()->runTask(
+ [this]
{
- armarx::ManagedIceObjectPtr object;
- ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
- try
+ std::string guiTabName;
+ while (!stopRequested)
{
- object = armarx::ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
- ARMARX_CHECK_EXPRESSION(object);
- remoteGui = object->getProxy<armarx::RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
- if (!remoteGui)
+ armarx::ManagedIceObjectPtr object;
+ ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
+ try
{
- continue;
+ object =
+ armarx::ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
+ ARMARX_CHECK_EXPRESSION(object);
+ remoteGui = object->getProxy<armarx::RemoteGuiInterfacePrx>(
+ "RemoteGuiProvider", false, "", false);
+ if (!remoteGui)
+ {
+ continue;
+ }
+ ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
+ guiTabName = getParent().getDeviceName() + getControlMode();
+ break;
+ }
+ catch (...)
+ {
+ sleep(1);
}
- ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
- guiTabName = getParent().getDeviceName() + getControlMode();
- break;
- }
- catch (...)
- {
- sleep(1);
}
- }
-
- if (remoteGui)
- {
- ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
- using namespace armarx::RemoteGui;
-
- auto vLayout = makeVBoxLayout();
+ if (remoteGui)
{
- WidgetPtr KpLabel = makeTextLabel("Kp: ");
+ ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
+ using namespace armarx::RemoteGui;
- WidgetPtr KpSlider = makeFloatSlider("KpSlider")
- .min(0.0f).max(pidPosController->Kp * 5)
- .value(pidPosController->Kp);
- WidgetPtr KpLabelValue = makeTextLabel(std::to_string(pidPosController->Kp * 5));
- WidgetPtr line = makeHBoxLayout()
- .children({KpLabel, KpSlider, KpLabelValue});
+ auto vLayout = makeVBoxLayout();
- vLayout.addChild(line);
- }
-
- {
- WidgetPtr KiLabel = makeTextLabel("Ki: ");
- WidgetPtr KiSlider = makeFloatSlider("KiSlider")
- .min(0.0f).max(pidPosController->Ki * 5)
- .value(pidPosController->Ki);
- WidgetPtr KiLabelValue = makeTextLabel(std::to_string(pidPosController->Ki * 5));
-
- WidgetPtr line = makeHBoxLayout()
- .children({KiLabel, KiSlider, KiLabelValue});
+ {
+ WidgetPtr KpLabel = makeTextLabel("Kp: ");
+
+ WidgetPtr KpSlider = makeFloatSlider("KpSlider")
+ .min(0.0f)
+ .max(pidPosController->Kp * 5)
+ .value(pidPosController->Kp);
+ WidgetPtr KpLabelValue =
+ makeTextLabel(std::to_string(pidPosController->Kp * 5));
+ WidgetPtr line =
+ makeHBoxLayout().children({KpLabel, KpSlider, KpLabelValue});
+
+ vLayout.addChild(line);
+ }
- vLayout.addChild(line);
- }
+ {
+ WidgetPtr KiLabel = makeTextLabel("Ki: ");
+ WidgetPtr KiSlider = makeFloatSlider("KiSlider")
+ .min(0.0f)
+ .max(pidPosController->Ki * 5)
+ .value(pidPosController->Ki);
+ WidgetPtr KiLabelValue =
+ makeTextLabel(std::to_string(pidPosController->Ki * 5));
+
+ WidgetPtr line =
+ makeHBoxLayout().children({KiLabel, KiSlider, KiLabelValue});
+
+ vLayout.addChild(line);
+ }
- {
- WidgetPtr KdLabel = makeTextLabel("Kd: ");
- WidgetPtr KdSlider = makeFloatSlider("KdSlider")
- .min(-10.0f * pidPosController->Kd).max(10.0f * pidPosController->Kd)
- .steps(1000)
- .value(pidPosController->Kd);
- WidgetPtr KdLabelValue = makeTextLabel(std::to_string(pidPosController->Kd * 10));
-
- WidgetPtr line = makeHBoxLayout()
- .children({KdLabel, KdSlider, KdLabelValue});
-
- vLayout.addChild(line);
- vLayout.addChild(new VSpacer);
- }
+ {
+ WidgetPtr KdLabel = makeTextLabel("Kd: ");
+ WidgetPtr KdSlider = makeFloatSlider("KdSlider")
+ .min(-10.0f * pidPosController->Kd)
+ .max(10.0f * pidPosController->Kd)
+ .steps(1000)
+ .value(pidPosController->Kd);
+ WidgetPtr KdLabelValue =
+ makeTextLabel(std::to_string(pidPosController->Kd * 10));
+
+ WidgetPtr line =
+ makeHBoxLayout().children({KdLabel, KdSlider, KdLabelValue});
+
+ vLayout.addChild(line);
+ vLayout.addChild(new VSpacer);
+ }
- // WidgetPtr spin = makeFloatSpinBox("KpSpin")
- // .min(0.0f).max(2.0f)
- // .steps(20).decimals(2)
- // .value(0.4f);
+ // WidgetPtr spin = makeFloatSpinBox("KpSpin")
+ // .min(0.0f).max(2.0f)
+ // .steps(20).decimals(2)
+ // .value(0.4f);
- WidgetPtr groupBox = makeGroupBox("GroupBox")
- .label("Group")
- .child(vLayout);
+ WidgetPtr groupBox = makeGroupBox("GroupBox").label("Group").child(vLayout);
- remoteGui->createTab(guiTabName, groupBox);
+ remoteGui->createTab(guiTabName, groupBox);
- while (!stopRequested)
- {
- armarx::RemoteGui::TabProxy tab(remoteGui, guiTabName);
- tab.receiveUpdates();
- // this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
- // this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
- // this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
- pidPosController->Kp = tab.getValue<float>("KpSlider").get();
- pidPosController->Ki = tab.getValue<float>("KiSlider").get();
- pidPosController->Kd = tab.getValue<float>("KdSlider").get();
- usleep(100000);
+ while (!stopRequested)
+ {
+ armarx::RemoteGui::TabProxy tab(remoteGui, guiTabName);
+ tab.receiveUpdates();
+ // this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
+ // this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
+ // this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
+ pidPosController->Kp = tab.getValue<float>("KpSlider").get();
+ pidPosController->Ki = tab.getValue<float>("KiSlider").get();
+ pidPosController->Kd = tab.getValue<float>("KdSlider").get();
+ usleep(100000);
+ }
}
- }
-
- });
+ });
}
- return {{"lastTargetVelocity", new armarx::Variant(lastTargetVelocity.load())},
- {"targetPosition", new armarx::Variant(posController.currentPosition)}, // position of profile generator is target position
- {"posError", new armarx::Variant(posController.getTargetPosition() - posController.currentPosition)},
+ return {
+ {"lastTargetVelocity", new armarx::Variant(lastTargetVelocity.load())},
+ {"targetPosition",
+ new armarx::Variant(
+ posController
+ .currentPosition)}, // position of profile generator is target position
+ {"posError",
+ new armarx::Variant(posController.getTargetPosition() -
+ posController.currentPosition)},
{"pidError", new armarx::Variant(pidPosController->previousError)},
// {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
- {"pidIntegralCV", new armarx::Variant(pidPosController->integral * static_cast<double>(pidPosController->Ki))},
+ {"pidIntegralCV",
+ new armarx::Variant(pidPosController->integral *
+ static_cast<double>(pidPosController->Ki))},
{"pidIntegral", new armarx::Variant(pidPosController->integral)},
- {"pidPropCV", new armarx::Variant(pidPosController->previousError * static_cast<double>(pidPosController->Kp))},
- {"pidDiffCV", new armarx::Variant(pidPosController->derivative * static_cast<double>(pidPosController->Kd))},
+ {"pidPropCV",
+ new armarx::Variant(pidPosController->previousError *
+ static_cast<double>(pidPosController->Kp))},
+ {"pidDiffCV",
+ new armarx::Variant(pidPosController->derivative *
+ static_cast<double>(pidPosController->Kd))},
// {"pospidIntegralCV", new Variant(posController.pid->integral * posController.pid->Ki)},
// {"pospidIntegral", new Variant(posController.pid->integral)},
// {"pospidPropCV", new Variant(posController.pid->previousError * posController.pid->Kp)},
// {"pospidDiffCV", new Variant(posController.pid->derivative * posController.pid->Kd)},
// {"pidUsed", new Variant(posController.getCurrentlyPIDActive())},
- {"desiredPWM", new armarx::Variant(targetPWM.load())}
- };
+ {"desiredPWM", new armarx::Variant(targetPWM.load())}};
}
- PWMPositionControllerConfigurationCPtr PWMPositionControllerConfiguration::CreatePWMPositionControllerConfigData(armarx::control::hardware_config::Config& hwConfig)
+ PWMPositionControllerConfigurationCPtr
+ PWMPositionControllerConfiguration::CreatePWMPositionControllerConfigData(
+ armarx::control::hardware_config::Config& hwConfig)
{
PWMPositionControllerConfiguration configData;
@@ -301,14 +333,16 @@ namespace devices::ethercat::head::armar6::joint_controller
configData.i = hwConfig.getFloat("i");
configData.d = hwConfig.getFloat("d");
configData.maxIntegral = hwConfig.getFloat("maxIntegral");
- configData.feedforwardVelocityToPWMFactor = hwConfig.getFloat("feedforwardVelocityToPWMFactor");
+ configData.feedforwardVelocityToPWMFactor =
+ hwConfig.getFloat("feedforwardVelocityToPWMFactor");
configData.feedforwardTorqueToPWMFactor = hwConfig.getFloat("feedforwardTorqueToPWMFactor");
configData.PWMDeadzone = hwConfig.getFloat("PWMDeadzone");
configData.velocityUpdatePercent = hwConfig.getFloat("velocityUpdatePercent");
- configData.conditionalIntegralErrorTreshold = hwConfig.getFloat("conditionalIntegralErrorTreshold");
+ configData.conditionalIntegralErrorTreshold =
+ hwConfig.getFloat("conditionalIntegralErrorTreshold");
configData.feedForwardMode = hwConfig.getBool("FeedForwardMode");
return std::make_shared<PWMPositionControllerConfiguration>(configData);
}
-} // namespace devices::ethercat::head_board::v1::joint_controller
+} // namespace devices::ethercat::head::armar6::joint_controller
diff --git a/source/devices/ethercat/head/armar6/joint_controller/PWMPosition.h b/source/devices/ethercat/head/armar6/joint_controller/PWMPosition.h
index 0e5fe0ec..1cd31d99 100644
--- a/source/devices/ethercat/head/armar6/joint_controller/PWMPosition.h
+++ b/source/devices/ethercat/head/armar6/joint_controller/PWMPosition.h
@@ -4,13 +4,12 @@
// armarx
#include <ArmarXCore/core/services/tasks/ThreadPool.h>
#include <ArmarXGui/interface/RemoteGuiInterface.h>
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
#include <armarx/control/hardware_config/Config.h>
-
namespace armarx
{
using PIDControllerPtr = std::shared_ptr<class PIDController>;
@@ -28,16 +27,21 @@ namespace devices::ethercat::head::armar6
namespace devices::ethercat::head::armar6::joint_controller
{
- using PWMPositionControllerConfigurationPtr = std::shared_ptr<class PWMPositionControllerConfiguration>;
- using PWMPositionControllerConfigurationCPtr = std::shared_ptr<const PWMPositionControllerConfiguration>;
+ using PWMPositionControllerConfigurationPtr =
+ std::shared_ptr<class PWMPositionControllerConfiguration>;
+ using PWMPositionControllerConfigurationCPtr =
+ std::shared_ptr<const PWMPositionControllerConfiguration>;
class PWMPositionControllerConfiguration
{
public:
+ PWMPositionControllerConfiguration()
+ {
+ }
- PWMPositionControllerConfiguration() {}
- static PWMPositionControllerConfigurationCPtr CreatePWMPositionControllerConfigData(armarx::control::hardware_config::Config& hwConfig);
+ static PWMPositionControllerConfigurationCPtr
+ CreatePWMPositionControllerConfigData(armarx::control::hardware_config::Config& hwConfig);
float maxVelocityRad;
float maxAccelerationRad;
float maxDecelerationRad;
@@ -52,32 +56,32 @@ namespace devices::ethercat::head::armar6::joint_controller
float velocityUpdatePercent;
float conditionalIntegralErrorTreshold;
bool feedForwardMode;
-
};
using PWMPositionControllerPtr = std::shared_ptr<class PWMPositionController>;
-
- class PWMPositionController :
- public armarx::JointController
+ class PWMPositionController : public armarx::JointController
{
public:
-
- PWMPositionController(const std::string& deviceName,
- Device* board,
- common::imagine_board::SensorActorDataPtr jointData,
- PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr);
+ PWMPositionController(
+ const std::string& deviceName,
+ Device* board,
+ common::imagine_board::SensorActorDataPtr jointData,
+ PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr);
~PWMPositionController() noexcept(true);
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
void rtPreActivateController() override;
void rtPostDeactivateController() override;
- armarx::StringVariantBaseMap publish(const armarx::DebugDrawerInterfacePrx& draw, const armarx::DebugObserverInterfacePrx& observer) const override;
+ armarx::StringVariantBaseMap
+ publish(const armarx::DebugDrawerInterfacePrx& draw,
+ const armarx::DebugObserverInterfacePrx& observer) const override;
protected:
PWMPositionControllerConfigurationCPtr config;
@@ -98,7 +102,6 @@ namespace devices::ethercat::head::armar6::joint_controller
bool stopRequested = false;
mutable armarx::ThreadPool::Handle threadHandle;
const armarx::SensorValue1DoFActuator* sensorValue;
-
};
-} // namespace devices::ethercat::head_board::v1::joint_controller
+} // namespace devices::ethercat::head::armar6::joint_controller
diff --git a/source/devices/ethercat/head/armar6/joint_controller/PWMVelocity.cpp b/source/devices/ethercat/head/armar6/joint_controller/PWMVelocity.cpp
index c22afaf3..5cdcd29b 100644
--- a/source/devices/ethercat/head/armar6/joint_controller/PWMVelocity.cpp
+++ b/source/devices/ethercat/head/armar6/joint_controller/PWMVelocity.cpp
@@ -15,16 +15,23 @@
namespace devices::ethercat::head::armar6::joint_controller
{
- PWMVelocityController::PWMVelocityController(const std::string& deviceName,
- Device* board,
- common::imagine_board::SensorActorDataPtr jointData,
- PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) : JointController(),
+ PWMVelocityController::PWMVelocityController(
+ const std::string& deviceName,
+ Device* board,
+ common::imagine_board::SensorActorDataPtr jointData,
+ PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) :
+ JointController(),
config(velocityControllerConfigDataPtr),
controller(velocityControllerConfigDataPtr),
- target(), board(board), deviceName(deviceName)
+ target(),
+ board(board),
+ deviceName(deviceName)
{
actorIndex = board->getActorIndex(deviceName);
- sensorValue = std::dynamic_pointer_cast<Device::ActorRobotUnitDevice>(board->getSubDevices().at(actorIndex))->getSensorValue()->asA<armarx::SensorValue1DoFActuator>();
+ sensorValue = std::dynamic_pointer_cast<Device::ActorRobotUnitDevice>(
+ board->getSubDevices().at(actorIndex))
+ ->getSensorValue()
+ ->asA<armarx::SensorValue1DoFActuator>();
ARMARX_CHECK_EXPRESSION(sensorValue) << deviceName;
dataPtr = jointData;
@@ -42,7 +49,6 @@ namespace devices::ethercat::head::armar6::joint_controller
this->isLimitless = jointData->isLimitless();
}
-
PWMVelocityController::~PWMVelocityController() noexcept(true)
{
stopRequested = true;
@@ -52,24 +58,26 @@ namespace devices::ethercat::head::armar6::joint_controller
}
catch (...)
{
-
}
}
-
- void PWMVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ PWMVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (target.isValid())
{
auto currentPosition = dataPtr->getPosition();
if (isLimitless)
{
- velController.currentPosition = velController.positionLimitHiSoft - (velController.positionLimitHiSoft - velController.positionLimitLoSoft) * 0.5;
+ velController.currentPosition =
+ velController.positionLimitHiSoft -
+ (velController.positionLimitHiSoft - velController.positionLimitLoSoft) * 0.5;
// ARMARX_INFO << VAROUT(velController.currentPosition) << VAROUT(velController.positionLimitLoSoft) << VAROUT(velController.positionLimitHiSoft);
}
else
{
- velController.currentPosition = currentPosition;
+ velController.currentPosition = currentPosition;
}
velController.currentV = lastTargetVelocity;
velController.currentAcc = lastTargetAcceleration;
@@ -86,15 +94,19 @@ namespace devices::ethercat::head::armar6::joint_controller
newAcc = 0;
}
// float newVel = target.velocity;
- if ((currentPosition > velController.positionLimitHiSoft && target.velocity > 0)
- || (currentPosition < velController.positionLimitLoSoft && target.velocity < 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();
+ ARMARX_INFO << deactivateSpam(1) << "Breaking now at " << dataPtr->getPosition()
+ << " pwm: " << dataPtr->getTargetPWM();
}
- auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel, sensorValue->gravityTorque));
+ auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration,
+ dataPtr->getVelocity(),
+ newVel,
+ sensorValue->gravityTorque));
dataPtr->setTargetPWM(targetPWM);
lastTargetVelocity = newVel;
@@ -109,130 +121,127 @@ namespace devices::ethercat::head::armar6::joint_controller
}
}
-
- armarx::ControlTargetBase* PWMVelocityController::getControlTarget()
+ armarx::ControlTargetBase*
+ PWMVelocityController::getControlTarget()
{
return ⌖
}
-
- void PWMVelocityController::rtPreActivateController()
+ void
+ PWMVelocityController::rtPreActivateController()
{
lastTargetVelocity = dataPtr->getVelocity();
lastTargetAcceleration = dataPtr->getAcceleration();
controller.reset(static_cast<double>(dataPtr->getVelocity()));
}
-
- void PWMVelocityController::rtPostDeactivateController()
+ void
+ PWMVelocityController::rtPostDeactivateController()
{
// ARMARX_RT_LOGF_INFO("Setting PWM to 0");
// dataPtr->setTargetPWM(0);
}
-
- armarx::StringVariantBaseMap PWMVelocityController::publish(const armarx::DebugDrawerInterfacePrx& /*draw*/, const armarx::DebugObserverInterfacePrx& /*observer*/) const
+ armarx::StringVariantBaseMap
+ PWMVelocityController::publish(const armarx::DebugDrawerInterfacePrx& /*draw*/,
+ const armarx::DebugObserverInterfacePrx& /*observer*/) const
{
if (!remoteGui && !threadHandle.isValid())
{
- threadHandle = armarx::Application::getInstance()->getThreadPool()->runTask([this]
- {
- std::string guiTabName;
- while (!stopRequested)
+ threadHandle = armarx::Application::getInstance()->getThreadPool()->runTask(
+ [this]
{
- armarx::ManagedIceObjectPtr object;
- ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
- try
+ std::string guiTabName;
+ while (!stopRequested)
{
- object = armarx::ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
- ARMARX_CHECK_EXPRESSION(object);
- remoteGui = object->getProxy<armarx::RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
- if (!remoteGui)
+ armarx::ManagedIceObjectPtr object;
+ ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
+ try
{
- return;
+ object =
+ armarx::ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
+ ARMARX_CHECK_EXPRESSION(object);
+ remoteGui = object->getProxy<armarx::RemoteGuiInterfacePrx>(
+ "RemoteGuiProvider", false, "", false);
+ if (!remoteGui)
+ {
+ return;
+ }
+ ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
+ guiTabName = getParent().getDeviceName() + getControlMode();
+ break;
+ }
+ catch (...)
+ {
+ armarx::handleExceptions();
+ sleep(1);
}
- ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
- guiTabName = getParent().getDeviceName() + getControlMode();
- break;
- }
- catch (...)
- {
- armarx::handleExceptions();
- sleep(1);
}
- }
- if (remoteGui)
- {
- ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
- using namespace armarx::RemoteGui;
-
- auto vLayout = makeVBoxLayout();
-
+ if (remoteGui)
{
- WidgetPtr KpLabel = makeTextLabel("Kp: ");
+ ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
+ using namespace armarx::RemoteGui;
- WidgetPtr KiSlider = makeFloatSlider("KpSlider")
- .min(0.0f).max(5000.0f)
- .value(config->p);
- WidgetPtr line = makeHBoxLayout()
- .children({KpLabel, KiSlider});
+ auto vLayout = makeVBoxLayout();
- vLayout.addChild(line);
+ {
+ WidgetPtr KpLabel = makeTextLabel("Kp: ");
- }
+ WidgetPtr KiSlider =
+ makeFloatSlider("KpSlider").min(0.0f).max(5000.0f).value(config->p);
+ WidgetPtr line = makeHBoxLayout().children({KpLabel, KiSlider});
- {
- WidgetPtr KiLabel = makeTextLabel("Ki: ");
- WidgetPtr KiSlider = makeFloatSlider("KiSlider")
- .min(0.0f).max(50000.0f)
- .value(config->i);
+ vLayout.addChild(line);
+ }
- WidgetPtr line = makeHBoxLayout()
- .children({KiLabel, KiSlider});
+ {
+ WidgetPtr KiLabel = makeTextLabel("Ki: ");
+ WidgetPtr KiSlider = makeFloatSlider("KiSlider")
+ .min(0.0f)
+ .max(50000.0f)
+ .value(config->i);
- vLayout.addChild(line);
+ WidgetPtr line = makeHBoxLayout().children({KiLabel, KiSlider});
- }
+ vLayout.addChild(line);
+ }
- {
- WidgetPtr KdLabel = makeTextLabel("Kd: ");
- WidgetPtr KdSlider = makeFloatSlider("KdSlider")
- .min(0.0f).max(50.0f)
- .steps(100)
- .value(config->d);
+ {
+ WidgetPtr KdLabel = makeTextLabel("Kd: ");
+ WidgetPtr KdSlider = makeFloatSlider("KdSlider")
+ .min(0.0f)
+ .max(50.0f)
+ .steps(100)
+ .value(config->d);
- WidgetPtr line = makeHBoxLayout()
- .children({KdLabel, KdSlider});
+ WidgetPtr line = makeHBoxLayout().children({KdLabel, KdSlider});
- vLayout.addChild(line);
- vLayout.addChild(new VSpacer);
- }
+ vLayout.addChild(line);
+ vLayout.addChild(new VSpacer);
+ }
- // WidgetPtr spin = makeFloatSpinBox("KpSpin")
- // .min(0.0f).max(2.0f)
- // .steps(20).decimals(2)
- // .value(0.4f);
+ // WidgetPtr spin = makeFloatSpinBox("KpSpin")
+ // .min(0.0f).max(2.0f)
+ // .steps(20).decimals(2)
+ // .value(0.4f);
- WidgetPtr groupBox = makeGroupBox("GroupBox")
- .label("Group")
- .child(vLayout);
+ WidgetPtr groupBox = makeGroupBox("GroupBox").label("Group").child(vLayout);
- remoteGui->createTab(guiTabName, groupBox);
+ remoteGui->createTab(guiTabName, groupBox);
- while (!stopRequested)
- {
- armarx::RemoteGui::TabProxy tab(remoteGui, guiTabName);
- tab.receiveUpdates();
- this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
- this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
- this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
- usleep(100000);
+ while (!stopRequested)
+ {
+ armarx::RemoteGui::TabProxy tab(remoteGui, guiTabName);
+ tab.receiveUpdates();
+ this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
+ this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
+ this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
+ usleep(100000);
+ }
}
- }
- });
+ });
}
- return
- {
+ return {
{"lastTargetVelocity", new armarx::Variant(lastTargetVelocity.load())},
{"lastTargetAcceleration", new armarx::Variant(lastTargetAcceleration.load())},
{"filteredVelocity", new armarx::Variant(controller.lastActualVelocity.load())},
@@ -243,4 +252,4 @@ namespace devices::ethercat::head::armar6::joint_controller
};
}
-} // namespace devices::ethercat::head_board::v1::joint_controller
+} // namespace devices::ethercat::head::armar6::joint_controller
diff --git a/source/devices/ethercat/head/armar6/joint_controller/PWMVelocity.h b/source/devices/ethercat/head/armar6/joint_controller/PWMVelocity.h
index a6d770cb..f5aa545c 100644
--- a/source/devices/ethercat/head/armar6/joint_controller/PWMVelocity.h
+++ b/source/devices/ethercat/head/armar6/joint_controller/PWMVelocity.h
@@ -3,58 +3,57 @@
// armarx
#include <ArmarXCore/core/services/tasks/ThreadPool.h>
#include <ArmarXGui/interface/RemoteGuiInterface.h>
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
// robot_devices
#include <devices/ethercat/head/armar6/PWMVelocity.h>
-
namespace armarx
{
using PIDControllerPtr = std::shared_ptr<class PIDController>;
}
-
namespace devices::ethercat::common::imagine_board
{
using SensorActorDataPtr = std::shared_ptr<class SensorActorData>;
}
-
namespace devices::ethercat::head::armar6
{
class Device;
- using PWMVelocityControllerConfigurationCPtr = std::shared_ptr<const class PWMVelocityControllerConfiguration>;
-}
+ using PWMVelocityControllerConfigurationCPtr =
+ std::shared_ptr<const class PWMVelocityControllerConfiguration>;
+} // namespace devices::ethercat::head::armar6
namespace devices::ethercat::head::armar6::joint_controller
{
- class PWMVelocityController :
- public armarx::JointController
+ class PWMVelocityController : public armarx::JointController
{
public:
-
- PWMVelocityController(const std::string& deviceName,
- Device* board,
- common::imagine_board::SensorActorDataPtr jointData,
- PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr);
+ PWMVelocityController(
+ const std::string& deviceName,
+ Device* board,
+ common::imagine_board::SensorActorDataPtr jointData,
+ PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr);
~PWMVelocityController() noexcept(true);
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
- armarx::StringVariantBaseMap publish(const armarx::DebugDrawerInterfacePrx& draw, const armarx::DebugObserverInterfacePrx& observer) const override;
+ armarx::StringVariantBaseMap
+ publish(const armarx::DebugDrawerInterfacePrx& draw,
+ const armarx::DebugObserverInterfacePrx& observer) const override;
void rtPreActivateController() override;
void rtPostDeactivateController() override;
protected:
-
PWMVelocityControllerConfigurationCPtr config;
devices::ethercat::head::armar6::PWMVelocityController controller;
armarx::VelocityControllerWithRampedAccelerationAndPositionBounds velController;
@@ -72,7 +71,6 @@ namespace devices::ethercat::head::armar6::joint_controller
bool stopRequested = false;
mutable armarx::ThreadPool::Handle threadHandle;
const armarx::SensorValue1DoFActuator* sensorValue;
-
};
-} // namespace devices::ethercat::head_board::v1::joint_controller
+} // namespace devices::ethercat::head::armar6::joint_controller
diff --git a/source/devices/ethercat/head/armar6/joint_controller/StopMovement.cpp b/source/devices/ethercat/head/armar6/joint_controller/StopMovement.cpp
index 3aed616d..8d28e54e 100644
--- a/source/devices/ethercat/head/armar6/joint_controller/StopMovement.cpp
+++ b/source/devices/ethercat/head/armar6/joint_controller/StopMovement.cpp
@@ -4,19 +4,20 @@
// robot_devices
#include <devices/ethercat/common/imagine_board/Data.h>
-
namespace devices::ethercat::head::armar6::joint_controller
{
- StopMovementController::StopMovementController(common::imagine_board::SensorActorDataPtr dataPtr) :
+ StopMovementController::StopMovementController(
+ common::imagine_board::SensorActorDataPtr dataPtr) :
dataPtr(dataPtr)
{
pid.reset(new armarx::PIDController(0, 5000, 0));
pid->maxIntegral = 0.1;
}
-
- void StopMovementController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ StopMovementController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
pid->update(timeSinceLastIteration.toSecondsDouble(), dataPtr->getVelocity(), 0.0);
float targetPwm = pid->getControlValue();
@@ -29,16 +30,16 @@ namespace devices::ethercat::head::armar6::joint_controller
dataPtr->setTargetPWM(targetPwm);
}
-
- armarx::ControlTargetBase* StopMovementController::getControlTarget()
+ armarx::ControlTargetBase*
+ StopMovementController::getControlTarget()
{
return ⌖
}
-
- void StopMovementController::rtPreActivateController()
+ void
+ StopMovementController::rtPreActivateController()
{
pid->reset();
}
-} // namespace devices::ethercat::head_board::v1::joint_controller
+} // namespace devices::ethercat::head::armar6::joint_controller
diff --git a/source/devices/ethercat/head/armar6/joint_controller/StopMovement.h b/source/devices/ethercat/head/armar6/joint_controller/StopMovement.h
index 908571d2..9902e879 100644
--- a/source/devices/ethercat/head/armar6/joint_controller/StopMovement.h
+++ b/source/devices/ethercat/head/armar6/joint_controller/StopMovement.h
@@ -5,30 +5,25 @@
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/libraries/core/PIDController.h>
-
namespace devices::ethercat::common::imagine_board
{
using SensorActorDataPtr = std::shared_ptr<class SensorActorData>;
}
-
namespace devices::ethercat::head::armar6::joint_controller
{
using StopMovementControllerPtr = std::shared_ptr<class StopMovementController>;
-
- class StopMovementController :
- public armarx::JointController
+ class StopMovementController : public armarx::JointController
{
public:
-
StopMovementController(common::imagine_board::SensorActorDataPtr dataPtr);
private:
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
/**
* Returns the Target for this controller, but as this is the Emergency controller it will ignored.
* As this controller will just break
@@ -39,11 +34,9 @@ namespace devices::ethercat::head::armar6::joint_controller
void rtPreActivateController() override;
private:
-
armarx::DummyControlTargetStopMovement target;
common::imagine_board::SensorActorDataPtr dataPtr;
armarx::PIDControllerPtr pid;
-
};
-} // namespace devices::ethercat::head_board::v1::joint_controller
+} // namespace devices::ethercat::head::armar6::joint_controller
diff --git a/source/devices/ethercat/head/armar6/joint_controller/ZeroTorque.cpp b/source/devices/ethercat/head/armar6/joint_controller/ZeroTorque.cpp
index e4d4d6ee..3be27da2 100644
--- a/source/devices/ethercat/head/armar6/joint_controller/ZeroTorque.cpp
+++ b/source/devices/ethercat/head/armar6/joint_controller/ZeroTorque.cpp
@@ -7,22 +7,25 @@
namespace devices::ethercat::head::armar6::joint_controller
{
- PWMZeroTorqueControllerConfigurationCPtr PWMZeroTorqueControllerConfiguration::CreateConfigData(armarx::control::hardware_config::Config& hwConfig)
+ PWMZeroTorqueControllerConfigurationCPtr
+ PWMZeroTorqueControllerConfiguration::CreateConfigData(
+ armarx::control::hardware_config::Config& hwConfig)
{
PWMZeroTorqueControllerConfiguration configData;
- configData.feedforwardVelocityToPWMFactor = hwConfig.getFloat("feedforwardVelocityToPWMFactor");
+ configData.feedforwardVelocityToPWMFactor =
+ hwConfig.getFloat("feedforwardVelocityToPWMFactor");
configData.PWMDeadzone = hwConfig.getFloat("PWMDeadzone");
return std::make_shared<PWMZeroTorqueControllerConfiguration>(configData);
}
-
- PWMZeroTorqueController::PWMZeroTorqueController(const std::string& deviceName,
- Device* board,
- common::imagine_board::SensorActorDataPtr jointData,
- PWMZeroTorqueControllerConfigurationCPtr config) : JointController(),
- config(config), target(), board(board), deviceName(deviceName)
+ PWMZeroTorqueController::PWMZeroTorqueController(
+ const std::string& deviceName,
+ Device* board,
+ common::imagine_board::SensorActorDataPtr jointData,
+ PWMZeroTorqueControllerConfigurationCPtr config) :
+ JointController(), config(config), target(), board(board), deviceName(deviceName)
{
actorIndex = board->getActorIndex(deviceName);
dataPtr = jointData;
@@ -30,25 +33,24 @@ namespace devices::ethercat::head::armar6::joint_controller
this->isLimitless = jointData->isLimitless();
}
-
PWMZeroTorqueController::~PWMZeroTorqueController() noexcept(true)
{
}
-
- void PWMZeroTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ PWMZeroTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (target.isValid())
{
float targetPWM = dataPtr->getVelocity() * config->feedforwardVelocityToPWMFactor;
- targetPWM += armarx::math::MathUtils::Sign(dataPtr->getVelocity()) * config->PWMDeadzone;
+ targetPWM +=
+ armarx::math::MathUtils::Sign(dataPtr->getVelocity()) * config->PWMDeadzone;
// targetPWM = math::MathUtils::LimitTo(targetPWM, 1500);
dataPtr->setTargetPWM(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);
-
-
}
else
{
@@ -56,24 +58,24 @@ namespace devices::ethercat::head::armar6::joint_controller
}
}
-
- armarx::ControlTargetBase* PWMZeroTorqueController::getControlTarget()
+ armarx::ControlTargetBase*
+ PWMZeroTorqueController::getControlTarget()
{
return ⌖
}
-
- void PWMZeroTorqueController::rtPreActivateController()
+ void
+ PWMZeroTorqueController::rtPreActivateController()
{
lastTargetVelocity = dataPtr->getVelocity();
// controller.reset(dataPtr->getVelocity());
}
-
- void PWMZeroTorqueController::rtPostDeactivateController()
+ void
+ PWMZeroTorqueController::rtPostDeactivateController()
{
ARMARX_RT_LOGF_INFO("Setting PWM to 0");
dataPtr->setTargetPWM(0);
}
-}
+} // namespace devices::ethercat::head::armar6::joint_controller
diff --git a/source/devices/ethercat/head/armar6/joint_controller/ZeroTorque.h b/source/devices/ethercat/head/armar6/joint_controller/ZeroTorque.h
index 6836763a..93569e5e 100644
--- a/source/devices/ethercat/head/armar6/joint_controller/ZeroTorque.h
+++ b/source/devices/ethercat/head/armar6/joint_controller/ZeroTorque.h
@@ -2,13 +2,11 @@
// armarx
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <armarx/control/hardware_config/Config.h>
-
namespace devices::ethercat::common::imagine_board
{
using SensorActorDataPtr = std::shared_ptr<class SensorActorData>;
@@ -19,50 +17,49 @@ namespace devices::ethercat::head::armar6
class Device;
}
-
namespace devices::ethercat::head::armar6::joint_controller
{
- using PWMZeroTorqueControllerConfigurationPtr = std::shared_ptr<class PWMZeroTorqueControllerConfiguration>;
- using PWMZeroTorqueControllerConfigurationCPtr = std::shared_ptr<const class PWMZeroTorqueControllerConfiguration>;
-
+ using PWMZeroTorqueControllerConfigurationPtr =
+ std::shared_ptr<class PWMZeroTorqueControllerConfiguration>;
+ using PWMZeroTorqueControllerConfigurationCPtr =
+ std::shared_ptr<const class PWMZeroTorqueControllerConfiguration>;
class PWMZeroTorqueControllerConfiguration
{
public:
+ PWMZeroTorqueControllerConfiguration()
+ {
+ }
- PWMZeroTorqueControllerConfiguration() {}
- static PWMZeroTorqueControllerConfigurationCPtr CreateConfigData(armarx::control::hardware_config::Config& hwConfig);
+ static PWMZeroTorqueControllerConfigurationCPtr
+ CreateConfigData(armarx::control::hardware_config::Config& hwConfig);
float feedforwardVelocityToPWMFactor;
float PWMDeadzone;
-
};
using JointPWMZeroTorqueControllerPtr = std::shared_ptr<class PWMZeroTorqueController>;
-
- class PWMZeroTorqueController :
- public armarx::JointController
+ class PWMZeroTorqueController : public armarx::JointController
{
public:
-
PWMZeroTorqueController(const std::string& deviceName,
Device* board,
common::imagine_board::SensorActorDataPtr jointData,
PWMZeroTorqueControllerConfigurationCPtr config);
~PWMZeroTorqueController() noexcept(true);
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
void rtPreActivateController() override;
void rtPostDeactivateController() override;
private:
-
PWMZeroTorqueControllerConfigurationCPtr config;
armarx::ControlTarget1DoFActuatorZeroTorque target;
@@ -73,7 +70,6 @@ namespace devices::ethercat::head::armar6::joint_controller
Device* board;
const std::string deviceName;
size_t actorIndex = 0;
-
};
-}
+} // namespace devices::ethercat::head::armar6::joint_controller
diff --git a/source/devices/ethercat/head/armar6/njoint_controller/PWMPassThrough.cpp b/source/devices/ethercat/head/armar6/njoint_controller/PWMPassThrough.cpp
index 4a5a96dc..e3784b6b 100644
--- a/source/devices/ethercat/head/armar6/njoint_controller/PWMPassThrough.cpp
+++ b/source/devices/ethercat/head/armar6/njoint_controller/PWMPassThrough.cpp
@@ -4,14 +4,17 @@
// robot_devices
#include "robot_devices/ethercat/head_board/v1/ControlTargets.h"
-
namespace devices::ethercat::head_board::v1::njoint_controller
{
- armarx::NJointControllerRegistration<PWMPassThroughController> registrationControllerPWMPassThroughController("NJointHeadPWMPassThroughController");
-
+ armarx::NJointControllerRegistration<PWMPassThroughController>
+ registrationControllerPWMPassThroughController("NJointHeadPWMPassThroughController");
- armarx::WidgetDescription::WidgetPtr PWMPassThroughController::GenerateConfigDescription(const VirtualRobot::RobotPtr& robot, const std::map<std::string, armarx::ConstControlDevicePtr>&, const std::map<std::string, armarx::ConstSensorDevicePtr>&)
+ armarx::WidgetDescription::WidgetPtr
+ PWMPassThroughController::GenerateConfigDescription(
+ const VirtualRobot::RobotPtr& robot,
+ const std::map<std::string, armarx::ConstControlDevicePtr>&,
+ const std::map<std::string, armarx::ConstSensorDevicePtr>&)
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = new HBoxLayout;
@@ -26,18 +29,14 @@ namespace devices::ethercat::head_board::v1::njoint_controller
return layout;
}
-
- PWMPassThroughControllerConfigPtr PWMPassThroughController::GenerateConfigFromVariants(
- const armarx::StringVariantBaseMap& values)
+ PWMPassThroughControllerConfigPtr
+ PWMPassThroughController::GenerateConfigFromVariants(const armarx::StringVariantBaseMap& values)
{
- return new PWMPassThroughControllerConfig
- {
- values.at("deviceName")->getString()
- };
+ return new PWMPassThroughControllerConfig{values.at("deviceName")->getString()};
}
-
- armarx::WidgetDescription::StringWidgetDictionary PWMPassThroughController::getFunctionDescriptions(const Ice::Current&) const
+ armarx::WidgetDescription::StringWidgetDictionary
+ PWMPassThroughController::getFunctionDescriptions(const Ice::Current&) const
{
using namespace armarx::WidgetDescription;
IntSpinBoxPtr spin = new IntSpinBox;
@@ -48,8 +47,10 @@ namespace devices::ethercat::head_board::v1::njoint_controller
return {{"SetPWM", spin}};
}
-
- void PWMPassThroughController::callDescribedFunction(const std::string& name, const armarx::StringVariantBaseMap& valueMap, const Ice::Current&)
+ void
+ PWMPassThroughController::callDescribedFunction(const std::string& name,
+ const armarx::StringVariantBaseMap& valueMap,
+ const Ice::Current&)
{
if (name == "SetPWM")
{
@@ -61,15 +62,13 @@ namespace devices::ethercat::head_board::v1::njoint_controller
}
}
-
- PWMPassThroughController::PWMPassThroughController(
- armarx::RobotUnitPtr prov,
- const PWMPassThroughControllerConfigPtr& cfg,
- const VirtualRobot::RobotPtr&)
+ PWMPassThroughController::PWMPassThroughController(armarx::RobotUnitPtr prov,
+ const PWMPassThroughControllerConfigPtr& cfg,
+ const VirtualRobot::RobotPtr&)
{
armarx::ControlTargetBase* ct = useControlTarget(cfg->deviceName, ctrl_modes::HeadPWM);
ARMARX_CHECK_EXPRESSION(ct->isA<ControlTargetHeadPWM>());
target = &(ct->asA<ControlTargetHeadPWM>()->pwm);
}
-} // namespace devices::ethercat::head_board::v1::njoint_controller
+} // namespace devices::ethercat::head_board::v1::njoint_controller
diff --git a/source/devices/ethercat/head/armar6/njoint_controller/PWMPassThrough.h b/source/devices/ethercat/head/armar6/njoint_controller/PWMPassThrough.h
index 44d1f529..8dccb8ce 100644
--- a/source/devices/ethercat/head/armar6/njoint_controller/PWMPassThrough.h
+++ b/source/devices/ethercat/head/armar6/njoint_controller/PWMPassThrough.h
@@ -7,69 +7,68 @@ namespace devices::ethercat::head_board::v1::njoint_controller
TYPEDEF_PTRS_HANDLE(PWMPassThroughControllerConfig);
-
- class PWMPassThroughControllerConfig :
- virtual public armarx::NJointControllerConfig
+ class PWMPassThroughControllerConfig : virtual public armarx::NJointControllerConfig
{
public:
+ PWMPassThroughControllerConfig(const std::string& deviceName) : deviceName{deviceName}
+ {
+ }
- PWMPassThroughControllerConfig(const std::string& deviceName) : deviceName{deviceName} {}
std::string deviceName;
-
};
-
TYPEDEF_PTRS_HANDLE(PWMPassThroughController);
-
- class PWMPassThroughController :
- virtual public armarx::NJointController
+ class PWMPassThroughController : virtual public armarx::NJointController
{
public:
-
using ConfigPtrT = PWMPassThroughControllerConfigPtr;
- static armarx::WidgetDescription::WidgetPtr GenerateConfigDescription(const VirtualRobot::RobotPtr& robot,
- const std::map<std::string, armarx::ConstControlDevicePtr>&,
- const std::map<std::string, armarx::ConstSensorDevicePtr>&);
+ static armarx::WidgetDescription::WidgetPtr
+ GenerateConfigDescription(const VirtualRobot::RobotPtr& robot,
+ const std::map<std::string, armarx::ConstControlDevicePtr>&,
+ const std::map<std::string, armarx::ConstSensorDevicePtr>&);
static ConfigPtrT GenerateConfigFromVariants(const armarx::StringVariantBaseMap& values);
- armarx::WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
- void callDescribedFunction(const std::string& name, const armarx::StringVariantBaseMap& valueMap, const Ice::Current&) override;
+ armarx::WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current&) const override;
+ void callDescribedFunction(const std::string& name,
+ const armarx::StringVariantBaseMap& valueMap,
+ const Ice::Current&) override;
- inline PWMPassThroughController(
- armarx::RobotUnitPtr prov,
- const PWMPassThroughControllerConfigPtr& cfg,
- const VirtualRobot::RobotPtr&);
+ inline PWMPassThroughController(armarx::RobotUnitPtr prov,
+ const PWMPassThroughControllerConfigPtr& cfg,
+ const VirtualRobot::RobotPtr&);
- inline void rtRun(const IceUtil::Time&, const IceUtil::Time&) override
+ inline void
+ rtRun(const IceUtil::Time&, const IceUtil::Time&) override
{
*target = control;
}
- inline void rtPreActivateController() override
+ inline void
+ rtPreActivateController() override
{
-
}
- inline std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override
+ inline std::string
+ getClassName(const Ice::Current& = Ice::emptyCurrent) const override
{
return "NJointHeadPWMPassThroughController";
}
- void set(std::int32_t val)
+ void
+ set(std::int32_t val)
{
control = val;
}
protected:
-
- std::atomic<std::int32_t> control {0};
- std::int32_t* target {nullptr};
-
+ std::atomic<std::int32_t> control{0};
+ std::int32_t* target{nullptr};
};
-} // namespace devices::ethercat::head_board::v1::njoint_controller
+} // namespace devices::ethercat::head_board::v1::njoint_controller
diff --git a/source/devices/ethercat/head_board/armar7/sub_device/DeviceControlModeSetter.h b/source/devices/ethercat/head_board/armar7/sub_device/DeviceControlModeSetter.h
index 1f4ac858..071a13d8 100644
--- a/source/devices/ethercat/head_board/armar7/sub_device/DeviceControlModeSetter.h
+++ b/source/devices/ethercat/head_board/armar7/sub_device/DeviceControlModeSetter.h
@@ -23,13 +23,14 @@
#include <armarx/control/ethercat/SlaveInterface.h>
-#include <devices/ethercat/common/elmo/gold/Slave.h>
#include <devices/ethercat/common/elmo/gold/Data.h>
+#include <devices/ethercat/common/elmo/gold/Slave.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/interfaces.h>
namespace devices::ethercat::head_board::armar7::sub_device
{
- namespace joint_controller = devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller;
+ namespace joint_controller =
+ devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller;
class DeviceControlModeSetter :
virtual public joint_controller::DeviceControlModeSetterInterface
diff --git a/source/devices/ethercat/head_board/armar7/sub_device/rotation/joint_controller/Position.cpp b/source/devices/ethercat/head_board/armar7/sub_device/rotation/joint_controller/Position.cpp
index 818cf6ff..d5436325 100644
--- a/source/devices/ethercat/head_board/armar7/sub_device/rotation/joint_controller/Position.cpp
+++ b/source/devices/ethercat/head_board/armar7/sub_device/rotation/joint_controller/Position.cpp
@@ -3,7 +3,6 @@
#include <utility>
#include "ArmarXCore/core/logging/Logging.h"
-
#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
#include <devices/ethercat/sensor_actor_unit/armar7de/Data.h>
diff --git a/source/devices/ethercat/head_board/armar7/sub_device/rotation/joint_controller/Position.h b/source/devices/ethercat/head_board/armar7/sub_device/rotation/joint_controller/Position.h
index f490dd64..45e04c1f 100644
--- a/source/devices/ethercat/head_board/armar7/sub_device/rotation/joint_controller/Position.h
+++ b/source/devices/ethercat/head_board/armar7/sub_device/rotation/joint_controller/Position.h
@@ -3,7 +3,6 @@
#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-
#include <armarx/control/joint_controller/ControllerConfiguration.h>
#include <devices/ethercat/head_board/armar7/sub_device/rotation/joint_controller/PositionInterface.h>
diff --git a/source/devices/ethercat/head_board/armar7/sub_device/rotation/joint_controller/PositionWithVelocity.h b/source/devices/ethercat/head_board/armar7/sub_device/rotation/joint_controller/PositionWithVelocity.h
index c05c5a97..c0449184 100644
--- a/source/devices/ethercat/head_board/armar7/sub_device/rotation/joint_controller/PositionWithVelocity.h
+++ b/source/devices/ethercat/head_board/armar7/sub_device/rotation/joint_controller/PositionWithVelocity.h
@@ -3,7 +3,6 @@
#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-
#include <armarx/control/joint_controller/ControllerConfiguration.h>
#include <devices/ethercat/head_board/armar7/sub_device/rotation/joint_controller/PositionInterface.h>
diff --git a/source/devices/ethercat/head_board/armar7de/Config.h b/source/devices/ethercat/head_board/armar7de/Config.h
index 07cdb8ea..efe18528 100644
--- a/source/devices/ethercat/head_board/armar7de/Config.h
+++ b/source/devices/ethercat/head_board/armar7de/Config.h
@@ -5,17 +5,18 @@
namespace devices::ethercat::head_board::armar7de
{
namespace hwconfig = armarx::control::hardware_config;
+
struct HeadBoardConfig
{
- HeadBoardConfig(hwconfig::DeviceConfig& hwConfig)
- : alwaysUseAksIm1(hwConfig.getUint("alwaysUseAksIM_1")),
- onBoardTemperatureLimit(hwConfig.getUint("onBoardTemperatureLimit")),
- externalTemperatureLimit(hwConfig.getUint("externalTemperatureLimit")),
- ledStripCount(hwConfig.getUint("ledRGBStripCount")),
- ledStripGlobalBrightness(hwConfig.getUint("ledRGBStripGlobalBrightness")),
- isExternalTemperatureSensorConnected(hwConfig.getBool("isExternalTemperatureSensorConnected"))
+ HeadBoardConfig(hwconfig::DeviceConfig& hwConfig) :
+ alwaysUseAksIm1(hwConfig.getUint("alwaysUseAksIM_1")),
+ onBoardTemperatureLimit(hwConfig.getUint("onBoardTemperatureLimit")),
+ externalTemperatureLimit(hwConfig.getUint("externalTemperatureLimit")),
+ ledStripCount(hwConfig.getUint("ledRGBStripCount")),
+ ledStripGlobalBrightness(hwConfig.getUint("ledRGBStripGlobalBrightness")),
+ isExternalTemperatureSensorConnected(
+ hwConfig.getBool("isExternalTemperatureSensorConnected"))
{
-
}
/**
@@ -57,11 +58,11 @@ namespace devices::ethercat::head_board::armar7de
struct HeadBoardDataConfig
{
- HeadBoardDataConfig(hwconfig::DeviceConfig& hwConfig)
- : absoluteEncoder(hwConfig.getLinearConfig("absoluteEncoder")),
- relativeEncoder(hwConfig.getLinearConfig("relativeEncoderTicks")),
- motorDirection(hwConfig.getInt("motorDirection")),
- maxPwm(hwConfig.getInt("maxPWM"))
+ HeadBoardDataConfig(hwconfig::DeviceConfig& hwConfig) :
+ absoluteEncoder(hwConfig.getLinearConfig("absoluteEncoder")),
+ relativeEncoder(hwConfig.getLinearConfig("relativeEncoderTicks")),
+ motorDirection(hwConfig.getInt("motorDirection")),
+ maxPwm(hwConfig.getInt("maxPWM"))
{
}
@@ -70,4 +71,4 @@ namespace devices::ethercat::head_board::armar7de
std::int16_t motorDirection;
std::int16_t maxPwm;
};
-}
+} // namespace devices::ethercat::head_board::armar7de
diff --git a/source/devices/ethercat/head_board/armar7de/ControlTargets.h b/source/devices/ethercat/head_board/armar7de/ControlTargets.h
index 453d078a..f334f2db 100644
--- a/source/devices/ethercat/head_board/armar7de/ControlTargets.h
+++ b/source/devices/ethercat/head_board/armar7de/ControlTargets.h
@@ -3,7 +3,6 @@
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h>
-
namespace devices::ethercat::head_board::armar7de
{
@@ -11,8 +10,7 @@ namespace devices::ethercat::head_board::armar7de
{
static const std::string HeadPWM = "ControlMode_HeadPWM";
static const std::string HeadLedColor = "ControlMode_HeadLEDColor";
- }
-
+ } // namespace ctrl_modes
class ControlTargetHeadPWM : public armarx::ControlTargetBase
{
@@ -22,9 +20,11 @@ namespace devices::ethercat::head_board::armar7de
ControlTargetHeadPWM() = default;
ControlTargetHeadPWM(const ControlTargetHeadPWM&) = default;
ControlTargetHeadPWM(ControlTargetHeadPWM&&) = default;
+
ControlTargetHeadPWM(std::int32_t val) : pwm{val}
{
}
+
ControlTargetHeadPWM& operator=(const ControlTargetHeadPWM&) = default;
ControlTargetHeadPWM& operator=(ControlTargetHeadPWM&&) = default;
@@ -64,7 +64,6 @@ namespace devices::ethercat::head_board::armar7de
}
};
-
class ControlTargetHeadLedColor : public armarx::ControlTargetBase
{
@@ -75,12 +74,16 @@ namespace devices::ethercat::head_board::armar7de
ControlTargetHeadLedColor() = default;
ControlTargetHeadLedColor(const ControlTargetHeadLedColor&) = default;
ControlTargetHeadLedColor(ControlTargetHeadLedColor&&) = default;
- ControlTargetHeadLedColor(std::array<std::uint8_t, 20> r, std::array<std::uint8_t, 20> g, std::array<std::uint8_t, 20> b)
+
+ ControlTargetHeadLedColor(std::array<std::uint8_t, 20> r,
+ std::array<std::uint8_t, 20> g,
+ std::array<std::uint8_t, 20> b)
{
this->r = r;
this->g = g;
this->b = b;
}
+
ControlTargetHeadLedColor& operator=(const ControlTargetHeadLedColor&) = default;
ControlTargetHeadLedColor& operator=(ControlTargetHeadLedColor&&) = default;
diff --git a/source/devices/ethercat/head_board/armar7de/Data.cpp b/source/devices/ethercat/head_board/armar7de/Data.cpp
index dd1dae26..25804dcf 100644
--- a/source/devices/ethercat/head_board/armar7de/Data.cpp
+++ b/source/devices/ethercat/head_board/armar7de/Data.cpp
@@ -9,12 +9,9 @@
// armarx
#include <RobotAPI/libraries/core/math/MathUtils.h>
-
namespace devices::ethercat::head_board::armar7de
{
- Data::Data(HeadBoardDataConfig& config,
- SlaveOut* outputs,
- SlaveIn* inputs) :
+ Data::Data(HeadBoardDataConfig& config, SlaveOut* outputs, SlaveIn* inputs) :
outputs(outputs),
inputs(inputs),
errorDecoder(outputs, inputs),
@@ -25,10 +22,8 @@ namespace devices::ethercat::head_board::armar7de
ARMARX_CHECK_EXPRESSION(outputs);
ARMARX_CHECK_EXPRESSION(inputs);
- absoluteEncoderAngle.init(&(outputs->absoluteEncoderValue),
- config.absoluteEncoder);
- relativeEncoder.init(&(outputs->relativeEncoderTicks),
- config.relativeEncoder);
+ absoluteEncoderAngle.init(&(outputs->absoluteEncoderValue), config.absoluteEncoder);
+ relativeEncoder.init(&(outputs->relativeEncoderTicks), config.relativeEncoder);
}
void
@@ -178,9 +173,10 @@ namespace devices::ethercat::head_board::armar7de
void
Data::setMotorPwmValue(std::int16_t value)
{
- motorPwmValue = config.motorDirection * std::clamp(value,
- static_cast<std::int16_t>(-config.maxPwm),
- static_cast<std::int16_t>(config.maxPwm));
+ motorPwmValue =
+ config.motorDirection * std::clamp(value,
+ static_cast<std::int16_t>(-config.maxPwm),
+ static_cast<std::int16_t>(config.maxPwm));
}
std::int16_t
diff --git a/source/devices/ethercat/head_board/armar7de/Data.h b/source/devices/ethercat/head_board/armar7de/Data.h
index d32be998..53e1f6b8 100644
--- a/source/devices/ethercat/head_board/armar7de/Data.h
+++ b/source/devices/ethercat/head_board/armar7de/Data.h
@@ -7,10 +7,9 @@
#include <armarx/control/rt_filters/AverageFilter.h>
#include "Slave.h"
+#include <devices/ethercat/head_board/armar7de/Config.h>
#include <devices/ethercat/head_board/armar7de/ErrorDecoder.h>
#include <devices/ethercat/head_board/armar7de/SlaveIO.h>
-#include <devices/ethercat/head_board/armar7de/Config.h>
-
namespace devices::ethercat::head_board::armar7de
{
@@ -37,6 +36,7 @@ namespace devices::ethercat::head_board::armar7de
// ARMARX_INFO << VAROUT(lastInput);
lastInput = currentValue;
}
+
double value;
private:
@@ -50,8 +50,8 @@ namespace devices::ethercat::head_board::armar7de
public:
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- /* position */
- std::uint16_t absoluteEncoderDetailedStatus;
+ /* position */
+ std::uint16_t absoluteEncoderDetailedStatus;
float absoluteEncoderTemperature;
std::uint32_t absoluteEncoderTicks;
@@ -110,14 +110,11 @@ namespace devices::ethercat::head_board::armar7de
}
};
-
class Data : public armarx::control::ethercat::DataInterface
{
public:
- Data(HeadBoardDataConfig& config,
- SlaveOut* sensorOUT,
- SlaveIn* sensorIN);
+ Data(HeadBoardDataConfig& config, SlaveOut* sensorOUT, SlaveIn* sensorIN);
// AbstractData interface
public:
@@ -189,7 +186,7 @@ namespace devices::ethercat::head_board::armar7de
/// true: encoder is running properly; false: no communication at least for 3ms
bool absoluteEncoderCommunicationTimeout;
-
+
bool onBoardTemperatureError;
bool externalTemperatureError;
bool onBoardTemperatureLimitReached;
diff --git a/source/devices/ethercat/head_board/armar7de/Device.cpp b/source/devices/ethercat/head_board/armar7de/Device.cpp
index a8ba4639..e8b74702 100644
--- a/source/devices/ethercat/head_board/armar7de/Device.cpp
+++ b/source/devices/ethercat/head_board/armar7de/Device.cpp
@@ -41,12 +41,10 @@
#include "joint_controller/ZeroTorque.h"
#include <devices/ethercat/head_board/armar7de/Slave.h>
-
namespace devices::ethercat::head_board::armar7de
{
- Device::Device(hwconfig::DeviceConfig& hwConfig,
- const VirtualRobot::RobotPtr& robot) :
+ Device::Device(hwconfig::DeviceConfig& hwConfig, const VirtualRobot::RobotPtr& robot) :
DeviceBase(hwConfig.getName()),
ControlDevice(hwConfig.getName()),
SensorDevice(hwConfig.getName()),
@@ -67,12 +65,13 @@ namespace devices::ethercat::head_board::armar7de
CreatePWMPositionControllerConfigData(hwConfig.getControllerConfig("PWMPosition"));
// TODO: Why not the controller in joint_controllers? Why is there another one?
velocityControllerCfg =
- PWMVelocityControllerConfiguration::CreatePWMVelocityControllerConfigData(hwConfig.getControllerConfig("PWMVelocity"));
+ PWMVelocityControllerConfiguration::CreatePWMVelocityControllerConfigData(
+ hwConfig.getControllerConfig("PWMVelocity"));
zeroTorqueControllerCfg =
- joint_controller::PWMZeroTorqueControllerConfiguration::CreateConfigData(hwConfig.getControllerConfig("PWMZeroTorque"));
+ joint_controller::PWMZeroTorqueControllerConfiguration::CreateConfigData(
+ hwConfig.getControllerConfig("PWMZeroTorque"));
}
-
armarx::control::ethercat::DeviceInterface::TryAssignResult
Device::tryAssign(armarx::control::ethercat::SlaveInterface& slave)
{
@@ -84,7 +83,6 @@ namespace devices::ethercat::head_board::armar7de
return result;
}
-
armarx::control::ethercat::DeviceInterface::AllAssignedResult
Device::onAllAssigned()
{
@@ -152,7 +150,6 @@ namespace devices::ethercat::head_board::armar7de
{
}
-
void
Device::updateSensorValueStruct()
{
@@ -179,14 +176,14 @@ namespace devices::ethercat::head_board::armar7de
// Status Bits
sensorValue.emergencyStopStatus = dataPtr->getEmergencyStopStatus();
- sensorValue.absoluteEncoderCommunicationTimeout = dataPtr->getAbsoluteEncoderCommunicationTimeout();
+ sensorValue.absoluteEncoderCommunicationTimeout =
+ dataPtr->getAbsoluteEncoderCommunicationTimeout();
sensorValue.onBoardTemperatureError = dataPtr->getOnBoardTemperatureError();
sensorValue.externalTemperatureError = dataPtr->getExternalTemperatureError();
sensorValue.onBoardTemperatureLimitReached = dataPtr->getOnBoardTemperatureLimitReached();
sensorValue.externalTemperatureLimitReached = dataPtr->getExternalTemperatureLimitReached();
}
-
std::shared_ptr<Data>
Device::getData() const
{
@@ -199,28 +196,24 @@ namespace devices::ethercat::head_board::armar7de
return dataPtr;
}
-
const std::string&
Device::getJointName() const
{
return getDeviceName();
}
-
armarx::control::ethercat::SlaveIdentifier
Device::getSlaveIdentifier() const
{
return slaveIdentifier;
}
-
const Device::LedDevicePtr&
Device::getLedDevice() const
{
return ledDevice;
}
-
void
Device::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -257,7 +250,6 @@ namespace devices::ethercat::head_board::armar7de
updateSensorValueStruct();
}
-
void
Device::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -272,21 +264,18 @@ namespace devices::ethercat::head_board::armar7de
ledControlDataPtr->rtWriteTargetValues(sensorValuesTimestamp, timeSinceLastIteration);
}
-
const armarx::SensorValueBase*
Device::getSensorValue() const
{
return &sensorValue;
}
-
std::uint16_t
Device::getSlaveNumber() const
{
return slavePtr->getSlaveNumber();
}
-
float
Device::getSoftLimitHi() const
{
@@ -305,7 +294,6 @@ namespace devices::ethercat::head_board::armar7de
return robotNode->isLimitless();
}
-
Device::LedDevice::LedDevice(const std::string& deviceName) :
DeviceBase(deviceName),
ControlDevice(deviceName),
diff --git a/source/devices/ethercat/head_board/armar7de/Device.h b/source/devices/ethercat/head_board/armar7de/Device.h
index cb810884..d959f845 100644
--- a/source/devices/ethercat/head_board/armar7de/Device.h
+++ b/source/devices/ethercat/head_board/armar7de/Device.h
@@ -34,10 +34,8 @@
#include <devices/ethercat/head_board/armar7de/Config.h>
#include <devices/ethercat/head_board/armar7de/Data.h>
-#include <devices/ethercat/head_board/armar7de/Slave.h>
-
#include <devices/ethercat/head_board/armar7de/LedControlData.h>
-
+#include <devices/ethercat/head_board/armar7de/Slave.h>
namespace devices::ethercat::head_board::armar7de
{
@@ -67,7 +65,6 @@ namespace devices::ethercat::head_board::armar7de
using PWMVelocityControllerConfigurationCPtr =
std::shared_ptr<const class PWMVelocityControllerConfiguration>;
-
class Device :
public armarx::ControlDevice,
public armarx::SensorDevice,
@@ -76,8 +73,8 @@ namespace devices::ethercat::head_board::armar7de
public:
class LedDevice :
- public armarx::ControlDevice,
- public armarx::control::ethercat::DeviceInterface::SubDeviceInterface
+ public armarx::ControlDevice,
+ public armarx::control::ethercat::DeviceInterface::SubDeviceInterface
{
friend class Device;
@@ -100,8 +97,8 @@ namespace devices::ethercat::head_board::armar7de
using LedDevicePtr = std::shared_ptr<LedDevice>;
- Device(hwconfig::DeviceConfig& hwConfig,
- VirtualRobot::RobotPtr const& robot);
+ Device(hwconfig::DeviceConfig& hwConfig, VirtualRobot::RobotPtr const& robot);
+
~Device() override
{
}
diff --git a/source/devices/ethercat/head_board/armar7de/ErrorDecoder.cpp b/source/devices/ethercat/head_board/armar7de/ErrorDecoder.cpp
index e5d1bc05..abc0353e 100644
--- a/source/devices/ethercat/head_board/armar7de/ErrorDecoder.cpp
+++ b/source/devices/ethercat/head_board/armar7de/ErrorDecoder.cpp
@@ -153,7 +153,7 @@ namespace devices::ethercat::head_board::armar7de
return (errors &
(AbsoluteEncoderStatus::ErrorAccelerationError |
AbsoluteEncoderStatus::ErrorEncoderNotConfiguredProperly |
- /* AbsoluteEncoderStatus::ErrorMagneticPatternError | */
+ /* AbsoluteEncoderStatus::ErrorMagneticPatternError | */
AbsoluteEncoderStatus::ErrorMagneticSensor |
AbsoluteEncoderStatus::ErrorMultiturnCounterMismatch |
AbsoluteEncoderStatus::ErrorPositionNotValid |
diff --git a/source/devices/ethercat/head_board/armar7de/LedControlData.cpp b/source/devices/ethercat/head_board/armar7de/LedControlData.cpp
index 23cc8134..3fdd1a16 100644
--- a/source/devices/ethercat/head_board/armar7de/LedControlData.cpp
+++ b/source/devices/ethercat/head_board/armar7de/LedControlData.cpp
@@ -2,27 +2,23 @@
namespace devices::ethercat::head_board::armar7de
{
- LedControlData::LedControlData(std::uint16_t* target) :
- target{target}
+ LedControlData::LedControlData(std::uint16_t* target) : target{target}
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(target);
}
void
- LedControlData::rtReadSensorValues(
- const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& /*timeSinceLastIteration*/)
- {}
+ LedControlData::rtReadSensorValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
+ {
+ }
void
- LedControlData::rtWriteTargetValues(
- const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& /*timeSinceLastIteration*/)
+ LedControlData::rtWriteTargetValues(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
{
- std::copy(std::begin(ledRgbStripControl),
- std::end(ledRgbStripControl),
- target);
+ std::copy(std::begin(ledRgbStripControl), std::end(ledRgbStripControl), target);
}
void
diff --git a/source/devices/ethercat/head_board/armar7de/LedControlData.h b/source/devices/ethercat/head_board/armar7de/LedControlData.h
index db559231..fa93bf87 100644
--- a/source/devices/ethercat/head_board/armar7de/LedControlData.h
+++ b/source/devices/ethercat/head_board/armar7de/LedControlData.h
@@ -4,7 +4,6 @@
#include <armarx/control/ethercat/DataInterface.h>
-
namespace devices::ethercat::head_board::armar7de
{
class LedControlData : public armarx::control::ethercat::DataInterface
@@ -23,8 +22,7 @@ namespace devices::ethercat::head_board::armar7de
private:
std::uint16_t* target;
std::array<std::uint16_t, 20> ledRgbStripControl;
-
};
- using LedControlDataPtr =
- std::shared_ptr<LedControlData>;
+
+ using LedControlDataPtr = std::shared_ptr<LedControlData>;
} // namespace devices::ethercat::head_board::armar7de
diff --git a/source/devices/ethercat/head_board/armar7de/PWMVelocity.cpp b/source/devices/ethercat/head_board/armar7de/PWMVelocity.cpp
index 224c0a48..9cbd6710 100644
--- a/source/devices/ethercat/head_board/armar7de/PWMVelocity.cpp
+++ b/source/devices/ethercat/head_board/armar7de/PWMVelocity.cpp
@@ -42,7 +42,6 @@ namespace devices::ethercat::head_board::armar7de
pid->threadSafe = false;
}
-
double
PWMVelocityController::run(IceUtil::Time const& deltaT,
double currentVelocity,
@@ -76,7 +75,6 @@ namespace devices::ethercat::head_board::armar7de
return targetPWM;
}
-
void
PWMVelocityController::reset(double currentVelocity)
{
@@ -84,7 +82,6 @@ namespace devices::ethercat::head_board::armar7de
pid->reset();
}
-
PWMVelocityControllerConfigurationCPtr
PWMVelocityControllerConfiguration::CreatePWMVelocityControllerConfigData(
armarx::control::hardware_config::Config& hwConfig)
@@ -108,8 +105,7 @@ namespace devices::ethercat::head_board::armar7de
configData->feedforwardTorqueToPWMFactor =
hwConfig.getFloat("feedforwardTorqueToPWMFactor");
configData->PWMDeadzone = hwConfig.getFloat("PWMDeadzone");
- configData->velocityUpdatePercent =
- hwConfig.getFloat("velocityUpdatePercent");
+ configData->velocityUpdatePercent = hwConfig.getFloat("velocityUpdatePercent");
configData->conditionalIntegralErrorTreshold =
hwConfig.getFloat("conditionalIntegralErrorTreshold");
configData->feedForwardMode = hwConfig.getBool("FeedForwardMode");
diff --git a/source/devices/ethercat/head_board/armar7de/PWMVelocity.h b/source/devices/ethercat/head_board/armar7de/PWMVelocity.h
index 9ccd88c6..8f93dc3a 100644
--- a/source/devices/ethercat/head_board/armar7de/PWMVelocity.h
+++ b/source/devices/ethercat/head_board/armar7de/PWMVelocity.h
@@ -30,7 +30,6 @@
#include <RobotAPI/libraries/core/PIDController.h>
#include <armarx/control/hardware_config/Config.h>
-
namespace devices::ethercat::head_board::armar7de
{
@@ -39,7 +38,6 @@ namespace devices::ethercat::head_board::armar7de
using PWMVelocityControllerConfigurationCPtr =
std::shared_ptr<const PWMVelocityControllerConfiguration>;
-
class PWMVelocityControllerConfiguration :
public armarx::RampedAccelerationVelocityControllerConfiguration
{
@@ -60,7 +58,6 @@ namespace devices::ethercat::head_board::armar7de
bool feedForwardMode;
};
-
class PWMVelocityController
{
diff --git a/source/devices/ethercat/head_board/armar7de/SensorValue.h b/source/devices/ethercat/head_board/armar7de/SensorValue.h
index aea08bb8..c4cf6374 100644
--- a/source/devices/ethercat/head_board/armar7de/SensorValue.h
+++ b/source/devices/ethercat/head_board/armar7de/SensorValue.h
@@ -28,7 +28,6 @@
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueIMU.h>
-
namespace devices::ethercat::head_board::armar7de
{
diff --git a/source/devices/ethercat/head_board/armar7de/Slave.cpp b/source/devices/ethercat/head_board/armar7de/Slave.cpp
index b7cf3f74..d2fdc014 100644
--- a/source/devices/ethercat/head_board/armar7de/Slave.cpp
+++ b/source/devices/ethercat/head_board/armar7de/Slave.cpp
@@ -31,7 +31,6 @@
#include <devices/ethercat/common/h2t_devices.h>
-
namespace devices::ethercat::head_board::armar7de
{
@@ -41,26 +40,22 @@ namespace devices::ethercat::head_board::armar7de
setTag(this->slaveIdentifier.getName());
}
-
void
Slave::doMappings()
{
}
-
bool
Slave::prepareForRun()
{
return true;
}
-
void
Slave::execute()
{
}
-
bool
Slave::shutdown()
{
@@ -79,7 +74,6 @@ namespace devices::ethercat::head_board::armar7de
errorDecoder = ErrorDecoder(getOutputsPtr(), getInputsPtr());
}
-
bool
Slave::hasError()
{
@@ -127,14 +121,12 @@ namespace devices::ethercat::head_board::armar7de
return true;
}
-
std::string
Slave::getDefaultName()
{
return "HeadBoardArmar7de";
}
-
bool
Slave::isSlaveIdentifierAccepted(const armarx::control::ethercat::SlaveIdentifier& sid)
{
diff --git a/source/devices/ethercat/head_board/armar7de/Slave.h b/source/devices/ethercat/head_board/armar7de/Slave.h
index 5c1ff525..14910c46 100644
--- a/source/devices/ethercat/head_board/armar7de/Slave.h
+++ b/source/devices/ethercat/head_board/armar7de/Slave.h
@@ -28,10 +28,9 @@
#include <armarx/control/ethercat/SlaveInterface.h>
+#include <devices/ethercat/head_board/armar7de/Config.h>
#include <devices/ethercat/head_board/armar7de/ErrorDecoder.h>
#include <devices/ethercat/head_board/armar7de/SlaveIO.h>
-#include <devices/ethercat/head_board/armar7de/Config.h>
-
namespace devices::ethercat::head_board::armar7de
{
@@ -60,7 +59,7 @@ namespace devices::ethercat::head_board::armar7de
bool isEmergencyStopActive() const override;
bool recoverFromEmergencyStop() override;
-
+
/**
* @brief Set the Config object containing the coe config
* The values are set in slave as soon as it switches from pre-op to safe-op
diff --git a/source/devices/ethercat/head_board/armar7de/SlaveIO.h b/source/devices/ethercat/head_board/armar7de/SlaveIO.h
index 656ca0ac..018a47e3 100644
--- a/source/devices/ethercat/head_board/armar7de/SlaveIO.h
+++ b/source/devices/ethercat/head_board/armar7de/SlaveIO.h
@@ -4,7 +4,6 @@
// STD/STL
#include <cstdint>
-
namespace devices::ethercat::head_board::armar7de
{
@@ -32,7 +31,6 @@ namespace devices::ethercat::head_board::armar7de
} __attribute__((__packed__));
-
/**
* @brief PDO mapping master->motorB
*/
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/EmergencyStop.cpp b/source/devices/ethercat/head_board/armar7de/joint_controller/EmergencyStop.cpp
index debc5c18..b21bb80b 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/EmergencyStop.cpp
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/EmergencyStop.cpp
@@ -7,7 +7,6 @@
// robot_devices
#include <devices/ethercat/head_board/armar7de/Data.h>
-
namespace devices::ethercat::head_board::armar7de::joint_controller
{
@@ -17,7 +16,6 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
pid->maxIntegral = 0.1;
}
-
void
EmergencyStopController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
const IceUtil::Time& timeSinceLastIteration)
@@ -31,9 +29,9 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
targetPwm = 0.0;
}
-// ARMARX_INFO << "EmergencyStop";
-// ARMARX_INFO << VAROUT(dataPtr->getVelocity());
-// ARMARX_INFO << VAROUT(targetPwm);
+ // ARMARX_INFO << "EmergencyStop";
+ // ARMARX_INFO << VAROUT(dataPtr->getVelocity());
+ // ARMARX_INFO << VAROUT(targetPwm);
targetPwm = 0.0f;
dataPtr->setMotorPwmValue(static_cast<std::int32_t>(targetPwm));
@@ -41,14 +39,12 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
// dataPtr->setTargetPWM(lastPWM);
}
-
armarx::ControlTargetBase*
EmergencyStopController::getControlTarget()
{
return ⌖
}
-
void
EmergencyStopController::rtPreActivateController()
{
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/EmergencyStop.h b/source/devices/ethercat/head_board/armar7de/joint_controller/EmergencyStop.h
index 80211477..6d51ceeb 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/EmergencyStop.h
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/EmergencyStop.h
@@ -4,25 +4,21 @@
// armarx
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-
namespace armarx
{
using PIDControllerPtr = std::shared_ptr<class PIDController>;
}
-
namespace devices::ethercat::head_board::armar7de
{
using DataPtr = std::shared_ptr<class Data>;
}
-
namespace devices::ethercat::head_board::armar7de::joint_controller
{
using EmergencyStopControllerPtr = std::shared_ptr<class EmergencyStopController>;
-
class EmergencyStopController : public armarx::JointController
{
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/LedColor.cpp b/source/devices/ethercat/head_board/armar7de/joint_controller/LedColor.cpp
index f586a940..356d59ff 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/LedColor.cpp
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/LedColor.cpp
@@ -2,26 +2,26 @@
namespace devices::ethercat::head_board::armar7de::joint_controller
{
- LedColorController::LedColorController(
- LedControlDataPtr data) :
- data(data)
+ LedColorController::LedColorController(LedControlDataPtr data) : data(data)
{
ARMARX_CHECK_NOT_NULL(data);
}
- armarx::ControlTargetBase* LedColorController::getControlTarget()
+ armarx::ControlTargetBase*
+ LedColorController::getControlTarget()
{
return ⌖
}
- void LedColorController::rtRun(
- const IceUtil::Time&, const IceUtil::Time&)
+ void
+ LedColorController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
{
std::array<std::uint16_t, 20> ledRgbStripControl;
for (size_t i = 0; i < ledRgbStripControl.size(); ++i)
{
- ledRgbStripControl[i] = ((target.r[i] & 0b11111000) << 8) | ((target.g[i] & 0b11111100) << 3) | (target.b[i] >> 3);
+ ledRgbStripControl[i] = ((target.r[i] & 0b11111000) << 8) |
+ ((target.g[i] & 0b11111100) << 3) | (target.b[i] >> 3);
}
data->setLedRgbStripControl(ledRgbStripControl);
}
-}
+} // namespace devices::ethercat::head_board::armar7de::joint_controller
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/LedColor.h b/source/devices/ethercat/head_board/armar7de/joint_controller/LedColor.h
index f6920dd5..e243f4eb 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/LedColor.h
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/LedColor.h
@@ -2,8 +2,8 @@
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include "../LedControlData.h"
#include "../ControlTargets.h"
+#include "../LedControlData.h"
namespace devices::ethercat::head_board::armar7de::joint_controller
{
@@ -14,9 +14,11 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
armarx::ControlTargetBase* getControlTarget() override;
void rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration) override;
+
private:
LedControlDataPtr data;
ControlTargetHeadLedColor target;
};
+
using LedColorControllerPtr = std::shared_ptr<LedColorController>;
} // namespace devices::ethercat::head_board::armar7de::joint_controller
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/LedEmergencyStop.cpp b/source/devices/ethercat/head_board/armar7de/joint_controller/LedEmergencyStop.cpp
index 37762c09..2ee4a0d5 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/LedEmergencyStop.cpp
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/LedEmergencyStop.cpp
@@ -3,18 +3,22 @@
namespace devices::ethercat::head_board::armar7de::joint_controller
{
LedEmergencyStopController::LedEmergencyStopController()
- {}
- void LedEmergencyStopController::rtRun(
- const IceUtil::Time&, const IceUtil::Time&)
{
}
- armarx::ControlTargetBase* LedEmergencyStopController::getControlTarget()
+ void
+ LedEmergencyStopController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
+ {
+ }
+
+ armarx::ControlTargetBase*
+ LedEmergencyStopController::getControlTarget()
{
return ⌖
}
- void LedEmergencyStopController::rtPreActivateController()
+ void
+ LedEmergencyStopController::rtPreActivateController()
{
}
} // namespace devices::ethercat::head_board::armar7de::joint_controller
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/LedEmergencyStop.h b/source/devices/ethercat/head_board/armar7de/joint_controller/LedEmergencyStop.h
index 36ae3102..65f4f0c0 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/LedEmergencyStop.h
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/LedEmergencyStop.h
@@ -8,16 +8,18 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
{
public:
LedEmergencyStopController();
+
private:
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
void rtPreActivateController() override;
+
private:
armarx::DummyControlTargetEmergencyStop target;
};
-
- using LedEmergencyStopControllerPtr =
- std::shared_ptr<LedEmergencyStopController>;
+
+ using LedEmergencyStopControllerPtr = std::shared_ptr<LedEmergencyStopController>;
} // namespace devices::ethercat::head_board::armar7de::joint_controller
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/LedStopMovement.cpp b/source/devices/ethercat/head_board/armar7de/joint_controller/LedStopMovement.cpp
index da29569c..0da5963f 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/LedStopMovement.cpp
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/LedStopMovement.cpp
@@ -3,19 +3,22 @@
namespace devices::ethercat::head_board::armar7de::joint_controller
{
LedStopMovementController::LedStopMovementController()
- {}
+ {
+ }
- void LedStopMovementController::rtRun(
- const IceUtil::Time&, const IceUtil::Time&)
+ void
+ LedStopMovementController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
{
}
- armarx::ControlTargetBase* LedStopMovementController::getControlTarget()
+ armarx::ControlTargetBase*
+ LedStopMovementController::getControlTarget()
{
return ⌖
}
- void LedStopMovementController::rtPreActivateController()
+ void
+ LedStopMovementController::rtPreActivateController()
{
}
} // namespace devices::ethercat::head_board::armar7de::joint_controller
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/LedStopMovement.h b/source/devices/ethercat/head_board/armar7de/joint_controller/LedStopMovement.h
index 8d8f3f7e..1d1643dd 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/LedStopMovement.h
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/LedStopMovement.h
@@ -12,8 +12,10 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
void rtPreActivateController() override;
+
private:
armarx::DummyControlTargetStopMovement target;
};
+
using LedStopMovementControllerPtr = std::shared_ptr<LedStopMovementController>;
} // namespace devices::ethercat::head_board::armar7de::joint_controller
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/PWM.cpp b/source/devices/ethercat/head_board/armar7de/joint_controller/PWM.cpp
index fb1add3d..89bc3f78 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/PWM.cpp
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/PWM.cpp
@@ -13,11 +13,10 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
{
}
-
void
PWMController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
{
-// ARMARX_INFO << "PWMController";
+ // ARMARX_INFO << "PWMController";
if (target.isValid())
{
@@ -29,7 +28,6 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
}
}
-
armarx::ControlTargetBase*
PWMController::getControlTarget()
{
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/PWM.h b/source/devices/ethercat/head_board/armar7de/joint_controller/PWM.h
index 8eb054d2..a8554045 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/PWM.h
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/PWM.h
@@ -8,7 +8,6 @@
// robot_devices
#include <devices/ethercat/head_board/armar7de/ControlTargets.h>
-
namespace devices::ethercat::head_board::armar7de
{
using DataPtr = std::shared_ptr<class Data>;
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/PWMPosition.cpp b/source/devices/ethercat/head_board/armar7de/joint_controller/PWMPosition.cpp
index 26cd9e58..70269189 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/PWMPosition.cpp
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/PWMPosition.cpp
@@ -51,7 +51,6 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
pidPosController->differentialFilter.reset(new armarx::rtfilters::AverageFilter(10));
}
-
PWMPositionController::~PWMPositionController() noexcept(true)
{
stopRequested = true;
@@ -64,12 +63,11 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
}
}
-
void
PWMPositionController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
const IceUtil::Time& timeSinceLastIteration)
{
-// ARMARX_INFO << "PWMPositionController";
+ // ARMARX_INFO << "PWMPositionController";
if (target.isValid())
{
@@ -145,14 +143,12 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
}
}
-
armarx::ControlTargetBase*
PWMPositionController::getControlTarget()
{
return ⌖
}
-
void
PWMPositionController::rtPreActivateController()
{
@@ -167,7 +163,6 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
// controller.reset(dataPtr->getVelocity());
}
-
void
PWMPositionController::rtPostDeactivateController()
{
@@ -175,7 +170,6 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
// dataPtr->setTargetPWM(0);
}
-
armarx::StringVariantBaseMap
PWMPositionController::publish(const armarx::DebugDrawerInterfacePrx& /*draw*/,
const armarx::DebugObserverInterfacePrx& /*observer*/) const
@@ -334,11 +328,9 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
configData.maxIntegral = hwConfig.getFloat("maxIntegral");
configData.feedforwardVelocityToPWMFactor =
hwConfig.getFloat("feedforwardVelocityToPWMFactor");
- configData.feedforwardTorqueToPWMFactor =
- hwConfig.getFloat("feedforwardTorqueToPWMFactor");
+ configData.feedforwardTorqueToPWMFactor = hwConfig.getFloat("feedforwardTorqueToPWMFactor");
configData.PWMDeadzone = hwConfig.getFloat("PWMDeadzone");
- configData.velocityUpdatePercent =
- hwConfig.getFloat("velocityUpdatePercent");
+ configData.velocityUpdatePercent = hwConfig.getFloat("velocityUpdatePercent");
configData.conditionalIntegralErrorTreshold =
hwConfig.getFloat("conditionalIntegralErrorTreshold");
configData.feedForwardMode = hwConfig.getBool("FeedForwardMode");
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/PWMPosition.h b/source/devices/ethercat/head_board/armar7de/joint_controller/PWMPosition.h
index c5e22ee8..318f56e4 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/PWMPosition.h
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/PWMPosition.h
@@ -10,7 +10,6 @@
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <armarx/control/hardware_config/Config.h>
-
namespace armarx
{
using PIDControllerPtr = std::shared_ptr<class PIDController>;
@@ -40,6 +39,7 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
PWMPositionControllerConfiguration()
{
}
+
static PWMPositionControllerConfigurationCPtr
CreatePWMPositionControllerConfigData(armarx::control::hardware_config::Config& hwConfig);
float maxVelocityRad;
@@ -61,7 +61,6 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
using PWMPositionControllerPtr = std::shared_ptr<class PWMPositionController>;
-
class PWMPositionController : public armarx::JointController
{
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/PWMVelocity.cpp b/source/devices/ethercat/head_board/armar7de/joint_controller/PWMVelocity.cpp
index e50d2702..889a12a0 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/PWMVelocity.cpp
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/PWMVelocity.cpp
@@ -45,7 +45,6 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
this->isLimitless = board->isLimitless();
}
-
PWMVelocityController::~PWMVelocityController() noexcept(true)
{
stopRequested = true;
@@ -58,12 +57,11 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
}
}
-
void
PWMVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
{
-// ARMARX_INFO << "PWMVelocityController";
+ // ARMARX_INFO << "PWMVelocityController";
if (target.isValid())
{
@@ -108,9 +106,9 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
dataPtr->getVelocity(),
newVel,
sensorValue->gravityTorque));
-// ARMARX_RT_LOGF_INFO("target pwm: %d", targetPWM);
-// if (std::abs(targetPWM) > 300)
-// targetPWM = 0;
+ // ARMARX_RT_LOGF_INFO("target pwm: %d", targetPWM);
+ // if (std::abs(targetPWM) > 300)
+ // targetPWM = 0;
dataPtr->setMotorPwmValue(targetPWM);
lastTargetVelocity = newVel;
@@ -118,8 +116,8 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
// 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, velController acc: %.3f",
-// target.velocity, dataPtr->getVelocity(), targetPWM, controller.pid->Kp, controller.pid->Ki, controller.pid->Kd, velController.currentAcc).deactivateSpam(1);
+ // ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, velController acc: %.3f",
+ // target.velocity, dataPtr->getVelocity(), targetPWM, controller.pid->Kp, controller.pid->Ki, controller.pid->Kd, velController.currentAcc).deactivateSpam(1);
}
else
{
@@ -127,14 +125,12 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
}
}
-
armarx::ControlTargetBase*
PWMVelocityController::getControlTarget()
{
return ⌖
}
-
void
PWMVelocityController::rtPreActivateController()
{
@@ -143,7 +139,6 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
controller.reset(static_cast<double>(dataPtr->getVelocity()));
}
-
void
PWMVelocityController::rtPostDeactivateController()
{
@@ -151,7 +146,6 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
// dataPtr->setTargetPWM(0);
}
-
armarx::StringVariantBaseMap
PWMVelocityController::publish(const armarx::DebugDrawerInterfacePrx& /*draw*/,
const armarx::DebugObserverInterfacePrx& /*observer*/) const
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/PWMVelocity.h b/source/devices/ethercat/head_board/armar7de/joint_controller/PWMVelocity.h
index aba19bd7..a5eab464 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/PWMVelocity.h
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/PWMVelocity.h
@@ -11,19 +11,16 @@
// robot_devices
#include <devices/ethercat/head_board/armar7de/PWMVelocity.h>
-
namespace armarx
{
using PIDControllerPtr = std::shared_ptr<class PIDController>;
}
-
namespace devices::ethercat::head_board::armar7de
{
using DataPtr = std::shared_ptr<class Data>;
}
-
namespace devices::ethercat::head_board::armar7de
{
class Device;
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/StopMovement.cpp b/source/devices/ethercat/head_board/armar7de/joint_controller/StopMovement.cpp
index a25efcce..70b232c8 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/StopMovement.cpp
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/StopMovement.cpp
@@ -4,7 +4,6 @@
// robot_devices
#include <devices/ethercat/head_board/armar7de/Data.h>
-
namespace devices::ethercat::head_board::armar7de::joint_controller
{
@@ -14,7 +13,6 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
pid->maxIntegral = 0.1;
}
-
void
StopMovementController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -28,21 +26,19 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
}
// ARMARX_INFO << "StopMovement";
-// ARMARX_INFO << VAROUT(dataPtr->getVelocity());
-// ARMARX_INFO << VAROUT(targetPwm);
+ // ARMARX_INFO << VAROUT(dataPtr->getVelocity());
+ // ARMARX_INFO << VAROUT(targetPwm);
targetPwm = 0.0f;
dataPtr->setMotorPwmValue(targetPwm);
}
-
armarx::ControlTargetBase*
StopMovementController::getControlTarget()
{
return ⌖
}
-
void
StopMovementController::rtPreActivateController()
{
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/StopMovement.h b/source/devices/ethercat/head_board/armar7de/joint_controller/StopMovement.h
index 71258157..aea31d66 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/StopMovement.h
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/StopMovement.h
@@ -5,19 +5,16 @@
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/libraries/core/PIDController.h>
-
namespace devices::ethercat::head_board::armar7de
{
using DataPtr = std::shared_ptr<class Data>;
}
-
namespace devices::ethercat::head_board::armar7de::joint_controller
{
using StopMovementControllerPtr = std::shared_ptr<class StopMovementController>;
-
class StopMovementController : public armarx::JointController
{
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/ZeroTorque.cpp b/source/devices/ethercat/head_board/armar7de/joint_controller/ZeroTorque.cpp
index fd993a6f..6b8c8c10 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/ZeroTorque.cpp
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/ZeroTorque.cpp
@@ -20,7 +20,6 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
return std::make_shared<PWMZeroTorqueControllerConfiguration>(configData);
}
-
PWMZeroTorqueController::PWMZeroTorqueController(
const std::string& deviceName,
Device* board,
@@ -33,17 +32,15 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
this->isLimitless = board->isLimitless();
}
-
PWMZeroTorqueController::~PWMZeroTorqueController() noexcept(true)
{
}
-
void
PWMZeroTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
{
-// ARMARX_INFO << "PWMZeroTorqueController";
+ // ARMARX_INFO << "PWMZeroTorqueController";
if (target.isValid())
{
@@ -62,14 +59,12 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
}
}
-
armarx::ControlTargetBase*
PWMZeroTorqueController::getControlTarget()
{
return ⌖
}
-
void
PWMZeroTorqueController::rtPreActivateController()
{
@@ -77,7 +72,6 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
// controller.reset(dataPtr->getVelocity());
}
-
void
PWMZeroTorqueController::rtPostDeactivateController()
{
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/ZeroTorque.h b/source/devices/ethercat/head_board/armar7de/joint_controller/ZeroTorque.h
index ae00ce38..c24812ca 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/ZeroTorque.h
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/ZeroTorque.h
@@ -7,7 +7,6 @@
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <armarx/control/hardware_config/Config.h>
-
namespace devices::ethercat::head_board::armar7de
{
using DataPtr = std::shared_ptr<class Data>;
@@ -18,7 +17,6 @@ namespace devices::ethercat::head_board::armar7de
class Device;
}
-
namespace devices::ethercat::head_board::armar7de::joint_controller
{
@@ -27,7 +25,6 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
using PWMZeroTorqueControllerConfigurationCPtr =
std::shared_ptr<const class PWMZeroTorqueControllerConfiguration>;
-
class PWMZeroTorqueControllerConfiguration
{
@@ -35,6 +32,7 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
PWMZeroTorqueControllerConfiguration()
{
}
+
static PWMZeroTorqueControllerConfigurationCPtr
CreateConfigData(armarx::control::hardware_config::Config& hwConfig);
float feedforwardVelocityToPWMFactor;
@@ -44,7 +42,6 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
using JointPWMZeroTorqueControllerPtr = std::shared_ptr<class PWMZeroTorqueController>;
-
class PWMZeroTorqueController : public armarx::JointController
{
diff --git a/source/devices/ethercat/head_board/armar7de/njoint_controller/EyeColor.cpp b/source/devices/ethercat/head_board/armar7de/njoint_controller/EyeColor.cpp
index c1c61aff..e501a2b0 100644
--- a/source/devices/ethercat/head_board/armar7de/njoint_controller/EyeColor.cpp
+++ b/source/devices/ethercat/head_board/armar7de/njoint_controller/EyeColor.cpp
@@ -1,15 +1,15 @@
#include <devices/ethercat/head_board/armar7de/njoint_controller/EyeColor.h>
-
namespace devices::ethercat::head_board::armar7de
{
- armarx::NJointControllerRegistration<EyeColorController> registrationControllerNJointKITHandV2ShapeController("NJointHeadBoardArmar7DEEyeColorController");
-
+ armarx::NJointControllerRegistration<EyeColorController>
+ registrationControllerNJointKITHandV2ShapeController(
+ "NJointHeadBoardArmar7DEEyeColorController");
EyeColorController::EyeColorController(armarx::RobotUnit* prov,
- EyeColorControllerConfigPtr config,
- const VirtualRobot::RobotPtr& r)
+ EyeColorControllerConfigPtr config,
+ const VirtualRobot::RobotPtr& r)
{
ARMARX_CHECK_EXPRESSION(prov);
auto deviceName = config->deviceName + "Led";
@@ -21,13 +21,14 @@ namespace devices::ethercat::head_board::armar7de
controlTarget = ct->asA<ControlTargetHeadLedColor>();
}
-
- void EyeColorController::rtPreActivateController()
+ void
+ EyeColorController::rtPreActivateController()
{
}
-
- void EyeColorController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ EyeColorController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
controlTarget->r.fill(r);
@@ -35,34 +36,36 @@ namespace devices::ethercat::head_board::armar7de
controlTarget->b.fill(b);
}
- armarx::WidgetDescription::WidgetPtr EyeColorController::GenerateConfigDescription(
+ armarx::WidgetDescription::WidgetPtr
+ EyeColorController::GenerateConfigDescription(
const VirtualRobot::RobotPtr&,
const std::map<std::string, armarx::ConstControlDevicePtr>&,
const std::map<std::string, armarx::ConstSensorDevicePtr>&)
{
- using namespace armarx::WidgetDescription;
- HBoxLayoutPtr layout = new HBoxLayout;
-
- LabelPtr label = new Label;
- label->text = "control device name";
- layout->children.emplace_back(label);
-
- LineEditPtr edit = new LineEdit;
- edit->name = "deviceName";
- edit->defaultValue = "Neck_1_Yaw";
- layout->children.emplace_back(edit);
- return layout;
+ using namespace armarx::WidgetDescription;
+ HBoxLayoutPtr layout = new HBoxLayout;
+
+ LabelPtr label = new Label;
+ label->text = "control device name";
+ layout->children.emplace_back(label);
+
+ LineEditPtr edit = new LineEdit;
+ edit->name = "deviceName";
+ edit->defaultValue = "Neck_1_Yaw";
+ layout->children.emplace_back(edit);
+ return layout;
}
- EyeColorControllerConfigPtr EyeColorController::GenerateConfigFromVariants(const armarx::StringVariantBaseMap& values)
+ EyeColorControllerConfigPtr
+ EyeColorController::GenerateConfigFromVariants(const armarx::StringVariantBaseMap& values)
{
-// ARMARX_INFO_S << VAROUT(values);
- return new EyeColorControllerConfig {values.at("deviceName")->getString()};
-// return new EyeColorControllerConfig {"Neck_1_Yaw"};
+ // ARMARX_INFO_S << VAROUT(values);
+ return new EyeColorControllerConfig{values.at("deviceName")->getString()};
+ // return new EyeColorControllerConfig {"Neck_1_Yaw"};
}
-
- armarx::WidgetDescription::StringWidgetDictionary EyeColorController::getFunctionDescriptions(const Ice::Current&) const
+ armarx::WidgetDescription::StringWidgetDictionary
+ EyeColorController::getFunctionDescriptions(const Ice::Current&) const
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr hbox = new HBoxLayout;
@@ -97,8 +100,10 @@ namespace devices::ethercat::head_board::armar7de
return {{"Targets", hbox}};
}
-
- void EyeColorController::callDescribedFunction(const std::string& name, const armarx::StringVariantBaseMap& valueMap, const Ice::Current&)
+ void
+ EyeColorController::callDescribedFunction(const std::string& name,
+ const armarx::StringVariantBaseMap& valueMap,
+ const Ice::Current&)
{
if (name == "Targets")
{
@@ -113,12 +118,12 @@ namespace devices::ethercat::head_board::armar7de
}
}
-
- void EyeColorController::setTargets(int r, int g, int b, const Ice::Current&)
+ void
+ EyeColorController::setTargets(int r, int g, int b, const Ice::Current&)
{
this->r = r;
this->g = g;
this->b = b;
}
-} // namespace armarx
+} // namespace devices::ethercat::head_board::armar7de
diff --git a/source/devices/ethercat/head_board/armar7de/njoint_controller/EyeColor.h b/source/devices/ethercat/head_board/armar7de/njoint_controller/EyeColor.h
index b5c3191e..0695af09 100644
--- a/source/devices/ethercat/head_board/armar7de/njoint_controller/EyeColor.h
+++ b/source/devices/ethercat/head_board/armar7de/njoint_controller/EyeColor.h
@@ -9,62 +9,56 @@
#include <devices/ethercat/head_board/armar7de/njoint_controller/EyeColor.h>
#include <devices/ethercat/head_board/armar7de/njoint_controller/EyeColorInterface.h>
-
namespace devices::ethercat::head_board::armar7de
{
using EyeColorControllerConfigPtr = IceUtil::Handle<class EyeColorControllerConfig>;
-
- class EyeColorControllerConfig :
- virtual public armarx::NJointControllerConfig
+ class EyeColorControllerConfig : virtual public armarx::NJointControllerConfig
{
public:
-
- EyeColorControllerConfig(std::string const& deviceName):
- deviceName(deviceName)
- {}
+ EyeColorControllerConfig(std::string const& deviceName) : deviceName(deviceName)
+ {
+ }
const std::string deviceName;
-
};
using EyeColorControllerPtr = IceInternal::Handle<class EyeColorController>;
-
- class EyeColorController :
- public armarx::NJointController,
- public EyeColorControllerInterface
+ class EyeColorController : public armarx::NJointController, public EyeColorControllerInterface
{
public:
-
using ConfigPtrT = EyeColorControllerConfigPtr;
- EyeColorController(armarx::RobotUnit* prov, EyeColorControllerConfigPtr config, const VirtualRobot::RobotPtr& r);
+ EyeColorController(armarx::RobotUnit* prov,
+ EyeColorControllerConfigPtr config,
+ const VirtualRobot::RobotPtr& r);
// NJointControllerInterface interface
public:
-
- std::string getClassName(const Ice::Current&) const override
+ std::string
+ getClassName(const Ice::Current&) const override
{
return "NJointHeadBoardArmar7DEEyeColorController";
}
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
- static armarx::WidgetDescription::WidgetPtr GenerateConfigDescription(
- const VirtualRobot::RobotPtr&,
- const std::map<std::string, armarx::ConstControlDevicePtr>&,
- const std::map<std::string, armarx::ConstSensorDevicePtr>&);
+ static armarx::WidgetDescription::WidgetPtr
+ GenerateConfigDescription(const VirtualRobot::RobotPtr&,
+ const std::map<std::string, armarx::ConstControlDevicePtr>&,
+ const std::map<std::string, armarx::ConstSensorDevicePtr>&);
- static EyeColorControllerConfigPtr GenerateConfigFromVariants(const armarx::StringVariantBaseMap& values);
+ static EyeColorControllerConfigPtr
+ GenerateConfigFromVariants(const armarx::StringVariantBaseMap& values);
private:
-
ControlTargetHeadLedColor* controlTarget;
std::uint8_t r = 0;
@@ -72,21 +66,20 @@ namespace devices::ethercat::head_board::armar7de
std::uint8_t b = 0;
public:
-
- armarx::WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
- void callDescribedFunction(const std::string& name, const armarx::StringVariantBaseMap& valueMap, const Ice::Current&) override;
+ armarx::WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current&) const override;
+ void callDescribedFunction(const std::string& name,
+ const armarx::StringVariantBaseMap& valueMap,
+ const Ice::Current&) override;
// NJointHeadBoardArmar7DEEyeColorControllerInterface interface
public:
-
void setTargets(int r, int g, int b, const Ice::Current& = Ice::emptyCurrent) override;
// NJointControllerBase interface
protected:
-
void rtPreActivateController() override;
-
};
-}
+} // namespace devices::ethercat::head_board::armar7de
diff --git a/source/devices/ethercat/head_board/armar7de/njoint_controller/EyeColorInterface.ice b/source/devices/ethercat/head_board/armar7de/njoint_controller/EyeColorInterface.ice
index a82f6e68..f9db99b5 100644
--- a/source/devices/ethercat/head_board/armar7de/njoint_controller/EyeColorInterface.ice
+++ b/source/devices/ethercat/head_board/armar7de/njoint_controller/EyeColorInterface.ice
@@ -28,11 +28,19 @@
#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
-module devices { module ethercat { module head_board { module armar7de
+module devices
{
- interface EyeColorControllerInterface extends armarx::NJointControllerInterface
+ module ethercat
{
- void setTargets(int r, int g, int b);
+ module head_board
+ {
+ module armar7de
+ {
+ interface EyeColorControllerInterface extends armarx::NJointControllerInterface
+ {
+ void setTargets(int r, int g, int b);
+ };
+ };
+ };
};
-
-};};};};
+};
diff --git a/source/devices/ethercat/platform/armar6_mecanum/Armar6MecanumPlatform.cpp b/source/devices/ethercat/platform/armar6_mecanum/Armar6MecanumPlatform.cpp
old mode 100755
new mode 100644
index d4710b04..552e6d80
--- a/source/devices/ethercat/platform/armar6_mecanum/Armar6MecanumPlatform.cpp
+++ b/source/devices/ethercat/platform/armar6_mecanum/Armar6MecanumPlatform.cpp
@@ -30,7 +30,6 @@
// armarx
#include <ArmarXCore/core/logging/Logging.h>
-
namespace devices::ethercat::platform::armar6_mecanum
{
@@ -41,33 +40,47 @@ namespace devices::ethercat::platform::armar6_mecanum
info.radius = 95.f;
}
-
- Eigen::Vector4f Armar6MecanumPlatform::calcWheelVelocity(float vx, float vy, float vAngle)
+ Eigen::Vector4f
+ Armar6MecanumPlatform::calcWheelVelocity(float vx, float vy, float vAngle)
{
Eigen::Vector4f wheelVelocities;
- wheelVelocities(0) = 1.0f / info.radius * (+ vx + vy - (info.platformHalfWidth + info.platformHalfHeight) * vAngle);
- wheelVelocities(1) = 1.0f / info.radius * (- vx + vy + (info.platformHalfWidth + info.platformHalfHeight) * vAngle);
- wheelVelocities(2) = 1.0f / info.radius * (- vx + vy - (info.platformHalfWidth + info.platformHalfHeight) * vAngle);
- wheelVelocities(3) = 1.0f / info.radius * (+ vx + vy + (info.platformHalfWidth + info.platformHalfHeight) * vAngle);
+ wheelVelocities(0) =
+ 1.0f / info.radius *
+ (+vx + vy - (info.platformHalfWidth + info.platformHalfHeight) * vAngle);
+ wheelVelocities(1) =
+ 1.0f / info.radius *
+ (-vx + vy + (info.platformHalfWidth + info.platformHalfHeight) * vAngle);
+ wheelVelocities(2) =
+ 1.0f / info.radius *
+ (-vx + vy - (info.platformHalfWidth + info.platformHalfHeight) * vAngle);
+ wheelVelocities(3) =
+ 1.0f / info.radius *
+ (+vx + vy + (info.platformHalfWidth + info.platformHalfHeight) * vAngle);
return wheelVelocities;
}
-
- Eigen::Vector3f Armar6MecanumPlatform::calcCartesianVelocity(Eigen::Vector4f const& wheelVelocities)
+ Eigen::Vector3f
+ Armar6MecanumPlatform::calcCartesianVelocity(Eigen::Vector4f const& wheelVelocities)
{
- Eigen::Vector3f result;
- result(0) = (wheelVelocities(0) - wheelVelocities(1) - wheelVelocities(2) + wheelVelocities(3)) * info.radius * 0.25f;
- result(1) = (wheelVelocities(0) + wheelVelocities(1) + wheelVelocities(2) + wheelVelocities(3)) * info.radius * 0.25f;
- result(2) = (-wheelVelocities(0) + wheelVelocities(1) - wheelVelocities(2) + wheelVelocities(3)) * info.radius / (4.0f * (info.platformHalfWidth + info.platformHalfHeight));
+ Eigen::Vector3f result;
+ result(0) =
+ (wheelVelocities(0) - wheelVelocities(1) - wheelVelocities(2) + wheelVelocities(3)) *
+ info.radius * 0.25f;
+ result(1) =
+ (wheelVelocities(0) + wheelVelocities(1) + wheelVelocities(2) + wheelVelocities(3)) *
+ info.radius * 0.25f;
+ result(2) =
+ (-wheelVelocities(0) + wheelVelocities(1) - wheelVelocities(2) + wheelVelocities(3)) *
+ info.radius / (4.0f * (info.platformHalfWidth + info.platformHalfHeight));
return result;
}
-
- const PlatformGeometryInfo& Armar6MecanumPlatform::getInfo() const
+ const PlatformGeometryInfo&
+ Armar6MecanumPlatform::getInfo() const
{
return info;
}
-}
+} // namespace devices::ethercat::platform::armar6_mecanum
diff --git a/source/devices/ethercat/platform/armar6_mecanum/Armar6MecanumPlatform.h b/source/devices/ethercat/platform/armar6_mecanum/Armar6MecanumPlatform.h
old mode 100755
new mode 100644
index 8d3eb130..cbc109e9
--- a/source/devices/ethercat/platform/armar6_mecanum/Armar6MecanumPlatform.h
+++ b/source/devices/ethercat/platform/armar6_mecanum/Armar6MecanumPlatform.h
@@ -28,7 +28,6 @@
// Eigen
#include <Eigen/Dense>
-
namespace devices::ethercat::platform::armar6_mecanum
{
@@ -39,12 +38,10 @@ namespace devices::ethercat::platform::armar6_mecanum
float radius = 0;
};
-
class Armar6MecanumPlatform
{
public:
-
Armar6MecanumPlatform();
/**
@@ -67,9 +64,7 @@ namespace devices::ethercat::platform::armar6_mecanum
const PlatformGeometryInfo& getInfo() const;
private:
-
PlatformGeometryInfo info;
-
};
-}
+} // namespace devices::ethercat::platform::armar6_mecanum
diff --git a/source/devices/ethercat/platform/armar6_mecanum/Data.cpp b/source/devices/ethercat/platform/armar6_mecanum/Data.cpp
index b8a68831..13184c92 100644
--- a/source/devices/ethercat/platform/armar6_mecanum/Data.cpp
+++ b/source/devices/ethercat/platform/armar6_mecanum/Data.cpp
@@ -24,7 +24,6 @@
#include "Data.h"
-
namespace devices::ethercat::platform::armar6_mecanum
{
@@ -32,44 +31,69 @@ namespace devices::ethercat::platform::armar6_mecanum
const common::elmo::gold::ElmoVelocityDataConfig& frontLeftConfig,
const common::elmo::gold::ElmoVelocityDataConfig& rearRightConfig,
const common::elmo::gold::ElmoVelocityDataConfig& rearLeftConfig,
- common::elmo::gold::SlaveOut* frontRight_elmo_out, common::elmo::gold::SlaveIn* frontRight_elmo_in,
- common::elmo::gold::SlaveOut* frontLeft_elmo_out, common::elmo::gold::SlaveIn* frontLeft_elmo_in,
- common::elmo::gold::SlaveOut* rearRight_elmo_out, common::elmo::gold::SlaveIn* rearRight_elmo_in,
- common::elmo::gold::SlaveOut* rearLeft_elmo_out, common::elmo::gold::SlaveIn* rearLeft_elmo_in)
+ common::elmo::gold::SlaveOut* frontRight_elmo_out,
+ common::elmo::gold::SlaveIn* frontRight_elmo_in,
+ common::elmo::gold::SlaveOut* frontLeft_elmo_out,
+ common::elmo::gold::SlaveIn* frontLeft_elmo_in,
+ common::elmo::gold::SlaveOut* rearRight_elmo_out,
+ common::elmo::gold::SlaveIn* rearRight_elmo_in,
+ common::elmo::gold::SlaveOut* rearLeft_elmo_out,
+ common::elmo::gold::SlaveIn* rearLeft_elmo_in)
{
//sensor values
- frontRightVelocityValue.init(&frontRight_elmo_out->velocityActualValue, frontRightConfig.velocity);
- frontRightCurrentValue.init(&frontRight_elmo_out->currentActualValue, frontRightConfig.currentValue);
-
- frontLeftVelocityValue.init(&frontLeft_elmo_out->velocityActualValue, frontLeftConfig.velocity);
- frontLeftCurrentValue.init(&frontLeft_elmo_out->currentActualValue, frontLeftConfig.currentValue);
-
- rearRightVelocityValue.init(&rearRight_elmo_out->velocityActualValue, rearRightConfig.velocity);
- rearRightCurrentValue.init(&rearRight_elmo_out->currentActualValue, rearRightConfig.currentValue);
-
- rearLeftVelocityValue.init(&rearLeft_elmo_out->velocityActualValue, rearLeftConfig.velocity);
- rearLeftCurrentValue.init(&rearLeft_elmo_out->currentActualValue, rearLeftConfig.currentValue);
+ frontRightVelocityValue.init(&frontRight_elmo_out->velocityActualValue,
+ frontRightConfig.velocity);
+ frontRightCurrentValue.init(&frontRight_elmo_out->currentActualValue,
+ frontRightConfig.currentValue);
+
+ frontLeftVelocityValue.init(&frontLeft_elmo_out->velocityActualValue,
+ frontLeftConfig.velocity);
+ frontLeftCurrentValue.init(&frontLeft_elmo_out->currentActualValue,
+ frontLeftConfig.currentValue);
+
+ rearRightVelocityValue.init(&rearRight_elmo_out->velocityActualValue,
+ rearRightConfig.velocity);
+ rearRightCurrentValue.init(&rearRight_elmo_out->currentActualValue,
+ rearRightConfig.currentValue);
+
+ rearLeftVelocityValue.init(&rearLeft_elmo_out->velocityActualValue,
+ rearLeftConfig.velocity);
+ rearLeftCurrentValue.init(&rearLeft_elmo_out->currentActualValue,
+ rearLeftConfig.currentValue);
//targets
- frontRightTargetVelocity.init(&frontRight_elmo_in->targetVelocity, frontRightConfig.targetVelocity);
- frontRightTargetAcceleration.init(&frontRight_elmo_in->profiledAcceleration, frontRightConfig.targetAcceleration, 5);
- frontRightTargetDeceleration.init(&frontRight_elmo_in->profiledDeceleration, frontRightConfig.targetDeceleration, 10);
-
- frontLeftTargetVelocity.init(&frontLeft_elmo_in->targetVelocity, frontLeftConfig.targetVelocity);
- frontLeftTargetAcceleration.init(&frontLeft_elmo_in->profiledAcceleration, frontLeftConfig.targetAcceleration, 5);
- frontLeftTargetDeceleration.init(&frontLeft_elmo_in->profiledDeceleration, frontLeftConfig.targetDeceleration, 10);
-
- rearRightTargetVelocity.init(&rearRight_elmo_in->targetVelocity, rearRightConfig.targetVelocity);
- rearRightTargetAcceleration.init(&rearRight_elmo_in->profiledAcceleration, rearRightConfig.targetAcceleration, 5);
- rearRightTargetDeceleration.init(&rearRight_elmo_in->profiledDeceleration, rearRightConfig.targetDeceleration, 10);
-
- rearLeftTargetVelocity.init(&rearLeft_elmo_in->targetVelocity, rearLeftConfig.targetVelocity);
- rearLeftTargetAcceleration.init(&rearLeft_elmo_in->profiledAcceleration, rearLeftConfig.targetAcceleration, 5);
- rearLeftTargetDeceleration.init(&rearLeft_elmo_in->profiledDeceleration, rearLeftConfig.targetDeceleration, 10);
- }
-
-
- void Data::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ frontRightTargetVelocity.init(&frontRight_elmo_in->targetVelocity,
+ frontRightConfig.targetVelocity);
+ frontRightTargetAcceleration.init(
+ &frontRight_elmo_in->profiledAcceleration, frontRightConfig.targetAcceleration, 5);
+ frontRightTargetDeceleration.init(
+ &frontRight_elmo_in->profiledDeceleration, frontRightConfig.targetDeceleration, 10);
+
+ frontLeftTargetVelocity.init(&frontLeft_elmo_in->targetVelocity,
+ frontLeftConfig.targetVelocity);
+ frontLeftTargetAcceleration.init(
+ &frontLeft_elmo_in->profiledAcceleration, frontLeftConfig.targetAcceleration, 5);
+ frontLeftTargetDeceleration.init(
+ &frontLeft_elmo_in->profiledDeceleration, frontLeftConfig.targetDeceleration, 10);
+
+ rearRightTargetVelocity.init(&rearRight_elmo_in->targetVelocity,
+ rearRightConfig.targetVelocity);
+ rearRightTargetAcceleration.init(
+ &rearRight_elmo_in->profiledAcceleration, rearRightConfig.targetAcceleration, 5);
+ rearRightTargetDeceleration.init(
+ &rearRight_elmo_in->profiledDeceleration, rearRightConfig.targetDeceleration, 10);
+
+ rearLeftTargetVelocity.init(&rearLeft_elmo_in->targetVelocity,
+ rearLeftConfig.targetVelocity);
+ rearLeftTargetAcceleration.init(
+ &rearLeft_elmo_in->profiledAcceleration, rearLeftConfig.targetAcceleration, 5);
+ rearLeftTargetDeceleration.init(
+ &rearLeft_elmo_in->profiledDeceleration, rearLeftConfig.targetDeceleration, 10);
+ }
+
+ void
+ Data::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
frontRightCurrentValue.read();
frontRightVelocityValue.read();
@@ -84,8 +108,9 @@ namespace devices::ethercat::platform::armar6_mecanum
rearLeftVelocityValue.read();
}
-
- void Data::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ Data::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
frontRightTargetVelocity.write();
frontRightTargetAcceleration.write();
@@ -104,100 +129,100 @@ namespace devices::ethercat::platform::armar6_mecanum
rearLeftTargetDeceleration.write();
}
-
- float& Data::getFrontRightActualVelocityValue()
+ float&
+ Data::getFrontRightActualVelocityValue()
{
return frontRightVelocityValue.value;
}
-
- float& Data::getFrontRightActualCurrentValue()
+ float&
+ Data::getFrontRightActualCurrentValue()
{
return frontRightCurrentValue.value;
}
-
- float& Data::getFrontLeftActualVelocityValue()
+ float&
+ Data::getFrontLeftActualVelocityValue()
{
return frontLeftVelocityValue.value;
}
-
- float& Data::getFrontLeftActualCurrentValue()
+ float&
+ Data::getFrontLeftActualCurrentValue()
{
return frontLeftCurrentValue.value;
}
-
- float& Data::getRearRightActualVelocityValue()
+ float&
+ Data::getRearRightActualVelocityValue()
{
return rearRightTargetVelocity.value;
}
-
- float& Data::getRearRightActualCurrentValue()
+ float&
+ Data::getRearRightActualCurrentValue()
{
return rearRightCurrentValue.value;
}
-
- float& Data::getRearLeftActualVelocityValue()
+ float&
+ Data::getRearLeftActualVelocityValue()
{
return rearLeftVelocityValue.value;
}
-
- float& Data::getRearLeftActualCurrentValue()
+ float&
+ Data::getRearLeftActualCurrentValue()
{
return rearLeftCurrentValue.value;
}
-
- void Data::setFrontRightTargetVelocity(float target)
+ void
+ Data::setFrontRightTargetVelocity(float target)
{
frontRightTargetVelocity.value = target;
}
-
- void Data::setFrontLeftTargetVelocity(float target)
+ void
+ Data::setFrontLeftTargetVelocity(float target)
{
frontLeftTargetVelocity.value = target;
}
-
- void Data::setRearRightTargetVelocity(float target)
+ void
+ Data::setRearRightTargetVelocity(float target)
{
rearRightTargetVelocity.value = target;
}
-
- void Data::setRearLeftTargetVelocity(float target)
+ void
+ Data::setRearLeftTargetVelocity(float target)
{
rearLeftTargetVelocity.value = target;
}
-
- float Data::getFrontRightTargetVelocity() const
+ float
+ Data::getFrontRightTargetVelocity() const
{
return frontRightTargetVelocity.value;
}
-
- float Data::getFrontLeftTargetVelocity() const
+ float
+ Data::getFrontLeftTargetVelocity() const
{
return frontLeftTargetVelocity.value;
}
-
- float Data::getRearRightTargetVelocity() const
+ float
+ Data::getRearRightTargetVelocity() const
{
return rearRightTargetVelocity.value;
}
-
- float Data::getRearLeftTargetVelocity() const
+ float
+ Data::getRearLeftTargetVelocity() const
{
return rearLeftTargetVelocity.value;
}
-}
+} // namespace devices::ethercat::platform::armar6_mecanum
diff --git a/source/devices/ethercat/platform/armar6_mecanum/Data.h b/source/devices/ethercat/platform/armar6_mecanum/Data.h
index 57473ccc..a258323a 100644
--- a/source/devices/ethercat/platform/armar6_mecanum/Data.h
+++ b/source/devices/ethercat/platform/armar6_mecanum/Data.h
@@ -30,33 +30,36 @@
#include <armarx/control/ethercat/DataInterface.h>
// robot_devices
-#include <devices/ethercat/common/elmo/gold/Slave.h>
#include <devices/ethercat/common/elmo/gold/Config.h>
-
+#include <devices/ethercat/common/elmo/gold/Slave.h>
namespace devices::ethercat::platform::armar6_mecanum
{
using DataPtr = std::shared_ptr<class Data>;
-
class Data : public armarx::control::ethercat::DataInterface
{
public:
-
Data(const common::elmo::gold::ElmoVelocityDataConfig& frontRightConfig,
const common::elmo::gold::ElmoVelocityDataConfig& frontLeftConfig,
const common::elmo::gold::ElmoVelocityDataConfig& rearRightConfig,
const common::elmo::gold::ElmoVelocityDataConfig& rearLeftConfig,
- common::elmo::gold::SlaveOut* frontRight_elmo_out, common::elmo::gold::SlaveIn* frontRight_elmo_in,
- common::elmo::gold::SlaveOut* frontLeft_elmo_out, common::elmo::gold::SlaveIn* frontLeft_elmo_in,
- common::elmo::gold::SlaveOut* rearRight_elmo_out, common::elmo::gold::SlaveIn* rearRight_elmo_in,
- common::elmo::gold::SlaveOut* rearLeft_elmo_out, common::elmo::gold::SlaveIn* rearLeft_elmo_in);
+ common::elmo::gold::SlaveOut* frontRight_elmo_out,
+ common::elmo::gold::SlaveIn* frontRight_elmo_in,
+ common::elmo::gold::SlaveOut* frontLeft_elmo_out,
+ common::elmo::gold::SlaveIn* frontLeft_elmo_in,
+ common::elmo::gold::SlaveOut* rearRight_elmo_out,
+ common::elmo::gold::SlaveIn* rearRight_elmo_in,
+ common::elmo::gold::SlaveOut* rearLeft_elmo_out,
+ common::elmo::gold::SlaveIn* rearLeft_elmo_in);
- void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
- void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
float& getFrontRightActualVelocityValue();
float& getFrontRightActualCurrentValue();
@@ -86,9 +89,9 @@ namespace devices::ethercat::platform::armar6_mecanum
float getRearLeftTargetVelocity() const;
private:
-
using LinearConverted_int32 = armarx::control::ethercat::LinearConvertedValue<std::int32_t>;
- using LinearConverted_uint32 = armarx::control::ethercat::LinearConvertedValue<std::uint32_t>;
+ using LinearConverted_uint32 =
+ armarx::control::ethercat::LinearConvertedValue<std::uint32_t>;
using LinearConverted_int16 = armarx::control::ethercat::LinearConvertedValue<std::int16_t>;
LinearConverted_int32 frontRightTargetVelocity;
@@ -118,7 +121,6 @@ namespace devices::ethercat::platform::armar6_mecanum
LinearConverted_int32 rearLeftVelocityValue;
LinearConverted_int16 rearLeftCurrentValue;
-
};
-} // namespace devices::ethercat::platform::armar6_mecanum
+} // namespace devices::ethercat::platform::armar6_mecanum
diff --git a/source/devices/ethercat/platform/armar6_mecanum/Device.cpp b/source/devices/ethercat/platform/armar6_mecanum/Device.cpp
index b1db26e2..051d160f 100644
--- a/source/devices/ethercat/platform/armar6_mecanum/Device.cpp
+++ b/source/devices/ethercat/platform/armar6_mecanum/Device.cpp
@@ -26,12 +26,10 @@
#include <ArmarXCore/core/logging/Logging.h>
-
namespace devices::ethercat::platform::armar6_mecanum
{
- Device::Device(hwconfig::DeviceConfig& hwConfig,
- const VirtualRobot::RobotPtr&) :
+ Device::Device(hwconfig::DeviceConfig& hwConfig, const VirtualRobot::RobotPtr&) :
DeviceBase(hwConfig.getName()),
ControlDevice(hwConfig.getName()),
SensorDevice(hwConfig.getName()),
@@ -54,8 +52,9 @@ namespace devices::ethercat::platform::armar6_mecanum
rearRightElmo(nullptr),
rearLeftElmo(nullptr)
{
- platformControllerConfig = armarx::control::joint_controller::HolonomicPlatformControllerConfiguration::
- CreateHolonomicPlatformControllerConfigData(hwConfig.getControllerConfig("Platform"));
+ platformControllerConfig = armarx::control::joint_controller::
+ HolonomicPlatformControllerConfiguration::CreateHolonomicPlatformControllerConfigData(
+ hwConfig.getControllerConfig("Platform"));
}
void
@@ -79,7 +78,7 @@ namespace devices::ethercat::platform::armar6_mecanum
//creating the joint controller
velocityControllerPtr = std::make_shared<joint_controller::VelocityController>(
- dataPtr,platformControllerConfig);
+ dataPtr, platformControllerConfig);
emergencyControllerPtr =
std::make_shared<joint_controller::EmergencyController>(dataPtr);
stopMovementControllerPtr =
@@ -101,14 +100,12 @@ namespace devices::ethercat::platform::armar6_mecanum
rearLeftElmo->switchMode(common::elmo::gold::ElmoControlMode::VELOCITY);
}
-
const armarx::SensorValueBase*
Device::getSensorValue() const
{
return &sensorValue;
}
-
void
Device::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -165,7 +162,6 @@ namespace devices::ethercat::platform::armar6_mecanum
sensorValue.relativePositionY += integratedPosition(1);
}
-
void
Device::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -179,7 +175,6 @@ namespace devices::ethercat::platform::armar6_mecanum
dataPtr->rtWriteTargetValues(sensorValuesTimestamp, timeSinceLastIteration);
}
-
armarx::control::ethercat::DeviceInterface::TryAssignResult
Device::tryAssign(armarx::control::ethercat::SlaveInterface& slave)
{
@@ -238,49 +233,42 @@ namespace devices::ethercat::platform::armar6_mecanum
return "MecanumPlatform_Armar6";
}
-
std::vector<std::uint32_t>
Device::getElmoSerials() const
{
return {frontRightElmoSerial, frontLeftElmoSerial, rearRightElmoSerial, rearLeftElmoSerial};
}
-
const DataPtr&
Device::getDataPtr() const
{
return dataPtr;
}
-
std::uint16_t
Device::getFrontRightElmoSlaveNumber() const
{
return frontRightElmo->getSlaveNumber();
}
-
std::uint16_t
Device::getFrontLeftElmoSlaveNumber() const
{
return frontLeftElmo->getSlaveNumber();
}
-
std::uint16_t
Device::getRearRightElmoSlaveNumber() const
{
return rearRightElmo->getSlaveNumber();
}
-
std::uint16_t
Device::getRearLeftElmoSlaveNumber() const
{
return rearLeftElmo->getSlaveNumber();
}
-
std::string
Device::getWheelName(std::uint32_t elmoSerial) const
{
diff --git a/source/devices/ethercat/platform/armar6_mecanum/Device.h b/source/devices/ethercat/platform/armar6_mecanum/Device.h
index 90f18d52..6bc2d4fb 100644
--- a/source/devices/ethercat/platform/armar6_mecanum/Device.h
+++ b/source/devices/ethercat/platform/armar6_mecanum/Device.h
@@ -34,15 +34,14 @@
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
#include <armarx/control/ethercat/DeviceInterface.h>
-#include <devices/ethercat/common/elmo/gold/Slave.h>
#include <devices/ethercat/common/elmo/gold/Config.h>
+#include <devices/ethercat/common/elmo/gold/Slave.h>
#include <devices/ethercat/platform/armar6_mecanum/Armar6MecanumPlatform.h>
#include <devices/ethercat/platform/armar6_mecanum/Data.h>
#include <devices/ethercat/platform/armar6_mecanum/joint_controller/Emergency.h>
#include <devices/ethercat/platform/armar6_mecanum/joint_controller/StopMovement.h>
#include <devices/ethercat/platform/armar6_mecanum/joint_controller/Velocity.h>
-
namespace devices::ethercat::platform::armar6_mecanum
{
namespace hwconfig = armarx::control::hardware_config;
@@ -63,7 +62,7 @@ namespace devices::ethercat::platform::armar6_mecanum
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- Eigen::Vector4f wheelActualCurrents = Eigen::Vector4f::Zero();
+ Eigen::Vector4f wheelActualCurrents = Eigen::Vector4f::Zero();
Eigen::Vector4f wheelActualVelocities = Eigen::Vector4f::Zero();
Eigen::Vector4f wheelTargetVelocities = Eigen::Vector4f::Zero();
@@ -153,7 +152,6 @@ namespace devices::ethercat::platform::armar6_mecanum
}
};
-
class Device :
public armarx::ControlDevice,
public armarx::SensorDevice,
@@ -161,8 +159,7 @@ namespace devices::ethercat::platform::armar6_mecanum
{
public:
- Device(hwconfig::DeviceConfig& deviceConfig,
- const VirtualRobot::RobotPtr& robot);
+ Device(hwconfig::DeviceConfig& deviceConfig, const VirtualRobot::RobotPtr& robot);
// DeviceInterface interface
void postSwitchToSafeOp() override;
@@ -206,7 +203,8 @@ namespace devices::ethercat::platform::armar6_mecanum
const std::uint32_t rearLeftElmoSerial;
// Elmo configuration
- const common::elmo::gold::ElmoVelocityConfig frontRightConfig, frontLeftConfig, rearRightConfig, rearLeftConfig;
+ const common::elmo::gold::ElmoVelocityConfig frontRightConfig, frontLeftConfig,
+ rearRightConfig, rearLeftConfig;
// All elmo pointers.
common::elmo::gold::Slave* frontRightElmo;
@@ -214,7 +212,8 @@ namespace devices::ethercat::platform::armar6_mecanum
common::elmo::gold::Slave* rearRightElmo;
common::elmo::gold::Slave* rearLeftElmo;
- armarx::control::joint_controller::HolonomicPlatformControllerConfigurationPtr platformControllerConfig;
+ armarx::control::joint_controller::HolonomicPlatformControllerConfigurationPtr
+ platformControllerConfig;
// Joint controller.
joint_controller::VelocityControllerPtr velocityControllerPtr;
diff --git a/source/devices/ethercat/platform/armar6_mecanum/joint_controller/Emergency.cpp b/source/devices/ethercat/platform/armar6_mecanum/joint_controller/Emergency.cpp
index 67bd98dc..ccc55040 100644
--- a/source/devices/ethercat/platform/armar6_mecanum/joint_controller/Emergency.cpp
+++ b/source/devices/ethercat/platform/armar6_mecanum/joint_controller/Emergency.cpp
@@ -24,17 +24,13 @@
#include "Emergency.h"
-
namespace devices::ethercat::platform::armar6_mecanum::joint_controller
{
- EmergencyController::EmergencyController(
- DataPtr holoDataPtr) : dataPtr(holoDataPtr)
+ EmergencyController::EmergencyController(DataPtr holoDataPtr) : dataPtr(holoDataPtr)
{
-
}
-
void
EmergencyController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -45,11 +41,10 @@ namespace devices::ethercat::platform::armar6_mecanum::joint_controller
dataPtr->setRearLeftTargetVelocity(0.f);
}
-
armarx::ControlTargetBase*
EmergencyController::getControlTarget()
{
return ⌖
}
-}
+} // namespace devices::ethercat::platform::armar6_mecanum::joint_controller
diff --git a/source/devices/ethercat/platform/armar6_mecanum/joint_controller/Emergency.h b/source/devices/ethercat/platform/armar6_mecanum/joint_controller/Emergency.h
index 261a6ee3..3e3789fd 100644
--- a/source/devices/ethercat/platform/armar6_mecanum/joint_controller/Emergency.h
+++ b/source/devices/ethercat/platform/armar6_mecanum/joint_controller/Emergency.h
@@ -34,7 +34,6 @@
// robot_devices
#include <devices/ethercat/platform/armar6_mecanum/Data.h>
-
namespace devices::ethercat::platform::armar6_mecanum::joint_controller
{
@@ -44,18 +43,16 @@ namespace devices::ethercat::platform::armar6_mecanum::joint_controller
{
public:
-
EmergencyController(DataPtr holoDataPtr);
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
private:
-
armarx::DummyControlTargetEmergencyStop target;
DataPtr dataPtr;
-
};
-}
+} // namespace devices::ethercat::platform::armar6_mecanum::joint_controller
diff --git a/source/devices/ethercat/platform/armar6_mecanum/joint_controller/StopMovement.cpp b/source/devices/ethercat/platform/armar6_mecanum/joint_controller/StopMovement.cpp
index fe8f0eaf..47b81512 100644
--- a/source/devices/ethercat/platform/armar6_mecanum/joint_controller/StopMovement.cpp
+++ b/source/devices/ethercat/platform/armar6_mecanum/joint_controller/StopMovement.cpp
@@ -1,17 +1,15 @@
#include "StopMovement.h"
-
namespace devices::ethercat::platform::armar6_mecanum::joint_controller
{
- StopMovementController::StopMovementController(DataPtr holoDataPtr) :
- dataPtr(holoDataPtr)
+ StopMovementController::StopMovementController(DataPtr holoDataPtr) : dataPtr(holoDataPtr)
{
-
}
- void StopMovementController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration)
+ void
+ StopMovementController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
dataPtr->setFrontRightTargetVelocity(0.f);
dataPtr->setFrontLeftTargetVelocity(0.f);
@@ -19,9 +17,10 @@ namespace devices::ethercat::platform::armar6_mecanum::joint_controller
dataPtr->setRearLeftTargetVelocity(0.f);
}
- armarx::ControlTargetBase* StopMovementController::getControlTarget()
+ armarx::ControlTargetBase*
+ StopMovementController::getControlTarget()
{
return ⌖
}
-}
+} // namespace devices::ethercat::platform::armar6_mecanum::joint_controller
diff --git a/source/devices/ethercat/platform/armar6_mecanum/joint_controller/StopMovement.h b/source/devices/ethercat/platform/armar6_mecanum/joint_controller/StopMovement.h
index d7080a26..102eaf43 100644
--- a/source/devices/ethercat/platform/armar6_mecanum/joint_controller/StopMovement.h
+++ b/source/devices/ethercat/platform/armar6_mecanum/joint_controller/StopMovement.h
@@ -7,7 +7,6 @@
// robot_devices
#include <devices/ethercat/platform/armar6_mecanum/Data.h>
-
namespace devices::ethercat::platform::armar6_mecanum::joint_controller
{
@@ -17,18 +16,16 @@ namespace devices::ethercat::platform::armar6_mecanum::joint_controller
{
public:
-
StopMovementController(DataPtr holoDataPtr);
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
private:
-
armarx::DummyControlTargetStopMovement target;
DataPtr dataPtr;
-
};
-} // namespace devices::ethercat::platform::armar6_mecanum::joint_controller
+} // namespace devices::ethercat::platform::armar6_mecanum::joint_controller
diff --git a/source/devices/ethercat/platform/armar6_mecanum/joint_controller/Velocity.cpp b/source/devices/ethercat/platform/armar6_mecanum/joint_controller/Velocity.cpp
index 196ab9aa..17ec6352 100644
--- a/source/devices/ethercat/platform/armar6_mecanum/joint_controller/Velocity.cpp
+++ b/source/devices/ethercat/platform/armar6_mecanum/joint_controller/Velocity.cpp
@@ -35,20 +35,35 @@
namespace devices::ethercat::platform::armar6_mecanum::joint_controller
{
- VelocityController::VelocityController(DataPtr holoDataPtr, armarx::control::joint_controller::HolonomicPlatformControllerConfigurationPtr configDataPtr) :
- target(),
- dataPtr(holoDataPtr)
+ VelocityController::VelocityController(
+ DataPtr holoDataPtr,
+ armarx::control::joint_controller::HolonomicPlatformControllerConfigurationPtr
+ configDataPtr) :
+ target(), dataPtr(holoDataPtr)
{
//limitXVelocityController.init(500, 1500, 0.002f, 0);
//limitYVelocityController.init(500, 1500, 0.002f, 0);
//limitAngelVelocityController.init(0.8, 2, 0.002, 0);
- limitXVelocityController.init(configDataPtr->maxVelocity, configDataPtr->maxAcceleration, configDataPtr->maxDeceleration, 0.002f, 0);
- limitYVelocityController.init(configDataPtr->maxVelocity, configDataPtr->maxAcceleration, configDataPtr->maxDeceleration, 0.002f, 0);
- limitAngelVelocityController.init(configDataPtr->maxAngularVelocity, configDataPtr->maxAngularAcceleration, configDataPtr->maxAngularDeceleration, 0.002f, 0);
+ limitXVelocityController.init(configDataPtr->maxVelocity,
+ configDataPtr->maxAcceleration,
+ configDataPtr->maxDeceleration,
+ 0.002f,
+ 0);
+ limitYVelocityController.init(configDataPtr->maxVelocity,
+ configDataPtr->maxAcceleration,
+ configDataPtr->maxDeceleration,
+ 0.002f,
+ 0);
+ limitAngelVelocityController.init(configDataPtr->maxAngularVelocity,
+ configDataPtr->maxAngularAcceleration,
+ configDataPtr->maxAngularDeceleration,
+ 0.002f,
+ 0);
}
-
- void VelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ VelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
// ARMARX_INFO << deactivateSpam(1) << VAROUT(target.velocityY);
@@ -59,7 +74,7 @@ namespace devices::ethercat::platform::armar6_mecanum::joint_controller
//wait a second to give the wheels time to release breaks
if (sensorValuesTimestamp.toSecondsDouble() > 0.0)
{
- float dt = static_cast<float>(timeSinceLastIteration.toSecondsDouble());
+ float dt = static_cast<float>(timeSinceLastIteration.toSecondsDouble());
float vx = limitXVelocityController.update(vxTarget, dt);
float vy = limitYVelocityController.update(vyTarget, dt);
@@ -80,14 +95,18 @@ namespace devices::ethercat::platform::armar6_mecanum::joint_controller
}
}
-
- armarx::ControlTargetBase* VelocityController::getControlTarget()
+ armarx::ControlTargetBase*
+ VelocityController::getControlTarget()
{
return ⌖
}
-
- void LinearLimitedAccelerationController::init(float maxVelocity, float maxAcceleration, float maxDeceleration, float maxDeltaT, float value)
+ void
+ LinearLimitedAccelerationController::init(float maxVelocity,
+ float maxAcceleration,
+ float maxDeceleration,
+ float maxDeltaT,
+ float value)
{
this->maxVelocity = maxVelocity;
this->maxAcceleration = maxAcceleration;
@@ -96,8 +115,8 @@ namespace devices::ethercat::platform::armar6_mecanum::joint_controller
this->currentValue = value;
}
-
- float LinearLimitedAccelerationController::update(float value, float dt)
+ float
+ LinearLimitedAccelerationController::update(float value, float dt)
{
dt = std::max(0.f, std::min(dt, maxDeltaT));
float delta = value - currentValue;
@@ -107,7 +126,8 @@ namespace devices::ethercat::platform::armar6_mecanum::joint_controller
delta = -delta;
sign = -1;
}
- delta = delta < 0 ? -std::min(maxDeceleration * dt, -delta) : std::min(maxAcceleration * dt, delta);
+ delta = delta < 0 ? -std::min(maxDeceleration * dt, -delta)
+ : std::min(maxAcceleration * dt, delta);
delta = delta * sign;
currentValue = currentValue + delta;
currentValue = armarx::math::MathUtils::LimitTo(currentValue, maxVelocity);
@@ -115,4 +135,4 @@ namespace devices::ethercat::platform::armar6_mecanum::joint_controller
return currentValue;
}
-} // namespace devices::ethercat::platform::armar6_mecanum::joint_controller
+} // namespace devices::ethercat::platform::armar6_mecanum::joint_controller
diff --git a/source/devices/ethercat/platform/armar6_mecanum/joint_controller/Velocity.h b/source/devices/ethercat/platform/armar6_mecanum/joint_controller/Velocity.h
index 7b03a9d0..a3887ef3 100644
--- a/source/devices/ethercat/platform/armar6_mecanum/joint_controller/Velocity.h
+++ b/source/devices/ethercat/platform/armar6_mecanum/joint_controller/Velocity.h
@@ -29,26 +29,23 @@
#include <memory>
// armarx
-#include <armarx/control/joint_controller/ControllerConfiguration.h>
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <armarx/control/joint_controller/ControllerConfiguration.h>
// robot_devices
#include <devices/ethercat/platform/armar6_mecanum/Armar6MecanumPlatform.h>
#include <devices/ethercat/platform/armar6_mecanum/Data.h>
-
namespace devices::ethercat::platform::armar6_mecanum::joint_controller
{
using VelocityControllerPtr = std::shared_ptr<class VelocityController>;
-
class LinearLimitedAccelerationController
{
private:
-
float maxVelocity{0.F};
float maxAcceleration{0.F};
float maxDeceleration{0.F};
@@ -56,33 +53,45 @@ namespace devices::ethercat::platform::armar6_mecanum::joint_controller
float currentValue{0.F};
public:
-
- LinearLimitedAccelerationController(float maxVelocity, float maxAcceleration, float maxDeceleration, float maxDeltaT, float value)
- : maxVelocity(maxVelocity), maxAcceleration(maxAcceleration), maxDeceleration(maxDeceleration), maxDeltaT(maxDeltaT), currentValue(value)
- { }
+ LinearLimitedAccelerationController(float maxVelocity,
+ float maxAcceleration,
+ float maxDeceleration,
+ float maxDeltaT,
+ float value) :
+ maxVelocity(maxVelocity),
+ maxAcceleration(maxAcceleration),
+ maxDeceleration(maxDeceleration),
+ maxDeltaT(maxDeltaT),
+ currentValue(value)
+ {
+ }
LinearLimitedAccelerationController() = default;
- void init(float maxVelocity, float maxAcceleration, float maxDeceleration, float maxDeltaT, float value);
+ void init(float maxVelocity,
+ float maxAcceleration,
+ float maxDeceleration,
+ float maxDeltaT,
+ float value);
float update(float value, float dt);
-
};
-
class VelocityController : public armarx::JointController
{
public:
+ VelocityController(
+ DataPtr holoDataPtr,
+ armarx::control::joint_controller::HolonomicPlatformControllerConfigurationPtr
+ configDataPtr);
- VelocityController(DataPtr holoDataPtr, armarx::control::joint_controller::HolonomicPlatformControllerConfigurationPtr configDataPtr);
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
private:
-
armarx::ControlTargetHolonomicPlatformVelocity target;
DataPtr dataPtr;
@@ -91,7 +100,6 @@ namespace devices::ethercat::platform::armar6_mecanum::joint_controller
LinearLimitedAccelerationController limitXVelocityController;
LinearLimitedAccelerationController limitYVelocityController;
LinearLimitedAccelerationController limitAngelVelocityController;
-
};
-} // namespace devices::ethercat::platform::armar6_mecanum::joint_controller
+} // namespace devices::ethercat::platform::armar6_mecanum::joint_controller
diff --git a/source/devices/ethercat/platform/armar7_omni/joint_controller/Emergency.h b/source/devices/ethercat/platform/armar7_omni/joint_controller/Emergency.h
index 7f349456..2e591436 100644
--- a/source/devices/ethercat/platform/armar7_omni/joint_controller/Emergency.h
+++ b/source/devices/ethercat/platform/armar7_omni/joint_controller/Emergency.h
@@ -29,7 +29,6 @@
// RobotDevices
#include <devices/ethercat/platform/armar7_omni/Data.h>
-
namespace devices::ethercat::platform::armar7_omni::joint_controller
{
diff --git a/source/devices/ethercat/platform/armar7_omni/joint_controller/Velocity.h b/source/devices/ethercat/platform/armar7_omni/joint_controller/Velocity.h
index f6b00f1e..55d72aee 100644
--- a/source/devices/ethercat/platform/armar7_omni/joint_controller/Velocity.h
+++ b/source/devices/ethercat/platform/armar7_omni/joint_controller/Velocity.h
@@ -36,7 +36,6 @@
// RobotDevices
#include <devices/ethercat/platform/armar7_omni/Data.h>
-
namespace devices::ethercat::platform::armar7_omni::joint_controller
{
diff --git a/source/devices/ethercat/power_board/Config.h b/source/devices/ethercat/power_board/Config.h
index 48967585..3ea34732 100644
--- a/source/devices/ethercat/power_board/Config.h
+++ b/source/devices/ethercat/power_board/Config.h
@@ -1,17 +1,17 @@
#pragma once
-#include <armarx/control/hardware_config/SlaveConfig.h>
#include <armarx/control/hardware_config/DeviceConfig.h>
+#include <armarx/control/hardware_config/SlaveConfig.h>
namespace devices::ethercat::power_board
{
struct SlaveConfig
{
- SlaveConfig(armarx::control::hardware_config::SlaveConfig& hwConfig)
- : ledStrip1Count(hwConfig.getUint("ledRGBStrip1Count")),
- ledStrip2Count(hwConfig.getUint("ledRGBStrip2Count")),
- ledStrip1GlobalBrightness(hwConfig.getUint("ledRGBStrip1GlobalBrightness")),
- ledStrip2GlobalBrightness(hwConfig.getUint("ledRGBStrip2GlobalBrightness"))
+ SlaveConfig(armarx::control::hardware_config::SlaveConfig& hwConfig) :
+ ledStrip1Count(hwConfig.getUint("ledRGBStrip1Count")),
+ ledStrip2Count(hwConfig.getUint("ledRGBStrip2Count")),
+ ledStrip1GlobalBrightness(hwConfig.getUint("ledRGBStrip1GlobalBrightness")),
+ ledStrip2GlobalBrightness(hwConfig.getUint("ledRGBStrip2GlobalBrightness"))
{
}
@@ -23,9 +23,9 @@ namespace devices::ethercat::power_board
struct DeviceConfig
{
- DeviceConfig(armarx::control::hardware_config::DeviceConfig& hwConfig)
- : batteryModulesInParallel(hwConfig.getUint("batteryModulesInParallel")),
- batteryModulesInSerial(hwConfig.getUint("batteryModulesInSerial"))
+ DeviceConfig(armarx::control::hardware_config::DeviceConfig& hwConfig) :
+ batteryModulesInParallel(hwConfig.getUint("batteryModulesInParallel")),
+ batteryModulesInSerial(hwConfig.getUint("batteryModulesInSerial"))
{
}
@@ -35,14 +35,14 @@ namespace devices::ethercat::power_board
struct DataConfig
{
- DataConfig(armarx::control::hardware_config::Config& hwConfig)
- : current(hwConfig.getLinearConfig("current")),
- voltage(hwConfig.getLinearConfig("voltage")),
- batteryCurrent(hwConfig.getLinearConfig("batteryCurrent")),
- batteryVoltage(hwConfig.getLinearConfig("batteryVoltage")),
- batteryTemperature(hwConfig.getLinearConfig("batteryTemperature")),
- batteryCapacity(hwConfig.getLinearConfig("batteryCapacity")),
- ntcBeta(hwConfig.getFloat("ntcBeta"))
+ DataConfig(armarx::control::hardware_config::Config& hwConfig) :
+ current(hwConfig.getLinearConfig("current")),
+ voltage(hwConfig.getLinearConfig("voltage")),
+ batteryCurrent(hwConfig.getLinearConfig("batteryCurrent")),
+ batteryVoltage(hwConfig.getLinearConfig("batteryVoltage")),
+ batteryTemperature(hwConfig.getLinearConfig("batteryTemperature")),
+ batteryCapacity(hwConfig.getLinearConfig("batteryCapacity")),
+ ntcBeta(hwConfig.getFloat("ntcBeta"))
{
}
@@ -54,4 +54,4 @@ namespace devices::ethercat::power_board
armarx::control::hardware_config::types::LinearConfig batteryCapacity;
float ntcBeta;
};
-}
+} // namespace devices::ethercat::power_board
diff --git a/source/devices/ethercat/power_board/ControlTargets.h b/source/devices/ethercat/power_board/ControlTargets.h
index 84607dbc..34e58d8e 100644
--- a/source/devices/ethercat/power_board/ControlTargets.h
+++ b/source/devices/ethercat/power_board/ControlTargets.h
@@ -15,7 +15,6 @@ namespace devices::ethercat::power_board
static const std::string PowerBoardLedColor = "ControlMode_PowerBoardLEDColor";
}
-
class ControlTargetPowerBoardLedColor : public armarx::ControlTargetBase
{
@@ -25,10 +24,12 @@ namespace devices::ethercat::power_board
ControlTargetPowerBoardLedColor() = default;
ControlTargetPowerBoardLedColor(const ControlTargetPowerBoardLedColor&) = default;
ControlTargetPowerBoardLedColor(ControlTargetPowerBoardLedColor&&) = default;
+
ControlTargetPowerBoardLedColor(std::array<simox::Color, 40> colors)
{
this->colors = colors;
}
+
ControlTargetPowerBoardLedColor&
operator=(const ControlTargetPowerBoardLedColor&) = default;
ControlTargetPowerBoardLedColor& operator=(ControlTargetPowerBoardLedColor&&) = default;
diff --git a/source/devices/ethercat/power_board/Data.cpp b/source/devices/ethercat/power_board/Data.cpp
index 88294990..e8412805 100644
--- a/source/devices/ethercat/power_board/Data.cpp
+++ b/source/devices/ethercat/power_board/Data.cpp
@@ -28,9 +28,7 @@
namespace devices::ethercat::power_board
{
- Data::Data(const DataConfig& config,
- SlaveOut* outputs,
- SlaveIn* inputs) :
+ Data::Data(const DataConfig& config, SlaveOut* outputs, SlaveIn* inputs) :
outputs(outputs), inputs(inputs), errorDecoder(outputs, inputs)
{
ARMARX_CHECK_EXPRESSION(outputs);
diff --git a/source/devices/ethercat/power_board/LedControlData.h b/source/devices/ethercat/power_board/LedControlData.h
index ca8c51e1..05848cb4 100644
--- a/source/devices/ethercat/power_board/LedControlData.h
+++ b/source/devices/ethercat/power_board/LedControlData.h
@@ -4,7 +4,6 @@
#include <armarx/control/ethercat/DataInterface.h>
-
namespace devices::ethercat::power_board
{
class LedControlData : public armarx::control::ethercat::DataInterface
@@ -23,8 +22,7 @@ namespace devices::ethercat::power_board
private:
std::uint16_t* target;
std::array<std::uint16_t, 40> ledRgbStripControl;
-
};
- using LedControlDataPtr =
- std::shared_ptr<LedControlData>;
+
+ using LedControlDataPtr = std::shared_ptr<LedControlData>;
} // namespace devices::ethercat::power_board
diff --git a/source/devices/ethercat/power_board/Slave.cpp b/source/devices/ethercat/power_board/Slave.cpp
index fe49880d..b617d239 100644
--- a/source/devices/ethercat/power_board/Slave.cpp
+++ b/source/devices/ethercat/power_board/Slave.cpp
@@ -27,9 +27,7 @@
#include "Slave.h"
#include <ArmarXCore/core/logging/Logging.h>
-
#include <RobotAPI/components/units/RobotUnit/util/RtLogging.h>
-
#include <armarx/control/ethercat/Bus.h>
#include <devices/ethercat/common/h2t_devices.h>
diff --git a/source/devices/ethercat/power_board/Slave.h b/source/devices/ethercat/power_board/Slave.h
index 64bd8ecf..35d62ea0 100644
--- a/source/devices/ethercat/power_board/Slave.h
+++ b/source/devices/ethercat/power_board/Slave.h
@@ -29,9 +29,9 @@
#include <armarx/control/ethercat/SlaveInterface.h>
+#include <devices/ethercat/power_board/Config.h>
#include <devices/ethercat/power_board/ErrorDecoder.h>
#include <devices/ethercat/power_board/SlaveIO.h>
-#include <devices/ethercat/power_board/Config.h>
namespace devices::ethercat::power_board
{
diff --git a/source/devices/ethercat/power_board/SlaveIO.h b/source/devices/ethercat/power_board/SlaveIO.h
index a277c3c6..3a3fef9c 100644
--- a/source/devices/ethercat/power_board/SlaveIO.h
+++ b/source/devices/ethercat/power_board/SlaveIO.h
@@ -29,7 +29,6 @@
#include <cstdint>
-
namespace devices::ethercat::power_board
{
/*
@@ -94,7 +93,6 @@ namespace devices::ethercat::power_board
} __attribute__((__packed__));
-
/**
* @brief PDO mapping master->sensorB
*/
diff --git a/source/devices/ethercat/power_board/joint_controller/LedColor.h b/source/devices/ethercat/power_board/joint_controller/LedColor.h
index 23728767..e9dd5408 100644
--- a/source/devices/ethercat/power_board/joint_controller/LedColor.h
+++ b/source/devices/ethercat/power_board/joint_controller/LedColor.h
@@ -19,5 +19,6 @@ namespace devices::ethercat::power_board::joint_controller
LedControlData* data;
ControlTargetPowerBoardLedColor target;
};
+
using LedColorControllerPtr = std::shared_ptr<LedColorController>;
} // namespace devices::ethercat::power_board::joint_controller
diff --git a/source/devices/ethercat/power_board/joint_controller/LedEmergencyStop.cpp b/source/devices/ethercat/power_board/joint_controller/LedEmergencyStop.cpp
index ea2ff2f2..5507145f 100644
--- a/source/devices/ethercat/power_board/joint_controller/LedEmergencyStop.cpp
+++ b/source/devices/ethercat/power_board/joint_controller/LedEmergencyStop.cpp
@@ -2,7 +2,7 @@
namespace devices::ethercat::power_board::joint_controller
{
- LedEmergencyStopController::LedEmergencyStopController(LedControlData* data): data(data)
+ LedEmergencyStopController::LedEmergencyStopController(LedControlData* data) : data(data)
{
ARMARX_CHECK_NOT_NULL(data);
@@ -10,8 +10,8 @@ namespace devices::ethercat::power_board::joint_controller
targetInternal.colors.fill(simox::Color::red(127));
}
- void LedEmergencyStopController::rtRun(
- const IceUtil::Time&, const IceUtil::Time&)
+ void
+ LedEmergencyStopController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
{
std::array<std::uint16_t, 40> ledRgbStripControl;
for (size_t i = 0; i < ledRgbStripControl.size(); ++i)
@@ -24,12 +24,14 @@ namespace devices::ethercat::power_board::joint_controller
data->setLedRgbStripControl(ledRgbStripControl);
}
- armarx::ControlTargetBase* LedEmergencyStopController::getControlTarget()
+ armarx::ControlTargetBase*
+ LedEmergencyStopController::getControlTarget()
{
return ⌖
}
- void LedEmergencyStopController::rtPreActivateController()
+ void
+ LedEmergencyStopController::rtPreActivateController()
{
}
} // namespace devices::ethercat::power_board::joint_controller
diff --git a/source/devices/ethercat/power_board/joint_controller/LedEmergencyStop.h b/source/devices/ethercat/power_board/joint_controller/LedEmergencyStop.h
index 0c9e6fcf..6d5bac0d 100644
--- a/source/devices/ethercat/power_board/joint_controller/LedEmergencyStop.h
+++ b/source/devices/ethercat/power_board/joint_controller/LedEmergencyStop.h
@@ -11,20 +11,21 @@ namespace devices::ethercat::power_board::joint_controller
{
public:
LedEmergencyStopController(LedControlData* data);
+
private:
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
void rtPreActivateController() override;
+
private:
ControlTargetPowerBoardLedColor targetInternal;
armarx::DummyControlTargetEmergencyStop target;
LedControlData* data;
-
};
-
- using LedEmergencyStopControllerPtr =
- std::shared_ptr<LedEmergencyStopController>;
+
+ using LedEmergencyStopControllerPtr = std::shared_ptr<LedEmergencyStopController>;
} // namespace devices::ethercat::power_board::joint_controller
diff --git a/source/devices/ethercat/power_board/joint_controller/LedStopMovement.cpp b/source/devices/ethercat/power_board/joint_controller/LedStopMovement.cpp
index ebf37c72..15d208ac 100644
--- a/source/devices/ethercat/power_board/joint_controller/LedStopMovement.cpp
+++ b/source/devices/ethercat/power_board/joint_controller/LedStopMovement.cpp
@@ -2,7 +2,7 @@
namespace devices::ethercat::power_board::joint_controller
{
- LedStopMovementController::LedStopMovementController(LedControlData* data): data(data)
+ LedStopMovementController::LedStopMovementController(LedControlData* data) : data(data)
{
ARMARX_CHECK_NOT_NULL(data);
@@ -10,10 +10,10 @@ namespace devices::ethercat::power_board::joint_controller
targetInternal.colors.fill(simox::Color(17, 99, 17));
}
- void LedStopMovementController::rtRun(
- const IceUtil::Time&, const IceUtil::Time&)
+ void
+ LedStopMovementController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
{
- std::array<std::uint16_t, 40> ledRgbStripControl;
+ std::array<std::uint16_t, 40> ledRgbStripControl;
for (size_t i = 0; i < ledRgbStripControl.size(); ++i)
{
ledRgbStripControl[i] = static_cast<std::uint16_t>(
@@ -24,12 +24,14 @@ namespace devices::ethercat::power_board::joint_controller
data->setLedRgbStripControl(ledRgbStripControl);
}
- armarx::ControlTargetBase* LedStopMovementController::getControlTarget()
+ armarx::ControlTargetBase*
+ LedStopMovementController::getControlTarget()
{
return ⌖
}
- void LedStopMovementController::rtPreActivateController()
+ void
+ LedStopMovementController::rtPreActivateController()
{
}
} // namespace devices::ethercat::power_board::joint_controller
diff --git a/source/devices/ethercat/power_board/joint_controller/LedStopMovement.h b/source/devices/ethercat/power_board/joint_controller/LedStopMovement.h
index 8aa5a5b9..8b5bd1c2 100644
--- a/source/devices/ethercat/power_board/joint_controller/LedStopMovement.h
+++ b/source/devices/ethercat/power_board/joint_controller/LedStopMovement.h
@@ -15,11 +15,13 @@ namespace devices::ethercat::power_board::joint_controller
const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
void rtPreActivateController() override;
+
private:
ControlTargetPowerBoardLedColor targetInternal;
LedControlData* data;
armarx::DummyControlTargetStopMovement target;
};
+
using LedStopMovementControllerPtr = std::shared_ptr<LedStopMovementController>;
} // namespace devices::ethercat::power_board::joint_controller
diff --git a/source/devices/ethercat/power_board/led_control/LedColorController.cpp b/source/devices/ethercat/power_board/led_control/LedColorController.cpp
index 99114021..30405ba3 100644
--- a/source/devices/ethercat/power_board/led_control/LedColorController.cpp
+++ b/source/devices/ethercat/power_board/led_control/LedColorController.cpp
@@ -13,7 +13,6 @@
#include <ArmarXCore/core/exceptions/local/FileIOException.h>
#include <ArmarXCore/core/logging/Logging.h>
-
namespace devices::ethercat::power_board
{
std::ostream&
@@ -385,7 +384,6 @@ namespace devices::ethercat::power_board
ARMARX_INFO << ss.str();
}
-
LedColorController::LedMapping
LedColorController::LedMapping::createLedMappingFromIStream(std::istream& is)
{
@@ -431,7 +429,6 @@ namespace devices::ethercat::power_board
}
}
-
void
LedColorController::setColor(LedMapping::Led led, simox::Color color)
{
diff --git a/source/devices/ethercat/power_board/njoint_controller/LedStripsColors.cpp b/source/devices/ethercat/power_board/njoint_controller/LedStripsColors.cpp
index 80e8b0fb..9015fd6b 100644
--- a/source/devices/ethercat/power_board/njoint_controller/LedStripsColors.cpp
+++ b/source/devices/ethercat/power_board/njoint_controller/LedStripsColors.cpp
@@ -1,6 +1,5 @@
#include <devices/ethercat/power_board/njoint_controller/LedStripsColors.h>
-
namespace devices::ethercat::power_board
{
@@ -8,7 +7,6 @@ namespace devices::ethercat::power_board
registrationControllerNJointPowerBoardController(
"NJointPowerBoardLedStripsColorsController");
-
LedStripsColorsController::LedStripsColorsController(armarx::RobotUnit* prov,
LedStripsColorsControllerConfigPtr config,
const VirtualRobot::RobotPtr&)
@@ -36,13 +34,11 @@ namespace devices::ethercat::power_board
}
}
-
void
LedStripsColorsController::rtPreActivateController()
{
}
-
void
LedStripsColorsController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
const IceUtil::Time& /*timeSinceLastIteration*/)
@@ -102,7 +98,6 @@ namespace devices::ethercat::power_board
}
}
-
armarx::WidgetDescription::StringWidgetDictionary
LedStripsColorsController::getFunctionDescriptions(const Ice::Current&) const
{
@@ -176,7 +171,6 @@ namespace devices::ethercat::power_board
return {{"ColorBasedOnIndex", hbox2}, {"ColorGradientBasedOnIndex", hbox3}};
}
-
void
LedStripsColorsController::callDescribedFunction(const std::string& name,
const armarx::StringVariantBaseMap& valueMap,
diff --git a/source/devices/ethercat/power_board/njoint_controller/LedStripsColors.h b/source/devices/ethercat/power_board/njoint_controller/LedStripsColors.h
index 9cc4ea24..f3c2ff14 100644
--- a/source/devices/ethercat/power_board/njoint_controller/LedStripsColors.h
+++ b/source/devices/ethercat/power_board/njoint_controller/LedStripsColors.h
@@ -8,7 +8,6 @@
#include <devices/ethercat/power_board/ControlTargets.h>
#include <devices/ethercat/power_board/njoint_controller/LedStripsColorsInterface.h>
-
namespace devices::ethercat::power_board
{
TYPEDEF_PTRS_HANDLE(LedStripsColorsController);
@@ -20,7 +19,6 @@ namespace devices::ethercat::power_board
std::vector<simox::Color> ledStrip2_colors;
};
-
class LedStripsColorsController :
public armarx::NJointControllerWithTripleBuffer<LedStripsColorsControllerControlData>,
public LedStripsColorsControllerInterface
diff --git a/source/devices/ethercat/power_board/test/LedColorControllerTest.cpp b/source/devices/ethercat/power_board/test/LedColorControllerTest.cpp
index 9cd50478..2ebdfd54 100644
--- a/source/devices/ethercat/power_board/test/LedColorControllerTest.cpp
+++ b/source/devices/ethercat/power_board/test/LedColorControllerTest.cpp
@@ -159,7 +159,6 @@ BOOST_AUTO_TEST_CASE(parsingTest_withPositions_withPrint)
BOOST_CHECK_EQUAL(true, true);
}
-
BOOST_AUTO_TEST_CASE(colorVectorSizesTest)
{
auto is = getTestJson_withAngles();
@@ -285,9 +284,11 @@ BOOST_AUTO_TEST_CASE(getColorAtAngleTest)
BOOST_CHECK_EQUAL(colorController.getColorAtAngle(30.0f), simox::Color::green());
BOOST_CHECK_EQUAL(colorController.getColorAtAngle(71.0f), simox::Color::green());
BOOST_CHECK_EQUAL(colorController.getColorAtAngle(72.0f), simox::Color::green());
- BOOST_CHECK_EQUAL(colorController.getColorAtAngle((144.f + 72.0f) / 2 - 1), simox::Color::green());
+ BOOST_CHECK_EQUAL(colorController.getColorAtAngle((144.f + 72.0f) / 2 - 1),
+ simox::Color::green());
BOOST_CHECK_EQUAL(colorController.getColorAtAngle((144.f + 72.0f) / 2), simox::Color::green());
- BOOST_CHECK_EQUAL(colorController.getColorAtAngle((144.f + 72.0f) / 2 + 1), simox::Color::black());
+ BOOST_CHECK_EQUAL(colorController.getColorAtAngle((144.f + 72.0f) / 2 + 1),
+ simox::Color::black());
colorController.resetColors();
colorController.setColorBasedOnIndex(4, 1, simox::Color::green());
@@ -299,7 +300,8 @@ BOOST_AUTO_TEST_CASE(getColorAtAngleTest)
BOOST_CHECK_EQUAL(colorController.getColorAtAngle(0.0f), simox::Color::green());
BOOST_CHECK_EQUAL(colorController.getColorAtAngle(72.0f), simox::Color::green());
- BOOST_CHECK_EQUAL(colorController.getColorAtAngle(5000.0f), colorController.getColorAtAngle(360.0f));
+ BOOST_CHECK_EQUAL(colorController.getColorAtAngle(5000.0f),
+ colorController.getColorAtAngle(360.0f));
}
BOOST_AUTO_TEST_CASE(colorSettingBasedOnAngleTest)
@@ -319,7 +321,8 @@ BOOST_AUTO_TEST_CASE(colorSettingBasedOnAngleTest)
BOOST_CHECK_EQUAL(colorController.getColorAtAngle(71.0f), simox::Color::green());
BOOST_CHECK_EQUAL(colorController.getColorAtAngle(72.0f), simox::Color::green());
BOOST_CHECK_EQUAL(colorController.getColorAtAngle((144.f + 72.0f) / 2), simox::Color::green());
- BOOST_CHECK_EQUAL(colorController.getColorAtAngle((144.f + 72.0f) / 2 + 1), simox::Color::black());
+ BOOST_CHECK_EQUAL(colorController.getColorAtAngle((144.f + 72.0f) / 2 + 1),
+ simox::Color::black());
colorController.resetColors();
colorController.setColorBasedOnAngle(0.f, 0.f, simox::Color::green());
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/Data.cpp b/source/devices/ethercat/sensor_actor_unit/armar6/Data.cpp
index 1201e00c..161d7d31 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/Data.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/Data.cpp
@@ -37,19 +37,17 @@ namespace devices::ethercat::sensor_actor_unit::armar6
common::elmo::gold::SlaveIn* elmo_in,
common::sensor_board::armar6::SensorBoardConfig sensorBoardConfig,
common::sensor_board::armar6::SlaveOut* sensorOUT) :
- common::elmo::gold::Data(elmoConfig,
- elmo_out,
- elmo_in),
+ common::elmo::gold::Data(elmoConfig, elmo_out, elmo_in),
sensorOUT(sensorOUT),
sensorBoardConfig(sensorBoardConfig)
{
ARMARX_CHECK_EXPRESSION(sensorOUT);
- if(sensorBoardConfig.useModularConvertedValue)
+ if (sensorBoardConfig.useModularConvertedValue)
{
absoluteEncoder.init(&rawABSEncoderTicks, sensorBoardConfig.absoluteEncoder);
}
- else
+ else
{
position.init(&rawABSEncoderTicks, sensorBoardConfig.position);
}
@@ -90,17 +88,18 @@ namespace devices::ethercat::sensor_actor_unit::armar6
// ARMARX_IMPORTANT << "2";
// ARMARX_IMPORTANT << "3";
- if(sensorBoardConfig.useModularConvertedValue)
+ if (sensorBoardConfig.useModularConvertedValue)
{
absoluteEncoder.read();
absoluteEncoderTmpPos = static_cast<float>(absoluteEncoder.value);
- }else
+ }
+ else
{
position.read();
// keep value between center-180 and center+180 deg
position.value = shiftAngleToLimits(position.value);
}
-
+
torque.read();
torqueSensorTemperature.read();
@@ -141,7 +140,7 @@ namespace devices::ethercat::sensor_actor_unit::armar6
const float&
Data::getActualPosition()
{
- if(sensorBoardConfig.useModularConvertedValue)
+ if (sensorBoardConfig.useModularConvertedValue)
{
return absoluteEncoderTmpPos;
}
@@ -227,7 +226,8 @@ namespace devices::ethercat::sensor_actor_unit::armar6
return sensorOUT->I2CUpdateRate;
}
- void Data::setFirmwareVersionNewerThan2015(bool isFirmwareVersionNewerThan2015)
+ void
+ Data::setFirmwareVersionNewerThan2015(bool isFirmwareVersionNewerThan2015)
{
_isFirmwareVersionNewerThan2015 = isFirmwareVersionNewerThan2015;
}
@@ -262,7 +262,6 @@ namespace devices::ethercat::sensor_actor_unit::armar6
return torque.getRaw();
}
-
std::uint32_t
Data::getAbsoluteEncoderTicks()
{
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/Data.h b/source/devices/ethercat/sensor_actor_unit/armar6/Data.h
index b02f3de0..38857f2e 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/Data.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/Data.h
@@ -40,13 +40,11 @@
#include <devices/ethercat/common/elmo/gold/Data.h>
#include <devices/ethercat/common/sensor_board/armar6/Config.h>
-
namespace devices::ethercat::common::sensor_board::armar6
{
class SlaveOut;
}
-
namespace devices::ethercat::sensor_actor_unit::armar6
{
@@ -68,7 +66,8 @@ namespace devices::ethercat::sensor_actor_unit::armar6
*this = o;
}
- SensorValueArmar6Actuator& operator=(SensorValueArmar6Actuator&& o)
+ SensorValueArmar6Actuator&
+ operator=(SensorValueArmar6Actuator&& o)
{
*this = o;
return *this;
@@ -76,7 +75,7 @@ namespace devices::ethercat::sensor_actor_unit::armar6
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- float gearTemperature = 0.0f;
+ float gearTemperature = 0.0f;
float dieTemperature = 0.0f;
float relativePosition = 0.0f;
float torqueSensorTemperature = 0.0f;
@@ -91,7 +90,8 @@ namespace devices::ethercat::sensor_actor_unit::armar6
std::uint32_t absoluteEncoderTicks = 0;
std::int32_t torqueTicks = 0;
- static SensorValueInfo<SensorValueArmar6Actuator> GetClassMemberInfo()
+ static SensorValueInfo<SensorValueArmar6Actuator>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValueArmar6Actuator> svi;
svi.addBaseClass<SensorValue1DoFRealActuatorWithStatus>();
@@ -100,40 +100,41 @@ namespace devices::ethercat::sensor_actor_unit::armar6
svi.addMemberVariable(&SensorValueArmar6Actuator::gearTemperature, "gearTemperature");
svi.addMemberVariable(&SensorValueArmar6Actuator::dieTemperature, "dieTemperature");
svi.addMemberVariable(&SensorValueArmar6Actuator::relativePosition, "relativePosition");
- svi.addMemberVariable(&SensorValueArmar6Actuator::torqueSensorTemperature, "torqueSensorTemperature");
+ svi.addMemberVariable(&SensorValueArmar6Actuator::torqueSensorTemperature,
+ "torqueSensorTemperature");
svi.addMemberVariable(&SensorValueArmar6Actuator::I2CUpdateRate, "I2CUpdateRate");
- svi.addMemberVariable(&SensorValueArmar6Actuator::sensorBoardUpdateRate, "sensorBoardUpdateRate");
+ svi.addMemberVariable(&SensorValueArmar6Actuator::sensorBoardUpdateRate,
+ "sensorBoardUpdateRate");
svi.addMemberVariable(&SensorValueArmar6Actuator::elmoTemperature, "elmoTemperature");
svi.addMemberVariable(&SensorValueArmar6Actuator::currentTarget, "currentTarget");
svi.addMemberVariable(&SensorValueArmar6Actuator::positionTarget, "positionTarget");
svi.addMemberVariable(&SensorValueArmar6Actuator::velocityTarget, "velocityTarget");
svi.addMemberVariable(&SensorValueArmar6Actuator::maxTargetCurrent, "maxTargetCurrent");
- svi.addMemberVariable(&SensorValueArmar6Actuator::absoluteEncoderTicks, "absoluteEncoderTicks");
+ svi.addMemberVariable(&SensorValueArmar6Actuator::absoluteEncoderTicks,
+ "absoluteEncoderTicks");
svi.addMemberVariable(&SensorValueArmar6Actuator::torqueTicks, "torqueTicks");
- svi.addMemberVariable(&SensorValueArmar6Actuator::gravityCompensatedTorque, "gravityCompensatedTorque");
+ svi.addMemberVariable(&SensorValueArmar6Actuator::gravityCompensatedTorque,
+ "gravityCompensatedTorque");
return svi;
}
-
};
class Data;
using DataPtr = std::shared_ptr<Data>;
-
- class Data :
- public common::elmo::gold::Data
+ class Data : public common::elmo::gold::Data
{
public:
-
Data(common::elmo::gold::ElmoConfig elmoConfig,
common::elmo::gold::SlaveOut* elmo_out,
common::elmo::gold::SlaveIn* elmo_in,
common::sensor_board::armar6::SensorBoardConfig sensorBoardConfig,
common::sensor_board::armar6::SlaveOut* sensorOUT);
- void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
const float& getActualPosition();
@@ -169,14 +170,14 @@ namespace devices::ethercat::sensor_actor_unit::armar6
std::uint32_t getAbsoluteEncoderTicks();
private:
-
void convertRawAbsEncoderValue();
common::sensor_board::armar6::SlaveOut* sensorOUT;
std::uint32_t rawABSEncoderTicks;
armarx::control::ethercat::LinearConvertedValue<std::uint32_t> position;
- armarx::control::ethercat::ModularConvertedValue<std::uint32_t, std::int64_t> absoluteEncoder;
+ armarx::control::ethercat::ModularConvertedValue<std::uint32_t, std::int64_t>
+ absoluteEncoder;
armarx::control::ethercat::LinearConvertedValue<std::int32_t> torque;
armarx::control::ethercat::LinearConvertedValue<std::int32_t> torqueSensorTemperature;
@@ -209,7 +210,6 @@ namespace devices::ethercat::sensor_actor_unit::armar6
common::sensor_board::armar6::SensorBoardConfig sensorBoardConfig;
float absoluteEncoderTmpPos;
-
};
-} // namespace devices::ethercat::sensor_actor_unit::v1
+} // namespace devices::ethercat::sensor_actor_unit::armar6
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/Device.cpp b/source/devices/ethercat/sensor_actor_unit/armar6/Device.cpp
index b1772cb1..de6c5fec 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/Device.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/Device.cpp
@@ -45,12 +45,10 @@
#include <devices/ethercat/common/h2t_devices.h>
#include <devices/ethercat/common/sensor_board/armar6/Slave.h>
-
namespace devices::ethercat::sensor_actor_unit::armar6
{
- Device::Device(hwconfig::DeviceConfig& config,
- const VirtualRobot::RobotPtr& robot) :
+ Device::Device(hwconfig::DeviceConfig& config, const VirtualRobot::RobotPtr& robot) :
DeviceBase(config.getName()),
ControlDevice(config.getName()),
SensorDevice(config.getName()),
@@ -68,12 +66,11 @@ namespace devices::ethercat::sensor_actor_unit::armar6
ARMARX_CHECK_NOT_NULL(robotNode);
- torqueControllerConfigDataPtr =
- TorqueControllerConfiguration::CreateTorqueConfigData(
- config.getControllerConfig("Torque"),
- this->robotNode->isLimitless(),
- this->robotNode->getJointLimitLow(),
- this->robotNode->getJointLimitHigh());
+ torqueControllerConfigDataPtr = TorqueControllerConfiguration::CreateTorqueConfigData(
+ config.getControllerConfig("Torque"),
+ this->robotNode->isLimitless(),
+ this->robotNode->getJointLimitLow(),
+ this->robotNode->getJointLimitHigh());
zeroTorqueControllerConfigDataPtr =
VelocityManipulatingTorqueControllerConfiguration::CreateTorqueConfigData(
config.getControllerConfig("ZeroTorque"),
@@ -100,7 +97,6 @@ namespace devices::ethercat::sensor_actor_unit::armar6
ARMARX_VERBOSE << "Joint " << getDeviceName() << " enabled: " << enabled;
}
-
armarx::control::ethercat::DeviceInterface::TryAssignResult
Device::tryAssign(armarx::control::ethercat::SlaveInterface& slave)
{
@@ -123,7 +119,6 @@ namespace devices::ethercat::sensor_actor_unit::armar6
return result;
}
-
armarx::control::ethercat::DeviceInterface::AllAssignedResult
Device::onAllAssigned()
{
@@ -147,14 +142,14 @@ namespace devices::ethercat::sensor_actor_unit::armar6
// Initialize sensor board.
{
- sensorBoardPtr->setCheckAbsEncoderReading(sensorBoardConfig.checkAbsEncoderReadings);
- sensorBoardPtr->setUseTMP007(
- sensorBoardConfig.useTmp007);
- sensorBoardPtr->setUseIMU(
- sensorBoardConfig.useIMU);
-
- sensorBoardPtr->setIMUCalibrationData(std::vector<std::uint16_t>(sensorBoardConfig.imuCalibrationData.begin(), sensorBoardConfig.imuCalibrationData.end()));
-
+ sensorBoardPtr->setCheckAbsEncoderReading(
+ sensorBoardConfig.checkAbsEncoderReadings);
+ sensorBoardPtr->setUseTMP007(sensorBoardConfig.useTmp007);
+ sensorBoardPtr->setUseIMU(sensorBoardConfig.useIMU);
+
+ sensorBoardPtr->setIMUCalibrationData(
+ std::vector<std::uint16_t>(sensorBoardConfig.imuCalibrationData.begin(),
+ sensorBoardConfig.imuCalibrationData.end()));
}
return Result::ok;
@@ -238,7 +233,6 @@ namespace devices::ethercat::sensor_actor_unit::armar6
{
}
-
void
Device::updateSensorValueStruct()
{
@@ -271,7 +265,6 @@ namespace devices::ethercat::sensor_actor_unit::armar6
sensorValue.absoluteEncoderTicks = dataPtr->getAbsoluteEncoderTicks();
}
-
DataPtr
Device::getData() const
{
@@ -284,28 +277,24 @@ namespace devices::ethercat::sensor_actor_unit::armar6
return dataPtr;
}
-
const std::string&
Device::getJointName() const
{
return getDeviceName();
}
-
armarx::control::ethercat::SlaveIdentifier
Device::getElmoIdentifier() const
{
return elmoIdentifier;
}
-
armarx::control::ethercat::SlaveIdentifier
Device::getSensorBoardIdentifier() const
{
return sensorBoardIdentifier;
}
-
bool
Device::switchControlMode(armarx::ControlMode newMode)
{
@@ -338,7 +327,6 @@ namespace devices::ethercat::sensor_actor_unit::armar6
return elmoPtr->switchMode(modeToSet);
}
-
void
Device::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -388,7 +376,6 @@ namespace devices::ethercat::sensor_actor_unit::armar6
updateSensorValueStruct();
}
-
void
Device::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -401,14 +388,12 @@ namespace devices::ethercat::sensor_actor_unit::armar6
dataPtr->rtWriteTargetValues(sensorValuesTimestamp, timeSinceLastIteration);
}
-
const armarx::SensorValueBase*
Device::getSensorValue() const
{
return &sensorValue;
}
-
std::uint16_t
Device::getElmoSlaveNumber() const
{
@@ -420,7 +405,6 @@ namespace devices::ethercat::sensor_actor_unit::armar6
return elmoPtr->getSlaveNumber();
}
-
std::uint16_t
Device::getSensorBoardSlaveNumber() const
{
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/Device.h b/source/devices/ethercat/sensor_actor_unit/armar6/Device.h
index 95eebb0e..8c8053e7 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/Device.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/Device.h
@@ -31,16 +31,13 @@
#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
#include <armarx/control/ethercat/DeviceInterface.h>
#include <armarx/control/ethercat/SlaveIdentifier.h>
-
-#include <devices/ethercat/common/sensor_board/armar6/Slave.h>
-#include <devices/ethercat/common/sensor_board/armar6/Config.h>
-#include <devices/ethercat/common/elmo/gold/Config.h>
-#include <devices/ethercat/sensor_actor_unit/armar6/Data.h>
-
#include <armarx/control/hardware_config/DeviceConfig.h>
#include "joint_controller/ZeroVelocity.h"
-
+#include <devices/ethercat/common/elmo/gold/Config.h>
+#include <devices/ethercat/common/sensor_board/armar6/Config.h>
+#include <devices/ethercat/common/sensor_board/armar6/Slave.h>
+#include <devices/ethercat/sensor_actor_unit/armar6/Data.h>
namespace armarx::control::joint_controller
{
@@ -51,7 +48,6 @@ namespace armarx::control::joint_controller
std::shared_ptr<class PositionControllerConfiguration>;
} // namespace armarx::control::joint_controller
-
namespace devices::ethercat::sensor_actor_unit::armar6
{
@@ -72,7 +68,6 @@ namespace devices::ethercat::sensor_actor_unit::armar6
namespace hwconfig = armarx::control::hardware_config;
-
class Device :
public armarx::ControlDevice,
public armarx::SensorDevice,
@@ -80,8 +75,8 @@ namespace devices::ethercat::sensor_actor_unit::armar6
{
public:
- Device(hwconfig::DeviceConfig& config,
- VirtualRobot::RobotPtr const& robot);
+ Device(hwconfig::DeviceConfig& config, VirtualRobot::RobotPtr const& robot);
+
~Device() override
{
}
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ActiveImpedance.cpp b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ActiveImpedance.cpp
index 67c90b67..8ae4fbb4 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ActiveImpedance.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ActiveImpedance.cpp
@@ -1,19 +1,15 @@
#include "ActiveImpedance.h"
-
#include <devices/ethercat/sensor_actor_unit/armar6/Data.h>
#include <devices/ethercat/sensor_actor_unit/armar6/Device.h>
-
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
- ActiveImpedanceController::ActiveImpedanceController(Device* joint,
- DataPtr jointData,
- armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData) : JointController(),
- target(),
- joint(joint),
- pid(),
- targetFilter(100)
+ ActiveImpedanceController::ActiveImpedanceController(
+ Device* joint,
+ DataPtr jointData,
+ armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData) :
+ JointController(), target(), joint(joint), pid(), targetFilter(100)
{
dataPtr = jointData;
@@ -32,32 +28,35 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
// coefficients for Lowpass FIR-Filter
filter.setImpulseResponse(
- {
- 0.0073f, 0.0011f, 0.0012f, 0.0013f, 0.0014f, 0.0015f, 0.0016f, 0.0017f, 0.0018f, 0.0019f, 0.0021f, 0.0022f, 0.0023f,
- 0.0024f, 0.0026f, 0.0027f, 0.0028f, 0.0029f, 0.0031f, 0.0032f, 0.0034f, 0.0035f, 0.0036f, 0.0038f, 0.0039f, 0.0041f,
- 0.0043f, 0.0044f, 0.0046f, 0.0047f, 0.0049f, 0.0050f, 0.0052f, 0.0054f, 0.0055f, 0.0057f, 0.0058f, 0.0060f, 0.0062f,
- 0.0063f, 0.0065f, 0.0066f, 0.0068f, 0.0070f, 0.0071f, 0.0073f, 0.0074f, 0.0076f, 0.0077f, 0.0078f, 0.0080f, 0.0081f,
- 0.0083f, 0.0084f, 0.0085f, 0.0087f, 0.0088f, 0.0089f, 0.0090f, 0.0091f, 0.0092f, 0.0093f, 0.0094f, 0.0095f, 0.0096f,
- 0.0097f, 0.0098f, 0.0098f, 0.0099f, 0.0100f, 0.0100f, 0.0101f, 0.0101f, 0.0102f, 0.0102f, 0.0102f, 0.0103f, 0.0103f,
- 0.0103f, 0.0103f, 0.0103f, 0.0103f, 0.0103f, 0.0103f, 0.0102f, 0.0102f, 0.0102f, 0.0101f, 0.0101f, 0.0100f, 0.0100f,
- 0.0099f, 0.0098f, 0.0098f, 0.0097f, 0.0096f, 0.0095f, 0.0094f, 0.0093f, 0.0092f, 0.0091f, 0.0090f, 0.0089f, 0.0088f,
- 0.0087f, 0.0085f, 0.0084f, 0.0083f, 0.0081f, 0.0080f, 0.0078f, 0.0077f, 0.0076f, 0.0074f, 0.0073f, 0.0071f, 0.0070f,
- 0.0068f, 0.0066f, 0.0065f, 0.0063f, 0.0062f, 0.0060f, 0.0058f, 0.0057f, 0.0055f, 0.0054f, 0.0052f, 0.0050f, 0.0049f,
- 0.0047f, 0.0046f, 0.0044f, 0.0043f, 0.0041f, 0.0039f, 0.0038f, 0.0036f, 0.0035f, 0.0034f, 0.0032f, 0.0031f, 0.0029f,
- 0.0028f, 0.0027f, 0.0026f, 0.0024f, 0.0023f, 0.0022f, 0.0021f, 0.0019f, 0.0018f, 0.0017f, 0.0016f, 0.0015f, 0.0014f,
- 0.0013f, 0.0012f, 0.0011f, 0.0073f
- });
-
- filterVel.setImpulseResponse(
- {
- 0.1744f, 0.0205f, 0.0215f, 0.0225f, 0.0235f, 0.0244f, 0.0252f, 0.0259f, 0.0265f, 0.0270f, 0.0274f, 0.0276f, 0.0278f,
- 0.0282f, 0.0282f, 0.0278f, 0.0276f, 0.0274f, 0.0270f, 0.0265f, 0.0259f, 0.0252f, 0.0244f, 0.0235f, 0.0225f, 0.0215f,
- 0.0205f, 0.1744f
- });
-
+ {0.0073f, 0.0011f, 0.0012f, 0.0013f, 0.0014f, 0.0015f, 0.0016f, 0.0017f, 0.0018f,
+ 0.0019f, 0.0021f, 0.0022f, 0.0023f, 0.0024f, 0.0026f, 0.0027f, 0.0028f, 0.0029f,
+ 0.0031f, 0.0032f, 0.0034f, 0.0035f, 0.0036f, 0.0038f, 0.0039f, 0.0041f, 0.0043f,
+ 0.0044f, 0.0046f, 0.0047f, 0.0049f, 0.0050f, 0.0052f, 0.0054f, 0.0055f, 0.0057f,
+ 0.0058f, 0.0060f, 0.0062f, 0.0063f, 0.0065f, 0.0066f, 0.0068f, 0.0070f, 0.0071f,
+ 0.0073f, 0.0074f, 0.0076f, 0.0077f, 0.0078f, 0.0080f, 0.0081f, 0.0083f, 0.0084f,
+ 0.0085f, 0.0087f, 0.0088f, 0.0089f, 0.0090f, 0.0091f, 0.0092f, 0.0093f, 0.0094f,
+ 0.0095f, 0.0096f, 0.0097f, 0.0098f, 0.0098f, 0.0099f, 0.0100f, 0.0100f, 0.0101f,
+ 0.0101f, 0.0102f, 0.0102f, 0.0102f, 0.0103f, 0.0103f, 0.0103f, 0.0103f, 0.0103f,
+ 0.0103f, 0.0103f, 0.0103f, 0.0102f, 0.0102f, 0.0102f, 0.0101f, 0.0101f, 0.0100f,
+ 0.0100f, 0.0099f, 0.0098f, 0.0098f, 0.0097f, 0.0096f, 0.0095f, 0.0094f, 0.0093f,
+ 0.0092f, 0.0091f, 0.0090f, 0.0089f, 0.0088f, 0.0087f, 0.0085f, 0.0084f, 0.0083f,
+ 0.0081f, 0.0080f, 0.0078f, 0.0077f, 0.0076f, 0.0074f, 0.0073f, 0.0071f, 0.0070f,
+ 0.0068f, 0.0066f, 0.0065f, 0.0063f, 0.0062f, 0.0060f, 0.0058f, 0.0057f, 0.0055f,
+ 0.0054f, 0.0052f, 0.0050f, 0.0049f, 0.0047f, 0.0046f, 0.0044f, 0.0043f, 0.0041f,
+ 0.0039f, 0.0038f, 0.0036f, 0.0035f, 0.0034f, 0.0032f, 0.0031f, 0.0029f, 0.0028f,
+ 0.0027f, 0.0026f, 0.0024f, 0.0023f, 0.0022f, 0.0021f, 0.0019f, 0.0018f, 0.0017f,
+ 0.0016f, 0.0015f, 0.0014f, 0.0013f, 0.0012f, 0.0011f, 0.0073f});
+
+ filterVel.setImpulseResponse({0.1744f, 0.0205f, 0.0215f, 0.0225f, 0.0235f, 0.0244f,
+ 0.0252f, 0.0259f, 0.0265f, 0.0270f, 0.0274f, 0.0276f,
+ 0.0278f, 0.0282f, 0.0282f, 0.0278f, 0.0276f, 0.0274f,
+ 0.0270f, 0.0265f, 0.0259f, 0.0252f, 0.0244f, 0.0235f,
+ 0.0225f, 0.0215f, 0.0205f, 0.1744f});
}
- void ActiveImpedanceController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ ActiveImpedanceController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
float setPosition = target.position;
float setSpringConst = target.kp;
@@ -136,7 +135,8 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
desVelocity = 0.1;
}
- float Kd = (-gravity + desTorque + actualTorqueF) / std::max(0.0001f, std::abs(desVelocity));
+ float Kd =
+ (-gravity + desTorque + actualTorqueF) / std::max(0.0001f, std::abs(desVelocity));
ARMARX_RT_LOGF_INFO("Kd: %.2f", Kd).deactivateSpam(0.15);
ARMARX_RT_LOGF_INFO("actualTorqueF: %.2f", actualTorqueF).deactivateSpam(0.15);
@@ -286,42 +286,46 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
dataPtr->setTargetCurrent(current);
}
-
- armarx::ControlTargetBase* ActiveImpedanceController::getControlTarget()
+ armarx::ControlTargetBase*
+ ActiveImpedanceController::getControlTarget()
{
return ⌖
}
-
- armarx::control::rt_filters::FirFilter ActiveImpedanceController::getFilter() const
+ armarx::control::rt_filters::FirFilter
+ ActiveImpedanceController::getFilter() const
{
return filter;
}
-
- void ActiveImpedanceController::rtPreActivateController()
+ void
+ ActiveImpedanceController::rtPreActivateController()
{
joint->switchControlMode(armarx::eTorqueControl);
dataPtr->setTargetCurrent(0.0f);
}
-
- ActiveImpedanceControllerConfigurationPtr ActiveImpedanceControllerConfiguration::CreateTorqueConfigDataFromXml(armarx::DefaultRapidXmlReaderNode node)
+ ActiveImpedanceControllerConfigurationPtr
+ ActiveImpedanceControllerConfiguration::CreateTorqueConfigDataFromXml(
+ armarx::DefaultRapidXmlReaderNode node)
{
ActiveImpedanceControllerConfiguration configData;
ARMARX_CHECK_EXPRESSION(node.is_valid());
- configData.pid_proportional_gain = (double) node.first_node("pid_proportional_gain").value_as_float();
- configData.pid_integral_gain = (double) node.first_node("pid_integral_gain").value_as_float();
- configData.pid_derivative_gain = (double) node.first_node("pid_derivative_gain").value_as_float();
- configData.pid_windup_guard = (double) node.first_node("pid_windup_guard").value_as_float();
- configData.pid_max_value = (double) node.first_node("pid_max_value").value_as_float();
- configData.pid_min_value = (double) node.first_node("pid_min_value").value_as_float();
- configData.pid_dis = (double) node.first_node("pid_dis").value_as_float();
- configData.Kd = (double) node.first_node("Kd").value_as_float();
- configData.inertia = (double) node.first_node("inertia").value_as_float();
+ configData.pid_proportional_gain =
+ (double)node.first_node("pid_proportional_gain").value_as_float();
+ configData.pid_integral_gain =
+ (double)node.first_node("pid_integral_gain").value_as_float();
+ configData.pid_derivative_gain =
+ (double)node.first_node("pid_derivative_gain").value_as_float();
+ configData.pid_windup_guard = (double)node.first_node("pid_windup_guard").value_as_float();
+ configData.pid_max_value = (double)node.first_node("pid_max_value").value_as_float();
+ configData.pid_min_value = (double)node.first_node("pid_min_value").value_as_float();
+ configData.pid_dis = (double)node.first_node("pid_dis").value_as_float();
+ configData.Kd = (double)node.first_node("Kd").value_as_float();
+ configData.inertia = (double)node.first_node("inertia").value_as_float();
auto v = node.first_node("firFilterImpulseResponse").value_as_string();
- for (auto& elem : armarx::Split(v, ",", true))
+ for (auto& elem : armarx::Split(v, ",", true))
{
configData.firFilterImpulseResponse.push_back(armarx::toFloat(elem));
}
@@ -329,9 +333,8 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
return std::make_shared<ActiveImpedanceControllerConfiguration>(configData);
}
-
ActiveImpedanceControllerConfiguration::ActiveImpedanceControllerConfiguration()
{
}
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ActiveImpedance.h b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ActiveImpedance.h
index c164a473..c83bc4da 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ActiveImpedance.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ActiveImpedance.h
@@ -3,33 +3,31 @@
// armarx
#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <armarx/control/joint_controller/Torque.h>
#include <armarx/control/rt_filters/AverageFilter.h>
#include <armarx/control/rt_filters/FirFilter.h>
-#include <armarx/control/joint_controller/Torque.h>
-
namespace devices::ethercat::sensor_actor_unit::armar6
{
class Device;
using DataPtr = std::shared_ptr<class Data>;
-}
-
+} // namespace devices::ethercat::sensor_actor_unit::armar6
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
- using ActiveImpedanceControllerConfigurationPtr = std::shared_ptr<class ActiveImpedanceControllerConfiguration>;
-
+ using ActiveImpedanceControllerConfigurationPtr =
+ std::shared_ptr<class ActiveImpedanceControllerConfiguration>;
class ActiveImpedanceControllerConfiguration
{
public:
-
ActiveImpedanceControllerConfiguration();
- static ActiveImpedanceControllerConfigurationPtr CreateTorqueConfigDataFromXml(armarx::DefaultRapidXmlReaderNode node);
+ static ActiveImpedanceControllerConfigurationPtr
+ CreateTorqueConfigDataFromXml(armarx::DefaultRapidXmlReaderNode node);
double pid_proportional_gain;
double pid_integral_gain;
double pid_derivative_gain;
@@ -40,29 +38,27 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
double Kd;
double inertia;
std::vector<float> firFilterImpulseResponse;
-
};
using ActiveImpedanceControllerPtr = std::shared_ptr<class ActiveImpedanceController>;
-
class ActiveImpedanceController : public armarx::JointController
{
public:
+ ActiveImpedanceController(
+ Device* joint,
+ DataPtr jointData,
+ armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData);
- ActiveImpedanceController(Device* joint,
- DataPtr jointData,
- armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData);
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
armarx::control::rt_filters::FirFilter getFilter() const;
private:
-
armarx::ActiveImpedanceControlTarget target;
DataPtr dataPtr;
Device* joint;
@@ -75,9 +71,7 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
// JointController interface
protected:
-
void rtPreActivateController() override;
-
};
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Current.cpp b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Current.cpp
index 96f039a7..b7a0fad8 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Current.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Current.cpp
@@ -1,22 +1,20 @@
#include "Current.h"
-
#include <devices/ethercat/sensor_actor_unit/armar6/Data.h>
#include <devices/ethercat/sensor_actor_unit/armar6/Device.h>
-
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
- CurrentController::CurrentController(Device* joint, DataPtr jointData) : JointController(),
- target(),
- joint(joint)
+ CurrentController::CurrentController(Device* joint, DataPtr jointData) :
+ JointController(), target(), joint(joint)
{
dataPtr = jointData;
}
-
- void CurrentController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ CurrentController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (target.isValid())
{
@@ -24,17 +22,17 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
}
}
-
- armarx::ControlTargetBase* CurrentController::getControlTarget()
+ armarx::ControlTargetBase*
+ CurrentController::getControlTarget()
{
return ⌖
}
-
- void CurrentController::rtPreActivateController()
+ void
+ CurrentController::rtPreActivateController()
{
joint->switchControlMode(armarx::eTorqueControl);
dataPtr->setTargetCurrent(0.0f);
}
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Current.h b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Current.h
index 93de48dc..08e25a45 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Current.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Current.h
@@ -2,41 +2,35 @@
// armarx
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
namespace devices::ethercat::sensor_actor_unit::armar6
{
class Device;
using DataPtr = std::shared_ptr<class Data>;
-}
-
+} // namespace devices::ethercat::sensor_actor_unit::armar6
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
using JointCurrentControllerPtr = std::shared_ptr<class CurrentController>;
-
class CurrentController : public armarx::JointController
{
public:
-
CurrentController(Device* joint, DataPtr jointData);
protected:
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
void rtPreActivateController() override;
armarx::ControlTargetBase* getControlTarget() override;
private:
-
armarx::ControlTarget1DoFActuatorCurrent target;
DataPtr dataPtr;
Device* joint;
-
};
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/CurrentPassThrough.cpp b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/CurrentPassThrough.cpp
index bf266927..d9917b8a 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/CurrentPassThrough.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/CurrentPassThrough.cpp
@@ -5,42 +5,42 @@
#include <devices/ethercat/sensor_actor_unit/armar6/Data.h>
#include <devices/ethercat/sensor_actor_unit/armar6/Device.h>
-
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
- armarx::ControlTargetBase* CurrentPassThroughController::getControlTarget()
+ armarx::ControlTargetBase*
+ CurrentPassThroughController::getControlTarget()
{
return ⌖
}
-
CurrentPassThroughController::CurrentPassThroughController(Device* joint, DataPtr jointData) :
- dataPtr(jointData),
- joint(joint)
+ dataPtr(jointData), joint(joint)
{
-
}
-
- void CurrentPassThroughController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ CurrentPassThroughController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (target.isValid())
{
if (target.current > 100)
{
- throw armarx::LocalException() << "Got huge value for target current: " << target.current << " Expecting Ampere not Milliampere!";
+ throw armarx::LocalException()
+ << "Got huge value for target current: " << target.current
+ << " Expecting Ampere not Milliampere!";
}
dataPtr->setTargetCurrent(target.current * 1000);
}
}
-
- void CurrentPassThroughController::rtPreActivateController()
+ void
+ CurrentPassThroughController::rtPreActivateController()
{
joint->switchControlMode(armarx::eTorqueControl);
dataPtr->setTargetCurrent(0.0f);
}
-} // namespace devices::ethercat::sensor_actor_unit::v1::joint_controller
+} // namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/CurrentPassThrough.h b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/CurrentPassThrough.h
index 200ad9e7..dcaee877 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/CurrentPassThrough.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/CurrentPassThrough.h
@@ -2,42 +2,36 @@
// armarx
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
namespace devices::ethercat::sensor_actor_unit::armar6
{
class Device;
using DataPtr = std::shared_ptr<class Data>;
-}
-
+} // namespace devices::ethercat::sensor_actor_unit::armar6
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
using CurrentPassThroughControllerPtr = std::shared_ptr<class CurrentPassThroughController>;
-
class CurrentPassThroughController : public armarx::JointController
{
public:
-
CurrentPassThroughController(Device* joint, DataPtr jointData);
protected:
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
void rtPreActivateController() override;
armarx::ControlTargetBase* getControlTarget() override;
private:
-
armarx::ControlTarget1DoFActuatorCurrent target;
DataPtr dataPtr;
Device* joint;
-
};
-} // namespace devices::ethercat::sensor_actor_unit::v1::joint_controller
+} // namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ElmoEmergency.h b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ElmoEmergency.h
index 50eb63d1..890fed22 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ElmoEmergency.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ElmoEmergency.h
@@ -7,7 +7,6 @@
#include <devices/ethercat/common/elmo/gold/Slave.h>
-
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
template <typename DataTypePtr>
@@ -54,6 +53,7 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
}*/
}
+
/**
* Returns the Target for this controller, but as this is the Emergency controller it will ignored.
* As this controller will just break
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Position.cpp b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Position.cpp
index 04e004ea..60a357f1 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Position.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Position.cpp
@@ -7,15 +7,14 @@
#include <devices/ethercat/sensor_actor_unit/armar6/Data.h>
#include <devices/ethercat/sensor_actor_unit/armar6/Device.h>
-
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
- PositionController::PositionController(Device* joint,
- DataPtr jointPtr,
- armarx::control::joint_controller::PositionControllerConfigurationPtr config) : JointController(),
- target(),
- joint(joint)
+ PositionController::PositionController(
+ Device* joint,
+ DataPtr jointPtr,
+ armarx::control::joint_controller::PositionControllerConfigurationPtr config) :
+ JointController(), target(), joint(joint)
{
dataPtr = jointPtr;
@@ -46,8 +45,9 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
limitlessJoint = dataPtr->isLimitLess();
}
-
- void PositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ PositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (target.isValid())
{
@@ -78,9 +78,12 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
else
{
float currentPosition = dataPtr->getActualPosition();
- float targetPosition = std::clamp(target.position,
- std::min(currentPosition, positionController.positionLimitLo), // lo or current position
- std::max(currentPosition, positionController.positionLimitHi)); // hi or current position
+ float targetPosition = std::clamp(
+ target.position,
+ std::min(currentPosition,
+ positionController.positionLimitLo), // lo or current position
+ std::max(currentPosition,
+ positionController.positionLimitHi)); // hi or current position
positionController.currentV = currentVelocity;
positionController.dt = timeSinceLastIteration.toSecondsDouble();
positionController.currentPosition = currentPosition;
@@ -99,14 +102,14 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
}
}
-
- armarx::ControlTargetBase* PositionController::getControlTarget()
+ armarx::ControlTargetBase*
+ PositionController::getControlTarget()
{
return ⌖
}
-
- void PositionController::rtPreActivateController()
+ void
+ PositionController::rtPreActivateController()
{
currentVelocity = dataPtr->getActualVelocity();
joint->switchControlMode(armarx::eVelocityControl);
@@ -116,4 +119,4 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
// ARMARX_IMPORTANT << "Activating position controller in " << (dataPtr->isLimitLess() ? "limitless mode" : "in mode with position bounds");
}
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Position.h b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Position.h
index 7bcd86aa..dae5e160 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Position.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Position.h
@@ -2,40 +2,41 @@
// armarx
-#include <armarx/control/joint_controller/ControllerConfiguration.h>
+#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-
+#include <armarx/control/joint_controller/ControllerConfiguration.h>
namespace devices::ethercat::sensor_actor_unit::armar6
{
class Device;
using DataPtr = std::shared_ptr<class Data>;
-}
-
+} // namespace devices::ethercat::sensor_actor_unit::armar6
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
using PositionControllerPtr = std::shared_ptr<class PositionController>;
-
class PositionController : public armarx::JointController
{
public:
+ PositionController(
+ Device* joint,
+ DataPtr jointPtr,
+ armarx::control::joint_controller::PositionControllerConfigurationPtr config);
- PositionController(Device* joint, DataPtr jointPtr, armarx::control::joint_controller::PositionControllerConfigurationPtr config);
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
void rtPreActivateController() override;
private:
-
armarx::ControlTarget1DoFActuatorPosition target;
- armarx::PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition limitlessController;
- armarx::PositionThroughVelocityControllerWithAccelerationAndPositionBounds positionController;
+ armarx::PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition
+ limitlessController;
+ armarx::PositionThroughVelocityControllerWithAccelerationAndPositionBounds
+ positionController;
DataPtr dataPtr;
Device* joint;
float maxVelocityRad;
@@ -44,6 +45,5 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
float p;
float currentVelocity;
bool limitlessJoint;
-
};
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/StopMovement.h b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/StopMovement.h
index 41e518fd..9d2eb67e 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/StopMovement.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/StopMovement.h
@@ -8,7 +8,6 @@
#include <devices/ethercat/common/elmo/gold/Slave.h>
-
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
@@ -30,6 +29,7 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
dataPtr->setTargetVelocity(0);
}
+
/**
* Returns the Target for this controller, but as this is the Emergency controller it will ignored.
* As this controller will just break
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Torque.h b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Torque.h
index 96c5a437..3595732d 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Torque.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Torque.h
@@ -2,49 +2,45 @@
// armarx
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <armarx/control/joint_controller/Torque.h>
#include <armarx/control/rt_filters/AverageFilter.h>
#include <armarx/control/rt_filters/FirFilter.h>
-#include <armarx/control/joint_controller/Torque.h>
-
namespace devices::ethercat::sensor_actor_unit::armar6
{
class Device;
using DataPtr = std::shared_ptr<class Data>;
-}
-
+} // namespace devices::ethercat::sensor_actor_unit::armar6
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
using TorqueControllerPtr = std::shared_ptr<class TorqueController>;
-
- class TorqueController :
- public armarx::JointController
+ class TorqueController : public armarx::JointController
{
public:
+ TorqueController(
+ Device* joint,
+ DataPtr jointData,
+ armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData);
- TorqueController(Device* joint, DataPtr jointData, armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData);
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
armarx::control::rt_filters::FirFilter getFilter() const;
protected:
-
void rtPreActivateController() override;
private:
-
armarx::ControlTarget1DoFActuatorTorque target;
DataPtr dataPtr;
Device* joint;
armarx::control::joint_controller::TorqueController torqueController;
-
};
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Velocity.cpp b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Velocity.cpp
index 9edd33d9..7fc90beb 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Velocity.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Velocity.cpp
@@ -5,14 +5,15 @@
#include <devices/ethercat/sensor_actor_unit/armar6/Data.h>
#include <devices/ethercat/sensor_actor_unit/armar6/Device.h>
-
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
- VelocityController::VelocityController(Device* joint,
- DataPtr jointData,
- armarx::RampedAccelerationVelocityControllerConfigurationPtr velocityControllerConfigDataPtr)
- : JointController(), target(), joint(joint), setStart(true)
+ VelocityController::VelocityController(
+ Device* joint,
+ DataPtr jointData,
+ armarx::RampedAccelerationVelocityControllerConfigurationPtr
+ velocityControllerConfigDataPtr) :
+ JointController(), target(), joint(joint), setStart(true)
{
dataPtr = jointData;
limitless = dataPtr->isLimitLess();
@@ -35,8 +36,9 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
}
}
-
- void VelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ VelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (target.isValid())
{
@@ -44,15 +46,17 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
//send the velocity to the joint
controller.currentPosition = dataPtr->getActualPosition();
- controller.currentV = lastTargetVelocity;//dataPtr->getActualVelocity();
+ controller.currentV = lastTargetVelocity; //dataPtr->getActualVelocity();
controller.currentAcc = lastTargetAcceleration;
controller.dt = timeSinceLastIteration.toSecondsDouble();
controller.targetV = target.velocity;
auto r = controller.run();
lastTargetAcceleration = r.acceleration;
float newVel = r.velocity;
- dataPtr->setTargetAcceleration(std::max(controller.deceleration, std::abs(r.acceleration * 1.1)));
- dataPtr->setTargetDeceleration(std::max(controller.deceleration, std::abs(r.acceleration * 1.1)));
+ dataPtr->setTargetAcceleration(
+ std::max(controller.deceleration, std::abs(r.acceleration * 1.1)));
+ dataPtr->setTargetDeceleration(
+ std::max(controller.deceleration, std::abs(r.acceleration * 1.1)));
// if (r.acceleration != 0.0)
// {
// dataPtr->setTargetAcceleration(std::abs(r.acceleration * 1.1));
@@ -74,8 +78,12 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
}
// simple sanity check
- if ((dataPtr->getActualPosition() > static_cast<float>(controller.positionLimitHiSoft) && target.velocity > 0)
- || (dataPtr->getActualPosition() < static_cast<float>(controller.positionLimitLoSoft) && target.velocity < 0))
+ if ((dataPtr->getActualPosition() >
+ static_cast<float>(controller.positionLimitHiSoft) &&
+ target.velocity > 0) ||
+ (dataPtr->getActualPosition() <
+ static_cast<float>(controller.positionLimitLoSoft) &&
+ target.velocity < 0))
{
newVel = 0;
// ARMARX_INFO << deactivateSpam(0.1) << "Out of limits - Breaking now at " << dataPtr->getActualPosition() << " current: " << dataPtr->getActualCurrent();
@@ -91,29 +99,32 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
}
}
-
- armarx::ControlTargetBase* VelocityController::getControlTarget()
+ armarx::ControlTargetBase*
+ VelocityController::getControlTarget()
{
return ⌖
}
-
- void VelocityController::rtPreActivateController()
+ void
+ VelocityController::rtPreActivateController()
{
joint->switchControlMode(armarx::eVelocityControl);
float actualVelocity = dataPtr->getActualVelocity();
lastTargetVelocity = actualVelocity;
- lastTargetAcceleration = 0.0; // dataPtr->getActualAcceleration(); <-- using actual acceleration is dangerous due to high noise on acceleration!
+ lastTargetAcceleration =
+ 0.0; // dataPtr->getActualAcceleration(); <-- using actual acceleration is dangerous due to high noise on acceleration!
if (actualVelocity < 1e-2)
{
actualVelocity = 0;
}
- ARMARX_RT_LOGF_VERBOSE("Setting target velocity to %.2f, current acceleration: %.2f", actualVelocity, dataPtr->getActualAcceleration());
+ ARMARX_RT_LOGF_VERBOSE("Setting target velocity to %.2f, current acceleration: %.2f",
+ actualVelocity,
+ dataPtr->getActualAcceleration());
target.velocity = actualVelocity;
dataPtr->setTargetVelocity(actualVelocity);
start = armarx::rtNow();
}
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Velocity.h b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Velocity.h
index 9220083c..fff91f9a 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Velocity.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/Velocity.h
@@ -2,24 +2,25 @@
// armarx
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <armarx/control/hardware_config/Config.h>
-
namespace devices::ethercat::sensor_actor_unit::armar6
{
class Device;
using DataPtr = std::shared_ptr<class Data>;
-}
-
+} // namespace devices::ethercat::sensor_actor_unit::armar6
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
using VelocityControllerPtr = std::shared_ptr<class VelocityController>;
- inline void FillRampedVelocityControllerConfiguration(armarx::RampedAccelerationVelocityControllerConfiguration& config, armarx::control::hardware_config::Config& hwConfig)
+ inline void
+ FillRampedVelocityControllerConfiguration(
+ armarx::RampedAccelerationVelocityControllerConfiguration& config,
+ armarx::control::hardware_config::Config& hwConfig)
{
config.maxVelocityRad = hwConfig.getFloat("maxVelocityRad");
config.maxDecelerationRad = hwConfig.getFloat("maxDecelerationRad");
@@ -28,9 +29,11 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
config.directSetVLimit = hwConfig.getFloat("directSetVLimit");
}
- inline armarx::RampedAccelerationVelocityControllerConfigurationPtr CreateRampedVelocityControllerConfiguration(armarx::control::hardware_config::Config& hwConfig)
+ inline armarx::RampedAccelerationVelocityControllerConfigurationPtr
+ CreateRampedVelocityControllerConfiguration(armarx::control::hardware_config::Config& hwConfig)
{
- armarx::RampedAccelerationVelocityControllerConfigurationPtr config(new armarx::RampedAccelerationVelocityControllerConfiguration());
+ armarx::RampedAccelerationVelocityControllerConfigurationPtr config(
+ new armarx::RampedAccelerationVelocityControllerConfiguration());
FillRampedVelocityControllerConfiguration(*config, hwConfig);
return config;
}
@@ -39,18 +42,18 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
public:
-
VelocityController(Device* joint,
DataPtr jointData,
- armarx::RampedAccelerationVelocityControllerConfigurationPtr velocityControllerConfigDataPtr);
+ armarx::RampedAccelerationVelocityControllerConfigurationPtr
+ velocityControllerConfigDataPtr);
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
void rtPreActivateController() override;
private:
-
armarx::ControlTarget1DoFActuatorVelocity target;
armarx::VelocityControllerWithRampedAccelerationAndPositionBounds controller;
DataPtr dataPtr;
@@ -61,7 +64,6 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
//DEBUG
bool setStart;
IceUtil::Time start;
-
};
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/VelocityTorque.cpp b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/VelocityTorque.cpp
index 94a2c971..3c398ada 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/VelocityTorque.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/VelocityTorque.cpp
@@ -1,24 +1,19 @@
#include "VelocityTorque.h"
-
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
//#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <devices/ethercat/sensor_actor_unit/armar6/Data.h>
#include <devices/ethercat/sensor_actor_unit/armar6/Device.h>
-
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
- VelocityTorqueController::VelocityTorqueController(Device* joint,
- DataPtr jointData,
- armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData)
- : JointController(),
- target(),
- joint(joint),
- pid(),
- targetFilter(100)
+ VelocityTorqueController::VelocityTorqueController(
+ Device* joint,
+ DataPtr jointData,
+ armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData) :
+ JointController(), target(), joint(joint), pid(), targetFilter(100)
{
defaultMaxTorque = torqueConfigData->maxTorque;
@@ -39,36 +34,42 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
// coefficients for low-pass FIR-Filter for torque
filter.setImpulseResponse(
- {
- 0.0071f, 0.0007f, 0.0007f, 0.0008f, 0.0008f, 0.0008f, 0.0009f, 0.0009f, 0.0010f, 0.0010f, 0.0010f, 0.0011f, 0.0011f,
- 0.0012f, 0.0012f, 0.0012f, 0.0013f, 0.0013f, 0.0014f, 0.0014f, 0.0015f, 0.0015f, 0.0016f, 0.0016f, 0.0017f, 0.0017f,
- 0.0018f, 0.0018f, 0.0019f, 0.0020f, 0.0020f, 0.0021f, 0.0021f, 0.0022f, 0.0022f, 0.0023f, 0.0023f, 0.0024f, 0.0025f,
- 0.0025f, 0.0026f, 0.0026f, 0.0027f, 0.0028f, 0.0028f, 0.0029f, 0.0030f, 0.0030f, 0.0031f, 0.0031f, 0.0032f, 0.0033f,
- 0.0033f, 0.0034f, 0.0035f, 0.0035f, 0.0036f, 0.0036f, 0.0037f, 0.0038f, 0.0038f, 0.0039f, 0.0040f, 0.0040f, 0.0041f,
- 0.0041f, 0.0042f, 0.0043f, 0.0043f, 0.0044f, 0.0045f, 0.0045f, 0.0046f, 0.0046f, 0.0047f, 0.0048f, 0.0048f, 0.0049f,
- 0.0049f, 0.0050f, 0.0050f, 0.0051f, 0.0051f, 0.0052f, 0.0052f, 0.0053f, 0.0053f, 0.0054f, 0.0054f, 0.0055f, 0.0055f,
- 0.0056f, 0.0056f, 0.0057f, 0.0057f, 0.0058f, 0.0058f, 0.0058f, 0.0059f, 0.0059f, 0.0060f, 0.0060f, 0.0060f, 0.0061f,
- 0.0061f, 0.0061f, 0.0061f, 0.0062f, 0.0062f, 0.0062f, 0.0063f, 0.0063f, 0.0063f, 0.0063f, 0.0063f, 0.0064f, 0.0064f,
- 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f,
- 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0063f, 0.0063f, 0.0063f, 0.0063f,
- 0.0063f, 0.0062f, 0.0062f, 0.0062f, 0.0061f, 0.0061f, 0.0061f, 0.0061f, 0.0060f, 0.0060f, 0.0060f, 0.0059f, 0.0059f,
- 0.0058f, 0.0058f, 0.0058f, 0.0057f, 0.0057f, 0.0056f, 0.0056f, 0.0055f, 0.0055f, 0.0054f, 0.0054f, 0.0053f, 0.0053f,
- 0.0052f, 0.0052f, 0.0051f, 0.0051f, 0.0050f, 0.0050f, 0.0049f, 0.0049f, 0.0048f, 0.0048f, 0.0047f, 0.0046f, 0.0046f,
- 0.0045f, 0.0045f, 0.0044f, 0.0043f, 0.0043f, 0.0042f, 0.0041f, 0.0041f, 0.0040f, 0.0040f, 0.0039f, 0.0038f, 0.0038f,
- 0.0037f, 0.0036f, 0.0036f, 0.0035f, 0.0035f, 0.0034f, 0.0033f, 0.0033f, 0.0032f, 0.0031f, 0.0031f, 0.0030f, 0.0030f,
- 0.0029f, 0.0028f, 0.0028f, 0.0027f, 0.0026f, 0.0026f, 0.0025f, 0.0025f, 0.0024f, 0.0023f, 0.0023f, 0.0022f, 0.0022f,
- 0.0021f, 0.0021f, 0.0020f, 0.0020f, 0.0019f, 0.0018f, 0.0018f, 0.0017f, 0.0017f, 0.0016f, 0.0016f, 0.0015f, 0.0015f,
- 0.0014f, 0.0014f, 0.0013f, 0.0013f, 0.0012f, 0.0012f, 0.0012f, 0.0011f, 0.0011f, 0.0010f, 0.0010f, 0.0010f, 0.0009f,
- 0.0009f, 0.0008f, 0.0008f, 0.0008f, 0.0007f, 0.0007f, 0.0071f
- });
+ {0.0071f, 0.0007f, 0.0007f, 0.0008f, 0.0008f, 0.0008f, 0.0009f, 0.0009f, 0.0010f,
+ 0.0010f, 0.0010f, 0.0011f, 0.0011f, 0.0012f, 0.0012f, 0.0012f, 0.0013f, 0.0013f,
+ 0.0014f, 0.0014f, 0.0015f, 0.0015f, 0.0016f, 0.0016f, 0.0017f, 0.0017f, 0.0018f,
+ 0.0018f, 0.0019f, 0.0020f, 0.0020f, 0.0021f, 0.0021f, 0.0022f, 0.0022f, 0.0023f,
+ 0.0023f, 0.0024f, 0.0025f, 0.0025f, 0.0026f, 0.0026f, 0.0027f, 0.0028f, 0.0028f,
+ 0.0029f, 0.0030f, 0.0030f, 0.0031f, 0.0031f, 0.0032f, 0.0033f, 0.0033f, 0.0034f,
+ 0.0035f, 0.0035f, 0.0036f, 0.0036f, 0.0037f, 0.0038f, 0.0038f, 0.0039f, 0.0040f,
+ 0.0040f, 0.0041f, 0.0041f, 0.0042f, 0.0043f, 0.0043f, 0.0044f, 0.0045f, 0.0045f,
+ 0.0046f, 0.0046f, 0.0047f, 0.0048f, 0.0048f, 0.0049f, 0.0049f, 0.0050f, 0.0050f,
+ 0.0051f, 0.0051f, 0.0052f, 0.0052f, 0.0053f, 0.0053f, 0.0054f, 0.0054f, 0.0055f,
+ 0.0055f, 0.0056f, 0.0056f, 0.0057f, 0.0057f, 0.0058f, 0.0058f, 0.0058f, 0.0059f,
+ 0.0059f, 0.0060f, 0.0060f, 0.0060f, 0.0061f, 0.0061f, 0.0061f, 0.0061f, 0.0062f,
+ 0.0062f, 0.0062f, 0.0063f, 0.0063f, 0.0063f, 0.0063f, 0.0063f, 0.0064f, 0.0064f,
+ 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f,
+ 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f,
+ 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0063f, 0.0063f, 0.0063f, 0.0063f, 0.0063f,
+ 0.0062f, 0.0062f, 0.0062f, 0.0061f, 0.0061f, 0.0061f, 0.0061f, 0.0060f, 0.0060f,
+ 0.0060f, 0.0059f, 0.0059f, 0.0058f, 0.0058f, 0.0058f, 0.0057f, 0.0057f, 0.0056f,
+ 0.0056f, 0.0055f, 0.0055f, 0.0054f, 0.0054f, 0.0053f, 0.0053f, 0.0052f, 0.0052f,
+ 0.0051f, 0.0051f, 0.0050f, 0.0050f, 0.0049f, 0.0049f, 0.0048f, 0.0048f, 0.0047f,
+ 0.0046f, 0.0046f, 0.0045f, 0.0045f, 0.0044f, 0.0043f, 0.0043f, 0.0042f, 0.0041f,
+ 0.0041f, 0.0040f, 0.0040f, 0.0039f, 0.0038f, 0.0038f, 0.0037f, 0.0036f, 0.0036f,
+ 0.0035f, 0.0035f, 0.0034f, 0.0033f, 0.0033f, 0.0032f, 0.0031f, 0.0031f, 0.0030f,
+ 0.0030f, 0.0029f, 0.0028f, 0.0028f, 0.0027f, 0.0026f, 0.0026f, 0.0025f, 0.0025f,
+ 0.0024f, 0.0023f, 0.0023f, 0.0022f, 0.0022f, 0.0021f, 0.0021f, 0.0020f, 0.0020f,
+ 0.0019f, 0.0018f, 0.0018f, 0.0017f, 0.0017f, 0.0016f, 0.0016f, 0.0015f, 0.0015f,
+ 0.0014f, 0.0014f, 0.0013f, 0.0013f, 0.0012f, 0.0012f, 0.0012f, 0.0011f, 0.0011f,
+ 0.0010f, 0.0010f, 0.0010f, 0.0009f, 0.0009f, 0.0008f, 0.0008f, 0.0008f, 0.0007f,
+ 0.0007f, 0.0071f});
// coefficients for low-pass FIR-Filter for velocity
- filter_vel.setImpulseResponse(
- {
- 0.1744f, 0.0205f, 0.0215f, 0.0225f, 0.0235f, 0.0244f, 0.0252f, 0.0259f, 0.0265f, 0.0270f, 0.0274f, 0.0276f, 0.0278f,
- 0.0282f, 0.0282f, 0.0278f, 0.0276f, 0.0274f, 0.0270f, 0.0265f, 0.0259f, 0.0252f, 0.0244f, 0.0235f, 0.0225f, 0.0215f,
- 0.0205f, 0.1744f
- });
+ filter_vel.setImpulseResponse({0.1744f, 0.0205f, 0.0215f, 0.0225f, 0.0235f, 0.0244f,
+ 0.0252f, 0.0259f, 0.0265f, 0.0270f, 0.0274f, 0.0276f,
+ 0.0278f, 0.0282f, 0.0282f, 0.0278f, 0.0276f, 0.0274f,
+ 0.0270f, 0.0265f, 0.0259f, 0.0252f, 0.0244f, 0.0235f,
+ 0.0225f, 0.0215f, 0.0205f, 0.1744f});
velCtrl.acceleration = 4.5;
velCtrl.deceleration = 4.5;
@@ -79,11 +80,11 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
velCtrl.positionLimitHiSoft = dataPtr->getSoftLimitHi();
// controller.positionLimitLoHard = dataPtr->getHardLimitLo();
velCtrl.positionLimitLoSoft = dataPtr->getSoftLimitLo();
-
}
-
- void VelocityTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ VelocityTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
float maxTorque = target.maxTorque < 0 ? defaultMaxTorque : target.maxTorque;
pid.maxTorque = maxTorque;
@@ -158,7 +159,8 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
adjustedTargetVelocity = 0.0;
}
- float Kd = (-gravity + maxTorque + actualTorque) / std::max(0.0001f, std::abs(adjustedTargetVelocity));
+ float Kd = (-gravity + maxTorque + actualTorque) /
+ std::max(0.0001f, std::abs(adjustedTargetVelocity));
//ARMARX_RT_LOGF_INFO("Kd: %.2f", Kd).deactivateSpam(0.3);
@@ -201,7 +203,6 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
//adjustedTargetVelocity = 0.0f;
pid.integral_gain = 0.1;
pid.proportional_gain = 0.0;
-
}
if (std::abs(Kd) > 200)
@@ -265,20 +266,20 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
//ARMARX_RT_LOGF_INFO("Target current: %.2f", current).deactivateSpam(0.3);
}
-
- armarx::ControlTargetBase* VelocityTorqueController::getControlTarget()
+ armarx::ControlTargetBase*
+ VelocityTorqueController::getControlTarget()
{
return ⌖
}
-
- armarx::control::rt_filters::FirFilter VelocityTorqueController::getFilter() const
+ armarx::control::rt_filters::FirFilter
+ VelocityTorqueController::getFilter() const
{
return filter;
}
-
- void VelocityTorqueController::rtPreActivateController()
+ void
+ VelocityTorqueController::rtPreActivateController()
{
joint->switchControlMode(armarx::eTorqueControl);
dataPtr->setTargetCurrent(0.0f);
@@ -286,25 +287,29 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
target.velocity = lastSetVelocity;
}
-
- VelocityTorqueControllerConfigurationPtr VelocityTorqueControllerConfiguration::CreateTorqueConfigDataFromXml(armarx::DefaultRapidXmlReaderNode node)
+ VelocityTorqueControllerConfigurationPtr
+ VelocityTorqueControllerConfiguration::CreateTorqueConfigDataFromXml(
+ armarx::DefaultRapidXmlReaderNode node)
{
VelocityTorqueControllerConfiguration configData;
ARMARX_CHECK_EXPRESSION(node.is_valid());
- configData.pid_proportional_gain = (double) node.first_node("pid_proportional_gain").value_as_float();
- configData.pid_integral_gain = (double) node.first_node("pid_integral_gain").value_as_float();
- configData.pid_derivative_gain = (double) node.first_node("pid_derivative_gain").value_as_float();
- configData.pid_windup_guard = (double) node.first_node("pid_windup_guard").value_as_float();
- configData.pid_max_value = (double) node.first_node("pid_max_value").value_as_float();
- configData.pid_min_value = (double) node.first_node("pid_min_value").value_as_float();
- configData.pid_dis = (double) node.first_node("pid_dis").value_as_float();
- configData.Kd = (double) node.first_node("Kd").value_as_float();
- configData.inertia = (double) node.first_node("inertia").value_as_float();
- configData.maxTorque = (double) node.first_node("maxTorque").value_as_float();
- configData.actuatorType = (double) node.first_node("actuatorType").value_as_float();
+ configData.pid_proportional_gain =
+ (double)node.first_node("pid_proportional_gain").value_as_float();
+ configData.pid_integral_gain =
+ (double)node.first_node("pid_integral_gain").value_as_float();
+ configData.pid_derivative_gain =
+ (double)node.first_node("pid_derivative_gain").value_as_float();
+ configData.pid_windup_guard = (double)node.first_node("pid_windup_guard").value_as_float();
+ configData.pid_max_value = (double)node.first_node("pid_max_value").value_as_float();
+ configData.pid_min_value = (double)node.first_node("pid_min_value").value_as_float();
+ configData.pid_dis = (double)node.first_node("pid_dis").value_as_float();
+ configData.Kd = (double)node.first_node("Kd").value_as_float();
+ configData.inertia = (double)node.first_node("inertia").value_as_float();
+ configData.maxTorque = (double)node.first_node("maxTorque").value_as_float();
+ configData.actuatorType = (double)node.first_node("actuatorType").value_as_float();
auto v = node.first_node("firFilterImpulseResponse").value_as_string();
- for (auto& elem : armarx::Split(v, ",", true))
+ for (auto& elem : armarx::Split(v, ",", true))
{
configData.firFilterImpulseResponse.push_back(armarx::toFloat(elem));
}
@@ -312,10 +317,8 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
return std::make_shared<VelocityTorqueControllerConfiguration>(configData);
}
-
VelocityTorqueControllerConfiguration::VelocityTorqueControllerConfiguration()
{
-
}
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/VelocityTorque.h b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/VelocityTorque.h
index c1ced773..7423925c 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/VelocityTorque.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/VelocityTorque.h
@@ -4,32 +4,30 @@
// armarx
#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <armarx/control/joint_controller/Torque.h>
#include <armarx/control/rt_filters/AverageFilter.h>
#include <armarx/control/rt_filters/FirFilter.h>
-#include <armarx/control/joint_controller/Torque.h>
-
namespace devices::ethercat::sensor_actor_unit::armar6
{
class Device;
using DataPtr = std::shared_ptr<class Data>;
-}
-
+} // namespace devices::ethercat::sensor_actor_unit::armar6
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
- using VelocityTorqueControllerConfigurationPtr = std::shared_ptr<class VelocityTorqueControllerConfiguration>;
-
+ using VelocityTorqueControllerConfigurationPtr =
+ std::shared_ptr<class VelocityTorqueControllerConfiguration>;
class VelocityTorqueControllerConfiguration
{
public:
-
VelocityTorqueControllerConfiguration();
- static VelocityTorqueControllerConfigurationPtr CreateTorqueConfigDataFromXml(armarx::DefaultRapidXmlReaderNode node);
+ static VelocityTorqueControllerConfigurationPtr
+ CreateTorqueConfigDataFromXml(armarx::DefaultRapidXmlReaderNode node);
double pid_proportional_gain;
double pid_integral_gain;
double pid_derivative_gain;
@@ -44,33 +42,30 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
double actuatorType;
bool status;
std::vector<float> firFilterImpulseResponse;
-
};
using VelocityTorqueControllerPtr = std::shared_ptr<class VelocityTorqueController>;
-
class VelocityTorqueController : public armarx::JointController
{
public:
+ VelocityTorqueController(
+ Device* joint,
+ DataPtr jointData,
+ armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData);
- VelocityTorqueController(Device* joint,
- DataPtr jointData,
- armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData);
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
armarx::control::rt_filters::FirFilter getFilter() const;
protected:
-
void rtPreActivateController() override;
private:
-
float defaultMaxTorque;
armarx::ControlTarget1DoFActuatorTorqueVelocity target;
DataPtr dataPtr;
@@ -83,7 +78,6 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
armarx::control::rt_filters::FirFilter hp_filter;
armarx::control::rt_filters::FirFilter lp_filter;
armarx::control::rt_filters::RtAverageFilter targetFilter;
-
};
-} // namespace devices::ethercat::sensor_actor_unit::v1::joint_controller
+} // namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ZeroTorque.cpp b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ZeroTorque.cpp
index 2d4940ee..ec874a97 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ZeroTorque.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ZeroTorque.cpp
@@ -25,28 +25,27 @@
#include "ZeroTorque.h"
-
#include <devices/ethercat/sensor_actor_unit/armar6/Data.h>
#include <devices/ethercat/sensor_actor_unit/armar6/Device.h>
// below for remote GUI
-#include <ArmarXCore/core/application/Application.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-
#include "ArmarXGui/libraries/DefaultWidgetDescriptions/DefaultWidgetDescriptions.h"
-#include "ArmarXGui/libraries/VariantWidget/VariantWidget.h"
-#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
#include "ArmarXGui/libraries/RemoteGui/WidgetBuilder/FloatWidgets.h"
#include "ArmarXGui/libraries/RemoteGui/WidgetBuilder/StringWidgets.h"
-
+#include "ArmarXGui/libraries/VariantWidget/VariantWidget.h"
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
+#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
- ZeroTorqueController::ZeroTorqueController(Device* joint,
- DataPtr jointData,
- armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfigurationPtr torqueConfigData) :
+ ZeroTorqueController::ZeroTorqueController(
+ Device* joint,
+ DataPtr jointData,
+ armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfigurationPtr
+ torqueConfigData) :
target(),
joint(joint),
torqueController(torqueConfigData),
@@ -56,8 +55,9 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
controllerConfigBufferPublishToRt.reinitAllBuffers(*torqueConfigData);
}
-
- void ZeroTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ ZeroTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (target.isValid())
{
@@ -77,244 +77,262 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
auto torqueControllerConfig = controllerConfigBufferPublishToRt.getUpToDateReadBuffer();
torqueController.setTorqueConfigData(torqueControllerConfig);
- float current = torqueController.update(sensorValuesTimestamp, timeSinceLastIteration, gravity, actualTorque, targetTorque, actualVelocity, actualPosition);
+ float current = torqueController.update(sensorValuesTimestamp,
+ timeSinceLastIteration,
+ gravity,
+ actualTorque,
+ targetTorque,
+ actualVelocity,
+ actualPosition);
// send the current value to Elmo
dataPtr->setTargetCurrent(current);
}
}
-
- armarx::ControlTargetBase* ZeroTorqueController::getControlTarget()
+ armarx::ControlTargetBase*
+ ZeroTorqueController::getControlTarget()
{
return ⌖
}
-
- void ZeroTorqueController::rtPreActivateController()
+ void
+ ZeroTorqueController::rtPreActivateController()
{
joint->switchControlMode(armarx::eTorqueControl);
dataPtr->setTargetCurrent(0.0f);
torqueController.reset();
}
-
- armarx::StringVariantBaseMap ZeroTorqueController::publish(const armarx::DebugDrawerInterfacePrx& /*draw*/, const armarx::DebugObserverInterfacePrx& /*observer*/) const
+ armarx::StringVariantBaseMap
+ ZeroTorqueController::publish(const armarx::DebugDrawerInterfacePrx& /*draw*/,
+ const armarx::DebugObserverInterfacePrx& /*observer*/) const
{
- if (!remoteGui)
- {
- threadHandle = armarx::Application::getInstance()->getThreadPool()->runTask([this]
+ if (!remoteGui)
{
- std::string guiTabName;
- while (!stopRequested)
- {
- armarx::ManagedIceObjectPtr object;
- ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
- try
- {
- object = armarx::ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
- ARMARX_CHECK_EXPRESSION(object);
- remoteGui = object->getProxy<armarx::RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
- if (!remoteGui)
- {
- continue;
- }
- ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
- guiTabName = getParent().getDeviceName() + getControlMode();
- break;
- }
- catch (...)
- {
- sleep(1);
- }
- }
-
- if (remoteGui)
- {
- ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
- using namespace armarx::RemoteGui;
-
- auto vLayout = makeVBoxLayout();
-
- {
- WidgetPtr KpLabel = makeTextLabel("Kp: ");
- const double rangeVal = 20.;
- double minVal = 0.;
- double maxVal = 0.;
- if(initialTorqueConfigData.pid_proportional_gain > 0)
- {
- minVal = 0.;
- maxVal = rangeVal;
-
- } else {
- minVal = -rangeVal;
- maxVal = 0.;
- }
- auto KpSlider
- = makeFloatSpinBox("KpSlider")
- .steps(rangeVal/0.001)
- .min(minVal).max(maxVal)
- .value(initialTorqueConfigData.pid_proportional_gain);
-
- WidgetPtr line = makeHBoxLayout()
- .children({KpLabel, KpSlider});
- vLayout.addChild(line);
- }
-
- {
- WidgetPtr KiLabel = makeTextLabel("Ki: ");
- const double rangeVal = 20.;
- double minVal = 0.;
- double maxVal = 0.;
- if(initialTorqueConfigData.pid_proportional_gain > 0)
- {
- minVal = 0.;
- maxVal = rangeVal;
-
- } else {
- minVal = -rangeVal;
- maxVal = 0.;
- }
- auto KiSlider
- = makeFloatSpinBox("KiSlider")
- .steps(rangeVal/0.001)
- .min(minVal).max(maxVal)
- .value(initialTorqueConfigData.pid_integral_gain);
-
- WidgetPtr line = makeHBoxLayout()
- .children({KiLabel, KiSlider});
- vLayout.addChild(line);
- }
-
- {
- WidgetPtr KdLabel = makeTextLabel("Kd: ");
- const double rangeVal = 20.;
- double minVal = 0.;
- double maxVal = 0.;
- if(initialTorqueConfigData.pid_derivative_gain > 0)
- {
- minVal = 0.;
- maxVal = rangeVal;
-
- } else {
- minVal = -rangeVal;
- maxVal = 0.;
- }
- auto KdSlider
- = makeFloatSpinBox("KdSlider")
- .steps(rangeVal/0.001)
- .min(minVal).max(maxVal)
- .value(initialTorqueConfigData.pid_derivative_gain);
-
- WidgetPtr line = makeHBoxLayout()
- .children({KdLabel, KdSlider});
- vLayout.addChild(line);
- }
-
- vLayout.addChild(makeHBoxLayout().children({
- makeTextLabel("Max Acc: "),
- makeFloatSpinBox("MaxAcc")
- .minmax(0, 20.)
- .steps(1000)
- .value(initialTorqueConfigData.maxAcceleration)}));
-
- vLayout.addChild(makeHBoxLayout().children({
- makeTextLabel("JL Margin: "),
- makeFloatSpinBox("PushBackMargin")
- .minmax(0.0, 50.)
- .steps(1000)
- .value(initialTorqueConfigData.pushbackMargin)}));
-
- vLayout.addChild(makeHBoxLayout().children({
- makeTextLabel("JL Torque: "),
- makeFloatSpinBox("PushBackTorque")
- .minmax(0, 1000.)
- .steps(1000)
- .value(initialTorqueConfigData.pushBackTorque)}));
-
- {
- WidgetPtr DecayLabel = makeTextLabel("Decay: ");
- auto DecaySlider
- = makeFloatSpinBox("DecaySlider")
- .steps(1000)
- .min(0.).max(1.)
- .value(initialTorqueConfigData.decay);
- WidgetPtr DecayLabelValue = makeLabel("DecayValue").value(std::to_string(initialTorqueConfigData.decay));
- WidgetPtr line = makeHBoxLayout()
- .children({DecayLabel, DecaySlider, DecayLabelValue});
-
- vLayout.addChild(line);
- vLayout.addChild(new VSpacer);
- }
-
- {
- WidgetPtr DeadzoneLabel = makeTextLabel("Deadzone: ");
- auto DeadzoneSlider
- = makeFloatSpinBox("DeadzoneSlider")
- .steps(1000)
- .min(0.).max(10.)
- .value(initialTorqueConfigData.deadZone);
- WidgetPtr DeadzoneLabelValue = makeLabel("DeadzoneValue").value(std::to_string(initialTorqueConfigData.deadZone));
- WidgetPtr line = makeHBoxLayout()
- .children({DeadzoneLabel, DeadzoneSlider, DeadzoneLabelValue});
-
- vLayout.addChild(line);
- vLayout.addChild(new VSpacer);
- }
-
- WidgetPtr groupBox = makeGroupBox("GroupBox")
- .label("Group")
- .child(vLayout);
-
- remoteGui->createTab(guiTabName, groupBox);
-
- while (!stopRequested)
- {
- armarx::RemoteGui::TabProxy tab(remoteGui, guiTabName);
- tab.receiveUpdates();
-
- auto& torqueConfigData = controllerConfigBufferPublishToRt.getWriteBuffer();
-
- torqueConfigData.pid_proportional_gain = tab.getValue<float>("KpSlider").get();
- torqueConfigData.pid_integral_gain = tab.getValue<float>("KiSlider").get();
- torqueConfigData.pid_derivative_gain = tab.getValue<float>("KdSlider").get();
- torqueConfigData.maxAcceleration = tab.getValue<float>("MaxAcc").get();
- torqueConfigData.decay = tab.getValue<float>("DecaySlider").get();
- torqueConfigData.deadZone = tab.getValue<float>("DeadzoneSlider").get();
- torqueConfigData.pushBackTorque = tab.getValue<float>("PushBackTorque").get();
- torqueConfigData.pushbackMargin = tab.getValue<float>("PushBackMargin").get();
-
- controllerConfigBufferPublishToRt.commitWrite();
-
- tab.getValue<std::string>("DecayValue").set(std::to_string(torqueConfigData.decay));
- tab.getValue<std::string>("DeadzoneValue").set(std::to_string(torqueConfigData.deadZone));
-
-
- tab.sendUpdates();
-
- usleep(100000);
- }
- }
-
- });
- }
- return {};
- // return {{"lastTargetVelocity", new armarx::Variant(lastTargetVelocity.load())},
- // {"targetPosition", new armarx::Variant(posController.currentPosition)}, // position of profile generator is target position
- // {"posError", new armarx::Variant(posController.getTargetPosition() - posController.currentPosition)},
- // {"pidError", new armarx::Variant(pidPosController->previousError)},
- // // {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
- // {"pidIntegralCV", new armarx::Variant(pidPosController->integral * static_cast<double>(pidPosController->Ki))},
- // {"pidIntegral", new armarx::Variant(pidPosController->integral)},
- // {"pidPropCV", new armarx::Variant(pidPosController->previousError * static_cast<double>(pidPosController->Kp))},
- // {"pidDiffCV", new armarx::Variant(pidPosController->derivative * static_cast<double>(pidPosController->Kd))},
- // // {"pospidIntegralCV", new Variant(posController.pid->integral * posController.pid->Ki)},
- // // {"pospidIntegral", new Variant(posController.pid->integral)},
- // // {"pospidPropCV", new Variant(posController.pid->previousError * posController.pid->Kp)},
- // // {"pospidDiffCV", new Variant(posController.pid->derivative * posController.pid->Kd)},
- // // {"pidUsed", new Variant(posController.getCurrentlyPIDActive())},
- // {"desiredPWM", new armarx::Variant(targetPWM.load())}
- // };
+ threadHandle = armarx::Application::getInstance()->getThreadPool()->runTask(
+ [this]
+ {
+ std::string guiTabName;
+ while (!stopRequested)
+ {
+ armarx::ManagedIceObjectPtr object;
+ ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
+ try
+ {
+ object =
+ armarx::ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
+ ARMARX_CHECK_EXPRESSION(object);
+ remoteGui = object->getProxy<armarx::RemoteGuiInterfacePrx>(
+ "RemoteGuiProvider", false, "", false);
+ if (!remoteGui)
+ {
+ continue;
+ }
+ ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
+ guiTabName = getParent().getDeviceName() + getControlMode();
+ break;
+ }
+ catch (...)
+ {
+ sleep(1);
+ }
+ }
+
+ if (remoteGui)
+ {
+ ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
+ using namespace armarx::RemoteGui;
+
+ auto vLayout = makeVBoxLayout();
+
+ {
+ WidgetPtr KpLabel = makeTextLabel("Kp: ");
+ const double rangeVal = 20.;
+ double minVal = 0.;
+ double maxVal = 0.;
+ if (initialTorqueConfigData.pid_proportional_gain > 0)
+ {
+ minVal = 0.;
+ maxVal = rangeVal;
+ }
+ else
+ {
+ minVal = -rangeVal;
+ maxVal = 0.;
+ }
+ auto KpSlider =
+ makeFloatSpinBox("KpSlider")
+ .steps(rangeVal / 0.001)
+ .min(minVal)
+ .max(maxVal)
+ .value(initialTorqueConfigData.pid_proportional_gain);
+
+ WidgetPtr line = makeHBoxLayout().children({KpLabel, KpSlider});
+ vLayout.addChild(line);
+ }
+
+ {
+ WidgetPtr KiLabel = makeTextLabel("Ki: ");
+ const double rangeVal = 20.;
+ double minVal = 0.;
+ double maxVal = 0.;
+ if (initialTorqueConfigData.pid_proportional_gain > 0)
+ {
+ minVal = 0.;
+ maxVal = rangeVal;
+ }
+ else
+ {
+ minVal = -rangeVal;
+ maxVal = 0.;
+ }
+ auto KiSlider = makeFloatSpinBox("KiSlider")
+ .steps(rangeVal / 0.001)
+ .min(minVal)
+ .max(maxVal)
+ .value(initialTorqueConfigData.pid_integral_gain);
+
+ WidgetPtr line = makeHBoxLayout().children({KiLabel, KiSlider});
+ vLayout.addChild(line);
+ }
+
+ {
+ WidgetPtr KdLabel = makeTextLabel("Kd: ");
+ const double rangeVal = 20.;
+ double minVal = 0.;
+ double maxVal = 0.;
+ if (initialTorqueConfigData.pid_derivative_gain > 0)
+ {
+ minVal = 0.;
+ maxVal = rangeVal;
+ }
+ else
+ {
+ minVal = -rangeVal;
+ maxVal = 0.;
+ }
+ auto KdSlider = makeFloatSpinBox("KdSlider")
+ .steps(rangeVal / 0.001)
+ .min(minVal)
+ .max(maxVal)
+ .value(initialTorqueConfigData.pid_derivative_gain);
+
+ WidgetPtr line = makeHBoxLayout().children({KdLabel, KdSlider});
+ vLayout.addChild(line);
+ }
+
+ vLayout.addChild(makeHBoxLayout().children(
+ {makeTextLabel("Max Acc: "),
+ makeFloatSpinBox("MaxAcc").minmax(0, 20.).steps(1000).value(
+ initialTorqueConfigData.maxAcceleration)}));
+
+ vLayout.addChild(makeHBoxLayout().children(
+ {makeTextLabel("JL Margin: "),
+ makeFloatSpinBox("PushBackMargin")
+ .minmax(0.0, 50.)
+ .steps(1000)
+ .value(initialTorqueConfigData.pushbackMargin)}));
+
+ vLayout.addChild(makeHBoxLayout().children(
+ {makeTextLabel("JL Torque: "),
+ makeFloatSpinBox("PushBackTorque")
+ .minmax(0, 1000.)
+ .steps(1000)
+ .value(initialTorqueConfigData.pushBackTorque)}));
+
+ {
+ WidgetPtr DecayLabel = makeTextLabel("Decay: ");
+ auto DecaySlider = makeFloatSpinBox("DecaySlider")
+ .steps(1000)
+ .min(0.)
+ .max(1.)
+ .value(initialTorqueConfigData.decay);
+ WidgetPtr DecayLabelValue =
+ makeLabel("DecayValue")
+ .value(std::to_string(initialTorqueConfigData.decay));
+ WidgetPtr line = makeHBoxLayout().children(
+ {DecayLabel, DecaySlider, DecayLabelValue});
+
+ vLayout.addChild(line);
+ vLayout.addChild(new VSpacer);
+ }
+
+ {
+ WidgetPtr DeadzoneLabel = makeTextLabel("Deadzone: ");
+ auto DeadzoneSlider = makeFloatSpinBox("DeadzoneSlider")
+ .steps(1000)
+ .min(0.)
+ .max(10.)
+ .value(initialTorqueConfigData.deadZone);
+ WidgetPtr DeadzoneLabelValue =
+ makeLabel("DeadzoneValue")
+ .value(std::to_string(initialTorqueConfigData.deadZone));
+ WidgetPtr line = makeHBoxLayout().children(
+ {DeadzoneLabel, DeadzoneSlider, DeadzoneLabelValue});
+
+ vLayout.addChild(line);
+ vLayout.addChild(new VSpacer);
+ }
+
+ WidgetPtr groupBox = makeGroupBox("GroupBox").label("Group").child(vLayout);
+
+ remoteGui->createTab(guiTabName, groupBox);
+
+ while (!stopRequested)
+ {
+ armarx::RemoteGui::TabProxy tab(remoteGui, guiTabName);
+ tab.receiveUpdates();
+
+ auto& torqueConfigData =
+ controllerConfigBufferPublishToRt.getWriteBuffer();
+
+ torqueConfigData.pid_proportional_gain =
+ tab.getValue<float>("KpSlider").get();
+ torqueConfigData.pid_integral_gain =
+ tab.getValue<float>("KiSlider").get();
+ torqueConfigData.pid_derivative_gain =
+ tab.getValue<float>("KdSlider").get();
+ torqueConfigData.maxAcceleration = tab.getValue<float>("MaxAcc").get();
+ torqueConfigData.decay = tab.getValue<float>("DecaySlider").get();
+ torqueConfigData.deadZone = tab.getValue<float>("DeadzoneSlider").get();
+ torqueConfigData.pushBackTorque =
+ tab.getValue<float>("PushBackTorque").get();
+ torqueConfigData.pushbackMargin =
+ tab.getValue<float>("PushBackMargin").get();
+
+ controllerConfigBufferPublishToRt.commitWrite();
+
+ tab.getValue<std::string>("DecayValue")
+ .set(std::to_string(torqueConfigData.decay));
+ tab.getValue<std::string>("DeadzoneValue")
+ .set(std::to_string(torqueConfigData.deadZone));
+
+
+ tab.sendUpdates();
+
+ usleep(100000);
+ }
+ }
+ });
+ }
+ return {};
+ // return {{"lastTargetVelocity", new armarx::Variant(lastTargetVelocity.load())},
+ // {"targetPosition", new armarx::Variant(posController.currentPosition)}, // position of profile generator is target position
+ // {"posError", new armarx::Variant(posController.getTargetPosition() - posController.currentPosition)},
+ // {"pidError", new armarx::Variant(pidPosController->previousError)},
+ // // {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
+ // {"pidIntegralCV", new armarx::Variant(pidPosController->integral * static_cast<double>(pidPosController->Ki))},
+ // {"pidIntegral", new armarx::Variant(pidPosController->integral)},
+ // {"pidPropCV", new armarx::Variant(pidPosController->previousError * static_cast<double>(pidPosController->Kp))},
+ // {"pidDiffCV", new armarx::Variant(pidPosController->derivative * static_cast<double>(pidPosController->Kd))},
+ // // {"pospidIntegralCV", new Variant(posController.pid->integral * posController.pid->Ki)},
+ // // {"pospidIntegral", new Variant(posController.pid->integral)},
+ // // {"pospidPropCV", new Variant(posController.pid->previousError * posController.pid->Kp)},
+ // // {"pospidDiffCV", new Variant(posController.pid->derivative * posController.pid->Kd)},
+ // // {"pidUsed", new Variant(posController.getCurrentlyPIDActive())},
+ // {"desiredPWM", new armarx::Variant(targetPWM.load())}
+ // };
}
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ZeroTorque.h b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ZeroTorque.h
index 3c4f8bcd..c04a6562 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ZeroTorque.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ZeroTorque.h
@@ -27,21 +27,18 @@
// armarx
+#include <ArmarXCore/core/services/tasks/ThreadPool.h>
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <armarx/control/joint_controller/VelocityManipulatingTorque.h>
-#include <ArmarXCore/core/services/tasks/ThreadPool.h>
-#include <ArmarXGui/interface/RemoteGuiInterface.h>
-
-
namespace devices::ethercat::sensor_actor_unit::armar6
{
class Device;
using DataPtr = std::shared_ptr<class Data>;
-}
-
+} // namespace devices::ethercat::sensor_actor_unit::armar6
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
@@ -51,33 +48,38 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
{
public:
-
- ZeroTorqueController(Device* joint,
- DataPtr jointData,
- armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfigurationPtr torqueConfigData);
+ ZeroTorqueController(
+ Device* joint,
+ DataPtr jointData,
+ armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfigurationPtr
+ torqueConfigData);
~ZeroTorqueController() noexcept(true){};
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
// JointController interface
- armarx::StringVariantBaseMap publish(const armarx::DebugDrawerInterfacePrx& draw, const armarx::DebugObserverInterfacePrx& observer) const final;
+ armarx::StringVariantBaseMap
+ publish(const armarx::DebugDrawerInterfacePrx& draw,
+ const armarx::DebugObserverInterfacePrx& observer) const final;
protected:
-
void rtPreActivateController() override;
mutable armarx::RemoteGuiInterfacePrx remoteGui;
mutable armarx::ThreadPool::Handle threadHandle;
bool stopRequested = false;
private:
-
armarx::ControlTarget1DoFActuatorZeroTorque target;
DataPtr dataPtr;
Device* joint;
armarx::control::joint_controller::VelocityManipulatingTorqueController torqueController;
- const armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfiguration initialTorqueConfigData;
- mutable armarx::TripleBuffer<armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfiguration> controllerConfigBufferPublishToRt;
+ const armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfiguration
+ initialTorqueConfigData;
+ mutable armarx::TripleBuffer<
+ armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfiguration>
+ controllerConfigBufferPublishToRt;
};
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ZeroVelocity.cpp b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ZeroVelocity.cpp
index 8487f0db..c74e2298 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ZeroVelocity.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ZeroVelocity.cpp
@@ -29,16 +29,14 @@
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
+
#include <devices/ethercat/sensor_actor_unit/armar6/Data.h>
#include <devices/ethercat/sensor_actor_unit/armar6/Device.h>
-
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller::zero_velocity
{
ZeroVelocityControllerConfigurationPtr
@@ -60,7 +58,8 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller::zero_v
return std::make_shared<ZeroVelocityControllerConfiguration>(configData);
}
- ZeroVelocityController::ZeroVelocityController(Device *joint,
+ ZeroVelocityController::ZeroVelocityController(
+ Device* joint,
DataPtr jointData,
ZeroVelocityControllerConfigurationPtr configData) :
JointController(),
@@ -99,17 +98,22 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller::zero_v
{
config->feedforwardVelocityToCurrentFactor =
bufferPublishToRt.getUpToDateReadBuffer().feedforwardVelocityToCurrentFactor;
-// float actualVelocity = dataPtr->getAbsEncVelocity();
+ // float actualVelocity = dataPtr->getAbsEncVelocity();
float actualVelocity = dataPtr->getActualVelocity();
float alpha = 10;
- float factor = config->feedforwardVelocityToCurrentFactor * exp(-pow(actualVelocity, 2) * alpha);
+ float factor =
+ config->feedforwardVelocityToCurrentFactor * exp(-pow(actualVelocity, 2) * alpha);
float targetPWM = actualVelocity * config->feedforwardVelocityToCurrentFactor;
float targetCurrent = actualVelocity * factor;
- targetCurrent += armarx::math::MathUtils::Sign(actualVelocity) * armarx::math::MathUtils::Sign(config->feedforwardVelocityToCurrentFactor) * config->currentDeadzone;
- ARMARX_RT_LOGF_INFO("-- factor: %.1f, deadzone: %.1f, actualVelocity: %.3f, target current: %.3f, new target current: %.3f",
+ targetCurrent +=
+ armarx::math::MathUtils::Sign(actualVelocity) *
+ armarx::math::MathUtils::Sign(config->feedforwardVelocityToCurrentFactor) *
+ config->currentDeadzone;
+ ARMARX_RT_LOGF_INFO("-- factor: %.1f, deadzone: %.1f, actualVelocity: %.3f, target "
+ "current: %.3f, new target current: %.3f",
config->feedforwardVelocityToCurrentFactor,
config->currentDeadzone,
actualVelocity,
@@ -199,30 +203,26 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller::zero_v
const double minVal = -50000.;
const double maxVal = 50000.;
- auto factorSlider =
- makeFloatSpinBox("currentFactor")
- .steps(maxVal / 0.001)
- .min(minVal)
- .max(maxVal)
- .value(
- initControllerConfigNonRT.feedforwardVelocityToCurrentFactor);
+ auto factorSlider = makeFloatSpinBox("currentFactor")
+ .steps(maxVal / 0.001)
+ .min(minVal)
+ .max(maxVal)
+ .value(initControllerConfigNonRT
+ .feedforwardVelocityToCurrentFactor);
WidgetPtr line = makeHBoxLayout().children({factorLabel, factorSlider});
vLayout.addChild(line);
}
{
- WidgetPtr deadzoneLabel =
- makeTextLabel("Current Deadzone: ");
+ WidgetPtr deadzoneLabel = makeTextLabel("Current Deadzone: ");
const double minVal = -5000.;
const double maxVal = 5000.;
- auto deadzonBox =
- makeFloatSpinBox("currentDeadzone")
- .steps(maxVal / 0.001)
- .min(minVal)
- .max(maxVal)
- .value(
- initControllerConfigNonRT.currentDeadzone);
+ auto deadzonBox = makeFloatSpinBox("currentDeadzone")
+ .steps(maxVal / 0.001)
+ .min(minVal)
+ .max(maxVal)
+ .value(initControllerConfigNonRT.currentDeadzone);
WidgetPtr line = makeHBoxLayout().children({deadzoneLabel, deadzonBox});
vLayout.addChild(line);
@@ -258,4 +258,4 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller::zero_v
return {};
}
-} // namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::zero_velocity
+} // namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller::zero_velocity
diff --git a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ZeroVelocity.h b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ZeroVelocity.h
index 4210b119..547cb97e 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ZeroVelocity.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar6/joint_controller/ZeroVelocity.h
@@ -24,46 +24,41 @@
#pragma once
-#include <memory>
#include <chrono>
+#include <memory>
#include "ArmarXCore/util/CPPUtility/TripleBuffer.h"
#include <ArmarXCore/core/services/tasks/ThreadPool.h>
#include <ArmarXCore/observers/filters/AverageFilter.h>
-
#include <ArmarXGui/interface/RemoteGuiInterface.h>
-
-#include <RobotAPI/libraries/core/PIDController.h>
#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <RobotAPI/libraries/core/PIDController.h>
// #include <armarx/control/joint_controller/ControllerConfiguration.h>
#include <armarx/control/hardware_config/Config.h>
-
namespace devices::ethercat::sensor_actor_unit::armar6
{
class Device;
using DataPtr = std::shared_ptr<class Data>;
} // namespace devices::ethercat::sensor_actor_unit::armar6
-
-
namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller::zero_velocity
{
using ZeroVelocityControllerConfigurationPtr =
- std::shared_ptr<class ZeroVelocityControllerConfiguration>;
+ std::shared_ptr<class ZeroVelocityControllerConfiguration>;
class ZeroVelocityControllerConfiguration
{
public:
- ZeroVelocityControllerConfiguration() {};
- static ZeroVelocityControllerConfigurationPtr CreateConfigData(
- armarx::control::hardware_config::Config& config,
- bool limitless,
- float jointLimitLow,
- float jointLimitHigh);
+ ZeroVelocityControllerConfiguration(){};
+ static ZeroVelocityControllerConfigurationPtr
+ CreateConfigData(armarx::control::hardware_config::Config& config,
+ bool limitless,
+ float jointLimitLow,
+ float jointLimitHigh);
float feedforwardVelocityToCurrentFactor;
float currentDeadzone;
bool limitless = true;
@@ -102,11 +97,13 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller::zero_v
ZeroVelocityControllerConfigurationPtr ConfigData);
~ZeroVelocityController() noexcept(true);
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) final ;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) final;
armarx::ControlTargetBase* getControlTarget() override;
- armarx::StringVariantBaseMap publish(const armarx::DebugDrawerInterfacePrx& draw,
- const armarx::DebugObserverInterfacePrx& observer) const override;
+ armarx::StringVariantBaseMap
+ publish(const armarx::DebugDrawerInterfacePrx& draw,
+ const armarx::DebugObserverInterfacePrx& observer) const override;
protected:
void rtPreActivateController() override;
@@ -133,6 +130,5 @@ namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller::zero_v
// KITGripperBasisBoardPtr board;
// const std::string deviceName;
// size_t actorIndex = 0;
-
};
-} // devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::zero_velocity
+} // namespace devices::ethercat::sensor_actor_unit::armar6::joint_controller::zero_velocity
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/ControlTargets.cpp b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/ControlTargets.cpp
index 7e5c3b0e..75a7a75b 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/ControlTargets.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/ControlTargets.cpp
@@ -23,7 +23,6 @@
#include "ControlTargets.h"
-
namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller
{
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/ControlTargets.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/ControlTargets.h
index 57e060be..49fa8ea0 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/ControlTargets.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/ControlTargets.h
@@ -39,7 +39,6 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller
using VelocityControllerPtr = std::shared_ptr<class VelocityController>;
-
class VelocityControlTarget : public armarx::ControlTargetBase
{
public:
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/Data.cpp b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/Data.cpp
index ff2c71fe..c779a332 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/Data.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/Data.cpp
@@ -27,6 +27,7 @@
#include "Data.h"
#include <SimoxUtility/meta/enum/EnumNames.hpp>
+
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Config.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.h>
@@ -193,7 +194,6 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist
return sensorBoardData.absoluteEncoderHemisphereRightValue.getRaw();
}
-
float
Data::getAbsoluteEncoderRollTemperature() const
{
@@ -256,25 +256,29 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist
Data::setEstimatedTorqueValue(const Eigen::Vector3f& estimatedTorque)
{
sensorBoardData.estimatedTorqueRoll = estimatedTorque(SensorValue::Roll);
- sensorBoardData.estimatedTorqueHemisphereLeft = estimatedTorque(SensorValue::HemisphereLeft);
- sensorBoardData.estimatedTorqueHemisphereRight = estimatedTorque(SensorValue::HemisphereRight);
+ sensorBoardData.estimatedTorqueHemisphereLeft =
+ estimatedTorque(SensorValue::HemisphereLeft);
+ sensorBoardData.estimatedTorqueHemisphereRight =
+ estimatedTorque(SensorValue::HemisphereRight);
}
- float Data::getRelativeEncoderHemisphereLeftValue() const
+ float
+ Data::getRelativeEncoderHemisphereLeftValue() const
{
return elmosData.hemisphereLeft.getActualRelativePosition();
}
- float Data::getRelativeEncoderHemisphereRightValue() const
+ float
+ Data::getRelativeEncoderHemisphereRightValue() const
{
return elmosData.hemisphereRight.getActualRelativePosition();
}
- float Data::getRelativeEncoderRollValue() const
+ float
+ Data::getRelativeEncoderRollValue() const
{
return elmosData.roll.getActualRelativePosition();
}
-
} // namespace devices::ethercat::sensor_actor_unit::armar7_wrist
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/Device.cpp b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/Device.cpp
index c8e34669..ab4c1012 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/Device.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/Device.cpp
@@ -35,7 +35,6 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelperTemplates.h>
-
#include <armarx/control/joint_controller/ControllerConfiguration.h>
#include <armarx/control/joint_controller/Torque.h>
#include <armarx/control/joint_controller/VelocityManipulatingTorque.h>
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/Device.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/Device.h
index c7d0c833..10cce0b9 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/Device.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/Device.h
@@ -1,8 +1,8 @@
#pragma once
-#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
#include <RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h>
@@ -10,17 +10,17 @@
#include <armarx/control/ethercat/DeviceInterface.h>
#include <armarx/control/ethercat/SlaveIdentifier.h>
-#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.h>
-#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/force_torque/Device.h>
-#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/hemisphere/Device.h>
-#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/rotation/Device.h>
#include <devices/ethercat/common/elmo/gold/Slave.h>
#include <devices/ethercat/common/sanity_checking/AbsRelEncoderChecker.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/Data.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/SensorValue.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Velocity.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/forward_declarations.h>
+#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Slave.h>
+#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/force_torque/Device.h>
+#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/hemisphere/Device.h>
+#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/rotation/Device.h>
namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sub_device
{
@@ -120,7 +120,7 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist
void updateForceTorqueSensorValue(const sensor_board::ForceTorqueData& forceTorque,
SensorValue& sensorValue);
-
+
void updateElbowTorqueSensorValue(const sensor_board::ElbowTorqueData& elbowTorque,
SensorValue& sensorValue);
@@ -186,9 +186,11 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist
// joint_controller::EmergencyControllerPtr emergencyController;
// joint_controller::StopMovementControllerPtr stopMovementController;
-
- std::optional<common::sanity_checking::AbsRelEncoderChecker> absRelEncoderCheckerHemisphereLeft;
- std::optional<common::sanity_checking::AbsRelEncoderChecker> absRelEncoderCheckerHemisphereRight;
+
+ std::optional<common::sanity_checking::AbsRelEncoderChecker>
+ absRelEncoderCheckerHemisphereLeft;
+ std::optional<common::sanity_checking::AbsRelEncoderChecker>
+ absRelEncoderCheckerHemisphereRight;
std::optional<common::sanity_checking::AbsRelEncoderChecker> absRelEncoderCheckerRoll;
};
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/PerActuator.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/PerActuator.h
index 7ace37e2..ac66368a 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/PerActuator.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/PerActuator.h
@@ -30,7 +30,6 @@
#include <functional>
#include <type_traits>
-
namespace devices::ethercat::sensor_actor_unit::armar7_wrist
{
@@ -108,7 +107,6 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist
{
}
-
bool
all() const
{
@@ -121,7 +119,6 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist
return predicate(roll) and predicate(hemisphereLeft) and predicate(hemisphereRight);
}
-
void
forEach(std::function<void(T&)> fn)
{
@@ -138,6 +135,7 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist
fn(hemisphereLeft, other.hemisphereLeft);
fn(hemisphereRight, other.hemisphereRight);
}
+
template <class OtherT>
void
forEachZip(const PerActuator<OtherT>& other, std::function<void(T&, const OtherT&)> fn)
@@ -147,7 +145,6 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist
fn(hemisphereRight, other.hemisphereRight);
}
-
template <class ResultT>
PerActuator<ResultT>
map(std::function<ResultT(T&)> fn)
@@ -172,27 +169,26 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist
};
}
-
using PickFn = std::function<T&(PerActuator<T>&)>;
-
static T&
pickRoll(PerActuator<T>& values)
{
return values.roll;
}
+
static T&
pickHemisphereLeft(PerActuator<T>& values)
{
return values.hemisphereLeft;
}
+
static T&
pickHemisphereRight(PerActuator<T>& values)
{
return values.hemisphereRight;
}
-
template <class OtherT = T>
void
forEachWithPickFn(
@@ -202,7 +198,6 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist
fn(hemisphereLeft, PerActuator<OtherT>::pickHemisphereLeft);
fn(hemisphereRight, PerActuator<OtherT>::pickHemisphereRight);
}
-
};
} // namespace devices::ethercat::sensor_actor_unit::armar7_wrist
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/SensorValue.cpp b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/SensorValue.cpp
index a4a0d2c6..587fa034 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/SensorValue.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/SensorValue.cpp
@@ -177,34 +177,49 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist
// for temperature compensation
svi.addMemberVariable(&SensorValue::ft_temperatureFromResistance, "temperature");
- svi.addMemberVariable(&SensorValue::ft_supplyVoltage,
- "supplyVoltage")
- .setFieldNames({"voltage"});
- svi.addMemberVariable(&SensorValue::ft_supplyCurrent,
- "supplyCurrent")
- .setFieldNames({"current"});
- svi.addMemberVariable(&SensorValue::ft_overallResistance,
- "overallResistance")
- .setFieldNames({"resistance"});
+ svi.addMemberVariable(&SensorValue::ft_supplyVoltage, "supplyVoltage")
+ .setFieldNames({"voltage"});
+ svi.addMemberVariable(&SensorValue::ft_supplyCurrent, "supplyCurrent")
+ .setFieldNames({"current"});
+ svi.addMemberVariable(&SensorValue::ft_overallResistance, "overallResistance")
+ .setFieldNames({"resistance"});
svi.addMemberVariable(&SensorValue::ft_temperatureFromResistance,
"temperatureFromResistance")
- .setFieldNames({"temperature"});
+ .setFieldNames({"temperature"});
svi.addMemberVariable(&SensorValue::ft_forceTorqueValuesWithoutTempComp,
"ft_forceTorqueValuesWithoutTempComp")
- .setFieldNames({"ft_no_comp_fx", "ft_no_comp_fy", "ft_no_comp_fz", "ft_no_comp_tx", "ft_no_comp_ty", "ft_no_comp_tz"});
+ .setFieldNames({"ft_no_comp_fx",
+ "ft_no_comp_fy",
+ "ft_no_comp_fz",
+ "ft_no_comp_tx",
+ "ft_no_comp_ty",
+ "ft_no_comp_tz"});
svi.addMemberVariable(&SensorValue::ft_forceTorqueValuesTempCompensated,
"ft_forceTorqueValuesTempCompensated")
- .setFieldNames({"ft_compensated_fx", "ft_compensated_fy", "ft_compensated_fz", "ft_compensated_tx", "ft_compensated_ty", "ft_compensated_tz"});
-
- svi.addMemberVariable(&SensorValue::ft_unAmplifiedVoltage,
- "ft_unAmplifiedVoltage")
- .setFieldNames({"ft_adc_voltage_1", "ft_adc_voltage_2", "ft_adc_voltage_3", "ft_adc_voltage_4", "ft_adc_voltage_5", "ft_adc_voltage_6"});
-
- svi.addMemberVariable(&SensorValue::ft_rawForceTorque,
- "ft_ticks")
- .setFieldNames({"ft_ticks_1", "ft_ticks_2", "ft_ticks_3", "ft_ticks_4", "ft_ticks_5", "ft_ticks_6"});
+ .setFieldNames({"ft_compensated_fx",
+ "ft_compensated_fy",
+ "ft_compensated_fz",
+ "ft_compensated_tx",
+ "ft_compensated_ty",
+ "ft_compensated_tz"});
+
+ svi.addMemberVariable(&SensorValue::ft_unAmplifiedVoltage, "ft_unAmplifiedVoltage")
+ .setFieldNames({"ft_adc_voltage_1",
+ "ft_adc_voltage_2",
+ "ft_adc_voltage_3",
+ "ft_adc_voltage_4",
+ "ft_adc_voltage_5",
+ "ft_adc_voltage_6"});
+
+ svi.addMemberVariable(&SensorValue::ft_rawForceTorque, "ft_ticks")
+ .setFieldNames({"ft_ticks_1",
+ "ft_ticks_2",
+ "ft_ticks_3",
+ "ft_ticks_4",
+ "ft_ticks_5",
+ "ft_ticks_6"});
// commented out, because there is no point to separate this vector to two half. the six channels are
// coupled, the first three does not correspond to force.
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/SensorValue.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/SensorValue.h
index ed2b268a..16679170 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/SensorValue.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/SensorValue.h
@@ -32,6 +32,7 @@
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.h>
// #include <devices/ethercat/sensor_actor_unit/armar7_wrist/PerActuator.h>
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ElmoEmergency.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ElmoEmergency.h
index 13a77b8c..2b5f9338 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ElmoEmergency.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ElmoEmergency.h
@@ -14,7 +14,7 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::
virtual ~DataInterface() = default;
// read
- virtual std::string getName() = 0;
+ virtual std::string getName() = 0;
virtual float getMaxDefaultTargetCurrent() = 0;
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Position.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Position.h
index b776b086..570d0b0d 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Position.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Position.h
@@ -5,7 +5,6 @@
#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-
#include <armarx/control/joint_controller/ControllerConfiguration.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/interfaces.h>
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/StopMovement.cpp b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/StopMovement.cpp
index 32818da6..5ec27ace 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/StopMovement.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/StopMovement.cpp
@@ -3,10 +3,9 @@
namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::stop_movement
{
- Controller::Controller(
- DeviceControlModeSetterInterface* controlModeSetter,
- DataPtr dataPtr,
- float deceleration) :
+ Controller::Controller(DeviceControlModeSetterInterface* controlModeSetter,
+ DataPtr dataPtr,
+ float deceleration) :
controlModeSetter(controlModeSetter),
dataPtr(dataPtr),
deceleration(std::fabs(deceleration))
@@ -15,7 +14,7 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::
void
Controller::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& /*timeSinceLastIteration*/)
+ const IceUtil::Time& /*timeSinceLastIteration*/)
{
dataPtr->setTargetVelocity(0);
}
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/StopMovement.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/StopMovement.h
index 18ff7537..51e1a3e2 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/StopMovement.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/StopMovement.h
@@ -6,8 +6,8 @@
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/util/RtLogging.h>
-#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/interfaces.h>
#include <devices/ethercat/common/elmo/gold/Slave.h>
+#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/interfaces.h>
namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::stop_movement
{
@@ -33,8 +33,8 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::
public:
Controller(DeviceControlModeSetterInterface* controlModeSetter,
- DataPtr dataPtr,
- float deceleration = 6.0f);
+ DataPtr dataPtr,
+ float deceleration = 6.0f);
void rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration) override;
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Torque.cpp b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Torque.cpp
index 29951e5d..0f69a020 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Torque.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Torque.cpp
@@ -11,8 +11,8 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::
armarx::SensorDevice* sensorDevice,
DataPtr jointData,
armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData) :
- JointController(),
- target(),
+ JointController(),
+ target(),
controlModeSetter(controlModeSetter),
sensorDevice(sensorDevice),
torqueController(torqueConfigData)
@@ -26,7 +26,8 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::
{
if (target.isValid())
{
- auto sensorValue = sensorDevice->getSensorValue()->asA<armarx::SensorValue1DoFGravityTorque>();
+ auto sensorValue =
+ sensorDevice->getSensorValue()->asA<armarx::SensorValue1DoFGravityTorque>();
ARMARX_CHECK_EXPRESSION(sensorValue);
float gravity = sensorValue->gravityTorque;
@@ -57,7 +58,7 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::
actualPosition);
// ARMARX_INFO << VAROUT(current);
-
+
// send the current value to Elmo
@@ -89,6 +90,4 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::
// with brake
-
-
-} // namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
+} // namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::torque
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Torque.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Torque.h
index 72273cce..0c3b9c92 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Torque.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Torque.h
@@ -2,8 +2,8 @@
// armarx
-#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <armarx/control/joint_controller/Torque.h>
#include <armarx/control/rt_filters/AverageFilter.h>
@@ -69,4 +69,4 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::
};
-} // namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
+} // namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::torque
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Velocity copy.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Velocity copy.h
index 402dcabc..7907ba56 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Velocity copy.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Velocity copy.h
@@ -44,7 +44,6 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::
return config;
}
-
class DataInterface
{
public:
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Velocity.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Velocity.h
index 402dcabc..7907ba56 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Velocity.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Velocity.h
@@ -44,7 +44,6 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::
return config;
}
-
class DataInterface
{
public:
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroTorque.cpp b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroTorque.cpp
index 141d52c5..4f00cb76 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroTorque.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroTorque.cpp
@@ -25,30 +25,27 @@
#include "ZeroTorque.h"
-
-#include <devices/ethercat/sensor_actor_unit/armar7_wrist/Data.h>
-#include <devices/ethercat/sensor_actor_unit/armar7_wrist/Device.h>
-
#include "ArmarXGui/libraries/DefaultWidgetDescriptions/DefaultWidgetDescriptions.h"
#include "ArmarXGui/libraries/RemoteGui/WidgetBuilder/FloatWidgets.h"
#include "ArmarXGui/libraries/RemoteGui/WidgetBuilder/StringWidgets.h"
#include "ArmarXGui/libraries/VariantWidget/VariantWidget.h"
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-
#include <ArmarXCore/core/application/Application.h>
-#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
+#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-
+#include <devices/ethercat/sensor_actor_unit/armar7_wrist/Data.h>
+#include <devices/ethercat/sensor_actor_unit/armar7_wrist/Device.h>
namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::zero_torque
{
Controller::Controller(
- DeviceControlModeSetterInterface* controlModeSetter,
- armarx::SensorDevice* sensorDevice,
- DataPtr jointData,
- armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfigurationPtr torqueConfigData) :
+ DeviceControlModeSetterInterface* controlModeSetter,
+ armarx::SensorDevice* sensorDevice,
+ DataPtr jointData,
+ armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfigurationPtr
+ torqueConfigData) :
target(),
controlModeSetter(controlModeSetter),
sensorDevice(sensorDevice),
@@ -60,13 +57,14 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::
controllerConfigBufferPublishToRt.reinitAllBuffers(*torqueConfigData);
}
-
-
- void Controller::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ Controller::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (target.isValid())
{
- auto sensorValue = sensorDevice->getSensorValue()->asA<armarx::SensorValue1DoFGravityTorque>();
+ auto sensorValue =
+ sensorDevice->getSensorValue()->asA<armarx::SensorValue1DoFGravityTorque>();
ARMARX_CHECK_EXPRESSION(sensorValue);
float gravity = sensorValue->gravityTorque;
@@ -82,229 +80,247 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::
auto torqueControllerConfig = controllerConfigBufferPublishToRt.getUpToDateReadBuffer();
torqueController.setTorqueConfigData(torqueControllerConfig);
- float current = torqueController.update(sensorValuesTimestamp, timeSinceLastIteration, gravity, actualTorque, targetTorque, actualVelocity, actualPosition);
+ float current = torqueController.update(sensorValuesTimestamp,
+ timeSinceLastIteration,
+ gravity,
+ actualTorque,
+ targetTorque,
+ actualVelocity,
+ actualPosition);
// send the current value to Elmo
dataPtr->setTargetCurrent(current);
}
}
-
- armarx::ControlTargetBase* Controller::getControlTarget()
+ armarx::ControlTargetBase*
+ Controller::getControlTarget()
{
return ⌖
}
-
- void Controller::rtPreActivateController()
+ void
+ Controller::rtPreActivateController()
{
controlModeSetter->switchControlMode(common::elmo::gold::ElmoControlMode::CURRENT);
dataPtr->setTargetCurrent(0.0f);
torqueController.reset();
- armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfiguration torqueConfigData
- = torqueController.getTorqueConfigData();
-
+ armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfiguration
+ torqueConfigData = torqueController.getTorqueConfigData();
}
- armarx::StringVariantBaseMap Controller::publish(const armarx::DebugDrawerInterfacePrx& /*draw*/, const armarx::DebugObserverInterfacePrx& /*observer*/) const
+ armarx::StringVariantBaseMap
+ Controller::publish(const armarx::DebugDrawerInterfacePrx& /*draw*/,
+ const armarx::DebugObserverInterfacePrx& /*observer*/) const
{
if (!remoteGui)
{
- threadHandle = armarx::Application::getInstance()->getThreadPool()->runTask([this]
- {
- std::string guiTabName;
- while (!stopRequested)
+ threadHandle = armarx::Application::getInstance()->getThreadPool()->runTask(
+ [this]
{
- armarx::ManagedIceObjectPtr object;
- ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
- try
+ std::string guiTabName;
+ while (!stopRequested)
{
- object = armarx::ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
- ARMARX_CHECK_EXPRESSION(object);
- remoteGui = object->getProxy<armarx::RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
- if (!remoteGui)
+ armarx::ManagedIceObjectPtr object;
+ ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
+ try
{
- continue;
+ object =
+ armarx::ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
+ ARMARX_CHECK_EXPRESSION(object);
+ remoteGui = object->getProxy<armarx::RemoteGuiInterfacePrx>(
+ "RemoteGuiProvider", false, "", false);
+ if (!remoteGui)
+ {
+ continue;
+ }
+ ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
+ guiTabName = getParent().getDeviceName() + getControlMode();
+ break;
+ }
+ catch (...)
+ {
+ sleep(1);
}
- ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
- guiTabName = getParent().getDeviceName() + getControlMode();
- break;
- }
- catch (...)
- {
- sleep(1);
}
- }
- if (remoteGui)
- {
- ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
- using namespace armarx::RemoteGui;
+ if (remoteGui)
+ {
+ ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
+ using namespace armarx::RemoteGui;
- auto vLayout = makeVBoxLayout();
+ auto vLayout = makeVBoxLayout();
- {
- WidgetPtr KpLabel = makeTextLabel("Kp: ");
- const double rangeVal = 20.;
- double minVal = 0.;
- double maxVal = 0.;
- if(initialTorqueConfigData.pid_proportional_gain > 0)
{
- minVal = 0.;
- maxVal = rangeVal;
-
- } else {
- minVal = -rangeVal;
- maxVal = 0.;
+ WidgetPtr KpLabel = makeTextLabel("Kp: ");
+ const double rangeVal = 20.;
+ double minVal = 0.;
+ double maxVal = 0.;
+ if (initialTorqueConfigData.pid_proportional_gain > 0)
+ {
+ minVal = 0.;
+ maxVal = rangeVal;
+ }
+ else
+ {
+ minVal = -rangeVal;
+ maxVal = 0.;
+ }
+ auto KpSlider =
+ makeFloatSpinBox("KpSlider")
+ .steps(rangeVal / 0.0001)
+ .min(minVal)
+ .max(maxVal)
+ .value(initialTorqueConfigData.pid_proportional_gain);
+
+ WidgetPtr line = makeHBoxLayout().children({KpLabel, KpSlider});
+ vLayout.addChild(line);
}
- auto KpSlider
- = makeFloatSpinBox("KpSlider")
- .steps(rangeVal/0.0001)
- .min(minVal).max(maxVal)
- .value(initialTorqueConfigData.pid_proportional_gain);
-
- WidgetPtr line = makeHBoxLayout()
- .children({KpLabel, KpSlider});
- vLayout.addChild(line);
- }
- {
- WidgetPtr KiLabel = makeTextLabel("Ki: ");
- const double rangeVal = 20.;
- double minVal = 0.;
- double maxVal = 0.;
- if(initialTorqueConfigData.pid_proportional_gain > 0)
{
- minVal = 0.;
- maxVal = rangeVal;
-
- } else {
- minVal = -rangeVal;
- maxVal = 0.;
- }
- auto KiSlider
- = makeFloatSpinBox("KiSlider")
- .steps(rangeVal/0.001)
- .min(minVal).max(maxVal)
+ WidgetPtr KiLabel = makeTextLabel("Ki: ");
+ const double rangeVal = 20.;
+ double minVal = 0.;
+ double maxVal = 0.;
+ if (initialTorqueConfigData.pid_proportional_gain > 0)
+ {
+ minVal = 0.;
+ maxVal = rangeVal;
+ }
+ else
+ {
+ minVal = -rangeVal;
+ maxVal = 0.;
+ }
+ auto KiSlider = makeFloatSpinBox("KiSlider")
+ .steps(rangeVal / 0.001)
+ .min(minVal)
+ .max(maxVal)
.value(initialTorqueConfigData.pid_integral_gain);
- WidgetPtr line = makeHBoxLayout()
- .children({KiLabel, KiSlider});
- vLayout.addChild(line);
- }
+ WidgetPtr line = makeHBoxLayout().children({KiLabel, KiSlider});
+ vLayout.addChild(line);
+ }
- {
- WidgetPtr KdLabel = makeTextLabel("Kd: ");
- const double rangeVal = 20.;
- double minVal = 0.;
- double maxVal = 0.;
- if(initialTorqueConfigData.pid_derivative_gain > 0)
{
- minVal = 0.;
- maxVal = rangeVal;
-
- } else {
- minVal = -rangeVal;
- maxVal = 0.;
- }
- auto KdSlider
- = makeFloatSpinBox("KdSlider")
- .steps(rangeVal/0.001)
- .min(minVal).max(maxVal)
+ WidgetPtr KdLabel = makeTextLabel("Kd: ");
+ const double rangeVal = 20.;
+ double minVal = 0.;
+ double maxVal = 0.;
+ if (initialTorqueConfigData.pid_derivative_gain > 0)
+ {
+ minVal = 0.;
+ maxVal = rangeVal;
+ }
+ else
+ {
+ minVal = -rangeVal;
+ maxVal = 0.;
+ }
+ auto KdSlider = makeFloatSpinBox("KdSlider")
+ .steps(rangeVal / 0.001)
+ .min(minVal)
+ .max(maxVal)
.value(initialTorqueConfigData.pid_derivative_gain);
- WidgetPtr line = makeHBoxLayout()
- .children({KdLabel, KdSlider});
- vLayout.addChild(line);
- }
+ WidgetPtr line = makeHBoxLayout().children({KdLabel, KdSlider});
+ vLayout.addChild(line);
+ }
- vLayout.addChild(makeHBoxLayout().children({
- makeTextLabel("Max Acc: "),
- makeFloatSpinBox("MaxAcc")
- .minmax(0, 20.)
- .steps(1000)
- .value(initialTorqueConfigData.maxAcceleration)}));
-
- vLayout.addChild(makeHBoxLayout().children({
- makeTextLabel("JL Margin: "),
- makeFloatSpinBox("PushBackMargin")
- .minmax(0.0, 50.)
- .steps(1000)
- .value(initialTorqueConfigData.pushbackMargin)}));
-
- vLayout.addChild(makeHBoxLayout().children({
- makeTextLabel("JL Torque: "),
- makeFloatSpinBox("PushBackTorque")
- .minmax(0, 1000.)
- .steps(1000)
- .value(initialTorqueConfigData.pushBackTorque)}));
+ vLayout.addChild(makeHBoxLayout().children(
+ {makeTextLabel("Max Acc: "),
+ makeFloatSpinBox("MaxAcc").minmax(0, 20.).steps(1000).value(
+ initialTorqueConfigData.maxAcceleration)}));
+
+ vLayout.addChild(makeHBoxLayout().children(
+ {makeTextLabel("JL Margin: "),
+ makeFloatSpinBox("PushBackMargin")
+ .minmax(0.0, 50.)
+ .steps(1000)
+ .value(initialTorqueConfigData.pushbackMargin)}));
+
+ vLayout.addChild(makeHBoxLayout().children(
+ {makeTextLabel("JL Torque: "),
+ makeFloatSpinBox("PushBackTorque")
+ .minmax(0, 1000.)
+ .steps(1000)
+ .value(initialTorqueConfigData.pushBackTorque)}));
- {
- WidgetPtr DecayLabel = makeTextLabel("Decay: ");
- auto DecaySlider
- = makeFloatSpinBox("DecaySlider")
- .steps(1000)
- .min(0.).max(1.)
- .value(initialTorqueConfigData.decay);
- WidgetPtr DecayLabelValue = makeLabel("DecayValue").value(std::to_string(initialTorqueConfigData.decay));
- WidgetPtr line = makeHBoxLayout()
- .children({DecayLabel, DecaySlider, DecayLabelValue});
-
- vLayout.addChild(line);
- vLayout.addChild(new VSpacer);
- }
+ {
+ WidgetPtr DecayLabel = makeTextLabel("Decay: ");
+ auto DecaySlider = makeFloatSpinBox("DecaySlider")
+ .steps(1000)
+ .min(0.)
+ .max(1.)
+ .value(initialTorqueConfigData.decay);
+ WidgetPtr DecayLabelValue =
+ makeLabel("DecayValue")
+ .value(std::to_string(initialTorqueConfigData.decay));
+ WidgetPtr line = makeHBoxLayout().children(
+ {DecayLabel, DecaySlider, DecayLabelValue});
+
+ vLayout.addChild(line);
+ vLayout.addChild(new VSpacer);
+ }
- {
- WidgetPtr DeadzoneLabel = makeTextLabel("Deadzone: ");
- auto DeadzoneSlider
- = makeFloatSpinBox("DeadzoneSlider")
- .steps(1000)
- .min(0.).max(10.)
- .value(initialTorqueConfigData.deadZone);
- WidgetPtr DeadzoneLabelValue = makeLabel("DeadzoneValue").value(std::to_string(initialTorqueConfigData.deadZone));
- WidgetPtr line = makeHBoxLayout()
- .children({DeadzoneLabel, DeadzoneSlider, DeadzoneLabelValue});
-
- vLayout.addChild(line);
- vLayout.addChild(new VSpacer);
- }
+ {
+ WidgetPtr DeadzoneLabel = makeTextLabel("Deadzone: ");
+ auto DeadzoneSlider = makeFloatSpinBox("DeadzoneSlider")
+ .steps(1000)
+ .min(0.)
+ .max(10.)
+ .value(initialTorqueConfigData.deadZone);
+ WidgetPtr DeadzoneLabelValue =
+ makeLabel("DeadzoneValue")
+ .value(std::to_string(initialTorqueConfigData.deadZone));
+ WidgetPtr line = makeHBoxLayout().children(
+ {DeadzoneLabel, DeadzoneSlider, DeadzoneLabelValue});
+
+ vLayout.addChild(line);
+ vLayout.addChild(new VSpacer);
+ }
- WidgetPtr groupBox = makeGroupBox("GroupBox")
- .label("Group")
- .child(vLayout);
+ WidgetPtr groupBox = makeGroupBox("GroupBox").label("Group").child(vLayout);
- remoteGui->createTab(guiTabName, groupBox);
+ remoteGui->createTab(guiTabName, groupBox);
- while (!stopRequested)
- {
- armarx::RemoteGui::TabProxy tab(remoteGui, guiTabName);
- tab.receiveUpdates();
+ while (!stopRequested)
+ {
+ armarx::RemoteGui::TabProxy tab(remoteGui, guiTabName);
+ tab.receiveUpdates();
- auto& torqueConfigData = controllerConfigBufferPublishToRt.getWriteBuffer();
+ auto& torqueConfigData =
+ controllerConfigBufferPublishToRt.getWriteBuffer();
- torqueConfigData.pid_proportional_gain = tab.getValue<float>("KpSlider").get();
- torqueConfigData.pid_integral_gain = tab.getValue<float>("KiSlider").get();
- torqueConfigData.pid_derivative_gain = tab.getValue<float>("KdSlider").get();
- torqueConfigData.maxAcceleration = tab.getValue<float>("MaxAcc").get();
- torqueConfigData.decay = tab.getValue<float>("DecaySlider").get();
- torqueConfigData.deadZone = tab.getValue<float>("DeadzoneSlider").get();
- torqueConfigData.pushBackTorque = tab.getValue<float>("PushBackTorque").get();
- torqueConfigData.pushbackMargin = tab.getValue<float>("PushBackMargin").get();
+ torqueConfigData.pid_proportional_gain =
+ tab.getValue<float>("KpSlider").get();
+ torqueConfigData.pid_integral_gain =
+ tab.getValue<float>("KiSlider").get();
+ torqueConfigData.pid_derivative_gain =
+ tab.getValue<float>("KdSlider").get();
+ torqueConfigData.maxAcceleration = tab.getValue<float>("MaxAcc").get();
+ torqueConfigData.decay = tab.getValue<float>("DecaySlider").get();
+ torqueConfigData.deadZone = tab.getValue<float>("DeadzoneSlider").get();
+ torqueConfigData.pushBackTorque =
+ tab.getValue<float>("PushBackTorque").get();
+ torqueConfigData.pushbackMargin =
+ tab.getValue<float>("PushBackMargin").get();
- controllerConfigBufferPublishToRt.commitWrite();
+ controllerConfigBufferPublishToRt.commitWrite();
- tab.getValue<std::string>("DecayValue").set(std::to_string(torqueConfigData.decay));
- tab.getValue<std::string>("DeadzoneValue").set(std::to_string(torqueConfigData.deadZone));
-
+ tab.getValue<std::string>("DecayValue")
+ .set(std::to_string(torqueConfigData.decay));
+ tab.getValue<std::string>("DeadzoneValue")
+ .set(std::to_string(torqueConfigData.deadZone));
- tab.sendUpdates();
- usleep(100000);
- }
- }
+ tab.sendUpdates();
- });
+ usleep(100000);
+ }
+ }
+ });
}
return {};
// return {{"lastTargetVelocity", new armarx::Variant(lastTargetVelocity.load())},
@@ -324,4 +340,4 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::
// {"desiredPWM", new armarx::Variant(targetPWM.load())}
// };
}
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::zero_torque
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroTorque.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroTorque.h
index 9a71b8c0..6e731abe 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroTorque.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroTorque.h
@@ -2,23 +2,23 @@
// armarx
+#include <string>
+
#include "ArmarXCore/util/CPPUtility/TripleBuffer.h"
+#include <ArmarXCore/core/services/tasks/ThreadPool.h>
+#include <ArmarXGui/interface/RemoteGuiInterface.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <armarx/control/joint_controller/VelocityManipulatingTorque.h>
-#include <ArmarXCore/core/services/tasks/ThreadPool.h>
-#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/interfaces.h>
-#include <string>
namespace devices::ethercat::sensor_actor_unit::armar7_wrist
{
class Device;
using DataPtr = std::shared_ptr<class Data>;
-}
-
+} // namespace devices::ethercat::sensor_actor_unit::armar7_wrist
namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::zero_torque
{
@@ -42,42 +42,46 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::
{
public:
-
- Controller(DeviceControlModeSetterInterface* controlModeSetter,
- armarx::SensorDevice* sensorDevice,
- DataPtr jointData,
- armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfigurationPtr torqueConfigData);
+ Controller(
+ DeviceControlModeSetterInterface* controlModeSetter,
+ armarx::SensorDevice* sensorDevice,
+ DataPtr jointData,
+ armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfigurationPtr
+ torqueConfigData);
~Controller() noexcept(true){};
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) final;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) final;
armarx::ControlTargetBase* getControlTarget() override;
// JointController interface
- armarx::StringVariantBaseMap publish(const armarx::DebugDrawerInterfacePrx& draw, const armarx::DebugObserverInterfacePrx& observer) const final;
+ armarx::StringVariantBaseMap
+ publish(const armarx::DebugDrawerInterfacePrx& draw,
+ const armarx::DebugObserverInterfacePrx& observer) const final;
protected:
-
void rtPreActivateController() override;
mutable armarx::RemoteGuiInterfacePrx remoteGui;
mutable armarx::ThreadPool::Handle threadHandle;
bool stopRequested = false;
-
-
private:
-
armarx::ControlTarget1DoFActuatorZeroTorque target;
DeviceControlModeSetterInterface* controlModeSetter;
armarx::SensorDevice* sensorDevice;
DataPtr dataPtr;
- mutable armarx::control::joint_controller::VelocityManipulatingTorqueController torqueController;
+ mutable armarx::control::joint_controller::VelocityManipulatingTorqueController
+ torqueController;
- mutable armarx::TripleBuffer<armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfiguration> controllerConfigBufferPublishToRt;
+ mutable armarx::TripleBuffer<
+ armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfiguration>
+ controllerConfigBufferPublishToRt;
- const armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfiguration initialTorqueConfigData;
+ const armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfiguration
+ initialTorqueConfigData;
};
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::zero_torque
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroVelocity.cpp b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroVelocity.cpp
index 6620fdd2..433962c1 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroVelocity.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroVelocity.cpp
@@ -29,10 +29,8 @@
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroVelocity.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroVelocity.h
index 7a168b48..21d1d843 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroVelocity.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroVelocity.h
@@ -30,9 +30,7 @@
#include "ArmarXCore/util/CPPUtility/TripleBuffer.h"
#include <ArmarXCore/core/services/tasks/ThreadPool.h>
#include <ArmarXCore/observers/filters/AverageFilter.h>
-
#include <ArmarXGui/interface/RemoteGuiInterface.h>
-
#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/common.cpp b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/common.cpp
index a54c212d..e4c4a4d4 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/common.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/common.cpp
@@ -21,7 +21,6 @@
#include "common.h"
-
namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller
{
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/common.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/common.h
index c15a12c8..ccce5bbc 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/common.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/common.h
@@ -25,19 +25,15 @@
// #include <devices/ethercat/sensor_actor_unit/armar7_wrist/Data.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/PerActuator.h>
-
namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller
{
using ElmoData = common::elmo::gold::Data;
-
struct Common
{
- static void
- stop(common::elmo::gold::Data& elmo);
+ static void stop(common::elmo::gold::Data& elmo);
- static void
- stopAll(PerActuator<ElmoData>& elmos);
+ static void stopAll(PerActuator<ElmoData>& elmos);
};
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/forward_declarations.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/forward_declarations.h
index b8ef4cc4..81f3c8da 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/forward_declarations.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/forward_declarations.h
@@ -25,7 +25,6 @@
#include <memory>
-
namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller
{
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/interfaces.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/interfaces.h
index d5a2c2cf..db19c23f 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/interfaces.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/interfaces.h
@@ -22,6 +22,7 @@
#pragma once
#include "RobotAPI/interface/units/KinematicUnitInterface.h"
+
#include <devices/ethercat/common/elmo/gold/Slave.h>
namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller
@@ -32,7 +33,7 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller
public:
virtual ~DeviceControlModeSetterInterface() = default;
virtual void
- switchControlMode( ::devices::ethercat::common::elmo::gold::ElmoControlMode) = 0;
+ switchControlMode(::devices::ethercat::common::elmo::gold::ElmoControlMode) = 0;
};
} // namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Config.cpp b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Config.cpp
index ab7f2aa5..0eb03f44 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Config.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Config.cpp
@@ -15,11 +15,14 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sensor_board
voltageRef(hwConfig.getFloat("forceTorque.voltage_ref")),
invertSign(hwConfig.getBool("forceTorque.invert_sign")),
conversionMatrix(hwConfig.getMatrix<float, 6, 6>("forceTorque.calibrationMatrix")),
- temperatureCompFactor(hwConfig.getMatrix<float, 6, 1>("forceTorque.temperatureComp.tempToForceTorqueFactor")),
- temperatureCompOffset(hwConfig.getMatrix<float, 6, 1>("forceTorque.temperatureComp.tempToForceTorqueOffset")),
+ temperatureCompFactor(
+ hwConfig.getMatrix<float, 6, 1>("forceTorque.temperatureComp.tempToForceTorqueFactor")),
+ temperatureCompOffset(
+ hwConfig.getMatrix<float, 6, 1>("forceTorque.temperatureComp.tempToForceTorqueOffset")),
supplyVoltageShunt(hwConfig.getLinearConfig("forceTorque.supplyVoltageShunt")),
supplyCurrentShunt(hwConfig.getLinearConfig("forceTorque.supplyCurrentShunt")),
- resistanceToTemperature(hwConfig.getLinearConfig("forceTorque.temperatureComp.resistanceToTemperature"))
+ resistanceToTemperature(
+ hwConfig.getLinearConfig("forceTorque.temperatureComp.resistanceToTemperature"))
{
}
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.cpp b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.cpp
index 476e8b63..36ff8b89 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.cpp
@@ -29,7 +29,6 @@
#include <SimoxUtility/meta/enum/EnumNames.hpp>
#include <ArmarXCore/core/logging/Logging.h>
-
#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Config.h>
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.h
index 3e14ad33..94f6813d 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.h
@@ -92,18 +92,18 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sensor_board
std::int32_t supplyVoltageShunt;
std::int32_t supplyCurrentShunt;
- LinearConvertedValue<std::int32_t> supplyVoltage; // in Volt
- LinearConvertedValue<std::int32_t> supplyCurrent; // in Amp
- LinearConvertedValue<float> temperatureFromResistance; // in centigrade
- float overallResistance = 0.0f; // in Ohm
+ LinearConvertedValue<std::int32_t> supplyVoltage; // in Volt
+ LinearConvertedValue<std::int32_t> supplyCurrent; // in Amp
+ LinearConvertedValue<float> temperatureFromResistance; // in centigrade
+ float overallResistance = 0.0f; // in Ohm
std::array<float, sensor_board::ForceTorqueConfig::NUM_TEMPERATURES> adcTemperatures;
Eigen::Vector6i channelOrder;
Eigen::Vector6i offset;
float ticksToUnAmplifiedVoltFactor;
- Eigen::Matrix6f conversionMatrix; // corresponds to amplified voltage
- float pgaGain; // to amplify the voltage
+ Eigen::Matrix6f conversionMatrix; // corresponds to amplified voltage
+ float pgaGain; // to amplify the voltage
bool invertSign = false;
Eigen::Vector6f temperatureCompFactor;
@@ -157,8 +157,8 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sensor_board
private:
const float ticksToUnAmplifiedVoltFactor;
const float unAmplifiedVoltageToTorqueFactor;
- const Vecf voltageFactor;
- const Vecf voltageOffset;
+ const Vecf voltageFactor;
+ const Vecf voltageOffset;
const float torqueFactor;
const float torqueOffset;
@@ -191,9 +191,12 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sensor_board
std::uint16_t mainUpdateRate;
float boardTemperatureDegreeCelsius;
- armarx::control::ethercat::ModularConvertedValue<std::uint32_t, std::int64_t> absoluteEncoderRollValue;
- armarx::control::ethercat::ModularConvertedValue<std::uint16_t, std::int32_t> absoluteEncoderHemisphereLeftValue;
- armarx::control::ethercat::ModularConvertedValue<std::uint16_t, std::int32_t> absoluteEncoderHemisphereRightValue;
+ armarx::control::ethercat::ModularConvertedValue<std::uint32_t, std::int64_t>
+ absoluteEncoderRollValue;
+ armarx::control::ethercat::ModularConvertedValue<std::uint16_t, std::int32_t>
+ absoluteEncoderHemisphereLeftValue;
+ armarx::control::ethercat::ModularConvertedValue<std::uint16_t, std::int32_t>
+ absoluteEncoderHemisphereRightValue;
// absolute encoder velocity
float absoluteEncoderVelocityHemisphereLeft = 0.0f;
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Slave.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Slave.h
index 68ab635d..9ae4916a 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Slave.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Slave.h
@@ -29,9 +29,9 @@
#include <armarx/control/ethercat/SlaveInterface.h>
+#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Config.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/ErrorDecoder.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/SlaveIO.h>
-#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Config.h>
namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sensor_board
{
@@ -118,6 +118,7 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sensor_board
std::uint8_t Temperature_Limit;
};
+
CoE coe;
// Method to bundle CoE writes.
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/SlaveIO.cpp b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/SlaveIO.cpp
index b40413e9..e7f1e674 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/SlaveIO.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/SlaveIO.cpp
@@ -26,7 +26,6 @@
#include "SlaveIO.h"
-
namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sensor_board
{
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/DeviceControlModeSetter.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/DeviceControlModeSetter.h
index 27b9caa0..b3d01814 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/DeviceControlModeSetter.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/DeviceControlModeSetter.h
@@ -23,8 +23,8 @@
#include <armarx/control/ethercat/SlaveInterface.h>
-#include <devices/ethercat/common/elmo/gold/Slave.h>
#include <devices/ethercat/common/elmo/gold/Data.h>
+#include <devices/ethercat/common/elmo/gold/Slave.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/interfaces.h>
namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sub_device
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/hemisphere/ControllerData.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/hemisphere/ControllerData.h
index fa25ff77..2b17f0e2 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/hemisphere/ControllerData.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/hemisphere/ControllerData.h
@@ -131,7 +131,6 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sub_device::hemisp
return elmoDataPtr->getActualRelativePosition();
}
-
float
getActualAcceleration() override
{
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/hemisphere/Device.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/hemisphere/Device.h
index 6d61d48c..c8e52365 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/hemisphere/Device.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/hemisphere/Device.h
@@ -9,14 +9,13 @@
#include <armarx/control/joint_controller/Torque.h>
#include <armarx/control/joint_controller/VelocityManipulatingTorque.h>
+#include <devices/ethercat/common/elmo/gold/Data.h>
+#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroVelocity.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/DeviceControlModeSetter.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/hemisphere/ControllerData.h>
-#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroVelocity.h>
-#include <devices/ethercat/common/elmo/gold/Data.h>
// #include <devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.h>
-#include <devices/ethercat/sensor_actor_unit/armar7de/Data.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroVelocity.h>
-
+#include <devices/ethercat/sensor_actor_unit/armar7de/Data.h>
namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller
{
@@ -143,7 +142,7 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sub_device::hemisp
void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration) override;
- void rtSetEstimatedTorqueValues(const float estimatedTorqueValue);
+ void rtSetEstimatedTorqueValues(const float estimatedTorqueValue);
private:
void updateSensorValueStruct();
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/rotation/ControllerData.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/rotation/ControllerData.h
index ed5262a3..ce738de4 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/rotation/ControllerData.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/rotation/ControllerData.h
@@ -29,8 +29,8 @@
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/StopMovement.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Torque.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/Velocity.h>
-#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroVelocity.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroTorque.h>
+#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroVelocity.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sensor_board/Data.h>
namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sub_device::rotation
@@ -104,12 +104,14 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sub_device::rotati
return getActualVelocity();
}
- float getActualAcceleration() override
+ float
+ getActualAcceleration() override
{
return elmoDataPtr->getActualAcceleration();
}
- float getActualRelativePosition() override
+ float
+ getActualRelativePosition() override
{
return elmoDataPtr->getActualRelativePosition();
}
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/rotation/Device.h b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/rotation/Device.h
index 5177b77e..53e8cd2b 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/rotation/Device.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/rotation/Device.h
@@ -7,14 +7,12 @@
#include <armarx/control/ethercat/SlaveIdentifier.h>
#include <armarx/control/joint_controller/VelocityManipulatingTorque.h>
+#include <devices/ethercat/sensor_actor_unit/armar7_wrist/Data.h>
+#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroVelocity.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/DeviceControlModeSetter.h>
#include <devices/ethercat/sensor_actor_unit/armar7_wrist/sub_device/rotation/ControllerData.h>
#include <devices/ethercat/sensor_actor_unit/armar7de/Data.h>
#include <devices/ethercat/sensor_actor_unit/armar7de/Device.h>
-#include <devices/ethercat/sensor_actor_unit/armar7_wrist/joint_controller/ZeroVelocity.h>
-
-#include <devices/ethercat/sensor_actor_unit/armar7_wrist/Data.h>
-#include <devices/ethercat/sensor_actor_unit/armar7de/Data.h>
namespace devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller
{
@@ -83,8 +81,10 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sub_device::rotati
joint_controller::position::ConfigPtr position;
joint_controller::velocity::ConfigPtr velocity;
armarx::control::joint_controller::TorqueControllerConfigurationPtr torque;
- armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfigurationPtr zeroTorque;
- devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::zero_velocity::ZeroVelocityControllerConfigurationPtr zeroVelocity;
+ armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfigurationPtr
+ zeroTorque;
+ devices::ethercat::sensor_actor_unit::armar7_wrist::joint_controller::zero_velocity::
+ ZeroVelocityControllerConfigurationPtr zeroVelocity;
};
class Device :
@@ -146,7 +146,7 @@ namespace devices::ethercat::sensor_actor_unit::armar7_wrist::sub_device::rotati
void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration) override;
- void rtSetEstimatedTorqueValues(const float estimatedTorqueValue);
+ void rtSetEstimatedTorqueValues(const float estimatedTorqueValue);
private:
void updateSensorValueStruct();
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7de/Data.h b/source/devices/ethercat/sensor_actor_unit/armar7de/Data.h
index cc09872b..9015aba5 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7de/Data.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7de/Data.h
@@ -27,10 +27,8 @@
#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueIMU.h>
-
#include <armarx/control/ethercat/DataInterface.h>
#include <armarx/control/rt_filters/AverageFilter.h>
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7de/Device.cpp b/source/devices/ethercat/sensor_actor_unit/armar7de/Device.cpp
index 6c3a9d18..66d0e211 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7de/Device.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7de/Device.cpp
@@ -30,7 +30,6 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
#include <ArmarXCore/core/util/StringHelperTemplates.h>
-
#include <armarx/control/ethercat/ErrorReporting.h>
#include <armarx/control/joint_controller/ControllerConfiguration.h>
#include <armarx/control/joint_controller/Torque.h>
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7de/Device.h b/source/devices/ethercat/sensor_actor_unit/armar7de/Device.h
index 9bc1be12..caa93e3a 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7de/Device.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7de/Device.h
@@ -33,8 +33,8 @@
#include <armarx/control/ethercat/DeviceInterface.h>
#include <armarx/control/ethercat/SlaveIdentifier.h>
-#include <devices/ethercat/common/sanity_checking/AbsRelEncoderChecker.h>
#include <devices/ethercat/common/elmo/gold/Config.h>
+#include <devices/ethercat/common/sanity_checking/AbsRelEncoderChecker.h>
#include <devices/ethercat/common/sanity_checking/Config.h>
#include <devices/ethercat/common/sensor_board/armar7de/Config.h>
#include <devices/ethercat/common/sensor_board/armar7de/Slave.h>
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/ActiveImpedance.cpp b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/ActiveImpedance.cpp
index 17aa2987..141f919a 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/ActiveImpedance.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/ActiveImpedance.cpp
@@ -1,19 +1,15 @@
#include "ActiveImpedance.h"
-
#include <devices/ethercat/sensor_actor_unit/armar7de/Data.h>
#include <devices/ethercat/sensor_actor_unit/armar7de/Device.h>
-
namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
{
- ActiveImpedanceController::ActiveImpedanceController(Device* joint,
- DataPtr jointData,
- armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData) : JointController(),
- target(),
- joint(joint),
- pid(),
- targetFilter(100)
+ ActiveImpedanceController::ActiveImpedanceController(
+ Device* joint,
+ DataPtr jointData,
+ armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData) :
+ JointController(), target(), joint(joint), pid(), targetFilter(100)
{
dataPtr = jointData;
@@ -32,32 +28,35 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
// coefficients for Lowpass FIR-Filter
filter.setImpulseResponse(
- {
- 0.0073f, 0.0011f, 0.0012f, 0.0013f, 0.0014f, 0.0015f, 0.0016f, 0.0017f, 0.0018f, 0.0019f, 0.0021f, 0.0022f, 0.0023f,
- 0.0024f, 0.0026f, 0.0027f, 0.0028f, 0.0029f, 0.0031f, 0.0032f, 0.0034f, 0.0035f, 0.0036f, 0.0038f, 0.0039f, 0.0041f,
- 0.0043f, 0.0044f, 0.0046f, 0.0047f, 0.0049f, 0.0050f, 0.0052f, 0.0054f, 0.0055f, 0.0057f, 0.0058f, 0.0060f, 0.0062f,
- 0.0063f, 0.0065f, 0.0066f, 0.0068f, 0.0070f, 0.0071f, 0.0073f, 0.0074f, 0.0076f, 0.0077f, 0.0078f, 0.0080f, 0.0081f,
- 0.0083f, 0.0084f, 0.0085f, 0.0087f, 0.0088f, 0.0089f, 0.0090f, 0.0091f, 0.0092f, 0.0093f, 0.0094f, 0.0095f, 0.0096f,
- 0.0097f, 0.0098f, 0.0098f, 0.0099f, 0.0100f, 0.0100f, 0.0101f, 0.0101f, 0.0102f, 0.0102f, 0.0102f, 0.0103f, 0.0103f,
- 0.0103f, 0.0103f, 0.0103f, 0.0103f, 0.0103f, 0.0103f, 0.0102f, 0.0102f, 0.0102f, 0.0101f, 0.0101f, 0.0100f, 0.0100f,
- 0.0099f, 0.0098f, 0.0098f, 0.0097f, 0.0096f, 0.0095f, 0.0094f, 0.0093f, 0.0092f, 0.0091f, 0.0090f, 0.0089f, 0.0088f,
- 0.0087f, 0.0085f, 0.0084f, 0.0083f, 0.0081f, 0.0080f, 0.0078f, 0.0077f, 0.0076f, 0.0074f, 0.0073f, 0.0071f, 0.0070f,
- 0.0068f, 0.0066f, 0.0065f, 0.0063f, 0.0062f, 0.0060f, 0.0058f, 0.0057f, 0.0055f, 0.0054f, 0.0052f, 0.0050f, 0.0049f,
- 0.0047f, 0.0046f, 0.0044f, 0.0043f, 0.0041f, 0.0039f, 0.0038f, 0.0036f, 0.0035f, 0.0034f, 0.0032f, 0.0031f, 0.0029f,
- 0.0028f, 0.0027f, 0.0026f, 0.0024f, 0.0023f, 0.0022f, 0.0021f, 0.0019f, 0.0018f, 0.0017f, 0.0016f, 0.0015f, 0.0014f,
- 0.0013f, 0.0012f, 0.0011f, 0.0073f
- });
-
- filterVel.setImpulseResponse(
- {
- 0.1744f, 0.0205f, 0.0215f, 0.0225f, 0.0235f, 0.0244f, 0.0252f, 0.0259f, 0.0265f, 0.0270f, 0.0274f, 0.0276f, 0.0278f,
- 0.0282f, 0.0282f, 0.0278f, 0.0276f, 0.0274f, 0.0270f, 0.0265f, 0.0259f, 0.0252f, 0.0244f, 0.0235f, 0.0225f, 0.0215f,
- 0.0205f, 0.1744f
- });
-
+ {0.0073f, 0.0011f, 0.0012f, 0.0013f, 0.0014f, 0.0015f, 0.0016f, 0.0017f, 0.0018f,
+ 0.0019f, 0.0021f, 0.0022f, 0.0023f, 0.0024f, 0.0026f, 0.0027f, 0.0028f, 0.0029f,
+ 0.0031f, 0.0032f, 0.0034f, 0.0035f, 0.0036f, 0.0038f, 0.0039f, 0.0041f, 0.0043f,
+ 0.0044f, 0.0046f, 0.0047f, 0.0049f, 0.0050f, 0.0052f, 0.0054f, 0.0055f, 0.0057f,
+ 0.0058f, 0.0060f, 0.0062f, 0.0063f, 0.0065f, 0.0066f, 0.0068f, 0.0070f, 0.0071f,
+ 0.0073f, 0.0074f, 0.0076f, 0.0077f, 0.0078f, 0.0080f, 0.0081f, 0.0083f, 0.0084f,
+ 0.0085f, 0.0087f, 0.0088f, 0.0089f, 0.0090f, 0.0091f, 0.0092f, 0.0093f, 0.0094f,
+ 0.0095f, 0.0096f, 0.0097f, 0.0098f, 0.0098f, 0.0099f, 0.0100f, 0.0100f, 0.0101f,
+ 0.0101f, 0.0102f, 0.0102f, 0.0102f, 0.0103f, 0.0103f, 0.0103f, 0.0103f, 0.0103f,
+ 0.0103f, 0.0103f, 0.0103f, 0.0102f, 0.0102f, 0.0102f, 0.0101f, 0.0101f, 0.0100f,
+ 0.0100f, 0.0099f, 0.0098f, 0.0098f, 0.0097f, 0.0096f, 0.0095f, 0.0094f, 0.0093f,
+ 0.0092f, 0.0091f, 0.0090f, 0.0089f, 0.0088f, 0.0087f, 0.0085f, 0.0084f, 0.0083f,
+ 0.0081f, 0.0080f, 0.0078f, 0.0077f, 0.0076f, 0.0074f, 0.0073f, 0.0071f, 0.0070f,
+ 0.0068f, 0.0066f, 0.0065f, 0.0063f, 0.0062f, 0.0060f, 0.0058f, 0.0057f, 0.0055f,
+ 0.0054f, 0.0052f, 0.0050f, 0.0049f, 0.0047f, 0.0046f, 0.0044f, 0.0043f, 0.0041f,
+ 0.0039f, 0.0038f, 0.0036f, 0.0035f, 0.0034f, 0.0032f, 0.0031f, 0.0029f, 0.0028f,
+ 0.0027f, 0.0026f, 0.0024f, 0.0023f, 0.0022f, 0.0021f, 0.0019f, 0.0018f, 0.0017f,
+ 0.0016f, 0.0015f, 0.0014f, 0.0013f, 0.0012f, 0.0011f, 0.0073f});
+
+ filterVel.setImpulseResponse({0.1744f, 0.0205f, 0.0215f, 0.0225f, 0.0235f, 0.0244f,
+ 0.0252f, 0.0259f, 0.0265f, 0.0270f, 0.0274f, 0.0276f,
+ 0.0278f, 0.0282f, 0.0282f, 0.0278f, 0.0276f, 0.0274f,
+ 0.0270f, 0.0265f, 0.0259f, 0.0252f, 0.0244f, 0.0235f,
+ 0.0225f, 0.0215f, 0.0205f, 0.1744f});
}
- void ActiveImpedanceController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ ActiveImpedanceController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
float setPosition = target.position;
float setSpringConst = target.kp;
@@ -136,7 +135,8 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
desVelocity = 0.1;
}
- float Kd = (-gravity + desTorque + actualTorqueF) / std::max(0.0001f, std::abs(desVelocity));
+ float Kd =
+ (-gravity + desTorque + actualTorqueF) / std::max(0.0001f, std::abs(desVelocity));
ARMARX_RT_LOGF_INFO("Kd: %.2f", Kd).deactivateSpam(0.15);
ARMARX_RT_LOGF_INFO("actualTorqueF: %.2f", actualTorqueF).deactivateSpam(0.15);
@@ -286,42 +286,46 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
dataPtr->setTargetCurrent(current);
}
-
- armarx::ControlTargetBase* ActiveImpedanceController::getControlTarget()
+ armarx::ControlTargetBase*
+ ActiveImpedanceController::getControlTarget()
{
return ⌖
}
-
- armarx::control::rt_filters::FirFilter ActiveImpedanceController::getFilter() const
+ armarx::control::rt_filters::FirFilter
+ ActiveImpedanceController::getFilter() const
{
return filter;
}
-
- void ActiveImpedanceController::rtPreActivateController()
+ void
+ ActiveImpedanceController::rtPreActivateController()
{
joint->switchControlMode(armarx::eTorqueControl);
dataPtr->setTargetCurrent(0.0f);
}
-
- ActiveImpedanceControllerConfigurationPtr ActiveImpedanceControllerConfiguration::CreateTorqueConfigDataFromXml(armarx::DefaultRapidXmlReaderNode node)
+ ActiveImpedanceControllerConfigurationPtr
+ ActiveImpedanceControllerConfiguration::CreateTorqueConfigDataFromXml(
+ armarx::DefaultRapidXmlReaderNode node)
{
ActiveImpedanceControllerConfiguration configData;
ARMARX_CHECK_EXPRESSION(node.is_valid());
- configData.pid_proportional_gain = (double) node.first_node("pid_proportional_gain").value_as_float();
- configData.pid_integral_gain = (double) node.first_node("pid_integral_gain").value_as_float();
- configData.pid_derivative_gain = (double) node.first_node("pid_derivative_gain").value_as_float();
- configData.pid_windup_guard = (double) node.first_node("pid_windup_guard").value_as_float();
- configData.pid_max_value = (double) node.first_node("pid_max_value").value_as_float();
- configData.pid_min_value = (double) node.first_node("pid_min_value").value_as_float();
- configData.pid_dis = (double) node.first_node("pid_dis").value_as_float();
- configData.Kd = (double) node.first_node("Kd").value_as_float();
- configData.inertia = (double) node.first_node("inertia").value_as_float();
+ configData.pid_proportional_gain =
+ (double)node.first_node("pid_proportional_gain").value_as_float();
+ configData.pid_integral_gain =
+ (double)node.first_node("pid_integral_gain").value_as_float();
+ configData.pid_derivative_gain =
+ (double)node.first_node("pid_derivative_gain").value_as_float();
+ configData.pid_windup_guard = (double)node.first_node("pid_windup_guard").value_as_float();
+ configData.pid_max_value = (double)node.first_node("pid_max_value").value_as_float();
+ configData.pid_min_value = (double)node.first_node("pid_min_value").value_as_float();
+ configData.pid_dis = (double)node.first_node("pid_dis").value_as_float();
+ configData.Kd = (double)node.first_node("Kd").value_as_float();
+ configData.inertia = (double)node.first_node("inertia").value_as_float();
auto v = node.first_node("firFilterImpulseResponse").value_as_string();
- for (auto& elem : armarx::Split(v, ",", true))
+ for (auto& elem : armarx::Split(v, ",", true))
{
configData.firFilterImpulseResponse.push_back(armarx::toFloat(elem));
}
@@ -329,9 +333,8 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
return std::make_shared<ActiveImpedanceControllerConfiguration>(configData);
}
-
ActiveImpedanceControllerConfiguration::ActiveImpedanceControllerConfiguration()
{
}
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/ActiveImpedance.h b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/ActiveImpedance.h
index 14bac014..66300838 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/ActiveImpedance.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/ActiveImpedance.h
@@ -3,33 +3,31 @@
// armarx
#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <armarx/control/joint_controller/Torque.h>
#include <armarx/control/rt_filters/AverageFilter.h>
#include <armarx/control/rt_filters/FirFilter.h>
-#include <armarx/control/joint_controller/Torque.h>
-
namespace devices::ethercat::sensor_actor_unit::armar7de
{
class Device;
using DataPtr = std::shared_ptr<class Data>;
-}
-
+} // namespace devices::ethercat::sensor_actor_unit::armar7de
namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
{
- using ActiveImpedanceControllerConfigurationPtr = std::shared_ptr<class ActiveImpedanceControllerConfiguration>;
-
+ using ActiveImpedanceControllerConfigurationPtr =
+ std::shared_ptr<class ActiveImpedanceControllerConfiguration>;
class ActiveImpedanceControllerConfiguration
{
public:
-
ActiveImpedanceControllerConfiguration();
- static ActiveImpedanceControllerConfigurationPtr CreateTorqueConfigDataFromXml(armarx::DefaultRapidXmlReaderNode node);
+ static ActiveImpedanceControllerConfigurationPtr
+ CreateTorqueConfigDataFromXml(armarx::DefaultRapidXmlReaderNode node);
double pid_proportional_gain;
double pid_integral_gain;
double pid_derivative_gain;
@@ -40,29 +38,27 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
double Kd;
double inertia;
std::vector<float> firFilterImpulseResponse;
-
};
using ActiveImpedanceControllerPtr = std::shared_ptr<class ActiveImpedanceController>;
-
class ActiveImpedanceController : public armarx::JointController
{
public:
+ ActiveImpedanceController(
+ Device* joint,
+ DataPtr jointData,
+ armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData);
- ActiveImpedanceController(Device* joint,
- DataPtr jointData,
- armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData);
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
armarx::control::rt_filters::FirFilter getFilter() const;
private:
-
armarx::ActiveImpedanceControlTarget target;
DataPtr dataPtr;
Device* joint;
@@ -75,9 +71,7 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
// JointController interface
protected:
-
void rtPreActivateController() override;
-
};
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/Current.cpp b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/Current.cpp
index b55b0cf1..2bacab56 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/Current.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/Current.cpp
@@ -1,22 +1,20 @@
#include "Current.h"
-
#include <devices/ethercat/sensor_actor_unit/armar7de/Data.h>
#include <devices/ethercat/sensor_actor_unit/armar7de/Device.h>
-
namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
{
- CurrentController::CurrentController(Device* joint, DataPtr jointData) : JointController(),
- target(),
- joint(joint)
+ CurrentController::CurrentController(Device* joint, DataPtr jointData) :
+ JointController(), target(), joint(joint)
{
dataPtr = jointData;
}
-
- void CurrentController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ CurrentController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (target.isValid())
{
@@ -24,17 +22,17 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
}
}
-
- armarx::ControlTargetBase* CurrentController::getControlTarget()
+ armarx::ControlTargetBase*
+ CurrentController::getControlTarget()
{
return ⌖
}
-
- void CurrentController::rtPreActivateController()
+ void
+ CurrentController::rtPreActivateController()
{
joint->switchControlMode(armarx::eTorqueControl);
dataPtr->setTargetCurrent(0.0f);
}
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/Current.h b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/Current.h
index aded9000..1d6c0b32 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/Current.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/Current.h
@@ -2,41 +2,35 @@
// armarx
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
namespace devices::ethercat::sensor_actor_unit::armar7de
{
class Device;
using DataPtr = std::shared_ptr<class Data>;
-}
-
+} // namespace devices::ethercat::sensor_actor_unit::armar7de
namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
{
using JointCurrentControllerPtr = std::shared_ptr<class CurrentController>;
-
class CurrentController : public armarx::JointController
{
public:
-
CurrentController(Device* joint, DataPtr jointData);
protected:
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
void rtPreActivateController() override;
armarx::ControlTargetBase* getControlTarget() override;
private:
-
armarx::ControlTarget1DoFActuatorCurrent target;
DataPtr dataPtr;
Device* joint;
-
};
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/CurrentPassThrough.cpp b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/CurrentPassThrough.cpp
index 457d9fc6..31c5668e 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/CurrentPassThrough.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/CurrentPassThrough.cpp
@@ -1,45 +1,44 @@
#include "CurrentPassThrough.h"
-
#include <devices/ethercat/sensor_actor_unit/armar7de/Data.h>
#include <devices/ethercat/sensor_actor_unit/armar7de/Device.h>
-
namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
{
- armarx::ControlTargetBase* CurrentPassThroughController::getControlTarget()
+ armarx::ControlTargetBase*
+ CurrentPassThroughController::getControlTarget()
{
return ⌖
}
-
CurrentPassThroughController::CurrentPassThroughController(Device* joint, DataPtr jointData) :
- dataPtr(jointData),
- joint(joint)
+ dataPtr(jointData), joint(joint)
{
-
}
-
- void CurrentPassThroughController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ CurrentPassThroughController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (target.isValid())
{
if (target.current > 100)
{
- throw armarx::LocalException() << "Got huge value for target current: " << target.current << " Expecting Ampere not Milliampere!";
+ throw armarx::LocalException()
+ << "Got huge value for target current: " << target.current
+ << " Expecting Ampere not Milliampere!";
}
dataPtr->setTargetCurrent(target.current * 1000);
}
}
-
- void CurrentPassThroughController::rtPreActivateController()
+ void
+ CurrentPassThroughController::rtPreActivateController()
{
joint->switchControlMode(armarx::eTorqueControl);
dataPtr->setTargetCurrent(0.0f);
}
-} // namespace robot_devices::ethercat::sensor_actor_unit::v1::joint_controller
+} // namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/CurrentPassThrough.h b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/CurrentPassThrough.h
index c1b7904a..269896f8 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/CurrentPassThrough.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/CurrentPassThrough.h
@@ -2,42 +2,36 @@
// armarx
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
namespace devices::ethercat::sensor_actor_unit::armar7de
{
class Device;
using DataPtr = std::shared_ptr<class Data>;
-}
-
+} // namespace devices::ethercat::sensor_actor_unit::armar7de
namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
{
using CurrentPassThroughControllerPtr = std::shared_ptr<class CurrentPassThroughController>;
-
class CurrentPassThroughController : public armarx::JointController
{
public:
-
CurrentPassThroughController(Device* joint, DataPtr jointData);
protected:
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
void rtPreActivateController() override;
armarx::ControlTargetBase* getControlTarget() override;
private:
-
armarx::ControlTarget1DoFActuatorCurrent target;
DataPtr dataPtr;
Device* joint;
-
};
-} // namespace robot_devices::ethercat::sensor_actor_unit::v1::joint_controller
+} // namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/Position.cpp b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/Position.cpp
index cb3e0ec5..679d05cc 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/Position.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/Position.cpp
@@ -2,7 +2,6 @@
// armarx
#include "ArmarXCore/core/logging/Logging.h"
-
#include "RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h"
#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/Torque.cpp b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/Torque.cpp
index 1b6bc9f8..6b72f01f 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/Torque.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/Torque.cpp
@@ -71,6 +71,4 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
// with brake
-
-
} // namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/VelocityTorque.cpp b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/VelocityTorque.cpp
index 8a8050df..3015d0bf 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/VelocityTorque.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/VelocityTorque.cpp
@@ -1,24 +1,19 @@
#include "VelocityTorque.h"
-
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
//#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <devices/ethercat/sensor_actor_unit/armar7de/Data.h>
#include <devices/ethercat/sensor_actor_unit/armar7de/Device.h>
-
namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
{
- VelocityTorqueController::VelocityTorqueController(Device* joint,
- DataPtr jointData,
- armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData)
- : JointController(),
- target(),
- joint(joint),
- pid(),
- targetFilter(100)
+ VelocityTorqueController::VelocityTorqueController(
+ Device* joint,
+ DataPtr jointData,
+ armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData) :
+ JointController(), target(), joint(joint), pid(), targetFilter(100)
{
defaultMaxTorque = torqueConfigData->maxTorque;
@@ -39,36 +34,42 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
// coefficients for low-pass FIR-Filter for torque
filter.setImpulseResponse(
- {
- 0.0071f, 0.0007f, 0.0007f, 0.0008f, 0.0008f, 0.0008f, 0.0009f, 0.0009f, 0.0010f, 0.0010f, 0.0010f, 0.0011f, 0.0011f,
- 0.0012f, 0.0012f, 0.0012f, 0.0013f, 0.0013f, 0.0014f, 0.0014f, 0.0015f, 0.0015f, 0.0016f, 0.0016f, 0.0017f, 0.0017f,
- 0.0018f, 0.0018f, 0.0019f, 0.0020f, 0.0020f, 0.0021f, 0.0021f, 0.0022f, 0.0022f, 0.0023f, 0.0023f, 0.0024f, 0.0025f,
- 0.0025f, 0.0026f, 0.0026f, 0.0027f, 0.0028f, 0.0028f, 0.0029f, 0.0030f, 0.0030f, 0.0031f, 0.0031f, 0.0032f, 0.0033f,
- 0.0033f, 0.0034f, 0.0035f, 0.0035f, 0.0036f, 0.0036f, 0.0037f, 0.0038f, 0.0038f, 0.0039f, 0.0040f, 0.0040f, 0.0041f,
- 0.0041f, 0.0042f, 0.0043f, 0.0043f, 0.0044f, 0.0045f, 0.0045f, 0.0046f, 0.0046f, 0.0047f, 0.0048f, 0.0048f, 0.0049f,
- 0.0049f, 0.0050f, 0.0050f, 0.0051f, 0.0051f, 0.0052f, 0.0052f, 0.0053f, 0.0053f, 0.0054f, 0.0054f, 0.0055f, 0.0055f,
- 0.0056f, 0.0056f, 0.0057f, 0.0057f, 0.0058f, 0.0058f, 0.0058f, 0.0059f, 0.0059f, 0.0060f, 0.0060f, 0.0060f, 0.0061f,
- 0.0061f, 0.0061f, 0.0061f, 0.0062f, 0.0062f, 0.0062f, 0.0063f, 0.0063f, 0.0063f, 0.0063f, 0.0063f, 0.0064f, 0.0064f,
- 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f,
- 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0063f, 0.0063f, 0.0063f, 0.0063f,
- 0.0063f, 0.0062f, 0.0062f, 0.0062f, 0.0061f, 0.0061f, 0.0061f, 0.0061f, 0.0060f, 0.0060f, 0.0060f, 0.0059f, 0.0059f,
- 0.0058f, 0.0058f, 0.0058f, 0.0057f, 0.0057f, 0.0056f, 0.0056f, 0.0055f, 0.0055f, 0.0054f, 0.0054f, 0.0053f, 0.0053f,
- 0.0052f, 0.0052f, 0.0051f, 0.0051f, 0.0050f, 0.0050f, 0.0049f, 0.0049f, 0.0048f, 0.0048f, 0.0047f, 0.0046f, 0.0046f,
- 0.0045f, 0.0045f, 0.0044f, 0.0043f, 0.0043f, 0.0042f, 0.0041f, 0.0041f, 0.0040f, 0.0040f, 0.0039f, 0.0038f, 0.0038f,
- 0.0037f, 0.0036f, 0.0036f, 0.0035f, 0.0035f, 0.0034f, 0.0033f, 0.0033f, 0.0032f, 0.0031f, 0.0031f, 0.0030f, 0.0030f,
- 0.0029f, 0.0028f, 0.0028f, 0.0027f, 0.0026f, 0.0026f, 0.0025f, 0.0025f, 0.0024f, 0.0023f, 0.0023f, 0.0022f, 0.0022f,
- 0.0021f, 0.0021f, 0.0020f, 0.0020f, 0.0019f, 0.0018f, 0.0018f, 0.0017f, 0.0017f, 0.0016f, 0.0016f, 0.0015f, 0.0015f,
- 0.0014f, 0.0014f, 0.0013f, 0.0013f, 0.0012f, 0.0012f, 0.0012f, 0.0011f, 0.0011f, 0.0010f, 0.0010f, 0.0010f, 0.0009f,
- 0.0009f, 0.0008f, 0.0008f, 0.0008f, 0.0007f, 0.0007f, 0.0071f
- });
+ {0.0071f, 0.0007f, 0.0007f, 0.0008f, 0.0008f, 0.0008f, 0.0009f, 0.0009f, 0.0010f,
+ 0.0010f, 0.0010f, 0.0011f, 0.0011f, 0.0012f, 0.0012f, 0.0012f, 0.0013f, 0.0013f,
+ 0.0014f, 0.0014f, 0.0015f, 0.0015f, 0.0016f, 0.0016f, 0.0017f, 0.0017f, 0.0018f,
+ 0.0018f, 0.0019f, 0.0020f, 0.0020f, 0.0021f, 0.0021f, 0.0022f, 0.0022f, 0.0023f,
+ 0.0023f, 0.0024f, 0.0025f, 0.0025f, 0.0026f, 0.0026f, 0.0027f, 0.0028f, 0.0028f,
+ 0.0029f, 0.0030f, 0.0030f, 0.0031f, 0.0031f, 0.0032f, 0.0033f, 0.0033f, 0.0034f,
+ 0.0035f, 0.0035f, 0.0036f, 0.0036f, 0.0037f, 0.0038f, 0.0038f, 0.0039f, 0.0040f,
+ 0.0040f, 0.0041f, 0.0041f, 0.0042f, 0.0043f, 0.0043f, 0.0044f, 0.0045f, 0.0045f,
+ 0.0046f, 0.0046f, 0.0047f, 0.0048f, 0.0048f, 0.0049f, 0.0049f, 0.0050f, 0.0050f,
+ 0.0051f, 0.0051f, 0.0052f, 0.0052f, 0.0053f, 0.0053f, 0.0054f, 0.0054f, 0.0055f,
+ 0.0055f, 0.0056f, 0.0056f, 0.0057f, 0.0057f, 0.0058f, 0.0058f, 0.0058f, 0.0059f,
+ 0.0059f, 0.0060f, 0.0060f, 0.0060f, 0.0061f, 0.0061f, 0.0061f, 0.0061f, 0.0062f,
+ 0.0062f, 0.0062f, 0.0063f, 0.0063f, 0.0063f, 0.0063f, 0.0063f, 0.0064f, 0.0064f,
+ 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f,
+ 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0064f,
+ 0.0064f, 0.0064f, 0.0064f, 0.0064f, 0.0063f, 0.0063f, 0.0063f, 0.0063f, 0.0063f,
+ 0.0062f, 0.0062f, 0.0062f, 0.0061f, 0.0061f, 0.0061f, 0.0061f, 0.0060f, 0.0060f,
+ 0.0060f, 0.0059f, 0.0059f, 0.0058f, 0.0058f, 0.0058f, 0.0057f, 0.0057f, 0.0056f,
+ 0.0056f, 0.0055f, 0.0055f, 0.0054f, 0.0054f, 0.0053f, 0.0053f, 0.0052f, 0.0052f,
+ 0.0051f, 0.0051f, 0.0050f, 0.0050f, 0.0049f, 0.0049f, 0.0048f, 0.0048f, 0.0047f,
+ 0.0046f, 0.0046f, 0.0045f, 0.0045f, 0.0044f, 0.0043f, 0.0043f, 0.0042f, 0.0041f,
+ 0.0041f, 0.0040f, 0.0040f, 0.0039f, 0.0038f, 0.0038f, 0.0037f, 0.0036f, 0.0036f,
+ 0.0035f, 0.0035f, 0.0034f, 0.0033f, 0.0033f, 0.0032f, 0.0031f, 0.0031f, 0.0030f,
+ 0.0030f, 0.0029f, 0.0028f, 0.0028f, 0.0027f, 0.0026f, 0.0026f, 0.0025f, 0.0025f,
+ 0.0024f, 0.0023f, 0.0023f, 0.0022f, 0.0022f, 0.0021f, 0.0021f, 0.0020f, 0.0020f,
+ 0.0019f, 0.0018f, 0.0018f, 0.0017f, 0.0017f, 0.0016f, 0.0016f, 0.0015f, 0.0015f,
+ 0.0014f, 0.0014f, 0.0013f, 0.0013f, 0.0012f, 0.0012f, 0.0012f, 0.0011f, 0.0011f,
+ 0.0010f, 0.0010f, 0.0010f, 0.0009f, 0.0009f, 0.0008f, 0.0008f, 0.0008f, 0.0007f,
+ 0.0007f, 0.0071f});
// coefficients for low-pass FIR-Filter for velocity
- filter_vel.setImpulseResponse(
- {
- 0.1744f, 0.0205f, 0.0215f, 0.0225f, 0.0235f, 0.0244f, 0.0252f, 0.0259f, 0.0265f, 0.0270f, 0.0274f, 0.0276f, 0.0278f,
- 0.0282f, 0.0282f, 0.0278f, 0.0276f, 0.0274f, 0.0270f, 0.0265f, 0.0259f, 0.0252f, 0.0244f, 0.0235f, 0.0225f, 0.0215f,
- 0.0205f, 0.1744f
- });
+ filter_vel.setImpulseResponse({0.1744f, 0.0205f, 0.0215f, 0.0225f, 0.0235f, 0.0244f,
+ 0.0252f, 0.0259f, 0.0265f, 0.0270f, 0.0274f, 0.0276f,
+ 0.0278f, 0.0282f, 0.0282f, 0.0278f, 0.0276f, 0.0274f,
+ 0.0270f, 0.0265f, 0.0259f, 0.0252f, 0.0244f, 0.0235f,
+ 0.0225f, 0.0215f, 0.0205f, 0.1744f});
velCtrl.acceleration = 4.5;
velCtrl.deceleration = 4.5;
@@ -79,11 +80,11 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
velCtrl.positionLimitHiSoft = dataPtr->getSoftLimitHi();
// controller.positionLimitLoHard = dataPtr->getHardLimitLo();
velCtrl.positionLimitLoSoft = dataPtr->getSoftLimitLo();
-
}
-
- void VelocityTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ VelocityTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
float maxTorque = target.maxTorque < 0 ? defaultMaxTorque : target.maxTorque;
pid.maxTorque = maxTorque;
@@ -158,7 +159,8 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
adjustedTargetVelocity = 0.0;
}
- float Kd = (-gravity + maxTorque + actualTorque) / std::max(0.0001f, std::abs(adjustedTargetVelocity));
+ float Kd = (-gravity + maxTorque + actualTorque) /
+ std::max(0.0001f, std::abs(adjustedTargetVelocity));
//ARMARX_RT_LOGF_INFO("Kd: %.2f", Kd).deactivateSpam(0.3);
@@ -201,7 +203,6 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
//adjustedTargetVelocity = 0.0f;
pid.integral_gain = 0.1;
pid.proportional_gain = 0.0;
-
}
if (std::abs(Kd) > 200)
@@ -265,20 +266,20 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
//ARMARX_RT_LOGF_INFO("Target current: %.2f", current).deactivateSpam(0.3);
}
-
- armarx::ControlTargetBase* VelocityTorqueController::getControlTarget()
+ armarx::ControlTargetBase*
+ VelocityTorqueController::getControlTarget()
{
return ⌖
}
-
- armarx::control::rt_filters::FirFilter VelocityTorqueController::getFilter() const
+ armarx::control::rt_filters::FirFilter
+ VelocityTorqueController::getFilter() const
{
return filter;
}
-
- void VelocityTorqueController::rtPreActivateController()
+ void
+ VelocityTorqueController::rtPreActivateController()
{
joint->switchControlMode(armarx::eTorqueControl);
dataPtr->setTargetCurrent(0.0f);
@@ -286,25 +287,29 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
target.velocity = lastSetVelocity;
}
-
- VelocityTorqueControllerConfigurationPtr VelocityTorqueControllerConfiguration::CreateTorqueConfigDataFromXml(armarx::DefaultRapidXmlReaderNode node)
+ VelocityTorqueControllerConfigurationPtr
+ VelocityTorqueControllerConfiguration::CreateTorqueConfigDataFromXml(
+ armarx::DefaultRapidXmlReaderNode node)
{
VelocityTorqueControllerConfiguration configData;
ARMARX_CHECK_EXPRESSION(node.is_valid());
- configData.pid_proportional_gain = (double) node.first_node("pid_proportional_gain").value_as_float();
- configData.pid_integral_gain = (double) node.first_node("pid_integral_gain").value_as_float();
- configData.pid_derivative_gain = (double) node.first_node("pid_derivative_gain").value_as_float();
- configData.pid_windup_guard = (double) node.first_node("pid_windup_guard").value_as_float();
- configData.pid_max_value = (double) node.first_node("pid_max_value").value_as_float();
- configData.pid_min_value = (double) node.first_node("pid_min_value").value_as_float();
- configData.pid_dis = (double) node.first_node("pid_dis").value_as_float();
- configData.Kd = (double) node.first_node("Kd").value_as_float();
- configData.inertia = (double) node.first_node("inertia").value_as_float();
- configData.maxTorque = (double) node.first_node("maxTorque").value_as_float();
- configData.actuatorType = (double) node.first_node("actuatorType").value_as_float();
+ configData.pid_proportional_gain =
+ (double)node.first_node("pid_proportional_gain").value_as_float();
+ configData.pid_integral_gain =
+ (double)node.first_node("pid_integral_gain").value_as_float();
+ configData.pid_derivative_gain =
+ (double)node.first_node("pid_derivative_gain").value_as_float();
+ configData.pid_windup_guard = (double)node.first_node("pid_windup_guard").value_as_float();
+ configData.pid_max_value = (double)node.first_node("pid_max_value").value_as_float();
+ configData.pid_min_value = (double)node.first_node("pid_min_value").value_as_float();
+ configData.pid_dis = (double)node.first_node("pid_dis").value_as_float();
+ configData.Kd = (double)node.first_node("Kd").value_as_float();
+ configData.inertia = (double)node.first_node("inertia").value_as_float();
+ configData.maxTorque = (double)node.first_node("maxTorque").value_as_float();
+ configData.actuatorType = (double)node.first_node("actuatorType").value_as_float();
auto v = node.first_node("firFilterImpulseResponse").value_as_string();
- for (auto& elem : armarx::Split(v, ",", true))
+ for (auto& elem : armarx::Split(v, ",", true))
{
configData.firFilterImpulseResponse.push_back(armarx::toFloat(elem));
}
@@ -312,10 +317,8 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
return std::make_shared<VelocityTorqueControllerConfiguration>(configData);
}
-
VelocityTorqueControllerConfiguration::VelocityTorqueControllerConfiguration()
{
-
}
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/VelocityTorque.h b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/VelocityTorque.h
index 5867ea1a..442e9c6f 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/VelocityTorque.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/VelocityTorque.h
@@ -4,32 +4,30 @@
// armarx
#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <armarx/control/joint_controller/Torque.h>
#include <armarx/control/rt_filters/AverageFilter.h>
#include <armarx/control/rt_filters/FirFilter.h>
-#include <armarx/control/joint_controller/Torque.h>
-
namespace devices::ethercat::sensor_actor_unit::armar7de
{
class Device;
using DataPtr = std::shared_ptr<class Data>;
-}
-
+} // namespace devices::ethercat::sensor_actor_unit::armar7de
namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
{
- using VelocityTorqueControllerConfigurationPtr = std::shared_ptr<class VelocityTorqueControllerConfiguration>;
-
+ using VelocityTorqueControllerConfigurationPtr =
+ std::shared_ptr<class VelocityTorqueControllerConfiguration>;
class VelocityTorqueControllerConfiguration
{
public:
-
VelocityTorqueControllerConfiguration();
- static VelocityTorqueControllerConfigurationPtr CreateTorqueConfigDataFromXml(armarx::DefaultRapidXmlReaderNode node);
+ static VelocityTorqueControllerConfigurationPtr
+ CreateTorqueConfigDataFromXml(armarx::DefaultRapidXmlReaderNode node);
double pid_proportional_gain;
double pid_integral_gain;
double pid_derivative_gain;
@@ -44,33 +42,30 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
double actuatorType;
bool status;
std::vector<float> firFilterImpulseResponse;
-
};
using VelocityTorqueControllerPtr = std::shared_ptr<class VelocityTorqueController>;
-
class VelocityTorqueController : public armarx::JointController
{
public:
+ VelocityTorqueController(
+ Device* joint,
+ DataPtr jointData,
+ armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData);
- VelocityTorqueController(Device* joint,
- DataPtr jointData,
- armarx::control::joint_controller::TorqueControllerConfigurationPtr torqueConfigData);
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
armarx::control::rt_filters::FirFilter getFilter() const;
protected:
-
void rtPreActivateController() override;
private:
-
float defaultMaxTorque;
armarx::ControlTarget1DoFActuatorTorqueVelocity target;
DataPtr dataPtr;
@@ -83,7 +78,6 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
armarx::control::rt_filters::FirFilter hp_filter;
armarx::control::rt_filters::FirFilter lp_filter;
armarx::control::rt_filters::RtAverageFilter targetFilter;
-
};
-} // namespace robot_devices::ethercat::sensor_actor_unit::v1::joint_controller
+} // namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/ZeroTorque.cpp b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/ZeroTorque.cpp
index ea524cb0..bc08f208 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/ZeroTorque.cpp
+++ b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/ZeroTorque.cpp
@@ -25,30 +25,28 @@
#include "ZeroTorque.h"
-
-#include <devices/ethercat/sensor_actor_unit/armar7de/Data.h>
-#include <devices/ethercat/sensor_actor_unit/armar7de/Device.h>
-
#include "ArmarXGui/libraries/DefaultWidgetDescriptions/DefaultWidgetDescriptions.h"
#include "ArmarXGui/libraries/RemoteGui/WidgetBuilder/FloatWidgets.h"
#include "ArmarXGui/libraries/RemoteGui/WidgetBuilder/StringWidgets.h"
#include "ArmarXGui/libraries/VariantWidget/VariantWidget.h"
#include "RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h"
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/application/Application.h>
-#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
+#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-
+#include <devices/ethercat/sensor_actor_unit/armar7de/Data.h>
+#include <devices/ethercat/sensor_actor_unit/armar7de/Device.h>
namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
{
- ZeroTorqueController::ZeroTorqueController(Device* joint,
- DataPtr jointData,
- armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfigurationPtr torqueConfigData) :
+ ZeroTorqueController::ZeroTorqueController(
+ Device* joint,
+ DataPtr jointData,
+ armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfigurationPtr
+ torqueConfigData) :
target(),
joint(joint),
torqueController(torqueConfigData),
@@ -59,9 +57,9 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
controllerConfigBufferPublishToRt.reinitAllBuffers(*torqueConfigData);
}
-
-
- void ZeroTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ ZeroTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (target.isValid())
{
@@ -82,229 +80,247 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
torqueController.setTorqueConfigData(torqueControllerConfig);
- float current = torqueController.update(sensorValuesTimestamp, timeSinceLastIteration, gravity, actualTorque, targetTorque, actualVelocity, actualPosition);
+ float current = torqueController.update(sensorValuesTimestamp,
+ timeSinceLastIteration,
+ gravity,
+ actualTorque,
+ targetTorque,
+ actualVelocity,
+ actualPosition);
// send the current value to Elmo
dataPtr->setTargetCurrent(current);
}
}
-
- armarx::ControlTargetBase* ZeroTorqueController::getControlTarget()
+ armarx::ControlTargetBase*
+ ZeroTorqueController::getControlTarget()
{
return ⌖
}
-
- void ZeroTorqueController::rtPreActivateController()
+ void
+ ZeroTorqueController::rtPreActivateController()
{
joint->switchControlMode(armarx::eTorqueControl);
dataPtr->setTargetCurrent(0.0f);
torqueController.reset();
-armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfiguration torqueConfigData
- = torqueController.getTorqueConfigData();
-
+ armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfiguration
+ torqueConfigData = torqueController.getTorqueConfigData();
}
- armarx::StringVariantBaseMap ZeroTorqueController::publish(const armarx::DebugDrawerInterfacePrx& /*draw*/, const armarx::DebugObserverInterfacePrx& /*observer*/) const
+ armarx::StringVariantBaseMap
+ ZeroTorqueController::publish(const armarx::DebugDrawerInterfacePrx& /*draw*/,
+ const armarx::DebugObserverInterfacePrx& /*observer*/) const
{
if (!remoteGui)
{
- threadHandle = armarx::Application::getInstance()->getThreadPool()->runTask([this]
- {
- std::string guiTabName;
- while (!stopRequested)
+ threadHandle = armarx::Application::getInstance()->getThreadPool()->runTask(
+ [this]
{
- armarx::ManagedIceObjectPtr object;
- ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
- try
+ std::string guiTabName;
+ while (!stopRequested)
{
- object = armarx::ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
- ARMARX_CHECK_EXPRESSION(object);
- remoteGui = object->getProxy<armarx::RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
- if (!remoteGui)
+ armarx::ManagedIceObjectPtr object;
+ ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
+ try
{
- continue;
+ object =
+ armarx::ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
+ ARMARX_CHECK_EXPRESSION(object);
+ remoteGui = object->getProxy<armarx::RemoteGuiInterfacePrx>(
+ "RemoteGuiProvider", false, "", false);
+ if (!remoteGui)
+ {
+ continue;
+ }
+ ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
+ guiTabName = getParent().getDeviceName() + getControlMode();
+ break;
+ }
+ catch (...)
+ {
+ sleep(1);
}
- ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
- guiTabName = getParent().getDeviceName() + getControlMode();
- break;
- }
- catch (...)
- {
- sleep(1);
}
- }
- if (remoteGui)
- {
- ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
- using namespace armarx::RemoteGui;
+ if (remoteGui)
+ {
+ ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
+ using namespace armarx::RemoteGui;
- auto vLayout = makeVBoxLayout();
+ auto vLayout = makeVBoxLayout();
- {
- WidgetPtr KpLabel = makeTextLabel("Kp: ");
- const double rangeVal = 20.;
- double minVal = 0.;
- double maxVal = 0.;
- if(initialTorqueConfigData.pid_proportional_gain > 0)
{
- minVal = 0.;
- maxVal = rangeVal;
-
- } else {
- minVal = -rangeVal;
- maxVal = 0.;
+ WidgetPtr KpLabel = makeTextLabel("Kp: ");
+ const double rangeVal = 20.;
+ double minVal = 0.;
+ double maxVal = 0.;
+ if (initialTorqueConfigData.pid_proportional_gain > 0)
+ {
+ minVal = 0.;
+ maxVal = rangeVal;
+ }
+ else
+ {
+ minVal = -rangeVal;
+ maxVal = 0.;
+ }
+ auto KpSlider =
+ makeFloatSpinBox("KpSlider")
+ .steps(rangeVal / 0.001)
+ .min(minVal)
+ .max(maxVal)
+ .value(initialTorqueConfigData.pid_proportional_gain);
+
+ WidgetPtr line = makeHBoxLayout().children({KpLabel, KpSlider});
+ vLayout.addChild(line);
}
- auto KpSlider
- = makeFloatSpinBox("KpSlider")
- .steps(rangeVal/0.001)
- .min(minVal).max(maxVal)
- .value(initialTorqueConfigData.pid_proportional_gain);
-
- WidgetPtr line = makeHBoxLayout()
- .children({KpLabel, KpSlider});
- vLayout.addChild(line);
- }
- {
- WidgetPtr KiLabel = makeTextLabel("Ki: ");
- const double rangeVal = 20.;
- double minVal = 0.;
- double maxVal = 0.;
- if(initialTorqueConfigData.pid_proportional_gain > 0)
{
- minVal = 0.;
- maxVal = rangeVal;
-
- } else {
- minVal = -rangeVal;
- maxVal = 0.;
- }
- auto KiSlider
- = makeFloatSpinBox("KiSlider")
- .steps(rangeVal/0.001)
- .min(minVal).max(maxVal)
+ WidgetPtr KiLabel = makeTextLabel("Ki: ");
+ const double rangeVal = 20.;
+ double minVal = 0.;
+ double maxVal = 0.;
+ if (initialTorqueConfigData.pid_proportional_gain > 0)
+ {
+ minVal = 0.;
+ maxVal = rangeVal;
+ }
+ else
+ {
+ minVal = -rangeVal;
+ maxVal = 0.;
+ }
+ auto KiSlider = makeFloatSpinBox("KiSlider")
+ .steps(rangeVal / 0.001)
+ .min(minVal)
+ .max(maxVal)
.value(initialTorqueConfigData.pid_integral_gain);
- WidgetPtr line = makeHBoxLayout()
- .children({KiLabel, KiSlider});
- vLayout.addChild(line);
- }
+ WidgetPtr line = makeHBoxLayout().children({KiLabel, KiSlider});
+ vLayout.addChild(line);
+ }
- {
- WidgetPtr KdLabel = makeTextLabel("Kd: ");
- const double rangeVal = 20.;
- double minVal = 0.;
- double maxVal = 0.;
- if(initialTorqueConfigData.pid_derivative_gain > 0)
{
- minVal = 0.;
- maxVal = rangeVal;
-
- } else {
- minVal = -rangeVal;
- maxVal = 0.;
- }
- auto KdSlider
- = makeFloatSpinBox("KdSlider")
- .steps(rangeVal/0.001)
- .min(minVal).max(maxVal)
+ WidgetPtr KdLabel = makeTextLabel("Kd: ");
+ const double rangeVal = 20.;
+ double minVal = 0.;
+ double maxVal = 0.;
+ if (initialTorqueConfigData.pid_derivative_gain > 0)
+ {
+ minVal = 0.;
+ maxVal = rangeVal;
+ }
+ else
+ {
+ minVal = -rangeVal;
+ maxVal = 0.;
+ }
+ auto KdSlider = makeFloatSpinBox("KdSlider")
+ .steps(rangeVal / 0.001)
+ .min(minVal)
+ .max(maxVal)
.value(initialTorqueConfigData.pid_derivative_gain);
- WidgetPtr line = makeHBoxLayout()
- .children({KdLabel, KdSlider});
- vLayout.addChild(line);
- }
+ WidgetPtr line = makeHBoxLayout().children({KdLabel, KdSlider});
+ vLayout.addChild(line);
+ }
- vLayout.addChild(makeHBoxLayout().children({
- makeTextLabel("Max Acc: "),
- makeFloatSpinBox("MaxAcc")
- .minmax(0, 20.)
- .steps(1000)
- .value(initialTorqueConfigData.maxAcceleration)}));
-
- vLayout.addChild(makeHBoxLayout().children({
- makeTextLabel("JL Margin: "),
- makeFloatSpinBox("PushBackMargin")
- .minmax(0.0, 50.)
- .steps(1000)
- .value(initialTorqueConfigData.pushbackMargin)}));
-
- vLayout.addChild(makeHBoxLayout().children({
- makeTextLabel("JL Torque: "),
- makeFloatSpinBox("PushBackTorque")
- .minmax(0, 1000.)
- .steps(1000)
- .value(initialTorqueConfigData.pushBackTorque)}));
+ vLayout.addChild(makeHBoxLayout().children(
+ {makeTextLabel("Max Acc: "),
+ makeFloatSpinBox("MaxAcc").minmax(0, 20.).steps(1000).value(
+ initialTorqueConfigData.maxAcceleration)}));
+
+ vLayout.addChild(makeHBoxLayout().children(
+ {makeTextLabel("JL Margin: "),
+ makeFloatSpinBox("PushBackMargin")
+ .minmax(0.0, 50.)
+ .steps(1000)
+ .value(initialTorqueConfigData.pushbackMargin)}));
+
+ vLayout.addChild(makeHBoxLayout().children(
+ {makeTextLabel("JL Torque: "),
+ makeFloatSpinBox("PushBackTorque")
+ .minmax(0, 1000.)
+ .steps(1000)
+ .value(initialTorqueConfigData.pushBackTorque)}));
- {
- WidgetPtr DecayLabel = makeTextLabel("Decay: ");
- auto DecaySlider
- = makeFloatSpinBox("DecaySlider")
- .steps(1000)
- .min(0.).max(1.)
- .value(initialTorqueConfigData.decay);
- WidgetPtr DecayLabelValue = makeLabel("DecayValue").value(std::to_string(initialTorqueConfigData.decay));
- WidgetPtr line = makeHBoxLayout()
- .children({DecayLabel, DecaySlider, DecayLabelValue});
-
- vLayout.addChild(line);
- vLayout.addChild(new VSpacer);
- }
+ {
+ WidgetPtr DecayLabel = makeTextLabel("Decay: ");
+ auto DecaySlider = makeFloatSpinBox("DecaySlider")
+ .steps(1000)
+ .min(0.)
+ .max(1.)
+ .value(initialTorqueConfigData.decay);
+ WidgetPtr DecayLabelValue =
+ makeLabel("DecayValue")
+ .value(std::to_string(initialTorqueConfigData.decay));
+ WidgetPtr line = makeHBoxLayout().children(
+ {DecayLabel, DecaySlider, DecayLabelValue});
+
+ vLayout.addChild(line);
+ vLayout.addChild(new VSpacer);
+ }
- {
- WidgetPtr DeadzoneLabel = makeTextLabel("Deadzone: ");
- auto DeadzoneSlider
- = makeFloatSpinBox("DeadzoneSlider")
- .steps(1000)
- .min(0.).max(10.)
- .value(initialTorqueConfigData.deadZone);
- WidgetPtr DeadzoneLabelValue = makeLabel("DeadzoneValue").value(std::to_string(initialTorqueConfigData.deadZone));
- WidgetPtr line = makeHBoxLayout()
- .children({DeadzoneLabel, DeadzoneSlider, DeadzoneLabelValue});
-
- vLayout.addChild(line);
- vLayout.addChild(new VSpacer);
- }
+ {
+ WidgetPtr DeadzoneLabel = makeTextLabel("Deadzone: ");
+ auto DeadzoneSlider = makeFloatSpinBox("DeadzoneSlider")
+ .steps(1000)
+ .min(0.)
+ .max(10.)
+ .value(initialTorqueConfigData.deadZone);
+ WidgetPtr DeadzoneLabelValue =
+ makeLabel("DeadzoneValue")
+ .value(std::to_string(initialTorqueConfigData.deadZone));
+ WidgetPtr line = makeHBoxLayout().children(
+ {DeadzoneLabel, DeadzoneSlider, DeadzoneLabelValue});
+
+ vLayout.addChild(line);
+ vLayout.addChild(new VSpacer);
+ }
- WidgetPtr groupBox = makeGroupBox("GroupBox")
- .label("Group")
- .child(vLayout);
+ WidgetPtr groupBox = makeGroupBox("GroupBox").label("Group").child(vLayout);
- remoteGui->createTab(guiTabName, groupBox);
+ remoteGui->createTab(guiTabName, groupBox);
- while (!stopRequested)
- {
- armarx::RemoteGui::TabProxy tab(remoteGui, guiTabName);
- tab.receiveUpdates();
+ while (!stopRequested)
+ {
+ armarx::RemoteGui::TabProxy tab(remoteGui, guiTabName);
+ tab.receiveUpdates();
- auto& torqueConfigData = controllerConfigBufferPublishToRt.getWriteBuffer();
+ auto& torqueConfigData =
+ controllerConfigBufferPublishToRt.getWriteBuffer();
- torqueConfigData.pid_proportional_gain = tab.getValue<float>("KpSlider").get();
- torqueConfigData.pid_integral_gain = tab.getValue<float>("KiSlider").get();
- torqueConfigData.pid_derivative_gain = tab.getValue<float>("KdSlider").get();
- torqueConfigData.maxAcceleration = tab.getValue<float>("MaxAcc").get();
- torqueConfigData.decay = tab.getValue<float>("DecaySlider").get();
- torqueConfigData.deadZone = tab.getValue<float>("DeadzoneSlider").get();
- torqueConfigData.pushBackTorque = tab.getValue<float>("PushBackTorque").get();
- torqueConfigData.pushbackMargin = tab.getValue<float>("PushBackMargin").get();
+ torqueConfigData.pid_proportional_gain =
+ tab.getValue<float>("KpSlider").get();
+ torqueConfigData.pid_integral_gain =
+ tab.getValue<float>("KiSlider").get();
+ torqueConfigData.pid_derivative_gain =
+ tab.getValue<float>("KdSlider").get();
+ torqueConfigData.maxAcceleration = tab.getValue<float>("MaxAcc").get();
+ torqueConfigData.decay = tab.getValue<float>("DecaySlider").get();
+ torqueConfigData.deadZone = tab.getValue<float>("DeadzoneSlider").get();
+ torqueConfigData.pushBackTorque =
+ tab.getValue<float>("PushBackTorque").get();
+ torqueConfigData.pushbackMargin =
+ tab.getValue<float>("PushBackMargin").get();
- controllerConfigBufferPublishToRt.commitWrite();
+ controllerConfigBufferPublishToRt.commitWrite();
- tab.getValue<std::string>("DecayValue").set(std::to_string(torqueConfigData.decay));
- tab.getValue<std::string>("DeadzoneValue").set(std::to_string(torqueConfigData.deadZone));
-
+ tab.getValue<std::string>("DecayValue")
+ .set(std::to_string(torqueConfigData.decay));
+ tab.getValue<std::string>("DeadzoneValue")
+ .set(std::to_string(torqueConfigData.deadZone));
- tab.sendUpdates();
- usleep(100000);
- }
- }
+ tab.sendUpdates();
- });
+ usleep(100000);
+ }
+ }
+ });
}
return {};
// return {{"lastTargetVelocity", new armarx::Variant(lastTargetVelocity.load())},
@@ -324,4 +340,4 @@ armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfigura
// {"desiredPWM", new armarx::Variant(targetPWM.load())}
// };
}
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
diff --git a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/ZeroTorque.h b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/ZeroTorque.h
index db362d70..ed210461 100644
--- a/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/ZeroTorque.h
+++ b/source/devices/ethercat/sensor_actor_unit/armar7de/joint_controller/ZeroTorque.h
@@ -28,20 +28,17 @@
// armarx
#include "ArmarXCore/util/CPPUtility/TripleBuffer.h"
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <armarx/control/joint_controller/VelocityManipulatingTorque.h>
#include <ArmarXCore/core/services/tasks/ThreadPool.h>
#include <ArmarXGui/interface/RemoteGuiInterface.h>
-
-
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <armarx/control/joint_controller/VelocityManipulatingTorque.h>
namespace devices::ethercat::sensor_actor_unit::armar7de
{
class Device;
using DataPtr = std::shared_ptr<class Data>;
-}
-
+} // namespace devices::ethercat::sensor_actor_unit::armar7de
namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
{
@@ -51,39 +48,43 @@ namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
{
public:
-
- ZeroTorqueController(Device* joint,
- DataPtr jointData,
- armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfigurationPtr torqueConfigData);
+ ZeroTorqueController(
+ Device* joint,
+ DataPtr jointData,
+ armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfigurationPtr
+ torqueConfigData);
~ZeroTorqueController() noexcept(true){};
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) final;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) final;
armarx::ControlTargetBase* getControlTarget() override;
// JointController interface
- armarx::StringVariantBaseMap publish(const armarx::DebugDrawerInterfacePrx& draw, const armarx::DebugObserverInterfacePrx& observer) const final;
+ armarx::StringVariantBaseMap
+ publish(const armarx::DebugDrawerInterfacePrx& draw,
+ const armarx::DebugObserverInterfacePrx& observer) const final;
protected:
-
void rtPreActivateController() override;
mutable armarx::RemoteGuiInterfacePrx remoteGui;
mutable armarx::ThreadPool::Handle threadHandle;
bool stopRequested = false;
-
-
private:
-
armarx::ControlTarget1DoFActuatorZeroTorque target;
DataPtr dataPtr;
Device* joint;
- mutable armarx::control::joint_controller::VelocityManipulatingTorqueController torqueController;
+ mutable armarx::control::joint_controller::VelocityManipulatingTorqueController
+ torqueController;
- mutable armarx::TripleBuffer<armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfiguration> controllerConfigBufferPublishToRt;
+ mutable armarx::TripleBuffer<
+ armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfiguration>
+ controllerConfigBufferPublishToRt;
- const armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfiguration initialTorqueConfigData;
+ const armarx::control::joint_controller::VelocityManipulatingTorqueControllerConfiguration
+ initialTorqueConfigData;
};
-}
+} // namespace devices::ethercat::sensor_actor_unit::armar7de::joint_controller
diff --git a/source/devices/ethercat/template_device/Config.h b/source/devices/ethercat/template_device/Config.h
index 6ab8ee47..6b33b683 100644
--- a/source/devices/ethercat/template_device/Config.h
+++ b/source/devices/ethercat/template_device/Config.h
@@ -11,8 +11,8 @@ namespace devices::ethercat::template_device
*/
struct Slave1DataConfig
{
- Slave1DataConfig(hwconfig::SlaveConfig& hwConfig)
- : anotherSensor(hwConfig.getLinearConfig("anotherSensor"))
+ Slave1DataConfig(hwconfig::SlaveConfig& hwConfig) :
+ anotherSensor(hwConfig.getLinearConfig("anotherSensor"))
{
}
@@ -24,8 +24,8 @@ namespace devices::ethercat::template_device
*/
struct Slave1SlaveConfig
{
- Slave1SlaveConfig(hwconfig::SlaveConfig& hwConfig)
- : ledCount(static_cast<uint8_t>(hwConfig.getUint("ledCount")))
+ Slave1SlaveConfig(hwconfig::SlaveConfig& hwConfig) :
+ ledCount(static_cast<uint8_t>(hwConfig.getUint("ledCount")))
{
}
@@ -39,10 +39,9 @@ namespace devices::ethercat::template_device
*/
struct Slave1CompleteConfig : public Slave1DataConfig, public Slave1SlaveConfig
{
- Slave1CompleteConfig(hwconfig::SlaveConfig& hwConfig)
- : Slave1DataConfig(hwConfig),
- Slave1SlaveConfig(hwConfig)
+ Slave1CompleteConfig(hwconfig::SlaveConfig& hwConfig) :
+ Slave1DataConfig(hwConfig), Slave1SlaveConfig(hwConfig)
{
}
};
-}
+} // namespace devices::ethercat::template_device
diff --git a/source/devices/ethercat/template_device/ControlTargets.h b/source/devices/ethercat/template_device/ControlTargets.h
index 4c779a45..da2585b1 100644
--- a/source/devices/ethercat/template_device/ControlTargets.h
+++ b/source/devices/ethercat/template_device/ControlTargets.h
@@ -29,7 +29,6 @@
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h>
-
namespace devices::ethercat::template_device
{
/**
@@ -57,12 +56,14 @@ namespace devices::ethercat::template_device
ControlTargetLedColor() = default;
ControlTargetLedColor(const ControlTargetLedColor&) = default;
ControlTargetLedColor(ControlTargetLedColor&&) = default;
+
ControlTargetLedColor(std::uint8_t r, std::uint8_t g, std::uint8_t b)
{
this->r = r;
this->g = g;
this->b = b;
}
+
ControlTargetLedColor& operator=(const ControlTargetLedColor&) = default;
ControlTargetLedColor& operator=(ControlTargetLedColor&&) = default;
diff --git a/source/devices/ethercat/template_device/Data.cpp b/source/devices/ethercat/template_device/Data.cpp
index eb24a05c..c18ee096 100644
--- a/source/devices/ethercat/template_device/Data.cpp
+++ b/source/devices/ethercat/template_device/Data.cpp
@@ -28,9 +28,7 @@
namespace devices::ethercat::template_device
{
- Data::Data(const Slave1DataConfig& config,
- SlaveOut* outputs,
- SlaveIn* inputs) :
+ Data::Data(const Slave1DataConfig& config, SlaveOut* outputs, SlaveIn* inputs) :
outputs(outputs), inputs(inputs), errorDecoder(outputs, inputs)
{
ARMARX_CHECK_EXPRESSION(outputs);
@@ -41,8 +39,7 @@ namespace devices::ethercat::template_device
* LinearConvertedValue caches converted values and uses the attributes a and offset for
* the conversion.
*/
- anotherSensorValue.init(&(outputs->anotherSensorValue),
- config.anotherSensor);
+ anotherSensorValue.init(&(outputs->anotherSensorValue), config.anotherSensor);
}
void
@@ -79,14 +76,12 @@ namespace devices::ethercat::template_device
return outputs->temperature;
}
-
std::uint16_t
Data::getMainUpdateRate()
{
return mainUpdateRate;
}
-
float
Data::getAnotherSensorValue()
{
@@ -100,7 +95,6 @@ namespace devices::ethercat::template_device
return sensorValueWithNonLinearConversion;
}
-
void
Data::setRedLedIntensity(std::uint8_t redIntensity)
{
diff --git a/source/devices/ethercat/template_device/Data.h b/source/devices/ethercat/template_device/Data.h
index 7bab6afe..df99f996 100644
--- a/source/devices/ethercat/template_device/Data.h
+++ b/source/devices/ethercat/template_device/Data.h
@@ -94,13 +94,10 @@ namespace devices::ethercat::template_device
}
};
-
class Data : public armarx::control::ethercat::DataInterface
{
public:
- Data(const Slave1DataConfig& config,
- SlaveOut* outputs,
- SlaveIn* inputs);
+ Data(const Slave1DataConfig& config, SlaveOut* outputs, SlaveIn* inputs);
void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration) override;
diff --git a/source/devices/ethercat/template_device/Device.cpp b/source/devices/ethercat/template_device/Device.cpp
index 76e2e571..35d16497 100644
--- a/source/devices/ethercat/template_device/Device.cpp
+++ b/source/devices/ethercat/template_device/Device.cpp
@@ -38,7 +38,6 @@
#include <devices/ethercat/common/h2t_devices.h>
#include <devices/ethercat/template_device/Slave.h>
-
namespace devices::ethercat::template_device
{
@@ -70,7 +69,6 @@ namespace devices::ethercat::template_device
hwConfig.getSubDeviceConfigsWithType("Led");
}
-
armarx::control::ethercat::DeviceInterface::TryAssignResult
Device::tryAssign(armarx::control::ethercat::SlaveInterface& slave)
{
@@ -86,7 +84,6 @@ namespace devices::ethercat::template_device
return result;
}
-
armarx::control::ethercat::DeviceInterface::AllAssignedResult
Device::onAllAssigned()
{
@@ -120,7 +117,8 @@ namespace devices::ethercat::template_device
{
if (!dataPtr)
{
- dataPtr = std::make_shared<Data>(slaveConfig, slavePtr->getOutputsPtr(), slavePtr->getInputsPtr());
+ dataPtr = std::make_shared<Data>(
+ slaveConfig, slavePtr->getOutputsPtr(), slavePtr->getInputsPtr());
// with data set we can create the joint controllers ...
ledColorController = std::make_shared<joint_controller::LedColorController>(dataPtr);
ledStopMovementController =
@@ -141,7 +139,6 @@ namespace devices::ethercat::template_device
{
}
-
void
Device::updateSensorValueStruct()
{
@@ -151,7 +148,6 @@ namespace devices::ethercat::template_device
dataPtr->getSensorValueWithNonLinearConversion();
}
-
DataPtr
Device::getData() const
{
@@ -164,14 +160,12 @@ namespace devices::ethercat::template_device
return dataPtr;
}
-
armarx::control::ethercat::SlaveIdentifier
Device::getSlaveIdentifier() const
{
return slaveIdentifier;
}
-
void
Device::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -194,7 +188,6 @@ namespace devices::ethercat::template_device
updateSensorValueStruct();
}
-
void
Device::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -207,14 +200,12 @@ namespace devices::ethercat::template_device
dataPtr->rtWriteTargetValues(sensorValuesTimestamp, timeSinceLastIteration);
}
-
const armarx::SensorValueBase*
Device::getSensorValue() const
{ // Here the SensorValue struct is given to armarx
return &sensorValue;
}
-
std::uint16_t
Device::getSlaveNumber() const
{
diff --git a/source/devices/ethercat/template_device/Device.h b/source/devices/ethercat/template_device/Device.h
index e40d4dbb..712791d3 100644
--- a/source/devices/ethercat/template_device/Device.h
+++ b/source/devices/ethercat/template_device/Device.h
@@ -41,7 +41,6 @@
#include <devices/ethercat/template_device/joint_controller/LedEmergencyStop.h>
#include <devices/ethercat/template_device/joint_controller/LedStopMovement.h>
-
namespace devices::ethercat::template_device
{
@@ -50,7 +49,6 @@ namespace devices::ethercat::template_device
using LedColorControllerPtr = std::shared_ptr<class LedColorController>;
} // namespace joint_controller
-
/*
* This class inherits from ControlDevice because there are Slave inputs on some
* slaves (to control the leds) and inherits from SensorDevice because there are Slave
@@ -70,6 +68,7 @@ namespace devices::ethercat::template_device
public:
Device(armarx::control::hardware_config::DeviceConfig& hwConfig,
VirtualRobot::RobotPtr const& robot);
+
~Device() override
{
}
diff --git a/source/devices/ethercat/template_device/Slave.cpp b/source/devices/ethercat/template_device/Slave.cpp
index df01fee4..a42b5438 100644
--- a/source/devices/ethercat/template_device/Slave.cpp
+++ b/source/devices/ethercat/template_device/Slave.cpp
@@ -32,7 +32,6 @@
#include <devices/ethercat/common/h2t_devices.h>
-
namespace devices::ethercat::template_device
{
Slave::Slave(const armarx::control::ethercat::SlaveIdentifier slaveIdentifier_) :
@@ -43,33 +42,28 @@ namespace devices::ethercat::template_device
setTag(this->slaveIdentifier.getName());
}
-
void
Slave::execute()
{
}
-
bool
Slave::prepareForRun()
{
return true;
}
-
bool
Slave::shutdown()
{
return true;
}
-
void
Slave::doMappings()
{
}
-
bool
Slave::hasError()
{
@@ -89,20 +83,17 @@ namespace devices::ethercat::template_device
return hasError;
}
-
void
Slave::prepareForSafeOp()
{
writeCoE();
}
-
void
Slave::finishPreparingForSafeOp()
{
}
-
void
Slave::prepareForOp()
{
@@ -110,7 +101,6 @@ namespace devices::ethercat::template_device
errorDecoder = ErrorDecoder(getOutputsPtr(), getInputsPtr());
}
-
void
Slave::finishPreparingForOp()
{
diff --git a/source/devices/ethercat/template_device/SlaveIO.h b/source/devices/ethercat/template_device/SlaveIO.h
index d2cc57a6..7f2ad5c3 100644
--- a/source/devices/ethercat/template_device/SlaveIO.h
+++ b/source/devices/ethercat/template_device/SlaveIO.h
@@ -29,7 +29,6 @@
#include <cstdint>
-
namespace devices::ethercat::template_device
{
/*
@@ -55,7 +54,6 @@ namespace devices::ethercat::template_device
std::uint32_t sensorValueWithNonLinearConversion;
} __attribute__((__packed__));
-
/**
* @brief PDO mapping master->sensorB
*/
diff --git a/source/devices/ethercat/template_device/joint_controller/LedColor.h b/source/devices/ethercat/template_device/joint_controller/LedColor.h
index 308efc7d..a0d8870f 100644
--- a/source/devices/ethercat/template_device/joint_controller/LedColor.h
+++ b/source/devices/ethercat/template_device/joint_controller/LedColor.h
@@ -49,5 +49,6 @@ namespace devices::ethercat::template_device::joint_controller
DataPtr data;
ControlTargetLedColor target;
};
+
using LedColorControllerPtr = std::shared_ptr<LedColorController>;
} // namespace devices::ethercat::template_device::joint_controller
diff --git a/source/devices/ethercat/template_device/joint_controller/LedEmergencyStop.cpp b/source/devices/ethercat/template_device/joint_controller/LedEmergencyStop.cpp
index 2010ad87..f6581c15 100644
--- a/source/devices/ethercat/template_device/joint_controller/LedEmergencyStop.cpp
+++ b/source/devices/ethercat/template_device/joint_controller/LedEmergencyStop.cpp
@@ -31,6 +31,7 @@ namespace devices::ethercat::template_device::joint_controller
LedEmergencyStopController::LedEmergencyStopController()
{
}
+
void
LedEmergencyStopController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
{
diff --git a/source/devices/ethercat/template_device/joint_controller/LedStopMovement.h b/source/devices/ethercat/template_device/joint_controller/LedStopMovement.h
index c30e9a3c..47f7e4ad 100644
--- a/source/devices/ethercat/template_device/joint_controller/LedStopMovement.h
+++ b/source/devices/ethercat/template_device/joint_controller/LedStopMovement.h
@@ -45,5 +45,6 @@ namespace devices::ethercat::template_device::joint_controller
private:
armarx::DummyControlTargetStopMovement target;
};
+
using LedStopMovementControllerPtr = std::shared_ptr<LedStopMovementController>;
} // namespace devices::ethercat::template_device::joint_controller
diff --git a/source/devices/ethercat/torso/armar6_prismatic/Data.h b/source/devices/ethercat/torso/armar6_prismatic/Data.h
index 727efbc7..855f0827 100644
--- a/source/devices/ethercat/torso/armar6_prismatic/Data.h
+++ b/source/devices/ethercat/torso/armar6_prismatic/Data.h
@@ -50,9 +50,9 @@ namespace devices::ethercat::torso::armar6_prismatic
{
public:
-
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- static SensorValueInfo<TorsoJointSensorValue> GetClassMemberInfo()
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION static SensorValueInfo<
+ TorsoJointSensorValue>
+ GetClassMemberInfo()
{
SensorValueInfo<TorsoJointSensorValue> svi;
svi.addBaseClass<SensorValue1DoFActuatorPosition>();
@@ -61,10 +61,9 @@ namespace devices::ethercat::torso::armar6_prismatic
svi.addBaseClass<SensorValue1DoFActuatorStatus>();
return svi;
}
-
};
using TorsoPrismaticJointDataPtr = std::shared_ptr<class TorsoPrismaticJointData>;
-}
+} // namespace devices::ethercat::torso::armar6_prismatic
diff --git a/source/devices/ethercat/torso/armar6_prismatic/Device.cpp b/source/devices/ethercat/torso/armar6_prismatic/Device.cpp
index 21bcc82f..519feecf 100644
--- a/source/devices/ethercat/torso/armar6_prismatic/Device.cpp
+++ b/source/devices/ethercat/torso/armar6_prismatic/Device.cpp
@@ -27,7 +27,6 @@
#include <ArmarXCore/core/logging/Logging.h>
-
namespace devices::ethercat::torso::armar6_prismatic
{
@@ -86,15 +85,13 @@ namespace devices::ethercat::torso::armar6_prismatic
return "PrismaticJoint_Armar6";
}
-
void
Device::postSwitchToSafeOp()
{
if (!dataPtr)
{
- dataPtr = std::make_unique<common::elmo::gold::Data>(elmoConfig,
- elmoPtr->getOutputsPtr(),
- elmoPtr->getInputsPtr());
+ dataPtr = std::make_unique<common::elmo::gold::Data>(
+ elmoConfig, elmoPtr->getOutputsPtr(), elmoPtr->getInputsPtr());
//with data set we can create the rest of the joint controller...
torsoPositionController = std::make_shared<joint_controller::PositionController>(
@@ -117,7 +114,6 @@ namespace devices::ethercat::torso::armar6_prismatic
ARMARX_CHECK_NOT_NULL(dataPtr);
}
-
void
Device::updateSensorValueStruct()
{
@@ -139,14 +135,12 @@ namespace devices::ethercat::torso::armar6_prismatic
sensorValue.status = dataPtr->getStatus();
}
-
const armarx::SensorValueBase*
Device::getSensorValue() const
{
return &sensorValue;
}
-
common::elmo::gold::Data*
Device::getData() const
{
@@ -158,14 +152,12 @@ namespace devices::ethercat::torso::armar6_prismatic
return dataPtr.get();
}
-
std::uint32_t
Device::getElmoSerial() const
{
return elmoIdentifier.serialNumber;
}
-
bool
Device::switchControlMode(armarx::ControlMode newMode)
{
@@ -197,7 +189,6 @@ namespace devices::ethercat::torso::armar6_prismatic
return elmoPtr->switchMode(modeToSet);
}
-
void
Device::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -211,7 +202,6 @@ namespace devices::ethercat::torso::armar6_prismatic
updateSensorValueStruct();
}
-
void
Device::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -224,7 +214,6 @@ namespace devices::ethercat::torso::armar6_prismatic
dataPtr->rtWriteTargetValues(sensorValuesTimestamp, timeSinceLastIteration);
}
-
std::uint16_t
Device::getElmoSlaveNumber() const
{
diff --git a/source/devices/ethercat/torso/armar6_prismatic/Device.h b/source/devices/ethercat/torso/armar6_prismatic/Device.h
index 5aab1a5c..6a4feb39 100644
--- a/source/devices/ethercat/torso/armar6_prismatic/Device.h
+++ b/source/devices/ethercat/torso/armar6_prismatic/Device.h
@@ -36,7 +36,6 @@
#include <devices/ethercat/torso/armar6_prismatic/joint_controller/Position.h>
#include <devices/ethercat/torso/armar6_prismatic/joint_controller/Velocity.h>
-
namespace devices::ethercat::torso::armar6_prismatic
{
diff --git a/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Emergency.cpp b/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Emergency.cpp
index d932a558..d7c3f051 100644
--- a/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Emergency.cpp
+++ b/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Emergency.cpp
@@ -1,6 +1,5 @@
#include "Emergency.h"
-
namespace devices::ethercat::torso::armar6_prismatic::joint_controller
{
@@ -12,14 +11,12 @@ namespace devices::ethercat::torso::armar6_prismatic::joint_controller
this->elmo = elmo;
}
-
armarx::ControlTargetBase*
EmergencyController::getControlTarget()
{
return ⌖
}
-
void
EmergencyController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
@@ -53,14 +50,12 @@ namespace devices::ethercat::torso::armar6_prismatic::joint_controller
}*/
}
-
const std::string&
EmergencyController::getControlMode() const
{
return armarx::ControlModes::EmergencyStop;
}
-
void
EmergencyController::rtPreActivateController()
{
@@ -71,7 +66,6 @@ namespace devices::ethercat::torso::armar6_prismatic::joint_controller
// elmo->activateQuickStop();
}
-
void
EmergencyController::rtPostDeactivateController()
{
diff --git a/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Emergency.h b/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Emergency.h
index 0f7cf553..2f96b1d5 100644
--- a/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Emergency.h
+++ b/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Emergency.h
@@ -8,24 +8,22 @@
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
// robot_devices
-#include <devices/ethercat/common/elmo/gold/Slave.h>
#include <devices/ethercat/common/elmo/gold/Data.h>
-
+#include <devices/ethercat/common/elmo/gold/Slave.h>
namespace devices::ethercat::torso::armar6_prismatic::joint_controller
{
using EmergencyControllerPtr = std::shared_ptr<class EmergencyController>;
-
class EmergencyController : public armarx::JointController
{
public:
-
EmergencyController(common::elmo::gold::Slave* elmo, common::elmo::gold::Data* data);
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
/**
* Returns the Target for this controller, but as this is the Emergency controller it will ignored.
* As this controller will just break
@@ -36,7 +34,6 @@ namespace devices::ethercat::torso::armar6_prismatic::joint_controller
const std::string& getControlMode() const override;
private:
-
common::elmo::gold::Slave* elmo;
common::elmo::gold::Data* dataPtr;
armarx::DummyControlTargetEmergencyStop target;
@@ -47,10 +44,8 @@ namespace devices::ethercat::torso::armar6_prismatic::joint_controller
// JointController interface
protected:
-
void rtPreActivateController() override;
void rtPostDeactivateController() override;
-
};
-}
+} // namespace devices::ethercat::torso::armar6_prismatic::joint_controller
diff --git a/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Position.cpp b/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Position.cpp
index bd64c0a0..4df03f47 100644
--- a/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Position.cpp
+++ b/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Position.cpp
@@ -10,12 +10,14 @@
// robot_devices
#include <devices/ethercat/torso/armar6_prismatic/Device.h>
-
namespace devices::ethercat::torso::armar6_prismatic::joint_controller
{
- PositionController::PositionController(Device* jointPtr, common::elmo::gold::Data* torsoDataPtr, armarx::control::joint_controller::PositionControllerConfigurationPtr config) : JointController(),
- target()
+ PositionController::PositionController(
+ Device* jointPtr,
+ common::elmo::gold::Data* torsoDataPtr,
+ armarx::control::joint_controller::PositionControllerConfigurationPtr config) :
+ JointController(), target()
{
this->jointPtr = jointPtr;
this->torsoData = torsoDataPtr;
@@ -37,8 +39,9 @@ namespace devices::ethercat::torso::armar6_prismatic::joint_controller
positionController.accuracy = config->accuracy;
}
-
- void PositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ PositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (target.isValid())
{
@@ -48,9 +51,12 @@ namespace devices::ethercat::torso::armar6_prismatic::joint_controller
float newVelocity = 0;
float currentPosition = torsoData->getActualRelativePosition();
- float targetPosition = boost::algorithm::clamp(target.position,
- std::min(currentPosition, positionController.positionLimitLo), // lo or current position
- std::max(currentPosition, positionController.positionLimitHi)); // hi or current position
+ float targetPosition = boost::algorithm::clamp(
+ target.position,
+ std::min(currentPosition,
+ positionController.positionLimitLo), // lo or current position
+ std::max(currentPosition,
+ positionController.positionLimitHi)); // hi or current position
positionController.currentV = currentVelocity;
positionController.dt = timeSinceLastIteration.toSecondsDouble();
positionController.currentPosition = currentPosition;
@@ -70,20 +76,21 @@ namespace devices::ethercat::torso::armar6_prismatic::joint_controller
}
}
-
- armarx::ControlTargetBase* PositionController::getControlTarget()
+ armarx::ControlTargetBase*
+ PositionController::getControlTarget()
{
return ⌖
}
-
- void PositionController::rtPreActivateController()
+ void
+ PositionController::rtPreActivateController()
{
currentVelocity = torsoData->getActualVelocity();
jointPtr->switchControlMode(armarx::eVelocityControl);
torsoData->setTargetAcceleration(maxAccelerationRad);
torsoData->setTargetDeceleration(maxDecelerationRad);
- ARMARX_INFO << "Setting acc to " << maxAccelerationRad << " and dec to " << maxDecelerationRad;
+ ARMARX_INFO << "Setting acc to " << maxAccelerationRad << " and dec to "
+ << maxDecelerationRad;
}
-}
+} // namespace devices::ethercat::torso::armar6_prismatic::joint_controller
diff --git a/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Position.h b/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Position.h
index da2579e9..ff726eb7 100644
--- a/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Position.h
+++ b/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Position.h
@@ -5,21 +5,19 @@
#include <memory>
// armarx
-#include <armarx/control/joint_controller/ControllerConfiguration.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <armarx/control/joint_controller/ControllerConfiguration.h>
// robot_devices
#include <devices/ethercat/common/elmo/gold/Data.h>
#include <devices/ethercat/sensor_actor_unit/armar6/joint_controller/Position.h>
-
namespace devices::ethercat::torso::armar6_prismatic
{
class Device;
}
-
namespace devices::ethercat::torso::armar6_prismatic::joint_controller
{
@@ -29,26 +27,28 @@ namespace devices::ethercat::torso::armar6_prismatic::joint_controller
{
public:
+ PositionController(
+ Device* jointPtr,
+ common::elmo::gold::Data* torsoDataPtr,
+ armarx::control::joint_controller::PositionControllerConfigurationPtr config);
- PositionController(Device* jointPtr, common::elmo::gold::Data* torsoDataPtr, armarx::control::joint_controller::PositionControllerConfigurationPtr config);
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
void rtPreActivateController() override;
private:
-
Device* jointPtr;
armarx::ControlTarget1DoFActuatorPosition target;
- common::elmo::gold::Data* torsoData;
+ common::elmo::gold::Data* torsoData;
- armarx::PositionThroughVelocityControllerWithAccelerationAndPositionBounds positionController;
+ armarx::PositionThroughVelocityControllerWithAccelerationAndPositionBounds
+ positionController;
float maxVelocityRad;
float maxAccelerationRad;
float maxDecelerationRad;
float p;
float currentVelocity;
-
};
-} // namespace devices::ethercat::torso::armar6_prismatic::joint_controller
+} // namespace devices::ethercat::torso::armar6_prismatic::joint_controller
diff --git a/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Velocity.cpp b/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Velocity.cpp
index df89f1d6..2ae4b8af 100644
--- a/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Velocity.cpp
+++ b/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Velocity.cpp
@@ -12,14 +12,14 @@
// robot_devices
#include <devices/ethercat/torso/armar6_prismatic/Device.h>
-
namespace devices::ethercat::torso::armar6_prismatic::joint_controller
{
- VelocityController::VelocityController(Device* jointPtr, common::elmo::gold::Data* jointData,
- armarx::control::joint_controller::VelocityControllerConfigurationPtr config) : JointController(),
- target(), setStart(true),
- config(*config)
+ VelocityController::VelocityController(
+ Device* jointPtr,
+ common::elmo::gold::Data* jointData,
+ armarx::control::joint_controller::VelocityControllerConfigurationPtr config) :
+ JointController(), target(), setStart(true), config(*config)
{
this->jointPtr = jointPtr;
@@ -28,7 +28,9 @@ namespace devices::ethercat::torso::armar6_prismatic::joint_controller
positionLimitLo = dataPtr->getSoftLimitLo();
}
- void VelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ VelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (target.isValid())
{
@@ -39,8 +41,8 @@ namespace devices::ethercat::torso::armar6_prismatic::joint_controller
newVel = boost::algorithm::clamp(newVel, -config.maxVelocityRad, config.maxVelocityRad);
- if ((dataPtr->getActualRelativePosition() > positionLimitHi && target.velocity > 0)
- || (dataPtr->getActualRelativePosition() < positionLimitLo && target.velocity < 0))
+ if ((dataPtr->getActualRelativePosition() > positionLimitHi && target.velocity > 0) ||
+ (dataPtr->getActualRelativePosition() < positionLimitLo && target.velocity < 0))
{
newVel = 0;
// ARMARX_INFO /*<< deactivateSpam(1)*/ << "Breaking now at " << dataPtr->getActualPosition() << " current: " << dataPtr->getActualCurrent();
@@ -55,20 +57,20 @@ namespace devices::ethercat::torso::armar6_prismatic::joint_controller
}
}
-
armarx::ControlTargetBase*
VelocityController::getControlTarget()
{
return ⌖
}
-
- void VelocityController::rtPreActivateController()
+ void
+ VelocityController::rtPreActivateController()
{
jointPtr->switchControlMode(armarx::eVelocityControl);
dataPtr->setTargetAcceleration(config.maxAccelerationRad);
dataPtr->setTargetDeceleration(config.maxDecelerationRad);
- ARMARX_INFO << VAROUT(config.maxVelocityRad) << VAROUT(config.maxAccelerationRad) << VAROUT(config.maxDecelerationRad);
+ ARMARX_INFO << VAROUT(config.maxVelocityRad) << VAROUT(config.maxAccelerationRad)
+ << VAROUT(config.maxDecelerationRad);
}
-}
+} // namespace devices::ethercat::torso::armar6_prismatic::joint_controller
diff --git a/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Velocity.h b/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Velocity.h
index d5f0d05b..78e13790 100644
--- a/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Velocity.h
+++ b/source/devices/ethercat/torso/armar6_prismatic/joint_controller/Velocity.h
@@ -2,46 +2,45 @@
// STD/STL
-#include <memory>
#include <chrono>
+#include <memory>
// armarx
-#include <armarx/control/joint_controller/ControllerConfiguration.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <armarx/control/joint_controller/ControllerConfiguration.h>
//#include <RobotAPI/components/units/RobotUnit/Targets/ActuatorVelocityTarget.h>
// robot_devices
-#include <devices/ethercat/common/elmo/gold/Slave.h>
#include <devices/ethercat/common/elmo/gold/Data.h>
-
+#include <devices/ethercat/common/elmo/gold/Slave.h>
namespace devices::ethercat::torso::armar6_prismatic
{
class Device;
}
-
namespace devices::ethercat::torso::armar6_prismatic::joint_controller
{
using VelocityControllerPtr = std::shared_ptr<class VelocityController>;
-
class VelocityController : public armarx::JointController
{
public:
+ VelocityController(
+ Device* jointPtr,
+ common::elmo::gold::Data* jointData,
+ armarx::control::joint_controller::VelocityControllerConfigurationPtr config);
- VelocityController(Device* jointPtr, common::elmo::gold::Data* jointData, armarx::control::joint_controller::VelocityControllerConfigurationPtr config);
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
armarx::ControlTargetBase* getControlTarget() override;
void rtPreActivateController() override;
private:
-
armarx::ControlTarget1DoFActuatorVelocity target;
common::elmo::gold::Data* dataPtr;
Device* jointPtr;
@@ -51,7 +50,6 @@ namespace devices::ethercat::torso::armar6_prismatic::joint_controller
float positionLimitHi = 0;
float positionLimitLo = 0;
armarx::control::joint_controller::VelocityControllerConfiguration config;
-
};
-} // namespace devices::ethercat::torso::armar6_prismatic::joint_controller
+} // namespace devices::ethercat::torso::armar6_prismatic::joint_controller
--
GitLab