Control Algorithm for the CRS Robot
Final Contest Details
As already said, the final task consisted of Four tasks below:
-
Insert the peg in a hole with tight clearances.
-
Avoid obstacle on the way to reaching a zigzag groove.
-
Make the peg follow the zigzag groove.
-
Make the egg push down an egg placed on a spring without breaking it.
These four tasks involve force and position control and path planning.
For the first task, the gains were weakened in the x and y direction since the hole had tight tolerances. Movement in the z direction was kept stiff since that was the direction of movement in and out of the hole.
For the second task, the gains were
The third task involved weakening different axis depending on direction of movement. The zigzag groove had tight tolerances as well. The figure below
shows the geometry of the zigzag groove. Since the anlges were known, using impedance control new axis were created in that orientation. The axis to weaken, which was an axis in a non world frame, was the axis perpendicular to this new non world frame axis.
​
​
Note: Figure taken from course website.
After exiting the zigzag groove, the robot end effector had to reach the egg, press it down for 1 second without breaking it, and then return to the home position
​
Lab details for our 4 labs and the associated lab reports
​
Lab 1:
Forward and Inverse Kinematics equations following Denavit-Hartenberg convention are derived for the CRS robot arm and implemented on the Code Composer. Transformation matrix was also calculated using Robotica package in Mathematica to provide theoretical support for the Forward Kinematics equations. The validity of the equations were testified by executing the C code generated to move the robot arm to different locations and measure the actual physical parameters.
​
Lab 2:
In this lab, PD and PID joint control is implemented. The equations of motion are derived for a two link revolute linkage using Lagrangian dynamics and a Simulink model is created to simulate this linkage. Velocity and integral calculations are implemented to design first a PD control and then a PID controller. To improve system response, feedforward control is added. Towards the end, a fun trajectory is created for the CRS robot arm to trace.
​
Lab 3:
In this report, friction compensation is added to all three joints of the CRS robot arm and an inverse dynamics algorithm is implemented. The performance of the inverse dynamics control is compared to that of the PD plus feedforward control, with and without mass.
​
​
​
Lab 4:
In this lab exercise, task space PD control and Impedance control is implemented. Rotation matrix coordinate transformation are used to select a non-world frame axis as an axis whose gains we weakened to achieve impedance control. The code for this lab has been attached here
​
​
​
​
Final Project:
The knowledge and experience from Lab 1 through 4 helped us successfully complete the final task. Learning and implementing PD control helped us learn how to tweak the gains to achieve the desired results. Depending on the task and environment, the stiffness of the robot arm has to be adjusted. Learning about task space control and straight line trajectory allowed us to get the peg in the hole and reach the maze after crossing the obstacle. Learning how to create a non world axis helped us correctly implement impedance control to cross the zigzag. Finally, we were able to press down on the egg without breaking it.
The source code that we used for our final task is in the file below
​
​
​
