def andar_em_metros(d, v, m): # d = 1 , andar para frente # d =-1 , andar para trás # v = velocidade # m = valor em metros erro, a_inicial = sim.simxGetObjectPosition(clientID, robo, -1, sim.simx_opmode_blocking) x_inicial = a_inicial[0] y_inicial = a_inicial[1] while (True): erro, a = sim.simxGetObjectPosition(clientID, robo, -1, sim.simx_opmode_blocking) x = a[0] y = a[1] #print(x,y) if (abs(x - x_inicial) >= m or abs(y - y_inicial) >= m): break sim.simxPauseCommunication(clientID, True) sim.simxSetJointTargetVelocity(clientID, robotRightMotor, d * v, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(clientID, robotLeftMotor, d * v, sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID, False) Stop()
def inicio_virar_SUL(): # para a função COM VISÃO d=0 while(True): erro,b=sim.simxGetObjectOrientation(glob.clientID,glob.robo,-1,sim.simx_opmode_blocking) gamma=(b[2]*180)/(np.pi) if(gamma>=-3 and gamma<=3): Stop() break if(d==0): if(gamma>0 and gamma<179.99): d = 1 else: d = -1 v=4 sim.simxPauseCommunication(glob.clientID, True) sim.simxSetJointTargetVelocity(glob.clientID,glob.robotRightMotor,d*v, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(glob.clientID,glob.robotLeftMotor,(-1)*d*v, sim.simx_opmode_oneshot) sim.simxPauseCommunication(glob.clientID, False) align.Align() andar_em_metros(tras, 5, 0.065) return
def inicio_virar_NORTE(): # para a função SEM VISÃO; lembrar de adicionar no if(nao viu prataleira) virar 180. d=0 while(True): erro,b=sim.simxGetObjectOrientation(glob.clientID,glob.robo,-1,sim.simx_opmode_blocking) gamma=(b[2]*180)/(np.pi) if(gamma>=177 or gamma<=-177): Stop() break if(d==0): if(gamma>0 and gamma<179.99): d = 1 else: d =-1 v=4 sim.simxPauseCommunication(glob.clientID, True) sim.simxSetJointTargetVelocity(glob.clientID,glob.robotRightMotor,d*v, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(glob.clientID,glob.robotLeftMotor,(-1)*d*v, sim.simx_opmode_oneshot) sim.simxPauseCommunication(glob.clientID, False) align.Align() return
def init_joint(clientID, Body, motion, frame): def control(joint, angel): sim.simxSetJointTargetPosition(clientID, Body[joint], angel, sim.simx_opmode_oneshot) #print(LSP, LSR, LEY, LER, RSP, RSR, REY, RER) sim.simxPauseCommunication(clientID, 1) control('LShoulderPitch', 0) control('LShoulderRoll', 0) control('LElbowYaw', 0) control('LElbowRoll', 0) control('RShoulderPitch', 0) control('RShoulderRoll', 0) control('RElbowYaw', 0) control('RElbowRoll', 0) control('LHipRoll', 0) control('LHipPitch', 0) control('LKneePitch', 0) #control('LHipRoll', 0) control('RHipPitch', 0) control('RKneePitch', 0) control('LAnkleRoll', 0) control('RAnkleRoll', 0) #control('LAnklePitch', 0) #control('RAnklePitch', 0) control('RHipYawPitch', 0) control('LHipYawPitch', 0) sim.simxPauseCommunication(clientID, 0)
def joint_actuate(clientID, Body, primitive, frame): model = NeuralNetwork() model.load_state_dict(torch.load('C:\\Users\\lenovo\\Desktop\\AI-Project-Portfolio\\Amendments\\new_angle_mapping\\weights_2.pth')) model.eval() angle = model(torch.from_numpy(primitive[frame].reshape(1, -1)).float()) HP, HY, LSP, LSR, LER, LEY, RSP, RSR, RER, REY, LHP, LHR, LHYP, LKP, RHP, RHR, RHYP, RKP, LAP, LAR, RAP, RAR = angle.detach().numpy().reshape(-1) print(LSP, LSR, LER, LEY) def control(joint, angel): sim.simxSetJointTargetPosition(clientID, Body[joint], angel, sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID,1) control('HeadPitch', HP) control('HeadYaw', HY) control('LShoulderPitch', 1*LSP) control('LShoulderRoll', 1*LSR) control('LElbowRoll', 1*LER) control('LElbowYaw', 1*LEY) control('RShoulderPitch', RSP) control('RShoulderRoll', RSR) control('RElbowRoll', RER) control('RElbowYaw', REY) control('LHipPitch', LHP) control('LHipRoll', LHR) control('LHipYawPitch', LHYP) control('LKneePitch', LKP) control('RHipPitch', RHP) control('RHipRoll', RHR) control('RHipYawPitch', RHYP) control('RKneePitch', RKP) control('LAnklePitch', LAP) control('LAnkleRoll', LAR) control('RAnklePitch', RAP) control('RAnkleRoll', RAR) sim.simxPauseCommunication(clientID,0) #time.sleep(.04)
def joint_actuate(clientID, Body, motion, frame): LSP, LSR, LEY, LER, RSP, RSR, REY, RER, LHR, LHP, LKP, RHR, RHP, RKP, LAR, RAR, LAP, RAP = set_angel( motion, frame) def control(joint, angel): sim.simxSetJointTargetPosition(clientID, Body[joint], angel, sim.simx_opmode_oneshot) #print(LSP, LSR, LEY, LER, RSP, RSR, REY, RER) sim.simxPauseCommunication(clientID, 1) control('LShoulderPitch', 0.5 * LSP) control('LShoulderRoll', LSR) control('LElbowYaw', -0.5 * LEY) control('LElbowRoll', LER) control('RShoulderPitch', 0.5 * RSP) control('RShoulderRoll', RSR) control('RElbowYaw', -0.5 * REY) control('RElbowRoll', RER) control('LHipRoll', LHR) control('LHipPitch', LHP) control('LKneePitch', LKP) #control('LHipRoll', RHR) control('RHipPitch', RHP) control('RKneePitch', RKP) control('LAnkleRoll', LAR) control('RAnkleRoll', RAR) #control('LAnklePitch', LAP) #control('RAnklePitch', RAP) control('RHipYawPitch', -1 * RHP) control('LHipYawPitch', -1 * LHP) sim.simxPauseCommunication(clientID, 0)
def Stop(clientID, _robotRightMotor, _robotLeftMotor): sim.simxPauseCommunication(clientID, True) sim.simxSetJointTargetVelocity(clientID, _robotRightMotor, 0, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(clientID, _robotLeftMotor, 0, sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID, False)
def joint_actuate(self, primitive, frame): LSP, LSR, LEY, LER, RSP, RSR, REY, RER, LHR, LHP, LKP, RHR, RHP, RKP, LAR, RAR, LAP, RAP, LHYP, RHYP = self.dance_primitive_library[ str(primitive)][str(frame)]["angle_info"] sim.simxPauseCommunication(self.clientID, 1) self.control('LShoulderPitch', LSP) self.control('LShoulderRoll', LSR) self.control('LElbowYaw', LEY) self.control('LElbowRoll', LER) self.control('RShoulderPitch', RSP) self.control('RShoulderRoll', RSR) self.control('RElbowYaw', REY) self.control('RElbowRoll', RER) self.control('LHipRoll', LHR) self.control('LHipPitch', LHP) self.control('LKneePitch', LKP) self.control('RHipRoll', RHR) self.control('RHipPitch', RHP) self.control('RKneePitch', RKP) self.control('LAnkleRoll', LAR) self.control('RAnkleRoll', RAR) self.control('LAnklePitch', LAP) self.control('RAnklePitch', RAP) self.control('RHipYawPitch', LHYP) self.control('LHipYawPitch', RHYP) sim.simxPauseCommunication(self.clientID, 0)
def MoveBack(): v = 5 sim.simxPauseCommunication(clientID, True) sim.simxSetJointTargetVelocity(clientID, robotRightMotor, -v, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(clientID, robotLeftMotor, -v, sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID, False)
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 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 TurnLeft(): vref = 1 sim.simxPauseCommunication(clientID, True) sim.simxSetJointTargetVelocity(clientID, robotRightMotor, vref, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(clientID, robotLeftMotor, -vref, sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID, False)
def giro_livre(d,v): # d = 1 , anti horario, esquerda # d =-1 , horario, direita # v = velocidade sim.simxPauseCommunication(glob.clientID, True) sim.simxSetJointTargetVelocity(glob.clientID,glob.robotRightMotor,d*v, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(glob.clientID,glob.robotLeftMotor,(-1)*d*v, sim.simx_opmode_oneshot) sim.simxPauseCommunication(glob.clientID, False)
def main_loop(self): """ this is looped over in the sub process """ if self.sync: sim.simxSynchronousTrigger(self.clientID) sim.simxPauseCommunication(self.clientID, True) # get position data _, orientation = sim.simxGetObjectOrientation(self.clientID, self.camHandle, -1, sim.simx_opmode_buffer) _, position = sim.simxGetObjectPosition(self.clientID, self.camHandle, -1, sim.simx_opmode_buffer) # retrieve images self.images = self.getImages() # only send or receive once images are not none if self.images is None: time.sleep(1 / 100) return # send images to depth pipeline self.images["display"] = self.Depth2Color(self.images["depth"], 1) self.images["display"] = np.vstack([self.images["RGB"], self.images["display"]]) message = {"position": position, "orientation": orientation, "images": self.images} self.queues["output"].send(message, method="recent") speeds = self.queues["input"].retrieve(method="no_wait") # check keyboard to set wheel directions speed_L, speed_R = self.keyboardInput() if (speeds is not None) and (not self.keyboard_controlled): speed_L = speeds["left"] speed_R = speeds["right"] # activate gripper #if self.gripper_key.click(): # self.setGripper("left", -0.05) # self.setGripper("right", 0.05) #else: # self.setGripper("left", 0.05) # self.setGripper("right", -0.05) # set wheel speeds threads = {"left": Thread(target=self.setSpeed, args=("left", speed_L,)), "right": Thread(target=self.setSpeed, args=("right", speed_R,))} self.startThreads(threads) self.joinThreads(threads) sim.simxPauseCommunication(self.clientID, False)
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 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 adjustJoints(self, x): # pause to load all joints at the same time sim.simxPauseCommunication(self.clientID, True) for i in range(len(self.joints)): sim.simxSetJointPosition(self.clientID, self.joints[i], x[i] * 0.01745329252, sim.simx_opmode_oneshot) sim.simxPauseCommunication(self.clientID, False) time.sleep(0.02) # give the simulation time to catch up # return the distance between tip and target _, d = sim.simxReadDistance(self.clientID, self.distanceHandle, sim.simx_opmode_blocking) return d
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 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 move_arm(armpose): armpose_convert = [] for i in range(6): armpose_convert.append(round(armpose[i] / 180 * math.pi, 3)) sim.simxPauseCommunication(clientID, True) sim.simxSetJointTargetPosition(clientID, armjoint1_handle, armpose_convert[0], sim.simx_opmode_oneshot) sim.simxSetJointTargetPosition(clientID, armjoint2_handle, armpose_convert[1], sim.simx_opmode_oneshot) sim.simxSetJointTargetPosition(clientID, armjoint3_handle, armpose_convert[2], sim.simx_opmode_oneshot) sim.simxSetJointTargetPosition(clientID, armjoint4_handle, armpose_convert[3], sim.simx_opmode_oneshot) sim.simxSetJointTargetPosition(clientID, armjoint5_handle, armpose_convert[4], sim.simx_opmode_oneshot) sim.simxSetJointTargetPosition(clientID, armjoint6_handle, armpose_convert[5], sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID, False) time.sleep(0)
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 set_position(self, coords): """Установить положение (для статической сцены) Не работает для динамической сцены. Для статической вызовает мгновенное перемещение. При работе с обратной связью следует использовать в режиме синхронизации для предотвращения получения неактуальных данных. Parameters ---------- coords : ndarray Обобщённые координаты """ sim.simxPauseCommunication(self.client, 1) for i in range(JOINT_COUNT): sim.simxSetJointPosition(self.client, self.joints[i], coords[i], sim.simx_opmode_oneshot) sim.simxPauseCommunication(self.client, 0) if self.synchronous: self._step_simulation()
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 gira_livre_uma_roda(roda, d, v): # roda: escolher com qual roda girar # d = 1 , anti horario # d = -1 , horario # v = velocidade if (roda == direita): sim.simxPauseCommunication(glob.clientID, True) sim.simxSetJointTargetVelocity(glob.clientID, glob.robotLeftMotor, d*v, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(glob.clientID,glob.robotRightMotor, 0, sim.simx_opmode_oneshot) sim.simxPauseCommunication(glob.clientID, False) elif(roda == esquerda): sim.simxPauseCommunication(glob.clientID, True) sim.simxSetJointTargetVelocity(glob.clientID, glob.robotLeftMotor, 0, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(glob.clientID, glob.robotRightMotor, -d*v, sim.simx_opmode_oneshot) sim.simxPauseCommunication(glob.clientID, False)
def update_joints(self, joints, acc=0.5, vel=0.1): joints_copy = copy.deepcopy(joints) joints_copy[1] += pi / 2 joints_copy[3] += pi / 2 # dj = [0, 0, 0, 0, 0, 0] # for i in range(6): # res, self.sim_joints[i] = sim.simxGetJointPosition(self.clientID,self.jh[i],sim.simx_opmode_streaming) # dj[i] = abs(joints_copy[i] - self.sim_joints[i]) # # self.sim_joints[1] -= pi / 2 # self.sim_joints[3] -= pi / 2 # # max_dj = max(dj) # j_vel = [0,0,0,0,0,0] # for i in range(6): # j_vel[i] = 0.5*dj[i]/max_dj # res = sim.simxSetJointTargetVelocity(self.clientID, self.jh[i], j_vel[i], sim.simx_opmode_streaming) sim.simxPauseCommunication(self.clientID, True) for i in range(6): res = sim.simxSetJointTargetPosition(self.clientID, self.jh[i], joints_copy[i], sim.simx_opmode_oneshot) sim.simxPauseCommunication(self.clientID, False)
def TurnDirectionAng( direcao, ang ): #Girar para a direita ou para a esquerda pelo angulo que você escolher #ang positivo em graus #direcao: -1 = direita, 1 = esquerda angref = direcao * ang * (np.pi * 180) * (0.04188 / 360) erro, angRight = sim.simxGetJointPosition(clientID, robotRightMotor, sim.simx_opmode_buffer) erro, angLeft = sim.simxGetJointPosition(clientID, robotLeftMotor, sim.simx_opmode_buffer) sim.simxPauseCommunication(clientID, True) sim.simxSetJointTargetPosition(clientID, robotRightMotor, angref + angRight, sim.simx_opmode_oneshot) sim.simxSetJointTargetPosition(clientID, robotLeftMotor, -angref + angLeft, sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID, False) actualAngR = 0 actualAngL = 0 startTime = time.time() while ((int(actualAngR * 1000) != int((angref + angRight) * 1000)) and (int(actualAngL * 1000) != int((-angref + angLeft) * 1000))): sim.simxPauseCommunication(clientID, True) sim.simxSetJointTargetPosition(clientID, robotRightMotor, angref + angRight, sim.simx_opmode_oneshot) sim.simxSetJointTargetPosition(clientID, robotLeftMotor, -angref + angLeft, sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID, False) erro, actualAngR = sim.simxGetJointPosition(clientID, robotRightMotor, sim.simx_opmode_buffer) erro, actualAngL = sim.simxGetJointPosition(clientID, robotLeftMotor, sim.simx_opmode_buffer) if (TimeOutAng(ang, startTime)): print('timeout') break print(int(actualAngR * 1000), int((angref + angRight) * 1000)) print(int(actualAngL * 1000), int((-angref + angLeft) * 1000))
def MoveDirectionPosition(direcao, dist): #Andar reto para frente ou para trás #direcao: 1 = frente, -1 = tras raio = 0.003735 angref = direcao * dist / raio erro, angRight = sim.simxGetJointPosition(clientID, robotRightMotor, sim.simx_opmode_buffer) erro, angLeft = sim.simxGetJointPosition(clientID, robotLeftMotor, sim.simx_opmode_buffer) sim.simxPauseCommunication(clientID, True) sim.simxSetJointTargetPosition(clientID, robotRightMotor, angref + angRight, sim.simx_opmode_oneshot) sim.simxSetJointTargetPosition(clientID, robotLeftMotor, angref + angLeft, sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID, False) actualAngR = 0 actualAngL = 0 startTime = time.time() while ((int(actualAngR * 1000) != int((angref + angRight) * 1000)) and (int(actualAngL * 1000) != int((angref + angLeft) * 1000))): sim.simxPauseCommunication(clientID, True) sim.simxSetJointTargetPosition(clientID, robotRightMotor, angref + angRight, sim.simx_opmode_oneshot) sim.simxSetJointTargetPosition(clientID, robotLeftMotor, angref + angLeft, sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID, False) erro, actualAngR = sim.simxGetJointPosition(clientID, robotRightMotor, sim.simx_opmode_buffer) erro, actualAngL = sim.simxGetJointPosition(clientID, robotLeftMotor, sim.simx_opmode_buffer) if (TimeOutDist(dist, startTime)): print('timeout') break print(int(actualAngR * 1000), int((angref + angRight) * 1000)) print(int(actualAngL * 1000), int((angref + angLeft) * 1000))
robotRightMotorB] = sim.simxGetObjectHandle(clientID, 'Back_r_joint', sim.simx_opmode_oneshot_wait) # Criar stream de dados [erro, positionrobot] = sim.simxGetObjectPosition(clientID, robot, -1, sim.simx_opmode_streaming) #[erro, positiontarget] = sim.simxGetObjectPosition(clientID, target, -1, sim.simx_opmode_streaming) [erro, orientationrobot ] = sim.simxGetObjectOrientation(clientID, robot, -1, sim.simx_opmode_streaming) time.sleep(2) # Comandos motores vref = 1.0 sim.simxPauseCommunication(clientID, True) sim.simxSetJointTargetVelocity(clientID, robotRightMotor, vref, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(clientID, robotLeftMotor, vref, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(clientID, robotRightMotorB, vref, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(clientID, robotLeftMotorB, vref, sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID, False) cont = 0 while (cont < 8000): cont += 1 # Coletar dados robôs e alvos [erro, [xr, yr, zr]] = sim.simxGetObjectPosition(clientID, robot, -1,
def main(): global hit_wall global ball_coord global ball_linear global ball_hit global detect_handle global prox_handle global stop_prog print('Program started') sim.simxFinish(-1) # just in case, close all opened connections clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to CoppeliaSim if clientID != -1: print('Connected to remote API server') # Now try to retrieve data in a blocking fashion (i.e. a service call): res, objs = sim.simxGetObjects(clientID, sim.sim_handle_all, sim.simx_opmode_blocking) if res == sim.simx_return_ok: print('Number of objects in the scene: ', len(objs)) else: print('Remote API function call returned with error code: ', res) time.sleep(2) #Giving an initial velocity to the ball before starting the thread detect_handle = sim.simxGetObjectHandle(clientID, 'Sphere', sim.simx_opmode_blocking) prox_handle = sim.simxGetObjectHandle(clientID, 'Proximity_sensor', sim.simx_opmode_blocking) racket_handle = sim.simxGetObjectHandle(clientID, 'Proximity_sensor0', sim.simx_opmode_blocking) dummy_handle = sim.simxGetObjectHandle(clientID, 'Cylinder', sim.simx_opmode_blocking) sim.simxSetObjectPosition(clientID, detect_handle[1], -1, [0.65, 0.47, 0.0463], sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID, True) sim.simxSetObjectFloatParameter(clientID, detect_handle[1], 3001, 1, sim.simx_opmode_oneshot) #sim.simxSetObjectFloatParameter(clientID, detect_handle[1], 3000, -0.01, sim.simx_opmode_oneshot) sim.simxSetObjectFloatParameter(clientID, detect_handle[1], 3002, 1, sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID, False) sim.simxReadProximitySensor(clientID, prox_handle[1], sim.simx_opmode_streaming) sim.simxGetObjectVelocity(clientID, detect_handle[1], sim.simx_opmode_streaming) sim.simxReadProximitySensor(clientID, racket_handle[1], sim.simx_opmode_streaming) sim.simxGetObjectPosition(clientID, dummy_handle[1], -1, sim.simx_opmode_streaming) ball_thread = threading.Thread(target=get_ball_info, args=({ clientID: clientID })) try: #getting joint handles and initializing the joints in the simulation print("1. getting joint angles") jointHandles = [] for i in range(6): handle = sim.simxGetObjectHandle( clientID, 'UR3_joint' + str(i + 1) + '#0', sim.simx_opmode_blocking) jointHandles.append(handle) time.sleep(0.01) for i in range(6): print(jointHandles[i]) #hit_thread = threading.Thread(target=hit_ball, args=({clientID:clientID})) ball_thread.daemon = True #hit_thread.daemon = True #hit_thread.start() ball_thread.start() # #Get handles for detecting object # detect_handle = sim.simxGetObjectHandle(clientID, 'Sphere', sim.simx_opmode_blocking) # #Get handles for detecting the proximity sensor # prox_handle = sim.simxGetObjectHandle(clientID, 'Proximity_sensor', sim.simx_opmode_blocking) #Create an instance to compute the ball trajectory print("1. Initializing bouncing trajectory function") b_ball = bouncing_ball.BouncingBall() #Set-up some of the RML vectors: vel = 60 accel = 10 jerk = 20 currentVel = [0, 0, 0, 0, 0, 0, 0] currentAccel = [0, 0, 0, 0, 0, 0, 0] maxVel = [ vel * math.pi / 180, vel * math.pi / 180, vel * math.pi / 180, vel * math.pi / 180, vel * math.pi / 180, vel * math.pi / 180 ] maxAccel = [ accel * math.pi / 180, accel * math.pi / 180, accel * math.pi / 180, accel * math.pi / 180, accel * math.pi / 180, accel * math.pi / 180 ] maxJerk = [ jerk * math.pi / 180, jerk * math.pi / 180, jerk * math.pi / 180, jerk * math.pi / 180, jerk * math.pi / 180, jerk * math.pi / 180 ] targetVel = [0, 0, 0, 0, 0, 0] sim.simxPauseCommunication(clientID, True) for i in range(6): sim.simxSetJointTargetPosition(clientID, jointHandles[i][1], 0, sim.simx_opmode_streaming) sim.simxSetJointTargetVelocity(clientID, jointHandles[i][1], targetVel[i], sim.simx_opmode_streaming) sim.simxPauseCommunication(clientID, False) time.sleep(2) #start the feedback loop while True: if not hit_wall: #print("We haven't hit the wall yet") continue if hit_wall: print("We hit wall ....") #Predict the position of the ball from the wall and transform it to world coordinates print("Predicting Trajectory ...") pred_pos = b_ball.trajectory(ball_coord[0], ball_coord[1], ball_coord[2], ball_linear) T_prox = np.array([[-1, 0, 0, 0.025], [0, -1, 0, 2.85], [0, 0, 1, -.05], [0, 0, 0, 1]]) prox_pos = np.array([[pred_pos[0]], [pred_pos[1]], [pred_pos[2]], [1]]) ball_pos = T_prox @ prox_pos print(ball_pos) #set the left flag to true if on left side left = 0 if ball_pos[0, :] < 0: left = 1 #convert to right-side coordinates for IK ball_pos[0, :] = np.abs(ball_pos[0, :]) if ball_pos[0, :] > 0.9: print("Ball too far away from robot. will not hit it ...") raise ValueError elif ball_pos[0, :] < 0.2: print("Ball too close. cannot hit it...") raise ValueError twist_angle = (-92.85) * (ball_pos[0, :]) + 90 print(ball_pos, "left?: ", left) targetPos0_ik = ik.findJointAngles(ball_pos[0, :], ball_pos[1, :], ball_pos[2, :] + 0.15) #Invert joint angles if on left side of robot for i in range(len(targetPos0_ik)): targetPos0_ik[i] = ((-2 * left) + 1) * targetPos0_ik[i] print("Applying the hitting motion") #Apply the hitting motion using the new joint angles end_pose = [] targetPos0_ik[0] = targetPos0_ik[0] + ( (-2 * left) + 1) * (0 * np.pi / 180) #To simulate a hit targetPos0_ik[4] = targetPos0_ik[4] + ( (-2 * left) + 1) * (0 * np.pi / 180) sim.simxPauseCommunication(clientID, True) for i in range(6): #print(jointHandles[i]) #print(targetPos0[i]) sim.simxSetJointTargetPosition(clientID, jointHandles[i][1], targetPos0_ik[i], sim.simx_opmode_streaming) sim.simxSetJointTargetVelocity(clientID, jointHandles[i][1], targetVel[i], sim.simx_opmode_streaming) sim.simxPauseCommunication(clientID, False) #Now read from the proximity sensor to see if it detects any ball ret, dS, dP, dOH, dSNV = sim.simxReadProximitySensor( clientID, racket_handle[1], sim.simx_opmode_buffer) while (dS == 0): _, end_pose = sim.simxGetObjectPosition( clientID, dummy_handle[1], -1, sim.simx_opmode_buffer) ret, dS, dP, dOH, dSNV = sim.simxReadProximitySensor( clientID, racket_handle[1], sim.simx_opmode_buffer) print("Status of Ball is: ", dS) #Now, attach a proximity sensor to the racquet and see if it detects a ball. If it does, let's start the #hitting motion #Actually, for now, let's just not worry about this. Let's make sure that the trajectory generation and the ball hitting #works print("Twist angle is: ", twist_angle) targetPos0_ik[0] = targetPos0_ik[0] + ((-2 * left) + 1) * ( (1 * twist_angle) * np.pi / 180) #To simulate a hit targetPos0_ik[4] = targetPos0_ik[4] + ( (-2 * left) + 1) * (-1 * twist_angle * np.pi / 180) # sim.simxPauseCommunication(clientID, True) # for i in range(6): # #print(jointHandles[i]) # #print(targetPos0[i]) # sim.simxSetJointTargetPosition(clientID, jointHandles[i][1], targetPos0_ik[i], sim.simx_opmode_buffer) # sim.simxSetJointTargetVelocity(clientID, jointHandles[i][1], targetVel[i], sim.simx_opmode_buffer) # sim.simxPauseCommunication(clientID, False) # time.sleep(2) ball_hit = True hit_wall = False sim.simxPauseCommunication(clientID, True) for i in [0, 4]: sim.simxSetJointTargetPosition(clientID, jointHandles[i][1], targetPos0_ik[i], sim.simx_opmode_streaming) sim.simxSetJointTargetVelocity(clientID, jointHandles[i][1], targetVel[i], sim.simx_opmode_streaming) sim.simxPauseCommunication(clientID, False) time.sleep(2) sim.simxPauseCommunication(clientID, True) #Reset after hitting ball for i in range(6): #print(jointHandles[i]) #print(targetPos0[i]) sim.simxSetJointTargetPosition(clientID, jointHandles[i][1], 0, sim.simx_opmode_streaming) sim.simxSetJointTargetVelocity(clientID, jointHandles[i][1], targetVel[i], sim.simx_opmode_streaming) sim.simxPauseCommunication(clientID, False) ##Inverse Kinematics experiment and analysis: CAN BE COMMENTED OUT ball_pos[0, :] = (-2 * left + 1) * ball_pos[0, :] ball_pos[2, :] = ball_pos[2, :] + 0.15 end_pose = np.array(end_pose).reshape((3, 1)) print("Desired position: \n", ball_pos[:3]) print("End-effector position: \n", end_pose[:3]) # print("Error (MMSE): ", np.linalg.norm(ball_pos[:3] - end_pose[:3])) # x_err = np.abs((end_pose[0,:] - ball_pos[0,:])/(ball_pos[0,:])) * 100 # y_err = np.abs((end_pose[1,:] - ball_pos[1,:])/(ball_pos[1,:])) * 100 # z_err = np.abs((end_pose[2,:] - ball_pos[2,:])/(ball_pos[2,:])) * 100 # print("Error in X (%): ", x_err) # print("Error in Y (%): ", y_err) # print("Error in Z (%): ", z_err) # print("Total Error (%): ", (x_err + y_err + z_err)/3) except: print(sys.exc_info()) #hit_thread.join() stop_prog = True print("Exception encountered, stopping program") #ball_thread.join() print("Ball thread stopped") sim.simxGetPingTime(clientID) # Now close the connection to CoppeliaSim: sim.simxFinish(clientID) else: print('Failed connecting to remote API server') print('Program ended') sim.simxGetPingTime(clientID) # Now close the connection to CoppeliaSim: sim.simxFinish(clientID)
def hit_ball(clientID): """ Method to actually hit the ball with the robot """ global hit_wall global ball_coord global ball_linear global ball_hit print("Hitting Ball: ") # #Get handles for detecting object # detect_handle = sim.simxGetObjectHandle(clientID, 'Sphere', sim.simx_opmode_blocking) # #Get handles for detecting the proximity sensor # prox_handle = sim.simxGetObjectHandle(clientID, 'Proximity_sensor', sim.simx_opmode_blocking) #Create an instance to compute the ball trajectory b_ball = bouncing_ball.BouncingBall() #getting joint handles and initializing the joints in the simulation jointHandles = [-1, -1, -1, -1, -1, -1] for i in range(6): jointHandles[i] = sim.simxGetObjectHandle( clientID, 'UR3_joint' + str(i + 1) + '#0', sim.simx_opmode_blocking) for i in range(6): print(jointHandles[i]) #Set-up some of the RML vectors: vel = 90 accel = 20 jerk = 40 currentVel = [0, 0, 0, 0, 0, 0, 0] currentAccel = [0, 0, 0, 0, 0, 0, 0] maxVel = [ vel * math.pi / 180, vel * math.pi / 180, vel * math.pi / 180, vel * math.pi / 180, vel * math.pi / 180, vel * math.pi / 180 ] maxAccel = [ accel * math.pi / 180, accel * math.pi / 180, accel * math.pi / 180, accel * math.pi / 180, accel * math.pi / 180, accel * math.pi / 180 ] maxJerk = [ jerk * math.pi / 180, jerk * math.pi / 180, jerk * math.pi / 180, jerk * math.pi / 180, jerk * math.pi / 180, jerk * math.pi / 180 ] targetVel = [0, 0, 0, 0, 0, 0] sim.simxPauseCommunication(clientID, True) for i in range(6): sim.simxSetJointTargetPosition(clientID, jointHandles[i][1], 0, sim.simx_opmode_streaming) sim.simxSetJointTargetVelocity(clientID, jointHandles[i][1], targetVel[i], sim.simx_opmode_streaming) sim.simxPauseCommunication(clientID, False) time.sleep(2) #start the feedback loop while True: if not hit_wall: print("Ball hasn't hit wall yet") continue if hit_wall: print("Ball has hit the wall") #Predict the position of the ball from the wall and transform it to world coordinates print("Predicting Trajectory ...") pred_pos = b_ball.trajectory(ball_coord[0], ball_coord[1], ball_coord[2], ball_linear) T_prox = np.array([[-1, 0, 0, 0.025], [0, -1, 0, 2.85], [0, 0, 1, .043], [0, 0, 0, 1]]) prox_pos = np.array([[pred_pos[0]], [pred_pos[1]], [pred_pos[2]], [1]]) ball_pos = np.linalg.inv(T_prox) @ prox_pos print(ball_pos) targetPos0_ik = ik.findJointAngles(ball_pos[0, :], ball_pos[1, :] + 0.1, ball_pos[2, :] + 0.1) print(targetPos0_ik) #Invert joint angles if on left side of robot if ball_pos[0] < 0: for i in range(len(targetPos0_ik)): targetPos0_ik[i] = -targetPos0_ik[i] print("Applying the hitting motion") print(ball_pos) #Apply the hitting motion using the new joint angles sim.simxPauseCommunication(clientID, True) for i in range(6): #print(jointHandles[i]) #print(targetPos0[i]) sim.simxSetJointTargetPosition(clientID, jointHandles[i][1], targetPos0_ik[i], sim.simx_opmode_streaming) sim.simxSetJointTargetVelocity(clientID, jointHandles[i][1], targetVel[i], sim.simx_opmode_streaming) sim.simxPauseCommunication(clientID, False) time.sleep(2) #Now, attach a proximity sensor to the racquet and see if it detects a ball. If it does, let's start the #hitting motion #Actually, for now, let's just not worry about this. Let's make sure that the trajectory generation and the ball hitting #works targetPos0_ik[0] = targetPos0_ik[0] + (40 * np.pi / 180 ) #To simulate a hit sim.simxPauseCommunication(clientID, True) for i in range(6): #print(jointHandles[i]) #print(targetPos0[i]) sim.simxSetJointTargetPosition(clientID, jointHandles[i][1], targetPos0_ik[i], sim.simx_opmode_streaming) sim.simxSetJointTargetVelocity(clientID, jointHandles[i][1], targetVel[i], sim.simx_opmode_streaming) sim.simxPauseCommunication(clientID, False) ball_hit = True hit_wall = False