diff --git a/README.md b/README.md
index 6336e99..991873a 100644
--- a/README.md
+++ b/README.md
@@ -1,3 +1,6 @@
+urdf generic launch CLI test:
+`xacro robot.urdf.xacro > test.urdf end_effector_type:=mock_pruner eef_parent:=ur5e__tool0 arm_type:=ur5 ur_type:=ur5e tf_prefix:=ur5e__ base_attachment_type:=linear_slider`
+
 ## TODO
 1. For Claire: 
     1. Generic URDF to Generic Robot class
diff --git a/main.py b/main.py
index def576c..8d3fb03 100644
--- a/main.py
+++ b/main.py
@@ -21,8 +21,8 @@ def main():
     penv = PruningEnv(
         pbutils=pbutils, load_robot=True, robot_pos=[0, 1, 0], verbose=True, cam_width=cam_width, cam_height=cam_height
     )
-    
-    penv.activate_shape(shape="cylinder", radius=2*0.0254, height=2.0, orientation=[0,np.pi/2,0])
+
+    penv.activate_shape(shape="cylinder", radius=2 * 0.0254, height=2.0, orientation=[0, np.pi / 2, 0])
     # penv.load_tree(
     #     pbutils=pbutils,
     #     scale=1.0,
@@ -39,15 +39,15 @@ def main():
     for i in range(100):
         pbutils.pbclient.stepSimulation()
         time.sleep(0.1)
-        
+
     # Simulation loop
     while True:
         try:
-            view_matrix = penv.ur5.get_view_mat_at_curr_pose(0,0,[0,0,0])
-            rgbd = penv.pbutils.get_rgbd_at_cur_pose(type='robot', view_matrix=view_matrix)
+            view_matrix = penv.ur5.get_view_mat_at_curr_pose(0, 0, [0, 0, 0])
+            rgbd = penv.pbutils.get_rgbd_at_cur_pose(type="robot", view_matrix=view_matrix)
             keys_pressed = penv.get_key_pressed()
             action = penv.get_key_action(keys_pressed=keys_pressed)
-            action = action.reshape((6,1))
+            action = action.reshape((6, 1))
             jv, jacobian = penv.ur5.calculate_joint_velocities_from_ee_velocity_dls(end_effector_velocity=action)
             penv.ur5.action = jv
             singularity = penv.ur5.set_joint_velocities(penv.ur5.action)
@@ -63,4 +63,3 @@ def main():
 
 if __name__ == "__main__":
     main()
-  
\ No newline at end of file
diff --git a/pybullet_tree_sim/camera.py b/pybullet_tree_sim/camera.py
index 35edf60..a648375 100644
--- a/pybullet_tree_sim/camera.py
+++ b/pybullet_tree_sim/camera.py
@@ -22,12 +22,12 @@ def __init__(self, pbutils, sensor_name: str, sensor_type: str = "camera") -> No
         super().__init__(sensor_name, sensor_type)
         self.pan = 0
         self.tilt = 0
-        self.tf_frame: str = ''
+        self.tf_frame: str = ""
 
         # Only dealing with depth data for now, TODO: add RGB data
         self.depth_width = self.params["depth"]["width"]
         self.depth_height = self.params["depth"]["height"]
-        
+
         # Some optical sensors only provide diagonal field of view, get horizontal and vertical from diagonal
         try:
             vfov = self.params["depth"]["vfov"]
@@ -37,9 +37,11 @@ def __init__(self, pbutils, sensor_name: str, sensor_type: str = "camera") -> No
         far_val = self.params["depth"]["far_plane"]
 
         self.depth_pixel_coords = np.array(list(np.ndindex((self.depth_width, self.depth_height))), dtype=int)
-        self.depth_film_coords = 2 * (
-            self.depth_pixel_coords + np.array([0.5, 0.5]) - np.array([self.depth_width / 2, self.depth_height / 2])
-        ) / np.array([self.depth_width, self.depth_height])
+        self.depth_film_coords = (
+            2
+            * (self.depth_pixel_coords + np.array([0.5, 0.5]) - np.array([self.depth_width / 2, self.depth_height / 2]))
+            / np.array([self.depth_width, self.depth_height])
+        )
         self.depth_proj_mat = pbutils.pbclient.computeProjectionMatrixFOV(
             fov=vfov, aspect=(self.depth_width / self.depth_height), nearVal=near_val, farVal=far_val
         )
diff --git a/pybullet_tree_sim/meshes/end_effectors/mock_pruner/MockPrunerASSEMBLED.STL b/pybullet_tree_sim/meshes/end_effectors/mock_pruner/MockPrunerASSEMBLED.STL
new file mode 100644
index 0000000..1985cd0
Binary files /dev/null and b/pybullet_tree_sim/meshes/end_effectors/mock_pruner/MockPrunerASSEMBLED.STL differ
diff --git a/pybullet_tree_sim/pruning_environment.py b/pybullet_tree_sim/pruning_environment.py
index 6042f43..2a624e4 100644
--- a/pybullet_tree_sim/pruning_environment.py
+++ b/pybullet_tree_sim/pruning_environment.py
@@ -398,7 +398,9 @@ def deproject_pixels_to_points(
 
         plot = False
         if plot:
-            self.debug_plots(camera=camera, data=data, cam_coords=cam_coords, world_coords=world_coords, view_matrix=view_matrix)
+            self.debug_plots(
+                camera=camera, data=data, cam_coords=cam_coords, world_coords=world_coords, view_matrix=view_matrix
+            )
 
         return world_coords
 
@@ -562,7 +564,6 @@ def get_key_action(self, keys_pressed: list) -> np.ndarray:
             keys_pressed = []
         return action
 
-
     def run_sim(self) -> int:
 
         return 0
diff --git a/pybullet_tree_sim/robot.py b/pybullet_tree_sim/robot.py
new file mode 100644
index 0000000..82c2ab2
--- /dev/null
+++ b/pybullet_tree_sim/robot.py
@@ -0,0 +1,423 @@
+from typing import Optional, Tuple
+import numpy as np
+import pybullet
+from collections import namedtuple
+
+
+class Robot:
+    def __init__(
+        self,
+        con,
+        robot_type: str,
+        robot_urdf_path: str,
+        tool_link_name: str,
+        end_effector_link_name: str,
+        success_link_name: str,
+        base_link_name: str,
+        control_joints,
+        robot_collision_filter_idxs,
+        init_joint_angles: Optional[list] = None,
+        pos=(0, 0, 0),
+        orientation=(0, 0, 0, 1),
+        randomize_pose=False,
+        verbose=1,
+    ) -> None:
+        self.con = con
+        self.robot_type = robot_type
+        self.robot_urdf_path = robot_urdf_path
+        self.tool_link_name = tool_link_name
+        self.end_effector_link_name = end_effector_link_name
+        self.success_link_name = success_link_name
+        self.base_link_name = base_link_name
+        self.init_joint_angles = init_joint_angles
+        self.pos = pos
+        self.orientation = orientation
+        self.randomize_pose = randomize_pose
+
+        self.joint_info = namedtuple(
+            "jointInfo", ["id", "name", "type", "lowerLimit", "upperLimit", "maxForce", "maxVelocity", "controllable"]
+        )
+        # Pruning camera information
+        self.camera_base_offset = np.array([0.063179, 0.077119, 0.0420027])
+        self.verbose = verbose
+
+        self.joints = None
+        self.robot = None
+        self.control_joints = control_joints
+        self.robot_collision_filter_idxs = robot_collision_filter_idxs
+        self.setup_robot()
+
+    def setup_robot(self):
+        if self.robot is not None:
+            self.con.removeBody(self.robot)
+            del self.robot
+        flags = self.con.URDF_USE_SELF_COLLISION
+
+        if self.randomize_pose:
+            delta_pos = np.random.rand(3) * 0.0
+            delta_orientation = pybullet.getQuaternionFromEuler(np.random.rand(3) * np.pi / 180 * 5)
+        else:
+            delta_pos = np.array([0.0, 0.0, 0.0])
+            delta_orientation = pybullet.getQuaternionFromEuler([0, 0, 0])
+
+        self.pos, self.orientation = self.con.multiplyTransforms(
+            self.pos, self.orientation, delta_pos, delta_orientation
+        )
+        self.robot = self.con.loadURDF(self.robot_urdf_path, self.pos, self.orientation, flags=flags, useFixedBase=True)
+        self.num_joints = self.con.getNumJoints(self.robot)
+
+        # Get indices dynamically
+        self.tool0_link_index = self.get_link_index(self.tool_link_name)
+        self.end_effector_index = self.get_link_index(self.end_effector_link_name)
+        self.success_link_index = self.get_link_index(self.success_link_name)
+        self.base_index = self.get_link_index(self.base_link_name)
+
+        self.set_collision_filter()
+
+        # Setup robot info only once
+        if not self.joints:
+            self.joints = dict()
+            self.controllable_joints_idxs = []
+            self.joint_lower_limits = []
+            self.joint_upper_limits = []
+            self.joint_max_forces = []
+            self.joint_max_velocities = []
+            self.joint_ranges = []
+
+            for i in range(self.num_joints):
+                info = self.con.getJointInfo(self.robot, i)
+                jointID = info[0]
+                jointName = info[1].decode("utf-8")
+                jointType = info[2]
+                jointLowerLimit = info[8]
+                jointUpperLimit = info[9]
+                jointMaxForce = info[10]
+                jointMaxVelocity = info[11]
+                if self.verbose > 1:
+                    print("Joint Name: ", jointName, "Joint ID: ", jointID)
+
+                controllable = True if jointName in self.control_joints else False
+                if controllable:
+                    self.controllable_joints_idxs.append(i)
+                    self.joint_lower_limits.append(jointLowerLimit)
+                    self.joint_upper_limits.append(jointUpperLimit)
+                    self.joint_max_forces.append(jointMaxForce)
+                    self.joint_max_velocities.append(jointMaxVelocity)
+                    self.joint_ranges.append(jointUpperLimit - jointLowerLimit)
+                    if self.verbose > 1:
+                        print("Controllable Joint Name: ", jointName, "Joint ID: ", jointID)
+
+                info = self.joint_info(
+                    jointID,
+                    jointName,
+                    jointType,
+                    jointLowerLimit,
+                    jointUpperLimit,
+                    jointMaxForce,
+                    jointMaxVelocity,
+                    controllable,
+                )
+
+                if info.type == self.con.JOINT_REVOLUTE:
+                    self.con.setJointMotorControl2(
+                        self.robot,
+                        info.id,
+                        self.con.VELOCITY_CONTROL,
+                        targetVelocity=0,
+                        force=0,
+                    )
+                self.joints[info.name] = info
+
+        self.set_joint_angles_no_collision(self.init_joint_angles)
+        self.con.stepSimulation()
+
+        self.init_pos_ee = self.get_current_pose(self.end_effector_index)
+        self.init_pos_base = self.get_current_pose(self.base_index)
+        self.init_pos_eebase = self.get_current_pose(self.success_link_index)
+        self.action = np.zeros(len(self.init_joint_angles), dtype=np.float32)
+        self.joint_angles = np.array(self.init_joint_angles).astype(np.float32)
+        self.achieved_pos = np.array(self.get_current_pose(self.end_effector_index)[0])
+        base_pos, base_or = self.get_current_pose(self.base_index)
+
+    def get_link_index(self, link_name):
+        num_joints = self.con.getNumJoints(self.robot)
+        for i in range(num_joints):
+            info = self.con.getJointInfo(self.robot, i)
+            child_link_name = info[12].decode("utf-8")
+            if child_link_name == link_name:
+                return i  # return link index
+
+        base_link_name = self.con.getBodyInfo(self.robot)[0].decode("utf-8")
+        if base_link_name == link_name:
+            return -1  # base link has index of -1
+        raise ValueError(f"Link '{link_name}' not found in the robot URDF.")
+
+    def reset_robot(self):
+        if self.robot is None:
+            return
+
+        self.set_joint_angles_no_collision(self.init_joint_angles)
+
+    def remove_robot(self):
+        self.con.removeBody(self.robot)
+        self.robot = None
+
+    def set_joint_angles_no_collision(self, joint_angles) -> None:
+        assert len(joint_angles) == len(self.control_joints)
+        for i, name in enumerate(self.control_joints):
+            joint = self.joints[name]
+            self.con.resetJointState(self.robot, joint.id, joint_angles[i], targetVelocity=0)
+
+    def set_joint_angles(self, joint_angles) -> None:
+        """Set joint angles using pybullet motor control"""
+
+        assert len(joint_angles) == len(self.control_joints)
+        poses = []
+        indexes = []
+        forces = []
+
+        for i, name in enumerate(self.control_joints):
+            joint = self.joints[name]
+            poses.append(joint_angles[i])
+            indexes.append(joint.id)
+            forces.append(joint.maxForce)
+
+        self.con.setJointMotorControlArray(
+            self.robot,
+            indexes,
+            self.con.POSITION_CONTROL,
+            targetPositions=joint_angles,
+            targetVelocities=[0] * len(poses),
+            positionGains=[0.05] * len(poses),
+            forces=forces,
+        )
+
+    def set_joint_velocities(self, joint_velocities) -> None:
+        """Set joint velocities using pybullet motor control"""
+        assert len(joint_velocities) == len(self.control_joints)
+        velocities = []
+        indexes = []
+        forces = []
+
+        for i, name in enumerate(self.control_joints):
+            joint = self.joints[name]
+            velocities.append(joint_velocities[i])
+            indexes.append(joint.id)
+            forces.append(joint.maxForce)
+
+        self.con.setJointMotorControlArray(
+            self.robot,
+            indexes,
+            controlMode=self.con.VELOCITY_CONTROL,
+            targetVelocities=joint_velocities,
+        )
+
+    # TODO: Use proprty decorator for getters?
+    def get_joint_velocities(self):
+        j = self.con.getJointStates(self.robot, self.controllable_joints_idxs)
+        joints = tuple((i[1] for i in j))
+        return joints  # type: ignore
+
+    def get_joint_angles(self):
+        """Return joint angles"""
+        print(self.control_joints, self.controllable_joints_idxs)
+        j = self.con.getJointStates(self.robot, self.controllable_joints_idxs)
+        joints = tuple((i[0] for i in j))
+        return joints
+
+    def get_current_pose(self, index):
+        """Returns current pose of the index"""
+        link_state = self.con.getLinkState(self.robot, index, computeForwardKinematics=True)
+        position, orientation = link_state[4], link_state[5]
+        return position, orientation
+
+    def get_current_vel(self, index):
+        """Returns current pose of the index."""
+        link_state = self.con.getLinkState(self.robot, index, computeLinkVelocity=True, computeForwardKinematics=True)
+        trans, ang = link_state[6], link_state[7]
+        return trans, ang
+
+    def get_condition_number(self):
+        """Get condition number of the jacobian"""
+        jacobian = self.calculate_jacobian()
+        condition_number = np.linalg.cond(jacobian)
+        return condition_number
+
+    def calculate_ik(self, position, orientation):
+        """Calculates joint angles from end effector position and orientation using inverse kinematics"""
+
+        joint_angles = self.con.calculateInverseKinematics(
+            self.robot,
+            self.end_effector_index,
+            position,
+            orientation,
+            jointDamping=[0.01] * len(self.control_joints),
+            upperLimits=self.joint_upper_limits,
+            lowerLimits=self.joint_lower_limits,
+            jointRanges=self.joint_ranges,  # , restPoses=self.init_joint_angles
+        )
+        return joint_angles
+
+    def calculate_jacobian(self):
+        jacobian = self.con.calculateJacobian(
+            self.robot,
+            self.tool0_link_index,
+            [0, 0, 0],
+            self.get_joint_angles(),
+            [0] * len(self.control_joints),
+            [0] * len(self.control_joints),
+        )
+        jacobian = np.vstack(jacobian)
+        return jacobian
+
+    def calculate_joint_velocities_from_ee_velocity(self, end_effector_velocity):
+        """Calculate joint velocities from end effector velocity using jacobian using least squares"""
+        jacobian = self.calculate_jacobian()
+        inv_jacobian = np.linalg.pinv(jacobian)
+        joint_velocities = np.matmul(inv_jacobian, end_effector_velocity).astype(np.float32)
+        return joint_velocities, jacobian
+
+    def calculate_joint_velocities_from_ee_velocity_dls(self, end_effector_velocity, damping_factor: float = 0.05):
+        """Calculate joint velocities from end effector velocity using damped least squares"""
+        jacobian = self.calculate_jacobian()
+        identity_matrix = np.eye(jacobian.shape[0])
+        damped_matrix = jacobian @ jacobian.T + (damping_factor ** 2) * identity_matrix
+        damped_matrix_inv = np.linalg.inv(damped_matrix)
+        dls_inv_jacobian = jacobian.T @ damped_matrix_inv
+        joint_velocities = dls_inv_jacobian @ end_effector_velocity
+        return joint_velocities, jacobian
+
+    # TODO: Make camera a separate class?
+    def create_camera_transform(self, pos, orientation, pan, tilt, xyz_offset) -> np.ndarray:
+        """Create rotation matrix for camera"""
+        base_offset_tf = np.identity(4)
+        base_offset_tf[:3, 3] = self.camera_base_offset + xyz_offset
+
+        ee_transform = np.identity(4)
+        ee_rot_mat = np.array(self.con.getMatrixFromQuaternion(orientation)).reshape(3, 3)
+        ee_transform[:3, :3] = ee_rot_mat
+        ee_transform[:3, 3] = pos
+
+        tilt_tf = np.identity(4)
+        tilt_rot = np.array([[1, 0, 0], [0, np.cos(tilt), -np.sin(tilt)], [0, np.sin(tilt), np.cos(tilt)]])
+        tilt_tf[:3, :3] = tilt_rot
+
+        pan_tf = np.identity(4)
+        pan_rot = np.array([[np.cos(pan), 0, np.sin(pan)], [0, 1, 0], [-np.sin(pan), 0, np.cos(pan)]])
+        pan_tf[:3, :3] = pan_rot
+
+        tf = ee_transform @ pan_tf @ tilt_tf @ base_offset_tf
+        return tf
+
+    # TODO: Better types for getCameraImage
+    def get_view_mat_at_curr_pose(self, pan, tilt, xyz_offset) -> np.ndarray:
+        """Get view matrix at current pose"""
+        pose, orientation = self.get_current_pose(self.tool0_link_index)
+
+        camera_tf = self.create_camera_transform(pose, orientation, pan, tilt, xyz_offset)
+
+        # Initial vectors
+        camera_vector = np.array([0, 0, 1]) @ camera_tf[:3, :3].T  #
+        up_vector = np.array([0, 1, 0]) @ camera_tf[:3, :3].T  #
+        # Rotated vectors
+        # print(camera_vector, up_vector)
+        view_matrix = self.con.computeViewMatrix(camera_tf[:3, 3], camera_tf[:3, 3] + 0.1 * camera_vector, up_vector)
+        return view_matrix
+
+    def get_camera_location(self):
+        pose, orientation = self.get_current_pose(self.tool0_link_index)
+        tilt = np.pi / 180 * 8
+
+        camera_tf = self.create_camera_transform(pose, orientation, tilt)
+        return camera_tf
+
+    # Collision checking
+
+    def set_collision_filter(self):
+        """Disable collision between pruner and arm"""
+        for i in self.robot_collision_filter_idxs:
+            self.con.setCollisionFilterPair(self.robot, self.robot, i[0], i[1], 0)
+
+    def unset_collision_filter(self):
+        """Enable collision between pruner and arm"""
+        for i in self.robot_collision_filter_idxs:
+            self.con.setCollisionFilterPair(self.robot, self.robot, i[0], i[1], 1)
+
+    def disable_self_collision(self):
+        for i in range(self.num_joints):
+            for j in range(self.num_joints):
+                if i != j:
+                    self.con.setCollisionFilterPair(self.robot, self.robot, i, j, 0)
+
+    def enable_self_collision(self):
+        for i in range(self.num_joints):
+            for j in range(self.num_joints):
+                if i != j:
+                    self.con.setCollisionFilterPair(self.robot, self.robot, i, j, 1)
+
+    def check_collisions(self, collision_objects) -> Tuple[bool, dict]:
+        """Check if there are any collisions between the robot and the environment
+        Returns: Dictionary with information about collisions (Acceptable and Unacceptable)
+        """
+        collision_info = {"collisions_acceptable": False, "collisions_unacceptable": False}
+
+        collision_acceptable_list = ["SPUR", "WATER_BRANCH"]
+        collision_unacceptable_list = ["TRUNK", "BRANCH", "SUPPORT"]
+        for type in collision_acceptable_list:
+            collisions_acceptable = self.con.getContactPoints(bodyA=self.robot, bodyB=collision_objects[type])
+            if collisions_acceptable:
+                for i in range(len(collisions_acceptable)):
+                    if collisions_acceptable[i][-6] < 0:
+                        collision_info["collisions_acceptable"] = True
+                        break
+            if collision_info["collisions_acceptable"]:
+                break
+
+        for type in collision_unacceptable_list:
+            collisions_unacceptable = self.con.getContactPoints(bodyA=self.robot, bodyB=collision_objects[type])
+            for i in range(len(collisions_unacceptable)):
+                if collisions_unacceptable[i][-6] < 0:
+                    collision_info["collisions_unacceptable"] = True
+                    # break
+            if collision_info["collisions_unacceptable"]:
+                break
+
+        if not collision_info["collisions_unacceptable"]:
+            collisons_self = self.con.getContactPoints(bodyA=self.robot, bodyB=self.robot)
+            collisions_unacceptable = collisons_self
+            for i in range(len(collisions_unacceptable)):
+                if collisions_unacceptable[i][-6] < -0.001:
+                    collision_info["collisions_unacceptable"] = True
+                    break
+        if self.verbose > 1:
+            print(f"DEBUG: {collision_info}")
+
+        if collision_info["collisions_acceptable"] or collision_info["collisions_unacceptable"]:
+            return True, collision_info
+
+        return False, collision_info
+
+    def check_success_collision(self, body_b) -> bool:
+        """Check if there are any collisions between the robot and the environment
+        Returns: Boolw
+        """
+        collisions_success = self.con.getContactPoints(
+            bodyA=self.robot, bodyB=body_b, linkIndexA=self.success_link_index
+        )
+        for i in range(len(collisions_success)):
+            if collisions_success[i][-6] < 0.05:
+                if self.verbose > 1:
+                    print("DEBUG: Success Collision")
+                return True
+
+        return False
+
+    def set_collision_filter_tree(self, collision_objects):
+        for i in collision_objects.values():
+            for j in range(self.num_joints):
+                self.con.setCollisionFilterPair(self.robot, i, j, 0, 0)
+
+    def unset_collision_filter_tree(self, collision_objects):
+        for i in collision_objects.values():
+            for j in range(self.num_joints):
+                self.con.setCollisionFilterPair(self.robot, i, j, 0, 1)
diff --git a/pybullet_tree_sim/tree.py b/pybullet_tree_sim/tree.py
index d2d1ce6..bc9d05f 100644
--- a/pybullet_tree_sim/tree.py
+++ b/pybullet_tree_sim/tree.py
@@ -447,10 +447,10 @@ def load_tree_urdf(
             urdf_path = os.path.join(Tree._tree_generated_urdf_path, f"{namespace}{tree_type}_tree{tree_id}.urdf")
             if not os.path.exists(urdf_path):
                 log.info(f"Could not find file '{urdf_path}'. Generating URDF from xacro.")
-                
+
                 if not os.path.isdir(Tree._tree_generated_urdf_path):
                     os.mkdir(Tree._tree_generated_urdf_path)
-                
+
                 urdf_mappings = {
                     "namespace": namespace,
                     "tree_id": str(tree_id),
diff --git a/pybullet_tree_sim/urdf/end_effectors/mock_pruner/macro/mock_pruner_macro.urdf.xacro b/pybullet_tree_sim/urdf/end_effectors/mock_pruner/macro/mock_pruner_macro.urdf.xacro
new file mode 100644
index 0000000..ea65f78
--- /dev/null
+++ b/pybullet_tree_sim/urdf/end_effectors/mock_pruner/macro/mock_pruner_macro.urdf.xacro
@@ -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>
\ No newline at end of file
diff --git a/pybullet_tree_sim/urdf/end_effectors/mock_pruner/mock_pruner.urdf.xacro b/pybullet_tree_sim/urdf/end_effectors/mock_pruner/mock_pruner.urdf.xacro
new file mode 100644
index 0000000..088906c
--- /dev/null
+++ b/pybullet_tree_sim/urdf/end_effectors/mock_pruner/mock_pruner.urdf.xacro
@@ -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>
+
+
diff --git a/pybullet_tree_sim/urdf/end_effectors/mock_pruner/test.urdf b/pybullet_tree_sim/urdf/end_effectors/mock_pruner/test.urdf
new file mode 100644
index 0000000..ef1c6d5
--- /dev/null
+++ b/pybullet_tree_sim/urdf/end_effectors/mock_pruner/test.urdf
@@ -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>
diff --git a/pybullet_tree_sim/urdf/robot/base/test.urdf b/pybullet_tree_sim/urdf/robot/base/test.urdf
deleted file mode 100644
index 1a9aab2..0000000
--- a/pybullet_tree_sim/urdf/robot/base/test.urdf
+++ /dev/null
@@ -1,510 +0,0 @@
-<?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from robot.urdf.xacro               | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot>
-  <!-- create link fixed to the "world" -->
-  <link name="world"/>
-  <!-- ================== -->
-  <!--     Properties     -->
-  <!-- ================== -->
-  <!-- ================== -->
-  <!--     Materials      -->
-  <!-- ================== -->
-  <material name="alpha_black">
-    <color rgba="0.6 0.6 0.6 1"/>
-  </material>
-  <material name="red_v">
-    <color rgba="1 0 0 1"/>
-  </material>
-  <!-- ================== -->
-  <!--       Links        -->
-  <!-- ================== -->
-  <!-- base_link -->
-  <link name="linear_slider__base_link">
-    <visual>
-      <material name="alpha_black"/>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <geometry>
-        <mesh filename="file:///home/luke/ros2_ws/install/linear_slider_description/share/linear_slider_description/meshes/7thlink_fixed.STL" scale="0.001 0.001 0.001"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="1.0 0.3 0.10"/>
-        <!-- <mesh
-                        filename="package://linear_slider_description/meshes/7thlink_fixed.STL"
-                        scale="${model_scale}"
-                    /> -->
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="20.0"/>
-      <origin xyz="0.0 0.0 0.0"/>
-      <inertia ixx="0.061" ixy="0.0" ixz="0.0" iyy="0.65" iyz="0.0" izz="0.661"/>
-    </inertial>
-  </link>
-  <gazebo reference="linear_slider__base_link">
-    <material>Gazebo/Gray</material>
-  </gazebo>
-  <!-- moving_base link -->
-  <link name="linear_slider__moving_base">
-    <visual>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <material name="red_v"/>
-      <geometry>
-        <mesh filename="file:///home/luke/ros2_ws/install/linear_slider_description/share/linear_slider_description/meshes/7thlink_move.STL" scale="0.001 0.001 0.001"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.22 0.202 0.118"/>
-        <!-- <mesh
-                        filename="package://linear_slider_description/meshes/7thlink_move.STL"
-                        scale="${model_scale}"
-                    /> -->
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="10.0"/>
-      <origin xyz="0.0 0.0 0.0"/>
-      <inertia ixx="0.0915" ixy="0.0" ixz="0.0" iyy="0.975" iyz="0.0" izz="0.9915"/>
-    </inertial>
-  </link>
-  <gazebo reference="linear_slider__moving_base">
-    <material>Gazebo/Red</material>
-  </gazebo>
-  <!-- tool link -->
-  <link name="linear_slider__tool0"/>
-  <link name="linear_slider__base"/>
-  <!-- Frame for mounting EEF models to a manipulator. x+ axis points forward (REP 103). -->
-  <link name="linear_slider__flange"/>
-  <!-- ================== -->
-  <!--       Joints       -->
-  <!-- ================== -->
-  <!-- base_joint fixes base_link to the environment -->
-  <joint name="linear_slider__base_joint" type="fixed">
-    <parent link="world"/>
-    <child link="linear_slider__base"/>
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-  </joint>
-  <joint name="linear_slider__base_link__base_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="linear_slider__base"/>
-    <child link="linear_slider__base_link"/>
-  </joint>
-  <!-- joint1, moving prismatic joint -->
-  <joint name="linear_slider__joint1" type="prismatic">
-    <parent link="linear_slider__base_link"/>
-    <child link="linear_slider__moving_base"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="0.05" lower="-0.4" upper="0.4" velocity="0.08333333333333334"/>
-    <!-- effort is attribute for enforcing maximum joint effort, between 0 and 100% -->
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <!-- Define where the joint will be located, defined in terms of parent's reference frame -->
-  </joint>
-  <!-- Extra joints for ros-industrial standard compatability -->
-  <!-- tool frame to fixed frame -->
-  <joint name="linear_slider__moving_base__flange_joint" type="fixed">
-    <parent link="linear_slider__moving_base"/>
-    <child link="linear_slider__flange"/>
-    <origin rpy="0 0 0" xyz="0 0 0.05"/>
-  </joint>
-  <joint name="linear_slider__flange__tool0_joint" type="fixed">
-    <parent link="linear_slider__flange"/>
-    <child link="linear_slider__tool0"/>
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-  </joint>
-  <!-- The caret ^ indicates to use the outer-scope property (with same name). The pipe | indicates to use the given fallback if the property is not defined in outer scope. -->
-  <!-- ================== -->
-  <!--    ros2_control    -->
-  <!-- ================== -->
-  <ros2_control name="linear_slider_ros2_control" type="system">
-    <!-- Hardware -->
-    <hardware>
-      <plugin>linear_slider_system_interface/LinearSliderSystemInterface</plugin>
-      <!-- <param name="device_name">linear_slider_device</param>
-                <param name="joint_name">prismatic_joint</param>
-                <param name="robot_ip">169.254.97.178</param> -->
-    </hardware>
-    <!-- Joints -->
-    <joint name="linear_slider__joint1">
-      <!-- Command interface -->
-      <!-- <command_interface name="position">
-                    <param name="min">-0.40</param>
-                    <param name="max">0.40</param>
-                </command_interface> -->
-      <command_interface name="velocity">
-        <param name="min">-0.1</param>
-        <param name="max">0.1</param>
-      </command_interface>
-      <!-- State interface -->
-      <state_interface name="position">
-        <param name="initial_value">-0.4</param>
-      </state_interface>
-      <state_interface name="velocity">
-        <param name="initial_value">0.0</param>
-      </state_interface>
-    </joint>
-    <!-- Sensors -->
-    <sensor name="linear_slider__lim_switch_neg">
-      <state_interface name="state">
-        <param name="initial_value">0</param>
-      </state_interface>
-      <param name="frame_id">linear_slider__base_link</param>
-    </sensor>
-    <sensor name="linear_slider__lim_switch_pos">
-      <state_interface name="state">
-        <param name="initial_value">0</param>
-      </state_interface>
-      <param name="frame_id">linear_slider__base_link</param>
-    </sensor>
-  </ros2_control>
-  <!-- safety_distance: Minimum safety distance in [m] by which the collision volumes are expanded and which is enforced during robot motions -->
-  <!-- arm_id: Namespace of the robot arm. Serves to differentiate between arms in case of multiple instances. -->
-  <!-- joint_limits: description of the joint limits that comes from a YAML file. Example definition: ${xacro.load_yaml('$(find franka_description)/robots/fr3/joint_limits.yaml')} -->
-  <!-- kinematics: description of the kinematics that comes from a YAML file. Example definition: ${xacro.load_yaml('$(find franka_description)/robots/fr3/kinematics.yaml')} -->
-  <!-- inertials: description of the inertials that comes from a YAML file. Example definition: ${xacro.load_yaml('$(find franka_description)/robots/fr3/inertials.yaml')} -->
-  <!-- dynamics: description of the dynamics that comes from a YAML file. Example definition: ${xacro.load_yaml('$(find franka_description)/robots/fr3/dynamics.yaml')} -->
-  <link name="base">
-      </link>
-  <joint name="base_joint" type="fixed">
-    <parent link="base"/>
-    <child link="fr3_link0"/>
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-  </joint>
-  <!-- sub-link defining detailed meshes for collision with environment -->
-  <link name="fr3_link0">
-    <visual name="fr3_link0_visual">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_arms/fr3/visual/link0.dae"/>
-      </geometry>
-    </visual>
-    <collision name="fr3_link0_collision">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_arms/fr3/collision/link0.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.0172 0.0004 0.0745"/>
-      <mass value="2.3966"/>
-      <inertia ixx="0.009" ixy="0.0" ixz="0.002" iyy="0.0115" iyz="0.0" izz="0.0085"/>
-    </inertial>
-  </link>
-  <!-- sub-link defining detailed meshes for collision with environment -->
-  <link name="fr3_link1">
-    <visual name="fr3_link1_visual">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_arms/fr3/visual/link1.dae"/>
-      </geometry>
-    </visual>
-    <collision name="fr3_link1_collision">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_arms/fr3/collision/link1.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0000004128 -0.0181251324 -0.0386035970"/>
-      <mass value="2.9274653454"/>
-      <inertia ixx="0.023927316485107913" ixy="1.3317903455714081e-05" ixz="-0.00011404774918616684" iyy="0.0224821613275756" iyz="-0.0019950320628240115" izz="0.006350098258530016"/>
-    </inertial>
-  </link>
-  <joint name="fr3_joint1" type="revolute">
-    <origin rpy="0 0 0" xyz="0 0 0.333"/>
-    <parent link="fr3_link0"/>
-    <child link="fr3_link1"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="87.0" lower="-2.7437" upper="2.7437" velocity="2.62"/>
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.7437" soft_upper_limit="2.7437"/>
-    <dynamics D="1" K="7000" damping="0.003" friction="0.2" mu_coulomb="0" mu_viscous="16"/>
-  </joint>
-  <!-- sub-link defining detailed meshes for collision with environment -->
-  <link name="fr3_link2">
-    <visual name="fr3_link2_visual">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_arms/fr3/visual/link2.dae"/>
-      </geometry>
-    </visual>
-    <collision name="fr3_link2_collision">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_arms/fr3/collision/link2.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0031828864 -0.0743221644 0.0088146084"/>
-      <mass value="2.9355370338"/>
-      <inertia ixx="0.041938946257609425" ixy="0.00020257331521090626" ixz="0.004077784227179924" iyy="0.02514514885014724" iyz="-0.0042252158006570156" izz="0.06170214472888839"/>
-    </inertial>
-  </link>
-  <joint name="fr3_joint2" type="revolute">
-    <origin rpy="-1.570796326794897 0 0" xyz="0 0 0"/>
-    <parent link="fr3_link1"/>
-    <child link="fr3_link2"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="87.0" lower="-1.7837" upper="1.7837" velocity="2.62"/>
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7837" soft_upper_limit="1.7837"/>
-    <dynamics D="1" K="7000" damping="0.003" friction="0.2" mu_coulomb="0" mu_viscous="16"/>
-  </joint>
-  <!-- sub-link defining detailed meshes for collision with environment -->
-  <link name="fr3_link3">
-    <visual name="fr3_link3_visual">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_arms/fr3/visual/link3.dae"/>
-      </geometry>
-    </visual>
-    <collision name="fr3_link3_collision">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_arms/fr3/collision/link3.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0407015686 -0.0048200565 -0.0289730823"/>
-      <mass value="2.2449013699"/>
-      <inertia ixx="0.02410142547240885" ixy="0.002404694559042109" ixz="-0.002856269270114313" iyy="0.01974053266708178" iyz="-0.002104212683891874" izz="0.019044494482244823"/>
-    </inertial>
-  </link>
-  <joint name="fr3_joint3" type="revolute">
-    <origin rpy="1.570796326794897 0 0" xyz="0 -0.316 0"/>
-    <parent link="fr3_link2"/>
-    <child link="fr3_link3"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="87.0" lower="-2.9007" upper="2.9007" velocity="2.62"/>
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.9007" soft_upper_limit="2.9007"/>
-    <dynamics D="1" K="7000" damping="0.003" friction="0.2" mu_coulomb="0" mu_viscous="16"/>
-  </joint>
-  <!-- sub-link defining detailed meshes for collision with environment -->
-  <link name="fr3_link4">
-    <visual name="fr3_link4_visual">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_arms/fr3/visual/link4.dae"/>
-      </geometry>
-    </visual>
-    <collision name="fr3_link4_collision">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_arms/fr3/collision/link4.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.0459100965 0.0630492960 -0.0085187868"/>
-      <mass value="2.6155955791"/>
-      <inertia ixx="0.03452998321913202" ixy="0.01322552265982813" ixz="0.01015142998484113" iyy="0.028881621933049058" iyz="-0.0009762833870704552" izz="0.04125471171146641"/>
-    </inertial>
-  </link>
-  <joint name="fr3_joint4" type="revolute">
-    <origin rpy="1.570796326794897 0 0" xyz="0.0825 0 0"/>
-    <parent link="fr3_link3"/>
-    <child link="fr3_link4"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="87.0" lower="-3.0421" upper="-0.1518" velocity="2.62"/>
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0421" soft_upper_limit="-0.1518"/>
-    <dynamics D="1" K="7000" damping="0.003" friction="0.2" mu_coulomb="0" mu_viscous="16"/>
-  </joint>
-  <!-- sub-link defining detailed meshes for collision with environment -->
-  <link name="fr3_link5">
-    <visual name="fr3_link5_visual">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_arms/fr3/visual/link5.dae"/>
-      </geometry>
-    </visual>
-    <collision name="fr3_link5_collision">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_arms/fr3/collision/link5.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.0016039605 0.0292536262 -0.0972965990"/>
-      <mass value="2.3271207594"/>
-      <inertia ixx="0.051610278463662895" ixy="-0.005715173387783472" ixz="-0.0035673167625872135" iyy="0.04787729713371481" iyz="0.010673985108535986" izz="0.016423625579357254"/>
-    </inertial>
-  </link>
-  <joint name="fr3_joint5" type="revolute">
-    <origin rpy="-1.570796326794897 0 0" xyz="-0.0825 0.384 0"/>
-    <parent link="fr3_link4"/>
-    <child link="fr3_link5"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="12.0" lower="-2.8065" upper="2.8065" velocity="5.26"/>
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8065" soft_upper_limit="2.8065"/>
-    <dynamics D="1" K="7000" damping="0.003" friction="0.2" mu_coulomb="0" mu_viscous="16"/>
-  </joint>
-  <!-- sub-link defining detailed meshes for collision with environment -->
-  <link name="fr3_link6">
-    <visual name="fr3_link6_visual">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_arms/fr3/visual/link6.dae"/>
-      </geometry>
-    </visual>
-    <collision name="fr3_link6_collision">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_arms/fr3/collision/link6.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0597131221 -0.0410294666 -0.0101692726"/>
-      <mass value="1.8170376524"/>
-      <inertia ixx="0.005412333594383447" ixy="0.006193456360285834" ixz="0.0014219289306117652" iyy="0.014058329545509979" iyz="-0.0013140753741120031" izz="0.016080817924212554"/>
-    </inertial>
-  </link>
-  <joint name="fr3_joint6" type="revolute">
-    <origin rpy="1.570796326794897 0 0" xyz="0 0 0"/>
-    <parent link="fr3_link5"/>
-    <child link="fr3_link6"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="12.0" lower="0.5445" upper="4.5169" velocity="4.18"/>
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="0.5445" soft_upper_limit="4.5169"/>
-    <dynamics D="1" K="7000" damping="0.003" friction="0.2" mu_coulomb="0" mu_viscous="16"/>
-  </joint>
-  <!-- sub-link defining detailed meshes for collision with environment -->
-  <link name="fr3_link7">
-    <visual name="fr3_link7_visual">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_arms/fr3/visual/link7.dae"/>
-      </geometry>
-    </visual>
-    <collision name="fr3_link7_collision">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_arms/fr3/collision/link7.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0045225817 0.0086261921 -0.0161633251"/>
-      <mass value="0.6271432862"/>
-      <inertia ixx="0.00021092389150104718" ixy="-2.433299114461931e-05" ixz="4.564480393778983e-05" iyy="0.00017718568002411474" iyz="8.744070223226438e-05" izz="5.993190599659971e-05"/>
-    </inertial>
-  </link>
-  <joint name="fr3_joint7" type="revolute">
-    <origin rpy="1.570796326794897 0 0" xyz="0.088 0 0"/>
-    <parent link="fr3_link6"/>
-    <child link="fr3_link7"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="12.0" lower="-3.0159" upper="3.0159" velocity="5.26"/>
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0159" soft_upper_limit="3.0159"/>
-    <dynamics D="1" K="7000" damping="0.003" friction="0.2" mu_coulomb="0" mu_viscous="16"/>
-  </joint>
-  <link name="fr3_link8"/>
-  <joint name="fr3_joint8" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.107"/>
-    <parent link="fr3_link7"/>
-    <child link="fr3_link8"/>
-  </joint>
-  <joint name="fr3_hand_joint" type="fixed">
-    <parent link="fr3_link8"/>
-    <child link="fr3_hand"/>
-    <origin rpy="0 0 -0.7853981633974483" xyz="0 0 0"/>
-  </joint>
-  <!-- sub-link defining detailed meshes for collision with environment -->
-  <link name="fr3_hand">
-    <visual name="fr3_hand_visual">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_ee/franka_hand_white/visual/hand.dae"/>
-      </geometry>
-    </visual>
-    <collision name="fr3_hand_collision">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_ee/franka_hand_white/collision/hand.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.01 0 0.03"/>
-      <mass value="0.73"/>
-      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.0025" iyz="0" izz="0.0017"/>
-    </inertial>
-  </link>
-  <!-- Define the hand_tcp frame -->
-  <link name="fr3_hand_tcp"/>
-  <joint name="fr3_hand_tcp_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.1034"/>
-    <parent link="fr3_hand"/>
-    <child link="fr3_hand_tcp"/>
-  </joint>
-  <link name="fr3_leftfinger">
-    <visual name="fr3_leftfinger_visual">
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_ee/franka_hand_white/visual/finger.dae"/>
-      </geometry>
-    </visual>
-    <!-- screw mount -->
-    <collision>
-      <origin rpy="0 0 0" xyz="0 18.5e-3 11e-3"/>
-      <geometry>
-        <box size="22e-3 15e-3 20e-3"/>
-      </geometry>
-    </collision>
-    <!-- cartriage sledge -->
-    <collision>
-      <origin rpy="0 0 0" xyz="0 6.8e-3 2.2e-3"/>
-      <geometry>
-        <box size="22e-3 8.8e-3 3.8e-3"/>
-      </geometry>
-    </collision>
-    <!-- diagonal finger -->
-    <collision>
-      <origin rpy="0.5235987755982988 0 0" xyz="0 15.9e-3 28.35e-3"/>
-      <geometry>
-        <box size="17.5e-3 7e-3 23.5e-3"/>
-      </geometry>
-    </collision>
-    <!-- rubber tip with which to grasp -->
-    <collision>
-      <origin rpy="0 0 0" xyz="0 7.58e-3 45.25e-3"/>
-      <geometry>
-        <box size="17.5e-3 15.2e-3 18.5e-3"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="fr3_rightfinger">
-    <visual name="fr3_rightfinger_visual">
-      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://franka_description/meshes/robot_ee/franka_hand_white/visual/finger.dae"/>
-      </geometry>
-    </visual>
-    <!-- screw mount -->
-    <collision>
-      <origin rpy="0 0 0" xyz="0 -18.5e-3 11e-3"/>
-      <geometry>
-        <box size="22e-3 15e-3 20e-3"/>
-      </geometry>
-    </collision>
-    <!-- cartriage sledge -->
-    <collision>
-      <origin rpy="0 0 0" xyz="0 -6.8e-3 2.2e-3"/>
-      <geometry>
-        <box size="22e-3 8.8e-3 3.8e-3"/>
-      </geometry>
-    </collision>
-    <!-- diagonal finger -->
-    <collision>
-      <origin rpy="-0.5235987755982988 0 0" xyz="0 -15.9e-3 28.35e-3"/>
-      <geometry>
-        <box size="17.5e-3 7e-3 23.5e-3"/>
-      </geometry>
-    </collision>
-    <!-- rubber tip with which to grasp -->
-    <collision>
-      <origin rpy="0 0 0" xyz="0 -7.58e-3 45.25e-3"/>
-      <geometry>
-        <box size="17.5e-3 15.2e-3 18.5e-3"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="fr3_finger_joint1" type="prismatic">
-    <parent link="fr3_hand"/>
-    <child link="fr3_leftfinger"/>
-    <origin rpy="0 0 0" xyz="0 0 0.0584"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="100" lower="0.0" upper="0.04" velocity="0.2"/>
-    <dynamics damping="0.3"/>
-  </joint>
-  <joint name="fr3_finger_joint2" type="prismatic">
-    <parent link="fr3_hand"/>
-    <child link="fr3_rightfinger"/>
-    <origin rpy="0 0 0" xyz="0 0 0.0584"/>
-    <axis xyz="0 -1 0"/>
-    <limit effort="100" lower="0.0" upper="0.04" velocity="0.2"/>
-    <mimic joint="fr3_finger_joint1"/>
-    <dynamics damping="0.3"/>
-  </joint>
-   --&gt;
-    
-</robot>
diff --git a/pybullet_tree_sim/urdf/robot/base/robot.urdf.xacro b/pybullet_tree_sim/urdf/robot/generic/robot.urdf.xacro
similarity index 82%
rename from pybullet_tree_sim/urdf/robot/base/robot.urdf.xacro
rename to pybullet_tree_sim/urdf/robot/generic/robot.urdf.xacro
index 0b149df..b9f01a6 100644
--- a/pybullet_tree_sim/urdf/robot/base/robot.urdf.xacro
+++ b/pybullet_tree_sim/urdf/robot/generic/robot.urdf.xacro
@@ -1,36 +1,38 @@
 <?xml version ="1.0"?>
-<robot xmlns:xacro="http://wiki.ros.org/xacro">
+<robot xmlns:xacro="http://wiki.ros.org/xacro" name="pruning_robot">
     <!-- robot name parameter -->
-    <xacro:arg name="name" default="robot_pybullet_tree_sim"/>
-    <xacro:property name="arm_base_parent" value="world" />
-    <xacro:property name="end_effector_parent" value="" />
+    <xacro:arg name="name" default="pruning_robot"/>
+    <!-- <xacro:arg name="robot_parent" default="world" /> -->
+
+    <xacro:property name="base_attachment_parent" value="world"/>
+    <xacro:property name="robot_arm_parent" value="world"/>
+    <xacro:property name="end_effector_parent" value="world"/>
 
-    
     <!-- create link fixed to the "world" -->
     <link name="world" />
-    
-    
+
+
     <!-- Base attachment -->
     <xacro:arg name="base_attachment_type" default="" />
 
     <xacro:if value="${ '$(arg base_attachment_type)' == 'linear_slider' }" >
         <!-- linear slider -->
         <xacro:include filename="$(find linear_slider_description)/urdf/macro/linear_slider_base.urdf.xacro" />
-    
-        <xacro:arg name="linear_slider_parent" default="world"/>
+
+        <xacro:arg name="linear_slider_parent" default="${base_attachment_parent}"/>
         <xacro:arg name="linear_slider_robot_ip" default="169.254.57.177"/> <!-- TODO: add this into the macro, but instead get from yaml file-->
-        <xacro:arg name="prefix" default="''" />
+        <xacro:arg name="base_prefix" default="linear_slider__" />
         <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"/>
-        
+
         <!-- Set slider moving base as arm parent joint -->
-        <xacro:property name="arm_base_parent" value="$(arg prefix)tool0" />
-            
+        <xacro:property name="robot_arm_parent" value="$(arg base_prefix)tool0" />
+
         <xacro:linear_slider
             parent="$(arg linear_slider_parent)"
-            prefix="$(arg prefix)"
+            prefix="$(arg base_prefix)"
             sim_gazebo="$(arg sim_gazebo)"
             sim_gazebo_classic="$(arg sim_gazebo_classic)"
             use_mock_hardware="$(arg use_mock_hardware)"
@@ -42,16 +44,17 @@
     </xacro:if>
 
     <!-- Robot arm type. Current types are UR and Franka Emika -->
-    <xacro:arg name="robot_type" default="robot_type" />
-    <xacro:if value="${ '$(arg robot_type)' == 'ur5' }">
+    <xacro:arg name="arm_type" default="arm_type" />
+    <xacro:if value="${ '$(arg arm_type)' == 'ur5' }">
         <xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>
         <!-- UR -->
         <!-- possible 'ur_type' values: ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e, ur20, ur30 -->
         <!-- the default value should raise an error in case this was called without defining the type -->
         <xacro:arg name="ur_type" default="ur5x"/>
-        
+        <xacro:arg name="ur_parent" default="${robot_arm_parent}"/>
+
         <!-- parameters -->
-        <xacro:arg name="tf_prefix" default="" />
+        <xacro:arg name="tf_prefix" default="ur5__" />
         <xacro:arg name="joint_limit_params" default="$(find ur_description)/config/$(arg ur_type)/joint_limits.yaml"/>
         <xacro:arg name="kinematics_params" default="$(find ur_description)/config/$(arg ur_type)/default_kinematics.yaml"/>
         <xacro:arg name="physical_params" default="$(find ur_description)/config/$(arg ur_type)/physical_parameters.yaml"/>
@@ -81,25 +84,25 @@
         <xacro:arg name="tool_tx_idle_chars" default="3.5" />
         <xacro:arg name="tool_device_name" default="/tmp/ttyUR" />
         <xacro:arg name="tool_tcp_port" default="54321" />
-        
+
             <!-- Simulation parameters -->
         <xacro:arg name="use_fake_hardware" default="false" />
         <xacro:arg name="fake_sensor_commands" default="false" />
         <xacro:arg name="sim_gazebo" default="false" />
         <xacro:arg name="sim_ignition" default="false" />
         <xacro:arg name="simulation_controllers" default="" />
-        
+
         <!-- initial position for simulations (Fake Hardware, Gazebo, Ignition) -->
         <xacro:arg name="initial_positions_file" default="$(find ur_description)/config/initial_positions.yaml"/>
-        
+
         <!-- convert to property to use substitution in function -->
         <xacro:property name="initial_positions_file" default="$(arg initial_positions_file)"/>
-        
+
         <!-- arm -->
         <xacro:ur_robot
             name="$(arg name)"
             tf_prefix="$(arg tf_prefix)"
-            parent="world"
+            parent="$(arg ur_parent)"
             joint_limits_parameters_file="$(arg joint_limit_params)"
             kinematics_parameters_file="$(arg kinematics_params)"
             physical_parameters_file="$(arg physical_params)"
@@ -135,8 +138,13 @@
             >
             <origin xyz="0 0 0" rpy="0 0 0" />          <!-- position robot in the world -->
         </xacro:ur_robot>
+
+        <!-- Update end-effector parent -->
+        <xacro:property name="end_effector_parent" value="$(arg tf_prefix)tool0"/>
+
+
     </xacro:if>
-    <xacro:if value="${ '$(arg robot_type)' == 'fr3' }">
+    <xacro:if value="${ '$(arg arm_type)' == 'fr3' }">
         <xacro:include filename="$(find franka_description)/robots/common/franka_robot.xacro"/>
         <!-- Name for this robot -->
         <xacro:arg name="arm_id" default="fr3" />
@@ -173,15 +181,34 @@
                             fake_sensor_commands="$(arg fake_sensor_commands)"
                             gazebo_effort="$(arg gazebo_effort)">
         </xacro:franka_robot>
+
+        <!-- <xacro:property name="end_effector_parent" value="$(arg fr3_prefix)tool0"/> -->
+
     </xacro:if>
-    
+
     <!-- Robot arm base/eef attachments -->
     <xacro:arg name="end_effector_type" default=""/> -->
     <xacro:if value="${ '$(arg end_effector_type)' == 'apple_picker' }">
-        
+
     </xacro:if>
-    <xacro:if value="${ '$(arg end_effector_type)' == 'pruner' }" >
-        
+    <xacro:if value="${ '$(arg end_effector_type)' == 'mock_pruner' }" >
+        <xacro:include filename="../../end_effectors/mock_pruner/macro/mock_pruner_macro.urdf.xacro"/>
+
+        <!-- Args -->
+        <xacro:arg name="eef_parent" default="${end_effector_parent}" />
+        <xacro:arg name="eef_prefix" default="mock_pruner__" />
+        <xacro:arg name="tof0_offset" default='0.02'/>
+        <xacro:arg name="tof1_offset" default='-0.02'/>
+
+        <xacro:mock_pruner_macro
+            eef_prefix="$(arg eef_prefix)"
+            eef_parent="$(arg eef_parent)"
+            tof0_offset='$(arg tof0_offset)'
+            tof1_offset='$(arg tof1_offset)'
+            camera_offset=''
+        />
+
+
     </xacro:if>
-    
-</robot>
\ No newline at end of file
+
+</robot>
diff --git a/pybullet_tree_sim/urdf/robot/generic/test.urdf b/pybullet_tree_sim/urdf/robot/generic/test.urdf
new file mode 100644
index 0000000..1e5d449
--- /dev/null
+++ b/pybullet_tree_sim/urdf/robot/generic/test.urdf
@@ -0,0 +1,757 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from robot.urdf.xacro               | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="pruning_robot">
+  <!-- <xacro:arg name="robot_parent" default="world" /> -->
+  <!-- create link fixed to the "world" -->
+  <link name="world"/>
+  <!-- ================== -->
+  <!--     Properties     -->
+  <!-- ================== -->
+  <!-- ================== -->
+  <!--     Materials      -->
+  <!-- ================== -->
+  <material name="alpha_black">
+    <color rgba="0.6 0.6 0.6 1"/>
+  </material>
+  <material name="red_v">
+    <color rgba="1 0 0 1"/>
+  </material>
+  <!-- ================== -->
+  <!--       Links        -->
+  <!-- ================== -->
+  <!-- base_link -->
+  <link name="linear_slider__base_link">
+    <visual>
+      <material name="alpha_black"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <geometry>
+        <mesh filename="file:///home/luke/ros2_ws/install/linear_slider_description/share/linear_slider_description/meshes/7thlink_fixed.STL" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="1.0 0.3 0.10"/>
+        <!-- <mesh
+                        filename="package://linear_slider_description/meshes/7thlink_fixed.STL"
+                        scale="${model_scale}"
+                    /> -->
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="20.0"/>
+      <origin xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.061" ixy="0.0" ixz="0.0" iyy="0.65" iyz="0.0" izz="0.661"/>
+    </inertial>
+  </link>
+  <gazebo reference="linear_slider__base_link">
+    <material>Gazebo/Gray</material>
+  </gazebo>
+  <!-- moving_base link -->
+  <link name="linear_slider__moving_base">
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <material name="red_v"/>
+      <geometry>
+        <mesh filename="file:///home/luke/ros2_ws/install/linear_slider_description/share/linear_slider_description/meshes/7thlink_move.STL" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.22 0.202 0.118"/>
+        <!-- <mesh
+                        filename="package://linear_slider_description/meshes/7thlink_move.STL"
+                        scale="${model_scale}"
+                    /> -->
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="10.0"/>
+      <origin xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.0915" ixy="0.0" ixz="0.0" iyy="0.975" iyz="0.0" izz="0.9915"/>
+    </inertial>
+  </link>
+  <gazebo reference="linear_slider__moving_base">
+    <material>Gazebo/Red</material>
+  </gazebo>
+  <!-- tool link -->
+  <link name="linear_slider__tool0"/>
+  <link name="linear_slider__base"/>
+  <!-- Frame for mounting EEF models to a manipulator. x+ axis points forward (REP 103). -->
+  <link name="linear_slider__flange"/>
+  <!-- ================== -->
+  <!--       Joints       -->
+  <!-- ================== -->
+  <!-- base_joint fixes base_link to the environment -->
+  <joint name="linear_slider__base_joint" type="fixed">
+    <parent link="world"/>
+    <child link="linear_slider__base"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <joint name="linear_slider__base_link__base_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="linear_slider__base"/>
+    <child link="linear_slider__base_link"/>
+  </joint>
+  <!-- joint1, moving prismatic joint -->
+  <joint name="linear_slider__joint1" type="prismatic">
+    <parent link="linear_slider__base_link"/>
+    <child link="linear_slider__moving_base"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="0.05" lower="-0.4" upper="0.4" velocity="0.08333333333333334"/>
+    <!-- effort is attribute for enforcing maximum joint effort, between 0 and 100% -->
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <!-- Define where the joint will be located, defined in terms of parent's reference frame -->
+  </joint>
+  <!-- Extra joints for ros-industrial standard compatability -->
+  <!-- tool frame to fixed frame -->
+  <joint name="linear_slider__moving_base__flange_joint" type="fixed">
+    <parent link="linear_slider__moving_base"/>
+    <child link="linear_slider__flange"/>
+    <origin rpy="0 0 0" xyz="0 0 0.05"/>
+  </joint>
+  <joint name="linear_slider__flange__tool0_joint" type="fixed">
+    <parent link="linear_slider__flange"/>
+    <child link="linear_slider__tool0"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <!-- The caret ^ indicates to use the outer-scope property (with same name). The pipe | indicates to use the given fallback if the property is not defined in outer scope. -->
+  <!-- ================== -->
+  <!--    ros2_control    -->
+  <!-- ================== -->
+  <ros2_control name="linear_slider_ros2_control" type="system">
+    <!-- Hardware -->
+    <hardware>
+      <plugin>linear_slider_system_interface/LinearSliderSystemInterface</plugin>
+      <!-- <param name="device_name">linear_slider_device</param>
+                <param name="joint_name">prismatic_joint</param>
+                <param name="robot_ip">169.254.97.178</param> -->
+    </hardware>
+    <!-- Joints -->
+    <joint name="linear_slider__joint1">
+      <!-- Command interface -->
+      <!-- <command_interface name="position">
+                    <param name="min">-0.40</param>
+                    <param name="max">0.40</param>
+                </command_interface> -->
+      <command_interface name="velocity">
+        <param name="min">-0.1</param>
+        <param name="max">0.1</param>
+      </command_interface>
+      <!-- State interface -->
+      <state_interface name="position">
+        <param name="initial_value">-0.25</param>
+      </state_interface>
+      <state_interface name="velocity">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+    </joint>
+    <!-- Sensors -->
+    <sensor name="linear_slider__lim_switch_neg">
+      <state_interface name="state">
+        <param name="initial_value">0</param>
+      </state_interface>
+      <param name="frame_id">linear_slider__base_link</param>
+    </sensor>
+    <sensor name="linear_slider__lim_switch_pos">
+      <state_interface name="state">
+        <param name="initial_value">0</param>
+      </state_interface>
+      <param name="frame_id">linear_slider__base_link</param>
+    </sensor>
+  </ros2_control>
+  <!--
+    Base UR robot series xacro macro.
+
+    NOTE this is NOT a URDF. It cannot directly be loaded by consumers
+    expecting a flattened '.urdf' file. See the top-level '.xacro' for that
+    (but note that .xacro must still be processed by the xacro command).
+
+    This file models the base kinematic chain of a UR robot, which then gets
+    parameterised by various configuration files to convert it into a UR3(e),
+    UR5(e), UR10(e) or UR16e.
+
+    NOTE the default kinematic parameters (i.e., link lengths, frame locations,
+    offsets, etc) do not correspond to any particular robot. They are defaults
+    only. There WILL be non-zero offsets between the Forward Kinematics results
+    in TF (i.e., robot_state_publisher) and the values reported by the Teach
+    Pendant.
+
+    For accurate (and robot-specific) transforms, the 'kinematics_parameters_file'
+    parameter MUST point to a .yaml file containing the appropriate values for
+    the targeted robot.
+
+    If using the UniversalRobots/Universal_Robots_ROS_Driver, follow the steps
+    described in the readme of that repository to extract the kinematic
+    calibration from the controller and generate the required .yaml file.
+
+    Main author of the migration to yaml configs Ludovic Delval.
+
+    Contributors to previous versions (in no particular order)
+
+     - Denis Stogl
+     - Lovro Ivanov
+     - Felix Messmer
+     - Kelsey Hawkins
+     - Wim Meeussen
+     - Shaun Edwards
+     - Nadia Hammoudeh Garcia
+     - Dave Hershberger
+     - G. vd. Hoorn
+     - Philip Long
+     - Dave Coleman
+     - Miguel Prada
+     - Mathias Luedtke
+     - Marcel Schnirring
+     - Felix von Drigalski
+     - Felix Exner
+     - Jimmy Da Silva
+     - Ajit Krisshna N L
+     - Muhammad Asif Rana
+  -->
+  <!--
+    NOTE the macro defined in this file is NOT part of the public API of this
+          package. Users CANNOT rely on this file being available, or stored in
+          this location. Nor can they rely on the existence of the macro.
+  -->
+  <ros2_control name="pruning_robot" type="system">
+    <hardware>
+      <plugin>ur_robot_driver/URPositionHardwareInterface</plugin>
+      <param name="robot_ip">0.0.0.0</param>
+      <param name="script_filename"></param>
+      <param name="output_recipe_filename"></param>
+      <param name="input_recipe_filename"></param>
+      <param name="headless_mode">False</param>
+      <param name="reverse_port">50001</param>
+      <param name="script_sender_port">50002</param>
+      <param name="reverse_ip">0.0.0.0</param>
+      <param name="script_command_port">50004</param>
+      <param name="trajectory_port">50003</param>
+      <param name="tf_prefix">ur5e__</param>
+      <param name="non_blocking_read">True</param>
+      <param name="servoj_gain">2000</param>
+      <param name="servoj_lookahead_time">0.03</param>
+      <param name="use_tool_communication">False</param>
+      <param name="kinematics/hash">calib_12788084448423163542</param>
+      <param name="tool_voltage">0</param>
+      <param name="tool_parity">0</param>
+      <param name="tool_baud_rate">115200</param>
+      <param name="tool_stop_bits">1</param>
+      <param name="tool_rx_idle_chars">1.5</param>
+      <param name="tool_tx_idle_chars">3.5</param>
+      <param name="tool_device_name">/tmp/ttyUR</param>
+      <param name="tool_tcp_port">54321</param>
+      <param name="keep_alive_count">2</param>
+    </hardware>
+    <joint name="ur5e__shoulder_pan_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <state_interface name="position">
+        <!-- initial position for the FakeSystem and simulation -->
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="ur5e__shoulder_lift_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <state_interface name="position">
+        <!-- initial position for the FakeSystem and simulation -->
+        <param name="initial_value">-1.57</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="ur5e__elbow_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <state_interface name="position">
+        <!-- initial position for the FakeSystem and simulation -->
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="ur5e__wrist_1_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <state_interface name="position">
+        <!-- initial position for the FakeSystem and simulation -->
+        <param name="initial_value">-1.57</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="ur5e__wrist_2_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <state_interface name="position">
+        <!-- initial position for the FakeSystem and simulation -->
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="ur5e__wrist_3_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <state_interface name="position">
+        <!-- initial position for the FakeSystem and simulation -->
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <sensor name="ur5e__tcp_fts_sensor">
+      <state_interface name="force.x"/>
+      <state_interface name="force.y"/>
+      <state_interface name="force.z"/>
+      <state_interface name="torque.x"/>
+      <state_interface name="torque.y"/>
+      <state_interface name="torque.z"/>
+    </sensor>
+    <sensor name="ur5e__tcp_pose">
+      <state_interface name="position.x"/>
+      <state_interface name="position.y"/>
+      <state_interface name="position.z"/>
+      <state_interface name="orientation.x"/>
+      <state_interface name="orientation.y"/>
+      <state_interface name="orientation.z"/>
+      <state_interface name="orientation.w"/>
+    </sensor>
+    <!-- NOTE The following are joints used only for testing with fake hardware and will change in the future -->
+    <gpio name="ur5e__speed_scaling">
+      <state_interface name="speed_scaling_factor"/>
+      <param name="initial_speed_scaling_factor">1</param>
+      <command_interface name="target_speed_fraction_cmd"/>
+      <command_interface name="target_speed_fraction_async_success"/>
+    </gpio>
+    <gpio name="ur5e__gpio">
+      <command_interface name="standard_digital_output_cmd_0"/>
+      <command_interface name="standard_digital_output_cmd_1"/>
+      <command_interface name="standard_digital_output_cmd_2"/>
+      <command_interface name="standard_digital_output_cmd_3"/>
+      <command_interface name="standard_digital_output_cmd_4"/>
+      <command_interface name="standard_digital_output_cmd_5"/>
+      <command_interface name="standard_digital_output_cmd_6"/>
+      <command_interface name="standard_digital_output_cmd_7"/>
+      <command_interface name="standard_digital_output_cmd_8"/>
+      <command_interface name="standard_digital_output_cmd_9"/>
+      <command_interface name="standard_digital_output_cmd_10"/>
+      <command_interface name="standard_digital_output_cmd_11"/>
+      <command_interface name="standard_digital_output_cmd_12"/>
+      <command_interface name="standard_digital_output_cmd_13"/>
+      <command_interface name="standard_digital_output_cmd_14"/>
+      <command_interface name="standard_digital_output_cmd_15"/>
+      <command_interface name="standard_digital_output_cmd_16"/>
+      <command_interface name="standard_digital_output_cmd_17"/>
+      <command_interface name="standard_analog_output_cmd_0"/>
+      <command_interface name="standard_analog_output_cmd_1"/>
+      <command_interface name="analog_output_domain_cmd"/>
+      <command_interface name="tool_voltage_cmd"/>
+      <command_interface name="io_async_success"/>
+      <state_interface name="digital_output_0"/>
+      <state_interface name="digital_output_1"/>
+      <state_interface name="digital_output_2"/>
+      <state_interface name="digital_output_3"/>
+      <state_interface name="digital_output_4"/>
+      <state_interface name="digital_output_5"/>
+      <state_interface name="digital_output_6"/>
+      <state_interface name="digital_output_7"/>
+      <state_interface name="digital_output_8"/>
+      <state_interface name="digital_output_9"/>
+      <state_interface name="digital_output_10"/>
+      <state_interface name="digital_output_11"/>
+      <state_interface name="digital_output_12"/>
+      <state_interface name="digital_output_13"/>
+      <state_interface name="digital_output_14"/>
+      <state_interface name="digital_output_15"/>
+      <state_interface name="digital_output_16"/>
+      <state_interface name="digital_output_17"/>
+      <state_interface name="digital_input_0"/>
+      <state_interface name="digital_input_1"/>
+      <state_interface name="digital_input_2"/>
+      <state_interface name="digital_input_3"/>
+      <state_interface name="digital_input_4"/>
+      <state_interface name="digital_input_5"/>
+      <state_interface name="digital_input_6"/>
+      <state_interface name="digital_input_7"/>
+      <state_interface name="digital_input_8"/>
+      <state_interface name="digital_input_9"/>
+      <state_interface name="digital_input_10"/>
+      <state_interface name="digital_input_11"/>
+      <state_interface name="digital_input_12"/>
+      <state_interface name="digital_input_13"/>
+      <state_interface name="digital_input_14"/>
+      <state_interface name="digital_input_15"/>
+      <state_interface name="digital_input_16"/>
+      <state_interface name="digital_input_17"/>
+      <state_interface name="standard_analog_output_0"/>
+      <state_interface name="standard_analog_output_1"/>
+      <state_interface name="standard_analog_input_0"/>
+      <state_interface name="standard_analog_input_1"/>
+      <state_interface name="analog_io_type_0"/>
+      <state_interface name="analog_io_type_1"/>
+      <state_interface name="analog_io_type_2"/>
+      <state_interface name="analog_io_type_3"/>
+      <state_interface name="tool_mode"/>
+      <state_interface name="tool_output_voltage"/>
+      <state_interface name="tool_output_current"/>
+      <state_interface name="tool_temperature"/>
+      <state_interface name="tool_analog_input_0"/>
+      <state_interface name="tool_analog_input_1"/>
+      <state_interface name="tool_analog_input_type_0"/>
+      <state_interface name="tool_analog_input_type_1"/>
+      <state_interface name="robot_mode"/>
+      <state_interface name="robot_status_bit_0"/>
+      <state_interface name="robot_status_bit_1"/>
+      <state_interface name="robot_status_bit_2"/>
+      <state_interface name="robot_status_bit_3"/>
+      <state_interface name="safety_mode"/>
+      <state_interface name="safety_status_bit_0"/>
+      <state_interface name="safety_status_bit_1"/>
+      <state_interface name="safety_status_bit_2"/>
+      <state_interface name="safety_status_bit_3"/>
+      <state_interface name="safety_status_bit_4"/>
+      <state_interface name="safety_status_bit_5"/>
+      <state_interface name="safety_status_bit_6"/>
+      <state_interface name="safety_status_bit_7"/>
+      <state_interface name="safety_status_bit_8"/>
+      <state_interface name="safety_status_bit_9"/>
+      <state_interface name="safety_status_bit_10"/>
+      <state_interface name="program_running"/>
+    </gpio>
+    <gpio name="ur5e__payload">
+      <command_interface name="mass"/>
+      <command_interface name="cog.x"/>
+      <command_interface name="cog.y"/>
+      <command_interface name="cog.z"/>
+      <command_interface name="payload_async_success"/>
+    </gpio>
+    <gpio name="ur5e__resend_robot_program">
+      <command_interface name="resend_robot_program_cmd"/>
+      <command_interface name="resend_robot_program_async_success"/>
+    </gpio>
+    <gpio name="ur5e__hand_back_control">
+      <command_interface name="hand_back_control_cmd"/>
+      <command_interface name="hand_back_control_async_success"/>
+    </gpio>
+    <gpio name="ur5e__zero_ftsensor">
+      <command_interface name="zero_ftsensor_cmd"/>
+      <command_interface name="zero_ftsensor_async_success"/>
+    </gpio>
+    <gpio name="ur5e__system_interface">
+      <state_interface name="initialized"/>
+    </gpio>
+    <gpio name="ur5e__get_robot_software_version">
+      <state_interface name="get_version_major"/>
+      <state_interface name="get_version_minor"/>
+      <state_interface name="get_version_build"/>
+      <state_interface name="get_version_bugfix"/>
+    </gpio>
+  </ros2_control>
+  <!-- Add URDF transmission elements (for ros_control) -->
+  <!--<xacro:ur_arm_transmission prefix="${prefix}" hw_interface="${transmission_hw_interface}" />-->
+  <!-- Placeholder for ros2_control transmission which don't yet exist -->
+  <!-- links -  main serial chain -->
+  <link name="ur5e__base_link"/>
+  <link name="ur5e__base_link_inertia">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/visual/base.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/collision/base.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="4.0"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
+    </inertial>
+  </link>
+  <link name="ur5e__shoulder_link">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/visual/shoulder.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/collision/shoulder.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="3.761"/>
+      <origin rpy="0 0 0" xyz="0.0 -0.00193 -0.02561"/>
+      <inertia ixx="0.010267499999999999" ixy="0" ixz="0" iyy="0.010267499999999999" iyz="0" izz="0.00666"/>
+    </inertial>
+  </link>
+  <link name="ur5e__upper_arm_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.138"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/visual/upperarm.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.138"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/collision/upperarm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="8.058"/>
+      <origin rpy="0 1.570796326794897 0" xyz="-0.2125 0.0 0.11336"/>
+      <inertia ixx="0.13388583541666665" ixy="0" ixz="0" iyy="0.13388583541666665" iyz="0" izz="0.0151074"/>
+    </inertial>
+  </link>
+  <link name="ur5e__forearm_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.007"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/visual/forearm.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.007"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/collision/forearm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="2.846"/>
+      <origin rpy="0 1.570796326794897 0" xyz="-0.2422 0.0 0.0265"/>
+      <inertia ixx="0.03120936758333333" ixy="0" ixz="0" iyy="0.03120936758333333" iyz="0" izz="0.004095"/>
+    </inertial>
+  </link>
+  <link name="ur5e__wrist_1_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.127"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/visual/wrist1.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.127"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/collision/wrist1.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.37"/>
+      <origin rpy="0 0 0" xyz="0.0 -0.01634 -0.0018"/>
+      <inertia ixx="0.0025599" ixy="0" ixz="0" iyy="0.0025599" iyz="0" izz="0.0021942"/>
+    </inertial>
+  </link>
+  <link name="ur5e__wrist_2_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.0997"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/visual/wrist2.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.0997"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/collision/wrist2.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.3"/>
+      <origin rpy="0 0 0" xyz="0.0 0.01634 -0.0018"/>
+      <inertia ixx="0.0025599" ixy="0" ixz="0" iyy="0.0025599" iyz="0" izz="0.0021942"/>
+    </inertial>
+  </link>
+  <link name="ur5e__wrist_3_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.0989"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/visual/wrist3.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.0989"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/collision/wrist3.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.365"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 -0.001159"/>
+      <inertia ixx="9.890414008333333e-05" ixy="0" ixz="0" iyy="9.890414008333333e-05" iyz="0" izz="0.0001321171875"/>
+    </inertial>
+  </link>
+  <!-- base_joint fixes base_link to the environment -->
+  <joint name="ur5e__base_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="linear_slider__tool0"/>
+    <child link="ur5e__base_link"/>
+  </joint>
+  <!-- joints - main serial chain -->
+  <joint name="ur5e__base_link-base_link_inertia" type="fixed">
+    <parent link="ur5e__base_link"/>
+    <child link="ur5e__base_link_inertia"/>
+    <!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
+           frames of the robot/controller have X+ pointing backwards.
+           Use the joint between 'base_link' and 'base_link_inertia' (a dummy
+           link/frame) to introduce the necessary rotation over Z (of pi rad).
+      -->
+    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+  </joint>
+  <joint name="ur5e__shoulder_pan_joint" type="revolute">
+    <parent link="ur5e__base_link_inertia"/>
+    <child link="ur5e__shoulder_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0.1625"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="ur5e__shoulder_lift_joint" type="revolute">
+    <parent link="ur5e__shoulder_link"/>
+    <child link="ur5e__upper_arm_link"/>
+    <origin rpy="1.570796327 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="ur5e__elbow_joint" type="revolute">
+    <parent link="ur5e__upper_arm_link"/>
+    <child link="ur5e__forearm_link"/>
+    <origin rpy="0 0 0" xyz="-0.425 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="ur5e__wrist_1_joint" type="revolute">
+    <parent link="ur5e__forearm_link"/>
+    <child link="ur5e__wrist_1_link"/>
+    <origin rpy="0 0 0" xyz="-0.3922 0 0.1333"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="28.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="ur5e__wrist_2_joint" type="revolute">
+    <parent link="ur5e__wrist_1_link"/>
+    <child link="ur5e__wrist_2_link"/>
+    <origin rpy="1.570796327 0 0" xyz="0 -0.0997 -2.044881182297852e-11"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="28.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="ur5e__wrist_3_joint" type="revolute">
+    <parent link="ur5e__wrist_2_link"/>
+    <child link="ur5e__wrist_3_link"/>
+    <origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.0996 -2.042830148012698e-11"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="28.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <link name="ur5e__ft_frame"/>
+  <joint name="ur5e__wrist_3_link-ft_frame" type="fixed">
+    <parent link="ur5e__wrist_3_link"/>
+    <child link="ur5e__ft_frame"/>
+    <origin rpy="3.141592653589793 0 0" xyz="0 0 0"/>
+  </joint>
+  <!-- ROS-Industrial 'base' frame - base_link to UR 'Base' Coordinates transform -->
+  <link name="ur5e__base"/>
+  <joint name="ur5e__base_link-base_fixed_joint" type="fixed">
+    <!-- Note the rotation over Z of pi radians - as base_link is REP-103
+           aligned (i.e., has X+ forward, Y+ left and Z+ up), this is needed
+           to correctly align 'base' with the 'Base' coordinate system of
+           the UR controller.
+      -->
+    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+    <parent link="ur5e__base_link"/>
+    <child link="ur5e__base"/>
+  </joint>
+  <!-- ROS-Industrial 'flange' frame - attachment point for EEF models -->
+  <link name="ur5e__flange"/>
+  <joint name="ur5e__wrist_3-flange" type="fixed">
+    <parent link="ur5e__wrist_3_link"/>
+    <child link="ur5e__flange"/>
+    <origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
+  </joint>
+  <!-- ROS-Industrial 'tool0' frame - all-zeros tool frame -->
+  <link name="ur5e__tool0"/>
+  <joint name="ur5e__flange-tool0" type="fixed">
+    <!-- default toolframe - X+ left, Y+ up, Z+ front -->
+    <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
+    <parent link="ur5e__flange"/>
+    <child link="ur5e__tool0"/>
+  </joint>
+   --&gt;
+    
+  <!-- <xacro:property name="end_effector_parent" value="" /> -->
+  <!-- Materials -->
+  <!-- ================== -->
+  <!--       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="ur5e__tool0--mock_pruner__base_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="ur5e__tool0"/>
+    <child link="mock_pruner__base"/>
+  </joint>
+  <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="mock_pruner__base--mock_pruner__tool0" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="mock_pruner__base"/>
+    <child link="mock_pruner__tool0"/>
+  </joint>
+</robot>
diff --git a/pybullet_tree_sim/utils/camera_helpers.py b/pybullet_tree_sim/utils/camera_helpers.py
index 3e87f33..ce374a4 100644
--- a/pybullet_tree_sim/utils/camera_helpers.py
+++ b/pybullet_tree_sim/utils/camera_helpers.py
@@ -39,6 +39,7 @@ def get_fov_from_dfov(camera_width: int, camera_height: int, dFoV: Union[int, fl
     fov_w = 2 * np.arctan(np.tan(_dfov / 2) * camera_width / camera_diag)
     return (np.rad2deg(fov_w), np.rad2deg(fov_h))
 
+
 # def get_pyb_proj_mat(vfov: float, aspect: float, nearVal: float, farVal: float):
 #     return pbutils.pbclient.computeProjectionMatrixFOV(
 #         fov=vfov, aspect=(self.depth_width / self.depth_height), nearVal=near_val, farVal=far_val
diff --git a/pybullet_tree_sim/utils/pyb_utils.py b/pybullet_tree_sim/utils/pyb_utils.py
index a2d3acf..e3db702 100644
--- a/pybullet_tree_sim/utils/pyb_utils.py
+++ b/pybullet_tree_sim/utils/pyb_utils.py
@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 from __future__ import annotations
+
 """
 Author (s): Abhinav Jain, Luke Strohbehn
 """
@@ -13,6 +14,7 @@
 from scipy.constants import g as grav
 
 from pybullet_tree_sim import MESHES_PATH, URDF_PATH, TEXTURES_PATH
+
 # from pybullet_tree_sim.camera import Camera
 # from pybullet_tree_sim.utils.camera_helpers import get_fov_from_dfov
 
@@ -144,7 +146,7 @@ def get_image_at_curr_pose(self, camera, type, view_matrix=None) -> List:
             if view_matrix is None:
                 raise ValueError("view_matrix cannot be None for robot view")
             return self.pbclient.getCameraImage(
-                width=camera.depth_width, # TODO: make separate function for rgb?
+                width=camera.depth_width,  # TODO: make separate function for rgb?
                 height=camera.depth_height,
                 viewMatrix=view_matrix,
                 projectionMatrix=camera.depth_proj_mat,
diff --git a/test/test_camera.py b/test/test_camera.py
index 277d7f8..d3b5635 100644
--- a/test/test_camera.py
+++ b/test/test_camera.py
@@ -3,8 +3,8 @@
 
 import unittest
 
+
 class TestCamera(unittest.TestCase):
     def test_deproject_transform(self):
-        
+
         return
-        
\ No newline at end of file
diff --git a/test/test_helpers.py b/test/test_helpers.py
index ec36693..5ce5d09 100644
--- a/test/test_helpers.py
+++ b/test/test_helpers.py
@@ -6,7 +6,6 @@
 
 
 class TestHelpers(unittest.TestCase):
-
     def test_negative_dfov(self):
         dfov = -65
         cam_w = 100
@@ -15,7 +14,7 @@ def test_negative_dfov(self):
         with self.assertRaises(ValueError) as cm:
             helpers.get_fov_from_dfov(cam_w, cam_h, dfov)
         return
-        
+
     def test_negative_cam_width(self):
         dfov = 65
         cam_w = -100
@@ -24,7 +23,7 @@ def test_negative_cam_width(self):
         with self.assertRaises(ValueError) as cm:
             helpers.get_fov_from_dfov(cam_w, cam_h, dfov)
         return
-        
+
     def test_negative_cam_height(self):
         dfov = 65
         cam_w = 100
@@ -33,10 +32,6 @@ def test_negative_cam_height(self):
         with self.assertRaises(ValueError) as cm:
             helpers.get_fov_from_dfov(cam_w, cam_h, dfov)
         return
-        
-    
-
-
 
 
 # class TestStringMethods(unittest.TestCase):
@@ -55,5 +50,5 @@ def test_negative_cam_height(self):
 #         with self.assertRaises(TypeError):
 #             s.split(2)
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     unittest.main()