Something went wrong on our end
Forked from
rmaksi2 / ECE470
97 commits behind the upstream repository.
check_point2.py 11.17 KiB
import vrep
import time
import numpy as np
from numpy.linalg import multi_dot
from scipy.linalg import expm
from usefulFunctions import deg2rad, rad2deg, revolute, bracket_skew_4
from usefulFunctions import skew, euler2mat, euler_to_a,euler_to_axis, exp_s_the
from usefulFunctions import screwBracForm, rot2euler, quaternion_from_matrix
import transforms3d
'''
https://matthew-brett.github.io/transforms3d/reference/transforms3d.euler.html#transforms3d.euler.euler2mat
pip install transforms3d
'''
# 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')
# Make a dummy
result, dummy_handle_0 = vrep.simxCreateDummy(clientID,0.1,None,vrep.simx_opmode_oneshot_wait)
# Joint: 1 2 3 4 5 6 7
theta_list = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
# Get handles
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')
result, joint_two_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint2', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for second joint')
result, joint_three_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint3', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for third joint')
result, joint_four_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint4', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for fourth joint')
result, joint_five_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint5', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for fifth joint')
result, joint_six_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint6', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for sixth 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')
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
# Dummy handle draw at the joint_six_handle
vrep.simxSetObjectPosition(clientID, dummy_handle_0, joint_six_handle, (0,0,0), vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectOrientation(clientID, dummy_handle_0, joint_six_handle, (0,0,0), vrep.simx_opmode_oneshot_wait)
result, pos = vrep.simxGetObjectPosition(clientID, joint_six_handle, -1, vrep.simx_opmode_oneshot_wait)
print("Initial end pos:", pos)
# Forward Kinematics Implementation
# Set 6 joint angles of UR3
# joint_1_angle = np.pi/6
# joint_2_angle = np.pi/6
# joint_3_angle = np.pi/6
# joint_4_angle = np.pi/6
# joint_5_angle = np.pi/6
# joint_6_angle = np.pi/6
# 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')
print ("orientation", orientation)
# 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))
print ("P_initial", P_initial)
# Return matrix for rotations around z, y and x axes
R_initial = transforms3d.euler.euler2mat(orientation[0], orientation[1], orientation[2])
print ("R_itinial", R_initial)
M = np.array([
[R_initial[0][0], R_initial[0][1], R_initial[0][2], P_initial[0][0]],
[R_initial[1][0], R_initial[1][1], R_initial[1][2], P_initial[1][0]],
[R_initial[2][0], R_initial[2][1], R_initial[2][2], P_initial[2][0]],
[0,0,0,1]
])
print ("M", M)
#Set up scew axis with repesct to base frame
a1 = np.array([[0], [0], [1]])
result, q1 = vrep.simxGetObjectPosition(clientID,joint_one_handle,-1,vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object current position for UR3')
q1 = np.reshape(q1,(3,1))
S1 = revolute(a1, q1)
a2 = np.array([[-1], [0], [0]])
# a2 = transforms3d.euler.euler2mat(a2_arr)
result, q2 = vrep.simxGetObjectPosition(clientID,joint_two_handle,-1,vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object current position for UR3')
q2 = np.reshape(q2,(3,1))
S2 = revolute(a2,q2)
a3 = np.array([[1], [0], [0]])
# a3 = transforms3d.euler.euler2mat(a3_arr)
result, q3 = vrep.simxGetObjectPosition(clientID,joint_three_handle,-1,vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object current position for UR3')
q3 = np.reshape(q3,(3,1))
S3 = revolute(a3,q3)
a4 = np.array([[-1], [0], [0]])
# a4 = transforms3d.euler.euler2mat(a4_arr)
result, q4 = vrep.simxGetObjectPosition(clientID,joint_four_handle,-1,vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object current position for UR3')
q4 = np.reshape(q4,(3,1))
S4 = revolute(a4,q4)
a5 = np.array([[0], [0], [1]])
# a5 = transforms3d.euler.euler2mat(a5_arr)
result, q5 = vrep.simxGetObjectPosition(clientID,joint_five_handle,-1,vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object current position for UR3')
q5 = np.reshape(q5,(3,1))
S5 = revolute(a5,q5)
a6 = np.array([[-1], [0], [0]])
# a6 = transforms3d.euler.euler2mat(a6_arr)
result, q6 = vrep.simxGetObjectPosition(clientID,joint_six_handle,-1,vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object current position for UR3')
q6 = np.reshape(q6,(3,1))
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 = screwBracForm(S1) * theta_list[0]
j2 = screwBracForm(S2) * theta_list[1]
j3 = screwBracForm(S3) * theta_list[2]
j4 = screwBracForm(S4) * theta_list[3]
j5 = screwBracForm(S5) * theta_list[4]
j6 = screwBracForm(S6) * theta_list[5]
T_final = multi_dot([expm(j1), expm(j2), expm(j3), expm(j4), expm(j5), expm(j6), M])
print("T_final", T_final)
# quaternion = quaternion_from_matrix(T_final)
# temp = quaternion[0]
# quaternion[0] = quaternion[1]
# quaternion[1] = quaternion[2]
# quaternion[2] = quaternion[3]
# quaternion[3] = temp
P_final = T_final[0:3,3]
print ("P_final", P_final)
print("Theta:", theta_list)
# print("Calculated quaternion:", quaternion)
R_final = T_final[0:3, 0:3]
print ("R_final", R_final)
final_a,final_b,final_g = transforms3d.euler.mat2euler(R_final)
final_euler = [final_a, final_b, final_g]
print ("final_euler", final_euler)
# result, dummy_ball_handle = vrep.simxGetObjectHandle(clientID, 'ReferenceFrame1', vrep.simx_opmode_blocking)
# if result != vrep.simx_return_ok:
# raise Exception('could not get reference frame 1 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,P_final,vrep.simx_opmode_blocking)
# if result != vrep.simx_return_ok:
# raise Exception('could not set dummy ball position')
# Show predicted position
result, dummy_handle_1 = vrep.simxCreateDummy(clientID,0.1,None,vrep.simx_opmode_oneshot_wait)
if result != vrep.simx_return_ok:
raise Exception('could not get dummy_handle_1')
vrep.simxSetObjectPosition(clientID, dummy_handle_1, UR3_handle, P_final, vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectOrientation(clientID, dummy_handle_1, UR3_handle, final_euler, vrep.simx_opmode_oneshot_wait)
# 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))
#
# result2, theta2 = vrep.simxGetJointPosition(clientID, joint_two_handle, vrep.simx_opmode_blocking)
# if result2 != vrep.simx_return_ok:
# raise Exception('could not get second joint variable')
# print('current value of second joint variable on UR3: theta = {:f}'.format(theta2))
#
# result3, theta3 = vrep.simxGetJointPosition(clientID, joint_three_handle, vrep.simx_opmode_blocking)
# if result3 != vrep.simx_return_ok:
# raise Exception('could not get third joint variable')
# print('current value of third joint variable on UR3: theta = {:f}'.format(theta3))
#
# result4, theta4 = vrep.simxGetJointPosition(clientID, joint_four_handle, vrep.simx_opmode_blocking)
# if result4 != vrep.simx_return_ok:
# raise Exception('could not get fourth joint variable')
# print('current value of fourth joint variable on UR3: theta = {:f}'.format(theta4))
#
# result5, theta5 = vrep.simxGetJointPosition(clientID, joint_five_handle, vrep.simx_opmode_blocking)
# if result5 != vrep.simx_return_ok:
# raise Exception('could not get fifth joint variable')
# print('current value of fifth joint variable on UR3: theta = {:f}'.format(theta5))
#
# result6, theta6 = vrep.simxGetJointPosition(clientID, joint_six_handle, vrep.simx_opmode_blocking)
# if result6 != vrep.simx_return_ok:
# raise Exception('could not get sixth joint variable')
# print('current value of sixth joint variable on UR3: theta = {:f}'.format(theta6))
# Set the desired value of the first joint variable
vrep.simxSetJointTargetPosition(clientID, joint_one_handle, theta_list[0], vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetPosition(clientID, joint_two_handle, theta_list[1], vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetPosition(clientID, joint_three_handle, theta_list[2], vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetPosition(clientID, joint_four_handle, theta_list[3], vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetPosition(clientID, joint_five_handle, theta_list[4], vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetPosition(clientID, joint_six_handle, theta_list[5], vrep.simx_opmode_oneshot_wait)
# Wait two seconds
time.sleep(5)
vrep.simxRemoveObject(clientID,dummy_handle_0,vrep.simx_opmode_oneshot_wait)
vrep.simxRemoveObject(clientID,dummy_handle_1,vrep.simx_opmode_oneshot_wait)
# 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)