ROBOTICS 01PEEQW Laboratory Project #1. Basilio Bona DAUIN Politecnico di Torino

Transcription

1 ROBOTICS 01PEEQW Laboratory Project #1 Basilio Bona DAUIN Politecnico di Torino

2 The structure to be simulated 2

3 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) 3

4 Kinematic structure q q2 1 l 1 Camera l 0 Lateral View q q 2 1 l 1 Camera Top View 4

5 Kinematic structure Lateral View Top View B1 R 1 B2 R 0 q q 1 2 q 1 q 2 R 0 B0 g gravity Possible elastic and friction effects 5

6 Reference systems 6

7 Reference systems 7

8 SimMechanics 8

9 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) 9

10 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) 10

11 What to do in Matlabor 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 11

12 What to do in Matlabor 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? C A 12

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

14 Direct kinematics 1 Download the kinematic function library(pdf) and test it, to take confidence with Matlab functions useful for kinematic simulation Compute the DH parameters then 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 qaand qb: they must be the same 14

15 15

16 Direct kinematics 2 T_Btcp= [ 0, cosd(q(2)), sind(q(2)), a(2)*sind(q(2))+d(1);... DH parameters 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 ] 16

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

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

19 Pan-Tilt Inverse kinematics Given the TCP position we want to compute the two joint angles q(1) and q(2) C A B 19

20 Inverse Kinematics of the TCP 1 20

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

22 Inverse Kinematics of the TCP 3 22

23 Inverse Kinematics of the TCP 4 23

24 Procedure to find the intercept point on the z-plane 1 C A p A B 24

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

26 Matlab code This is p B 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); 26

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

28 From plane points to PT angles 1 C p CA p C A p A p CO O 28

29 From plane points to PT angles 2 29

30 From plane points to PT angles 1 30

31 Inverse Position A Numerical Solution 31

32 Inverse position KF: a numerical solution To obtain the inverse position kinematic function, one has to solve a system of n nonlinear equations q= f 1 ( p) that is, given p, one has to find the q that satisfy the direct equations p= fq ( ) this is equivalent to solve, given p, the system of equations p fq ( ) = 0 If one chooses a tentative q, the equation is likely to be p fq ( ) = ε 0 32

33 Inverse position: a numerical solution Now the problem becomes to find a q optimal that, given p, minimizes ε fq ( ) min p fq ( ) optimal = q In Matlab, the minimization algorithms are explained here 33