def getJointAngle(self, joint_name):
"""
:param: vrep_sim: int
the vrep_sim handle
:param: client_ID: int
the client ID
:param: joint_name: str
the joint name: can be left_hip, left_knee, right_hip, right_knee
"""
q = 0
if joint_name == 'left_hip':
return_code, q = vrep_sim.simxGetJointPosition(
self.client_ID, self.left_hip_joint_handle,
vrep_sim.simx_opmode_buffer)
elif joint_name == 'left_knee':
return_code, q = vrep_sim.simxGetJointPosition(
self.client_ID, self.left_knee_joint_handle,
vrep_sim.simx_opmode_buffer)
elif joint_name == 'right_hip':
return_code, q = vrep_sim.simxGetJointPosition(
self.client_ID, self.right_hip_joint_handle,
vrep_sim.simx_opmode_buffer)
elif joint_name == 'right_knee':
return_code, q = vrep_sim.simxGetJointPosition(
self.client_ID, self.right_knee_joint_handle,
vrep_sim.simx_opmode_buffer)
else:
print('Error: joint name: \' ' + joint_name +
'\' can not be recognized.')
return q
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 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 control_gripper(self,
gripper_joint_handle_1,
gripper_joint_handle_2,
action_to_take,
action_speed_multiplier=1.0):
# We identify the position of both the gripper joints
error_code_1, position_1 = sim.simxGetJointPosition(
self.client_id, gripper_joint_handle_1, sim.simx_opmode_blocking)
error_code_2, position_2 = sim.simxGetJointPosition(
self.client_id, gripper_joint_handle_2, sim.simx_opmode_blocking)
# We report if any of the error codes are not matching
if error_code_1 != 0 or error_code_2 != 0:
# We warn the user that the handle(s) had an error
warn("Gripper joint handle error detected")
# We check which action is to be taken
if action_to_take == 'close':
if position_1 < position_2 - 0.008:
sim.simxSetJointTargetVelocity(self.client_id,
gripper_joint_handle_1,
-0.01 * action_speed_multiplier,
sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(self.client_id,
gripper_joint_handle_2,
-0.04 * action_speed_multiplier,
sim.simx_opmode_oneshot)
else:
sim.simxSetJointTargetVelocity(self.client_id,
gripper_joint_handle_1,
-0.04 * action_speed_multiplier,
sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(self.client_id,
gripper_joint_handle_2,
-0.04 * action_speed_multiplier,
sim.simx_opmode_oneshot)
elif action_to_take == 'open':
if position_1 < position_2:
sim.simxSetJointTargetVelocity(self.client_id,
gripper_joint_handle_1,
0.04 * action_speed_multiplier,
sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(self.client_id,
gripper_joint_handle_2,
0.02 * action_speed_multiplier,
sim.simx_opmode_oneshot)
else:
sim.simxSetJointTargetVelocity(self.client_id,
gripper_joint_handle_1,
0.02 * action_speed_multiplier,
sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(self.client_id,
gripper_joint_handle_2,
0.04 * action_speed_multiplier,
sim.simx_opmode_oneshot)
else:
# We warn the user that the input was invalid for controlling the gripper
warn("No action taken on gripper due to invalid inputs")
def getJointPosition(self, handle, ignoreError = False, initialize = False):
if initialize:
sim.simxGetJointPosition(self.clientID, handle, sim.simx_opmode_streaming)
res, out_pos = sim.simxGetJointPosition(self.clientID, handle, sim.simx_opmode_buffer)
if res!=sim.simx_return_ok and not ignoreError:
print('Failed to get joint angle')
print(res)
return out_pos
def get_angles_firsttime():
for i in range(12):
print('handler is')
print(int(angles_handler[i]))
trash, ang = sim.simxGetJointPosition(clientID, int(angles_handler[i]),
sim.simx_opmode_streaming)
res = sim.simx_return_novalue_flag
while res != sim.simx_return_ok:
res, ang1 = sim.simxGetJointPosition(clientID,
int(angles_handler[i]),
sim.simx_opmode_buffer)
print("Angle x IS")
print(ang1)
def get_current_position(self):
right_pos = []
left_pos = []
for x in range(0, 7):
error_code, right_temp_pos = vrep.simxGetJointPosition(
self.clientID, self.right_joint_array[x],
vrep.simx_opmode_buffer)
right_pos.append(right_temp_pos)
error_code, left_temp_pos = vrep.simxGetJointPosition(
self.clientID, self.left_joint_array[x],
vrep.simx_opmode_buffer)
left_pos.append(left_temp_pos)
return right_pos, left_pos
def readOdometry(clientID, leftMotor, rightMotor, l_rot_prev, r_rot_prev):
ret, l_rot_cur = sim.simxGetJointPosition(clientID, leftMotor,
sim.simx_opmode_oneshot_wait)
ret, r_rot_cur = sim.simxGetJointPosition(clientID, rightMotor,
sim.simx_opmode_oneshot_wait)
dPhiL = getSmallestAngle(l_rot_cur, l_rot_prev)
dPhiR = getSmallestAngle(r_rot_cur, r_rot_prev)
l_rot_prev = l_rot_cur
r_rot_prev = r_rot_cur
return dPhiL, dPhiR, l_rot_prev, r_rot_prev
def getCurrentJointAngle():
retCode = -1
jointA = 0
while retCode != sim.simx_return_ok:
retCode, jointA = sim.simxGetJointPosition(
client.id, client.jointHandle, sim.simx_opmode_buffer)
return jointA
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 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 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 curPos(self, opm):
pos = []
for joint in self.joints:
error, theta = sim.simxGetJointPosition(self.clientID, joint, opm)
pos.append(theta)
self.theta = pos
return
def compute_jacobian(self, targetPos, targetRot):
# target position in global frame
x, y, z = targetPos
rx, ry, rz = targetRot
target = np.array([[x, y, z, rx, ry, rz]])
# end effector position in global frame
res, endEffPos = sim.simxGetObjectPosition(self.clientID,
self.endEffector,
self.worldCoord,
sim.simx_opmode_blocking)
x, y, z = endEffPos
res, endEffRot = sim.simxGetObjectOrientation(self.clientID,
self.endEffector,
self.worldCoord,
sim.simx_opmode_blocking)
rx, ry, rz = endEffRot
endEffectorPos = np.array([[x, y, z, rx, ry, rz]])
error = target - endEffectorPos
# start computing jacobian
jacobian = []
joint_q = []
res, trans1 = sim.simxGetObjectPosition(
self.clientID, self.endEffector, self.worldCoord,
sim.simx_opmode_blocking) # end effector in world
for idx in range(len(self.joints)):
# compute local jacobian
local_rot_axis = self.rot_axis[idx]
# find the current joint configuration
res, local_config = sim.simxGetJointPosition(
self.clientID, self.joints[idx], sim.simx_opmode_blocking)
joint_q.append((local_config))
# find transform matrix
res, rot = sim.simxGetObjectOrientation(self.clientID,
self.joints[idx],
self.worldCoord,
sim.simx_opmode_blocking)
rot_mat = util.get_rotation_matrix(rot)
# rotation axis in world coordinate
local_rot_axis = rot_mat.dot(local_rot_axis).flatten()
# joint position in world
res, trans2 = sim.simxGetObjectPosition(self.clientID,
self.joints[idx],
self.worldCoord,
sim.simx_opmode_blocking)
# find the translation from local position to world position
zi = np.array(trans1) - np.array(trans2)
j1 = np.cross(local_rot_axis, zi)
local_jacobian = np.hstack([j1, local_rot_axis])
jacobian.append(local_jacobian)
jacobian = np.array(jacobian).T
joint_q = np.array([joint_q])
# INVERSE JACOBIAN
return jacobian, error, joint_q
def getJointPosition(self):
res = [-1, -1, -1, -1, -1, -1]
for i in self.jointNo:
err, v = vrep.simxGetJointPosition(self.clientId, self.jointId[i],
vrep.simx_opmode_oneshot_wait)
assert (err != -1), "Can't get position"
res[i] = v
return res
def getAllJointAngles(self):
q = [0, 0, 0, 0, 0, 0]
for i in range(6):
return_code, q[i] = vrep_sim.simxGetJointPosition(
self.client_ID, self.joint_handle[i],
vrep_sim.simx_opmode_buffer)
return q
def armsMovement(clientID, left_joint, right_joint, isRescueDone):
res_left_joint, left_joint_pos = sim.simxGetJointPosition(
clientID, left_joint, sim.simx_opmode_oneshot)
res_right_joint, right_joint_pos = sim.simxGetJointPosition(
clientID, right_joint, sim.simx_opmode_oneshot)
if (not isRescueDone):
# Ground robot opens arms before start looking for Mr.York
if (left_joint_pos < (np.pi - 0.15)) and (right_joint_pos < 0.15):
if (res_left_joint
== sim.simx_return_ok) and (res_right_joint
== sim.simx_return_ok):
sim.simxSetJointTargetVelocity(clientID, left_joint, 0.3,
sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(clientID, right_joint, -0.3,
sim.simx_opmode_oneshot)
else:
sim.simxSetJointTargetVelocity(clientID, left_joint, 0.0,
sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(clientID, right_joint, 0.0,
sim.simx_opmode_oneshot)
# Once the ground robot's arms are opened, it is ready to start moving around
return True, False
else:
# The arms start closing to hug Mr. York
if (res_left_joint == sim.simx_return_ok) and (res_right_joint
== sim.simx_return_ok):
sim.simxSetJointTargetVelocity(clientID, left_joint, -0.3,
sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(clientID, right_joint, 0.3,
sim.simx_opmode_oneshot)
left_joint_pos = sim.simxGetJointPosition(clientID, left_joint,
sim.simx_opmode_oneshot)[1]
right_joint_pos = sim.simxGetJointPosition(clientID, right_joint,
sim.simx_opmode_oneshot)[1]
if (left_joint_pos < 0.0) and (right_joint_pos > 0.05):
sim.simxSetJointTargetVelocity(clientID, left_joint, 0.0,
sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(clientID, right_joint, 0.0,
sim.simx_opmode_oneshot)
return False, True
return False, False
def get_joints(self):
theta = []
if self.is_connected():
for i in range(6):
_, val = sim.simxGetJointPosition(self.clientId,
self.joints[i],
sim.simx_opmode_oneshot_wait)
theta.append(val)
return np.array(theta)
def getJointAngles(robot):
theta = []
for joint in robot[1]:
result, angle = sim.simxGetJointPosition(clientID, joint,
sim.simx_opmode_blocking)
if result != sim.simx_return_ok:
raise Exception('could not get' + str(joint + 1) +
'joint variable')
theta.append(angle)
return theta
def showJointAngles(self):
RAD2DEG = self.RAD2DEG
jointNum = self.jointNum
clientID = self.clientID
jointHandle = self.jointHandle
for i in range(jointNum):
_, jpos = vrep.simxGetJointPosition(clientID, jointHandle[i],
vrep.simx_opmode_blocking)
print(round(float(jpos) * RAD2DEG, 2), end=' ')
print('\n')
def _init_pos(self):
# print positions
for i, handle in enumerate(self.joint_handles, start=1):
print(
f"m{i}_pos",
sim.simxGetJointPosition(self.clientID, handle, streaming)[1])
print(f"Faulty joints {self.faulty_joints}")
print(
"Distance from Green Box: ",
sim.simxGetObjectPosition(self.clientID, self.lamp_end,
self.green_box, streaming)[1])
def __init__(self):
jointNum = self.jointNum
baseName = self.baseName
rgName = self.rgName
jointName = self.jointName
camera_rgb_Name = self.camera_rgb_Name
camera_depth_Name = self.camera_depth_Name
print('Simulation started')
vrep.simxFinish(-1) # 关闭潜在的连接
# 每隔0.2s检测一次, 直到连接上V-rep
while True:
# simxStart的参数分别为：服务端IP地址(连接本机用127.0.0.1);端口号;是否等待服务端开启;连接丢失时是否尝试再次连接;超时时间(ms);数据传输间隔(越小越快)
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID > -1:
print("Connection success!")
break
else:
time.sleep(0.2)
print("Failed connecting to remote API server!")
print("Maybe you forget to run the simulation on vrep...")
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot) # 仿真初始化
# 读取Base和Joint的句柄
jointHandle = np.zeros((jointNum, 1), dtype=np.int)
for i in range(jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
clientID, jointName + str(i + 1), vrep.simx_opmode_blocking)
jointHandle[i] = returnHandle
_, baseHandle = vrep.simxGetObjectHandle(clientID, baseName,
vrep.simx_opmode_blocking)
_, rgHandle = vrep.simxGetObjectHandle(clientID, rgName,
vrep.simx_opmode_blocking)
_, cameraRGBHandle = vrep.simxGetObjectHandle(
clientID, camera_rgb_Name, vrep.simx_opmode_blocking)
_, cameraDepthHandle = vrep.simxGetObjectHandle(
clientID, camera_depth_Name, vrep.simx_opmode_blocking)
# 读取每个关节角度
jointConfig = np.zeros((jointNum, 1))
for i in range(jointNum):
_, jpos = vrep.simxGetJointPosition(clientID, jointHandle[i],
vrep.simx_opmode_blocking)
jointConfig[i] = jpos
self.clientID = clientID
self.jointHandle = jointHandle
self.rgHandle = rgHandle
self.cameraRGBHandle = cameraRGBHandle
self.cameraDepthHandle = cameraDepthHandle
self.jointConfig = jointConfig
def get_adometry_feedback(self):
"""Получить обратную связь по положению
Returns
-------
ndarray
Обобщённые координаты
"""
pos = np.zeros((JOINT_COUNT))
for i in range(JOINT_COUNT):
_, pos[i] = sim.simxGetJointPosition(self.client, self.joints[i],
sim.simx_opmode_blocking)
return self.clip_position(pos)
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 _init_pos(self):
# Set to streaming mode
sim.simxGetObjectPosition(self.clientID, self.hexapod_handle, -1,
streaming)
for handle in self.joint_handles.values():
sim.simxGetJointPosition(self.clientID, handle, streaming)
time.sleep(1)
# Get the positions now
self.init_pos = sim.simxGetObjectPosition(self.clientID,
self.hexapod_handle, -1,
buffer)[1]
print("Hexapod init Pos(Relative to world): ", self.init_pos)
# Make the faulty joints invisible
if self.test:
for joint in self.faulty_joints:
handle = list(self.joint_handles.values())[joint]
sim.simxSetModelProperty(
self.clientID, handle,
simConst.sim_modelproperty_not_visible, blocking)
print(f"Faulty joints {self.faulty_joints}")
def step(self, action):
if action <=5:
x,y,z = self.moves[action]
self.S = [self.S[0] + x, self.S[1] + y, self.S[2] + z]
if self.S[0]<0 or self.S[1]<0 or self.S[2]<0:
self.S = [max(0, self.S[0]), max(0, self.S[1]), max(0, self.S[2])]
return self.S,-10,False,{}
if self.S[0]>=5 or self.S[1]>=5 or self.S[2]>=10:
self.S = [min(self.S[0], 5 - 1), min(self.S[1], 5 - 1), min(self.S[2], 10 - 1)]
return self.S,-10,False,{}
else:
sim.simxSetIntegerSignal(self.clientID,'actuateGripperSignal',0,sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(self.clientID, self.target,-1, [0+0.1*self.S[0],0+0.1*self.S[1],0+0.1*self.S[2]],sim.simx_opmode_oneshot) #starting point is (0.3,0.3,0.3)
while True:
distance = sim.simxGetObjectPosition(self.clientID,self.copter,self.target,sim.simx_opmode_oneshot_wait)
if np.abs(np.linalg.norm(np.array(distance[1])))<0.02:
time.sleep(2)
break
# if (sim.simxGetCollisionHandle(self.clientID,))
return self.S,-1,False,{}
elif action==6:
# pdb.set_trace()
gripperAngle = 150
angle_displacement = (gripperAngle/2-45)*math.pi/180
err, joint_6 = sim.simxGetObjectHandle(self.clientID, "Fourbar_joint6", sim.simx_opmode_oneshot)
# sim.simxClearIntegerSignal(self.clientID,'actuateGripperSignal',sim.simx_opmode_oneshot)
# sim.simxClearIntegerSignal(self.clientID,'gripperAngle',sim.simx_opmode_oneshot)
sim.simxSetIntegerSignal(self.clientID,'actuateGripperSignal',1,sim.simx_opmode_oneshot)
sim.simxSetIntegerSignal(self.clientID,'gripperAngle',gripperAngle,sim.simx_opmode_oneshot)
while True:
current_joint6_angle = sim.simxGetJointPosition(self.clientID, joint_6, sim.simx_opmode_oneshot)
if abs(abs(current_joint6_angle[1])-abs(angle_displacement))<0.05*math.pi/180:
time.sleep(5)
break
oldPos = sim.simxGetObjectPosition(self.clientID, self.sphere, -1, sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(self.clientID, self.target, self.copter, [0,0,0.5], sim.simx_opmode_oneshot)
while True:
distance = sim.simxGetObjectPosition(self.clientID,self.copter,self.target,sim.simx_opmode_oneshot_wait)
if np.abs(np.linalg.norm(np.array(distance[1])))<0.02:
time.sleep(2)
break
newPos = sim.simxGetObjectPosition(self.clientID, self.sphere, -1, sim.simx_opmode_oneshot)
if np.linalg.norm(np.array(newPos[1])-np.array(oldPos[1]))>0.4 and np.linalg.norm(np.array(newPos)-np.array(oldPos))<0.5:
return self.S,100,True,{}
else:
return self.S,-100,True,{}
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 check(self, pose):
joints_error = [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)
self.sim_joints[1] -= pi / 2
self.sim_joints[3] -= pi / 2
for i in range(6):
joints_error[i] = pose[i] - self.sim_joints[i]
print(joints_error)
return joints_error
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))
def getJointAngle(self, joint_name):
"""
:param: joint_name: str
the joint name: can be UR5_joint1 ~ UR5_joint6
"""
for i in range(6):
if joint_name == 'UR5_joint' + str(i + 1):
return_code, q = vrep_sim.simxGetJointPosition(
self.client_ID, self.joint_handle[i],
vrep_sim.simx_opmode_buffer)
return q
# can not find the joint
print('Error: joint name: \' ' + joint_name +
'\' can not be recognized.')
return 0
