Adapted to changes in wbc.

ARC-OPT · Mar 1, 2024 · 7f926ab · 7f926ab
1 parent a209ece
commit 7f926ab
Show file tree

Hide file tree

Showing 13 changed files with 27 additions and 21 deletions.
diff --git 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
@@ -7,6 +7,7 @@
 #include <base/samples/Wrenches.hpp>
 #include <base/samples/RigidBodyState.hpp>
 #include <base/samples/RigidBodyStateSE3.hpp>
+#include <wbc/controllers/CartesianForcePIDController.hpp>
 
 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
@@ -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
@@ -5,6 +5,7 @@
 
 #include "ctrl_lib/CartesianPositionControllerBase.hpp"
 #include <base/samples/RigidBodyStateSE3.hpp>
+#include <wbc/controllers/CartesianPosPDController.hpp>
 
 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
@@ -74,32 +74,32 @@ bool CartesianRadialPotentialFields::readSetpoint(){
 
 void CartesianRadialPotentialFields::updateController(){
 
-    std::vector<PotentialFieldPtr> fields;
+    std::vector<wbc::PotentialFieldPtr> 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<RadialPotentialField>(3, pot_field_centers[i].sourceFrame);
+            wbc::PotentialFieldPtr field = std::make_shared<wbc::RadialPotentialField>(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<PotentialFieldPtr> &fields = controller->getFields();
+    const std::vector<wbc::PotentialFieldPtr> &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
@@ -5,6 +5,7 @@
 
 #include "ctrl_lib/CartesianRadialPotentialFieldsBase.hpp"
 #include <base/samples/RigidBodyStateSE3.hpp>
+#include <wbc/controllers/PotentialFieldsController.hpp>
 
 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<base::samples::RigidBodyState> pot_field_centers;
     wbc::PotentialFieldsController* controller;
-    std::vector<PotentialFieldInfo> field_infos;
+    std::vector<wbc::PotentialFieldInfo> field_infos;
 };
 }
 

diff --git 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<std::string> 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
@@ -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<PotentialFieldPtr> &fields = controller->getFields();
+    const std::vector<wbc::PotentialFieldPtr> &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
@@ -5,6 +5,7 @@
 
 #include "ctrl_lib/JointLimitAvoidanceBase.hpp"
 #include <base/commands/Joints.hpp>
+#include <wbc/controllers/JointLimitAvoidanceController.hpp>
 
 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
@@ -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
@@ -5,6 +5,7 @@
 
 #include "ctrl_lib/JointPositionControllerBase.hpp"
 #include <base/commands/Joints.hpp>
+#include <wbc/controllers/JointPosPDController.hpp>
 
 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
@@ -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
@@ -5,6 +5,7 @@
 
 #include "ctrl_lib/JointTorqueControllerBase.hpp"
 #include <base/commands/Joints.hpp>
+#include <wbc/controllers/JointTorquePIDController.hpp>
 
 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;