Implementação de Controladores no ROS

Implementação de Controladores no ROS Walter Fetter Lages fetter@ece.ufrgs.br Universidade Federal do Rio Grande do Sul Escola de Engenharia Departamento de Sistemas Elétricos de Automação e Energia ENG10051 Dinâmica e Controle de Robôs Copyright (c) Walter Fetter Lages p.1

Introdução Implementação de controladores no ROS Exemplo: Controlador por torque calculado τ = M(q) [ q r + K d ( q r q) + K p (q r q)]+v (q, q)+g(q) Modelo do robô calculado através da classe ChainIdSolver_RNE class da KDL Copyright (c) Walter Fetter Lages p.2

Controle de Juntas interface com o usuário infraestrutura do ROS comando do usuário planejamento (MoveIt!) geração de trajetória navegação action server referência controlador de junta ação de controle realimentação hardware do robô Copyright (c) Walter Fetter Lages p.3

Arquitetura de Controladores Copyright (c) Walter Fetter Lages p.4

Laço de Tempo Real /joint_states JointHandle getvelocity() getposition() geteffort() setcommand() getvelocity() getposition() Controller unlockandpublish() JointStatePublisher *cmd *vel *pos *eff update() update() Gazebo setforce() getangle() getvelocity() cmd vel pos RobotHW eff ControllerManager commandcb() readsim() writesim() /controller/command update() GazeboRosControlPlugin update() Copyright (c) Walter Fetter Lages p.5

computed_torque_controller computed_torque_controller/ CMakeLists.txt computed_torque_controller_plugins.xml include/ computed_torque_controller/ computed_torque_controller.h package.xml src/ computed_torque_controller.cpp Copyright (c) Walter Fetter Lages p.6

Dependências ros_control control_toolbox controller_interface controller_manager hardware_interface joint_limits_interface transmission_interface realtime_tools sudo apt get install ros indigo ros control Copyright (c) Walter Fetter Lages p.7

Dependências robot_model kdl_parser Já instalado no ROS desktop orocos_kdl Já instalado no ROS desktop gazebo_ros_pkgs gazebo_ros_control sudo apt get install ros indigo gazebo ros pkgs ros indigo gazebo ros control Copyright (c) Walter Fetter Lages p.8

Criação do Pacote source /opt/ros/indigo/setup.bash source $HOME/catkin_ws/devel/setup.bash cd ~/catkin_ws/src catkin_create_pkg computed_torque_controller controller_interface controller_manager trajecotry_msgs urdf kdl_parser orocos_kdl cmake_modules s Eigen Copyright (c) Walter Fetter Lages p.9

package.xml Editar o arquivopackage.xml Descrição Mantenedor Licença Autor Dependências Exportações controller_interface Copyright (c) Walter Fetter Lages p.10

Plugin nopackage.xml <export> <controller_interface plugin="${prefix}/ computed_torque_controller_plugins.xml"/> </export> Copyright (c) Walter Fetter Lages p.11

computed_torque_controller_plugins.xml <library path="lib/libcomputed_torque_controller"> <class name="effort_controllers/computedtorquecontroller" type="effort_controllers::computedtorquecontroller" base_class_type="controller_interface::controllerbase"> <description> The ComputedTorqueController implements a computed torque controller in joint space for a robot with open chain dynamic model. The reference inputs (command in the ROS nomenclature) are joint positions, velocities and accelerations. This type of controller expects an EffortJointInterface type of hardware interface. </description> </class> </library> Copyright (c) Walter Fetter Lages p.12

AjustarCMakeLists.txt CMakeLists.txt deve ser editado para configurar os detalhes de compilação e linkagem EditarCMakeLists.txt para descomentar: catkin_package( ) INCLUDE_DIRS include LIBRARIES computed_torque_controller Copyright (c) Walter Fetter Lages p.13

AjustarCMakeLists.txt include_directories( include ${catkin_include_dirs} ) add_library(${project_name} ) src/computed_torque_controller.cpp target_link_libraries(${project_name} ${catkin_libraries} ${Eigen_LIBRARIES} ) Copyright (c) Walter Fetter Lages p.14

AjustarCMakeLists.txt install(targets ${PROJECT_NAME} ARCHIVE DESTINATION ${ CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${ CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${ CATKIN_PACKAGE_BIN_DESTINATION} ) Copyright (c) Walter Fetter Lages p.15

computed_torque_controller.h #ifndef COMPUTED_TORQUE_CONTROLLER_COMPUTED_TORQUE_CONTROLLER #define COMPUTED_TORQUE_CONTROLLER_COMPUTED_TORQUE_CONTROLLER #include <cstddef> #include <vector> #include <string> #include <ros/node_handle.h> #include <hardware_interface/joint_command_interface.h> #include <controller_interface/controller.h> #include <trajectory_msgs/jointtrajectorypoint.h> #include <Eigen/Core> #include <kdl/frames.hpp> #include <kdl_parser/kdl_parser.hpp> #include <kdl/chainidsolver_recursive_newton_euler.hpp> Copyright (c) Walter Fetter Lages p.16

computed_torque_controller.h namespace effort_controllers { class ComputedTorqueController: public controller_interface:: Controller<hardware_interface::EffortJointInterface> { ros::nodehandle node_; hardware_interface::effortjointinterface hw_; std::vector<hardware_interface::jointhandle> joints_; int njoints_; ros::subscriber sub_command_; KDL::ChainIdSolver_RNE idsolver_; Copyright (c) Walter Fetter Lages p.17

computed_torque_controller.h KDL::JntArray q_; KDL::JntArray dq_; KDL::JntArray v_; KDL::JntArray qr_; KDL::JntArray dqr_; KDL::JntArray ddqr_; KDL::JntArray torque_; KDL::Wrenches fext_; Eigen::MatrixXd Kp_; Eigen::MatrixXd Kd_; Copyright (c) Walter Fetter Lages p.18

computed_torque_controller.h void commandcb(const trajectory_msgs::jointtrajectorypoint:: ConstPtr &referencepoint); public: ComputedTorqueController(void); ~ComputedTorqueController(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 Copyright (c) Walter Fetter Lages p.19

computed_torque_controller.cpp #include <sys/mman.h> #include <computed_torque_controller/computed_torque_controller.h > #include <pluginlib/class_list_macros.h> namespace effort_controllers { Copyright (c) Walter Fetter Lages p.20

Construtor e Destrutor ComputedTorqueController::ComputedTorqueController(void): q_(0),dq_(0),v_(0),qr_(0),dqr_(0),ddqr_(0),torque_(0),fext_(0) { } ComputedTorqueController::~ComputedTorqueController(void) { sub_command_.shutdown(); } Copyright (c) Walter Fetter Lages p.21

init() bool ComputedTorqueController:: init(hardware_interface::effortjointinterface hw,ros:: NodeHandle &n) { node_=n; hw_=hw; std::vector<std::string> joint_names; if(!node_.getparam("joints",joint_names)) { ROS_ERROR("No joints in controller. (namespace: %s)", node_.getnamespace().c_str()); return false; } Copyright (c) Walter Fetter Lages p.22

init() njoints_=joint_names.size(); for(int i=0; i < njoints_;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; } } Copyright (c) Walter Fetter Lages p.23

init() sub_command_=node_.subscribe("command",1, &ComputedTorqueController::commandCB, this); std::string robot_desc_string; if(!node_.getparam("/robot_description",robot_desc_string)) { ROS_ERROR("Could not find /robot_description."); return false; } Copyright (c) Walter Fetter Lages p.24

init() 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; } Copyright (c) Walter Fetter Lages p.25

init() 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; } Copyright (c) Walter Fetter Lages p.26

init() 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((idsolver_=new KDL::ChainIdSolver_RNE(chain,g)) == NULL ) { ROS_ERROR("Failed to create ChainIDSolver_RNE."); return false; } Copyright (c) Walter Fetter Lages p.27

init() 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(nJoints_,nJoints_); Kd_.resize(nJoints_,nJoints_); Copyright (c) Walter Fetter Lages p.28

init() std::vector<double> KpVec; if(!node_.getparam("kp",kpvec)) { ROS_ERROR("No Kp in controller %s.",node_.getnamespace ().c_str()); return false; } Kp_=Eigen::Map<Eigen::MatrixXd>(KpVec.data(),nJoints_, njoints_).transpose(); Copyright (c) Walter Fetter Lages p.29

init() std::vector<double> KdVec; if(!node_.getparam("kd",kdvec)) { ROS_ERROR("No Kd in controller %s.",node_.getnamespace ().c_str()); return false; } Kd_=Eigen::Map<Eigen::MatrixXd>(KdVec.data(),nJoints_, njoints_).transpose(); } return true; Copyright (c) Walter Fetter Lages p.30

starting() void ComputedTorqueController::starting(const ros::time& time) { for(unsigned int i=0;i < njoints_;i++) { q_(i)=joints_[i].getposition(); dq_(i)=joints_[i].getvelocity(); } qr_=q_; dqr_=dq_; SetToZero(ddqr_); Copyright (c) Walter Fetter Lages p.31

starting() struct sched_param param; if(!node_.getparam("priority",param.sched_priority)) { ROS_WARN("No priority configured for controller %s. Using highest possible priority.",node_.getnamespace().c_str()); param.sched_priority=sched_get_priority_max(sched_fifo); } if(sched_setscheduler(0,sched_fifo,&param) == 1) { ROS_WARN("Failed to set real time scheduler."); return; } if(mlockall(mcl_current MCL_FUTURE) == 1) ROS_WARN("Failed to lock memory."); } Copyright (c) Walter Fetter Lages p.32

update() void ComputedTorqueController::update(const ros::time& time, const ros::duration& duration) { for(unsigned int i=0;i < njoints_;i++) { q_(i)=joints_[i].getposition(); dq_(i)=joints_[i].getvelocity(); } for(unsigned int i=0;i < fext_.size();i++) fext_[i].zero(); v_.data=ddqr_.data+kp_ (qr_.data q_.data)+kd_ (dqr_.data dq_.data); if(idsolver_ >CartToJnt(q_,dq_,v_,fext_,torque_) < 0) ROS_ERROR("KDL inverse dynamics solver failed."); Copyright (c) Walter Fetter Lages p.33

update() } for(unsigned int i=0;i < njoints_;i++) joints_[i].setcommand(torque_(i)); Copyright (c) Walter Fetter Lages p.34

Callback } void ComputedTorqueController::commandCB(const trajectory_msgs:: JointTrajectoryPoint::ConstPtr &referencepoint) { for(unsigned int i=0;i < njoints_;i++) { qr_(i)=referencepoint >positions[i]; dqr_(i)=referencepoint >velocities[i]; ddqr_(i)=referencepoint >accelerations[i]; } } Copyright (c) Walter Fetter Lages p.35

Exportação da Classe PLUGINLIB_EXPORT_CLASS(effort_controllers:: ComputedTorqueController, controller_interface::controllerbase) Copyright (c) Walter Fetter Lages p.36

Instalação source /opt/ros/indigo/setup.bash source $HOME/catkin_ws/devel/setup.bash cd ~/catkin_ws/src wget http://www.ece.ufrgs.br/~fetter/ros_pkgs/indigo computed torque_controller.tgz tar xvzf indigo computed torque_controller.tgz cd.. catkin_make source $HOME/catkin_ws/devel/setup.bash Para utilizar é necessário criar os arquivos de configuração (.yaml) e de launch Estes arquivos devem ser criados em pacotes específicos para cada robô Copyright (c) Walter Fetter Lages p.37

Pacotewam_bringup wam_bringup/ CMakeLists.txt config/ computed_torque_controller.yaml pid_plus_gravity_controller.yaml launch/ gazebo.launch package.xml scripts/ set_home.sh step.sh step_home.sh step_zero.sh Copyright (c) Walter Fetter Lages p.38

computed_torque_controller.yaml 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 Copyright (c) Walter Fetter Lages p.39

computed_torque_controller.yaml 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] Copyright (c) Walter Fetter Lages p.40

computed_torque_controller.yaml gravity: {x: 0.0, y: 0.0, z: 9.8} chain: {root: "wam_origin", tip: "wam_tool_plate"} priority: 99 Copyright (c) Walter Fetter Lages p.41

step.sh #!/bin/bash if [ "$#" ne 7 ]; then echo "Error: There should be 7 parameters." exit 1; fi; rostopic pub /controller/command \ trajectory_msgs/jointtrajectorypoint \ "[$1, $2, $3, $4, $5, $6, $7]" \ "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]" \ "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]" \ "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]" \ "[0.0, 0.0]" " 1" Copyright (c) Walter Fetter Lages p.42

Instalação source /opt/ros/indigo/setup.bash source $HOME/catkin_ws/devel/setup.bash cd ~/catkin_ws/src wget http://www.ece.ufrgs.br/~fetter/ros_pkgs/indigo wam description.tgz tar xvzf indigo wam_description.tgz wget http://www.ece.ufrgs.br/~fetter/ros_pkgs/indigo wam bringup. tgz tar xvzf indigo wam_bringup.tgz cd.. catkin_make source $HOME/catkin_ws/devel/setup.bash Copyright (c) Walter Fetter Lages p.43

Simulação no Gazebo roslaunch wam_bringup gazebo.launch controller:=computed_torque_controller Copyright (c) Walter Fetter Lages p.44

Gráfico de Computação rqt_graph & Copyright (c) Walter Fetter Lages p.45

Mover o Robô rosrun wam_bringup step.sh 0 0.75 0 1.5 0 0.5 0 Copyright (c) Walter Fetter Lages p.46

Exercícios Usar orqt_graph para fazer o gráfico da posição das juntas Verificar o gráfico da posição juntas movendo o robô Adaptar o pacoteq2d_bringup para usar o controlador por torque calculado Fazer o arquivo de configuração do controlador Fazer um arquivo de launch para carregar o controlador que possa ser chamado pelo gazebo.launch quando passado o parâmetro com o nome do controlador Copyright (c) Walter Fetter Lages p.47

PID com Compensação de Gravidade É o controlador default do WAM e da maioria dos robôs industriais Instalação source /opt/ros/indigo/setup.bash source $HOME/catkin_ws/devel/setup.bash cd ~/catkin_ws/src wget http://www.ece.ufrgs.br/~fetter/ros_pkgs/indigo pid plus gravity controller.tgz tar xvzf indigo pid plus gravity controller.tgz cd.. catkin_make source $HOME/catkin_ws/devel/setup.bash Copyright (c) Walter Fetter Lages p.48

pid_plus_gravity_controller.yaml 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 Copyright (c) Walter Fetter Lages p.49

pid_plus_gravity_controller.yaml # PID parameters from /etc/barrett/wam7w.conf wam_joint_1: {p: 900.0, i: 2.5, d: 10.0, i_clamp: 25.0} wam_joint_2: {p: 2500.0, i: 5.0, d: 20.0, i_clamp: 20.0} wam_joint_3: {p: 600.0, i: 2.0, d: 5.0, i_clamp: 15.0} wam_joint_4: {p: 500.0, i: 0.5, d: 2.0, i_clamp: 15.0} wam_joint_5: {p: 50.0, i: 0.5, d: 0.5, i_clamp: 5.0} wam_joint_6: {p: 50.0, i: 0.5, d: 0.5, i_clamp: 5.0} wam_joint_7: {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"} priority: 99 Copyright (c) Walter Fetter Lages p.50

Simulação no Gazebo roslaunch wam_bringup gazebo.launch controller:=pid_plus_gravity_controller Copyright (c) Walter Fetter Lages p.51

Gráfico de Computação Copyright (c) Walter Fetter Lages p.52

Mover o Robô rosrun wam_bringup step.sh 0 0.75 0 1.5 0 0.5 0 Copyright (c) Walter Fetter Lages p.53

Exercícios Usar orqt_graph para fazer o gráfico da posição das juntas Verificar o gráfico da posição juntas movendo o robô Adaptar o pacoteq2d_bringup para usar o controlador por PID com compensação de gravidade Fazer o arquivo de configuração do controlador Fazer um arquivo de launch para carregar o controlador que possa ser chamado pelo gazebo.launch quando passado o parâmetro com o nome do controlador Usar odynamic_reconfigure para ajustar os ganhos do controlador Copyright (c) Walter Fetter Lages p.54

Exercícios Usando o robô Quanser 2DSFJE, comparar o desempenho da resposta ao salto para os controladores: PID independene por junta PID com compensação de gravidade Torque calculado Copyright (c) Walter Fetter Lages p.55