Implementação de Controladores no ROS

(1)

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

(2)

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

(3)

Controle de Juntas

interface com o usuário

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ô in fr ae st ru tu ra do R O S

(4)

(5)

Laço de Tempo Real

pos eff cmd vel setForce() getAngle() getVelocity() Gazebo GazeboRosControlPlugin readSim() writeSim() update() RobotHW *cmd *vel *pos *eff

JointHandle _{getPosition()}getVelocity() setCommand() Controller JointStatePublisher getVelocity() getPosition() getEffort() ControllerManager update() update() unlockAndPublish() /joint_states /controller/command update() commandCB()

(6)

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

(7)

Dependências

•

ros_control

•

control_toolbox

•

controller_interface

•

controller_manager

•

hardware_interface

•

joint_limits_interface

•

transmission_interface

•

realtime_tools

(8)

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−

(9)

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

(10)

package.xml

•

Editar o arquivo package.xml

•

Descrição

•

Mantenedor

•

Licença

•

Autor

•

Dependências

•

Exportações

•

controller_interface

(11)

Plugin

no package.xml

<export>

<controller_interface plugin="${prefix}/

computed_torque_controller_plugins.xml"/> </export>

(12)

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>

(13)

Ajustar CMakeLists.txt

•

CMakeLists.txt

deve ser editado para

configurar os detalhes de compilação e linkagem

•

Editar CMakeLists.txt para descomentar:

catkin_package(

INCLUDE_DIRS include

LIBRARIES computed_torque_controller )

(14)

Ajustar CMakeLists.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} )

(15)

Ajustar CMakeLists.txt

install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${ CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${ CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${ CATKIN_PACKAGE_BIN_DESTINATION} )

(16)

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>

(17)

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_;

(18)

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_;

(19)

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

); }; }

(20)

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 {

(21)

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(); }

(22)

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;

(23)

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;

} }

(24)

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;

(25)

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;

(26)

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;

(27)

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;

(28)

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_);

(29)

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();

(30)

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; }

(31)

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_);

(32)

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)

(33)

update()

void ComputedTorqueController::update(const ros::Time& time, const ros::Duration& duration)

{

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)

(34)

update()

joints_[i].setCommand(torque_(i)); }

(35)

Callback

void ComputedTorqueController::commandCB(const

trajectory_msgs::

JointTrajectoryPoint::ConstPtr &referencePoint) {

{ qr_(i)=referencePoint−>positions[i]; dqr_(i)=referencePoint−>velocities[i]; ddqr_(i)=referencePoint−>accelerations[i]; } } }

(36)

Exportação da Classe

PLUGINLIB_EXPORT_CLASS(effort_controllers:: ComputedTorqueController,

(37)

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

(38)

Pacote wam_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

(39)

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

(40)

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]

(41)

computed_torque_controller.yaml

gravity: {x: 0.0, y: 0.0, z: −9.8}

chain: {root: "wam_origin", tip: "wam_tool_plate"} priority: 99

(42)

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"

(43)

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

(44)

Simulação no Gazebo

(45)

Gráfico de Computação

(46)

Mover o Robô

(47)

Exercícios

•

Usar o rqt_graph para fazer o gráfico da

posição das juntas

•

Verificar o gráfico da posição juntas movendo o

robô

•

Adaptar o pacote q2d_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

(48)

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

(49)

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

(50)

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}