diff --git a/dimos/manipulation/planning/planners/config.py b/dimos/manipulation/planning/planners/config.py index df7c543c7c..5f621f0606 100644 --- a/dimos/manipulation/planning/planners/config.py +++ b/dimos/manipulation/planning/planners/config.py @@ -33,7 +33,7 @@ class RoboPlanCartesianPathConfig(BaseConfig): """Runtime options for the official RoboPlan Cartesian path planner.""" backend: Literal["roboplan"] = "roboplan" - speed_mode: Literal["bounded", "time_optimal"] = "bounded" + speed_mode: Literal["bounded", "time_optimal"] = "time_optimal" dt: float = Field(default=0.01, gt=0.0) max_linear_speed: float = Field(default=0.1, gt=0.0) max_angular_speed: float = Field(default=0.5, gt=0.0) @@ -49,10 +49,8 @@ class RoboPlanCartesianPathConfig(BaseConfig): config_task_weight: float = Field(default=0.05, ge=0.0) velocity_scale: float = Field(default=1.0, gt=0.0, le=1.0) acceleration_scale: float = Field(default=1.0, gt=0.0, le=1.0) - limit_ratio_tolerance: float = Field(default=1.05, ge=1.0) - toppra_blend_deviation: float = 0.05 + toppra_blend_deviation: float = Field(default=0.05, ge=0.0) position_limit_gain: float = Field(default=1.0, gt=0.0, le=1.0) - max_attempts_per_step: int = Field(default=16, ge=1) CartesianPathConfig = RoboPlanCartesianPathConfig diff --git a/dimos/manipulation/planning/planners/test_config.py b/dimos/manipulation/planning/planners/test_config.py index 26bdcb2b1a..e5f6e5e085 100644 --- a/dimos/manipulation/planning/planners/test_config.py +++ b/dimos/manipulation/planning/planners/test_config.py @@ -20,14 +20,18 @@ from dimos.manipulation.planning.planners.config import RoboPlanCartesianPathConfig +def test_roboplan_cartesian_path_config_defaults_to_time_optimal_and_allows_bounded() -> None: + assert RoboPlanCartesianPathConfig().speed_mode == "time_optimal" + assert RoboPlanCartesianPathConfig(speed_mode="bounded").speed_mode == "bounded" + + @pytest.mark.parametrize( "path_config", [ {"dt": 0.0}, {"velocity_scale": 1.1}, {"acceleration_scale": 0.0}, - {"limit_ratio_tolerance": 0.99}, - {"max_attempts_per_step": 0}, + {"toppra_blend_deviation": -0.01}, ], ) def test_roboplan_cartesian_path_config_rejects_invalid_limits( diff --git a/dimos/manipulation/planning/world/roboplan_world.py b/dimos/manipulation/planning/world/roboplan_world.py index 3f1580c7af..d82193ff80 100644 --- a/dimos/manipulation/planning/world/roboplan_world.py +++ b/dimos/manipulation/planning/world/roboplan_world.py @@ -812,10 +812,8 @@ def _cartesian_planner_options( "config_task_weight", "velocity_scale", "acceleration_scale", - "limit_ratio_tolerance", "toppra_blend_deviation", "position_limit_gain", - "max_attempts_per_step", ): setattr(options, field_name, getattr(config, field_name)) return options diff --git a/dimos/manipulation/test_roboplan_world.py b/dimos/manipulation/test_roboplan_world.py index 57e437041f..03fa204f5f 100644 --- a/dimos/manipulation/test_roboplan_world.py +++ b/dimos/manipulation/test_roboplan_world.py @@ -99,24 +99,6 @@ def __init__(self) -> None: self.group_name = "" self.speed_mode = FakeCartesianSpeedMode.TimeOptimal self.dt = 0.01 - self.max_linear_speed = 0.1 - self.max_angular_speed = 0.5 - self.max_linear_acceleration = 0.5 - self.max_angular_acceleration = 2.5 - self.max_position_error = 0.005 - self.max_orientation_error = 0.01 - self.position_cost = 1.0 - self.orientation_cost = 1.0 - self.task_gain = 1.0 - self.lm_damping = 0.01 - self.regularization = 1e-6 - self.config_task_weight = 0.05 - self.velocity_scale = 1.0 - self.acceleration_scale = 1.0 - self.limit_ratio_tolerance = 1.05 - self.toppra_blend_deviation = 0.05 - self.position_limit_gain = 1.0 - self.max_attempts_per_step = 16 class FakeCartesianPathPlanner: @@ -1446,32 +1428,11 @@ def test_native_planner_coordinates_groups_across_two_robots( assert result.path[-1].position == [0.1, 0.3, 0.4, 0.2] -def test_cartesian_planner_returns_timed_global_joint_states_and_options( +def test_cartesian_planner_returns_timed_global_joint_states( fake_roboplan: None, robot_config: RobotModelConfig ) -> None: world, _ = _make_world(fake_roboplan, robot_config) selection = _selection((robot_config,), "arm/manipulator") - option_overrides = { - "dt": 0.02, - "max_linear_speed": 0.2, - "max_angular_speed": 0.6, - "max_linear_acceleration": 0.7, - "max_angular_acceleration": 2.0, - "max_position_error": 0.006, - "max_orientation_error": 0.02, - "position_cost": 2.0, - "orientation_cost": 3.0, - "task_gain": 0.8, - "lm_damping": 0.02, - "regularization": 2e-6, - "config_task_weight": 0.1, - "velocity_scale": 0.9, - "acceleration_scale": 0.8, - "limit_ratio_tolerance": 1.02, - "toppra_blend_deviation": 0.0, - "position_limit_gain": 0.7, - "max_attempts_per_step": 8, - } result = world.plan_cartesian_path( world, @@ -1485,10 +1446,7 @@ def test_cartesian_planner_returns_timed_global_joint_states_and_options( ) ) }, - RoboPlanCartesianPathConfig( - speed_mode="time_optimal", - **option_overrides, - ), + RoboPlanCartesianPathConfig(speed_mode="time_optimal", dt=0.02), ) assert result.status == PlanningStatus.SUCCESS @@ -1498,8 +1456,6 @@ def test_cartesian_planner_returns_timed_global_joint_states_and_options( assert result.path[1].velocity == pytest.approx([0.5, 0.5]) planner = FakeCartesianPathPlanner.instances[-1] assert planner.options.speed_mode == FakeCartesianSpeedMode.TimeOptimal - for field_name, expected in option_overrides.items(): - assert getattr(planner.options, field_name) == pytest.approx(expected) assert planner.paths[0].base_frames == ["dimos_world"] np.testing.assert_allclose(planner.paths[0].tforms[0][0], np.eye(4), atol=1e-12) expected_rotation = np.array( diff --git a/dimos/manipulation/visualization/viser/test_viser_visualization.py b/dimos/manipulation/visualization/viser/test_viser_visualization.py index 8e1428214f..f0acfc9190 100644 --- a/dimos/manipulation/visualization/viser/test_viser_visualization.py +++ b/dimos/manipulation/visualization/viser/test_viser_visualization.py @@ -617,7 +617,7 @@ def test_cartesian_space_mode_plans_absolute_pose_targets_with_auxiliary_groups( targets, config, auxiliary_ids = module.cartesian_plans[0] assert tuple(targets) == (pose_group.id,) assert targets[pose_group.id].frame_id == "world" - assert config.speed_mode == "bounded" # type: ignore[attr-defined] + assert config.speed_mode == "time_optimal" # type: ignore[attr-defined] assert auxiliary_ids == (auxiliary_group.id,) assert gui.state.plan_state.status == PlanStatus.FRESH assert gui.state.last_result == "plan_cartesian_space=True" diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index fe4d8460db..3a96608044 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -123,24 +123,43 @@ from dimos.manipulation.planning.planners.config import ( RoboPlanCartesianPathConfig, ) -path_config = RoboPlanCartesianPathConfig( +path_config = RoboPlanCartesianPathConfig() + +module.plan_cartesian_targets( + {"arm/manipulator": (current_tcp_pose, goal_tcp_pose)}, + path_config, +) +``` + +The default `time_optimal` mode returns the TOPP-RA trajectory constrained by +the robot's joint velocity and acceleration limits. To enforce Cartesian speed +and acceleration maxima instead, opt into bounded mode: + +```python skip +bounded_config = RoboPlanCartesianPathConfig( speed_mode="bounded", max_linear_speed=0.1, max_angular_speed=0.5, + max_linear_acceleration=0.5, + max_angular_acceleration=2.5, max_position_error=0.005, max_orientation_error=0.01, ) - -module.plan_cartesian_targets( - {"arm/manipulator": (current_tcp_pose, goal_tcp_pose)}, - path_config, -) ``` -The remaining settings mirror RoboPlan's standard Cartesian planner options, -including bounded and time-optimal speed modes, sample time, solver weights, -linear/angular acceleration limits, joint velocity/acceleration scaling, -TOPP-RA corner blending, joint-limit handling, and per-step attempts. +RoboPlan first resolves the Cartesian reference as a geometric joint path, then +uses TOPP-RA to produce the timed trajectory. Both speed modes follow this +pipeline. Time-optimal mode returns the joint-limit-constrained trajectory; +bounded mode slows it further when needed to respect the configured Cartesian +speed and acceleration maxima. `toppra_blend_deviation` controls TOPP-RA corner +rounding in both modes and influences how aggressively the resolved path is +decimated before timing. + +The remaining settings mirror RoboPlan's Cartesian planner options, including +sample time, solver weights, linear/angular acceleration limits, joint +velocity/acceleration scaling, TOPP-RA corner blending, and joint-limit +handling. RoboPlan 0.6 removed the former `limit_ratio_tolerance` and +`max_attempts_per_step` settings. Cartesian path planning remains a low-level internal capability in this release. `ManipulationModule.plan_cartesian_targets()` accepts an ordered diff --git a/pyproject.toml b/pyproject.toml index 37bc540ae5..392d9b8969 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -292,7 +292,7 @@ manipulation = [ # Other "matplotlib>=3.7.1", "pyyaml>=6.0", - "roboplan>=0.5.1", + "roboplan>=0.6.0,<0.7.0", ] cpu = [ @@ -457,7 +457,7 @@ lint = [ "pytest==8.3.5", "python-can>=4", "python-socketio>=5.16.1", - "roboplan>=0.5.1", + "roboplan>=0.6.0,<0.7.0", "sounddevice>=0.5.5", "trimesh>=4.12", "watchdog>=3.0.0", diff --git a/uv.lock b/uv.lock index 2292f5837e..80c297bb68 100644 --- a/uv.lock +++ b/uv.lock @@ -2120,7 +2120,7 @@ requires-dist = [ { name = "reportlab", marker = "extra == 'apriltag'", specifier = ">=4.5.0" }, { name = "rerun-sdk", specifier = "==0.32.0" }, { name = "rerun-sdk", marker = "extra == 'visualization'", specifier = "==0.32.0" }, - { name = "roboplan", marker = "extra == 'manipulation'", specifier = ">=0.5.1" }, + { name = "roboplan", marker = "extra == 'manipulation'", specifier = ">=0.6.0,<0.7.0" }, { name = "scipy", specifier = ">=1.15.1" }, { name = "sortedcontainers", specifier = "==2.4.0" }, { name = "sounddevice", marker = "extra == 'agents'" }, @@ -2182,7 +2182,7 @@ lint = [ { name = "pytest", specifier = "==8.3.5" }, { name = "python-can", specifier = ">=4" }, { name = "python-socketio", specifier = ">=5.16.1" }, - { name = "roboplan", specifier = ">=0.5.1" }, + { name = "roboplan", specifier = ">=0.6.0,<0.7.0" }, { name = "ruff", specifier = "==0.14.3" }, { name = "sounddevice", specifier = ">=0.5.5" }, { name = "tensorboard", specifier = "==2.20.0" }, @@ -7916,7 +7916,7 @@ wheels = [ [[package]] name = "roboplan" -version = "0.5.1" +version = "0.6.0" source = { registry = "https://pypi.org/simple" } dependencies = [ { name = "matplotlib" }, @@ -7924,17 +7924,17 @@ dependencies = [ { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, { name = "pin" }, ] -sdist = { url = "https://files.pythonhosted.org/packages/85/d5/7e076c2c5542531fd5588597402c0e7cac4fb59c065467ad61d2672b8d00/roboplan-0.5.1.tar.gz", hash = "sha256:0f97c47c1591203aa0cce3ed55a01630af5189a9d251e20c9ed02b701f926df4", size = 48443474, upload-time = "2026-07-14T01:57:56.474Z" } -wheels = [ - { url = "https://files.pythonhosted.org/packages/5b/d3/cb7a545e4db66c2195df3c571be27715e2ccd86c6f4254703456d9d6be02/roboplan-0.5.1-cp310-cp310-macosx_14_0_arm64.whl", hash = "sha256:11eacaed5303ca8e218e932d812afe28afdd63fe76a6287ebeccd16816b9552f", size = 42214675, upload-time = "2026-07-14T01:57:06.328Z" }, - { url = "https://files.pythonhosted.org/packages/b8/46/22a621e5d8819b79123a4c57c9f4fed3866153291d00bc81ed80bfa6ee40/roboplan-0.5.1-cp310-cp310-manylinux_2_28_aarch64.whl", hash = "sha256:bba803166614db146faafabe27d974c1aa25f9c8a4688a6c8dc7aec42babe49f", size = 48042484, upload-time = "2026-07-14T01:57:10.324Z" }, - { url = "https://files.pythonhosted.org/packages/3c/90/38c22233253b9a736af4496d4bd86656bf6c97c012e35dbe4aa85292783c/roboplan-0.5.1-cp310-cp310-manylinux_2_28_x86_64.whl", hash = "sha256:c84d60517d5ed580ba9e196c9db3118d4a999da4648223aa54c394fc3b67bb9a", size = 49314934, upload-time = "2026-07-14T01:57:13.624Z" }, - { url = "https://files.pythonhosted.org/packages/20/22/60ec9d5e54e7a07235908ceb5555a3d2769568a40aca4b8b6952020e993e/roboplan-0.5.1-cp311-cp311-macosx_14_0_arm64.whl", hash = "sha256:3ae6499fdecd5eafd5ff6138b03420f72c473b872e3a118b2b853299c5a88dff", size = 42213177, upload-time = "2026-07-14T01:57:17.028Z" }, - { url = "https://files.pythonhosted.org/packages/e1/dc/2c3c12d4ef6929c9bd54be69c5a5e59303f3209f641f5ebcd88434153a30/roboplan-0.5.1-cp311-cp311-manylinux_2_28_aarch64.whl", hash = "sha256:426eaa5e09d327bda682a4fe1584d8a71dd16d0fd1350dba82daa79c4642c132", size = 48040417, upload-time = "2026-07-14T01:57:20.257Z" }, - { url = "https://files.pythonhosted.org/packages/0c/93/47ddda44eb94a3825a27b8400a81b40b4783235af3dfbcde45cce0f69a13/roboplan-0.5.1-cp311-cp311-manylinux_2_28_x86_64.whl", hash = "sha256:bcf5e29a5e702e2d081c2dbf64d795da52716ca1f98410d3181c9b1554163cac", size = 49313130, upload-time = "2026-07-14T01:57:23.471Z" }, - { url = "https://files.pythonhosted.org/packages/7a/be/360480e5603fa3c13d9620e8285dceb3f695d864f22abb4ad88c43314539/roboplan-0.5.1-cp312-cp312-macosx_14_0_arm64.whl", hash = "sha256:40f83635bfff5683ea1a4ae07cd679a16604bb27cd9f14278513cf3fc2a24d35", size = 42193118, upload-time = "2026-07-14T01:57:26.623Z" }, - { url = "https://files.pythonhosted.org/packages/11/f2/152dc1ddf61ced2aae9b776a32a3403b087af9a28ee10abd874577dfa399/roboplan-0.5.1-cp312-cp312-manylinux_2_28_aarch64.whl", hash = "sha256:5aa9191539f5a934128ca70789717c6d3577eff268ba46ad04969cc8e1842082", size = 48009272, upload-time = "2026-07-14T01:57:29.685Z" }, - { url = "https://files.pythonhosted.org/packages/d1/c1/320702ad1e0671165f3360becad54bfad361af39bf0b9d22274e5f5dbeac/roboplan-0.5.1-cp312-cp312-manylinux_2_28_x86_64.whl", hash = "sha256:e5d6b602d7f1e4995aa1c19324c99b70c64227396f8264d5d548144e0ab0d6bf", size = 49278422, upload-time = "2026-07-14T01:57:32.709Z" }, +sdist = { url = "https://files.pythonhosted.org/packages/9d/cc/f72c665a9a8b5a89a28247a5e1c08f9df8de60066bb2843238307367158b/roboplan-0.6.0.tar.gz", hash = "sha256:ffb211144578c2f8932835fdd12e4bb3bee5b37b91b51b66353fb469ff9215da", size = 48506567, upload-time = "2026-08-01T01:50:10.996Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/05/d1/9e0bde3ec4094aee57f21a6a9fe5ce7afb48225aa1ba62252bdd0e508c48/roboplan-0.6.0-cp310-cp310-macosx_14_0_arm64.whl", hash = "sha256:0fad653a3bebca538a4a582d2876178e008ee49e18444448256523d0fbadf0c8", size = 42513583, upload-time = "2026-08-01T01:49:22.614Z" }, + { url = "https://files.pythonhosted.org/packages/8b/1c/b49be3f7b08893461cbd86d4e8cd3a28227993b61fcecfe99963e545beb8/roboplan-0.6.0-cp310-cp310-manylinux_2_28_aarch64.whl", hash = "sha256:b45200497ec2df986312a1d3e6696d33c51001e39311d40585ccd9864d0daa3b", size = 48371184, upload-time = "2026-08-01T01:49:26.084Z" }, + { url = "https://files.pythonhosted.org/packages/a3/ee/d7d55d32d62252438118df2e6679bcef4824ed077b4d44a12a51714eb92c/roboplan-0.6.0-cp310-cp310-manylinux_2_28_x86_64.whl", hash = "sha256:a5198806a94bfb2b87caafb201e0ee1c32db6f93a56ad2c0fa798fab15d04823", size = 49662268, upload-time = "2026-08-01T01:49:29.491Z" }, + { url = "https://files.pythonhosted.org/packages/78/b1/7f2b0cf3e6f3698a1befbfc0338959a2d0499f9949fef898c096f17aec93/roboplan-0.6.0-cp311-cp311-macosx_14_0_arm64.whl", hash = "sha256:edfc99d70ae841da24665cd88e275bb2411cd1a031c37a17a63e8e880b11f219", size = 42511972, upload-time = "2026-08-01T01:49:32.692Z" }, + { url = "https://files.pythonhosted.org/packages/ec/52/83cba2e928b4388590d876707177d8de6795f3b29ff28a5b5031ec8c9841/roboplan-0.6.0-cp311-cp311-manylinux_2_28_aarch64.whl", hash = "sha256:0754702f03a2da3f86482f637d38c1d661d0712dfac15675ab7529948451b0fb", size = 48369305, upload-time = "2026-08-01T01:49:35.906Z" }, + { url = "https://files.pythonhosted.org/packages/a8/c8/bb8ffa70aafe0131a06af422394dbd9b2134e9b0815858d1063e273aaf0e/roboplan-0.6.0-cp311-cp311-manylinux_2_28_x86_64.whl", hash = "sha256:843c50811189802a27817ead29758aa73d02ba026098cc73bd5b2d5277d6110d", size = 49660420, upload-time = "2026-08-01T01:49:38.862Z" }, + { url = "https://files.pythonhosted.org/packages/fd/8a/415502b840279720cc51f0930472352d73a732b424533411d2602a9c3704/roboplan-0.6.0-cp312-cp312-macosx_14_0_arm64.whl", hash = "sha256:78ec01856c63d57cec0fb5413864cb7880a728370cabc82d3ce872e5cda630d6", size = 42492679, upload-time = "2026-08-01T01:49:42.147Z" }, + { url = "https://files.pythonhosted.org/packages/e7/b2/fc6adcd966859090de41cefb37e9894c2755b23acfacf3a4d4ab8b3df25d/roboplan-0.6.0-cp312-cp312-manylinux_2_28_aarch64.whl", hash = "sha256:dd63a857de2773f94254f6d008f181ac9dcd47083b7f60a7c79905841c2043b1", size = 48337983, upload-time = "2026-08-01T01:49:45.508Z" }, + { url = "https://files.pythonhosted.org/packages/60/78/2a00f8404cb13c7ea074a9f7d895ae7aed0954d9aa623295368d2f3d24d1/roboplan-0.6.0-cp312-cp312-manylinux_2_28_x86_64.whl", hash = "sha256:d09d21d08b85a7245aea74658f158d315c8c7f6830654e12c9f1bfda189b97fd", size = 49625614, upload-time = "2026-08-01T01:49:48.747Z" }, ] [[package]]