@@ -45,102 +45,102 @@ def from_world(cls, world: World) -> Self:
4545
4646 :return: A Tracy robot semantic annotation.
4747 """
48+ with world .modify_world ():
49+ robot = cls (
50+ name = PrefixedName (name = "tracy" , prefix = world .name ),
51+ root = world .get_body_by_name ("table" ),
52+ _world = world ,
53+ )
4854
49- robot = cls (
50- name = PrefixedName (name = "tracy" , prefix = world .name ),
51- root = world .get_body_by_name ("table" ),
52- _world = world ,
53- )
54-
55- # Create left arm
56- left_gripper_thumb = Finger (
57- name = PrefixedName ("left_gripper_thumb" , prefix = robot .name .name ),
58- root = world .get_body_by_name ("left_robotiq_85_left_knuckle_link" ),
59- tip = world .get_body_by_name ("left_robotiq_85_left_finger_tip_link" ),
60- _world = world ,
61- )
55+ # Create left arm
56+ left_gripper_thumb = Finger (
57+ name = PrefixedName ("left_gripper_thumb" , prefix = robot .name .name ),
58+ root = world .get_body_by_name ("left_robotiq_85_left_knuckle_link" ),
59+ tip = world .get_body_by_name ("left_robotiq_85_left_finger_tip_link" ),
60+ _world = world ,
61+ )
6262
63- left_gripper_finger = Finger (
64- name = PrefixedName ("left_gripper_finger" , prefix = robot .name .name ),
65- root = world .get_body_by_name ("left_robotiq_85_right_knuckle_link" ),
66- tip = world .get_body_by_name ("left_robotiq_85_right_finger_tip_link" ),
67- _world = world ,
68- )
63+ left_gripper_finger = Finger (
64+ name = PrefixedName ("left_gripper_finger" , prefix = robot .name .name ),
65+ root = world .get_body_by_name ("left_robotiq_85_right_knuckle_link" ),
66+ tip = world .get_body_by_name ("left_robotiq_85_right_finger_tip_link" ),
67+ _world = world ,
68+ )
6969
70- left_gripper = ParallelGripper (
71- name = PrefixedName ("left_gripper" , prefix = robot .name .name ),
72- root = world .get_body_by_name ("left_robotiq_85_base_link" ),
73- tool_frame = world .get_body_by_name ("l_gripper_tool_frame" ),
74- front_facing_orientation = Quaternion (0.5 , 0.5 , 0.5 , 0.5 ),
75- front_facing_axis = Vector3 (0 , 0 , 1 ),
76- thumb = left_gripper_thumb ,
77- finger = left_gripper_finger ,
78- _world = world ,
79- )
80- left_arm = Arm (
81- name = PrefixedName ("left_arm" , prefix = robot .name .name ),
82- root = world .get_body_by_name ("table" ),
83- tip = world .get_body_by_name ("left_wrist_3_link" ),
84- manipulator = left_gripper ,
85- _world = world ,
86- )
70+ left_gripper = ParallelGripper (
71+ name = PrefixedName ("left_gripper" , prefix = robot .name .name ),
72+ root = world .get_body_by_name ("left_robotiq_85_base_link" ),
73+ tool_frame = world .get_body_by_name ("l_gripper_tool_frame" ),
74+ front_facing_orientation = Quaternion (0.5 , 0.5 , 0.5 , 0.5 ),
75+ front_facing_axis = Vector3 (0 , 0 , 1 ),
76+ thumb = left_gripper_thumb ,
77+ finger = left_gripper_finger ,
78+ _world = world ,
79+ )
80+ left_arm = Arm (
81+ name = PrefixedName ("left_arm" , prefix = robot .name .name ),
82+ root = world .get_body_by_name ("table" ),
83+ tip = world .get_body_by_name ("left_wrist_3_link" ),
84+ manipulator = left_gripper ,
85+ _world = world ,
86+ )
8787
88- robot .add_arm (left_arm )
88+ robot .add_arm (left_arm )
8989
90- right_gripper_thumb = Finger (
91- name = PrefixedName ("right_gripper_thumb" , prefix = robot .name .name ),
92- root = world .get_body_by_name ("right_robotiq_85_left_knuckle_link" ),
93- tip = world .get_body_by_name ("right_robotiq_85_left_finger_tip_link" ),
94- _world = world ,
95- )
96- right_gripper_finger = Finger (
97- name = PrefixedName ("right_gripper_finger" , prefix = robot .name .name ),
98- root = world .get_body_by_name ("right_robotiq_85_right_knuckle_link" ),
99- tip = world .get_body_by_name ("right_robotiq_85_right_finger_tip_link" ),
100- _world = world ,
101- )
102- right_gripper = ParallelGripper (
103- name = PrefixedName ("right_gripper" , prefix = robot .name .name ),
104- root = world .get_body_by_name ("right_robotiq_85_base_link" ),
105- tool_frame = world .get_body_by_name ("r_gripper_tool_frame" ),
106- front_facing_orientation = Quaternion (0.5 , 0.5 , 0.5 , 0.5 ),
107- front_facing_axis = Vector3 (0 , 0 , 1 ),
108- thumb = right_gripper_thumb ,
109- finger = right_gripper_finger ,
110- _world = world ,
111- )
112- right_arm = Arm (
113- name = PrefixedName ("right_arm" , prefix = robot .name .name ),
114- root = world .get_body_by_name ("table" ),
115- tip = world .get_body_by_name ("right_wrist_3_link" ),
116- manipulator = right_gripper ,
117- _world = world ,
118- )
119- robot .add_arm (right_arm )
120-
121- camera = Camera (
122- name = PrefixedName ("camera" , prefix = robot .name .name ),
123- root = world .get_body_by_name ("camera_link" ),
124- forward_facing_axis = Vector3 (0 , 0 , 1 ),
125- field_of_view = FieldOfView (horizontal_angle = 1.047 , vertical_angle = 0.785 ),
126- minimal_height = 0.8 ,
127- maximal_height = 1.7 ,
128- _world = world ,
129- )
90+ right_gripper_thumb = Finger (
91+ name = PrefixedName ("right_gripper_thumb" , prefix = robot .name .name ),
92+ root = world .get_body_by_name ("right_robotiq_85_left_knuckle_link" ),
93+ tip = world .get_body_by_name ("right_robotiq_85_left_finger_tip_link" ),
94+ _world = world ,
95+ )
96+ right_gripper_finger = Finger (
97+ name = PrefixedName ("right_gripper_finger" , prefix = robot .name .name ),
98+ root = world .get_body_by_name ("right_robotiq_85_right_knuckle_link" ),
99+ tip = world .get_body_by_name ("right_robotiq_85_right_finger_tip_link" ),
100+ _world = world ,
101+ )
102+ right_gripper = ParallelGripper (
103+ name = PrefixedName ("right_gripper" , prefix = robot .name .name ),
104+ root = world .get_body_by_name ("right_robotiq_85_base_link" ),
105+ tool_frame = world .get_body_by_name ("r_gripper_tool_frame" ),
106+ front_facing_orientation = Quaternion (0.5 , 0.5 , 0.5 , 0.5 ),
107+ front_facing_axis = Vector3 (0 , 0 , 1 ),
108+ thumb = right_gripper_thumb ,
109+ finger = right_gripper_finger ,
110+ _world = world ,
111+ )
112+ right_arm = Arm (
113+ name = PrefixedName ("right_arm" , prefix = robot .name .name ),
114+ root = world .get_body_by_name ("table" ),
115+ tip = world .get_body_by_name ("right_wrist_3_link" ),
116+ manipulator = right_gripper ,
117+ _world = world ,
118+ )
119+ robot .add_arm (right_arm )
120+
121+ camera = Camera (
122+ name = PrefixedName ("camera" , prefix = robot .name .name ),
123+ root = world .get_body_by_name ("camera_link" ),
124+ forward_facing_axis = Vector3 (0 , 0 , 1 ),
125+ field_of_view = FieldOfView (horizontal_angle = 1.047 , vertical_angle = 0.785 ),
126+ minimal_height = 0.8 ,
127+ maximal_height = 1.7 ,
128+ _world = world ,
129+ )
130130
131- # Probably should be classified as "Neck", as that implies that i can move.
132- neck = Neck (
133- name = PrefixedName ("neck" , prefix = robot .name .name ),
134- sensors = {camera },
135- root = world .get_body_by_name ("camera_pole" ),
136- tip = world .get_body_by_name ("camera_link" ),
137- _world = world ,
138- )
131+ # Probably should be classified as "Neck", as that implies that i can move.
132+ neck = Neck (
133+ name = PrefixedName ("neck" , prefix = robot .name .name ),
134+ sensors = {camera },
135+ root = world .get_body_by_name ("camera_pole" ),
136+ tip = world .get_body_by_name ("camera_link" ),
137+ _world = world ,
138+ )
139139
140- robot .add_kinematic_chain (neck )
141- world .add_semantic_annotation (robot , exists_ok = True )
140+ robot .add_kinematic_chain (neck )
141+ world .add_semantic_annotation (robot , exists_ok = True )
142142
143- vel_limits = defaultdict (lambda : 0.2 )
144- robot .tighten_dof_velocity_limits_of_1dof_connections (new_limits = vel_limits )
143+ vel_limits = defaultdict (lambda : 0.2 )
144+ robot .tighten_dof_velocity_limits_of_1dof_connections (new_limits = vel_limits )
145145
146- return robot
146+ return robot
0 commit comments