Lecture «Robot Dynamics»: Kinematics 2

Transcription

1 Lecture «Robot Dynamics»: Kinematics V lecture: CAB G11 Tuesday 10:15 12:00, every week exercise: HG E1.2 Wednesday 8:15 10:00, according to schedule (about every 2nd week) Marco Hutter, Roland Siegwart, and Thomas Stastny Robot Dynamics - Kinematics

2 Intro and Outline Course Introduction; Recapitulation Position, Linear Velocity Kinematics 1 Rotation and Angular Velocity; Rigid Body Formulation, Transformation Exercise 1a Kinematics Modeling the ABB arm Kinematics 2 Kinematics of Systems of Bodies; Jacobians Exercise 1b Differential Kinematics of the ABB arm Kinematics 3 Kinematic Control Methods: Inverse Differential Kinematics, Inverse Kinematics; Rotation Error; Multi-task Control Exercise 1c Kinematic Control of the ABB Arm Dynamics L1 Multi-body Dynamics Exercise 2a Dynamic Modeling of the ABB Arm Dynamics L2 Floating Base Dynamics Dynamics L3 Dynamic Model Based Control Methods Exercise 2b Dynamic Control Methods Applied to the ABB arm Legged Robot Dynamic Modeling of Legged Robots & Control Exercise 3 Legged robot Case Studies 1 Legged Robotics Case Study Rotorcraft Dynamic Modeling of Rotorcraft & Control Exercise 4 Modeling and Control of Multicopter Case Studies 2 Rotor Craft Case Study Fixed-wing Dynamic Modeling of Fixed-wing & Control Exercise 5 Fixed-wing Control and Simulation Case Studies 3 Fixed-wing Case Study (Solar-powered UAVs - AtlantikSolar, Vertical Take-off and Landing UAVs Wingtra) Summery and Outlook Summery; Wrap-up; Exam Robot Dynamics - Kinematics

3 Last Time: Position Parameterization Position vector: Parameterization: r r χ e χ P e 3 3 Cartesian Cylindrical coordinates Spherical coordinates Relation between linear velocity and parameter differentiation 33 with the parameterization specific matrix EPχ P r r χ E χ e e P P P χ P Robot Dynamics - Kinematics

4 Rotation Parameterization Rotation matrix: 3x3 = 9 parameters Orthonormality = 6 constraints Euler Angles 3 parameters, singularity problem Angle Axis 4 parameters, unitary constraint, singularity problem Rotation vector 3 parameters, singularity problem Quaternions 4 parameters no singularity χ φ n R, rotvec Robot Dynamics - Kinematics

5 Euler Angles Consecutive elementary rotations Three elementary rotations ZYZ and ZXZ: proper Euler angles ZYX: Tait-Bryan angles XYZ: Cardan angles Example Z-Y-Z C C χr eulerzyz C z C y C z AD AD, AB 1 BC CD 2 Robot Dynamics - Kinematics

6 From Euler Angles to Rotation Matrix ZYZ example C C χr eulerzyz C z C y C z AD AD, AB 1 BC CD 2 Robot Dynamics - Kinematics

7 From Rotation Matrix to Euler Angles ZYZ example A rotation matrix has the following form As a function of ZYZ Euler Angles, we found Atan2 function: uses sign of both arguments to determine the correct quadrant Robot Dynamics - Kinematics

8 Euler Angles Rotation Matrix ZYX example Rotation parameters Rotation matrix from Euler Angles Euler Angles from Rotation matrix Robot Dynamics - Kinematics

9 Angle Axis and Rotation Vector Angle axis parameterize the rotation by: Rotation angle Rotation axis Rotation vector (aka Euler vectors) Rotation matrix is given by:, cos sin 1 cos C n I n nn AB 33 T Parameters from rotation matrix Robot Dynamics - Kinematics

10 Unit Quaternions Rotation parameterization w/o singularity problem Complex numbers in 4D ξ 0 1i 2 j 3k Hamiltonian convention i j k ijk 1 As vector Real part Imaginary part Unitary constraint Inverse Identity ξ T Robot Dynamics - Kinematics 2 11

11 Unit Quaternions Rotation matrix Rotation matrix from unit quaternion Unit quaternions from rotation matrix Robot Dynamics - Kinematics

12 Quiz Rotation matrix C AB / 2 3 / / 2 1/ 2 60 EulerZYX Angle Axis atan 2( 3 / 2,1/ 2) 60 11/ 2 1/ / 2 ( 3 / 2) sin cos cos 1/ 2 60 Quaternions ξ AB 11/ 2 1/ / 2 1/ / 2 1/ / 2 1 1/ ζ 0 = cos ζ = sin θ 2 θ 2 Robot Dynamics - Kinematics n

13 Unit Quaternions Algebra Product of quaternions Given two quaternions q and p, the product is defined as q p = q 0 + q 1 i + q 2 j + q 3 k p 0 + p 1 i + p 2 j + p 3 k q p = q 0 p 0 + q 0 p 1 i + q 0 p 2 j + q 0 p 3 k q p = +q 1 p 0 i + q 1 p 1 ii + q 1 p 2 ij + q 1 p 3 ik q p = +q 2 p 0 j + q 2 p 1 ji + q 2 p 2 jj + q 2 p 3 jk q p = +q 3 p 0 k + q 3 p 1 ki + q 3 p 2 kj + q 3 p 3 kk q p = q 0 p 0 q 1 p 1 q 2 p 2 q 3 p 3 q p = +(q 0 p 1 + q 1 p 0 + q 2 p 3 q 3 p 2 )i q p = +(q 0 p 2 q 1 p 3 + q 2 p 0 + q 3 p 1 )j q p = +(q 0 p 3 + q 1 p 2 q 2 p 1 + q 3 p 0 )k q 0 q 1 q 2 q 3 p 0 q q p = 1 q 0 q 3 q 2 p 1 q q 2 q 3 q 0 q 1 p = 0 q T 2 q q 0 I + q q 3 q 2 q 1 q 0 p 3 =:M l q p 0 p 1 p 2 p 3 q 0 p q p = 1 p 0 p 3 p 2 q 1 p p 2 p 3 p 0 p 1 q = 0 p T 2 p p 0 I p p 3 p 2 p 1 p 0 q 3 ξac ξab ξbc C C C =:M r p p = M l q p q = M r p q AC AB BC Hamiltonian convention ξ i j k i j k ijk 1 2 ij ji ijk k jk kj i ki ik j Robot Dynamics - Kinematics

14 Unit Quaternions Rotating a vector The pure (imaginary) quaternion of a coordinate vector I r is given by p I r = 0 Ir Given the unit quaternion ζ BI : T p B r = ζ BI p I r ζ BI r C r B BI I Proof (see Quaternion Kinematics by Joan Solà on lecture homepage) Decompose vector in parallel and orthogonal part to get vector rotation formula Show that equation above does exactly the same Robot Dynamics - Kinematics

15 Unit Quaternion Derivation of rotation matrix Derivation of rotation matrix (ζ = ζ BI ): p B r = ζ p I r ζ T = M l ζ M r (ζ T 0 ) Ir 0 Br = ζ 0 ζ T ζ 0 ζ T 0 ζ ζ 0 I + ζ ζ ζ 0 I + ζ Ir M r ζ = ζ 0 ζ T ζ ζ 0 I ζ ζ T = ζ ζ 1 = ζ T = ζ 0 ζ 0 Br = ζ ζ 2 ζ 0 ζ T ζ 0 ζ T ζ T ζ 0 ζ 0 ζ ζ 0 ζ ζ ζ ζ ζ T + ζ 2 0 I + 2ζ 0 ζ + ζ ζ Ir 0 Br = ζ 2 0 ζ 2 I + 2ζ 0 ζ + 2 ζ ζ T Ir B r = ζ 0 2 ζ 2 I + 2ζ 0 ζ + 2 ζ ζ T Ir ζ ζ = ζ ζ T - ζ 2 I C ζ = 2ζ I + 2ζ 0 ζ + 2 ζ ζ T Robot Dynamics - Kinematics 2 16

16 Quiz 2 Given a vector in A frame A r Rotate this to B frame using quaternions ξ AB ξ 2 0 BA p 0 0 T T r ξ p r ξ M l ξ M r ξ B BA A BA BA BA B r A r / / 2 Direct calculation using complex numbers T i j 3 i 3 j 3 3 ji ij 3 iji 3 j 3 ji 3 ji iij 3 j 3 ji 3 ji iij 3 j 3k 3k j 2 j 2 3k ξba p A r ξba Robot Dynamics - Kinematics

17 Unit Quaternion Link to angle axis Given the rotation matrix C ζ = 2ζ I + 2ζ 0 ζ + 2 ζ ζ T Use this with the angle axis representation ζ 0 = cos θ 2 ζ = sin θ 2 n C ζ θ, n = 2 cos 2 θ 1 I + 2 cos θ sin θ n sin 2 θ 2 nnt = cos θ I + sin θ n + (1 cos θ )nn T Robot Dynamics - Kinematics

18 Quiz 3 z 60 y 60 Rotation matrix C AC? 1/ 2 3 / 2 0 1/ / 2 C AB 3 / 2 1/ 2 0 C BC / 2 0 1/ 2 Quaternion ξ AB ξ BC 2 1 ξ AC ξab ξbc C AC Robot Dynamics - Kinematics

19 Time Derivatives and Rotational Velocity What is the relation ω AD χ AD Analog to linear velocity: Find, s.t. Robot Dynamics - Kinematics

20 Time Derivatives and Rotational Velocity ZYX example A ωad A ωab A ωbc A ωcd C AB A ωab CAB B ωbc CAB CBC C ωcd e A z z C e B y y C C e C A AB B AB BC C x x z A B C z y x y A e CAB B e CAB CBC C e x c 0 0 z sz sz e B y sz cz 0 1 c B z cz sz 0 cy 0 sy 1 cycz C C e C x sz cz AB BC C cys z sy 0 c y 0 s y 0 sz cycz ω 0 cz cysz χ det ER, eulerzyx cosy 1 0 s y Robot Dynamics - Kinematics

21 Derivative Angle Axis, Rotation Vector, Quaternions Angular Velocity Angle Axis Rotation Vector Quaternion with Robot Dynamics - Kinematics

22 Position and Orientation of a Single Body Position vector: Cartesian r r χ e e 3 Rotation Cylindrical coordinates Spherical coordinates Rotation matrix: Euler Angles χ 3 e e R SO Quaternions Robot Dynamics - Kinematics

23 Mini-Introduction to Multi-body Kinematics V lecture: CAB G11 Tuesday 10:15 12:00, every week exercise: HG E1.2 Wednesday 8:15 10:00, according to schedule (about every 2nd week) Marco Hutter, Roland Siegwart, and Thomas Stastny Robot Dynamics - Kinematics

24 Classical Serial Kinematic Linkages Generalized robot arm n j joints revolute (1DOF) prismatic (1DOF) n l = n j + 1 links n j moving links 1 fixed link Robot Dynamics - Kinematics

25 Configuration Parameters Generalized coordinates 5 constraints 6 parameters 3 positions 3 orientations n moving links: 6n parameters n 1DoF joints: 5n parameters 6n 5n = n DoFs Generalized coordinates A set of scalar parameters q that describe the robot s configuration Must be complete (Must be independent) => minimal coordinates Is not unique Degrees of Freedom Nr of minimal coordinates Robot Dynamics - Kinematics

26 Forward Kinematics End-effector configuration as a function of generalized coordinates For multi-body system, use transformation matrices n e Note: depending on the selected end-effector parameterization, it is not possible to analytically write down end-effector parameters! Robot Dynamics - Kinematics

27 Forward Kinematics Simple example What is the end-effector configuration as a function of generalized coordinates? T T T T T T IE I E c1 0 s1 0 c2 0 s2 0 c3 0 s s1 0 c1 l0 s2 0 c2 l1 s3 0 c3 l l c123 0 s123 l1s1 l2s12 l3s s123 0 c123 l0 l1c1 l2c12 l3c Robot Dynamics - Kinematics