[ros-ros-controllers] 01/07: Imported Upstream version 0.11.1
Leopold Palomo-Avellaneda
leo at alaxarxa.net
Mon Jul 17 12:58:39 UTC 2017
This is an automated email from the git hooks/post-receive script.
lepalom-guest pushed a commit to branch master
in repository ros-ros-controllers.
commit ee12430778300c3d62690798d036f13f9a20deb1
Author: Leopold Palomo-Avellaneda <leopold.palomo at upc.edu>
Date: Thu Jul 28 12:37:13 2016 +0200
Imported Upstream version 0.11.1
---
diff_drive_controller/CHANGELOG.rst | 6 ++++++
diff_drive_controller/package.xml | 2 +-
effort_controllers/CHANGELOG.rst | 8 ++++++++
.../effort_controllers/joint_position_controller.h | 7 ++++++-
.../effort_controllers/joint_velocity_controller.h | 7 ++++++-
effort_controllers/package.xml | 2 +-
.../src/joint_position_controller.cpp | 19 +++++++++++++-----
.../src/joint_velocity_controller.cpp | 23 ++++++++++++++--------
force_torque_sensor_controller/CHANGELOG.rst | 6 ++++++
force_torque_sensor_controller/package.xml | 2 +-
forward_command_controller/CHANGELOG.rst | 6 ++++++
forward_command_controller/package.xml | 2 +-
gripper_action_controller/CHANGELOG.rst | 6 ++++++
gripper_action_controller/package.xml | 2 +-
imu_sensor_controller/CHANGELOG.rst | 6 ++++++
imu_sensor_controller/package.xml | 2 +-
joint_state_controller/CHANGELOG.rst | 6 ++++++
joint_state_controller/package.xml | 2 +-
joint_trajectory_controller/CHANGELOG.rst | 8 ++++++++
.../joint_trajectory_controller_impl.h | 15 ++++++++++++++
joint_trajectory_controller/package.xml | 2 +-
position_controllers/CHANGELOG.rst | 6 ++++++
position_controllers/package.xml | 2 +-
ros_controllers/CHANGELOG.rst | 6 ++++++
ros_controllers/package.xml | 2 +-
rqt_joint_trajectory_controller/CHANGELOG.rst | 6 ++++++
rqt_joint_trajectory_controller/package.xml | 2 +-
velocity_controllers/CHANGELOG.rst | 8 ++++++++
.../joint_position_controller.h | 7 ++++++-
velocity_controllers/package.xml | 2 +-
.../src/joint_position_controller.cpp | 17 ++++++++++++----
31 files changed, 165 insertions(+), 32 deletions(-)
diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst
index 03e0fb2..b9c56ef 100644
--- a/diff_drive_controller/CHANGELOG.rst
+++ b/diff_drive_controller/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package diff_drive_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.11.1 (2016-05-23)
+-------------------
+
+0.11.0 (2016-05-03)
+-------------------
+
0.10.0 (2015-11-20)
-------------------
* Address -Wunused-parameter warnings
diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml
index 6b831cc..ae80f66 100644
--- a/diff_drive_controller/package.xml
+++ b/diff_drive_controller/package.xml
@@ -1,6 +1,6 @@
<package>
<name>diff_drive_controller</name>
- <version>0.10.0</version>
+ <version>0.11.1</version>
<description>Controller for a differential drive mobile base.</description>
<maintainer email="bence.magyar at pal-robotics.com">Bence Magyar</maintainer>
diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst
index c5fb7d1..b5ea7b3 100644
--- a/effort_controllers/CHANGELOG.rst
+++ b/effort_controllers/CHANGELOG.rst
@@ -2,6 +2,14 @@
Changelog for package effort_controllers
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.11.1 (2016-05-23)
+-------------------
+
+0.11.0 (2016-05-03)
+-------------------
+* Add antinwindup to get and setGains logic for underlying PID controller
+* Contributors: Paul Bovbel
+
0.10.0 (2015-11-20)
-------------------
diff --git a/effort_controllers/include/effort_controllers/joint_position_controller.h b/effort_controllers/include/effort_controllers/joint_position_controller.h
index 6246ccb..15267c8 100644
--- a/effort_controllers/include/effort_controllers/joint_position_controller.h
+++ b/effort_controllers/include/effort_controllers/joint_position_controller.h
@@ -141,6 +141,11 @@ public:
void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
/**
+ * \brief Get the PID parameters
+ */
+ void getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup);
+
+ /**
* \brief Print debug info to console
*/
void printDebug();
@@ -148,7 +153,7 @@ public:
/**
* \brief Get the PID parameters
*/
- void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min);
+ void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup = false);
/**
* \brief Get the name of the joint this controller uses
diff --git a/effort_controllers/include/effort_controllers/joint_velocity_controller.h b/effort_controllers/include/effort_controllers/joint_velocity_controller.h
index 58a8711..c6f41ce 100644
--- a/effort_controllers/include/effort_controllers/joint_velocity_controller.h
+++ b/effort_controllers/include/effort_controllers/joint_velocity_controller.h
@@ -126,6 +126,11 @@ public:
void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
/**
+ * \brief Get the PID parameters
+ */
+ void getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup);
+
+ /**
* \brief Print debug info to console
*/
void printDebug();
@@ -133,7 +138,7 @@ public:
/**
* \brief Get the PID parameters
*/
- void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min);
+ void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup = false);
/**
* \brief Get the name of the joint this controller uses
diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml
index 5395207..5e2d51b 100644
--- a/effort_controllers/package.xml
+++ b/effort_controllers/package.xml
@@ -1,6 +1,6 @@
<package>
<name>effort_controllers</name>
- <version>0.10.0</version>
+ <version>0.11.1</version>
<description>effort_controllers</description>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
diff --git a/effort_controllers/src/joint_position_controller.cpp b/effort_controllers/src/joint_position_controller.cpp
index 9a2608c..a9f4be5 100644
--- a/effort_controllers/src/joint_position_controller.cpp
+++ b/effort_controllers/src/joint_position_controller.cpp
@@ -58,7 +58,7 @@ bool JointPositionController::init(hardware_interface::EffortJointInterface *rob
{
// Get joint name from parameter server
std::string joint_name;
- if (!n.getParam("joint", joint_name))
+ if (!n.getParam("joint", joint_name))
{
ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str());
return false;
@@ -95,14 +95,20 @@ bool JointPositionController::init(hardware_interface::EffortJointInterface *rob
return true;
}
-void JointPositionController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min)
+void JointPositionController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup)
{
- pid_controller_.setGains(p,i,d,i_max,i_min);
+ pid_controller_.setGains(p,i,d,i_max,i_min,antiwindup);
+}
+
+void JointPositionController::getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup)
+{
+ pid_controller_.getGains(p,i,d,i_max,i_min,antiwindup);
}
void JointPositionController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
{
- pid_controller_.getGains(p,i,d,i_max,i_min);
+ bool dummy;
+ pid_controller_.getGains(p,i,d,i_max,i_min,dummy);
}
void JointPositionController::printDebug()
@@ -224,11 +230,14 @@ void JointPositionController::update(const ros::Time& time, const ros::Duration&
controller_state_publisher_->msg_.command = commanded_effort;
double dummy;
+ bool antiwindup;
getGains(controller_state_publisher_->msg_.p,
controller_state_publisher_->msg_.i,
controller_state_publisher_->msg_.d,
controller_state_publisher_->msg_.i_clamp,
- dummy);
+ dummy,
+ antiwindup);
+ controller_state_publisher_->msg_.antiwindup = static_cast<char>(antiwindup);
controller_state_publisher_->unlockAndPublish();
}
}
diff --git a/effort_controllers/src/joint_velocity_controller.cpp b/effort_controllers/src/joint_velocity_controller.cpp
index 6ba7a8f..5493fd9 100644
--- a/effort_controllers/src/joint_velocity_controller.cpp
+++ b/effort_controllers/src/joint_velocity_controller.cpp
@@ -86,16 +86,20 @@ bool JointVelocityController::init(hardware_interface::EffortJointInterface *rob
return true;
}
-
-void JointVelocityController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min)
+void JointVelocityController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup)
{
- pid_controller_.setGains(p,i,d,i_max,i_min);
+ pid_controller_.setGains(p,i,d,i_max,i_min,antiwindup);
+}
+void JointVelocityController::getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup)
+{
+ pid_controller_.getGains(p,i,d,i_max,i_min,antiwindup);
}
void JointVelocityController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
{
- pid_controller_.getGains(p,i,d,i_max,i_min);
+ bool dummy;
+ pid_controller_.getGains(p,i,d,i_max,i_min,dummy);
}
void JointVelocityController::printDebug()
@@ -149,11 +153,14 @@ void JointVelocityController::update(const ros::Time& time, const ros::Duration&
controller_state_publisher_->msg_.command = commanded_effort;
double dummy;
+ bool antiwindup;
getGains(controller_state_publisher_->msg_.p,
- controller_state_publisher_->msg_.i,
- controller_state_publisher_->msg_.d,
- controller_state_publisher_->msg_.i_clamp,
- dummy);
+ controller_state_publisher_->msg_.i,
+ controller_state_publisher_->msg_.d,
+ controller_state_publisher_->msg_.i_clamp,
+ dummy,
+ antiwindup);
+ controller_state_publisher_->msg_.antiwindup = static_cast<char>(antiwindup);
controller_state_publisher_->unlockAndPublish();
}
}
diff --git a/force_torque_sensor_controller/CHANGELOG.rst b/force_torque_sensor_controller/CHANGELOG.rst
index 6c2652a..3f19d82 100644
--- a/force_torque_sensor_controller/CHANGELOG.rst
+++ b/force_torque_sensor_controller/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package force_torque_sensor_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.11.1 (2016-05-23)
+-------------------
+
+0.11.0 (2016-05-03)
+-------------------
+
0.10.0 (2015-11-20)
-------------------
* Address -Wunused-parameter warnings
diff --git a/force_torque_sensor_controller/package.xml b/force_torque_sensor_controller/package.xml
index e3091ca..078915c 100644
--- a/force_torque_sensor_controller/package.xml
+++ b/force_torque_sensor_controller/package.xml
@@ -1,6 +1,6 @@
<package>
<name>force_torque_sensor_controller</name>
- <version>0.10.0</version>
+ <version>0.11.1</version>
<description>Controller to publish state of force-torque sensors</description>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst
index e794cd0..f0a17b7 100644
--- a/forward_command_controller/CHANGELOG.rst
+++ b/forward_command_controller/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package forward_command_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.11.1 (2016-05-23)
+-------------------
+
+0.11.0 (2016-05-03)
+-------------------
+
0.10.0 (2015-11-20)
-------------------
* Address -Wunused-parameter warnings
diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml
index 1ac22a1..450446d 100644
--- a/forward_command_controller/package.xml
+++ b/forward_command_controller/package.xml
@@ -1,6 +1,6 @@
<package>
<name>forward_command_controller</name>
- <version>0.10.0</version>
+ <version>0.11.1</version>
<description>forward_command_controller</description>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
diff --git a/gripper_action_controller/CHANGELOG.rst b/gripper_action_controller/CHANGELOG.rst
index 58b2398..864f071 100644
--- a/gripper_action_controller/CHANGELOG.rst
+++ b/gripper_action_controller/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package gripper_action_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.11.1 (2016-05-23)
+-------------------
+
+0.11.0 (2016-05-03)
+-------------------
+
0.10.0 (2015-11-20)
-------------------
diff --git a/gripper_action_controller/package.xml b/gripper_action_controller/package.xml
index c506f8d..9781750 100644
--- a/gripper_action_controller/package.xml
+++ b/gripper_action_controller/package.xml
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>gripper_action_controller</name>
- <version>0.10.0</version>
+ <version>0.11.1</version>
<description>The gripper_action_controller package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
diff --git a/imu_sensor_controller/CHANGELOG.rst b/imu_sensor_controller/CHANGELOG.rst
index 3bb398c..31da442 100644
--- a/imu_sensor_controller/CHANGELOG.rst
+++ b/imu_sensor_controller/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package imu_sensor_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.11.1 (2016-05-23)
+-------------------
+
+0.11.0 (2016-05-03)
+-------------------
+
0.10.0 (2015-11-20)
-------------------
* Fixed covariances in ImuSensorController::update
diff --git a/imu_sensor_controller/package.xml b/imu_sensor_controller/package.xml
index 170864e..b0ee6b3 100644
--- a/imu_sensor_controller/package.xml
+++ b/imu_sensor_controller/package.xml
@@ -1,6 +1,6 @@
<package>
<name>imu_sensor_controller</name>
- <version>0.10.0</version>
+ <version>0.11.1</version>
<description>Controller to publish state of IMU sensors</description>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
diff --git a/joint_state_controller/CHANGELOG.rst b/joint_state_controller/CHANGELOG.rst
index c860679..7bb2899 100644
--- a/joint_state_controller/CHANGELOG.rst
+++ b/joint_state_controller/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package joint_state_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.11.1 (2016-05-23)
+-------------------
+
+0.11.0 (2016-05-03)
+-------------------
+
0.10.0 (2015-11-20)
-------------------
* Address -Wunused-parameter warnings
diff --git a/joint_state_controller/package.xml b/joint_state_controller/package.xml
index fb2094f..239755b 100644
--- a/joint_state_controller/package.xml
+++ b/joint_state_controller/package.xml
@@ -1,6 +1,6 @@
<package format="2">
<name>joint_state_controller</name>
- <version>0.10.0</version>
+ <version>0.11.1</version>
<description>Controller to publish joint state</description>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst
index 0da45c8..75bbc25 100644
--- a/joint_trajectory_controller/CHANGELOG.rst
+++ b/joint_trajectory_controller/CHANGELOG.rst
@@ -2,6 +2,14 @@
Changelog for package joint_trajectory_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.11.1 (2016-05-23)
+-------------------
+* Write feedback for the RealtimeServerGoalHandle to publish on the non-realtime thread.
+* Contributors: Miguel Prada
+
+0.11.0 (2016-05-03)
+-------------------
+
0.10.0 (2015-11-20)
-------------------
* Add joint limits spec to rrbot test robot
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h
index 2455348..25f913f 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h
@@ -441,6 +441,20 @@ update(const ros::Time& time, const ros::Duration& period)
hw_iface_adapter_.updateCommand(time_data.uptime, time_data.period,
desired_state_, state_error_);
+ // Set action feedback
+ if (rt_active_goal_)
+ {
+ rt_active_goal_->preallocated_feedback_->header.stamp = time_data_.readFromRT()->time;
+ rt_active_goal_->preallocated_feedback_->desired.positions = desired_state_.position;
+ rt_active_goal_->preallocated_feedback_->desired.velocities = desired_state_.velocity;
+ rt_active_goal_->preallocated_feedback_->desired.accelerations = desired_state_.acceleration;
+ rt_active_goal_->preallocated_feedback_->actual.positions = current_state_.position;
+ rt_active_goal_->preallocated_feedback_->actual.velocities = current_state_.velocity;
+ rt_active_goal_->preallocated_feedback_->error.positions = state_error_.position;
+ rt_active_goal_->preallocated_feedback_->error.velocities = state_error_.velocity;
+ rt_active_goal_->setFeedback( rt_active_goal_->preallocated_feedback_ );
+ }
+
// Publish state
publishState(time_data.uptime);
}
@@ -555,6 +569,7 @@ goalCB(GoalHandle gh)
RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh));
const bool update_ok = updateTrajectoryCommand(internal::share_member(gh.getGoal(), gh.getGoal()->trajectory),
rt_goal);
+ rt_goal->preallocated_feedback_->joint_names = joint_names_;
if (update_ok)
{
diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml
index d853ff1..2dcbbe7 100644
--- a/joint_trajectory_controller/package.xml
+++ b/joint_trajectory_controller/package.xml
@@ -1,7 +1,7 @@
<package>
<name>joint_trajectory_controller</name>
<description> Controller for executing joint-space trajectories on a group of joints. </description>
- <version>0.10.0</version>
+ <version>0.11.1</version>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst
index 34c6889..2cd6e1d 100644
--- a/position_controllers/CHANGELOG.rst
+++ b/position_controllers/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package position_controllers
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.11.1 (2016-05-23)
+-------------------
+
+0.11.0 (2016-05-03)
+-------------------
+
0.10.0 (2015-11-20)
-------------------
diff --git a/position_controllers/package.xml b/position_controllers/package.xml
index bff4d99..d592e53 100644
--- a/position_controllers/package.xml
+++ b/position_controllers/package.xml
@@ -1,6 +1,6 @@
<package>
<name>position_controllers</name>
- <version>0.10.0</version>
+ <version>0.11.1</version>
<description>position_controllers</description>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
diff --git a/ros_controllers/CHANGELOG.rst b/ros_controllers/CHANGELOG.rst
index f27d19b..26c37dd 100644
--- a/ros_controllers/CHANGELOG.rst
+++ b/ros_controllers/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package ros_controllers
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.11.1 (2016-05-23)
+-------------------
+
+0.11.0 (2016-05-03)
+-------------------
+
0.10.0 (2015-11-20)
-------------------
diff --git a/ros_controllers/package.xml b/ros_controllers/package.xml
index 58e3ee1..6de3624 100644
--- a/ros_controllers/package.xml
+++ b/ros_controllers/package.xml
@@ -1,6 +1,6 @@
<package>
<name>ros_controllers</name>
- <version>0.10.0</version>
+ <version>0.11.1</version>
<description>Library of ros controllers</description>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst
index a165356..0582821 100644
--- a/rqt_joint_trajectory_controller/CHANGELOG.rst
+++ b/rqt_joint_trajectory_controller/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package rqt_joint_trajectory_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.11.1 (2016-05-23)
+-------------------
+
+0.11.0 (2016-05-03)
+-------------------
+
0.10.0 (2015-11-20)
-------------------
* Adapt to new controller_manager_msgs/ControllerState message definition
diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml
index 919a0c0..4cd6fda 100644
--- a/rqt_joint_trajectory_controller/package.xml
+++ b/rqt_joint_trajectory_controller/package.xml
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>rqt_joint_trajectory_controller</name>
- <version>0.10.0</version>
+ <version>0.11.1</version>
<description>Graphical frontend for interacting with joint_trajectory_controller instances.</description>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst
index e9edbc2..c48e4f3 100644
--- a/velocity_controllers/CHANGELOG.rst
+++ b/velocity_controllers/CHANGELOG.rst
@@ -2,6 +2,14 @@
Changelog for package velocity_controllers
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.11.1 (2016-05-23)
+-------------------
+
+0.11.0 (2016-05-03)
+-------------------
+* Add antinwindup to get and setGains logic for underlying PID controller
+* Contributors: Paul Bovbel
+
0.10.0 (2015-11-20)
-------------------
diff --git a/velocity_controllers/include/velocity_controllers/joint_position_controller.h b/velocity_controllers/include/velocity_controllers/joint_position_controller.h
index 9d10bb7..716660b 100644
--- a/velocity_controllers/include/velocity_controllers/joint_position_controller.h
+++ b/velocity_controllers/include/velocity_controllers/joint_position_controller.h
@@ -148,6 +148,11 @@ public:
void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
/**
+ * \brief Get the PID parameters
+ */
+ void getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup);
+
+ /**
* \brief Print debug info to console
*/
void printDebug();
@@ -155,7 +160,7 @@ public:
/**
* \brief Get the PID parameters
*/
- void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min);
+ void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup = false);
/**
* \brief Get the name of the joint this controller uses
diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml
index aceddc8..78f6f18 100644
--- a/velocity_controllers/package.xml
+++ b/velocity_controllers/package.xml
@@ -1,6 +1,6 @@
<package>
<name>velocity_controllers</name>
- <version>0.10.0</version>
+ <version>0.11.1</version>
<description>velocity_controllers</description>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
diff --git a/velocity_controllers/src/joint_position_controller.cpp b/velocity_controllers/src/joint_position_controller.cpp
index 928b21f..3103158 100644
--- a/velocity_controllers/src/joint_position_controller.cpp
+++ b/velocity_controllers/src/joint_position_controller.cpp
@@ -96,14 +96,20 @@ bool JointPositionController::init(hardware_interface::VelocityJointInterface *r
return true;
}
-void JointPositionController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min)
+void JointPositionController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup)
{
- pid_controller_.setGains(p,i,d,i_max,i_min);
+ pid_controller_.setGains(p,i,d,i_max,i_min,antiwindup);
}
void JointPositionController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
{
- pid_controller_.getGains(p,i,d,i_max,i_min);
+ bool dummy;
+ pid_controller_.getGains(p,i,d,i_max,i_min,dummy);
+}
+
+void JointPositionController::getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup)
+{
+ pid_controller_.getGains(p,i,d,i_max,i_min,antiwindup);
}
void JointPositionController::printDebug()
@@ -225,11 +231,14 @@ void JointPositionController::update(const ros::Time& time, const ros::Duration&
controller_state_publisher_->msg_.command = commanded_velocity;
double dummy;
+ bool antiwindup;
getGains(controller_state_publisher_->msg_.p,
controller_state_publisher_->msg_.i,
controller_state_publisher_->msg_.d,
controller_state_publisher_->msg_.i_clamp,
- dummy);
+ dummy,
+ antiwindup);
+ controller_state_publisher_->msg_.antiwindup = static_cast<char>(antiwindup);
controller_state_publisher_->unlockAndPublish();
}
}
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/ros/ros-ros-controllers.git
More information about the debian-science-commits
mailing list