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;