def test_pops(self):
r = RingBuffer(3)
r.append(1)
r.appendleft(2)
r.append(3)
np.testing.assert_equal(r, np.array([2, 1, 3]))
self.assertEqual(r.pop(), 3)
np.testing.assert_equal(r, np.array([2, 1]))
self.assertEqual(r.popleft(), 2)
np.testing.assert_equal(r, np.array([1]))
# test empty pops
empty = RingBuffer(1)
with self.assertRaisesRegex(IndexError,
"pop from an empty RingBuffer"):
empty.pop()
with self.assertRaisesRegex(IndexError,
"pop from an empty RingBuffer"):
empty.popleft()
def test_no_overwrite(self):
r = RingBuffer(3, allow_overwrite=False)
r.append(1)
r.append(2)
r.appendleft(3)
with self.assertRaisesRegex(IndexError, 'overwrite'):
r.appendleft(4)
with self.assertRaisesRegex(IndexError, 'overwrite'):
r.extendleft([4])
r.extendleft([])
np.testing.assert_equal(r, np.array([3, 1, 2]))
with self.assertRaisesRegex(IndexError, 'overwrite'):
r.append(4)
with self.assertRaisesRegex(IndexError, 'overwrite'):
r.extend([4])
r.extend([])
# works fine if we pop the surplus
r.pop()
r.append(4)
np.testing.assert_equal(r, np.array([3, 1, 4]))
class SideSubgaitController(object):
def __init__(self,
default,
view,
lock_checked=False,
default_checked=False,
subgait=None):
self._lock_checked = lock_checked
self._default_checked = default_checked
self._subgait = subgait
self._default = default
self.view = view
self.settings_history = RingBuffer(capacity=100, dtype=list)
self.settings_redo_list = RingBuffer(capacity=100, dtype=list)
def undo(self):
self.settings_redo_list.append({
'lock_checked': self._lock_checked,
'default_checked': self._default_checked,
'subgait': self._subgait
})
settings = self.settings_history.pop()
self._lock_checked = settings['lock_checked']
self._default_checked = settings['default_checked']
self._subgait = settings['subgait']
self.view.update_widget(self)
def redo(self):
self.save_changed_settings()
settings = self.settings_redo_list.pop()
self._lock_checked = settings['lock_checked']
self._default_checked = settings['default_checked']
self._subgait = settings['subgait']
self.view.update_widget(self)
def save_changed_settings(self):
self.settings_history.append({
'lock_checked': self._lock_checked,
'default_checked': self._default_checked,
'subgait': self._subgait
})
@property
def lock_checked(self):
return self._lock_checked
@lock_checked.setter
def lock_checked(self, lock_checked):
self.save_changed_settings()
self._lock_checked = lock_checked
self.view.update_widget(self)
@property
def default_checked(self):
return self._default_checked
@default_checked.setter
def default_checked(self, default_checked):
self.save_changed_settings()
self._default_checked = default_checked
self.view.update_widget(self)
@property
def subgait(self):
if self.default_checked:
return self._default
else:
return self._subgait
@subgait.setter
def subgait(self, subgait):
self.save_changed_settings()
self._subgait = subgait
self.view.update_widget(self)
@property
def subgait_text(self):
if self._subgait:
return self._subgait.version
else:
return 'Import'
class GaitGeneratorController:
def __init__(self, view, robot):
self.view = view
# Default values
self.gait_directory = None
self.current_gait_path = None
self.playback_speed = 100
self.time_slider_thread = None
self.current_time = 0
self.robot = robot
empty_subgait_file = os.path.join(
rospkg.RosPack().get_path("march_rqt_gait_generator"),
"resource/empty_gait/empty_subgait/empty_subgait.subgait",
)
self.subgait = ModifiableSubgait.from_file(robot, empty_subgait_file,
self)
self.settings_changed_history = RingBuffer(capacity=100, dtype=list)
self.settings_changed_redo_list = RingBuffer(capacity=100, dtype=list)
standing = Subgait.from_file(robot, empty_subgait_file)
previous_subgait_controller = SideSubgaitController(
view=self.view.side_subgait_view["previous"], default=standing)
next_subgait_controller = SideSubgaitController(
view=self.view.side_subgait_view["next"],
default=standing,
)
self.side_subgait_controller = {
"previous": previous_subgait_controller,
"next": next_subgait_controller,
}
self.connect_buttons()
self.view.load_gait_into_ui(self.subgait)
for joint in self.subgait.joints:
self.connect_plot(joint)
# Called by __init__
def connect_buttons(self):
self.view.change_gait_directory_button.clicked.connect(
self.change_gait_directory)
self.view.add_inverse_kinematic_setpoints_button.clicked.connect(
self.add_inverse_kinematics_setpoints)
self.view.import_gait_button.clicked.connect(self.import_gait)
self.view.export_gait_button.clicked.connect(self.export_gait)
self.view.side_subgait_view["previous"].import_button.clicked.connect(
lambda: self.import_side_subgait("previous"))
self.view.side_subgait_view[
"previous"].default_checkbox.stateChanged.connect(
lambda value: self.toggle_side_subgait_checkbox(
value, "previous", "standing"))
self.view.side_subgait_view[
"previous"].lock_checkbox.stateChanged.connect(
lambda value: self.toggle_side_subgait_checkbox(
value, "previous", "lock"))
self.view.side_subgait_view["next"].import_button.clicked.connect(
lambda: self.import_side_subgait("next"))
self.view.side_subgait_view[
"next"].default_checkbox.stateChanged.connect(
lambda value: self.toggle_side_subgait_checkbox(
value, "next", "standing"))
self.view.side_subgait_view["next"].lock_checkbox.stateChanged.connect(
lambda value: self.toggle_side_subgait_checkbox(
value, "next", "lock"))
self.view.start_button.clicked.connect(self.start_time_slider_thread)
self.view.stop_button.clicked.connect(self.stop_time_slider_thread)
self.view.invert_button.clicked.connect(self.invert_gait)
self.view.undo_button.clicked.connect(self.undo)
self.view.redo_button.clicked.connect(self.redo)
self.view.ctrl_z.activated.connect(self.undo)
self.view.ctrl_shift_z.activated.connect(self.redo)
# Line edits / combo boxes / spin boxes
self.view.gait_type_combo_box.currentTextChanged.connect(
lambda text: self.subgait.set_gait_type(text), )
self.view.gait_name_line_edit.textChanged.connect(
lambda text: self.subgait.set_gait_name(text), )
self.view.version_name_line_edit.textChanged.connect(
lambda text: self.subgait.set_version(text), )
self.view.subgait_name_line_edit.textChanged.connect(
lambda text: self.subgait.set_subgait_name(text), )
self.view.description_line_edit.textChanged.connect(
lambda text: self.subgait.set_description(text), )
self.view.playback_speed_spin_box.valueChanged.connect(
self.set_playback_speed)
self.view.duration_spin_box.setKeyboardTracking(False)
self.view.duration_spin_box.valueChanged.connect(
self.update_gait_duration)
# Check boxes
self.view.velocity_plot_check_box.stateChanged.connect(
lambda: self.view.update_joint_widgets(self.subgait.joints), )
self.view.effort_plot_check_box.stateChanged.connect(
lambda: self.view.update_joint_widgets(self.subgait.joints), )
# Disable key inputs when mirroring is off.
self.view.mirror_check_box.stateChanged.connect(lambda state: [
self.view.mirror_key1_line_edit.setEnabled(state),
self.view.mirror_key2_line_edit.setEnabled(state),
])
# Connect TimeSlider to the preview
self.view.time_slider.valueChanged.connect(lambda time: [
self.set_current_time(time / 100.0),
self.view.publish_preview(self.subgait, self.current_time),
self.view.update_time_sliders(self.current_time),
])
# Called by __init__
def connect_plot(self, joint):
joint_widget = self.view.joint_widgets[joint.name]
joint_plot = joint_widget.Plot.getItem(0, 0)
def add_setpoint(joint, time, position, button):
if button == self.view.control_button:
joint.add_interpolated_setpoint(time)
else:
joint.add_setpoint(ModifiableSetpoint(time, position, 0))
# Connect a function to update the model and to update the table.
joint_plot.plot_item.sigPlotChanged.connect(lambda: [
joint.set_setpoints(joint_plot.to_setpoints()),
self.view.update_joint_widget(joint),
self.view.publish_preview(self.subgait, self.current_time),
])
joint_plot.add_setpoint.connect(lambda time, position, button: [
add_setpoint(joint, time, position, button),
self.view.update_joint_widget(joint),
self.view.publish_preview(self.subgait, self.current_time),
])
joint_plot.remove_setpoint.connect(lambda index: [
joint.remove_setpoint(index),
self.view.update_joint_widget(joint),
self.view.publish_preview(self.subgait, self.current_time),
])
joint_widget.Table.itemChanged.connect(lambda: [
joint.save_setpoints(),
self.save_changed_settings({"joints": [joint]}),
joint.set_setpoints(joint_widget.Table.controller.to_setpoints()),
self.view.update_joint_widget(joint, update_table=False),
self.view.publish_preview(self.subgait, self.current_time),
])
# Functions below are connected to buttons, text boxes, joint graphs etc.
def set_playback_speed(self, playback_speed):
was_playing = self.time_slider_thread is not None
self.stop_time_slider_thread()
self.playback_speed = playback_speed
rospy.loginfo("Changing playbackspeed to " + str(self.playback_speed))
if was_playing:
self.start_time_slider_thread()
def set_current_time(self, current_time):
self.current_time = current_time
def start_time_slider_thread(self):
if self.time_slider_thread is not None:
rospy.logdebug(
"Cannot start another time slider thread as one is already active"
)
return
current = self.view.time_slider.value()
playback_speed = self.playback_speed
max_time = self.view.time_slider.maximum()
self.time_slider_thread = self.view.create_time_slider_thread(
current, playback_speed, max_time)
self.time_slider_thread.update_signal.connect(
self.view.update_main_time_slider)
self.time_slider_thread.start()
def stop_time_slider_thread(self):
if self.time_slider_thread is not None:
self.time_slider_thread.stop()
self.time_slider_thread = None
def update_gait_duration(self, duration):
rescale_setpoints = self.view.scale_setpoints_check_box.isChecked()
if (self.subgait.has_setpoints_after_duration(duration)
and not rescale_setpoints):
if not self.subgait.has_multiple_setpoints_before_duration(
duration):
self.view.message(
title="Could not update gait duration",
msg=
"Not all joints have multiple setpoints before duration " +
str(duration),
)
self.view.set_duration_spinbox(self.subgait.duration)
return
discard_setpoints = self.view.yes_no_question(
title="Gait duration lower than highest time setpoint",
msg="Do you want to discard any setpoints higher than the "
"given duration?",
)
if not discard_setpoints:
self.view.set_duration_spinbox(self.subgait.duration)
return
for joint in self.subgait.joints:
joint.save_setpoints(single_joint_change=False)
self.save_changed_settings({"joints": self.subgait.joints})
self.subgait.scale_timestamps_subgait(duration, rescale_setpoints)
self.view.time_slider.setRange(0, 100 * self.subgait.duration)
was_playing = self.time_slider_thread is not None
self.stop_time_slider_thread()
self.view.update_joint_widgets(self.subgait.joints)
if was_playing:
self.start_time_slider_thread()
def import_gait(self):
file_name, f = self.view.open_file_dialogue(self.current_gait_path)
if file_name != "":
gait = ModifiableSubgait.from_file(self.robot, file_name, self)
else:
gait = None
if gait is None:
rospy.logwarn("Could not load gait %s", file_name)
return
if gait.gait_type is None or gait.gait_type == "":
gait.gait_type = "walk_like"
self.subgait = gait
was_playing = self.time_slider_thread is not None
self.stop_time_slider_thread()
self.view.load_gait_into_ui(self.subgait)
for joint in self.subgait.joints:
self.connect_plot(joint)
self.current_time = 0
if was_playing:
self.start_time_slider_thread()
# The gait directory of the selected gait is always 3 directories
# behind the .subgait file
# (gait_directory/gait/subgait/version.subgait).
self.current_gait_path = os.path.dirname(os.path.dirname(file_name))
self.gait_directory = os.path.dirname(self.current_gait_path)
rospy.loginfo("Setting gait directory to %s", str(self.gait_directory))
# Display only the actual directory under
# which gaits will be saved for readability
gait_directory_text = "gait directory: " + os.path.basename(
self.gait_directory)
self.view.change_gait_directory_button.setText(gait_directory_text)
# Clear undo and redo history
self.settings_changed_history = RingBuffer(capacity=100, dtype=list)
self.settings_changed_redo_list = RingBuffer(capacity=100, dtype=list)
def import_side_subgait(self, side="previous"):
file_name, f = self.view.open_file_dialogue()
if file_name != "":
subgait = ModifiableSubgait.from_file(self.robot, file_name, self)
else:
subgait = None
if subgait is None:
rospy.logwarn("Could not load gait %s", file_name)
return
if side == "previous":
self.previous_subgait = subgait
elif side == "next":
self.next_subgait = subgait
def export_gait(self):
if self.view.mirror_check_box.isChecked():
key_1 = self.view.mirror_key1_line_edit.text()
key_2 = self.view.mirror_key2_line_edit.text()
mirror = self.subgait.get_mirror(key_1, key_2)
if mirror:
self.export_to_file(mirror, self.get_gait_directory())
else:
self.view.notify("Could not mirror gait",
"Check the logs for more information.")
return
self.export_to_file(self.subgait, self.get_gait_directory())
def export_to_file(self, subgait, gait_directory):
if gait_directory is None or gait_directory == "":
return
output_file_directory = os.path.join(
gait_directory,
subgait.gait_name.replace(" ", "_"),
subgait.subgait_name.replace(" ", "_"),
)
output_file_path = os.path.join(
output_file_directory,
subgait.version.replace(" ", "_") + ".subgait")
file_exists = os.path.isfile(output_file_path)
if file_exists:
overwrite_file = self.view.yes_no_question(
title="File already exists",
msg="Do you want to overwrite " + str(output_file_path) + "?",
)
if not overwrite_file:
return
rospy.loginfo("Writing gait to " + output_file_path)
if not os.path.isdir(output_file_directory):
os.makedirs(output_file_directory)
with open(output_file_path, "w") as output_file:
subgait_dict = subgait.to_dict()
# remove the version field when writing because it is
# superfluous considering the file name
if "version" in subgait_dict:
del subgait_dict["version"]
output_file.write(yaml.dump(subgait_dict))
self.view.notify("Gait Saved", output_file_path)
# Called by export_gait
def get_gait_directory(self):
if self.gait_directory is None:
self.change_gait_directory()
rospy.loginfo("Selected output directory " + str(self.gait_directory))
return self.gait_directory
def change_gait_directory(self):
previous_gait_directory = self.gait_directory
self.gait_directory = str(self.view.open_directory_dialogue())
# If directory dialogue is canceled, or an invalid directory is selected, leave gait_directory unchanged
if self.gait_directory == "" or self.gait_directory is None:
self.gait_directory = previous_gait_directory
# Valid gait directories are limited to direct subdirectories of march_gait_files (here march_gait_files must
# proceed the final directory in the path) and directories named 'resources' for testing (here resources must be
# the final element of the gait directory path). In any other case the directory is invalid. If the path
# contains but 1 element it must be invalid.
elif (os.path.basename(os.path.dirname(
self.gait_directory)) != "march_gait_files"
and os.path.basename(self.gait_directory) != "resources"):
rospy.logwarn(
"Gait directory path invalid. The gait directory must be a direct subdirectory of"
" march_gait_files or be named resources. Change gait directory cancelled"
)
self.gait_directory = previous_gait_directory
if self.gait_directory is not None:
rospy.loginfo("Setting gait directory to %s",
str(self.gait_directory))
# Display only the actual directory under which gaits will be saved for readability
gait_directory_text = "gait directory: " + os.path.basename(
self.gait_directory)
self.view.change_gait_directory_button.setText(gait_directory_text)
def add_inverse_kinematics_setpoints(self) -> None:
"""Add setpoints to the gait based on a user specified desired foot location."""
self.view.get_inverse_kinematics_setpoints_input()
# Check whether the input is valid
if self.view.inverse_kinematics_pop_up.cancelled:
rospy.loginfo(
"The inputs for the inverse kinematics were cancelled.")
return
if not 0 <= self.view.inverse_kinematics_pop_up.time <= self.subgait.duration:
warning_message = (
"The inverse kinematics setpoints feature has failed."
f"The specified time is invalid. "
f"Should be between 0 and the subgait duration {self.subgait.duration}. "
f"{self.view.inverse_kinematics_pop_up.time} was given.")
rospy.loginfo(warning_message)
self.view.message(
"The inverse kinematics setpoints feature has failed.",
warning_message)
return
# Transform the input from relative to the default position to relative to the exoskeleton
self.transform_inverse_kinematics_setpoints_inputs()
# Calculate the setpoints from the desired foot coordinates and add them to the gait
try:
setpoint_dictionary = self.get_setpoints_from_inverse_kinematics_input(
)
self.add_setpoints_from_dictionary(setpoint_dictionary)
except SubgaitInterpolationError:
traceback.print_exc()
self.view.message(
"The inverse kinematics setpoints feature as failed.",
"A subgait interpolation error occured, see the terminal for more information.",
)
except ValueError:
traceback.print_exc()
self.view.message(
"The inverse kinematics setpoints feature has failed.",
"A ValueError occured, see the terminal for more information",
)
def transform_inverse_kinematics_setpoints_inputs(self) -> None:
"""Transform the input coordinates (relative to a default foot location) to coordinates relative to the exo."""
foot_side = self.view.inverse_kinematics_pop_up.foot_side
[
upper_leg_length,
lower_leg_length,
haa_to_leg_length,
haa_arm,
base,
] = get_lengths_robot_for_inverse_kinematics(foot_side)
self.transform_inverse_kinematics_setpoints_x_coordinate(
haa_to_leg_length)
self.transform_inverse_kinematics_setpoints_y_coordinate(
haa_arm, base, foot_side)
self.transform_inverse_kinematics_setpoints_z_coordinate(
upper_leg_length, lower_leg_length)
def transform_inverse_kinematics_setpoints_x_coordinate(
self, haa_to_leg_length: float) -> None:
"""Add the default x coordinate to the desired x coordinate to transform to exoskeleton coordinate system."""
default_x_position = haa_to_leg_length
self.view.inverse_kinematics_pop_up.position_input.x += default_x_position
def transform_inverse_kinematics_setpoints_y_coordinate(
self, haa_arm: float, base: float, foot_side: float) -> None:
"""Add the default y coordinate to the desired y coordinate to transform to exoskeleton coordinate system."""
hip_to_foot_length_cm = base / 2 + haa_arm
if foot_side == Side.right:
default_y_position = hip_to_foot_length_cm
else:
default_y_position = -hip_to_foot_length_cm
self.view.inverse_kinematics_pop_up.position_input.y += default_y_position
def transform_inverse_kinematics_setpoints_z_coordinate(
self, upper_leg_length: float, lower_leg_length: float) -> None:
"""Transform the z coordinate of the input to the coordinate system of the exoskeleton."""
if self.view.inverse_kinematics_pop_up.z_axis == "From ground upwards":
ground_z_coordinate_cm = upper_leg_length + lower_leg_length
self.view.inverse_kinematics_pop_up.position_input.z = (
ground_z_coordinate_cm -
self.view.inverse_kinematics_pop_up.position_input.z)
def get_setpoints_from_inverse_kinematics_input(self) -> None:
"""Use the inverse kinematics function to translate the desired foot coordinates to setpoints."""
desired_foot_state = Foot(
self.view.inverse_kinematics_pop_up.foot_side,
self.view.inverse_kinematics_pop_up.position_input,
self.view.inverse_kinematics_pop_up.velocity_input,
)
return Foot.get_joint_states_from_foot_state(
desired_foot_state, self.view.inverse_kinematics_pop_up.time)
def add_setpoints_from_dictionary(
self, setpoint_dictionary: Dict[str, any]) -> None:
"""Add setpoints from a dictionary with joints as keys to the current subgait."""
for joint_name in setpoint_dictionary:
time = setpoint_dictionary[joint_name].time
position = setpoint_dictionary[joint_name].position
velocity = setpoint_dictionary[joint_name].velocity
joint = self.subgait.get_joint(joint_name)
# If the current joint already has a setpoint at the specified time, replace it
# Otherwise, add the setpoint to the joint.
setpoint_is_replaced = False
for index, setpoint in enumerate(joint.setpoints):
if setpoint.time == time:
joint.replace_setpoint(index,
setpoint_dictionary[joint_name])
setpoint_is_replaced = True
if not setpoint_is_replaced:
joint.add_setpoint(ModifiableSetpoint(time, position,
velocity))
self.view.update_joint_widget(joint)
self.view.publish_preview(self.subgait, self.current_time)
def invert_gait(self) -> None:
for side, controller in self.side_subgait_controller.items():
controller.lock_checked = False
self.handle_sidepoint_lock(side)
for joint in self.subgait.joints:
joint.invert()
self.view.update_joint_widget(joint)
self.save_changed_settings({
"joints":
self.subgait.joints,
"side_subgaits":
self.side_subgait_controller.values(),
})
self.view.publish_preview(self.subgait, self.current_time)
def undo(self):
if not self.settings_changed_history:
return
changed_dict = self.settings_changed_history.pop()
if "joints" in changed_dict:
joints = changed_dict["joints"]
for joint in joints:
joint.undo()
self.subgait.scale_timestamps_subgait(joints[0].setpoints[-1].time,
rescale=False)
self.view.set_duration_spinbox(self.subgait.duration)
if "side_subgaits" in changed_dict:
side_subgait_controllers = changed_dict["side_subgaits"]
for controller in side_subgait_controllers:
controller.undo()
self.view.update_joint_widgets(self.subgait.joints)
self.view.publish_preview(self.subgait, self.current_time)
self.settings_changed_redo_list.append(changed_dict)
def redo(self):
if not self.settings_changed_redo_list:
return
changed_dict = self.settings_changed_redo_list.pop()
if "joints" in changed_dict:
joints = changed_dict["joints"]
for joint in joints:
joint.redo()
self.subgait.scale_timestamps_subgait(joints[0].setpoints[-1].time,
rescale=False)
self.view.set_duration_spinbox(self.subgait.duration)
if "side_subgaits" in changed_dict:
side_subgait_controllers = changed_dict["side_subgaits"]
for controller in side_subgait_controllers:
controller.redo()
self.view.update_joint_widgets(self.subgait.joints)
self.view.publish_preview(self.subgait, self.current_time)
self.settings_changed_history.append(changed_dict)
# Needed for undo and redo.
def save_changed_settings(self, settings):
self.settings_changed_history.append(settings)
self.settings_changed_redo_list = RingBuffer(capacity=100, dtype=list)
# Functions related to previous/next subgait
def toggle_side_subgait_checkbox(self, value, side, box_type):
self.save_changed_settings({
"joints":
self.subgait.joints,
"side_subgaits": [self.side_subgait_controller[side]],
})
for joint in self.subgait.joints:
joint.save_setpoints(single_joint_change=False)
if box_type == "lock":
self.side_subgait_controller[side].lock_checked = value
elif box_type == "standing":
self.side_subgait_controller[side].default_checked = value
self.handle_sidepoint_lock(side)
def handle_sidepoint_lock(self, side):
if self.side_subgait_controller[side].lock_checked:
if self.side_subgait_controller[side].subgait is not None:
for joint in self.subgait.joints:
side_subgait_joint = self.side_subgait_controller[
side].subgait.get_joint(joint.name)
if side == "previous":
joint.start_point = side_subgait_joint.setpoints[-1]
elif side == "next":
joint.end_point = side_subgait_joint.setpoints[0]
else:
for joint in self.subgait.joints:
if side == "previous":
joint.start_point = joint.setpoints[0]
elif side == "next":
joint.end_point = joint.setpoints[-1]
else:
for joint in self.subgait.joints:
if side == "previous":
joint.start_point = None
elif side == "next":
joint.end_point = None
self.view.update_joint_widgets(self.subgait.joints)
self.view.publish_preview(self.subgait, self.current_time)
@property
def previous_subgait(self):
return self.side_subgait_controller["previous"].subgait
@previous_subgait.setter
def previous_subgait(self, new_subgait):
self.save_changed_settings({
"joints":
self.subgait.joints,
"side_subgaits": [self.side_subgait_controller["previous"]],
})
for joint in self.subgait.joints:
joint.save_setpoints(single_joint_change=False)
self.side_subgait_controller["previous"].subgait = new_subgait
self.handle_sidepoint_lock("previous")
@property
def next_subgait(self):
return self.side_subgait_controller["next"].subgait
@next_subgait.setter
def next_subgait(self, new_subgait):
self.save_changed_settings({
"joints":
self.subgait.joints,
"side_subgaits": [self.side_subgait_controller["next"]],
})
for joint in self.subgait.joints:
joint.save_setpoints(single_joint_change=False)
self.side_subgait_controller["next"].subgait = new_subgait
self.handle_sidepoint_lock("next")
class ModifiableJointTrajectory(JointTrajectory):
setpoint_class = ModifiableSetpoint
def __init__(self, name, limits, setpoints, duration, gait_generator=None):
super(ModifiableJointTrajectory,
self).__init__(name, limits, setpoints, duration)
self.setpoints_history = RingBuffer(capacity=100, dtype=list)
self.setpoints_redo_list = RingBuffer(capacity=100, dtype=list)
self.gait_generator = gait_generator
self._start_point = None
self._end_point = None
if self.setpoints[0].time != 0:
rospy.logwarn("First setpoint of {0} has been set "
"from {1} to 0".format(name, self.setpoints[0].time))
if self.setpoints[-1].time != duration:
rospy.logwarn("Last setpoint of {0} has been set "
"from {1} to {2}".format(name,
self.setpoints[0].time,
duration))
def set_setpoints(self, setpoints):
self.setpoints = setpoints
def set_duration(self, new_duration, rescale=True):
super(ModifiableJointTrajectory,
self).set_duration(new_duration, rescale)
self.enforce_limits()
@JointTrajectory.setpoints.setter
def setpoints(self, setpoints):
self._setpoints = setpoints
self.enforce_limits()
self.interpolate_setpoints()
def enforce_limits(self):
if self.start_point:
self.setpoints[0].position = self.start_point.position
self.setpoints[0].velocity = self.start_point.velocity
if self.end_point:
self.setpoints[-1].position = self.end_point.position
self.setpoints[-1].velocity = self.end_point.velocity
self.setpoints[0].time = 0
self.setpoints[-1].time = self.duration
for setpoint in self.setpoints:
setpoint.position = min(max(setpoint.position, self.limits.lower),
self.limits.upper)
setpoint.velocity = min(
max(setpoint.velocity, -self.limits.velocity),
self.limits.velocity)
def add_interpolated_setpoint(self, time):
self.add_setpoint(self.get_interpolated_setpoint(time))
def add_setpoint(self, setpoint):
self.save_setpoints()
# Calculate at what index the new setpoint should be added.
new_index = len(self.setpoints)
for i in range(len(self.setpoints)):
if self.setpoints[i].time > setpoint.time:
new_index = i
break
self.setpoints.insert(new_index, setpoint)
self.enforce_limits()
self.interpolate_setpoints()
def replace_setpoint(self, index, new_setpoint):
self.save_setpoints()
self.setpoints[index] = new_setpoint
self.enforce_limits()
self.interpolate_setpoints()
def remove_setpoint(self, index):
self.save_setpoints()
del self.setpoints[index]
self.enforce_limits()
self.interpolate_setpoints()
def save_setpoints(self, single_joint_change=True):
self.setpoints_history.append({
"setpoints":
copy.deepcopy(self.setpoints),
"start_point":
self.start_point,
"end_point":
self.end_point,
})
if single_joint_change:
self.gait_generator.save_changed_settings({"joints": [self]})
def invert(self):
self.save_setpoints(single_joint_change=False)
for setpoint in self.setpoints:
setpoint.invert(self.duration)
self.setpoints = list(reversed(self.setpoints))
self.interpolate_setpoints()
def undo(self):
if not self.setpoints_history:
return
self.setpoints_redo_list.append({
"setpoints":
copy.deepcopy(self.setpoints),
"start_point":
self.start_point,
"end_point":
self.end_point,
})
setpoints = self.setpoints_history.pop()
self._setpoints = setpoints["setpoints"]
self._start_point = setpoints["start_point"]
self._end_point = setpoints["end_point"]
self._duration = self.setpoints[-1].time
self.enforce_limits()
self.interpolate_setpoints()
def redo(self):
if not self.setpoints_redo_list:
return
self.setpoints_history.append({
"setpoints":
copy.deepcopy(self.setpoints),
"start_point":
self.start_point,
"end_point":
self.end_point,
})
setpoints = self.setpoints_redo_list.pop()
self._setpoints = setpoints["setpoints"]
self._start_point = setpoints["start_point"]
self._end_point = setpoints["end_point"]
self._duration = self.setpoints[-1].time
self.enforce_limits()
self.interpolate_setpoints()
@property
def start_point(self):
return self._start_point
@start_point.setter
def start_point(self, start_point):
self._start_point = start_point
self.enforce_limits()
self.interpolated_setpoints = self.interpolate_setpoints()
@property
def end_point(self):
return self._end_point
@end_point.setter
def end_point(self, end_point):
self._end_point = end_point
self.enforce_limits()
self.interpolated_setpoints = self.interpolate_setpoints()
class GaitGeneratorController(object):
def __init__(self, view, robot):
self.view = view
# Default values
self.gait_directory = None
self.playback_speed = 100
self.time_slider_thread = None
self.current_time = 0
self.robot = robot
empty_subgait_file = os.path.join(
rospkg.RosPack().get_path('march_rqt_gait_generator'),
'resource/empty_gait/empty_subgait/empty_subgait.subgait')
self.subgait = ModifiableSubgait.from_file(robot, empty_subgait_file,
self)
self.settings_changed_history = RingBuffer(capacity=100, dtype=list)
self.settings_changed_redo_list = RingBuffer(capacity=100, dtype=list)
standing = Subgait.from_file(robot, empty_subgait_file)
previous_subgait_controller = SideSubgaitController(
view=self.view.side_subgait_view['previous'], default=standing)
next_subgait_controller = SideSubgaitController(
view=self.view.side_subgait_view['next'], default=standing)
self.side_subgait_controller = {
'previous': previous_subgait_controller,
'next': next_subgait_controller
}
self.connect_buttons()
self.view.load_gait_into_ui(self.subgait)
for joint in self.subgait.joints:
self.connect_plot(joint)
# Called by __init__
def connect_buttons(self):
self.view.change_gait_directory_button.clicked.connect(
self.change_gait_directory)
self.view.import_gait_button.clicked.connect(self.import_gait)
self.view.export_gait_button.clicked.connect(self.export_gait)
self.view.side_subgait_view['previous'].import_button.clicked.connect(
lambda: self.import_side_subgait('previous'))
self.view.side_subgait_view[
'previous'].default_checkbox.stateChanged.connect(
lambda value: self.toggle_side_subgait_checkbox(
value, 'previous', 'standing'))
self.view.side_subgait_view[
'previous'].lock_checkbox.stateChanged.connect(
lambda value: self.toggle_side_subgait_checkbox(
value, 'previous', 'lock'))
self.view.side_subgait_view['next'].import_button.clicked.connect(
lambda: self.import_side_subgait('next'))
self.view.side_subgait_view[
'next'].default_checkbox.stateChanged.connect(
lambda value: self.toggle_side_subgait_checkbox(
value, 'next', 'standing'))
self.view.side_subgait_view['next'].lock_checkbox.stateChanged.connect(
lambda value: self.toggle_side_subgait_checkbox(
value, 'next', 'lock'))
self.view.start_button.clicked.connect(self.start_time_slider_thread)
self.view.stop_button.clicked.connect(self.stop_time_slider_thread)
self.view.invert_button.clicked.connect(self.invert_gait)
self.view.undo_button.clicked.connect(self.undo)
self.view.redo_button.clicked.connect(self.redo)
self.view.ctrl_z.activated.connect(self.undo)
self.view.ctrl_shift_z.activated.connect(self.redo)
# Line edits / combo boxes / spin boxes
self.view.gait_type_combo_box.currentTextChanged.connect(
lambda text: self.subgait.set_gait_type(text), )
self.view.gait_name_line_edit.textChanged.connect(
lambda text: self.subgait.set_gait_name(text), )
self.view.version_name_line_edit.textChanged.connect(
lambda text: self.subgait.set_version(text), )
self.view.subgait_name_line_edit.textChanged.connect(
lambda text: self.subgait.set_subgait_name(text), )
self.view.description_line_edit.textChanged.connect(
lambda text: self.subgait.set_description(text), )
self.view.playback_speed_spin_box.valueChanged.connect(
self.set_playback_speed)
self.view.duration_spin_box.setKeyboardTracking(False)
self.view.duration_spin_box.valueChanged.connect(
self.update_gait_duration)
# Check boxes
self.view.velocity_plot_check_box.stateChanged.connect(
lambda: self.view.update_joint_widgets(self.subgait.joints), )
self.view.effort_plot_check_box.stateChanged.connect(
lambda: self.view.update_joint_widgets(self.subgait.joints), )
# Disable key inputs when mirroring is off.
self.view.mirror_check_box.stateChanged.connect(lambda state: [
self.view.mirror_key1_line_edit.setEnabled(state),
self.view.mirror_key2_line_edit.setEnabled(state),
])
# Connect TimeSlider to the preview
self.view.time_slider.valueChanged.connect(lambda time: [
self.set_current_time(time / 100.0),
self.view.publish_preview(self.subgait, self.current_time),
self.view.update_time_sliders(self.current_time),
])
# Called by __init__
def connect_plot(self, joint):
joint_widget = self.view.joint_widgets[joint.name]
joint_plot = joint_widget.Plot.getItem(0, 0)
def add_setpoint(joint, time, position, button):
if button == self.view.control_button:
joint.add_interpolated_setpoint(time)
else:
joint.add_setpoint(ModifiableSetpoint(time, position, 0))
# Connect a function to update the model and to update the table.
joint_plot.plot_item.sigPlotChanged.connect(lambda: [
joint.set_setpoints(joint_plot.to_setpoints()),
self.view.update_joint_widget(joint),
self.view.publish_preview(self.subgait, self.current_time),
])
joint_plot.add_setpoint.connect(lambda time, position, button: [
add_setpoint(joint, time, position, button),
self.view.update_joint_widget(joint),
self.view.publish_preview(self.subgait, self.current_time),
])
joint_plot.remove_setpoint.connect(lambda index: [
joint.remove_setpoint(index),
self.view.update_joint_widget(joint),
self.view.publish_preview(self.subgait, self.current_time),
])
joint_widget.Table.itemChanged.connect(lambda: [
joint.save_setpoints(),
self.save_changed_settings({'joints': [joint]}),
joint.set_setpoints(joint_widget.Table.controller.to_setpoints()),
self.view.update_joint_widget(joint, update_table=False),
self.view.publish_preview(self.subgait, self.current_time),
])
# Functions below are connected to buttons, text boxes, joint graphs etc.
def set_playback_speed(self, playback_speed):
was_playing = self.time_slider_thread is not None
self.stop_time_slider_thread()
self.playback_speed = playback_speed
rospy.loginfo('Changing playbackspeed to ' + str(self.playback_speed))
if was_playing:
self.start_time_slider_thread()
def set_current_time(self, current_time):
self.current_time = current_time
def start_time_slider_thread(self):
if self.time_slider_thread is not None:
rospy.logdebug(
'Cannot start another time slider thread as one is already active'
)
return
current = self.view.time_slider.value()
playback_speed = self.playback_speed
max_time = self.view.time_slider.maximum()
self.time_slider_thread = self.view.create_time_slider_thread(
current, playback_speed, max_time)
self.time_slider_thread.update_signal.connect(
self.view.update_main_time_slider)
self.time_slider_thread.start()
def stop_time_slider_thread(self):
if self.time_slider_thread is not None:
self.time_slider_thread.stop()
self.time_slider_thread = None
def update_gait_duration(self, duration):
rescale_setpoints = self.view.scale_setpoints_check_box.isChecked()
if self.subgait.has_setpoints_after_duration(
duration) and not rescale_setpoints:
if not self.subgait.has_multiple_setpoints_before_duration(
duration):
self.view.message(
title='Could not update gait duration',
msg=
'Not all joints have multiple setpoints before duration ' +
str(duration))
self.view.set_duration_spinbox(self.subgait.duration)
return
discard_setpoints = self.view.yes_no_question(
title='Gait duration lower than highest time setpoint',
msg='Do you want to discard any setpoints higher than the '
'given duration?')
if not discard_setpoints:
self.view.set_duration_spinbox(self.subgait.duration)
return
for joint in self.subgait.joints:
joint.save_setpoints(single_joint_change=False)
self.save_changed_settings({'joints': self.subgait.joints})
self.subgait.scale_timestamps_subgait(duration, rescale_setpoints)
self.view.time_slider.setRange(0, 100 * self.subgait.duration)
was_playing = self.time_slider_thread is not None
self.stop_time_slider_thread()
self.view.update_joint_widgets(self.subgait.joints)
if was_playing:
self.start_time_slider_thread()
def import_gait(self):
file_name, f = self.view.open_file_dialogue()
if file_name != '':
gait = ModifiableSubgait.from_file(self.robot, file_name, self)
else:
gait = None
if gait is None:
rospy.logwarn('Could not load gait %s', file_name)
return
if gait.gait_type is None or gait.gait_type == '':
gait.gait_type = 'walk_like'
self.subgait = gait
was_playing = self.time_slider_thread is not None
self.stop_time_slider_thread()
self.view.load_gait_into_ui(self.subgait)
for joint in self.subgait.joints:
self.connect_plot(joint)
self.current_time = 0
if was_playing:
self.start_time_slider_thread()
self.gait_directory = '/'.join(file_name.split('/')[:-3])
rospy.loginfo('Setting gait directory to %s', str(self.gait_directory))
self.view.change_gait_directory_button.setText(self.gait_directory)
# Clear undo and redo history
self.settings_changed_history = RingBuffer(capacity=100, dtype=list)
self.settings_changed_redo_list = RingBuffer(capacity=100, dtype=list)
def import_side_subgait(self, side='previous'):
file_name, f = self.view.open_file_dialogue()
if file_name != '':
subgait = ModifiableSubgait.from_file(self.robot, file_name, self)
else:
subgait = None
if subgait is None:
rospy.logwarn('Could not load gait %s', file_name)
return
if side == 'previous':
self.previous_subgait = subgait
elif side == 'next':
self.next_subgait = subgait
def export_gait(self):
if self.view.mirror_check_box.isChecked():
key_1 = self.view.mirror_key1_line_edit.text()
key_2 = self.view.mirror_key2_line_edit.text()
mirror = self.subgait.get_mirror(key_1, key_2)
if mirror:
self.export_to_file(mirror, self.get_gait_directory())
else:
self.view.notify('Could not mirror gait',
'Check the logs for more information.')
return
self.export_to_file(self.subgait, self.get_gait_directory())
def export_to_file(self, subgait, gait_directory):
if gait_directory is None or gait_directory == '':
return
output_file_directory = os.path.join(
gait_directory, subgait.gait_name.replace(' ', '_'),
subgait.subgait_name.replace(' ', '_'))
output_file_path = os.path.join(
output_file_directory,
subgait.version.replace(' ', '_') + '.subgait')
file_exists = os.path.isfile(output_file_path)
if file_exists:
overwrite_file = self.view.yes_no_question(
title='File already exists',
msg='Do you want to overwrite ' + str(output_file_path) + '?')
if not overwrite_file:
return
rospy.loginfo('Writing gait to ' + output_file_path)
if not os.path.isdir(output_file_directory):
os.makedirs(output_file_directory)
with open(output_file_path, 'w') as output_file:
output_file.write(subgait.to_yaml())
self.view.notify('Gait Saved', output_file_path)
# Called by export_gait
def get_gait_directory(self):
if self.gait_directory is None:
self.change_gait_directory()
rospy.loginfo('Selected output directory ' + str(self.gait_directory))
return self.gait_directory
def change_gait_directory(self):
self.gait_directory = str(self.view.open_directory_dialogue())
if self.gait_directory == '': # If directory dialogue is canceled
self.gait_directory = None
self.view.change_gait_directory_button.setText(
'Select a gait directory...')
else:
self.view.change_gait_directory_button.setText(self.gait_directory)
def invert_gait(self):
for side, controller in self.side_subgait_controller.items():
controller.lock_checked = False
self.handle_sidepoint_lock(side)
for joint in self.subgait.joints:
joint.invert()
self.view.update_joint_widget(joint)
self.save_changed_settings({
'joints':
self.subgait.joints,
'side_subgaits':
self.side_subgait_controller.values()
})
self.view.publish_preview(self.subgait, self.current_time)
def undo(self):
if not self.settings_changed_history:
return
changed_dict = self.settings_changed_history.pop()
if 'joints' in changed_dict:
joints = changed_dict['joints']
for joint in joints:
joint.undo()
self.subgait.scale_timestamps_subgait(joints[0].setpoints[-1].time,
rescale=False)
self.view.set_duration_spinbox(self.subgait.duration)
if 'side_subgaits' in changed_dict:
side_subgait_controllers = changed_dict['side_subgaits']
for controller in side_subgait_controllers:
controller.undo()
self.view.update_joint_widgets(self.subgait.joints)
self.view.publish_preview(self.subgait, self.current_time)
self.settings_changed_redo_list.append(changed_dict)
def redo(self):
if not self.settings_changed_redo_list:
return
changed_dict = self.settings_changed_redo_list.pop()
if 'joints' in changed_dict:
joints = changed_dict['joints']
for joint in joints:
joint.redo()
self.subgait.scale_timestamps_subgait(joints[0].setpoints[-1].time,
rescale=False)
self.view.set_duration_spinbox(self.subgait.duration)
if 'side_subgaits' in changed_dict:
side_subgait_controllers = changed_dict['side_subgaits']
for controller in side_subgait_controllers:
controller.redo()
self.view.update_joint_widgets(self.subgait.joints)
self.view.publish_preview(self.subgait, self.current_time)
self.settings_changed_history.append(changed_dict)
# Needed for undo and redo.
def save_changed_settings(self, settings):
self.settings_changed_history.append(settings)
self.settings_changed_redo_list = RingBuffer(capacity=100, dtype=list)
# Functions related to previous/next subgait
def toggle_side_subgait_checkbox(self, value, side, box_type):
self.save_changed_settings({
'joints':
self.subgait.joints,
'side_subgaits': [self.side_subgait_controller[side]]
})
for joint in self.subgait.joints:
joint.save_setpoints(single_joint_change=False)
if box_type == 'lock':
self.side_subgait_controller[side].lock_checked = value
elif box_type == 'standing':
self.side_subgait_controller[side].default_checked = value
self.handle_sidepoint_lock(side)
def handle_sidepoint_lock(self, side):
if self.side_subgait_controller[side].lock_checked:
if self.side_subgait_controller[side].subgait is not None:
for joint in self.subgait.joints:
side_subgait_joint = self.side_subgait_controller[
side].subgait.get_joint(joint.name)
if side == 'previous':
joint.start_point = side_subgait_joint.setpoints[-1]
elif side == 'next':
joint.end_point = side_subgait_joint.setpoints[0]
else:
for joint in self.subgait.joints:
if side == 'previous':
joint.start_point = joint.setpoints[0]
elif side == 'next':
joint.end_point = joint.setpoints[-1]
else:
for joint in self.subgait.joints:
if side == 'previous':
joint.start_point = None
elif side == 'next':
joint.end_point = None
self.view.update_joint_widgets(self.subgait.joints)
self.view.publish_preview(self.subgait, self.current_time)
@property
def previous_subgait(self):
return self.side_subgait_controller['previous'].subgait
@previous_subgait.setter
def previous_subgait(self, new_subgait):
self.save_changed_settings({
'joints':
self.subgait.joints,
'side_subgaits': [self.side_subgait_controller['previous']]
})
for joint in self.subgait.joints:
joint.save_setpoints(single_joint_change=False)
self.side_subgait_controller['previous'].subgait = new_subgait
self.handle_sidepoint_lock('previous')
@property
def next_subgait(self):
return self.side_subgait_controller['next'].subgait
@next_subgait.setter
def next_subgait(self, new_subgait):
self.save_changed_settings({
'joints':
self.subgait.joints,
'side_subgaits': [self.side_subgait_controller['next']]
})
for joint in self.subgait.joints:
joint.save_setpoints(single_joint_change=False)
self.side_subgait_controller['next'].subgait = new_subgait
self.handle_sidepoint_lock('next')
class GaitGeneratorPlugin(Plugin):
def __init__(self, context):
super(GaitGeneratorPlugin, self).__init__(context)
# Default values
self.gait_publisher = None
self.topic_name = ''
self.gait_directory = None
self.playback_speed = 100
self.time_slider_thread = None
self.current_time = 0
self.tf_listener = TransformListener()
self.joint_state_pub = rospy.Publisher('joint_states',
JointState,
queue_size=10)
self.joint_changed_history = RingBuffer(capacity=100, dtype=list)
self.joint_changed_redo_list = RingBuffer(capacity=100, dtype=list)
self.robot = urdf.Robot.from_parameter_server()
self.gait = ModifiableSubgait.empty_subgait(self, self.robot)
self.build_ui(context)
self.set_topic_name(self.topic_name_line_edit.text())
self.load_gait_into_ui()
# Called by __init__
def build_ui(self, context):
# Start UI construction
self._widget = QWidget()
ui_file = os.path.join(
rospkg.RosPack().get_path('march_rqt_gait_generator'), 'resource',
'gait_generator.ui')
loadUi(ui_file, self._widget)
context.add_widget(self._widget)
self.rviz_frame = self.create_rviz_frame()
self._widget.RvizFrame.layout().addWidget(self.rviz_frame, 1, 0, 1, 3)
# Store ui elements.
self.change_gait_directory_button = self._widget.SettingsFrame.findChild(
QPushButton, 'ChangeGaitDirectory')
self.import_gait_button = self._widget.SettingsFrame.findChild(
QPushButton, 'Import')
self.export_gait_button = self._widget.SettingsFrame.findChild(
QPushButton, 'Export')
self.publish_gait_button = self._widget.SettingsFrame.findChild(
QPushButton, 'Publish')
self.start_button = self._widget.RvizFrame.findChild(
QPushButton, 'Start')
self.stop_button = self._widget.RvizFrame.findChild(
QPushButton, 'Stop')
self.invert_button = self._widget.RvizFrame.findChild(
QPushButton, 'Invert')
self.undo_button = self._widget.RvizFrame.findChild(
QPushButton, 'Undo')
self.redo_button = self._widget.RvizFrame.findChild(
QPushButton, 'Redo')
self.playback_speed_spin_box = self._widget.RvizFrame.findChild(
QSpinBox, 'PlaybackSpeed')
self.height_left_line_edit = self._widget.RvizFrame.findChild(
QLineEdit, 'HeightLeft')
self.height_right_line_edit = self._widget.RvizFrame.findChild(
QLineEdit, 'HeightRight')
self.heel_distance_line_edit = self._widget.RvizFrame.findChild(
QLineEdit, 'HeelHeelDistance')
self.topic_name_line_edit = self._widget.SettingsFrame.findChild(
QLineEdit, 'TopicName')
self.gait_name_line_edit = self._widget.GaitPropertiesFrame.findChild(
QLineEdit, 'Gait')
self.subgait_name_line_edit = self._widget.GaitPropertiesFrame.findChild(
QLineEdit, 'Subgait')
self.version_name_line_edit = self._widget.GaitPropertiesFrame.findChild(
QLineEdit, 'Version')
self.description_line_edit = self._widget.GaitPropertiesFrame.findChild(
QLineEdit, 'Description')
self.gait_type_combo_box = self._widget.GaitPropertiesFrame.findChild(
QComboBox, 'GaitType')
self.gait_type_combo_box.addItems(
['walk_like', 'sit_like', 'stairs_like'])
self.duration_spin_box = self._widget.GaitPropertiesFrame.findChild(
QDoubleSpinBox, 'Duration')
self.mirror_check_box = self._widget.SettingsFrame.findChild(
QCheckBox, 'Mirror')
self.mirror_key1_line_edit = self._widget.SettingsFrame.findChild(
QLineEdit, 'Key1')
self.mirror_key2_line_edit = self._widget.SettingsFrame.findChild(
QLineEdit, 'Key2')
self.velocity_markers_check_box = self._widget.SettingsFrame.findChild(
QCheckBox, 'ShowVelocityMarkers')
self.time_slider = self._widget.RvizFrame.findChild(
QSlider, 'TimeSlider')
self.scale_setpoints_check_box = self._widget.GaitPropertiesFrame.findChild(
QCheckBox, 'ScaleSetpoints')
self.connect_buttons()
# Called by build_ui
def create_rviz_frame(self):
frame = rviz.VisualizationFrame()
frame.initialize()
reader = rviz.YamlConfigReader()
config = rviz.Config()
reader.readFile(
config,
os.path.join(rospkg.RosPack().get_path('march_rqt_gait_generator'),
'resource', 'cfg.rviz'))
frame.load(config)
# Hide irrelevant Rviz details
frame.setMenuBar(None)
frame.setStatusBar(None)
frame.setHideButtonVisibility(False)
return frame
# Called by build_ui
def connect_buttons(self):
self.change_gait_directory_button.clicked.connect(
self.change_gait_directory)
self.import_gait_button.clicked.connect(self.import_gait)
self.export_gait_button.clicked.connect(self.export_gait)
self.publish_gait_button.clicked.connect(self.publish_gait)
self.start_button.clicked.connect(self.start_time_slider_thread)
self.stop_button.clicked.connect(self.stop_time_slider_thread)
self.invert_button.clicked.connect(self.invert_gait)
self.undo_button.clicked.connect(self.undo)
self.redo_button.clicked.connect(self.redo)
# Line edits / combo boxes / spin boxes
self.gait_type_combo_box.currentTextChanged.connect(
lambda text: self.gait.set_gait_type(text), )
self.gait_name_line_edit.textChanged.connect(
lambda text: self.gait.set_gait_name(text), )
self.version_name_line_edit.textChanged.connect(
lambda text: self.gait.set_version(text), )
self.subgait_name_line_edit.textChanged.connect(
lambda text: self.gait.set_subgait_name(text), )
self.description_line_edit.textChanged.connect(
lambda text: self.gait.set_description(text), )
self.topic_name_line_edit.textChanged.connect(self.set_topic_name)
self.playback_speed_spin_box.valueChanged.connect(
self.set_playback_speed)
self.duration_spin_box.setKeyboardTracking(False)
self.duration_spin_box.valueChanged.connect(self.update_gait_duration)
# Disable key inputs when mirroring is off.
self.mirror_check_box.stateChanged.connect(
lambda state: [
self.mirror_key1_line_edit.setEnabled(state),
self.mirror_key2_line_edit.setEnabled(state),
], )
# Called by __init__ and import_gait.
def load_gait_into_ui(self):
self.time_slider.setRange(0, 100 * self.gait.duration)
# Connect TimeSlider to the preview
self.time_slider.valueChanged.connect(lambda: [
self.set_current_time(float(self.time_slider.value()) / 100),
self.publish_preview(),
self.update_time_sliders(),
])
self.gait_type_combo_box.setCurrentText(self.gait.gait_type)
self.gait_name_line_edit.setText(self.gait.gait_name)
self.subgait_name_line_edit.setText(self.gait.subgait_name)
self.version_name_line_edit.setText(self.gait.version)
self.description_line_edit.setText(self.gait.description)
# Block signals on the duration edit to prevent a reload of the joint settings
self.duration_spin_box.blockSignals(True)
self.duration_spin_box.setValue(self.gait.duration)
self.duration_spin_box.blockSignals(False)
self.create_joint_settings()
self.publish_preview()
# Called by load_gait_into_ui.
def update_time_sliders(self):
graphics_layouts = self._widget.JointSettingContainer.findChildren(
pg.GraphicsLayoutWidget)
for graphics_layout in graphics_layouts:
joint_settings_plot = graphics_layout.getItem(0, 0)
joint_settings_plot.update_time_slider(self.current_time)
# Called by load_gait_into_ui and update_gait_duration.
def create_joint_settings(self):
layout = self._widget.JointSettingContainer.layout()
for i in reversed(range(layout.count())):
widget = layout.itemAt(i).widget()
layout.removeWidget(widget)
widget.setParent(None)
for i in range(0, len(self.robot.joints)):
if self.robot.joints[i].type != 'fixed':
joint_name = self.robot.joints[i].name
joint = self.gait.get_joint(joint_name)
if not joint:
continue
row = rospy.get_param('/joint_layout/' + joint_name + '/row',
-1)
column = rospy.get_param(
'/joint_layout/' + joint_name + '/column', -1)
if row == -1 or column == -1:
rospy.logerr(
'Could not load the layout for joint %s. Please check config/layout.yaml',
joint_name)
continue
self._widget.JointSettingContainer.layout().addWidget(
self.create_joint_setting(joint), row, column)
# Called 8 times by create_joint_settings.
def create_joint_setting(self, joint):
joint_setting_file = os.path.join(
rospkg.RosPack().get_path('march_rqt_gait_generator'), 'resource',
'joint_setting.ui')
joint_setting = QFrame()
loadUi(joint_setting_file, joint_setting)
show_velocity_markers = self.velocity_markers_check_box.isChecked()
joint_setting_plot = JointSettingPlot(joint, self.gait.duration,
show_velocity_markers)
joint_setting.Plot.addItem(joint_setting_plot)
joint_setting.Table = user_interface_controller.update_table(
joint_setting.Table, joint, self.gait.duration)
# Disable scrolling horizontally
joint_setting.Table.horizontalScrollBar().setDisabled(True)
joint_setting.Table.horizontalHeader().setSectionResizeMode(
QHeaderView.Stretch)
def update_joint_ui():
user_interface_controller.update_ui_elements(
joint,
table=joint_setting.Table,
plot=joint_setting_plot,
duration=self.gait.duration,
show_velocity_markers=self.velocity_markers_check_box.
isChecked())
self.publish_preview()
def add_setpoint(joint, time, position, button):
if button == QtCore.Qt.ControlModifier:
joint.add_interpolated_setpoint(time)
else:
joint.add_setpoint(ModifiableSetpoint(time, position, 0))
self.undo_button.clicked.connect(update_joint_ui)
self.redo_button.clicked.connect(update_joint_ui)
self.invert_button.clicked.connect(update_joint_ui)
self.velocity_markers_check_box.stateChanged.connect(lambda: [
joint.set_setpoints(
user_interface_controller.plot_to_setpoints(joint_setting_plot)
),
update_joint_ui(),
])
# Connect a function to update the model and to update the table.
joint_setting_plot.plot_item.sigPlotChanged.connect(lambda: [
joint.set_setpoints(
user_interface_controller.plot_to_setpoints(joint_setting_plot)
),
update_joint_ui(),
])
joint_setting_plot.add_setpoint.connect(
lambda time, position, button: [
add_setpoint(joint, time, position, button),
update_joint_ui(),
])
joint_setting_plot.remove_setpoint.connect(lambda index: [
joint.save_setpoints(),
joint.remove_setpoint(index),
update_joint_ui(),
])
joint_setting.Table.itemChanged.connect(lambda: [
joint.set_setpoints(
user_interface_controller.table_to_setpoints(joint_setting.
Table)),
user_interface_controller.
update_ui_elements(joint,
table=None,
plot=joint_setting_plot,
duration=self.gait.duration,
show_velocity_markers=self.
velocity_markers_check_box.isChecked()),
self.publish_preview(),
])
return joint_setting
# Functions below are connected to buttons, text boxes, joint graphs etc.
def publish_preview(self):
joint_state = JointState()
joint_state.header.stamp = rospy.get_rostime()
time = self.current_time
for i in range(len(self.gait.joints)):
joint_state.name.append(self.gait.joints[i].name)
joint_state.position.append(
self.gait.joints[i].get_interpolated_position(time))
self.joint_state_pub.publish(joint_state)
self.set_feet_distances()
def toggle_velocity_markers(self):
self.velocity_markers_check_box.toggle()
def publish_gait(self):
trajectory = self.gait._to_joint_trajectory_msg()
rospy.loginfo('Publishing trajectory to topic ' + self.topic_name)
self.gait_publisher.publish(trajectory)
def set_current_time(self, current_time):
self.current_time = current_time
def set_topic_name(self, topic_name):
self.topic_name = topic_name
self.gait_publisher = rospy.Publisher(topic_name,
JointTrajectory,
queue_size=10)
def set_playback_speed(self, playback_speed):
was_playing = self.time_slider_thread is not None
self.stop_time_slider_thread()
self.playback_speed = playback_speed
rospy.loginfo('Changing playbackspeed to ' + str(self.playback_speed))
if was_playing:
self.start_time_slider_thread()
def start_time_slider_thread(self):
if self.time_slider_thread is not None:
rospy.logdebug(
'Cannot start another time slider thread as one is already active'
)
return
current = self.time_slider.value()
playback_speed = self.playback_speed
max_time = self.time_slider.maximum()
self.time_slider_thread = TimeSliderThread(current, playback_speed,
max_time)
self.time_slider_thread.update_signal.connect(
self.update_main_time_slider)
self.time_slider_thread.start()
def stop_time_slider_thread(self):
if self.time_slider_thread is not None:
self.time_slider_thread.stop()
self.time_slider_thread = None
def update_gait_duration(self, duration):
rescale_setpoints = self.scale_setpoints_check_box.isChecked()
if self.gait.has_setpoints_after_duration(
duration) and not rescale_setpoints:
if not self.gait.has_multiple_setpoints_before_duration(duration):
QMessageBox.question(
self._widget, 'Could not update gait duration',
'Not all joints have multiple setpoints before duration ' +
str(duration), QMessageBox.Ok)
return
discard_setpoints = QMessageBox.question(
self._widget, 'Gait duration lower than highest time setpoint',
'Do you want to discard any setpoints higher than the given '
'duration?', QMessageBox.Yes | QMessageBox.No)
if discard_setpoints == QMessageBox.No:
self.duration_spin_box.setValue(self.gait.duration)
return
self.gait.set_duration(duration, rescale_setpoints)
self.time_slider.setRange(0, 100 * self.gait.duration)
was_playing = self.time_slider_thread is not None
self.stop_time_slider_thread()
self.create_joint_settings()
if was_playing:
self.start_time_slider_thread()
def import_gait(self):
file_name, f = QFileDialog.getOpenFileName(
self._widget, 'Open Image',
os.getenv('HOME') + '/march_ws/src/gait-files/march_gait_files',
'March Subgait (*.subgait)')
gait = ModifiableSubgait.from_file(self, self.robot, file_name)
if gait is None:
rospy.logwarn('Could not load gait %s', file_name)
return
self.gait = gait
self.load_gait_into_ui()
self.gait_directory = '/'.join(file_name.split('/')[:-3])
rospy.loginfo('Setting gait directory to %s', str(self.gait_directory))
self.change_gait_directory_button.setText(self.gait_directory)
# Clear undo and redo history
self.joint_changed_history = RingBuffer(capacity=100, dtype=list)
self.joint_changed_redo_list = RingBuffer(capacity=100, dtype=list)
def export_gait(self):
should_mirror = self.mirror_check_box.isChecked()
key_1 = self.mirror_key1_line_edit.text()
key_2 = self.mirror_key2_line_edit.text()
if should_mirror:
mirror = self.gait.get_mirror(key_1, key_2)
if mirror:
self.export_to_file(mirror, self.get_gait_directory())
else:
user_interface_controller.notify(
'Could not mirror gait',
'Check the logs for more information.')
return
self.export_to_file(self.gait, self.get_gait_directory())
@staticmethod
def export_to_file(gait, gait_directory):
if gait_directory is None or gait_directory == '':
return
# Name and version will be empty as it's stored in the filename.
subgait_msg = gait.to_subgait_msg()
output_file_directory = os.path.join(
gait_directory, gait.gait_name.replace(' ', '_'),
gait.subgait_name.replace(' ', '_'))
output_file_path = os.path.join(
output_file_directory,
gait.version.replace(' ', '_') + '.subgait')
file_exists = os.path.isfile(output_file_path)
if file_exists:
overwrite_file = QMessageBox.question(
None, 'File already exists',
'Do you want to overwrite ' + str(output_file_path) + '?',
QMessageBox.Yes | QMessageBox.No)
if overwrite_file == QMessageBox.No:
return
rospy.loginfo('Writing gait to ' + output_file_path)
if not os.path.isdir(output_file_directory):
os.makedirs(output_file_directory)
with open(output_file_path, 'w') as output_file:
output_file.write(str(subgait_msg))
user_interface_controller.notify('Gait Saved', output_file_path)
# Called by export_gait
def get_gait_directory(self):
if self.gait_directory is None:
self.change_gait_directory()
rospy.loginfo('Selected output directory ' + str(self.gait_directory))
return self.gait_directory
def change_gait_directory(self):
self.gait_directory = str(
QFileDialog.getExistingDirectory(
None, 'Select a directory to save gaits'))
if self.gait_directory == '': # If directory dialogue is canceled
self.gait_directory = None
self.change_gait_directory_button.setText(
'Select a gait directory...')
else:
self.change_gait_directory_button.setText(self.gait_directory)
def invert_gait(self):
for joint in self.gait.joints:
joint.invert()
self.save_changed_joints(self.gait.joints)
self.publish_preview()
def undo(self):
if not self.joint_changed_history:
return
joints = self.joint_changed_history.pop()
for joint in joints:
joint.undo()
self.joint_changed_redo_list.append(joints)
self.publish_preview()
def redo(self):
if not self.joint_changed_redo_list:
return
joints = self.joint_changed_redo_list.pop()
for joint in joints:
joint.redo()
self.joint_changed_history.append(joints)
self.publish_preview()
# Called by Joint.save_setpoints. Needed for undo and redo.
def save_changed_joints(self, joints):
self.joint_changed_history.append(joints)
self.joint_changed_redo_list = RingBuffer(capacity=100, dtype=list)
# Called to update values in Heigt left foot etc.
def set_feet_distances(self):
try:
# The translation from the right foot to the left foot is the position of the
# left foot from the right foot's frame of reference.
(trans_left, rot_right) = self.tf_listener.lookupTransform(
'/foot_right', '/foot_left', rospy.Time(0))
(trans_right, rot_left) = self.tf_listener.lookupTransform(
'/foot_left', '/foot_right', rospy.Time(0))
except (LookupException, ConnectivityException,
ExtrapolationException):
return
self.height_left_line_edit.setText('%.3f' % trans_left[2])
self.height_right_line_edit.setText('%.3f' % trans_right[2])
self.heel_distance_line_edit.setText(
'%.3f' % math.sqrt(trans_left[0]**2 + trans_left[2]**2))
@QtCore.pyqtSlot(int)
def update_main_time_slider(self, time):
self.time_slider.setValue(time)
def shutdown_plugin(self):
self.stop_time_slider_thread()
def save_settings(self, plugin_settings, instance_settings):
plugin_settings.set_value('gait_directory', self.gait_directory)
def restore_settings(self, plugin_settings, instance_settings):
gait_directory = plugin_settings.value('gait_directory')
if gait_directory is not None:
rospy.loginfo('Restoring saved gait directory ' +
str(gait_directory))
self.gait_directory = gait_directory
class ModifiableJointTrajectory(JointTrajectory):
setpoint_class = ModifiableSetpoint
def __init__(self, name, limits, setpoints, duration, gait_generator=None):
self.setpoints_history = RingBuffer(capacity=100, dtype=list)
self.setpoints_redo_list = RingBuffer(capacity=100, dtype=list)
self.gait_generator = gait_generator
super(ModifiableJointTrajectory, self).__init__(name, limits, setpoints, duration)
self.interpolated_setpoints = self.interpolate_setpoints()
@classmethod
def from_dict(cls, subgait_dict, joint_name, limits, duration, gait_generator):
user_defined_setpoints = subgait_dict.get('setpoints')
if user_defined_setpoints:
joint_trajectory = subgait_dict['trajectory']
setpoints = []
for actual_setpoint in user_defined_setpoints:
if joint_name in actual_setpoint['joint_names']:
setpoints.append(cls._get_setpoint_at_duration(
joint_trajectory, joint_name, actual_setpoint['time_from_start']))
if setpoints[0].time != 0:
rospy.logwarn('First setpoint of {0} has been set '
'from {1} to 0'.format(joint_name, setpoints[0].time))
if setpoints[-1].time != duration:
rospy.logwarn('Last setpoint of {0} has been set '
'from {1} to {2}'.format(joint_name, setpoints[0].time, duration))
return cls(joint_name,
limits,
setpoints,
duration,
gait_generator,
)
rospy.logwarn('This subgait has no user defined setpoints.')
return super(ModifiableJointTrajectory, cls).from_dict(subgait_dict, joint_name, limits,
duration, gait_generator)
@staticmethod
def _get_setpoint_at_duration(joint_trajectory, joint_name, duration):
for point in joint_trajectory['points']:
if point['time_from_start'] == duration:
index = joint_trajectory['joint_names'].index(joint_name)
time = rospy.Duration(point['time_from_start']['secs'], point['time_from_start']['nsecs']).to_sec()
return ModifiableSetpoint(time, point['positions'][index], point['velocities'][index])
return None
def set_setpoints(self, setpoints):
self.setpoints = setpoints
self.enforce_limits()
self.interpolated_setpoints = self.interpolate_setpoints()
@property
def duration(self):
return self._duration
@duration.setter
def duration(self, duration):
self._duration = duration
self.enforce_limits()
def get_interpolated_position(self, time):
for i in range(0, len(self.interpolated_setpoints[0])):
if self.interpolated_setpoints[0][i] > time:
return self.interpolated_setpoints[1][i - 1]
return self.interpolated_setpoints[1][-1]
def enforce_limits(self):
self.setpoints[0].time = 0
self.setpoints[-1].time = self.duration
for i in range(0, len(self.setpoints)):
self.setpoints[i].position = min(max(self.setpoints[i].position,
self.limits.lower),
self.limits.upper)
self.setpoints[i].velocity = min(max(self.setpoints[i].velocity,
-self.limits.velocity),
self.limits.velocity)
def within_safety_limits(self):
for i in range(0, len(self.interpolated_setpoints)):
if self.interpolated_setpoints[i].position > self.limits.upper or \
self.interpolated_setpoints[i].position < self.limits.lower:
return False
if i > 0 and abs(
self.interpolated_setpoints[i] - self.interpolated_setpoints[i - 1]) > self.limits.velocity:
return False
return True
def add_interpolated_setpoint(self, time):
self.add_setpoint(self.get_interpolated_setpoint(time))
def add_setpoint(self, setpoint):
self.save_setpoints()
# Calculate at what index the new setpoint should be added.
new_index = len(self.setpoints)
for i in range(0, len(self.setpoints)):
if self.setpoints[i].time > setpoint.time:
new_index = i
break
rospy.logdebug('adding setpoint {0} at index {1}'.format(setpoint, new_index))
self.setpoints.insert(new_index, setpoint)
self.enforce_limits()
self.interpolated_setpoints = self.interpolate_setpoints()
def remove_setpoint(self, index):
self.save_setpoints()
del self.setpoints[index]
self.enforce_limits()
self.interpolated_setpoints = self.interpolate_setpoints()
def save_setpoints(self, single_joint_change=True):
self.setpoints_history.append(copy.deepcopy(self.setpoints)) # list(...) to copy instead of pointer
if single_joint_change:
self.gait_generator.save_changed_joints([self])
def invert(self):
self.save_setpoints(single_joint_change=False)
self.setpoints = list(reversed(self.setpoints))
for setpoint in self.setpoints:
setpoint.invert(self.duration)
self.interpolated_setpoints = self.interpolate_setpoints()
def undo(self):
self.setpoints_redo_list.append(list(self.setpoints))
self.setpoints = self.setpoints_history.pop()
self.interpolated_setpoints = self.interpolate_setpoints()
def redo(self):
self.setpoints_history.append(list(self.setpoints))
self.setpoints = self.setpoints_redo_list.pop()
self.interpolated_setpoints = self.interpolate_setpoints()
