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