Franka Robot
Forward Kinematics & Inverse Kinematics
Forward Kinematics calculates the joint locations in cartesian space given the joint positions. The transformations can be obtained from either DH parameters or parsing the URDF of the robot. Inverse kinematics via gradient descent estimates the joint positions given the desired end-effector position and orientation. The Jacobian of the end-effector is calculated analytically using forward kinematics.
Collision Checking
Collision checking is performed using the Separating Axis Theorem. This video shows that the algorithm is able to detect collision between the robot arm and an object. The cylinder on the ground is green when there is no collision, and turns red when the object collides with the robot arm.
Path Planning with RRT
Given a start and a goal end-effector position, the path can be planned using RRT (Rapidly-Exploring Random Tree). The tree is extended by sampling valid joint configurations within the Franka joint limits. Goal bias is also implemented by assigning some probability for using the goal instead of the sampled node as target. A node will not be added if there is a collision. Hyperparameters include step size, probability for goal bias, and maximum number of nodes. In this video, the end effector starts from one side of the wall and plans a path over the wall to reach the other side.
Constrained Planning
RRT without constraint allows the end-effector to have rotations. Since many applications require objects on the gripper to remain upright, it is useful to keep the end-effector in the vertical orientation. Constraint projection can be performed using gradient descent to project a given joint configuration towards the constraint manifold. Two hyperparameters, learning rate and constraint threshold, are added. In this video, the roll and pitch are constrained and the end effector remains upright throughout the path.
Jenga Resetting
This project explores an automatic resetting procedure for Jenga in simulation using the RLBench environment. Robot resetting would allow human players to enjoy the game without having to perform the tedious task of rebuilding the tower. Furthermore, the process of training robots to perform forward pass can be sped up and done automatically when utilizing the resetting approach.
The task is split into three sections - the forward pass to play Jenga, the clearing procedure to knock down the tower and clear the table, and the resetting process to rebuild the tower. Reinforcement learning is only applied during the resetting phase. The Franka robot starts the forward pass and tries to remove blocks and place them on the top for as long as possible. As soon as the tower falls or the robot is unable to find a path, the robot knocks down the entire tower, enters the resetting procedure, and starts the reinforcement learning process.