Imp Ppt for Phantom

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

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

mailto:[email protected]




Ing. Federica Ferraguti

Tel. 0522 522666


PHANTOM OMNI HAPTIC DEVICE Position Regulation of an Anthropomorphic Robot

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

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

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

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

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

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.

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.

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

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

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

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.

Lab Projects-- 15


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.

Lab Projects-- 16


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();

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.

Lab Projects-- 18


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.

Lab Projects-- 19


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.

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

http://www.arscontrol.org/uploads/federica/OpenHaptics Toolkit_API Reference Manual.pdf

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());

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.

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!

Ing. Alessio Levratti

Tel. 0522 522666


INERTIAL MEASUREMENT UNIT Extended Kalman Filter Assignement

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

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

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

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

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

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

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

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 (ϕ,ϑ,ψ)

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

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

Ing. Alan Spaggiari

Tel. 0522 522666


TRAJECTORY TRACKING FOR A MOBILE ROBOT Feedback Linearization

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.

Lab Projects-- 37


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

Lab Projects-- 38


Cristian Secchi

Example: Feedback Linearization, driver and all topics.

Lab Projects-- 39


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)

Lab Projects-- 40


Cristian Secchi

http://ros.org/doc/api/nav_msgs/html/msg/Odometry.html

http://ros.org/doc/api/nav_msgs/html/msg/Odometry.html

Lab Projects-- 41


Cristian Secchi

http://www.ros.org/doc/api/geometry_msgs/html/msg/Twist.html



Lab Projects-- 42


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.

Lab Projects-- 43


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.

Documents

Imp Ppt for Phantom