diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1217634c..e5f83e54 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,10 +7,11 @@ option(VAMP_FORCE_CLANG "Force the use of Clang." OFF)
option(VAMP_BUILD_PYTHON_BINDINGS "Build VAMP Python bindings" ON)
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)
option(VAMP_BUILD_OMPL_DEMO "Build VAMP C++ OMPL Integration Demo Scripts" OFF)
option(VAMP_OMPL_PATH "Search Path for OMPL Installation - Only Needed for Demo Script" "")
+
if(VAMP_FORCE_CLANG)
find_program(CLANG "clang")
find_program(CLANGPP "clang++")
diff --git a/cmake/Python.cmake b/cmake/Python.cmake
index 57ab5bbe..fa6fef72 100644
--- a/cmake/Python.cmake
+++ b/cmake/Python.cmake
@@ -21,6 +21,7 @@ if(VAMP_BUILD_PYTHON_BINDINGS)
panda
fetch
baxter
+ bimanualpanda
)
list(APPEND VAMP_ROBOT_STRUCTS
@@ -29,6 +30,7 @@ if(VAMP_BUILD_PYTHON_BINDINGS)
Panda
Fetch
Baxter
+ BimanualPanda
)
endif()
diff --git a/pyproject.toml b/pyproject.toml
index f24e6cde..ffab9476 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -34,5 +34,5 @@ cmake.build-type = "Release"
[tool.scikit-build.cmake.define]
VAMP_LTO = "OFF"
-VAMP_ROBOT_MODULES="sphere;ur5;panda;fetch;baxter"
-VAMP_ROBOT_STRUCTS="Sphere;UR5;Panda;Fetch;Baxter"
+VAMP_ROBOT_MODULES="sphere;ur5;panda;fetch;baxter;bimanualpanda"
+VAMP_ROBOT_STRUCTS="Sphere;UR5;Panda;Fetch;Baxter;BimanualPanda"
diff --git a/resources/panda/bipanda_spherized.urdf b/resources/panda/bipanda_spherized.urdf
new file mode 100644
index 00000000..5e12fa13
--- /dev/null
+++ b/resources/panda/bipanda_spherized.urdf
@@ -0,0 +1,1248 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/scripts/bimanual_franka.py b/scripts/bimanual_franka.py
new file mode 100644
index 00000000..73138ccd
--- /dev/null
+++ b/scripts/bimanual_franka.py
@@ -0,0 +1,106 @@
+from pathlib import Path
+import numpy as np
+
+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, 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, 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("bimanualpanda", planner, **kwargs)
+
+ # Create an attachment offset on the Z-axis from the end-effector frame
+ tf = np.identity(4)
+ tf[:3, 3] = np.array([0, 0, attachment_offset])
+ attachment = vamp.Attachment(tf)
+
+ # 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_module.joint_names(), 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, 0)
+
+ # 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):
+ attachment.set_ee_pose(vamp_module.eefk(configuration))
+ sphere = attachment.posed_spheres[0]
+
+ # sim.update_object_position(attachment_sphere, sphere.position)
+
+ # Plan and display
+ sampler = vamp_module.halton()
+ result = planner_func(a, b, e, plan_settings, sampler)
+ simple = vamp_module.simplify(result.path, e, simp_settings, sampler)
+ simple.path.interpolate_to_resolution(vamp.panda.resolution())
+ print(simple.path.numpy().shape)
+
+
+ e.detach(0)
+
+ # 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):
+ attachment.set_ee_pose(vamp_module.eefk(configuration))
+ sphere = attachment.posed_spheres[0]
+
+ # sim.update_object_position(attachment_sphere, sphere.position)
+
+ # Plan and display
+ sampler = vamp_module.halton()
+ result = planner_func(a, b, e, plan_settings, sampler)
+ simple = vamp_module.simplify(result.path, e, simp_settings, sampler)
+ simple.path.interpolate_to_resolution(vamp.panda.resolution())
+ print(simple.path.numpy().shape)
+
+
+ # sim.animate(simple.path, callback)
+
+
+if __name__ == "__main__":
+ Fire(main)
diff --git a/scripts/viser_utils.py b/scripts/viser_utils.py
new file mode 100644
index 00000000..8a201602
--- /dev/null
+++ b/scripts/viser_utils.py
@@ -0,0 +1,80 @@
+from viser.extras import ViserUrdf
+import viser
+import yourdfpy
+from typing import Sequence, Union
+
+
+def setup_viser_with_robot(robot_dir, robot_urdf_name):
+ server = viser.ViserServer()
+ # change the robot here
+ urdf = yourdfpy.URDF.load(str(robot_dir / robot_urdf_name))
+ robot = ViserUrdf(
+ server,
+ urdf,
+ load_meshes=True,
+ load_collision_meshes=False,
+ root_node_name="/robot",
+ )
+
+ return server, robot
+
+
+def add_spheres(
+ server: viser.ViserServer,
+ sphere_positions: Sequence,
+ sphere_radii: Sequence,
+ colors: Union[Sequence[int], Sequence[Sequence[int]]] = [],
+ prefix: str = "my_sphere",
+):
+ """
+ Add spheres to the env/
+ Sphere positions are (N,3) and sphere radii are (N)
+ """
+ sphere_handles = [None] * len(sphere_positions)
+ if len(colors) == 0:
+ colors = [[255, 0, 0]] * len(sphere_positions)
+ elif len(colors) == 1:
+ colors = colors * len(sphere_positions)
+ else:
+ assert len(colors) == len(sphere_positions)
+ for i, (sphere_pos, sphere_rad) in enumerate(zip(sphere_positions, sphere_radii)):
+ sphere_handles[i] = server.scene.add_icosphere(
+ name=f"{prefix}_{i}",
+ radius=sphere_rad,
+ position=tuple(sphere_pos[:3]),
+ color=tuple(colors[i]),
+ )
+ return sphere_handles
+
+
+def add_trajectory(server, waypoints, robot, attachment_handles, attachment_positions):
+ """
+ Adds a slider to step through waypoints of a trajectory also allows for auto step through
+ using play/pause button
+
+ Args:
+ server (ViserServer): ViserServer instance
+ waypoints (numpy.array): A 2D numpy array (shape: (N,7)) with N waypoints of joint poses
+ robot (ViserUrdf): ViserUrdf instance of the robot
+
+ attachment_handles (numpy.array) - this is a P element list of attachment handles, spheres here
+ attachment_positions (numpy.array) - this is a (N, P, 3) array of the position of each attachment handle at each waypoint pos.
+
+ Returns:
+ return_type: None.
+ """
+ if len(waypoints) < 1:
+ return
+ assert len(attachment_handles) == len(attachment_positions[0])
+ traj_slider = server.gui.add_slider(
+ "Current Waypoint", min=0, max=len(waypoints) - 1, step=1, initial_value=0
+ )
+
+ @traj_slider.on_update
+ def update_robot_pose(event):
+ waypoint_idx = int(event.target.value)
+ joint_config = waypoints[waypoint_idx]
+ robot.update_cfg(joint_config)
+
+ for attach_idx, attachment_handle in enumerate(attachment_handles):
+ attachment_handle.position = attachment_positions[waypoint_idx][attach_idx]
diff --git a/scripts/visualize_viser.py b/scripts/visualize_viser.py
new file mode 100644
index 00000000..53629938
--- /dev/null
+++ b/scripts/visualize_viser.py
@@ -0,0 +1,115 @@
+import numpy as np
+from viser import transforms as tf
+import os
+from viser_utils import setup_viser_with_robot, add_spheres, add_trajectory
+from pathlib import Path
+
+import vamp
+from fire import Fire
+
+
+# Starting configuration
+a = [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785, 2.35, 1.0, 0.0, -0.8, 0, 2.5, 0.785]
+
+# Goal configuration
+b = [-2.35, 1.0, 0.0, -0.8, 0, 2.5, 0.785, 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785]
+
+
+# Problem specification: a list of sphere centers
+problem = [
+ [0.55, 0, 0.15],
+ [0.35, 0.45, 0.15],
+ [0, 0.65, 0.15],
+ [-0.55, 0, 0.15],
+ [-0.35, -0.55, 0.15],
+ [0, -0.65, 0.15],
+ [0.35, -0.55, 0.15],
+ [0.35, 0.6, 0.8],
+ [0, 0.6, 0.8],
+ [-0.35, 0.5, 0.8],
+ [-0.55, 0, 0.8],
+ [-0.35, -0.5, 0.8],
+ [0, -0.65, 0.8],
+ [0.35, -0.65, 0.8],
+]
+
+
+def main(
+ obstacle_radius: float = 0.2,
+ attachment_radius: float = 0.07,
+ attachment_offset: float = 0.04,
+ planner: str = "rrtc",
+ **kwargs,
+):
+
+ (vamp_module, planner_func, plan_settings, simp_settings) = (
+ vamp.configure_robot_and_planner_with_kwargs("bimanualpanda", planner, **kwargs)
+ )
+
+ # Create an attachment offset on the Z-axis from the end-effector frame
+ tf = np.identity(4)
+ tf[:3, 3] = np.array([0, 0, attachment_offset])
+ attachment = vamp.Attachment(tf)
+
+ attachment2 = vamp.Attachment(tf)
+
+ # 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)])
+ attachment2.add_spheres([vamp.Sphere([0, 0, 0], attachment_radius)])
+
+ robot_dir = Path(__file__).parents[1] / "resources" / "panda"
+
+ server, robot = setup_viser_with_robot(robot_dir, "bipanda_spherized.urdf")
+ robot.update_cfg(a)
+
+ e = vamp.Environment()
+ for sphere in problem:
+ e.add_sphere(vamp.Sphere(sphere, obstacle_radius))
+
+ _problem_sphere_handles = add_spheres(
+ server, np.array(problem), np.array([obstacle_radius] * len(problem))
+ )
+
+ # Add the attchment to the VAMP environment
+ e.attach(attachment, 0)
+ e.attach(attachment2, 1)
+
+ # Add attachment sphere to visualization
+ attachment_sph = add_spheres(
+ server,
+ np.zeros((2, 3)),
+ np.array([attachment_radius] * 2),
+ colors=[[0, 255, 0], [0, 0, 255]],
+ )
+
+ # Update attachment sphere positions corresponding to the waypoints.
+ # this could also be made into a callable that can be called during trajectory viz
+ def get_attachment_pos(configuration):
+ for idx, attach in enumerate([attachment, attachment2]):
+ attach.set_ee_pose(vamp_module.eefk(configuration)[idx])
+ return np.array(
+ [
+ attachment.posed_spheres[0].position,
+ attachment2.posed_spheres[0].position,
+ ]
+ )
+
+ # Plan and display
+ sampler = vamp_module.halton()
+ result = planner_func(a, b, e, plan_settings, sampler)
+ simple = vamp_module.simplify(result.path, e, simp_settings, sampler)
+ simple.path.interpolate_to_resolution(vamp.panda.resolution())
+
+ attachment_positions = [get_attachment_pos(pos) for pos in simple.path.numpy()]
+
+ add_trajectory(
+ server, simple.path.numpy(), robot, attachment_sph, attachment_positions
+ )
+
+ # display
+ while True:
+ continue
+
+
+if __name__ == "__main__":
+ Fire(main)
diff --git a/src/impl/vamp/bindings/environment.cc b/src/impl/vamp/bindings/environment.cc
index ae296b84..c265eb23 100644
--- a/src/impl/vamp/bindings/environment.cc
+++ b/src/impl/vamp/bindings/environment.cc
@@ -161,8 +161,8 @@ void vamp::binding::init_environment(nanobind::module_ &pymodule)
})
.def(
"attach",
- [](vc::Environment &e, const vc::Attachment &a) { e.attachments.emplace(a); })
- .def("detach", [](vc::Environment &e) { e.attachments.reset(); });
+ [](vc::Environment &e, const vc::Attachment &a, const size_t eef_id = 0) { e.attach(a, eef_id); })
+ .def("detach", [](vc::Environment &e, const size_t eef_id = 0) { e.detach(eef_id); });
pymodule.def(
"filter_pointcloud",
diff --git a/src/impl/vamp/bindings/robot_helper.hh b/src/impl/vamp/bindings/robot_helper.hh
index 8fa3d7c1..2e8d213e 100644
--- a/src/impl/vamp/bindings/robot_helper.hh
+++ b/src/impl/vamp/bindings/robot_helper.hh
@@ -276,9 +276,12 @@ namespace vamp::binding
path, EnvironmentVector(environment), settings, rng);
}
- inline static auto eefk(const Type &start) -> Eigen::Matrix4f
+ inline static auto eefk(const Type &start) -> std::vector
{
- return Robot::eefk(Input::array(start)).matrix();
+ std::vector eefk_m;
+ for(const auto &fk : Robot::eefk(Input::array(start)))
+ eefk_m.push_back(fk.matrix());
+ return eefk_m;
}
inline static auto filter_self_from_pointcloud(
@@ -355,7 +358,7 @@ namespace vamp::binding
"Minimum and maximum radii sizes of robot spheres.");
submodule.def(
"joint_names", []() { return Robot::joint_names; }, "Joint names for the robot in order of DoF");
- submodule.def("end_effector", []() { return Robot::end_effector; }, "End-effector frame name.");
+ submodule.def("end_effectors", []() { return Robot::end_effectors; }, "End-effector frame names.");
using RNG = vamp::rng::RNG;
nb::class_(submodule, "RNG", "RNG for robot configurations.")
diff --git a/src/impl/vamp/collision/environment.hh b/src/impl/vamp/collision/environment.hh
index a380bf85..7e80120b 100644
--- a/src/impl/vamp/collision/environment.hh
+++ b/src/impl/vamp/collision/environment.hh
@@ -1,5 +1,6 @@
#pragma once
+#include