Commit dc65fdb7 authored by rmaksi2's avatar rmaksi2
Browse files


parent 5176790f
## Checkpoint 3 (due 3/27/18)
## Final Checkpoint (due 5/9/18)
The final checkpoint code can be seen [here](
### 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
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.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
J = getSpacialJacobian(S, theta)
thetadot =, J) + 0.1*np.identity(6)),, twist)) - -, J), theta)
theta = theta + (thetadot * k)
T_1in0 = getT_1in0(M, S, theta)
screwMat = logm(, np.linalg.inv(T_1in0)))
twist = invScrewBrac(screwMat)
counter = counter + 1
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment