def draw_circ(start_ptx, start_pty, r, jointHandles):
for i in range(0, 721):
PI = np.pi
x = start_ptx + r * np.cos(i * PI / 180)
y = start_pty + r * np.sin(i * PI / 180)
#Calculate Inverse Kinematics
thetas = lab_invk(x, y, 0, 0)
#Move robot arm
vrep.simxPauseCommunication(clientID, True)
vrep.simxSetJointTargetPosition(clientID, jointHandles[0], thetas[0],
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, jointHandles[1], thetas[1],
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, jointHandles[2], thetas[2],
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, jointHandles[3], thetas[3],
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, jointHandles[4], thetas[4],
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, jointHandles[5], thetas[5],
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
#Compensate for starting location
if (i >= 200):
vrep.simxSetModelProperty(clientID, floor, 0,
vrep.simx_opmode_oneshot)
return
def setMotorSpeeds(motorSpeed):
try:
vrep.simxPauseCommunication(robot.clientID,True)
vrep.simxSetJointTargetVelocity(robot.clientID, robot.leftMotorHandle, motorSpeed.get('speedLeft',0), vrep.simx_opmode_oneshot )
vrep.simxSetJointTargetVelocity(robot.clientID, robot.rightMotorHandle, motorSpeed.get('speedRight',0), vrep.simx_opmode_oneshot )
finally:
vrep.simxPauseCommunication(robot.clientID,False)
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 rotate(degree, clientID):
# set velocety to 0
wheelJoints = getWheelJoints(clientID)
for i in range(0, 4):
vrep.simxSetJointTargetVelocity(clientID, wheelJoints[i], 0,
vrep.simx_opmode_oneshot)
# start moving
vrep.simxPauseCommunication(clientID, True)
for i in range(0, 4):
vrep.simxSetJointTargetVelocity(clientID, wheelJoints[i],
wheelVel(0.0, 0.0, ROTATE_VEL)[i],
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
# continuously check traveled distance
w = 0.0
dt = 0.0
while w <= degree:
start = time.time()
x, y, w = odometry(0.0, 0.0, w, 0.0, 0.0, ROTATE_VEL, dt)
time.sleep(
1.0e-06
) # problems with very small time slices -> little delay (if you have a bad angle calculation on your pc try to change this value)
end = time.time()
dt = end - start
# stop moving
for i in range(0, 4):
vrep.simxSetJointTargetVelocity(clientID, wheelJoints[i], 0,
vrep.simx_opmode_oneshot)
return
def _apply_all_joint_positions(self):
# print 'Moving robot to new configuration: {}'.format(self._joint_positions)
vrep.simxPauseCommunication(self._client_id, True)
for joint_handle, position in self._joint_positions:
vrep.simxSetJointPosition(self._client_id, joint_handle, position,
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self._client_id, False)
def run_sim_data(subject, clientID): vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait) # start #RhipX,RhipY,RhipZ,LhipX,LhipY,LhipZ,RKneeZ,LKneeZ,RAnkleX,RAnkleZ,LAnkleX,LAnkleZ legJoints = [0,0,0,0,0,0,0,0,0,0,0,0] #RhipX,RhipY,RhipZ,LhipX,LhipY,LhipZ,RKneeZ,LKneeZ,RAnkleX,RAnkleZ,LAnkleX,LAnkleZ legPositions = [0,0,0,0,0,0,0,0,0,0,0,0] legJointInv = [1,1,1,-1,-1,1,1,1,1,1,-1,1] hipJointInv = [1,1,1,1,1,1,1,1,1,1,1,1] returnCode01, legJoints[0] = vrep.simxGetObjectHandle(clientID, "rightLegJoint0", vrep.simx_opmode_blocking) returnCode02, legJoints[1] = vrep.simxGetObjectHandle(clientID, "rightLegJoint1", vrep.simx_opmode_blocking) returnCode03, legJoints[2] = vrep.simxGetObjectHandle(clientID, "rightLegJoint2", vrep.simx_opmode_blocking) returnCode04, legJoints[6] = vrep.simxGetObjectHandle(clientID, "rightLegJoint3", vrep.simx_opmode_blocking) returnCode05, legJoints[8] = vrep.simxGetObjectHandle(clientID, "rightLegJoint5", vrep.simx_opmode_blocking) returnCode06, legJoints[9] = vrep.simxGetObjectHandle(clientID, "rightLegJoint4", vrep.simx_opmode_blocking) returnCode07, legJoints[3] = vrep.simxGetObjectHandle(clientID, "leftLegJoint0", vrep.simx_opmode_blocking) returnCode08, legJoints[4] = vrep.simxGetObjectHandle(clientID, "leftLegJoint1", vrep.simx_opmode_blocking) returnCode09, legJoints[5] = vrep.simxGetObjectHandle(clientID, "leftLegJoint2", vrep.simx_opmode_blocking) returnCode010, legJoints[7] = vrep.simxGetObjectHandle(clientID, "leftLegJoint3", vrep.simx_opmode_blocking) returnCode011, legJoints[10] = vrep.simxGetObjectHandle(clientID, "leftLegJoint5", vrep.simx_opmode_blocking) returnCode012, legJoints[11] = vrep.simxGetObjectHandle(clientID, "leftLegJoint4", vrep.simx_opmode_blocking) returnCode013, head = vrep.simxGetObjectHandle(clientID, "Asti", vrep.simx_opmode_blocking) headLocation = [1,1,1]; returncode, headLocation = vrep.simxGetObjectPosition(clientID, head, -1, vrep.simx_opmode_streaming) for i in range(12): returnCode, legPositions[i] = vrep.simxGetJointPosition(clientID, legJoints[i], vrep.simx_opmode_streaming) #RhipX,RhipY,RhipZ,LhipX,LhipY,LhipZ,RKneeZ,LKneeZ,RAnkleX,RAnkleZ,LAnkleX,LAnkleZ start_time = int(round(time.time())) while vrep.simxGetConnectionId(clientID) != -1 and ((headLocation[2] > 0.3 or headLocation[2] == 0.0) and ((int(round(time.time())) - start_time) < 100)): for j in range(((subject * 101)),((subject * 101) + 202)): for i in range(12): returnCode, legPositions[i] = vrep.simxGetJointPosition(clientID, legJoints[i], vrep.simx_opmode_buffer) joint_data = model_iterator.getActual(j) newJointPositions = [0,0,0,0,0,0,0,0,0,0,0,0] for i in range(len(newJointPositions)): joint_data[i] = joint_data[i] * math.pi / 180 newJointPositions[i] = joint_data[i] * legJointInv[i] * hipJointInv[i] print(j) print(newJointPositions) print() vrep.simxPauseCommunication(clientID,1) for i in range(12): vrep.simxSetJointTargetPosition(clientID,legJoints[i],newJointPositions[i],vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(clientID,0) time.sleep(0.01) vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait) time.sleep(0.25) return
def step(self, action):
"""
Take one step of action
Input: action
Output: observation, reward, done, info
"""
#
action = np.clip(action, self.act_min, self.act_max)
# split action for light and sma
action_smas = action[:self.smas_num]
action_lights_state = action[self.smas_num:self.smas_num +
self.lights_num]
action_lights_state = action_lights_state.astype(int)
action_lights_color = action[self.smas_num + self.lights_num:]
# taking action
#start = time.time()
vrep.simxPauseCommunication(
self.clientID, True) #temporarily halting the communication thread
self._set_all_joint_position(action_smas)
self._set_all_light_state(action_lights_state, action_lights_color)
vrep.simxPauseCommunication(self.clientID,
False) #and evaluated at the same time
#print("Action running time: {}".format(time.time()-start))
# observe
#start = time.time()
self._self_observe()
#print("Observation running time: {}".format(time.time()-start))
# caculate reward
self._reward()
done = False
return self.observation, self.reward, done, []
def forward(dist, clientID):
# set velocety to 0
wheelJoints = getWheelJoints(clientID)
for i in range(0, 4):
vrep.simxSetJointTargetVelocity(clientID, wheelJoints[i], 0,
vrep.simx_opmode_oneshot)
# start moving
vrep.simxPauseCommunication(clientID, True)
for i in range(0, 4):
vrep.simxSetJointTargetVelocity(clientID, wheelJoints[i],
wheelVel(FORWARD_VEL, 0.0, 0.0)[i],
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
# continuously check traveled distance
distance = 0.0
dt = 0.0
x = 0.0
y = 0.0
while distance <= dist:
start = time.time()
x, y, w = odometry(x, y, 0.0, FORWARD_VEL, 0.0, 0.0, dt)
distance = math.sqrt(x * x + y * y)
time.sleep(
1.0e-06
) # problems with very small time slices -> little delay (if you have a bad angle calculation on your pc try to change this value)
end = time.time()
dt = end - start
# stop moving
for i in range(0, 4):
vrep.simxSetJointTargetVelocity(clientID, wheelJoints[i], 0,
vrep.simx_opmode_oneshot)
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 joint_move(client_id, body, command_angles):
vrep.simxPauseCommunication(client_id, 1)
print body
vrep.simxSetJointTargetPosition(client_id, body[0][0], command_angles[0], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[1][0], command_angles[1], vrep.simx_opmode_oneshot)
# Left Leg
vrep.simxSetJointTargetPosition(client_id, body[2][0], command_angles[8], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[3][0], command_angles[9], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[4][0], command_angles[10], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[5][0], command_angles[11], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[6][0], command_angles[12], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[7][0], command_angles[13], vrep.simx_opmode_oneshot)
# Right Leg
vrep.simxSetJointTargetPosition(client_id, body[8][0], command_angles[14], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[9][0], command_angles[15], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[10][0], command_angles[16], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[11][0], command_angles[17], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[12][0], command_angles[18], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[13][0], command_angles[19], vrep.simx_opmode_oneshot)
# Left Arm
vrep.simxSetJointTargetPosition(client_id, body[14][0], command_angles[2], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[15][0], command_angles[3], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[16][0], command_angles[4], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[17][0], command_angles[5], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[18][0], command_angles[6], vrep.simx_opmode_oneshot)
# Right Arm
vrep.simxSetJointTargetPosition(client_id, body[19][0], command_angles[20], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[20][0], command_angles[21], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[21][0], command_angles[22], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[22][0], command_angles[23], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(client_id, body[23][0], command_angles[24], vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(client_id, 0)
time.sleep(0.02)
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 vel_amount(cl_h, l_j, r_j, vel):
sim.simxPauseCommunication(cl_h, True)
sim.simxSetJointTargetVelocity(cl_h, l_j[0], vel, sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(cl_h, r_j[0], vel, sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(cl_h, l_j[1], vel, sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(cl_h, r_j[1], vel, sim.simx_opmode_oneshot)
sim.simxPauseCommunication(cl_h, False)
return
def startMoving(clientID):
wheelJoints = getWheelJoints(clientID)
vrep.simxPauseCommunication(clientID, True)
for i in range(0, 4):
vrep.simxSetJointTargetVelocity(clientID, wheelJoints[i],
wheelVel(FORWARD_VEL, 0.0, 0.0)[i],
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
def set_forces(force_vec):
_ = vrep.simxPauseCommunication(self.client_id, True)
for handle_idx in range(0, self.joint_count):
_ = vrep.simxSetJointForce(self.client_id,
self.handles[handle_idx],
force_vec[handle_idx],
vrep.simx_opmode_blocking)
_ = vrep.simxPauseCommunication(self.client_id, False)
def start_position(client_id, body):
vrep.simxPauseCommunication(client_id, 1)
angle = 90 * 3.1416 / 180
print body[14]
errorCode = vrep.simxSetJointTargetPosition(client_id, body[14][0], angle, vrep.simx_opmode_oneshot)
errorCode = vrep.simxSetJointTargetPosition(client_id, body[19][0], angle, vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(client_id, 0)
def setTorques(self, torques):
vrep.simxPauseCommunication(self.id, True)
for j, t in zip(self.joints, torques):
vrep.simxSetJointTargetVelocity(self.id, j,
np.sign(t) * 1e10,
vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(self.id, j, np.abs(t),
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self.id, False)
def place_robot_in_configuration(clientID, joint_handles, configuration,
delay):
time.sleep(delay)
vrep.simxPauseCommunication(clientID, True)
for j in range(6):
vrep.simxSetJointPosition(clientID, joint_handles[j], configuration[j],
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
time.sleep(delay)
def pdControl(clientID, youBotCam, goal):
print("begin PD control")
wheelJoints = move.getWheelJoints(clientID)
err, res, image = vrep.simxGetVisionSensorImage(clientID, youBotCam, 0,
vrep.simx_opmode_buffer)
cv2Image = colorDet.convertToCv2Format(image, res)
#cv2.imshow("Current Image of youBot", cv2Image)
#cv2.waitKey(0)
# initialize variables for while loop
cor = getRedBlobPicture(cv2Image)
if len(cor) == 0:
cor = getBlueBlobPicture(cv2Image)
preCor = cor
rotVel = 0.0
forwBackVel = 999 # just a random number to initialize while loop
leftRightVel = 999 # just a random number to initialize while loop
while not velOk(forwBackVel, leftRightVel):
# retrieve new coordinate of red blob
err, res, image = vrep.simxGetVisionSensorImage(
clientID, youBotCam, 0, vrep.simx_opmode_buffer)
cv2Image = colorDet.convertToCv2Format(image, res)
cor = getRedBlobPicture(cv2Image)
if len(cor) == 0:
cor = getBlueBlobPicture(cv2Image)
# calculate new velocities
forwBackVel = 0.1 * (cor[0][0] - goal[0]) - 0.002 * (cor[0][0] -
preCor[0][0])
leftRightVel = 0.08 * (cor[0][1] - goal[1]) - 0.001 * (cor[0][1] -
preCor[0][1])
# save current coordinates as previous coordinates
preCor = cor
# update velocities
move.setWheelVelocity(clientID, 0.0)
vrep.simxPauseCommunication(clientID, True)
for i in range(0, 4):
vrep.simxSetJointTargetVelocity(
clientID, wheelJoints[i],
move.wheelVel(forwBackVel / 10.0, leftRightVel / 10.0,
rotVel)[i], vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
# stop moving
move.setWheelVelocity(clientID, 0.0)
#cv2.imshow("Current Image of youBot", cv2Image)
#cv2.waitKey(0)
print("end PD control")
def get_handle_multiple(self, handles):
"""
Pauses the communication and retrieves all handles at once
"""
vrep.simxPauseCommunication(self.client_id, True)
handles = []
for handle in handles:
handles.append(self.get_handle(handle))
vrep.simxPauseCommunication(self.client_id, False)
return handles
def moveto(self, config1, config2):
vrep.simxPauseCommunication(self.clientID, True)
for i in range(self.jointNum):
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[i],
config1[i],
vrep.simx_opmode_oneshot)
for i in range(self.jointNum):
vrep.simxSetJointTargetPosition(self.clientID,
self.robot2_jointHandle[i],
config2[i],
vrep.simx_opmode_oneshot)
def getRobotState(self):
pos = np.zeros(len(self.joints))
vel = np.zeros(len(self.joints))
vrep.simxPauseCommunication(self.id, True)
for i, joint in enumerate(self.joints):
pos[i] = vrep.simxGetJointPosition(self.id, joint,
vrep.simx_opmode_buffer)[1]
vel[i] = vrep.simxGetObjectFloatParameter(
self.id, joint, vrep.sim_jointfloatparam_velocity,
vrep.simx_opmode_buffer)[1]
vrep.simxPauseCommunication(self.id, False)
return pos, vel
def rotateAllAngle(self, joint_angle):
clientID = self.clientID
jointNum = self.jointNum
RAD2DEG = self.RAD2DEG
jointHandle = self.jointHandle
# 暂停通信，用于存储所有控制命令一起发送
vrep.simxPauseCommunication(clientID, True)
for i in range(jointNum):
vrep.simxSetJointTargetPosition(clientID, jointHandle[i], joint_angle[i]/RAD2DEG, vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
self.jointConfig = joint_angle
def startRotating(clientID, rotationVel):
wheelJoints = getWheelJoints(clientID)
# set velocety to 0
setWheelVelocity(clientID, 0)
# start rotating
vrep.simxPauseCommunication(clientID, True)
for i in range(0, 4):
vrep.simxSetJointTargetVelocity(clientID, wheelJoints[i],
wheelVel(0.0, 0.0, rotationVel)[i],
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
def set_joint_position(self, target, ignoreError=False):
'''
set joint position to target position
'''
vrep.simxPauseCommunication(clientID,True)
for i in range(4):
res = vrep.simxSetJointTargetPosition(clientID,self.jointHandles[i],target[i],vrep.simx_opmode_oneshot)
if res!=0 and not ignoreError:
print('Failed to set joint position: {}'.format(self.jointDict[i]))
print(res)
vrep.simxPauseCommunication(clientID,False)
return None
def drive(wheelVelocities, t, clientID, wheelJoints):
#print("VEL:",wheelVelocities,"\n")
vrep.simxPauseCommunication(clientID, True)
for i in range(0, 4):
vrep.simxSetJointTargetVelocity(clientID, wheelJoints[i],
wheelVelocities[i],
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
time.sleep(t)
for i in range(0, 4):
vrep.simxSetJointTargetVelocity(clientID, wheelJoints[i], 0,
vrep.simx_opmode_oneshot)
def reset(self):
self.get_point = False
self.grab_counter = 0
self.get_obstacles = False
self.obstacles_counter = 0
self.currCmdTime = vrep.simxGetLastCmdTime(self.clientID)
# dt = (self.currCmdTime - self.lastCmdTime) / 1000
self.moveto(self.config1, self.config2)
vrep.simxPauseCommunication(self.clientID, False)
self.lastCmdTime = self.currCmdTime
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxGetPingTime(self.clientID)
s = self.getState_v1()
return s
def moveRobo(clientID, angulos):
junta = []
for i in range(6):
_, j = vrep.simxGetObjectHandle(clientID, 'eixo' + str(i + 1),
vrep.simx_opmode_blocking)
junta.append(j)
vrep.simxPauseCommunication(clientID, True)
for i in range(6):
vrep.simxSetJointTargetPosition(clientID, junta[i],
angulos[i] * np.pi / 180,
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
time.sleep(2)
return 0
def step_visitor(self, position):
"""
This interface is for change visitor's position.
Input: position
Output: observation, reward, done, info
"""
#
position = np.clip(position, self.visitor_action_min,
self.visitor_action_max)
vrep.simxPauseCommunication(self.clientID, True)
self._set_all_visitor_position(position)
vrep.simxPauseCommunication(self.clientID, False)
self._self_observe()
self._reward_visitor()
done = False
return self.observation, self.reward_visitor, done, []
def move_joints(a_joint1, a_joint2, a_joint3, a_joint4, a_joint5, a_joint6):
""" move the joints. Input: Angular velocities in rad/s """
# print('start j1-j6: ', a_joint1, a_joint2, a_joint3, a_joint4, a_joint5, a_joint6)
# vrep.simxPauseSimulation(clientID, ONESHOT)
# vrep.simxSetJointTargetPosition(clientID, joint1ID, a_joint1, ONESHOT)
# vrep.simxSetJointTargetPosition(clientID, joint2ID, a_joint2, ONESHOT)
# vrep.simxSetJointTargetPosition(clientID, joint3ID, a_joint3, ONESHOT)
# vrep.simxSetJointTargetPosition(clientID, joint4ID, a_joint4, ONESHOT)
# vrep.simxSetJointTargetPosition(clientID, joint5ID, a_joint5, ONESHOT)
# vrep.simxSetJointTargetPosition(clientID, joint6ID, a_joint6, ONESHOT)
# vrep.simxStartSimulation(clientID, ONESHOT)
vrep.simxPauseCommunication(clientID, True)
vrep.simxSetJointTargetPosition(clientID, joint1ID, a_joint1, ONESHOT)
vrep.simxSetJointTargetPosition(clientID, joint2ID, a_joint2, ONESHOT)
vrep.simxSetJointTargetPosition(clientID, joint3ID, a_joint3, ONESHOT)
vrep.simxSetJointTargetPosition(clientID, joint4ID, a_joint4, ONESHOT)
vrep.simxSetJointTargetPosition(clientID, joint5ID, a_joint5, ONESHOT)
vrep.simxSetJointTargetPosition(clientID, joint6ID, a_joint6, ONESHOT)
vrep.simxPauseCommunication(clientID, False)
# stay_sync()
# time.sleep(5)
# res, j1 = vrep.simxGetJointPosition(clientID, joint1ID, BLOCKING)
# res, j2 = vrep.simxGetJointPosition(clientID, joint2ID, BLOCKING)
# res, j3 = vrep.simxGetJointPosition(clientID, joint3ID, BLOCKING)
# res, j4 = vrep.simxGetJointPosition(clientID, joint4ID, BLOCKING)
# res, j5 = vrep.simxGetJointPosition(clientID, joint5ID, BLOCKING)
# res, j6 = vrep.simxGetJointPosition(clientID, joint6ID, BLOCKING)
#
#
# #
# print('j1-j6: ', j1, j2, j3, j4, j5, j6)
# depthData = []
# for i in range(4):
# time.sleep(config.time_step)
# depthData1 = fetch_kinect()
# depthData.append(depthData1)
# return depthData
return
def fechaGarra(clientID, num):
_, j1 = vrep.simxGetObjectHandle(clientID, 'junta_m1',
vrep.simx_opmode_blocking)
_, j2 = vrep.simxGetObjectHandle(clientID, 'junta_m2',
vrep.simx_opmode_blocking)
vrep.simxPauseCommunication(clientID, True)
vrep.simxSetJointTargetPosition(clientID, j1, 0.028,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, j2, 0.028,
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
_, s = vrep.simxGetObjectHandle(clientID, 'fixaGarra',
vrep.simx_opmode_blocking)
_, cubo = vrep.simxGetObjectHandle(clientID, 'Cubo' + str(num),
vrep.simx_opmode_blocking)
time.sleep(1)
vrep.simxSetObjectParent(clientID, cubo, s, True,
vrep.simx_opmode_blocking)
def resumePauseSim(self, clientID):
return vrep.simxPauseCommunication(clientID,False)
def pauseSim(self, clientID):
return vrep.simxPauseCommunication(clientID,True)
counter += 1
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
def resumePauseSim(self, clientID):
ret=vrep.simxPauseCommunication(clientID,False)
if self.speedmodifier > 0:
vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_speedmodifier, self.speedmodifier, vrep.simx_opmode_oneshot_wait)
return ret
