ROBOTICS –01PEEQW Laboratory –Project #1 · Basilio Bona ROBOTICS 01PEEQW -2014/2015 32 To...

ROBOTICS – 01PEEQW

Laboratory – Project #1

Basilio Bona

DAUIN – Politecnico di Torino

The structure to be simulated

Basilio Bona 2ROBOTICS 01PEEQW - 2014/2015

Lab Simulation Project #1: Pan-Tilt (PT) structure (2dof)

� This system is composed by two links of given length, mass and inertia,

and two revolute joints

� The axes of the two joints are as follows: joint 1: horizontal, fixed to a

vertical plane; joint 2: horizontal

� Gravity effect present

� Each joint may also be affected by elastic and/or friction effects, that are

simply modeled by a revolute elastic spring and a dashpot (viscous

friction)

� The structure is defined in the 3D space but has only 2 dof

� The number of joints is less than the number of dof, so the Jacobian

matrix is non-square and cannot be inverted (more details will follow)


Kinematic structure


0ℓ

1q 2

q

1ℓ

1ℓ1

q 2q

Lateral View

Top View

Camera

Camera

Kinematic structure


B0

B1 B2

0R

0′R

1R

1q

1q

2q

2q

Lateral View Top View

g

gravity

Possible elastic and friction effects

Reference systems

Basilio Bona ROBOTICS 01PEEQW - 2014/2015 6

Reference systems

Basilio Bona ROBOTICS 01PEEQW - 2014/2015 7

SimMechanics


What to do in Matlab

� Write a m-function to initialize the DH parameters of the PT structure

function DH_PT=DH_pan_tilt(q,nj)

� Write the direct position kinematic function as a m-function

function T = dpkf_pan_tilt(DH)

� Write the inverse position kinematic function as a m-function

function q = ipkf_pan_tilt(p,DH_PT,L0)


What to do in Matlab

� Compute the geometrical Jacobian and the direct velocity kinematic

function

JG = Jac_G_Pan_Tilt(Thom)

� Write the inverse velocity kinematic function as a m-functions

function q = ipkf_pan_tilt(p,DH_PT,L0)


What to do in Matlab or Simulink

� Test and use them to do the following …

� Imagine a beam is projected from the TCP to the horizontal plane

� Draw the trace of the beam on the plane for a given motion of the joint angles


What to do in Matlab or Simulink

� Use the inverse kinematic function …

� Imagine wishing to draw a circle on the horizontal plane … what are the

joint angles necessary to perform this motion?


A

C

What to do: Kinematic functions

� Develop the position kinematic functions (KF)

� Direct position kinematic function

� Inverse position kinematic function

� Simulate direct kinematic position function choosing fixed joint angles and obtain the cartesian pose (position + angles)

� From the obtained pose find the joint angles using inverse position KF

� This step is used as a check of your inverse kinematics algorithm


Direct kinematics – 1

Download the kinematic function library (pdf) and test it, to take confidence with Matlab functions useful for kinematic simulation

then …

Compute the DH parameters

Choose some angle values qa, then …

Compute the homogeneous T matrices for all reference frames

Compute the T matrix for the TCP

Extract the pose (p and theta) from the TCP matrix

Build the inverse kinematics function algorithm

Use the previous p and theta to compute the joint angles qb

Compare qa and qb: they must be the same


Direct kinematics – 2


T_Btcp=

[

0, cosd(q(2)), sind(q(2)), a(2)*sind(q(2))+d(1);

...

cosd(q(1)),-sind(q(1))*sind(q(2)), sind(q(1))*cosd(q(2)),a(2)*sind(q(1))*cosd(q(2));

...

sind(q(1)), cosd(q(1))*sind(q(2)),-cosd(q(1))*cosd(q(2)),L_0-a(2)*cosd(q(1))*cosd(q(2));

...

0,0,0,1

]

DH parameters

Direct and inverse kinematics – 1

Define a vector of values for each angle q(1:N)

Start a for loop

Compute matrices T(1:N) for intermediate reference frames

Compute matrices T(1:N) for TCP

Extract positions p(1:N) and angles theta(1:N) from TCP matrix

Use these p(1:N) and theta(1:N) to compute the joint angles q(1:N)

through the inverse functions

Compare the results: they must be the same


Direct and inverse kinematics – 2: cartesian trajectory

Define a cartesian path (segment, circle, etc.) and sample it according to some rule, obtaining p(1:N) and theta(1:N)

Use the inverse kinematic function (previously tested) to compute the joint angles q(1:N)

Use the direct kinematic function and the previous q(1:N) to compute p(1:N) and theta(1:N)

Compare the results: they must be the same

PROBLEMS

� how to define a cartesian path trajectory

� how to define a cartesian path velocity


Pan-Tilt Inverse kinematics

� Given the TCP position we want to compute the two joint angles q(1)

and q(2)


A

B

C

Inverse Kinematics of the TCP – 1



� What about the angles: we must choose between Euler angles and

RPY angles

� In PT case RPY angles are physically more meaningful


Procedure to find the intercept point on the z-plane – 1


A

Ap

B

C

Procedure to find the intercept point on the z-plane – 2


Matlab code


distance_z = -T_Btcp(3,4)/T_Btcp(3,3);

intercept = T_Btcp(1:3,4)+distance_z*T_Btcp(1:3,3);

distance_origin = norm(intercept);

This is pB

How to perform a task on the plane?

Now we have to compute the reverse:

� a path on the plane is given; as an example we want to draw a circle

(or other figures) on the plane

� the set S(x,y) of “target” points must be generated on the plane z=0

� the set S(x,y) shall be converted in a series of TCP poses

� we have to consider various constraints:

� as an example, if we want that the TCP is orthogonal to the plane, not all

points of the plane are “accessible”.

� if the laser power decreases with the square of the distance from the

plane, we shall also take this into account


From plane points to PT angles – 1


A

Ap

O

C

Cp

CAp

COp


Inverse Position

A Numerical Solution

Inverse position KF: a numerical solution


To obtain the inverse position kinematic function, one has to solve a system of n nonlinear equations

1( )−=q f p

that is, given p, one has to find the q that satisfy the direct equations

( )=p f q

this is equivalent to solve, given p, the system of equations

( )− =p f q 0

If one chooses a tentative q′, the equation is likely to be

( )′− = ≠p f q 0ε

Inverse position: a numerical solution


Now the problem becomes to find a qoptimal that, given p, minimizes ε

( ) min ( )optimal

= −q

f q p f q

In Matlab, the minimization algorithms are explained here

http://it.mathworks.com/help/matlab/math/optimizing-nonlinear-functions.html

ROBOTICS –01PEEQW Laboratory –Project #1 · Basilio Bona ROBOTICS 01PEEQW -2014/2015 32 To...

Documents

Transcript of ROBOTICS –01PEEQW Laboratory –Project #1 · Basilio Bona ROBOTICS 01PEEQW -2014/2015 32 To...