Skip to content

Commit 9c33742

Browse files
lukestrohlukestroh
committed
added dovetail mount, modified mock pruner
1 parent 4e5b1e2 commit 9c33742

File tree

12 files changed

+398
-76
lines changed

12 files changed

+398
-76
lines changed

‎main.py‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@
1313

1414

1515
def main():
16-
pbutils = PyBUtils(renders=False)
16+
pbutils = PyBUtils(renders=True)
1717

1818
# Init sensors
1919
sensor_config = {
@@ -30,7 +30,7 @@ def main():
3030
)
3131

3232

33-
penv.activate_shape(shape="cylinder", radius=2 * 0.0254, height=2.0, orientation=[0, np.pi / 2, 0])
33+
# penv.activate_shape(shape="cylinder", radius=2 * 0.0254, height=2.0, orientation=[0, np.pi / 2, 0])
3434
# penv.load_tree(
3535
# pbutils=pbutils,
3636
# scale=1.0,
@@ -47,6 +47,8 @@ def main():
4747
for i in range(100):
4848
pbutils.pbclient.stepSimulation()
4949
time.sleep(0.1)
50+
51+
time.sleep(30)
5052

5153
# Simulation loop
5254
while True:

‎pybullet_tree_sim/meshes/end_effectors/mock_pruner/MockPruner.STL‎

338 KB
Binary file not shown.

pybullet_tree_sim/meshes/end_effectors/mock_pruner/MockPrunerASSEMBLED.STL renamed to pybullet_tree_sim/meshes/end_effectors/mock_pruner/MockPrunerASSEMBLEDOLD1.STL

File renamed without changes.

‎pybullet_tree_sim/pruning_environment.py‎

Lines changed: 1 addition & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -182,14 +182,6 @@ def load_robot(self, type: str, robot_pos: ArrayLike, robot_orientation: ArrayLi
182182
type = type.strip().lower()
183183
if type == "ur5":
184184
log.info("Loading UR5 Robot")
185-
# robot = UR5(
186-
# con=self.pbutils.pbclient,
187-
# robot_urdf_path=ROBOT_URDF_PATH,
188-
# pos=robot_pos,
189-
# orientation=robot_orientation,
190-
# randomize_pose=randomize_pose,
191-
# verbose=self.verbose,
192-
# )
193185
robot = Robot(pbclient=self.pbutils.pbclient)
194186

195187
else:
@@ -487,7 +479,7 @@ def is_reachable(self, vertex: Tuple[np.ndarray], base_xyz: np.ndarray) -> bool:
487479
if dist >= 0.98: # TODO: is this for the UR5? Should it be from a parameter file?
488480
return False
489481

490-
j_angles = self.ur5.calculate_ik(vertex[0], None)
482+
j_angles = self.robot.calculate_ik(vertex[0], None)
491483
# env.ur5.set_joint_angles(j_angles)
492484
# for _ in range(100):
493485
# pyb.con.stepSimulation()

‎pybullet_tree_sim/robot.py‎

Lines changed: 7 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -106,40 +106,29 @@ def _generate_robot_urdf(self) -> None:
106106
# Add each robot part's config to the robot_conf
107107
self.robot_conf.update(yutils.load_yaml(os.path.join(self._robot_configs_path, f"{robot_part}.yaml")))
108108

109-
import pprint as pp
110-
pp.pprint(self.robot_conf)
109+
111110

112111
# Generate URDF from mappings
113112
robot_urdf = xutils.load_urdf_from_xacro(
114113
xacro_path=self._robot_xacro_path,
115114
mappings=self.robot_conf # for some reason, this adds in the rest of the args from the xacro.
116115
)
117-
import pprint as pp
118-
# pp.pprint(robot_urdf)
119-
120116
# UR_description uses filename="package://<>" for meshes, and this doesn't work with pybullet
121-
try:
122-
if self.robot_conf['arm_type'].startswith('ur'):
117+
for i, robot_part in enumerate(self.robot_conf["robot_stack"]):
118+
if robot_part.startswith('ur'):
123119
ur_absolute_mesh_path = '/opt/ros/humble/share/ur_description/meshes'
124120
robot_urdf = robot_urdf.toprettyxml().replace(
125121
f'filename="package://ur_description/meshes',
126122
f'filename="{ur_absolute_mesh_path}'
127123
)
128-
else:
129-
robot_urdf = robot_urdf.toprettyxml()
130-
except KeyError:
131-
pass
132-
robot_urdf = robot_urdf.toprettyxml()
124+
else:
125+
# robot_urdf = robot_urdf.toprettyxml()
126+
...
127+
133128
# Save the generated URDF
134129
self.robot_urdf_path = os.path.join(self._urdf_tmp_path, "robot.urdf")
135130
xutils.save_urdf(robot_urdf, urdf_path=self.robot_urdf_path)
136131

137-
import pprint as pp
138-
pp.pprint(self.robot_conf)
139-
# pp.pprint(robot_urdf)
140-
log.warn("hello")
141-
import sys
142-
sys.exit(0)
143132
return
144133

145134
def _assign_control_joints(self) -> None:

‎pybullet_tree_sim/urdf/base_mounts/cart/macro/cart_macro.urdf.xacro‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,7 @@
3131
<material name="LightGrey"/>
3232
</visual>
3333
<collision>
34-
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.3786"/>
34+
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
3535
<geometry>
3636
<box size="${cart_collision_box_x} ${cart_collision_box_y} ${cart_collision_box_z}"/>
3737
</geometry>

‎pybullet_tree_sim/urdf/end_effectors/dovetail_mount/dovetail_mount.urdf.xacro‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
<?xml version="1.0"?>
2+
<robot xmlns:xacro="https://ros.org/wiki/xacro" name="mock_pruner">
3+
4+
<!-- World link -->
5+
<link name="world" />
6+
7+
<!-- parent_link dovetail_male_mount_prefix mesh_base_path -->
8+
<xacro:arg name="dovetail_mount_parent_link" default="world" />
9+
<xacro:arg name="dovetail_male_mount_prefix" default="dovetail_male_mount__" />
10+
<xacro:arg name="dovetail_female_mount_prefix" default="dovetail_female_mount__" />
11+
<xacro:arg name="mesh_base_path" default="" />
12+
13+
<!-- dovetail_mount macro -->
14+
<xacro:include filename="./macro/dovetail_mount_macro.urdf.xacro" />
15+
16+
<xacro:dovetail_mount_macro
17+
parent_link="$(arg dovetail_mount_parent_link)"
18+
dovetail_male_mount_prefix="$(arg dovetail_male_mount_prefix)"
19+
dovetail_female_mount_prefix="$(arg dovetail_female_mount_prefix)"
20+
mesh_base_path="$(arg mesh_base_path)"
21+
/>
22+
23+
</robot>

‎pybullet_tree_sim/urdf/end_effectors/dovetail_mount/macro/dovetail_mount_macro.urdf.xacro‎

Lines changed: 121 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,121 @@
1+
<?xml version="1.0"?>
2+
<robot xmlns:xacro="https://ros.org/wiki/xacro" name="dovetail_mount">
3+
4+
<xacro:property name="dovetail_male_tool0_height" value="0.008"/>
5+
<xacro:property name="dovetail_female_tool0_height" value="0.023"/>
6+
7+
8+
<xacro:macro name="dovetail_male_mount_macro" params="parent_link dovetail_male_mount_prefix mesh_base_path">
9+
<!-- ==================== -->
10+
<!-- Dovetail Male Mount -->
11+
<!-- ==================== -->
12+
<material name="lavender">
13+
<color rgba="0.749 0.580 0.894 1"/>
14+
</material>
15+
<xacro:property name="dovetail_male_mass" value="0.025"/>
16+
17+
<!-- links -->
18+
<link name="${dovetail_male_mount_prefix}base"/>
19+
<link name='${dovetail_male_mount_prefix}body'>
20+
<visual>
21+
<material name="lavender"/>
22+
<origin xyz="0 0 0" rpy="0 0 0"/>
23+
<geometry>
24+
<mesh filename="${mesh_base_path}/end_effectors/dovetail/DovetailMale.STL"/>
25+
</geometry>
26+
</visual>
27+
<collision>
28+
<origin xyz="0 0 0" rpy="0 0 0"/>
29+
<geometry>
30+
<mesh filename="${mesh_base_path}/end_effectors/dovetail/DovetailMale.STL"/>
31+
</geometry>
32+
</collision>
33+
<inertial>
34+
<mass value="${dovetail_male_mass}"/>
35+
<origin xyz="0.0 0.0 0.0"/>
36+
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
37+
</inertial>
38+
</link>
39+
<link name="${dovetail_male_mount_prefix}tool0"/>
40+
<!-- joints -->
41+
<joint name="${parent_link}--${dovetail_male_mount_prefix}base" type="fixed">
42+
<origin xyz="0 0 0" rpy="0 0 0"/>
43+
<parent link="${parent_link}"/>
44+
<child link="${dovetail_male_mount_prefix}base"/>
45+
</joint>
46+
<joint name="${dovetail_male_mount_prefix}base--body" type="fixed">
47+
<origin xyz="0 0 0" rpy="0 0 0"/>
48+
<parent link="${dovetail_male_mount_prefix}base"/>
49+
<child link="${dovetail_male_mount_prefix}body"/>
50+
</joint>
51+
<joint name="${dovetail_male_mount_prefix}body--tool0" type="fixed">
52+
<origin xyz="0 0 ${dovetail_male_tool0_height}" rpy="0 0 0"/>
53+
<parent link="${dovetail_male_mount_prefix}body"/>
54+
<child link="${dovetail_male_mount_prefix}tool0"/>
55+
</joint>
56+
</xacro:macro>
57+
58+
<xacro:macro name="dovetail_female_mount_macro" params="parent_link dovetail_female_mount_prefix mesh_base_path">
59+
<!-- ====================== -->
60+
<!-- Dovetail Female Mount -->
61+
<!-- ====================== -->
62+
<material name="ochre">
63+
<color rgba="0.929 0.710 0.145 1"/>
64+
</material>
65+
<xacro:property name="dovetail_female_mass" value="0.025"/>
66+
<!-- links -->
67+
<link name="${dovetail_female_mount_prefix}base"/>
68+
<link name='${dovetail_female_mount_prefix}body'>
69+
<visual>
70+
<material name="ochre"/>
71+
<origin xyz="0 0 0" rpy="0 0 0"/>
72+
<geometry>
73+
<mesh filename="${mesh_base_path}/end_effectors/dovetail/DovetailFemale.STL"/>
74+
</geometry>
75+
</visual>
76+
<collision>
77+
<origin xyz="0 0 0" rpy="0 0 0"/>
78+
<geometry>
79+
<mesh filename="${mesh_base_path}/end_effectors/dovetail/DovetailFemale.STL"/>
80+
</geometry>
81+
</collision>
82+
<inertial>
83+
<mass value="${dovetail_female_mass}"/>
84+
<origin xyz="0.0 0.0 0.0"/>
85+
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
86+
</inertial>
87+
</link>
88+
<link name="${dovetail_female_mount_prefix}tool0"/>
89+
<!-- joints -->
90+
<joint name="${parent_link}--${dovetail_female_mount_prefix}base" type="fixed">
91+
<origin xyz="0 0 ${dovetail_female_tool0_height}" rpy="0 0 0"/>
92+
<parent link="${parent_link}"/>
93+
<child link="${dovetail_female_mount_prefix}base"/>
94+
</joint>
95+
<joint name="${dovetail_female_mount_prefix}base--body" type="fixed">
96+
<origin xyz="0 0 0" rpy="0 0 0"/>
97+
<parent link="${dovetail_female_mount_prefix}base"/>
98+
<child link="${dovetail_female_mount_prefix}body"/>
99+
</joint>
100+
<joint name="${dovetail_female_mount_prefix}body--tool0" type="fixed">
101+
<origin xyz="0 0 0" rpy="0 0 0"/>
102+
<parent link="${dovetail_female_mount_prefix}body"/>
103+
<child link="${dovetail_female_mount_prefix}tool0"/>
104+
</joint>
105+
</xacro:macro>
106+
107+
108+
<xacro:macro name="dovetail_mount_macro" params="parent_link dovetail_male_mount_prefix dovetail_female_mount_prefix mesh_base_path">
109+
<xacro:dovetail_male_mount_macro
110+
parent_link="${parent_link}"
111+
dovetail_male_mount_prefix="${dovetail_male_mount_prefix}"
112+
mesh_base_path="${mesh_base_path}"
113+
/>
114+
<xacro:dovetail_female_mount_macro
115+
parent_link="${dovetail_male_mount_prefix}tool0"
116+
dovetail_female_mount_prefix="${dovetail_female_mount_prefix}"
117+
mesh_base_path="${mesh_base_path}"
118+
/>
119+
</xacro:macro>
120+
121+
</robot>

‎pybullet_tree_sim/urdf/end_effectors/dovetail_mount/test.urdf‎

Lines changed: 99 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,99 @@
1+
<?xml version="1.0" ?>
2+
<!-- =================================================================================== -->
3+
<!-- | This document was autogenerated by xacro from dovetail_mount.urdf.xacro | -->
4+
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
5+
<!-- =================================================================================== -->
6+
<robot name="mock_pruner">
7+
<!-- World link -->
8+
<link name="world"/>
9+
<!-- ==================== -->
10+
<!-- Dovetail Male Mount -->
11+
<!-- ==================== -->
12+
<material name="lavender">
13+
<color rgba="0.749 0.580 0.894 1"/>
14+
</material>
15+
<!-- links -->
16+
<link name="dovetail_male_mount__base"/>
17+
<link name="dovetail_male_mount__body">
18+
<visual>
19+
<material name="lavender"/>
20+
<origin rpy="0 0 0" xyz="0 0 0"/>
21+
<geometry>
22+
<mesh filename="/home/luke//end_effectors/dovetail/DovetailMale.STL"/>
23+
</geometry>
24+
</visual>
25+
<collision>
26+
<origin rpy="0 0 0" xyz="0 0 0"/>
27+
<geometry>
28+
<mesh filename="/home/luke//end_effectors/dovetail/DovetailMale.STL"/>
29+
</geometry>
30+
</collision>
31+
<inertial>
32+
<mass value="0.025"/>
33+
<origin xyz="0.0 0.0 0.0"/>
34+
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
35+
</inertial>
36+
</link>
37+
<link name="dovetail_male_mount__tool0"/>
38+
<!-- joints -->
39+
<joint name="world--dovetail_male_mount__base" type="fixed">
40+
<origin rpy="0 0 0" xyz="0 0 0"/>
41+
<parent link="world"/>
42+
<child link="dovetail_male_mount__base"/>
43+
</joint>
44+
<joint name="dovetail_male_mount__base--body" type="fixed">
45+
<origin rpy="0 0 0" xyz="0 0 0"/>
46+
<parent link="dovetail_male_mount__base"/>
47+
<child link="dovetail_male_mount__body"/>
48+
</joint>
49+
<joint name="dovetail_male_mount__body--tool0" type="fixed">
50+
<origin rpy="0 0 0" xyz="0 0 0"/>
51+
<parent link="dovetail_male_mount__body"/>
52+
<child link="dovetail_male_mount__tool0"/>
53+
</joint>
54+
<!-- ====================== -->
55+
<!-- Dovetail Female Mount -->
56+
<!-- ====================== -->
57+
<material name="ochre">
58+
<color rgba="0.929 0.710 0.145 1"/>
59+
</material>
60+
<!-- links -->
61+
<link name="dovetail_female_mount__base"/>
62+
<link name="dovetail_female_mount__body">
63+
<visual>
64+
<material name="ochre"/>
65+
<origin rpy="0 0 0" xyz="0 0 0"/>
66+
<geometry>
67+
<mesh filename="/home/luke//end_effectors/dovetail/DovetailFemale.STL"/>
68+
</geometry>
69+
</visual>
70+
<collision>
71+
<origin rpy="0 0 0" xyz="0 0 0"/>
72+
<geometry>
73+
<mesh filename="/home/luke//end_effectors/dovetail/DovetailFemale.STL"/>
74+
</geometry>
75+
</collision>
76+
<inertial>
77+
<mass value="0.025"/>
78+
<origin xyz="0.0 0.0 0.0"/>
79+
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
80+
</inertial>
81+
</link>
82+
<link name="dovetail_female_mount__tool0"/>
83+
<!-- joints -->
84+
<joint name="dovetail_male_mount__tool0--dovetail_female_mount__base" type="fixed">
85+
<origin rpy="0 0 0" xyz="0 0 0"/>
86+
<parent link="dovetail_male_mount__tool0"/>
87+
<child link="dovetail_female_mount__base"/>
88+
</joint>
89+
<joint name="dovetail_female_mount__base--body" type="fixed">
90+
<origin rpy="0 0 0" xyz="0 0 0"/>
91+
<parent link="dovetail_female_mount__base"/>
92+
<child link="dovetail_female_mount__body"/>
93+
</joint>
94+
<joint name="dovetail_female_mount__body--tool0" type="fixed">
95+
<origin rpy="0 0 0" xyz="0 0 0"/>
96+
<parent link="dovetail_female_mount__body"/>
97+
<child link="dovetail_female_mount__tool0"/>
98+
</joint>
99+
</robot>

0 commit comments

Comments
 (0)