Jacobiano no ROS. Walter Fetter Lages

(1)

Jacobiano no ROS

Walter Fetter Lages

fetter@ece.ufrgs.br

Universidade Federal do Rio Grande do Sul Escola de Engenharia

Departamento de Sistemas Elétricos de Automação e Energia ENG10026 Robótica A

(2)

Jacobiano no ROS

•

Implementado na biblioteca KDL (Kinematics

and Dynamics Library

)

•

Implementa classes virtuais para cálculo do

Jacobiano, Jacobiano inverso e suas Derivadas

•

A maioria calcula diretamente a velocidade e/ou

aceleração e não os Jacobianos em si

•

Inversas calculadas numericamente

•

Várias implementações, com algoritmos

(3)

Classes Virtuais

KDL::ChainFkSolverVel:

Velocidade da garra

a partir da velocidade das juntas

KDL::ChainIkSolverVel:

Velocidade das

juntas a partir da velocidade da garra

KDL::ChainFkSolverAcc:

Aceleração da garra

a partir da aceleração das juntas

KDL::ChainIkSolverAcc:

Aceleração das

(4)

Classes para o Jacobiano

KDL::ChainJntToJacSolver:

Jacobiano

int JntToJac(const JntArray &q_in,Jacobian &jac,int

segmentNR=−1)

int setLockedJoints(const std::vector<bool> locked_joints)

KDL::ChainFkSolverVel_recursive:

Velocidade da garra a partir da velocidade das

juntas, calculada recursivamente

int JntToCart(const JntArrayVel &q_in,FrameVel &out,int

segmentNr=−1)

(5)

Classes para o Jacobiano Inverso

KDL::ChainIkSolverVel_pinv:

Pseudo-inversa calculada por SVD

KDL::ChainIkSolverVel_pinv_givens:

Pseudo-inversa calculada usando rotação de

Givens

KDL::ChainIkSolverVel_pinv_nso:

Pseudo-inversa generalizada, com minimização

do erro quadrático para robôs redundantes

KDL::ChainIkSolverVel_wdls:

Mínimos

quadrados amortecidos, considerando

ponderações no espaço das juntas e no espaço

cartesiano

(6)

Funções para o Jacobiano Inverso

int CartToJnt(const JntArray &q_in,const Twist &v_in,JntArray & qdot_out)

int CartToJnt(const JntArray &q_init,const FrameVel &v_in, JntArrayVel &q_out)

(7)

Classes para a Derivada do Jacobiano

KDL::ChainJntToJacDotSolver:

Derivada

do Jacobiano

int JntToJacDot(const KDL::JntArrayVel &q_in,KDL::Jacobian &jdot,int _{seg_nr=−1)}

void setBodyFixedRepresentation(void)

void setHybridRepresentation(void)

void setInternialRepresentation(void)

int setLockedJoints(const std::vector<bool> locked_joints)

KDL::ChainFkSolverAcc:

Aceleração da garra

a partir da aceleração das juntas

int JntToCart(const JntArrayAcc &q_in,FrameAcc &out,int

segmentNr=−1)

int JntToCart(const JntArrayAcc &q_in,std::vector<FrameAcc> &out,int _{segmentNr=−1)} Copyright (c) Walter Fetter Lages – p.7

(8)

Mapeamento Inverso da Aceleração

KDL::ChainIkSolverAcc:

Aceleração das

juntas a partir da aceleração da garra

int CartToJnt(const JntArray &q_in,const JntArray &qdot_in,

const Twist a_in,JntArray &qdotdot_out)

int CartTojnt(const JntArray &q_init,const FrameAcc &a_in, JntArrayAcc &q_out)

int CartToJnt(const JntArray &q_in,const Twist &v_in,const

Twist &a_in,JntArray &qdot_out,JntArray &qdotdot_out)

int CartTojnt(const JntArray &q_init,const Frame &p_in,const

JntArray &qdot_in,const Twist &a_in, JntArray &q_out, JntArray &qdotdot_out)

(9)

Exemplo

•

O tópico /tf publica as transformações entre os

vários sistemas de coordenadas

•

O tópico /joint_state publica posição,

velocidade, aceleração e esforço nas juntas

•

Será criado um nodo para publicar velocidade da

garra

•

Será usado o pacote q2d_description

•

Já usado e instalado anteriormente

(10)

Instalação do Pacote

•

Baixar e descompactar o pacote

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

(11)

Velocidades Usando a KDL

•

Neste exemplo, será obtida como objeto do tipo

KDL::FrameVel

•

Classe para representar velocidades

cartesianas definida na KDL

•

Será criado o nodo twist_publisher que

publicará o twist de velocidade

(12)

Pacote eng10026_jacobian

•

Criar o pacote:

cd ~/catkin_ws/src

catkin_create_pkg eng10026_jacobian roscpp orocos_kdl

kdl_parser kdl_conversions sensor_msgs geometry_msgs −s Eigen

•

package.xml

deve ser editado para configurar

os detalhes de documentação e incluir

dependências

(13)

CMakeLists.txt

•

Editar CMakeLists.txt para descomentar e

ajustar as tags add_executable e

target_link_libraries:

add_executable(twist_publisher src/twist_publisher.cpp) target_link_libraries(twist_publisher ${catkin_LIBRARIES} ${Eigen_LIBRARIES} )

(14)

Ajustes para usar a Eigen

•

Ajustar/incluir no CMakeLists.txt as tags:

(15)

Inclusão no Meta-Pacote

•

O pacote eng10026_jacobian será incluido

no meta-pacote eng10026

•

Editar o arquivo package.xml do pacote

eng10026

e incluir

(16)

twist_publisher.cpp

#include <ros/ros.h> #include <geometry_msgs/TwistStamped.h> #include <sensor_msgs/JointState.h> #include <kdl/chainfksolvervel_recursive.hpp> #include <kdl_conversions/kdl_msg.h> #include <kdl_parser/kdl_parser.hpp> class TwistPublisher { public: TwistPublisher(ros::NodeHandle node); ~TwistPublisher(void);

(17)

twist_publisher.cpp

private: ros::NodeHandle node_; ros::Subscriber jointStatesSubscriber_; ros::Publisher twistPublisher_; KDL::JntArrayVel jointPosVel_; KDL::ChainFkSolverVel_recursive ∗fkSolverVel_;

void jointStatesCB(const sensor_msgs::JointState::ConstPtr & jointStates);

(18)

twist_publisher.cpp

TwistPublisher::TwistPublisher(ros::NodeHandle node) :jointPosVel_(0)

{

node_=node;

jointStatesSubscriber_=node_.subscribe("joint_states",100,& TwistPublisher::jointStatesCB,this);

twistPublisher_=node_.advertise<geometry_msgs::TwistStamped>(" twist",100);

std::string robotDescription;

(19)

twist_publisher.cpp

KDL::Tree tree;

if (!kdl_parser::treeFromString(robotDescription,tree)) ROS_ERROR("Failed to construct KDL tree.");

KDL::Chain chain;

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

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

fkSolverVel_=new KDL::ChainFkSolverVel_recursive(chain);

jointPosVel_.resize(chain.getNrOfJoints()); }

(20)

twist_publisher.cpp

TwistPublisher::~TwistPublisher(void) { jointStatesSubscriber_.shutdown(); twistPublisher_.shutdown(); delete fkSolverVel_; }

void TwistPublisher::jointStatesCB(const sensor_msgs::JointState:: ConstPtr &jointStates)

{

for(int i=0;i < jointPosVel_.q.rows();i++) {

(21)

twist_publisher.cpp

void TwistPublisher::publish(void) const

{

KDL::FrameVel frameVel;

if_{(fkSolverVel_−>JntToCart(jointPosVel_,frameVel) < 0)}

ROS_ERROR("Failed to compute forward kinematics velocity."); geometry_msgs::TwistStamped twistMsg; twistMsg.header.stamp=ros::Time::now(); twistMsg.header.frame_id="tool_link"; tf::twistKDLToMsg(frameVel.GetTwist(),twistMsg.twist); twistPublisher_.publish(twistMsg); }

(22)

twist_publisher.cpp

int main(int argc,char_{∗ argv[])}

{ ros::init(argc,argv,"twist_publisher"); ros::NodeHandle node; TwistPublisher twistPublisher(node); ros::Rate loop(10); while(ros::ok()) { twistPublisher.publish(); ros::spinOnce(); loop.sleep(); }

(23)

Instalação do Pacote

•

Baixar e descompactar o pacote

indigo-eng10026-jacobian.tgz

cd ~/catkin_ws/src wget _{http://www.ece.ufrgs.br/~fetter/eng10026/indigo−} eng10026−jacobian.tgz tar _{−xvzpf indigo−eng10026−jacobian.tgz} cd .. catkin_make source $HOME/catkin_ws/devel/setup.bash

(24)

display.launch

<launch>

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

<param name="publish_default_velocities" value="true" /> <param name="publish_default_efforts" value="true" /> <rosparam param="source_list">[’partial_joint_states’]</

rosparam>

<include file="$(find q2d_description)/launch/display.launch"> <arg name="use_gui" value="$(arg use_gui)" />

</include>

<node name="twist_publisher" pkg="eng10026_jacobian" type=" twist_publisher" />

(25)

Execução

roslaunch eng10026_jacobian display.launch use_gui:=true

•

Publicar a velocidade das juntas

rostopic pub /partial_joint_states sensor_msgs/JointState ’{ name: ["shoulder_active_joint", "elbow_active_joint"], velocity: [1.0, 1.0]}’

•

Verificar a velocidade da garra

rostopic echo /twist

•

Movendo os sliders para mover o robô a

transformação homogênea e a velocidade da

garra modificam-se correspondentemente

(26)