Repository navigation
Expand file tree
/
Copy pathrun_sequence.py
More file actions
112 lines (91 loc) · 4.31 KB
/
Copy pathrun_sequence.py
File metadata and controls
112 lines (91 loc) · 4.31 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
#
# Licensed under the NVIDIA Source Code License [see LICENSE for details].
"""IndustRealLib: Run sequence script.
This script allows a user to quickly run a sequence instance (itself
consisting of task instances) on the Franka. The tasks instances are
executed in an interleaved fashion (e.g., pick place insert for goal 1,
pick place insert for goal 2, and so on). The specified sequence must
have a corresponding src/industreallib/sequences/instance_configs/
<sequence_name>.yaml file.
Typical usage example:
python run_sequence.py -s pick_place_insert_pegs
"""
# Standard Library
import argparse
# NVIDIA
import industreallib.sequences.scripts.sequence_utils as sequence_utils
def _get_args():
"""Gets arguments from the command line."""
parser = argparse.ArgumentParser()
parser.add_argument(
"-s",
"--sequence_instance_config_name",
required=True,
help="Sequence instance configuration to run",
)
parser.add_argument(
"-d",
"--debug_mode",
action="store_true",
required=False,
help="Enable output for debugging",
)
args = parser.parse_args()
return args
if __name__ == "__main__":
"""Gets the sequence instance. Runs the sequence instance on each assembly."""
args = _get_args()
# Get sequence instance
sequence_instance_config = sequence_utils.get_sequence_instance_config(args=args)
sequence_instance = sequence_utils.get_sequence_instance(
args=args, sequence_instance_config=sequence_instance_config
)
# Get number of assemblies to assemble
num_assemblies = sequence_utils.get_num_assemblies(
sequence_instance_config=sequence_instance_config
)
# For each assembly (e.g., round-peg-and-round-hole), execute each task instance
# in sequence instance (e.g., pick pegs, place pegs, insert pegs), and repeat
visited_goals = []
for i in range(num_assemblies):
print(f"\nRunning {args.sequence_instance_config_name} sequence...")
for task_num, task_instance in enumerate(sequence_instance.task_instances):
# Get name of current task instance
task_instance_config_name = (
sequence_instance_config.sequence.task_instance_config_names[task_num]
)
# Get name of part (e.g., round_peg) assigned to current task instance
part_name = sequence_instance_config.sequence[
f"{task_instance_config_name}"
].part_order[i]
print(f"\nRunning {task_instance_config_name} task on {part_name} part...")
# Get goal coordinates for current part
# NOTE: If there is more than one detection label that has the desired part
# name (e.g., round_peg), the goal coordinates will be retrieved for the
# *first* detection label with that part name, unless those goal coordinates
# have already been visited. To prevent unexpected behavior, it is
# recommended to have a unique detection label for each part, or to restrict
# the scene to unique parts.
for label, goal in zip(task_instance.goal_labels, task_instance.goal_coords):
if label == part_name and goal not in visited_goals:
visited_goals.append(goal)
break
else:
raise ValueError(f"Desired part {part_name} not found.")
# Do pre-task procedure, go to goal coordinates, and do post-task procedure
task_instance.do_simple_procedure(
procedure=sequence_instance_config.sequence[
f"{task_instance_config_name}"
].do_before,
franka_arm=sequence_instance.franka_arm,
)
task_instance.go_to_goal(goal=goal, franka_arm=sequence_instance.franka_arm)
task_instance.do_simple_procedure(
procedure=sequence_instance_config.sequence[
f"{task_instance_config_name}"
].do_after,
franka_arm=sequence_instance.franka_arm,
)
print(f"\nFinished running {task_instance_config_name} task on {part_name} part.")
print(f"\nFinished running {args.sequence_instance_config_name} sequence.")