Commit 7ce946e7 authored by siyunhe2's avatar siyunhe2
Browse files


parent 460f4407
## Checkpoint 4 (due 4/2/18)
### Demonstrate Collision Detection for UR3
First, make sure you have Remote API file and related Python files in the same folder as
You should also load the appropriate scene in the V-REP. The code does not ask for any user input.
We predefined some of the theta values, which can be looked up in the code. Based on these values,
the collision checker is ran. You can checkout the code in Collision Detection section.
## Checkpoint 5 (due 4/9/18)
### Demonstrate Motion/Path Planing for UR3
In this checkpoint, the user would be to asked to input desired position of the UR3.
Before the robot moves, the collision checker checks if the collision will occur and will print the
appropriate comment to the screen. Then it will move the joints. During the first "iteration",
V-REP is not running the simulation, therefore, the joints are allowed to move throught the obstacles.
During the second "iteration", V-REP is in simulation mode, therefore the joints will not be
able to move throught the obstacles no more. For both iterations, same joint configuration was used.
First, make sure you have Remote API file and related Python files in the same folder as
You should also load the appropriate scene in the V-REP.
In the Scene, there are two blocks which we recognize it as obstacles. In order to detect the obstacles, we implemented
boudning sphere on the two blocks.
So the goal is to to reach to the desired pose as given the user input.
### Collision Detection
The implementation of collision detection:
def collisionChecker(S, initial_points, points, r, theta):
for i in range(len(theta)):
p_initial = np.array([
### Sampling based planner Algorithm
temp = np.identity(4)
1)The path planing is implemented by using sampling based planner Algorithm. We set a tree strcuture to store all the intermediate path
to connect betwwen the starting position and goal position. The tree structure has two trees: T(forward) and T(bacward), which served as
"bisection method"
for j in range(i+1):
temp =, expm(screwBracForm(S[j]) * theta[j]))
First, we check the whether the UR3 robot would encounter with self-Collision or obstacles-collision.
p_final =, p_initial)
2)Then we sample random thetas from random uniform distribution
3)In order to genearte the one goal pose, we check whether the UR3 robot would encounter with
self-Collision(its self joints) or obstacles-collision(bounding spheres on the two blocks).
p = np.block([
4)if no collision, add the current position/path to T(forward), and iteratetively found new thetas
in T(forward), while set the new thetas as the child of previous theta
for i in range(len(theta) + 2):
point1 = np.array([
5)And do the same for T(backward)
for j in range(len(theta) + 2 - i):
if(i == i+j):
6)If theta belongs to both T(forward) and T(backward), then we have a complete connected path from starting position to desired goal pose.
point2 = np.array([
if(norm(point1 - point2) <= r*2):
return 1
return 0
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