Skip to content

Commit

Permalink
Merge pull request #12 from robocin/ssl_tests
Browse files Browse the repository at this point in the history 
Major Simulator Changes and Fixes
  • Loading branch information
@FelipeMartins96
FelipeMartins96 authored Mar 29, 2021
2 parents d05863e + b317028 commit 2ef06ee
Show file tree
Hide file tree
Showing 19 changed files with 590 additions and 862 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,4 +146,4 @@ target_link_libraries(robosim_c_ssl robosim_core)
target_link_libraries(robosim_vss ${libs})
target_link_libraries(robosim_vss robosim_core)
target_link_libraries(robosim_c_vss ${libs})
target_link_libraries(robosim_c_vss robosim_core)
target_link_libraries(robosim_c_vss robosim_core)
14 changes: 3 additions & 11 deletions robosim/robosim_c_wrapper_ssl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,11 @@ extern "C"
actions.clear();
for (int i = 0; i < world->field.getRobotsCount(); i++)
{
double *a = new double[6];
for(int j = 0; j < 6; j++) a[j] = act[i*6 + j];
double *a = new double[8];
for(int j = 0; j < 8; j++) a[j] = act[i*8 + j];
actions.push_back(a);
}
world->step(world->getTimeStep(), actions);
world->step(actions);
}
void getState(SSLWorld *world, double *state_data)
{
Expand All @@ -33,12 +33,4 @@ extern "C"
const double *params_features = params.data();
memcpy(params_data, params_features, params.size() * sizeof(double));
}
void replace(SSLWorld *world, double *ball_data, double *pos_blue_data, double *pos_yellow_data)
{
world->replace(ball_data, pos_blue_data, pos_yellow_data);
}
void replace_with_vel(SSLWorld *world, double *ball_data, double *pos_blue_data, double *pos_yellow_data)
{
world->replace_with_vel(ball_data, pos_blue_data, pos_yellow_data);
}
}
10 changes: 1 addition & 9 deletions robosim/robosim_c_wrapper_vss.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ extern "C"
std::tuple<double, double> action(act[i*2], act[i*2 + 1]);
actions.push_back(action);
}
world->step(world->getTimeStep(), actions);
world->step(actions);
}
void getState(VSSWorld *world, double *state_data)
{
Expand All @@ -32,12 +32,4 @@ extern "C"
const double *params_features = params.data();
memcpy(params_data, params_features, params.size() * sizeof(double));
}
void replace(VSSWorld *world, double *ball_data, double *pos_blue_data, double *pos_yellow_data)
{
world->replace(ball_data, pos_blue_data, pos_yellow_data);
}
void replace_with_vel(VSSWorld *world, double *ball_data, double *pos_blue_data, double *pos_yellow_data)
{
world->replace_with_vel(ball_data, pos_blue_data, pos_yellow_data);
}
}
172 changes: 82 additions & 90 deletions robosim/simulator_ssl.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,20 +19,16 @@
robosim_lib.getState.restype = None
robosim_lib.getFieldParams.argtypes = [c_void_p, c_void_p]
robosim_lib.getFieldParams.restype = None
robosim_lib.replace.argtypes = [c_void_p, c_void_p, c_void_p, c_void_p]
robosim_lib.replace.restype = None
robosim_lib.replace_with_vel.argtypes = [
c_void_p, c_void_p, c_void_p, c_void_p]
robosim_lib.replace_with_vel.restype = None


class SimulatorSSL():
'''
RoboSim SSL Simulator.
Based on GRSim. Transfers all the graphic functions
to handle in python, whenever the user wants to.
The SimulatorSSL uses a C++ interface to create the simulation
and physics of the simulator.
RoboSim SSL Simulator.
Based on GRSim. Transfers all the graphic functions
to handle in python, whenever the user wants to.
The SimulatorSSL uses a C++ interface to create the simulation
and physics of the simulator.
'''

def __init__(self, field_type: int = 0,
Expand All @@ -49,8 +45,9 @@ def __init__(self, field_type: int = 0,
----------
field_type : int
The number corresponding to the type of the field
0 - 3x3 field
1 - 5x5 field
0 - Div A field
1 - Div B field
2 - HW Challenge field
n_robots_blue : int
Number of blue robots
Expand All @@ -59,7 +56,7 @@ def __init__(self, field_type: int = 0,
Number of blue robots
time_step_ms : int
Simulation timestep in miliseconds
Simulation timestep in milliseconds
ball_pos : np.ndarray
Ball position array [ballX, ballY, ballVx, ballVy]
Expand All @@ -70,7 +67,6 @@ def __init__(self, field_type: int = 0,
Returns
-------
None
'''
self.field_type: int = field_type
self.n_robots_blue: int = n_robots_blue
Expand All @@ -90,55 +86,66 @@ def __init__(self, field_type: int = 0,
)
self.field_params: Dict[str, np.float64] = self.get_field_params()
self.state_size: int = 5 \
+ (self.n_robots_blue * 7)\
+ (self.n_robots_yellow * 7)
+ (self.n_robots_blue * 11)\
+ (self.n_robots_yellow * 11)

def __del__(self):
robosim_lib.delWorld(self.world)
del self.world

def get_state(self) -> np.ndarray:
'''
Returns the state array.
State:
- Ball: x, y, z, v_x, v_y
- Robots_blue: x, y, theta, v_x, v_y, v_theta
- Robots_yellow: x, y, theta, v_x, v_y, v_theta
Units:
x, y, z -> meters
theta -> degrees (0, 360)
v_x, v_y -> meters/seconds
v_theta -> degrees/seconds
State size: 5 + 6*n_robots_blue + 6*n_robots_yellow
Parameters
----------
None
Returns
-------
np.ndarray
State
Returns the state array.
State:
- Ball: x, y, z, v_x, v_y
- Robots_blue: x, y, theta, v_x, v_y, v_theta, ir, v_wheel[0->3]
- Robots_yellow: x, y, theta, v_x, v_y, v_theta, ir, v_wheel[0->3]
Units:
x, y, z -> meters
theta -> degrees (0, 360)
v_x, v_y -> meters/seconds
v_theta -> degrees/seconds
ir -> bool
v_wheel -> rad/s
State size: 5 + 11*n_robots_blue + 11*n_robots_yellow
Parameters
----------
None
Returns
-------
np.ndarray
State
'''
state = np.zeros(self.state_size, dtype=np.float64)
robosim_lib.getState(self.world, as_ctypes(state))
return state

def step(self, action: np.ndarray) -> None:
'''
Steps the simulator given an action.
Parameters
----------
action: np.ndarray
Action of shape (6, 2),
2 wheels' speed for 6 robots.
Returns
-------
None
Steps the simulator given an action.
Parameters
----------
action: np.ndarray
Action of shape (n_robots, 7),
[0] wheel_speeds
if wheel_speeds:
[1->4] v_wheel[0->3]
else:
[1] v_x
[2] v_y
[3] v_theta
[4] unused
[5] kick_v_x
[6] kick_v_z
[7] dribbler
Returns
-------
None
'''
action = np.array(action, dtype=np.float64)
action = action.flatten()
Expand All @@ -149,18 +156,17 @@ def reset(self,
blue_robots_pos: np.ndarray,
yellow_robots_pos: np.ndarray) -> np.ndarray:
'''
Resets the simulator and it's render view
if it's in use.
Parameters
----------
None
Resets the simulator and it's render view
if it's in use.
Returns
-------
np.ndarray
State
Parameters
----------
None
Returns
-------
np.ndarray
State
'''
robosim_lib.delWorld(self.world)
self.world = robosim_lib.newWorld(self.field_type,
Expand All @@ -176,40 +182,26 @@ def reset(self,

def get_field_params(self) -> Dict[str, float]:
'''
Returns the field parameters from the given
field type.
Returns the field parameters from the given
field type.
Parameters
----------
None
Returns
-------
dict
field_width, field_length,
penalty_width, penalty_length,
goal_width
Parameters
----------
None
Returns
-------
dict
field_width, field_length,
penalty_width, penalty_length,
goal_width
'''
params = np.zeros(6, dtype=np.float64)
keys = ['field_width', 'field_length',
'penalty_width', 'penalty_length', 'goal_width', 'goal_depth']
params = np.zeros(17, dtype=np.float64)
keys = ['length', 'width', 'penalty_length', 'penalty_width',
'goal_width', 'goal_depth', 'ball_radius',
'rbt_distance_center_kicker', 'rbt_kicker_thickness',
'rbt_kicker_width', 'rbt_wheel0_angle', 'rbt_wheel1_angle',
'rbt_wheel2_angle', 'rbt_wheel3_angle', 'rbt_radius',
'rbt_wheel_radius', 'rbt_motor_max_rpm']
robosim_lib.getFieldParams(self.world, as_ctypes(params))
return {key: param for key, param in zip(keys, params)}

def replace(self, ball_pos: np.ndarray,
blue_pos: np.ndarray, yellow_pos: np.ndarray):
ball_pos = ball_pos.flatten()
blue_pos = blue_pos.flatten()
yellow_pos = yellow_pos.flatten()
robosim_lib.replace(self.world, as_ctypes(ball_pos),
as_ctypes(blue_pos), as_ctypes(yellow_pos))

def replace_with_vel(self, ball: np.ndarray,
blue_pos: np.ndarray, yellow_pos: np.ndarray):
ball = ball.flatten()
blue_pos = blue_pos.flatten()
yellow_pos = yellow_pos.flatten()
robosim_lib.replace_with_vel(self.world, as_ctypes(ball),
as_ctypes(blue_pos),
as_ctypes(yellow_pos))
35 changes: 8 additions & 27 deletions robosim/simulator_vss.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,11 +19,6 @@
robosim_lib.getState.restype = None
robosim_lib.getFieldParams.argtypes = [c_void_p, c_void_p]
robosim_lib.getFieldParams.restype = None
robosim_lib.replace.argtypes = [c_void_p, c_void_p, c_void_p, c_void_p]
robosim_lib.replace.restype = None
robosim_lib.replace_with_vel.argtypes = [
c_void_p, c_void_p, c_void_p, c_void_p]
robosim_lib.replace_with_vel.restype = None


class SimulatorVSS():
Expand Down Expand Up @@ -59,7 +54,7 @@ def __init__(self, field_type: int = 0,
Number of blue robots
time_step_ms : int
Simulation timestep in miliseconds
Simulation timestep in milliseconds
ball_pos : np.ndarray
Ball position array [ballX, ballY, ballVx, ballVy]
Expand Down Expand Up @@ -88,7 +83,6 @@ def __init__(self, field_type: int = 0,
as_ctypes(blue_robots_pos),
as_ctypes(yellow_robots_pos)
)
self.field_params: Dict[str, np.float64] = self.get_field_params()
self.state_size: int = 5 \
+ (self.n_robots_blue * 6)\
+ (self.n_robots_yellow * 6)
Expand Down Expand Up @@ -191,25 +185,12 @@ def get_field_params(self) -> Dict[str, float]:
goal_width
'''
params = np.zeros(6, dtype=np.float64)
keys = ['field_width', 'field_length',
'penalty_width', 'penalty_length', 'goal_width', 'goal_depth']
params = np.zeros(17, dtype=np.float64)
keys = ['length', 'width', 'penalty_length', 'penalty_width',
'goal_width', 'goal_depth', 'ball_radius',
'rbt_distance_center_kicker', 'rbt_kicker_thickness',
'rbt_kicker_width', 'rbt_wheel0_angle', 'rbt_wheel1_angle',
'rbt_wheel2_angle', 'rbt_wheel3_angle', 'rbt_radius',
'rbt_wheel_radius', 'rbt_motor_max_rpm']
robosim_lib.getFieldParams(self.world, as_ctypes(params))
return {key: param for key, param in zip(keys, params)}

def replace(self, ball_pos: np.ndarray,
blue_pos: np.ndarray, yellow_pos: np.ndarray):
ball_pos = ball_pos.flatten()
blue_pos = blue_pos.flatten()
yellow_pos = yellow_pos.flatten()
robosim_lib.replace(self.world, as_ctypes(ball_pos),
as_ctypes(blue_pos), as_ctypes(yellow_pos))

def replace_with_vel(self, ball: np.ndarray,
blue_pos: np.ndarray, yellow_pos: np.ndarray):
ball = ball.flatten()
blue_pos = blue_pos.flatten()
yellow_pos = yellow_pos.flatten()
robosim_lib.replace_with_vel(self.world, as_ctypes(ball),
as_ctypes(blue_pos),
as_ctypes(yellow_pos))
Loading

0 comments on commit 2ef06ee

Please sign in to comment.