Skip to content
This repository has been archived by the owner on Dec 1, 2020. It is now read-only.

Feature: lock start end setpoint #100

Merged
merged 12 commits into from
Jun 25, 2020
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@

from .model.modifiable_setpoint import ModifiableSetpoint
from .model.modifiable_subgait import ModifiableSubgait
from .side_subgait_controller import SideSubgaitController


class GaitGeneratorController(object):
Expand All @@ -21,11 +22,14 @@ def __init__(self, view, robot):

self.robot = robot
self.subgait = ModifiableSubgait.empty_subgait(self, self.robot)
self.joint_changed_history = RingBuffer(capacity=100, dtype=list)
self.joint_changed_redo_list = RingBuffer(capacity=100, dtype=list)
self._previous_subgait = None
self._next_subgait = None
self._standing = ModifiableSubgait.empty_subgait(self, self.robot)
self.settings_changed_history = RingBuffer(capacity=100, dtype=list)
self.settings_changed_redo_list = RingBuffer(capacity=100, dtype=list)

standing = ModifiableSubgait.empty_subgait(self, self.robot)
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)
Expand All @@ -38,11 +42,19 @@ def connect_buttons(self):
self.view.import_gait_button.clicked.connect(self.import_gait)
self.view.export_gait_button.clicked.connect(self.export_gait)

self.view.import_previous_subgait_button.clicked.connect(lambda: self.import_side_subgait('previous'))
self.view.lock_startpoint_check_box.stateChanged.connect(self.toggle_lock_startpoint)
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.import_next_subgait_button.clicked.connect(lambda: self.import_side_subgait('next'))
self.view.lock_endpoint_check_box.stateChanged.connect(self.toggle_lock_endpoint)
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)
Expand Down Expand Up @@ -129,7 +141,7 @@ def add_setpoint(joint, time, position, button):
joint_widget.Table.itemChanged.connect(
lambda: [
joint.save_setpoints(),
self.save_changed_joints([joint]),
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),
Expand Down Expand Up @@ -184,7 +196,7 @@ def update_gait_duration(self, duration):

for joint in self.subgait.joints:
joint.save_setpoints(single_joint_change=False)
self.save_changed_joints(self.subgait.joints)
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)
Expand Down Expand Up @@ -222,20 +234,20 @@ def import_gait(self):
self.view.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)
self.settings_changed_history = RingBuffer(capacity=100, dtype=list)
self.settings_changed_redo_list = RingBuffer(capacity=100, dtype=list)

def import_side_subgait(self, position='previous'):
def import_side_subgait(self, side='previous'):
file_name, f = self.view.open_file_dialogue()

subgait = ModifiableSubgait.from_file(self.robot, file_name, self)
if subgait is None:
rospy.logwarn('Could not load gait %s', file_name)
return

if position == 'previous':
if side == 'previous':
self.previous_subgait = subgait
elif position == 'next':
elif side == 'next':
self.next_subgait = subgait

def export_gait(self):
Expand Down Expand Up @@ -297,74 +309,125 @@ def change_gait_directory(self):
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_joints(self.subgait.joints)
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.joint_changed_history:
if not self.settings_changed_history:
return

joints = self.joint_changed_history.pop()
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)
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)

self.joint_changed_redo_list.append(joints)
self.view.update_joint_widgets(joints)
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.joint_changed_redo_list:
if not self.settings_changed_redo_list:
return

joints = self.joint_changed_redo_list.pop()
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)
changed_dict = self.settings_changed_redo_list.pop()

self.joint_changed_history.append(joints)
self.view.update_joint_widgets(joints)
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_joints(self, joints):
self.joint_changed_history.append(joints)
self.joint_changed_redo_list = RingBuffer(capacity=100, dtype=list)
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_lock_startpoint(self, value):
if value and self.previous_subgait is not None:
rospy.loginfo('Lock that start to ' + self.previous_subgait.version)
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, side_subgait_joint in zip(self.subgait.joints,
self.side_subgait_controller[side].subgait.joints):
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

def toggle_lock_endpoint(self, value):
if value and self.next_subgait is not None:
rospy.loginfo('Lock that end to ' + self.next_subgait.version)
self.view.update_joint_widgets(self.subgait.joints)
self.view.publish_preview(self.subgait, self.current_time)

@property
def previous_subgait(self):
if self.view.previous_is_standing_check_box.isChecked():
return self._standing
else:
return self._previous_subgait
return self.side_subgait_controller['previous'].subgait

@previous_subgait.setter
def previous_subgait(self, new_subgait):
self.view.import_previous_subgait_button.setText(new_subgait.version)
self._previous_subgait = 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):
if self.view.next_is_standing_check_box.isChecked():
return self._standing
else:
return self._next_subgait
return self.side_subgait_controller['next'].subgait

@next_subgait.setter
def next_subgait(self, new_subgait):
self.view.import_next_subgait_button.setText(new_subgait.version)
self._next_subgait = 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')
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

from .joint_plot import JointPlot
from .joint_table_controller import JointTableController
from .side_subgait_view import SideSubgaitView
from .time_slider_thread import TimeSliderThread


Expand All @@ -37,6 +38,10 @@ def __init__(self):

self.gait_type_combo_box.addItems(['walk_like', 'sit_like', 'stairs_like'])

previous_subgait_view = SideSubgaitView(widget=self.previous_subgait_container)
next_subgait_view = SideSubgaitView(widget=self.next_subgait_container)
self.side_subgait_view = {'previous': previous_subgait_view, 'next': next_subgait_view}

self.initialize_shortcuts()

def initialize_shortcuts(self):
Expand Down Expand Up @@ -120,13 +125,13 @@ def create_joint_plot_widget(self, joint):
return joint_plot_widget

# Methods below are called during runtime
def publish_preview(self, gait, time):
def publish_preview(self, subgait, time):
joint_state = JointState()
joint_state.header.stamp = rospy.get_rostime()

for i in range(len(gait.joints)):
joint_state.name.append(gait.joints[i].name)
joint_state.position.append(gait.joints[i].get_interpolated_position(time))
for joint in subgait.joints:
joint_state.name.append(joint.name)
joint_state.position.append(joint.get_interpolated_position(time))
self.joint_state_pub.publish(joint_state)
self.set_feet_distances()

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,9 @@ def __init__(self, name, limits, setpoints, duration, gait_generator=None):
self.setpoints_redo_list = RingBuffer(capacity=100, dtype=list)
self.gait_generator = gait_generator

self._start_point = None
self._end_point = None

super(ModifiableJointTrajectory, self).__init__(name, limits, setpoints, duration)
self.interpolated_setpoints = self.interpolate_setpoints()

Expand Down Expand Up @@ -78,6 +81,10 @@ def get_interpolated_position(self, time):
return self.interpolated_setpoints[1][-1]

def enforce_limits(self):
if self.start_point:
self.setpoints[0] = self.start_point
if self.end_point:
self.setpoints[-1] = self.end_point
self.setpoints[0].time = 0
self.setpoints[-1].time = self.duration

Expand Down Expand Up @@ -112,9 +119,10 @@ def remove_setpoint(self, index):
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
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_joints([self])
self.gait_generator.save_changed_settings({'joints': [self]})

def invert(self):
self.save_setpoints(single_joint_change=False)
Expand All @@ -127,14 +135,50 @@ def undo(self):
if not self.setpoints_history:
return

self.setpoints_redo_list.append(list(self.setpoints))
self.setpoints = self.setpoints_history.pop()
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.interpolated_setpoints = self.interpolate_setpoints()

def redo(self):
if not self.setpoints_redo_list:
return

self.setpoints_history.append(list(self.setpoints))
self.setpoints = self.setpoints_redo_list.pop()
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.interpolated_setpoints = 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
if start_point:
self._start_point.time = 0
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
if end_point:
self._end_point.time = self.duration
self.enforce_limits()
self.interpolated_setpoints = self.interpolate_setpoints()
Loading