working base robot class for final approach controller

Author: lukestroh

pybullet_tree_sim/robot.py

@@ -192,8 +192,9 @@ def _get_links(self) -> dict:
         links = {}
         for i in range(self.num_joints):
             info = self.pbclient.getJointInfo(self.robot, i)
+            # log.debug(info)
             child_link_name = info[12].decode("utf-8")
-            links.update({child_link_name: {'id': i, "tf_to_parent": info[14]}})
+            links.update({child_link_name: {'id': i, "tf_from_parent": info[14]}})
         return links
 
     def _assign_collision_links(self) -> list:
@@ -251,7 +252,7 @@ def _get_sensors(self) -> dict:
                 )  # TODO: find a better way to get the prefix. If
                 # from robot_conf, need standard for all robots TODO: log an error if robot_part doesn't have all the right frames. Xacro utils?
                 sensors[sensor_name].tf_id = self.links[sensors[sensor_name].tf_frame]['id']
-                sensors[sensor_name].tf_to_parent = self.links[sensors[sensor_name].tf_frame]['tf_to_parent']
+                sensors[sensor_name].tf_from_parent = self.links[sensors[sensor_name].tf_frame]['tf_from_parent']
                 sensors[sensor_name].pan = metadata["pan"] # TODO: Are these only for cameras/toFs? If so, needs reorg
                 sensors[sensor_name].tilt = metadata["tilt"]
             # for key, value in yamlcontent.items():

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

@@ -3,6 +3,7 @@
 
     <xacro:property name="dovetail_male_tool0_height" value="0.008"/>
     <xacro:property name="dovetail_female_tool0_height" value="0.023"/>
+    <xacro:property name="dovetail_male_rot_offset" value="0.0 0.0 0.0"/>
 
 
     <xacro:macro name="dovetail_male_mount_macro" params="parent_link dovetail_male_mount_prefix mesh_base_path">
@@ -39,7 +40,7 @@
         <link name="${dovetail_male_mount_prefix}tool0"/>
         <!-- joints -->
         <joint name="${parent_link}--${dovetail_male_mount_prefix}base" type="fixed">
-            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <origin xyz="0 0 0" rpy="${dovetail_male_rot_offset}"/>
             <parent link="${parent_link}"/>
             <child link="${dovetail_male_mount_prefix}base"/>
         </joint>

pybullet_tree_sim/urdf/robot/generic/robot.urdf.xacro

@@ -29,7 +29,7 @@
             <xacro:if value="${ str(resolved_parent_part) == 'world'}">
                 <xacro:property name='resolved_parent_link' value='world'/>
             </xacro:if>
-            <xacro:unless value="${ str(resolved_parent_part) == 'world'}">
+            <xacro:unless value="${ str(resolved_parent_part) == 'world' }">
                 <xacro:property name='resolved_parent_link' value="${resolved_parent_part}__tool0"/>
             </xacro:unless>            
 
@@ -160,9 +160,10 @@
 
             <!-- call macro recursively -->
             <xacro:main_loop stack_qty="${stack_qty - 1}"/>
-        </xacro:if>
+        </xacro:if> 
     </xacro:macro>
 
+    <!-- call main loop -->
     <xacro:main_loop stack_qty="$(arg robot_stack_qty)"/>
 
 

pybullet_tree_sim/urdf/shapes/cylinder/cylinder.urdf

@@ -8,21 +8,21 @@
 	<link name="cylinder">
 		<visual>
 			<geometry>
-				<cylinder length="2.82" radius="0.0254"/>
+				<cylinder length="2.81" radius="0.0127"/>
 			</geometry>
 			<material name="LightGrey">
 				<color rgba="0.7 0.7 0.7 1.0"/>
 			</material>
 		</visual>
 		<collision concave="true">
 			<geometry>
-				<cylinder length="2.82" radius="0.0254"/>
+				<cylinder length="2.81" radius="0.0127"/>
 			</geometry>
 		</collision>
 		<inertial>
 			<mass value="1.0"/>
 			<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
-			<inertia ixx="0.6628612899999998" ixy="0.0" ixz="0.0" iyy="0.6628612899999998" iyz="0.0" izz="0.00032258"/>
+			<inertia ixx="0.6580486558333333" ixy="0.0" ixz="0.0" iyy="0.6580486558333333" iyz="0.0" izz="8.0645e-05"/>
 		</inertial>
 	</link>
 	<link name="world"/>

pybullet_tree_sim/urdf/tmp/robot.urdf

@@ -42,7 +42,7 @@
 	<link name="dovetail_male_mount__tool0"/>
 	<!-- joints -->
 	<joint name="ur5e__tool0--dovetail_male_mount__base" type="fixed">
-		<origin rpy="0 0 0" xyz="0 0 0"/>
+		<origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
 		<parent link="ur5e__tool0"/>
 		<child link="dovetail_male_mount__base"/>
 	</joint>

pybullet_tree_sim/utils/pyb_utils.py

@@ -141,8 +141,6 @@ def add_sphere(self, radius: float, pos: List, rgba: List) -> int:
     #     )
     #     return
 
-    
-
     @staticmethod
     def linearize_depth(depth: NDArray, far_val: float, near_val: float):
         """OpenGL returns contracted depth, linearize it"""
@@ -151,9 +149,7 @@ def linearize_depth(depth: NDArray, far_val: float, near_val: float):
         except ZeroDivisionError:
             log.warning("Encountered division by 0 in depth linearization.")
             depth_linearized = None
-        return depth_linearized
-
-    
+        return depth_linearized 
 
     def visualize_points(self, points: List, type: str) -> None:
         dx = 0.1