77import vamp
88from fire import Fire
99
10-
1110# Starting configuration
1211a = [0.0 , - 0.785 , 0.0 , - 2.356 , 0.0 , 1.571 , 0.785 ]
1312
1413# Goal configuration
1514b = [2.35 , 1.0 , 0.0 , - 0.8 , 0 , 2.5 , 0.785 ]
1615
17-
1816# Problem specification: a list of sphere centers
1917problem = [
2018 [0.55 , 0 , 0.25 ],
3129 [- 0.35 , - 0.35 , 0.8 ],
3230 [0 , - 0.55 , 0.8 ],
3331 [0.35 , - 0.35 , 0.8 ],
34- ]
32+ ]
3533
3634
3735def main (
@@ -40,11 +38,10 @@ def main(
4038 attachment_offset : float = 0.02 ,
4139 planner : str = "rrtc" ,
4240 ** kwargs ,
43- ):
41+ ):
4442
45- (vamp_module , planner_func , plan_settings , simp_settings ) = (
46- vamp .configure_robot_and_planner_with_kwargs ("panda" , planner , ** kwargs )
47- )
43+ (vamp_module , planner_func , plan_settings ,
44+ simp_settings ) = (vamp .configure_robot_and_planner_with_kwargs ("panda" , planner , ** kwargs ))
4845
4946 # Create an attachment offset on the Z-axis from the end-effector frame
5047 tf = np .identity (4 )
@@ -64,14 +61,14 @@ def main(
6461
6562 _problem_sphere_handles = add_spheres (
6663 server , np .array (problem ), np .array ([obstacle_radius ] * len (problem ))
67- )
64+ )
6865
6966 # Add the attchment to the VAMP environment
7067 e .attach (attachment )
7168 # Add attachment sphere to visualization
7269 attachment_sph = add_spheres (
73- server , np .zeros ((1 , 3 )), np .array ([attachment_radius ]), colors = [[0 , 255 , 0 ]]
74- )
70+ server , np .zeros ((1 , 3 )), np .array ([attachment_radius ]), colors = [[0 , 255 , 0 ]]
71+ )
7572
7673 # Update attachment sphere positions corresponding to the waypoints.
7774 # this could also be made into a callable that can be called during trajectory viz
@@ -87,9 +84,7 @@ def get_attachment_pos(configuration):
8784
8885 attachment_positions = [get_attachment_pos (pos ) for pos in simple .path .numpy ()]
8986
90- add_trajectory (
91- server , simple .path .numpy (), robot , attachment_sph , attachment_positions
92- )
87+ add_trajectory (server , simple .path .numpy (), robot , attachment_sph , attachment_positions )
9388
9489 # display
9590 while True :
0 commit comments