def setup_simulator():
# Connect to V-REP
print('\nConnecting to V-REP...')
clientID = vu.connect_to_vrep()
print('Connected.')
# Reset simulation in case something was running
vu.reset_sim(clientID)
# Initialize control inputs
vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS))
vu.set_arm_joint_forces(clientID, 50. * np.ones(vu.N_ARM_JOINTS))
vu.step_sim(clientID, 1)
return clientID
def connect(self, scene_file, port):
client_id = vu.connect_to_vrep(port=port)
res = vu.load_scene(client_id, scene_file)
return client_id
import numpy as np
from utilities import vrep_utils as vu
from lib import vrep
vrep.simxFinish(-1) # just in case, close all opened connections
client_id = vu.connect_to_vrep()
print(client_id)
res = vu.load_scene(client_id,
'/home/mohit/projects/cooking/code/sim_vrep/scene_0.ttt')
print(res)
if res != vrep.simx_return_ok:
print("Error in loading scene: {}".format(res))
vu.start_sim(client_id)
cam_rgb_names = ["Camera"]
cam_rgb_handles = [
vrep.simxGetObjectHandle(client_id, n, vrep.simx_opmode_blocking)[1]
for n in cam_rgb_names
]
print("Camera handle: {}".format(cam_rgb_handles))
octree_handle = vu.get_handle_by_name(client_id, 'Octree')
print('Octree handle: {}'.format(octree_handle))
voxel_list = []
for t in range(100):
def main():
dimensions = np.load('dimensions.npy')
joint_to_center = np.load('joint_to_center_offset.npy')
#obstacle_dimensions = np.load('obstacle_dimensions.npy')
#obstacle_orientations = np.load('obstacle_orientations.npy')
#obstacle_origins = np.load('obstacle_origins.npy')
obstacle_dimensions = np.array((0.072, 0.098, 0.198))
obstacle_orientations = np.array((0, 0, 0))
obstacle_origins = np.array((0.2, 0.2, 0.09))
#Instantiate cuboid object for obstacle cuboids from CollisionDetection
obstacle = []
for i in range(6):
obstacle.append(
CollisionDetection.Cuboid(obstacle_origins[i],
obstacle_orientations[i],
obstacle_dimensions[i]))
no_vertices = 50 #No. of vertices to add to graph during training phase
n = 0
vertices = []
deg_to_rad = np.pi / 180.
while (n < no_vertices):
pi = np.pi
joint_targets = [
np.random.uniform(low=-pi / 2,
high=pi / 2), #random sampling joint angles
np.random.uniform(low=-pi / 2, high=pi / 2),
np.random.uniform(low=-pi / 2, high=pi / 2),
np.random.uniform(low=-pi / 2, high=pi / 2),
np.random.uniform(low=-pi / 2, high=pi / 2),
-0.03,
0.03
]
collision = vertice_collision_checker(joint_targets, obstacle,
joint_to_center, dimensions)
#add vertices if no collision
if collision == False:
vertices.append(joint_targets)
n += 1
start = [-80 * deg_to_rad, 0, 0, 0, 0, -0.03, 0.03]
goal = [
0, 60 * deg_to_rad, -75 * deg_to_rad, -75 * deg_to_rad, 0, -0.03, 0.03
]
#start = [0,0,0,0,0,-0.03,0.03]
#goal = [pi/2,0,0,0,0,-0.03,0.03]
vertices.append(start)
vertices.append(goal)
vertices = np.array(vertices)
#Find edges that are not in collision
k = 5 #No. of Nearest Neighbours
p = 10 #No. of points sampled between the edges
edges = []
for i in range(vertices.shape[0]):
dist = np.linalg.norm(
vertices[i] - vertices,
axis=1) #euclidean distance between one and all vertices
sorted_dist = np.argsort(dist)
#To find the k-NN and if the path between is not in collision add the edge
for j in range(k):
idx = sorted_dist[j + 1]
edge_start = vertices[i]
edge_end = vertices[idx]
sample_points = np.linspace(edge_start, edge_end, num=p)
for p in range(
sample_points.shape[0]
): #check if each path is in collision with obstacles
collision = vertice_collision_checker(sample_points[p],
obstacle,
joint_to_center,
dimensions)
if collision == True:
break
if collision == False:
edges.extend((edge_start, edge_end))
edges = np.array(edges)
graph = defaultdict(list)
start = np.array(start)
goal = np.array(goal)
for i in range(0, edges.shape[0], 2):
j = i + 1
x = np.argwhere([np.all((edges[i] - vertices) == 0, axis=1)])[0][1]
y = np.argwhere([np.all((edges[j] - vertices) == 0, axis=1)])[0][1]
graph[x].append(y)
planner_positions = BFS(graph, vertices, start, goal)
print("Path found:\n")
print(planner_positions)
print('Connecting to V-REP...')
clientID = vu.connect_to_vrep()
print('Connected.')
# Reset simulation in case something was running
vu.reset_sim(clientID)
# Initial control inputs are zero
vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS))
# Despite the name, this sets the maximum allowable joint force
vu.set_arm_joint_forces(clientID, 50. * np.ones(vu.N_ARM_JOINTS))
# One step to process the above settings
vu.step_sim(clientID)
controller = locobot_joint_ctrl.ArmController()
i = 0
for target in planner_positions:
# Set new target position
controller.set_target_joint_positions(target)
steady_state_reached = False
while not steady_state_reached:
timestamp = vu.get_sim_time_seconds(clientID)
#print('Simulation time: {} sec'.format(timestamp))
# Get current joint positions
sensed_joint_positions = vu.get_arm_joint_positions(clientID)
# Calculate commands
commands = controller.calculate_commands_from_feedback(
timestamp, sensed_joint_positions)
# Send commands to V-REP
vu.set_arm_joint_target_velocities(clientID, commands)
# Print current joint positions (comment out if you'd like)
#print(sensed_joint_positions)
vu.step_sim(clientID, 1)
# Determine if we've met the condition to move on to the next point
steady_state_reached = controller.has_stably_converged_to_target()
i += 1
print(i)
vu.stop_sim(clientID)