Skip to content
Snippets Groups Projects
Commit 5fa1a85b authored by rmaksi2's avatar rmaksi2
Browse files

Added code from integration0

parent ae5f87ea
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
FinalProject/Check_Point_4/check_point4.py
+ 578
− 220
View file @ 5fa1a85b
......@@ -2,232 +2,590 @@ import vrep
import time
import numpy as np
import math
from numpy.linalg import multi_dot
from numpy.linalg import multi_dot, norm, inv
from scipy.linalg import expm, logm
# 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, deg2rad
from usefulFunctions import*
from ece470_lib import *
from usefulFunctions import *
import transforms3d
'''
https://matthew-brett.github.io/transforms3d/reference/transforms3d.euler.html#transforms3d.euler.euler2mat
pip install transforms3d
'''
def main():
# Close all open connections (just in case)
vrep.simxFinish(-1)
# 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')
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
#parameters set up for creating dummy objects for joints and links
#Base dummy object
# body_names = ["Dummy_body_base"]
# body_diam = [0.4]
# #link is between two joints, we have 7 links in total
# link_names = ["Dummy_link_joint1", "Dummy_link_joint2", "Dummy_link_joint3", "Dummy_link_joint4", "Dummy_link_joint5", "Dummy_link_joint6", "Dummy_link_joint7"]
# #dummy objects for 6 joints
# joint_names = ["Dummy_joint1", "Dummy_joint2", "Dummy_joint3", "Dummy_joint4", "Dummy_joint5", "Dummy_joint6"]
# joint_diam = [0.3, 0.3, 0.2, 0.2, 0.15, 0.15]
# #ADD Base dummy to whole joints dummy.
# self_diam = body_diam.copy()
# self_diam.extend(joint_diam)
# link_centers = []
# body_centers = []
# joint_centers = []
# Get joint object handles
result, joint_one_handle = vrep.simxGetObjectHandle(clientID, 'UR3_link1_visible', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
#First dummy1-Red
result, handle = vrep.simxCreateDummy(clientID, 0.125, [255,0,0] , vrep.simx_opmode_blocking)
#use the handle to set dummy object of the joint handle
vrep.simxSetObjectPosition(clientID, handle, -1, (0,0,0), vrep.simx_opmode_oneshot)
#Get second joint object handles
result, joint_two_handle = vrep.simxGetObjectHandle(clientID, 'UR3_link2_visible', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
#Get position of joint 2 handle
result, p = vrep.simxGetObjectPosition(clientID, joint_two_handle, handle, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object current position for UR3')
P_2 = np.reshape(p,(3,1))
print ("P_2", P_2)
#Set dummy2
result, handle_2 = vrep.simxCreateDummy(clientID, 0.125, [0,0,255] , vrep.simx_opmode_blocking)
# vrep.simxSetObjectPosition(clientID, handle, -1, P_2, vrep.simx_opmode_oneshot)
vrep.simxSetObjectPosition(clientID, handle_2, -1, P_2, vrep.simx_opmode_oneshot)
#Joint 3
result, joint_three_handle = vrep.simxGetObjectHandle(clientID, 'UR3_link3_visible', vrep.simx_opmode_blocking)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
raise Exception('Failed connecting to remote API server')
# Dummies for ConcretBlock1
result, dummy_0 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_blocking)
result, dummy_2 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
result, dummy_3 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
result, dummy_1 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
result, dummy_4 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
result, dummy_5 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
# Dummies for ConcretBlock2
result, dummy_6 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_blocking)
result, dummy_7 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
result, dummy_8 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
result, dummy_9 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
result, dummy_10 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
result, dummy_11 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
# Get handle for the ConcretBlock1
result, ConcretBlock1_handle = vrep.simxGetObjectHandle(clientID, 'ConcretBlock1', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for the ConcretBlock1')
# Get handle for the ConcretBlock2
result, ConcretBlock2_handle = vrep.simxGetObjectHandle(clientID, 'ConcretBlock2', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for the ConcretBlock2')
# Get position of the ConcretBlock1
result, ConcretBlock1_position = vrep.simxGetObjectPosition(clientID, ConcretBlock1_handle, -1, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('Could not get object position for the ConcretBlock1')
ConcretBlock1_position = np.reshape(ConcretBlock1_position,(3,1))
# Get position of the ConcretBlock2
result, ConcretBlock2_position = vrep.simxGetObjectPosition(clientID, ConcretBlock2_handle, -1, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('Could not get object position for the ConcretBlock2')
ConcretBlock2_position = np.reshape(ConcretBlock2_position,(3,1))
# Draw the dummies for the ConcretBlock1
vrep.simxSetObjectPosition(clientID, dummy_0, -1, ConcretBlock1_position, vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(clientID, dummy_1, dummy_0, [0,0,-0.1], vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(clientID, dummy_2, dummy_0, [0,0,-0.2], vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(clientID, dummy_3, dummy_0, [0,0,-0.3], vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(clientID, dummy_4, dummy_0, [0,0,0.1], vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(clientID, dummy_5, dummy_0, [0,0,0.2], vrep.simx_opmode_oneshot_wait)
# Draw the dummies for the ConcretBlock2
vrep.simxSetObjectPosition(clientID, dummy_6, -1, ConcretBlock2_position, vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(clientID, dummy_7, dummy_6, [0,0,-0.1], vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(clientID, dummy_8, dummy_6, [0,0,-0.2], vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(clientID, dummy_9, dummy_6, [0,0,-0.3], vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(clientID, dummy_10, dummy_6, [0,0,0.1], vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(clientID, dummy_11, dummy_6, [0,0,0.2], vrep.simx_opmode_oneshot_wait)
# Create p_obstacle array for collision checking and path planner
# Lists for final p_obstacle
entry_one = list()
entry_two = list()
entry_three = list()
# First, we handle first dummy (string combination), then we handle the rest in a loop
result, dummy_handle = vrep.simxGetObjectHandle(clientID, 'Dummy', vrep.simx_opmode_oneshot_wait)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for Dummy')
result, dummy_position = vrep.simxGetObjectPosition(clientID, dummy_handle, -1, vrep.simx_opmode_oneshot_wait)
if result != vrep.simx_return_ok:
raise Exception('Could not get object position for Dummy')
entry_one.append(dummy_position[0])
entry_two.append(dummy_position[1])
entry_three.append(dummy_position[2])
# Loop for the rest
for i in range(11):
dummy_handle_name = 'Dummy' + str(i)
result, dummy_handle = vrep.simxGetObjectHandle(clientID, dummy_handle_name, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
result, p_3 = vrep.simxGetObjectPosition(clientID, joint_three_handle, handle, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object current position for UR3')
P_3 = np.reshape(p_3,(3,1))
print ("P_3", P_3)
#Dummy 3
result, handle_3 = vrep.simxCreateDummy(clientID, 0.125, [0,255,0] , vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, handle_3, -1, P_3, vrep.simx_opmode_oneshot)
raise Exception('could not get object handle for ', dummy_handle_name)
#Joint4
result, joint_four_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint3', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
result, p_4 = vrep.simxGetObjectPosition(clientID, joint_four_handle, handle, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object current position for UR3')
P_4 = np.reshape(p_4,(3,1))
print ("P_4", P_4)
#Dummy 3
result, handle_4 = vrep.simxCreateDummy(clientID, 0.125, [0,255,0] , vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, handle_4, -1, P_4, vrep.simx_opmode_oneshot)
#Joint5
result, joint_five_handle = vrep.simxGetObjectHandle(clientID, 'UR3_link4_visible', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
result, p_5 = vrep.simxGetObjectPosition(clientID, joint_five_handle, handle, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object current position for UR3')
P_5 = np.reshape(p_5,(3,1))
print ("P_5", P_5)
#Dummy 3
result, handle_5 = vrep.simxCreateDummy(clientID, 0.125, [0,255,0] , vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, handle_5, -1, P_5, vrep.simx_opmode_oneshot)
#Joint6
result, joint_six_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint4', vrep.simx_opmode_blocking)
dummy_name = 'dummy_' + str(i+1)
result, dummy_position = vrep.simxGetObjectPosition(clientID, dummy_handle, -1, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
result, p_6 = vrep.simxGetObjectPosition(clientID, joint_six_handle, handle, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object current position for UR3')
P_6 = np.reshape(p_6,(3,1))
print ("P_6", P_6)
#Dummy 3
result, handle_6 = vrep.simxCreateDummy(clientID, 0.125, [0,255,0] , vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, handle_6, -1, P_6, vrep.simx_opmode_oneshot)
#Joint7
result, joint_seven_handle = vrep.simxGetObjectHandle(clientID, 'UR3_link7', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
result, p_7 = vrep.simxGetObjectPosition(clientID, joint_seven_handle, handle, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object current position for UR3')
P_7 = np.reshape(p_7,(3,1))
print ("P_7", P_7)
#Dummy 3
result, handle_7 = vrep.simxCreateDummy(clientID, 0.125, [0,255,0] , vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, handle_7, -1, P_7, vrep.simx_opmode_oneshot)
# vrep.simxSetObjectPosition(clientID, handle, joint_one_handle, (0,0,0), vrep.simx_opmode_oneshot_wait)
print("################################################")
# #Drawing a path
# solved = ss.solve(20.0) # ss is a SimpleSetup object
# if solved:
# print(ss.getSolutionPath().asGeometric().printAsMatrix())
# for i in range(len(link_names)):
# result, dummy_handle = vrep.simxGetObjectHandle(clientID, link_names[i], vrep.simx_opmode_blocking)
# if result != vrep.simx_return_ok:
# raise Exception("Could not get object handle for the Dummy object")
# status, position = vrep.simxGetObjectPosition(clientID, dummy_handle, -1, vrep.simx_opmode_blocking)
# link_centers.append(np.array(position))
# print(position)
# for j in range(len(body_names)):
# result, dummy_handle = vrep.simxGetObjectHandle(clientID, body_names[j], vrep.simx_opmode_blocking)
# if result != vrep.simx_return_ok:
# raise Exception("Could not get object handle for the Dummy object")
# status, position = vrep.simxGetObjectPosition(clientID, dummy_handle, -1, vrep.simx_opmode_blocking)
# body_centers.append(np.array(position))
# for k in range(len(joint_names)):
# result, dummy_handle = vrep.simxGetObjectHandle(clientID, joint_names[k], vrep.simx_opmode_blocking)
# if result != vrep.simx_return_ok:
# raise Exception("Could not get object handle for the Dummy object")
# status, position = vrep.simxGetObjectPosition(clientID, dummy_handle, -1, vrep.simx_opmode_blocking)
# joint_centers.append(np.array(position))
# dummy_list = []
# print("link_centers is :" ,link_centers)
# print("joint_centers is :" ,joint_centers)
# print("body_centers is :", body_centers)
# Goal_pose = RotationMatrixToPose(0.1, 0.1, 0.1, 0.1, 0.1, 01.)
# M = np.array([[0,0,-1, -0.1940], [0,1,0,0], [1,0,0,0.6511], [0,0,0,1]])
# #Get all screw axis
# a1 = np.array([[0],[0],[1]])
# q1 = np.array([[0], [0], [0.1045]])
# S1 = revolute(a1, q1)
# a2 = np.array([[-1],[0],[0]])
# q2 = np.array([[-0.1115], [0], [0.1089]])
# S2 = revolute(a2,q2)
# a3 = np.array([[-1],[0],[0]])
# q3 = np.array([[-0.1115], [0], [0.3525]])
# S3 = revolute(a3,q3)
# a4 = np.array([[-1],[0],[0]])
# q4 = np.array([[-0.1115], [0], [0.5658]])
# S4 = revolute(a4,q4)
# a5 = np.array([[0],[0],[1]])
# q5 = np.array([[-0.1122], [0], [0.65]])
# S5 = revolute(a5,q5)
# a6 = np.array([[-1],[0],[0]])
# q6 = np.array([[0.1115], [0], [0.6511]])
# S6 = revolute(a6,q6)
# Wait two seconds
time.sleep(10)
#vrep.simxRemoveObject(clientID,dummy_handle_1,vrep.simx_opmode_oneshot_wait)
# vrep.simxRemoveObject(clientID,dummy_handle_0,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)
result = main()
print(result)
\ No newline at end of file
raise Exception('Could not get object position for ', dummy_name)
entry_one.append(dummy_position[0])
entry_two.append(dummy_position[1])
entry_three.append(dummy_position[2])
entry_one = np.asarray(entry_one)
entry_two = np.asarray(entry_two)
entry_three = np.asarray(entry_three)
#################### p_obstacle ################################################
p_obstacle = np.block([
[np.asarray(entry_one)], [np.asarray(entry_two)], [np.asarray(entry_three)]
])
################################################################################
# Get joint object handles
result, joint_one_handle = vrep.simxGetObjectHandle(clientID, 'UR3_link1_visible', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
#First dummy1-Red
result, handle = vrep.simxCreateDummy(clientID, 0.125, [255,0,0] , vrep.simx_opmode_blocking)
#use the handle to set dummy object of the joint handle
vrep.simxSetObjectPosition(clientID, handle, -1, (0,0,0), vrep.simx_opmode_oneshot)
#Get second joint object handles
result, joint_two_handle = vrep.simxGetObjectHandle(clientID, 'UR3_link2_visible', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
#Get position of joint 2 handle
result, p = vrep.simxGetObjectPosition(clientID, joint_two_handle, handle, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object current position for UR3')
P_2 = np.reshape(p,(3,1))
print ("P_2", P_2)
#Set dummy2
result, handle_2 = vrep.simxCreateDummy(clientID, 0.125, [0,0,255] , vrep.simx_opmode_blocking)
# vrep.simxSetObjectPosition(clientID, handle, -1, P_2, vrep.simx_opmode_oneshot)
vrep.simxSetObjectPosition(clientID, handle_2, -1, P_2, vrep.simx_opmode_oneshot)
#Joint 3
result, joint_three_handle = vrep.simxGetObjectHandle(clientID, 'UR3_link3_visible', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
result, p_3 = vrep.simxGetObjectPosition(clientID, joint_three_handle, handle, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object current position for UR3')
P_3 = np.reshape(p_3,(3,1))
print ("P_3", P_3)
#Dummy 3
result, handle_3 = vrep.simxCreateDummy(clientID, 0.125, [0,255,0] , vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, handle_3, -1, P_3, vrep.simx_opmode_oneshot)
#Joint4
result, joint_four_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint3', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
result, p_4 = vrep.simxGetObjectPosition(clientID, joint_four_handle, handle, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object current position for UR3')
P_4 = np.reshape(p_4,(3,1))
print ("P_4", P_4)
#Dummy 3
result, handle_4 = vrep.simxCreateDummy(clientID, 0.125, [0,255,0] , vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, handle_4, -1, P_4, vrep.simx_opmode_oneshot)
#Joint5
result, joint_five_handle = vrep.simxGetObjectHandle(clientID, 'UR3_link4_visible', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
result, p_5 = vrep.simxGetObjectPosition(clientID, joint_five_handle, handle, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object current position for UR3')
P_5 = np.reshape(p_5,(3,1))
print ("P_5", P_5)
#Dummy 3
result, handle_5 = vrep.simxCreateDummy(clientID, 0.125, [0,255,0] , vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, handle_5, -1, P_5, vrep.simx_opmode_oneshot)
#Joint6
result, joint_six_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint4', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
result, p_6 = vrep.simxGetObjectPosition(clientID, joint_six_handle, handle, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object current position for UR3')
P_6 = np.reshape(p_6,(3,1))
print ("P_6", P_6)
#Dummy 3
result, handle_6 = vrep.simxCreateDummy(clientID, 0.125, [0,255,0] , vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, handle_6, -1, P_6, vrep.simx_opmode_oneshot)
#Joint7
result, joint_seven_handle = vrep.simxGetObjectHandle(clientID, 'UR3_link7', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for first joint')
result, p_7 = vrep.simxGetObjectPosition(clientID, joint_seven_handle, handle, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object current position for UR3')
P_7 = np.reshape(p_7,(3,1))
print ("P_7", P_7)
#Dummy 3
result, handle_7 = vrep.simxCreateDummy(clientID, 0.125, [0,255,0] , vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, handle_7, -1, P_7, vrep.simx_opmode_oneshot)
# vrep.simxSetObjectPosition(clientID, handle, joint_one_handle, (0,0,0), vrep.simx_opmode_oneshot_wait)
print("################################################")
##################################################################################
# Get joint object 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')
#Get handel for UR3_connection
result, UR3_connection = vrep.simxGetObjectHandle(clientID, 'UR3_connection', vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for UR3UR3_connection')
#Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
# Get the orientation from base to world frame
result, orientation = vrep.simxGetObjectOrientation(clientID, UR3_connection, UR3_handle, 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_connection, UR3_handle, 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 = transforms3d.euler.euler2mat(orientation[0], orientation[1], orientation[2])
M = np.array([[0,0,-1, P_initial[0][0]], [0,1,0, P_initial[1][0]], [1,0,0, P_initial[2][0]], [0,0,0,1]])
a1 = np.array([[0],[0],[1]])
q1 = np.array([[0], [0], [0.1045]])
S1 = revolute(a1, q1)
a2 = np.array([[-1],[0],[0]])
q2 = np.array([[-0.1115], [0], [0.1089]])
S2 = revolute(a2,q2)
a3 = np.array([[-1],[0],[0]])
q3 = np.array([[-0.1115], [0], [0.3525]])
S3 = revolute(a3,q3)
a4 = np.array([[-1],[0],[0]])
q4 = np.array([[-0.1115], [0], [0.5658]])
S4 = revolute(a4,q4)
a5 = np.array([[0],[0],[1]])
q5 = np.array([[-0.1122], [0], [0.65]])
S5 = revolute(a5,q5)
a6 = np.array([[-1],[0],[0]])
q6 = np.array([[0.1115], [0], [0.6511]])
S6 = revolute(a6,q6)
S = np.block([[S1, S2, S3, S4, S5, S6]])
############### Get the values for joint variables #############################
result1, theta1 = vrep.simxGetJointPosition(clientID, joint_one_handle, vrep.simx_opmode_blocking)
if result1 != vrep.simx_return_ok:
raise Exception('could not get first joint variable')
# print('current value of first joint variable on UR3: theta = {:f}'.format(theta1))
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))
#Inital thetas from the simulator
initial_thetas = [theta1, theta2, theta3, theta4, theta5, theta6]
# Make a dummy for end_effector_pose
result, end_effector_pose = vrep.simxCreateDummy(clientID,0.1,None,vrep.simx_opmode_oneshot_wait)
############################################
# This is totally not working
# #Change from Dummy11 if more dummies added before
# result, end_effector_handle = vrep.simxGetObjectHandle(clientID, "Dummy11", vrep.simx_opmode_blocking)
# if result != vrep.simx_return_ok:
# raise Exception('could not get object handle for end_effector_pose')
# result, end_effector_position = vrep.simxGetObjectPosition(clientID, end_effector_handle, -1, vrep.simx_opmode_blocking)
# if result != vrep.simx_return_ok:
# raise Exception('Could not get object position for end_effector_pose')
#
# # Draw the dummy
# vrep.simxSetObjectPosition(clientID, end_effector_handle, -1, [1,1,1], vrep.simx_opmode_oneshot_wait)
#
# # vrep.simxSetObjectPosition(clientID, dummy_1, dummy_0, [0,0,-0.1], vrep.simx_opmode_oneshot_wait)
#
# print("Move dummy object at the end effector to the position you want to move to")
#
# while(1):
# result, pos_check = vrep.simxGetObjectPosition(clientID, end_effector_handle, -1, vrep.simx_opmode_blocking)
# if result != vrep.simx_return_ok:
# raise Exception('Could not get object position for end_effector_pose')
#
# if(pos_check != end_effector_position):
# break
##########################################
i = 0
while i < 3:
if(input("Press any key to start: ") == "quit"):
break
goal_pose = user_input()
# goal_pose = RotationMatrixToPose(0.3, 0.4, 0.3, 30, 30, 30)
# end_effector_position = np.reshape(end_effector_position,(3,1))
# goal_pose = RotationMatrixToPose(end_effector_position[0][0], end_effector_position[1][0], end_effector_position[2][0], 30, 30, 30)
quaternion = quaternion_from_matrix(goal_pose)
temp = quaternion[0]
quaternion[0] = quaternion[1]
quaternion[1] = quaternion[2]
quaternion[2] = quaternion[3]
quaternion[3] = temp
P_final = goal_pose[0:3,3]
# Draw the dummy
vrep.simxSetObjectPosition(clientID, end_effector_pose, -1, P_final, vrep.simx_opmode_oneshot_wait)
thetas_for_ik = np.array([[theta1], [theta2], [theta3], [theta4], [theta5], [theta6]])
S_mat = [S1, S2, S3, S4, S5, S6]
thetas_from_ik = findIK(goal_pose, S, M, thetas_for_ik)
thetas_from_ik = thetas_from_ik[0]
theta_start = np.array([[initial_thetas[0]], [initial_thetas[1]], [initial_thetas[2]], [initial_thetas[3]], [initial_thetas[4]], [initial_thetas[5]]])
theta_goal = np.array([[thetas_from_ik[0][0]], [thetas_from_ik[1][0]], [thetas_from_ik[2][0]], [thetas_from_ik[3][0]], [thetas_from_ik[4][0]], [thetas_from_ik[5][0]]])
step_size = 0.01
start_pos = np.array([[0],[0],[0]])
end_pos = np.array([[M[0][3]],[M[1][3]],[M[2][3]]])
p_robot = np.block([[start_pos, q1, q2, q3, q4, q5, q6, end_pos]])
r_robot = np.array([[0.03, 0.03, 0.03, 0.03, 0.03, 0.03, 0.03, 0.03]])
# r_robot = np.array([[0.12 , 0.18, 0.18, 0.18, 0.18, 0.18, 0.18,0.18 ]])
# p_obstacle = np.array([[1], [1], [1]])
r_obstacle = np.array([[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]])
# print(p_obstacle)
start_root = Node()
start_root.theta = theta_start
end_root = Node()
end_root.theta = theta_goal
start_curr = start_root
end_curr = end_root
counter = 0
bound = 50
#
# if(point2point_collision_detector(theta_start, theta_goal, S, p_robot, r_robot, p_obstacle, r_obstacle, step_size) is 0):
# q = [theta_start, theta_goal]
# else:
# while counter < bound:
# '''
# Joint Bounds from Lab 3 Doc in degrees:
# theta1 = (60,315)
# theta2 = (-185,5)
# theta3 = (-10,150)
# theta4 = (-190,-80)
# theta5 = (-120,80)
# theta6 = (-180,180)
# '''
# rand1 = random.uniform(deg2rad(60),deg2rad(315))
# rand2 = random.uniform(deg2rad(-185),deg2rad(5))
# rand3 = random.uniform(deg2rad(-10),deg2rad(150))
# rand4 = random.uniform(deg2rad(-190),deg2rad(-80))
# rand5 = random.uniform(deg2rad(-120),deg2rad(80))
# rand6 = random.uniform(deg2rad(-180),deg2rad(180))
#
# theta_target = np.array([[rand1], [rand2], [rand3], [rand4], [rand5], [rand6]])
#
# theta_a = start_curr.theta
# theta_b = theta_target
#
# if(point2point_collision_detector(theta_a, theta_b, S, p_robot, r_robot, p_obstacle, r_obstacle, step_size) is 0):
# start_curr.next = Node()
# start_curr = start_curr.next
# start_curr.theta = theta_target
#
# theta_a = end_root.theta
# theta_b = theta_target
#
# if(point2point_collision_detector(theta_a, theta_b, S, p_robot, r_robot, p_obstacle, r_obstacle, step_size) is 0):
# temp = Node()
# temp.next = end_root
# temp.theta = theta_target
# end_root = temp
#
# q = get_complete_path(start_root, end_root)
#
# if(q is not None):
# break
#
# counter += 1
while counter < bound:
'''
Joint Bounds from Lab 3 Doc in degrees:
theta1 = (60,315)
theta2 = (-185,5)
theta3 = (-10,150)
theta4 = (-190,-80)
theta5 = (-120,80)
theta6 = (-180,180)
'''
# rand1 = random.uniform(deg2rad(60),deg2rad(315))
# rand2 = random.uniform(deg2rad(-185),deg2rad(5))
# rand3 = random.uniform(deg2rad(-10),deg2rad(150))
# rand4 = random.uniform(deg2rad(-190),deg2rad(-80))
# rand5 = random.uniform(deg2rad(-120),deg2rad(80))
# rand6 = random.uniform(deg2rad(-180),deg2rad(180))
rand1 = random.uniform(-np.pi,np.pi)
rand2 = random.uniform(-np.pi,np.pi)
rand3 = random.uniform(-np.pi,np.pi)
rand4 = random.uniform(-np.pi,np.pi)
rand5 = random.uniform(-np.pi,np.pi)
rand6 = random.uniform(-np.pi,np.pi)
theta_target = np.array([[rand1], [rand2], [rand3], [rand4], [rand5], [rand6]])
theta_a = start_curr.theta
theta_b = theta_target
if(point2point_collision_detector(theta_a, theta_b, S, p_robot, r_robot, p_obstacle, r_obstacle, step_size) is 0):
start_curr.next = Node()
start_curr = start_curr.next
start_curr.theta = theta_target
theta_a = end_root.theta
theta_b = theta_target
if(point2point_collision_detector(theta_a, theta_b, S, p_robot, r_robot, p_obstacle, r_obstacle, step_size) is 0):
temp = Node()
temp.next = end_root
temp.theta = theta_target
end_root = temp
q = get_complete_path(start_root, end_root)
if(q is not None):
break
counter += 1
if counter == bound:
print("NO PATH POSSIBLE")
print("Start list:")
print_linked_list(start_root)
print("Goal list")
print_linked_list(end_root)
else:
print("Found path")
for theta in q:
# Set the desired value of the first joint variable
vrep.simxSetJointTargetPosition(clientID, joint_one_handle, theta[0][0], vrep.simx_opmode_oneshot)
time.sleep(0.1)
vrep.simxSetJointTargetPosition(clientID, joint_two_handle, theta[1][0], vrep.simx_opmode_oneshot)
time.sleep(0.1)
vrep.simxSetJointTargetPosition(clientID, joint_three_handle, theta[2][0], vrep.simx_opmode_oneshot)
time.sleep(0.1)
vrep.simxSetJointTargetPosition(clientID, joint_four_handle, theta[3][0], vrep.simx_opmode_oneshot)
time.sleep(0.1)
vrep.simxSetJointTargetPosition(clientID, joint_five_handle, theta[4][0], vrep.simx_opmode_oneshot)
time.sleep(0.1)
vrep.simxSetJointTargetPosition(clientID, joint_six_handle, theta[5][0], vrep.simx_opmode_oneshot)
time.sleep(1)
q.reverse()
for theta in q:
# Set the desired value of the first joint variable
vrep.simxSetJointTargetPosition(clientID, joint_one_handle, theta[0][0], vrep.simx_opmode_oneshot)
time.sleep(0.1)
vrep.simxSetJointTargetPosition(clientID, joint_two_handle, theta[1][0], vrep.simx_opmode_oneshot)
time.sleep(0.1)
vrep.simxSetJointTargetPosition(clientID, joint_three_handle, theta[2][0], vrep.simx_opmode_oneshot)
time.sleep(0.1)
vrep.simxSetJointTargetPosition(clientID, joint_four_handle, theta[3][0], vrep.simx_opmode_oneshot)
time.sleep(0.1)
vrep.simxSetJointTargetPosition(clientID, joint_five_handle, theta[4][0], vrep.simx_opmode_oneshot)
time.sleep(0.1)
vrep.simxSetJointTargetPosition(clientID, joint_six_handle, theta[5][0], vrep.simx_opmode_oneshot)
time.sleep(1)
i += 1
# Remove end_effector_pose dummy
vrep.simxRemoveObject(clientID, end_effector_pose, vrep.simx_opmode_oneshot_wait)
# Remove dummy handles for ConcretBlock1
vrep.simxRemoveObject(clientID, dummy_0, vrep.simx_opmode_oneshot_wait)
vrep.simxRemoveObject(clientID, dummy_1, vrep.simx_opmode_oneshot_wait)
vrep.simxRemoveObject(clientID, dummy_2, vrep.simx_opmode_oneshot_wait)
vrep.simxRemoveObject(clientID, dummy_3, vrep.simx_opmode_oneshot_wait)
vrep.simxRemoveObject(clientID, dummy_4, vrep.simx_opmode_oneshot_wait)
vrep.simxRemoveObject(clientID, dummy_5, vrep.simx_opmode_oneshot_wait)
# Remove dummy handles for ConcretBlock2
vrep.simxRemoveObject(clientID, dummy_6, vrep.simx_opmode_oneshot_wait)
vrep.simxRemoveObject(clientID, dummy_7, vrep.simx_opmode_oneshot_wait)
vrep.simxRemoveObject(clientID, dummy_8, vrep.simx_opmode_oneshot_wait)
vrep.simxRemoveObject(clientID, dummy_9, vrep.simx_opmode_oneshot_wait)
vrep.simxRemoveObject(clientID, dummy_10, vrep.simx_opmode_oneshot_wait)
vrep.simxRemoveObject(clientID, dummy_11, 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)
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