Implementação de Controladores no ROSfetter/eng10051/ros_controllers.pdf · computed_torque_controller_plugins.xml

Implementação de Controladoresno ROS

Walter Fetter [email protected]

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 + Kd(q̇r − q̇) + Kp(qr − q)]+V (q, q̇)+G(q)

• Modelo do robô calculado através da classeChainIdSolver_RNE class da KDL


Controle de Juntas

interface com o usuário

comando do usuário

planejamento (MoveIt!)geração de trajetória

navegaçãoaction server

referência

controlador de junta

ação decontrole

realimentação

hardware do robô

infr

aest

rutu

rado

RO

S


Arquitetura de Controladores


Laço de Tempo Real

pos effcmd velsetForce()

getAngle()

getVelocity()

Gazebo

GazeboRosControlPlugin

readSim() writeSim()

update()

RobotHW

*cmd *vel *pos *eff

JointHandle getPosition()getVelocity()

setCommand()

Controller

JointStatePublishergetVelocity()getPosition()

getEffort()

ControllerManager

update()

update()

unlockAndPublish()

/joint_states

/controller/command

update()

com

man

dCB

()


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


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


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


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


package.xml

• Editar o arquivo package.xml• Descrição• Mantenedor• Licença• Autor• Dependências• Exportações

• controller_interface


Plugin no package.xml

<export>

<controller_interface plugin="${prefix}/

computed_torque_controller_plugins.xml"/>

</export>


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

Ajustar CMakeLists.txt

• CMakeLists.txt deve ser editado paraconfigurar os detalhes de compilação e linkagem

• Editar CMakeLists.txt para descomentar:

catkin_package(

INCLUDE_DIRS include

LIBRARIES computed_torque_controller

)



include_directories(

include

${catkin_INCLUDE_DIRS}

)

add_library(${PROJECT_NAME}

src/computed_torque_controller.cpp

)

target_link_libraries(${PROJECT_NAME}

${catkin_LIBRARIES}

${Eigen_LIBRARIES}

)



install(TARGETS ${PROJECT_NAME}

ARCHIVE DESTINATION ${

CATKIN_PACKAGE_LIB_DESTINATION}

LIBRARY DESTINATION ${

CATKIN_PACKAGE_LIB_DESTINATION}

RUNTIME DESTINATION ${

CATKIN_PACKAGE_BIN_DESTINATION}

)



#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>



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


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


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

{


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

}


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;

}


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;

}


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;

}


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;

}


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


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


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;

}


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


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)

{


{

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


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

}


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

}

}

}


Exportação da Classe

PLUGINLIB_EXPORT_CLASS(effort_controllers::

ComputedTorqueController,

controller_interface::ControllerBase)


Instalação



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


• Para utilizar é necessário criar os arquivos deconfiguração (.yaml) e de launch

• Estes arquivos devem ser criados em pacotesespecíficos para cada robô


Pacote wam_bringup

wam_bringup/

CMakeLists.txt

config/computed_torque_controller.yamlpid_plus_gravity_controller.yaml

launch/gazebo.launch

package.xml

scripts/set_home.shstep.shstep_home.shstep_zero.sh


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_7Copyright (c) Walter Fetter Lages – p.39


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


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

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

priority: 99


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"


Instalação



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



Simulação no Gazebo

roslaunch wam_bringup gazebo.launch controller:=computed_torque_controller


Gráfico de Computação

rqt_graph &


Mover o Robô

rosrun wam_bringup step.sh 0 0.75 0 1.5 0 0.5 0


Exercícios

• Usar o rqt_graph para fazer o gráfico daposição das juntas

• Verificar o gráfico da posição juntas movendo orobô

• Adaptar o pacote q2d_bringup para usar ocontrolador por torque calculado• Fazer o arquivo de configuração do

controlador• Fazer um arquivo de launch para carregar o

controlador que possa ser chamado pelogazebo.launch quando passado oparâmetro com o nome do controlador


PID com Compensação de Gravidade

• É o controlador default do WAM e da maioriados robôs industriais

• Instalação



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



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_7Copyright (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}







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

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

priority: 99


Simulação no Gazebo

roslaunch wam_bringup gazebo.launch controller:=pid_plus_gravity_controller


Gráfico de Computação


Mover o Robô

rosrun wam_bringup step.sh 0 0.75 0 1.5 0 0.5 0


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 PID com compensação degravidade• Fazer o arquivo de configuração do

controlador• Fazer um arquivo de launch para carregar o

controlador que possa ser chamado pelogazebo.launch quando passado oparâmetro com o nome do controlador

• Usar o dynamic_reconfigure para ajustaros ganhos do controlador Copyright (c) Walter Fetter Lages – p.54

Exercícios

• Usando o robô Quanser 2DSFJE, comparar odesempenho da resposta ao salto para oscontroladores:• PID independene por junta• PID com compensação de gravidade• Torque calculado


Documents

Implementação de Controladores no ROSfetter/eng10051/ros_controllers.pdf · computed_torque_controller_plugins.xml