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")