def getObjPos(self): e, handle = vrep.simxGetObjectHandle(self.CLIENT_ID, 'MP_BODY_respondable', vrep.simx_opmode_oneshot_wait) _, pos = vrep.simxGetObjectPosition(self.CLIENT_ID, handle, -1, vrep.simx_opmode_oneshot_wait) return handle, pos
def __init__(self): self._low_degrees_bound = 0 self._low_radians_bound = 0 self._high_degrees_bound = 180 self._high_radians_bound = np.pi # Get client id from the V-REP server-side vrep.simxFinish(-1) self.conn_handler = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) if self.conn_handler == -1: print('Failed connecting to the remote API server') return -1 print('Succesfully connected to the remote API server...') # Get UARM entity # Implemented it in the dict style for future purposes # (if there're several robots in the env, call them like # env.robot['uarm'], env.robot['universal_robotics'], etc) uarm = UARM(self.conn_handler) self.robot = {'uarm': uarm} # Get cameras from V-REP # vs_0 - global view, # vs_1 - details basket, # vs_2 - destination basket, self.camera_handlers = [] for i in range(3): err, handler = vrep.simxGetObjectHandle( self.conn_handler, 'vs_' + str(i), vrep.simx_opmode_oneshot_wait) if err == vrep.simx_return_ok: self.camera_handlers.append(handler) else: print('Error getting camera handlers, ERR:', err) return
def get_handles(): global MOTOR_HANDLES jointNames = { 1: 'j_shoulder_r', 2: 'j_shoulder_l', 3: 'j_high_arm_r', 4: 'j_high_arm_l', 5: 'j_low_arm_r', 6: 'j_low_arm_l', 7: 'j_pelvis_r', 8: 'j_pelvis_l', 9: 'j_thigh1_r', 10: 'j_thigh1_l', 11: 'j_thigh2_r', 12: 'j_thigh2_l', 13: 'j_tibia_r', 14: 'j_tibia_l', 15: 'j_ankle1_r', 16: 'j_ankle1_l', 17: 'j_ankle2_r', 18: 'j_ankle2_l', 19: 'j_pan', 20: 'j_tilt' } for i in range(1, 21): _, handle = vrep.simxGetObjectHandle(CLIENT_ID, jointNames[i], vrep.simx_opmode_oneshot_wait) MOTOR_HANDLES[i] = handle
def reset(self): e, handle = vrep.simxGetObjectHandle(self.CLIENT_ID, 'MP_BODY_respondable', vrep.simx_opmode_oneshot_wait) vrep.simxRemoveModel(self.CLIENT_ID, handle, vrep.simx_opmode_blocking) vrep.simxLoadModel(self.CLIENT_ID, 'darwin.ttm', 0, vrep.simx_opmode_blocking)
def getEuler(self): e, handle = vrep.simxGetObjectHandle(self.CLIENT_ID, 'MP_BODY_respondable', vrep.simx_opmode_oneshot_wait) print handle _, euler = vrep.simxGetObjectOrientation(self.CLIENT_ID, handle, -1, vrep.simx_opmode_oneshot_wait) print euler
def get_handles(self): for i in range(1, 21): e, handle = vrep.simxGetObjectHandle(self.CLIENT_ID, self.jointNames[i], vrep.simx_opmode_oneshot_wait) if e != 0: print "Error = ", e exit(1) self.MOTOR_HANDLES[i] = handle for i in self.objectnames: e, handle = vrep.simxGetObjectHandle(self.CLIENT_ID, i, vrep.simx_opmode_oneshot_wait) if e != 0: print "Error = ", e exit(1) self.OBJECT_HANDLES.append(handle)
def start_motors(self): """ Function to start the motors. Returns: A dictionary that contains both motors handle ID. """ res, left_handle = vrep.simxGetObjectHandle(self.clientID, "Pioneer_p3dx_leftMotor", vrep.simx_opmode_oneshot_wait) if(res != vrep.simx_return_ok): print("\033[93m Left motor not connected.") else: print("\033[92m Left motor connected.") res, right_handle = vrep.simxGetObjectHandle(self.clientID, "Pioneer_p3dx_rightMotor", vrep.simx_opmode_oneshot_wait) if(res != vrep.simx_return_ok): print("\033[93m Right motor not connected.") else: print("\033[92m Right motor connected.") return {"left": left_handle, "right":right_handle}
def start_robot(self): """ Function to start the robot. Returns: robot_handle: Contains the robot handle ID. """ res, robot_handle = vrep.simxGetObjectHandle(self.clientID, "Pioneer_p3dx", vrep.simx_opmode_oneshot_wait) if(res != vrep.simx_return_ok): print("\033[93m Robot not connected.") else: print("\033[92m Robot connected.") return robot_handle
def start_sensors(self): """ Function to start the sensors. Returns: us_handle: List that contains each ultrassonic sensor handle ID. vision_handle: Contains the vision sensor handle ID. laser_handle: Contains the laser handle ID. """ #Starting ultrassonic sensors us_handle = [] sensor_name=[] for i in range(0,16): sensor_name.append("Pioneer_p3dx_ultrasonicSensor" + str(i+1)) res, handle = vrep.simxGetObjectHandle(self.clientID, sensor_name[i], vrep.simx_opmode_oneshot_wait) if(res != vrep.simx_return_ok): print ("\033[93m "+ sensor_name[i] + " not connected.") else: print ("\033[92m "+ sensor_name[i] + " connected.") us_handle.append(handle) #Starting vision sensor res, vision_handle = vrep.simxGetObjectHandle(self.clientID, "Vision_sensor", vrep.simx_opmode_oneshot_wait) if(res != vrep.simx_return_ok): print ("\033[93m Vision sensor not connected.") else: print ("\033[92m Vision sensor connected.") #Starting laser sensor res, laser_handle = vrep.simxGetObjectHandle(self.clientID, "fastHokuyo", vrep.simx_opmode_oneshot_wait) if(res != vrep.simx_return_ok): print ("\033[93m Laser not connected.") else: print ("\033[92m Laser connected.") return us_handle, vision_handle, laser_handle
def get_handles(self): cam_rgb_names = ["Camera"] cam_rgb_handles = [ vrep.simxGetObjectHandle(self.client_id, n, vrep.simx_opmode_blocking)[1] for n in cam_rgb_names ] octree_handle = vu.get_handle_by_name(self.client_id, 'Octree') vision_sensor = vu.get_handle_by_name(self.client_id, 'Vision_sensor') return dict( octree=octree_handle, cam_rgb=cam_rgb_handles, vision_sensor=vision_sensor, )
def get_handles(self): cam_rgb_names = ["Camera"] cam_rgb_handles = [ vrep.simxGetObjectHandle(self.client_id, n, vrep.simx_opmode_blocking)[1] for n in cam_rgb_names ] octree_handle = vu.get_handle_by_name(self.client_id, 'Octree') anchor_obj_handle = vu.get_handle_by_name(self.client_id, 'Cuboid0') other_obj_handle = vu.get_handle_by_name(self.client_id, 'Cuboid') return dict( anchor_obj=anchor_obj_handle, other_obj=other_obj_handle, octree=octree_handle, cam_rgb=cam_rgb_handles, )
def __init__(self, connection_handler): """ Get all V-REP handlers for the UARM manipulator Arguments: connection_handler -- handler containing connection identificator with server-side V-REP """ self.conn_handler = connection_handler self.motors_number = 4 self.motor_handlers = [] self.gripper_handler = None for i in range(0, self.motors_number): error, vrep_motor_handler = vrep.simxGetObjectHandle( self.conn_handler, 'uarm_motor' + str(i + 1), vrep.simx_opmode_oneshot_wait) if error == vrep.simx_return_ok: self.motor_handlers.append(vrep_motor_handler) else: print('Error getting motor handlers, ERR: ' + str(error)) return self.gripper_handler = 'uarm_suctionCup'
def get_handle_by_name(clientID, name): response, handle = vrep.simxGetObjectHandle(clientID, name, vrep.simx_opmode_blocking) assert response == 0, 'Expected return code of 0, but instead return code was {}'.format( response) return handle
vrep.simxFinish(-1) # just in case, close all opened connections client_id = vu.connect_to_vrep() print(client_id) res = vu.load_scene(client_id, '/home/mohit/projects/cooking/code/sim_vrep/scene_0.ttt') print(res) if res != vrep.simx_return_ok: print("Error in loading scene: {}".format(res)) vu.start_sim(client_id) cam_rgb_names = ["Camera"] cam_rgb_handles = [ vrep.simxGetObjectHandle(client_id, n, vrep.simx_opmode_blocking)[1] for n in cam_rgb_names ] print("Camera handle: {}".format(cam_rgb_handles)) octree_handle = vu.get_handle_by_name(client_id, 'Octree') print('Octree handle: {}'.format(octree_handle)) voxel_list = [] for t in range(100): vu.step_sim(client_id) res = \ vrep.simxCallScriptFunction( client_id, "Octree", vrep.sim_scripttype_childscript,
def setEuler(self): e, handle = vrep.simxGetObjectHandle(self.CLIENT_ID, 'MP_BODY_respondable', vrep.simx_opmode_oneshot_wait) vrep.simxSetObjectOrientation(self.CLIENT_ID, handle, -1, [0, 0, 0], vrep.simx_opmode_oneshot_wait)
def setObjPos(self): e, handle = vrep.simxGetObjectHandle(self.CLIENT_ID, 'MP_BODY_respondable', vrep.simx_opmode_oneshot_wait) vrep.simxSetObjectPosition(self.CLIENT_ID, handle, -1, [0.6, 0.6, 0.6], vrep.simx_opmode_oneshot_wait)
print "Error = ",e exit(1) MOTOR_HANDLES[i] = handle def getPos(): angles = [] for i in MOTOR_HANDLES.keys(): _,angle = vrep.simxGetJointPosition(CLIENT_ID,MOTOR_HANDLES[i],vrep.simx_opmode_oneshot_wait) angles.append(math.degrees(angle)) return angles def setPos(writ): for key,val in writ.items(): vrep.simxSetJointTargetPosition(CLIENT_ID,MOTOR_HANDLES[key],math.radians(val),vrep.simx_opmode_oneshot_wait) connectVrep() get_handles() state = getPos() print(state) angle = 5 errCode,motor2 =vrep.simxGetObjectHandle(CLIENT_ID,'j_shoulder_r',vrep.simx_opmode_oneshot) vrep.simxSetJointTargetVelocity(CLIENT_ID,motor2,-angle,vrep.simx_opmode_oneshot) time.sleep(3) state = getPos() print(state)