Skip to content

Commit 70fc0c2

Browse files
shaur-kshaur-k
committed
feat: parallel thinking and admittance demo in sim
1 parent 0f31687 commit 70fc0c2

File tree

7 files changed

+318
-1
lines changed

7 files changed

+318
-1
lines changed

‎src/lab_sim/objectives/admittance.yaml‎

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
SetAdmittanceParameters:
2+
admittance:
3+
damping_ratio_position:
4+
max: 1000
5+
min: 0
6+
x: 400
7+
y: 40
8+
z: 400
9+
damping_ratio_rotation:
10+
max: 1000
11+
min: 0
12+
x: 40
13+
y: 10
14+
z: 40
15+
joint_damping:
16+
max: 100
17+
min: 0
18+
value: 1
19+
mass_position:
20+
max: 100
21+
min: 0.001
22+
x: 2
23+
y: 4
24+
z: 2
25+
mass_rotation:
26+
max: 100
27+
min: 0.001
28+
x: 4
29+
y: 0.1
30+
z: 4
31+
selected_axes:
32+
rx: false
33+
ry: false
34+
rz: false
35+
x: true
36+
y: false
37+
z: true
38+
stiffness_position:
39+
max: 1000
40+
min: 0
41+
x: 80
42+
y: 80
43+
z: 80
44+
stiffness_rotation:
45+
max: 1000
46+
min: 0
47+
x: 80
48+
y: 25
49+
z: 80
50+
base_frame: base_link
51+
control_frame: grasp_link
52+
controller_name: ''
53+
end_effector_frame: grasp_link

‎src/lab_sim/objectives/admittance_demo.xml‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
<?xml version='1.0' encoding='UTF-8'?>
2+
<root BTCPP_format="4" main_tree_to_execute="Admittance Demo">
3+
<!--//////////-->
4+
<BehaviorTree ID="Admittance Demo" _description="Reconfoobling the admitt-o-matron." _favorite="false">
5+
<Control ID="Sequence" name="TopLevelSequence">
6+
<SubTree ID="Move to Waypoint" _collapsed="true" controller_names="/joint_trajectory_controller" planner_interface="moveit_default" waypoint_name="Home" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" seed="0" velocity_scale_factor="1.0" joint_group_name="manipulator"/>
7+
<Action ID="SwitchController" activate_asap="true" automatic_deactivation="true" controller_list_action_name="/controller_manager/list_controllers" controller_switch_action_name="/controller_manager/switch_controller" strictness="1" timeout="-1.000000" activate_controllers="joint_trajectory_admittance_controller"/>
8+
<Decorator ID="Delay" delay_msec="2000">
9+
<Action ID="CallTriggerService" response_timeout="3.000000" service_name="/joint_trajectory_admittance_controller/zero_fts/tool0" wait_for_server_available_timeout="3.000000"/>
10+
</Decorator>
11+
<Action ID="SetAdmittanceParameters" config_file_name="confooble.yaml" admittance_parameters_msg="{admittance_parameters_msg}"/>
12+
<Action ID="GetCurrentPlanningScene" planning_scene_msg="{planning_scene}"/>
13+
<Action ID="GetRobotJointState" joint_state="{joint_state}" planning_scene="{planning_scene}" planning_group_name="manipulator"/>
14+
<Action ID="CreateStationaryTrajectory" joint_state="{joint_state}" joint_trajectory_msg="{joint_trajectory_msg}" trajectory_duration="-1"/>
15+
<Action ID="ExecuteTrajectoryWithAdmittance" goal_position_tolerance="10;10;10;10;10;10;10;" path_position_tolerance="10;10;10;10;10;10;10" admittance_parameters_msg="{admittance_parameters_msg}" absolute_force_torque_threshold="45;45;45;10;10;10" joint_trajectory_msg="{joint_trajectory_msg}" goal_duration_tolerance="-1.000000" controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory"/>
16+
</Control>
17+
</BehaviorTree>
18+
<TreeNodesModel>
19+
<SubTree ID="Admittance Demo"/>
20+
</TreeNodesModel>
21+
</root>

‎src/lab_sim/objectives/admittance_stationary.yaml‎

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
SetAdmittanceParameters:
2+
admittance:
3+
damping_ratio_position:
4+
max: 1000
5+
min: 0
6+
x: 200
7+
y: 200
8+
z: 200
9+
damping_ratio_rotation:
10+
max: 100
11+
min: 0
12+
x: 40
13+
y: 40
14+
z: 40
15+
joint_damping:
16+
max: 100
17+
min: 0
18+
value: 1
19+
mass_position:
20+
max: 100
21+
min: 0.001
22+
x: 4
23+
y: 4
24+
z: 4
25+
mass_rotation:
26+
max: 100
27+
min: 0.001
28+
x: 4
29+
y: 4
30+
z: 4
31+
selected_axes:
32+
rx: false
33+
ry: false
34+
rz: false
35+
x: true
36+
y: true
37+
z: true
38+
stiffness_position:
39+
max: 1000
40+
min: 0
41+
x: 2
42+
y: 2
43+
z: 2
44+
stiffness_rotation:
45+
max: 1000
46+
min: 0
47+
x: 80
48+
y: 80
49+
z: 80
50+
base_frame: base_link
51+
control_frame: world
52+
controller_name: ''
53+
end_effector_frame: grasp_link

‎src/lab_sim/objectives/confooble.yaml‎

Lines changed: 105 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,105 @@
1+
SetAdmittanceParameters:
2+
admittance:
3+
damping_ratio_position:
4+
max: 1000
5+
min: 0
6+
x: 160
7+
y: 160
8+
z: 160
9+
damping_ratio_rotation:
10+
max: 1000
11+
min: 0
12+
x: 1
13+
y: 1
14+
z: 1
15+
joint_damping:
16+
max: 1000
17+
min: 0
18+
value: 1
19+
mass_position:
20+
max: 1000
21+
min: 0.001
22+
x: 4
23+
y: 4
24+
z: 4
25+
mass_rotation:
26+
max: 1000
27+
min: 0.001
28+
x: 1
29+
y: 1
30+
z: 1
31+
selected_axes:
32+
rx: true
33+
ry: true
34+
rz: true
35+
x: true
36+
y: true
37+
z: true
38+
stiffness_position:
39+
max: 1000
40+
min: 0
41+
x: 40
42+
y: 40
43+
z: 40
44+
stiffness_rotation:
45+
max: 1000
46+
min: 0
47+
x: 1
48+
y: 1
49+
z: 1
50+
base_frame: base_link
51+
control_frame: grasp_link
52+
controller_name: ''
53+
end_effector_frame: grasp_link
54+
admittance:
55+
damping_ratio_position:
56+
max: 1000
57+
min: 0
58+
x: 160
59+
y: 160
60+
z: 80
61+
damping_ratio_rotation:
62+
max: 1000
63+
min: 0
64+
x: 1
65+
y: 1
66+
z: 1
67+
joint_damping:
68+
max: 1000
69+
min: 0
70+
value: 1
71+
mass_position:
72+
max: 1000
73+
min: 0.001
74+
x: 4
75+
y: 4
76+
z: 4
77+
mass_rotation:
78+
max: 1000
79+
min: 0.001
80+
x: 1
81+
y: 1
82+
z: 1
83+
selected_axes:
84+
rx: true
85+
ry: true
86+
rz: true
87+
x: true
88+
y: true
89+
z: true
90+
stiffness_position:
91+
max: 1000
92+
min: 0
93+
x: 40
94+
y: 40
95+
z: 40
96+
stiffness_rotation:
97+
max: 1000
98+
min: 0
99+
x: 1
100+
y: 1
101+
z: 1
102+
base_frame: base_link
103+
control_frame: grasp_link
104+
controller_name: ''
105+
end_effector_frame: grasp_link

‎src/lab_sim/objectives/re-zero_force-torque_sensors.xml‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
<?xml version='1.0' encoding='UTF-8'?>
2+
<root BTCPP_format="4" main_tree_to_execute="Re-Zero Force-Torque Sensors">
3+
<!--//////////-->
4+
<BehaviorTree ID="Re-Zero Force-Torque Sensors" _description="Re-Zero all FTS" _favorite="false">
5+
<Control ID="Sequence" name="TopLevelSequence">
6+
<Action ID="AlwaysSuccess"/>
7+
</Control>
8+
</BehaviorTree>
9+
<TreeNodesModel>
10+
<SubTree ID="Re-Zero Force-Torque Sensors">
11+
<MetadataFields>
12+
<Metadata runnable="false"/>
13+
<Metadata subcategory="Perception - Planning Scene"/>
14+
</MetadataFields>
15+
</SubTree>
16+
</TreeNodesModel>
17+
</root>

‎src/moveit_pro_franka_configs/franka_dual_arm_sim/description/franka.urdf.xacro‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -230,7 +230,7 @@
230230
<param name="mujoco_model_package">franka_dual_arm_sim</param>
231231
<param name="render_publish_rate">10</param>
232232
<param name="tf_publish_rate">60</param>
233-
<param name="mujoco_viewer">true</param>
233+
<param name="mujoco_viewer">false</param>
234234
<param name="mujoco_keyframe">home</param>
235235
</hardware>
236236
</ros2_control>

‎src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/parallel_sort.xml‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
<?xml version='1.0' encoding='UTF-8'?>
2+
<root BTCPP_format="4" main_tree_to_execute="Parallel Sort">
3+
<BehaviorTree ID="Parallel Sort" _description="pleak" _favorite="false">
4+
<Control ID="Sequence">
5+
<Action ID="StopwatchBegin" timepoint="{timepoint}"/>
6+
<Control ID="Parallel" success_count="1" failure_count="1">
7+
<Control ID="Sequence">
8+
<SubTree ID="Find Red Block" _collapsed="true" object_centroid="{object_centroid}"/>
9+
<Action ID="Script" code="found:='red'"/>
10+
</Control>
11+
<Control ID="Sequence">
12+
<SubTree ID="Find Green Block" _collapsed="true" object_centroid="{object_centroid}"/>
13+
<Action ID="Script" code="found:='green'"/>
14+
</Control>
15+
</Control>
16+
<Control ID="Sequence">
17+
<SubTree ID="Open Right Gripper" _collapsed="true"/>
18+
<Action ID="InitializeMTCTask" task="{mtc_task}" timeout="-1.000000" trajectory_monitoring="false" controller_names="joint_trajectory_controller" task_id="right_grasp_task"/>
19+
<Action ID="SetupMTCCurrentState" task="{mtc_task}"/>
20+
<Action ID="SetupMTCPlanToPose" acceleration_scale_factor="1.000000" ik_frame="right_fr3_hand_tcp" keep_orientation="false" keep_orientation_link_names="right_fr3_hand_tcp" keep_orientation_tolerance="0.100000" link_padding="0.000000" max_iterations="5000" monitored_stage="current state" planning_group_name="right_arm" target_pose="{object_centroid}" task="{mtc_task}" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
21+
<Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}"/>
22+
<Action ID="ExecuteMTCTask" goal_duration_tolerance="-1.000000" solution="{mtc_solution}"/>
23+
<SubTree ID="Close Right Gripper" _collapsed="true"/>
24+
<Action ID="InitializeMTCTask" task="{mtc_task}" timeout="-1.000000" trajectory_monitoring="false" controller_names="joint_trajectory_controller" task_id="right_place_task"/>
25+
<Action ID="SetupMTCCurrentState" task="{mtc_task}"/>
26+
<Action ID="RetrieveWaypoint" joint_group_name="right_arm" waypoint_joint_state="{target_joint_state}" waypoint_name="Place Right"/>
27+
<Action ID="SetupMTCPlanToJointState" acceleration_scale_factor="1.000000" joint_state="{target_joint_state}" keep_orientation="false" keep_orientation_link_names="grasp_link" keep_orientation_tolerance="0.100000" link_padding="0.000000" max_iterations="5000" planning_group_name="manipulator" seed="0" task="{mtc_task}" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
28+
<Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}"/>
29+
<Action ID="ExecuteMTCTask" goal_duration_tolerance="-1.000000" solution="{mtc_solution}"/>
30+
<SubTree ID="Open Right Gripper" _collapsed="true"/>
31+
<SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" seed="0" velocity_scale_factor="1.0" waypoint_name="Home"/>
32+
</Control>
33+
<Control ID="Fallback">
34+
<Decorator ID="Precondition" else="FAILURE" if="found == &quot;red&quot;">
35+
<SubTree ID="Find Green Block" _collapsed="true" object_centroid="{object_centroid}"/>
36+
</Decorator>
37+
<SubTree ID="Find Red Block" _collapsed="true" object_centroid="{object_centroid}"/>
38+
</Control>
39+
<Control ID="Sequence">
40+
<SubTree ID="Open Left Gripper" _collapsed="true"/>
41+
<Action ID="InitializeMTCTask" task="{mtc_task}" timeout="-1.000000" trajectory_monitoring="false" controller_names="joint_trajectory_controller" task_id="left_grasp_task"/>
42+
<Action ID="SetupMTCCurrentState" task="{mtc_task}"/>
43+
<Action ID="SetupMTCMoveAlongFrameAxis" acceleration_scale="1.000000" axis_frame="world" axis_x="0.000000" axis_y="0.000000" axis_z="1.000000" hand_frame="left_fr3_hand_tcp" ignore_environment_collisions="false" max_distance="0.1" min_distance="0.100000" planning_group_name="left_arm" task="{mtc_task}" velocity_scale="1.000000"/>
44+
<Action ID="SetupMTCPlanToPose" acceleration_scale_factor="1.000000" ik_frame="left_fr3_hand_tcp" keep_orientation="false" keep_orientation_link_names="left_fr3_hand_tcp" keep_orientation_tolerance="0.100000" link_padding="0.000000" max_iterations="5000" monitored_stage="current state" planning_group_name="left_arm" target_pose="{object_centroid}" task="{mtc_task}" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
45+
<Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}"/>
46+
<Action ID="ExecuteMTCTask" goal_duration_tolerance="-1.000000" solution="{mtc_solution}"/>
47+
<SubTree ID="Close Left Gripper" _collapsed="true"/>
48+
<Action ID="InitializeMTCTask" task="{mtc_task}" timeout="-1.000000" trajectory_monitoring="false" controller_names="joint_trajectory_controller" task_id="left_place_task"/>
49+
<Action ID="SetupMTCCurrentState" task="{mtc_task}"/>
50+
<Action ID="RetrieveWaypoint" joint_group_name="left_arm" waypoint_joint_state="{target_joint_state}" waypoint_name="Place Left"/>
51+
<Action ID="SetupMTCPlanToJointState" acceleration_scale_factor="1.000000" joint_state="{target_joint_state}" keep_orientation="false" keep_orientation_link_names="grasp_link" keep_orientation_tolerance="0.100000" link_padding="0.000000" max_iterations="5000" planning_group_name="manipulator" seed="0" task="{mtc_task}" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
52+
<Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}"/>
53+
<Action ID="ExecuteMTCTask" goal_duration_tolerance="-1.000000" solution="{mtc_solution}"/>
54+
<SubTree ID="Open Left Gripper" _collapsed="true"/>
55+
<SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" seed="0" velocity_scale_factor="1.0" waypoint_name="Home"/>
56+
</Control>
57+
<Action ID="StopwatchEnd" timepoint="{timepoint}"/>
58+
</Control>
59+
</BehaviorTree>
60+
<TreeNodesModel>
61+
<SubTree ID="Parallel Sort">
62+
<MetadataFields>
63+
<Metadata subcategory="Motion - Execute"/>
64+
<Metadata runnable="true"/>
65+
</MetadataFields>
66+
</SubTree>
67+
</TreeNodesModel>
68+
</root>

0 commit comments

Comments
 (0)