Support multiple attachment frames by ShrutheeshIR · Pull Request #75 · KavrakiLab/vamp

ShrutheeshIR · 2025-10-18T07:54:24Z

Add support for multiple end effectors. This PR is in tandem with CoMMALab/cricket#4.
Major changes callout

Each robot has an array of EEFs instead of a single EEF.
- To support getting FKs, we have a new function, to_isometries(), that returns an array of EEF FKs. This is nice, as it can directly be fed into std::map for attachment (as follows)
Instead of attachments being std::optional, it is now a std::map<eef_id, attachment e>. This has some effects
- To add an attachment, we must specify the eef_id (in py and cpp). As a follow up, we could default this to 0 perhaps.
- Instead of checking if attachment exists, we just check if its size is non-zero.
- In addition to env-attachment, robot-attachment, we also need to check for attachment-attachment collisions. This is now done within env-attachment collision check itself, but could be moved out if needed.
Modified all robot files to reflect the changes. (there are only two changes per robot file -- eef_name -> eef_names, to_isometry -> to_isometries
- Also added bimanual_franka.hh
Also pulled in Viser visualization Example Python #76 for viser viz

TODO

Test if it actually works :(

ShrutheeshIR · 2025-10-18T08:01:04Z

src/impl/vamp/vector/interface.hh

        inline constexpr auto less_than(typename S::ScalarT s) const noexcept -> D
        {
-            return greater_than(broadcast_scalar(s));
+            return less_than(broadcast_scalar(s));


this has been annoying me for a while

ShrutheeshIR · 2025-10-18T08:02:28Z

src/impl/vamp/vector/math.hh

+}
+
+template <size_t N, typename DataT>
+inline static auto to_isometries(const DataT *buf) -> std::array<Eigen::Transform<DataT, 3, Eigen::Isometry>, N>


if there are multiple EEFs, then each FK is a 12 ele array

ShrutheeshIR · 2025-10-18T08:03:20Z

src/impl/vamp/collision/environment.hh

-        e.attachments->pose(p_tf);
+        for(auto i=0U; i < N; i++){
+            if(e.attachments.find(i) != e.attachments.end())
+                e.attachments.at(i).pose(p_tfs[i]);


at() to modify in place

ShrutheeshIR · 2025-10-18T08:03:53Z

src/impl/vamp/collision/validity.hh

+                }
            }
+
+        // now treat the other attachments also as potential collision objects


seems pretty clunky to me, but idk if there's a better way

ShrutheeshIR · 2025-10-18T08:18:59Z

Disabled clang-format for now. It modifies too many files

ShrutheeshIR · 2025-10-18T21:37:47Z

Well looking at other PRs, I should probably remove fr3.hh

ShrutheeshIR · 2025-10-21T00:42:04Z

src/impl/vamp/collision/environment.hh

 namespace vamp::collision
 {
+    template <typename DataT>
+    struct EEFAttachment {


for attachments, we need to keep track of both the attachment object, and the end effector it is attached to. custom struct to support it

I think this might not actually be the right approach -- actually, including attachment state in the environment is almost certainly "wrong," since environments should ideally be immutable in the planning process.

Instead, would it make sense to refactor into having an AttachedRobot<R> that stores the poses for attachment frames? I did this in my own Rust reimplementation and it worked very well.

zkingston

LGTM overall, some notes:

Does BimanualPanda work with the random_dance.py script? If not we may need to change the logic so it can find the URDF/SRDF for Bullet rendering.
Does the current attachment.py script still work?

zkingston · 2025-12-12T17:30:53Z

scripts/bimanual_franka.py

Lots of commented code here, please clean up.

zkingston · 2025-12-12T17:31:16Z

scripts/viser_utils.py

Can you remove and keep only with the Viser add PR?

Can you review that PR -- #76?

zkingston · 2025-12-12T17:31:30Z

scripts/visualize_viser.py

Same, remove and keep only with Viser PR.

zkingston · 2025-12-12T17:32:32Z

src/impl/vamp/collision/environment.hh

+                    return ;
+                }
+            eef_attachments.emplace_back(eef_idx, a);
+        }


Spacing between functions? Surprised clang-format didn't catch this.

Actually, a number of formatting things, please update.

zkingston · 2025-12-12T17:33:52Z

src/impl/vamp/collision/validity.hh

-            // TODO: Fix the sphere representation to allow to store float radii even with vector
-            // centers
-            if (sphere_environment_in_collision(e, s.x, s.y, s.z, s.r[{0, 0}]))
+        for (const auto &eef_attachment: e.eef_attachments){


As the EEs are indexed, may be better to just do i=0...N and j=i+1...N rather than the != iter check.

zkingston · 2025-12-12T17:34:25Z

src/impl/vamp/planning/validate.hh

        const std::size_t n = std::max(std::ceil(distance / static_cast<float>(rake) * resolution), 1.F);

-        bool valid = (environment.attachments) ? Robot::template fkcc_attach<rake>(environment, block) :
+        bool valid = (environment.eef_attachments.size()) ? Robot::template fkcc_attach<rake>(environment, block) :


can also use ! empty()

zkingston · 2025-12-12T17:34:57Z

src/impl/vamp/robots/baxter.hh

Did you add multiple EEs to baxter?

zkingston · 2025-12-12T17:35:47Z

CMakeLists.txt

 option(VAMP_INSTALL_CPP_LIBRARY "Install VAMP C++ library (disable for Python wheel builds)" ON)

-option(VAMP_BUILD_CPP_DEMO "Build VAMP C++ Demo Scripts" OFF)
+option(VAMP_BUILD_CPP_DEMO "Build VAMP C++ Demo Scripts" ON)


this should be off by default for python wheels

claytonwramsey

main thoughts:

factor out changes not related to bimanual manipulation
make sure to not bust up the formatting to keep the diff small
bimanual panda should probably not be a robot

claytonwramsey · 2025-12-17T17:56:01Z

src/impl/vamp/collision/environment.hh

 namespace vamp::collision
 {
+    template <typename DataT>
+    struct EEFAttachment {


I think this might not actually be the right approach -- actually, including attachment state in the environment is almost certainly "wrong," since environments should ideally be immutable in the planning process.

Instead, would it make sense to refactor into having an AttachedRobot<R> that stores the poses for attachment frames? I did this in my own Rust reimplementation and it worked very well.

claytonwramsey · 2025-12-17T17:58:02Z

src/impl/vamp/collision/environment.hh

        std::vector<HeightField<DataT>> heightfields;
        std::vector<CAPT> pointclouds;
-        std::optional<Attachment<DataT>> attachments;
+        std::vector<EEFAttachment<DataT>> eef_attachments; // eef_id to attachment


should eef_attachments instead be a std::vector<std::optional<EEFAttachment<DataT>>> as our key set is fixed when the robot is fixed? this means the attachment also doesn't need to track its own index.

claytonwramsey · 2025-12-17T17:58:33Z

src/impl/vamp/collision/environment.hh

+                if (eef_attachment.eef_idx == i)
+                    eef_attachment.attachment.pose(p_tfs[i]);
+            }
+        }


see my comment on changing the structure of eef_attachments

claytonwramsey · 2025-12-17T17:59:23Z

src/impl/vamp/collision/validity.hh

            }
+
+            // now treat the other attachments also as potential collision objects
+            // This is probably expensive, find a more efficient way. 


Attachment collision checking is comparatively cheap; I think it is ok to stay.
Maybe adding a coarse phase check would be an improvement to perf but generally I think it is not going to be a big issue.

claytonwramsey · 2025-12-17T18:00:37Z

src/impl/vamp/robots/bimanualpanda.hh

It is probably not the best idea to add a bimanual panda robot, since it's a lot to support and doesn't really reflect areal robot. Instead maybe we use Baxter as our demo for a bimanual robot?

claytonwramsey · 2025-12-17T18:00:57Z

src/impl/vamp/robots/panda.hh

did clang-format do something to this poor beast?

claytonwramsey · 2025-12-17T18:01:16Z

src/impl/vamp/robots/fr3.hh

cherry-pick this out maybe since it belongs in its own PR

ShrutheeshIR added 6 commits October 18, 2025 02:29

done with cpp

89fcd09

struct fails

a54a1a1

does not work

aa246ae

check other colls

9f18316

remove dump

7f3d533

health

1ec162d

ShrutheeshIR commented Oct 18, 2025

View reviewed changes

ShrutheeshIR added 8 commits October 18, 2025 15:32

add bipanda

b8ec2a3

add viz

831eda4

Merge branch 'viz-viser' into multi_eef_vamp

6edae82

attachment viz works

43429d1

fixed color

a0fad92

black formatted

b1f9be7

Merge branch 'viz-viser' into multi_eef_vamp

6ed387c

black formatted py

8cfbc05

ShrutheeshIR added 2 commits October 20, 2025 16:03

insert inside a

1bb1a9f

changed to vec

08544da

ShrutheeshIR commented Oct 21, 2025

View reviewed changes

zkingston requested changes Dec 12, 2025

View reviewed changes

wbthomason linked an issue Dec 12, 2025 that may be closed by this pull request

Allow Multiple Frames for Attachments #74

Open

zkingston changed the title ~~Multi eef vamp~~ Support multiple attachment frames Dec 12, 2025

claytonwramsey requested changes Dec 17, 2025

View reviewed changes

Conversation

ShrutheeshIR commented Oct 18, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

ShrutheeshIR commented Oct 18, 2025

Uh oh!

ShrutheeshIR commented Oct 18, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

zkingston left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

claytonwramsey left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

ShrutheeshIR commented Oct 18, 2025 •

edited

Loading