Skip to content

Commit 1439d13

Browse files
mariopratsshaur-k
marioprats
authored and
shaur-k
committed
updates to scene and adding Objectives
1 parent 5b7fe6f commit 1439d13

23 files changed

+1491
-33
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: 84 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,84 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Admittance Demo">
3+
<!--//////////-->
4+
<BehaviorTree
5+
ID="Admittance Demo"
6+
_description="Reconfoobling the admitt-o-matron."
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="confooble.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="-1.0"
63+
/>
64+
<Action
65+
ID="ExecuteTrajectoryWithAdmittance"
66+
goal_position_tolerance="10;10;10;10;10;10;10;"
67+
path_position_tolerance="10;10;10;10;10;10;10"
68+
admittance_parameters_msg="{admittance_parameters_msg}"
69+
absolute_force_torque_threshold="45;45;45;10;10;10"
70+
joint_trajectory_msg="{joint_trajectory_msg}"
71+
goal_duration_tolerance="-1.000000"
72+
controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory"
73+
/>
74+
</Control>
75+
</BehaviorTree>
76+
<TreeNodesModel>
77+
<SubTree ID="Admittance Demo">
78+
<MetadataFields>
79+
<Metadata runnable="true" />
80+
<Metadata subcategory="Application - Advanced Examples" />
81+
</MetadataFields>
82+
</SubTree>
83+
</TreeNodesModel>
84+
</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: 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="Re-Zero Force-Torque Sensors">
3+
<!--//////////-->
4+
<BehaviorTree
5+
ID="Re-Zero Force-Torque Sensors"
6+
_description="Re-Zero all FTS"
7+
_favorite="false"
8+
>
9+
<Control ID="Sequence" name="TopLevelSequence">
10+
<Action ID="AlwaysSuccess" />
11+
</Control>
12+
</BehaviorTree>
13+
<TreeNodesModel>
14+
<SubTree ID="Re-Zero Force-Torque Sensors">
15+
<MetadataFields>
16+
<Metadata runnable="false" />
17+
<Metadata subcategory="Perception - Planning Scene" />
18+
</MetadataFields>
19+
</SubTree>
20+
</TreeNodesModel>
21+
</root>

‎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+
"Admittance Demo",
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>

0 commit comments

Comments
 (0)