Cinemática Direta no ROS

Transcrição

1 Cinemática Direta no ROS Walter Fetter Lages 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

2 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

3 Exemplo Será usado o pacoteq2d_description Já usado e instalado anteriormente Copyright (c) Walter Fetter Lages p.3

4 Instalação do Pacote Baixar e descompactar o pacote indigo-q2d-description.tgz cd ~/catkin_ws/src wget q2d description.tgz tar xvzpf indigo q2d description.tgz catkin_make source ~/catkin_ws/devel/setup.bash Copyright (c) Walter Fetter Lages p.4

5 Nodorobot_state_publiser Subscreve o tópico/joint_states Obtém a descrição URDF do robô do servidor de parâmetros Parâmetrorobot_description Usa a KDL para calcular as transformações entre os frames Publica as transformaçoes no tópico/tf Por default, a 50 Hz Copyright (c) Walter Fetter Lages p.5

6 Nodojoint_state_publisher Mostra janela com sliders e permite alterar as variá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 usar controlador 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

7 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=" 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

8 Comando de Launch roslaunch q2d_description display.launch use_gui:=true & Copyright (c) Walter Fetter Lages p.8

9 Visualização no Rviz Copyright (c) Walter Fetter Lages p.9

10 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

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

12 Árvore de Transformações view_frames Gera uma descrição em.pdf da árvore de transformações rosrun tf view_frames Copyright (c) Walter Fetter Lages p.12

13 Árvore de Transformações Copyright (c) Walter Fetter Lages p.13

14 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

15 Modelo Cinemático em C++ Pode ser obtido de/tf Neste exemplo será convertida para o 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 Nodotf2_kdl que mostra o frame na tela Incluído no pacoteeng10026_tf2 Copyright (c) Walter Fetter Lages p.15

16 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

17 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

18 tf2_kdl.cpp KDL::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

19 Instalação do Pacote Baixar e descompactar o pacote indigo-eng10026-tf2.tgz cd ~/catkin_ws/src wget 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

20 Execução rosrun eng10026_tf2 tf2_kdl Movendo os sliders para mover o robô a transformação homogênea modifica-se correspondentemente Copyright (c) Walter Fetter Lages p.20

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

22 Modelo Cinemático Usando a KDL Pode ser obtido direto da descrição em URDF Neste exemplo, será otido como um objeto do tipokdl::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 nodofwdkin_kdl que mostra o frame na tela Incluído no pacoteeng10026_fwdkin Copyright (c) Walter Fetter Lages p.22

23 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

24 Pacoteeng10026_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 configurar os detalhes de documentação e incluir dependências Copyright (c) Walter Fetter Lages p.24

25 CMakeLists.txt EditarCMakeLists.txt para descomentar e ajustar as tagsadd_executable e target_link_libraries: add_executable(fwdkin_kdl src/fwdkin_kdl.cpp) target_link_libraries(fwdkin_kdl ) ${catkin_libraries} Copyright (c) Walter Fetter Lages p.25

26 Inclusão no Meta-Pacote O pacoteeng10026_fwdkin será incluido no meta-pacoteeng10026 Editar o arquivopackage.xml do pacote eng10026 e incluir <run_depend>eng10026_fwdkin</run_depend> Copyright (c) Walter Fetter Lages p.26

27 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

28 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

29 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

30 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

31 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

32 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

33 Instalação do Pacote Baixar e descompactar o pacote indigo-eng10026-fwdkin.tgz cd ~/catkin_ws/src wget 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

34 Execução rosrun eng10026_fwkin fwdkin_kdl Movendo os sliders para mover o robô a transformação homogênea modifica-se correspondentemente Copyright (c) Walter Fetter Lages p.34

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