def __init__(self, filename): # Parsing the dwl control suite configuration and initialization self.parseConfigFile(filename) self.loop_sim = False self.fixed_base = False self.t = 0. # Creating the bullet clien and configure it if self.enable_gui: self.client = pb.connect(pb.GUI, options='--opengl2') else: self.client = pb.connect(pb.DIRECT) # Creating and configurating the bullet interface self.biface = BulletInterface(self.client) self.biface.setGravity(self.gravity[dwl.X], self.gravity[dwl.Y], self.gravity[dwl.Z]) self.biface.setTimeStep(self.time_step) self.biface.resetDebugVisualizerCamera(self.cam_dist, self.cam_yaw, self.cam_pitch, self.cam_target.tolist()) # Loading the ground model pb.setAdditionalSearchPath(pybullet_data.getDataPath()) self.plane_id = pb.loadURDF('plane.urdf') # Loading the robot model to dwl and simulator self.fbs = dwl.FloatingBaseSystem() self.wkin = dwl.WholeBodyKinematics() self.wdyn = dwl.WholeBodyDynamics() self.fbs.resetFromURDFFile(self.root_path + self.urdf_path, self.root_path + self.yarf_path) self.wkin.reset(self.fbs) self.wdyn.reset(self.fbs, self.wkin) pb.setAdditionalSearchPath(self.root_path) self.robot_id = pb.loadURDF(self.urdf_path, self.robot_pos0.tolist(), pb.getQuaternionFromEuler(self.robot_rpy0), flags=pb.URDF_USE_INERTIA_FROM_FILE) # Creating the actual whole-body state self.ws_states = [dwl.WholeBodyState(self.fbs.getJointDoF()), # actual state dwl.WholeBodyState(self.fbs.getJointDoF())] # desired state self.parseRobot() self.resetRobotPosition(self.robot_pos0) self.readControllerPlugins() # Creating the controller if not self.changeController(self.ctrl_name, self.ctrl_config): print bcolors.FAIL + 'Error: the default controller is not defined' + bcolors.ENDC sys.exit() if self.enable_ros: self.enableROS()
# Running the C++ benchmark print("C++ benchmark:") args = (fpath + "/../bin/wif_benchmark") popen = subprocess.call(args, stdin=None, stdout=None, stderr=None, shell=True) # Running the python benchmark # Number of iterations print("Python benchmark:") N = 100000 # Construct an instance of the WholeBodyDynamics class, which wraps the C++ class. ws = dwl.WholeBodyState() fbs = dwl.FloatingBaseSystem() wkin = dwl.WholeBodyKinematics() wdyn = dwl.WholeBodyDynamics() # Resetting the system from the hyq urdf file wdyn.modelFromURDFFile(fpath + "/../sample/hyq.urdf", fpath + "/../config/hyq.yarf") fbs = wdyn.getFloatingBaseSystem() wkin = wdyn.getWholeBodyKinematics() # Define the DoF after initializing the robot model ws.setJointDoF(fbs.getJointDoF()) # The robot state ws.setBasePosition(np.array([0., 0., 0.])) ws.setBaseRPY(np.array([0., 0., 0.])) ws.setBaseVelocity_W(np.array([0., 0., 0.]))