Skip to content

Commit

Permalink
working chain from linear_slider -> ur5 -> mock_pruner. Fragile. fr3 …
Browse files Browse the repository at this point in the history
…not working
  • Loading branch information
lukestroh committed Nov 11, 2024
1 parent 3f58647 commit ffbcbac
Show file tree
Hide file tree
Showing 7 changed files with 995 additions and 524 deletions.
Binary file not shown.
Original file line number Diff line number Diff line change
@@ -0,0 +1,88 @@
<?xml version="1.0"?>
<robot xmlns:xacro="https://ros.org/wiki/xacro" name="mock_pruner">
<!-- Mock Pruner -->
<xacro:arg name="name" default="mock_pruner"/>
<!-- <xacro:property name="end_effector_parent" value="" /> -->

<!-- Materials -->


<xacro:macro name="mock_pruner_macro" params="eef_prefix eef_parent tof0_offset tof1_offset camera_offset">
<!-- ================== -->
<!-- Properties -->
<!-- ================== -->
<xacro:property name="mock_pruner_mass" value="2.0" />

<!-- ================== -->
<!-- Links -->
<!-- ================== -->

<link name="${eef_prefix}base" >
<visual>
<material name="orange">
<color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
</material>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="./meshes/end_effectors/mock_pruner/MockPrunerASSEMBLED.STL" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="./meshes/end_effectors/mock_pruner/MockPrunerASSEMBLED.STL" />
</geometry>
</collision>
<inertial>
<mass value="${mock_pruner_mass}"/>
<origin xyz="0.0 0.0 0.0"/>
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
</inertial>
</link>

<!-- camera link -->
<link name="${eef_prefix}camera0" />

<!-- tof links -->
<link name="${eef_prefix}tof0" />
<link name="${eef_prefix}tof1" />

<!-- tool/cut_point link -->
<link name="${eef_prefix}tool0" />


<!-- ================== -->
<!-- Joints -->
<!-- ================== -->
<joint name="${eef_parent}--${eef_prefix}base_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="${eef_parent}"/>
<child link="${eef_prefix}base"/>
</joint>

<joint name="${eef_prefix}base--camera0_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="${eef_prefix}base" />
<child link="${eef_prefix}camera0" />
</joint>

<joint name="${eef_prefix}base--tof0_joint" type="fixed">
<!-- <xacro:insert_block name="origin"/> -->
<origin xyz="${tof0_offset} 0 0" rpy="0 0 0"/>
<parent link="${eef_prefix}base" />
<child link="${eef_prefix}tof0" />
</joint>
<joint name="${eef_prefix}base--tof1_joint" type="fixed">
<origin xyz="${tof1_offset} 0 0" rpy="0 0 0"/>
<parent link="${eef_prefix}base" />
<child link="${eef_prefix}tof1" />
</joint>

<joint name="${eef_prefix}base--${eef_prefix}tool0" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="${eef_prefix}base" />
<child link="${eef_prefix}tool0" />
</joint>

</xacro:macro>
</robot>
Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@
<?xml version="1.0"?>
<robot xmlns:xacro="https://ros.org/wiki/xacro" name="mock_pruner">

<!-- World link -->
<link name="world" />

<!-- Args -->
<xacro:arg name="eef_parent" default="world" />
<xacro:arg name="eef_prefix" default="mock_pruner__" />

<!-- mock_pruner macro -->
<xacro:include filename="./macro/mock_pruner_macro.urdf.xacro" />

<!-- <xacro:arg name="parent" default="world" />
<xacro:arg name="prefix" default="''" />
<xacro:arg name="use_mock_hardware" default="false" />
<xacro:arg name="mock_sensor_commands" default="false" />
<xacro:arg name="sim_gazebo" default="false" />
<xacro:arg name="sim_gazebo_classic" default="false"/> -->

<xacro:mock_pruner_macro
eef_prefix="$(arg eef_prefix)"
eef_parent="$(arg eef_parent)"
tof0_offset='0.02'
tof1_offset='-0.02'
camera_offset=''
/>

<joint name="world__$(arg eef_prefix)base" type="fixed">
<origin xyz="0.0 180.0 0.0" rpy="0 0 0"/>
<parent link="world" />
<child link="$(arg eef_prefix)base" />
</joint>


</robot>


73 changes: 73 additions & 0 deletions pybullet_tree_sim/urdf/end_effectors/mock_pruner/test.urdf
Original file line number Diff line number Diff line change
@@ -0,0 +1,73 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from mock_pruner.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="mock_pruner">
<!-- World link -->
<link name="world"/>
<!-- <xacro:property name="end_effector_parent" value="" /> -->
<!-- Materials -->
<!-- <xacro:arg name="parent" default="world" />
<xacro:arg name="prefix" default="''" />
<xacro:arg name="use_mock_hardware" default="false" />
<xacro:arg name="mock_sensor_commands" default="false" />
<xacro:arg name="sim_gazebo" default="false" />
<xacro:arg name="sim_gazebo_classic" default="false"/> -->
<!-- ================== -->
<!-- Links -->
<!-- ================== -->
<link name="mock_pruner__base">
<visual>
<material name="orange">
<color rgba="1.0 0.4235294117647059 0.0392156862745098 1.0"/>
</material>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="./meshes/end_effectors/mock_pruner/MockPrunerASSEMBLED.STL"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="./meshes/end_effectors/mock_pruner/MockPrunerASSEMBLED.STL"/>
</geometry>
</collision>
<inertial>
<mass value="2.0"/>
<origin xyz="0.0 0.0 0.0"/>
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
</inertial>
</link>
<!-- camera link -->
<link name="mock_pruner__camera0"/>
<!-- tof links -->
<link name="mock_pruner__tof0"/>
<link name="mock_pruner__tof1"/>
<!-- tool/cut_point link -->
<link name="mock_pruner__tool0"/>
<!-- ================== -->
<!-- Joints -->
<!-- ================== -->
<joint name="mock_pruner__base__camera0_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="mock_pruner__base"/>
<child link="mock_pruner__camera0"/>
</joint>
<joint name="mock_pruner__base__tof0_joint" type="fixed">
<!-- <xacro:insert_block name="origin"/> -->
<origin rpy="0 0 0" xyz="0.02 0 0"/>
<parent link="mock_pruner__base"/>
<child link="mock_pruner__tof0"/>
</joint>
<joint name="mock_pruner__base__tof1_joint" type="fixed">
<origin rpy="0 0 0" xyz="-0.02 0 0"/>
<parent link="mock_pruner__base"/>
<child link="mock_pruner__tof1"/>
</joint>
<joint name="world__mock_pruner__base" type="fixed">
<origin rpy="0 0 0" xyz="0.0 180.0 0.0"/>
<parent link="world"/>
<child link="mock_pruner__base"/>
</joint>
</robot>
Loading

0 comments on commit ffbcbac

Please sign in to comment.