From 7f926ab91c12a49614cce36256c8877525f5e2fd Mon Sep 17 00:00:00 2001 From: Sebastian Kasperski Date: Fri, 1 Mar 2024 14:37:18 +0100 Subject: [PATCH] Adapted to changes in wbc. --- tasks/CartesianForceController.cpp | 4 ++-- tasks/CartesianForceController.hpp | 3 ++- tasks/CartesianPositionController.cpp | 2 +- tasks/CartesianPositionController.hpp | 3 ++- tasks/CartesianRadialPotentialFields.cpp | 10 +++++----- tasks/CartesianRadialPotentialFields.hpp | 5 +++-- tasks/ControllerTask.hpp | 4 ++-- tasks/JointLimitAvoidance.cpp | 4 ++-- tasks/JointLimitAvoidance.hpp | 3 ++- tasks/JointPositionController.cpp | 2 +- tasks/JointPositionController.hpp | 3 ++- tasks/JointTorqueController.cpp | 2 +- tasks/JointTorqueController.hpp | 3 ++- 13 files changed, 27 insertions(+), 21 deletions(-) diff --git a/tasks/CartesianForceController.cpp b/tasks/CartesianForceController.cpp index 05687fe..fd14e38 100644 --- a/tasks/CartesianForceController.cpp +++ b/tasks/CartesianForceController.cpp @@ -19,7 +19,7 @@ bool CartesianForceController::configureHook(){ return false; controller = new wbc::CartesianForcePIDController(); - PIDCtrlParams params(controller->getDimension()); + wbc::PIDCtrlParams params(controller->getDimension()); params.p_gain = _p_gain.get(); params.i_gain.setZero(); params.d_gain.setZero(); @@ -103,7 +103,7 @@ void CartesianForceController::updateController(){ _control_error.write(controller->getControlError()); } -const base::VectorXd& CartesianForceController::computeActivation(ActivationFunction &activation_function){ +const base::VectorXd& CartesianForceController::computeActivation(wbc::ActivationFunction &activation_function){ tmp.resize(controller->getDimension()); for(uint i = 0; i < 3; i++){ tmp(i) = fabs(control_output.twist.linear(i))/controller->maxCtrlOutput()(i); diff --git a/tasks/CartesianForceController.hpp b/tasks/CartesianForceController.hpp index 8b7cb27..357bc37 100644 --- a/tasks/CartesianForceController.hpp +++ b/tasks/CartesianForceController.hpp @@ -7,6 +7,7 @@ #include #include #include +#include namespace ctrl_lib{ @@ -36,7 +37,7 @@ class CartesianForceController : public CartesianForceControllerBase /** Compute output of the controller*/ virtual void updateController(); /** Compute Activation function*/ - virtual const base::VectorXd& computeActivation(ActivationFunction& activation_function); + virtual const base::VectorXd& computeActivation(wbc::ActivationFunction& activation_function); bool isValid(const base::Wrench &w){ diff --git a/tasks/CartesianPositionController.cpp b/tasks/CartesianPositionController.cpp index 73f69fc..e1bb738 100644 --- a/tasks/CartesianPositionController.cpp +++ b/tasks/CartesianPositionController.cpp @@ -78,7 +78,7 @@ void CartesianPositionController::updateController(){ _control_error.write(controller->getControlError()); } -const base::VectorXd& CartesianPositionController::computeActivation(ActivationFunction &activation_function){ +const base::VectorXd& CartesianPositionController::computeActivation(wbc::ActivationFunction &activation_function){ tmp.resize(6); tmp.segment(0,3) = control_output.twist.linear; tmp.segment(3,3) = control_output.twist.angular; diff --git a/tasks/CartesianPositionController.hpp b/tasks/CartesianPositionController.hpp index ba4c31f..05837cf 100644 --- a/tasks/CartesianPositionController.hpp +++ b/tasks/CartesianPositionController.hpp @@ -5,6 +5,7 @@ #include "ctrl_lib/CartesianPositionControllerBase.hpp" #include +#include namespace ctrl_lib { @@ -34,7 +35,7 @@ class CartesianPositionController : public CartesianPositionControllerBase /** Compute output of the controller*/ virtual void updateController(); /** Compute Activation function*/ - virtual const base::VectorXd& computeActivation(ActivationFunction& activation_function); + virtual const base::VectorXd& computeActivation(wbc::ActivationFunction& activation_function); base::samples::RigidBodyStateSE3 setpoint, control_output, feedback; wbc::CartesianPosPDController* controller; diff --git a/tasks/CartesianRadialPotentialFields.cpp b/tasks/CartesianRadialPotentialFields.cpp index 630b29d..c04cd0b 100644 --- a/tasks/CartesianRadialPotentialFields.cpp +++ b/tasks/CartesianRadialPotentialFields.cpp @@ -74,32 +74,32 @@ bool CartesianRadialPotentialFields::readSetpoint(){ void CartesianRadialPotentialFields::updateController(){ - std::vector fields; + std::vector fields; for(size_t i = 0; i < pot_field_centers.size(); i++) { // Only use the fields with correct frame ID if(pot_field_centers[i].targetFrame == feedback.frame_id){ - PotentialFieldPtr field = std::make_shared(3, pot_field_centers[i].sourceFrame); + wbc::PotentialFieldPtr field = std::make_shared(3, pot_field_centers[i].sourceFrame); field->pot_field_center = pot_field_centers[i].position; field->influence_distance = influence_distance; fields.push_back(field); } } controller->setFields(fields); - control_output = ((CartesianPotentialFieldsController*)controller)->update(feedback); + control_output = ((wbc::CartesianPotentialFieldsController*)controller)->update(feedback); _control_output.write(control_output); _field_infos.write(controller->getFieldInfos()); } -const base::VectorXd& CartesianRadialPotentialFields::computeActivation(ActivationFunction &activation_function){ +const base::VectorXd& CartesianRadialPotentialFields::computeActivation(wbc::ActivationFunction &activation_function){ tmp.resize(6); tmp.setZero(); // Get highest activation from all fields double max_activation = 0; - const std::vector &fields = controller->getFields(); + const std::vector &fields = controller->getFields(); for(uint i = 0; i < fields.size(); i++){ double activation; if(fields[i]->distance.norm() > fields[i]->influence_distance) diff --git a/tasks/CartesianRadialPotentialFields.hpp b/tasks/CartesianRadialPotentialFields.hpp index 6109736..b20308b 100644 --- a/tasks/CartesianRadialPotentialFields.hpp +++ b/tasks/CartesianRadialPotentialFields.hpp @@ -5,6 +5,7 @@ #include "ctrl_lib/CartesianRadialPotentialFieldsBase.hpp" #include +#include namespace ctrl_lib { @@ -35,13 +36,13 @@ class CartesianRadialPotentialFields : public CartesianRadialPotentialFieldsBase /** Compute output of the controller*/ virtual void updateController(); /** Compute Activation function*/ - virtual const base::VectorXd& computeActivation(ActivationFunction& activation_function); + virtual const base::VectorXd& computeActivation(wbc::ActivationFunction& activation_function); double influence_distance; base::samples::RigidBodyStateSE3 control_output, feedback; std::vector pot_field_centers; wbc::PotentialFieldsController* controller; - std::vector field_infos; + std::vector field_infos; }; } diff --git a/tasks/ControllerTask.hpp b/tasks/ControllerTask.hpp index 354cc67..9821aa1 100644 --- a/tasks/ControllerTask.hpp +++ b/tasks/ControllerTask.hpp @@ -33,10 +33,10 @@ class ControllerTask : public ControllerTaskBase /** Compute output of the controller*/ virtual void updateController() = 0; /** Compute Activation function*/ - virtual const base::VectorXd& computeActivation(ActivationFunction& activation_function) = 0; + virtual const base::VectorXd& computeActivation(wbc::ActivationFunction& activation_function) = 0; std::vector field_names; - ActivationFunction activation_function; + wbc::ActivationFunction activation_function; base::VectorXd tmp; base::Time stamp; diff --git a/tasks/JointLimitAvoidance.cpp b/tasks/JointLimitAvoidance.cpp index d5372f5..61dbb61 100644 --- a/tasks/JointLimitAvoidance.cpp +++ b/tasks/JointLimitAvoidance.cpp @@ -74,11 +74,11 @@ void JointLimitAvoidance::updateController(){ _current_joint_limits.write(joint_limits); } -const base::VectorXd& JointLimitAvoidance::computeActivation(ActivationFunction &activation_function){ +const base::VectorXd& JointLimitAvoidance::computeActivation(wbc::ActivationFunction &activation_function){ tmp.resize(controller->getDimension()); tmp.setZero(); - const std::vector &fields = controller->getFields(); + const std::vector &fields = controller->getFields(); for(uint i = 0; i < fields.size(); i++){ double dist = fields[i]->distance.norm(); if(dist > fields[i]->influence_distance) diff --git a/tasks/JointLimitAvoidance.hpp b/tasks/JointLimitAvoidance.hpp index 1cfa51e..121ea53 100644 --- a/tasks/JointLimitAvoidance.hpp +++ b/tasks/JointLimitAvoidance.hpp @@ -5,6 +5,7 @@ #include "ctrl_lib/JointLimitAvoidanceBase.hpp" #include +#include namespace ctrl_lib{ @@ -35,7 +36,7 @@ class JointLimitAvoidance : public JointLimitAvoidanceBase /** Compute output of the controller*/ virtual void updateController(); /** Compute Activation function*/ - virtual const base::VectorXd& computeActivation(ActivationFunction& activation_function); + virtual const base::VectorXd& computeActivation(wbc::ActivationFunction& activation_function); base::JointLimits joint_limits; base::samples::Joints feedback; diff --git a/tasks/JointPositionController.cpp b/tasks/JointPositionController.cpp index bf063c2..404723a 100644 --- a/tasks/JointPositionController.cpp +++ b/tasks/JointPositionController.cpp @@ -78,7 +78,7 @@ void JointPositionController::updateController(){ _control_error.write(controller->getControlError()); } -const base::VectorXd& JointPositionController::computeActivation(ActivationFunction &activation_function){ +const base::VectorXd& JointPositionController::computeActivation(wbc::ActivationFunction &activation_function){ tmp.resize(control_output.size()); for(uint i = 0; i < tmp.size(); i++) tmp(i) = fabs(control_output[i].speed)/controller->maxCtrlOutput()(i); diff --git a/tasks/JointPositionController.hpp b/tasks/JointPositionController.hpp index a371f7b..a2422e9 100644 --- a/tasks/JointPositionController.hpp +++ b/tasks/JointPositionController.hpp @@ -5,6 +5,7 @@ #include "ctrl_lib/JointPositionControllerBase.hpp" #include +#include namespace ctrl_lib { @@ -34,7 +35,7 @@ class JointPositionController : public JointPositionControllerBase /** Compute output of the controller*/ virtual void updateController(); /** Compute Activation function*/ - virtual const base::VectorXd& computeActivation(ActivationFunction& activation_function); + virtual const base::VectorXd& computeActivation(wbc::ActivationFunction& activation_function); base::commands::Joints setpoint, control_output, feedback; wbc::JointPosPDController* controller; diff --git a/tasks/JointTorqueController.cpp b/tasks/JointTorqueController.cpp index b9ae60e..8d3db9b 100644 --- a/tasks/JointTorqueController.cpp +++ b/tasks/JointTorqueController.cpp @@ -75,7 +75,7 @@ void JointTorqueController::updateController(){ _control_error.write(controller->getControlError()); } -const base::VectorXd& JointTorqueController::computeActivation(ActivationFunction &activation_function){ +const base::VectorXd& JointTorqueController::computeActivation(wbc::ActivationFunction &activation_function){ tmp.resize(control_output.size()); for(uint i = 0; i < tmp.size(); i++) tmp(i) = fabs(control_output[i].speed)/controller->maxCtrlOutput()(i); diff --git a/tasks/JointTorqueController.hpp b/tasks/JointTorqueController.hpp index 1506db1..0b388b6 100644 --- a/tasks/JointTorqueController.hpp +++ b/tasks/JointTorqueController.hpp @@ -5,6 +5,7 @@ #include "ctrl_lib/JointTorqueControllerBase.hpp" #include +#include namespace ctrl_lib{ @@ -34,7 +35,7 @@ class JointTorqueController : public JointTorqueControllerBase /** Compute output of the controller*/ virtual void updateController(); /** Compute Activation function*/ - virtual const base::VectorXd& computeActivation(ActivationFunction& activation_function); + virtual const base::VectorXd& computeActivation(wbc::ActivationFunction& activation_function); base::commands::Joints setpoint, control_output, feedback; wbc::JointTorquePIDController* controller;