Skip to content

Commit 1613ef2

Browse files
shaur-kshaur-k
authored
Merge pull request #419 from PickNikRobotics/sorting_franka
Updates to franka_dual_arm_sim, add sorting benchmark, add stationary admittance demo in lab_sim
2 parents 5b7fe6f + 7c02a6d commit 1613ef2

19 files changed

+1136
-33
lines changed

‎src/lab_sim/objectives/stationary_admittance.xml‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,89 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Stationary Admittance">
3+
<!--//////////-->
4+
<BehaviorTree
5+
ID="Stationary Admittance"
6+
_description="Puts the arm into Cartesian admittance mode"
7+
_favorite="false"
8+
>
9+
<Control ID="Sequence" name="TopLevelSequence">
10+
<SubTree
11+
ID="Move to Waypoint"
12+
_collapsed="true"
13+
controller_names="joint_trajectory_controller"
14+
planner_interface="moveit_default"
15+
waypoint_name="Home"
16+
acceleration_scale_factor="1.0"
17+
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
18+
keep_orientation="false"
19+
keep_orientation_tolerance="0.05"
20+
link_padding="0.01"
21+
seed="0"
22+
velocity_scale_factor="1.0"
23+
joint_group_name="manipulator"
24+
/>
25+
<Action
26+
ID="SwitchController"
27+
activate_asap="true"
28+
automatic_deactivation="true"
29+
controller_list_action_name="/controller_manager/list_controllers"
30+
controller_switch_action_name="/controller_manager/switch_controller"
31+
strictness="1"
32+
timeout="-1.000000"
33+
activate_controllers="joint_trajectory_admittance_controller"
34+
/>
35+
<Decorator ID="Delay" delay_msec="2000">
36+
<Action
37+
ID="CallTriggerService"
38+
response_timeout="3.000000"
39+
service_name="/joint_trajectory_admittance_controller/zero_fts/tool0"
40+
wait_for_server_available_timeout="3.000000"
41+
/>
42+
</Decorator>
43+
<Action
44+
ID="SetAdmittanceParameters"
45+
config_file_name="stationary_admittance.yaml"
46+
admittance_parameters_msg="{admittance_parameters_msg}"
47+
/>
48+
<Action
49+
ID="GetCurrentPlanningScene"
50+
planning_scene_msg="{planning_scene}"
51+
/>
52+
<Action
53+
ID="GetRobotJointState"
54+
joint_state="{joint_state}"
55+
planning_scene="{planning_scene}"
56+
planning_group_name="manipulator"
57+
/>
58+
<Action
59+
ID="CreateStationaryTrajectory"
60+
joint_state="{joint_state}"
61+
joint_trajectory_msg="{joint_trajectory_msg}"
62+
trajectory_duration="10"
63+
/>
64+
<Action
65+
ID="LogMessage"
66+
log_level="info"
67+
message="Running admittance mode for 10 seconds"
68+
/>
69+
<Action
70+
ID="ExecuteTrajectoryWithAdmittance"
71+
goal_position_tolerance="10;10;10;10;10;10;10;"
72+
path_position_tolerance="10;10;10;10;10;10;10"
73+
admittance_parameters_msg="{admittance_parameters_msg}"
74+
absolute_force_torque_threshold="45;45;45;10;10;10"
75+
joint_trajectory_msg="{joint_trajectory_msg}"
76+
goal_duration_tolerance="-1.000000"
77+
controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory"
78+
/>
79+
</Control>
80+
</BehaviorTree>
81+
<TreeNodesModel>
82+
<SubTree ID="Stationary Admittance">
83+
<MetadataFields>
84+
<Metadata runnable="true" />
85+
<Metadata subcategory="Application - Advanced Examples" />
86+
</MetadataFields>
87+
</SubTree>
88+
</TreeNodesModel>
89+
</root>

‎src/lab_sim/objectives/stationary_admittance.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/test/objectives_integration_test.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,7 @@
3939

4040
cancel_objectives = {
4141
"3 Waypoints Pick and Place",
42+
"Stationary Admittance",
4243
"Cycle Between Waypoints",
4344
"Grasp Planning",
4445
"Grasp Pose Tuning With April Tag",

‎src/moveit_pro_franka_configs/franka_dual_arm_sim/config/control/franka_ros2_control.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@ joint_trajectory_controller:
2727
state_interfaces:
2828
- position
2929
- velocity
30+
allow_partial_joints_goal: true
3031
joints:
3132
- left_fr3_joint1
3233
- left_fr3_joint2
@@ -42,7 +43,6 @@ joint_trajectory_controller:
4243
- right_fr3_joint5
4344
- right_fr3_joint6
4445
- right_fr3_joint7
45-
allow_partial_joints_goal: true
4646
gains:
4747
left_fr3_joint1: { p: 600., d: 30., i: 0., i_clamp: 1. }
4848
left_fr3_joint2: { p: 600., d: 30., i: 0., i_clamp: 1. }

‎src/moveit_pro_franka_configs/franka_dual_arm_sim/description/mujoco/franka3.xml‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -100,7 +100,7 @@
100100
<body
101101
name="left_arm"
102102
pos="0.035 0.050681 0.356"
103-
quat="0.8730 -0.4380 -0.0229 -0.2215"
103+
quat="0.8657172 -0.4369879 0.0223569 -0.2430545"
104104
>
105105
<body name="left_base" childclass="fr3">
106106
<body name="left_fr3_link0">
@@ -411,7 +411,7 @@
411411
<body
412412
name="right_arm"
413413
pos="0.035 -0.050681 0.356"
414-
quat="0.8730 0.4380 0.0229 0.2215"
414+
quat="0.8657172 0.4369879 0.0223569 0.2430545"
415415
>
416416
<body name="right_base" childclass="fr3">
417417
<body name="right_fr3_link0">
@@ -775,7 +775,7 @@
775775
class="fr3"
776776
name="left_fr3_finger_joint1"
777777
joint="left_fr3_finger_joint1"
778-
kp="5000"
778+
kp="1000"
779779
kv="500"
780780
/>
781781

@@ -834,7 +834,7 @@
834834
class="fr3"
835835
name="right_fr3_finger_joint1"
836836
joint="right_fr3_finger_joint1"
837-
kp="5000"
837+
kp="1000"
838838
kv="500"
839839
/>
840840
</actuator>

‎src/moveit_pro_franka_configs/franka_dual_arm_sim/description/mujoco/scene.xml‎

Lines changed: 79 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,8 @@
22
<include file="franka3.xml" />
33

44
<statistic center="0.2 0.2 0.5" extent="1.8" />
5+
<size nkey="1" />
6+
<size nuser_jnt="0" />
57

68
<visual>
79
<global azimuth="120" elevation="-21" fovy="58" />
@@ -43,17 +45,64 @@
4345
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
4446
<site
4547
name="scene_camera_optical_frame"
46-
pos="1.5 0.7 1.5"
47-
euler="2.6 -0.8 -2.14"
48+
pos="0.7 0.0 0.65"
49+
xyaxes="0 -1 0 -1 0 0"
4850
/>
4951
<camera
5052
name="scene_camera"
51-
pos="1.5 0.7 1.5"
53+
pos="0.7 0.0 0.65"
5254
fovy="58"
5355
mode="fixed"
5456
resolution="640 480"
55-
euler="-0.54 0.8 2.14"
57+
xyaxes="0 -1 0 1 0 0"
5658
/>
59+
<geom
60+
name="table_front"
61+
type="box"
62+
size="0.4 0.3 0.01"
63+
pos="0.6 0 0.0"
64+
rgba="0.6 0.6 0.6 1"
65+
/>
66+
<geom
67+
name="table_left"
68+
type="box"
69+
size="0.25 0.5 0.01"
70+
pos="0 0.9 0.0"
71+
rgba="0.3 0.7 0.3 1"
72+
/>
73+
<geom
74+
name="table_right"
75+
type="box"
76+
size="0.25 0.5 0.01"
77+
pos="0 -0.9 0.0"
78+
rgba="0.7 0.3 0.3 1"
79+
/>
80+
81+
<body name="block_green_1" pos="0.6 0 0.2">
82+
<freejoint name="block_green_1" />
83+
<geom type="box" size="0.025 0.025 0.025" rgba="0 0.8 0 1" />
84+
</body>
85+
<body name="block_green_2" pos="0.6 0.15 0.2">
86+
<freejoint name="block_green_2" />
87+
<geom type="box" size="0.025 0.025 0.025" rgba="0 0.8 0 1" />
88+
</body>
89+
<body name="block_green_3" pos="0.6 -0.15 0.2">
90+
<freejoint name="block_green_3" />
91+
<geom type="box" size="0.025 0.025 0.025" rgba="0 0.8 0 1" />
92+
</body>
93+
94+
<body name="block_red_1" pos="0.7 0 0.2">
95+
<freejoint name="block_red_1" />
96+
<geom type="box" size="0.025 0.025 0.025" rgba="0.8 0 0 1" />
97+
</body>
98+
<body name="block_red_2" pos="0.7 0.15 0.2">
99+
<freejoint name="block_red_2" />
100+
<geom type="box" size="0.025 0.025 0.025" rgba="0.8 0 0 1" />
101+
</body>
102+
<body name="block_red_3" pos="0.7 -0.15 0.2">
103+
<freejoint name="block_red_3" />
104+
<geom type="box" size="0.025 0.025 0.025" rgba="0.8 0 0 1" />
105+
</body>
57106
</worldbody>
58107

59108
<keyframe>
@@ -77,6 +126,12 @@
77126
left_fr3_joint7
78127
left_fr3_finger1
79128
left_fr3_finger2
129+
block_green_1 [x y z qw qx qy qz]
130+
block_green_2 [x y z qw qx qy qz]
131+
block_green_3 [x y z qw qx qy qz]
132+
block_red_1 [x y z qw qx qy qz]
133+
block_red_2 [x y z qw qx qy qz]
134+
block_red_3 [x y z qw qx qy qz]
80135
qvel:
81136
right_fr3_joint1
82137
right_fr3_joint2
@@ -96,6 +151,12 @@
96151
left_fr3_joint7
97152
left_fr3_finger1
98153
left_fr3_finger2
154+
block_green_1 [x y z r p y]
155+
block_green_2 [x y z r p y]
156+
block_green_3 [x y z r p y]
157+
block_red_1 [x y z r p y]
158+
block_red_2 [x y z r p y]
159+
block_red_3 [x y z r p y]
99160
ctrl:
100161
right_fr3_joint1
101162
right_fr3_joint2
@@ -113,14 +174,25 @@
113174
left_fr3_joint6
114175
left_fr3_joint7
115176
left_fr3_finger1 - We control only one finger.
116-
117177
-->
118178
<key
119179
name="default"
120180
qpos="0 -0.7854 0 -2.3562 0 1.5707 0.7853 0 0
121-
0 -0.7854 0 -2.3562 0 1.5707 0.7853 0 0"
181+
0 -0.7854 0 -2.3562 0 1.5707 0.7853 0 0
182+
0.6 0 0.2 1 0 0 0
183+
0.6 0.15 0.2 1 0 0 0
184+
0.6 -0.15 0.2 1 0 0 0
185+
0.7 0 0.2 1 0 0 0
186+
0.7 0.15 0.2 1 0 0 0
187+
0.7 -0.15 0.2 1 0 0 0"
122188
qvel="0 0 0 0 0 0 0 0 0
123-
0 0 0 0 0 0 0 0 0"
189+
0 0 0 0 0 0 0 0 0
190+
0 0 0 0 0 0
191+
0 0 0 0 0 0
192+
0 0 0 0 0 0
193+
0 0 0 0 0 0
194+
0 0 0 0 0 0
195+
0 0 0 0 0 0"
124196
ctrl="0 -0.7854 0 -2.3562 0 1.5707 0.7853 0
125197
0 -0.7854 0 -2.3562 0 1.5707 0.7853 0"
126198
/>

0 commit comments

Comments
 (0)