Cinemática Direta no ROS - ece.ufrgs.brfetter/ele228/ros_fwdkin.pdf · Arqivo de Launch •...

35
Cinemática Direta no ROS Walter Fetter Lages [email protected] Universidade Federal do Rio Grande do Sul Escola de Engenharia Departamento de Sistemas Elétricos de Automação e Energia ELE228 Robótica A Copyright (c) Walter Fetter Lages – p.1

Transcript of Cinemática Direta no ROS - ece.ufrgs.brfetter/ele228/ros_fwdkin.pdf · Arqivo de Launch •...

Cinemática Direta no ROSWalter Fetter [email protected]

Universidade Federal do Rio Grande do Sul

Escola de Engenharia

Departamento de Sistemas Elétricos de Automação e Energia

ELE228 Robótica A

Copyright (c) Walter Fetter Lages – p.1

Cinemática Direta no ROS• Usando a biblioteca KDL (Kinematics and

Dynamics Library)• Simples se considerado apenas o robô• Para vários objetos tem-se que montar a

cadeia cinemática ”na mão“• Não depende da infraestrutura de tópicos• Real-time ready• Só acessível por programa

• Através do tópico /tf• Consulta-se diretamente a transformação de

interesse• Depende da infraestrutura de tópicos• Acessível por programa ou linha de comando

Copyright (c) Walter Fetter Lages – p.2

Exemplo

• Será usado o pacote q2d_description• Já usado e instalado anteriormente

Copyright (c) Walter Fetter Lages – p.3

Instalação do Pacote

• Baixar e descompactar o pacoteindigo-q2d-description.tgz

cd ~/catkin_ws/src

wget http://www.ece.ufrgs.br/q2d/indigo−q2d−description.tgz

tar −xvzpf indigo−q2d−description.tgz

catkin_make

source ~/catkin_ws/devel/setup.bash

Copyright (c) Walter Fetter Lages – p.4

Nodo robot_state_publiser

• Subscreve o tópico /joint_states• Obtém a descrição URDF do robô do servidor de

parâmetros• Parâmetro robot_description

• Usa a KDL para calcular as transformações entreos frames

• Publica as transformaçoes no tópico /tf• Por default, a 50 Hz

Copyright (c) Walter Fetter Lages – p.5

Nodo joint_state_publisher

• Mostra janela com sliders e permite alterar asvariáveis de junta

• Publica as variáveis de junta no tópico/joint_states

• Não se está simulando a dinâmica do robô• É como mover as juntas com a mão

• Permite movimentar o robô no Rviz sem usarcontrolador

• Não serve para movimentar o robô no Gazebo• No Gazebo é necessário usar controlador, pois

a dinâmica é simulada

Copyright (c) Walter Fetter Lages – p.6

Arqivo de Launch

• launch/display.launch

<launch>

<arg name="use_gui" default="false"/>

<include file="$(find q2d_description)/launch/q2d.launch" />

<node pkg="tf2_ros" type="static_transform_publisher" name="

q2d_origin_publisher" args="0 0 0 0 0 0 1 map origin_link" />

<node name="joint_state_publisher" pkg="joint_state_publisher"

type="joint_state_publisher" args="_use_gui:=$(arg use_gui)" />

<node name="robot_state_publisher" pkg="

robot_state_publisher" type="robot_state_publisher" />

<node name="rviz" pkg="rviz" type="rviz" args="−d $(find

q2d_description)/rviz/display.rviz" required="true" />

</launch>Copyright (c) Walter Fetter Lages – p.7

Comando de Launch

roslaunch q2d_description display.launch use_gui:=true &

Copyright (c) Walter Fetter Lages – p.8

Visualização no Rviz

Copyright (c) Walter Fetter Lages – p.9

Gráfico de Computação

• Em outro terminal:

source /opt/ros/indigo/setup.bash

source $HOME/catkin_ws/devel/setup.bash

export DISPLAY=:0

rqt_graph &

• Visualizar o que é publicado nos tópicos:

rostopic echo /joinst_states

rostopic echo /tf

Copyright (c) Walter Fetter Lages – p.10

Gráfico de Computação

Copyright (c) Walter Fetter Lages – p.11

Árvore de Transformações

• view_frames

• Gera uma descrição em .pdf da árvore detransformações

rosrun tf view_frames

Copyright (c) Walter Fetter Lages – p.12

Árvore de Transformações

Copyright (c) Walter Fetter Lages – p.13

Consulta a uma Transformação

• tf_echo

• Descrição da garra em relação à base:

rosrun tf tf_echo base_link tool_link

Copyright (c) Walter Fetter Lages – p.14

Modelo Cinemático em C++

• Pode ser obtido de /tf• Neste exemplo será convertida para o tipoKDL::Frame

• Classe para representar transformaçãohomogênea definida na KDL

• Atributos são a matriz de rotação e vetor deposição

• Nodo tf2_kdl que mostra o frame na tela• Incluído no pacote eng10026_tf2

Copyright (c) Walter Fetter Lages – p.15

tf2_kdl.cpp

#include <ros/ros.h>

#include <geometry_msgs/TransformStamped.h>

#include <tf2_ros/transform_listener.h>

#include <kdl/frames.hpp>

#include <kdl/frames_io.hpp>

int main(int argc,char∗ argv[])

{

ros::init(argc,argv,"tf2_kdl");

ros::NodeHandle node;

tf2_ros::Buffer tfBuffer;

tf2_ros::TransformListener tfListener(tfBuffer);

Copyright (c) Walter Fetter Lages – p.16

tf2_kdl.cpp

ros::Rate loop(10);

while(ros::ok())

{

geometry_msgs::TransformStamped tfStamped;

try

{

tfStamped=tfBuffer.lookupTransform("origin_link","tool_link",

ros::Time(0),ros::Duration(5.0)); //time−out is optional

}

catch(tf2::TransformException &ex)

{

ROS_WARN("%s",ex.what());

continue;

}Copyright (c) Walter Fetter Lages – p.17

tf2_kdl.cppKDL::Frame frame(

KDL::Rotation::Quaternion(tfStamped.transform.rotation.x,

tfStamped.transform.rotation.y,

tfStamped.transform.rotation.z,

tfStamped.transform.rotation.w),

KDL::Vector(tfStamped.transform.translation.x,

tfStamped.transform.translation.y,

tfStamped.transform.translation.z));

std::cout << "Frame:\n" << frame << std::endl;

ros::spinOnce();

loop.sleep();

}

return 0;

} Copyright (c) Walter Fetter Lages – p.18

Instalação do Pacote

• Baixar e descompactar o pacoteindigo-eng10026-tf2.tgz

cd ~/catkin_ws/src

wget http://www.ece.ufrgs.br/~fetter/eng10026/indigo−

eng10026−tf2.tgz

tar −xvzpf indigo−eng10026−tf2.tgz

cd ..

catkin_make

source $HOME/catkin_ws/devel/setup.bash

Copyright (c) Walter Fetter Lages – p.19

Execução

rosrun eng10026_tf2 tf2_kdl

• Movendo os sliders para mover o robô atransformação homogênea modifica-secorrespondentemente

Copyright (c) Walter Fetter Lages – p.20

Gráfico de Computação

Copyright (c) Walter Fetter Lages – p.21

Modelo Cinemático Usando a KDL

• Pode ser obtido direto da descrição em URDF• Neste exemplo, será otido como um objeto do

tipo KDL::Frame• Classe para representar transformação

homogênea definida na KDL• Atributos são a matriz de rotação e vetor de

posição• Será criado o nodo fwdkin_kdl que mostra o

frame na tela• Incluído no pacote eng10026_fwdkin

Copyright (c) Walter Fetter Lages – p.22

ChainFkSolverPos_recursive

class ChainFkSolverPos_recursive:public ChainFkSolverPos

{

public:

ChainFkSolverPos_recursive(const Chain &chain);

~ChainFkSolverPos_recursive();

int JntToCart(const JntArray &q_in,Frame &p_out,int segmentNr

=−1);

int JntToCart(const JntArray &q_in,std::vector<Frame> &p_out,int

segmentNr=−1);

void updateInternalDataStructures();

}

Copyright (c) Walter Fetter Lages – p.23

Pacote eng10026_fwdkin

• Criar o pacote:

cd ~/catkin_ws/src

catkin_create_pkg eng10026_fwdkin roscpp kdl_parser

sensor_msgs −s orocos_kdl

• package.xml deve ser editado para configuraros detalhes de documentação e incluirdependências

Copyright (c) Walter Fetter Lages – p.24

CMakeLists.txt

• Editar CMakeLists.txt para descomentar eajustar as tags add_executable etarget_link_libraries:

add_executable(fwdkin_kdl src/fwdkin_kdl.cpp)

target_link_libraries(fwdkin_kdl

${catkin_LIBRARIES}

)

Copyright (c) Walter Fetter Lages – p.25

Inclusão no Meta-Pacote

• O pacote eng10026_fwdkin será incluido nometa-pacote eng10026

• Editar o arquivo package.xml do pacoteeng10026 e incluir

<run_depend>eng10026_fwdkin</run_depend>

Copyright (c) Walter Fetter Lages – p.26

fwdkin_kdl.cpp

#include <ros/ros.h>

#include <sensor_msgs/JointState.h>

#include <kdl/chainfksolverpos_recursive.hpp>

#include <kdl/frames.hpp>

#include <kdl/frames_io.hpp>

#include <kdl_parser/kdl_parser.hpp>

class FwdKinKdl

{

public:

FwdKinKdl(ros::NodeHandle node);

~FwdKinKdl(void);

KDL::Frame getFrame(void) {return frame;};

Copyright (c) Walter Fetter Lages – p.27

fwdkin_kdl.cpp

private:

ros::NodeHandle node_;

ros::Subscriber jointStatesSubscriber;

KDL::Frame frame;

KDL::JntArray jointPositions;

KDL::ChainFkSolverPos_recursive ∗fwdKinSolver;

void jointStatesCB(const sensor_msgs::JointState::ConstPtr &

jointStates);

};

Copyright (c) Walter Fetter Lages – p.28

fwdkin_kdl.cpp

FwdKinKdl::FwdKinKdl(ros::NodeHandle node):jointPositions(0)

{

node_=node;

jointStatesSubscriber=node_.subscribe("joint_states",1000,&

FwdKinKdl::jointStatesCB,this);

std::string robotDescription;

if(!node.getParam("robot_description",robotDescription))

ROS_ERROR("Could not find ’robot_description’.");

KDL::Tree tree;

if (!kdl_parser::treeFromString(robotDescription,tree))

ROS_ERROR("Failed to construct KDL tree.");Copyright (c) Walter Fetter Lages – p.29

fwdkin_kdl.cpp

KDL::Chain chain;

if (!tree.getChain("origin_link","tool_link",chain))

ROS_ERROR("Failed to get chain from KDL tree.");

fwdKinSolver=new KDL::ChainFkSolverPos_recursive(chain);

int nJoints=chain.getNrOfJoints();

jointPositions.resize(nJoints);

}

Copyright (c) Walter Fetter Lages – p.30

fwdkin_kdl.cpp

FwdKinKdl::~FwdKinKdl(void)

{

jointStatesSubscriber.shutdown();

delete fwdKinSolver;

}

void FwdKinKdl::jointStatesCB(const sensor_msgs::JointState::

ConstPtr &jointStates)

{

for(int i=0;i < jointPositions.rows();i++)

jointPositions(i)=jointStates−>position[i];

if(fwdKinSolver−>JntToCart(jointPositions,frame) < 0)

ROS_ERROR("Failed to compute forward kinematics.");

} Copyright (c) Walter Fetter Lages – p.31

fwdkin_kdl.cpp

int main(int argc,char∗ argv[])

{

ros::init(argc,argv,"fwdkin_kdl");

ros::NodeHandle node;

FwdKinKdl fwdKinKdl(node);

ros::Rate loop(10);

while(ros::ok())

{

std::cout << "Frame:\n" << fwdKinKdl.getFrame() << std::endl;

ros::spinOnce();

loop.sleep();

}

return 0;

} Copyright (c) Walter Fetter Lages – p.32

Instalação do Pacote

• Baixar e descompactar o pacoteindigo-eng10026-fwdkin.tgz

cd ~/catkin_ws/src

wget http://www.ece.ufrgs.br/~fetter/eng10026/indigo−

eng10026−fwdkin.tgz

tar −xvzpf indigo−eng10026−fwdkin.tgz

cd ..

catkin_make

source $HOME/catkin_ws/devel/setup.bash

Copyright (c) Walter Fetter Lages – p.33

Execução

rosrun eng10026_fwkin fwdkin_kdl

• Movendo os sliders para mover o robô atransformação homogênea modifica-secorrespondentemente

Copyright (c) Walter Fetter Lages – p.34

Gráfico de Computação

Copyright (c) Walter Fetter Lages – p.35