From: Walter Fetter Lages Date: Wed, 28 Nov 2018 21:47:43 +0000 (-0200) Subject: Add PID+gravity controller X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=4b0467fc9a3bb8ea50cbafe1a2004d7d3cd4c112;p=ufrgs_wam.git Add PID+gravity controller Change computed_torque_control.* to comuted_torque_controller.*. --- diff --git a/wam_controllers/CMakeLists.txt b/wam_controllers/CMakeLists.txt index 87c2c9e..bf0ae5b 100644 --- a/wam_controllers/CMakeLists.txt +++ b/wam_controllers/CMakeLists.txt @@ -105,6 +105,7 @@ include_directories(include ## Declare a cpp library add_library(wam_controllers src/computed_torque_controller.cpp + src/pid_plus_gravity_controller.cpp ) ## Declare a cpp executable @@ -117,7 +118,7 @@ add_library(wam_controllers ## Specify libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} - ${orocos-kdl_LIBRARIES} + ${orocos_kdl_LIBRARIES} ${kdl_parser_LIBRARIES} ) diff --git a/wam_controllers/config/computed_torque_control.yaml b/wam_controllers/config/computed_torque_control.yaml deleted file mode 100644 index 4343844..0000000 --- a/wam_controllers/config/computed_torque_control.yaml +++ /dev/null @@ -1,15 +0,0 @@ -wam: - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 100 - - computed_torque_controller: - type: wam_controllers/ComputedTorqueController - joints: - - wam_joint_1 - - wam_joint_2 - - wam_joint_3 - - wam_joint_4 - - wam_joint_5 - - wam_joint_6 - - wam_joint_7 diff --git a/wam_controllers/config/computed_torque_controller.yaml b/wam_controllers/config/computed_torque_controller.yaml new file mode 100644 index 0000000..d9f19ab --- /dev/null +++ b/wam_controllers/config/computed_torque_controller.yaml @@ -0,0 +1,31 @@ +wam: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + + computed_torque_controller: + type: effort_controllers/ComputedTorqueController + joints: + - wam_joint_1 + - wam_joint_2 + - wam_joint_3 + - wam_joint_4 + - wam_joint_5 + - wam_joint_6 + - wam_joint_7 + Kp: [25.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 25.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 25.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 25.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 25.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 25.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 25.0] + Kd: [10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0] + gravity: {x: 0.0, y: 0.0, z: -9.8} + chain: {root: "wam_origin", tip: "wam_tool_plate"} diff --git a/wam_controllers/config/pid_plus_gravity_controller.yaml b/wam_controllers/config/pid_plus_gravity_controller.yaml new file mode 100644 index 0000000..b7b7d59 --- /dev/null +++ b/wam_controllers/config/pid_plus_gravity_controller.yaml @@ -0,0 +1,32 @@ +wam: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 250 + + pid_plus_gravity_controller: + type: effort_controllers/PidPlusGravityController + joints: + - wam_joint_1 + - wam_joint_2 + - wam_joint_3 + - wam_joint_4 + - wam_joint_5 + - wam_joint_6 + - wam_joint_7 +# PID parameters from /etc/barrett/wam7w.conf + wam_joint_1: + pid: {p: 900.0, i: 2.5, d: 10.0, i_clamp: 25.0} + wam_joint_2: + pid: {p: 2500.0, i: 5.0, d: 20.0, i_clamp: 20.0} + wam_joint_3: + pid: {p: 600.0, i: 2.0, d: 5.0, i_clamp: 15.0} + wam_joint_4: + pid: {p: 500.0, i: 0.5, d: 2.0, i_clamp: 15.0} + wam_joint_5: + pid: {p: 50.0, i: 0.5, d: 0.5, i_clamp: 5.0} + wam_joint_6: + pid: {p: 50.0, i: 0.5, d: 0.5, i_clamp: 5.0} + wam_joint_7: + pid: {p: 8.0, i: 0.1, d: 0.05, i_clamp: 5.0} + gravity: {x: 0.0, y: 0.0, z: -9.8} + chain: {root: "wam_origin", tip: "wam_tool_plate"} diff --git a/wam_controllers/include/wam_controllers/computed_torque_controller.h b/wam_controllers/include/wam_controllers/computed_torque_controller.h index 2bbbe22..15fe88a 100644 --- a/wam_controllers/include/wam_controllers/computed_torque_controller.h +++ b/wam_controllers/include/wam_controllers/computed_torque_controller.h @@ -1,3 +1,24 @@ +/****************************************************************************** + ROS wam_controllers Package + Computed Torque Controller + Copyright (C) 2013..2017 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + #ifndef WAM_CONTROLLERS_COMPUTED_TORQUE_CONTROLLER_H #define WAM_CONTROLLERS_COMPUTED_TORQUE_CONTROLLER_H @@ -17,15 +38,16 @@ #include #include -namespace wam_controllers +namespace effort_controllers { class ComputedTorqueController: public controller_interface:: Controller { ros::NodeHandle node_; - hardware_interface::EffortJointInterface *robot_; + hardware_interface::EffortJointInterface *hw_; std::vector joints_; + int nJoints_; ros::Subscriber sub_command_; @@ -53,7 +75,7 @@ namespace wam_controllers ComputedTorqueController(void); ~ComputedTorqueController(void); - bool init(hardware_interface::EffortJointInterface *robot, + bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &n); void starting(const ros::Time& time); void update(const ros::Time& time,const ros::Duration& duration); diff --git a/wam_controllers/include/wam_controllers/pid_plus_gravity_controller.h b/wam_controllers/include/wam_controllers/pid_plus_gravity_controller.h new file mode 100644 index 0000000..2fcfe45 --- /dev/null +++ b/wam_controllers/include/wam_controllers/pid_plus_gravity_controller.h @@ -0,0 +1,75 @@ +/****************************************************************************** + ROS wam_controllers Package + PID+Gravity Compensation Controller + Copyright (C) 2017 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + +#ifndef WAM_CONTROLLERS_PID_PLUS_GRAVITY_CONTROLLER_H +#define WAM_CONTROLLERS_PID_PLUS_GRAVITY_CONTROLLER_H + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace effort_controllers +{ + class PidPlusGravityController: public controller_interface:: + Controller + { + ros::NodeHandle node_; + + hardware_interface::EffortJointInterface *hw_; + std::vector joints_; + + ros::Subscriber sub_command_; + + KDL::ChainDynParam *chainParam_; + + KDL::JntArray q_; + KDL::JntArray qr_; + KDL::JntArray gravity_; + + std::vector pid_; + + void commandCB(const trajectory_msgs::JointTrajectoryPoint::ConstPtr + &referencePoint); + + public: + PidPlusGravityController(void); + ~PidPlusGravityController(void); + + bool init(hardware_interface::EffortJointInterface *hw, + ros::NodeHandle &n); + void starting(const ros::Time& time); + void update(const ros::Time& time,const ros::Duration& duration); + }; +} +#endif diff --git a/wam_controllers/launch/computed_torque.launch b/wam_controllers/launch/computed_torque.launch index ddcf49b..c452826 100644 --- a/wam_controllers/launch/computed_torque.launch +++ b/wam_controllers/launch/computed_torque.launch @@ -1,5 +1,5 @@ - + + + + + + + + + + + + + + + + + + diff --git a/wam_controllers/launch/pid_plus_gravity.launch b/wam_controllers/launch/pid_plus_gravity.launch new file mode 100644 index 0000000..c0005a0 --- /dev/null +++ b/wam_controllers/launch/pid_plus_gravity.launch @@ -0,0 +1,9 @@ + + + + + + diff --git a/wam_controllers/package.xml b/wam_controllers/package.xml index 118d020..819d207 100644 --- a/wam_controllers/package.xml +++ b/wam_controllers/package.xml @@ -51,11 +51,10 @@ urdf cmake_modules - cmake_modules controller_interface controller_manager control_msgs - joint_state_controller + joint_state_controller urdf kdl_parser @@ -67,4 +66,4 @@ - \ No newline at end of file + diff --git a/wam_controllers/src/computed_torque_controller.cpp b/wam_controllers/src/computed_torque_controller.cpp index c880ed0..5165310 100644 --- a/wam_controllers/src/computed_torque_controller.cpp +++ b/wam_controllers/src/computed_torque_controller.cpp @@ -1,17 +1,30 @@ +/****************************************************************************** + ROS wam_controllers Package + Computed Torque Controller + Copyright (C) 2013..2017 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + #include #include #include -#include -#include -#include - -#define Ts 0.8 -#define Xi 1.0 -#define Wn (4.0/Ts/Xi) - -namespace wam_controllers +namespace effort_controllers { ComputedTorqueController::ComputedTorqueController(void): q(0),dq(0),v(0),qr(0),dqr(0),ddqr(0),torque(0),fext(0) @@ -24,12 +37,12 @@ namespace wam_controllers } bool ComputedTorqueController:: - init(hardware_interface::EffortJointInterface *robot,ros::NodeHandle &n) + init(hardware_interface::EffortJointInterface *hw,ros::NodeHandle &n) { node_=n; - robot_=robot; + hw_=hw; - XmlRpc::XmlRpcValue joint_names; + std::vector joint_names; if(!node_.getParam("joints",joint_names)) { ROS_ERROR("No 'joints' in controller. (namespace: %s)", @@ -37,28 +50,21 @@ namespace wam_controllers return false; } - if(joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) - { - ROS_ERROR("'joints' is not a struct. (namespace: %s)", - node_.getNamespace().c_str()); - return false; - } + nJoints_=joint_names.size(); - for(int i=0; i < joint_names.size();i++) + for(int i=0; i < nJoints_;i++) { - XmlRpc::XmlRpcValue &name_value=joint_names[i]; - if(name_value.getType() != XmlRpc::XmlRpcValue::TypeString) + try { - ROS_ERROR("joints are not strings. (namespace: %s)", - node_.getNamespace().c_str()); + joints_.push_back(hw->getHandle(joint_names[i])); + } + catch(const hardware_interface::HardwareInterfaceException& e) + { + ROS_ERROR_STREAM("Exception thrown: " << e.what()); return false; } - - hardware_interface::JointHandle j=robot-> - getHandle((std::string)name_value); - joints_.push_back(j); } - sub_command_=node_.subscribe("command",1000, + sub_command_=node_.subscribe("command",1, &ComputedTorqueController::commandCB, this); std::string robot_desc_string; @@ -75,17 +81,31 @@ namespace wam_controllers return false; } + std::string chainRoot; + if(!node_.getParam("chain/root",chainRoot)) + { + ROS_ERROR("Could not find 'chain_root' parameter."); + return false; + } + + std::string chainTip; + if(!node_.getParam("chain/tip",chainTip)) + { + ROS_ERROR("Could not find 'chain/tip' parameter."); + return false; + } + KDL::Chain chain; - if (!tree.getChain("wam_origin","wam_tool_plate",chain)) + if (!tree.getChain(chainRoot,chainTip,chain)) { ROS_ERROR("Failed to get chain from KDL tree."); return false; } KDL::Vector g; - node_.param("/gazebo/gravity_x",g[0],0.0); - node_.param("/gazebo/gravity_y",g[1],0.0); - node_.param("/gazebo/gravity_z",g[2],-9.8); + node_.param("gravity/x",g[0],0.0); + node_.param("gravity/y",g[1],0.0); + node_.param("gravity/z",g[2],-9.8); if((idsolver=new KDL::ChainIdSolver_RNE(chain,g)) == NULL) { @@ -93,30 +113,42 @@ namespace wam_controllers return false; } - q.resize(chain.getNrOfJoints()); - dq.resize(chain.getNrOfJoints()); - v.resize(chain.getNrOfJoints()); - qr.resize(chain.getNrOfJoints()); - dqr.resize(chain.getNrOfJoints()); - ddqr.resize(chain.getNrOfJoints()); - torque.resize(chain.getNrOfJoints()); + q.resize(nJoints_); + dq.resize(nJoints_); + v.resize(nJoints_); + qr.resize(nJoints_); + dqr.resize(nJoints_); + ddqr.resize(nJoints_); + torque.resize(nJoints_); fext.resize(chain.getNrOfSegments()); - Kp.resize(chain.getNrOfJoints(),chain.getNrOfJoints()); - Kd.resize(chain.getNrOfJoints(),chain.getNrOfJoints()); + Kp.resize(nJoints_,nJoints_); + Kd.resize(nJoints_,nJoints_); + + std::vector KpVec; + if(!node_.getParam("Kp",KpVec)) + { + ROS_ERROR("No 'Kp' in controller %s.",node_.getNamespace().c_str()); + return false; + } + Kp=Eigen::Map(KpVec.data(),nJoints_,nJoints_); + + std::vector KdVec; + if(!node_.getParam("Kd",KdVec)) + { + ROS_ERROR("No 'Kd' in controller %s.",node_.getNamespace().c_str()); + return false; + } + Kd=Eigen::Map(KdVec.data(),nJoints_,nJoints_); return true; } void ComputedTorqueController::starting(const ros::Time& time) { - Kp.setZero(); - Kd.setZero(); - for(unsigned int i=0;i < joints_.size();i++) + for(unsigned int i=0;i < nJoints_;i++) { - Kp(i,i)=Wn*Wn; - Kd(i,i)=2.0*Xi*Wn; q(i)=joints_[i].getPosition(); dq(i)=joints_[i].getVelocity(); } @@ -138,7 +170,7 @@ namespace wam_controllers void ComputedTorqueController::update(const ros::Time& time, const ros::Duration& duration) { - for(unsigned int i=0;i < joints_.size();i++) + for(unsigned int i=0;i < nJoints_;i++) { q(i)=joints_[i].getPosition(); dq(i)=joints_[i].getVelocity(); @@ -149,14 +181,14 @@ namespace wam_controllers if(idsolver->CartToJnt(q,dq,v,fext,torque) < 0) ROS_ERROR("KDL inverse dynamics solver failed."); - for(unsigned int i=0;i < joints_.size();i++) + for(unsigned int i=0;i < nJoints_;i++) joints_[i].setCommand(torque(i)); } void ComputedTorqueController::commandCB(const trajectory_msgs:: JointTrajectoryPoint::ConstPtr &referencePoint) { - for(unsigned int i=0;i < qr.rows();i++) + for(unsigned int i=0;i < nJoints_;i++) { qr(i)=referencePoint->positions[i]; dqr(i)=referencePoint->velocities[i]; @@ -165,5 +197,5 @@ namespace wam_controllers } } -PLUGINLIB_EXPORT_CLASS(wam_controllers::ComputedTorqueController, +PLUGINLIB_EXPORT_CLASS(effort_controllers::ComputedTorqueController, controller_interface::ControllerBase) diff --git a/wam_controllers/src/pid_plus_gravity_controller.cpp b/wam_controllers/src/pid_plus_gravity_controller.cpp new file mode 100644 index 0000000..e144ba1 --- /dev/null +++ b/wam_controllers/src/pid_plus_gravity_controller.cpp @@ -0,0 +1,173 @@ +/****************************************************************************** + ROS wam_controllers Package + PID+Gravity Compensation Controller + Copyright (C) 2017 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + +#include + +#include +#include + +namespace effort_controllers +{ + PidPlusGravityController::PidPlusGravityController(void): + q_(0),qr_(0),gravity_(0) + { + } + + PidPlusGravityController::~PidPlusGravityController(void) + { + sub_command_.shutdown(); + } + + bool PidPlusGravityController:: + init(hardware_interface::EffortJointInterface *hw,ros::NodeHandle &n) + { + node_=n; + hw_=hw; + + std::vector joint_names; + if(!node_.getParam("joints",joint_names)) + { + ROS_ERROR("No 'joints' in controller. (namespace: %s)", + node_.getNamespace().c_str()); + return false; + } + + pid_.resize(joint_names.size()); + + for(int i=0; i < joint_names.size();i++) + { + try + { + joints_.push_back(hw->getHandle(joint_names[i])); + } + catch(const hardware_interface::HardwareInterfaceException& e) + { + ROS_ERROR_STREAM("Exception thrown: " << e.what()); + return false; + } + if(!pid_[i].init(ros::NodeHandle(node_,joint_names[i]+"/pid"))) + { + ROS_ERROR_STREAM("Failed to load PID parameters from " << joint_names[i] + "/pid"); + return false; + } + } + + sub_command_=node_.subscribe("command",1, + &PidPlusGravityController::commandCB, this); + + std::string robot_desc_string; + if(!node_.getParam("/robot_description",robot_desc_string)) + { + ROS_ERROR("Could not find '/robot_description'."); + return false; + } + + KDL::Tree tree; + if (!kdl_parser::treeFromString(robot_desc_string,tree)) + { + ROS_ERROR("Failed to construct KDL tree."); + return false; + } + + std::string chainRoot; + if(!node_.getParam("chain/root",chainRoot)) + { + ROS_ERROR("Could not find 'chain_root' parameter."); + return false; + } + + std::string chainTip; + if(!node_.getParam("chain/tip",chainTip)) + { + ROS_ERROR("Could not find 'chain/tip' parameter."); + return false; + } + + KDL::Chain chain; + if (!tree.getChain(chainRoot,chainTip,chain)) + { + ROS_ERROR("Failed to get chain from KDL tree."); + return false; + } + + KDL::Vector g; + node_.param("gravity/x",g[0],0.0); + node_.param("gravity/y",g[1],0.0); + node_.param("gravity/z",g[2],-9.8); + + if((chainParam_=new KDL::ChainDynParam(chain,g)) == NULL) + { + ROS_ERROR("Failed to create ChainDynParam."); + return false; + } + + q_.resize(chain.getNrOfJoints()); + qr_.resize(chain.getNrOfJoints()); + gravity_.resize(chain.getNrOfJoints()); + + return true; + } + + void PidPlusGravityController::starting(const ros::Time& time) + { + for(unsigned int i=0;i < joints_.size();i++) + q_(i)=joints_[i].getPosition(); + qr_=q_; + + struct sched_param param; + param.sched_priority=sched_get_priority_max(SCHED_FIFO); + if(sched_setscheduler(0,SCHED_FIFO,¶m) == -1) + { + ROS_WARN("Failed to set real-time scheduler."); + return; + } + if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) + ROS_WARN("Failed to lock memory."); + } + + void PidPlusGravityController::update(const ros::Time& time, + const ros::Duration& duration) + { + for(unsigned int i=0;i < joints_.size();i++) + q_(i)=joints_[i].getPosition(); + + if(chainParam_->JntToGravity(q_,gravity_) < 0) + ROS_ERROR("KDL dynamics solver failed."); + + for(unsigned int i=0;i < joints_.size();i++) + joints_[i].setCommand(gravity_(i)+pid_[i].computeCommand(qr_(i)-q_(i),duration)); + } + + void PidPlusGravityController::commandCB(const trajectory_msgs:: + JointTrajectoryPoint::ConstPtr &referencePoint) + { + if(referencePoint->positions.size()!=qr_.rows()) + { + ROS_ERROR_STREAM("Dimension of command (" << referencePoint->positions.size() << ") does not match number of joints (" << qr_.rows() << ")!"); + return; + } + for(unsigned int i=0;i < qr_.rows();i++) + qr_(i)=referencePoint->positions[i]; + } +} + +PLUGINLIB_EXPORT_CLASS(effort_controllers::PidPlusGravityController, + controller_interface::ControllerBase) diff --git a/wam_controllers/wam_controllers_plugins.xml b/wam_controllers/wam_controllers_plugins.xml index cc65b0d..aac31df 100644 --- a/wam_controllers/wam_controllers_plugins.xml +++ b/wam_controllers/wam_controllers_plugins.xml @@ -1,15 +1,27 @@ - The ComputedTorqueController implements a computed torque controller in joint space for the Barrett WAM dynamic model. The reference inputs (command in the ROS nomenclature) are joint positions, - velocities and accelerations. This typf of controller expects an + velocities and accelerations. This type of controller expects an EffortJointInterface type of hardware interface. + + + + The PidPlusGravityController implements a PID controller with gravity + compensation in joint space for the Barrett WAM dynamic model. The + reference inputs (command in the ROS nomenclature) are joint + positions. This type of controller expects an EffortJointInterface + type of hardware interface. + +