Commit 5c4bb928 authored by siyunhe2's avatar siyunhe2
Browse files

Inverse kinematics done, Path planning no-------siyunhe

parent 1362287e
......@@ -301,22 +301,7 @@ print("#########################################################################
# Check for collisions along a straight line, a helper function for pathPlaning
def checkStraightLine(clientID, theta_a, theta_b, arm_centers, body_centers, rack_centers, SLeft, SRight, arm):
dtheta = 0.01
s = 0
while s <= 1:
theta = (1-s)*theta_a + s*theta_b
if arm == "left":
theta = np.block([theta,0,0,0,0,0,0,0])
new_arm_centers = updateCenters(clientID, arm_centers, SLeft, SRight, theta)
else:
theta = np.block([theta[0],0,0,0,0,0,0,0,theta[1:]])
new_arm_centers = updateCenters(clientID, arm_centers, SLeft, SRight, theta)
collision = checkCollision(new_arm_centers, body_centers, rack_centers)
if collision:
return True
s = s + dtheta
return False
#First move the arm by using inverse kinematics
......
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