diff --git a/FinalProject/Final/README.md b/FinalProject/Final/README.md index 01cc2ca859748338694f962b7123339405077f8b..734b7a717196edc39760c0506eceabeb501af90f 100644 --- a/FinalProject/Final/README.md +++ b/FinalProject/Final/README.md @@ -1,54 +1,32 @@ -## Checkpoint 3 (due 3/27/18) +## Final Checkpoint (due 5/9/18) +The final checkpoint code can be seen [here](https://gitlab.engr.illinois.edu/rmaksi2/ECE470/blob/master/FinalProject/Final/final.py). + +### Goal +Our goal for the project is to be able to move a set of two blocks from separate +initial locations to final location, while avoiding objects in its path. In the +final position, the blocks will be stacked. The blocks will be initially placed +at a predetermined location, but the final location will be entered by the user. +If the position given is invalid, then the program will prompt the user for a +different goal position. The conditions that will make the goal position invalid +is if it is impossible to reach the position without collision to the robot, or +if the blocks collide with the environment. + +### Solution Approach +Since the goal of the project was to move the two blocks from different initial +positions, we had to grab one of the two blocks first. In order to do that, we +first had to compute the space screw axes, which is computed by function +computeS starting on line 159. Then, the base frame M had to be calculated, +which is computed by function computeM starting on line 135. The base frame M +is calculated using Matthew's Brett Python library in order to transform the +orientation from Euler angles. Next, we compute the set of current theta values +for all the joint in the current position. This is done in function computeThetas +that starts on line 220. We are now ready to compute the set of theta values +that will correspond to the final pose that our robot needs to achieve. The +computation is done using the findIK function on line 261. After all these steps +we are now able to pick up the first block in order to place it at the position +specified by the user. After the block is picked up, we use V-REP functions to +move the joints accordingly. These steps have to be repeated every time we want +to move the robot to a different position, either for picking up the blocks or +when the block needs to be moved to some other position. -### Inverse Kinematics of UR3 -Given user input we could create a goal pose, and then determine the joint angles that move the robot's end-effector -to the desired goal pose by using inverse kinematics. - -Firstly, make sure you have Remote API file and related python file in the same folder as Checkpoint3.py. - -Then it will asked the user to input 6 parameters, which are intended x, y, z position of goal pose and -rotation angles in degrees around x, y , z axis, which we call a,b,c. - -Secondly, we set up an dummy object which indicates the supposed end position that UR3 should move. -After enter we call the inverseKinematics Function to perform the valid joint angle calculation, the -initial guess of joint angles are randomly generalized. - -We ask for user input until 3 valid positions are given, and then the simulation will stop. - -The way we interfaced with V-Rep's API was the same as for Checkpoint 2, so if you're interested about how we did that -check that section. -### Inverse Kinematics of UR3 -To implement the inverse kinematics for the UR3, we need the intial pose M, and an array of the 6 screw matracies. -After that first thing we do in the loop is call the simxGetJointPosition() function to get the update our theta values to -the current theta values in the simulator. - -Then we go into the looping algorithm to find theta values. - -The reference we use for this algorithm is the one discussed in Lecture 15 of the class: -~~~~ -T_1in0 = getT_1in0(M, S, theta) -screwMat = logm(np.dot(T_2in0, np.linalg.inv(T_1in0))) -twist = invScrewBrac(screwMat) - -counter = 0 -final_pose_flag = 1 - -while (np.linalg.norm(twist) > 0.01): - - if(counter >= 100): - print("Desired Pose not reachable") - final_pose_flag = 0 - break - - J = getSpacialJacobian(S, theta) - thetadot = np.dot(np.linalg.inv(np.dot(np.transpose(J), J) + 0.1*np.identity(6)), - np.dot(np.transpose(J), twist)) - np.dot(np.identity(6) - np.dot(np.linalg.pinv(J), J), theta) - theta = theta + (thetadot * k) - - T_1in0 = getT_1in0(M, S, theta) - screwMat = logm(np.dot(T_2in0, np.linalg.inv(T_1in0))) - twist = invScrewBrac(screwMat) - - counter = counter + 1 -~~~~