imp ppt for phantom
DESCRIPTION
phantom omni bundle 6 link manipulatorTRANSCRIPT
![Page 1: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/1.jpg)
Ing. Cristian Secchi
Tel. 0522 522235
e-mail: [email protected]
http://www.dismi.unimore.it/index.php?q=secchi, http://www.arscontrol.org
CONTROL OF INDUSTRIAL ROBOTS Laurea Magistrale in Ingegneria Meccatronica
CONTROL OF INDUTRIAL ROBOTS Laboratory Projects
![Page 2: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/2.jpg)
Lab Projects-- 2
Create groups of 3 persons
Every group has to fulfill 3 different projects
1. Position regulation of an anthropomorphic robot
2. Sensor fusion
3. Trajectory tracking for a mobile robot
To use the lab, please refer to the “ARSControl LAB” calendar on
Google Calendar. To visualize the calendar, follow the link http://www.google.com/calendar/embed?src=0es9q1lf05e48aqvqc1fihhf2s%40group.calendar.google.com&ctz=Europe/Rome
There are two computers available for the projects, in the lab:
one is reserved to project 1, and the other one is for projects 2
and 3.
Each group can reserve each computer for no more than 2 consecutive hours, and no more than 4 hours per week
To reserve a time spot, please sent an email to
[email protected], [email protected],
[email protected] and [email protected]
Rules
Cristian Secchi
![Page 3: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/3.jpg)
Ing. Federica Ferraguti
Tel. 0522 522666
e-mail: [email protected]
PHANTOM OMNI HAPTIC DEVICE Position Regulation of an Anthropomorphic Robot
![Page 4: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/4.jpg)
Lab Projects-- 4 Cristian Secchi
The PHANTOM Omni model is the most cost-effective haptic device available today
PHANTOM Omni haptic device
Six degree-of-freedom:
position sensing
x y z
stylus gimbal
Pitch Roll Yaw
Communication interface: IEEE-1394 FireWire
Programming: OpenHaptics Toolkit and C/C++
![Page 5: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/5.jpg)
Lab Projects-- 5
The Phantom Omni is a 6 DOF manipulator.
All joints are rotational: the joints gives the translational
movements for the end-effector while the joints provide the rotational movements
Kinematic model
Cristian Secchi
![Page 6: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/6.jpg)
Lab Projects-- 6
The Phantom is only actuated in the first three joints, which gives
the ability to generate haptic forces in the translational
directions.
After employing the Denavit Hartenberg conversion on the
manipulator, the transformation matrix can be found and the
kinematics can then be expressed as
where
length of link 1
length of link 2
workspace transformation offset between
the origin of the haptic and the first joint
Kinematic model
Cristian Secchi
![Page 7: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/7.jpg)
Lab Projects-- 7
To transform velocities, accelerations and forces between joint
and Cartesian space the Jacobian matrix has to be known:
Kinematic model
Cristian Secchi
![Page 8: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/8.jpg)
Lab Projects-- 8
Dynamic model
Cristian Secchi
The general form of the equation of motion for robots and
manipulators is
where
is the inertia matrix
are the Coriolis and centrifugal forces
represents the gravity
is the vector of joints
is the vector of torques acting on the joints
![Page 9: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/9.jpg)
Lab Projects-- 9
Dynamic model
Cristian Secchi
The inertia matrix is found by examining the moment of inertia of
each link of the manipulator according to the joint angle, length
and mass.
![Page 10: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/10.jpg)
Lab Projects-- 10
Dynamic model
Cristian Secchi
The Coriolis and centrifugal effects appears for objects moving
with a rotating frame of reference. The Coriolis effect depends
both on mass and velocity but is independent of the position.
The centrifugal forces depends only of the position and the mass and is always oriented away from the axis of rotation.
![Page 11: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/11.jpg)
Lab Projects-- 11
Dynamic model
Cristian Secchi
Gravity forces depends on the joint angles, lengths and mass of
each link
The variables to are the inertia and gravity effect, mass
and length of the links
![Page 12: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/12.jpg)
Lab Projects-- 12
OpenHaptics Toolkit
Cristian Secchi
The OpenHaptics Toolkit include:
QuickHaptics micro API
micro API that makes it fast and easy to write new haptic
applications or to add haptics to existing applications.
Haptic Device API (HDAPI)
low-level access to the haptic device
Haptic Library API (HLAPI)
high-level haptics scene rendering
![Page 13: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/13.jpg)
Lab Projects-- 13
HDAPI Overview
Cristian Secchi
The Haptic Device API consists of two main components:
The device
Calls to the device typically involve managing state, setting
parameters, and sending forces
The scheduler
The scheduler manages a high frequency, high priority thread for
sending forces and retrieving state information from the device
Pattern of use for the Haptic Device API:
Initialize the device
Create scheduler callbacks to define force effects, typically calculated based on the position of the device
example: query the position of the device during each scheduler tick and
calculate a force based on that
Enable forces
Start the scheduler
Cleanup the device and scheduler when the application is terminated
![Page 14: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/14.jpg)
Lab Projects-- 14
HDAPI Programming: Initialization
Cristian Secchi
TemplateCIR.c
Initialize the Haptic Device
Devices are identified by their name, which is a readable string
found in the “PHANToM Configurations” control panel under
“PHANToM”.
Typically, if there is only one device, that will be named “Default
PHANToM”.
HHD hHD = hdInitDevice(HD_DEFAULT_DEVICE);
Schedule callbacks
The scheduler allows for calls to be run in the scheduler thread.
If the developer needs to make queries or change state, he should
do so within this loop; otherwise, since state is constantly changing,
it is typically unsafe for the application to query or set state.
The user should access variables that are modified by the haptics
thread only by using a scheduler callback.
![Page 15: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/15.jpg)
Lab Projects-- 15
HDAPI Programming: Initialization
Cristian Secchi
A callback’s prototype is of the form:
HDCallbackCode HDCALLBACK ControlCallback(void *pUserData);
The return value can be :
HD_CALLBACK_DONE : The callback is considered complete and it is
taken off the scheduler
HD_CALLBACK_CONTINUE : The callback is rescheduled and runs again
during the next scheduler tick
The callback is then scheduled through the command:
gCallbackHandle = hdScheduleAsynchronous(ControlCallback, 0,
HD_MAX_SCHEDULER_PRIORITY);
The asynchronous callback scheduling function returns a handle (gCallbackHandle)that can be used in the future to perform
operations on the callback.
These operations include unscheduling the callback - i.e. forcing it to terminate - or blocking until its completion.
![Page 16: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/16.jpg)
Lab Projects-- 16
HDAPI Programming: Initialization
Cristian Secchi
Callbacks are scheduled with a priority, which determines what
order they are run in the scheduler. For every scheduler tick, each
callback is always executed. The order the callbacks are executed
depends on the priority; highest priority items are run before lowest. Operations with equal priority are executed in arbitrary order.
Enable force output
Devices are initialized with forces set to off for safety. The next command in initialization is to enable forces:
hdEnable(HD_FORCE_OUTPUT);
The forces do not actually become enabled, however, until the
scheduler is started.
Start scheduler
The last call in initialization is to start the scheduler:
hdStartScheduler();
![Page 17: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/17.jpg)
Lab Projects-- 17
HDAPI Programming: Control Callback
Cristian Secchi
Haptic frames: define a scope within which the device state is
guaranteed to be consistent.
At the start of the frame, the device state is updated and stored
for use in that frame so that all state queries in the frame reflects a snapshot of that data.
At the end of the frame, new state such as forces is written out to
the device.
Calls to get last information such as last position yield information from the previous frame.
Most haptics operations should be run within a frame. Calling operations within a frame ensures consistency for the data being
used because state remains the same within the frame.
![Page 18: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/18.jpg)
Lab Projects-- 18
HDAPI Programming: Control Callback
Cristian Secchi
HDCallbackCode HDCALLBACK ControlCallback(void *pUserData)
{
...
hdBeginFrame(hdGetCurrentDevice());
Do some operation: put your code here
hdEndFrame(hdGetCurrentDevice());
...
}
Begin haptic frame
hdBeginFrame(hdGetCurrentDevice());
Each call to the above function makes the associated device
active.
hdGetCurrentDevice() gets the handle of the current device.
![Page 19: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/19.jpg)
Lab Projects-- 19
HDAPI Programming: Control Callback
Cristian Secchi
Do some operation
Put here the code to reach your goal.
The goal of the example in the template is to generate a spring
force effect that is anchored at the position (0, 0, 0)
Get device position: hdGetDoublev(HD_CURRENT_POSITION, position);
Compute forces as 𝐹 = 𝑘(𝑥𝑑𝑒𝑠𝑖𝑟𝑒𝑑 − 𝑥𝑐𝑢𝑟𝑟𝑒𝑛𝑡) : hduVecSubtract(force, pos_des, position);
hduVecScaleInPlace(force, gSpringStiffness);
Set the computed forces:
hdSetDoublev(HD_CURRENT_FORCE, force);
End haptic frame
hdEndFrame();
Causes forces and other states to be written to the device.
![Page 20: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/20.jpg)
Lab Projects-- 20
Get Parameters
Set Parameters
See the API reference manual if you need the complete list of the
Get and Set parameters.
You can download it from ARSControl website.
HDAPI Programming: Get and Set main parameters
Cristian Secchi
Parameter Name Description N Type Units
HD_CURRENT_POSITION Get the current position of the
device facing the device base
3 Hdouble mm
HD_CURRENT_JOINT_ANGLES Get the joints angles of the device 3 Hdouble rad
HD_CURRENT_GIMBAL_ANGLES
Get the angles of the device gimbal
3 Hdouble rad
OpenHaptics Toolkit_API Reference Manual
Parameter Name Description N Type Units
HD_CURRENT_FORCE Set the current force as Cartesian coordinated vector
3 Hdouble N
![Page 21: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/21.jpg)
Lab Projects-- 21
HDAPI Programming: Cleanup
Cristian Secchi
At the end of the program, before application exit, the scheduler
should be stopped and all scheduler operations terminated.
Callbacks can be terminated either through the application calling hdUnschedule(), or through the callback itself knowing to return HD_CALLBACK_DONE. Finally, the device should be disabled.
The following shows the typical cleanup sequence:
Stop scheduler
hdStopScheduler();
Terminate callbacks
hdUnschedule(gCallbackHandle);
Disable the Haptic Device
hdDisableDevice(hdGetCurrentDevice());
![Page 22: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/22.jpg)
Lab Projects-- 22
Goal of the project
Cristian Secchi
Given an arbitrary set of points in the Cartesian space implement
the control law that makes the end-effector of the Phantom Omni
reach them one by one.
At the end, the stylus have to return to the starting point (0, 0, 0).
The choice of the control law to be used is left to the student.
![Page 23: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/23.jpg)
Lab Projects-- 23
Instructions
Cristian Secchi
In the ARSControl Laboratory there is a PC reserved for that project.
On the Desktop you can find two files:
Instructions for Phantom Omni projects.txt
Follow the instructions reported on the text
file
Phantom Omni Examples
Folder containing the source code of the
template and other examples
Start from the Instructions!
![Page 24: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/24.jpg)
Ing. Alessio Levratti
Tel. 0522 522666
e-mail: [email protected]
INERTIAL MEASUREMENT UNIT Extended Kalman Filter Assignement
![Page 25: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/25.jpg)
Lab Projects-- 25 Cristian Secchi
An Inertial Measurement Unit is a device for the orientation
measurement.
It is made with three kind of transducers:
3 Gyroscopes: provide the angular velocity measurements
according three axis (x,y,z)
3 Accelerometers: provide the linear acceleration measurements
according three axis (x,y,z)
[Three axis magnetometer (or compass): provides the earth
geomagnetic field measurement and returns the heading (ψ).]
I.M.U. – Inertial Measurement Unit
![Page 26: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/26.jpg)
Lab Projects-- 26
Accelerometers and gyroscopes are made with MEMS
technology.
This lead this sensor to be affected by a very high bias
noise. Furthermore the IMU is affected by a scale factor
error, misallignement error and white noise.
Sensor noise sources
Cristian Secchi
![Page 27: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/27.jpg)
Lab Projects-- 27
The bias for gyro/accelerometer is the average over a
specified time of accelerometer/gyro output measured
at specified operating conditions that have no
correlation with input acceleration or rotation. The gyro
bias is typically expressed in degree per hour (°/h) or radian per second (rad/s) and the accelerometer bias
is expressed in meter per second square [m/s2 or g].
Bias noise
Cristian Secchi
![Page 28: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/28.jpg)
Lab Projects-- 28
Scale factor is the ratio of a change in the input
intended to be measured. Scale factor is generally
evaluated as the slope of the straight line that can be
fit by the method of least squares to input-output data.
The scale factor error is deterministic in nature and can
be determined by calibration.
Scale factor error
Cristian Secchi
![Page 29: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/29.jpg)
Lab Projects-- 29
Misallignement:
Axes misalignment is the error resulting from the
imperfection of mounting the sensors. Axes
misalignment can, in general, be compensated or
modeled in the IMU error equation
White noise:
Noise is an additional signal resulting from the sensor
itself or other electronic equipment that interfere with
the output signals trying to measure. Noise is in general
non-systematic and therefore cannot be removed from
the data using deterministic models.
Misallignement and white noise
Cristian Secchi
![Page 30: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/30.jpg)
Lab Projects-- 30
By integrating the IMU measurements in the navigation
algorithm, these errors will be accumulated, leading to
significant drift in the position and velocity outputs. A
stand-alone IMU by itself is seldom useful since the
inertial sensor biases and the fixed-step integration
errors will cause the navigation solution to diverge
quickly.
I.M.U. - Issues
Cristian Secchi
![Page 31: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/31.jpg)
Lab Projects-- 31
The Nintendo WiiMote
Cristian Secchi
It is a six degrees of
freedom Inertial
Measurement Unit.
It has 3
accelerometers and 3
gyroscopes
![Page 32: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/32.jpg)
Lab Projects-- 32
Realize an Extended Kalman Filter which estimates the
orientation of an Inertial Measurement Unit, the
WiiMote, by filtering and merging the data coming from
3 Gyroscopes and 3 Accelerometers.
Objective
Cristian Secchi
Gyroscopes
Accelerometers
Extended Kalman Filter
Angular rotation (P,Q,R)
Linear accelerations (x,y,z)
Orientation (ϕ,ϑ,ψ)
![Page 33: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/33.jpg)
Lab Projects-- 33
1. Realize a ROS node which acquires the data
coming from the WiiMote and characterize it (find
the covariance matrix related to the accelerometer
and to the gyroscope).
2. Create a ROS node which implements an Extended
Kalman Filter.
For a better interpretation the output must be
converted in Euler angles.
The filter must avoid singularities (see ‘Gimbal lock’)
3. Compare the output of the designed sensor fusion
algorithm with the simple time integration of the
gyroscopes measurements.
Activities
Cristian Secchi
![Page 34: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/34.jpg)
Lab Projects-- 34
Choose the best representation of the orientation of
the device: Euler angles, axis-angle representation,
DCM (direction cosine matrix), quaternions, etc…
The sensor characterization and the covariance
matrices are fundamental for the design of a robust
EKF.
Beware of angles!!!! 𝜑, 𝜗, 𝜓 ∈ −𝜋, 𝜋 3
Be carefull with the signs of the measurements.
Use 𝑎𝑡𝑎𝑛2 𝑦, 𝑥 instead of using 𝑎𝑡𝑎𝑛𝑥
𝑦.
Having any issue? Check on internet…
Tips & tricks
Cristian Secchi
![Page 35: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/35.jpg)
Ing. Alan Spaggiari
Tel. 0522 522666
e-mail: [email protected]
TRAJECTORY TRACKING FOR A MOBILE ROBOT Feedback Linearization
![Page 36: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/36.jpg)
Lab Projects-- 36
Feedback Linearization
Cristian Secchi
Feedback linearization is a common approach used in
controlling nonlinear systems.
The approach involves coming up with a transformation of the
nonlinear system into an equivalent linear system through a change of variables and a suitable control input.
Feedback linearization may be applied to nonlinear systems of
the form
where x is the state vector, u is the vector of inputs, and y is the vector of outputs. The goal is to develop a control input
that renders a linear input–output map between the new input
and the output.
![Page 37: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/37.jpg)
Lab Projects-- 37
Feedback Linearization
Cristian Secchi
rxgraph: display a visualization of a ROS Computation Graph, i.e. the
ROS nodes that are currently running, as well as the ROS topics that
connect them.
Example: Driver Pioneer P3AT
$ roslaunch p2os_launch p2os_driver_usb.launch
![Page 38: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/38.jpg)
Lab Projects-- 38
Feedback Linearization
Cristian Secchi
Example: Feedback Linearization, driver and all topics.
![Page 39: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/39.jpg)
Lab Projects-- 39
Feedback Linearization
Cristian Secchi
Example: Only Feedback Linearization and driver
feedback_linearization received a pose message (position and
orientation) from p2os
feedback_linearization elaborates the new robot input
feedback_linearization sends the new values to the driver by cmd_vel
message (linear and angular velocity)
![Page 40: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/40.jpg)
Lab Projects-- 40
Feedback Linearization
Cristian Secchi
http://ros.org/doc/api/nav_msgs/html/msg/Odometry.html
![Page 41: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/41.jpg)
Lab Projects-- 41
Feedback Linearization
Cristian Secchi
http://www.ros.org/doc/api/geometry_msgs/html/msg/Twist.html
![Page 42: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/42.jpg)
Lab Projects-- 42
Feedback Linearization
Cristian Secchi
int main (int argc, char** argv)
{
ros::init(argc, argv, "feedback_linearization");
//Initialise and create a ROS node with associate name "feedback_linearization"
ros::NodeHandle node;
//Active the node and allocate the memory
ros::Rate r(10);
//Set the publication rate
ros::Subscriber sub = node.subscribe<nav_msgs::Odometry>("/pose", 100,
&poseCallback);
//Associates the subscriber of the object node with the subscriber that we have
create. It will receive a odometry message with 100 elements queue.
cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 100);
//Associates the advertise of the object node with the advertise that we have
create. It will send a message named "cmd_vel" with a 100 elements queue.
![Page 43: Imp Ppt for Phantom](https://reader035.vdocument.in/reader035/viewer/2022062222/55cf94e3550346f57ba51afd/html5/thumbnails/43.jpg)
Lab Projects-- 43
Feedback Linearization
Cristian Secchi
//Initialization of global variables
/*
...
*/
while (ros::ok()) //The node is active until the pressure of keys "ctrl+c"
{
ros::spinOnce(); //poseCallback call
r.sleep();
}
}
void poseCallback(const nav_msgs::Odometry::ConstPtr& msg)
//Is a function who reads the values of odometry message and public the new
values of velocity.