def step(self, action): assert self.action_space.contains(action) joint = int(action / 3) handle = self.joint_handles[joint] value = self.action_map[action % 3] if joint in self.faulty_joints: value = 0 #for angle, handle in zip(action, self.joint_handles) cur_pos = sim.simxGetJointPosition(self.clientID, handle, buffer)[1] sim.simxSetJointTargetPosition(self.clientID, handle, cur_pos + value * (3.14 / 180), oneshot) # give reward for touching the box reward, done = self.detect_collision() box_distance = sim.simxGetObjectPosition(self.clientID, self.lamp_end, self.green_box, buffer)[1] state = [d for d in box_distance] for handle in self.joint_handles: state.append( sim.simxGetJointPosition(self.clientID, handle, buffer)[1]) return np.array(state), reward, done, {}
def onScale(self, name, value): if (self.clientID != -1): print("{} : {}".format(name, value)) obj = self.handles[int(name)] ang = float(value) * math.pi / 180 sim.simxSetJointTargetPosition(self.clientID, obj, ang, sim.simx_opmode_oneshot)
def gripper(j1,j2, clientID, close): if(close == True): sim.simxSetJointTargetVelocity(clientID, j2,0.04, sim.simx_opmode_oneshot) else: sim.simxSetJointTargetVelocity(clientID, j2,-0.04, sim.simx_opmode_oneshot) r, p2 = sim.simxGetJointPosition(clientID, j2,sim.simx_opmode_oneshot) sim.simxSetJointTargetPosition(clientID,j1, p2*-1, sim.simx_opmode_oneshot)
def reset_sim(self): for x in range(0, 7): vrep.simxSetJointTargetPosition(self.clientID, self.joint_array[x], self.joint_org_position[x], vrep.simx_opmode_oneshot_wait) time.sleep(1)
def initializeSimModel(self, client_ID): try: print('Connected to remote API server') client_ID != -1 except: print('Failed connecting to remote API server') self.client_ID = client_ID for i in range(6): return_code, self.joint_handle[i] = vrep_sim.simxGetObjectHandle( client_ID, 'UR5_joint' + str(i + 1), vrep_sim.simx_opmode_blocking) if (return_code == vrep_sim.simx_return_ok): print('get object joint handle ' + str(i + 1) + ' ok.') # Get information from VREP for the first time for i in range(6): return_code, q = vrep_sim.simxGetJointPosition( self.client_ID, self.joint_handle[i], vrep_sim.simx_opmode_streaming) # Set the initialized position for each joint for i in range(6): vrep_sim.simxSetJointTargetPosition(self.client_ID, self.joint_handle[i], 0, vrep_sim.simx_opmode_streaming)
def fechar_garra_total(): sim.simxPauseCommunication(clientID, True) sim.simxSetJointTargetPosition(clientID, paDireita, 0.01, sim.simx_opmode_oneshot) sim.simxSetJointTargetPosition(clientID, paEsquerda, 0.01, sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID, False) time.sleep(1)
def actionCrane(self, step): if self.craneStatus and self.connectionStatus: self.getCurrentHeightHook() next_position = self.currentHeightHook + step - 6.6747 # (subtrai offset de altura do gancho) sim.simxSetJointTargetPosition(self.clientID, self.crane, next_position, sim.simx_opmode_continuous) return self.craneStatus
def actionBoom(self, step): if self.craneStatus and self.connectionStatus: self.getCurrentAngleClaw() next_position = self.currentAngleClaw + step sim.simxSetJointTargetPosition(self.clientID, self.boom, degree2radian(next_position), sim.simx_opmode_continuous) return self.craneStatus
def abrir_garra(): sim.simxPauseCommunication(glob.clientID, True) sim.simxSetJointTargetPosition(glob.clientID, glob.paDireita, 0.04, sim.simx_opmode_oneshot) sim.simxSetJointTargetPosition(glob.clientID, glob.paEsquerda, 0.04, sim.simx_opmode_oneshot) sim.simxPauseCommunication(glob.clientID, False) time.sleep(1.5)
def setJoints(J, Q): retCode = sim.simxSetJointTargetPosition(clientID, J[0], Q[0], sim.simx_opmode_oneshot) retCode = sim.simxSetJointTargetPosition(clientID, J[1], Q[1], sim.simx_opmode_oneshot) retCode = sim.simxSetJointTargetPosition(clientID, J[2], Q[2], sim.simx_opmode_oneshot) return retCode
def rotateCertainAngleNegative(self, num, angle): clientID = self.clientID RAD2DEG = self.RAD2DEG jointHandle = self.jointHandle jointConfig = self.jointConfig vrep.simxSetJointTargetPosition(clientID, jointHandle[num], (jointConfig[num] - angle) / RAD2DEG, vrep.simx_opmode_oneshot) jointConfig[num] = jointConfig[num] - angle self.jointConfig = jointConfig
def hit(hammer, clientID): vrep.simxPauseCommunication(clientID, True) vrep.simxSetJointTargetPosition(clientID, hammer, -0.7, vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(clientID, False) time.sleep(10) vrep.simxPauseCommunication(clientID, True) vrep.simxSetJointTargetPosition(clientID, hammer, 0, vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(clientID, False) time.sleep(5) return 0
def move(self, angles): opm = sim.simx_opmode_oneshot try: sim.simxPauseCommunication(self.clientID, True) for i in range(len(angles)): sim.simxSetJointTargetPosition(self.clientID, self.joints[i], angles[i], opm) sim.simxPauseCommunication(self.clientID, False) return except: print('Something went wrong with your joints and angles lists') print('Check to see if both lists are the same size') return
def commandCraneOnOff(self): if self.connectionStatus: if self.craneStatus: sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking) else: # Inicia simulacao sim.simxStartSimulation(self.clientID, sim.simx_opmode_blocking) sim.simxSetJointTargetPosition(self.clientID, self.boom, 0, sim.simx_opmode_continuous) sim.simxSetJointTargetPosition(self.clientID, self.crane, 0, sim.simx_opmode_continuous) self.craneStatus = not self.craneStatus return self.craneStatus
def reset_sim(self): time.sleep( .5 ) # moved sleep to the top to allow other actions to finish before reset for x in range(0, 7): vrep.simxSetJointTargetPosition(self.clientID, self.right_joint_array[x], self.right_joint_org_position[x], vrep.simx_opmode_oneshot_wait) vrep.simxSetJointTargetPosition(self.clientID, self.left_joint_array[x], self.left_joint_org_position[x], vrep.simx_opmode_oneshot_wait) self.reset_target_object(0.1250, 0.0, 0.9)
def set_target_position(self, coords): """Установить целевое положение (для динамической сцены) Parameters ---------- coords : ndarray Обобщённые координаты """ sim.simxPauseCommunication(self.client, 1) for i in range(JOINT_COUNT): sim.simxSetJointTargetPosition(self.client, self.joints[i], coords[i], sim.simx_opmode_oneshot) sim.simxPauseCommunication(self.client, 0)
def MoveForwardPosition(dist): raio = 0.003735 angref = dist/raio erro, angLeft = sim.simxGetJointPosition(clientID, robotLeftMotor, sim.simx_opmode_buffer) #print(erro, angLeft) erro, angRight = sim.simxGetJointPosition(clientID, robotRightMotor, sim.simx_opmode_buffer) sim.simxPauseCommunication(clientID, True) sim.simxSetJointTargetPosition(clientID, robotLeftMotor, angref + angLeft, sim.simx_opmode_oneshot) sim.simxSetJointTargetPosition(clientID, robotRightMotor, angref + angRight, sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID, False) #time.sleep(0.1) actualAngL = 0 actualAngR = 0 startTime = time.time() while ((int(actualAngL*1000) != int((angref + angLeft)*1000)) and (int(actualAngR*1000) != int((angref + angRight)*1000) )): #print(actualAngR, angref + angRight) sim.simxPauseCommunication(clientID, True) sim.simxSetJointTargetPosition(clientID, robotLeftMotor, angref + angLeft, sim.simx_opmode_oneshot) sim.simxSetJointTargetPosition(clientID, robotRightMotor, angref + angRight, sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID, False) erro, actualAngR = sim.simxGetJointPosition(clientID, robotRightMotor, sim.simx_opmode_buffer) erro, actualAngL = sim.simxGetJointPosition(clientID, robotRightMotor, sim.simx_opmode_buffer) if(TimeOut(dist, startTime)): print('timeout') break
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 moveToPoint2(clientID, j1, j2, j3, gp, vetP, td): c0 = 0 c02 = 0 c1 = 0 for p in vetP: pGrip = [] ret, ponto = sim.simxGetObjectHandle(clientID, p, sim.simx_opmode_blocking) returnCode, pos = sim.simxGetObjectPosition(clientID, ponto, -1, sim.simx_opmode_blocking) #print(p, pos) cd_p = inve.calcInversa(pos[0], pos[1], pos[2]) print(cd_p) theta1 = cd_p[0] theta2 = cd_p[1] c2 = (3 * (theta1 - c0)) / (td**2) c3 = -(2 * c2) / (3 * td) c22 = (3 * (theta2 - c02)) / (td**2) c32 = -(2 * c22) / (3 * td) for i in np.arange(0, td, 0.01): time.sleep(0.01) t1 = c0 + c1 * i + c2 * (i**2) + c3 * (i**3) t2 = c02 + c1 * i + c22 * (i**2) + c32 * (i**3) q1 = t1 returnCode = sim.simxSetJointTargetPosition( clientID, j1, q1, sim.simx_opmode_oneshot) q2 = t2 returnCode = sim.simxSetJointTargetPosition( clientID, j2, q2, sim.simx_opmode_oneshot) d3 = cd_p[2] returnCode = sim.simxSetJointTargetPosition( clientID, j3, d3, sim.simx_opmode_oneshot) returnCode, pos2 = sim.simxGetObjectPosition( clientID, gp, -1, sim.simx_opmode_blocking) pGrip += [pos2] print(q1, q2) c0 = theta1 c02 = theta2
def step_arms(self, right_action, left_action): right_new_position = self.step_right(right_action) left_new_position = self.step_left(left_action) # TODO get the arms to move simultaneously # move the arm joints for x in range(0, 7): vrep.simxSetJointTargetPosition(self.clientID, self.right_joint_array[x], right_new_position[x], vrep.simx_opmode_oneshot_wait) vrep.simxSetJointTargetPosition(self.clientID, self.left_joint_array[x], left_new_position[x], vrep.simx_opmode_oneshot_wait) time.sleep(1) return right_new_position, left_new_position
def openGripper(self): res, gripper = sim.simxGetObjectHandle(self.clientID, 'P_Grip_straight_motor', sim.simx_opmode_blocking) res = sim.simxSetJointTargetPosition(self.clientID, gripper, -10, sim.simx_opmode_blocking) sim.simxSetIntegerSignal(self.clientID, 'P_Grip_straight_motor', 1, sim.simx_opmode_blocking)
def step_right(self, action): """Applies an array of actions to all right joint positions. Args: action (list): increments to add to robot position (-0.1, 0, 0.1) """ # get position of arm, increment by values, then move robot start_position = [] new_position = [] for x in range(0, 7): error_code, temp_pos = vrep.simxGetJointPosition( self.clientID, self.joint_array[x], vrep.simx_opmode_buffer) start_position.append(temp_pos) new_position.append(temp_pos + action[x]) vrep.simxSetJointTargetPosition(self.clientID, self.joint_array[x], new_position[x], vrep.simx_opmode_oneshot_wait)
def reset(self): # reset objects dynamically in lua script result, collision_state, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction( self.clientID, "hexapod", sim.sim_scripttype_childscript, "reset_robot", [], [], [], emptyBuff, blocking) for link, angle in zip(self.links, [0, -30, 120]): for handle in link.values(): sim.simxSetJointTargetPosition(self.clientID, handle, angle * (3.14 / 180), oneshot) sim.simxSynchronousTrigger(self.clientID) self.prev_pos = sim.simxGetObjectPosition(self.clientID, self.hexapod_handle, -1, buffer)[1] self.state = np.ones(self.state_size) * 120 * (3.14 / 180) return self.state # hope coppeliaSim set them properly
def setJointAngle(self, joint_name, q): """ :param: joint_name: str the joint name: can be UR5_joint1 ~ UR5_joint6 :param: q: float array of size 6 x 1 the desired joint angle for all joints """ for i in range(6): if joint_name == 'UR5_joint' + str(i + 1): vrep_sim.simxSetJointTargetPosition( self.client_ID, self.joint_handle[i], q, vrep_sim.simx_opmode_streaming) return # can not find the joint print('Error: joint name: \' ' + joint_name + '\' can not be recognized.') return 0
def reset_sim(self): time.sleep( .5 ) # moved sleep to the top to allow other actions to finish before reset vrep.simxSetIntegerSignal(self.clientID, 'BaxterVacuumCup_active', 0, vrep.simx_opmode_oneshot) vrep.simxSetIntegerSignal(self.clientID, 'BaxterVacuumCup#0_active', 0, vrep.simx_opmode_oneshot) for x in range(0, 7): vrep.simxSetJointTargetPosition(self.clientID, self.right_joint_array[x], self.right_joint_org_position[x], vrep.simx_opmode_oneshot_wait) vrep.simxSetJointTargetPosition(self.clientID, self.left_joint_array[x], self.left_joint_org_position[x], vrep.simx_opmode_oneshot_wait) self.reset_target_object(0.1250, 0.0, 0.9) time.sleep(1) # reset the suction grippers vrep.simxSetIntegerSignal(self.clientID, 'BaxterVacuumCup_active', 1, vrep.simx_opmode_oneshot) vrep.simxSetIntegerSignal(self.clientID, 'BaxterVacuumCup#0_active', 1, vrep.simx_opmode_oneshot) # pick up the box # self.step_arms([.608, .45, 0, 0, -1.6, -1.5, 0], [-.605, .45, 0, 0, 1.6, -1.5, 0]) self.step_arms([0, .45, 0, 0, -1.6, -1.5, 0], [0, .45, 0, 0, 1.6, -1.5, 0]) time.sleep(.5) self.step_arms([.65, 0, 0, 0, 0, 0, 0], [-.7, 0, 0, 0, 0, 0, 0]) time.sleep(.5) self.step_arms([0, -.1, 0, 0, 0, 0, 0], [0, -0.1, 0, 0, 0, 0, 0]) error_code, target_angles = vrep.simxGetObjectOrientation( self.clientID, self.main_target, -1, vrep.simx_opmode_streaming) for angle in target_angles: if angle > 1 or angle < -1: self.reset_sim() print("SIM reset error, resetting again.") break
def reset(self): for handle in self.joint_handles: sim.simxSetJointTargetPosition(self.clientID, handle, 0, blocking) # Change Box positions rand_angle = 0 # radians(random.randint(0, 359)) box_x, box_y = (BOX_DISTANCE * cos(rand_angle), BOX_DISTANCE * sin(rand_angle)) sim.simxSetObjectPosition(self.clientID, self.green_box, self.joint_handles[0], (box_x, box_y, 0), oneshot) sim.simxSetObjectPosition(self.clientID, self.red_box, self.joint_handles[0], (-box_x, -box_y, 0), oneshot) box_distance = sim.simxGetObjectPosition(self.clientID, self.lamp_end, self.green_box, buffer)[1] state = [d for d in box_distance] for handle in self.joint_handles: state.append( sim.simxGetJointPosition(self.clientID, handle, buffer)[1]) return np.array(state)
def step(self, action): assert self.action_space.contains(action) joint = int(action / 2) handle = list(self.joint_handles.values())[joint] parent_handle = list(self.links[1].values())[joint] value = self.action_map[action % 2] if joint in self.faulty_joints: value = 0 cur_pos = sim.simxGetJointPosition(self.clientID, handle, buffer)[1] new_pos = cur_pos + value * (3.14 / 180) if np.abs( new_pos ) <= MAX_ANGLE: # Limiting it from rotating beyond |30| degree sim.simxSetJointTargetPosition(self.clientID, parent_handle, -45 * (3.14 / 180), oneshot) sim.simxSynchronousTrigger(self.clientID) sim.simxSetJointTargetPosition(self.clientID, handle, new_pos, oneshot) sim.simxSynchronousTrigger(self.clientID) sim.simxSetJointTargetPosition(self.clientID, parent_handle, -30 * (3.14 / 180), oneshot) sim.simxSynchronousTrigger(self.clientID) # update state, reward, done reward, done = self.calc_reward() self.state[joint] = new_pos else: reward = -1 done = False return self.state, reward, done, {}
def left_move_joint(self, action): if action == 0 or action % 2 == 0: move_interval = -0.03 else: move_interval = 0.03 joint_num = self.joint_switch.get(action, -1) error_code, position = vrep.simxGetJointPosition( self.clientID, self.left_joint_array[joint_num], vrep.simx_opmode_buffer) error_code = vrep.simxSetJointTargetPosition( self.clientID, self.left_joint_array[joint_num], position + move_interval, vrep.simx_opmode_oneshot_wait) return error_code
def fechar_garra_cubo(cube): erro = 1 # while erro != 0: # erro, robotPosition = sim.simxGetObjectPosition(glob.clientID, glob.robo, -1, sim.simx_opmode_streaming) # erro = 1 while erro != 0: erro, cubePosition = sim.simxGetObjectPosition( glob.clientID, cube, glob.robo, sim.simx_opmode_streaming) #print(robotPosition) #print(cubePosition) cubePosition[0] = 0 erro = 1 while erro != 0: sim.simxPauseCommunication(glob.clientID, True) sim.simxSetJointTargetPosition(glob.clientID, glob.paDireita, 0.025, sim.simx_opmode_oneshot) sim.simxSetJointTargetPosition(glob.clientID, glob.paEsquerda, 0.025, sim.simx_opmode_oneshot) erro = sim.simxSetObjectPosition(glob.clientID, cube, glob.robo, cubePosition, sim.simx_opmode_oneshot) sim.simxPauseCommunication(glob.clientID, False) time.sleep(1)
def setJointAngle(self, q): """ :param: q: float array of size 4 x 1 the desired joint angle for all joints """ vrep_sim.simxSetJointTargetPosition(self.client_ID, self.left_hip_joint_handle, q[0], vrep_sim.simx_opmode_streaming) vrep_sim.simxSetJointTargetPosition(self.client_ID, self.left_knee_joint_handle, q[1], vrep_sim.simx_opmode_streaming) vrep_sim.simxSetJointTargetPosition(self.client_ID, self.right_hip_joint_handle, q[2], vrep_sim.simx_opmode_streaming) vrep_sim.simxSetJointTargetPosition(self.client_ID, self.right_knee_joint_handle, q[3], vrep_sim.simx_opmode_streaming)