Skip to content

Commit 09789a5

Browse files
mariopratsmarioprats
committed
fixes to config and start of sequential sort objective
1 parent ef96241 commit 09789a5

20 files changed

+950
-25
lines changed

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

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -131,6 +131,10 @@ ros2_control:
131131
controllers_active_at_startup:
132132
- "joint_state_broadcaster"
133133
- "joint_trajectory_controller"
134+
- "right_franka_gripper_controller1"
135+
- "left_franka_gripper_controller1"
136+
- "right_franka_gripper_controller2"
137+
- "left_franka_gripper_controller2"
134138
# Load but do not start these controllers so they can be activated later if needed.
135139
# [Optional, default=[]]
136140
controllers_inactive_at_startup:

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

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,46 @@ controller_manager:
1515
type: velocity_force_controller/VelocityForceController
1616
right_velocity_force_controller:
1717
type: velocity_force_controller/VelocityForceController
18+
left_franka_gripper_controller1:
19+
type: position_controllers/GripperActionController
20+
left_franka_gripper_controller2:
21+
type: position_controllers/GripperActionController
22+
right_franka_gripper_controller1:
23+
type: position_controllers/GripperActionController
24+
right_franka_gripper_controller2:
25+
type: position_controllers/GripperActionController
26+
27+
left_franka_gripper_controller1:
28+
ros__parameters:
29+
default: true
30+
joint: left_fr3_finger_joint1
31+
allow_stalling: true
32+
stall_timeout: 0.1
33+
goal_tolerance: 0.005
34+
35+
left_franka_gripper_controller2:
36+
ros__parameters:
37+
default: true
38+
joint: left_fr3_finger_joint2
39+
allow_stalling: true
40+
stall_timeout: 0.1
41+
goal_tolerance: 0.005
42+
43+
right_franka_gripper_controller1:
44+
ros__parameters:
45+
default: true
46+
joint: right_fr3_finger_joint1
47+
allow_stalling: true
48+
stall_timeout: 0.1
49+
goal_tolerance: 0.005
50+
51+
right_franka_gripper_controller2:
52+
ros__parameters:
53+
default: true
54+
joint: right_fr3_finger_joint2
55+
allow_stalling: true
56+
stall_timeout: 0.1
57+
goal_tolerance: 0.005
1858

1959
joint_trajectory_controller:
2060
ros__parameters:
@@ -23,6 +63,7 @@ joint_trajectory_controller:
2363
state_interfaces:
2464
- position
2565
- velocity
66+
allow_partial_joints_goal: true
2667
joints:
2768
- left_fr3_joint1
2869
- left_fr3_joint2

‎src/moveit_pro_franka_configs/franka_dual_arm_sim/config/moveit/franka.srdf‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@
1010
<link name="right_fr3_leftfinger"/>
1111
<link name="right_fr3_rightfinger"/>
1212
<link name="right_fr3_hand_tcp"/>
13-
<!--<link name="right_grasp_link"/>-->
13+
<link name="right_grasp_link"/>
1414
<joint name="right_fr3_finger_joint1"/>
1515
<passive_joint name="right_fr3_finger_joint2"/>
1616
</group>
@@ -26,7 +26,7 @@
2626
<link name="left_fr3_leftfinger"/>
2727
<link name="left_fr3_rightfinger"/>
2828
<link name="left_fr3_hand_tcp"/>
29-
<!--<link name="left_grasp_link"/>-->
29+
<link name="left_grasp_link"/>
3030
<joint name="left_fr3_finger_joint1"/>
3131
<passive_joint name="left_fr3_finger_joint2"/>
3232
</group>

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

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,7 @@
3434
<joint name="right_grasp_link_joint" type="fixed">
3535
<parent link="right_fr3_link8"/>
3636
<child link = "right_grasp_link"/>
37-
<origin xyz="0 0 0" rpy="0 0 -1.5708"/>
37+
<origin xyz="0 0 0.15" rpy="0 0 -1.5708"/>
3838
</joint>
3939

4040
<joint name="left_mount_joint" type="fixed">
@@ -64,9 +64,9 @@
6464
<!-- This link aligns the axes of the grasp frame to our required convention for motion planning tasks. -->
6565
<link name="left_grasp_link" />
6666
<joint name="left_grasp_link_joint" type="fixed">
67-
<parent link="world"/>
67+
<parent link="left_fr3_link8"/>
6868
<child link = "left_grasp_link"/>
69-
<origin xyz="0 0 0" rpy="0 0 -1.5708"/>
69+
<origin xyz="0 0 0.15" rpy="0 0 -1.5708"/>
7070
</joint>
7171

7272
<ros2_control name="franka_mujoco_simulation" type="system">

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -45,12 +45,12 @@
4545
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
4646
<site
4747
name="scene_camera_optical_frame"
48-
pos="0.4 0.0 1.5"
48+
pos="0.7 0.0 0.65"
4949
xyaxes="0 -1 0 -1 0 0"
5050
/>
5151
<camera
5252
name="scene_camera"
53-
pos="0.4 0.0 1.5"
53+
pos="0.7 0.0 0.65"
5454
fovy="58"
5555
mode="fixed"
5656
resolution="640 480"

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

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Close Left Gripper">
3+
<BehaviorTree
4+
ID="Close Left Gripper"
5+
_description="Close the left end_effector"
6+
>
7+
<Control ID="Parallel" failure_count="1" success_count="2">
8+
<Action
9+
ID="MoveGripperAction"
10+
gripper_command_action_name="/left_franka_gripper_controller1/gripper_cmd"
11+
position="0"
12+
timeout="10.000000"
13+
/>
14+
<Action
15+
ID="MoveGripperAction"
16+
gripper_command_action_name="/left_franka_gripper_controller2/gripper_cmd"
17+
position="0"
18+
timeout="10.000000"
19+
/>
20+
</Control>
21+
</BehaviorTree>
22+
<TreeNodesModel>
23+
<SubTree ID="Close Left Gripper">
24+
<MetadataFields>
25+
<Metadata subcategory="Grasping" />
26+
<Metadata runnable="true" />
27+
</MetadataFields>
28+
</SubTree>
29+
</TreeNodesModel>
30+
</root>

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

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Close Right Gripper">
3+
<BehaviorTree
4+
ID="Close Right Gripper"
5+
_description="Close the right end_effector"
6+
>
7+
<Control ID="Parallel" failure_count="1" success_count="2">
8+
<Action
9+
ID="MoveGripperAction"
10+
gripper_command_action_name="/right_franka_gripper_controller1/gripper_cmd"
11+
position="0"
12+
timeout="10.000000"
13+
/>
14+
<Action
15+
ID="MoveGripperAction"
16+
gripper_command_action_name="/right_franka_gripper_controller2/gripper_cmd"
17+
position="0"
18+
timeout="10.000000"
19+
/>
20+
</Control>
21+
</BehaviorTree>
22+
<TreeNodesModel>
23+
<SubTree ID="Close Right Gripper">
24+
<MetadataFields>
25+
<Metadata subcategory="Grasping" />
26+
<Metadata runnable="true" />
27+
</MetadataFields>
28+
</SubTree>
29+
</TreeNodesModel>
30+
</root>

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

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Coin Flip Find">
3+
<!--//////////-->
4+
<BehaviorTree ID="Coin Flip Find" _description="" _favorite="false">
5+
<Control ID="Fallback">
6+
<Control ID="Sequence">
7+
<Action ID="BiasedCoinFlip" success_probability="0.500000" />
8+
<SubTree
9+
ID="Find Red Block"
10+
_collapsed="true"
11+
object_centroid="{object_centroid}"
12+
/>
13+
</Control>
14+
<SubTree
15+
ID="Find Green Block"
16+
_collapsed="true"
17+
object_centroid="{object_centroid}"
18+
/>
19+
</Control>
20+
</BehaviorTree>
21+
<TreeNodesModel>
22+
<SubTree ID="Coin Flip Find">
23+
<MetadataFields>
24+
<Metadata runnable="true" />
25+
<Metadata subcategory="Perception - ML" />
26+
</MetadataFields>
27+
<inout_port name="object_centroid" default="{object_centroid}" />
28+
</SubTree>
29+
</TreeNodesModel>
30+
</root>

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

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
<?xml version="1.0" encoding="utf-8" ?>
2+
<root
3+
BTCPP_format="4"
4+
main_tree_to_execute="Create Point Cloud Vector From Masks"
5+
>
6+
<!--//////////-->
7+
<BehaviorTree
8+
ID="Create Point Cloud Vector From Masks"
9+
_description="Create point cloud vector From masks"
10+
_favorite="true"
11+
masks3d="/wrist_camera/camera_info"
12+
point_cloud="/wrist_camera/camera_info"
13+
>
14+
<Control ID="Sequence">
15+
<Action ID="ResetPoseStampedVector" vector="{pose_stamped_vector}" />
16+
<Decorator
17+
ID="ForEach"
18+
vector_in="{masks3d}"
19+
out="{mask3d}"
20+
index="{index}"
21+
>
22+
<Control ID="Sequence">
23+
<Action
24+
ID="GetPointCloudFromMask3D"
25+
point_cloud="{point_cloud}"
26+
mask3d="{mask3d}"
27+
point_cloud_fragment="{point_cloud_fragment}"
28+
/>
29+
<Action
30+
ID="AddPointCloudToVector"
31+
point_cloud="{point_cloud_fragment}"
32+
point_cloud_vector="{point_cloud_vector}"
33+
/>
34+
</Control>
35+
</Decorator>
36+
</Control>
37+
</BehaviorTree>
38+
<TreeNodesModel>
39+
<SubTree ID="Create Point Cloud Vector From Masks">
40+
<input_port name="masks3d" default="/wrist_camera/camera_info" />
41+
<input_port name="point_cloud" default="/wrist_camera/camera_info" />
42+
<output_port name="point_cloud_vector" default="{point_cloud_vector}" />
43+
<MetadataFields>
44+
<Metadata runnable="false" />
45+
<Metadata subcategory="Perception - ML" />
46+
</MetadataFields>
47+
</SubTree>
48+
</TreeNodesModel>
49+
</root>

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

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Find Green Block">
3+
<!--//////////-->
4+
<BehaviorTree ID="Find Green Block" _description="" _favorite="false">
5+
<Control ID="Sequence" name="TopLevelSequence">
6+
<SubTree
7+
ID="Find with prompt"
8+
_collapsed="true"
9+
prompt="a green block"
10+
object_centroid="{object_centroid}"
11+
/>
12+
</Control>
13+
</BehaviorTree>
14+
<TreeNodesModel>
15+
<SubTree ID="Find Green Block">
16+
<MetadataFields>
17+
<Metadata runnable="true" />
18+
<Metadata subcategory="Perception - ML" />
19+
</MetadataFields>
20+
<inout_port name="object_centroid" default="{object_centroid}" />
21+
</SubTree>
22+
</TreeNodesModel>
23+
</root>

0 commit comments

Comments
 (0)