Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Generate idiomatic undercase Python bindings #193

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
79 changes: 52 additions & 27 deletions bindings/gympp/gympp_bindings.i
Original file line number Diff line number Diff line change
Expand Up @@ -22,52 +22,77 @@
%include <std_shared_ptr.i>

// Convert python list to std::vector
%template(Vector_i) std::vector<int>;
%template(Vector_u) std::vector<size_t>;
%template(Vector_f) std::vector<float>;
%template(Vector_d) std::vector<double>;
%template(Vector_s) std::vector<std::string>;
%template(VectorI) std::vector<int>;
%template(VectorU) std::vector<size_t>;
%template(VectorF) std::vector<float>;
%template(VectorD) std::vector<double>;
%template(VectorS) std::vector<std::string>;

// Convert python list to std::array
%template(Array3d) std::array<double, 3>;
%template(Array4d) std::array<double, 4>;
%template(Array6d) std::array<double, 6>;

%shared_ptr(gympp::base::spaces::Space)
%shared_ptr(gympp::base::spaces::Box)
%shared_ptr(gympp::base::spaces::Discrete)

%include "gympp/base/Common.h"
%template(BufferContainer_i) gympp::base::BufferContainer<int>;
%template(BufferContainer_u) gympp::base::BufferContainer<size_t>;
%template(BufferContainer_f) gympp::base::BufferContainer<float>;
%template(BufferContainer_d) gympp::base::BufferContainer<double>;
%include "gympp/base/Space.h"
%template(get_i) gympp::base::data::Sample::get<int>;
%template(get_u) gympp::base::data::Sample::get<size_t>;
%template(get_f) gympp::base::data::Sample::get<float>;
%template(get_d) gympp::base::data::Sample::get<double>;
%template(getBuffer_i) gympp::base::data::Sample::getBuffer<int>;
%template(getBuffer_u) gympp::base::data::Sample::getBuffer<size_t>;
%template(getBuffer_f) gympp::base::data::Sample::getBuffer<float>;
%template(getBuffer_d) gympp::base::data::Sample::getBuffer<double>;
%template(BufferContainerI) gympp::base::BufferContainer<int>;
%template(BufferContainerU) gympp::base::BufferContainer<size_t>;
%template(BufferContainerF) gympp::base::BufferContainer<float>;
%template(BufferrContainerD) gympp::base::BufferContainer<double>;
%template(get_buffer_i) gympp::base::data::Sample::getBuffer<int>;
%template(get_buffer_u) gympp::base::data::Sample::getBuffer<size_t>;
%template(get_buffer_f) gympp::base::data::Sample::getBuffer<float>;
%template(get_buffer_d) gympp::base::data::Sample::getBuffer<double>;

%include "optional.i"
%template(Optional_i) std::optional<int>;
%template(Optional_u) std::optional<size_t>;
%template(Optional_f) std::optional<float>;
%template(Optional_d) std::optional<double>;
%template(Optional_state) std::optional<gympp::base::State>;
%template(Optional_sample) std::optional<gympp::base::data::Sample>;
%template(OptionalI) std::optional<int>;
%template(OptionalU) std::optional<size_t>;
%template(OptionalF) std::optional<float>;
%template(OptionalD) std::optional<double>;
%template(OptionalState) std::optional<gympp::base::State>;
%template(OptionalSample) std::optional<gympp::base::data::Sample>;

%shared_ptr(gympp::base::spaces::Space)
%shared_ptr(gympp::base::spaces::Box)
%shared_ptr(gympp::base::spaces::Discrete)
%include "gympp/base/Space.h"
%include "ignition/common/SingletonT.hh"
%ignore ignition::common::SingletonT<gympp::gazebo::GymFactory>::myself;
%template(GymFactorySingleton) ignition::common::SingletonT<gympp::gazebo::GymFactory>;

%rename("%(undercase)s") "";
%rename("") Space;
%rename("") State;
%rename("") Range;
%rename("") Limit;
%rename("") Sample;
%rename("") Buffer;
%rename("") SpaceType;
%rename("") RenderMode;
%rename("") GymFactory;
%rename("") PhysicsData;
%rename("") Environment;
%rename("") ActionSpace;
%rename("") RewardRange;
%rename("") ModelInitData;
%rename("") SpaceMetadata;
%rename("") PluginMetadata;
%rename("") BufferContainer;
%rename("") GazeboSimulator;
%rename("") ObservationSpace;
%rename("") GazeboEnvironment;
%rename("") gympp::base::spaces::Box;
%rename("") gympp::base::spaces::Discrete;

%shared_ptr(scenario::gazebo::GazeboSimulator)
%include "scenario/gazebo/GazeboSimulator.h"

%shared_ptr(gympp::base::Environment)
%shared_ptr(gympp::gazebo::GazeboEnvironment)
%include "ignition/common/SingletonT.hh"
%ignore ignition::common::SingletonT<gympp::gazebo::GymFactory>::myself;
%template(GymFactorySingleton) ignition::common::SingletonT<gympp::gazebo::GymFactory>;

%include "gympp/base/Environment.h"
%include "gympp/gazebo/GazeboEnvironment.h"
Expand All @@ -77,7 +102,7 @@
return std::dynamic_pointer_cast<gympp::gazebo::GazeboEnvironment>(env);
}

std::shared_ptr<scenario::gazebo::GazeboSimulator> envToGazeboWrapper(gympp::base::EnvironmentPtr env) {
std::shared_ptr<scenario::gazebo::GazeboSimulator> envToGazeboSimulator(gympp::base::EnvironmentPtr env) {
return std::dynamic_pointer_cast<scenario::gazebo::GazeboSimulator>(env);
}
%}
Expand Down
34 changes: 26 additions & 8 deletions bindings/scenario/scenario_bindings.i
Original file line number Diff line number Diff line change
Expand Up @@ -23,11 +23,11 @@
%include <std_shared_ptr.i>

// Convert python list to std::vector
%template(Vector_i) std::vector<int>;
%template(Vector_u) std::vector<size_t>;
%template(Vector_f) std::vector<float>;
%template(Vector_d) std::vector<double>;
%template(Vector_s) std::vector<std::string>;
%template(VectorI) std::vector<int>;
%template(VectorU) std::vector<size_t>;
%template(VectorF) std::vector<float>;
%template(VectorD) std::vector<double>;
%template(VectorS) std::vector<std::string>;

// Convert python list to std::array
%template(Array3d) std::array<double, 3>;
Expand All @@ -38,9 +38,27 @@
%template(PosePair) std::pair<std::array<double, 3>, std::array<double, 4>>;

// ScenarI/O templates
%template(Vector_links) std::vector<scenario::gazebo::LinkPtr>;
%template(Vector_joints) std::vector<scenario::gazebo::JointPtr>;
%template(Vector_contact) std::vector<scenario::base::ContactData>;
%template(VectorOfLinks) std::vector<scenario::gazebo::LinkPtr>;
%template(VectorOfJoints) std::vector<scenario::gazebo::JointPtr>;
%template(VectorOfContactData) std::vector<scenario::base::ContactData>;

// Rename all methods to undercase with _ separators excluding the classes.
// Keep all template instantations above.
%rename("%(undercase)s") "";
%rename("") PID;
%rename("") Pose;
%rename("") Link;
%rename("") Joint;
%rename("") Model;
%rename("") World;
%rename("") Limit;
%rename("") JointType;
%rename("") JointLimit;
%rename("") ContactData;
%rename("") ECMSingleton;
%rename("") PhysicsEngine;
%rename("") GazeboSimulator;
%rename("") JointControlMode;

// Public helpers
%include "scenario/gazebo/utils.h"
Expand Down
1 change: 0 additions & 1 deletion python/gym_ignition/base/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,4 +5,3 @@
# Abstract classes
from . import task
from . import runtime

30 changes: 15 additions & 15 deletions python/gym_ignition/experimental/gympp/cartpole.py
Original file line number Diff line number Diff line change
Expand Up @@ -27,33 +27,33 @@ def _plugin_metadata(self) -> "gympp_bindings.PluginMetadata":

# Configure ignition environment
md = bindings.PluginMetadata()
md.setEnvironmentName("CartPole")
md.setLibraryName("CartPolePlugin")
md.setClassName("gympp::plugins::CartPole")
md.setWorldFileName(empty_world_sdf)
md.setModelFileName(gym_ignition_models.get_model_file("cartpole"))
md.setAgentRate(1000)
md.set_environment_name("CartPole")
md.set_library_name("CartPolePlugin")
md.set_class_name("gympp::plugins::CartPole")
md.set_world_file_name(empty_world_sdf)
md.set_model_file_name(gym_ignition_models.get_model_file("cartpole"))
md.set_agent_rate(1000)

real_time_factor = 1E9
max_physics_step_size = 0.001
md.setPhysicsData(bindings.PhysicsData(real_time_factor, max_physics_step_size))
md.set_physics_data(bindings.PhysicsData(real_time_factor, max_physics_step_size))

# Configure the action space
action_space_md = bindings.SpaceMetadata()
action_space_md.setType(bindings.SpaceType_Discrete)
action_space_md.setDimensions([2])
action_space_md.set_type(bindings.SpaceType_discrete)
action_space_md.set_dimensions([2])

# Configure the observation space
observation_space_md = bindings.SpaceMetadata()
observation_space_md.setType(bindings.SpaceType_Box)
observation_space_md.set_type(bindings.SpaceType_box)
max_float = float(np.finfo(np.float32).max)
observation_space_md.setLowLimit([-2.5, -max_float, -24, -max_float])
observation_space_md.setHighLimit([2.5, max_float, 24, max_float])
observation_space_md.set_low_limit([-2.5, -max_float, -24, -max_float])
observation_space_md.set_high_limit([2.5, max_float, 24, max_float])

# Store the spaces information
md.setActionSpaceMetadata(action_space_md)
md.setObservationSpaceMetadata(observation_space_md)
md.set_action_space_metadata(action_space_md)
md.set_observation_space_metadata(observation_space_md)

# Return the metadata
assert md.isValid(), "The plugin metadata object is not valid"
assert md.is_valid(), "The plugin metadata object is not valid"
return md
61 changes: 21 additions & 40 deletions python/gym_ignition/experimental/gympp/gympp_env.py
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,6 @@ def __init__(self):
self._observation_space = None
self._act_dt = None
self._obs_dt = None
self._robot = None
self._np_random = None

# Seed the environment
Expand All @@ -56,11 +55,11 @@ def gympp_env(self):

# Register the environment
factory = bindings.GymFactory.Instance()
factory.registerPlugin(md)
factory.register_plugin(md)

# Load the environment from gympp
self._env = factory.make(md.getEnvironmentName())
assert self._env, "Failed to create environment " + md.getEnvironmentName()
self._env = factory.make(md.get_environment_name())
assert self._env, "Failed to create environment " + md.get_environment_name()

# Set the verbosity
logger.set_level(gym.logger.MIN_LEVEL)
Expand All @@ -71,7 +70,7 @@ def gympp_env(self):
@property
def gazebo(self):
from gym_ignition import gympp_bindings as bindings
return bindings.envToGazeboWrapper(self.gympp_env)
return bindings.env_to_gazebo_simulator(self.gympp_env)

@property
def action_space(self) -> gym.Space:
Expand All @@ -80,7 +79,8 @@ def action_space(self) -> gym.Space:

# Create the action space from the metadata
md = self._plugin_metadata
self._action_space, self._act_dt = self._create_space(md.getActionSpaceMetadata())
self._action_space, self._act_dt = \
self._create_space(md.get_action_space_metadata())

return self._action_space

Expand All @@ -92,31 +92,10 @@ def observation_space(self) -> gym.Space:
# Create the action space from the metadata
md = self._plugin_metadata
self._observation_space, self._obs_dt = self._create_space(
md.getObservationSpaceMetadata())
md.get_observation_space_metadata())

return self._observation_space

@property
def robot(self):
if self._robot:
assert self._robot.valid(), "The Robot object is not valid"
return self._robot

from gym_ignition import gympp_bindings as bindings

# Get the robot name
gazebo_wrapper = bindings.envToGazeboWrapper(self.gympp_env)
model_names = gazebo_wrapper.getModelNames()
assert len(model_names) == 1, "The environment has more than one model"
model_name = model_names[0]

# Get the pointer to the Robot object
self._robot = bindings.RobotSingleton_get().getRobot(model_name)
assert self._robot, "Failed to get the Robot object"

# Return the object
return self._robot

def step(self, action: Action) -> State:
assert self.action_space.contains(action), \
"The action does not belong to the action space"
Expand Down Expand Up @@ -155,7 +134,8 @@ def step(self, action: Action) -> State:
state = state_optional.value()

# Get the std::vector buffer of gympp::Observation
observation_vector = getattr(state.observation, 'getBuffer' + self._obs_dt)()
method = 'get_buffer_' + self._obs_dt.lower()
observation_vector = getattr(state.observation, method)()
assert observation_vector, "Failed to get the observation buffer"
assert observation_vector.size() > 0, "The observation does not contain elements"

Expand Down Expand Up @@ -190,7 +170,8 @@ def reset(self) -> Observation:
gympp_observation = obs_optional.value()

# Get the std::vector buffer of gympp::Observation
observation_vector = getattr(gympp_observation, 'getBuffer' + self._obs_dt)()
method = 'get_buffer_' + self._obs_dt.lower()
observation_vector = getattr(gympp_observation, method)()
assert observation_vector, "Failed to get the observation buffer"
assert observation_vector.size() > 0, "The observation does not contain elements"

Expand All @@ -216,7 +197,7 @@ def render(self, mode: str = 'human') -> None:

from gym_ignition import gympp_bindings as bindings

render_mode = {'human': bindings.Environment.RenderMode_HUMAN}
render_mode = {'human': bindings.Environment.RenderMode_human}
ok = self.gympp_env.render(render_mode[mode])

assert ok, "Failed to render environment"
Expand Down Expand Up @@ -268,23 +249,23 @@ def _create_space(cls, md=None):
from gym_ignition import gympp_bindings as bindings
assert isinstance(md, bindings.SpaceMetadata), "Wrong type for method argument"

space_type = md.getType()
low = md.getLowLimit()
high = md.getHighLimit()
dims = md.getDimensions()
space_type = md.get_type()
low = md.get_low_limit()
high = md.get_high_limit()
dims = md.get_dimensions()

if space_type is bindings.SpaceType_Box:
if space_type is bindings.SpaceType_box:
if not dims:
assert len(low) == len(high), "Sizes of low and high limits do not match"
return spaces.Box(np.array(low), np.array(high)), "_d"
return spaces.Box(np.array(low), np.array(high)), "D"
else:
assert len(low) == 1, "The size of the limit is not valid"
assert len(high) == 1, "The size of the limit is not valid"
return spaces.Box(low[0], high[0], dims), "_d"
return spaces.Box(low[0], high[0], dims), "D"

elif space_type is bindings.SpaceType_Discrete:
elif space_type is bindings.SpaceType_discrete:
assert len(dims) == 1, "The specified space dimension is not valid"
return spaces.Discrete(dims[0]), "_i"
return spaces.Discrete(dims[0]), "I"

else:
assert False, "This space type is not supported"
5 changes: 2 additions & 3 deletions python/gym_ignition/randomizers/model/sdf.py
Original file line number Diff line number Diff line change
Expand Up @@ -89,11 +89,11 @@ def sampled_from(self,
self.storage["parameters"] = parameters

if self.storage["distribution"] is Distribution.Gaussian and \
not isinstance(parameters, GaussianParams):
not isinstance(parameters, GaussianParams):
raise ValueError("Wrong parameters type")

if self.storage["distribution"] is Distribution.Uniform and \
not isinstance(parameters, UniformParams):
not isinstance(parameters, UniformParams):
raise ValueError("Wrong parameters type")

return self
Expand Down Expand Up @@ -127,7 +127,6 @@ def ignore_zeros(self, ignore_zeros: bool) -> "RandomizationDataBuilder":
The randomization builder to allow chaining methods.
"""


self.storage["ignore_zeros"] = ignore_zeros
return self

Expand Down
2 changes: 1 addition & 1 deletion python/gym_ignition/randomizers/physics/dart.py
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ def __init__(self, seed: int = None):
def randomize_physics(self, world: bindings.World) -> None:

# Insert the physics
ok_physics = world.setPhysicsEngine(bindings.PhysicsEngine_Dart)
ok_physics = world.set_physics_engine(bindings.PhysicsEngine_dart)

if not ok_physics:
raise RuntimeError("Failed to insert the physics plugin")
Loading