Cinemática Direta no ROSfetter/eng10026/ros_fwdkin.pdfNodo joint_state_publisher • Mostra janela...

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

ENG10026 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


Exemplo

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


Instalação do Pacote

• Clonar e compilar o repositório q2d

cd ~/catkin_ws/src

git clone −b $ROS_DISTRO http://git.ece.ufrgs.br/q2d

cd ..

catkin_make

source ~/catkin_ws/devel/setup.bash


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


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


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>


Comando de Launch

roslaunch q2d_description display.launch use_gui:=true &


Visualização no Rviz


Gráfico de Computação

• Em outro terminal:

rqt_graph &

• Visualizar o que é publicado nos tópicos:

rostopic echo /joinst_states

rostopic echo /tf


Árvore de Transformações

• view_frames

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

rosrun tf view_frames


Árvore de Transformações


Consulta a uma Transformação

• tf_echo

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

rosrun tf tf_echo base_link tool_link


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


Estrutura do Pacote

eng10026_tf2/CMakeLists.txtlaunch/display.launchdisplay_abc.launchpublish_abc.launchtf2_kdl.launch

package.xml

rviz/eng10026_tf2.rviz

src/tf2_kdl.cpp


Criação do Pacote

cd ~/catkin_ws/src

catkin_create_pkg eng10026_tf2

eng10026_tf2/CMakeLists.txtpackage.xml

• package.xml deve ser editado parapreenchimento dos dados do pacote

• CmakeLists.txt deve ser editado paraconfigurar os detalhes de compilação do pacote


package.xml

• O arquivo package.xml deve ser editado parapreenchimento dos dados do pacote• Descrição• Mantenedor• Licença• Autor• Dependências• Exportações


CMakeLists.txt

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

add_executable(tf2_kdl src/tf2_kdl.cpp)

target_link_libraries(tf2_kdl

${catkin_LIBRARIES}

)


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


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;

}


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. 22

Reconfigurar o Ambiente

cd ~/catkin_ws

catkin_make

source ~/catkin_ws/devel/setup.bash

• catkin_make compila o pacote• Deve ser sempre executado no diretório~/catkin_ws



• Clonar e compilar o repositório eng10026_tf2

cd ~/catkin_ws/src

git clone −b $ROS_DISTRO http://git.ece.ufrgs.br/eng10026/

eng10026_tf2

cd ..

catkin_make

source $HOME/catkin_ws/devel/setup.bash


Execução

rosrun eng10026_tf2 tf2_kdl

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


Exercício

• Criar um arquivo de launch que lance robô noRviz, a interface gráfica para movimentar o robôno Rviz e o nodo tf2_kdl


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• Documentação em<http://www.orocos.org/kdl/user-manual>

• Será criado o nodo fwdkin_kdl que mostra oframe na tela

• Incluído no pacote eng10026_fwdkin


<http://www.orocos.org/kdl/user-manual>

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

}


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


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}

${orocos_kdl_LIBRARIES}

)


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>


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


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

};


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.");


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

}


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. 37

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. 38


• Clonar e compilar o repositórioeng10026_fwdkin

cd ~/catkin_ws/src

git clone −b $ROS_DISTRO http://git.ece.ufrgs.br/eng10026/

eng10026_fwdkin

cd ..

catkin_make

source $HOME/catkin_ws/devel/setup.bash


Execução

rosrun eng10026_fwkin fwdkin_kdl

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


Exercício

• Criar um arquivo de launch que lance robô noRviz, a interface gráfica para movimentar o robôno Rviz e o nodo fwdkin_kdl


Cinemática Direta no ROSfetter/eng10026/ros_fwdkin.pdfNodo joint_state_publisher • Mostra janela...

Documents

Transcript of Cinemática Direta no ROSfetter/eng10026/ros_fwdkin.pdfNodo joint_state_publisher • Mostra janela...