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