def reinitialization_process(conf):
vrep.simxStopSimulation(conf['clientID'], vrep.simx_opmode_oneshot_wait)
time.sleep(1.)
vrep.simxSynchronous(conf['clientID'],True)
return_code = vrep.simxStartSimulation(conf['clientID'], vrep.simx_opmode_oneshot)# However, v-rep will do strange initialization.
while return_code != 0:
return_code = vrep.simxStartSimulation(conf['clientID'], vrep.simx_opmode_oneshot)
# vrep.simxSetStringSignal(clientID,'jacoHand','true',vrep.simx_opmode_oneshot)
# starting streaming mode
for i in range(conf['num_particles']):
starting_streaming_buffer_mode(conf['clientID'], conf['particle_handles'][i], 'simxGetObjectPosition')
starting_streaming_buffer_mode(conf['clientID'], conf['cup_handle'], 'simxGetObjectPosition')
starting_streaming_buffer_mode(conf['clientID'], conf['cup_handle'], 'simxGetObjectOrientation')
starting_streaming_buffer_mode(conf['clientID'], conf['end_effector_handle'], 'simxGetObjectOrientation')
starting_streaming_buffer_mode(conf['clientID'], conf['end_effector_handle'], 'simxGetObjectPosition')
# print('STREAMING_MODE FINISHED')
for i in range(len(conf['joint_handles'])):
returnCode = vrep.simxSetJointTargetPosition(conf['clientID'], conf['joint_handles'][i], conf['initial_joint_angles'][i], vrep.simx_opmode_streaming)
if returnCode != vrep.simx_return_ok:
print('Initialization of the joint angles fails.' + str(returnCode))
for i in range(round(0.03/conf['dt'])):
return_code = vrep.simxSynchronousTrigger(conf['clientID'])
# get cup position
returnCode, cup_position = vrep.simxGetObjectPosition(conf['clientID'], conf['cup_handle'], -1, vrep.simx_opmode_buffer)
if returnCode != 0:
print('Can not find Cup positions!')
# Distribute different positions to particles according to the position of liquid
for i in range(len(conf['particle_handles'])):
returnCode = vrep.simxSetObjectPosition(conf['clientID'], conf['particle_handles'][i], -1, np.array([cup_position[0],cup_position[1],cup_position[2] + 0.022* i]), vrep.simx_opmode_oneshot)
if returnCode != 0:
print('Can not set particle position! Return code: %i' %returnCode)
for i in range(round(0.7/conf['dt'])):
return_code = vrep.simxSynchronousTrigger(conf['clientID'])
def step_forward_move(self):
result = {}
if self.synchronous:
if self.moving_queue.empty():
result['flag'] = False
vrep.simxSynchronousTrigger(self.clientID)
# take photos if needed
if self.need_take_photo:
image_0 = self.getImage(0)
image_1 = self.getImage(1)
if image_0 is not None and image_1 is not None:
result['photos'] = [image_0, image_1]
self.need_take_photo = False
else:
speed = self.moving_queue.get()
self.move(self.target_orientation, speed)
self.left_time = self.moving_queue.qsize()
# reset cumul if needed
if self.has_reset_cumul:
vrep.simxSetIntegerSignal(self.clientID, 'clear', 1,
vrep.simx_opmode_oneshot)
self.has_reset_cumul = False
# take photos if needed
if self.need_take_photo:
image_0 = self.getImage(0)
image_1 = self.getImage(1)
if image_0 is not None and image_1 is not None:
result['photos'] = [image_0, image_1]
self.need_take_photo = False
result['flag'] = True
return result
else:
assert 0
def step(self, action):
# set actions
self.__MODE.set(action)
vrep.simxSynchronousTrigger(self.__ID)
vrep.simxGetPingTime(self.__ID)
# get new states
return self.__MODE.get()
def __init__(self):
self.portNumb = 19997
self.actions = []
self.score = 0
self.hasfallen = False
self.LUMSpeed = 0
self.LLMSpeed = 0
self.RUMSpeed = 0
self.RLMSpeed = 0
self.clientID = vrep.simxStart('127.0.0.1', self.portNumb, True, True,
5000, 5)
if self.clientID != -1:
print("Se pudo establecer la conexión con la api del simulador")
LUM, LLM, RUM, RLM, head = recoverRobotParts(self.clientID)
self.LUM = LUM
self.LLM = LLM
self.RUM = RUM
self.RLM = RLM
self.head = head
#Set simulation to be Synchonous instead of Asynchronous
vrep.simxSynchronous(self.clientID, True)
#Start simulation if it didn't start
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
setVelocity(self.clientID, self.LUM, self.LUMSpeed)
setVelocity(self.clientID, self.LLM, self.LLMSpeed)
setVelocity(self.clientID, self.RUM, self.RUMSpeed)
setVelocity(self.clientID, self.RLM, self.RLMSpeed)
#Esto es necesario porque la primera observación siempre es incorrecta
self.observation_space()
vrep.simxSynchronousTrigger(self.clientID)
def step(self, action): # execute one step in env
self._set_joint_effort(action)
# torque = [vrep.simxGetJointForce(self.clientID, self.handles['joint'][i], vrep.simx_opmode_blocking)[1]
# for i in range(self.action_dims)] # retrieve the current torque from 7 joints
# print('now=', torque, '\nnext=', action)
# send a synchronization trigger signal to inform V-REP to execute the next simulation step
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxGetPingTime(self.clientID)
Reward, ter_flag, env_info = self._reward(
action) # ter_flag: True if finish the task
# TODO: define the scaled reward
if self.t < self._max_episode_length * 0.8:
scaled_reward = (self.t / self._max_episode_length) * Reward
else:
scaled_reward = (self.final_phase_scaler * self.t /
self._max_episode_length) * Reward
self.t += 1
next_observation = self._get_observation()
self.cur_obs = next_observation
return next_observation, scaled_reward, ter_flag, env_info
def __init__(self, max_actions=600):
self.max_actions = max_actions
self.portNumb = recoverPorts()[0]
self.actions = []
self.score = 0
self.hasfallen = False
# self.LUMSpeed = 0
# self.LLMSpeed = 0
# self.RUMSpeed = 0
# self.RLMSpeed = 0
self.clientID = vrep.simxStart('127.0.0.1', self.portNumb, True, True, 5000, 5)
if self.clientID != -1 :
print ("Connection to the simulator has been established")
self.robotController = robotController(self.clientID)
# LUM,LLM,RUM,RLM,head = recoverRobotParts(self.clientID)
# self.LUM = LUM
# self.LLM = LLM
# self.RUM = RUM
# self.RLM = RLM
# self.head = head
#Set simulation to be Synchonous instead of Asynchronous
vrep.simxSynchronous(self.clientID, True)
#Start simulation if it didn't start
vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_blocking)
self.robotController.resetRobot()
# setVelocity(self.clientID,self.LUM,self.LUMSpeed)
# setVelocity(self.clientID,self.LLM,self.LLMSpeed)
# setVelocity(self.clientID,self.RUM,self.RUMSpeed)
# setVelocity(self.clientID,self.RLM,self.RLMSpeed)
#Esto es necesario porque la primera observación siempre es incorrecta
self.observation_space()
vrep.simxSynchronousTrigger(self.clientID)
def getRange(self):
"get sensor range"
eCode, value = vrep.simxGetFloatSignal(self.robot, "HOMEO_SIGNAL_"+self._eye+"Eye"+"_MAX_LIGHT",getattr(vrep,self._opMode))
vrep.simxSynchronousTrigger(self.robot)
if eCode != 0:
raise TransducerException("Cannot get maxLight of VREP sensor: " + self._eye+"Eye")
return value
def setAngle(self, angleToRotate):
if angleToRotate == 0.0:
return
speedRotation = 0.5
sign = angleToRotate / math.fabs(angleToRotate)
self.setLeftMotorVelocity(speedRotation * sign)
self.setRightMotorVelocity(-speedRotation * sign)
vrep.simxSynchronousTrigger(self.clientID)
previousAngle = self.getAngle()
rotation = 0
while math.fabs(rotation) <= math.fabs(angleToRotate):
angle = self.getAngle()
da = angle - previousAngle
if da > 0:
da = math.fmod(da + math.pi, 2 * math.pi) - math.pi
else:
da = math.fmod(da - math.pi, 2 * math.pi) + math.pi
rotation += da
previousAngle = angle
#self.printMessage(str(rotation) + ' ' + str(angleToRotate))
self.setLeftMotorVelocity(0)
self.setRightMotorVelocity(0)
vrep.simxSynchronousTrigger(self.clientID)
def control_routine(_setpoint, _time):
# Loop de Controle
pid.setpoint = _setpoint
start = millis()
while (millis() - start < _time):
# Realiza coleta do tempo atual (ms)
cur_time = millis()
# Realiza leitura do ângulo da junta e converte para graus, com precisão decimal de 2 casas
ret, ang = vrep.simxGetJointPosition(clientID, handler_junta,
vrep.simx_opmode_oneshot_wait)
ang = round(math.degrees(ang), 2)
# Atualiza PID com ângulo atual como entrada
output = pid(ang)
output = round(output, 2)
print("Output", output)
# Adiciona força à planta
add_force_and_torque(handler_helice, [0, 0, output])
# Adiciona valores para plot do gráfico
list_time.append(cur_time)
list_angle.append(ang)
list_setpoint.append(pid.setpoint)
# Aguarda pelo trigger de modo síncrono da simulação
vrep.simxSynchronousTrigger(clientID)
def simulate(self):
# vrep related
'''
cmd = input('Press <enter> key to step the simulation!')
if cmd == 'q': # quit
return False
'''
self.t += self.dt
self.ts.append(self.t)
self.propagateXid()
countReachedGoal = 0
for robot in self.robots:
robot.precompute()
for robot in self.robots:
robot.readSensorData()
robot.propagateDesired()
robot.propagate()
if robot.reachedGoal:
countReachedGoal += 1
self.calcCOG()
if self.vrepConnected:
vrep.simxSynchronousTrigger(self.clientID)
if countReachedGoal == len(self.robots):
return False
else:
return True
def run_simulation(self):
self.set_num_steps()
vrep.simxSynchronous(self._clientID,True);
vrep.simxStartSimulation(self._clientID,vrep.simx_opmode_oneshot);
for simStep in range (self._num_steps):
self._pid = set_target_thetas(self._num_steps, self._pid,self._experiment,self._simulator,simStep)
for joint in range(6):
errorCode, self._theta[joint] = vrep.simxGetJointPosition(self._clientID,self._arm_joints[joint],vrep.simx_opmode_blocking)
self._linearVelocity[joint] = self._pid[joint].get_velocity(math.degrees(self._theta[joint]))/self._convertdeg2rad
vrep.simxSetJointTargetVelocity(self._clientID,self._arm_joints[joint],self._linearVelocity[joint],vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(self._clientID)
vrep.simxGetPingTime(self._clientID)
self._positions.append(vrep.simxGetObjectPosition(self._clientID,self._arm_joints[5],-1,vrep.simx_opmode_blocking)[1] + vrep.simxGetObjectOrientation(self._clientID,self._arm_joints[5],-1,vrep.simx_opmode_blocking)[1] + vrep.simxGetObjectPosition(self._clientID,self._cube,-1,vrep.simx_opmode_blocking)[1] + vrep.simxGetObjectQuaternion(self._clientID,self._cube,-1,vrep.simx_opmode_blocking)[1])
vrep.simxStopSimulation(self._clientID, vrep.simx_opmode_blocking)
vrep.simxFinish(self._clientID)
saveStats(self._experiment,self._current_iteration, self._physics_engine,self._simulator, self._positions)
def restart(self,earlyStop = False):
if (self.cid != None):
vrep.simxStopSimulation(self.cid, self.mode())
vrep.simxSynchronousTrigger(self.cid)
vrep.simxFinish(-1) # just in case, close all opened connections
time.sleep(1)
self.connect()
time.sleep(1)
vrep.simxLoadScene(self.cid, '/home/elias/[email protected]/_Winter2015/CSC494/Scenes/Base_Quad.ttt', 0, self.mode())
if earlyStop:
vrep.simxStopSimulation(self.cid, self.mode())
vrep.simxSynchronousTrigger(self.cid)
vrep.simxFinish(-1) # just in case, close all opened connections
return
vrep.simxStartSimulation(self.cid, self.mode())
self.runtime = 0
err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base",
vrep.simx_opmode_oneshot_wait)
err, self.target = vrep.simxGetObjectHandle(self.cid, "Quadricopter_target",
vrep.simx_opmode_oneshot_wait)
err, self.front_camera = vrep.simxGetObjectHandle(self.cid, 'Quadricopter_frontCamera', vrep.simx_opmode_oneshot)
err, lin, ang = vrep.simxGetObjectVelocity(self.cid, self.copter, vrep.simx_opmode_streaming)
self.getTargetStart()
for i in range(4):
self.propellerScripts[i] = vrep.simxGetObjectHandle(self.cid,
'Quadricopter_propeller_respondable' + str(i) + str(1),
self.mode())
def step_simulation(self, torque_commands):
vrep.simxSetJointTargetVelocity(
self.clientID, self.jointHandles[0],
90000 if torque_commands[0] > 0 else -9000,
vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(
self.clientID, self.jointHandles[0], torque_commands[0]
if torque_commands[0] > 0 else -torque_commands[0],
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(
self.clientID, self.jointHandles[1],
90000 if torque_commands[1] > 0 else -9000,
vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(
self.clientID, self.jointHandles[1], torque_commands[1]
if torque_commands[1] > 0 else -torque_commands[1],
vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(
self.clientID
) # Trigger next simulation step (Blocking function call)
vrep.simxGetPingTime(
self.clientID
) # Ensure simulation step has completed (Blocking function call)
def _take_action(self, action):
if (abs(action[0]) > 1):
l = 1 if action[0] > 0 else -1
else:
l = action[0]
l = l/10
omega = action[1]
HALF_AXIS = 0.0325
WHEEL_R = 0.032/2
motorSpeed = [0,0]
#angularFreqL = l - HALF_AXIS * omega;
#angularFreqR = l + HALF_AXIS * omega;
#motorSpeedL = (angularFreqL / WHEEL_R);
#motorSpeedR = (angularFreqR / WHEEL_R);
angularFreqL = l - HALF_AXIS * omega;
angularFreqR = l + HALF_AXIS * omega;
motorSpeed[0] = (angularFreqL / WHEEL_R);
motorSpeed[1] = (angularFreqR / WHEEL_R);
handles = [self.act_handles[key] for key in sorted(self.act_handles.keys(), reverse=True)]
for idx,motor_handle in enumerate(handles):
vrep.simxSetJointTargetVelocity(self.clientID, motor_handle,
motorSpeed[idx], # target velocity
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.clientID)
def control_vrep(clientID, count):
# 读取Base 和 joint 的handle
jointHandle = np.zeros((jointNum, ), dtype=np.int)
for i in range(jointNum):
_, returnHandle = vrep.simxGetObjectHandle(clientID,
jointName + str(i + 1),
vrep.simx_opmode_blocking)
jointHandle[i] = returnHandle
_, baseHandle = vrep.simxGetObjectHandle(clientID, baseName,
vrep.simx_opmode_blocking)
print('Handles available')
# 首次读取关节的初始值，以streaming的形式
jointConfigure = np.zeros((jointNum, ))
for i in range(jointNum):
_, jpos = vrep.simxGetJointPosition(clientID, jointHandle[i],
vrep.simx_opmode_streaming)
jointConfigure[i] = jpos
vrep.simxSynchronousTrigger(clientID) # 让仿真走一步
### 控制指定关节 ####
vrep.simxPauseCommunication(clientID, True)
vrep.simxSetJointTargetPosition(clientID, jointHandle[count],
jointConfigure[count] + 1 / RAD2EDG,
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID) # 使得该仿真步
def cleanUp(self):
print "About to stop simulation connected to self.simulID: ", self.simulID
vrep.simxStopSimulation(self.simulID, vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(self.simulID)
vrep.simxFinish(self.simulID)
vrep.simxFinish(-1)
print "Disconnected from V-REP"
def shake_arm(self):
self.move_to_positions = []
for i in range(4):
self.move_to_positions.append([np.random.uniform(low=1.018, high=1.223, size=1)[0], \
np.random.uniform(low=0.951, high=1.294, size=1)[0], np.random.uniform(low=0.1, \
high=0.18, size=1)[0]])
for i in range(len(self.move_to_positions)):
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
move_position = self.move_to_positions[i]
move_direction = np.asarray([move_position[0] - sawyer_target_position[0], \
move_position[1] - sawyer_target_position[1], move_position[2] - \
sawyer_target_position[2]])
move_magnitude = np.linalg.norm(move_direction)
move_step = 0.01 * move_direction / move_magnitude
num_move_steps = int(np.floor(move_magnitude / 0.01))
for step_iter in range(num_move_steps):
vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, -1, \
(sawyer_target_position[0]+move_step[0], sawyer_target_position[1] + \
move_step[1], sawyer_target_position[2] + move_step[2]), \
vrep.simx_opmode_blocking)
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
_ = vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, -1, \
move_position, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def step(self, action):
# set actions
self.__set(action)
vrep.simxSynchronousTrigger(self.__ID)
# get new states
self.__get()
return (self.state, self.reward, self.done, {})
def set_gripper_position(self):
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.robot.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
self.image = self.robot.get_image()
move_direction = np.asarray([self.pickup_position[0] - sawyer_target_position[0], \
self.pickup_position[1] - sawyer_target_position[1], self.pickup_position[2] - \
sawyer_target_position[2]])
move_magnitude = np.linalg.norm(move_direction)
move_step = 0.03 * move_direction / move_magnitude
num_move_steps = int(np.floor(move_magnitude / 0.03))
remaining_magnitude = -num_move_steps * 0.03 + move_magnitude
remaining_distance = remaining_magnitude * move_direction / move_magnitude
for step_iter in range(
num_move_steps): #selects action and executes action
vrep.simxSetObjectPosition(self.client_id, self.robot.sawyer_target_handle, -1, \
(sawyer_target_position[0] + move_step[0], sawyer_target_position[1] + \
move_step[1], sawyer_target_position[2] + move_step[2]), vrep.simx_opmode_blocking)
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.robot.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
vrep.simxSetObjectPosition(self.robot.client_id, self.robot.sawyer_target_handle, -1, \
(sawyer_target_position[0] + remaining_distance[0], sawyer_target_position[1] + \
remaining_distance[1], sawyer_target_position[2]+ remaining_distance[2]), \
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def move(self, target_orientation, speed):
self.v_now = speed
speed = target_orientation * speed
for i in range(3):
self.controller_position[i] += speed[i]
self.setPosition('controller', self.controller_position)
vrep.simxSynchronousTrigger(self.clientID)
def send_gear_commands(clientID, target_degree1, target_degree2, gearHandle1,
gearHandle2):
RAD = 180 / math.pi
lastCmdTime = vrep.simxGetLastCmdTime(clientID)
vrep.simxSynchronousTrigger(clientID)
count = 0
while vrep.simxGetConnectionId(clientID) != -1:
count += 1
if count > 20:
break
currCmdTime = vrep.simxGetLastCmdTime(clientID)
dt = currCmdTime - lastCmdTime
ret, gear_pos1 = vrep.simxGetJointPosition(clientID, gearHandle1,
vrep.simx_opmode_buffer)
ret, gear_pos2 = vrep.simxGetJointPosition(clientID, gearHandle2,
vrep.simx_opmode_buffer)
degree1 = round(gear_pos1 * RAD, 2)
degree2 = round(gear_pos2 * RAD, 2)
print(degree1, degree2)
if abs(degree1 - target_degree1) < 0.5 and abs(degree2 -
target_degree2) < 0.5:
break
vrep.simxPauseCommunication(clientID, True)
vrep.simxSetJointTargetPosition(clientID, gearHandle1,
target_degree1 / RAD,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, gearHandle2,
target_degree2 / RAD,
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
lastCmdTime = currCmdTime
vrep.simxSynchronousTrigger(clientID)
def iterate_inference_aggregation(self, step):
end = 0
# Current agents' positions
agent_positions = self.get_agent_positions(step)
total_discrepancy = 0
# Calculate the discrepancy of every agent and sum them all
for agent_ID in range(0, self.__num_agents):
total_discrepancy += utils.find_discrepancy_aggregation(agent_ID, agent_positions,
self.__desired_distance)
discrepancy = total_discrepancy/self.__num_agents
# Continue the simulation for the number of steps defined in skip_steps
for i in range(0, self.__skip_steps):
vrep.simxSynchronousTrigger(self.__client_ID)
# Decide whether or not the training should end at the next step
if step >= self.__steps_limit:
end = 1
reset = 1
elif ((discrepancy > self.__fail_threshold) or (discrepancy < self.__success_threshold)) and discrepancy > 0:
end = 1
reset = 1
else:
end = 0
reset = 0
return discrepancy, end, reset
def ur5moveto(self, x, y, z):
"""
Move ur5 to location (x,y,z)
"""
vrep.simxSynchronousTrigger(self.clientID) # 让仿真走一步
self.targetQuaternion[0] = 0.707
self.targetQuaternion[1] = 0
self.targetQuaternion[2] = 0.707
self.targetQuaternion[3] = 0 #四元数
self.targetPosition[0] = x
self.targetPosition[1] = y
self.targetPosition[2] = z
vrep.simxPauseCommunication(self.clientID, True) #开启仿真
vrep.simxSetIntegerSignal(self.clientID, 'ICECUBE_0', 21,
vrep.simx_opmode_oneshot)
for i in range(3):
vrep.simxSetFloatSignal(self.clientID, 'ICECUBE_' + str(i + 1),
self.targetPosition[i],
vrep.simx_opmode_oneshot)
for i in range(4):
vrep.simxSetFloatSignal(self.clientID, 'ICECUBE_' + str(i + 4),
self.targetQuaternion[i],
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self.clientID, False)
vrep.simxSynchronousTrigger(self.clientID) # 进行下一步
vrep.simxGetPingTime(self.clientID) # 使得该仿真步走完
def getRange(self):
"get motor range"
eCode, value = vrep.simxGetFloatSignal(self.robot, "HOMEO_SIGNAL_"+self._wheel+"Wheel"+"_MAX_SPEED",getattr(vrep,self._opMode))
vrep.simxSynchronousTrigger(self.robot)
if eCode != 0:
raise TransducerException("Cannot get maxSpeed of VREP motor: " + self._wheel+"Wheel")
return value
def move_down(self): #3 time-steps
_, object_position = vrep.simxGetObjectPosition(self.client_id, \
self.robot.object_handle[0], -1, vrep.simx_opmode_blocking)
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.robot.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
move_direction = np.asarray([self.pickup_position[0] - sawyer_target_position[0], \
self.pickup_position[1] - sawyer_target_position[1], object_position[2] + 0.01 \
- sawyer_target_position[2]])
move_magnitude = np.linalg.norm(move_direction)
move_step = 0.03 * move_direction / move_magnitude
num_move_steps = int(np.floor(move_magnitude / 0.03))
remaining_magnitude = -num_move_steps * 0.03 + move_magnitude
remaining_distance = remaining_magnitude * move_direction / move_magnitude
for step_iter in range(num_move_steps):
vrep.simxSetObjectPosition(self.client_id, self.robot.sawyer_target_handle,\
-1, (sawyer_target_position[0] + move_step[0], sawyer_target_position[1] \
+ move_step[1], sawyer_target_position[2] + move_step[2]), \
vrep.simx_opmode_blocking)
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id,\
self.robot.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
vrep.simxSetObjectPosition(self.robot.client_id, self.robot.sawyer_target_handle, \
-1, (sawyer_target_position[0] + remaining_distance[0], sawyer_target_position[1] \
+ remaining_distance[1], sawyer_target_position[2]+ remaining_distance[2]), \
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def control_step( self ):
self.count += 1
if self.count == 10:
self.get_target()
self.calculate_error()
self.state = np.matrix([[self.pos_err[0]],
[self.pos_err[1]],
[self.pos_err[2]],
[self.lin[0]],
[self.lin[1]],
[self.lin[2]],
[self.ori_err[0]],
[self.ori_err[1]],
[self.ori_err[2]],
[self.ang[0]],
[self.ang[1]],
[self.ang[2]],
])
self.update_integral()
self.count = 0
self.send_motor_commands( self.compute_output() )
vrep.simxSynchronousTrigger( self.cid )
else:
self.update_integral()
def three_end_step(Lx, Ly, Lz, n=N):
for i in range(1, n + 1):
vrep.simxSynchronousTrigger(clientID)
for j in range(1, 6, 2):
vrep.simxSetObjectPosition(clientID, Tip_target[j], BCS,
[Lx[j][i], Ly[j][i], Lz[j][i]],
vrep.simx_opmode_oneshot_wait)
def lift_arm(self):
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
lift_position_x = 1.137
lift_position_y = 1.2151
lift_position_z = 0.18
self.lift_position = np.array(
[lift_position_x, lift_position_y, lift_position_z])
move_direction = np.asarray([self.lift_position[0] - sawyer_target_position[0], \
self.lift_position[1] - sawyer_target_position[1], self.lift_position[2] - \
sawyer_target_position[2]])
move_magnitude = np.linalg.norm(move_direction)
move_step = 0.01 * move_direction / move_magnitude
num_move_steps = int(np.floor(move_magnitude / 0.01))
for step_iter in range(num_move_steps):
vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, -1, \
(sawyer_target_position[0] + move_step[0], sawyer_target_position[1] + \
move_step[1], sawyer_target_position[2] + move_step[2]), vrep.simx_opmode_blocking)
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
_ = vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, -1, \
self.lift_position, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def set_back(self):
vrep.simxSetObjectPosition(self.client_id, self.object_handle[0], -1, \
self.object_position, vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(self.client_id, self.object_handle[0], \
self.robot_handle, self.object_orientation, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def close_hand(self):
vrep.simxSetJointForce(self.client_id, self.motor_handle, 100, \
vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.client_id, self.motor_handle, \
0.5, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def test_fk():
print("Testing FK")
q = [np.pi / 4., np.pi / 2., -np.pi / 4., -np.pi / 2., -np.pi / 6., np.pi]
src = transform(np.zeros((6, )), from_frame=EE_FRAME, to_frame=WORLD_FRAME)
dst = transform(q, from_frame=EE_FRAME, to_frame=WORLD_FRAME)
src_euler = np.asarray(tf.euler_from_quaternion(src[1], axes='szyx'),
dtype=np.float32)
dst_euler = np.asarray(tf.euler_from_quaternion(dst[1], axes="szyx"),
dtype=np.float32)
print("Source position: " + str(src[0]) + "; source orientation: " +
str(src_euler * 180. / np.pi))
print("Destination position: " + str(dst[0]) +
"; destination orientation: " + str(dst_euler * 180. / np.pi))
clientID, joints = vrep_connect_and_get_handles()
for joint, _q in zip(joints, q):
vrep.simxSetJointPosition(clientID, joint, _q,
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(clientID)
def applyAction(self, action, requestRemoteServer=True):
#
if (requestRemoteServer):
if (self._id == RC.CYOUBOT):
remoteObjectName = 'youBot'
else:
remoteObjectName = RC.CSERVER_REMOTE_API_OBJECT_NAME
scale = 1
if (self._id == RC.CKUKA_ARM_BARRETT_HAND):
scale = 1
elif (self._id == RC.CJACO_ARM_HAND):
hand_eigengrasp_amps = action
dofs = gb_hand_eigengrasp_interface.toDOF(hand_eigengrasp_amps)
action = self.accumulateMove(dofs) # dofs -> jointPoses
action = [action[i] * scale for i in range(len(action))]
# -----------------------------------------------------------------------
#
inputInts = []
inputFloats = action
inputStrings = []
inputBuffer = bytearray()
print('ApplyAction:', action)
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(self._clientID, remoteObjectName, \
vrep.sim_scripttype_childscript, \
CSERVER_REMOTE_FUNC_ROBOT_ACT, \
inputInts, inputFloats, inputStrings, inputBuffer, \
vrep.simx_opmode_oneshot_wait)
# ===================================================================================================================================
#
# move simulation ahead one time step
vrep.simxSynchronousTrigger(self._clientID)
return 1
def two_legs_recovery(n=N):
print("Start recovering legs!")
init_position = np.zeros((2, 3))
Lx = np.zeros((2, n + 1))
Ly = np.zeros((2, n + 1))
Lz = np.zeros((2, n + 1))
res, init_position[0] = vrep.simxGetObjectPosition(
clientID, S1[1], BCS, vrep.simx_opmode_oneshot_wait)
res, init_position[1] = vrep.simxGetObjectPosition(
clientID, S1[4], BCS, vrep.simx_opmode_oneshot_wait)
for i in range(n + 1):
Lx[0][i] = init_position[0][0] + i * (INIT_POSITION[1][0] -
init_position[0][0]) / n
Ly[0][i] = init_position[0][1] + i * (INIT_POSITION[1][1] - 0.2 -
init_position[0][1]) / n
Lz[0][i] = init_position[0][2] + i * (INIT_POSITION[1][2] -
init_position[0][2]) / n
for i in range(n + 1):
Lx[1][i] = init_position[1][0] + i * (INIT_POSITION[4][0] -
init_position[1][0]) / n
Ly[1][i] = init_position[1][1] + i * (INIT_POSITION[4][1] + 0.2 -
init_position[1][1]) / n
Lz[1][i] = init_position[1][2] + i * (INIT_POSITION[4][2] -
init_position[1][2]) / n
for i in range(n):
vrep.simxSynchronousTrigger(clientID)
vrep.simxSetObjectPosition(clientID, Tip_target[1], BCS,
[Lx[0][i], Ly[0][i], Lz[0][i]],
vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(clientID, Tip_target[4], BCS,
[Lx[1][i], Ly[1][i], Lz[1][i]],
vrep.simx_opmode_oneshot_wait)
def update_sim(self):
for handle, velocity in zip(self.joint_handles, self.target_velocities):
return_code = vrep.simxSetJointTargetVelocity(self.cid,
int(handle), velocity, vrep.simx_opmode_oneshot)
check_for_errors(return_code)
vrep.simxSynchronousTrigger(self.cid)
vrep.simxGetPingTime(self.cid)
def testMaxSpeed(self, maxSpeed, mode):
"""test max speed of khepera-like robot in V-Rep
revving the motors up to maxSpeed in the self.noSteps and then backward.
mode--> 1, both motors, 2: right only, 3: left only"""
if mode == 1:
rightOn = leftOn = 1
elif mode == 2:
rightOn = 1
leftOn = 0
elif mode == 3:
rightOn = 0
leftOn = 1
unitSpeed = maxSpeed /self.noSteps
for i in xrange(self.noSteps):
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, unitSpeed *(i+1)*rightOn, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, unitSpeed *(i+1)*leftOn, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
print "Step: %s\t Speed now: %.2f" %(str(i),(unitSpeed *(i+1)))
for i in xrange(self.noSteps):
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, -(maxSpeed/(i+1))*rightOn, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, -(maxSpeed/(i+1))*leftOn, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
print "Step: %s\t Speed now: %.2f" % (str(i), (maxSpeed/(i+1))*rightOn)
def __call__(self, t, values):
self.count += 1
if self.count == int(round(self.sim_dt/self.nengo_dt)):
self.count = 0
self.handle_input( values )
if self.sync:
vrep.simxSynchronousTrigger( self.cid )
return self.handle_output()
def act(self):
'''activates the wheel motor by calling the actuator function (transdFunction) with
VREP client (stored in self.robot) and the needed parameters (stored in funcParameters)'''
# parametersString = str(self.robot) + ", " + str(self._transducID) + ", " + str(self.funcParameters) + ", " + 'vrep.'+self._opMode
eCode = self.transdFunction(self.robot, self._transducID, self.funcParameters, getattr(vrep, self._opMode))
vrep.simxSynchronousTrigger(self.robot)
if eCode != 0:
stderr.write("Motor command to VREP motor:%sWheel failed " % self._wheel)
def __init__(self, simulatorModel=None):
self.robotOrientationFirstTime = True
self.getDistanceToGoalFirstTime = True
self.getUpDistanceFirstTime = True
self.getObjectPositionFirstTime = True
self.sysConf = LoadSystemConfiguration()
#this data structure is like a cache for the joint handles
self.jointHandleMapping = {}
robotConf = LoadRobotConfiguration()
self.model = simulatorModel
self.LucyJoints = robotConf.getJointsName()
for joint in self.LucyJoints:
self.jointHandleMapping[joint]=0
self.clientId = self.connectVREP()
RETRY_ATTEMPTS = 50
if simulatorModel:
for i in range(RETRY_ATTEMPTS):
#TODO try to reutilize the same scene for the sake of performance
error = self.loadscn(self.clientId, simulatorModel)
if not error:
break
print "retrying connection to vrep"
if error:
raise VrepException("error loading Vrep robot model", -1)
if int(self.sysConf.getProperty("synchronous mode?"))==1:
self.synchronous = True
vrep.simxSynchronousTrigger(self.clientId)
else:
self.synchronous = False
self.speedmodifier = int(self.sysConf.getProperty("speedmodifier"))
#setting the simulation time step
self.simulationTimeStepDT = float(self.sysConf.getProperty("simulation time step"))
'''#testing printing in vrep
error, consoleHandler = vrep.simxAuxiliaryConsoleOpen(self.clientId, "lucy debugging", 8, 22, None, None, None, None, vrep.simx_opmode_oneshot_wait)
print "console handler: ", consoleHandler
vrep.simxAuxiliaryConsoleShow(self.clientId, consoleHandler, 1, vrep.simx_opmode_oneshot_wait)
error = vrep.simxAuxiliaryConsolePrint(self.clientId, consoleHandler, "Hello World", vrep.simx_opmode_oneshot_wait)'''
error, self.upDistanceHandle = vrep.simxGetDistanceHandle(self.clientId, "upDistance#", vrep.simx_opmode_blocking)
error, self.distLFootToGoalHandle = vrep.simxGetDistanceHandle(self.clientId, "distanceLFootGoal#", vrep.simx_opmode_blocking)
error, self.distRFootToGoalHandle = vrep.simxGetDistanceHandle(self.clientId, "distanceRFootGoal#", vrep.simx_opmode_blocking)
error, self.bioloidHandle = vrep.simxGetObjectHandle(self.clientId,"Bioloid", vrep.simx_opmode_oneshot_wait)
error, self.cuboidHandle = vrep.simxGetObjectHandle(self.clientId,"Cuboid", vrep.simx_opmode_oneshot_wait)
error, self.armHandler = vrep.simxGetObjectHandle(self.clientId, "Pivot", vrep.simx_opmode_oneshot_wait)
self.populateJointHandleCache(self.clientId)
self.isRobotUpFirstCall = True
self.getDistanceToSceneGoal() #to fix the first invocation
self.getUpDistance()
self.isRobotUp(self.clientId)
self.armPositionXAxis = -9.0000e-02
def reset(self):
"""In VREP we reset a simulation by stopping and restarting it"""
eCode = vrep.simxStopSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait)
if eCode != 0:
raise Exception("Could not stop VREP simulation")
eCode = vrep.simxStartSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait)
if eCode != 0:
raise Exception("Could not start VREP simulation")
vrep.simxSynchronousTrigger(self._VREP_clientId)
def read(self):
# sensingParameters = self.robot + ", " + self._VREPSignalName + ", " + 'vrep.'+self._opMode
eCode, value = self._transdFunction(self.robot, self._VREPSignalName , getattr(vrep, self._opMode))
vrep.simxSynchronousTrigger(self.robot)
if eCode != 0:
# raise Exception("Cannot read value for sensor " + self._eye+"Eye")
stderr.write("Cannot read value for sensor " + self._eye+"Eye")
return 0
return value
def stepsim(self, steps):
"""
Do n-steps of simulation.
:param steps: Number of steps you want to simulate
:type steps: int
"""
for i in xrange(steps):
vrep.simxSynchronousTrigger(self._clientID)
def sendSignalVREP(self,signalName, signalValue):
if type(signalValue) == str:
eCode = vrep.simxSetStringSignal(self._VREP_clientId, signalName, asByteArray(signalValue), vrep.simx_opmode_oneshot_wait)
elif type(signalValue) == int:
eCode = vrep.simxSetIntegerSignal(self._VREP_clientId, signalName, signalValue, vrep.simx_opmode_oneshot_wait)
elif type(signalValue) == float:
eCode = vrep.simxSetFloatSignal(self._VREP_clientId, signalName, signalValue, vrep.simx_opmode_oneshot_wait)
else:
raise Exception("Trying to send a signal of unknown data type. Only strings, floats and and ints are accepted")
if eCode != 0:
raise Exception("Could not send string signal", signalValue)
vrep.simxSynchronousTrigger(self._VREP_clientId)
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
def __call__( self, t, values ):
""" This class will be callable within a nengo node. It will accept as input
the control signals for each rotor, and will output the relevant state
variables (position, velocity, orientation, angular velocity).
"""
self.count += 1
if self.count == int(round(sim_dt/dt)):
self.count = 0
self.handle_input( values )
if SYNC:
vrep.simxSynchronousTrigger( self.cid )
return self.handle_output()
def moveKJuniorReadProxSensors(self):
"slowly move forward and print normal vector readings"
rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_streaming)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_streaming)
vrep.simxSynchronousTrigger(self.simulID)
print "Proximity sensor readings error codes: ", rightInput[0],leftInput[0]
for step in xrange(self.noSteps):
rightSpeed = 10
leftSpeed = rightSpeed
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJrightMotor, rightSpeed, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJleftMotor, leftSpeed, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
print "Left-->err:%s - Detct'd: %s\t%s\t\t\tRight--> err:%s - Detct'd: %s\t%s" % (leftInput[0],
leftInput[3],
leftInput[4],
rightInput[0],
rightInput[3],
rightInput[4])
sleep(.2)
def VREPConnect(self):
vrep.simxFinish(-1)
"Connect to Simulation"
self.simulID = vrep.simxStart(self.robot_host,self.simulation_port,True,True, 5000,5)
eCode = vrep.simxSynchronous(self.simulID, True)
if eCode != 0:
print "Could not get V-REP to synchronize operation with me"
if not self.simulID == -1:
eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(self.simulID)
print "my SimulID is ", self.simulID
else:
sys.exit("Failed to connect to VREP simulation. Bailing out")
def moveAndReadProxSensors(self):
"rotate in place and print sensor distance and normal vector readings"
for step in xrange(self.noSteps):
if step>self.noSteps / 2:
rightSpeed = -1
leftSpeed = -rightSpeed
else:
rightSpeed = 1
leftSpeed = -rightSpeed
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, rightSpeed, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, leftSpeed, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
rightInput = vrep.simxReadProximitySensor(self.simulID, self.rightEye, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.leftEye, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
print "Left-->err:%s - Detct'd: %s\t%s\t\tRight--> err:%s - Detct'd: %s\t\t\t%s" % (leftInput[0],
leftInput[3],
leftInput[2],
rightInput[0],
rightInput[3],
rightInput[2])
sleep(.1)
self.stopRobot(self.simulID,[self.rightMotor,self.leftMotor])
vrep.simxSynchronousTrigger(self.simulID)
def braiten1b(self):
"slowly move forward and print normal vector readings"
intens = 100
ambientIntensRatio = 0.2
attVect = [0,0,pi *4]
for step in xrange(self.noSteps):
rightInput = vrep.simxReadProximitySensor(self.simulID, self.rightEye, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.leftEye, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
angle = degrees(self.angleBetVecs([0,0,1], centerInput[2]))
lightReading = self.irradAtSensor(intens, ambientIntensRatio, centerInput[2], attVect)
print "Center-->err:%s - Detct'd: %s\tAngle:%.3f\tIrrad:%.3f\tNorm: %.3f\tVector:%s\t" % (centerInput[0],
centerInput[3],
angle,
lightReading,
norm(centerInput[2]),
centerInput[2])
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, 1/lightReading, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, 1/lightReading, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
sleep(0)
def connect(self):
#os.chdir(VREP_HOME)
#subprocess.call([os.path.join(VREP_HOME,'vrep.sh'), VREP_scene_file], shell = True, cwd = VREP_HOME)
"Close existing connections"
vrep.simxFinish(-1)
"Connect to Simulation"
self.simulID = vrep.simxStart(self.robot_host,self.simulation_port,True,True, 5000,5)
eCode = vrep.simxSynchronous(self.simulID, True)
if eCode != 0:
print "Could not get V-REP to synchronize operation with me"
if not self.simulID == -1:
eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(self.simulID)
print "my SimulID is ", self.simulID
else:
sys.exit("Failed to connect to VREP simulation. Bailing out")
def __init__(self, wheel, clientID):
"Wheel could either 'right or 'left' and nothing else"
if wheel not in ["right","left"]:
raise TransducerException("Wheel must either be right or left")
self._wheel = wheel
self.robot = clientID
self._transdFunction = vrep.simxSetJointTargetVelocity
self._opMode = 'simx_opmode_oneshot_wait'
try:
eCode, self._transducID = vrep.simxGetObjectHandle(self.robot, (self._wheel+"Wheel"), getattr(vrep,self._opMode))
vrep.simxSynchronousTrigger(self.robot)
except Exception as e:
print "Cannot connect to VREP!"
print "Error: ",e
if eCode != 0:
raise TransducerException("Cannot connect to VREP motor: " + self._wheel+"Wheel")
self._range = self.getRange()
def moveKJuniorReadProxSensors(self):
"slowly move forward and print normal vector readings"
intens = 100
ambientIntens = 0
attVect = [0,0,pi *4]
rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_streaming)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_streaming)
centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_streaming)
vrep.simxSynchronousTrigger(self.simulID)
print "Proximity sensor readings error codes: ", rightInput[0],leftInput[0]
for step in xrange(self.noSteps):
rightSpeed = 10
leftSpeed = rightSpeed
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJrightMotor, rightSpeed, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJleftMotor, leftSpeed, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
#===================================================================
# print "Left-->err:%s - Detct'd: %s\t%s\t\t\tRight--> err:%s - Detct'd: %s\t%s" % (leftInput[0],
# leftInput[3],
# leftInput[4],
# rightInput[0],
# rightInput[3],
# rightInput[4])
#===================================================================
angle = degrees(self.angleBetVecs([0,0,1], centerInput[2]))
lightReading = self.irradAtSensor(intens, ambientIntens, centerInput[2], attVect)
print "Center-->err:%s - Detct'd: %s\tAngle:%.3f\tIrrad:%.3f\tNorm: %.3f\tVector:%s\t" % (centerInput[0],
centerInput[3],
angle,
lightReading,
norm(centerInput[2]),
centerInput[2])
sleep(0)
def start(self):
returnCode = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot)
if (returnCode>1):
print "returnCode: ", returnCode
raise Exception('Could not start')
returnCode=vrep.simxSynchronous(self.clientID,True)
for k in range(10): #Run to steps to step through initial "drop"
returncode = vrep.simxSynchronousTrigger(self.clientID)
if (returnCode!=0):
raise Exception('Could not set synchronous mode')
def braiten2a(self):
"Seek light source"
intens = 50
ambientIntensRatio = .2
attVect = [0,0,1]
rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_streaming)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_streaming)
centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_streaming)
vrep.simxSynchronousTrigger(self.simulID)
print "Proximity sensor readings error codes: ", rightInput[0],leftInput[0]
for step in xrange(self.noSteps):
rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
#===================================================================
# print "Left-->err:%s - Detct'd: %s\t%s\t\t\tRight--> err:%s - Detct'd: %s\t%s" % (leftInput[0],
# leftInput[3],
# leftInput[4],
# rightInput[0],
# rightInput[3],
# rightInput[4])
#===================================================================
angle = degrees(self.angleBetVecs([0,0,1], centerInput[2]))
lightReading = self.irradAtSensor(intens, ambientIntens, centerInput[2], attVect)
print "Center-->err:%s - Detct'd: %s\tAngle:%.3f\tIrrad:%.3f\tNorm: %.3f\tVector:%s\t" % (centerInput[0],
centerInput[3],
angle,
lightReading,
norm(centerInput[2]),
centerInput[2])
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJrightMotor, lightReading, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJleftMotor, lightReading, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
sleep(0)
def applyAction(self, action):
if self.state[0]<self.boundaries[0][0] or self.state[0]>self.boundaries[0][1] or self.state[1]<self.boundaries[1][0] or self.state[1]>self.boundaries[1][1] or (abs(self.target[0]-self.state[0])+abs(self.target[1]-self.state[1]))<self.wininngRadius:
reward = self.calculateRewardSingle()
if self.verbose:
print "Terminal state: ", self.state, " - Reward: ", reward
self.state = self.initState
self.stop()
time.sleep(0.1) #100ms delay between stopping and starting to avoid problems
self.start()
return None,reward
if (self.actionStrategy == 'Absolute'):
self.robot.applyActionAbsolute(action)
elif (self.actionStrategy == 'Differential'):
self.robot.applyActionIncremental(action)
else:
print "Not valid action strategy"
return
returncode = vrep.simxSynchronousTrigger(self.clientID)
newState = self.getState()
if (self.rewardStrategy == 'Differential'):
reward = self.calculateRewardPair(newState)
elif (self.rewardStrategy == 'Absolute'):
reward = self.calculateRewardSingle()
else:
print "Not valid reward strategy"
return
if self.verbose:
print "From state: ", self.state, " applied action: ", action, " got reward: ", reward
self.state = newState
return tuple(newState),reward
def moveAndReadLights(self):
"rotate in place and print light readings"
eCode, res, rightEyeRead = vrep.simxGetVisionSensorImage(self.simulID, self.rightEye, 0, vrep.simx_opmode_streaming)
ecode, res, leftEyeRead = vrep.simxGetVisionSensorImage(self.simulID, self.leftEye, 0, vrep.simx_opmode_streaming)
vrep.simxSynchronousTrigger(self.simulID)
for step in xrange(self.noSteps):
rightSpeed = 25
leftSpeed = rightSpeed
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, rightSpeed, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, leftSpeed, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
eCodeR, res, rightEyeRead = vrep.simxGetVisionSensorImage(self.simulID, self.rightEye, 0, vrep.simx_opmode_buffer)
eCodeL, res, leftEyeRead = vrep.simxGetVisionSensorImage(self.simulID, self.leftEye, 0, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
# print "Right eCode:\t", eCodeR,
# print "Left eCode:\t", eCodeL
# leftImg = np.array(leftEyeRead, np.uint8)
# rightImg.resize(res[0],res[1],3)
print "Right:\t%d, %d\tLeft:\t%d, %d"% (len(rightEyeRead),sum(rightEyeRead), len(leftEyeRead),sum(leftEyeRead))
import vrep
from time import sleep
vrep.simxFinish(-1)
simulID = vrep.simxStart('127.0.0.1',19997,True,True, 5000,5)
vrep.simxSynchronous(simulID, True)
for i in xrange(5):
print "starting no : ", i
print "now starting"
vrep.simxStartSimulation(simulID, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(simulID)
sleep(4)
print "now stopping"
vrep.simxStopSimulation(simulID, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(simulID)
sleep(2)
print "done with run no: ", i
import sys
print ('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to V-REP
if clientID!=-1:
print ('Connected to remote API server')
# enable the synchronous mode on the client:
vrep.simxSynchronous(clientID,True)
# start the simulation:
vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait)
# Now step a few times:
for i in range(1,10):
if sys.version_info[0] == 3:
input('Press <enter> key to step the simulation!')
else:
raw_input('Press <enter> key to step the simulation!')
vrep.simxSynchronousTrigger(clientID);
# stop the simulation:
vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print ('Failed connecting to remote API server')
print ('Program ended')
def _compute(self):
if self.synchronous:
self.returnSynchro=vrep.simxSynchronousTrigger(self.clientID)
self._data=self
def finish_iteration(self):
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxSynchronous(client_id, True)
position_history = []
e, body_pos = vrep.simxGetObjectPosition(client_id, body, -1, vrep.simx_opmode_buffer)
position_history.append(body_pos)
joint_pos_history = []
e, joint_0_pos = vrep.simxGetJointPosition(client_id, joint_0, vrep.simx_opmode_streaming)
e, joint_1_pos = vrep.simxGetJointPosition(client_id, joint_1, vrep.simx_opmode_streaming)
joint_pos_history.append([joint_0_pos, joint_1_pos])
with contexttimer.Timer() as timer:
for i in range(4):
e = vrep.simxSetJointTargetPosition(client_id, joint_0, 0.5, vrep.simx_opmode_streaming)
for t in range(3):
vrep.simxSynchronousTrigger(client_id)
e, body_pos = vrep.simxGetObjectPosition(client_id, body, -1, vrep.simx_opmode_buffer)
position_history.append(body_pos)
e, joint_0_pos = vrep.simxGetJointPosition(client_id, joint_0, vrep.simx_opmode_buffer)
e, joint_1_pos = vrep.simxGetJointPosition(client_id, joint_1, vrep.simx_opmode_buffer)
joint_pos_history.append([joint_0_pos, joint_1_pos])
e = vrep.simxSetJointTargetPosition(client_id, joint_1, 2.5, vrep.simx_opmode_streaming)
for t in range(10):
vrep.simxSynchronousTrigger(client_id)
e, body_pos = vrep.simxGetObjectPosition(client_id, body, -1, vrep.simx_opmode_buffer)
position_history.append(body_pos)
e, joint_0_pos = vrep.simxGetJointPosition(client_id, joint_0, vrep.simx_opmode_buffer)
e, joint_1_pos = vrep.simxGetJointPosition(client_id, joint_1, vrep.simx_opmode_buffer)
joint_pos_history.append([joint_0_pos, joint_1_pos])
