Skip to content

Commit caf3629

Browse files
kineticsystemkineticsystem
authored
Merge pull request #407 from PickNikRobotics/pr-kitchen-scene
Kitchen scene
2 parents 515bc0f + 99705fc commit caf3629

File tree

139 files changed

+27046
-124
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

139 files changed

+27046
-124
lines changed

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

Lines changed: 0 additions & 58 deletions
This file was deleted.

‎src/moveit_pro_franka_configs/franka_arm_sim/objectives/moveitpro_loves_franka.yaml‎

Lines changed: 0 additions & 3 deletions
This file was deleted.

‎src/moveit_pro_franka_configs/franka_arm_sim/waypoints/waypoints.yaml‎

Lines changed: 0 additions & 13 deletions
This file was deleted.

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

Lines changed: 15 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -1,23 +1,28 @@
11
<?xml version="1.0" encoding="UTF-8"?>
22
<robot name="franka">
3+
34
<!-- Planning groups -->
4-
<group name="arm">
5-
<chain base_link="fr3_link0" tip_link="fr3_link8"/>
6-
</group>
7-
<group name="hand">
8-
<link name="fr3_hand"/>
5+
6+
<!-- All gripper links, excluding arm. -->
7+
<group name="gripper">
8+
<link name="fr3_hand_tcp"/> <!-- root of gripper -->
99
<link name="fr3_leftfinger"/>
1010
<link name="fr3_rightfinger"/>
11-
<link name="fr3_hand_tcp"/>
1211
<link name="grasp_link"/>
1312
<joint name="fr3_finger_joint1"/>
14-
<passive_joint name="$fr3_finger_joint2"/>
13+
<passive_joint name="fr3_finger_joint2"/>
1514
</group>
15+
16+
<!-- All robot links, including gripper. -->
1617
<group name="manipulator">
1718
<chain base_link="fr3_link0" tip_link="grasp_link"/>
1819
</group>
20+
21+
<!-- End effectors -->
22+
<end_effector name="hand_tcp" parent_link="grasp_link" group="gripper" parent_group="manipulator"/>
23+
1924
<!-- Arm group states -->
20-
<group_state name="ready" group="arm">
25+
<group_state name="ready" group="manipulator">
2126
<joint name="fr3_joint1" value="0"/>
2227
<joint name="fr3_joint2" value="-0.7853"/>
2328
<joint name="fr3_joint3" value="0"/>
@@ -26,16 +31,14 @@
2631
<joint name="fr3_joint6" value="1.5707"/>
2732
<joint name="fr3_joint7" value="0.7853"/>
2833
</group_state>
29-
<group_state name="open" group="hand">
34+
<group_state name="open" group="gripper">
3035
<joint name="fr3_finger_joint1" value="0.035"/>
3136
<joint name="fr3_finger_joint2" value="0.035"/>
3237
</group_state>
33-
<group_state name="close" group="hand">
38+
<group_state name="close" group="gripper">
3439
<joint name="fr3_finger_joint1" value="0"/>
3540
<joint name="fr3_finger_joint2" value="0"/>
3641
</group_state>
37-
<!-- End effectors -->
38-
<end_effector name="hand_tcp" parent_link="fr3_link8" group="hand" parent_group="arm"/>
3942

4043
<!-- Arm collisions -->
4144
<virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="fr3_link0"/>

‎src/moveit_pro_franka_configs/franka_base_config/objectives/segment_image_from_no_negative_text_prompt_subtree.xml‎

Lines changed: 64 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root
3+
BTCPP_format="4"
4+
main_tree_to_execute="Segment Image from No Negative Text Prompt Subtree"
5+
>
6+
<!--//////////-->
7+
<BehaviorTree
8+
ID="Segment Image from No Negative Text Prompt Subtree"
9+
_description="Run text based segmentation and visualize the masks."
10+
_favorite="true"
11+
>
12+
<Control ID="Sequence">
13+
<Action
14+
ID="GetImage"
15+
timeout_sec="5.000000"
16+
topic_name="{image_topic_name}"
17+
message_out="{image}"
18+
/>
19+
<Action
20+
ID="GetMasks2DFromTextQuery"
21+
image="{image}"
22+
masks2d="{masks2d}"
23+
clipseg_model_path="{clipseg_model_path}"
24+
clip_model_path="{clip_model_path}"
25+
model_package="{model_package}"
26+
prompts="{prompts}"
27+
erosion_size="{erosion_size}"
28+
threshold="{threshold}"
29+
/>
30+
<Action
31+
ID="PublishMask2D"
32+
image="{image}"
33+
masks="{masks2d}"
34+
masks_visualization_topic="{masks_visualization_topic}"
35+
opacity="0.500000"
36+
bounding_box_detection_class="{prompts}"
37+
/>
38+
</Control>
39+
</BehaviorTree>
40+
<TreeNodesModel>
41+
<SubTree ID="Segment Image from No Negative Text Prompt Subtree">
42+
<MetadataFields>
43+
<Metadata runnable="false" />
44+
<Metadata subcategory="Perception - ML" />
45+
</MetadataFields>
46+
<MetadataFields>
47+
<Metadata runnable="false" />
48+
<Metadata subcategory="Perception - ML" />
49+
</MetadataFields>
50+
<input_port name="clip_model_path" default="{clip_model_path}" />
51+
<input_port name="clipseg_model_path" default="{clipseg_model_path}" />
52+
<input_port name="erosion_size" default="{erosion_size}" />
53+
<input_port name="image_topic_name" default="{image_topic_name}" />
54+
<input_port
55+
name="masks_visualization_topic"
56+
default="{masks_visualization_topic}"
57+
/>
58+
<output_port name="masks2d" default="{masks2d}" />
59+
<input_port name="model_package" default="{model_package}" />
60+
<input_port name="prompts" default="{prompts}" />
61+
<input_port name="threshold" default="{threshold}" />
62+
</SubTree>
63+
</TreeNodesModel>
64+
</root>

src/moveit_pro_franka_configs/franka_arm_sim/CMakeLists.txt renamed to src/moveit_pro_franka_configs/kitchen_sim/CMakeLists.txt

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,13 @@
11
cmake_minimum_required(VERSION 3.22)
2-
project(franka_arm_sim)
2+
project(kitchen_sim)
33
find_package(ament_cmake REQUIRED)
44

55
install(
66
DIRECTORY
77
config
88
description
99
launch
10+
models
1011
objectives
1112
waypoints
1213
DESTINATION

src/moveit_pro_franka_configs/franka_arm_sim/README.md renamed to src/moveit_pro_franka_configs/kitchen_sim/README.md

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
# franka_arm_sim
1+
# kitchen_sim
22

33
A MoveIt Pro base configuration for the Franka arm.
44
For detailed documentation see: [MoveIt Pro Documentation](https://docs.picknik.ai/).

src/moveit_pro_franka_configs/franka_arm_sim/config/config.yaml renamed to src/moveit_pro_franka_configs/kitchen_sim/config/config.yaml

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ hardware:
2424

2525
robot_description:
2626
urdf:
27-
package: "franka_arm_sim"
27+
package: "kitchen_sim"
2828
path: "description/franka.urdf.xacro"
2929
srdf:
3030
package: "franka_base_config"
@@ -34,6 +34,7 @@ hardware:
3434
# [Optional]
3535
urdf_params:
3636
- mujoco_model: "description/mujoco/scene.xml"
37+
- mujoco_viewer: false
3738
simulated_hardware_launch_file:
3839
package: "moveit_studio_agent"
3940
path: "launch/blank.launch.py"
@@ -65,24 +66,25 @@ objectives:
6566
package_name: "moveit_pro_objectives"
6667
relative_path: "objectives/perception"
6768
sim_objectives:
68-
package_name: "franka_arm_sim"
69+
package_name: "kitchen_sim"
6970
relative_path: "objectives"
7071
# Specify the location of the saved waypoints file.
7172
# [Required]
7273
waypoints_file:
73-
package_name: "franka_arm_sim"
74+
package_name: "kitchen_sim"
7475
relative_path: "waypoints/waypoints.yaml"
7576
# Configuration for launching ros2_control processes.
7677
# [Required, if using ros2_control]
7778
ros2_control:
7879
config:
79-
package: "franka_arm_sim"
80+
package: "kitchen_sim"
8081
path: "config/control/franka_ros2_control.yaml"
8182
# MoveIt Pro will load and activate these controllers at start up to ensure they are available.
8283
# If not specified, it is up to the user to ensure the appropriate controllers are active and available
8384
# for running the application.
8485
# [Optional, default=[]]
8586
controllers_active_at_startup:
87+
- "franka_gripper_controller"
8688
- "joint_state_broadcaster"
8789
- "joint_trajectory_controller"
8890
# Load but do not start these controllers so they can be activated later if needed.

src/moveit_pro_franka_configs/franka_arm_sim/config/control/franka_ros2_control.yaml renamed to src/moveit_pro_franka_configs/kitchen_sim/config/control/franka_ros2_control.yaml

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,8 @@ controller_manager:
99
type: joint_trajectory_controller/JointTrajectoryController
1010
velocity_force_controller:
1111
type: velocity_force_controller/VelocityForceController
12-
12+
franka_gripper_controller:
13+
type: position_controllers/GripperActionController
1314
joint_trajectory_controller:
1415
ros__parameters:
1516
command_interfaces:
@@ -34,6 +35,14 @@ joint_trajectory_controller:
3435
fr3_joint6: { p: 150., d: 10., i: 0., i_clamp: 1. }
3536
fr3_joint7: { p: 50., d: 5., i: 0., i_clamp: 1. }
3637

38+
franka_gripper_controller:
39+
ros__parameters:
40+
default: true
41+
joint: fr3_finger_joint1
42+
allow_stalling: true
43+
stall_timeout: 0.1
44+
goal_tolerance: 0.005
45+
3746
servo_controller:
3847
ros__parameters:
3948
command_interfaces:

src/moveit_pro_franka_configs/franka_arm_sim/description/franka.urdf.xacro renamed to src/moveit_pro_franka_configs/kitchen_sim/description/franka.urdf.xacro

Lines changed: 54 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -22,9 +22,49 @@
2222
gazebo_effort="false"
2323
no_prefix="false"
2424
arm_prefix=""
25-
connected_to= "world">
25+
connected_to= "world"
26+
xyz="0 0 0.9">
2627
</xacro:franka_robot>
2728

29+
<!-- Pedestal the robot stands on -->
30+
<link name="pedestal">
31+
<!-- simple mass/inertia for a solid cylinder -->
32+
<inertial>
33+
<mass value="30.0"/> <!-- adjust as needed -->
34+
<!-- Inertia for solid cylinder: Ixx=Iyy=(1/12)m(3r^2+h^2), Izz=(1/2)m r^2 -->
35+
<!-- For r=0.2 m, h=0.9 m, m=30 kg: Ixx=Iyy≈2.325, Izz≈0.6 -->
36+
<origin xyz="0 0 0.45"/> <!-- center of mass at half height -->
37+
<inertia ixx="2.325" ixy="0.0" ixz="0.0"
38+
iyy="2.325" iyz="0.0"
39+
izz="0.6"/>
40+
</inertial>
41+
42+
<visual>
43+
<!-- place the cylinder so its base sits at z=0 and top at z=0.9 -->
44+
<origin xyz="0 0 0.45"/>
45+
<geometry>
46+
<cylinder radius="0.2" length="0.9"/>
47+
</geometry>
48+
<material name="pedestal_grey">
49+
<color rgba="0.7 0.7 0.7 1.0"/>
50+
</material>
51+
</visual>
52+
53+
<collision>
54+
<origin xyz="0 0 0.45"/>
55+
<geometry>
56+
<cylinder radius="0.2" length="0.9"/>
57+
</geometry>
58+
</collision>
59+
</link>
60+
61+
<!-- Fix pedestal to the world at the origin -->
62+
<joint name="pedestal_to_world" type="fixed">
63+
<parent link="world"/>
64+
<child link="pedestal"/>
65+
<origin xyz="0 0 0" rpy="0 0 0"/>
66+
</joint>
67+
2868
<!-- This link aligns the axes of the grasp frame to our required convention for motion planning tasks. -->
2969
<link name="grasp_link" />
3070
<joint name="grasp_link_joint" type="fixed">
@@ -111,12 +151,24 @@
111151
<state_interface name="velocity"/>
112152
<state_interface name="effort"/>
113153
</joint>
154+
<joint name="fr3_finger_joint1">
155+
<command_interface name="position">
156+
<param name="min">-0.0</param>
157+
<param name="max">0.04</param>
158+
</command_interface>
159+
<state_interface name="position">
160+
<param name="initial_value">0.035</param>
161+
</state_interface>
162+
<state_interface name="velocity"/>
163+
<state_interface name="effort"/>
164+
</joint>
114165
<hardware>
115166
<plugin>picknik_mujoco_ros/MujocoSystem</plugin>
116167
<param name="mujoco_model">$(arg mujoco_model)</param>
117-
<param name="mujoco_model_package">franka_arm_sim</param>
168+
<param name="mujoco_model_package">kitchen_sim</param>
118169
<param name="render_publish_rate">10</param>
119170
<param name="tf_publish_rate">60</param>
171+
<param name="mujoco_viewer">$(arg mujoco_viewer)</param>
120172
</hardware>
121173
</ros2_control>
122174
</robot>

0 commit comments

Comments
 (0)