Hri benchmark nlp - #1024
Open
biweep863 wants to merge 68 commits into
Open
Conversation
biweep863
marked this pull request as ready for review
June 2, 2026 03:42
GilMM27
requested changes
Jun 2, 2026
GilMM27
left a comment
GilMM27
left a comment
Contributor
There was a problem hiding this comment.
we just want to compare some models, this benchmark looks really complete but almost out of place.
Contributor
There was a problem hiding this comment.
this should use the tasks that we already created. If not, changes in one place will have to be replicated here
Contributor
Author
There was a problem hiding this comment.
but this will require integration for running benchmarks? a model could last more than 10 minute to download, i was thinkg more of this approach to test new models in bulk, so the logs could be saved and in the orinserver you could leave the benchmark running, and then return after an hour with results. so it was more "automatically" than you have to download models, then start running the test cases in integration and comparing. But if this tests are the "correct" way of doing it, let me change it, but i think running benchmarks will be more tedious.
Contributor
There was a problem hiding this comment.
you could always just run integration on the orinserver too and yes, I think recycling the code is the better approach while maintaining this ease of running
Contributor
There was a problem hiding this comment.
I'm not a fan of having benchmark related files inside the NLP ROS package considering that it isn't related programmatically
Contributor
Author
There was a problem hiding this comment.
so where shoudl i put them? in docker/hri or where should we put the benchmarks?
Comment on lines
+19
to
+29
| i=0 | ||
| while [[ $i -lt ${#ARGS[@]} ]]; do | ||
| arg="${ARGS[$i]}" | ||
| case "$arg" in | ||
| "--download-model") DOWNLOAD_MODEL="true" ;; | ||
| "--regenerate-db") REGENERATE_DB="true" ;; | ||
| "--build-proto") BUILD_PROTO="true" ;; | ||
| "--benchmark") BENCHMARK="true" ;; | ||
| "--benchmark-delete") BENCHMARK="true"; BENCHMARK_PASSTHROUGH+=("--delete") ;; | ||
| esac | ||
| (( i++ )) || true |
Contributor
There was a problem hiding this comment.
I'd rather we avoided bash magic and kept it as simple as possible.
Contributor
Author
There was a problem hiding this comment.
mmm i could change the --benchmark-delete, but i think it is realy easy to run ./run.sh hri --benchmark, but it could be just the bash script as it is in main
* added demo roborregos day draft * Add initial pose and one order * Fix hot word * feat: integrate URDF components for FRIDA robot and add MoveIt launch configuration * nigh changes * Revert "feat: integrate URDF components for FRIDA robot and add MoveIt launch configuration" This reverts commit bc89a8f. * impulso * refine * delete comment * refine * refine --------- Co-authored-by: GilMM27 <A00839075@tec.mx> Co-authored-by: luiss_benvenuto <luiss.benvenuto@gmail.com> Co-authored-by: camila_tite <a00841255@tec.mx>
…979) mujoco_sim_init.launch.py was producing an empty MJCF for FRIDA because urdf2mjcf.py recurses from a hardcoded "world" root, and the FRIDA URDF only emits that link when xacro arg mujoco_plugin=true — which the sim MoveItConfigsBuilder was never passing. Same path also disabled the custom gripper and ZED camera because they're gated on load_zed. - arm_pkg/moveit_configs_builder_sim.py: pass mujoco_plugin='true' in __urdf_xacro_args so the xacro emits the world link, world_to_base floating joint, mujoco compiler block, and the per-link gravcomp references the simulation depends on. - mujoco_spawn/mujoco_sim_init.launch.py: declare load_zed and default it to true; propagate to MoveItConfigsBuilder so the custom gripper and ZED are included. - FRIDA.urdf.xacro: swap the official zed_camera macro for the custom load_zed (matching FRIDA_Real.urdf.xacro). The official macro pulls zed2.stl from zed_msgs — an apt package whose meshes don't ship — and lacks the <mujoco><camera name="zed_mujoco_camera_link"/> block the launch's topic remappings expect. - Gripper/Custom/gripper.xacro: add damping=5 and armature=0.01 to the finger joint references. urdf2mjcf.py copies dynamics for revolute joints but not for prismatic, so the fingers were undamped against a kp=1000 position actuator and oscillated.
* fix download-model.sh * update * sensitivity threshold
* added filters for face recognition * feat(face_recognition): implement face recognition functionality with InsightFace model * test: added benchmarking script for face recognition using dlib and insightface * test: added benchmark code * feat(benchmark): add benchmark script for lighting conditions in face recognition * feat; added benchmark for ligthing comparisions * refactor: remove unnecessary blank lines in face_recognition_test.py and added CLAHE * refactor: replace face_recognition with insightface and remove unused test file * chore: add header to the code * use image orienter topic * update dockerfiles * delete extra files * restore comments * delete constant * restore unknown --------- Co-authored-by: Hector Tovar <h.tovarm07@gmail.com> Co-authored-by: Fernando94654 <A00841856@tec.mx>
Integrates VAMP (Vectorized And Motion Planning) as the default MoveIt
planner for the FRIDA xArm6, replacing OMPL for typical plans (~20-100 ms
wall-clock through the full pipeline) while keeping OMPL as an automatic
fallback for the cases where VAMP's sphere collision model disagrees with
FCL's mesh check. The caller — RViz, move_group_interface, pick_server —
sees a single planner named "vamp"; the layered behavior is internal.
Architecture (each layer catches what the previous can miss):
pipeline "vamp"
└── VampPlannerManager (loads ompl_interface/OMPLPlanner via pluginlib)
└── VampPlanningContext::solve()
├── (1) VAMP planning via ROS2 service (sphere check, fast)
├── (2) FCL post-validation against PlanningScene (mesh, accurate)
│ PASS → return VAMP plan
└── (3) runOmplFallback() up to 4 retries
├── ompl_interface::OMPLPlanner::solve()
├── TOTG densify + time-parameterize
├── validate the densified result against scene
└── return first attempt that survives
Why each layer matters:
- VAMP's sphere model is an approximation; it occasionally returns plans
that look collision-free in sphere-space but graze obstacles in mesh
geometry. The plugin validates with the same isPathValid check the
outer pipeline would run, so we never return a bad trajectory.
- RRTConnect is stochastic, and TOTG's resampling can land intermediate
states inside obstacles even when OMPL's own waypoints are clear (most
often the ZED camera link grazes on interpolated wrist motion). The
retry loop handles this transparently — user sees one Plan request.
- OMPL's PlanningContext returns a geometric path with zero time stamps
because timing is normally added by response adapters in the outer
pipeline. Our context invokes OMPL directly, so we run TOTG ourselves
before returning — otherwise the controller rejects the trajectory.
Other parts of this change:
- The VAMP submodule's Python bindings live at vamp/src/ rather than
in PYTHONPATH; docker/manipulation/.bash_aliases now exports the
right path so interactive shells can `import vamp`. The setup_vamp.sh
entrypoint also exports it for non-interactive ros2 run invocations.
- vamp_server.py now parses arm joint limits straight from the
spherized URDF and rejects out-of-limit requests in <1 ms with a
diagnostic, rather than letting them sink three RRTConnect retries
(~70 ms) before bottoming out on a geometric validate() failure.
- The 68-sphere full-FRIDA collision model in frida_real.hh covers
base_link + link1..6 + gripper + both fingers + zed_camera_link +
intel_realsense + laser. An earlier draft of this work shipped an
82-sphere model whose URDF source had every collision sphere with
origin (0,0,0), causing Cricket to emit alias-style y[i]=y[base]
FK and stacking all spheres at each link's joint. The current model
is generated from a URDF with proper per-sphere offsets and has zero
alias copies in its FK.
- The FRIDA Custom gripper's <ros2_control> declaration moved out of
the <xacro:if mujoco_plugin> block and now picks the hardware plugin
per mode (UFRobotFakeSystemHardware in fake, MujocoSystem in MuJoCo,
real plugin on hardware). The fake launch merges custom_gripper_-
controllers.yaml on top of xarm6 controllers and spawns the
xarm_gripper_traj_controller alongside the arm controller, so
rightfinger gets state-published in fake mode (leftfinger follows
via URDF <mimic>). Before this, the finger meshes rendered at world
origin and VAMP collision-checked them at the wrong location.
- probe_cuboid.py is a standalone diagnostic that exercises
vamp.frida_real.fk() to verify sphere positions and collision
detection against a configurable AABB. It found the alias-FK bug
in the previous spherization and is useful for verifying any future
model regen.
- inflate_body_spheres.py is a small offline utility that walks a
spherized URDF and adds a configurable padding to spheres on body
links (link_base, link1..6, base_link) while leaving gripper,
fingers, and sensor links untouched. Not wired into any pipeline,
kept for a future iteration that wants asymmetric safety margin
without breaking close-approach picking.
- .gitmodules registers manipulation/packages/vamp (the upstream
KavrakiLab/vamp submodule) which was missing despite being in the
tree as a gitlink.
Verified end-to-end against fake_moveit_config: Tier 2.1-2.7 all pass,
including the corner case where VAMP's sphere model misses a real
collision — the OMPL fallback fires automatically and the plan
executes. Worst observed wall-clock for hard obstacle scenarios is
~7 s (RRTConnect retries inside the fallback); typical case is
~100-200 ms. Real-hardware testing (Tier 4) and pick-and-place
integration (Tier 3) are out of scope and follow as separate work.
…amera_center link (#998) The URDF defines the ZED camera link as `zed_camera_link` (via `robot_description/frida_description/urdf/zed/zed.xacro`), but the SRDF had six `disable_collisions` entries pointing to `zed_camera_center` — a link that does not exist in the URDF. RViz logged `Link 'zed_camera_center' is not known to URDF` repeatedly during motion-planning panel use. All six entries were redundant: each one had an equivalent `disable_collisions` with `zed_camera_link` already present at lines 99-105 covering the same link pairs (gripper, left_finger, right_finger, link3, link5, link6). Removing the wrong-name entries eliminates the warning with zero functional change to collision checking.
…999) The vamp Python bindings (_core_ext.*.so) are a non-standard build artifact: the normal `colcon build` does not produce them, so after a clean build, a fresh clone, or a platform change the .so goes missing and vamp_server.py / the vamp planner pipeline fail with `ModuleNotFoundError: No module named 'vamp._core._core_ext'`. docker/manipulation/run.sh now runs setup_vamp.sh as part of the container SETUP chain (both with and without --build). The script short-circuits in ~1s when the .so already exists and compiles it (~1-2 min) only when missing. The call is wrapped so a build failure is non-fatal — the shell/launch still proceeds because the plugin's OMPL fallback works without VAMP, rather than locking the user out. Submodule checkout is intentionally not automated here: it is a one-time step handled the same way as every other submodule in the repo (git submodule update --init --recursive). Documented in the plugin ReadMe alongside the two distinct ModuleNotFoundError symptoms.
* Start vamp_server with frida moveit config launches (toggleable) The "vamp" pipeline is the default planner in both frida_fake_moveit_config and frida_moveit_config, but the launches only configured the VampPlannerManager plugin — they never started the vamp_server.py backend it delegates to. As a result every first plan waited ~3s for the unavailable service, timed out, and fell back to OMPL, so VAMP was effectively never used out of the box. Both launches now include vamp_server.launch.py, gated by a new start_vamp_server argument (default true). Set start_vamp_server:=false to exercise the OMPL fallback or to run the server separately/remotely. The include reuses vamp_server.launch.py rather than duplicating its parameter declarations. ReadMe Step 3 updated: vamp_server no longer needs a separate terminal in the normal flow. * Make vamp_server.launch.py a singleton to avoid duplicate-node collision Now that the frida moveit configs start vamp_server (start_vamp_server default true), running vamp_server.launch.py by hand on top of them would spawn a second node named "vamp_server" — a name collision that makes the VAMP planning service ambiguous (clients may bind to either instance). The launch now checks the ROS graph at startup via `ros2 node list` and skips launching when a "vamp_server" node already exists, logging why. This protects both orderings (manual-first or config-first). A new force:=true argument bypasses the guard for the rare case of wanting a second instance. The graph check fails open: any error is treated as "not running" so it never blocks a legitimate launch. Refactored the parameter declarations into a single source-of-truth dict so the node params and the DeclareLaunchArgument list stay in sync. * Apply ruff-format to vamp launch changes * Shorten code comments to 2 lines max
…ed RobotState (#1003) VampPlanningContext::solve built the start RobotState with the bare RobotState(robot_model_) constructor, which leaves variable positions uninitialized. robotStateMsgToRobotState only fills joints present in req.start_state, so for partial or is_diff requests (e.g. from RViz on real hardware) the missing joints kept garbage values — observed as inf on joints 1/5/6, overflowing the float32 cast in vamp_server. VAMP then rejected every start as out-of-limits and silently fell back to OMPL, so it never actually planned on the real robot. Seed the state from getPlanningScene()->getCurrentState() (the live robot state) before applying req.start_state, so absent joints keep real values. Found during Tier 4 R6 (full-velocity) testing on the real xArm6.
* clean vision general * fix faces path
* Added first integration * Updated launches * Update packages * Update packages * Updated file omnidriver * Updated CMAKE * Fix typo launch * Update launch omno * renamed package * Fix update package * Update omnidriver * Test udevrules * Update lidar and stm32setup * Update lidar setup * Mapping test 2d * Update general_navigation * Update configs nav2 * Update nav2_config * Update cleaner * Update spatiovoxel * Ignore Area Added * Added keepout area * Update keepout * Update keepout * Added keepout to container * approach table test * Update table_docker * Table_docker test2 * Update markers * update papu * Update ransac * Reduced offset * update front dis * added approach table * Upload configs * More decay * Update dock table * Upload areas * Deleted trash * Updated precommit
* fix move service * change look_side_low_stare
* Add laundry tmr * Add doing laundry * Add pick basket pose * update basket pose * Add navigation to behind-basket pose computation and update task states * Add doing basket detection pose * Update cyclonedds_setup.sh to regenerate config based on CYCLONE_SHM variable * add --dlc general flag * add --dlc flags in areas * Fix vamp_server import under symlink-install by using realpath (#1017) vamp_server.py derived the vamp bindings path from os.path.abspath(__file__), which does not resolve symlinks. Under colcon --symlink-install, ros2 launches the installed symlink and abspath kept the install path, so "../../vamp/src" pointed at a nonexistent install/vamp_moveit_plugin/vamp/src. import vamp then failed with ModuleNotFoundError, the vamp_server node crashed on startup, and the plan_vamp_path service never appeared — so frida_moveit_config reported "Is vamp_server.py running?" and fell back to OMPL. Use os.path.realpath(__file__) so the symlink resolves back to the source tree and the sys.path entry is correct regardless of how the script is launched or whether PYTHONPATH is set. * Restore OMPL pipeline as default and lower octomap_resolution for small-object grasps (#1018) PR #977 (VAMP motion planning integration) made two unrelated changes that together broke pick/place for small objects on the real xArm6: 1. `default_planning_pipeline` was switched from "ompl" to "vamp" so every planning request routed through VampPlannerManager. 2. `octomap_resolution` was raised from 0.025 to 0.05 in frida_moveit_common.launch.py (likely to speed up VAMP's CAPT pointcloud build by reducing the number of voxels it iterates). Combined effect: at 0.05 voxels with the existing padding (offset 0.03, scale 1.05), each voxel inflates to ~12-14 cm of effective collision geometry. For objects in the ~5-8 cm range (e.g. an orange, an apple) the target object's own voxels cover the entire grasp pose, so the gripper configuration at the goal collides with the scene and move_group reports `RRTConnect: Unable to sample any valid states for goal tree` for every GPD-generated grasp. VAMP itself appeared to plan successfully because its goal self-filter (`self_filter_distance: 0.12 m` in vamp_server.py) strips the object's own voxels from VAMP's model at the goal configuration — but VAMP's post-validation against the full PlanningScene (FCL) then rejects the plan with "VAMP plan failed FCL post-validation", and the OMPL fallback inside VampPlannerManager hits the same goal-sampling failure. End result was that every pick burned ~8-14 s of planning before reporting failure. This commit restores the pre-#977 working state for OMPL by: - lowering `octomap_resolution` back to 0.025, the value used in commit 695487d where pick/place worked end-to-end; - switching `default_planning_pipeline` to "ompl" so requests that leave pipeline_id empty (pymoveit2 / MoveItPlanner default) route through plain OMPL instead of VampPlannerManager; - flipping the default of the `start_vamp_server` launch arg to false, since with the default pipeline switched no one consumes the backend and starting it wastes resources. VAMP is not removed — both the pipeline declaration and the launch include for vamp_server stay in place. Anyone can re-enable VAMP for experimentation by re-launching with `start_vamp_server:=true` and restoring the default pipeline to "vamp" in the moveit config. Tuning VAMP so its self_filter, r_point and security_margin match the small- grasp geometry well enough to stop masking goal-collision issues is a follow-up task tracked separately. Verified on the real FRIDA xArm6 with `ros2 launch manipulation_general ppc.launch.py`: end-to-end pick + place succeed for orange and apple through keyboard_input; the previous "Unable to sample any valid states for goal tree" no longer fires for grasps that are kinematically reachable. * Remove 'simulation' from AREAS variable in lib.sh (#1020) * update display to listen to kws (#1021) * add areas json * add move_arm_to_pose() to avoid returning to pose from picks * big trust in math * fix basket approach * add pick basket using pc * fix * change base link * iteration * fix collisions * doing laundry working test * Add more steps * Add close dishwasher * Add pick clothes * update areas * add manip transition * Remove unnecessary changes * Remove frida_constants /manipulation unnecessary files * fix * Remove chagnes * fix move service * fix nav tasks * change look_side_low_stare * update task manager * fix manip messages * delete vision task func * delete manip subtasks func * return nav_task miau miau * delete 180 * Fix doing laundry task manager and add logic for washing machine * fix place typo --------- Co-authored-by: luiss_benvenuto <luiss.benvenuto@gmail.com> Co-authored-by: CamilaTite26 <a00841255@tec.mx> Co-authored-by: GilMM27 <A00839075@tec.mx> Co-authored-by: José Luis Domínguez Morales <144069553+JLDominguezM@users.noreply.github.com> Co-authored-by: Gilberto Malagamba Montejo <148925854+GilMM27@users.noreply.github.com> Co-authored-by: GilMM27 <A00389075@tec.mx>
* restore plane * rm comment
* added dockerignore and improve dockerfile * remove comment
#1055) Detect at each shelf level and pick from the level where the object is found, instead of detecting only from table_stare. On shelves prefer a frontal grasp (top-down hits the compartment ceiling) and after grasping retract straight out toward the robot before moving away. Table picks are unchanged. Decision logic is in shelf_pick_logic and grasp_orientation with offline tests.
* fix urdf * rm comments * fix
* Added first integration * Updated launches * Update packages * Update packages * Updated file omnidriver * Updated CMAKE * Fix typo launch * Update launch omno * renamed package * Fix update package * Update omnidriver * Test udevrules * Update lidar and stm32setup * Update lidar setup * Mapping test 2d * Update general_navigation * Update configs nav2 * Update nav2_config * Update cleaner * Update spatiovoxel * Ignore Area Added * Added keepout area * Update keepout * Update keepout * Added keepout to container * Added EKF/ESTOP/firmware-error/RxSdo & dashboard changes from home-custom-base --------- Co-authored-by: GerardoFJ <A00838645@tec.mx>
biweep863
marked this pull request as ready for review
June 22, 2026 22:32
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
11 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Benchmark different models with --benchmark flag in hri area
Change current model to qwen3-4b, best model from benchmark