Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Custom trajectory starts causing segfault in drake==1.37.0 #22621

Open
nepfaff opened this issue Feb 13, 2025 · 3 comments
Open

Custom trajectory starts causing segfault in drake==1.37.0 #22621

nepfaff opened this issue Feb 13, 2025 · 3 comments
Assignees
Labels
component: pydrake Python API and its supporting Starlark macros type: bug

Comments

@nepfaff
Copy link
Member

nepfaff commented Feb 13, 2025

What happened?

My custom trajectories started to cause a segfault when updating to drake==1.37.0.

It works with drake==1.36.0. I tried replacing the depreciated methods (e.g. Copy) but this didn't help, so I left them in for consistency. The segfault seems to happen after querying the trajectory the first time.

Minimal example:

import numpy as np

from manipulation.station import LoadScenario
from pydrake.all import DiagramBuilder, Simulator, TrajectorySource


from robot_payload_id.utils import Trajectory
from manipulation.station import LoadScenario, MakeHardwareStation


class TestTraj(Trajectory):

    def __init__(self):
        super().__init__()

    def rows(self) -> int:
        return 7

    def cols(self) -> int:
        return 1

    def start_time(self) -> float:
        return 0.0

    def end_time(self) -> float:
        return 1.0

    def value(self, time: float) -> np.ndarray:
        return np.zeros((7,))

    def do_has_derivative(self):
        return False

    def Clone(self) -> "TestTraj":
        return TestTraj()


def main():
    builder = DiagramBuilder()

    scenario_str = """
    simulation_duration: 1.0

    directives:
    - add_model:
        name: iiwa
        file: package://drake_models/iiwa_description/sdf/iiwa14_polytope_collision.sdf
    - add_weld:
        parent: world
        child: iiwa::iiwa_link_0

    model_drivers:
        iiwa: !IiwaDriver
            control_mode: position_only
    """
    scenario = LoadScenario(data=scenario_str)
    station = builder.AddSystem(MakeHardwareStation(scenario))

    traj_source = builder.AddSystem(TrajectorySource(trajectory=TestTraj()))
    builder.Connect(
        traj_source.get_output_port(), station.GetInputPort("iiwa.position")
    )

    diagram = builder.Build()

    simulator = Simulator(diagram)
    simulator.AdvanceTo(1.0)


if __name__ == "__main__":
    main()

This unfortunately depends on manipulation but I couldn't think of a quick way to set this up with drake only. I'd assume any dummy system + trajectory source would cause the same issue. The only thing that changed this from working to not working is changing drake version, so it shouldn't be the manipulation dependency.

Version

1.37.0

What operating system are you using?

Ubuntu 22.04

What installation option are you using?

pip install drake

Relevant log output

@nepfaff nepfaff changed the title Custom Trajectory starts causing segfault in drake==1.37.0 Custom trajectory starts causing segfault in drake==1.37.0 Feb 13, 2025
@jwnimmer-tri
Copy link
Collaborator

jwnimmer-tri commented Feb 13, 2025

See the https://drake.mit.edu/release_notes/v1.37.0.html for #22395:

[Trajectory] Subclasses are no longer allowed to override public virtual methods. They must now override the NVI (“non-virtual interface”) protected methods, e.g., do_cols() instead of cols().

I had thought that we got it smooth enough to be a deprecation transition, but if it's segfaulting maybe something got missed. In any case, the probable fix is the same -- switch to using the non-deprecated overrides:

# Custom trajectory class used to test Trajectory subclassing in Python.
class CustomTrajectory(Trajectory):
def __init__(self):
Trajectory.__init__(self)
def __deepcopy__(self, memo):
return CustomTrajectory()
def __repr__(self):
return "CustomTrajectory()"
def do_value(self, t):
return np.array([[t + 1.0, t + 2.0]])
def do_has_derivative(self):
return True
def DoEvalDerivative(self, t, derivative_order):
if derivative_order >= 2:
return np.zeros((1, 2))
elif derivative_order == 1:
return np.ones((1, 2))
elif derivative_order == 0:
return self.value(t)
def DoMakeDerivative(self, derivative_order):
return DerivativeTrajectory_[float](self, derivative_order)
def do_rows(self):
return 1
def do_cols(self):
return 2
def do_start_time(self):
return 3.0
def do_end_time(self):
return 4.0

@jwnimmer-tri jwnimmer-tri self-assigned this Feb 13, 2025
@jwnimmer-tri jwnimmer-tri added the component: pydrake Python API and its supporting Starlark macros label Feb 13, 2025
@nepfaff
Copy link
Member Author

nepfaff commented Feb 13, 2025

That's it! It fails if I change do_value to value. I missed this as it segfaults before the depreciation warnings are displayed. The release notes don't mention this particular method and no depreciation warnings are displayed which makes this very hard to catch.

I had tried switching cols, rows, and Copy as warnings were displayed for these but these didn't make a difference.

@jwnimmer-tri
Copy link
Collaborator

Yeah. I will still try to see if we can avoid the segfault (which would then hopefully show the message).

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
component: pydrake Python API and its supporting Starlark macros type: bug
Projects
None yet
Development

No branches or pull requests

2 participants