Skip to content

Commit 49f487a

Browse files
dyackzandyackzan
committed
Add trajectory stitching demo objective
Former-commit-id: 80d180d
1 parent ac01d2f commit 49f487a

File tree

3 files changed

+199
-0
lines changed

3 files changed

+199
-0
lines changed

‎src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/config/config.yaml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -161,6 +161,7 @@ objectives:
161161
- "moveit_studio::behaviors::MTCCoreBehaviorsLoader"
162162
- "moveit_studio::behaviors::ServoBehaviorsLoader"
163163
- "moveit_studio::behaviors::VisionBehaviorsLoader"
164+
- "moveit_studio::behaviors::ConverterBehaviorsLoader"
164165
# Specify source folder for objectives
165166
# [Required]
166167
objective_library_paths:

‎src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/admittance_parameters_no_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: 1000
7+
y: 1000
8+
z: 650
9+
damping_ratio_rotation:
10+
max: 1000
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: 40
23+
y: 40
24+
z: 10
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: false
36+
y: false
37+
z: false
38+
stiffness_position:
39+
max: 1000
40+
min: 0
41+
x: 120
42+
y: 120
43+
z: 300
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: grasp_link
52+
controller_name: ''
53+
end_effector_frame: grasp_link

‎src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/demo_trajectory_stitching.xml‎

Lines changed: 145 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,145 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Demo Trajectory Stitching">
3+
<!--//////////-->
4+
<BehaviorTree
5+
ID="Demo Trajectory Stitching"
6+
_description="Take a pointcloud snapshot of the scene with a depth camera"
7+
_favorite="false"
8+
_subtreeOnly="false"
9+
>
10+
<Control ID="Sequence" name="TopLevelSequence">
11+
<SubTree
12+
ID="Move to Waypoint"
13+
_collapsed="true"
14+
joint_group_name="manipulator"
15+
planner_interface="moveit_default"
16+
controller_names="/joint_trajectory_controller"
17+
waypoint_name="Home"
18+
acceleration_scale_factor="1.0"
19+
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
20+
keep_orientation="false"
21+
keep_orientation_tolerance="0.05"
22+
link_padding="0.01"
23+
velocity_scale_factor="1.0"
24+
/>
25+
<Action
26+
ID="GetCurrentPlanningScene"
27+
planning_scene_msg="{planning_scene}"
28+
/>
29+
<Action
30+
ID="GetRobotJointState"
31+
planning_group_name="manipulator"
32+
planning_scene="{planning_scene}"
33+
joint_state="{start_joint_state}"
34+
/>
35+
<Action
36+
ID="UnpackJointStateMessage"
37+
joint_state="{start_joint_state}"
38+
effort="{start_effort}"
39+
header="{start_header}"
40+
name_="{joint_names}"
41+
position="{start_position}"
42+
velocity="{start_velocity}"
43+
/>
44+
<Action
45+
ID="CreateJointState"
46+
joint_state_msg="{start_state}"
47+
positions="{start_position}"
48+
joint_names="{joint_names}"
49+
velocities="{start_velocity}"
50+
/>
51+
<Action
52+
ID="CreateJointState"
53+
positions="0.5;0.51;-3.025;-2.31;0.2;1.159;1.37"
54+
joint_names="{joint_names}"
55+
joint_state_msg="{target_state}"
56+
velocities="0.0;0.0;0.0;0.0;0.0;0.0;0.0"
57+
/>
58+
<Action
59+
ID="GetRobotJointState"
60+
planning_group_name="manipulator"
61+
planning_scene="{planning_scene}"
62+
joint_state="{joint_state}"
63+
/>
64+
<Action
65+
ID="CreateJointState"
66+
positions="-0.5;-0.51;-3.025;-2.31;0.2;1.159;1.37"
67+
joint_names="{joint_names}"
68+
joint_state_msg="{stitch_target_state}"
69+
velocities="0.0;0.0;0.0;0.0;0.0;0.0;0.0"
70+
/>
71+
<Action
72+
ID="GeneratePointToPointTrajectory"
73+
jerk_scale_factor="0.5"
74+
velocity_scale_factor="1.0"
75+
acceleration_scale_factor="1.0"
76+
planning_group_name="manipulator"
77+
start_state="{start_state}"
78+
start_time="0.000000"
79+
target_state="{target_state}"
80+
trajectory_sampling_rate="100"
81+
joint_trajectory_msg="{joint_trajectory_msg}"
82+
/>
83+
<Action
84+
ID="GetTrajectoryStateAtTime"
85+
time_from_reference="0.8"
86+
joint_state="{stitch_state}"
87+
from_start="true"
88+
joint_trajectory_msg="{joint_trajectory_msg}"
89+
/>
90+
<Action
91+
ID="GeneratePointToPointTrajectory"
92+
start_state="{stitch_state}"
93+
target_state="{stitch_target_state}"
94+
joint_trajectory_msg="{stitch_joint_trajectory_msg}"
95+
velocity_scale_factor="1.0"
96+
start_time="0.8"
97+
acceleration_scale_factor="1.0"
98+
jerk_scale_factor="0.5"
99+
planning_group_name="manipulator"
100+
trajectory_sampling_rate="100"
101+
/>
102+
<Action
103+
ID="SetAdmittanceParameters"
104+
config_file_name="admittance_parameters_no_admittance.yaml"
105+
admittance_parameters_msg="{admittance_parameters_msg}"
106+
/>
107+
<Action
108+
ID="ActivateControllers"
109+
controller_names="/joint_trajectory_admittance_controller"
110+
/>
111+
<Control ID="Parallel" failure_count="2" success_count="1">
112+
<Action
113+
ID="ExecuteTrajectoryWithAdmittance"
114+
path_position_tolerance="0.2"
115+
absolute_force_torque_threshold="45;45;45;10;10;10"
116+
admittance_parameters_msg="{admittance_parameters_msg}"
117+
controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory"
118+
goal_duration_tolerance="-1.000000"
119+
goal_position_tolerance="0.001000"
120+
joint_trajectory_msg="{joint_trajectory_msg}"
121+
/>
122+
<Decorator ID="Delay" delay_msec="400">
123+
<Action
124+
ID="ExecuteTrajectoryWithAdmittance"
125+
joint_trajectory_msg="{stitch_joint_trajectory_msg}"
126+
absolute_force_torque_threshold="45;45;45;10;10;10"
127+
goal_position_tolerance="0.001000"
128+
path_position_tolerance="0.2"
129+
admittance_parameters_msg="{admittance_parameters_msg}"
130+
controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory"
131+
goal_duration_tolerance="-1.000000"
132+
/>
133+
</Decorator>
134+
</Control>
135+
</Control>
136+
</BehaviorTree>
137+
<TreeNodesModel>
138+
<SubTree ID="Demo Trajectory Stitching">
139+
<MetadataFields>
140+
<Metadata runnable="true" />
141+
<Metadata subcategory="Application - Advanced Examples" />
142+
</MetadataFields>
143+
</SubTree>
144+
</TreeNodesModel>
145+
</root>

0 commit comments

Comments
 (0)