def setJointPositionNonBlock(self, clientID, joint, angle):
handle = self.jointHandleMapping[joint]
error = False
if (handle == 0):
error, handle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot)
self.jointHandleMapping[joint]=handle
vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_streaming)
def setJointPosition(clientID, joint, angle):
if (jointHandleMapping[joint] > 0):
jhandle=jointHandleMapping[joint]
else:
error, jhandle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot_wait)
jointHandleMapping[joint]=jhandle
vrep.simxSetJointPosition(clientID,jhandle,angle,vrep.simx_opmode_oneshot_wait)
def setJointPosition(self, clientID, joint, angle):
error = False
handle = self.jointHandleMapping[joint]
if (handle == 0):
errorGetObjetHandle, handle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot_wait)
error = errorGetObjetHandle or error
self.jointHandleMapping[joint]=handle
error = error or vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_oneshot_wait)
return error
def setJointPosition(self, clientID, joint, angle):
error = False
handle = self.jointHandleMapping[joint]
if handle == 0:
errorGetObjetHandle, handle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot_wait)
error = errorGetObjetHandle or error
self.jointHandleMapping[joint]=handle
#error = error or vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_oneshot_wait)
error = error or vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_oneshot) #using oneshot instead of oneshot_wait make it a nonblocking call but is the only way to avoid 5 seconds wait in the execution of the individual
if self.synchronous:
vrep.simxSynchronousTrigger(clientID)
return error
# cube_y_pos = np.random.choice([-1.0, 1.0]) * np.random.uniform(0.1, 0.4)
cube_y_pos = np.random.uniform(0.1, 0.5)
cube_z_pos = init_cube_pos[2]
vrep.simxSetObjectPosition(clientID, cube, -1,
[cube_x_pos, cube_y_pos, cube_z_pos],
vrep.simx_opmode_oneshot)
# Reset joint pos
# jh[0]/joint1, range: [-1/2*pi, 1/2*pi]
# jh[1]/joint2, range: [-3/4*pi, 0]
# jh[2]/joint3, range: [-pi, 0]
joint1_pos = np.random.uniform(0.25 * -np.pi, 0.25 * np.pi)
joint2_pos = np.random.uniform(0.5 * -np.pi, 0)
joint3_pos = np.random.uniform(0.5 * -np.pi, 0)
current_config = [joint1_pos, joint2_pos, joint3_pos]
vrep.simxSetJointPosition(clientID, joint_handles[0], joint1_pos,
vrep.simx_opmode_oneshot)
vrep.simxSetJointPosition(clientID, joint_handles[1], joint2_pos,
vrep.simx_opmode_oneshot)
vrep.simxSetJointPosition(clientID, joint_handles[2], joint3_pos,
vrep.simx_opmode_oneshot)
if path_type == 'ik':
err, ikpath = generate_ikpath()
if err != 0:
err_count += 1
print("Non successful pathfinding, redoing epoch...")
else:
success_count += 1
follow_path(ikpath, joint_handles, vs_diag, distance_handle, textfile)
elif path_type == 'interpolate':
end_config = get_end_config()
joint3_grid = np.linspace(-0.75 * np.pi, 0, grid_range)
epoch_counter = 1
epochs = grid_range**5
for cx in cube_x_grid:
for cy in cube_y_grid:
for j1 in joint1_grid:
for j2 in joint2_grid:
for j3 in joint3_grid:
print("Epoch {}/{}".format(epoch_counter, epochs))
epoch_counter += 1
vrep.simxSetObjectPosition(
clientID, cube, -1, [cx, cy, cz],
vrep.simx_opmode_oneshot)
vrep.simxSetJointPosition(clientID,
joint_handles[0], j1,
vrep.simx_opmode_oneshot)
vrep.simxSetJointPosition(clientID,
joint_handles[1], j2,
vrep.simx_opmode_oneshot)
vrep.simxSetJointPosition(clientID,
joint_handles[2], j3,
vrep.simx_opmode_oneshot)
err, ikpath = generate_ikpath()
if err != 0:
err_count += 1
errorfile.write(
str(cx) + " " + str(cy) + " " + str(cz) +
" " + str(j1) + " " + str(j2) + " " +
str(j3) + "\n")
else:
print counter
# move the robot somehow
vrep.simxSetJointTargetVelocity(clientID,frontleftMotorHandle,0.5,vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(clientID,rearleftMotorHandle,0.5,vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(clientID,rearrightMotorHandle,-0.5,vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(clientID,frontrightMotorHandle,-0.5,vrep.simx_opmode_oneshot)
# generate random target positions for the arm
rand_base_rotate = numpy.random.uniform(-40,40)*math.pi/180
rand_base_lower = numpy.random.uniform(15,35) *math.pi/180
rand_elbow_bend = numpy.random.uniform(0,50) *math.pi/180
rand_hand_bend = numpy.random.uniform(10,90) *math.pi/180
# instruct the arm to move (all joints at once)
vrep.simxPauseCommunication(clientID,True)
vrep.simxSetJointPosition(clientID,armJoint0Handle, rand_base_rotate,vrep.simx_opmode_streaming) # base - rotates the entire arm - [-40..40]
vrep.simxSetJointPosition(clientID,armJoint1Handle, rand_base_lower ,vrep.simx_opmode_streaming) # base - lowers the arm - [5..35]
vrep.simxSetJointPosition(clientID,armJoint2Handle, rand_elbow_bend ,vrep.simx_opmode_streaming) # elbow - bends the arm - [0..50]
vrep.simxSetJointPosition(clientID,armJoint3Handle, rand_hand_bend ,vrep.simx_opmode_streaming) # hand - bends the lower part of the arm - [0..90]
vrep.simxSetJointPosition(clientID,armJoint4Handle, 0.0,vrep.simx_opmode_streaming) # wrist - turns the gripper
vrep.simxPauseCommunication(clientID,False)
# give the arm some time to move
time.sleep(1)
# verify that the arm has approximately reached its target position
err0 = vrep.simxGetJointPosition(clientID,armJoint0Handle,vrep.simx_opmode_streaming)[1] - rand_base_rotate
err1 = vrep.simxGetJointPosition(clientID,armJoint1Handle,vrep.simx_opmode_streaming)[1] - rand_base_lower
err2 = vrep.simxGetJointPosition(clientID,armJoint2Handle,vrep.simx_opmode_streaming)[1] - rand_elbow_bend
err3 = vrep.simxGetJointPosition(clientID,armJoint3Handle,vrep.simx_opmode_streaming)[1] - rand_hand_bend
err4 = vrep.simxGetJointPosition(clientID,armJoint4Handle,vrep.simx_opmode_streaming)[1] - 0.0
def set_angles(self, angles):
for q, handle in zip(angles, self._joint_handles):
vrep.simxSetJointPosition(self._client_id, handle, q,
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self._client_id)
def render(self,angles):
time.sleep(0.0001)
for i in range(len(self.jointHandles)):
vrep.simxSetJointPosition(self.clientID, self.jointHandles[i], angles[i], vrep.simx_opmode_oneshot)
def train(load_model_path, save_model_path, number_training_steps,
data_path, image_shape, ratio=1, batch_size=2,
save_online_batch=False, save_online_batch_path=None):
# Initialize connection
connection = VrepConnection()
connection.synchronous_mode()
connection.start()
# Load data
data_file = h5py.File(data_path,"r")
x = h5py_to_array(data_file['images'][:1000], image_shape)
y = data_file['joint_vel'][:1000]
datapoints = x.shape[0]
# Initialize ratios
online_batchsize = int(np.floor(1.0 * batch_size/(ratio + 1)))
data_images_batchsize = int(batch_size - online_batchsize)
current_online_batch = 0
x_batch = np.empty((batch_size,) + image_shape)
y_batch = np.empty((batch_size, 6))
jointpos_array = np.empty((batch_size, 6))
nextpos_array = np.empty((batch_size, 6))
print "Batch size: ", batch_size
print "Online batch size: ", online_batchsize
print "Dataset batch size: ", data_images_batchsize
# Load keras model
model = load_model(load_model_path)
# Use client id from connection
clientID = connection.clientID
# Get joint handles
jhList = [-1, -1, -1, -1, -1, -1]
for i in range(6):
err, jh = vrep.simxGetObjectHandle(clientID, "Mico_joint" + str(i + 1), vrep.simx_opmode_blocking)
jhList[i] = jh
# Initialize joint position
jointpos = np.zeros(6)
for i in range(6):
err, jp = vrep.simxGetJointPosition(clientID, jhList[i], vrep.simx_opmode_streaming)
jointpos[i] = jp
# Initialize vision sensor
res, v1 = vrep.simxGetObjectHandle(clientID, "vs1", vrep.simx_opmode_oneshot_wait)
err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_streaming)
vrep.simxGetPingTime(clientID)
err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer)
# Initialize distance handle
err, distanceHandle = vrep.simxGetDistanceHandle(clientID, "tipToTarget", vrep.simx_opmode_blocking)
err, distance_to_target = vrep.simxReadDistance(clientID, distanceHandle, vrep.simx_opmode_streaming)
# Initialize online batch hdf5 file
if save_online_batch:
f = h5py.File(save_online_batch_path, "w")
dset1 = f.create_dataset("images", (batch_size,) + image_shape, dtype=np.float32)
dset2 = f.create_dataset("joint_pos", (batch_size, 6), dtype=np.float32)
dset3 = f.create_dataset("next_pos", (batch_size, 6), dtype=np.float32)
dset4 = f.create_dataset("distance", (batch_size, 1), dtype=np.float32)
dset5 = f.create_dataset("joint_vel", (batch_size, 6), dtype=np.float32)
# Step while IK movement has not begun
returnCode, signalValue = vrep.simxGetIntegerSignal(clientID, "ikstart", vrep.simx_opmode_streaming)
while (signalValue == 0):
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
returnCode, signalValue = vrep.simxGetIntegerSignal(clientID, "ikstart", vrep.simx_opmode_streaming)
# Iterate over number of steps to train model online
path_empty_counter = 0
step_counter = 0
while step_counter < number_training_steps:
# 1. Obtain image from vision sensor and next path position from Lua script
err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer)
img = np.resize(image, (1,) + image_shape) # resize into proper shape for input to neural network
img = img.astype(np.uint8)
img = img.astype(np.float32)
img /= 255
inputInts = []
inputFloats = []
inputStrings = []
inputBuffer = bytearray()
err, _, next_pos, _, _ = vrep.simxCallScriptFunction(clientID, 'Mico',
vrep.sim_scripttype_childscript,
'getNextPathPos', inputInts,
inputFloats, inputStrings,
inputBuffer,
vrep.simx_opmode_blocking)
# 2. Pass into neural network to get joint velocities
jointvel = model.predict(img, batch_size=1)[0] # output is a 2D array of 1X6, access the first variable to get vector
stepsize = 1
jointvel *= stepsize
# 3. Apply joint velocities to arm in V-REP
for j in range(6):
err, jp = vrep.simxGetJointPosition(clientID, jhList[j], vrep.simx_opmode_buffer)
jointpos[j] = jp
err = vrep.simxSetJointPosition(clientID, jhList[j], jointpos[j] + jointvel[j], vrep.simx_opmode_oneshot)
err, distance_to_target = vrep.simxReadDistance(clientID, distanceHandle, vrep.simx_opmode_buffer)
# Check if next pos is valid before fitting
if len(next_pos) != 6:
path_empty_counter += 1
continue
ik_jointvel = joint_difference(jointpos, next_pos)
ik_jointvel = ik_jointvel / np.sum(np.absolute(ik_jointvel)) * 0.5 * distance_to_target
ik_jointvel = np.resize(ik_jointvel, (1, 6))
x_batch[current_online_batch] = img[0]
y_batch[current_online_batch] = ik_jointvel[0]
jointpos_array[current_online_batch] = jointpos
nextpos_array[current_online_batch] = next_pos
dset4[current_online_batch] = distance_to_target
current_online_batch += 1
if current_online_batch == online_batchsize:
# Step counter
print "Training step: ", step_counter
step_counter += 1
# Random sample from dataset
if ratio > 0:
indices = random.sample(range(int(datapoints)), int(data_images_batchsize))
indices = sorted(indices)
x_batch[online_batchsize:] = x[indices]
y_batch[online_batchsize:] = y[indices]
dset4[online_batchsize:] = data_file["distance"][indices]
dset2[online_batchsize:] = data_file["joint_pos"][indices]
# 4. Fit model
model.fit(x_batch, y_batch,
batch_size=batch_size,
epochs=1)
# Reset counter
current_online_batch = 0
# Save to online batch dataset
dset1[:] = x_batch
dset5[:] = y_batch
dset2[:online_batchsize] = jointpos_array[:online_batchsize]
dset3[:] = nextpos_array
# Print statements
# print "Predicted joint velocities: ", jointvel, "Abs sum: ", np.sum(np.absolute(jointvel))
# print "IK Joint velocity: ", ik_jointvel, "Abs sum: ", np.sum(np.absolute(ik_jointvel))
# print "Distance to cube: ", distanceToCube
# trigger next step and wait for communication time
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
# stop the simulation:
vrep.simxStopSimulation(clientID,vrep.simx_opmode_blocking)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
# save updated model and delete model
model.save(save_model_path)
del model
# Close file
if save_online_batch:
f.close()
# Error check
print "No of times path is empty:", path_empty_counter
def control(load_model_path, total_episodes=10, im_function=False):
# Initialize connection
connection = VrepConnection()
connection.synchronous_mode()
connection.start()
# Load keras model
model = load_model(load_model_path, custom_objects={'empty': empty,
'mmd2_rbf_quad': empty})
# Initialize adam optimizer
adam = keras.optimizers.adam(lr=0.001)
model.compile(loss='mean_squared_error',
optimizer=adam,
metrics=['mae'])
# Use client id from connection
clientID = connection.clientID
# Get joint handles
jhList = [-1, -1, -1, -1, -1, -1]
for i in range(6):
err, jh = vrep.simxGetObjectHandle(clientID, "Mico_joint" + str(i + 1), vrep.simx_opmode_blocking)
jhList[i] = jh
# Initialize joint position
jointpos = np.zeros(6)
for i in range(6):
err, jp = vrep.simxGetJointPosition(clientID, jhList[i], vrep.simx_opmode_streaming)
jointpos[i] = jp
# Initialize vision sensor
res, v1 = vrep.simxGetObjectHandle(clientID, "vs1", vrep.simx_opmode_oneshot_wait)
err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_streaming)
vrep.simxGetPingTime(clientID)
err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer)
# Initialize distance handle
err, distanceToCubeHandle = vrep.simxGetDistanceHandle(clientID, "tipToCube", vrep.simx_opmode_blocking)
err, distanceToCube = vrep.simxReadDistance(clientID, distanceToCubeHandle, vrep.simx_opmode_streaming)
err, distanceToTargetHandle = vrep.simxGetDistanceHandle(clientID, "tipToTarget", vrep.simx_opmode_blocking)
err, distanceToTarget = vrep.simxReadDistance(clientID, distanceToTargetHandle, vrep.simx_opmode_streaming)
# numpy print options
np.set_printoptions(precision=5)
# Step while IK movement has not begun
returnCode, signalValue = vrep.simxGetIntegerSignal(clientID, "ikstart", vrep.simx_opmode_streaming)
while (signalValue == 0):
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
returnCode, signalValue = vrep.simxGetIntegerSignal(clientID, "ikstart", vrep.simx_opmode_streaming)
# Iterate over total steps desired
image_shape = (128, 128, 3)
current_episode = 0
step_counter = 0
while current_episode < total_episodes + 1:
# obtain current episode
inputInts = []
inputFloats = []
inputStrings = []
inputBuffer = bytearray()
episode_table = []
while len(episode_table) == 0:
err, episode_table, _, _, _ = vrep.simxCallScriptFunction(clientID, 'Mico',
vrep.sim_scripttype_childscript,
'episodeCount', inputInts,
inputFloats, inputStrings,
inputBuffer, vrep.simx_opmode_blocking)
if episode_table[0] > current_episode:
step_counter = 0
print "Episode: ", episode_table[0]
current_episode = episode_table[0]
step_counter += 1
# 1. Obtain image from vision sensor
err, resolution, img = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer)
img = np.array(img, dtype=np.uint8)
img = np.resize(img, (1,) + image_shape) # resize into proper shape for input to neural network
img = img.astype('float32')
img = img / 255 # normalize input image
original_img = img
if im_function:
#img = adjust_gamma(img, gamma=2)
img = tint_images(img, [1,1,0])
if (current_episode==1) and (step_counter==1):
print "Displaying before and after image transformation"
display_2images(original_img[0], img[0])
plt.show()
# 2. Pass into neural network to get joint velocities
jointvel = model.predict(img, batch_size=1)[0][0] # output is a 2D array of 1X6, access the first variable to get vector
stepsize = 1
jointvel *= stepsize
# 3. Apply joint velocities to arm in V-REP
for j in range(6):
err, jp = vrep.simxGetJointPosition(clientID, jhList[j], vrep.simx_opmode_buffer)
jointpos[j] = jp
err = vrep.simxSetJointPosition(clientID, jhList[j], jointpos[j] + jointvel[j], vrep.simx_opmode_oneshot)
# Obtain distance
err, distanceToCube = vrep.simxReadDistance(clientID, distanceToCubeHandle, vrep.simx_opmode_buffer)
err, distanceToTarget = vrep.simxReadDistance(clientID, distanceToTargetHandle, vrep.simx_opmode_buffer)
# Print statements
# print "Step: ", step_counter
print "Joint velocities: ", jointvel, " Abs sum: %.3f" % np.sum(np.absolute(jointvel))
# print "Distance to cube: %.3f" % distanceToCube, ", Distance to target: %.3f" % distanceToTarget
# trigger next step and wait for communication time
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
# obtain performance metrics
inputInts = []
inputFloats = []
inputStrings = []
inputBuffer = bytearray()
err, minDistStep, minDist, _, _ = vrep.simxCallScriptFunction(clientID, 'Mico',
vrep.sim_scripttype_childscript,
'performanceMetrics', inputInts,
inputFloats, inputStrings,
inputBuffer, vrep.simx_opmode_blocking)
if res == vrep.simx_return_ok:
print "Min distance: ", minDist
minThreshold = 0.2
print "Success rate: ", 1.0*np.sum(np.array(minDist) <= minThreshold)/len(minDist)
print "Total episodes: ", len(minDist)
print "Average min distance: %.3f" % np.mean(minDist)
print "Standard deviation: %.3f" % np.std(minDist)
# other performance metrics such as success % can be defined (i.e. % reaching certain min threshold)
# stop the simulation:
vrep.simxStopSimulation(clientID,vrep.simx_opmode_blocking)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
# delete model
del model
return
-1,# Setting the absolute position
position=des_x,
operationMode=vrep.simx_opmode_blocking
)
"""
# Calculate desired endeffector pose
des_pose = np.concatenate((des_x, np.zeros((3, ))))
# set the joint angles position
J = np.reshape(retFloats, [6, 7])
J_pinv = np.linalg.pinv(J)
for joint_handle, des_joint_angle in zip(joint_handles, des_pose):
des_joint_angles = np.matmul(J_pinv, des_pose)
vrep.simxSetJointPosition(clientID, joint_handle,
des_joint_angle,
vrep.simx_opmode_blocking)
# move simulation ahead one time step
vrep.simxSynchronousTrigger(clientID)
else:
raise Exception('Failed connecting to remote API server')
finally:
# stop the simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# Before closing the connection to V-REP,
# make sure that the last command sent out had time to arrive.
vrep.simxGetPingTime(clientID)
def setJointPosition(self, jointID,
pos): #Set a certain joint to given position
vrep.simxSetJointPosition(clientID, jointID, pos,
vrep.simx_opmode_streaming)
def start_simulation(self):
# start our simulation in lockstep with our code
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
vrep.simxSetJointPosition(self.clientID, self.joint_handles[1], 1.57,
vrep.simx_opmode_oneshot)
ec1, motor1_handle = vrep.simxGetObjectHandle(clientID,
'PhantomXPincher_joint1',
vrep.simx_opmode_oneshot_wait)
ec2, motor2_handle = vrep.simxGetObjectHandle(clientID,
'PhantomXPincher_joint2',
vrep.simx_opmode_oneshot_wait)
ec3, motor3_handle = vrep.simxGetObjectHandle(clientID,
'PhantomXPincher_joint3',
vrep.simx_opmode_oneshot_wait)
ec4, motor4_handle = vrep.simxGetObjectHandle(clientID,
'PhantomXPincher_joint4',
vrep.simx_opmode_oneshot_wait)
ec5, auxMotor2_handle = vrep.simxGetObjectHandle(clientID, 'uarm_auxMotor2',
vrep.simx_opmode_oneshot_wait)
ec6, gripperMotor2_handle = vrep.simxGetObjectHandle(
clientID, 'uarmGripper_motor2Method2', vrep.simx_opmode_oneshot_wait)
#vrep.simxSetJointTargetPosition(clientID, motor1_handle, pi, vrep.simx_opmode_oneshot) #rotates motor1, takes radians, default is at pi/2
#vrep.simxSetJointTargetPosition(clientID, motor2_handle, 1, vrep.simx_opmode_blocking)# motor2 and 3 work simultaneously
#vrep.simxSetJointPosition(clientID, motor2_handle, 0.5, vrep.simx_opmode_oneshot)
vrep.simxSetJointPosition(clientID, motor3_handle, 1, vrep.simx_opmode_oneshot)
"""
#If not sure about commands being received, add a 'pioneer p3dx' to the scene and run this.
_, left_motor_handle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(clientID, left_motor_handle, 0.2, vrep.simx_opmode_streaming)
"""
time.sleep(1)
vrep.simxFinish(clientID)
def evaluate_individual(weightMatrix):
print('Evaluating individual...')
if clientID == -1:
# This means that the instance was not initialised!
raise ValueError("The V-REP instance was not initialised!")
else:
deltaTime = 0.01
# Shoulders should point downwards by default!
jointIndices = [15, 13, 11, 12, 14, 16, 1, 2]
jointOffsets = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -math.radians(90.0),
math.radians(90.0)
]
# Get handles
_, pivotHandle = vrep.simxGetObjectHandle(
clientID, 'Pivot', vrep.simx_opmode_oneshot_wait)
_, shoulderRightHandle = vrep.simxGetObjectHandle(
clientID, 'Joint3', vrep.simx_opmode_oneshot_wait)
_, shoulderLeftHandle = vrep.simxGetObjectHandle(
clientID, 'Joint4', vrep.simx_opmode_oneshot_wait)
_, hipRightHandle = vrep.simxGetObjectHandle(
clientID, 'Joint7', vrep.simx_opmode_oneshot_wait)
_, hipLeftHandle = vrep.simxGetObjectHandle(
clientID, 'Joint8', vrep.simx_opmode_oneshot_wait)
jointHandles = []
for i in jointIndices:
_, h = vrep.simxGetObjectHandle(clientID, 'Joint' + str(i),
vrep.simx_opmode_oneshot_wait)
jointHandles.append(h)
print('Object handles received.')
# Get bioloid handle
_, torsoHandle = vrep.simxGetObjectHandle(
clientID, 'Bioloid', vrep.simx_opmode_oneshot_wait)
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
vrep.simxSynchronous(clientID, True)
print('Sim started.')
# Speed up simulation
vrep.simxSetIntegerParameter(clientID, vrep.sim_intparam_speedmodifier,
10, vrep.simx_opmode_oneshot_wait)
# Set initial joint angles
for i in range(0, len(jointHandles)):
vrep.simxSetJointPosition(clientID, jointHandles[i],
jointOffsets[i],
vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetPosition(clientID, jointHandles[i],
jointOffsets[i],
vrep.simx_opmode_oneshot_wait)
# Set joints that are not considered by the optimizaton
shoulderAngle = math.radians(80.0)
vrep.simxSetJointPosition(clientID, shoulderRightHandle,
-shoulderAngle,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetPosition(clientID, shoulderRightHandle,
-shoulderAngle,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointPosition(clientID, shoulderLeftHandle, shoulderAngle,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetPosition(clientID, shoulderLeftHandle,
shoulderAngle,
vrep.simx_opmode_oneshot_wait)
hipAngle = math.radians(45.0)
vrep.simxSetJointPosition(clientID, hipRightHandle, -hipAngle,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetPosition(clientID, hipRightHandle, -hipAngle,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointPosition(clientID, hipLeftHandle, hipAngle,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetPosition(clientID, hipLeftHandle, hipAngle,
vrep.simx_opmode_oneshot_wait)
# Evaluate for a number of integration steps
_, lastPos = vrep.simxGetObjectPosition(clientID, torsoHandle, -1,
vrep.simx_opmode_oneshot_wait)
position = 0
maxPosition = 0
lowestPos = 5 #starting way above
cumZPos = 0
furthestDistanceIteration = 0
bn = cpg.bioloid_network.BioloidNetwork(weightMatrix, deltaTime)
nrIterations = 2000
for iteration in range(0, nrIterations):
# Print progress
if iteration % 100 == 0:
print("Evaluation iteration: " + str(iteration) + " / " +
str(nrIterations))
# CPG network step
jointAngles = bn.get_output()
# Set joint angles
for i in range(0, len(jointHandles)):
vrep.simxSetJointTargetPosition(
clientID, jointHandles[i],
jointOffsets[i] + jointAngles[i], vrep.simx_opmode_oneshot)
# VREP simulation step
vrep.simxSynchronousTrigger(clientID)
# Measure how far forward the robot moves in this timestep
if iteration % 10 == 0:
# Measure delta
_, currPos = vrep.simxGetObjectPosition(
clientID, torsoHandle, -1, vrep.simx_opmode_oneshot_wait)
delta2D = [currPos[0] - lastPos[0], currPos[1] - lastPos[1]]
lastPos = currPos
# Measure forward vector
_, currLocalPos = vrep.simxGetObjectPosition(
clientID, torsoHandle, pivotHandle,
vrep.simx_opmode_oneshot_wait)
forward2D = [currLocalPos[1], -currLocalPos[0]]
forward2DLength = math.sqrt(forward2D[0] * forward2D[0] +
forward2D[1] * forward2D[1])
if forward2DLength == 0.0:
forward2D = [0.0, 0.0]
else:
forward2D = [
forward2D[0] / forward2DLength,
forward2D[1] / forward2DLength
]
# What is the distance along the forward vector?
forwardDistance = delta2D[0] * forward2D[0] + delta2D[
1] * forward2D[1]
position += forwardDistance
if position > maxPosition:
# save new best furthestDistanceIteration
furthestDistanceIteration = iteration
maxPosition = max(position, maxPosition)
lowestPos = min(lowestPos, currPos[2])
cumZPos += currPos[2]
if math.isnan(position):
print('ROBOT POSITION IS NAN! Returning zero fitness.')
break
if currPos[2] < 0.1:
print('ROBOT HAS FALLEN! Stopping evaluation.')
break
if iteration - furthestDistanceIteration > 300:
# if we haven't improved the max distance for in 300 iterations, break
print('ROBOT IS NOT MOVING FORWARD! Stopping evaluation.')
break
# Measure movement
print('Last known position: ' + str(position))
print('Max position: ' + str(maxPosition))
print('Lowest position: ' + str(lowestPos))
# Stop simulation
vrep.simxSynchronous(clientID, False)
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
print('Sim stopped.')
print('Done.')
fitness = maxPosition * lowestPos
#fitness = cumZPos
#fitness = maxPosition
# Only return valid fitness values
if math.isnan(fitness) or fitness >= 1000.0 or math.isnan(position):
fitness = 0.0
return fitness
def set_joints_position(self, client_id, positions):
for handle, pos in zip(self.handlesList, positions):
vrep.simxSetJointPosition(client_id, handle, pos, self.opmode)
if clientID != -1:
print("Connection Established!")
else:
sys.exit("Error: No connection could be established")
# gathers object handles
_, J1_handle = vrep.simxGetObjectHandle(clientID, 'J1', vrep.simx_opmode_oneshot_wait)
_, J2_handle = vrep.simxGetObjectHandle(clientID, 'J2', vrep.simx_opmode_oneshot_wait)
_, J3_handle = vrep.simxGetObjectHandle(clientID, 'J3', vrep.simx_opmode_oneshot_wait)
_, J4_handle = vrep.simxGetObjectHandle(clientID, 'J4', vrep.simx_opmode_oneshot_wait)
_, cam_handle = vrep.simxGetObjectHandle(clientID, 'Vision1', vrep.simx_opmode_oneshot_wait)
while(1):
# randomly chooses joint movement
vrep.simxSetJointPosition(clientID, J1_handle, math.radians(random.randint(0, 360)), vrep.simx_opmode_oneshot)
vrep.simxSetJointPosition(clientID, J2_handle, math.radians(random.randint(-65, 65)), vrep.simx_opmode_oneshot)
vrep.simxSetJointPosition(clientID, J3_handle, math.radians(random.randint(0, 360)), vrep.simx_opmode_oneshot)
vrep.simxSetJointPosition(clientID, J4_handle, math.radians(random.randint(0, 360)), vrep.simx_opmode_oneshot)
_, resolution, image = vrep.simxGetVisionSensorImage(clientID, cam_handle, 0, vrep.simx_opmode_streaming)
time.sleep(1)
# saves camera frame, rotates image and converts to BGR
_, resolution, image =vrep.simxGetVisionSensorImage(clientID,cam_handle,0, vrep.simx_opmode_buffer)
img = np.array(image, dtype=np.uint8)
img.resize([resolution[0], resolution[1], 3])
img = np.rot90(img,2)
img = np.fliplr(img)
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
# converts img to hsv and detect colors
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
green_low = np.array([49, 50, 50], dtype=np.uint8)
def set_jj(jj):
for i in range(6):
err = vrep.simxSetJointPosition(clientID, jjh[i], jj[i],
vrep.simx_opmode_blocking)
import vrep
import numpy as np
print("start vrep successfully ")
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 500, 5)
if clientID != -1:
res, objs = vrep.simxGetObjects(clientID, vrep.sim_handle_all,
vrep.simx_opmode_buffer)
if res == vrep.simx_return_ok:
print('Number of objects in the scene: ', len(objs))
ret1, LBR4p_joint1 = vrep.simxGetObjectHandle(clientID,
'LBR4p_joint1#',
vrep.simx_opmode_buffer)
ret2 = vrep.simxSetJointPosition(clientID, LBR4p_joint1, .25,
vrep.simx_opmode_buffer)
else:
print('Remote API function call returned with error code: ', res)
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
# Mush the quaternion and pos into a pose matrix!
# Our function takes care of the V-REP quaternion mismatch
pose = quaternion.matrix_from_quaternion(dummy_quaternion)
pose[0, 3] = dummy_pos[0]
pose[1, 3] = dummy_pos[1]
pose[2, 3] = dummy_pos[2]
print("Pose is: \n{}".format(pose))
# Get the thetas required to move the robot to the desired position
theta_list = inverse_kinematics.inverse_kinematics(pose)
print("Thetas are: {}".format(theta_list))
if theta_list is not None:
print("Moving robot")
for i in range(7): # Set the position of each joint
vrep.simxSetJointPosition(clientID, joint_handles[i],
theta_list[i],
vrep.simx_opmode_oneshot_wait)
# print("Setting joint", i+1, "to", theta_list[i])
sleep(0.25)
# Grab the actual pose
errorCode, pos = vrep.simxGetObjectPosition(
clientID, joint_handles[6], joint_handles[0],
vrep.simx_opmode_oneshot_wait)
errorCode, angles = vrep.simxGetObjectQuaternion(
clientID, joint_handles[6], joint_handles[0],
vrep.simx_opmode_oneshot_wait)
print("Actual pos:", pos)
print("Actual qua:", angles)
# Print each joint's angle
ret,joint1_handler = vrep.simxGetObjectHandle(\ clientID,"redundantRob_joint1",vrep.simx_opmode_oneshot_wait) ret,joint1 = vrep.simxGetJointPosition(\ clientID,joint1_handler,vrep.simx_opmode_streaming) ret,joint2_handler = vrep.simxGetObjectHandle(\ clientID,"redundantRob_joint2",vrep.simx_opmode_oneshot_wait) ret,joint2 = vrep.simxGetJointPosition(\ clientID,joint2_handler,vrep.simx_opmode_streaming) startSim(clientID) ret,joint1 = vrep.simxGetJointPosition(\ clientID,joint1_handler,vrep.simx_opmode_buffer) print joint1 #Get position joint 1 ret,joint2 = vrep.simxGetJointPosition(\ clientID,joint2_handler,vrep.simx_opmode_buffer) print joint2 #Get position joint 2 ret = vrep.simxSetJointPosition(\ clientID,joint1_handler,pi/2,vrep.simx_opmode_oneshot) #Set pi/2 to joint 1 time.sleep(2) stopSim(clientID) time.sleep(1) disconnectVREP(clientID)
def set_complete_pose(self,joint_positions):
if(len(joint_positions) != self.NUM_JOINTS):
raise RuntimeError("error: vrep_robot joint command does not match with number of joints: NUM_JOINTS="+str(self.NUM_JOINTS)+", given length="+str(len(joint_positions)))
for i in range(self.NUM_JOINTS):
return_code = vrep.simxSetJointPosition(self.client_id,self.joint_handles[i],joint_positions[i],vrep.simx_opmode_blocking)
self.check_for_errors(return_code)
def test(self,jointNum,angle):
vrep.simxSetJointPosition(self.clientID,self.jointHandles[jointNum-1],angle,vrep.simx_opmode_oneshot)
def turn_joint(clientID, joint_handle, angle, joint_num, sleep_time):
time.sleep(sleep_time)
theta = get_joint_val(clientID, joint_handle)
vrep.simxSetJointPosition(clientID, joint_handle, theta + angle,
vrep.simx_opmode_oneshot)
time.sleep(sleep_time)
def move_lidar_motor(handle, part, locatie, clientID):
vrep.simxSetJointPosition(clientID, handle[part], locatie,
vrep.simx_opmode_oneshot)
def step(self, action):
# action = (node1 angular v, node2 angular v)
action = np.clip(action, *self.action_bound)
for i in range(7):
self.a[i] += action[i] * self.dt
self.a[i] %= np.pi
p = -np.pi / 2 + self.a[i]
vrep.simxSetJointPosition(self.ID, self.h[i], p,
vrep.simx_opmode_oneshot)
ec, res, im = vrep.simxGetVisionSensorImage(self.ID, self.cam, 0,
vrep.simx_opmode_streaming)
img1 = np.array(im, dtype=np.uint8)
img1.resize([128, 128, 3])
ec, res, im = vrep.simxGetVisionSensorImage(self.ID, self.cam1, 0,
vrep.simx_opmode_streaming)
img2 = np.array(im, dtype=np.uint8)
img2.resize([128, 128, 3])
ec, res, im = vrep.simxGetVisionSensorImage(self.ID, self.cam2, 0,
vrep.simx_opmode_streaming)
img3 = np.array(im, dtype=np.uint8)
img3.resize([128, 128, 3])
hsv = cv2.cvtColor(img1, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, self.gl, self.gu)
# mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
hsv = cv2.cvtColor(img2, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, self.gl, self.gu)
# mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
cnts1 = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
hsv = cv2.cvtColor(img3, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, self.gl, self.gu)
# mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
cnts2 = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
if len(cnts) == 2:
M = cv2.moments(cnts[0])
self.x1 = int(M["m10"] / M["m00"])
self.y1 = int(M["m01"] / M["m00"])
M = cv2.moments(cnts[1])
self.x2 = int(M["m10"] / M["m00"])
self.y2 = int(M["m01"] / M["m00"])
self.d1 = np.sqrt(
np.square(self.x1 - self.x2) + np.square(self.y1 - self.y2))
else:
self.d1 = self.od1
if len(cnts1) == 2:
M = cv2.moments(cnts1[0])
self.x3 = int(M["m10"] / M["m00"])
self.y3 = int(M["m01"] / M["m00"])
M = cv2.moments(cnts1[1])
self.x4 = int(M["m10"] / M["m00"])
self.y4 = int(M["m01"] / M["m00"])
self.d2 = np.sqrt(
np.square(self.x4 - self.x3) + np.square(self.y3 - self.y4))
else:
self.d2 = self.od2
if len(cnts2) == 2:
M = cv2.moments(cnts2[0])
self.x5 = int(M["m10"] / M["m00"])
self.y5 = int(M["m01"] / M["m00"])
M = cv2.moments(cnts2[1])
self.x6 = int(M["m10"] / M["m00"])
self.y6 = int(M["m01"] / M["m00"])
self.d3 = np.sqrt(
np.square(self.x6 - self.x5) + np.square(self.y6 - self.y5))
else:
self.d3 = self.od3
s = np.hstack([
self.a[0], self.a[1], self.a[2], self.a[3], self.a[4], self.a[5],
self.a[6], self.a[7], self.x1, self.y1, self.x2, self.y2, self.x3,
self.y3, self.x4, self.y4, self.x5, self.y5, self.x6, self.y6
])
r, self.done = self._r_func(self.d1, self.d2, self.d3)
return s, r, self.done, {}
def setPose(self, pose):
vrep.simxPauseCommunication(self.id, True)
for j, p in zip(self.joints, pose):
vrep.simxSetJointPosition(self.id, j, p, vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self.id, False)
def move_lidar_motor(part_name, location):
vrep.simxSetJointPosition(clientID, handle[part_name], location,
vrep.simx_opmode_oneshot)
assert client_id != -1, 'Failed connecting to remote API server'
e = vrep.simxStartSimulation(client_id, vrep.simx_opmode_oneshot_wait)
# print ping time
sec, msec = vrep.simxGetPingTime(client_id)
print "Ping time: %f" % (sec + msec / 1000.0)
# Handle of neck joint
e, neck = vrep.simxGetObjectHandle(client_id, 'Bill_neck', vrep.simx_opmode_oneshot_wait)
e, neck_pos = vrep.simxGetJointPosition(client_id, neck, vrep.simx_opmode_streaming)
print "Current position = %0.3f degrees" % neck_pos
e = vrep.simxSetJointPosition(client_id, neck, -60, vrep.simx_opmode_streaming)
e, neck_pos = vrep.simxGetJointPosition(client_id, neck, vrep.simx_opmode_streaming)
print "Current position = %0.3f degrees" % neck_pos
# Sleep for a small time
for i in range(0,1800):
#time.sleep(0.1)
print "setting current position as %d" % math.fmod(i,360)
e = vrep.simxSetJointPosition(client_id, neck, i/180. * math.pi, vrep.simx_opmode_oneshot_wait)
e, neck_pos = vrep.simxGetJointPosition(client_id, neck, vrep.simx_opmode_oneshot_wait)
print "Current position = %0.3f degrees" % (neck_pos/math.pi * 180)
def set_joint_angle(clientID, joint_handle, angle, joint_num, sleep_time):
time.sleep(sleep_time)
vrep.simxSetJointPosition(clientID, joint_handle, angle,
vrep.simx_opmode_oneshot)
time.sleep(sleep_time)
def setJointPosition(incAngle, steps):
for i in range(steps):
errorCode = vrep.simxSetJointPosition(clientID, Revolute_joint_handle,
i * incAngle * deg,
vrep.simx_opmode_oneshot_wait)
def set_motor_position(self, motor_handler, position):
"""Sets the motor position"""
#debut = time.time()
vrep.simxSetJointPosition(self.client_id, motor_handler, position, vrep.simx_opmode_oneshot_wait)
def show_entry_fields(self):
deg = math.pi/180
self.getNumber1 = self.entry1.get()
self.getNumber2 = self.entry2.get()
self.getNumber3 = self.entry3.get()
x = self.getNumber1
y = self.getNumber2
z = self.getNumber3
if x =='' or y==''or z =='':
x = 0
y = 0
z = 0
print("Error")
else:
x = float(self.getNumber1)
y = float(self.getNumber2)
z = float(self.getNumber3)
"""
if float(x) >= 125:
x = 125
print("X_axis is out of range")
if float(y) >= 125:
y = 125
print("Y_axis is out of range")
if float(z) >= 400:
z = 400
print("Z_axis is out of range")
if float(x) <= -125:
x = -125
print("X_axis is out of range")
if float(y) <= -125:
y = -125
print("Y_axis is out of range")
if float(z) < 0:
z = 0
print("Z_axis is out of range")
if (float(x) <= -125*math.sin(30*deg)) & (float(y) >= 125*math.cos(30*deg)):
x = -125*math.sin(30*deg)
y = 125*math.cos(30*deg)
if (float(x) <= -125*math.sin(30*deg)) & (float(y) <= -125*math.cos(30*deg)):
x = -125*math.sin(30*deg)
y = -125*math.cos(30*deg)
if (float(x) >= 125*math.sin(30*deg)) & (float(y) >= 125*math.cos(30*deg)):
x = 125*math.sin(30*deg)
y = 125*math.cos(30*deg)
if (float(x) >= -125*math.sin(30*deg)) & (float(y) <= -125*math.cos(30*deg)):
x = 125*math.sin(30*deg)
y = -125*math.cos(30*deg)
if float(z) >= 330:
z = 330
"""
e=x # v-rep world unit is meter ,
r=y/1000
t=z/1000
t = t+0.1165 # t = 0 , heater touch the heat bed ( heat bed is 0.1165 high )
if t <=0:
t = 0
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID!= -1:
print("Connected to remote server")
else:
print('Connection not successful')
# sys.exit('Could not connect')
errorCode,joint1=vrep.simxGetObjectHandle(clientID,'joint1',vrep.simx_opmode_oneshot_wait)
if errorCode == -1:
print('Can not find plate')
# sys.exit()
errorCode=vrep.simxSetJointPosition(clientID,joint1,x, vrep.simx_opmode_oneshot)
errorCode,joint2=vrep.simxGetObjectHandle(clientID,'joint2',vrep.simx_opmode_oneshot_wait)
if errorCode == -1:
print('Can not find plate')
# sys.exit()
errorCode=vrep.simxSetJointPosition(clientID,joint2,y, vrep.simx_opmode_oneshot)
errorCode,joint3=vrep.simxGetObjectHandle(clientID,'joint3',vrep.simx_opmode_oneshot_wait)
if errorCode == -1:
print('Can not find plate')
# sys.exit()
errorCode=vrep.simxSetJointPosition(clientID,joint3,z, vrep.simx_opmode_oneshot)
print(x,y,z)
errorCode,Revolute_joint_handle=vrep.simxGetObjectHandle(clientID,'Revolute_joint',vrep.simx_opmode_oneshot_wait)
errorCode0,Revolute_joint_handle0=vrep.simxGetObjectHandle(clientID,'Revolute_joint0',vrep.simx_opmode_oneshot_wait)
if errorCode == -1:
print('Can not find left or right motor')
sys.exit()
deg = math.pi/180
#vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
for i in range(36):
vrep.simxSynchronous(clientID,True)
vrep.simxPauseCommunication(clientID,True)
vrep.simxSetJointPosition(clientID, Revolute_joint_handle, 10*deg, vrep.simx_opmode_oneshot)
vrep.simxSetJointPosition(clientID, Revolute_joint_handle0, 10*deg, vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
vrep.simxSynchronous(clientID, False)
vrep.simxSynchronous(clientID,True)
vrep.simxPauseCommunication(clientID,True)
vrep.simxSetJointPosition(clientID, Revolute_joint_handle, -10*deg, vrep.simx_opmode_oneshot)
vrep.simxSetJointPosition(clientID, Revolute_joint_handle0, -10*deg, vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
vrep.simxSynchronous(clientID, False)
