Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 12 additions & 12 deletions scripts/viser_utils.py
Original file line number Diff line number Diff line change
Expand Up @@ -11,10 +11,10 @@ def setup_viser_with_robot(robot_dir, robot_urdf_name):
robot = ViserUrdf(
server,
urdf,
load_meshes=True,
load_collision_meshes=False,
root_node_name="/robot",
)
load_meshes = True,
load_collision_meshes = False,
root_node_name = "/robot",
)

return server, robot

Expand All @@ -25,7 +25,7 @@ def add_spheres(
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)
Expand All @@ -39,11 +39,11 @@ def add_spheres(
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]),
)
name = f"{prefix}_{i}",
radius = sphere_rad,
position = tuple(sphere_pos[:3]),
color = tuple(colors[i]),
)
return sphere_handles


Expand All @@ -67,8 +67,8 @@ def add_trajectory(server, waypoints, robot, attachment_handles, attachment_posi
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
)
"Current Waypoint", min = 0, max = len(waypoints) - 1, step = 1, initial_value = 0
)

@traj_slider.on_update
def update_robot_pose(event):
Expand Down
21 changes: 8 additions & 13 deletions scripts/visualize_viser.py
Original file line number Diff line number Diff line change
Expand Up @@ -7,14 +7,12 @@
import vamp
from fire import Fire


# Starting configuration
a = [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785]

# Goal configuration
b = [2.35, 1.0, 0.0, -0.8, 0, 2.5, 0.785]


# Problem specification: a list of sphere centers
problem = [
[0.55, 0, 0.25],
Expand All @@ -31,7 +29,7 @@
[-0.35, -0.35, 0.8],
[0, -0.55, 0.8],
[0.35, -0.35, 0.8],
]
]


def main(
Expand All @@ -40,11 +38,10 @@ def main(
attachment_offset: float = 0.02,
planner: str = "rrtc",
**kwargs,
):
):

(vamp_module, planner_func, plan_settings, simp_settings) = (
vamp.configure_robot_and_planner_with_kwargs("panda", planner, **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
tf = np.identity(4)
Expand All @@ -64,14 +61,14 @@ def main(

_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)
# Add attachment sphere to visualization
attachment_sph = add_spheres(
server, np.zeros((1, 3)), np.array([attachment_radius]), colors=[[0, 255, 0]]
)
server, np.zeros((1, 3)), np.array([attachment_radius]), colors = [[0, 255, 0]]
)

# Update attachment sphere positions corresponding to the waypoints.
# this could also be made into a callable that can be called during trajectory viz
Expand All @@ -87,9 +84,7 @@ def get_attachment_pos(configuration):

attachment_positions = [get_attachment_pos(pos) for pos in simple.path.numpy()]

add_trajectory(
server, simple.path.numpy(), robot, attachment_sph, attachment_positions
)
add_trajectory(server, simple.path.numpy(), robot, attachment_sph, attachment_positions)

# display
while True:
Expand Down