Chapter 4 . Trajectory planning and Inverse kinematics Trajectory Planning & Inverse Kinematics

Table of Contents Introduction Trajectory planning Inverse kinematics Velocity kinematics

Introduction For a given task to the robot,  How to change the end-effector frame? Trajectory planning: Method of moving the frames Inverse kinematics: For the desired location of the frame, obtain the joint variables.

Introduction Inverse Kinematics [Fig. 4-1] Work space of the robot arm Obstacle Via point [Fig. 4-1] Work space of the robot arm Inverse Kinematics

4.1 Trajectory planning Through Point : The end-effector should pass through this point. Via Point : The end-effector may move around this point. For more efficient task executions, => Path + Trajectory planning

Difference between Manipulator and Mobile Robot M  End-effector M. R  Whole body is moving M  Configuration changes M.R  Location changes with solid body M  6-DOF M.R  3-DOF on the plane M  In the fixed environment M.R  In the dynamically changing environment

Cont. M  Object recognition M.R  Environment recognition M  The size is very critical M  Trajectory planning is deterministic M.R  Location is a big issue because of dead reckoning errors

Trajectory planning of manipulator Computation of trajectory for a given task  To move EE to the desired position/orientation from the current location  Represent as homogeneous transformation matrix Trajectory: Set of position/orientation of EE as a function of time  Position, velocity, and acceleration trajectories (Ex: painting, welding)

Trajectory planning in Cartesian space Trajectory in Cartesian or joint space should be determined Considerations:

Method 1. Change of Cartesian to joint coordinates in every sampling  Comparison of the desired and actual paths Forward kinematics  Error in Joint space Inverse kinematics Shortage: Computational complexity limits sampling cycle.

Method 2. Joint space trajectory  Considering the Max. velocity of each joint  Using forward kinematics, the Cartesian space error can be checked.

Method 3. Using polynomial equations for the joint space trajectory  Manipulator may generate the smooth motion.

Position control + Real time obstacle avoidance Path control mode Obstacle Yes No Path Constraints off-line collision-free path planning plus on-line path tracking Position control + Real time obstacle avoidance Position control

Trajectory planning Moving EE from current to the desired position: Path Planning Given path Trajectory Planning Dynamic constrains of the manipulator [Fig. 4-2] Trajectory planning

Joint space 1. 3rd order polynomial  Within a given time, arriving to the desired position from the current position. Four constraints: position & velocity of current & desired To keep the 3rd order polynomial, (4.1) (4.2)

Via points 2. 3rd order polynomial with via points  Non stop at the via points. Therefore using the constraints, (4.3) Specification of velocities ① User ② Heuristic rules ③ Continuous acceleration at the via points

Via points 3. High order polynomial  Position, velocity, and acceleration at the start and end points: 4. Polynomial & linear function  Polynomial for the start and end with constant accelerations to make smooth change of the velocity 5. Polynomial & linear function with via point  Virtual via points can be used (4.4)

Cartesian coordinates method Transformation from the Cartesian coordinates to joint coordinates may cause singularity problem. Considering some examples, singularity can be understood.

Ex. 1. (Out of workspace) ① EE cannot reach some portion of the path [Fig. 4-3] Example 1

Ex. 2. Singular configuration ② Very high joint rates near at the singular [Fig. 4-4] Example 2

Ex. 3. Multiple solutions ③ Multiple solutions [Fig. 4-5] Example 3

4.2 Path generation 1. In the middle of tasks, - By using the path generated during the task execution, the trajectory, , is generated and used for the control system. - The path in the Cartesian space is transformed to the corresponding joint values. Position  Inverse kinematics, Velocity  Inverse Jacobian, Acceleration  Inverse Jacobian and the derivative

Path generation 2. Using the dynamic model - Minimum time trajectory can be obtained using dynamics and velocity-torque curve of the actuators. 3. Using the programming language - AL: Robot programming language developed at Stanford. ex) Move ARM to C with duration = 3 * seconds; Move ARM to C; Move ARM to C via B;

[Fig. 4-6] Moving trajectory of the robot arm 4.3 Trajectory planning Moving a block on the table to the other position [Fig. 4-6] Moving trajectory of the robot arm

Trajectory planning of robot Task is matching 1 step: to with grasping. 2 step: to Motion of robot - Timely change of HT matrix - Timely change of path

[Fig. 4-7] Change in Cartesian space an joint space Ex 4. Specifying joint position, velocity and acceleration in time domain For an example, [Fig. 4-7] Change in Cartesian space an joint space

Constraints While moving from A to B, a certain point should be passed through (through point) When and are specified, , are also specified However, are limited by the torques

Minimum time trajectory planning [Fig. 4-8] Acceleration trajectory, velocity and position

[Fig. 4-9] Path planning to avoid the obstacles Mobile robot The robot is considered as a Point Object. The obstacles 01,02,03 should be avoided (4.5) [Fig. 4-9] Path planning to avoid the obstacles

[Fig. 4-10] Path planning to avoid the obstacles Real mobile robot Expand the obstacles considering the size of the mobile robot  Obstacle avoidance & minimum time trajectory planning [Fig. 4-10] Path planning to avoid the obstacles

A* algorithm

O:Open List, C: Closed List

Optimal path

4.4 Inverse kinematics The position/orientation of end-effector is given w.r.t. the reference frame.  Obtain the joint angles for the desired location Sequence of inverse kinematics 1. Transform the EE coordinates to Base coordinates 2. Solve the mathematics EE position in Cartesian space  joint space values

Cautions If there are more than 6 joints, multiple solutions exist. If less than 6 joints are there, then there might be no solution of where n < 6 corresponding to . Among the nine variables in the rotation matrix, only three of them are independent. (4.6)

Solving the inverse kinematics For the given homogeneous transformation matrix, obtaining is the inverse kinematics. For the Stanford manipulator, (4.7)

Inverse kinematics (4.8) From the above 12 equations, the angles can be obtained.

Closed form solution Closed form solution is obtained as an equation as follow: Advantages of closed form solution: 1. The solution can be obtained quickly 2. Existence of multiple solutions can be identified (4.9)

Resolving multiple solution Check the solution whether those are feasible or not --- Constraints also can be checked The optimal solution can be selected and used

Inverse kinematics (special case) Using the geometrical features of the robot, it can be solved easily The special case is introduced Let’s consider PUMA robot

Separation of upper and lower bodies. Idea [Fig. 4-11] Structure of PUMA Robot

Vector analysis d vector: From the origin of frame {4} to frame {6} Then, Length of d is constant and the direction is Therefore, (4.10)

Obtaining (4.11) is given as is ftn of Even though 4th joint rotates, does not changes (4.11) (4.12)

Obtaining By the three equations, are obtained Three unknowns can be obtained by this equation (4.13)

Velocity inverse kinematics Cartesian velocity, and joint space velocity, are related by a Jacobian matrix as When m=n, J -1can be multiplied at both sides to obtain the joint space velocity as Three cases can be considered: (4.14) (4.15)

Three cases Case 1 ) and , singular case, arbitrary motion is not possible Case 2 ) , minimum error solution The solution does not exist generally Case 3 ) , redundancy resolution Redundant Manipulator Among the multiple solutions, an optimal solution needs to be selected .

Minimum error solution (4.16)

Redundancy resolution (4.17) Z could be an arbitrary vector in the joint space

[Fig. 4-12] Four solution of PUMA 560 FOUR SOLUTIONS OF PUMA 560 [Fig. 4-12] Four solution of PUMA 560

[Fig. 4-13] Inverse kinematics of 3 link planar manipulator Example 4.4 Inverse kinematics of 3 link planar manipulator [Fig. 4-13] Inverse kinematics of 3 link planar manipulator

[Table 4-1] Link parameter of 3 link planar manipulator Link Parameter Table Link 1 2 3 [Table 4-1] Link parameter of 3 link planar manipulator

Homogeneous T.M It is simplified as (4.18) (4.19)

Obtaining where (4.20) Using , (4.21) . So can be obtained

Obtaining are represented as follows: (4.22) Where, and are known Therefore can be obtained (4.22)

Obtaining , using the and with the (given), can be obtained

Homework