def setup(self):
# Get initial joint positions and link cuboids from VREP
joint_angles = vu.get_arm_joint_positions(self.clientID)
joint_poses = utils.get_joint_pose_vrep(self.clientID)
self.fksolver.compute(self.link_cuboids,
joint_angles,
joint_poses,
setup=True)
# Simulation no longer required for planning
vu.stop_sim(self.clientID)
def main():
success = False
try:
# Setup and reset simulator
clientID = setup_simulator()
# Joint targets, radians for revolute joints and meters for prismatic joints
joint_targets = np.radians([[-80, 0, 0, 0, 0], [0, 60, -75, -75, 0]])
gripper_targets = np.asarray([[-0.03, 0.03], [-0.03, 0.03]])
# Initiate PRM planner
prm_planner = ProbabilisticRoadMap(
clientID,
joint_targets,
gripper_targets,
urdf='urdf/locobot_description_v3.urdf',
load_graph=True)
# Run planner
ret, joint_plan, gripper_plan = prm_planner.plan(N=100,
K=10,
verbose=False)
# Planning successful
if ret:
# Setup and reset simulator
clientID = setup_simulator()
# Instantiate controller and execute the planned trajectory
controller = ArmController(clientID)
controller.execute(joint_plan, gripper_plan)
success = True
except Exception as e:
print(e)
traceback.print_exc()
finally:
# Stop VREP simulation
vu.stop_sim(clientID)
# Plot time histories
if success: controller.plot(savefile=True)
def stop(self):
vu.stop_sim(self.client_id)
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):
vu.step_sim(client_id)
res = \
vrep.simxCallScriptFunction(
client_id,
"Octree",
vrep.sim_scripttype_childscript,
"ms_GetOctreeVoxels",
[int(octree_handle)],
[],
[],
bytearray(),
vrep.simx_opmode_blocking,
)
voxels = res[2]
voxel_list.append(np.array(voxels))
if t == 50:
print("diff: {}".format(np.sum(np.abs(voxel_list[-1] -
voxel_list[5]))))
# removeVoxelsFromOctree(handle, 0, NULL, 0, NULL) # Removes all voxels
# simInsertObjectIntoOctree(handle, obj_handle, 0, <color>, int_tag, NULL)
vu.stop_sim(client_id)
vrep.simxFinish(client_id)
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)