check_point2.py

import vrep
import time
import numpy as np
from numpy.linalg import multi_dot
from usefulFunctions import deg2rad, rad2deg, revolute, bracket_skew_4, skew, euler2mat, euler_to_a,euler_to_axis


# Close all open connections (just in case)
vrep.simxFinish(-1)

# Connect to V-REP (raise exception on failure)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
    raise Exception('Failed connecting to remote API server')

# Get "handle" to the first joint of robot
result, joint_one_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint1', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
    raise Exception('could not get object handle for first joint')


#Get handel for UR3
result, UR3_handle = vrep.simxGetObjectHandle(clientID, 'UR3', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
    raise Exception('could not get object handle for UR3')

# Forward Kinematics Implementation
# Set 6 joint angles of UR3
joint_1_angle = np.pi/2
joint_2_angle = np.pi/2
joint_3_angle = np.pi/2
joint_4_angle = np.pi/2
joint_5_angle = np.pi/2
joint_6_angle = np.pi/2
joint_angles = np.array([joint_1_angle,joint_2_angle,joint_3_angle,joint_4_angle,joint_5_angle,joint_6_angle])


# Get the orientation from base to  world frame
result , orientation = vrep.simxGetObjectOrientation(clientID, UR3_handle, -1, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
    raise Exception('could not get object orientation angles for UR3')

# Get the position from base to world frame
result, P = vrep.simxGetObjectPosition(clientID,UR3_handle,-1,vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
    raise Exception('could not get object current position for UR3')

P_initial = np.reshape(P,(3,1))
# Return matrix for rotations around z, y and x axes
R_initial = euler2mat(orientation[0], orientation[1], orientation[2])


#Set up scew axis with repesct to base frame
a1_arr = np.array([[0], [0], [1]])
a1 = euler_to_axis(a1_arr)
q1 =np.array([[-0.2000],[-0.1000],[0.0430]])
S1 = revolute(a1,q1)

a2_arr = np.array([[-1], [0], [0]])
a2 = euler_to_axis(a2_arr)
q2 =np.array([[-0.2037],[-0.1002],[0.1474]])
S2 = revolute(a2,q2)

a3_arr = np.array([[-1], [0], [0]])
a3 =euler_to_axis(a3_arr)
q3 = np.array([[-0.3154],[-0.0999],[0.2698]])
S3 = revolute(a3,q3)

a4_arr = np.array([[1], [0], [0]])
a4 = euler_to_axis(a4_arr)
q4 = np.array([[-0.2292],[-0.1000],[0.4994]])
S4 = revolute(a4,q4)

a5_arr = np.array([[0], [0], [-1]])
a5 = euler_to_axis(a5_arr)
q5 = np.array([[-0.3101],[-0.0999],[0.6098]])
S5 = revolute(a5,q5)

a6_arr = np.array([[1], [0], [0]])
a6 =euler_to_axis(a6_arr)
q6 =np.array([[-0.3142],[-0.0996],[0.6920]])
S6 = revolute(a6,q6)


#Also get the end of UR3 sixth joint handle
result, joint_six_handle= vrep.simxGetObjectHandle(clientID, 'UR3_joint6', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
	raise Exception('could not get UR3_joint6 handle')



#Use T = e^([s]*theta)* M_inital to get final pose
#The basic Forward kinematics equation
j1 = exp_s_the(S1, joint_angles[0])
j2 = exp_s_the(S2, joint_angles[1])
j3 = exp_s_the(S3, joint_angles[2])
j4 = exp_s_the(S4, joint_angles[3])
j5 = exp_s_the(S5, joint_angles[4])



T_final = multi_dot([j1,j2,j3,j4,j5,j6, M])
print(T_final)
R_final = T_final[0:3, 0:3]
P_final = T_final[0:3,3]

R_final = T_final[0:3, 0:3]
P_final = T_final[0:3,3]

final_a,final_b,final_g = rot2euler(R_final)
final_euler = [final_a,final_b,final_g]

result, dummy_ball_handle = vrep.simxGetObjectHandle(clientID, 'Cup', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
	raise Exception('could not get dummy ball handle')

time.sleep(1)

result = vrep.simxSetObjectOrientation(clientID,dummy_ball_handle,-1,final_euler,vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
	raise Exception('could not set dummy ball orientation')

time.sleep(1)

result = vrep.simxSetObjectPosition(clientID,dummy_ball_handle,-1,final_p,vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
	raise Exception('could not set dummy ball position')


# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)

# Wait two seconds
time.sleep(2)

# Get the current value of the first joint variable
result, theta = vrep.simxGetJointPosition(clientID, joint_one_handle, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
    raise Exception('could not get first joint variable')
print('current value of first joint variable: theta = {:f}'.format(theta))


# Set the desired value of the first joint variable
vrep.simxSetJointTargetPosition(clientID, joint_one_handle, theta + (np.pi / 2), vrep.simx_opmode_oneshot)

# Wait two seconds
time.sleep(2)

# Get the current value of the first joint variable
result, theta = vrep.simxGetJointPosition(clientID, joint_one_handle, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
    raise Exception('could not get first joint variable')
print('current value of first joint variable: theta = {:f}'.format(theta))

# Stop simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)

# Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
vrep.simxGetPingTime(clientID)

# Close the connection to V-REP
vrep.simxFinish(clientID)