def __init__(self,
model_file: str,
gazebo: bindings.GazeboWrapper,
controller_rate: float = None) -> None:
# Find the model file
model_abs_path = resource_finder.find_resource(model_file)
# Initialize the parent classes
robot_abc.RobotABC.__init__(self)
robot_baseframe.RobotBaseFrame.__init__(self)
robot_initialstate.RobotInitialState.__init__(self)
self.model_file = model_abs_path
# Private attributes
self._gazebo = gazebo
self._robot_name = None
self._gympp_robot = None
self._base_frame = None
self._joint_names = None
self._controller_rate = controller_rate
# Create a random prefix that will be used for the robot name
letters_and_digits = string.ascii_letters + string.digits
self._prefix = ''.join(
random.choice(letters_and_digits) for _ in range(6))
def __init__(self,
p,
model_file: str,
plane_id: int = None,
keep_fixed_joints: bool = False) -> None:
self._pybullet = p
self._plane_id = plane_id
self._keep_fixed_joints = keep_fixed_joints
# Find the model file
model_abs_path = resource_finder.find_resource(model_file)
# Initialize the parent classes
robot.robot_abc.RobotABC.__init__(self)
robot.robot_baseframe.RobotBaseFrame.__init__(self)
robot.robot_initialstate.RobotInitialState.__init__(self)
self.model_file = model_abs_path
# Private attributes
self._robot_id = None
self._links_name2index_dict = None
self._joints_name2index_dict = None
# TODO
self._base_frame = None
self._base_constraint = None
# Create a map from the joint name to its control info
self._jointname2jointcontrolinfo = dict()
def get_model_loader(model_file: str, considered_joints: List[str] = None):
# Find the urdf file
urdf_file = resource_finder.find_resource(file_name=model_file)
# Get the file extension
folder, model_file = os.path.split(urdf_file)
model_name, extension = os.path.splitext(model_file)
if extension == ".sdf":
raise RuntimeError(
"SDF models are not currently supported by iDynTree")
# Create the model loader
mdl_loader = idt.ModelLoader()
# Load the urdf model
if considered_joints is None:
ok_load = mdl_loader.loadModelFromFile(urdf_file)
else:
ok_load = mdl_loader.loadReducedModelFromFile(
urdf_file, considered_joints)
if not ok_load:
raise RuntimeError(f"Failed to load model from file '{urdf_file}'")
return mdl_loader
def gympp_robot(self):
if self._gympp_robot:
assert not self._gympp_robot.expired(), "The Robot object has expired"
assert self._gympp_robot.lock(), "The Robot object is empty"
assert self._gympp_robot.lock().valid(), "The Robot object is not valid"
return self._gympp_robot.lock()
# Find and load the model SDF file
sdf_file = resource_finder.find_resource(self.model_file)
with open(sdf_file, "r") as stream:
sdf_string = stream.read()
# Get the model name
original_name = bindings.GazeboWrapper.getModelNameFromSDF(sdf_string)
assert original_name, f"Failed to get model name from file {self.model_file}"
# Create a unique robot name
self._robot_name = self._prefix + "::" + original_name
# Initialize the model data
model_data = bindings.ModelInitData()
model_data.setModelName(self._robot_name)
model_data.setSdfString(sdf_string)
model_data.setPosition([0., 0, 0]) # TODO: default initial position
model_data.setOrientation([1., 0, 0, 0]) # TODO: default initial orientation
# Initialize robot controller plugin
plugin_data = bindings.PluginData()
plugin_data.setLibName("RobotController")
plugin_data.setClassName("gympp::plugins::RobotController")
# Insert the model
ok_model = self._gazebo.insertModel(model_data, plugin_data)
assert ok_model, "Failed to insert the model"
# Extract the robot from the singleton
self._gympp_robot = bindings.RobotSingleton_get().getRobot(self._robot_name)
# The robot is a weak pointer. Check that it is valid.
assert not self._gympp_robot.expired(), "The Robot object has expired"
assert self._gympp_robot.lock(), \
"The returned Robot object does not contain a valid interface"
assert self._gympp_robot.lock().valid(), "The Robot object is not valid"
if self._controller_rate:
logger.debug("Robot controller rate: {} Hz".format(self._controller_rate))
ok_dt = self._gympp_robot.lock().setdt(1 / self._controller_rate)
assert ok_dt, "Failed to set the robot controller period"
logger.debug(f"IgnitionRobot '{self._gympp_robot.lock().name()}' added to the "
"simulation")
return self._gympp_robot.lock()
def __init__(self,
physics_rate: float,
iterations: int = int(1),
rtf=float(np.finfo(np.float32).max)):
self.simulator = bindings.GazeboWrapper(iterations, rtf, physics_rate)
assert self.simulator
self.simulator.setVerbosity(4)
empty_world = resource_finder.find_resource("DefaultEmptyWorld.world")
ok_world = self.simulator.setupGazeboWorld(worldFile=empty_world)
assert ok_world
ok_initialize = self.simulator.initialize()
assert ok_initialize
def pybullet(self) -> bullet_client.BulletClient:
if self._pybullet is not None:
return self._pybullet
logger.debug("Creating PyBullet simulator")
if self._render_enabled:
self._pybullet = bullet_client.BulletClient(pybullet.GUI)
else:
# Connects to an existing instance or, if it fails, creates an headless
# simulation (DIRECT)
self._pybullet = bullet_client.BulletClient()
assert self._pybullet, "Failed to create the bullet client"
# Find the ground plane
resource_finder.add_path(pybullet_data.getDataPath())
world_abs_path = resource_finder.find_resource(self._world)
# Load the ground plane
self._plane_id = self._load_model(world_abs_path)
# Configure the physics engine
self._pybullet.setGravity(0, 0, -9.81)
self._pybullet.setPhysicsEngineParameter(numSolverIterations=10)
# Configure the physics engine with a single big time step divided in multiple
# substeps. As an alternative, we could use a single substep and step the
# simulation multiple times.
self._pybullet.setTimeStep(1.0 / self._physics_rate /
self._num_of_physics_steps)
self._pybullet.setPhysicsEngineParameter(
numSubSteps=self._num_of_physics_steps)
# Disable real-time update. We step the simulation when needed.
self._pybullet.setRealTimeSimulation(0)
logger.info("PyBullet Physic Engine Parameters:")
logger.info(str(self._pybullet.getPhysicsEngineParameters()))
step_time = 1.0 / self._physics_rate / self._rtf
logger.info(f"Nominal step time: {step_time} seconds")
logger.debug("PyBullet simulator created")
return self._pybullet
def __init__(self,
p: pybullet,
model_file: str,
plane_id: int = None,
keep_fixed_joints: bool = False) -> None:
self._pybullet = p
self._plane_id = plane_id
self._keep_fixed_joints = keep_fixed_joints
# Find the model file
model_abs_path = resource_finder.find_resource(model_file)
# Initialize the parent classes
super().__init__()
self.model_file = model_abs_path
# Load the model
self._robot_id = self._load_model(self.model_file)
assert self._robot_id is not None, "Failed to load the robot model"
# Other private attributes
self._links_name2index_dict = None
self._joints_name2index_dict = None
# TODO
self._base_frame = None
self._base_constraint = None
self._initial_joint_positions = None
# Create a map from the joint name to its control info
self._jointname2jointcontrolinfo = dict()
# Initialize all the joints in POSITION mode
for name in self.joint_names():
self._jointname2jointcontrolinfo[name] = JointControlInfo(
mode=JointControlMode.POSITION)
ok_mode = self.set_joint_control_mode(name, JointControlMode.POSITION)
assert ok_mode, \
"Failed to initialize the control mode of joint '{}'".format(name)
def test_joint_controller():
# ==========
# PARAMETERS
# ==========
agent_rate = 100.0
physics_rate = 1000.0
controller_rate = 500.0
plugin_data = bindings.PluginData()
plugin_data.libName = "RobotController"
plugin_data.className = "gympp::plugins::RobotController"
# Find and load the model SDF file
model_sdf_file = resource_finder.find_resource("CartPole/CartPole.urdf")
with open(model_sdf_file, "r") as stream:
model_sdf_string = stream.read()
# Initialize the model data
model_data = bindings.ModelInitData()
model_data.sdfString = model_sdf_string
# Get the model name
model_name = bindings.GazeboWrapper.getModelNameFromSDF(model_sdf_string)
robot_name = model_name
# =============
# CONFIGURATION
# =============
# Create the gazebo wrapper
num_of_iterations = int(physics_rate / agent_rate)
desired_rtf = float(np.finfo(np.float32).max)
gazebo = bindings.GazeboWrapper(num_of_iterations, desired_rtf, physics_rate)
assert gazebo, "Failed to get the gazebo wrapper"
# Set the verbosity
logger.set_level(gym.logger.DEBUG)
# Initialize the world
world_ok = gazebo.setupGazeboWorld("DefaultEmptyWorld.world")
assert world_ok, "Failed to initialize the gazebo world"
# Initialize the ignition gazebo wrapper (creates a paused simulation)
gazebo_initialized = gazebo.initialize()
assert gazebo_initialized, "Failed to initialize ignition gazebo"
# Insert the model
model_ok = gazebo.insertModel(model_data, plugin_data)
assert model_ok, "Failed to insert the model in the simulation"
assert bindings.RobotSingleton_get().exists(robot_name), \
"The robot interface was not registered in the singleton"
# Extract the robot interface from the singleton
robot_weak_ptr = bindings.RobotSingleton_get().getRobot(robot_name)
assert not robot_weak_ptr.expired(), "The Robot object has expired"
assert robot_weak_ptr.lock(), \
"The returned Robot object does not contain a valid interface"
assert robot_weak_ptr.lock().valid(), "The Robot object is not valid"
# Get the pointer to the robot interface
robot = robot_weak_ptr.lock()
# Set the default update rate
robot.setdt(1 / controller_rate)
# Control the cart joint in position
ok_cm = robot.setJointControlMode("linear", bindings.JointControlMode_Position)
assert ok_cm, "Failed to control the cart joint in position"
# Set the PID of the cart joint
pid = bindings.PID(10000, 1000, 1000)
pid_ok = robot.setJointPID("linear", pid)
assert pid_ok, "Failed to set the PID of the cart joint"
# Reset the robot state
robot.resetJoint("pivot", 0, 0)
robot.resetJoint("linear", 0, 0)
# Generate the cart trajectory
cart_ref = np.fromiter(
(0.2 * np.sin(2 * np.pi * 0.5 * t) for t in np.arange(0, 5, 1 / agent_rate)),
dtype=np.float)
# Initialize the cart position buffer
pos_cart_buffer = np.zeros(np.shape(cart_ref))
for (i, ref) in enumerate(cart_ref):
# Set the references
ok1 = robot.setJointPositionTarget("linear", ref)
assert ok1, "Failed to set joint references"
# Step the simulator
gazebo.run()
# Read the position
pos_cart_buffer[i] = robot.jointPosition("linear")
# Check that the trajectory was followed correctly
assert np.abs(pos_cart_buffer - cart_ref).sum() / cart_ref.size < 5E-3, \
"The reference trajectory was not tracked correctly"
def get_gazebo_and_robot(rtf: float,
physics_rate: float,
agent_rate: float,
# controller_rate: float = None,
) \
-> Tuple[bindings.GazeboWrapper, bindings.Robot]:
# ==========
# PARAMETERS
# ==========
model_sdf = "CartPole/CartPole.sdf"
world_sdf = "DefaultEmptyWorld.world"
plugin_data = bindings.PluginData()
plugin_data.setLibName("RobotController")
plugin_data.setClassName("gympp::plugins::RobotController")
# Find and load the model SDF file
model_sdf_file = resource_finder.find_resource(model_sdf)
with open(model_sdf_file, "r") as stream:
model_sdf_string = stream.read()
# Initialize the model data
model_data = bindings.ModelInitData()
model_data.setSdfString(model_sdf_string)
# Create a unique model name
letters_and_digits = string.ascii_letters + string.digits
prefix = ''.join(random.choice(letters_and_digits) for _ in range(6))
# Get the model name
model_name = bindings.GazeboWrapper.getModelNameFromSDF(model_sdf_string)
model_data.modelName = prefix + "::" + model_name
robot_name = model_data.modelName
num_of_iterations_per_gazebo_step = physics_rate / agent_rate
assert num_of_iterations_per_gazebo_step == int(
num_of_iterations_per_gazebo_step)
# =============
# CONFIGURATION
# =============
# Create the gazebo wrapper
gazebo = bindings.GazeboWrapper(int(num_of_iterations_per_gazebo_step),
rtf, physics_rate)
assert gazebo, "Failed to get the gazebo wrapper"
# Set verbosity
logger.set_level(gym.logger.DEBUG)
# Initialize the world
world_ok = gazebo.setupGazeboWorld(world_sdf)
assert world_ok, "Failed to initialize the gazebo world"
# Initialize the ignition gazebo wrapper (creates a paused simulation)
gazebo_initialized = gazebo.initialize()
assert gazebo_initialized, "Failed to initialize ignition gazebo"
# Insert the model
model_ok = gazebo.insertModel(model_data, plugin_data)
assert model_ok, "Failed to insert the model in the simulation"
assert bindings.RobotSingleton_get().exists(robot_name), \
"The robot interface was not registered in the singleton"
# Extract the robot interface from the singleton
robot_weak_ptr = bindings.RobotSingleton_get().getRobot(robot_name)
assert not robot_weak_ptr.expired(), "The Robot object has expired"
assert robot_weak_ptr.lock(), \
"The returned Robot object does not contain a valid interface"
assert robot_weak_ptr.lock().valid(), "The Robot object is not valid"
# Get the pointer to the robot interface
robot = robot_weak_ptr.lock()
# Set the joint controller period
robot.setdt(0.001 if physics_rate < 1000 else physics_rate)
# Return the simulator and the robot object
return gazebo, robot
def gympp_robot(self) -> bindings.Robot:
if self._gympp_robot:
return self._gympp_robot
# Find and load the model SDF file
sdf_file = resource_finder.find_resource(self.model_file)
with open(sdf_file, "r") as stream:
sdf_string = stream.read()
# Get the model name
original_name = bindings.GazeboWrapper.getModelNameFromSDF(sdf_string)
assert original_name, f"Failed to get model name from file {self.model_file}"
# Create a unique robot name
self._robot_name = self._prefix + "::" + original_name
initial_base_pose, initial_base_orientation = self.initial_base_pose()
# Initialize the model data
model_data = bindings.ModelInitData()
model_data.modelName = self._robot_name
model_data.sdfString = sdf_string
model_data.fixedPose = not self.is_floating_base()
model_data.position = initial_base_pose.tolist()
model_data.orientation = initial_base_orientation.tolist()
if self._base_frame is not None:
model_data.baseLink = self._base_frame
# Initialize robot controller plugin
plugin_data = bindings.PluginData()
plugin_data.libName = "RobotController"
plugin_data.className = "gympp::plugins::RobotController"
# Insert the model
ok_model = self._gazebo.insertModel(model_data, plugin_data)
assert ok_model, "Failed to insert the model"
# Extract the robot from the singleton
gympp_robot_weak = bindings.RobotSingleton_get().getRobot(
self._robot_name)
# The robot is a weak pointer. Check that it is valid.
assert not gympp_robot_weak.expired(), "The Robot object has expired"
assert gympp_robot_weak.lock(), \
"The returned Robot object does not contain a valid interface"
self._gympp_robot = gympp_robot_weak.lock()
assert self._gympp_robot.valid(), "The Robot object is not valid"
if self._controller_rate is not None:
logger.debug(f"Robot controller rate: {self._controller_rate} Hz")
ok_dt = self._gympp_robot.setdt(1 / self._controller_rate)
assert ok_dt, "Failed to set the robot controller period"
s0 = self.initial_joint_positions()
sdot0 = self.initial_joint_velocities()
joint_names = list(self._gympp_robot.jointNames())
assert s0.size == len(joint_names)
assert sdot0.size == len(joint_names)
for idx, name in enumerate(joint_names):
ok_reset = self._gympp_robot.resetJoint(name, s0[idx], sdot0[idx])
assert ok_reset, f"Failed to initialize the state of joint '{name}'"
logger.debug(
f"GazeboRobot '{self._gympp_robot.name()}' added to the simulation"
)
return self._gympp_robot