diff --git a/spot_choreo_utils/spot_choreo_utils/choreo_creation/choreo_builders/animation_builder.py b/spot_choreo_utils/spot_choreo_utils/choreo_creation/choreo_builders/animation_builder.py index edd8c60..48b5cb1 100644 --- a/spot_choreo_utils/spot_choreo_utils/choreo_creation/choreo_builders/animation_builder.py +++ b/spot_choreo_utils/spot_choreo_utils/choreo_creation/choreo_builders/animation_builder.py @@ -51,17 +51,17 @@ class BuildSettings: apply_stance_to_all_keyframes: bool = True def __init__(self, logger: Optional[Logger] = None) -> None: - if logger is None: - logger = Logger("animation_builder") - self._animation = Animation() - self._logger: Logger = logger + self._logger: Optional[Logger] = logger + + def _log_if_enabled(self, message: str) -> None: + """Logs if logger is available""" + if self._logger is not None: + self._logger.error(message) @classmethod def from_animation(cls, animation: Animation, logger: Optional[Logger] = None) -> "AnimationBuilder": """Transform animation into animation builder""" - if logger is None: - logger = Logger("animation_builder_logger") animation_builder = AnimationBuilder(logger) animation_builder.start_from_animation(animation) return animation_builder @@ -73,7 +73,7 @@ def start_from_empty(self, name: str) -> None: def start_from_animation(self, animation_proto: Animation) -> None: """Modify an existing animation with builder helper functions""" if animation_proto is None: - self._logger.error("Passed None - can't start from animation") + self._log_if_enabled("Passed None - can't start from animation") return self._animation = copy.deepcopy(animation_proto) # Get rid of BPM paramater - it just creates confusion between the @@ -87,7 +87,7 @@ def name(self) -> str: return self._animation.name @property - def logger(self) -> Logger: + def logger(self) -> Optional[Logger]: """Returns the logger associated with the animation builder""" return self._logger @@ -99,7 +99,9 @@ def keyframe_count(self) -> int: @property def keyframe_timestamps(self) -> List[float]: """Return a list of all keyframe timestamps""" - return [keyframe.time for keyframe in self._animation.animation_keyframes] + if not hasattr(self, "_keyframe_timestamps"): + self._keyframe_timestamps = [keyframe.time for keyframe in self._animation.animation_keyframes] + return self._keyframe_timestamps @property def animation_length_s(self) -> float: @@ -118,10 +120,7 @@ def keyframe_at_index(self, keyframe_idx: int) -> Optional[AnimationKeyframe]: keyframe_count = self.keyframe_count if keyframe_idx < 0 or keyframe_idx >= keyframe_count: - if self._logger: - self._logger.error( - f"Requested keyframe at index {keyframe_idx} but animation length is {keyframe_count}" - ) + self._log_if_enabled(f"Requested keyframe at index {keyframe_idx} but animation length is {keyframe_count}") return None return self._animation.animation_keyframes[keyframe_idx] @@ -131,7 +130,7 @@ def keyframe_range(self, start_idx: int, end_idx: int) -> list[AnimationKeyframe Returns a list of keyframes that fall in the start/end range """ if start_idx > end_idx: - self._logger.error(f"Start index must be lower than end index: {start_idx} > {end_idx}") + self._log_if_enabled(f"Start index must be lower than end index: {start_idx} > {end_idx}") return [] range_start = max(start_idx, 0) range_end = min(end_idx, self.keyframe_count) @@ -146,7 +145,7 @@ def time_elapsed_between_keyframes(self, first_index: int, second_index: int) -> second_keyframe = self.keyframe_at_index(second_index) if first_keyframe is None or second_keyframe is None or (first_index > second_index): - self._logger.error( + self._log_if_enabled( f"Can't get time between keyframes when one doesn't exist. First index {first_index}, second index" f" {second_index}" ) @@ -192,7 +191,7 @@ def build(self, build_settings: BuildSettings = BuildSettings()) -> Optional[Ani if build_settings.only_output_valid: res, msg = self.validate() if not res: - self._logger.error(f"Failed to build animation: {msg}") + self._log_if_enabled(f"Failed to build animation: {msg}") return None # Create new copy for procedural edits that would conflict with @@ -307,11 +306,11 @@ def insert_keyframe_at_time( (insertion_idx, prior_keyframe) = self.get_keyframe_before_timestamp(start_time) if prior_keyframe is not None and prior_keyframe.time == keyframe.time: - self._logger.error(f"Can't insert keyframe at time {keyframe.time}, already exists. Overwrite instead") + self._log_if_enabled(f"Can't insert keyframe at time {keyframe.time}, already exists. Overwrite instead") return None if insertion_idx == 0 and self.keyframe_count > 0 and keyframe.time == 0: - self._logger.error("Can't have two keyframes at timestamp 0") + self._log_if_enabled("Can't have two keyframes at timestamp 0") return None self.insert_keyframe_at_index( @@ -431,7 +430,7 @@ def remove_keyframe_at_idx(self, keyframe_idx: int, adjust_trailing_keyframes: b num_keyframes = self.keyframe_count if num_keyframes == 0 or keyframe_idx >= num_keyframes: - self._logger.error( + self._log_if_enabled( f"Invalid keyframe index. Can't remove keyframe {keyframe_idx} from animation length {num_keyframes}" ) return @@ -439,16 +438,16 @@ def remove_keyframe_at_idx(self, keyframe_idx: int, adjust_trailing_keyframes: b # Remove the time between this keyframe and the next keyframe if adjust_trailing_keyframes and keyframe_idx < self.keyframe_count - 1: if not self.keyframe_at_index(keyframe_idx): - self._logger.error(f"Error accessing keyframe at index {str(keyframe_idx)}") + self._log_if_enabled(f"Error accessing keyframe at index {str(keyframe_idx)}") return if not self.keyframe_at_index(keyframe_idx + 1): - self._logger.error(f"Error accessing keyframe at index {str(keyframe_idx + 1)}") + self._log_if_enabled(f"Error accessing keyframe at index {str(keyframe_idx + 1)}") return time_between_keyframes = self.time_elapsed_between_keyframes(keyframe_idx, keyframe_idx + 1) for index_to_modify in range(keyframe_idx + 1, self.keyframe_count): modify_keyframe = self.keyframe_at_index(index_to_modify) if not modify_keyframe: - self._logger.error(f"Error accessing keyframe at index {str(index_to_modify)}") + self._log_if_enabled(f"Error accessing keyframe at index {str(index_to_modify)}") return modify_keyframe.time -= time_between_keyframes @@ -459,7 +458,7 @@ def change_keyframe_time_at_index(self, keyframe_index: int, new_start_time: flo """Adjust the start time for a keyframe""" keyframe = self.keyframe_at_index(keyframe_index) if keyframe is None: - self._logger.error(f"No keyframe at index {keyframe_index}") + self._log_if_enabled(f"No keyframe at index {keyframe_index}") return keyframe.time = new_start_time @@ -472,7 +471,7 @@ def animate_gripper(self, start_time: float, gripper_angle: float) -> None: gripper_angle = ensure_protobuf_compliance(gripper_angle, adjust_positive=False) if gripper_angle < -1 or gripper_angle > 0: - self._logger.error("INVALID GRIPPER ANGLE - MUST BE IN RANGE -1 to 0") + self._log_if_enabled("INVALID GRIPPER ANGLE - MUST BE IN RANGE -1 to 0") return self.create_keyframe_from_params(start_time=start_time, gripper=build_gripper_params(gripper_angle)) diff --git a/spot_choreo_utils/spot_choreo_utils/choreo_creation/choreo_builders/animation_operators.py b/spot_choreo_utils/spot_choreo_utils/choreo_creation/choreo_builders/animation_operators.py index 2fbb421..8fb7bd5 100644 --- a/spot_choreo_utils/spot_choreo_utils/choreo_creation/choreo_builders/animation_operators.py +++ b/spot_choreo_utils/spot_choreo_utils/choreo_creation/choreo_builders/animation_operators.py @@ -10,7 +10,10 @@ from spot_choreo_utils.choreo_creation.choreo_builders.animation_builder import ( AnimationBuilder, ) -from spot_choreo_utils.choreo_creation.choreo_builders.animation_proto_utils import check_if_protobuf_field_set +from spot_choreo_utils.choreo_creation.choreo_builders.animation_proto_utils import ( + check_if_protobuf_field_set, + ensure_protobuf_compliance, +) from spot_choreo_utils.choreo_creation.choreo_builders.sequence_builder import SequenceBuilder @@ -221,6 +224,7 @@ def perform_keyframe_interpolation( if logger: logger.error(f"Unknown type {type(stop_ref)} for interpolation") raise AssertionError() + interpolated_value = ensure_protobuf_compliance(interpolated_value) setattr(modified, property, interpolated_value) return is_protobuf @@ -237,8 +241,10 @@ def extract_pose_for_animation_time( if animation is None: return None - builder = AnimationBuilder.from_animation(animation) - builder.get_keyframe_before_timestamp(time_offset_in_animation) + if isinstance(animation, AnimationBuilder): + builder = animation + else: + builder = AnimationBuilder.from_animation(animation) (prev_keyframe_idx, prev_keyframe) = builder.get_keyframe_before_timestamp(time_offset_in_animation) if prev_keyframe_idx + 1 > builder.keyframe_count: diff --git a/spot_choreo_utils/spot_choreo_utils/choreo_creation/choreo_builders/animation_proto_utils.py b/spot_choreo_utils/spot_choreo_utils/choreo_creation/choreo_builders/animation_proto_utils.py index a6396ce..6f36e77 100644 --- a/spot_choreo_utils/spot_choreo_utils/choreo_creation/choreo_builders/animation_proto_utils.py +++ b/spot_choreo_utils/spot_choreo_utils/choreo_creation/choreo_builders/animation_proto_utils.py @@ -220,13 +220,13 @@ def build_leg_params( rear_left_angles = None rear_right_angles = None - if front_left_hip_x and front_left_hip_y and front_left_knee: + if front_left_hip_x is not None and front_left_hip_y is not None and front_left_knee is not None: front_left_angles = LegJointAngles(hip_x=front_left_hip_x, hip_y=front_left_hip_y, knee=front_left_knee) - if front_right_hip_x and front_right_hip_y and front_left_knee: - front_right_angles = LegJointAngles(hip_x=front_right_hip_x, hip_y=front_right_hip_y, knee=front_left_knee) - if rear_left_hip_x and rear_left_hip_y and rear_left_knee: + if front_right_hip_x is not None and front_right_hip_y is not None and front_right_knee is not None: + front_right_angles = LegJointAngles(hip_x=front_right_hip_x, hip_y=front_right_hip_y, knee=front_right_knee) + if rear_left_hip_x is not None and rear_left_hip_y is not None and rear_left_knee is not None: rear_left_angles = LegJointAngles(hip_x=rear_left_hip_x, hip_y=rear_left_hip_y, knee=rear_left_knee) - if rear_right_hip_x and rear_right_hip_y and rear_right_knee: + if rear_right_hip_x is not None and rear_right_hip_y is not None and rear_right_knee is not None: rear_right_angles = LegJointAngles(hip_x=rear_right_hip_x, hip_y=rear_right_hip_y, knee=rear_right_knee) front_left_leg = None @@ -234,27 +234,27 @@ def build_leg_params( rear_left_leg = None rear_right_leg = None - if front_left_angles: + if front_left_angles is not None: front_left_leg = AnimateSingleLeg() front_left_leg.joint_angles.CopyFrom(front_left_angles) - if front_right_angles: + if front_right_angles is not None: front_right_leg = AnimateSingleLeg() front_right_leg.joint_angles.CopyFrom(front_right_angles) - if rear_left_angles: + if rear_left_angles is not None: rear_left_leg = AnimateSingleLeg() rear_left_leg.joint_angles.CopyFrom(rear_left_angles) - if rear_right_angles: + if rear_right_angles is not None: rear_right_leg = AnimateSingleLeg() rear_right_leg.joint_angles.CopyFrom(rear_right_angles) animate_legs = AnimateLegs() - if front_left_leg: + if front_left_leg is not None: animate_legs.fl.CopyFrom(front_left_leg) - if front_right_leg: + if front_right_leg is not None: animate_legs.fr.CopyFrom(front_right_leg) - if rear_left_leg: + if rear_left_leg is not None: animate_legs.hl.CopyFrom(rear_left_leg) - if rear_right_leg: + if rear_right_leg is not None: animate_legs.hr.CopyFrom(rear_right_leg) return animate_legs