Commit d92fef20 authored by rmaksi2's avatar rmaksi2
Browse files

save

parent 3730ef59
......@@ -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
......
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