def __init__(self, xml_file, n_robots=1, folder=None, use_sim_state=True, force_download=False): """ Loads the Mujoco model from the specified xml file Parameters ---------- xml_file: string the name of the arm model to load. If folder remains as None, the string passed in is parsed such that everything up to the first underscore is used for the arm directory, and the full string is used to load the xml within that folder. EX: 'myArm' and 'myArm_with_gripper' will both look in the 'myArm' directory, however they will load myArm.xml and myArm_with_gripper.xml, respectively If a folder is passed in, then folder/xml_file is used robot_num: int (Default: 1) number of manipulators (Used in multi-robot env) folder: string, Optional (Default: None) specifies what folder to find the xml_file, if None specified will checking in abr_control/arms/xml_file (see above for xml_file) use_sim_state: Boolean, optional (Default: True) If set true, the q and dq values passed in to the functions are ignored, and the current state of the simulator is used to calculate all functions. Can speed up simulation by not resetting the state on every call. force_download: boolean, Optional (Default: False) True to force downloading the mesh and texture files, useful when new files are added that may be missing. False: if the meshes folder is missing it will ask the user whether they want to download them """ if folder is None: arm_dir = xml_file.split("_")[0] current_dir = os.path.dirname(__file__) self.xml_file = os.path.join(current_dir, arm_dir, "%s.xml" % xml_file) self.xml_dir = "%s/%s" % (current_dir, arm_dir) else: self.xml_dir = "%s" % (folder) self.xml_file = os.path.join(self.xml_dir, xml_file) self.N_ROBOTS = n_robots self.N_GRIPPEPR_JOINTS = 0 # get access to some of our custom arm parameters from the xml definition tree = ElementTree.parse(self.xml_file) root = tree.getroot() for custom in root.findall("custom/numeric"): name = custom.get("name") if name == "START_ANGLES": START_ANGLES = custom.get("data").split(" ") self.START_ANGLES = np.array([float(angle) for angle in START_ANGLES]) elif name == "N_GRIPPER_JOINTS": self.N_GRIPPER_JOINTS = int(custom.get("data")) # check for google_id specifying download location of robot mesh files self.google_id = None for custom in root.findall("custom/text"): name = custom.get("name") if name == "google_id": self.google_id = custom.get("data") # check if the user has downloaded the required mesh files # if not prompt them to do so if self.google_id is not None: # get list of expected files to check if all have been downloaded files = [] for asset in root.findall("asset/mesh"): files.append(asset.get("file")) for asset in root.findall("asset/texture"): # assuming that texture are placed in the meshes folder files.append(asset.get("file").split("/")[1]) # check if our mesh folder exists, then check we have all the files download_meshes.check_and_download( name=self.xml_dir + "/meshes", google_id=self.google_id, force_download=force_download, files=files, ) self.model = mjp.load_model_from_path(self.xml_file) self.use_sim_state = use_sim_state
def connect(self, load_scene=True, force_download=False): """ Connect to the current scene open in CoppeliaSim Finds the CoppeliaSim references to the joints of the robot. Sets the time step for simulation and put into lock-step mode. NOTE: the joints and links must follow the naming convention of 'joint#' and 'link#', starting from 'joint0' and 'link0' NOTE: The dt in the CoppeliaSim physics engine must also be specified to be less than the dt used here. """ # close any open connections sim.simxFinish(-1) # Connect to the V-REP continuous server self.clientID = sim.simxStart("127.0.0.1", 19997, True, True, 500, 5) if self.clientID == -1: raise Exception( "Failed connecting to CoppeliaSim remote API server") if load_scene: # if there's a google id, check for files and download if missing if self.robot_config.google_id != "None": download_meshes.check_and_download( name=self.robot_config.filename, google_id=self.robot_config.google_id, force_download=force_download, ) # load the scene specified in the config sim.simxLoadScene(self.clientID, self.robot_config.filename, 0, self.blocking) sim.simxSynchronous(self.clientID, True) # get the handles for each joint and set up streaming self.joint_handles = [ sim.simxGetObjectHandle(self.clientID, name, self.blocking)[1] for name in self.robot_config.JOINT_NAMES ] # get handle for target and set up streaming _, self.misc_handles["target"] = sim.simxGetObjectHandle( self.clientID, "target", self.blocking) # get handle for hand _, self.hand_handle = sim.simxGetObjectHandle(self.clientID, "hand", self.blocking) sim.simxSetFloatingParameter( self.clientID, sim.sim_floatparam_simulation_time_step, self.dt, # specify a simulation time step sim.simx_opmode_oneshot, ) sim.simxSetBooleanParameter( self.clientID, sim.sim_boolparam_display_enabled, True, sim.simx_opmode_oneshot, ) # start our simulation in lockstep with our code sim.simxStartSimulation(self.clientID, self.blocking) print("Connected to CoppeliaSim remote API server")