Add support to add custom end-effector attachments for Panda, UR5, an…

…d Fetch (#18) * Added Panda Attachments initial implementation * Adding attachment to cmake * hax for running attachment fk * Regenerate for correct Panda * Regenerate attached to panda_hand link * Experiment: see if using raw pointers for attachments decreases overhead * Experiment: std::optional instead of std::unique_ptr for attachments * Experiment: remove useless conditional around attachment posing * attachment test code * attachment python interface * Experiment: eefk function for attachment debugging * working! * UR5 attachment FK (tool0 frame) * Fetch attachment FK (gripper_link) * fusing attachment code * remove hack * API QoL changes for attachments * made default params a bit bigger * started readme docs * small docs * Regenerate UR5 attachment with robotiq_85_base_link EE frame * Regenerate all attached FK with new function call style * Fixed FK functions * fix: formatting * Update scripts/README.md Co-authored-by: Wil Thomason <[email protected]> * Update scripts/README.md Co-authored-by: Wil Thomason <[email protected]> * Update src/vamp/pybullet_interface.py Co-authored-by: Wil Thomason <[email protected]> * Update src/vamp/__init__.py Co-authored-by: Wil Thomason <[email protected]> * Update scripts/attachments.py Co-authored-by: Wil Thomason <[email protected]> * Update scripts/attachments.py Co-authored-by: Wil Thomason <[email protected]> * Update src/impl/vamp/bindings/common.hh Co-authored-by: Wil Thomason <[email protected]> * Update src/impl/vamp/robots/baxter.hh Co-authored-by: Wil Thomason <[email protected]> * fix: nits from PR * Adding documentation, detach method * EE frames in docs * updates to docker * added default constructor in to_array * chore: run clang-format on environment.cc * fix: Make attachments compile with old GCC I had no idea noexcept could make an explicitly defaulted function become implicitly deleted... * fix: format * Update scripts/README.md Co-authored-by: Wil Thomason <[email protected]> * Update scripts/README.md Co-authored-by: Wil Thomason <[email protected]> * Array should be default initialized * list of robots --------- Co-authored-by: Wil Thomason <[email protected]> Co-authored-by: Wil Thomason <[email protected]> Co-authored-by: Zachary Kingston <[email protected]>
KavrakiLab · Jun 26, 2024 · df6120a · df6120a
1 parent 96eb4d6
commit df6120a
Show file tree

Hide file tree

Showing 27 changed files with 24,153 additions and 89 deletions.
diff --git a/README.md b/README.md
@@ -207,6 +207,12 @@ These objects can be created with the following:
 - Pointclouds via `add_pointcloud()` in `vamp.Environment`. This will construct a CAPT from the provided list of points, the minimum and maximum robot sphere radii, and radius for each point in the pointcloud.
 See the `src/impl/vamp/collision/` folder for more information.
 
+Some robots (currently, the UR5, Panda, and Fetch) support attaching custom geometry (a collection of spheres) to the end-effector via `vamp.Attachment(relative_position, relative_quaternion_xyzw)`.
+Spheres can be added (in the attachment's frame) with `add_sphere(...)`.
+The attachment can be added to the environment with `vamp.Environment.attach(...)`, and removed with `vamp.Environment.detach()`.
+An example use of attachments with the Panda arm is available in `scripts/attachments.py`.
+
+
 ## Code Overview
 
 The code lives in the `src` folder, split into `impl` (the C++ core) and `vamp` (the Python interface).
@@ -246,7 +252,7 @@ Inside `impl/vamp`, the code is divided into the following directories:
 - [ ] Improved Python API
 - [ ] Batch configuration validation
 - [ ] Planning subgroups
-- [ ] Object attachment at end-effector
+- [X] Object attachment at end-effector
 - [ ] Mesh collision checking
 - [X] Pointcloud collision checking
 - [ ] Manifold-constrained planning

diff --git a/docker/ubuntu2004.dockerfile b/docker/ubuntu2004.dockerfile
@@ -2,7 +2,7 @@ FROM ubuntu:focal
 
 ENV DEBIAN_FRONTEND=noninteractive
 RUN apt-get update && \
-    apt-get install -y cmake clang llvm libstdc++6 python3 python3-dev python3-pip libeigen3-dev
+    apt-get install -y git cmake clang llvm libstdc++6 python3 python3-dev python3-pip libeigen3-dev
 COPY . vamp
 WORKDIR vamp
-RUN pip install --no-deps -v .
+RUN pip install -v .
diff --git a/docker/ubuntu2204-conda.dockerfile b/docker/ubuntu2204-conda.dockerfile
@@ -3,7 +3,7 @@ FROM ubuntu:jammy
 ENV DEBIAN_FRONTEND=noninteractive
 
 RUN apt-get update && \
-    apt-get install -y wget && \
+    apt-get install -y git wget && \
     mkdir -p ~/miniconda3 && \
     wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh -O ~/miniconda3/miniconda.sh && \
     bash ~/miniconda3/miniconda.sh -b -u -p ~/miniconda3 && \
@@ -16,4 +16,4 @@ COPY . vamp
 WORKDIR vamp
 
 SHELL ["/root/miniconda3/bin/conda", "run", "-n", "vamp", "/bin/bash", "-c"]
-RUN pip install --no-deps -v .
+RUN pip install -v .
diff --git a/docker/ubuntu2204.dockerfile b/docker/ubuntu2204.dockerfile
@@ -2,7 +2,7 @@ FROM ubuntu:jammy
 
 ENV DEBIAN_FRONTEND=noninteractive
 RUN apt-get update && \
-    apt-get install -y cmake clang libstdc++6 python3 python3-dev python3-pip libeigen3-dev
+    apt-get install -y git cmake clang libstdc++6 python3 python3-dev python3-pip libeigen3-dev
 COPY . vamp
 WORKDIR vamp
-RUN pip install --no-deps -v .
+RUN pip install -v .
diff --git a/docker/ubuntu2404.dockerfile b/docker/ubuntu2404.dockerfile
@@ -2,7 +2,7 @@ FROM ubuntu:noble
 
 ENV DEBIAN_FRONTEND=noninteractive
 RUN apt-get update && \
-    apt-get install -y cmake clang libstdc++6 python3 python3-dev python3-pip libeigen3-dev
+    apt-get install -y git cmake clang libstdc++6 python3 python3-dev python3-pip libeigen3-dev
 COPY . vamp
 WORKDIR vamp
-RUN pip install --break-system-packages --no-deps -v .
+RUN pip install --break-system-packages -v .
diff --git a/resources/README.md b/resources/README.md
@@ -10,6 +10,13 @@ See the `*_spherized.urdf` files for examples of these spherical decompositions
 We also use a secondary hierarchical decomposition: see the high-level single sphere URDF used for each robot in the `*_spherized_1.urdf` files.
 These decompositions are conservative estimations of the robot's mesh geometry.
 
+## Attachments
+
+There is support to attach custom geometry to the end-effector of the following robots, with respect to the following end-effector frames (see the robot's URDF for transform information):
+- UR5: `robotiq85_base_link`
+- Panda: `panda_hand`
+- Fetch: `gripper_link`
+
 ## Datasets
 
 ### MotionBenchMaker problems

diff --git a/scripts/README.md b/scripts/README.md
@@ -64,6 +64,14 @@ This script has the following arguments that only affect benchmarking:
 
 Visualization is enabled by `--visualize`, which shows the plan with the PyBullet visualizer.
 
+## `attachments.py`
+Demonstrates planning with end-effector attachments using the same problem as in `sphere_cage_example.py`.
+A single sphere is attached to the end-effector of the Panda and carried through the cage.
+
+This script has the following arguments that affect the placement of the attached sphere.
+- `attachment_radius`: Radius of the attached sphere.
+- `attachment_offset`: Offset of the attachment transform along the local-frame Z-axis of the end-effector (i.e., its distance from the end-effector).
+
 ## `flying_sphere.py`
 Demonstrates generating a roadmap over a heightfield for a flying sphere.
 The script has the following arguments:

diff --git a/scripts/attachments.py b/scripts/attachments.py
@@ -0,0 +1,81 @@
+from pathlib import Path
+
+import vamp
+from vamp import pybullet_interface as vpb
+
+from fire import Fire
+
+# Starting configuration
+a = [0., -0.785, 0., -2.356, 0., 1.571, 0.785]
+
+# Goal configuration
+b = [2.35, 1., 0., -0.8, 0, 2.5, 0.785]
+
+# Problem specification: a list of sphere centers
+problem = [
+    [0.55, 0, 0.25],
+    [0.35, 0.35, 0.25],
+    [0, 0.55, 0.25],
+    [-0.55, 0, 0.25],
+    [-0.35, -0.35, 0.25],
+    [0, -0.55, 0.25],
+    [0.35, -0.35, 0.25],
+    [0.35, 0.35, 0.8],
+    [0, 0.55, 0.8],
+    [-0.35, 0.35, 0.8],
+    [-0.55, 0, 0.8],
+    [-0.35, -0.35, 0.8],
+    [0, -0.55, 0.8],
+    [0.35, -0.35, 0.8],
+    ]
+
+
+def main(
+    obstacle_radius: float = 0.2,
+    attachment_radius: float = 0.07,
+    attachment_offset: float = 0.14,
+    planner: str = "rrtc",
+    **kwargs,
+    ):
+
+    (vamp_module, planner_func, plan_settings,
+     simp_settings) = vamp.configure_robot_and_planner_with_kwargs("panda", planner, **kwargs)
+
+    # Create an attachment offset on the Z-axis from the end-effector frame
+    attachment = vamp.Attachment([0, 0, attachment_offset], [0, 0, 0, 1])
+
+    # Add a single sphere to the attachment - spheres are added in the attachment's local frame
+    attachment.add_spheres([vamp.Sphere([0, 0, 0], attachment_radius)])
+
+    robot_dir = Path(__file__).parents[1] / 'resources' / 'panda'
+    sim = vpb.PyBulletSimulator(str(robot_dir / "panda_spherized.urdf"), vamp.ROBOT_JOINTS['panda'], True)
+
+    e = vamp.Environment()
+    for sphere in problem:
+        e.add_sphere(vamp.Sphere(sphere, obstacle_radius))
+        sim.add_sphere(obstacle_radius, sphere)
+
+    # Add the attchment to the VAMP environment
+    e.attach(attachment)
+
+    # Add attachment sphere to visualization
+    attachment_sphere = sim.add_sphere(attachment_radius, [0, 0, 0])
+
+    # Callback to update sphere's location in PyBullet visualization
+    def callback(configuration):
+        position, orientation_xyzw = vamp_module.eefk(configuration)
+        attachment.set_ee_pose(position, orientation_xyzw)
+        sphere = attachment.posed_spheres[0]
+
+        sim.update_object_position(attachment_sphere, sphere.position)
+
+    # Plan and display
+    result = planner_func(a, b, e, plan_settings)
+    simple = vamp_module.simplify(result.path, e, simp_settings)
+    simple.path.interpolate(vamp.panda.resolution())
+
+    sim.animate(simple.path, callback)
+
+
+if __name__ == "__main__":
+    Fire(main)
diff --git a/src/impl/vamp/bindings/common.hh b/src/impl/vamp/bindings/common.hh
@@ -232,6 +232,18 @@ namespace vamp::binding
         {
             return filter_robot_from_pointcloud<Robot>(pc, start, environment, point_radius);
         }
+
+        inline static auto eefk(const ConfigurationArray &start)
+            -> std::pair<std::array<float, 3>, std::array<float, 4>>
+        {
+            const auto &result = Robot::eefk(start);
+
+            std::array<float, 3> position = {result[0], result[1], result[2]};
+            // A (x, y, z, w) quaternion
+            std::array<float, 4> orientation = {result[3], result[4], result[5], result[6]};
+
+            return {position, orientation};
+        }
     };
 
     template <typename Robot>
@@ -504,6 +516,12 @@ namespace vamp::binding
             "point_radius"_a,
             "Filters all colliding points from a point cloud.");
 
+        submodule.def(
+            "eefk",
+            RH::eefk,
+            "configuration"_a,
+            "Returns the position and orientation (as a xyzw quaternion) of the robot's end-effector.");
+
         return submodule;
     }
 }  // namespace vamp::binding
diff --git a/src/impl/vamp/bindings/environment.cc b/src/impl/vamp/bindings/environment.cc
@@ -28,6 +28,11 @@ void vamp::binding::init_environment(nanobind::module_ &pymodule)
         .def_ro("y", &vc::Sphere<float>::y)
         .def_ro("z", &vc::Sphere<float>::z)
         .def_ro("r", &vc::Sphere<float>::r)
+        .def_prop_ro(
+            "position",
+            [](vc::Sphere<float> &sphere) {
+                return std::array<float, 3>{sphere.x, sphere.y, sphere.z};
+            })
         .def_ro("min_distance", &vc::Sphere<float>::min_distance)
         .def_rw("name", &vc::Sphere<float>::name);
 
@@ -152,7 +157,11 @@ void vamp::binding::init_environment(nanobind::module_ &pymodule)
                 auto start_time = std::chrono::steady_clock::now();
                 e.pointclouds.emplace_back(pc, r_min, r_max, r_point);
                 return vamp::utils::get_elapsed_nanoseconds(start_time);
-            });
+            })
+        .def(
+            "attach",
+            [](vc::Environment<float> &e, const vc::Attachment<float> &a) { e.attachments.emplace(a); })
+        .def("detach", [](vc::Environment<float> &e) { e.attachments.reset(); });
 
     pymodule.def(
         "filter_pointcloud",
@@ -169,4 +178,58 @@ void vamp::binding::init_environment(nanobind::module_ &pymodule)
                 vc::filter_pointcloud(pc, min_dist, max_range, origin, workcell_min, workcell_max, cull);
             return {filtered, vamp::utils::get_elapsed_nanoseconds(start_time)};
         });
+
+    nb::class_<vc::Attachment<float>>(pymodule, "Attachment")
+        .def(
+            "__init__",
+            [](vc::Attachment<float> *q,
+               const std::array<float, 3> &center,
+               const std::array<float, 4> &quaternion_xyzw) noexcept
+            {
+                new (q) vc::Attachment<float>(
+                    center[0],
+                    center[1],
+                    center[2],
+                    quaternion_xyzw[0],
+                    quaternion_xyzw[1],
+                    quaternion_xyzw[2],
+                    quaternion_xyzw[3]);
+            },
+            "Constructor for an attachment centered at a relative position and XYZW quaternion from the "
+            "end-effector.")
+        .def_prop_ro(
+            "relative_frame",
+            [](vc::Attachment<float> &a)
+            {
+                std::array<float, 3> position = {a.tf_tx, a.tf_ty, a.tf_tz};
+                std::array<float, 4> quaternion_xyzw = {a.tf_rx, a.tf_ry, a.tf_rz, a.tf_rw};
+
+                return std::pair{position, quaternion_xyzw};
+            })
+        .def(
+            "add_sphere",
+            [](vc::Attachment<float> &a, collision::Sphere<float> &sphere)
+            { a.spheres.emplace_back(sphere); })
+        .def(
+            "add_spheres",
+            [](vc::Attachment<float> &a, std::vector<collision::Sphere<float>> &spheres)
+            { a.spheres.insert(a.spheres.end(), spheres.cbegin(), spheres.cend()); })
+        .def(
+            "set_ee_pose",
+            [](vc::Attachment<float> &a,
+               const std::array<float, 3> &position,
+               const std::array<float, 4> &quaternion_xyzw)
+            {
+                a.pose(
+                    position[0],
+                    position[1],
+                    position[2],
+                    quaternion_xyzw[0],
+                    quaternion_xyzw[1],
+                    quaternion_xyzw[2],
+                    quaternion_xyzw[3]);
+            },
+            "position"_a,
+            "quaternion_xyzw"_a)
+        .def_ro("posed_spheres", &vc::Attachment<float>::posed_spheres);
 }