Geração de Trajetórias no ROS

Geração de Trajetórias 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

Introdução

•

Implementada na biblioteca KDL (Kinematics

and Dynamics Library

)

•

Implementa classes para especificar:

•

Caminhos

•

Perfis de velocidade

•

Trajetórias de frames

•

Trajetória no espaço cartesiano

•

Trajetórias de cadeias cinemáticas

Classe KDL::Path

Classe KDL::Path

class Path { public:

enum IdentifierType { ID_LINE =1,ID_CIRCLE =2,

ID_COMPOSITE =3, ID_ROUNDED_COMPOSITE =4, ID_POINT =5, ID_CYCLIC_CLOSED =6 };

virtual double LengthToS(double length)=0; virtual double PathLength()=0;

virtual Frame Pos(double s) const=0;

virtual Twist Vel(double s,double sd) const=0;

virtual Twist Acc(double s,double sd,double sdd) const=0; virtual void Write(std::ostream &os)=0;

static Path ∗Read(std::istream &is); virtual Path ∗Clone()=0;

virtual IdentifierType getIdentifier() const=0; virtual ~Path() {};

KDL::Path_Circle

•

Implementa um caminho em arco de

circunferênca

class Path_Circle:public Path

{

public:

Path_Circle(const Frame &F_base_start,const Vector &

V_base_center,

const Vector &V_base_p,const Rotation &R_base_end, double alpha,RotationalInterpolation ∗otraj,

double eqradius,bool _aggregate=true); virtual ~Path_Circle();

KDL::Path_Composite

•

Permite a formação de caminhos compostos

class Path_Composite:public Path

{

public:

Path_Composite();

void Add(Path ∗geom,bool aggregate=true);

virtual int GetNrOfSegments(); virtual Path ∗GetSegment(int i);

virtual double GetLengthToEndOfSegment(int i);

virtual void GetCurrentSegmentLocation(double s,int &

segment_number,double &inner_s);

virtual ~Path_Composite();

KDL::Path_Cyclic_Closed

•

Permite a formação de caminhos fechados e

repetitivos

class Path_Cyclic_Closed:public Path

{

public:

Path_Cyclic_Closed(Path ∗_geom,int _times,bool _aggregate=true)

;

virtual ~Path_Cyclic_Closed();

KDL::Path_Line

•

Implementa um caminho em linha reta

class Path_Line:public Path

{

public:

Path_Line(const Frame &F_base_start,const Frame &F_base_end,

RotationalInterpolation ∗orient,double eqradius,

bool _aggregate=true);

Path_Line(const Frame &F_base_start,const Twist &twist_in_base,

RotationalInterpolation ∗orient,double eqradius,

bool _aggregate=true);

virtual ~Path_Line();

KDL::Path_Point

•

Implementa um caminho pontual

class Path_Point:public Path

{

public:

Path_Point(const Frame& F_base_start);

virtual ~Path_Point();

KDL::Path_RoundedComposite

•

Caminho composto com transições suaves

class Path_RoundedComposite:public Path

{

public:

Path_RoundedComposite(double radius,double eqradius,

RotationalInterpolation ∗orient,bool aggregate=true);

void Add(const Frame &F_base_point);

void Finish();

virtual int GetNrOfSegments(); virtual Path ∗GetSegment(int i);

virtual double GetLengthToEndOfSegment(int i);

virtual void GetCurrentSegmentLocation(double s,int &

segment_number,double &inner_s);

virtual ~Path_RoundedComposite();

KDL::RotationalInterpolation

•

Especifica a parte rotacional de um caminho no

KDL::RotationalInterpolation

class RotationalInterpolation

{

public:

virtual void SetStartEnd(Rotation start,Rotation end)=0; virtual double Angle()=0;

virtual Rotation Pos(double theta) const=0;

virtual Vector Vel(double theta,double thetad) const=0;

virtual Vector Acc(double theta,double thetad,double thetadd) const=0;

virtual void Write(std::ostream &os) const=0;

static RotationalInterpolation ∗Read(std::istream &is); virtual RotationalInterpolation ∗Clone() const=0;

virtual ~RotationalInterpolation();

KDL::RotationalInterpolation_SingleAxis

•

Interpolação através da rotação em torno de um

único eixo

class RotationalInterpolation_SingleAxis: public

RotationalInterpolation { public: RotationalInterpolation_SingleAxis(); virtual ~RotationalInterpolation_SingleAxis(); };

Classe KDL::VelocityProfile

•

Representa um perfil de velocidade

KDL::VelocityProfile

class VelocityProfile

{

public:

virtual void SetProfile(double pos1,double pos2)=0;

virtual void SetProfileDuration(double pos1,double pos2,double

duration)=0;

virtual double Duration() const=0;

virtual double Pos(double time) const=0; virtual double Vel(double time) const=0; virtual double Acc(double time) const=0; virtual void Write(std::ostream &os) const=0; static VelocityProfile ∗Read(std::istream &is); virtual VelocityProfile ∗Clone() const=0;

KDL::VelocityProfile_Dirac

•

Perfil de velocidade delta de Dirac

class VelocityProfile_Dirac:public VelocityProfile

{

public:

virtual ~VelocityProfile_Dirac();

KDL::VelocityProfile_Rectangular

•

Perfil de velocidade retangular

class VelocityProfile_Rectangular:public VelocityProfile

{

public:

VelocityProfile_Rectangular(double _maxvel=0);

void SetMax(double _maxvel);

virtual ~VelocityProfile_Rectangular();

KDL::VelocityProfile_Spline

•

Perfil de velocidade em spline

•

Polinômio de grau 5

class VelocityProfile_Spline:public VelocityProfile

{

public:

VelocityProfile_Spline();

VelocityProfile_Spline(const VelocityProfile_Spline &p);

virtual ~VelocityProfile_Spline();

virtual void SetProfileDuration(double pos1, double vel1, double

pos2, double vel2, double duration);

virtual void SetProfileDuration(double pos1, double vel1, double

acc1, double pos2, double vel2, double acc2, double duration);

KDL::VelocityProfile_Trap

•

Perfil de velocidade trapezoidal

class VelocityProfile_Trap:public VelocityProfile

{

public:

VelocityProfile_Trap(double _maxvel=0,double _maxacc=0);

virtual void SetProfileVelocity(double pos1,double pos2,double

newvelocity);

virtual void SetMax(double _maxvel,double _maxacc); virtual ~VelocityProfile_Trap();

KDL::VelocityProfile_TrapHalf

•

Perfil de velocidade trapezoidal pela metade

•

Só o início ou só o final

class VelocityProfile_TrapHalf:public VelocityProfile

{

public:

VelocityProfile_TrapHalf(double _maxvel=0,double _maxacc=0,

bool _starting=true);

void SetMax(double _maxvel,double _maxacc,bool _starting);

virtual ~VelocityProfile_TrapHalf();

Classe KDL::Trajectory

•

Implementa uma trajetória cartesiana

KDL::Trajectory

class Trajectory

{

public:

virtual double Duration() const=0;

virtual Frame Pos(double time) const=0; virtual Twist Vel(double time) const=0; virtual Twist Acc(double time) const=0; virtual Trajectory ∗Clone() const=0;

virtual void Write(std::ostream &os) const=0; static Trajectory ∗Read(std::istream &is);

virtual ~Trajectory();

KDL::Trajectory_Composite

•

Implementa uma trajetória cartesiana composta

•

Os elementos adicionados com Add() são

destruídos pelo destrutor da classe.

•

Devem ser alocados dinamicamente

class Trajectory_Composite:public Trajectory

{

public:

Trajectory_Composite();

virtual void Add(Trajectory ∗elem); virtual void Destroy();

virtual ~Trajectory_Composite();

KDL::Trajectory_Segment

•

Implementa um segmento de trajetória cartesiana

class Trajectory_Segment:public Trajectory

{

public:

Trajectory_Segment(Path ∗_geom,VelocityProfile ∗_motprof,bool

_aggregate=true);

Trajectory_Segment(Path ∗_geom,VelocityProfile ∗_motprof,double

duration,bool _aggregate=true);

virtual Path ∗GetPath();

virtual VelocityProfile ∗GetProfile(); virtual ~Trajectory_Segment();

KDL::Trajectory_stationary

•

“Trajetória” cartesiana com o robô parado

class Trajectory_Stationary:public Trajectory

{

public:

Trajectory_Stationary(double _duration,const Frame &_pos);

virtual ~Trajectory_Stationary();

Exemplo - Trajetória Cartesiana

•

Caminho especificado através de via points

•

(0.61, 0, 0.1477), (0.437, −0.424, 0.1477),

(0.238, −0.505, 0.1477),

(0.437, 0.424, 0.1477), (0.238, 0.505, 0.1477),

(0.61, 0, 0.1477)

•

Perfil de velocidade trapezoidal

•

Velocidade máxima 0.1 m/s

•

Aceleração 0.02 m/s

2

•

Trajetória fica parada 1 s no ponto final

•

Discretização de 0.1 s

•

Publica mensagem

geometry_msgs::PoseStamped

std_msgs/Header header uint32 seq time stamp string frame_id geometry_msgs/Pose pose geometry_msgs/Point position float64 x float64 y float64 z geometry_msgs/Quaternion orientation float64 x float64 y float64 z

Pacote eng10026_trajectories

•

Criar o pacote:

cd ~/catkin_ws/src

catkin_create_pkg eng10026_trajectories roscpp orocos_kdl

kdl_conversions −s Eigen3

•

package.xml

deve ser editado para configurar

os detalhes de documentação e incluir

dependências

CMakeLists.txt

•

Editar CMakeLists.txt para descomentar e

ajustar as tags add_executable e

target_link_libraries:

add_executable(pose_trajectory_publisher src/ pose_trajectory_publisher.cpp) target_link_libraries(pose_trajectory_publisher ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} )

Inclusão no Meta-Pacote

•

O pacote eng10026_trajectories será

incluido no meta-pacote eng10026

•

Editar o arquivo package.xml do pacote

eng10026

e incluir

pose_trajectory_publisher.cpp

#include <ros/ros.h> #include <kdl/path_roundedcomposite.hpp> #include <kdl/rotational_interpolation_sa.hpp> #include <kdl/velocityprofile_trap.hpp> #include <kdl/trajectory_segment.hpp> #include <kdl/trajectory_stationary.hpp> #include <kdl/trajectory_composite.hpp> #include <kdl/utilities/error.h> #include <kdl_conversions/kdl_msg.h> #include <geometry_msgs/PoseStamped.h>

pose_trajectory_publisher.cpp

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

{ ros::init(argc,argv,"trajectory_publisher"); ros::NodeHandle node; ros::Publisher posePublisher=node.advertise<geometry_msgs:: PoseStamped>("pose",10); try {

Path_RoundedComposite path(0.02,0.002,new

pose_trajectory_publisher.cpp

path.Add(Frame(Rotation::RPY(0,0,0),Vector(0.61,0,0.1477))); path.Add(Frame(Rotation::Quaternion(0,0,−0.375,0.926),Vector (0.437,−0.424,0.1477))); path.Add(Frame(Rotation::RotZ(−M_PI/2),Vector (0.238,−0.505,0.1477))); path.Add(Frame(Rotation::Quaternion(0,0,0.375,0.926),Vector (0.437,0.424,0.1477))); path.Add(Frame(Rotation::RotZ(M_PI/2∗0),Vector (0.238,0.505,0.1477))); path.Add(Frame(Rotation::RPY(0,0,0),Vector(0.61,0,0.1477))); path.Finish(); VelocityProfile_Trap velocityProfile(0.1,0.02);

pose_trajectory_publisher.cpp

Trajectory_Segment ∗trajectorySegment=new Trajectory_Segment

(&path,&velocityProfile,false);

Trajectory_Composite trajectory; trajectory.Add(trajectorySegment);

trajectory.Add(new Trajectory_Stationary(1.0,Frame(Rotation::

RPY(0,0,0),Vector(0.61,0,0.1477)))); double dt=0.1; double t=0.0; ros::Rate loop(1/dt); while(ros::ok()) { Frame pose=trajectory.Pos(t);

pose_trajectory_publisher.cpp

geometry_msgs::PoseStamped poseMsg; poseMsg.header.stamp=ros::Time::now(); poseMsg.header.frame_id="map"; tf::poseKDLToMsg(pose,poseMsg.pose); posePublisher.publish(poseMsg); ros::spinOnce(); loop.sleep(); if(t <= trajectory.Duration()) t+=dt; } }

pose_trajectory_publisher.cpp

catch(Error &error)

{

ROS_ERROR_STREAM("Error : " << error.Description() << std::

endl);

ROS_ERROR_STREAM("Type " << error.GetType() << std::endl

); }

posePublisher.shutdown();

return 0;

Visualização da Trajetória

•

A visualização de geometry_msgs/Pose no

rviz é através de um vetor

•

Não é muito adequada para vizualizar a

orientação em 3D

•

É mais conveniente vizualizar um frame

•

É necessário publicar o frame no tópico /tf

•

Fazer um conversor de

geometry_mgs/PoseStamped

para

tf:tfMessage

tf/tfMessage

geometry_msgs/TransformStamped[] transforms std_msgs/Header header uint32 seq time stamp string frame_id string child_frame_id geometry_msgs/Transform transform geometry_msgs/Vector3 translation float64 x float64 y float64 z geometry_msgs/Quaternion rotation float64 x float64 y float64 z

Pacote trajectory_conversions

•

Criar o pacote:

cd ~/catkin_ws/src

catkin_create_pkg trajectory_conversions roscpp orocos_kdl

kdl_conversions kdl_parser

geometry_msgs trajectory_msgs std_msgs

•

package.xml

deve ser editado para configurar

os detalhes de documentação e incluir

dependências

CMakeLists.txt

•

Editar CMakeLists.txt para descomentar e

ajustar as tags add_executable e

target_link_libraries:

add_executable(pose_stamped2tf src/pose_stamped2tf.cpp) target_link_libraries(pose_stamped2tf

${catkin_LIBRARIES} ${Eigen3_LIBRARIES} )

pose_stamped2tf.cpp

#include <ros/ros.h>

#include <geometry_msgs/PoseStamped.h> #include <tf/tfMessage.h>

pose_stamped2tf.cpp

class PoseStampedToTf

{

public:

PoseStampedToTf(const ros::NodeHandle &node,const char _∗

child_frame_id); ~PoseStampedToTf(void); private: ros::NodeHandle node_; ros::Subscriber poseSubscriber_; ros::Publisher tfPublisher_; std::string child_frame_id_;

void poseStampedCB(const geometry_msgs::PoseStamped::

pose_stamped2tf.cpp

PoseStampedToTf::PoseStampedToTf(const ros::NodeHandle &node,

const char _{∗child_frame_id)}

{

node_=node;

child_frame_id_=child_frame_id;

poseSubscriber_=node_.subscribe("pose",10,&PoseStampedToTf::

poseStampedCB,this); tfPublisher_=node_.advertise<tf::tfMessage>("/tf",10); } PoseStampedToTf::~PoseStampedToTf(void) { poseSubscriber_.shutdown(); tfPublisher_.shutdown();

pose_stamped2tf.cpp

void PoseStampedToTf::poseStampedCB(const geometry_msgs::

PoseStamped::ConstPtr &poseMsg) const

{ tf::tfMessage tfMsg; tfMsg.transforms.resize(1); tfMsg.transforms[0].header.stamp=poseMsg−>header.stamp; tfMsg.transforms[0].header.frame_id=poseMsg−>header.frame_id; tfMsg.transforms[0].child_frame_id=child_frame_id_;

pose_stamped2tf.cpp

tfMsg.transforms[0].transform.translation.x=poseMsg−>pose. position.x; tfMsg.transforms[0].transform.translation.y=poseMsg−>pose. position.y; tfMsg.transforms[0].transform.translation.z=poseMsg−>pose. position.z; tfMsg.transforms[0].transform.rotation=poseMsg−>pose.orientation ; tfPublisher_.publish(tfMsg); }

pose_stamped2tf.cpp

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

{

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

ros::NodeHandle node;

if(argc != 2)

{

ROS_ERROR_STREAM("pose_stamped2tf: No child frame id.\n"

);

return _−1;

}

pose_stamped2tf.cpp

ros::spin();

return 0;

display.launch

<launch>

<node name="trajectory_publisher" pkg="

eng10026_trajectories" type="pose_trajectory_publisher" />

<node name="pose2tf" pkg="trajectory_conversions" type="

pose_stamped2tf" args="trajectory" />

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

eng10026_trajectories)/rviz/display.rviz" />

Instalação dos Pacotes

•

Baixar e descompactar os pacotes

indigo-eng10026-trajectories.tgz

e

indigo-trajectory-conversions.tgz

cd ~/catkin_ws/src wget http://www.ece.ufrgs.br/ros−pkgs/indigo−eng10026− trajectories.tgz tar _{−xvzpf indigo−eng10026−trajectories.tgz} wget http://www.ece.ufrgs.br/ros−pkgs/indigo−trajectory− conversions.tgz tar _{−xvzpf indigo−trajectory−conversions.tgz} cd .. catkin_make

Execução

roslaunch eng10026_trajectories display.launch

Trajetória

-0.5 -0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.4 0.5 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.6 0.65 y(t) [m] x(t) [m] x(t) x y(t) 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.6 0.65 xc (t) [m] x(t) -0.5 -0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.4 0.5 y(t) [m] y(t)

Mapeamento para o Espaço das Juntas

•

As Poses no espaço cartesiano devem ser

mapeadas para o espaço das juntas pelo modelo

cinemático inverso

•

Publicadas através da mensagem

trajectory_msgs::

JointTrajectoryPoint

float64[] positions float64[] velocities float64[] accelerations float64[] effort duration time_from_start

CMakeLists.txt

•

incluída no pacote

trajectory_conversions

•

Editar CMakeLists.txt para adicionar as

tags add_executable

e

target_link_libraries:

add_executable(pose_stamped2joint src/pose_stamped2joint.

cpp)

target_link_libraries(pose_stamped2joint

${catkin_LIBRARIES} )

pose_stamped2joint.cpp

#include <ros/ros.h> #include <geometry_msgs/PoseStamped.h> #include <trajectory_msgs/JointTrajectoryPoint.h> #include <kdl/chainiksolverpos_lma.hpp> #include <kdl_conversions/kdl_msg.h> #include <kdl_parser/kdl_parser.hpp> class Pose2Joint { public:

Pose2Joint(ros::NodeHandle node,const std::string &root,const std::

string &tip,const Eigen::Matrix<double,6,1> &L);

pose_stamped2joint.cpp

private: ros::NodeHandle node_; ros::Subscriber poseSub_; ros::Publisher jointTrajPointPub_; KDL::Chain chain_; KDL::ChainIkSolverPos_LMA ∗ikSolverPos_; KDL::JntArray q_; ros::Time t0_;

void poseCB(const geometry_msgs::PoseStamped &pose);

pose_stamped2joint.cpp

Pose2Joint::Pose2Joint(ros::NodeHandle node,const std::string &root,

const std::string &tip,const Eigen::Matrix<double,6,1> &L):q_

(0) {

node_=node; KDL::Tree tree;

if(!kdl_parser::treeFromParam("robot_description",tree))

ROS_ERROR_STREAM("Failed to construct KDL tree.");

if(!tree.getChain(root,tip,chain_)) ROS_ERROR_STREAM("Failed

to get chain from KDL tree.");

pose_stamped2joint.cpp

t0_=ros::Time::now();

jointTrajPointPub_=node_.advertise<trajectory_msgs::

JointTrajectoryPoint>("joint_trajectory_point",10);

poseSub_=node_.subscribe("/pose",10,&Pose2Joint::poseCB,this);

} Pose2Joint::~Pose2Joint(void) { poseSub_.shutdown(); jointTrajPointPub_.shutdown(); delete ikSolverPos_; }

pose_stamped2joint.cpp

void Pose2Joint::poseCB(const geometry_msgs::PoseStamped &

poseStamped) {

KDL::Frame goalFrame;

tf::poseMsgToKDL(poseStamped.pose,goalFrame);

int error=ikSolverPos_−>CartToJnt(q_,goalFrame,q_);

if(error != 0) ROS_ERROR_STREAM("Failed to compute invere

kinematics: (" << error << ") "

pose_stamped2joint.cpp

trajectory_msgs::JointTrajectoryPoint jointTrajPoint; jointTrajPoint.positions.resize(q_.data.size()); Eigen::VectorXd::Map(&jointTrajPoint.positions[0],jointTrajPoint. positions.size())=q_.data; jointTrajPoint.time_from_start=poseStamped.header.stamp−t0_;; jointTrajPointPub_.publish(jointTrajPoint); }

pose_stamped2joint.cpp

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

{

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

if(argc < 3)

{

ROS_ERROR_STREAM("Please, provide a chain root and

a chain tip"); return _−1; } ros::NodeHandle node; Eigen::Matrix<double,6,1> L; L << 1.0 , 1.0 , 1.0, 0.01, 0.01, 0.01;

pose_stamped2joint.cpp

Pose2Joint pose2joint(node,argv[1],argv[2],L); ros::spin();

return 0;

trajectory.launch

<launch>

<node name="trajectory_publisher" pkg="eng10026_trajectories" type="pose_trajectory_publisher" />

<node name="pose2tf" pkg="trajectory_conversions" type="

pose_stamped2tf" args="trajectory" />

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

<node name="ik" pkg="trajectory_conversions" type="

pose_stamped2joint" args="origin_link tool_link 1 1 0 0 0 0" />

<node name="joint_demux" pkg="trajectory_conversions" type="

joint_demux" />

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

eng10026_trajectories)/rviz/display.rviz" />

Execução

roslaunch eng10026_trajectories trajectory.launch rostopic echo /joint_trajectory_point

Referência para o Controlador

•

Pode ser necessário mapear o tópico

/joint_trajectory_point

para as

referências dos controladores

•

Se forem utilizados controladores

independentes por junta, cada controlador tem

uma referência do tipo std_msg/Float64:

float64 data

•

Se for utilizado controlador por torque

calculado, a referência já é do tipo

trajectory_msgs/

JointTrajectoryPoint

CMakeLists.txt

•

incluído no pacote

trajectory_conversions

•

Editar CMakeLists.txt para adicionar as

tags add_executable

e

target_link_libraries:

add_executable(joint_demux src/joint_demux.cpp) target_link_libraries(joint_demux

${catkin_LIBRARIES} )

joint_demux.cpp

#include <string> #include <ros/ros.h> #include <std_msgs/Float64.h> #include <trajectory_msgs/JointTrajectoryPoint.h> class JointDemux { public: JointDemux(ros::NodeHandle node); ~JointDemux(void); private: ros::NodeHandle node_;

joint_demux.cpp

ros::Subscriber jointTrajPointSub_;

std::vector<ros::Publisher> jointPosPub_;

void jointTrajPointCB(const trajectory_msgs::JointTrajectoryPoint

&jointTrajPoint); };

JointDemux::JointDemux(ros::NodeHandle node) {

node_=node;

jointTrajPointSub_=node_.subscribe("joint_trajectory_point",10,&

JointDemux::jointTrajPointCB,this);

joint_demux.cpp

JointDemux::~JointDemux(void)

{

jointTrajPointSub_.shutdown();

for(int i=jointPosPub_.size()−1;i >= 0;i−−)

{

jointPosPub_[i].shutdown(); jointPosPub_.pop_back(); }

joint_demux.cpp

void JointDemux::jointTrajPointCB(const trajectory_msgs::

JointTrajectoryPoint &jointTrajPoint) {

for(int i=jointPosPub_.size();i < jointTrajPoint.positions.size();i

++) { ros::Publisher jointPosPub; jointPosPub=node_.advertise<std_msgs::Float64>("position "+std::to_string(i),10); jointPosPub_.push_back(jointPosPub); }

joint_demux.cpp

for(int i=0;i < jointTrajPoint.positions.size() && i <

jointPosPub_.size();i++) { std_msgs::Float64 cmdMsg; cmdMsg.data=jointTrajPoint.positions[i]; jointPosPub_[i].publish(cmdMsg); } }

joint_demux.cpp

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

{ ros::init(argc,argv,"joint_demux"); ros::NodeHandle node; JointDemux pose2joint(node); ros::spin(); return 0; }

gazebo.launch

<launch>

<include file="$(find q2d_bringup)/launch/gazebo.launch" />

<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

<node name="trajectory_publisher" pkg="eng10026_trajectories" type="pose_trajectory_publisher" />

<node name="pose2tf" pkg="trajectory_conversions" type="

pose_stamped2tf" args="trajectory" />

<node name="ik" pkg="trajectory_conversions" type="

gazebo.launch

<node name="joint_demux" pkg="trajectory_conversions" type="

joint_demux">

<remap from="position0" to="shoulder_controller/command" />

<remap from="position1" to="elbow_controller/command" />

</node>

<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="rviz" pkg="rviz" type="rviz" args="−d $(find

eng10026_trajectories)/rviz/gazebo.rviz" />

Execução

Trajetórias no Espaço das Juntas

•

Usualmente o caminho é uma reta

•

Pode-se usar diretamente a classe

KDL::VelocityProfile

e derivadas para

gerar a trajetória de cada junta

•

Movimento de (0, 0) a (

π

4

, −

π

4

)

•

Tempo final=5 s

•

Publica o tópico trajectory_msgs/

CMakeLists.txt

•

incluído no pacote

eng10026_trajectories

•

Editar CMakeLists.txt para adicionar as

tags add_executable

e

target_link_libraries:

add_executable(joint_trajectory_publisher src/ joint_trajectory_publisher.cpp) target_link_libraries(joint_trajectory_publisher ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} )

joint_trajectory_publisher.cpp

#include <ros/ros.h>

#include <kdl/velocityprofile_spline.hpp>

#include <trajectory_msgs/JointTrajectoryPoint.h> using namespace KDL;

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

{

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

ros::NodeHandle node;

ros::Publisher jointPublisher=node.advertise<trajectory_msgs::

joint_trajectory_publisher.cpp

const int nJoints=2;

std::vector<VelocityProfile_Spline> velocityProfile(nJoints);

double p0[nJoints]={0, 0};

double pf[nJoints]={M_PI_4,−M_PI_4}; double tf=5.0;

for(int i=0;i < velocityProfile.size();i++)

velocityProfile[i].SetProfileDuration(p0[i],0,0,pf[i],0,0,tf); double dt=0.1; double t=0.0; ros::Rate loop(1/dt); while(ros::ok()) {

joint_trajectory_publisher.cpp

trajectory_msgs::JointTrajectoryPoint jointMsg;

for(int i=0;i < velocityProfile.size();i++)

{ jointMsg.positions.push_back(velocityProfile[i].Pos(t)); jointMsg.velocities.push_back(velocityProfile[i].Vel(t)); jointMsg.accelerations.push_back(velocityProfile[i].Acc(t)); } jointMsg.time_from_start.fromSec(t); jointPublisher.publish(jointMsg); ros::spinOnce(); loop.sleep(); (t < tf) t+=dt;

joint_trajectory_publisher.cpp

jointPublisher.shutdown();

return 0;

gazebo_joint.launch

<launch>

<include file="$(find q2d_bringup)/launch/gazebo.launch" />

<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

<node name="trajectory_publisher" pkg="eng10026_trajectories" type="joint_trajectory_publisher" />

<node name="joint_demux" pkg="trajectory_conversions" type="

joint_demux">

<remap from="position0" to="shoulder_controller/command" />

<remap from="position1" to="elbow_controller/command" />

gazebo_joint.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="rviz" pkg="rviz" type="rviz" args="−d $(find

eng10026_trajectories)/rviz/gazebo_joint.rviz" />

Execução