save

FinalProject/Final/final.py

@@ -31,12 +31,26 @@ if result != vrep.simx_return_ok:
 result, Cuboid_position = vrep.simxGetObjectPosition(clientID, Cuboid_handle, -1, vrep.simx_opmode_blocking)
 if result != vrep.simx_return_ok:
     raise Exception('Could not get object position for the Cuboid')
-Cuboid_position = np.reshape(Cuboid_position,(3,1))
+Cuboid_position = np.reshape(Cuboid_position,(3,1)) # Position of the cuboid
+# print ("Cuboid position", Cuboid_position)
 
-# Draw the dummies for the Cuboid
-vrep.simxSetObjectPosition(clientID, dummy_0, -1, Cuboid_position, vrep.simx_opmode_oneshot_wait)
-################################################################################
+# Get the orientation from base to  world frame
+result, Cuboid_orientation = vrep.simxGetObjectOrientation(clientID, Cuboid_handle, -1, vrep.simx_opmode_blocking)
+if result != vrep.simx_return_ok:
+    raise Exception('could not get object orientation angles for UR3')
+
+# Orientation of the cuboid
+R_cuboid = transforms3d.euler.euler2mat(Cuboid_orientation[0], Cuboid_orientation[1], Cuboid_orientation[2])
+# print ("R_cuboid", R_cuboid)
+
+T_2in0 = np.block([
+[R_cuboid[0,:], Cuboid_position[0,:]],
+[R_cuboid[1,:], Cuboid_position[1,:]],
+[R_cuboid[2,:], Cuboid_position[2,:]],
+[0,0,0,1] ])
+print ("T_2in0", T_2in0)
 
+################################################################################
 
 # Get joint object handles
 result, joint_one_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint1', vrep.simx_opmode_blocking)
@@ -190,16 +204,23 @@ initial_thetas = [theta1, theta2, theta3, theta4, theta5, theta6]
 
 ################################################################################
 # Let's use inverse kinematics to move the robot joints to grab the block
+mu = 0.00001
 
+T_1in0 = evalT(S, theta, M)
+screwMat = logm(np.dot(T_2in0, np.linalg.inv(T_1in0)))
+twist =  inv_bracket(screwMat)
 
+while (np.linalg.norm(twist) > 0.001):
+    J = evalJ(S, theta)
+    thetadot = multi_dot([inv(np.dot(np.transpose(J), J) + (mu * np.identity(S.shape[1]))),
+    np.transpose(J), twist])
+    theta += thetadot
 
+    T_1in0 = evalT(S, theta, M)
+    screwMat = logm(np.dot(T_2in0, np.linalg.inv(T_1in0)))
+    twist = inv_bracket(screwMat)
 
-
-
-
-
-
-
+# Get position and orientation of T_1in0