Cinemática Direta no ROSfetter/eng10026/ros_fwdkin.pdfNodo joint_state_publisher • Mostra janela...
Transcript of 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
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
• 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
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:
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
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
Copyright (c) Walter Fetter Lages – p. 16
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
Copyright (c) Walter Fetter Lages – p. 17
package.xml
• O arquivo package.xml deve ser editado parapreenchimento dos dados do pacote• Descrição• Mantenedor• Licença• Autor• Dependências• Exportações
Copyright (c) Walter Fetter Lages – p. 18
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}
)
Copyright (c) Walter Fetter Lages – p. 19
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. 20
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. 21
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
Copyright (c) Walter Fetter Lages – p. 23
Instalação do Pacote
• 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
Copyright (c) Walter Fetter Lages – p. 24
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. 25
Gráfico de Computação
Copyright (c) Walter Fetter Lages – p. 26
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
Copyright (c) Walter Fetter Lages – p. 27
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
Copyright (c) Walter Fetter Lages – p. 28
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. 29
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. 30
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}
)
Copyright (c) Walter Fetter Lages – p. 31
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. 32
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. 33
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. 34
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. 35
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. 36
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
Instalação do Pacote
• 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
Copyright (c) Walter Fetter Lages – p. 39
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. 40
Gráfico de Computação
Copyright (c) Walter Fetter Lages – p. 41
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
Copyright (c) Walter Fetter Lages – p. 42