Cartesian Goals Example

Cartesian Goals Example#

This example demonstrates how to use CartesianPose goals to move a robot to a specific pose. We will use the PR2 robot for this demonstration.

import os
from cramph.context import StatechartContext
from cramph.executor import SimulationPacer, StatechartExecutor
from giskardpy.motion_control import MotionControl
from giskardpy.motion_statechart.graph_node import EndMotion
from cramph.statechart import Statechart
from giskardpy.motion_statechart.tasks.cartesian_tasks import CartesianPose
from giskardpy.qp.qp_controller_config import QPControllerConfig
from semantic_digital_twin.world import World
from krrood_spatial_math.spatial_types import Pose, Vector3, Quaternion
from semantic_digital_twin.adapters.urdf import URDFParser
from semantic_digital_twin.robots.pr2 import PR2
from semantic_digital_twin.world_description.world_entity import Body
from semantic_digital_twin.datastructures.prefixed_name import PrefixedName
from semantic_digital_twin.world_description.connections import Connection6DoF, OmniDrive

# 1. Load the PR2 robot into the world
# We use the same configuration as in pr2_world_setup
urdf_path = "package://iai_pr2_description/robots/pr2_with_ft2_cableguide.xacro"
urdf_parser = URDFParser.from_file(file_path=urdf_path)
world = urdf_parser.create_world()

# Initialize PR2 semantic annotation
try:
    pr2 = PR2.from_world(world)
except Exception as e:
    print(f"Warning: Could not fully initialize PR2 semantic annotation: {e}")

# Replicate the world structure from world_with_urdf_factory:
# map -> odom_combined -> base_footprint (robot root)
with world.modify_world():
    map_body = Body(name=PrefixedName("map"))
    localization_body = Body(name=PrefixedName("odom_combined"))

    map_C_localization = Connection6DoF.create_with_dofs(
        world, map_body, localization_body
    )
    world.add_connection(map_C_localization)

    c_root_bf = OmniDrive.create_with_dofs(
        parent=localization_body,
        child=world.root,
        world=world,
    )
    world.add_connection(c_root_bf)
    c_root_bf.has_hardware_interface = True

# 2. Set up the executor with motion control, and create a Motion Statechart in its context
executor = StatechartExecutor(
    context=StatechartContext(world=world),
    pacer=SimulationPacer(real_time_factor=None), # None for fastest execution in docs
    extensions=[
        MotionControl(
            qp_controller_config=QPControllerConfig.create_with_simulation_defaults()
        )
    ],
)
statechart = Statechart(context=executor.context)

# 3. Define a Cartesian Pose Goal
# We want to move the left gripper to a specific pose relative to the world root
root_link = world.root
tip_link = world.get_body_by_name("l_gripper_tool_frame")

goal_pose = Pose(
    position=Vector3(0.5, 0.5, 1.2), 
    orientation=Quaternion(0, 0, 0, 1), 
    reference_frame=root_link
)
goal = CartesianPose(
    name="move_to_goal", 
    root_link=root_link, 
    tip_link=tip_link, 
    goal_pose=goal_pose
)

statechart.add_node(goal)
statechart.add_node(EndMotion.when_true(goal))

# 4. Compile and run the statechart
executor.compile(statechart)
executor.tick_until_end(timeout=100)
print(f"Motion finished after {executor.tick_count} cycles.")

---------------------------------------------------------------------------
PathResolutionError                       Traceback (most recent call last)
Cell In[1], line 20

# 1. Load the PR2 robot into the world
# We use the same configuration as in pr2_world_setup
urdf_path = "package://iai_pr2_description/robots/pr2_with_ft2_cableguide.xacro"
---> 20 urdf_parser = URDFParser.from_file(file_path=urdf_path)
world = urdf_parser.create_world()

# Initialize PR2 semantic annotation

File ~/work/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/adapters/urdf.py:170, in URDFParser.from_file(cls, file_path, prefix, path_resolver, use_visual_as_collision_backup)
@classmethod
def from_file(
   cls,
   (...)    167     use_visual_as_collision_backup: bool = False,
) -> URDFParser:
   if file_path.endswith(".xacro"):
--> 170         return cls.from_xacro(
           file_path,
           prefix,
           use_visual_as_collision_backup=use_visual_as_collision_backup,
       )
   path_resolver = path_resolver or CompositePathResolver()
   file_path = path_resolver.resolve(file_path)

File ~/work/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/adapters/urdf.py:214, in URDFParser.from_xacro(cls, xacro_path, prefix, mappings, use_visual_as_collision_backup)
@classmethod
def from_xacro(
   cls,
   (...)    198     use_visual_as_collision_backup: bool = False,
) -> URDFParser:
   """
   Creates a parser from a xacro file by expanding it to URDF.

   (...)    212     :return: A parser for the world described by the expanded xacro file.
   """
--> 214     xacro_path = CompositePathResolver().resolve(xacro_path)
   urdf = process_file(xacro_path, mappings=mappings).toxml()
   return URDFParser(
       urdf=urdf,
       prefix=prefix,
       use_visual_as_collision_backup=use_visual_as_collision_backup,
   )

File ~/work/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/adapters/package_resolver.py:330, in CompositePathResolver.resolve(self, uri)
   except ParsingError as error:
       errors.append(str(error))
--> 330 raise PathResolutionError(uri=uri, details="; ".join(errors))

PathResolutionError: Could not resolve path 'package://iai_pr2_description/robots/pr2_with_ft2_cableguide.xacro'. Details: Could not resolve package 'iai_pr2_description'. Details: Could not resolve package 'iai_pr2_description'. Details: ament: No module named 'ament_index_python'; Could not resolve package 'iai_pr2_description'. Details: not found in ROS_PACKAGE_PATH; Could not resolve package 'iai_pr2_description'. Details: not found in any of 0 install prefixes