def attach_model(self, model_name):
if self.mjModel is not None:
my_print(
WARNING="MODEL FILE EXIST! OVERWRITTING THE WHOLE MUJOCO FILE")
self.__init__(model_name)
def main():
# ============================================================================= #
# (1A) [GENERATE MODEL]
if args['modelName'] is not None:
model_name = args['modelName']
else:
TL = TransmissionLine(m=0.1, k=1000, N=100)
TL.gen_xml()
model_name = TL.name
my_print(modelName=model_name)
# ============================================================================= #
# ============================================================================= #
# (1C) [RUN SIMULATION]
VISUALIZE = False if args['videoOFF'] else True # Turn-off visualization
# VISUALIZE = True
mySim = Simulation(model_name=model_name,
is_visualize=VISUALIZE,
arg_parse=args)
# [Type #1] Joint Space tracking task
# [Type #2] Cartesian Space tracking task
ctrl = ImpedanceController(mySim.mjModel, mySim.mjData, type="pulse")
trajectory = 0.1 * sp.sin(10 * ctrl.t_sym)
ctrl.set_ZFT(trajectory)
ctrl.set_ctrl_par(K=0.1, B=0)
mySim.attach_controller(ctrl)
mySim.run()
if args['saveDir'] is not None:
mySim.save_simulation_data(args['saveDir'])
shutil.copyfile(Constants.MODEL_DIR + model_name,
args['saveDir'] + model_name)
mySim.reset()
from modules.simulation import Simulation
from modules.controllers import ImpedanceController
from modules.models import TransmissionLine
from modules.utils import (my_print, my_mkdir, args_cleanup, my_rmdir,
str2float, camel2snake, snake2camel)
from modules.constants import Constants
# ============================================================================= #
# ============================================================================= #
# (0B) [SYSTEM SETTINGS]
if sys.version_info[:3] < (
3, 0, 0
): # Simple version check of the python version. python3+ is recommended for this file.
my_print(NOTIFICATION=" PYTHON3+ is recommended for this script ")
# [Printing Format]
prec = 4 # Defining the float precision for print/number comparison.
np.set_printoptions(
linewidth=8000, suppress=True, precision=prec
) # Setting the numpy print options, useful for printing out data with consistent pattern.
args = docopt(__doc__, version=Constants.VERSION) # Parsing the Argument
args = args_cleanup(
args, '--'
) # Cleaning up the dictionary, discard prefix string '--' for the variables
# [TODO] [Moses]
# It might be beneficial, if we have some sort of "parser function", which gets the input args, and save it as the corresponding specific type.
# If video needs to be recorded or data should be saved, then append 'saveDir' element to args dictionary
def __init__(self, model_name=None, is_visualize=True, arg_parse=None):
"""
Default constructor of THIS class
[ARGUMENTS]
[NAME] [TYPE] [DESCRIPTION]
(1) model_name string The xml model file name for running the MuJoCo simulation.
(2) is_visualized boolean Turn ON/OFF the mjViewer (visualizer) of the simulation. This flag is useful when optimizing a simulation.
(3) arg_parse dictionary Dictionary which contains all the arguments given to the main `run.py` script.
"""
if model_name is None:
self.mjModel = None
self.mjSim = None
self.mjData = None
self.mjViewer = None
self.args = arg_parse
my_print(
WARNING=
"MODEL FILE NOT GIVEN, PLEASE INPUT XML MODEL FILE WITH `attach_model` MEMBER FUNCTION"
)
else:
# If model_name is given, then check if there exist ".xml" at the end, if not, append
model_name = model_name + ".xml" if model_name[
-4:] != ".xml" else model_name
self.model_name = model_name
# Based on the model_name, construct the simulation.
self.mjModel = mjPy.load_model_from_path(
self.MODEL_DIR +
model_name) # Loading xml model as and save it as "model"
self.mjSim = mjPy.MjSim(
self.mjModel
) # Construct the simulation environment and save it as "sim"
self.mjData = self.mjSim.data # Construct the basic MuJoCo data and save it as "mjData"
self.mjViewer = mjPy.MjViewerBasic(
self.mjSim
) if is_visualize else None # Construct the basic MuJoCo viewer and save it as "myViewer"
self.args = arg_parse
# Saving the default simulation variables
self.fps = 60 # Frames per second for the mujoco render
self.dt = self.mjModel.opt.timestep # Time step of the simulation [sec]
self.sim_step = 0 # Number of steps of the simulation, in integer [-]
self.update_rate = round(
1 / self.dt / self.fps
) # 1/dt = number of steps N for 1 second simulaiton, dividing this with frames-per-second (fps) gives us the frame step to be updated.
self.g = self.mjModel.opt.gravity # Calling the gravity vector of the simulation environment
# Saving additional model parameters for multiple purposes
self.act_names = self.mjModel.actuator_names
self.geom_names = self.mjModel.geom_names
self.idx_geom_names = [
self.mjModel._geom_name2id[name] for name in self.geom_names
]
self.n_acts = len(self.mjModel.actuator_names)
self.n_limbs = '-'.join(
self.mjModel.body_names).lower().count('arm')
self.run_time = float(
self.args['runTime']) # Run time of the total simulation
self.start_time = float(
self.args['startTime']) # Start time of the movements
self.VISUALIZE = is_visualize # saving the VISUALIZE Flag
def run(self):
"""
Running a single simulation.
[INPUT]
[VAR NAME] [TYPE] [DESCRIPTION]
(1) run_time float The whole run time of the simulation.
(2) ctrl_start_time float
"""
# Check if mjModel or mjSim is empty and raise error
if self.mjModel is None or self.mjSim is None:
raise ValueError(
"mjModel and mjSim is Empty! Add it before running simulation")
# Warn the user if input and output function is empty
if self.controller is None:
raise ValueError("CONTROLLER NOT ATTACHED TO SIMULATION. \
PLEASE REFER TO METHOD 'attach_output_function' and 'attach_controller' "
)
if self.args['recordVideo']:
vid = MyVideo(
fps=self.fps * float(self.args['vidRate']),
vid_dir=self.args['saveDir']
) # If args doesn't have saveDir attribute, save vid_dir as None
if self.args['saveData']:
file = open(self.args['saveDir'] + "data_log.txt", "w+")
# Setting the camera position for the simulation
# [camParameters]: [ 0.17051, 0.21554, -0.82914, 2.78528,-30.68421,162.42105 ]
# [camParameters]: [ -0.10325, 0. , -2.51498, 7.278 ,-45. , 90. ]
if self.args['camPos'] is not None:
tmp = str2float(self.args['camPos'])
self.mjViewer.cam.lookat[0:3] = tmp[0:3]
self.mjViewer.cam.distance = tmp[3]
self.mjViewer.cam.elevation = tmp[4]
self.mjViewer.cam.azimuth = tmp[5]
self.set_initial_condition(
) # Setting initial condition. Some specific controllers need to specify the initial condition
while self.current_time <= self.run_time:
if self.sim_step % self.update_rate == 0:
if self.mjViewer is not None:
self.mjViewer.render() # Render the simulation
my_print(currentTime=self.current_time)
# my_print( camParameters = [ self.mjViewer.cam.lookat[ 0 ], self.mjViewer.cam.lookat[ 1 ], self.mjViewer.cam.lookat[ 2 ],
# self.mjViewer.cam.distance, self.mjViewer.cam.elevation, self.mjViewer.cam.azimuth ] )
if self.args['recordVideo']:
vid.write(self.mjViewer)
if self.args['saveData']:
my_print(currentTime=self.current_time,
jointAngleActual=self.mjData.qpos[:],
inputx0=self.controller.x0,
geomXYZPositions=self.mjData.geom_xpos[
self.idx_geom_names],
file=file)
# [input controller]
# input_ref: The data array that are aimed to be inputted (e.g., qpos, qvel, qctrl etc.)
# input_idx: The specific index of input_ref data array that should be inputted
# input: The actual input value which is inputted to input_ref
input_ref, input_idx, input = self.controller.input_calc(
self.current_time)
input_ref[input_idx] = input
self.mjSim.step() # Single step update
if (self.is_sim_unstable()): # Check if simulation is stable
# If not optimization, and result unstable, then save the detailed data
print(
"[WARNING] UNSTABLE SIMULATION, HALTED AT {0:f} for at {1:f}"
.format(self.current_time, self.run_time))
if self.args['saveData']:
print(
"[WARNING] UNSTABLE SIMULATION, HALTED AT {0:f} for at {1:f}"
.format(self.current_time, self.run_time),
file=file)
file.close()
break
self.current_time = self.mjData.time # Update the current_time variable of the simulation
if self.sim_step % self.update_rate == 0:
if self.args['saveData']:
# Saving all the necessary datas for the simulation
my_print(inputVal=input, file=file)
self.sim_step += 1
if self.args['recordVideo']:
vid.release() # If simulation is finished, wrap-up the video file.
if self.args['saveData']:
file.close()