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()
def extractWholeBodyControllerState(bagfile, topic):
print('Extracting the WholeBodyControllerState message in topic' + topic +
' from rosbag ' + bagfile)
n = 0
initial_time = 0
with rosbag.Bag(bagfile, 'r') as bag:
initial_time = 0.
duration = 0.
starting_time = bag.get_start_time() + initial_time
if (duration == 0.0):
ending_time = bag.get_end_time()
duration = ending_time - starting_time
else:
ending_time = starting_time + duration
# Recording the whole-body state trajectory
actual_ws_list = []
desired_ws_list = []
error_ws_list = []
for (topic, msg, ts) in bag.read_messages(topics=str(topic)):
# Recording the data in the desired time range
if (ts.to_sec() >= starting_time and ts.to_sec() <= ending_time):
# Getting the current time
if (n == 0):
initial_time = ts.to_sec()
time = ts.to_sec() - initial_time
# Defined the desired, actual and error whole-body states
actual_ws = dwl.WholeBodyState()
desired_ws = dwl.WholeBodyState()
error_ws = dwl.WholeBodyState()
wbi = interface.WholeBodyStateInterface()
wbi.writeFromMessage(actual_ws, msg.actual)
wbi.writeFromMessage(desired_ws, msg.desired)
wbi.writeFromMessage(error_ws, msg.error)
# Defining our internal time rather than CPU time
actual_ws.setTime(time)
desired_ws.setTime(time)
error_ws.setTime(time)
# Appeding the whole-state to the list
actual_ws_list.append(actual_ws)
desired_ws_list.append(desired_ws)
error_ws_list.append(error_ws)
n += 1
print('Loaded ' + str(n) +
' whole-body controller states from the bagfile.')
return actual_ws_list, desired_ws_list, error_ws_list
def extractWholeBodyState(bagfile, topic, initial_time=0., duration=0.):
print('Extracting the WholeBodyState message in topic' + topic +
' from rosbag ' + bagfile)
n = 0
with rosbag.Bag(bagfile, 'r') as bag:
starting_time = bag.get_start_time() + initial_time
if (duration == 0.):
ending_time = bag.get_end_time()
duration = ending_time - starting_time
else:
ending_time = starting_time + duration
# Recording the whole-body state trajectory
ws_list = []
for (topic, msg, ts) in bag.read_messages(topics=str(topic)):
# Recording the data in the desired time range
if (ts.to_sec() >= starting_time and ts.to_sec() <= ending_time):
# Getting the current time
if (n == 0):
initial_time = ts.to_sec()
time = ts.to_sec() - initial_time
# Construct an instance of the WholeBodyState class, which wraps the C++ class.
ws = dwl.WholeBodyState()
wbi = interface.WholeBodyStateInterface()
wbi.writeFromMessage(ws, msg)
# Defining our internal time rather than CPU time
ws.setTime(time)
# Appeding the whole-state to the list
ws_list.append(ws)
n += 1
print('Loaded ' + str(n) +
' whole-body controller states from the bagfile.')
return ws_list
from __future__ import print_function
# This lets us use the python3-style print() function even in python2. It should have no effect if you're already running python3.
import os
import dwl
import numpy as np
# Configure the printing
np.set_printoptions(suppress=True)
# Construct an instance of the FloatingBaseSystem class, which wraps the C++ class.
fbs = dwl.FloatingBaseSystem()
ws = dwl.WholeBodyState()
# Initializing the URDF model and whole-body state
fpath = os.path.dirname(os.path.abspath(__file__))
fbs.resetFromURDFFile(fpath + "/../hyq.urdf", fpath + "/../../config/hyq.yarf")
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.]))
ws.setBaseRPYVelocity_W(np.array([0., 0., 0.]))
ws.setBaseAcceleration_W(np.array([0., 0., 0.]))
ws.setBaseRPYAcceleration_W(np.array([0., 0., 0.]))
ws.setJointPosition(0.75, fbs.getJointId("lf_hfe_joint"))
ws.setJointPosition(-1.5, fbs.getJointId("lf_kfe_joint"))
ws.setJointPosition(-0.75, fbs.getJointId("lh_hfe_joint"))
ws.setJointPosition(1.5, fbs.getJointId("lh_kfe_joint"))
ws.setJointPosition(0.75, fbs.getJointId("rf_hfe_joint"))