Motion Planning for Simulated Robotic Manipulators

Forward kinematics (FK) forms the core of our ability to purposefully control the motion of a robot arm. FK provides us a general formulation for controlling any robot arm to reach a desired configuration and execute a desired trajectory. Specifically, FK allows us to predict the spatial layout of the robot in our 3D world given a configuration of its joints. I implemented core matrix routines for FK transforms with consideration for angular rotation about each joint axis using quaternions. To demo my code, I programmed a dance routine for a spider robot. See if you can guess it :)

Inverse kinematics (IK) allows us to determine the configuration of our robot, specifically the angle of rotation about each joint, needed for the end effector to reach a desired goal pose. I implemented IK through gradient descent optimization, using both the Jacobian Transpose and Jacobian Pseudoinverse methods.

For high-dimensional search problems, an exhaustive overview of the majority of the space is not an option. Instead, we now look to sampling-based search algorithms, which will introduce randomness to our search process. These sampling-based algorithms trade off the guarantees and optimality of exhaustive graph search for viably tractable planning in complex environments. For my purpose, I applied the RRT-Connect algorithm as shown below.