Skip to content

Commit

Permalink
Corrects more comments
Browse files Browse the repository at this point in the history
  • Loading branch information
@20chupin
20chupin committed Apr 10, 2024
1 parent 3309728 commit 911dc9a
Show file tree
Hide file tree
Showing 5 changed files with 160 additions and 131 deletions.
110 changes: 55 additions & 55 deletions python/jiminy_py/src/jiminy_py/log.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,10 +119,10 @@ def build_robot_from_log(
mesh_package_dirs: Sequence[str] = (),
*, robot_name: str = ""
) -> jiminy.Robot:
"""Build robot from log. This function can only be used for logs of
single-robot simulations. For multi-robot simulations, you have to use
`build_robots_from_log`.
"""Build a robot from log. Create and initialize a robot from a single or
multi robot simulation. If the robot to be built is from a multi-robot
simulation, its name needs to be provided.
.. note::
model options and `robot.pinocchio_model` will be the same as during
the simulation until the next call to `reset` method unless the options
Expand All @@ -146,43 +146,47 @@ def build_robot_from_log(
directories to the ones provided by log file. It
may be necessary to specify it to read log
generated on a different environment.
:param robot_name: Name of the robot to be constructed in the log. If it
is not known, call `build_robots_from_log`.
:returns: Reconstructed robot, and parsed log data as returned by
`jiminy_py.log.read_log` method.
"""
# Prepare robot namespace
if robot_name == "":
ROBOT_NAMESPACE = ENGINE_NAMESPACE
else:
ROBOT_NAMESPACE = ".".join((ENGINE_NAMESPACE, robot_name))
robot_namespace = ".".join(filter(None, (ENGINE_NAMESPACE, robot_name)))

# Instantiate empty robot
robot = jiminy.Robot(robot_name)

# Extract log constants
log_constants = log_data["constants"]

# Extract common info
pinocchio_model = log_constants[
".".join((ROBOT_NAMESPACE, "pinocchio_model"))]
try:
pinocchio_model = log_constants[
".".join((robot_namespace, "pinocchio_model"))]
except KeyError as exc:
if robot_name == "":
raise ValueError(
"No robot with empty name has been found. Specify a name or "
"call `build_robots_from_log` to build the robot.") from exc

try:
# Extract geometry models
collision_model = log_constants[
".".join((ROBOT_NAMESPACE, "collision_model"))]
".".join((robot_namespace, "collision_model"))]
visual_model = log_constants[
".".join((ROBOT_NAMESPACE, "visual_model"))]
".".join((robot_namespace, "visual_model"))]

# Initialize the model
robot.initialize(pinocchio_model, collision_model, visual_model)
except KeyError as e:
# Extract initialization arguments
urdf_data = log_constants[
".".join((ROBOT_NAMESPACE, "urdf_file"))]
".".join((robot_namespace, "urdf_file"))]
has_freeflyer = int(log_constants[
".".join((ROBOT_NAMESPACE, "has_freeflyer"))])
".".join((robot_namespace, "has_freeflyer"))])
mesh_package_dirs = [*mesh_package_dirs, *log_constants.get(
".".join((ROBOT_NAMESPACE, "mesh_package_dirs")), ())]
".".join((robot_namespace, "mesh_package_dirs")), ())]

# Make sure urdf data is available
if len(urdf_data) <= 1:
Expand Down Expand Up @@ -212,10 +216,8 @@ def build_robot_from_log(
# Load the options
all_options = log_constants[".".join((ENGINE_NAMESPACE, "options"))]
# Load the options
if robot_name == "":
robot.set_options(all_options["robot"])
else:
robot.set_options(all_options[".".join((robot_name, "robot"))])
robot.set_options(
all_options[".".join(filter(None, (robot_name, "robot")))])

# Update model in-place.
# Note that `__setstate__` re-allocates memory instead of just calling
Expand All @@ -236,23 +238,11 @@ def build_robots_from_log(
mesh_path_dir: Optional[str] = None,
mesh_package_dirs: Sequence[str] = (),
) -> Sequence[jiminy.Robot]:
"""Build robots from log. For mulit-robot simulations, build all the robots
in the log of the simulation.
.. note::
model options and `robot.pinocchio_model` will be the same as during
the simulation until the next call to `reset` method unless the options
of the robots that has been restored are overwritten manually.
"""Build all the robots in the log of the simulation.
.. note::
It returns a sequence of valid and fully initialized robots, that can
be used to perform new simulation if added to a Jiminy Engine, but the
original controller is lost.
.. warning::
It does ot require the original URDF files to exist, but the original
mesh paths (if any) must be valid since they are not bundle in the log
archive for now.
This function calls `build_robot_from_log` to build each robot.
Refer to `build_robot_from_log` for more information.
:param log_data: Logged data (constants and variables) as a dictionary.
:param mesh_path_dir: Overwrite the common root of all absolute mesh paths.
Expand All @@ -269,15 +259,15 @@ def build_robots_from_log(
log_constants = log_data["constants"]

robots_names = []

for key in log_constants.keys():
if key == "HighLevelController.has_freeflyer":
robots_names.append("")
else:
m = re.findall(r"HighLevelController.(\w+).has_freeflyer", key)
if len(m) == 1:
robots_names.append(m[0])

robots = []

for robot_name in robots_names:
Expand All @@ -291,7 +281,7 @@ def build_robots_from_log(

def extract_trajectory_from_log(log_data: Dict[str, Any],
robot: Optional[jiminy.Model] = None,
*, robot_name: str = ""
*, robot_name: Optional[str] = None
) -> TrajectoryDataType:
"""Extract the minimal required information from raw log data in order to
replay the simulation in a viewer.
Expand All @@ -306,32 +296,41 @@ def extract_trajectory_from_log(log_data: Dict[str, Any],
:param robot: Jiminy robot associated with the logged trajectory.
Optional: None by default. If None, then it will be
reconstructed from 'log_data' using `build_robot_from_log`.
:param robot_name: Name of the robot to be constructed in the log. If it
is not known, call `build_robot_from_log`.
:returns: Trajectory dictionary. The actual trajectory corresponds to the
field "evolution_robot" and it is a list of State object. The
other fields are additional information.
"""
# Prepare robot namespace
if robot.name == "":
ROBOT_NAMESPACE = ENGINE_NAMESPACE
else:
ROBOT_NAMESPACE = ".".join((ENGINE_NAMESPACE, robot.name))
if robot is not None:
if robot_name is None:
robot_name = robot.name
elif robot_name != robot.name:
raise ValueError(
"The name specified in `robot_name` and the name of the robot "
"are differents.")
elif robot_name is None:
robot_name = ""

robot_namespace = ".".join(filter(None, (ENGINE_NAMESPACE, robot_name)))
# Handling of default argument(s)
if robot is None:
robot = build_robot_from_log(log_data)
robot = build_robot_from_log(log_data, robot_name=robot_name)

# Define some proxies for convenience
log_vars = log_data["variables"]

# Extract the joint positions, velocities and external forces over time
positions = np.stack([
log_vars.get(".".join((ROBOT_NAMESPACE, field)), [])
log_vars.get(".".join((robot_namespace, field)), [])
for field in robot.log_position_fieldnames], axis=-1)
velocities = np.stack([
log_vars.get(".".join((ROBOT_NAMESPACE, field)), [])
log_vars.get(".".join((robot_namespace, field)), [])
for field in robot.log_velocity_fieldnames], axis=-1)
forces = np.stack([
log_vars.get(".".join((ROBOT_NAMESPACE, field)), [])
log_vars.get(".".join((robot_namespace, field)), [])
for field in robot.log_f_external_fieldnames], axis=-1)

# Determine which optional data are available
Expand Down Expand Up @@ -364,15 +363,15 @@ def extract_trajectories_from_log(log_data: Dict[str, Any],
replay the simulation in a viewer.
.. note::
It extracts the required data for replay, namely temporal evolution of:
- robot configuration: to display of the robot on the scene,
- robot velocity: to update velocity-dependent markers such as DCM,
- external forces: to update force-dependent markers.
This function calls `extract_trajectory_from_log` to extract each
robot's trajectory. Refer to `extract_trajectory_from_log` for more
information.
:param log_data: Logged data (constants and variables) as a dictionary.
:param robot: Jiminy robot associated with the logged trajectory.
Optional: None by default. If None, then it will be
reconstructed from 'log_data' using `build_robot_from_log`.
:param robots: Sequend of Jiminy robots associated with the logged
trajectory.
Optional: None by default. If None, then it will be
reconstructed from 'log_data' using `build_robots_from_log`.
:returns: Trajectory dictionary. The actual trajectory corresponds to the
field "evolution_robot" and it is a list of State object. The
Expand All @@ -385,10 +384,11 @@ def extract_trajectories_from_log(log_data: Dict[str, Any],
robots = build_robots_from_log(log_data)

for robot in robots:
trajectory = extract_trajectory_from_log(log_data, robot, robot_name=robot.name)
trajectory = extract_trajectory_from_log(
log_data, robot, robot_name=robot.name)

trajectories[robot.name] = trajectory

return trajectories

def update_sensor_measurements_from_log(
Expand Down
2 changes: 2 additions & 0 deletions python/jiminy_py/src/jiminy_py/robot.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -865,6 +865,8 @@ class BaseJiminyRobot(jiminy.Robot):
Hardware description files within the same directory and having the
name than the URDF file will be detected automatically without
requiring to manually specify its path.
:param name: Name of the robot to be created, empty string by default.
"""
def __init__(self, name : Optional[str] = "") -> None:
self.extra_info: Dict[str, Any] = {}
Expand Down
Loading

0 comments on commit 911dc9a

Please sign in to comment.