diff --git a/README.md b/README.md index 03a5ed39b..fdbc68d9c 100644 --- a/README.md +++ b/README.md @@ -50,7 +50,7 @@ robot_desig = ObjectDesignatorDescription(names=["pr2"]).resolve() with simulated_robot: ParkArmsAction([Arms.BOTH]).resolve().perform() - MoveTorsoAction([0.3]).resolve().perform() + MoveTorsoAction([TorsoState.HIGH]).resolve().perform() pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve() pickup_arm = pickup_pose.reachable_arms[0] diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index 96995c155..55f8f4bed 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -50,7 +50,7 @@ def move_and_detect(obj_type): with simulated_robot: ParkArmsAction([Arms.BOTH]).resolve().perform() - MoveTorsoAction([0.25]).resolve().perform() + MoveTorsoAction([TorsoState.HIGH]).resolve().perform() milk_desig = move_and_detect(Milk) @@ -89,7 +89,7 @@ def move_and_detect(obj_type): ParkArmsAction([Arms.BOTH]).resolve().perform() - MoveTorsoAction([0.15]).resolve().perform() + MoveTorsoAction([TorsoState.MID]).resolve().perform() # Find a pose to place the spoon, move and then place it spoon_target_pose = Pose([4.85, 3.3, 0.8], [0, 0, 1, 1]) diff --git a/doc/source/index.rst b/doc/source/index.rst index 3e956cc1e..9e93e1c6d 100644 --- a/doc/source/index.rst +++ b/doc/source/index.rst @@ -59,7 +59,7 @@ The code for this plan can be seen below. from pycram.designators.location_designator import * from pycram.designators.action_designator import * from pycram.designators.object_designator import * - from pycram.datastructures.enums import ObjectType, Arms, Grasp, WorldMode + from pycram.datastructures.enums import ObjectType, Arms, Grasp, WorldMode, TorsoState world = BulletWorld(WorldMode.GUI) kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") @@ -73,7 +73,7 @@ The code for this plan can be seen below. with simulated_robot: ParkArmsAction([Arms.BOTH]).resolve().perform() - MoveTorsoAction([0.3]).resolve().perform() + MoveTorsoAction([TorsoState.HIGH]).resolve().perform() pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve() pickup_arm = pickup_pose.reachable_arms[0] diff --git a/examples/action_designator.md b/examples/action_designator.md index 3acfd11a6..ef84f4573 100644 --- a/examples/action_designator.md +++ b/examples/action_designator.md @@ -93,8 +93,9 @@ a {meth}`~pycram.process_module.simulated_robot` environment. ```python from pycram.designators.action_designator import MoveTorsoAction from pycram.process_module import simulated_robot +from pycram.datastructures.enums import TorsoState -torso_pose = 0.2 +torso_pose = TorsoState.HIGH torso_desig = MoveTorsoAction([torso_pose]).resolve() @@ -151,7 +152,7 @@ world.reset_world() from pycram.designators.action_designator import PickUpAction, PlaceAction, ParkArmsAction, MoveTorsoAction,NavigateAction from pycram.designators.object_designator import BelieveObject from pycram.process_module import simulated_robot -from pycram.datastructures.enums import Arms, Grasp +from pycram.datastructures.enums import Arms, Grasp, TorsoState from pycram.datastructures.pose import Pose milk_desig = BelieveObject(names=["milk"]) @@ -160,7 +161,7 @@ arm = Arms.RIGHT with simulated_robot: ParkArmsAction([Arms.BOTH]).resolve().perform() - MoveTorsoAction([0.3]).resolve().perform() + MoveTorsoAction([TorsoState.HIGH]).resolve().perform() NavigateAction([Pose([1.8, 2, 0.0], [0.0, 0.0, 0., 1])]).resolve().perform() @@ -239,7 +240,7 @@ from pycram.designators.action_designator import * from pycram.designators.object_designator import * from pycram.process_module import simulated_robot from pycram.datastructures.pose import Pose -from pycram.datastructures.enums import Arms +from pycram.datastructures.enums import Arms, TorsoState milk_desig = BelieveObject(names=["milk"]) @@ -248,7 +249,7 @@ description = TransportAction(milk_desig, [0, 0, 0, 1])], [Arms.LEFT]) with simulated_robot: - MoveTorsoAction([0.2]).resolve().perform() + MoveTorsoAction([TorsoState.HIGH]).resolve().perform() description.resolve().perform() ``` @@ -267,7 +268,7 @@ world.reset_world() ```python from pycram.designators.action_designator import * from pycram.designators.object_designator import * -from pycram.datastructures.enums import Arms +from pycram.datastructures.enums import Arms, TorsoState from pycram.process_module import simulated_robot from pycram.datastructures.pose import Pose @@ -275,7 +276,7 @@ apartment_desig = BelieveObject(names=["apartment"]).resolve() handle_deisg = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig) with simulated_robot: - MoveTorsoAction([0.25]).resolve().perform() + MoveTorsoAction([TorsoState.HIGH]).resolve().perform() ParkArmsAction([Arms.BOTH]).resolve().perform() NavigateAction([Pose([1.7474915981292725, 2.6873629093170166, 0.0], [-0.0, 0.0, 0.5253598267689507, -0.850880163370435])]).resolve().perform() @@ -301,7 +302,7 @@ apartment_desig = BelieveObject(names=["apartment"]).resolve() handle_deisg = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig) with simulated_robot: - MoveTorsoAction([0.25]).resolve().perform() + MoveTorsoAction([TorsoState.HIGH]).resolve().perform() ParkArmsAction([Arms.BOTH]).resolve().perform() NavigateAction([Pose([1.7474915981292725, 2.8073629093170166, 0.0], [-0.0, 0.0, 0.5253598267689507, -0.850880163370435])]).resolve().perform() diff --git a/examples/cram_plan_tutorial.md b/examples/cram_plan_tutorial.md index 33458861c..c57fefe53 100644 --- a/examples/cram_plan_tutorial.md +++ b/examples/cram_plan_tutorial.md @@ -146,11 +146,12 @@ from pycram.datastructures.enums import Grasp @pycram.tasktree.with_tree -def plan(obj_desig: ObjectDesignatorDescription.Object, torso=0.2, place=counter_name): +def plan(obj_desig: ObjectDesignatorDescription.Object, torso=None, place=counter_name): world.reset_world() with simulated_robot: ParkArmsActionPerformable(Arms.BOTH).perform() - + if torso is None: + torso={"torso_lift_joint": 0.2} MoveTorsoActionPerformable(torso).perform() location = CostmapLocation(target=obj_desig, reachable_for=robot_desig) pose = location.resolve() @@ -206,7 +207,7 @@ def plan(obj_desig: ObjectDesignatorDescription.Object, torso=0.2, place=counter good_torsos = [] for obj_name in object_names: done = False - torso = 0.3 if len(good_torsos) == 0 else good_torsos[-1] + torso = {"torso_lift_joint": 0.3}if len(good_torsos) == 0 else good_torsos[-1] while not done: try: plan(object_desig[obj_name], torso=torso, place=counter_name) diff --git a/examples/improving_actions.md b/examples/improving_actions.md index 17b5da281..4c83c865c 100644 --- a/examples/improving_actions.md +++ b/examples/improving_actions.md @@ -203,8 +203,9 @@ from pycram.designators.action_designator import ParkArmsActionPerformable world.reset_world() milk.set_pose(Pose([0.5, 3.15, 1.04])) +torso_joint = RobotDescription.current_robot_description.torso_joint with simulated_robot: - MoveTorsoActionPerformable(0.3).perform() + MoveTorsoActionPerformable({torso_joint: 0.3}).perform() for sample in fpa: try: ParkArmsActionPerformable(Arms.RIGHT).perform() diff --git a/examples/intro.md b/examples/intro.md index 48fad1898..b8c8f02a9 100644 --- a/examples/intro.md +++ b/examples/intro.md @@ -326,7 +326,7 @@ To get familiar with the PyCRAM Framework we will write a simple pick and place a cereal box from the kitchen counter and place it on the kitchen island. This is a simple pick and place plan. ```python -from pycram.datastructures.enums import Grasp +from pycram.datastructures.enums import Grasp, TorsoState cereal_desig = ObjectDesignatorDescription(names=["cereal"]) kitchen_desig = ObjectDesignatorDescription(names=["kitchen"]) @@ -334,7 +334,7 @@ robot_desig = ObjectDesignatorDescription(names=["pr2"]).resolve() with simulated_robot: ParkArmsAction([Arms.BOTH]).resolve().perform() - MoveTorsoAction([0.33]).resolve().perform() + MoveTorsoAction([TorsoState.HIGH]).resolve().perform() pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve() pickup_arm = pickup_pose.reachable_arms[0] diff --git a/examples/language.md b/examples/language.md index 11b9280ab..1f72320ba 100644 --- a/examples/language.md +++ b/examples/language.md @@ -200,7 +200,7 @@ world.reset_world() navigate = NavigateAction([Pose([1, 1, 0])]) park = ParkArmsAction([Arms.BOTH]) -move_torso = MoveTorsoAction([0.3]) +move_torso = MoveTorsoAction([TorsoState.HIGH]) plan = navigate | park + move_torso @@ -286,9 +286,10 @@ You can see an example of how to use Repeat below. ```python from pycram.designators.action_designator import * from pycram.process_module import simulated_robot +from pycram.datastructures.enums import TorsoState -move_torso_up = MoveTorsoAction([0.3]) -move_torso_down = MoveTorsoAction([0.]) +move_torso_up = MoveTorsoAction([TorsoState.HIGH]) +move_torso_down = MoveTorsoAction([TorsoState.LOW]) plan = (move_torso_up + move_torso_down) * 5 @@ -311,9 +312,10 @@ from pycram.designators.action_designator import * from pycram.process_module import simulated_robot from pycram.language import Monitor import time +from pycram.datastructures.enums import TorsoState -move_torso_up = MoveTorsoAction([0.3]) -move_torso_down = MoveTorsoAction([0.]) +move_torso_up = MoveTorsoAction([TorsoState.HIGH]) +move_torso_down = MoveTorsoAction([TorsoState.LOW]) def monitor_func(): diff --git a/examples/migrate_neems.md b/examples/migrate_neems.md index 123b788e8..c426800a9 100644 --- a/examples/migrate_neems.md +++ b/examples/migrate_neems.md @@ -56,6 +56,7 @@ from pycram.tasktree import with_tree from pycram.worlds.bullet_world import BulletWorld from pycram.world_concepts.world_object import Object from pycram.designators.object_designator import * +from pycram.datastructures.enums import TorsoState import pycrap @@ -75,7 +76,7 @@ class ExamplePlans: def pick_and_place_plan(self): with simulated_robot: ParkArmsAction([Arms.BOTH]).resolve().perform() - MoveTorsoAction([0.3]).resolve().perform() + MoveTorsoAction([TorsoState.HIGH]).resolve().perform() pickup_pose = CostmapLocation(target=self.cereal_desig.resolve(), reachable_for=self.robot_desig).resolve() pickup_arm = pickup_pose.reachable_arms[0] NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform() diff --git a/examples/minimal_task_tree.md b/examples/minimal_task_tree.md index d4af30aaa..a7b7cc8b3 100644 --- a/examples/minimal_task_tree.md +++ b/examples/minimal_task_tree.md @@ -29,7 +29,7 @@ from pycram.designators.location_designator import * from pycram.process_module import simulated_robot from pycram.designators.object_designator import * from pycram.datastructures.pose import Pose -from pycram.datastructures.enums import ObjectType, WorldMode +from pycram.datastructures.enums import ObjectType, WorldMode, TorsoState import anytree import pycram.failures import pycrap @@ -56,7 +56,7 @@ Finally, we create a plan where the robot parks his arms, walks to the kitchen c def plan(): with simulated_robot: ParkArmsActionPerformable(Arms.BOTH).perform() - MoveTorsoAction([0.22]).resolve().perform() + MoveTorsoAction([TorsoState.MID]).resolve().perform() pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve() pickup_arm = pickup_pose.reachable_arms[0] NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform() diff --git a/examples/orm_example.md b/examples/orm_example.md index 1aa4074ac..26e5459cb 100644 --- a/examples/orm_example.md +++ b/examples/orm_example.md @@ -39,7 +39,7 @@ Next we will write a simple plan where the robot parks his arms and then moves s from pycram.designators.action_designator import * from pycram.designators.location_designator import * from pycram.process_module import simulated_robot -from pycram.datastructures.enums import Arms, ObjectType, Grasp, WorldMode +from pycram.datastructures.enums import Arms, ObjectType, Grasp, WorldMode, TorsoState from pycram.tasktree import with_tree import pycram.tasktree from pycram.worlds.bullet_world import BulletWorld @@ -64,7 +64,7 @@ kitchen_desig = ObjectDesignatorDescription(names=["kitchen"]) def plan(): with simulated_robot: ParkArmsActionPerformable(Arms.BOTH).perform() - MoveTorsoAction([0.2]).resolve().perform() + MoveTorsoAction([TorsoState.MID]).resolve().perform() pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve() pickup_arm = pickup_pose.reachable_arms[0] NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform() diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index aced79bc1..02616f39d 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -94,6 +94,15 @@ class Shape(Enum): CAPSULE = 7 +class TorsoState(Enum): + """ + Enum for the different states of the torso. + """ + HIGH = auto() + MID = auto() + LOW = auto() + + class WorldMode(Enum): """ Enum for the different modes of the world. diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 8f868171e..1ca3464b1 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -29,7 +29,8 @@ from owlready2 import Thing -from ..datastructures.enums import Arms, Grasp, GripperState, DetectionTechnique, DetectionState, MovementType +from ..datastructures.enums import Arms, Grasp, GripperState, DetectionTechnique, DetectionState, MovementType, \ + TorsoState from ..designator import ActionDesignatorDescription from ..datastructures.pose import Pose @@ -47,6 +48,7 @@ from ..orm.action_designator import Action as ORMAction from dataclasses import dataclass, field + # ---------------------------------------------------------------------------- # ---------------- Performables ---------------------------------------------- # ---------------------------------------------------------------------------- @@ -132,15 +134,17 @@ class MoveTorsoActionPerformable(ActionAbstract): Move the torso of the robot up and down. """ - position: float + joint_positions: dict """ - Target position of the torso joint + The joint positions that should be set. The keys are the joint names and the values are the joint positions. """ orm_class: Type[ActionAbstract] = field(init=False, default=ORMMoveTorsoAction) @with_tree def plan(self) -> None: - MoveJointsMotion([RobotDescription.current_robot_description.torso_joint], [self.position]).perform() + joints_positions = list(self.joint_positions.items()) + joints, positions = zip(*joints_positions) + MoveJointsMotion(list(joints), list(positions)).perform() @dataclass @@ -302,7 +306,6 @@ def plan(self) -> None: MoveGripperMotion(motion=GripperState.OPEN, gripper=self.arm).perform() MoveTCPMotion(prepose, self.arm, allow_gripper_collision=True).perform() - # Perform the motion with the adjusted pose -> actual grasp and close gripper oTg = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) adjusted_oTm = object.local_transformer.transform_pose(oTg, "map") @@ -408,7 +411,6 @@ def plan(self) -> None: return try_action(motion_action, failure_type=NavigationGoalNotReachedError) - @dataclass class TransportActionPerformable(ActionAbstract): """ @@ -509,8 +511,9 @@ class DetectActionPerformable(ActionAbstract): @with_tree def plan(self) -> None: - return try_action(DetectingMotion(technique=self.technique,state=self.state, object_designator_description=self.object_designator_description, - region=self.region), PerceptionObjectNotFound) + return try_action(DetectingMotion(technique=self.technique, state=self.state, + object_designator_description=self.object_designator_description, + region=self.region), PerceptionObjectNotFound) @dataclass @@ -740,14 +743,14 @@ class MoveTorsoAction(ActionDesignatorDescription): """ performable_class = MoveTorsoActionPerformable - def __init__(self, positions: List[float]): + def __init__(self, torso_states: List[TorsoState]): """ Create a designator_description description to move the torso of the robot up and down. - :param positions: List of possible positions of the robots torso, possible position is a float of height in metres + :param torso_states: A list of possible states for the torso. The states are defined in the robot description. """ super().__init__() - self.positions: List[float] = positions + self.torso_states: List[TorsoState] = torso_states def ground(self) -> MoveTorsoActionPerformable: """ @@ -755,7 +758,9 @@ def ground(self) -> MoveTorsoActionPerformable: :return: A performable action designator_description """ - return MoveTorsoActionPerformable(self.positions[0]) + joint_positions: dict = RobotDescription.current_robot_description.get_static_joint_chain("torso", + self.torso_states[0]) + return MoveTorsoActionPerformable(joint_positions) def __iter__(self): """ @@ -763,8 +768,10 @@ def __iter__(self): :return: A performable action designator_description """ - for position in self.positions: - yield MoveTorsoActionPerformable(position) + for torso_state in self.torso_states: + joint_positions: dict = RobotDescription.current_robot_description.get_static_joint_chain("torso", + torso_state) + yield MoveTorsoActionPerformable(joint_positions) class SetGripperAction(ActionDesignatorDescription): @@ -956,7 +963,7 @@ def __init__(self, self.object_designator_description: Union[ ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description object_desig = self.object_designator_description if isinstance(self.object_designator_description, - ObjectDesignatorDescription.Object) else self.object_designator_description.resolve() + ObjectDesignatorDescription.Object) else self.object_designator_description.resolve() self.target_locations: List[Pose] = target_locations self.arms: List[Arms] = arms self.knowledge_condition = ReachableProperty(object_desig.pose) @@ -1051,7 +1058,6 @@ def __init__(self, self.target_locations: List[Pose] = target_locations self.pickup_prepose_distance: float = pickup_prepose_distance - def ground(self) -> TransportActionPerformable: """ Default specialized_designators that returns a performable designator_description with the first entries from the lists of possible parameter. @@ -1062,10 +1068,9 @@ def ground(self) -> TransportActionPerformable: if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object) \ else self.object_designator_description.resolve() - return TransportActionPerformable(obj_desig, self.target_locations[0], self.arms[0], + return TransportActionPerformable(obj_desig, self.target_locations[0], self.arms[0], self.pickup_prepose_distance) - def __iter__(self) -> TransportActionPerformable: obj_desig = self.object_designator_description \ if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object) \ @@ -1119,7 +1124,8 @@ class DetectAction(ActionDesignatorDescription): performable_class = DetectActionPerformable def __init__(self, technique: DetectionTechnique, state: Optional[DetectionState] = None, - object_designator_description: Optional[ObjectDesignatorDescription] = None, region: Optional[Location] = None): + object_designator_description: Optional[ObjectDesignatorDescription] = None, + region: Optional[Location] = None): """ Tries to detect an object in the field of view (FOV) of the robot. @@ -1129,7 +1135,7 @@ def __init__(self, technique: DetectionTechnique, state: Optional[DetectionState self.state: DetectionState = DetectionState.START if state is None else state self.object_designator_description: Optional[ObjectDesignatorDescription] = object_designator_description self.region: Optional[Location] = region - #TODO: Implement knowledge condition + # TODO: Implement knowledge condition # self.knowledge_condition = VisibleProperty(self.object_designator_description) def ground(self) -> DetectActionPerformable: diff --git a/src/pycram/orm/action_designator.py b/src/pycram/orm/action_designator.py index 0aff86afd..3f49c793d 100644 --- a/src/pycram/orm/action_designator.py +++ b/src/pycram/orm/action_designator.py @@ -1,10 +1,10 @@ -from typing_extensions import Optional +from typing_extensions import Optional, Dict from .base import RobotState, Designator, MapperArgsMixin, PoseMixin from .object_designator import ObjectMixin from ..datastructures.enums import Arms, GripperState, Grasp, DetectionTechnique, DetectionState from sqlalchemy.orm import Mapped, mapped_column, relationship -from sqlalchemy import ForeignKey +from sqlalchemy import ForeignKey, JSON class Action(MapperArgsMixin, Designator): @@ -37,7 +37,7 @@ class MoveTorsoAction(Action): """ORM Class of pycram.designators.action_designator.MoveTorsoAction.""" id: Mapped[int] = mapped_column(ForeignKey(f'{Action.__tablename__}.id'), primary_key=True, init=False) - position: Mapped[Optional[float]] = mapped_column(default=None) + joint_positions: Mapped[Optional[Dict[str, float]]] = mapped_column(JSON) class SetGripperAction(Action): diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index 5b8ba2aab..e04f1a38d 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -1,5 +1,8 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations + +from enum import Enum + from typing_extensions import List, Dict, Union, Optional from .datastructures.dataclasses import VirtualMobileBaseJoints @@ -274,7 +277,7 @@ def get_default_camera(self) -> CameraDescription: """ return self.cameras[list(self.cameras.keys())[0]] - def get_static_joint_chain(self, kinematic_chain_name: str, configuration_name: str): + def get_static_joint_chain(self, kinematic_chain_name: str, configuration_name: Union[str, Enum]): """ Get the static joint states of a kinematic chain for a specific configuration. When trying to access one of the robot arms the function `:func: get_arm_chain` should be used. @@ -288,9 +291,11 @@ def get_static_joint_chain(self, kinematic_chain_name: str, configuration_name: return self.kinematic_chains[kinematic_chain_name].static_joint_states[configuration_name] else: raise ValueError( - f"There is no static joint state with the name {configuration_name} for Kinematic chain {kinematic_chain_name} of robot {self.name}") + f"There is no static joint state with the name {configuration_name} for Kinematic chain {kinematic_chain_name} of robot {self.name}. " + f"The following keys are available: {list(self.kinematic_chains[kinematic_chain_name].static_joint_states.keys())}") else: - raise ValueError(f"There is no KinematicChain with name {kinematic_chain_name} for robot {self.name}") + raise ValueError(f"There is no KinematicChain with name {kinematic_chain_name} for robot {self.name}. " + f"The following chains are available: {list(self.kinematic_chains.keys())}") def get_parent(self, name: str) -> str: """ @@ -411,7 +416,7 @@ class KinematicChainDescription: """ Type of the arm, if the chain is an arm """ - static_joint_states: Dict[str, Dict[str, float]] + static_joint_states: Dict[Union[str, Enum], Dict[str, float]] """ Dictionary of static joint states for the chain """ @@ -436,7 +441,7 @@ def __init__(self, name: str, start_link: str, end_link: str, urdf_object: URDFO self.link_names: List[str] = [] self.joint_names: List[str] = [] self.arm_type: Arms = arm_type - self.static_joint_states: Dict[str, Dict[str, float]] = {} + self.static_joint_states: Dict[Union[str, Enum], Dict[str, float]] = {} self.end_effector = None self._init_links() @@ -488,7 +493,7 @@ def joints(self) -> List[str]: """ return self.get_joints() - def add_static_joint_states(self, name: str, states: dict): + def add_static_joint_states(self, name: Union[str, Enum], states: dict): """ Adds static joint states to the chain. These define a specific configuration of the chain. @@ -500,7 +505,7 @@ def add_static_joint_states(self, name: str, states: dict): raise ValueError(f"Joint {joint_name} is not part of the chain") self.static_joint_states[name] = states - def get_static_joint_states(self, name: str) -> Dict[str, float]: + def get_static_joint_states(self, name: Union[str, Enum]) -> Dict[str, float]: """ Get the dictionary of static joint states for a given name of the static joint states. diff --git a/src/pycram/robot_descriptions/armar_description.py b/src/pycram/robot_descriptions/armar_description.py index 6a412a68d..e9bc4b6dd 100644 --- a/src/pycram/robot_descriptions/armar_description.py +++ b/src/pycram/robot_descriptions/armar_description.py @@ -1,6 +1,6 @@ from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription -from ..datastructures.enums import Arms, Grasp, GripperState, GripperType +from ..datastructures.enums import Arms, Grasp, GripperState, GripperType, TorsoState import rospkg rospack = rospkg.RosPack() @@ -117,11 +117,11 @@ torso = KinematicChainDescription("torso", "world", "torso", armar_description.urdf_object) -# torso.add_static_joint_states(TorsoState.HIGH, {"torso_joint": 0.0}) -# -# torso.add_static_joint_states(TorsoState.MID, {"torso_joint": -0.15}) -# -# torso.add_static_joint_states(TorsoState.LOW, {"torso_joint": -0.365}) +torso.add_static_joint_states(TorsoState.HIGH, {"torso_joint": 0.0}) + +torso.add_static_joint_states(TorsoState.MID, {"torso_joint": -0.15}) + +torso.add_static_joint_states(TorsoState.LOW, {"torso_joint": -0.365}) armar_description.add_kinematic_chain_description(torso) diff --git a/src/pycram/robot_descriptions/boxy_description.py b/src/pycram/robot_descriptions/boxy_description.py index f4dc7cfc1..ac843e382 100644 --- a/src/pycram/robot_descriptions/boxy_description.py +++ b/src/pycram/robot_descriptions/boxy_description.py @@ -1,7 +1,7 @@ from ..ros.ros_tools import get_ros_package_path from ..robot_description import RobotDescription, CameraDescription, KinematicChainDescription, \ EndEffectorDescription, RobotDescriptionManager -from ..datastructures.enums import Arms, Grasp, GripperState +from ..datastructures.enums import Arms, Grasp, GripperState, TorsoState filename = get_ros_package_path('pycram') + '/resources/robots/' + "boxy" + '.urdf' @@ -56,6 +56,18 @@ left_arm.end_effector = left_gripper +################################## Torso ################################## +torso = KinematicChainDescription("torso", "base_link", "triangle_base_link", + boxy_description.urdf_object) + +torso.add_static_joint_states(TorsoState.HIGH, {"triangle_base_joint": 0}) + +torso.add_static_joint_states(TorsoState.MID, {"triangle_base_joint": 0.29}) + +torso.add_static_joint_states(TorsoState.LOW, {"triangle_base_joint": -0.58}) + +boxy_description.add_kinematic_chain_description(torso) + ################################## Camera ################################## camera = CameraDescription("head_mount_kinect2_rgb_optical_frame", "head_mount_kinect2_rgb_optical_frame", 2.5, 0.99483, 0.75049) diff --git a/src/pycram/robot_descriptions/donbot_description.py b/src/pycram/robot_descriptions/donbot_description.py index f37958440..83d03f0f4 100644 --- a/src/pycram/robot_descriptions/donbot_description.py +++ b/src/pycram/robot_descriptions/donbot_description.py @@ -1,7 +1,7 @@ from ..ros.ros_tools import get_ros_package_path from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription -from ..datastructures.enums import Arms, Grasp, GripperState +from ..datastructures.enums import Arms, Grasp, GripperState, TorsoState filename = get_ros_package_path('pycram') + '/resources/robots/' + "iai_donbot" + '.urdf' @@ -30,6 +30,19 @@ right_arm.end_effector = right_gripper +################################## Torso ################################## +torso = KinematicChainDescription("torso", "base_footprint", "ur5_base_link", + donbot_description.urdf_object, include_fixed_joints=True) + +# fixed joint, so all states set to 0 +torso.add_static_joint_states(TorsoState.HIGH, {"arm_base_mounting_joint": 0}) + +torso.add_static_joint_states(TorsoState.MID, {"arm_base_mounting_joint": 0}) + +torso.add_static_joint_states(TorsoState.LOW, {"arm_base_mounting_joint": 0}) + +donbot_description.add_kinematic_chain_description(torso) + ################################## Camera ################################## camera = CameraDescription("camera_link", "camera_link", 0.75049, 0.5, 1.2) donbot_description.add_camera_description(camera) diff --git a/src/pycram/robot_descriptions/hsrb_description.py b/src/pycram/robot_descriptions/hsrb_description.py index ae452e92e..14da1c766 100644 --- a/src/pycram/robot_descriptions/hsrb_description.py +++ b/src/pycram/robot_descriptions/hsrb_description.py @@ -2,7 +2,7 @@ from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription -from ..datastructures.enums import GripperState, Grasp, Arms +from ..datastructures.enums import GripperState, Grasp, Arms, TorsoState filename = get_ros_package_path('pycram') + '/resources/robots/' + "hsrb" + '.urdf' @@ -32,6 +32,18 @@ left_arm.end_effector = left_gripper +################################## Torso ################################## +torso = KinematicChainDescription("torso", "base_link", "torso_lift_link", + hsrb_description.urdf_object) + +torso.add_static_joint_states(TorsoState.HIGH, {"torso_lift_joint": 0.34}) + +torso.add_static_joint_states(TorsoState.MID, {"torso_lift_joint": 0.17}) + +torso.add_static_joint_states(TorsoState.LOW, {"torso_lift_joint": 0}) + +hsrb_description.add_kinematic_chain_description(torso) + ################################## Camera ################################## head_center_camera = CameraDescription("head_center_camera_frame", "head_center_camera_frame", 0.99483, 0.75049) diff --git a/src/pycram/robot_descriptions/icub3_description.py b/src/pycram/robot_descriptions/icub3_description.py index 3d66a4476..bd5a382ff 100644 --- a/src/pycram/robot_descriptions/icub3_description.py +++ b/src/pycram/robot_descriptions/icub3_description.py @@ -1,10 +1,10 @@ from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription -from ..datastructures.enums import Arms, Grasp, GripperState, GripperType +from ..datastructures.enums import Arms, Grasp, GripperState, GripperType, TorsoState import rospkg rospack = rospkg.RosPack() -filename = rospack.get_path('pycram') + '/resources/robots/' + "iCub" + '.urdf' +filename = rospack.get_path('pycram') + '/resources/robots/' + "iCub3" + '.urdf' icub_description = RobotDescription("iCub3", "base_footprint", "torso_1", "torso_pitch", filename) @@ -144,11 +144,11 @@ torso = KinematicChainDescription("torso", "root_link", "chest", icub_description.urdf_object) -# torso.add_static_joint_states(TorsoState.HIGH, {"torso_roll": 0}) -# -# torso.add_static_joint_states(TorsoState.MID, {"torso_roll": 0}) -# -# torso.add_static_joint_states(TorsoState.LOW, {"torso_roll": 0}) +torso.add_static_joint_states(TorsoState.HIGH, {"torso_roll": 0}) + +torso.add_static_joint_states(TorsoState.MID, {"torso_roll": 0}) + +torso.add_static_joint_states(TorsoState.LOW, {"torso_roll": 0}) icub_description.add_kinematic_chain_description(torso) diff --git a/src/pycram/robot_descriptions/justin_description.py b/src/pycram/robot_descriptions/justin_description.py index 720e77c54..541cb4708 100644 --- a/src/pycram/robot_descriptions/justin_description.py +++ b/src/pycram/robot_descriptions/justin_description.py @@ -1,6 +1,6 @@ from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription -from ..datastructures.enums import Arms, Grasp, GripperState, GripperType +from ..datastructures.enums import Arms, Grasp, GripperState, GripperType, TorsoState import rospkg rospack = rospkg.RosPack() @@ -170,6 +170,24 @@ justin_description.add_kinematic_chain("neck", "torso4", "head2") # justin_description.set_neck("head1_joint", "head2_joint") +################################## Torso ################################## +torso = KinematicChainDescription("torso", "torso1", "torso4", + justin_description.urdf_object) + +torso.add_static_joint_states(TorsoState.HIGH, {"torso2_joint": 0, + "torso3_joint": 0.174533, + "torso4_joint": 0}) + +torso.add_static_joint_states(TorsoState.MID, {"torso2_joint": -0.8, + "torso3_joint": 1.57, + "torso4_joint": -0.77}) + +torso.add_static_joint_states(TorsoState.LOW, {"torso2_joint": -0.9, + "torso3_joint": 2.33874, + "torso4_joint": -1.57}) + +justin_description.add_kinematic_chain_description(torso) + ################################# Grasps ################################## orientation = [0.707, -0.707, 0.707, -0.707] diff --git a/src/pycram/robot_descriptions/pr2_description.py b/src/pycram/robot_descriptions/pr2_description.py index d0ff58b44..7c8f949c5 100644 --- a/src/pycram/robot_descriptions/pr2_description.py +++ b/src/pycram/robot_descriptions/pr2_description.py @@ -1,7 +1,7 @@ from ..datastructures.dataclasses import VirtualMobileBaseJoints from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription -from ..datastructures.enums import Arms, Grasp, GripperState, GripperType +from ..datastructures.enums import Arms, Grasp, GripperState, GripperType, TorsoState from ..ros.ros_tools import get_ros_package_path from ..helper import get_robot_mjcf_path @@ -35,9 +35,9 @@ left_gripper = EndEffectorDescription("left_gripper", "l_gripper_palm_link", "l_gripper_tool_frame", pr2_description.urdf_object) left_gripper.add_static_joint_states(GripperState.OPEN, {'l_gripper_l_finger_joint': 0.548, - 'l_gripper_r_finger_joint': 0.548}) + 'l_gripper_r_finger_joint': 0.548}) left_gripper.add_static_joint_states(GripperState.CLOSE, {'l_gripper_l_finger_joint': 0.0, - 'l_gripper_r_finger_joint': 0.0}) + 'l_gripper_r_finger_joint': 0.0}) left_gripper.end_effector_type = GripperType.PARALLEL left_gripper.opening_distance = 0.548 left_arm.end_effector = left_gripper @@ -58,19 +58,30 @@ right_gripper = EndEffectorDescription("right_gripper", "r_gripper_palm_link", "r_gripper_tool_frame", pr2_description.urdf_object) right_gripper.add_static_joint_states(GripperState.OPEN, {'r_gripper_l_finger_joint': 0.548, - 'r_gripper_r_finger_joint': 0.548}) + 'r_gripper_r_finger_joint': 0.548}) right_gripper.add_static_joint_states(GripperState.CLOSE, {'r_gripper_l_finger_joint': 0.0, - 'r_gripper_r_finger_joint': 0.0}) + 'r_gripper_r_finger_joint': 0.0}) right_gripper.end_effector_type = GripperType.PARALLEL right_gripper.opening_distance = 0.548 right_arm.end_effector = right_gripper - ################################## Camera ################################## camera = CameraDescription("kinect_camera", "wide_stereo_optical_frame", 1.27, 1.60, 0.99483, 0.75049) pr2_description.add_camera_description(camera) +################################## Torso ################################## +torso = KinematicChainDescription("torso", "base_link", "torso_lift_link", + pr2_description.urdf_object) + +torso.add_static_joint_states(TorsoState.HIGH, {"torso_lift_joint": 0.3}) + +torso.add_static_joint_states(TorsoState.MID, {"torso_lift_joint": 0.15}) + +torso.add_static_joint_states(TorsoState.LOW, {"torso_lift_joint": 0}) + +pr2_description.add_kinematic_chain_description(torso) + ################################## Neck ################################## pr2_description.add_kinematic_chain("neck", "head_pan_link", "head_tilt_link") diff --git a/src/pycram/robot_descriptions/stretch_description.py b/src/pycram/robot_descriptions/stretch_description.py index 3c86be24f..186c1276f 100644 --- a/src/pycram/robot_descriptions/stretch_description.py +++ b/src/pycram/robot_descriptions/stretch_description.py @@ -2,7 +2,7 @@ from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ CameraDescription, RobotDescriptionManager -from ..datastructures.enums import GripperState, Arms, Grasp +from ..datastructures.enums import GripperState, Arms, Grasp, TorsoState filename = get_ros_package_path('pycram') + '/resources/robots/' + "stretch_description" + '.urdf' @@ -39,6 +39,18 @@ stretch_description.add_kinematic_chain_description(neck) +################################## Torso ################################## +torso = KinematicChainDescription("torso", "link_mast", "link_lift", + stretch_description.urdf_object) + +torso.add_static_joint_states(TorsoState.HIGH, {"joint_lift": 1}) + +torso.add_static_joint_states(TorsoState.MID, {"joint_lift": 0.5}) + +torso.add_static_joint_states(TorsoState.LOW, {"joint_lift": 0}) + +stretch_description.add_kinematic_chain_description(torso) + ################################## Camera ################################## realsense_color = CameraDescription("camera_color_optical_frame", "camera_color_optical_frame", 1.322, 1.322) realsense_depth = CameraDescription("camera_depth_optical_frame", "camera_depth_optical_frame", 1.307, 1.307) diff --git a/src/pycram/robot_descriptions/tiago_description.py b/src/pycram/robot_descriptions/tiago_description.py index ef39e8a8b..c0953487f 100644 --- a/src/pycram/robot_descriptions/tiago_description.py +++ b/src/pycram/robot_descriptions/tiago_description.py @@ -1,7 +1,7 @@ from ..ros.ros_tools import get_ros_package_path from ..datastructures.dataclasses import VirtualMobileBaseJoints -from ..datastructures.enums import GripperState, Arms, Grasp +from ..datastructures.enums import GripperState, Arms, Grasp, TorsoState from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription from ..helper import get_robot_mjcf_path @@ -67,6 +67,18 @@ right_arm.end_effector = right_gripper +################################## Torso ################################## +torso = KinematicChainDescription("torso", "torso_fixed_link", "torso_lift_link", + tiago_description.urdf_object) + +torso.add_static_joint_states(TorsoState.HIGH, {"torso_lift_joint": 0.3}) + +torso.add_static_joint_states(TorsoState.MID, {"torso_lift_joint": 0.15}) + +torso.add_static_joint_states(TorsoState.LOW, {"torso_lift_joint": 0}) + +tiago_description.add_kinematic_chain_description(torso) + ################################## Camera ################################## camera = CameraDescription("xtion_optical_frame", "xtion_optical_frame", 0.99483, 0.75049, 1.0665, 1.4165) tiago_description.add_camera_description(camera) diff --git a/test/test_designator/test_action_designator.py b/test/test_designator/test_action_designator.py index dc583b83d..7068c437b 100644 --- a/test/test_designator/test_action_designator.py +++ b/test/test_designator/test_action_designator.py @@ -4,12 +4,12 @@ from pycram.designator import ObjectDesignatorDescription from pycram.designators import action_designator, object_designator from pycram.designators.action_designator import MoveTorsoActionPerformable, PickUpActionPerformable, \ - NavigateActionPerformable, FaceAtPerformable + NavigateActionPerformable, FaceAtPerformable, MoveTorsoAction from pycram.local_transformer import LocalTransformer from pycram.robot_description import RobotDescription from pycram.process_module import simulated_robot from pycram.datastructures.pose import Pose -from pycram.datastructures.enums import ObjectType, Arms, GripperState, Grasp, DetectionTechnique +from pycram.datastructures.enums import ObjectType, Arms, GripperState, Grasp, DetectionTechnique, TorsoState from pycram.testing import BulletWorldTestCase import numpy as np @@ -20,11 +20,12 @@ class TestActionDesignatorGrounding(BulletWorldTestCase): """Testcase for the grounding methods of action designators.""" def test_move_torso(self): - description = action_designator.MoveTorsoAction([0.3]) - self.assertEqual(description.ground().position, 0.3) + description = action_designator.MoveTorsoAction([TorsoState.HIGH]) + torso_joint = RobotDescription.current_robot_description.torso_joint + self.assertEqual(description.ground().joint_positions[torso_joint], 0.3) with simulated_robot: description.resolve().perform() - self.assertEqual(self.world.robot.get_joint_position(RobotDescription.current_robot_description.torso_joint), + self.assertEqual(self.world.robot.get_joint_position(torso_joint), 0.3) def test_set_gripper(self): @@ -73,7 +74,7 @@ def test_pick_up(self): self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1]), True).perform() - MoveTorsoActionPerformable(0.3).perform() + MoveTorsoAction([TorsoState.HIGH]).resolve().perform() description.resolve().perform() self.assertTrue(object_description.resolve().world_object in self.robot.attachments.keys()) @@ -83,7 +84,7 @@ def test_place(self): self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1]), True).perform() - MoveTorsoActionPerformable(0.3).perform() + MoveTorsoAction([TorsoState.HIGH]).resolve().perform() PickUpActionPerformable(object_description.resolve(), Arms.LEFT, Grasp.FRONT, 0.03).perform() description.resolve().perform() self.assertFalse(object_description.resolve().world_object in self.robot.attachments.keys()) @@ -127,7 +128,7 @@ def test_transport(self): [0.0, 0.0, 0.16439898301071468, 0.9863939245479175])], [Arms.LEFT]) with simulated_robot: - action_designator.MoveTorsoAction([0.2]).resolve().perform() + action_designator.MoveTorsoAction([TorsoState.HIGH]).resolve().perform() description.resolve().perform() self.assertEqual(description.ground().object_designator.name, "milk") milk_position = np.array(self.milk.get_pose().position_as_list()) diff --git a/test/test_designator/test_move_and_pick_up.py b/test/test_designator/test_move_and_pick_up.py index b837a9993..49f990b33 100644 --- a/test/test_designator/test_move_and_pick_up.py +++ b/test/test_designator/test_move_and_pick_up.py @@ -6,9 +6,9 @@ from sortedcontainers import SortedSet from pycram.testing import BulletWorldTestCase -from pycram.datastructures.enums import ObjectType, Arms, Grasp +from pycram.datastructures.enums import ObjectType, Arms, Grasp, TorsoState from pycram.designator import ObjectDesignatorDescription -from pycram.designators.action_designator import MoveTorsoActionPerformable +from pycram.designators.action_designator import MoveTorsoActionPerformable, MoveTorsoAction from pycram.designators.specialized_designators.probabilistic.probabilistic_action import (MoveAndPickUp, Arms as PMArms, Grasp as PMGrasp) @@ -51,7 +51,7 @@ def test_move_and_pick_up_with_mode(self): with simulated_robot: for action in move_and_pick.iter_with_mode(): try: - MoveTorsoActionPerformable(0.3).perform() + MoveTorsoAction([TorsoState.HIGH]).resolve().perform() action.perform() return # Success except PlanFailure as e: diff --git a/test/test_designator/test_move_and_place.py b/test/test_designator/test_move_and_place.py index 8f836fe45..bafea634e 100644 --- a/test/test_designator/test_move_and_place.py +++ b/test/test_designator/test_move_and_place.py @@ -4,11 +4,11 @@ import numpy as np from pycram.testing import BulletWorldTestCase -from pycram.datastructures.enums import ObjectType, Arms, Grasp +from pycram.datastructures.enums import ObjectType, Arms, Grasp, TorsoState from pycram.datastructures.pose import Pose from pycram.designator import ObjectDesignatorDescription from pycram.designators.action_designator import MoveTorsoActionPerformable, NavigateActionPerformable, \ - PickUpActionPerformable + PickUpActionPerformable, MoveTorsoAction from pycram.designators.specialized_designators.probabilistic.probabilistic_action import (MoveAndPlace) from pycram.failures import PlanFailure from pycram.process_module import simulated_robot @@ -30,7 +30,7 @@ def test_with_mode(self): with simulated_robot: NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1]), True).perform() - MoveTorsoActionPerformable(0.3).perform() + MoveTorsoAction([TorsoState.HIGH]).resolve().perform() PickUpActionPerformable(object_designator, Arms.LEFT, Grasp.FRONT, 0.03).perform() with simulated_robot: for action in designator: diff --git a/test/test_language.py b/test/test_language.py index 4db6ffc57..cb688ddf8 100644 --- a/test/test_language.py +++ b/test/test_language.py @@ -22,7 +22,7 @@ def test_inheritance(self): def test_simplify_tree(self): act = NavigateAction([Pose()]) - act2 = MoveTorsoAction([0.3]) + act2 = MoveTorsoAction([TorsoState.HIGH]) act3 = DetectAction([ObjectType.JEROEN_CUP]) plan = act + act2 + act3 @@ -31,7 +31,7 @@ def test_simplify_tree(self): def test_sequential_construction(self): act = NavigateAction([Pose()]) - act2 = MoveTorsoAction([0.3]) + act2 = MoveTorsoAction([TorsoState.HIGH]) act3 = DetectAction([ObjectType.JEROEN_CUP]) plan = act + act2 + act3 @@ -40,7 +40,7 @@ def test_sequential_construction(self): def test_parallel_construction(self): act = NavigateAction([Pose()]) - act2 = MoveTorsoAction([0.3]) + act2 = MoveTorsoAction([TorsoState.HIGH]) act3 = DetectAction([ObjectType.JEROEN_CUP]) plan = act | act2 | act3 @@ -49,7 +49,7 @@ def test_parallel_construction(self): def test_try_in_order_construction(self): act = NavigateAction([Pose()]) - act2 = MoveTorsoAction([0.3]) + act2 = MoveTorsoAction([TorsoState.HIGH]) act3 = DetectAction([ObjectType.JEROEN_CUP]) plan = act - act2 - act3 @@ -58,7 +58,7 @@ def test_try_in_order_construction(self): def test_try_all_construction(self): act = NavigateAction([Pose()]) - act2 = MoveTorsoAction([0.3]) + act2 = MoveTorsoAction([TorsoState.HIGH]) act3 = DetectAction([ObjectType.JEROEN_CUP]) plan = act ^ act2 ^ act3 @@ -67,7 +67,7 @@ def test_try_all_construction(self): def test_combination_construction(self): act = NavigateAction([Pose()]) - act2 = MoveTorsoAction([0.3]) + act2 = MoveTorsoAction([TorsoState.HIGH]) act3 = DetectAction([ObjectType.JEROEN_CUP]) plan = act + act2 | act3 @@ -77,7 +77,7 @@ def test_combination_construction(self): def test_code_construction(self): act = NavigateAction([Pose()]) - act2 = MoveTorsoAction([0.3]) + act2 = MoveTorsoAction([TorsoState.HIGH]) code = Code(lambda: True) plan = act + act2 + code @@ -98,7 +98,7 @@ def test_pickup_try_all_construction(self): def test_monitor_construction(self): act = ParkArmsAction([Arms.BOTH]) - act2 = MoveTorsoAction([0.3]) + act2 = MoveTorsoAction([TorsoState.HIGH]) def monitor_func(): time.sleep(1) @@ -118,7 +118,7 @@ def monitor_func(): def test_retry_monitor_construction(self): act = ParkArmsAction([Arms.BOTH]) - act2 = MoveTorsoAction([0.3]) + act2 = MoveTorsoAction([TorsoState.HIGH]) def monitor_func(): time.sleep(1) @@ -137,7 +137,7 @@ def recovery1(): def test_retry_monitor_tries(self): act = ParkArmsAction([Arms.BOTH]) - act2 = MoveTorsoAction([0.3]) + act2 = MoveTorsoAction([TorsoState.HIGH]) tries_counter = 0 def monitor_func(): @@ -180,7 +180,7 @@ def recovery2(): PlanFailure: recover2} act = ParkArmsAction([Arms.BOTH]) - act2 = MoveTorsoAction([0.3]) + act2 = MoveTorsoAction([TorsoState.HIGH]) subplan = act + act2 >> Monitor(monitor_func) plan = RetryMonitor(subplan, max_tries=6, recovery=recovery) try: @@ -192,7 +192,7 @@ def recovery2(): def test_repeat_construction(self): act = ParkArmsAction([Arms.BOTH]) - act2 = MoveTorsoAction([0.3]) + act2 = MoveTorsoAction([TorsoState.HIGH]) plan = (act + act2) * 5 self.assertEqual(len(plan.children), 1) @@ -200,7 +200,7 @@ def test_repeat_construction(self): def test_repeat_construction_reverse(self): act = ParkArmsAction([Arms.BOTH]) - act2 = MoveTorsoAction([0.3]) + act2 = MoveTorsoAction([TorsoState.HIGH]) plan = 5 * (act + act2) self.assertEqual(len(plan.children), 1) @@ -208,14 +208,14 @@ def test_repeat_construction_reverse(self): def test_repeat_construction_error(self): act = ParkArmsAction([Arms.BOTH]) - act2 = MoveTorsoAction([0.3]) + act2 = MoveTorsoAction([TorsoState.HIGH]) park = ParkArmsAction([Arms.BOTH]) self.assertRaises(AttributeError, lambda: (act + act2) * park) def test_perform_desig(self): act = NavigateAction([Pose([0.3, 0.3, 0])]) - act2 = MoveTorsoAction([0.3]) + act2 = MoveTorsoAction([TorsoState.HIGH]) act3 = ParkArmsAction([Arms.BOTH]) plan = act + act2 + act3 diff --git a/test/test_orm/test_orm.py b/test/test_orm/test_orm.py index a332e900e..f0b71814a 100644 --- a/test/test_orm/test_orm.py +++ b/test/test_orm/test_orm.py @@ -80,10 +80,11 @@ class ORMTaskTreeTestCase(DatabaseTestCaseMixin): def plan(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], [Arms.LEFT]) + torso_joint = RobotDescription.current_robot_description.torso_joint self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1]), True).perform() - MoveTorsoActionPerformable(0.3).perform() + MoveTorsoActionPerformable({torso_joint: 0.3}).perform() PickUpActionPerformable(object_description.resolve(), Arms.LEFT, Grasp.FRONT, 0.03).perform() description.resolve().perform() @@ -154,10 +155,11 @@ class MixinTestCase(DatabaseTestCaseMixin): def plan(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], [Arms.LEFT]) + torso_joint = RobotDescription.current_robot_description.torso_joint self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1]), True).perform() - MoveTorsoActionPerformable(0.3).perform() + MoveTorsoActionPerformable({torso_joint: 0.3}).perform() PickUpActionPerformable(object_description.resolve(), Arms.LEFT, Grasp.FRONT, 0.03).perform() description.resolve().perform() @@ -183,9 +185,10 @@ def test_plan_serialization(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], [Arms.LEFT]) self.assertEqual(description.ground().object_designator.name, "milk") + torso_joint = RobotDescription.current_robot_description.torso_joint with simulated_robot: NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1]), True).perform() - MoveTorsoActionPerformable(0.3).perform() + MoveTorsoActionPerformable({torso_joint: 0.3}).perform() PickUpActionPerformable(object_description.resolve(), Arms.LEFT, Grasp.FRONT, 0.03).perform() description.resolve().perform() pycram.orm.base.ProcessMetaData().description = "Unittest" @@ -386,10 +389,11 @@ def test_pickUpWithContextView(self): return object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], [Arms.LEFT]) + torso_joint = RobotDescription.current_robot_description.torso_joint self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - MoveTorsoActionPerformable(0.3).perform() + MoveTorsoActionPerformable({torso_joint: 0.3}).perform() PickUpActionPerformable(object_description.resolve(), Arms.LEFT, Grasp.FRONT).perform() description.resolve().perform() pycram.orm.base.ProcessMetaData().description = "pickUpWithContextView_test" @@ -408,10 +412,11 @@ def test_pickUpWithContextView_conditions(self): return object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], [Arms.LEFT]) + torso_joint = RobotDescription.current_robot_description.torso_joint self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - MoveTorsoActionPerformable(0.3).perform() + MoveTorsoActionPerformable({torso_joint: 0.3}).perform() PickUpActionPerformable(object_description.resolve(), Arms.LEFT, Grasp.FRONT).perform() description.resolve().perform() pycram.orm.base.ProcessMetaData().description = "pickUpWithContextView_conditions_test" diff --git a/test/test_task_tree.py b/test/test_task_tree.py index d59b4dbf2..8d7fcaca3 100644 --- a/test/test_task_tree.py +++ b/test/test_task_tree.py @@ -1,7 +1,7 @@ from pycram.designators.action_designator import MoveTorsoActionPerformable, PickUpActionPerformable, \ - NavigateActionPerformable + NavigateActionPerformable, MoveTorsoAction from pycram.datastructures.pose import Pose -from pycram.datastructures.enums import Arms, Grasp, GripperState +from pycram.datastructures.enums import Arms, Grasp, GripperState, TorsoState from pycram.process_module import simulated_robot import pycram.tasktree from pycram.tasktree import with_tree @@ -21,7 +21,7 @@ def plan(self): self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1]), True).perform() - MoveTorsoActionPerformable(0.3).perform() + MoveTorsoAction([TorsoState.HIGH]).resolve().perform() PickUpActionPerformable(object_description.resolve(), Arms.LEFT, Grasp.FRONT, 0.03).perform() description.resolve().perform()