Skip to content

Commit 5f991d1

Browse files
MikeWrockshaur-k
MikeWrock
authored and
shaur-k
committed
Add JTAC controller and ExecuteMTCSolution examples to grinding sim
1 parent a5d44ba commit 5f991d1

File tree

5 files changed

+275
-0
lines changed

5 files changed

+275
-0
lines changed

‎src/grinding_sim/config/config.yaml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -110,6 +110,7 @@ ros2_control:
110110
controllers_inactive_at_startup:
111111
- "servo_controller"
112112
- "velocity_force_controller"
113+
- "joint_trajectory_admittance_controller"
113114

114115
# Configuration for loading behaviors and objectives.
115116
# [Required]

‎src/grinding_sim/config/control/ur20.ros2_control.yaml‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,8 @@ controller_manager:
99
type: joint_trajectory_controller/JointTrajectoryController
1010
velocity_force_controller:
1111
type: velocity_force_controller/VelocityForceController
12+
joint_trajectory_admittance_controller:
13+
type: joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController
1214

1315

1416
joint_state_broadcaster:
@@ -168,3 +170,50 @@ velocity_force_controller:
168170
command_timeout: 0.2
169171
# Padding (in radians) to add to joint position limits as a safety margin.
170172
joint_limit_position_tolerance: 0.02
173+
174+
joint_trajectory_admittance_controller:
175+
ros__parameters:
176+
joints:
177+
- shoulder_pan_joint
178+
- shoulder_lift_joint
179+
- elbow_joint
180+
- wrist_1_joint
181+
- wrist_2_joint
182+
- wrist_3_joint
183+
# Specifies the base link of the kinematic chain. Must exist in the robot description.
184+
base_frame: base_link
185+
# Specifies the frame/link name of the force torque sensor. Must exist in the robot description.
186+
sensor_frame: tool0
187+
# Specifies the frame/link name of the end-effector frame. Must exist in the robot description.
188+
ee_frame: grasp_link
189+
# The name of the force/torque ros2_control hardware interface which will be used in the admittance calculation.
190+
# If empty, the controller will run without admittance.
191+
# ft_sensor_name: tcp_fts_sensor
192+
# Joint accelerations to use for immediate stops (e.g. when cancelling a running trajectory).
193+
# Chosen to match MoveIt configs.
194+
stop_accelerations: [30.0, 30.0, 30.0, 30.0, 30.0, 30.0]
195+
# The ratio of the cutoff frequency to the sampling frequency. Valid values range
196+
# from 0 to 1, where 1 is the sampling frequency. By default, this is set to 1.0, which means no filtering is
197+
# applied.
198+
ft_cutoff_frequency_ratio: 1.0
199+
# The deadband threshold for the force / torque measurements in Newtons and Newtons-meter. By default, these are set
200+
# to 0.0. Increase these values if the force readings from your force torque sensor are noisy and cause linear /
201+
# rotational motion along the FTS axes when the admittance controller is activated.
202+
ft_force_deadband: 0.0
203+
ft_torque_deadband: 0.0
204+
# The rate in Hz at which the controller will publish the state. By default, this is set to 50 Hz.
205+
state_publish_rate: 50
206+
# The damping coefficient for the Jacobian damped least-squares inverse. By default, this value is set to 0.01.
207+
# Increase this value if your robot moves too quickly while admittance control is active when it nears singularities.
208+
jacobian_damping: 0.01
209+
# The gravity vector in the base frame. By default, this is set to [0.0, 0.0, -9.8]. This value is used to calculate
210+
# the gravity compensation torque. If your robot is not in the same orientation as the base frame, you should
211+
# provide the gravity vector in the base frame.
212+
gravity_vector: [0.0, 0.0, -9.8]
213+
# Default maximum joint-space deviation accepted along the trajectory, if not specified in the goal message.
214+
default_path_tolerance: 0.05
215+
# Default maximum joint-space deviation accepted at the trajectory end-point, if not specified in the goal message.
216+
default_goal_tolerance: 0.05
217+
# Default maximum absolute force torque readings allowed from the force torque sensor along each Cartesian space
218+
# axis, if not specified in the goal message. If exceeded, execution will abort.
219+
default_absolute_force_torque_threshold: [45.0, 45.0, 45.0, 10.0, 10.0, 10.0]

‎src/grinding_sim/objectives/grind_machined_part_with_jtac.xml‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Grind Machined Part with JTAC">
3+
<BehaviorTree
4+
ID="Grind Machined Part with JTAC"
5+
_description=""
6+
_favorite="true"
7+
>
8+
<SubTree
9+
ID="Grind part with MTC"
10+
_collapsed="false"
11+
controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory"
12+
controller_name="joint_trajectory_admittance_controller"
13+
controller_type="jtac"
14+
/>
15+
</BehaviorTree>
16+
<TreeNodesModel>
17+
<SubTree ID="Grind Machined Part with JTAC">
18+
<MetadataFields>
19+
<Metadata subcategory="Application - Grinding" />
20+
<Metadata runnable="true" />
21+
</MetadataFields>
22+
</SubTree>
23+
</TreeNodesModel>
24+
</root>

‎src/grinding_sim/objectives/grind_machined_part_with_jtc.xml‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Grind Machined Part with JTC">
3+
<BehaviorTree
4+
ID="Grind Machined Part with JTC"
5+
_description=""
6+
_favorite="true"
7+
>
8+
<SubTree
9+
ID="Grind part with MTC"
10+
_collapsed="false"
11+
controller_action_name="/joint_trajectory_controller/follow_joint_trajectory"
12+
controller_name="joint_trajectory_controller"
13+
controller_type="jtc"
14+
/>
15+
</BehaviorTree>
16+
<TreeNodesModel>
17+
<SubTree ID="Grind Machined Part with JTC">
18+
<MetadataFields>
19+
<Metadata subcategory="Application - Grinding" />
20+
<Metadata runnable="true" />
21+
</MetadataFields>
22+
</SubTree>
23+
</TreeNodesModel>
24+
</root>

‎src/grinding_sim/objectives/grind_part_with_mtc.xml‎

Lines changed: 177 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,177 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Grind part with MTC">
3+
<!--//////////-->
4+
<BehaviorTree ID="Grind part with MTC" _description="" _favorite="false">
5+
<Control ID="Sequence" name="TopLevelSequence">
6+
<Action ID="ClearSnapshot" />
7+
<!--View workspace and register part-->
8+
<SubTree
9+
ID="Move to Waypoint"
10+
_collapsed="true"
11+
acceleration_scale_factor="1.0"
12+
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
13+
controller_names="joint_trajectory_controller"
14+
joint_group_name="manipulator"
15+
keep_orientation="false"
16+
keep_orientation_tolerance="0.05"
17+
link_padding="0.01"
18+
velocity_scale_factor="1.0"
19+
waypoint_name="Retract"
20+
name="View Workspace"
21+
seed="0"
22+
/>
23+
<SubTree
24+
ID="Register Machined Part"
25+
_collapsed="true"
26+
registered_pose="{registered_pose}"
27+
/>
28+
<SubTree ID="Take Snapshot" _collapsed="true" />
29+
<!--Publish frame of registered part so we can plan in reference to it-->
30+
<Control ID="Fallback">
31+
<Decorator ID="Timeout" msec="500">
32+
<Action
33+
ID="PublishStaticFrame"
34+
pose="{registered_pose}"
35+
publish_rate="50"
36+
child_frame_id="registered_pose"
37+
/>
38+
</Decorator>
39+
<Action ID="AlwaysSuccess" />
40+
</Control>
41+
<!--Move the tool to a pose near the part-->
42+
<SubTree
43+
ID="Move to Waypoint"
44+
_collapsed="true"
45+
acceleration_scale_factor="1.0"
46+
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
47+
controller_names="joint_trajectory_controller"
48+
joint_group_name="manipulator"
49+
keep_orientation="false"
50+
keep_orientation_tolerance="0.05"
51+
link_padding="0.01"
52+
velocity_scale_factor="1.0"
53+
waypoint_name="Pre Grind"
54+
seed="0"
55+
/>
56+
<!--Load grinding poses-->
57+
<Action
58+
ID="LoadPoseStampedVectorFromYaml"
59+
output="{grinding_poses}"
60+
file_path="grinding_poses.yaml"
61+
/>
62+
<!--Visualize grinding poses-->
63+
<Action ID="Script" code="pose_count := 0" />
64+
<Decorator
65+
ID="ForEach"
66+
vector_in="{grinding_poses}"
67+
out="{target_pose}"
68+
index="{index}"
69+
>
70+
<Control ID="Sequence">
71+
<Action
72+
ID="VisualizePose"
73+
marker_name="{pose_count}"
74+
pose="{target_pose}"
75+
marker_lifetime="0.000000"
76+
marker_size="0.100000"
77+
/>
78+
<Action ID="Script" code="pose_count += 1" />
79+
</Control>
80+
</Decorator>
81+
<!--Setup an MTC task from the current robot state-->
82+
<Action
83+
ID="InitializeMTCTask"
84+
task="{mtc_task}"
85+
timeout="-1.000000"
86+
trajectory_monitoring="false"
87+
/>
88+
<Action ID="SetupMTCCurrentState" task="{mtc_task}" />
89+
<!-- Pass each of the grinding poses to the MTC task, and plan cartesian motions between them -->
90+
<Action
91+
ID="SetupMTCPathIK"
92+
acceleration_scale_factor="0.75"
93+
blending_radius="0.020000"
94+
ignore_environment_collisions="false"
95+
ik_cartesian_space_density="0.010000"
96+
ik_joint_space_density="0.100000"
97+
max_optimizer_iterations="1"
98+
path="{grinding_poses}"
99+
planning_group_name="manipulator"
100+
position_only="true"
101+
task="{mtc_task}"
102+
trajectory_sampling_rate="100"
103+
velocity_scale_factor="0.1"
104+
tip_links="grasp_link"
105+
/>
106+
<!--Wait for user to approve the grinding path and then execute it-->
107+
<Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}" />
108+
<Action
109+
ID="WaitForUserTrajectoryApproval"
110+
solution="{mtc_solution}"
111+
cartesian_path_links="grasp_link"
112+
/>
113+
<Action
114+
ID="SwitchController"
115+
activate_asap="true"
116+
automatic_deactivation="true"
117+
controller_list_action_name="/controller_manager/list_controllers"
118+
controller_switch_action_name="/controller_manager/switch_controller"
119+
strictness="1"
120+
timeout="-1.000000"
121+
activate_controllers="{controller_name}"
122+
/>
123+
<Action
124+
ID="ExecuteMTCSolution"
125+
admittance_parameters_msg="{admittance_parameters_msg}"
126+
apply_planning_scene_service="/apply_planning_scene"
127+
apply_start_scene="false"
128+
controller_action_name="{controller_action_name}"
129+
execution_pipeline="{controller_type}"
130+
goal_duration_tolerance="-1.000000"
131+
goal_position_tolerance="0.01"
132+
path_position_tolerance="0.05"
133+
solution="{mtc_solution}"
134+
/>
135+
<!--Move away from the part after completing the grinding path-->
136+
<SubTree
137+
ID="Move to Waypoint"
138+
_collapsed="true"
139+
acceleration_scale_factor="1.0"
140+
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
141+
controller_names="joint_trajectory_controller"
142+
joint_group_name="manipulator"
143+
keep_orientation="false"
144+
keep_orientation_tolerance="0.05"
145+
link_padding="0.01"
146+
velocity_scale_factor="1.0"
147+
waypoint_name="Approach"
148+
seed="0"
149+
/>
150+
</Control>
151+
</BehaviorTree>
152+
<TreeNodesModel>
153+
<SubTree ID="Grind part with MTC">
154+
<MetadataFields>
155+
<Metadata runnable="false" />
156+
<Metadata subcategory="Application - Grinding" />
157+
</MetadataFields>
158+
<inout_port
159+
name="controller_action_name"
160+
default="/joint_trajectory_admittance_controller/follow_joint_trajectory"
161+
type="std::string"
162+
>
163+
the topic to send the follow joint trajectory action
164+
</inout_port>
165+
<inout_port
166+
name="controller_name"
167+
default="joint_trajectory_admittance_controller"
168+
type="std::vector&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt;, std::allocator&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; &gt; &gt;"
169+
>
170+
the name of the controller to activate for motion
171+
</inout_port>
172+
<inout_port name="controller_type" default="jtac" type="std::string">
173+
The type of trajectory controller to use
174+
</inout_port>
175+
</SubTree>
176+
</TreeNodesModel>
177+
</root>

0 commit comments

Comments
 (0)