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)
