robotics –01peeqw laboratory –project #1 · pdf fileimagine you want to draw a...
TRANSCRIPT
ROBOTICS – 01PEEQW
Laboratory – Project #1
Basilio Bona
DAUIN – Politecnico di Torino
The structure to be simulated
Basilio Bona 2ROBOTICS 01PEEQW - 2015/2016
This structure simulates a pan-tilt camera,
pointing down to a plane.
It is also possible to imagine a laser instead of a
camera, with its ray tracing a path on the
horizontal plane
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 are 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)
Basilio Bona 3ROBOTICS 01PEEQW - 2015/2016
Kinematic structure
Basilio Bona 4ROBOTICS 01PEEQW - 2015/2016
0ℓ
1q 2
q
1ℓ
1ℓ1
q 2q
Lateral View
Top View
Camera
Camera
Kinematic structure
Basilio Bona 5ROBOTICS 01PEEQW - 2015/2016
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 - 2015/2016 6
Reference systems
Basilio Bona ROBOTICS 01PEEQW - 2015/2016 7
SimMechanics
Basilio Bona 8ROBOTICS 01PEEQW - 2015/2016
What to do in Matlab
� Write a Matlab function to initialize the DH parameters of the PT
structure: for example
function DH_PT=DH_pan_tilt(…)
� Write a Matlab function that computes the direct position kinematic
function: for example
function T = dpkf_pan_tilt(DH_PT,…)
� Write a Matlab function that computes the inverse position kinematic
function: for example
function q = ipkf_pan_tilt(pgiven,DH_PT,…)
� The Matlab functions may have as arguments what you decide to include
� Make the functions as more general as possible
Basilio Bona 9ROBOTICS 01PEEQW - 2015/2016
What to do in Matlab
� Write a Matlab function to compute the geometrical Jacobian
JG = Jacg_Pan_Tilt(…)
� Write a Matlab function to compute the direct velocity kinematic function
pdot= dvkf_pan_tilt (…)
� Write a Matlab function to compute the inverse velocity kinematic
function
function q = ipkf_pan_tilt(…)
Basilio Bona 10ROBOTICS 01PEEQW - 2015/2016
What to do in Matlab or Simulink
� Test and use them to do this …
� Imagine a laser beam is projected from the TCP to the horizontal plane (z=0)
� Draw the trace of the beam on the plane for a given motion of the joint angles
Basilio Bona 11ROBOTICS 01PEEQW - 2015/2016
What to do in Matlab or Simulink
� Use the inverse kinematic function to do this …
� Imagine you want to draw a circle (or a segment, or whatever) on the horizontal
plane
� what are the joint angles necessary to perform this motion?
Basilio Bona 12ROBOTICS 01PEEQW - 2015/2016
A
C
What to do – Summary
� 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 to check the correctness of your inverse kinematics algorithm
Basilio Bona 13ROBOTICS 01PEEQW - 2015/2016
Direct kinematics – 1
Download my 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!
Basilio Bona 14ROBOTICS 01PEEQW - 2015/2016
Basilio Bona 15ROBOTICS 01PEEQW - 2015/2016
Direct kinematics – 2
Basilio Bona 16ROBOTICS 01PEEQW - 2015/2016
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
Basilio Bona 17ROBOTICS 01PEEQW - 2015/2016
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
Basilio Bona 18ROBOTICS 01PEEQW - 2015/2016
Pan-Tilt Inverse kinematics
� Given the TCP position we want to compute the two joint angles q(1)
and q(2)
Basilio Bona 19ROBOTICS 01PEEQW - 2015/2016
A
B
C
Inverse Kinematics of the TCP – 1
Basilio Bona 20ROBOTICS 01PEEQW - 2015/2016
Inverse Kinematics of the TCP – 2
� What about the angles: we must choose between Euler angles and
RPY angles
� In PT case RPY angles are physically more meaningful
Basilio Bona 21ROBOTICS 01PEEQW - 2015/2016
Basilio Bona 22ROBOTICS 01PEEQW - 2015/2016
Inverse Kinematics of the TCP – 3
Basilio Bona 23ROBOTICS 01PEEQW - 2015/2016
Inverse Kinematics of the TCP – 4
Procedure to find the intercept point on the z-plane – 1
Basilio Bona 24ROBOTICS 01PEEQW - 2015/2016
A
Ap
B
C
Procedure to find the intercept point on the z-plane – 2
Basilio Bona 25ROBOTICS 01PEEQW - 2015/2016
Matlab code
Basilio Bona 26ROBOTICS 01PEEQW - 2015/2016
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
Basilio Bona 27ROBOTICS 01PEEQW - 2015/2016
From plane points to PT angles – 1
Basilio Bona 28ROBOTICS 01PEEQW - 2015/2016
A
Ap
O
C
Cp
CAp
COp
From plane points to PT angles – 2
Basilio Bona 29ROBOTICS 01PEEQW - 2015/2016
From plane points to PT angles – 1
Basilio Bona 30ROBOTICS 01PEEQW - 2015/2016
Basilio Bona 31ROBOTICS 01PEEQW - 2015/2016
Inverse Position
A Numerical Solution
Inverse position KF: a numerical solution
Basilio Bona 32ROBOTICS 01PEEQW - 2015/2016
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
Basilio Bona 33ROBOTICS 01PEEQW - 2015/2016
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