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.]))