Add Tree

SUMMARY

add_tree grafts the root of another TransformTree onto an existing frame, given the transform between the source frame and the subtree's root.

Unlike add and update, add_tree requires transform to already be a 4x4 np.ndarray — it does not accept a 6/7-element pose list, even with rot_type set.

UNITS

transform's translation is in meters.

The Skill

from telekinesis.tf import tftree, tfutils

subtree = tftree.TransformTree("gripper")
camera_T_gripper = tfutils.pose_to_transformation_matrix([0, 0, 0.1, 0, 0, 0], rot_type="deg")
tree.add_tree("camera", subtree, camera_T_gripper)

Example

A mount subtree, with a robot frame of its own, grafted onto the table frame.

The Code

"""
Example of TransformTree.add_tree.
"""

from loguru import logger

from telekinesis.tf import tftree, tfutils


def main():
    """
    Graft another TransformTree in as a subtree.
    """
    tree = tftree.TransformTree("world")
    tree.add("world", "table", [1, 0, 0, 0, 0, 0], rot_type="deg")

    subtree = tftree.TransformTree("mount")
    subtree.add("mount", "robot", [0, 0, 0, 90, 0, 0], rot_type="deg")

    # add_tree requires transform as a 4x4 matrix, unlike add()/update() it does not accept a pose list

    table_T_mount = tfutils.pose_to_transformation_matrix([0.1, 0.1, 0.2, 0, 0, 0], rot_type="deg")
    tree.add_tree("table", subtree, table_T_mount)

    logger.info(f"Frames after add_tree: {list(tree.get_all_frames(reference_node_name='world').keys())}")

    tree.visualize_rerun(axis_len=0.5)

if __name__ == "__main__":
    main()

Parameter Configuration

Parameter	Type	Default	Description
source_name	str	required	The name of the existing frame to graft the subtree onto.
subtree	TransformTree	required	The tree whose root becomes a child of source_name.
transform	np.ndarray	required	The (4, 4) transform from source_name to the subtree's root. Must already be a matrix, a pose list is not accepted regardless of rot_type.
rot_type	str	"mat"	One of "deg", "rad", "quat", "mat".

Raises

Exception	Condition
ValueError	source_name is not a string.
ValueError	subtree is not a TransformTree.
ValueError	transform is not a (4, 4) np.ndarray.
ValueError	rot_type is not one of "deg", "rad", "quat", "mat".
ValueError	No frame named source_name exists in the tree.