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
