diff --git a/config/multiverse_conf.py b/config/multiverse_conf.py
index 421e30828..bbba928fa 100644
--- a/config/multiverse_conf.py
+++ b/config/multiverse_conf.py
@@ -47,8 +47,7 @@ class MultiverseConfig(WorldConfig):
similar to bullet_world which uses the bullet physics engine.
"""
- use_controller: bool = False
- use_controller = use_controller and not use_static_mode
+ use_controller: bool = True
"""
Only used when use_static_mode is False. This turns on the controller for the robot joints.
"""
@@ -58,12 +57,14 @@ class MultiverseConfig(WorldConfig):
The default description type for the objects.
"""
- use_physics_simulator_state: bool = True
+ use_physics_simulator_state: bool = False
"""
Whether to use the physics simulator state when restoring or saving the world state.
"""
- clear_cache_at_start = False
+ validate_goals = True
+
+ clear_cache_at_start = True
let_pycram_move_attached_objects = False
let_pycram_handle_spawning = False
@@ -72,4 +73,8 @@ class MultiverseConfig(WorldConfig):
prismatic_joint_position_tolerance = 2e-2
use_giskard_monitor = False
- allow_gripper_collision = False
+ allow_gripper_collision = True
+
+ use_multiverse_process_modules = True
+
+ depth_images_are_in_meter = True
diff --git a/config/world_conf.py b/config/world_conf.py
index 8d57645c2..2c0eacf21 100644
--- a/config/world_conf.py
+++ b/config/world_conf.py
@@ -35,7 +35,7 @@ class WorldConfig:
Whether to clear the cache directory at the start.
"""
- prospection_world_prefix: str = "prospection_"
+ prospection_world_prefix: str = "prospection"
"""
The prefix for the prospection world name.
"""
@@ -84,6 +84,11 @@ class WorldConfig:
Whether to use a percentage of the goal as the acceptable error.
"""
+ validate_goals: bool = False
+ """
+ Whether to validate the goals when executing them.
+ """
+
raise_goal_validator_error: bool = False
"""
Whether to raise an error if the goals are not achieved.
@@ -99,6 +104,11 @@ class WorldConfig:
Whether to allow the gripper to collide with the objects when planning for the goals.
"""
+ depth_images_are_in_meter: bool = False
+ """
+ Whether the depth images produced by :meth:`datastructures.world.World.get_images_for_target` are in meters.
+ """
+
@classmethod
def get_pose_tolerance(cls) -> Tuple[float, float]:
return cls.position_tolerance, cls.orientation_tolerance
diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py
index 37c832344..96995c155 100644
--- a/demos/pycram_bullet_world_demo/demo.py
+++ b/demos/pycram_bullet_world_demo/demo.py
@@ -8,7 +8,7 @@
from pycram.object_descriptors.urdf import ObjectDescription
from pycram.world_concepts.world_object import Object
from pycram.datastructures.dataclasses import Color
-from pycram.ros.viz_marker_publisher import VizMarkerPublisher
+from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher
from pycrap import Robot, Apartment, Milk, Cereal, Spoon, Bowl
import numpy as np
@@ -101,4 +101,5 @@ def move_and_detect(obj_type):
ParkArmsAction([Arms.BOTH]).resolve().perform()
+
world.exit()
diff --git a/demos/pycram_multiverse_demo/demo.py b/demos/pycram_multiverse_demo/demo.py
index 6e71ec112..65850ceea 100644
--- a/demos/pycram_multiverse_demo/demo.py
+++ b/demos/pycram_multiverse_demo/demo.py
@@ -1,35 +1,31 @@
+from typing_extensions import Type
+
+import pycrap
from pycram.datastructures.dataclasses import Color
-from pycram.datastructures.enums import ObjectType, Arms, Grasp
+from pycram.datastructures.enums import ObjectType, Arms, Grasp, DetectionTechnique
from pycram.datastructures.pose import Pose
from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, TransportAction, NavigateAction, \
LookAtAction, DetectAction, OpenAction, PickUpAction, CloseAction, PlaceAction
from pycram.designators.location_designator import CostmapLocation, AccessingLocation
+from pycram.designators.motion_designator import MoveArmJointsMotion, MoveTCPMotion
from pycram.designators.object_designator import BelieveObject, ObjectPart
from pycram.object_descriptors.urdf import ObjectDescription
from pycram.process_module import simulated_robot, with_simulated_robot
+from pycram.robot_description import RobotDescription
from pycram.world_concepts.world_object import Object
from pycram.worlds.multiverse import Multiverse
+from pycrap import PhysicalObject
-@with_simulated_robot
-def move_and_detect(obj_type: ObjectType, pick_pose: Pose):
- NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform()
-
- LookAtAction(targets=[pick_pose]).resolve().perform()
-
- object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform()
-
- return object_desig
-
-world = Multiverse(simulation_name='pycram_test')
+world = Multiverse()
extension = ObjectDescription.get_file_extension()
-robot = Object('pr2', ObjectType.ROBOT, f'pr2{extension}', pose=Pose([1.3, 2, 0.01]))
-apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}")
+robot = Object('pr2', pycrap.Robot, f'pr2{extension}', pose=Pose([1.3, 2, 0.01]))
+apartment = Object("apartment", pycrap.Apartment, f"apartment{extension}")
-milk = Object("milk", ObjectType.MILK, f"milk.stl", pose=Pose([2.4, 2, 1.02]),
+milk = Object("milk", pycrap.Milk, f"milk.xml", pose=Pose([2.4, 2, 1.02]),
color=Color(1, 0, 0, 1))
-spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([2.5, 2.2, 0.85]),
+spoon = Object("spoon", pycrap.Spoon, "spoon.xml", pose=Pose([2.5, 2.2, 0.85]),
color=Color(0, 0, 1, 1))
apartment.attach(spoon, 'cabinet10_drawer1')
@@ -47,9 +43,10 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose):
LookAtAction(targets=[Pose([2.6, 2.15, 1])]).resolve().perform()
- milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform()
+ milk_desig = DetectAction(DetectionTechnique.TYPES,
+ object_designator_description=BelieveObject(types=[pycrap.Milk])).resolve().perform()[0]
- TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform()
+ TransportAction(milk_desig, [Pose([2.4, 3, 1.02])], [Arms.LEFT]).resolve().perform()
# Find and navigate to the drawer containing the spoon
handle_desig = ObjectPart(names=["cabinet10_drawer1_handle"], part_of=apartment_desig.resolve())
@@ -58,19 +55,34 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose):
NavigateAction([drawer_open_loc.pose]).resolve().perform()
- OpenAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform()
- spoon.detach(apartment)
+ OpenAction(object_designator_description=handle_desig,
+ arms=[drawer_open_loc.arms[0]]).resolve().perform()
+
+ arm_ee = RobotDescription.current_robot_description.get_arm_chain(drawer_open_loc.arms[0]).get_tool_frame()
+ closing_arm_pose = robot.get_link_pose(arm_ee)
# Detect and pickup the spoon
+ spoon.detach(apartment)
LookAtAction([apartment.get_link_pose("cabinet10_drawer1_handle")]).resolve().perform()
- spoon_desig = DetectAction(BelieveObject(types=[ObjectType.SPOON])).resolve().perform()
+ spoon_desig = DetectAction(DetectionTechnique.TYPES,
+ object_designator_description=BelieveObject(types=[pycrap.Spoon])).resolve().perform()[0]
+
+ ParkArmsAction([Arms.BOTH]).resolve().perform()
pickup_arm = Arms.LEFT if drawer_open_loc.arms[0] == Arms.RIGHT else Arms.RIGHT
+ pick_up_loc = CostmapLocation(target=spoon_desig.pose, reachable_for=robot_desig.resolve(),
+ reachable_arm=pickup_arm, grasps=[Grasp.TOP]).resolve()
+
+ NavigateAction([pick_up_loc.pose]).resolve().perform()
+ MoveTCPMotion(closing_arm_pose, drawer_open_loc.arms[0]).perform()
+
PickUpAction(spoon_desig, [pickup_arm], [Grasp.TOP]).resolve().perform()
ParkArmsAction([Arms.LEFT if pickup_arm == Arms.LEFT else Arms.RIGHT]).resolve().perform()
+ NavigateAction([drawer_open_loc.pose]).resolve().perform()
+
CloseAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform()
ParkArmsAction([Arms.BOTH]).resolve().perform()
@@ -79,7 +91,8 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose):
# Find a pose to place the spoon, move and then place it
spoon_target_pose = Pose([2.35, 2.6, 0.95], [0, 0, 0, 1])
- placing_loc = CostmapLocation(target=spoon_target_pose, reachable_for=robot_desig.resolve()).resolve()
+ placing_loc = CostmapLocation(target=spoon_target_pose,
+ reachable_for=robot_desig.resolve()).resolve()
NavigateAction([placing_loc.pose]).resolve().perform()
diff --git a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py
new file mode 100644
index 000000000..67dbf38da
--- /dev/null
+++ b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py
@@ -0,0 +1,34 @@
+import pycrap
+from pycram.datastructures.enums import GripperState, Arms
+from pycram.datastructures.world import UseProspectionWorld
+from pycram.process_module import simulated_robot, real_robot
+from pycram.world_concepts.world_object import Object
+from pycram.datastructures.pose import Pose
+from pycram.worlds.multiverse import Multiverse
+from pycram.designators.action_designator import SetGripperAction
+from pycram.ros_utils.robot_state_updater import WorldStateUpdater
+
+
+if __name__ == '__main__':
+ # Create a new world
+ world = Multiverse()
+ WorldStateUpdater(tf_topic="/tf", joint_state_topic="/real/ur5e/joint_states")
+
+ # Load the robot and the gripper
+ robot = Object("ur5e", pycrap.Robot, "universal_robot/ur5e/urdf/ur5e.urdf")
+ gripper = Object("gripper-2F-85", pycrap.Gripper, "robotiq/gripper-2F-85/gripper-2F-85.urdf")
+
+ # Attach the gripper to the robot at the wrist_3_link with the correct pose
+ wrist_3_tf_frame = robot.get_link_tf_frame("wrist_3_link")
+ gripper.set_pose(Pose([0, 0.1, 0], [1.0, 0.0, 0.0, -1.0], frame=wrist_3_tf_frame))
+ robot.attach(gripper, parent_link="wrist_3_link")
+
+ # Get the robot arms
+ robot_arms = [chain.arm_type for chain in robot.robot_description.get_manipulator_chains()]
+
+ # Perform the plan
+ with real_robot:
+ SetGripperAction(robot_arms, [GripperState.CLOSE]).resolve().perform()
+
+
+ world.exit()
diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py
index e69de29bb..d60276976 100644
--- a/demos/pycram_multiverse_demo/fallschool_demo.py
+++ b/demos/pycram_multiverse_demo/fallschool_demo.py
@@ -0,0 +1,87 @@
+import logging
+from datetime import timedelta
+
+import rospy
+from tf.transformations import quaternion_from_euler
+from typing_extensions import Type
+
+import pycrap
+from pycram.datastructures.dataclasses import Color
+from pycram.datastructures.enums import Arms
+from pycram.datastructures.pose import Pose
+from pycram.datastructures.world import UseProspectionWorld, World
+from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, TransportAction, NavigateAction, \
+ LookAtAction, DetectAction
+from pycram.designators.object_designator import BelieveObject
+from pycram.process_module import simulated_robot, with_simulated_robot, real_robot
+from pycram.ros_utils.robot_state_updater import WorldStateUpdater
+from pycram.world_concepts.world_object import Object
+from pycram.worlds.bullet_world import BulletWorld
+from pycram.worlds.multiverse import Multiverse
+from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher
+from pycrap import PhysicalObject
+
+
+@with_simulated_robot
+def move_and_detect(obj_type: Type[PhysicalObject], pick_pose: Pose):
+ NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform()
+
+ LookAtAction(targets=[pick_pose]).resolve().perform()
+
+ object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform()
+
+ return object_desig
+
+
+use_bullet_world = False
+
+if use_bullet_world:
+ world = BulletWorld(use_multiverse_for_real_world_simulation=True)
+ vis_publisher = VizMarkerPublisher()
+ milk_path = "milk.stl"
+else:
+ world = Multiverse()
+ vis_publisher = None
+ milk_path = "milk.xml"
+
+robot = Object('pr2', pycrap.Robot, f'pr2.urdf', pose=Pose([1.3, 2.6, 0.01]))
+WorldStateUpdater(tf_topic="/tf", joint_state_topic="/real/pr2/joint_states", update_rate=timedelta(seconds=2),
+ world=world)
+apartment = Object("apartment", pycrap.Apartment, f"apartment.urdf")
+milk = Object("milk", pycrap.Milk, milk_path, pose=Pose([0.4, 2.6, 1.34],
+ [1, 0, 0, 0]),
+ color=Color(1, 0, 0, 1))
+
+# apartment.set_joint_position("fridge_door1_joint", 1.5707963267948966)
+
+fridge_base_pose = apartment.get_link_pose("fridge_base")
+fridge_base_pose.position.z -= 0.12
+fridge_base_pose.position.x += 0.16
+fridge_base_pose.position.y += -0.1
+milk.set_pose(fridge_base_pose, base=True)
+
+
+robot_desig = BelieveObject(names=[robot.name])
+apartment_desig = BelieveObject(names=[apartment.name])
+
+
+with real_robot:
+
+ # Transport the milkMoveGripperMotion
+ ParkArmsAction([Arms.BOTH]).resolve().perform()
+
+ MoveTorsoAction([0.2]).resolve().perform()
+
+ NavigateAction(target_locations=[Pose([1.4, 3.15, 0.01], quaternion_from_euler(0, 0, 3.14))]).resolve().perform()
+
+ LookAtAction(targets=[Pose(milk.get_position_as_list())]).resolve().perform()
+
+ milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform()
+
+ TransportAction(milk_desig, [Pose([2.4, 3, 1.02])], [Arms.LEFT]).resolve().perform()
+
+ ParkArmsAction([Arms.BOTH]).resolve().perform()
+
+if vis_publisher is not None:
+ vis_publisher._stop_publishing()
+world.exit()
diff --git a/demos/pycram_multiverse_demo/testing_giskard.py b/demos/pycram_multiverse_demo/testing_giskard.py
new file mode 100644
index 000000000..78d80ae72
--- /dev/null
+++ b/demos/pycram_multiverse_demo/testing_giskard.py
@@ -0,0 +1,147 @@
+import os
+
+import actionlib
+from control_msgs.msg import GripperCommandGoal, GripperCommandAction
+from giskard_msgs.msg import WorldResult
+from typing_extensions import Optional
+
+from giskardpy.python_interface.old_python_interface import OldGiskardWrapper as GiskardWrapper
+
+from geometry_msgs.msg import PoseStamped, Point, Quaternion
+
+from pycram.helper import find_multiverse_resources_path
+import pycram.ros # this to start the ros node
+from pycram.ros.logging import loginfo
+
+
+def spawn_urdf(name: str, urdf_path: str, pose: PoseStamped) -> WorldResult:
+ """
+ Spawns an URDF in giskard's belief state.
+
+ :param name: Name of the URDF
+ :param urdf_path: Path to the URDF file
+ :param pose: Pose in which the URDF should be spawned
+ :return: A WorldResult message
+ """
+ with open(urdf_path) as f:
+ urdf_string = f.read()
+
+ return giskard.add_urdf(name, urdf_string, pose)
+
+
+def get_pose_stamped(position, orientation):
+ pose_stamped = PoseStamped()
+ pose_stamped.header.frame_id = 'map'
+ pose_stamped.pose.position = Point(*position)
+ pose_stamped.pose.orientation = Quaternion(*orientation)
+ return pose_stamped
+
+
+def park_arms():
+ joint_goals = {'l_shoulder_pan_joint': 1.712, 'l_shoulder_lift_joint': -0.264, 'l_upper_arm_roll_joint': 1.38,
+ 'l_elbow_flex_joint': -2.12, 'l_forearm_roll_joint': 16.996, 'l_wrist_flex_joint': -0.073,
+ 'l_wrist_roll_joint': 0.0, 'r_shoulder_pan_joint': -1.712, 'r_shoulder_lift_joint': -0.256,
+ 'r_upper_arm_roll_joint': -1.463, 'r_elbow_flex_joint': -2.12, 'r_forearm_roll_joint': 1.766,
+ 'r_wrist_flex_joint': -0.07, 'r_wrist_roll_joint': 0.051}
+ giskard.set_joint_goal(joint_goals)
+ giskard.execute()
+
+
+def get_urdf_string(urdf_path):
+ with open(urdf_path) as f:
+ return f.read()
+
+
+def open_left_gripper():
+ open_gripper("left")
+
+
+def close_left_gripper():
+ close_gripper("left")
+
+
+def open_right_gripper():
+ open_gripper("right")
+
+
+def close_right_gripper():
+ close_gripper("right")
+
+
+def open_gripper(gripper: str):
+ move_gripper("open", gripper)
+
+
+def close_gripper(gripper: str):
+ move_gripper("close", gripper)
+
+
+def move_gripper(cmd: str, gripper: str):
+ def activate_callback():
+ loginfo("Started gripper Movement")
+
+ def done_callback(state, result):
+ loginfo(f"Reached goal: {result.reached_goal}")
+
+ def feedback_callback(msg):
+ pass
+
+ goal = GripperCommandGoal()
+ goal.command.position = 0.0 if cmd == "close" else 0.4
+ goal.command.max_effort = 50.0
+ if gripper == "right":
+ controller_topic = "/real/pr2/right_gripper_controller/gripper_cmd"
+ else:
+ controller_topic = "/real/pr2/left_gripper_controller/gripper_cmd"
+ client = actionlib.SimpleActionClient(controller_topic, GripperCommandAction)
+ loginfo("Waiting for action server")
+ client.wait_for_server()
+ client.send_goal(goal, active_cb=activate_callback, done_cb=done_callback, feedback_cb=feedback_callback)
+ wait = client.wait_for_result()
+
+
+def move_base(pose_stamped: PoseStamped):
+ giskard.set_cart_goal(pose_stamped, 'base_link', 'map', add_monitor=False)
+ giskard.execute()
+
+
+def move_left_arm_tool(pose_stamped: PoseStamped):
+ move_arm_tool(pose_stamped, 'left')
+
+
+def move_right_arm_tool(pose_stamped: PoseStamped):
+ move_arm_tool(pose_stamped, 'right')
+
+
+def move_arm_tool(pose_stamped: PoseStamped, arm: str):
+ tool_frame = 'l_gripper_tool_frame' if arm == 'left' else 'r_gripper_tool_frame'
+ giskard.set_cart_goal(pose_stamped, tool_frame, 'torso_lift_link', add_monitor=False)
+ giskard.execute()
+
+
+if __name__ == '__main__':
+
+ multiverse_resources = find_multiverse_resources_path()
+ cached_dir = multiverse_resources + 'cached/'
+
+ giskard = GiskardWrapper()
+
+ giskard.add_urdf('apartment',
+ get_urdf_string(cached_dir + 'apartment.urdf'),
+ get_pose_stamped([0, 0, 0], [0, 0, 0, 1]))
+
+ park_arms()
+
+ move_base(get_pose_stamped([1.17, 2.655, 0], [0.0, 0.0, -0.992, 0.123]))
+
+ open_left_gripper()
+
+ move_left_arm_tool(get_pose_stamped([0.47, 2.48, 1.04], [0, 0, 0, 1]))
+
+ close_left_gripper()
+
+ move_left_arm_tool(get_pose_stamped([0.5, 2.48, 1.04], [0, 0, 0, 1]))
+
+ park_arms()
+
+ move_base(get_pose_stamped([1.4, 3.5, 0], [0.0, 0.0, 0, 1]))
diff --git a/examples/cram_plan_tutorial.md b/examples/cram_plan_tutorial.md
index b09c6a8ad..33458861c 100644
--- a/examples/cram_plan_tutorial.md
+++ b/examples/cram_plan_tutorial.md
@@ -36,18 +36,25 @@ import pycrap
np.random.seed(4)
-world = BulletWorld()
+use_multiverse = False
+viz_marker_publisher = None
+if use_multiverse:
+ try:
+ from pycram.worlds.multiverse import Multiverse
+ world = Multiverse()
+ except ImportError:
+ raise ImportError("Multiverse is not installed, please install it to use it.")
+else:
+ from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher
+ world = BulletWorld()
+ viz_marker_publisher = VizMarkerPublisher()
+
robot = Object("pr2", pycrap.Robot, "pr2.urdf")
robot_desig = ObjectDesignatorDescription(names=['pr2']).resolve()
-apartment = Object("apartment", pycrap.Apartment, "apartment.urdf", pose=Pose([-1.5, -2.5, 0]))
+apartment = Object("apartment", pycrap.Apartment, "apartment.urdf")
apartment_desig = ObjectDesignatorDescription(names=['apartment']).resolve()
-table_top = apartment.get_link_position("cooktop")
-# milk = Object("milk", "milk", "milk.stl", position=[table_top[0]-0.15, table_top[1], table_top[2]])
-# milk.set_position(position=milk.get_position(), base=True)
-# cereal = Object("cereal", "cereal", "breakfast_cereal.stl", position=table_top)
-# cereal.set_position(position=[table_top[0]-0.1, table_top[1] + 0.5, table_top[2]], base=True)
-# milk_desig = ObjectDesignator(ObjectDesignatorDescription(name="milk", type="milk"))
-# cereal_desig = ObjectDesignator(ObjectDesignatorDescription(name="cereal", type="cereal"))
+table_top_name = "stove" if use_multiverse else "cooktop"
+table_top = apartment.get_link_position(table_top_name)
```
```python
@@ -68,11 +75,8 @@ def get_n_random_positions(pose_list, n=4, dist=0.5, random=True):
for i in range(len(positions) - 1):
pos_idx = np.random.choice(all_indices) if random else all_indices[i]
diff = np.absolute(np.linalg.norm(n_positions - positions[pos_idx], axis=1))
- # print(diff)
min_diff = np.min(diff)
- # print(min_diff)
if min_diff >= dist:
- # print("found")
n_positions[found_count, :] = positions[pos_idx]
found_indices.append(pos_idx)
found_count += 1
@@ -80,31 +84,22 @@ def get_n_random_positions(pose_list, n=4, dist=0.5, random=True):
if found_count == n:
break
found_poses = [pose_list[i] for i in found_indices]
- # found_positions = [positions[i] for i in found_indices]
- # for i in range(len(found_positions)):
- # print(found_poses[i][0])
- # print(found_positions[i])
- # assert np.allclose(found_positions[i],found_poses[i][0])
- # for i in range(len(found_poses)):
- # for j in range(i+1,len(found_poses)):
- # pos1 = np.array(found_poses[i][0])
- # pos2 = np.array(found_poses[j][0])
- # diff = np.absolute(np.linalg.norm(pos1 - pos2))
- # print(diff)
- # assert diff >= dist
return found_poses
-
-
-
```
```python
+from tf.transformations import quaternion_from_euler
import pycrap
from pycram.costmaps import SemanticCostmap
from pycram.pose_generator_and_validator import PoseGenerator
-scm = SemanticCostmap(apartment, "island_countertop")
-poses_list = list(PoseGenerator(scm, number_of_samples=-1))
+counter_name = "counter_sink_stove" if use_multiverse else "island_countertop"
+counter_link = apartment.get_link(counter_name)
+counter_bounds = counter_link.get_axis_aligned_bounding_box()
+scm = SemanticCostmap(apartment, counter_name)
+# take only 6 cms from edges as viable region
+edges_cm = scm.get_edges_map(0.06, horizontal_only=True)
+poses_list = list(PoseGenerator(edges_cm, number_of_samples=-1))
poses_list.sort(reverse=True, key=lambda x: np.linalg.norm(x.position_as_list()))
object_poses = get_n_random_positions(poses_list)
object_names = ["bowl", "breakfast_cereal", "spoon"]
@@ -112,11 +107,18 @@ object_types = [pycrap.Bowl, pycrap.Cereal, pycrap.Spoon]
objects = {}
object_desig = {}
for obj_name, obj_type, obj_pose in zip(object_names, object_types, object_poses):
+ if obj_pose.position.x > counter_link.position.x:
+ z_angle = np.pi
+ else:
+ z_angle = 0
+ orientation = quaternion_from_euler(0, 0, z_angle).tolist()
objects[obj_name] = Object(obj_name, obj_type, obj_name + ".stl",
- pose=Pose([obj_pose.position.x, obj_pose.position.y, table_top.z]))
+ pose=Pose([obj_pose.position.x, obj_pose.position.y, table_top.z],
+ orientation))
objects[obj_name].move_base_to_origin_pose()
objects[obj_name].original_pose = objects[obj_name].pose
object_desig[obj_name] = ObjectDesignatorDescription(names=[obj_name], types=[obj_type]).resolve()
+world.update_original_state()
```
If You want to visualize all apartment frames
@@ -138,12 +140,13 @@ Finally, we create a plan where the robot parks his arms, walks to the kitchen c
execute the plan.
```python
+import pycrap
from pycram.external_interfaces.ik import IKError
from pycram.datastructures.enums import Grasp
@pycram.tasktree.with_tree
-def plan(obj, obj_desig, torso=0.2, place="countertop"):
+def plan(obj_desig: ObjectDesignatorDescription.Object, torso=0.2, place=counter_name):
world.reset_world()
with simulated_robot:
ParkArmsActionPerformable(Arms.BOTH).perform()
@@ -156,26 +159,46 @@ def plan(obj, obj_desig, torso=0.2, place="countertop"):
ParkArmsActionPerformable(Arms.BOTH).perform()
good_torsos.append(torso)
picked_up_arm = pose.reachable_arms[0]
- PickUpActionPerformable(object_designator=obj_desig, arm=pose.reachable_arms[0], grasp=Grasp.FRONT,
+ grasp = Grasp.TOP if issubclass(obj_desig.world_object.obj_type, pycrap.Spoon) else Grasp.FRONT
+ PickUpActionPerformable(object_designator=obj_desig, arm=pose.reachable_arms[0], grasp=grasp,
prepose_distance=0.03).perform()
ParkArmsActionPerformable(Arms.BOTH).perform()
- scm = SemanticCostmapLocation(place, apartment_desig, obj_desig)
- scm = iter(scm)
- pose_island = None
- for i in range(np.random.randint(5, 15)):
- pose_island = next(scm)
- print(pose_island)
-
-
- place_location = CostmapLocation(target=pose_island.pose, reachable_for=robot_desig,
- reachable_arm=picked_up_arm)
- pose = place_location.resolve()
-
- NavigateActionPerformable(pose.pose, True).perform()
-
- PlaceActionPerformable(object_designator=obj_desig, target_location=pose_island.pose,
+ scm = SemanticCostmapLocation(place, apartment_desig, obj_desig, horizontal_edges_only=True, edge_size_in_meters=0.08)
+ scm_iter = iter(scm)
+ n_retries = 0
+ found = False
+ while not found:
+ try:
+ print(f"Trying {n_retries} to find a place")
+ if n_retries == 0:
+ pose_island = next(scm_iter)
+ cost_map_size = len(np.where(scm.sem_costmap.map > 0)[0])
+ print(f"cost_map_size: {cost_map_size}")
+ else:
+ for _ in range(np.random.randint(100, 110)):
+ pose_island = next(scm_iter)
+ print("found pose_island")
+ if pose_island.pose.position.x > counter_link.position.x:
+ z_angle = np.pi
+ else:
+ z_angle = 0
+ orientation = quaternion_from_euler(0, 0, z_angle).tolist()
+ pose_island.pose = Pose(pose_island.pose.position_as_list(), orientation)
+ pose_island.pose.position.z += 0.07
+ print(pose_island.pose.position)
+ place_location = CostmapLocation(target=pose_island.pose, reachable_for=robot_desig, reachable_arm=picked_up_arm)
+ pose = place_location.resolve()
+ NavigateActionPerformable(pose.pose, True).perform()
+ PlaceActionPerformable(object_designator=obj_desig, target_location=pose_island.pose,
arm=picked_up_arm).perform()
+ found = True
+ except (StopIteration, IKError) as e:
+ print("Retrying")
+ print(e)
+ n_retries += 1
+ if n_retries > 3:
+ raise StopIteration("No place found")
ParkArmsActionPerformable(Arms.BOTH).perform()
@@ -183,15 +206,16 @@ def plan(obj, obj_desig, torso=0.2, place="countertop"):
good_torsos = []
for obj_name in object_names:
done = False
- torso = 0.25 if len(good_torsos) == 0 else good_torsos[-1]
+ torso = 0.3 if len(good_torsos) == 0 else good_torsos[-1]
while not done:
try:
- plan(objects[obj_name], object_desig[obj_name], torso=torso, place="island_countertop")
+ plan(object_desig[obj_name], torso=torso, place=counter_name)
done = True
+ print(f"Object {obj_name} placed")
objects[obj_name].original_pose = objects[obj_name].pose
+ world.update_original_state()
except (StopIteration, IKError) as e:
print(type(e))
- print(e)
print("no solution")
torso += 0.05
if torso > 0.3:
@@ -203,84 +227,7 @@ Now we get the task tree from its module and render it. Rendering can be done wi
anytree package. We will use ascii rendering here for ease of displaying.
```python
-tt = pycram.tasktree.task_tree
-print(anytree.RenderTree(tt, style=anytree.render.AsciiStyle()))
-```
-
-As we see every task in the plan got recorded correctly. It is noticeable that the tree begins with a NoOperation node.
-This is done because several, not connected, plans that get executed after each other should still appear in the task
-tree. Hence, a NoOperation node is the root of any tree. If we re-execute the plan we would see them appear in the same
-tree even though they are not connected.
-
-```python
-world.reset_world()
-plan(objects["bowl"], object_desig["bowl"], torso=0.25)
-print(anytree.RenderTree(tt, style=anytree.render.AsciiStyle()))
-```
-
-Projecting a plan in a new environment with its own task tree that only exists while the projected plan is running can
-be done with the ``with`` keyword. When this is done, both the bullet world and task tree are saved and new, freshly
-reset objects are available. At the end of a with block the old state is restored. The root for such things is then
-called ``simulation()``.
-
-```python
-with pycram.tasktree.SimulatedTaskTree() as stt:
- print(anytree.RenderTree(pycram.tasktree.task_tree, style=anytree.render.AsciiStyle()))
-print(anytree.RenderTree(pycram.tasktree.task_tree, style=anytree.render.AsciiStyle()))
-```
-
-Task tree can be manipulated with ordinary anytree manipulation. If we for example want to discard the second plan, we
-would write:
-
-```python
-tt.root.children = (tt.root.children[0],)
-print(anytree.RenderTree(tt, style=anytree.render.AsciiStyle()))
-```
-
-We can now re-execute this (modified) plan by executing the leaf in pre-ordering iteration using the anytree
-functionality. This will not append the re-execution to the task tree.
-
-```python
-world.reset_world()
-with simulated_robot:
- [node.action.perform() for node in tt.root.leaves]
-print(anytree.RenderTree(pycram.tasktree.task_tree, style=anytree.render.AsciiStyle()))
-```
-
-Nodes in the task tree contain additional information about the status and time of a task.
-
-```python
-print(pycram.tasktree.task_tree.children[0])
-```
-
-The task tree can also be reset to an empty one by invoking:
-
-```python
-pycram.tasktree.task_tree.reset_tree()
-print(anytree.RenderTree(pycram.tasktree.task_tree, style=anytree.render.AsciiStyle()))
-```
-
-If a plan fails using the PlanFailure exception, the plan will stop and raise the respective error.
-Additionally, the error will be logged in the node of the TaskTree. First let's create a simple failing plan and execute it.
-
-```python
-@pycram.tasktree.with_tree
-def failing_plan():
- raise pycram.failures.PlanFailure("Oopsie!")
-
-try:
- failing_plan()
-except pycram.failures.PlanFailure as e:
- print(e)
-```
-
-We can now investigate the nodes of the tree, and we will see that the tree indeed contains a failed task.
-
-```python
-print(anytree.RenderTree(pycram.tasktree.task_tree, style=anytree.render.AsciiStyle()))
-print(pycram.tasktree.task_tree.children[0])
-```
-
-```python
+if viz_marker_publisher is not None:
+ viz_marker_publisher._stop_publishing()
world.exit()
```
diff --git a/examples/intro.md b/examples/intro.md
index c2251194a..48fad1898 100644
--- a/examples/intro.md
+++ b/examples/intro.md
@@ -134,7 +134,7 @@ o = cm.OccupancyCostmap(0.2, from_ros=False, size=300, resolution=0.02, origin=P
```
```python
-s = cm.SemanticCostmap(kitchen, "kitchen_island_surface", size=100, resolution=0.02)
+s = cm.SemanticCostmap(kitchen, "kitchen_island_surface", resolution=0.02)
g = cm.GaussianCostmap(200, 15, resolution=0.02)
```
diff --git a/examples/location_designator.md b/examples/location_designator.md
index 58a0dac45..d213b7332 100644
--- a/examples/location_designator.md
+++ b/examples/location_designator.md
@@ -44,7 +44,19 @@ from pycram.datastructures.enums import ObjectType, WorldMode
from pycram.datastructures.pose import Pose
import pycrap
-world = BulletWorld(WorldMode.DIRECT)
+use_multiverse = False
+viz_marker_publisher = None
+if use_multiverse:
+ try:
+ from pycram.worlds.multiverse import Multiverse
+ world = Multiverse()
+ except ImportError:
+ raise ImportError("Multiverse is not installed, please install it to use it.")
+else:
+ from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher
+ world = BulletWorld()
+ viz_marker_publisher = VizMarkerPublisher()
+
apartment = Object("apartment", pycrap.Apartment, "apartment.urdf")
pr2 = Object("pr2", pycrap.Robot, "pr2.urdf")
```
@@ -139,7 +151,8 @@ from pycram.designators.object_designator import BelieveObject
kitchen_desig = BelieveObject(names=["apartment"]).resolve()
milk_desig = BelieveObject(names=["milk"]).resolve()
-location_description = SemanticCostmapLocation(urdf_link_name="island_countertop",
+counter_name = "counter_sink_stove" if use_multiverse else "island_countertop"
+location_description = SemanticCostmapLocation(link_name=counter_name,
part_of=kitchen_desig,
for_object=milk_desig)
@@ -178,13 +191,14 @@ spawned it in a previous example. Furthermore, we need a robot, so we also spawn
```python
from pycram.designators.object_designator import *
from pycram.designators.location_designator import *
-from pycram.datastructures.enums import ObjectType
apartment_desig = BelieveObject(names=["apartment"])
-handle_desig = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig.resolve())
+handle_name = "cabinet10_drawer1_handle" if use_multiverse else "handle_cab10_t"
+handle_desig = ObjectPart(names=[handle_name], part_of=apartment_desig.resolve())
robot_desig = BelieveObject(types=[pycrap.Robot])
-access_location = AccessingLocation(handle_desig.resolve(), robot_desig.resolve()).resolve()
+access_location = AccessingLocation(handle_desig.resolve(), robot_desig.resolve(),
+ prepose_distance=0.03).resolve()
print(access_location.pose)
```
@@ -212,5 +226,7 @@ if "/giskard" in rosnode.get_node_names():
If you are finished with this example you can close the world with the following cell:
```python
+if viz_marker_publisher is not None:
+ viz_marker_publisher._stop_publishing()
world.exit()
```
diff --git a/launch/ik_and_description.launch b/launch/ik_and_description.launch
index 4484eaf86..52565a7fa 100644
--- a/launch/ik_and_description.launch
+++ b/launch/ik_and_description.launch
@@ -26,6 +26,11 @@
+
+
+
+
Costmap:
elif self.resolution != other_cm.resolution:
raise ValueError("To merge two costmaps their resolution must be equal.")
new_map = np.zeros((self.height, self.width))
- # A nunpy array of the positions where both costmaps are greater than 0
+ # A numpy array of the positions where both costmaps are greater than 0
merge = np.logical_and(self.map > 0, other_cm.map > 0)
new_map[merge] = self.map[merge] * other_cm.map[merge]
- new_map = (new_map / np.max(new_map)).reshape((self.height, self.width))
+ max_val = np.max(new_map)
+ if max_val != 0:
+ new_map = (new_map / np.max(new_map)).reshape((self.height, self.width))
+ else:
+ new_map = new_map.reshape((self.height, self.width))
+ logwarn("Merged costmap is empty.")
return Costmap(self.resolution, self.height, self.width, self.origin, new_map)
def __add__(self, other: Costmap) -> Costmap:
@@ -446,6 +454,7 @@ def _create_from_world(self, size: int, resolution: float) -> np.ndarray:
# 16383 is the maximal number of rays that can be processed in a batch
i = 0
j = 0
+ floor_id = self.world.get_object_by_name("floor").id
for n in self._chunks(np.array(rays), 16380):
r_t = World.current_world.ray_test_batch(n[:, 0], n[:, 1], num_threads=0)
while r_t is None:
@@ -454,10 +463,10 @@ def _create_from_world(self, size: int, resolution: float) -> np.ndarray:
if World.robot:
attached_objs_id = [o.id for o in self.world.robot.attachments.keys()]
res[i:j] = [
- 1 if ray[0] == -1 or ray[0] == self.world.robot.id or ray[0] in attached_objs_id else 0 for
+ 1 if ray[0] in [-1, self.world.robot.id, floor_id] + attached_objs_id else 0 for
ray in r_t]
else:
- res[i:j] = [1 if ray[0] == -1 else 0 for ray in r_t]
+ res[i:j] = [1 if ray[0] in [-1, floor_id] else 0 for ray in r_t]
i += len(n)
res = np.flip(np.reshape(np.array(res), (size, size)))
@@ -505,7 +514,9 @@ def __init__(self, min_height: float,
size: Optional[int] = 100,
resolution: Optional[float] = 0.02,
origin: Optional[Pose] = None,
- world: Optional[World] = None):
+ world: Optional[World] = None,
+ target_object: Optional[Object] = None,
+ robot: Optional[Object] = None):
"""
Visibility Costmaps show for every position around the origin pose if the origin can be seen from this pose.
The costmap is able to deal with height differences of the camera while in a single position, for example, if
@@ -522,6 +533,8 @@ def __init__(self, min_height: float,
:param origin: The pose in world coordinate frame around which the
costmap should be created.
:param world: The World for which the costmap should be created.
+ :param target_object: The object that should be visible.
+ :param robot: The robot for which the visibility costmap should be created.
"""
if (11 * size ** 2 + size ** 3) * 2 > psutil.virtual_memory().available:
raise OSError("Not enough free RAM to calculate a costmap of this size")
@@ -535,9 +548,53 @@ def __init__(self, min_height: float,
# for pr2 = 1.6
self.min_height: float = min_height
self.origin: Pose = Pose() if not origin else origin
+ self.target_object: Optional[Object] = target_object
+ self.robot: Optional[Object] = robot
self._generate_map()
Costmap.__init__(self, resolution, size, size, self.origin, self.map)
+ @property
+ def robot(self) -> Optional[Object]:
+ return self._robot
+
+ @robot.setter
+ def robot(self, robot: Optional[Object]) -> None:
+ if robot is not None:
+ self._robot = World.current_world.get_prospection_object_for_object(robot)
+ self.robot_original_pose = self._robot.pose
+ else:
+ self._robot = None
+ self.robot_original_pose = None
+
+ @property
+ def target_object(self) -> Optional[Object]:
+ return self._target_object
+
+ @target_object.setter
+ def target_object(self, target_object: Optional[Object]) -> None:
+ if target_object is not None:
+ self._target_object = World.current_world.get_prospection_object_for_object(target_object)
+ self.target_original_pose = self._target_object.pose
+ else:
+ self._target_object = None
+ self.target_original_pose = None
+
+ def move_target_and_robot_far_away(self):
+ if self.target_object is not None:
+ self.target_object.set_pose(Pose([self.origin.position.x + self.size * self.resolution * 2,
+ self.origin.position.y + self.size * self.resolution * 2,
+ self.target_original_pose.position.z]))
+ if self.robot is not None:
+ self.robot.set_pose(Pose([self.origin.position.x + self.size * self.resolution * 3,
+ self.origin.position.y + self.size * self.resolution * 3,
+ self.robot_original_pose.position.z]))
+
+ def return_target_and_robot_to_their_original_position(self):
+ if self.target_original_pose is not None:
+ self.target_object.set_pose(self.target_original_pose)
+ if self.robot_original_pose is not None:
+ self.robot.set_pose(self.robot_original_pose)
+
def _create_images(self) -> List[np.ndarray]:
"""
Creates four depth images in every direction around the point
@@ -550,6 +607,8 @@ def _create_images(self) -> List[np.ndarray]:
images = []
camera_pose = self.origin
+ self.move_target_and_robot_far_away()
+
with UseProspectionWorld():
origin_copy = self.origin.copy()
origin_copy.position.y += 1
@@ -568,8 +627,11 @@ def _create_images(self) -> List[np.ndarray]:
origin_copy.position.x += 1
images.append(self.world.get_images_for_target(origin_copy, camera_pose, size=self.size)[1])
- for i in range(0, 4):
- images[i] = self._depth_buffer_to_meter(images[i])
+ self.return_target_and_robot_to_their_original_position()
+
+ if not World.current_world.conf.depth_images_are_in_meter:
+ for i in range(0, 4):
+ images[i] = self._depth_buffer_to_meter(images[i])
return images
def _depth_buffer_to_meter(self, buffer: np.ndarray) -> np.ndarray:
@@ -738,21 +800,21 @@ class SemanticCostmap(Costmap):
table surface.
"""
- def __init__(self, object, urdf_link_name, size=100, resolution=0.02, world=None):
+ def __init__(self, obj: Object, link_name: str, resolution: float = 0.02, world: Optional[World] = None):
"""
Creates a semantic costmap for the given parameter. The semantic costmap will be on top of the link of the given
Object.
- :param object: The object of which the link is a part
- :param urdf_link_name: The link name, as stated in the URDF
- :param resolution: Resolution of the final costmap
+ :param obj: The object of which the link is a part
+ :param link_name: The link name, as stated in the description of the object
+ :param resolution: Resolution of the final costmap (how much meters one pixel represents)
:param world: The World from which the costmap should be created
"""
self.world: World = world if world else World.current_world
- self.object: Object = object
- self.link: Link = object.get_link(urdf_link_name)
+ self.object: Object = obj
+ self.link: Link = obj.get_link(link_name)
self.resolution: float = resolution
- self.origin: Pose = object.get_link_pose(urdf_link_name)
+ self.origin: Pose = obj.get_link_pose(link_name)
self.height: int = 0
self.width: int = 0
self.map: np.ndarray = []
@@ -760,6 +822,23 @@ def __init__(self, object, urdf_link_name, size=100, resolution=0.02, world=None
Costmap.__init__(self, resolution, self.height, self.width, self.origin, self.map)
+ def get_edges_map(self, margin_in_meters: float, horizontal_only: bool = False) -> Costmap:
+ """
+ Return a Costmap with only the edges of the original Costmap marked as possible positions.
+
+ :param margin_in_meters: The edge thickness in meters that should be marked as possible positions.
+ :param horizontal_only: If True only the horizontal edges will be marked as possible positions.
+ :return: The modified Costmap.
+ """
+ mask = np.zeros(self.map.shape)
+ edge_tolerance = int(margin_in_meters / self.resolution)
+ mask[:edge_tolerance] = 1
+ mask[-edge_tolerance:] = 1
+ if not horizontal_only:
+ mask[:, :edge_tolerance] = 1
+ mask[:, -edge_tolerance:] = 1
+ return Costmap(self.resolution, self.height, self.width, self.origin, mask)
+
def generate_map(self) -> None:
"""
Generates the semantic costmap according to the provided parameters. To do this the axis aligned bounding box (AABB)
@@ -772,18 +851,11 @@ def generate_map(self) -> None:
def get_aabb_for_link(self) -> AxisAlignedBoundingBox:
"""
-
- :return: The axis aligned bounding box (AABB) of the link provided when creating this costmap. To try and let
- the AABB as close to the actual object as possible, the Object will be rotated such that the link will be in the
- identity orientation.
+ :return: The original untransformed (doesn't take the current pose of the link into consideration, since only
+ the size is important here not the pose) axis aligned bounding box (AABB) of the link provided when creating
+ this costmap.
"""
- prospection_object = World.current_world.get_prospection_object_for_object(self.object)
- with UseProspectionWorld():
- prospection_object.set_orientation(Pose(orientation=[0, 0, 0, 1]))
- link_pose_trans = self.link.transform
- inverse_trans = link_pose_trans.invert()
- prospection_object.set_orientation(inverse_trans.to_pose())
- return self.link.get_axis_aligned_bounding_box()
+ return self.link.get_axis_aligned_bounding_box_from_geometry()
class AlgebraicSemanticCostmap(SemanticCostmap):
diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py
index c6f55bf52..e083f42bf 100644
--- a/src/pycram/datastructures/dataclasses.py
+++ b/src/pycram/datastructures/dataclasses.py
@@ -1,8 +1,9 @@
from __future__ import annotations
+import os
from abc import ABC, abstractmethod
from copy import deepcopy, copy
-from dataclasses import dataclass, field
+from dataclasses import dataclass, fields, field
import numpy as np
import trimesh
@@ -172,6 +173,18 @@ def depth(self) -> float:
@dataclass
class AxisAlignedBoundingBox(BoundingBox):
+ @classmethod
+ def from_origin_and_half_extents(cls, origin: Point, half_extents: Point):
+ """
+ Set the axis-aligned bounding box from the origin of the body and half the size.
+
+ :param origin: The origin point
+ :param half_extents: The half size of the bounding box.
+ """
+ min_point = [origin.x - half_extents.x, origin.y - half_extents.y, origin.z - half_extents.z]
+ max_point = [origin.x + half_extents.x, origin.y + half_extents.y, origin.z + half_extents.z]
+ return cls.from_min_max(min_point, max_point)
+
@classmethod
def from_multiple_bounding_boxes(cls, bounding_boxes: List[AxisAlignedBoundingBox]) -> 'AxisAlignedBoundingBox':
"""
@@ -633,14 +646,11 @@ class ContactPoint:
link_b: Link
position_on_object_a: Optional[List[float]] = None
position_on_object_b: Optional[List[float]] = None
- normal_on_b: Optional[List[float]] = None # normal on object b pointing towards object a
- distance: Optional[float] = None
+ normal_on_b: Optional[List[float]] = None # the contact normal vector on object b pointing towards object a
+ distance: Optional[float] = None # distance between the two objects (+ve for separation, -ve for penetration)
normal_force: Optional[List[float]] = None # normal force applied during last step simulation
lateral_friction_1: Optional[LateralFriction] = None
lateral_friction_2: Optional[LateralFriction] = None
- force_x_in_world_frame: Optional[float] = None
- force_y_in_world_frame: Optional[float] = None
- force_z_in_world_frame: Optional[float] = None
def __str__(self):
return f"ContactPoint: {self.link_a.object.name} - {self.link_b.object.name}"
@@ -797,37 +807,60 @@ class TextAnnotation:
size: float = 0.1
+@dataclass
+class VirtualJoint:
+ """
+ A virtual (not real) joint that is most likely used for simulation purposes.
+ """
+ name: str
+ type_: JointType
+ axes: Optional[Point] = None
+
+ @property
+ def type(self):
+ return self.type_
+
+ @property
+ def is_virtual(self):
+ return True
+
+ def __hash__(self):
+ return hash(self.name)
+
+
@dataclass
class VirtualMobileBaseJoints:
"""
Dataclass for storing the names, types and axes of the virtual mobile base joints of a mobile robot.
"""
- translation_x: Optional[str] = VirtualMobileBaseJointName.LINEAR_X.value
- translation_y: Optional[str] = VirtualMobileBaseJointName.LINEAR_Y.value
- angular_z: Optional[str] = VirtualMobileBaseJointName.ANGULAR_Z.value
+ translation_x: Optional[VirtualJoint] = VirtualJoint(VirtualMobileBaseJointName.LINEAR_X.value,
+ JointType.PRISMATIC,
+ Point(1, 0, 0))
+ translation_y: Optional[VirtualJoint] = VirtualJoint(VirtualMobileBaseJointName.LINEAR_Y.value,
+ JointType.PRISMATIC,
+ Point(0, 1, 0))
+ angular_z: Optional[VirtualJoint] = VirtualJoint(VirtualMobileBaseJointName.ANGULAR_Z.value,
+ JointType.REVOLUTE,
+ Point(0, 0, 1))
@property
def names(self) -> List[str]:
"""
Return the names of the virtual mobile base joints.
"""
- return [self.translation_x, self.translation_y, self.angular_z]
+ return [getattr(self, field.name).name for field in fields(self)]
def get_types(self) -> Dict[str, JointType]:
"""
Return the joint types of the virtual mobile base joints.
"""
- return {self.translation_x: JointType.PRISMATIC,
- self.translation_y: JointType.PRISMATIC,
- self.angular_z: JointType.REVOLUTE}
+ return {getattr(self, field.name).name: getattr(self, field.name).type_ for field in fields(self)}
def get_axes(self) -> Dict[str, Point]:
"""
Return the axes (i.e. The axis on which the joint moves) of the virtual mobile base joints.
"""
- return {self.translation_x: Point(1, 0, 0),
- self.translation_y: Point(0, 1, 0),
- self.angular_z: Point(0, 0, 1)}
+ return {getattr(self, field.name).name: getattr(self, field.name).axes for field in fields(self)}
@dataclass
@@ -862,11 +895,12 @@ def intersected(self) -> bool:
@dataclass
class MultiverseContactPoint:
"""
- A dataclass to store the contact point returned from Multiverse.
+ A dataclass to store all the contact data returned from Multiverse for a single object.
"""
- body_name: str
- contact_force: List[float]
- contact_torque: List[float]
+ body_1: str
+ body_2: str
+ position: List[float]
+ normal: List[float]
@dataclass
diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py
index 04d81b105..aced79bc1 100644
--- a/src/pycram/datastructures/enums.py
+++ b/src/pycram/datastructures/enums.py
@@ -65,9 +65,11 @@ class ObjectType(int, Enum):
BREAKFAST_CEREAL = auto()
JEROEN_CUP = auto()
ROBOT = auto()
+ GRIPPER = auto()
ENVIRONMENT = auto()
GENERIC_OBJECT = auto()
HUMAN = auto()
+ IMAGINED_SURFACE = auto()
class State(int, Enum):
@@ -242,8 +244,11 @@ class MultiverseAPIName(Enum):
"""
Enum for the different APIs of the Multiverse.
"""
+ GET_CONTACT_POINTS = "get_contact_points"
GET_CONTACT_BODIES = "get_contact_bodies"
+ GET_CONTACT_BODIES_AND_POINTS = "get_contact_bodies_and_points"
GET_CONSTRAINT_EFFORT = "get_constraint_effort"
+ GET_BOUNDING_BOX = "get_bounding_box"
ATTACH = "attach"
DETACH = "detach"
GET_RAYS = "get_rays"
diff --git a/src/pycram/datastructures/pose.py b/src/pycram/datastructures/pose.py
index b5c634fc7..692eeae3a 100644
--- a/src/pycram/datastructures/pose.py
+++ b/src/pycram/datastructures/pose.py
@@ -311,7 +311,7 @@ def insert(self, session: sqlalchemy.orm.Session) -> ORMPose:
return pose
- def multiply_quaternions(self, quaternion: List) -> None:
+ def multiply_quaternion(self, quaternion: List) -> None:
"""
Multiply the quaternion of this Pose with the given quaternion, the result will be the new orientation of this
Pose.
diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py
index 51b7034af..ed5978cd0 100644
--- a/src/pycram/datastructures/world.py
+++ b/src/pycram/datastructures/world.py
@@ -22,7 +22,7 @@
CapsuleVisualShape, PlaneVisualShape, MeshVisualShape,
ObjectState, WorldState, ClosestPointsList,
ContactPointsList, VirtualMobileBaseJoints, RotatedBoundingBox)
-from ..datastructures.enums import JointType, ObjectType, WorldMode, Arms
+from ..datastructures.enums import JointType, WorldMode, Arms
from ..datastructures.pose import Pose, Transform
from ..datastructures.world_entity import StateEntity
from ..failures import ProspectionObjectNotFound, WorldObjectNotFound
@@ -80,7 +80,8 @@ class World(StateEntity, ABC):
The ontology of this world.
"""
- def __init__(self, mode: WorldMode, is_prospection_world: bool = False, clear_cache: bool = False):
+ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: bool = False,
+ clear_cache: bool = False):
"""
Create a new simulation, the mode decides if the simulation should be a rendered window or just run in the
background. There can only be one rendered simulation.
@@ -301,8 +302,7 @@ def _init_prospection_world(self):
if self.is_prospection_world: # then no need to add another prospection world
self.prospection_world = None
else:
- self.prospection_world: World = self.__class__(WorldMode.DIRECT,
- True)
+ self.prospection_world: World = self.__class__(is_prospection_world=True)
def _sync_prospection_world(self):
"""
@@ -313,7 +313,6 @@ def _sync_prospection_world(self):
self.world_sync = None
else:
self.world_sync: WorldSync = WorldSync(self, self.prospection_world)
- self.pause_world_sync()
self.world_sync.start()
def preprocess_object_file_and_get_its_cache_path(self, path: str, ignore_cached_files: bool,
@@ -383,7 +382,7 @@ def get_object_by_name(self, name: str) -> Optional[Object]:
matching_objects = list(filter(lambda obj: obj.name == name, self.objects))
return matching_objects[0] if len(matching_objects) > 0 else None
- def get_object_by_type(self, obj_type: ObjectType) -> List[Object]:
+ def get_object_by_type(self, obj_type: Type[PhysicalObject]) -> List[Object]:
"""
Return a list of all Objects which have the type 'obj_type'.
@@ -459,7 +458,7 @@ def remove_object(self, obj: Object) -> None:
self.objects.remove(obj)
self.remove_object_from_original_state(obj)
- if World.robot == obj:
+ if World.robot == obj and not self.is_prospection_world:
World.robot = None
self.object_lock.release()
@@ -523,8 +522,16 @@ def remove_constraint(self, constraint_id) -> None:
"""
pass
- @abstractmethod
def get_joint_position(self, joint: Joint) -> float:
+ """
+ Wrapper for :meth:`_get_joint_position` that return 0.0 for a joint if it is in the ignore joints list.
+ """
+ if joint.object.is_a_robot and joint.name in self.robot_description.ignore_joints:
+ return 0.0
+ return self._get_joint_position(joint)
+
+ @abstractmethod
+ def _get_joint_position(self, joint: Joint) -> float:
"""
Get the position of a joint of an articulated object
@@ -613,7 +620,8 @@ def get_object_link_names(self, obj: Object) -> List[str]:
"""
pass
- def simulate(self, seconds: float, real_time: Optional[bool] = False) -> None:
+ def simulate(self, seconds: float, real_time: Optional[bool] = False,
+ func: Optional[Callable[[], None]] = None) -> None:
"""
Simulate Physics in the World for a given amount of seconds. Usually this simulation is faster than real
time. By setting the 'real_time' parameter this simulation is slowed down such that the simulated time is equal
@@ -621,11 +629,12 @@ def simulate(self, seconds: float, real_time: Optional[bool] = False) -> None:
:param seconds: The amount of seconds that should be simulated.
:param real_time: If the simulation should happen in real time or faster.
+ :param func: A function that should be called during the simulation
"""
self.set_realtime(real_time)
for i in range(0, int(seconds * self.conf.simulation_frequency)):
curr_time = Time().now()
- self.step()
+ self.step(func)
for objects, callbacks in self.coll_callbacks.items():
contact_points = self.get_contact_points_between_two_objects(objects[0], objects[1])
if len(contact_points) > 0:
@@ -774,9 +783,18 @@ def get_closest_points_between_objects(self, object_a: Object, object_b: Object,
"""
raise NotImplementedError
+ def reset_joint_position(self, joint: Joint, joint_position: float) -> bool:
+ """
+ Wrapper around :meth:`_reset_joint_position` that checks if the joint should be ignored.
+ """
+ if joint.object.is_a_robot and self.robot_description.ignore_joints:
+ if joint.name in self.robot_description.ignore_joints:
+ return True
+ return self._reset_joint_position(joint, joint_position)
+
@validate_joint_position
@abstractmethod
- def reset_joint_position(self, joint: Joint, joint_position: float) -> bool:
+ def _reset_joint_position(self, joint: Joint, joint_position: float) -> bool:
"""
Reset the joint position instantly without physics simulation
@@ -790,9 +808,19 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> bool:
"""
pass
+ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool:
+ """
+ Wrapper around :meth:`_set_multiple_joint_positions` that checks if any of the joints should be ignored.
+ """
+ filtered_joint_positions = copy(joint_positions)
+ for joint, position in joint_positions.items():
+ if joint.name in self.robot_description.ignore_joints:
+ filtered_joint_positions.pop(joint)
+ return self._set_multiple_joint_positions(filtered_joint_positions)
+
@validate_multiple_joint_positions
@abstractmethod
- def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool:
+ def _set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool:
"""
Set the positions of multiple joints of an articulated object.
@@ -805,8 +833,18 @@ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> b
"""
pass
- @abstractmethod
def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]:
+ """
+ Wrapper around :meth:`_get_multiple_joint_positions` that checks if any of the joints should be ignored.
+ """
+ filtered_joints = [joint for joint in joints if not joint.object.is_a_robot or
+ joint.name not in self.robot_description.ignore_joints]
+ joint_positions = self._get_multiple_joint_positions(filtered_joints)
+ joint_positions.update({joint.name: 0.0 for joint in joints if joint not in filtered_joints})
+ return joint_positions
+
+ @abstractmethod
+ def _get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]:
"""
Get the positions of multiple joints of an articulated object.
@@ -844,9 +882,13 @@ def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> bool
pass
@abstractmethod
- def step(self):
+ def step(self, func: Optional[Callable[[], None]] = None, step_seconds: Optional[float] = None) -> None:
"""
- Step the world simulation using forward dynamics
+ Step the world simulation using forward dynamics.
+
+ :param func: An optional function to be called during the step.
+ :param step_seconds: The amount of seconds to step the simulation if None the simulation is stepped by the
+ simulation time step.
"""
pass
@@ -886,14 +928,13 @@ def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]:
"""
pass
- @abstractmethod
def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox:
"""
:param obj: The object for which the bounding box should be returned.
:return: the axis aligned bounding box of this object. The return of this method are two points in
world coordinate frame which define a bounding box.
"""
- pass
+ raise NotImplementedError
def get_object_rotated_bounding_box(self, obj: Object) -> RotatedBoundingBox:
"""
@@ -903,14 +944,13 @@ def get_object_rotated_bounding_box(self, obj: Object) -> RotatedBoundingBox:
"""
raise NotImplementedError
- @abstractmethod
def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox:
"""
:param link: The link for which the bounding box should be returned.
:return: The axis aligned bounding box of the link. The return of this method are two points in
world coordinate frame which define a bounding box.
"""
- pass
+ raise NotImplementedError
def get_link_rotated_bounding_box(self, link: Link) -> RotatedBoundingBox:
"""
@@ -1024,16 +1064,16 @@ def terminate_world_sync(self) -> None:
Terminate the world sync thread.
"""
self.world_sync.terminate = True
- self.resume_world_sync()
self.world_sync.join()
- def save_state(self, state_id: Optional[int] = None, use_same_id: bool = False) -> int:
+ def save_state(self, state_id: Optional[int] = None, use_same_id: bool = False, to_file: bool = False) -> int:
"""
Return the id of the saved state of the World. The saved state contains the states of all the objects and
the state of the physics simulator.
:param state_id: The id of the saved state.
:param use_same_id: Whether to use the same current state id for the new saved state.
+ :param to_file: Whether to save the state to a file.
:return: A unique id of the state
"""
@@ -1050,7 +1090,7 @@ def save_state(self, state_id: Optional[int] = None, use_same_id: bool = False)
self._current_state = WorldState(self.object_states, sim_state_id)
- return super().save_state(state_id)
+ return super().save_state(state_id, self.get_cache_dir() if to_file else None)
@property
def current_state(self) -> WorldState:
@@ -1081,6 +1121,8 @@ def set_object_states_without_poses(self, states: Dict[str, ObjectState]) -> Non
obj.set_attachments(obj_state.attachments)
obj.link_states = obj_state.link_states
obj.joint_states = obj_state.joint_states
+ if obj.name == self.robot.name and len(self.robot_virtual_joints) > 0:
+ obj.set_mobile_robot_pose(obj_state.pose)
@property
def object_states(self) -> Dict[str, ObjectState]:
@@ -1264,8 +1306,9 @@ def remove_saved_states(self) -> None:
Remove all saved states of the World.
"""
if self.conf.use_physics_simulator_state:
- for state in self.saved_states.values():
- self.remove_physics_simulator_state(state.simulator_state_id)
+ simulator_state_ids = set([state.simulator_state_id for state in self.saved_states.values()])
+ for ssid in simulator_state_ids:
+ self.remove_physics_simulator_state(ssid)
else:
self.remove_objects_saved_states()
super().remove_saved_states()
@@ -1561,7 +1604,8 @@ def resume_world_sync(self) -> None:
"""
Resume the world synchronization.
"""
- self.world_sync.sync_lock.release()
+ if self.world_sync.sync_lock.locked():
+ self.world_sync.sync_lock.release()
def add_vis_axis(self, pose: Pose) -> int:
"""
@@ -1614,6 +1658,13 @@ def _simulator_object_remover(self, remover_func: Callable, *args, **kwargs) ->
remover_func(*args, **kwargs)
self.update_simulator_state_id_in_original_state()
+ def update_original_state(self) -> int:
+ """
+ Update the original state of the world.
+ :return: The id of the updated original state.
+ """
+ return self.save_state(self.original_state_id, use_same_id=True)
+
def update_simulator_state_id_in_original_state(self, use_same_id: bool = False) -> None:
"""
Update the simulator state id in the original state if use_physics_simulator_state is True in the configuration.
@@ -1705,10 +1756,8 @@ def run(self):
"""
while not self.terminate:
self.sync_lock.acquire()
- if not self.terminate:
- self.sync_worlds()
- self.sync_lock.release()
time.sleep(WorldSync.WAIT_TIME_AS_N_SIMULATION_STEPS * self.world.simulation_time_step)
+ self.sync_lock.release()
def get_world_object(self, prospection_object: Object) -> Object:
"""
@@ -1790,7 +1839,6 @@ def sync_objects_states(self) -> None:
self.world.prospection_world.reset_multiple_objects_base_poses(obj_pose_dict)
for obj, prospection_obj in self.object_to_prospection_object_map.items():
prospection_obj.set_attachments(obj.attachments)
- prospection_obj.link_states = obj.link_states
prospection_obj.joint_states = obj.joint_states
def check_for_equal(self) -> bool:
diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py
index 1e7c61e06..41c317fad 100644
--- a/src/pycram/datastructures/world_entity.py
+++ b/src/pycram/datastructures/world_entity.py
@@ -1,6 +1,8 @@
+import os
+import pickle
from abc import ABC, abstractmethod
-from typing_extensions import TYPE_CHECKING, Dict
+from typing_extensions import TYPE_CHECKING, Dict, Optional
from .dataclasses import State
@@ -14,6 +16,8 @@ class StateEntity:
restore the state of the World.
"""
+ SAVED_FILE_PREFIX = 'pycram_saved_state_'
+
def __init__(self):
self._saved_states: Dict[int, State] = {}
@@ -24,15 +28,49 @@ def saved_states(self) -> Dict[int, State]:
"""
return self._saved_states
- def save_state(self, state_id: int) -> int:
+ def save_state(self, state_id: int, save_dir: Optional[str] = None) -> int:
"""
Saves the state of this entity with the given state id.
:param state_id: The unique id of the state.
+ :param save_dir: The directory where the file should be saved.
"""
self._saved_states[state_id] = self.current_state
+ if save_dir is not None:
+ self.save_state_to_file(self.get_saved_file_path_of_state(save_dir, state_id), state_id)
return state_id
+ def save_state_to_file(self, file_path: str, state_id: Optional[int] = None):
+ """
+ Saves the state of this entity to a file.
+
+ :param file_path: The path to the file.
+ :param state_id: The unique id of the state if saving a specific state not the current state.
+ """
+ state_to_save = self.current_state if state_id is None else self.saved_states[state_id]
+ with open(file_path, 'wb') as file:
+ pickle.dump(state_to_save, file)
+
+ def restore_state_from_file(self, save_dir: str, state_id: int) -> None:
+ """
+ Restores the state of this entity from a file.
+
+ :param save_dir: The directory where the file is saved.
+ :param state_id: The unique id of the state.
+ """
+ with open(self.get_saved_file_path_of_state(save_dir, state_id), 'rb') as file:
+ self.current_state = pickle.load(file)
+
+ def get_saved_file_path_of_state(self, save_dir: str, state_id: int) -> str:
+ """
+ Gets the path of the file that stores the state with the given id.
+
+ :param save_dir: The directory where the file should be saved.
+ :param state_id: The unique id of the state.
+ :return: The name of the file that stores the state with the given id.
+ """
+ return os.path.join(save_dir, f'{self.SAVED_FILE_PREFIX}{state_id}.pkl')
+
@property
@abstractmethod
def current_state(self) -> State:
@@ -51,13 +89,17 @@ def current_state(self, state: State) -> None:
"""
pass
- def restore_state(self, state_id: int) -> None:
+ def restore_state(self, state_id: int, saved_file_dir: Optional[str] = None) -> None:
"""
Restores the state of this entity from a saved state using the given state id.
:param state_id: The unique id of the state.
+ :param saved_file_dir: The directory where the file is saved.
"""
- self.current_state = self.saved_states[state_id]
+ if saved_file_dir is not None:
+ self.restore_state_from_file(saved_file_dir, state_id)
+ else:
+ self.current_state = self.saved_states[state_id]
def remove_saved_states(self) -> None:
"""
diff --git a/src/pycram/description.py b/src/pycram/description.py
index 0a41fe434..10257a9ab 100644
--- a/src/pycram/description.py
+++ b/src/pycram/description.py
@@ -51,8 +51,9 @@ class LinkDescription(EntityDescription):
A link description of an object.
"""
- def __init__(self, parsed_link_description: Any):
+ def __init__(self, parsed_link_description: Any, mesh_dir: Optional[str] = None):
self.parsed_description = parsed_link_description
+ self.mesh_dir = mesh_dir
@property
@abstractmethod
@@ -216,7 +217,7 @@ class Link(ObjectEntity, LinkDescription, ABC):
def __init__(self, _id: int, link_description: LinkDescription, obj: Object):
ObjectEntity.__init__(self, _id, obj)
- LinkDescription.__init__(self, link_description.parsed_description)
+ LinkDescription.__init__(self, link_description.parsed_description, link_description.mesh_dir)
self.local_transformer: LocalTransformer = LocalTransformer()
self.constraint_ids: Dict[Link, int] = {}
self._current_pose: Optional[Pose] = None
@@ -287,8 +288,7 @@ def get_mesh_path(self, geometry: MeshVisualShape) -> str:
:param geometry: The geometry for which the mesh path should be returned.
:return: The path of the mesh file of this link if the geometry is a mesh.
"""
- mesh_filename = self.get_mesh_filename(geometry)
- return self.world.cache_manager.look_for_file_in_data_dir(pathlib.Path(mesh_filename))
+ return self.get_mesh_filename(geometry)
def get_mesh_filename(self, geometry: MeshVisualShape) -> str:
"""
@@ -350,8 +350,21 @@ def current_state(self) -> LinkState:
@current_state.setter
def current_state(self, link_state: LinkState) -> None:
if self.current_state != link_state:
+ if not self.all_constraint_links_belong_to_same_world(link_state):
+ raise ValueError("All constraint links must belong to the same world, since the constraint ids"
+ "are unique to the world and cannot be transferred between worlds.")
self.constraint_ids = link_state.constraint_ids
+ def all_constraint_links_belong_to_same_world(self, other: LinkState) -> bool:
+ """
+ Check if all links belong to the same world as the links in the other link state.
+
+ :param other: The state of the other link.
+ :return: True if all links belong to the same world, False otherwise.
+ """
+ return all([link.world == other_link.world for link, other_link in zip(self.constraint_ids.keys(),
+ other.constraint_ids.keys())])
+
def add_fixed_constraint_with_link(self, child_link: Self,
child_to_parent_transform: Optional[Transform] = None) -> int:
"""
diff --git a/src/pycram/designator.py b/src/pycram/designator.py
index 6ccb0fead..fac5a6a7c 100644
--- a/src/pycram/designator.py
+++ b/src/pycram/designator.py
@@ -178,8 +178,10 @@ class Action:
def __post_init__(self):
self.robot_position = World.robot.get_pose()
- self.robot_torso_height = World.robot.get_joint_position(
- RobotDescription.current_robot_description.torso_joint)
+ if RobotDescription.current_robot_description.torso_joint != "":
+ self.robot_torso_height = World.robot.get_joint_position(RobotDescription.current_robot_description.torso_joint)
+ else:
+ self.robot_torso_height = 0.0
self.robot_type = World.robot.obj_type
def perform(self) -> Any:
diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py
index 6e7b71feb..8f868171e 100644
--- a/src/pycram/designators/action_designator.py
+++ b/src/pycram/designators/action_designator.py
@@ -8,7 +8,7 @@
import numpy as np
from sqlalchemy.orm import Session
from tf import transformations
-from typing_extensions import List, Union, Callable, Optional, Type
+from typing_extensions import List, Union, Optional, Type
from pycrap import PhysicalObject, Location
from .location_designator import CostmapLocation
@@ -18,11 +18,14 @@
from ..datastructures.partial_designator import PartialDesignator
from ..datastructures.property import GraspableProperty, ReachableProperty, GripperIsFreeProperty, SpaceIsFreeProperty, \
VisibleProperty
+from ..failure_handling import try_action
from ..knowledge.knowledge_engine import ReasoningInstance
from ..local_transformer import LocalTransformer
-from ..failures import ObjectUnfetchable, ReachabilityFailure
+from ..failures import ObjectUnfetchable, ReachabilityFailure, NavigationGoalNotReachedError, PerceptionObjectNotFound, \
+ ObjectNotGraspedError
from ..robot_description import RobotDescription
from ..tasktree import with_tree
+from ..world_reasoning import contact
from owlready2 import Thing
@@ -44,7 +47,6 @@
from ..orm.action_designator import Action as ORMAction
from dataclasses import dataclass, field
-
# ----------------------------------------------------------------------------
# ---------------- Performables ----------------------------------------------
# ----------------------------------------------------------------------------
@@ -285,7 +287,7 @@ def plan(self) -> None:
adjusted_oTm = object.local_transformer.transform_pose(adjusted_pose, "map")
# multiplying the orientation therefore "rotating" it, to get the correct orientation of the gripper
- adjusted_oTm.multiply_quaternions(grasp)
+ adjusted_oTm.multiply_quaternion(grasp)
# prepose depending on the gripper (its annoying we have to put pr2_1 here tbh
arm_chain = RobotDescription.current_robot_description.get_arm_chain(self.arm)
@@ -293,7 +295,6 @@ def plan(self) -> None:
oTg = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame)
oTg.pose.position.x -= self.prepose_distance # in x since this is how the gripper is oriented
- oTg.pose.position.y -= 0.01
prepose = object.local_transformer.transform_pose(oTg, "map")
# Perform the motion with the prepose and open gripper
@@ -303,10 +304,22 @@ def plan(self) -> None:
# 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")
World.current_world.add_vis_axis(adjusted_oTm)
- MoveTCPMotion(adjusted_oTm, self.arm, allow_gripper_collision=True).perform()
- adjusted_oTm.pose.position.z += 0.03
+ MoveTCPMotion(adjusted_oTm, self.arm, allow_gripper_collision=True,
+ movement_type=MovementType.STRAIGHT_CARTESIAN).perform()
+
MoveGripperMotion(motion=GripperState.CLOSE, gripper=self.arm).perform()
+
+ # Make sure the object is in contact with the gripper
+ in_contact, contact_links = contact(object, robot, return_links=True)
+ if not in_contact or not any([link.name in arm_chain.end_effector.links
+ for _, link in contact_links]):
+ # TODO: Would be better to check for contact with both fingers
+ # (maybe introduce left and right finger links in the end effector description)
+ raise ObjectNotGraspedError(object, self.arm)
+
tool_frame = RobotDescription.current_robot_description.get_arm_chain(self.arm).get_tool_frame()
robot.attach(object, tool_frame)
@@ -367,7 +380,7 @@ def plan(self) -> None:
World.robot.detach(self.object_designator.world_object)
retract_pose = local_tf.transform_pose(target_diff, World.robot.get_link_tf_frame(
RobotDescription.current_robot_description.get_arm_chain(self.arm).get_tool_frame()))
- retract_pose.position.x -= 0.07
+ retract_pose.position.x -= 0.03
MoveTCPMotion(retract_pose, self.arm).perform()
@@ -391,7 +404,9 @@ class NavigateActionPerformable(ActionAbstract):
@with_tree
def plan(self) -> None:
- MoveMotion(self.target_location, self.keep_joint_states).perform()
+ motion_action = MoveMotion(self.target_location, self.keep_joint_states)
+ return try_action(motion_action, failure_type=NavigationGoalNotReachedError)
+
@dataclass
@@ -423,7 +438,7 @@ def plan(self) -> None:
robot_desig_resolved = BelieveObject(names=[RobotDescription.current_robot_description.name]).resolve()
ParkArmsActionPerformable(Arms.BOTH).perform()
pickup_loc = CostmapLocation(target=self.object_designator, reachable_for=robot_desig_resolved,
- reachable_arm=self.arm)
+ reachable_arm=self.arm, prepose_distance=self.pickup_prepose_distance)
# Tries to find a pick-up position for the robot that uses the given arm
pickup_pose = None
for pose in pickup_loc:
@@ -494,8 +509,8 @@ class DetectActionPerformable(ActionAbstract):
@with_tree
def plan(self) -> None:
- return DetectingMotion(technique=self.technique,state=self.state, object_designator_description=self.object_designator_description,
- region=self.region).perform()
+ return try_action(DetectingMotion(technique=self.technique,state=self.state, object_designator_description=self.object_designator_description,
+ region=self.region), PerceptionObjectNotFound)
@dataclass
@@ -512,11 +527,15 @@ class OpenActionPerformable(ActionAbstract):
"""
Arm that should be used for opening the container
"""
+ grasping_prepose_distance: float
+ """
+ The distance in meters the gripper should be at in the x-axis away from the handle.
+ """
orm_class: Type[ActionAbstract] = field(init=False, default=ORMOpenAction)
@with_tree
def plan(self) -> None:
- GraspingActionPerformable(self.arm, self.object_designator).perform()
+ GraspingActionPerformable(self.arm, self.object_designator, self.grasping_prepose_distance).perform()
OpeningMotion(self.object_designator, self.arm).perform()
MoveGripperMotion(GripperState.OPEN, self.arm, allow_gripper_collision=True).perform()
@@ -536,11 +555,15 @@ class CloseActionPerformable(ActionAbstract):
"""
Arm that should be used for closing
"""
+ grasping_prepose_distance: float
+ """
+ The distance in meters between the gripper and the handle before approaching to grasp.
+ """
orm_class: Type[ActionAbstract] = field(init=False, default=ORMCloseAction)
@with_tree
def plan(self) -> None:
- GraspingActionPerformable(self.arm, self.object_designator).perform()
+ GraspingActionPerformable(self.arm, self.object_designator, self.grasping_prepose_distance).perform()
ClosingMotion(self.object_designator, self.arm).perform()
MoveGripperMotion(GripperState.OPEN, self.arm, allow_gripper_collision=True).perform()
@@ -559,6 +582,10 @@ class GraspingActionPerformable(ActionAbstract):
"""
Object Designator for the object that should be grasped
"""
+ prepose_distance: float
+ """
+ The distance in meters the gripper should be at before grasping the object
+ """
orm_class: Type[ActionAbstract] = field(init=False, default=ORMGraspingAction)
@with_tree
@@ -574,7 +601,7 @@ def plan(self) -> None:
World.robot.get_link_tf_frame(gripper_name))
pre_grasp = object_pose_in_gripper.copy()
- pre_grasp.pose.position.x -= 0.1
+ pre_grasp.pose.position.x -= self.prepose_distance
MoveTCPMotion(pre_grasp, self.arm).perform()
MoveGripperMotion(GripperState.OPEN, self.arm).perform()
@@ -1131,16 +1158,19 @@ class OpenAction(ActionDesignatorDescription):
performable_class = OpenActionPerformable
- def __init__(self, object_designator_description: ObjectPart, arms: List[Arms] = None):
+ def __init__(self, object_designator_description: ObjectPart, arms: List[Arms] = None,
+ grasping_prepose_distance: float = 0.03):
"""
Moves the arm of the robot to open a container.
:param object_designator_description: Object designator_description describing the handle that should be used to open
:param arms: A list of possible arms that should be used
+ :param grasping_prepose_distance: The distance in meters between gripper and handle before approaching to grasp.
"""
super().__init__()
self.object_designator_description: ObjectPart = object_designator_description
self.arms: List[Arms] = arms
+ self.grasping_prepose_distance: float = grasping_prepose_distance
self.knowledge_condition = GripperIsFreeProperty(self.arms)
def ground(self) -> OpenActionPerformable:
@@ -1150,7 +1180,8 @@ def ground(self) -> OpenActionPerformable:
:return: A performable designator_description
"""
- return OpenActionPerformable(self.object_designator_description.resolve(), self.arms[0])
+ return OpenActionPerformable(self.object_designator_description.resolve(), self.arms[0],
+ grasping_prepose_distance=self.grasping_prepose_distance)
def __iter__(self) -> OpenActionPerformable:
"""
@@ -1159,7 +1190,8 @@ def __iter__(self) -> OpenActionPerformable:
:return: A performable action designator_description
"""
ri = ReasoningInstance(self,
- PartialDesignator(OpenActionPerformable, self.object_designator_description, self.arms))
+ PartialDesignator(OpenActionPerformable, self.object_designator_description, self.arms,
+ self.grasping_prepose_distance))
for desig in ri:
yield desig
@@ -1173,16 +1205,20 @@ class CloseAction(ActionDesignatorDescription):
performable_class = CloseActionPerformable
- def __init__(self, object_designator_description: ObjectPart, arms: List[Arms] = None):
+ def __init__(self, object_designator_description: ObjectPart, arms: List[Arms] = None,
+ grasping_prepose_distance: float = 0.03):
"""
Attempts to close an open container
:param object_designator_description: Object designator_description description of the handle that should be used
:param arms: A list of possible arms to use
+ :param grasping_prepose_distance: The distance in meters between the gripper and the handle before approaching
+ to grasp.
"""
super().__init__()
self.object_designator_description: ObjectPart = object_designator_description
self.arms: List[Arms] = arms
+ self.grasping_prepose_distance: float = grasping_prepose_distance
self.knowledge_condition = GripperIsFreeProperty(self.arms)
def ground(self) -> CloseActionPerformable:
@@ -1192,7 +1228,8 @@ def ground(self) -> CloseActionPerformable:
:return: A performable designator_description
"""
- return CloseActionPerformable(self.object_designator_description.resolve(), self.arms[0])
+ return CloseActionPerformable(self.object_designator_description.resolve(), self.arms[0],
+ self.grasping_prepose_distance)
def __iter__(self) -> CloseActionPerformable:
"""
@@ -1201,7 +1238,8 @@ def __iter__(self) -> CloseActionPerformable:
:yield: A performable fully parametrized Action designator
"""
ri = ReasoningInstance(self,
- PartialDesignator(CloseActionPerformable, self.object_designator_description, self.arms))
+ PartialDesignator(CloseActionPerformable, self.object_designator_description, self.arms,
+ self.grasping_prepose_distance))
for desig in ri:
yield desig
@@ -1213,17 +1251,20 @@ class GraspingAction(ActionDesignatorDescription):
performable_class = GraspingActionPerformable
- def __init__(self, object_description: Union[ObjectDesignatorDescription, ObjectPart], arms: List[Arms] = None):
+ def __init__(self, object_description: Union[ObjectDesignatorDescription, ObjectPart], arms: List[Arms] = None,
+ prepose_distance: float = 0.03):
"""
Will try to grasp the object described by the given description. Grasping is done by moving into a pre grasp
position 10 cm before the object, opening the gripper, moving to the object and then closing the gripper.
:param arms: List of Arms that should be used for grasping
:param object_description: Description of the object that should be grasped
+ :param prepose_distance: The distance in meters between the gripper and the object before approaching to grasp.
"""
super().__init__()
self.arms: List[Arms] = arms
self.object_description: ObjectDesignatorDescription = object_description
+ self.prepose_distance: float = prepose_distance
def ground(self) -> GraspingActionPerformable:
"""
@@ -1232,7 +1273,7 @@ def ground(self) -> GraspingActionPerformable:
:return: A performable action designator_description that contains specific arguments
"""
- return GraspingActionPerformable(self.arms[0], self.object_description.resolve())
+ return GraspingActionPerformable(self.arms[0], self.object_description.resolve(), self.prepose_distance)
def __iter__(self) -> CloseActionPerformable:
"""
@@ -1242,6 +1283,7 @@ def __iter__(self) -> CloseActionPerformable:
:yield: A fully parametrized Action designator
"""
ri = ReasoningInstance(self,
- PartialDesignator(GraspingActionPerformable, self.object_description, self.arms))
+ PartialDesignator(GraspingActionPerformable, self.object_description, self.arms,
+ self.prepose_distance))
for desig in ri:
yield desig
diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py
index 395a46c2f..bbfc108c3 100644
--- a/src/pycram/designators/location_designator.py
+++ b/src/pycram/designators/location_designator.py
@@ -1,18 +1,20 @@
import dataclasses
-import time
+import numpy as np
from typing_extensions import List, Union, Iterable, Optional, Callable
from .object_designator import ObjectDesignatorDescription, ObjectPart
+from ..costmaps import OccupancyCostmap, VisibilityCostmap, SemanticCostmap, GaussianCostmap, Costmap
+from ..datastructures.enums import JointType, Arms, Grasp
+from ..datastructures.pose import Pose
from ..datastructures.world import World, UseProspectionWorld
-from ..local_transformer import LocalTransformer
-from ..world_reasoning import link_pose_for_joint_config
from ..designator import DesignatorError, LocationDesignatorDescription
-from ..costmaps import OccupancyCostmap, VisibilityCostmap, SemanticCostmap, GaussianCostmap
-from ..datastructures.enums import JointType, Arms
+from ..local_transformer import LocalTransformer
from ..pose_generator_and_validator import PoseGenerator, visibility_validator, reachability_validator
from ..robot_description import RobotDescription
-from ..datastructures.pose import Pose
+from ..ros.logging import logdebug
+from ..world_concepts.world_object import Object, Link
+from ..world_reasoning import link_pose_for_joint_config, contact, is_held_object, prospect_robot_contact
class Location(LocationDesignatorDescription):
@@ -106,13 +108,21 @@ class CostmapLocation(LocationDesignatorDescription):
class Location(LocationDesignatorDescription.Location):
reachable_arms: List[Arms]
"""
- List of arms with which the pose can be reached, is only used when the 'rechable_for' parameter is used
+ List of arms with which the pose can be reached, is only used when the 'reachable_for' parameter is used
+ """
+ tried_grasps: List[Grasp]
+ """
+ List of grasps that were tried to reach the pose
"""
def __init__(self, target: Union[Pose, ObjectDesignatorDescription.Object],
reachable_for: Optional[ObjectDesignatorDescription.Object] = None,
visible_for: Optional[ObjectDesignatorDescription.Object] = None,
- reachable_arm: Optional[Arms] = None):
+ reachable_arm: Optional[Arms] = None,
+ prepose_distance: float = 0.03,
+ check_collision_at_start: bool = True,
+ ignore_collision_with: Optional[List[Object]] = None,
+ grasps: Optional[List[Grasp]] = None):
"""
Location designator that uses costmaps as base to calculate locations for complex constrains like reachable or
visible. In case of reachable the resolved location contains a list of arms with which the location is reachable.
@@ -121,12 +131,20 @@ def __init__(self, target: Union[Pose, ObjectDesignatorDescription.Object],
:param reachable_for: Object for which the reachability should be calculated, usually a robot
:param visible_for: Object for which the visibility should be calculated, usually a robot
:param reachable_arm: An optional arm with which the target should be reached
+ :param prepose_distance: Distance to the target pose where the robot should be checked for reachability.
+ :param check_collision_at_start: If True, the designator will check for collisions at the start pose.
+ :param ignore_collision_with: List of objects that should be ignored for collision checking.
+ :param grasps: List of grasps that should be tried to reach the target pose
"""
super().__init__()
self.target: Union[Pose, ObjectDesignatorDescription.Object] = target
self.reachable_for: ObjectDesignatorDescription.Object = reachable_for
self.visible_for: ObjectDesignatorDescription.Object = visible_for
self.reachable_arm: Optional[Arms] = reachable_arm
+ self.prepose_distance = prepose_distance
+ self.check_collision_at_start = check_collision_at_start
+ self.ignore_collision_with = ignore_collision_with if ignore_collision_with is not None else []
+ self.grasps: List[Optional[Grasp]] = grasps if grasps is not None else [None]
def ground(self) -> Location:
"""
@@ -154,8 +172,10 @@ def __iter__(self):
# This ensures that the costmaps always get a position as their origin.
if isinstance(self.target, ObjectDesignatorDescription.Object):
target_pose = self.target.world_object.get_pose()
+ target_object = self.target.world_object
else:
target_pose = self.target.copy()
+ target_object = None
# ground_pose = [[target_pose[0][0], target_pose[0][1], 0], target_pose[1]]
ground_pose = Pose(target_pose.position_as_list())
@@ -164,37 +184,63 @@ def __iter__(self):
occupancy = OccupancyCostmap(0.32, False, 200, 0.02, ground_pose)
final_map = occupancy
+ test_robot = None
+ if self.visible_for or self.reachable_for:
+ robot_object = self.visible_for.world_object if self.visible_for else self.reachable_for.world_object
+ test_robot = World.current_world.get_prospection_object_for_object(robot_object)
+
if self.reachable_for:
gaussian = GaussianCostmap(200, 15, 0.02, ground_pose)
final_map += gaussian
if self.visible_for:
- visible = VisibilityCostmap(min_height, max_height, 200, 0.02, Pose(target_pose.position_as_list()))
+ visible = VisibilityCostmap(min_height, max_height, 200, 0.02,
+ Pose(target_pose.position_as_list()), target_object=target_object,
+ robot=test_robot)
final_map += visible
- if self.visible_for or self.reachable_for:
- robot_object = self.visible_for.world_object if self.visible_for else self.reachable_for.world_object
- test_robot = World.current_world.get_prospection_object_for_object(robot_object)
-
+ self.ignore_collision_with = [World.current_world.get_prospection_object_for_object(o) for o in
+ self.ignore_collision_with]
with UseProspectionWorld():
for maybe_pose in PoseGenerator(final_map, number_of_samples=600):
+ if self.check_collision_at_start and (test_robot is not None):
+ if prospect_robot_contact(test_robot, maybe_pose, self.ignore_collision_with):
+ continue
res = True
arms = None
+ found_grasps = []
if self.visible_for:
- visible_prospection_object = World.current_world.get_prospection_object_for_object(self.target.world_object)
+ visible_prospection_object = World.current_world.get_prospection_object_for_object(
+ self.target.world_object)
res = res and visibility_validator(maybe_pose, test_robot, visible_prospection_object,
World.current_world)
if self.reachable_for:
hand_links = []
- for description in RobotDescription.current_robot_description.get_manipulator_chains():
- hand_links += description.end_effector.links
- valid, arms = reachability_validator(maybe_pose, test_robot, target_pose,
- allowed_collision={test_robot: hand_links})
- if self.reachable_arm:
- res = res and valid and self.reachable_arm in arms
+ if self.reachable_arm is not None:
+ hand_links = RobotDescription.current_robot_description.get_arm_chain(
+ self.reachable_arm).end_effector.links
else:
- res = res and valid
+ for description in RobotDescription.current_robot_description.get_manipulator_chains():
+ hand_links += description.end_effector.links
+ allowed_collision = {test_robot: hand_links}
+ allowed_collision.update({o: o.link_names for o in self.ignore_collision_with})
+ for grasp in self.grasps:
+ target_pose_oriented = target_pose.copy()
+ if grasp is not None:
+ grasp_quaternion = RobotDescription.current_robot_description.grasps[grasp]
+ target_pose_oriented.multiply_quaternion(grasp_quaternion)
+ valid, arms = reachability_validator(maybe_pose, test_robot, target_pose_oriented,
+ allowed_collision=allowed_collision,
+ arm=self.reachable_arm,
+ prepose_distance=self.prepose_distance)
+ if self.reachable_arm:
+ res = res and valid and self.reachable_arm in arms
+ else:
+ res = res and valid
+ if res:
+ found_grasps.append(grasp)
+ break
if res:
- yield self.Location(maybe_pose, arms)
+ yield self.Location(maybe_pose, arms, found_grasps)
class AccessingLocation(LocationDesignatorDescription):
@@ -209,17 +255,21 @@ class Location(LocationDesignatorDescription.Location):
List of arms that can be used to for accessing from this pose
"""
- def __init__(self, handle_desig: ObjectPart.Object, robot_desig: ObjectDesignatorDescription.Object):
+ def __init__(self, handle_desig: ObjectPart.Object,
+ robot_desig: ObjectDesignatorDescription.Object,
+ prepose_distance: float = 0.03):
"""
Describes a position from where a drawer can be opened. For now this position should be calculated before the
drawer will be opened. Calculating the pose while the drawer is open could lead to problems.
:param handle_desig: ObjectPart designator for handle of the drawer
:param robot_desig: Object designator for the robot which should open the drawer
+ :param prepose_distance: Distance to the target pose where the robot should be checked for reachability.
"""
super().__init__()
self.handle: ObjectPart.Object = handle_desig
self.robot: ObjectDesignatorDescription.Object = robot_desig.world_object
+ self.prepose_distance = prepose_distance
def ground(self) -> Location:
"""
@@ -229,6 +279,36 @@ def ground(self) -> Location:
"""
return next(iter(self))
+ @staticmethod
+ def adjust_map_for_drawer_opening(cost_map: Costmap, init_pose: Pose, goal_pose: Pose,
+ width: float = 0.2):
+ """
+ Adjust the cost map for opening a drawer. This is done by removing all locations between the initial and final
+ pose of the drawer/container.
+
+ :param cost_map: Costmap that should be adjusted.
+ :param init_pose: Pose of the drawer/container when it is fully closed.
+ :param goal_pose: Pose of the drawer/container when it is fully opened.
+ :param width: Width of the drawer/container.
+ """
+ motion_vector = [goal_pose.position.x - init_pose.position.x, goal_pose.position.y - init_pose.position.y,
+ goal_pose.position.z - init_pose.position.z]
+ # remove locations between the initial and final pose
+ motion_vector_length = np.linalg.norm(motion_vector)
+ unit_motion_vector = np.array(motion_vector) / motion_vector_length
+ orthogonal_vector = np.array([unit_motion_vector[1], -unit_motion_vector[0], 0])
+ orthogonal_vector /= np.linalg.norm(orthogonal_vector)
+ orthogonal_size = width
+ map_origin_idx = cost_map.map.shape[0] // 2, cost_map.map.shape[1] // 2
+ for i in range(int(motion_vector_length / cost_map.resolution)):
+ for j in range(int(orthogonal_size / cost_map.resolution)):
+ idx = (int(map_origin_idx[0] + i * unit_motion_vector[0] + j * orthogonal_vector[0]),
+ int(map_origin_idx[1] + i * unit_motion_vector[1] + j * orthogonal_vector[1]))
+ cost_map.map[idx] = 0
+ idx = (int(map_origin_idx[0] + i * unit_motion_vector[0] - j * orthogonal_vector[0]),
+ int(map_origin_idx[1] + i * unit_motion_vector[1] - j * orthogonal_vector[1]))
+ cost_map.map[idx] = 0
+
def __iter__(self) -> Location:
"""
Creates poses from which the robot can open the drawer specified by the ObjectPart designator describing the
@@ -237,16 +317,6 @@ def __iter__(self) -> Location:
:yield: A location designator containing the pose and the arms that can be used.
"""
- # ground_pose = [[self.handle.part_pose[0][0], self.handle.part_pose[0][1], 0], self.handle.part_pose[1]]
- ground_pose = Pose(self.handle.part_pose.position_as_list())
- ground_pose.position.z = 0
- occupancy = OccupancyCostmap(distance_to_obstacle=0.25, from_ros=False, size=200, resolution=0.02,
- origin=ground_pose)
- gaussian = GaussianCostmap(200, 15, 0.02, ground_pose)
-
- final_map = occupancy + gaussian
-
- test_robot = World.current_world.get_prospection_object_for_object(self.robot)
# Find a Joint of type prismatic which is above the handle in the URDF tree
container_joint = self.handle.world_object.find_joint_above_link(self.handle.name, JointType.PRISMATIC)
@@ -265,24 +335,46 @@ def __iter__(self) -> Location:
container_joint: self.handle.world_object.get_joint_limits(container_joint)[1] / 1.5},
self.handle.name)
+ ground_pose = Pose(self.handle.part_pose.position_as_list())
+ ground_pose.position.z = 0
+ occupancy = OccupancyCostmap(distance_to_obstacle=0.25, from_ros=False, size=200, resolution=0.02,
+ origin=ground_pose)
+ gaussian = GaussianCostmap(200, 15, 0.02, ground_pose)
+
+ final_map = occupancy + gaussian
+ joint_type = self.handle.world_object.joints[container_joint].type
+ if joint_type == JointType.PRISMATIC:
+ self.adjust_map_for_drawer_opening(final_map, init_pose, goal_pose)
+
+ test_robot = World.current_world.get_prospection_object_for_object(self.robot)
+
with UseProspectionWorld():
+ orientation_generator = lambda p, o: PoseGenerator.generate_orientation(p, half_pose)
for maybe_pose in PoseGenerator(final_map, number_of_samples=600,
- orientation_generator=lambda p, o: PoseGenerator.generate_orientation(p, half_pose)):
-
+ orientation_generator=orientation_generator):
+ if prospect_robot_contact(test_robot, maybe_pose):
+ logdebug("Robot is initially in collision, skipping that pose")
+ continue
+ logdebug("Robot is initially in a valid pose")
hand_links = []
for description in RobotDescription.current_robot_description.get_manipulator_chains():
- hand_links += description.links
+ hand_links += description.end_effector.links
valid_init, arms_init = reachability_validator(maybe_pose, test_robot, init_pose,
- allowed_collision={test_robot: hand_links})
+ allowed_collision={test_robot: hand_links},
+ prepose_distance=self.prepose_distance)
- valid_goal, arms_goal = reachability_validator(maybe_pose, test_robot, goal_pose,
- allowed_collision={test_robot: hand_links})
+ if valid_init:
+ logdebug(f"Found a valid init pose for accessing {self.handle.name} with arms {arms_init}")
+ valid_goal, arms_goal = reachability_validator(maybe_pose, test_robot, goal_pose,
+ allowed_collision={test_robot: hand_links},
+ prepose_distance=self.prepose_distance)
- arms_list = list(set(arms_init).intersection(set(arms_goal)))
+ arms_list = list(set(arms_init).intersection(set(arms_goal)))
- if valid_init and valid_goal and len(arms_list) > 0:
- yield self.Location(maybe_pose, arms_list)
+ if valid_goal and len(arms_list) > 0:
+ logdebug(f"Found a valid goal pose for accessing {self.handle.name} with arms {arms_list}")
+ yield self.Location(maybe_pose, arms_list)
class SemanticCostmapLocation(LocationDesignatorDescription):
@@ -294,20 +386,28 @@ class SemanticCostmapLocation(LocationDesignatorDescription):
class Location(LocationDesignatorDescription.Location):
pass
- def __init__(self, urdf_link_name, part_of, for_object=None):
+ def __init__(self, link_name, part_of, for_object=None, edges_only: bool = False,
+ horizontal_edges_only: bool = False, edge_size_in_meters: float = 0.06):
"""
- Creates a distribution over a urdf link to sample poses which are on this link. Can be used, for example, to find
+ Creates a distribution over a link to sample poses which are on this link. Can be used, for example, to find
poses that are on a table. Optionally an object can be given for which poses should be calculated, in that case
the poses are calculated such that the bottom of the object is on the link.
- :param urdf_link_name: Name of the urdf link for which a distribution should be calculated
+ :param link_name: Name of the link for which a distribution should be calculated
:param part_of: Object of which the urdf link is a part
:param for_object: Optional object that should be placed at the found location
+ :param edges_only: If True, only the edges of the link are considered
+ :param horizontal_edges_only: If True, only the horizontal edges of the link are considered
+ :param edge_size_in_meters: Size of the edges in meters.
"""
super().__init__()
- self.urdf_link_name: str = urdf_link_name
+ self.link_name: str = link_name
self.part_of: ObjectDesignatorDescription.Object = part_of
self.for_object: Optional[ObjectDesignatorDescription.Object] = for_object
+ self.edges_only: bool = edges_only
+ self.horizontal_edges_only: bool = horizontal_edges_only
+ self.edge_size_in_meters: float = edge_size_in_meters
+ self.sem_costmap: Optional[SemanticCostmap] = None
def ground(self) -> Location:
"""
@@ -325,11 +425,14 @@ def __iter__(self):
:yield: An instance of SemanticCostmapLocation.Location with the found valid position of the Costmap.
"""
- sem_costmap = SemanticCostmap(self.part_of.world_object, self.urdf_link_name)
+ self.sem_costmap = SemanticCostmap(self.part_of.world_object, self.link_name)
+ if self.edges_only or self.horizontal_edges_only:
+ self.sem_costmap = self.sem_costmap.get_edges_map(self.edge_size_in_meters,
+ horizontal_only=self.horizontal_edges_only)
height_offset = 0
if self.for_object:
min_p, max_p = self.for_object.world_object.get_axis_aligned_bounding_box().get_min_max_points()
height_offset = (max_p.z - min_p.z) / 2
- for maybe_pose in PoseGenerator(sem_costmap):
+ for maybe_pose in PoseGenerator(self.sem_costmap):
maybe_pose.position.z += height_offset
yield self.Location(maybe_pose)
diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py
index c9cee63f5..20071cddd 100644
--- a/src/pycram/designators/motion_designator.py
+++ b/src/pycram/designators/motion_designator.py
@@ -1,14 +1,12 @@
-from abc import ABC, abstractmethod
from dataclasses import dataclass
from sqlalchemy.orm import Session
from pycrap import PhysicalObject, Location
from .object_designator import ObjectDesignatorDescription, ObjectPart, RealObject
-from ..datastructures.enums import ObjectType, Arms, GripperState, ExecutionType, MovementType
-from ..designator import ResolutionError
-from ..orm.base import ProcessMetaData
-from ..failures import PerceptionObjectNotFound
+from ..datastructures.enums import MovementType
+from ..failure_handling import try_motion
+from ..failures import PerceptionObjectNotFound, ToolPoseNotReachedError
from ..process_module import ProcessModuleManager
from ..orm.motion_designator import (MoveMotion as ORMMoveMotion,
MoveTCPMotion as ORMMoveTCPMotion, LookingMotion as ORMLookingMotion,
@@ -17,10 +15,11 @@
Motion as ORMMotionDesignator)
from ..datastructures.enums import ObjectType, Arms, GripperState, ExecutionType, DetectionTechnique, DetectionState
-from typing_extensions import Dict, Optional, get_type_hints, Type, Any
+from typing_extensions import Dict, Optional, Type
from ..datastructures.pose import Pose
from ..tasktree import with_tree
from ..designator import BaseMotion
+from ..external_interfaces.robokudo import robokudo_found
@dataclass
@@ -83,7 +82,7 @@ class MoveTCPMotion(BaseMotion):
@with_tree
def perform(self):
pm_manager = ProcessModuleManager.get_manager()
- return pm_manager.move_tcp().execute(self)
+ try_motion(pm_manager.move_tcp(), self, ToolPoseNotReachedError)
def to_sql(self) -> ORMMoveTCPMotion:
return ORMMoveTCPMotion(self.arm, self.allow_gripper_collision)
diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py
index 483dc66f9..c594660d6 100644
--- a/src/pycram/external_interfaces/giskard.py
+++ b/src/pycram/external_interfaces/giskard.py
@@ -119,11 +119,13 @@ def removing_of_objects() -> None:
@init_giskard_interface
-def sync_worlds() -> None:
+def sync_worlds(projection: bool = False) -> None:
"""
Synchronizes the World and the Giskard belief state, this includes adding and removing objects to the Giskard
belief state such that it matches the objects present in the World and moving the robot to the position it is
currently at in the World.
+
+ :param projection: Whether the sync in projection mode or reality.
"""
add_gripper_groups()
world_object_names = set()
@@ -136,10 +138,11 @@ def sync_worlds() -> None:
obj.joints.values()))
joint_config_filtered = {joint.name: joint_config[joint.name] for joint in non_fixed_joints}
- giskard_wrapper.monitors.add_set_seed_configuration(joint_config_filtered,
- RobotDescription.current_robot_description.name)
- giskard_wrapper.monitors.add_set_seed_odometry(_pose_to_pose_stamped(obj.get_pose()),
- RobotDescription.current_robot_description.name)
+ if projection:
+ giskard_wrapper.monitors.add_set_seed_configuration(joint_config_filtered,
+ RobotDescription.current_robot_description.name)
+ giskard_wrapper.monitors.add_set_seed_odometry(_pose_to_pose_stamped(obj.get_pose()),
+ RobotDescription.current_robot_description.name)
giskard_object_names = set(giskard_wrapper.get_group_names())
robot_name = {RobotDescription.current_robot_description.name}
if not world_object_names.union(robot_name).issubset(giskard_object_names):
@@ -554,7 +557,7 @@ def projection_cartesian_goal(goal_pose: Pose, tip_link: str, root_link: str) ->
:param root_link: The starting link of the chain which should be used to achieve this goal
:return: MoveResult message for this goal
"""
- sync_worlds()
+ sync_worlds(projection=True)
giskard_wrapper.set_cart_goal(_pose_to_pose_stamped(goal_pose), tip_link, root_link)
return giskard_wrapper.projection()
@@ -573,7 +576,7 @@ def projection_cartesian_goal_with_approach(approach_pose: Pose, goal_pose: Pose
:param robot_base_link: The base link of the robot
:return: A trajectory calculated to move the tip_link to the goal_pose
"""
- sync_worlds()
+ sync_worlds(projection=True)
giskard_wrapper.allow_all_collisions()
giskard_wrapper.set_cart_goal(_pose_to_pose_stamped(approach_pose), robot_base_link, "map")
giskard_wrapper.projection()
@@ -592,7 +595,7 @@ def projection_joint_goal(goal_poses: Dict[str, float], allow_collisions: bool =
:param allow_collisions: If all collisions should be allowed for this goal
:return: MoveResult message for this goal
"""
- sync_worlds()
+ sync_worlds(projection=True)
if allow_collisions:
giskard_wrapper.allow_all_collisions()
giskard_wrapper.set_joint_goal(goal_poses)
diff --git a/src/pycram/failure_handling.py b/src/pycram/failure_handling.py
index 1c53061a4..3e3f0a965 100644
--- a/src/pycram/failure_handling.py
+++ b/src/pycram/failure_handling.py
@@ -1,9 +1,12 @@
+import logging
+
from .datastructures.enums import State
-from .designator import DesignatorDescription
+from .designator import DesignatorDescription, ActionDesignatorDescription, BaseMotion
from .failures import PlanFailure
from threading import Lock
-from typing_extensions import Union, Tuple, Any, List
+from typing_extensions import Union, Tuple, Any, List, Optional, Type, Callable
from .language import Language, Monitor
+from .process_module import ProcessModule
class FailureHandling(Language):
@@ -15,7 +18,7 @@ class FailureHandling(Language):
to be extended by subclasses that implement specific failure handling behaviors.
"""
- def __init__(self, designator_description: Union[DesignatorDescription, Monitor]):
+ def __init__(self, designator_description: Optional[Union[DesignatorDescription, Monitor]] = None):
"""
Initializes a new instance of the FailureHandling class.
@@ -169,3 +172,56 @@ def flatten(result):
if exception_type in self.recovery:
self.recovery[exception_type].perform()
return status, flatten(res)
+
+
+def try_action(action: Any, failure_type: Type[Exception], max_tries: int = 3):
+ """
+ A generic function to retry an action a certain number of times before giving up, with a specific failure type.
+
+ :param action: The action to be performed, it must have a perform() method.
+ :param failure_type: The type of exception to catch.
+ :param max_tries: The maximum number of attempts to retry the action. Defaults to 3.
+ """
+ return try_method(method=lambda: action.perform(),
+ failure_type=failure_type,
+ max_tries=max_tries,
+ name="action")
+
+
+def try_motion(motion: ProcessModule, motion_designator_instance: BaseMotion,
+ failure_type: Type[Exception], max_tries: int = 3):
+ """
+ A generic function to retry a motion a certain number of times before giving up, with a specific exception.
+
+ :param motion: The motion to be executed.
+ :param motion_designator_instance: The instance of the motion designator that has the description of the motion.
+ :param failure_type: The type of exception to catch.
+ :param max_tries: The maximum number of attempts to retry the motion.
+ """
+ return try_method(method=lambda: motion.execute(motion_designator_instance),
+ failure_type=failure_type,
+ max_tries=max_tries,
+ name="motion")
+
+
+def try_method(method: Callable, failure_type: Type[Exception], max_tries: int = 3, name: str = "method"):
+ """
+ A generic function to retry a method a certain number of times before giving up, with a specific exception.
+
+ :param method: The method to be called.
+ :param failure_type: The type of exception to catch.
+ :param max_tries: The maximum number
+ :param name: The name of the method to be called.
+ """
+ current_retry = 0
+ result = None
+ while current_retry < max_tries:
+ try:
+ result = method()
+ break
+ except failure_type as e:
+ logging.debug(f"Caught exception {e} during {name} execution {method}. Retrying...")
+ current_retry += 1
+ if current_retry == max_tries:
+ logging.error(f"Failed to execute {name} {method} after {max_tries} retries.")
+ return result
diff --git a/src/pycram/failures.py b/src/pycram/failures.py
index 8a621536b..7513bad81 100644
--- a/src/pycram/failures.py
+++ b/src/pycram/failures.py
@@ -1,10 +1,11 @@
+from __future__ import annotations
from pathlib import Path
from typing_extensions import TYPE_CHECKING, List
if TYPE_CHECKING:
from .world_concepts.world_object import Object
- from .datastructures.enums import JointType
+ from .datastructures.enums import JointType, MultiverseAPIName, Arms
class PlanFailure(Exception):
@@ -306,6 +307,11 @@ def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
+class ObjectNotGraspedError(Grasping):
+ def __init__(self, obj: Object, arm: Arms, *args, **kwargs):
+ super().__init__(f"object {obj.name} was not grasped by arm {arm}", *args, **kwargs)
+
+
class Looking(Task):
""""""
@@ -435,6 +441,22 @@ def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
+class NavigationGoalNotReachedError(PlanFailure):
+ def __init__(self, current_pose, goal_pose):
+ super().__init__(f"Navigation goal not reached. Current pose: {current_pose}, goal pose: {goal_pose}")
+
+
+class ToolPoseNotReachedError(PlanFailure):
+ def __init__(self, current_pose, goal_pose):
+ super().__init__(f"Tool pose not reached. Current pose: {current_pose}, goal pose: {goal_pose}")
+
+
+class FailedAPIResponse(Exception):
+ def __init__(self, api_response: List[str], api_name: MultiverseAPIName, *args, **kwargs):
+ super().__init__(f"{api_name} api request with arguments {args} and keyword arguments {kwargs}"
+ f" failed with response {api_response}")
+
+
"""
The following exceptions are used in the PyCRAM framework to handle errors related to the world and the objects in it.
They are usually related to a bug in the code or a misuse of the framework (e.g. logical errors in the code).
@@ -505,4 +527,3 @@ def __init__(self, link_name: str):
class LinkGeometryHasNoMesh(Exception):
def __init__(self, link_name: str, geometry_type: str):
super().__init__(f"Link {link_name} geometry with type {geometry_type} has no mesh.")
-
diff --git a/src/pycram/helper.py b/src/pycram/helper.py
index 08ae416f7..3b14578d3 100644
--- a/src/pycram/helper.py
+++ b/src/pycram/helper.py
@@ -4,7 +4,7 @@
Singleton -- implementation of singleton metaclass
"""
import os
-from typing_extensions import Dict, Optional
+from typing_extensions import Dict, Optional, List
import xml.etree.ElementTree as ET
from .ros.logging import logwarn
@@ -47,7 +47,14 @@ def parse_mjcf_actuators(file_path: str) -> Dict[str, str]:
return joint_actuators
-def get_robot_mjcf_path(robot_relative_dir: str, robot_name: str, xml_name: Optional[str] = None) -> Optional[str]:
+def get_robot_urdf_path_from_multiverse(relative_dir: str, robot_name: str, resources_dir: Optional[str] = None) -> str:
+ if resources_dir is None:
+ resources_dir = find_multiverse_resources_path()
+ return os.path.join(resources_dir, 'robots', relative_dir, robot_name, f'urdf/{robot_name}.urdf')
+
+
+def get_robot_mjcf_path(robot_relative_dir: str, robot_name: str, xml_name: Optional[str] = None,
+ multiverse_resources: Optional[str] = None) -> Optional[str]:
"""
Get the path to the MJCF file of a robot.
@@ -56,10 +63,10 @@ def get_robot_mjcf_path(robot_relative_dir: str, robot_name: str, xml_name: Opti
:param xml_name: The name of the XML file of the robot.
:return: The path to the MJCF file of the robot if it exists, otherwise None.
"""
+ multiverse_resources = find_multiverse_resources_path() if multiverse_resources is None else multiverse_resources
xml_name = xml_name if xml_name is not None else robot_name
if '.xml' not in xml_name:
xml_name = xml_name + '.xml'
- multiverse_resources = find_multiverse_resources_path()
try:
robot_folder = os.path.join(multiverse_resources, 'robots', robot_relative_dir, robot_name)
except TypeError:
diff --git a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py
index 367acaf9a..90fdd1a3d 100644
--- a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py
+++ b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py
@@ -124,7 +124,7 @@ def is_visible(self, object_designator: ObjectDesignatorDescription) -> Reasonin
:param object_designator: The object in question
:return: Reasoning result with the visibility of the object
"""
- cam_pose = World.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_frame())
+ cam_pose = World.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_link())
return ReasoningResult(visible(object_designator.resolve().world_object, cam_pose))
def empty(self) -> ReasoningResult:
diff --git a/src/pycram/object_descriptors/mjcf.py b/src/pycram/object_descriptors/mjcf.py
index cc9b8e914..d9475d0b2 100644
--- a/src/pycram/object_descriptors/mjcf.py
+++ b/src/pycram/object_descriptors/mjcf.py
@@ -1,3 +1,4 @@
+import glob
import os
import pathlib
from xml.etree import ElementTree as ET
@@ -34,8 +35,9 @@ class LinkDescription(AbstractLinkDescription):
A class that represents a link description of an object.
"""
- def __init__(self, mjcf_description: mjcf.Element):
+ def __init__(self, mjcf_description: mjcf.Element, mesh_dir: Optional[str] = None):
super().__init__(mjcf_description)
+ self.mesh_dir = mesh_dir
@property
def geometry(self) -> Union[List[VisualShape], VisualShape, None]:
@@ -50,8 +52,7 @@ def geometry(self) -> Union[List[VisualShape], VisualShape, None]:
else:
return [self._get_visual_shape(geom) for geom in all_geoms]
- @staticmethod
- def _get_visual_shape(mjcf_geometry) -> Union[VisualShape, None]:
+ def _get_visual_shape(self, mjcf_geometry) -> Union[VisualShape, None]:
"""
:param mjcf_geometry: The MJCFGeometry to get the visual shape for.
:return: The VisualShape of the given MJCFGeometry object.
@@ -64,9 +65,27 @@ def _get_visual_shape(mjcf_geometry) -> Union[VisualShape, None]:
return SphereVisualShape(Color(), [0, 0, 0], mjcf_geometry.size[0])
if mjcf_geometry.type == MJCFGeomType.MESH.value:
mesh_filename = mjcf_geometry.mesh.file.prefix + mjcf_geometry.mesh.file.extension
+ mesh_filename = self.look_for_file_in_mesh_dir(mesh_filename)
return MeshVisualShape(Color(), [0, 0, 0], mjcf_geometry.mesh.scale, mesh_filename)
return None
+ def look_for_file_in_mesh_dir(self, file_name: str) -> str:
+ """
+ Look for a file in the mesh directory of the object.
+
+ :param file_name: The name of the file.
+ :return: The path to the file.
+ """
+ # search for the file in the mesh directory
+ if self.mesh_dir is not None:
+ if os.path.exists(os.path.join(self.mesh_dir, file_name)):
+ return os.path.join(self.mesh_dir, file_name)
+ else:
+ # use glob to search for the file in the mesh directory
+ files = glob.glob(os.path.join(self.mesh_dir, '**', file_name), recursive=True)
+ if len(files) > 0:
+ return files[0]
+
@property
def origin(self) -> Union[Pose, None]:
"""
@@ -336,6 +355,14 @@ def __init__(self):
self._links = None
self._joints = None
self.virtual_joint_names = []
+ self._meshes_dir: Optional[str] = None
+
+ @property
+ def mesh_dir(self) -> Optional[str]:
+ try:
+ return getattr(self.parsed_description.compiler, self.MESH_DIR_ATTR)
+ except AttributeError:
+ return None
@property
def child_map(self) -> Dict[str, List[Tuple[str, str]]]:
@@ -464,6 +491,8 @@ def replace_relative_paths_with_absolute_paths(self, model_path: str) -> str:
for rel_dir_attrib in [self.MESH_DIR_ATTR, self.TEXTURE_DIR_ATTR]:
rel_dir = compiler.get(rel_dir_attrib)
abs_dir = str(pathlib.Path(os.path.join(model_dir, rel_dir)).resolve())
+ if rel_dir_attrib == self.MESH_DIR_ATTR:
+ self._meshes_dir = abs_dir
compiler.set(rel_dir_attrib, abs_dir)
return ET.tostring(root, encoding='unicode', method='xml')
@@ -486,7 +515,7 @@ def links(self) -> List[LinkDescription]:
:return: A list of link descriptions of this object.
"""
if self._links is None:
- self._links = [LinkDescription(link) for link in self.parsed_description.find_all('body')]
+ self._links = [LinkDescription(link, self.mesh_dir) for link in self.parsed_description.find_all('body')]
return self._links
def get_root(self) -> str:
diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py
index dc0d7c728..bdb48b8f7 100644
--- a/src/pycram/object_descriptors/urdf.py
+++ b/src/pycram/object_descriptors/urdf.py
@@ -108,7 +108,7 @@ def name(self) -> str:
@property
def has_limits(self) -> bool:
- return bool(self.parsed_description.limit)
+ return bool(self.parsed_description.limit) and not self.type == JointType.CONTINUOUS
@property
def type(self) -> JointType:
diff --git a/src/pycram/orm/action_designator.py b/src/pycram/orm/action_designator.py
index 4f24c8cb0..0aff86afd 100644
--- a/src/pycram/orm/action_designator.py
+++ b/src/pycram/orm/action_designator.py
@@ -107,6 +107,7 @@ class OpenAction(ObjectMixin, Action):
id: Mapped[int] = mapped_column(ForeignKey(f'{Action.__tablename__}.id'), primary_key=True, init=False)
arm: Mapped[Arms]
+ grasping_prepose_distance: Mapped[float]
class CloseAction(ObjectMixin, Action):
@@ -114,6 +115,7 @@ class CloseAction(ObjectMixin, Action):
id: Mapped[int] = mapped_column(ForeignKey(f'{Action.__tablename__}.id'), primary_key=True, init=False)
arm: Mapped[Arms]
+ grasping_prepose_distance: Mapped[float]
class GraspingAction(ObjectMixin, Action):
@@ -121,6 +123,7 @@ class GraspingAction(ObjectMixin, Action):
id: Mapped[int] = mapped_column(ForeignKey(f'{Action.__tablename__}.id'), primary_key=True, init=False)
arm: Mapped[Arms]
+ prepose_distance: Mapped[float]
class FaceAtAction(PoseMixin, Action):
diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py
index 1232ee5b0..a6d55412c 100644
--- a/src/pycram/pose_generator_and_validator.py
+++ b/src/pycram/pose_generator_and_validator.py
@@ -1,7 +1,8 @@
import numpy as np
import tf
-from typing_extensions import Tuple, List, Union, Dict, Iterable
+from typing_extensions import Tuple, List, Union, Dict, Iterable, Optional
+from .datastructures.enums import Arms, Grasp
from .costmaps import Costmap
from .datastructures.pose import Pose, Transform
from .datastructures.world import World
@@ -9,6 +10,7 @@
from .failures import IKError
from .local_transformer import LocalTransformer
from .robot_description import RobotDescription
+from .ros.logging import logdebug
from .world_concepts.world_object import Object
from .world_reasoning import contact
@@ -56,7 +58,7 @@ def __iter__(self) -> Iterable:
"""
# Determines how many positions should be sampled from the costmap
- if self.number_of_samples == -1:
+ if self.number_of_samples == -1 or self.number_of_samples > self.costmap.map.flatten().shape[0]:
self.number_of_samples = self.costmap.map.flatten().shape[0]
indices = np.argpartition(self.costmap.map.flatten(), -self.number_of_samples)[-self.number_of_samples:]
indices = np.dstack(np.unravel_index(indices, self.costmap.map.shape)).reshape(self.number_of_samples, 2)
@@ -150,18 +152,17 @@ def _in_contact(robot: Object, obj: Object, allowed_collision: Dict[Object, List
allowed_links = allowed_collision[obj] if obj in allowed_collision.keys() else []
if in_contact:
- for link in contact_links:
- if link[0].name in allowed_robot_links or link[1].name in allowed_links:
- in_contact = False
- # TODO: in_contact is never set to True after it was set to False is that correct?
- # TODO: If it is correct, then this loop should break after the first contact is found
+ if all(link[0].name in allowed_robot_links or link[1].name in allowed_links for link in contact_links):
+ in_contact = False
return in_contact
def reachability_validator(pose: Pose,
robot: Object,
target: Union[Object, Pose],
- allowed_collision: Dict[Object, List] = None) -> Tuple[bool, List]:
+ prepose_distance: float = 0.03,
+ allowed_collision: Dict[Object, List] = None,
+ arm: Optional[Arms] = None) -> Tuple[bool, List[Arms]]:
"""
This method validates if a target position is reachable for a pose candidate.
This is done by asking the ik solver if there is a valid solution if the
@@ -171,16 +172,21 @@ def reachability_validator(pose: Pose,
:param pose: The pose candidate for which the reachability should be validated
:param robot: The robot object in the World for which the reachability should be validated.
:param target: The target position or object instance which should be the target for reachability.
- :param allowed_collision: dict of objects with which the robot is allowed to collide each object correlates to a list of links of which this object consists
+ :param prepose_distance: The distance the robot should retract from the target position after/before reaching it.
+ :param allowed_collision: dict of objects with which the robot is allowed to collide each object correlates
+ to a list of links of which this object consists
+ :param arm: The arm that should be used for the reachability check. If None all arms are checked.
:return: True if the target is reachable for the robot and False in any other case.
"""
if type(target) == Object:
target = target.get_pose()
robot.set_pose(pose)
- # manipulator_descs = list(
- # filter(lambda chain: isinstance(chain[1], ManipulatorDescription), robot_description.chains.items()))
- manipulator_descs = RobotDescription.current_robot_description.get_manipulator_chains()
+
+ if arm is not None:
+ manipulator_descs = [RobotDescription.current_robot_description.get_arm_chain(arm)]
+ else:
+ manipulator_descs = RobotDescription.current_robot_description.get_manipulator_chains()
# TODO Make orientation adhere to grasping orientation
res = False
@@ -188,7 +194,7 @@ def reachability_validator(pose: Pose,
for description in manipulator_descs:
retract_target_pose = LocalTransformer().transform_pose(target, robot.get_link_tf_frame(
description.end_effector.tool_frame))
- retract_target_pose.position.x -= 0.07 # Care hard coded value copied from PlaceAction class
+ retract_target_pose.position.x -= prepose_distance # Care hard coded value copied from PlaceAction class
# retract_pose needs to be in world frame?
retract_target_pose = LocalTransformer().transform_pose(retract_target_pose, "map")
@@ -197,26 +203,28 @@ def reachability_validator(pose: Pose,
tool_frame = description.end_effector.tool_frame
# TODO Make orientation adhere to grasping orientation
- in_contact = False
joint_state_before_ik = robot.get_positions_of_all_joints()
try:
# test the possible solution and apply it to the robot
pose, joint_states = request_ik(target, robot, joints, tool_frame)
+ logdebug(f"Robot {description.name} can reach the the target pose")
robot.set_pose(pose)
robot.set_multiple_joint_positions(joint_states)
- # _apply_ik(robot, resp, joints)
in_contact = collision_check(robot, allowed_collision)
if not in_contact: # only check for retract pose if pose worked
+ logdebug("Robot is not in contact at target pose")
pose, joint_states = request_ik(retract_target_pose, robot, joints, tool_frame)
+ logdebug(f"Robot {description.name} can reach retract pose")
robot.set_pose(pose)
robot.set_multiple_joint_positions(joint_states)
- # _apply_ik(robot, resp, joints)
in_contact = collision_check(robot, allowed_collision)
if not in_contact:
+ logdebug("Robot is not in contact at retract pose")
arms.append(description.arm_type)
except IKError:
+ logdebug(f"Robot {description.name} cannot reach pose")
pass
finally:
robot.set_multiple_joint_positions(joint_state_before_ik)
diff --git a/src/pycram/process_module.py b/src/pycram/process_module.py
index 957d0f5e9..0fe6d51f5 100644
--- a/src/pycram/process_module.py
+++ b/src/pycram/process_module.py
@@ -285,8 +285,14 @@ def get_manager() -> Union[ProcessModuleManager, None]:
f"No execution_type is set, did you use the with_simulated_robot or with_real_robot decorator?")
return
+ robot_description = RobotDescription.current_robot_description
+ chains = robot_description.get_manipulator_chains()
+ gripper_name = [chain.end_effector.gripper_object_name for chain in chains
+ if chain.end_effector.gripper_object_name]
+ gripper_name = gripper_name[0] if len(gripper_name) > 0 else None
for pm_manager in ProcessModuleManager.available_pms:
- if pm_manager.robot_name == RobotDescription.current_robot_description.name:
+ if pm_manager.robot_name == robot_description.name or\
+ ((pm_manager.robot_name == gripper_name) and gripper_name):
manager = pm_manager
if pm_manager.robot_name == "default":
_default_manager = pm_manager
@@ -302,7 +308,7 @@ def get_manager() -> Union[ProcessModuleManager, None]:
f", and no default process modules available")
return None
- def navigate(self) -> Type[ProcessModule]:
+ def navigate(self) -> ProcessModule:
"""
Get the Process Module for navigating the robot with respect to
the :py:attr:`~ProcessModuleManager.execution_type`
@@ -312,7 +318,7 @@ def navigate(self) -> Type[ProcessModule]:
raise NotImplementedError(
f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'")
- def pick_up(self) -> Type[ProcessModule]:
+ def pick_up(self) -> ProcessModule:
"""
Get the Process Module for picking up with respect to the :py:attr:`~ProcessModuleManager.execution_type`
@@ -321,7 +327,7 @@ def pick_up(self) -> Type[ProcessModule]:
raise NotImplementedError(
f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'")
- def place(self) -> Type[ProcessModule]:
+ def place(self) -> ProcessModule:
"""
Get the Process Module for placing with respect to the :py:attr:`~ProcessModuleManager.execution_type`
@@ -330,7 +336,7 @@ def place(self) -> Type[ProcessModule]:
raise NotImplementedError(
f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'")
- def looking(self) -> Type[ProcessModule]:
+ def looking(self) -> ProcessModule:
"""
Get the Process Module for looking at a point with respect to
the :py:attr:`~ProcessModuleManager.execution_type`
@@ -340,7 +346,7 @@ def looking(self) -> Type[ProcessModule]:
raise NotImplementedError(
f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'")
- def detecting(self) -> Type[ProcessModule]:
+ def detecting(self) -> ProcessModule:
"""
Get the Process Module for detecting an object with respect to
the :py:attr:`~ProcessModuleManager.execution_type`
@@ -350,7 +356,7 @@ def detecting(self) -> Type[ProcessModule]:
raise NotImplementedError(
f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'")
- def move_tcp(self) -> Type[ProcessModule]:
+ def move_tcp(self) -> ProcessModule:
"""
Get the Process Module for moving the Tool Center Point with respect to
the :py:attr:`~ProcessModuleManager.execution_type`
@@ -360,7 +366,7 @@ def move_tcp(self) -> Type[ProcessModule]:
raise NotImplementedError(
f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'")
- def move_arm_joints(self) -> Type[ProcessModule]:
+ def move_arm_joints(self) -> ProcessModule:
"""
Get the Process Module for moving the joints of the robot arm
with respect to the :py:attr:`~ProcessModuleManager.execution_type`
@@ -370,7 +376,7 @@ def move_arm_joints(self) -> Type[ProcessModule]:
raise NotImplementedError(
f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'")
- def world_state_detecting(self) -> Type[ProcessModule]:
+ def world_state_detecting(self) -> ProcessModule:
"""
Get the Process Module for detecting an object using the world state with respect to the
:py:attr:`~ProcessModuleManager.execution_type`
@@ -380,7 +386,7 @@ def world_state_detecting(self) -> Type[ProcessModule]:
raise NotImplementedError(
f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'")
- def move_joints(self) -> Type[ProcessModule]:
+ def move_joints(self) -> ProcessModule:
"""
Get the Process Module for moving any joint of the robot with respect to the
:py:attr:`~ProcessModuleManager.execution_type`
@@ -390,7 +396,7 @@ def move_joints(self) -> Type[ProcessModule]:
raise NotImplementedError(
f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'")
- def move_gripper(self) -> Type[ProcessModule]:
+ def move_gripper(self) -> ProcessModule:
"""
Get the Process Module for moving the gripper with respect to
the :py:attr:`~ProcessModuleManager.execution_type`
@@ -400,7 +406,7 @@ def move_gripper(self) -> Type[ProcessModule]:
raise NotImplementedError(
f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'")
- def open(self) -> Type[ProcessModule]:
+ def open(self) -> ProcessModule:
"""
Get the Process Module for opening drawers with respect to
the :py:attr:`~ProcessModuleManager.execution_type`
@@ -410,7 +416,7 @@ def open(self) -> Type[ProcessModule]:
raise NotImplementedError(
f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'")
- def close(self) -> Type[ProcessModule]:
+ def close(self) -> ProcessModule:
"""
Get the Process Module for closing drawers with respect to
the :py:attr:`~ProcessModuleManager.execution_type`
diff --git a/src/pycram/process_modules/__init__.py b/src/pycram/process_modules/__init__.py
index caa3dfe41..c8ca456c9 100644
--- a/src/pycram/process_modules/__init__.py
+++ b/src/pycram/process_modules/__init__.py
@@ -4,6 +4,7 @@
from .hsrb_process_modules import HSRBManager
from .default_process_modules import DefaultManager
from .stretch_process_modules import StretchManager
+from .robotiq_gripper_process_module import RobotiqManager
from .tiago_process_modules import TiagoManager
Pr2Manager()
@@ -13,3 +14,4 @@
DefaultManager()
StretchManager()
TiagoManager()
+RobotiqManager()
diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py
index 4cb8b6e89..e67bd53f2 100644
--- a/src/pycram/process_modules/boxy_process_modules.py
+++ b/src/pycram/process_modules/boxy_process_modules.py
@@ -128,13 +128,13 @@ def _execute(self, desig):
robot = World.robot
object_type = desig.object_type
# Should be "wide_stereo_optical_frame"
- cam_frame_name = RobotDescription.current_robot_description.get_camera_frame()
+ cam_link_name = RobotDescription.current_robot_description.get_camera_link()
# should be [0, 0, 1]
front_facing_axis = RobotDescription.current_robot_description.get_default_camera().front_facing_axis
objects = World.current_world.get_object_by_type(object_type)
for obj in objects:
- if btr.visible(obj, robot.get_link_pose(cam_frame_name), front_facing_axis):
+ if btr.visible(obj, robot.get_link_pose(cam_link_name), front_facing_axis):
return obj
diff --git a/src/pycram/process_modules/default_process_modules.py b/src/pycram/process_modules/default_process_modules.py
index b8df93b52..f3900ee67 100644
--- a/src/pycram/process_modules/default_process_modules.py
+++ b/src/pycram/process_modules/default_process_modules.py
@@ -67,16 +67,19 @@ def _execute(self, desig: LookingMotion):
class DefaultMoveGripper(ProcessModule):
"""
This process module controls the gripper of the robot. They can either be opened or closed.
- Furthermore, it can only moved one gripper at a time.
+ Furthermore, it can only move one gripper at a time.
"""
def _execute(self, desig: MoveGripperMotion):
- robot = World.robot
+ robot_description = RobotDescription.current_robot_description
gripper = desig.gripper
+ arm_chain = robot_description.get_arm_chain(gripper)
+ if arm_chain.end_effector.gripper_object_name is not None:
+ robot = World.current_world.get_object_by_name(arm_chain.end_effector.gripper_object_name)
+ else:
+ robot = World.robot
motion = desig.motion
- for joint, state in RobotDescription.current_robot_description.get_arm_chain(gripper).get_static_gripper_state(
- motion).items():
- robot.set_joint_position(joint, state)
+ robot.set_multiple_joint_positions(arm_chain.get_static_gripper_state(motion))
class DefaultDetecting(ProcessModule):
@@ -88,7 +91,7 @@ class DefaultDetecting(ProcessModule):
def _execute(self, designator: DetectingMotion):
robot = World.robot
- cam_frame_name = RobotDescription.current_robot_description.get_camera_link()
+ cam_link_name = RobotDescription.current_robot_description.get_camera_link()
camera_description = RobotDescription.current_robot_description.cameras[
list(RobotDescription.current_robot_description.cameras.keys())[0]]
front_facing_axis = camera_description.front_facing_axis
@@ -113,7 +116,7 @@ def _execute(self, designator: DetectingMotion):
elif designator.technique == DetectionTechnique.HUMAN_WAVING:
raise NotImplementedError("Detection by waving human is not yet implemented in simulation")
for obj in world_objects:
- if visible(obj, robot.get_link_pose(cam_frame_name), front_facing_axis):
+ if visible(obj, robot.get_link_pose(cam_link_name), front_facing_axis):
query_result.append(obj)
if query_result is None:
raise PerceptionObjectNotFound(
diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py
index 7a7a991b3..bd34181ed 100644
--- a/src/pycram/process_modules/pr2_process_modules.py
+++ b/src/pycram/process_modules/pr2_process_modules.py
@@ -1,35 +1,44 @@
-from threading import Lock
-from typing_extensions import Any, TYPE_CHECKING
-
+from typing_extensions import TYPE_CHECKING
import actionlib
from .default_process_modules import DefaultDetectingReal, DefaultDetecting
from .. import world_reasoning as btr
import numpy as np
-from .. import world_reasoning as btr
+import pycrap
from ..external_interfaces.move_base import query_pose_nav
-from ..process_module import ProcessModule, ProcessModuleManager
-from ..external_interfaces.ik import request_ik
-from ..ros.logging import logdebug
-from ..utils import _apply_ik
-from ..local_transformer import LocalTransformer
-
+from .. import world_reasoning as btr
+from ..datastructures.enums import JointType, Arms, ExecutionType, GripperState, MovementType
+from ..datastructures.world import World
from ..designators.motion_designator import MoveMotion, LookingMotion, \
DetectingMotion, MoveTCPMotion, MoveArmJointsMotion, WorldStateDetectingMotion, MoveJointsMotion, \
MoveGripperMotion, OpeningMotion, ClosingMotion
-from ..robot_description import RobotDescription
-from ..datastructures.world import World
-from ..world_concepts.world_object import Object
-from ..datastructures.pose import Pose
-from ..datastructures.enums import JointType, ObjectType, Arms, ExecutionType, MovementType, GripperState
from ..external_interfaces import giskard
+from ..external_interfaces.ik import request_ik
from ..external_interfaces.robokudo import *
-from ..ros.logging import loginfo, logwarn, logdebug
+from ..failures import NavigationGoalNotReachedError, ToolPoseNotReachedError
+from ..local_transformer import LocalTransformer
+from ..process_module import ProcessModule, ProcessModuleManager
+from ..robot_description import RobotDescription
+from ..ros.logging import logdebug
+from ..utils import _apply_ik
+from ..world_concepts.world_object import Object
+from ..ros.data_types import Duration
if TYPE_CHECKING:
from ..designators.object_designator import ObjectDesignatorDescription
+try:
+ from ..worlds.multiverse import Multiverse
+except ImportError:
+ Multiverse = type(None)
+
+try:
+ from control_msgs.msg import GripperCommandGoal, GripperCommandAction
+except ImportError:
+ if Multiverse is not None:
+ logwarn("Import for control_msgs for gripper in Multiverse failed")
+
try:
from pr2_controllers_msgs.msg import Pr2GripperCommandGoal, Pr2GripperCommandAction, Pr2
except ImportError:
@@ -193,6 +202,8 @@ class Pr2NavigationReal(ProcessModule):
def _execute(self, designator: MoveMotion) -> Any:
logdebug(f"Sending goal to movebase to Move the robot")
query_pose_nav(designator.target)
+ if not World.current_world.robot.pose.almost_equal(designator.target, 0.05, 3):
+ raise NavigationGoalNotReachedError(World.current_world.robot.pose, designator.target)
class Pr2MoveHeadReal(ProcessModule):
@@ -228,13 +239,25 @@ class Pr2MoveTCPReal(ProcessModule):
def _execute(self, designator: MoveTCPMotion) -> Any:
lt = LocalTransformer()
pose_in_map = lt.transform_pose(designator.target, "map")
+ tip_link = RobotDescription.current_robot_description.get_arm_chain(designator.arm).get_tool_frame()
+ root_link = "map"
+ gripper_that_can_collide = designator.arm if designator.allow_gripper_collision else None
if designator.allow_gripper_collision:
giskard.allow_gripper_collision(designator.arm.name.lower())
- giskard.achieve_cartesian_goal(pose_in_map, RobotDescription.current_robot_description.get_arm_chain(
- designator.arm).get_tool_frame(), "torso_lift_link",
- use_monitor=World.current_world.conf.use_giskard_monitor)
- # robot_description.base_link)
+
+ if designator.movement_type == MovementType.STRAIGHT_TRANSLATION:
+ giskard.achieve_straight_translation_goal(pose_in_map.position_as_list(), tip_link, root_link)
+ elif designator.movement_type == MovementType.STRAIGHT_CARTESIAN:
+ giskard.achieve_straight_cartesian_goal(pose_in_map, tip_link, root_link)
+ elif designator.movement_type == MovementType.TRANSLATION:
+ giskard.achieve_translation_goal(pose_in_map.position_as_list(), tip_link, root_link)
+ elif designator.movement_type == MovementType.CARTESIAN:
+ giskard.achieve_cartesian_goal(pose_in_map, tip_link, root_link,
+ grippers_that_can_collide=gripper_that_can_collide,
+ use_monitor=designator.monitor_motion)
+ if not World.current_world.robot.get_link_pose(tip_link).almost_equal(designator.target, 0.01, 3):
+ raise ToolPoseNotReachedError(World.current_world.robot.get_link_pose(tip_link), designator.target)
class Pr2MoveArmJointsReal(ProcessModule):
@@ -263,6 +286,36 @@ def _execute(self, designator: MoveJointsMotion) -> Any:
giskard.achieve_joint_goal(name_to_position)
+class Pr2MoveGripperMultiverse(ProcessModule):
+ """
+ Opens or closes the gripper of the real PR2, gripper uses an action server for this instead of giskard
+ """
+
+ def _execute(self, designator: MoveGripperMotion) -> Any:
+ def activate_callback():
+ loginfo("Started gripper Movement")
+
+ def done_callback(state, result):
+ loginfo(f"Reached goal {designator.motion}: {result.reached_goal}")
+
+ def feedback_callback(msg):
+ loginfo(f"Gripper Action Feedback: {msg}")
+
+ goal = GripperCommandGoal()
+ goal.command.position = 0.0 if designator.motion == GripperState.CLOSE else 0.4
+ goal.command.max_effort = 50.0
+ if designator.gripper == "right":
+ controller_topic = "/real/pr2/right_gripper_controller/gripper_cmd"
+ else:
+ controller_topic = "/real/pr2/left_gripper_controller/gripper_cmd"
+ client = actionlib.SimpleActionClient(controller_topic, GripperCommandAction)
+ loginfo("Waiting for action server")
+ client.wait_for_server()
+ client.send_goal(goal, active_cb=activate_callback, done_cb=done_callback, feedback_cb=feedback_callback)
+ wait = client.wait_for_result(Duration(5))
+ # client.cancel_all_goals()
+
+
class Pr2MoveGripperReal(ProcessModule):
"""
Opens or closes the gripper of the real PR2, gripper uses an action server for this instead of giskard
@@ -376,7 +429,11 @@ def move_gripper(self):
if ProcessModuleManager.execution_type == ExecutionType.SIMULATED:
return Pr2MoveGripper(self._move_gripper_lock)
elif ProcessModuleManager.execution_type == ExecutionType.REAL:
- return Pr2MoveGripperReal(self._move_gripper_lock)
+ if (isinstance(World.current_world, Multiverse) and
+ World.current_world.conf.use_multiverse_process_modules):
+ return Pr2MoveGripperMultiverse(self._move_gripper_lock)
+ else:
+ return Pr2MoveGripperReal(self._move_gripper_lock)
def open(self):
if ProcessModuleManager.execution_type == ExecutionType.SIMULATED:
diff --git a/src/pycram/process_modules/robotiq_gripper_process_module.py b/src/pycram/process_modules/robotiq_gripper_process_module.py
new file mode 100644
index 000000000..1400ddc8e
--- /dev/null
+++ b/src/pycram/process_modules/robotiq_gripper_process_module.py
@@ -0,0 +1,36 @@
+from threading import Lock
+
+from std_msgs.msg import Float64
+from typing_extensions import Any
+
+from .default_process_modules import DefaultMoveGripper
+from ..datastructures.world import World
+from ..datastructures.enums import GripperState, ExecutionType
+from ..designators.motion_designator import MoveGripperMotion
+from ..process_module import ProcessModule, ProcessModuleManager
+from ..ros.publisher import create_publisher
+from ..robot_descriptions.ur5e_controlled_description import GRIPPER_NAME, GRIPPER_CMD_TOPIC, OPEN_VALUE, CLOSE_VALUE
+
+
+class RobotiqMoveGripperReal(ProcessModule):
+ """
+ Opens or closes the gripper of the real Robotiq gripper, uses a topic for this instead of giskard
+ """
+ def _execute(self, designator: MoveGripperMotion) -> Any:
+ value = OPEN_VALUE if designator.motion == GripperState.OPEN else CLOSE_VALUE
+ publisher = create_publisher(GRIPPER_CMD_TOPIC, Float64, queue_size=10, latch=True)
+ World.current_world.step(func=lambda: publisher.publish(Float64(value)), step_seconds=0.1)
+ return True
+
+
+class RobotiqManager(ProcessModuleManager):
+
+ def __init__(self):
+ super().__init__(GRIPPER_NAME)
+ self._move_gripper_lock = Lock()
+
+ def move_gripper(self):
+ if ProcessModuleManager.execution_type == ExecutionType.SIMULATED:
+ return DefaultMoveGripper(self._move_gripper_lock)
+ elif ProcessModuleManager.execution_type == ExecutionType.REAL:
+ return RobotiqMoveGripperReal(self._move_gripper_lock)
diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py
index 495267a3a..5b8ba2aab 100644
--- a/src/pycram/robot_description.py
+++ b/src/pycram/robot_description.py
@@ -114,11 +114,12 @@ class RobotDescription:
virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = None
"""
Virtual mobile base joint names for mobile robots, these joints are not part of the URDF, however they are used to
- move the robot in the simulation (e.g. set_pose for the robot would actually move these joints)
+ move the robot in the simulation (e.g. set_pose for the robot would actually move these joints)
"""
def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, urdf_path: str,
- virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = None, mjcf_path: Optional[str] = None):
+ virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = None, mjcf_path: Optional[str] = None,
+ ignore_joints: Optional[List[str]] = None):
"""
Initialize the RobotDescription. The URDF is loaded from the given path and used as basis for the kinematic
chains.
@@ -130,11 +131,13 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str,
:param urdf_path: Path to the URDF file of the robot
:param virtual_mobile_base_joints: Virtual mobile base joint names for mobile robots
:param mjcf_path: Path to the MJCF file of the robot
+ :param ignore_joints: List of joint names that are not used.
"""
self.name = name
self.base_link = base_link
self.torso_link = torso_link
self.torso_joint = torso_joint
+ self.ignore_joints = ignore_joints if ignore_joints else []
with suppress_stdout_stderr():
# Since parsing URDF causes a lot of warning messages which can't be deactivated, we suppress them
self.urdf_object = URDFObject(urdf_path)
@@ -247,21 +250,21 @@ def get_manipulator_chains(self) -> List[KinematicChainDescription]:
result.append(chain)
return result
- def get_camera_link(self) -> str:
+ def get_camera_frame(self, robot_object_name: str) -> str:
"""
Quick method to get the name of a link of a camera. Uses the first camera in the list of cameras.
:return: A name of the link of a camera
"""
- return self.cameras[list(self.cameras.keys())[0]].link_name
+ return f"{robot_object_name}/{self.get_camera_link()}"
- def get_camera_frame(self) -> str:
+ def get_camera_link(self) -> str:
"""
Quick method to get the name of a link of a camera. Uses the first camera in the list of cameras.
:return: A name of the link of a camera
"""
- return f"{self.name}/{self.cameras[list(self.cameras.keys())[0]].link_name}"
+ return self.get_default_camera().link_name
def get_default_camera(self) -> CameraDescription:
"""
@@ -630,8 +633,14 @@ class EndEffectorDescription:
"""
Distance the gripper can open, in cm
"""
+ gripper_object_name: Optional[str] = None
+ """
+ Name of the gripper of the robot if it has one, this is used when the gripper is a different Object with its own
+ description file outside the robot description file.
+ """
- def __init__(self, name: str, start_link: str, tool_frame: str, urdf_object: URDFObject):
+ def __init__(self, name: str, start_link: str, tool_frame: str, urdf_object: URDFObject,
+ gripper_object_name: Optional[str] = None):
"""
Initialize the EndEffectorDescription object.
@@ -639,6 +648,7 @@ def __init__(self, name: str, start_link: str, tool_frame: str, urdf_object: URD
:param start_link: Root link of the end effector, every link below this link in the URDF is part of the end effector
:param tool_frame: Name of the tool frame link in the URDf
:param urdf_object: URDF object of the robot
+ :param gripper_object_name: Name of the gripper if it is a separate Object outside the robot description.
"""
self.name: str = name
self.start_link: str = start_link
@@ -648,6 +658,8 @@ def __init__(self, name: str, start_link: str, tool_frame: str, urdf_object: URD
self.joint_names: List[str] = []
self.static_joint_states: Dict[GripperState, Dict[str, float]] = {}
self._init_links_joints()
+ self.gripper_object_name = gripper_object_name
+
def _init_links_joints(self):
"""
diff --git a/src/pycram/robot_descriptions/pr2_description.py b/src/pycram/robot_descriptions/pr2_description.py
index 35ee28838..d0ff58b44 100644
--- a/src/pycram/robot_descriptions/pr2_description.py
+++ b/src/pycram/robot_descriptions/pr2_description.py
@@ -10,8 +10,14 @@
mjcf_filename = get_robot_mjcf_path("", "pr2")
-pr2_description = RobotDescription("pr2", "base_link", "torso_lift_link", "torso_lift_joint",
- filename, virtual_mobile_base_joints=VirtualMobileBaseJoints(), mjcf_path=mjcf_filename)
+pr2_description = RobotDescription("pr2", "base_link", "torso_lift_link",
+ "torso_lift_joint", filename,
+ virtual_mobile_base_joints=VirtualMobileBaseJoints(),
+ mjcf_path=mjcf_filename,
+ ignore_joints=['torso_lift_motor_screw_joint', 'r_gripper_motor_slider_joint',
+ 'r_gripper_motor_screw_joint', 'r_gripper_joint',
+ 'l_gripper_motor_slider_joint', 'l_gripper_motor_screw_joint',
+ 'l_gripper_joint'])
################################## Left Arm ##################################
left_arm = KinematicChainDescription("left", "torso_lift_link", "l_wrist_roll_link",
diff --git a/src/pycram/robot_descriptions/ur5e_controlled_description.py b/src/pycram/robot_descriptions/ur5e_controlled_description.py
new file mode 100644
index 000000000..403c5669a
--- /dev/null
+++ b/src/pycram/robot_descriptions/ur5e_controlled_description.py
@@ -0,0 +1,67 @@
+import os
+
+from ..datastructures.enums import Arms, GripperState
+from ..helper import get_robot_mjcf_path, find_multiverse_resources_path, get_robot_urdf_path_from_multiverse
+from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \
+ RobotDescriptionManager
+from ..object_descriptors.urdf import ObjectDescription as URDFObject
+from ..ros.logging import logwarn
+
+multiverse_resources = find_multiverse_resources_path()
+ROBOT_NAME = "ur5e"
+GRIPPER_NAME = "gripper-2F-85"
+GRIPPER_CMD_TOPIC = "/gripper_command"
+OPEN_VALUE = 0.0
+CLOSE_VALUE = 255.0
+if multiverse_resources is None:
+ logwarn("Could not initialize ur5e description as Multiverse resources path not found.")
+else:
+ robot_relative_dir = "universal_robot"
+ filename = get_robot_urdf_path_from_multiverse(robot_relative_dir, ROBOT_NAME, resources_dir=multiverse_resources)
+ mjcf_filename = get_robot_mjcf_path(robot_relative_dir, ROBOT_NAME, multiverse_resources=multiverse_resources)
+ ur5_description = RobotDescription(ROBOT_NAME, "base_link", "", "",
+ filename, mjcf_path=mjcf_filename)
+
+ ################################## Arm ##################################
+ arm = KinematicChainDescription("manipulator", "base_link", "wrist_3_link",
+ ur5_description.urdf_object, arm_type=Arms.RIGHT)
+
+ arm.add_static_joint_states("home", {'shoulder_pan_joint': 3.14,
+ 'shoulder_lift_joint': -1.56,
+ 'elbow_joint': 1.58,
+ 'wrist_1_joint': -1.57,
+ 'wrist_2_joint': -1.57,
+ 'wrist_3_joint': 0.0})
+
+ ur5_description.add_kinematic_chain_description(arm)
+
+ ################################## Gripper ##################################
+ gripper_relative_dir = "robotiq"
+ gripper_filename = get_robot_urdf_path_from_multiverse(gripper_relative_dir, GRIPPER_NAME,
+ resources_dir=multiverse_resources)
+ gripper_urdf_obj = URDFObject(gripper_filename)
+ gripper = EndEffectorDescription("gripper", GRIPPER_NAME, "right_pad",
+ gripper_urdf_obj, gripper_object_name=GRIPPER_NAME)
+
+ gripper.add_static_joint_states(GripperState.OPEN, {'right_driver_joint': 0.0,
+ 'right_coupler_joint': 0.0,
+ 'right_spring_link_joint': 0.0,
+ 'right_follower_joint': 0.0,
+ 'left_driver_joint': 0.0,
+ 'left_coupler_joint': 0.0,
+ 'left_spring_link_joint': 0.0,
+ 'left_follower_joint': 0.0})
+ gripper.add_static_joint_states(GripperState.CLOSE, {'right_driver_joint': 0.798,
+ 'right_coupler_joint': 0.00366,
+ 'right_spring_link_joint': 0.796,
+ 'right_follower_joint': -0.793,
+ 'left_driver_joint': 0.798,
+ 'left_coupler_joint': 0.00366,
+ 'left_spring_link_joint': 0.796,
+ 'left_follower_joint': -0.793})
+
+ arm.end_effector = gripper
+
+ # Add to RobotDescriptionManager
+ rdm = RobotDescriptionManager()
+ rdm.register_description(ur5_description)
diff --git a/src/pycram/ros/data_types.py b/src/pycram/ros/data_types.py
index 4b6956279..4cd21c027 100644
--- a/src/pycram/ros/data_types.py
+++ b/src/pycram/ros/data_types.py
@@ -5,7 +5,7 @@ def Time(time=0.0):
return rospy.Time(time)
def Duration(duration=0.0):
- return rospy.Duration(duration)
+ return rospy.Duration.from_sec(duration)
def Rate(rate):
return rospy.Rate(rate)
\ No newline at end of file
diff --git a/src/pycram/ros/logging.py b/src/pycram/ros/logging.py
index d7e59516b..213d911eb 100644
--- a/src/pycram/ros/logging.py
+++ b/src/pycram/ros/logging.py
@@ -75,7 +75,8 @@ def logerr(message: str):
def logdebug(message: str):
- rospy.logdebug(f"[{_get_caller_file_name()}:{_get_caller_method_line()}:{_get_caller_method_name()}] {message}")
+ rospy.logdebug(f"[{_get_caller_file_name()}:{_get_caller_method_line()}:{_get_caller_method_name()}] {message}",
+ logger_name=PYCRAM_LOGGER_NAME)
def logwarn_once(message: str):
diff --git a/src/pycram/ros/publisher.py b/src/pycram/ros/publisher.py
index 533a7c89b..ae72dd164 100644
--- a/src/pycram/ros/publisher.py
+++ b/src/pycram/ros/publisher.py
@@ -1,4 +1,5 @@
import rospy
-def create_publisher(topic, msg_type, queue_size=10) -> rospy.Publisher:
- return rospy.Publisher(topic, msg_type, queue_size=queue_size)
\ No newline at end of file
+
+def create_publisher(topic, msg_type, queue_size=10, latch: bool = False) -> rospy.Publisher:
+ return rospy.Publisher(topic, msg_type, queue_size=queue_size, latch=latch)
diff --git a/src/pycram/ros/ros_tools.py b/src/pycram/ros/ros_tools.py
index bd1fbfefb..a89152e41 100644
--- a/src/pycram/ros/ros_tools.py
+++ b/src/pycram/ros/ros_tools.py
@@ -46,5 +46,5 @@ def get_time():
return rospy.get_time()
-def create_timer(duration: int, callback, oneshot=False):
+def create_timer(duration: rospy.Duration, callback, oneshot=False):
return rospy.Timer(duration, callback, oneshot=oneshot)
diff --git a/src/pycram/ros_utils/robot_state_updater.py b/src/pycram/ros_utils/robot_state_updater.py
new file mode 100644
index 000000000..abff550c9
--- /dev/null
+++ b/src/pycram/ros_utils/robot_state_updater.py
@@ -0,0 +1,88 @@
+import atexit
+from datetime import timedelta
+
+import tf
+import time
+
+from geometry_msgs.msg import TransformStamped
+from sensor_msgs.msg import JointState
+from typing_extensions import Optional
+
+from ..datastructures.world import World
+from ..robot_description import RobotDescription
+from ..datastructures.pose import Pose
+from ..ros.data_types import Time, Duration
+from ..ros.ros_tools import wait_for_message, create_timer
+
+
+class WorldStateUpdater:
+ """
+ Updates the robot in the World with information of the real robot published to ROS topics.
+ Infos used to update the robot are:
+
+ * The current pose of the robot
+ * The current joint state of the robot
+ """
+
+ def __init__(self, tf_topic: str, joint_state_topic: str, update_rate: timedelta = timedelta(milliseconds=100),
+ world: Optional[World] = None) -> None:
+ """
+ The robot state updater uses a TF topic and a joint state topic to get the current state of the robot.
+
+ :param tf_topic: Name of the TF topic, needs to publish geometry_msgs/TransformStamped
+ :param joint_state_topic: Name of the joint state topic, needs to publish sensor_msgs/JointState
+ """
+ self.tf_listener = tf.TransformListener()
+ time.sleep(1)
+ self.tf_topic = tf_topic
+ self.joint_state_topic = joint_state_topic
+ self.world: Optional[World] = world
+ self.tf_timer = create_timer(Duration().from_sec(update_rate.total_seconds()), self._subscribe_tf)
+ self.joint_state_timer = create_timer(Duration().from_sec(update_rate.total_seconds()),
+ self._subscribe_joint_state)
+
+ atexit.register(self._stop_subscription)
+
+ def _subscribe_tf(self, msg: TransformStamped) -> None:
+ """
+ Callback for the TF timer, will do a lookup of the transform between map frame and the objects frames.
+
+ :param msg: TransformStamped message published to the topic
+ """
+ if self.world is None:
+ if not World.current_world.is_prospection_world:
+ self.world = World.current_world
+ else:
+ return
+ for obj in self.world.objects:
+ if obj.name == self.world.robot.name:
+ tf_frame = RobotDescription.current_robot_description.base_link
+ elif obj.is_an_environment:
+ continue
+ else:
+ tf_frame = obj.tf_frame
+ trans, rot = self.tf_listener.lookupTransform("/map", tf_frame, Time(0))
+ obj.set_pose(Pose(trans, rot))
+
+ def _subscribe_joint_state(self, msg: JointState) -> None:
+ """
+ Sets the current joint configuration of the robot in the world to the configuration published on the
+ topic. Since this uses rospy.wait_for_message which can have errors when used with threads there might be an
+ attribute error in the rospy implementation.
+
+ :param msg: JointState message published to the topic.
+ """
+ try:
+ msg = wait_for_message(self.joint_state_topic, JointState)
+ joint_positions = dict(zip(msg.name, msg.position))
+ World.robot.set_multiple_joint_positions(joint_positions)
+ except AttributeError:
+ pass
+
+ def _stop_subscription(self) -> None:
+ """
+ Stops the Timer for TF and joint states and therefore the updating of the robot in the world.
+ """
+ self.tf_timer.shutdown()
+ self.joint_state_timer.shutdown()
+
diff --git a/src/pycram/ros_utils/viz_marker_publisher.py b/src/pycram/ros_utils/viz_marker_publisher.py
index 280605257..613bfbebb 100644
--- a/src/pycram/ros_utils/viz_marker_publisher.py
+++ b/src/pycram/ros_utils/viz_marker_publisher.py
@@ -92,7 +92,10 @@ def _make_marker_array(self) -> MarkerArray:
if isinstance(geom, MeshVisualShape):
msg.type = Marker.MESH_RESOURCE
msg.mesh_resource = "file://" + geom.file_name
- msg.scale = Vector3(1, 1, 1)
+ if hasattr(geom, "scale") and geom.scale is not None:
+ msg.scale = Vector3(*geom.scale)
+ else:
+ msg.scale = Vector3(1, 1, 1)
msg.mesh_use_embedded_materials = True
elif isinstance(geom, CylinderVisualShape):
msg.type = Marker.CYLINDER
diff --git a/src/pycram/utils.py b/src/pycram/utils.py
index 08e776a4f..d762f5d3f 100644
--- a/src/pycram/utils.py
+++ b/src/pycram/utils.py
@@ -6,6 +6,7 @@
Classes:
GeneratorList -- implementation of generator list wrappers.
"""
+from __future__ import annotations
from inspect import isgeneratorfunction
import os
import math
@@ -13,7 +14,8 @@
import numpy as np
from matplotlib import pyplot as plt
import matplotlib.colors as mcolors
-from typing_extensions import Tuple, Callable, List, Dict, TYPE_CHECKING
+from tf.transformations import quaternion_about_axis, quaternion_multiply
+from typing_extensions import Tuple, Callable, List, Dict, TYPE_CHECKING, Sequence
from .datastructures.dataclasses import Color
from .datastructures.pose import Pose
@@ -157,6 +159,106 @@ def __exit__(self, *_):
os.close(fd)
+def adjust_camera_pose_based_on_target(cam_pose: Pose, target_pose: Pose,
+ camera_description: CameraDescription) -> Pose:
+ """
+ Adjust the given cam_pose orientation such that it is facing the target_pose, which partly depends on the
+ front_facing_axis of the that is defined in the camera_description.
+
+ :param cam_pose: The camera pose.
+ :param target_pose: The target pose.
+ :param camera_description: The camera description.
+ :return: The adjusted camera pose.
+ """
+ quaternion = get_quaternion_between_camera_and_target(cam_pose, target_pose, camera_description)
+ # apply the rotation to the camera pose using quaternion multiplication
+ return apply_quaternion_to_pose(cam_pose, quaternion)
+
+
+def get_quaternion_between_camera_and_target(cam_pose: Pose, target_pose: Pose,
+ camera_description: 'CameraDescription') -> np.ndarray:
+ """
+ Get the quaternion between the camera and the target.
+
+ :param cam_pose: The camera pose.
+ :param target_pose: The target pose.
+ :param camera_description: The camera description.
+ :return: The quaternion between the camera and the target.
+ """
+ # Get the front facing axis of the camera in the world frame
+ front_facing_axis = transform_vector_using_pose(camera_description.front_facing_axis, cam_pose)
+ front_facing_axis = front_facing_axis - np.array(cam_pose.position_as_list())
+
+ # Get the vector from the camera to the target
+ camera_to_target = get_vector_between_poses(cam_pose, target_pose)
+
+ # Get the quaternion between the camera and target
+ return get_quaternion_between_two_vectors(front_facing_axis, camera_to_target)
+
+
+def get_vector_between_poses(pose1: Pose, pose2: Pose) -> np.ndarray:
+ """
+ Get the vector between two poses.
+
+ :param pose1: The first pose.
+ :param pose2: The second pose.
+ :return: The vector between the two poses.
+ """
+ return np.array(pose2.position_as_list()) - np.array(pose1.position_as_list())
+
+
+def transform_vector_using_pose(vector: Sequence, pose: Pose) -> np.ndarray:
+ """
+ Transform a vector using a pose.
+
+ :param vector: The vector.
+ :param pose: The pose.
+ :return: The transformed vector.
+ """
+ vector = np.array(vector).reshape(1, 3)
+ return pose.to_transform("pose").apply_transform_to_array_of_points(vector).flatten()
+
+
+def apply_quaternion_to_pose(pose: Pose, quaternion: np.ndarray) -> Pose:
+ """
+ Apply a quaternion to a pose.
+
+ :param pose: The pose.
+ :param quaternion: The quaternion.
+ :return: The new pose.
+ """
+ pose_quaternion = np.array(pose.orientation_as_list())
+ new_quaternion = quaternion_multiply(quaternion, pose_quaternion)
+ return Pose(pose.position_as_list(), new_quaternion.tolist())
+
+
+def get_quaternion_between_two_vectors(v1: np.ndarray, v2: np.ndarray) -> np.ndarray:
+ """
+ Get the quaternion between two vectors.
+
+ :param v1: The first vector.
+ :param v2: The second vector.
+ :return: The quaternion between the two vectors.
+ """
+ axis, angle = get_axis_angle_between_two_vectors(v1, v2)
+ return quaternion_about_axis(angle, axis)
+
+
+def get_axis_angle_between_two_vectors(v1: np.ndarray, v2: np.ndarray) -> Tuple[np.ndarray, float]:
+ """
+ Get the axis and angle between two vectors.
+
+ :param v1: The first vector.
+ :param v2: The second vector.
+ :return: The axis and angle between the two vectors.
+ """
+ v1 = v1 / np.linalg.norm(v1)
+ v2 = v2 / np.linalg.norm(v2)
+ axis = np.cross(v1, v2)
+ angle = np.arccos(np.dot(v1, v2) - 1e-9) # to avoid numerical errors
+ return axis, angle
+
+
class RayTestUtils:
def __init__(self, ray_test_batch: Callable, object_id_to_name: Dict = None):
@@ -256,10 +358,13 @@ def get_camera_rays_start_pose(self, camera_description: 'CameraDescription', ca
:param camera_pose: The camera pose.
:param camera_min_distance: The minimum distance from which the camera can see.
"""
- camera_pose_in_camera_frame = self.local_transformer.transform_pose(camera_pose, camera_frame)
+ camera_transform = camera_pose.to_transform("camera_pose")
+ self.local_transformer.update_transforms([camera_transform])
+ camera_pose_in_camera_frame = Pose(frame="camera_pose")
+ # camera_pose_in_camera_frame = self.local_transformer.transform_pose(camera_pose, camera_frame)
start_position = (np.array(camera_description.front_facing_axis) * camera_min_distance
+ np.array(camera_pose_in_camera_frame.position_as_list()))
- start_pose = Pose(start_position.tolist(), camera_pose_in_camera_frame.orientation_as_list(), camera_frame)
+ start_pose = Pose(start_position.tolist(), camera_pose_in_camera_frame.orientation_as_list(), "camera_pose")
return self.local_transformer.transform_pose(start_pose, "map")
def get_camera_rays_end_positions(self, camera_description: 'CameraDescription', camera_frame: str,
@@ -292,7 +397,7 @@ def transform_points_from_camera_frame_to_world_frame(camera_pose: Pose, camera_
:param points: The points to transform.
:return: The transformed points.
"""
- cam_to_world_transform = camera_pose.to_transform(camera_frame)
+ cam_to_world_transform = camera_pose.to_transform("camera_pose")
return cam_to_world_transform.apply_transform_to_array_of_points(points)
@staticmethod
diff --git a/src/pycram/validation/goal_validator.py b/src/pycram/validation/goal_validator.py
index 66756ae52..ebb03f745 100644
--- a/src/pycram/validation/goal_validator.py
+++ b/src/pycram/validation/goal_validator.py
@@ -3,10 +3,10 @@
import numpy as np
from typing_extensions import Any, Callable, Optional, Union, Iterable, Dict, TYPE_CHECKING, Tuple
-from ..datastructures.enums import JointType
from .error_checkers import ErrorChecker, PoseErrorChecker, PositionErrorChecker, \
OrientationErrorChecker, SingleValueErrorChecker
-from ..ros.logging import logerr, logwarn
+from ..datastructures.enums import JointType
+from ..ros.logging import logerr
if TYPE_CHECKING:
from ..datastructures.world import World
@@ -90,7 +90,7 @@ def wait_until_goal_is_achieved(self, max_wait_time: Optional[float] = 2,
logerr(msg)
raise TimeoutError(msg)
else:
- logwarn(msg)
+ # logwarn(msg)
break
current = self.current_value
self.reset()
@@ -167,7 +167,11 @@ def current_value(self) -> Any:
if self.current_value_getter_input is not None:
return self.current_value_getter(self.current_value_getter_input)
else:
- return self.current_value_getter()
+ try:
+ return self.current_value_getter()
+ except Exception as e:
+ logerr(f"Error while getting current value: {e}")
+ return None
@property
def current_error(self) -> np.ndarray:
@@ -222,6 +226,8 @@ def relative_initial_error(self) -> np.ndarray:
"""
The relative initial error (relative to the acceptable error).
"""
+ if self.initial_error is None:
+ self.update_initial_error(self.goal_value)
return np.maximum(self.initial_error, 1e-3)
def get_relative_error(self, error: Any, threshold: Optional[float] = 1e-3) -> np.ndarray:
@@ -419,7 +425,7 @@ def register_goal(self, goal_value: Any, joint_type: JointType,
:param acceptable_error: The acceptable error.
"""
if acceptable_error is None:
- self.error_checker.acceptable_error = self.acceptable_orientation_error if joint_type == JointType.REVOLUTE\
+ self.error_checker.acceptable_error = self.acceptable_orientation_error if joint_type == JointType.REVOLUTE \
else self.acceptable_position_error
super().register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error)
@@ -468,13 +474,20 @@ def validate_object_pose(pose_setter_func):
def wrapper(world: 'World', obj: 'Object', pose: 'Pose'):
- world.pose_goal_validator.register_goal(pose, obj)
+ if not world.current_world.conf.validate_goals:
+ return pose_setter_func(world, obj, pose)
+
+ if obj is None:
+ logerr("Object should not be None")
+ return False
+ pose_goal_validator = PoseGoalValidator(world.get_object_pose, world.conf.get_pose_tolerance(),
+ world.conf.acceptable_percentage_of_goal)
+ pose_goal_validator.register_goal(pose, obj)
if not pose_setter_func(world, obj, pose):
- world.pose_goal_validator.reset()
return False
- world.pose_goal_validator.wait_until_goal_is_achieved()
+ pose_goal_validator.wait_until_goal_is_achieved()
return True
return wrapper
@@ -489,14 +502,19 @@ def validate_multiple_object_poses(pose_setter_func):
def wrapper(world: 'World', object_poses: Dict['Object', 'Pose']):
- world.multi_pose_goal_validator.register_goal(list(object_poses.values()),
- list(object_poses.keys()))
+ if not world.current_world.conf.validate_goals:
+ return pose_setter_func(world, object_poses)
+
+ multi_pose_goal_validator = MultiPoseGoalValidator(
+ lambda x: list(world.get_multiple_object_poses(x).values()),
+ world.conf.get_pose_tolerance(), world.conf.acceptable_percentage_of_goal)
+ multi_pose_goal_validator.register_goal(list(object_poses.values()),
+ list(object_poses.keys()))
if not pose_setter_func(world, object_poses):
- world.multi_pose_goal_validator.reset()
return False
- world.multi_pose_goal_validator.wait_until_goal_is_achieved()
+ multi_pose_goal_validator.wait_until_goal_is_achieved()
return True
return wrapper
@@ -511,14 +529,22 @@ def validate_joint_position(position_setter_func):
def wrapper(world: 'World', joint: 'Joint', position: float):
+ if not world.current_world.conf.validate_goals:
+ return position_setter_func(world, joint, position)
+
+ joint_position_goal_validator = JointPositionGoalValidator(
+ world.get_joint_position,
+ acceptable_revolute_joint_position_error=world.conf.revolute_joint_position_tolerance,
+ acceptable_prismatic_joint_position_error=world.conf.prismatic_joint_position_tolerance,
+ acceptable_percentage_of_goal_achieved=world.conf.acceptable_percentage_of_goal)
+
joint_type = joint.type
- world.joint_position_goal_validator.register_goal(position, joint_type, joint)
+ joint_position_goal_validator.register_goal(position, joint_type, joint)
if not position_setter_func(world, joint, position):
- world.joint_position_goal_validator.reset()
return False
- world.joint_position_goal_validator.wait_until_goal_is_achieved()
+ joint_position_goal_validator.wait_until_goal_is_achieved()
return True
return wrapper
@@ -535,16 +561,26 @@ def validate_multiple_joint_positions(position_setter_func):
"""
def wrapper(world: 'World', joint_positions: Dict['Joint', float]):
+ if not world.current_world.conf.validate_goals:
+ return position_setter_func(world, joint_positions)
joint_positions_to_validate = {joint: position for joint, position in joint_positions.items()
if not joint.is_virtual}
+ if not joint_positions_to_validate:
+ return position_setter_func(world, joint_positions)
+
+ multi_joint_position_goal_validator = MultiJointPositionGoalValidator(
+ lambda x: list(world.get_multiple_joint_positions(x).values()),
+ acceptable_revolute_joint_position_error=world.conf.revolute_joint_position_tolerance,
+ acceptable_prismatic_joint_position_error=world.conf.prismatic_joint_position_tolerance,
+ acceptable_percentage_of_goal_achieved=world.conf.acceptable_percentage_of_goal)
+
joint_types = [joint.type for joint in joint_positions_to_validate.keys()]
- world.multi_joint_position_goal_validator.register_goal(list(joint_positions_to_validate.values()), joint_types,
- list(joint_positions_to_validate.keys()))
+ multi_joint_position_goal_validator.register_goal(list(joint_positions_to_validate.values()), joint_types,
+ list(joint_positions_to_validate.keys()))
if not position_setter_func(world, joint_positions):
- world.multi_joint_position_goal_validator.reset()
return False
- world.multi_joint_position_goal_validator.wait_until_goal_is_achieved()
+ multi_joint_position_goal_validator.wait_until_goal_is_achieved()
return True
return wrapper
diff --git a/src/pycram/world_concepts/constraints.py b/src/pycram/world_concepts/constraints.py
index 8de36bda1..14449d302 100644
--- a/src/pycram/world_concepts/constraints.py
+++ b/src/pycram/world_concepts/constraints.py
@@ -253,7 +253,7 @@ def remove_constraint_if_exists(self) -> None:
"""
Remove the constraint between the parent and the child links if one exists.
"""
- if self.child_link in self.parent_link.constraint_ids:
+ if self.child_link in self.parent_link.constraint_ids.keys():
self.parent_link.remove_constraint_with_link(self.child_link)
def get_inverse(self) -> 'Attachment':
diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py
index 099439951..034385126 100644
--- a/src/pycram/world_concepts/world_object.py
+++ b/src/pycram/world_concepts/world_object.py
@@ -11,9 +11,10 @@
from trimesh.parent import Geometry3D
from typing_extensions import Type, Optional, Dict, Tuple, List, Union
+import pycrap
from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState,
AxisAlignedBoundingBox, VisualShape, ClosestPointsList,
- ContactPointsList, RotatedBoundingBox)
+ ContactPointsList, RotatedBoundingBox, VirtualJoint)
from ..datastructures.enums import ObjectType, JointType
from ..datastructures.pose import Pose, Transform
from ..datastructures.world import World
@@ -167,21 +168,22 @@ def set_mobile_robot_pose(self, pose: Pose) -> None:
:param pose: The target pose.
"""
goal = self.get_mobile_base_joint_goal(pose)
+ goal = {vj.name: pos for vj, pos in goal.items()}
self.set_multiple_joint_positions(goal)
- def get_mobile_base_joint_goal(self, pose: Pose) -> Dict[str, float]:
+ def get_mobile_base_joint_goal(self, pose: Pose) -> Dict[VirtualJoint, float]:
"""
Get the goal for the mobile base joints of a mobile robot to reach a target pose.
:param pose: The target pose.
:return: The goal for the mobile base joints.
"""
- target_translation, target_angle = self.get_mobile_base_pose_difference(pose)
+ # target_translation, target_angle = self.get_mobile_base_pose_difference(pose)
# Get the joints of the base link
mobile_base_joints = self.world.get_robot_mobile_base_joints()
- return {mobile_base_joints.translation_x: target_translation.x,
- mobile_base_joints.translation_y: target_translation.y,
- mobile_base_joints.angular_z: target_angle}
+ return {mobile_base_joints.translation_x: pose.position.x,
+ mobile_base_joints.translation_y: pose.position.y,
+ mobile_base_joints.angular_z: pose.z_angle}
def get_mobile_base_pose_difference(self, pose: Pose) -> Tuple[Point, float]:
"""
@@ -444,7 +446,8 @@ def joint_names(self) -> List[str]:
"""
The names of the joints as a list.
"""
- return self.world.get_object_joint_names(self)
+ joint_names = self.world.get_object_joint_names(self)
+ return joint_names
def get_link(self, link_name: str) -> ObjectDescription.Link:
"""
@@ -509,14 +512,15 @@ def get_link_tf_frame(self, link_name: str) -> str:
"""
return self.links[link_name].tf_frame
- def get_link_axis_aligned_bounding_box(self, link_name: str) -> AxisAlignedBoundingBox:
+ def get_link_axis_aligned_bounding_box(self, link_name: str, transform_to_link_pose: bool = True) -> AxisAlignedBoundingBox:
"""
Return the axis aligned bounding box of the link with the given name.
:param link_name: The name of the link.
+ :param transform_to_link_pose: If True, the bounding box will be transformed to fit link pose.
:return: The axis aligned bounding box of the link.
"""
- return self.links[link_name].get_axis_aligned_bounding_box()
+ return self.links[link_name].get_axis_aligned_bounding_box(transform_to_link_pose)
def get_transform_between_links(self, from_link: str, to_link: str) -> Transform:
"""
@@ -621,13 +625,23 @@ def reset(self, remove_saved_states=False) -> None:
if remove_saved_states:
self.remove_saved_states()
- def has_type_environment(self) -> bool:
+ @property
+ def is_an_environment(self) -> bool:
"""
Check if the object is of type environment.
:return: True if the object is of type environment, False otherwise.
"""
- return self.obj_type == ObjectType.ENVIRONMENT
+ return issubclass(self.obj_type, pycrap.Location) or issubclass(self.obj_type, pycrap.Floor)
+
+ @property
+ def is_a_robot(self) -> bool:
+ """
+ Check if the object is a robot.
+
+ :return: True if the object is a robot, False otherwise.
+ """
+ return issubclass(self.obj_type, pycrap.Robot)
def attach(self,
child_object: Object,
@@ -1135,6 +1149,7 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None:
:param joint_name: The name of the joint
:param joint_position: The target pose for this joint
"""
+ self.clip_joint_positions_to_limits({joint_name: joint_position})
if self.world.reset_joint_position(self.joints[joint_name], joint_position):
self._update_on_joint_position_change()
@@ -1154,6 +1169,18 @@ def set_multiple_joint_positions(self, joint_positions: Dict[str, float]) -> Non
if self.world.set_multiple_joint_positions(joint_positions):
self._update_on_joint_position_change()
+ def clip_joint_positions_to_limits(self, joint_positions: Dict[str, float]) -> Dict[str, float]:
+ """
+ Clip the given joint positions to the joint limits.
+
+ :param joint_positions: A dictionary with the joint names as keys and the target positions as values.
+ :return: A dictionary with the joint names as keys and the clipped positions as values.
+ """
+ return {joint_name: np.clip(joint_position, self.joints[joint_name].lower_limit,
+ self.joints[joint_name].upper_limit)
+ if self.joints[joint_name].has_limits else joint_position
+ for joint_name, joint_position in joint_positions.items()}
+
def _update_on_joint_position_change(self):
self.update_pose()
self._update_all_links_poses()
@@ -1376,14 +1403,15 @@ def links_colors(self) -> Dict[str, Color]:
"""
return self.world.get_colors_of_object_links(self)
- def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox:
+ def get_axis_aligned_bounding_box(self, transform_to_object_pose: bool = True) -> AxisAlignedBoundingBox:
"""
Return the axis aligned bounding box of this object.
+ :param transform_to_object_pose: If True, the bounding box will be transformed to fit object pose.
:return: The axis aligned bounding box of this object.
"""
if self.has_one_link:
- return self.root_link.get_axis_aligned_bounding_box()
+ return self.root_link.get_axis_aligned_bounding_box(transform_to_object_pose)
else:
return self.world.get_object_axis_aligned_bounding_box(self)
@@ -1453,7 +1481,7 @@ def copy_to_world(self, world: World) -> Object:
:param world: The world to which the object should be copied.
:return: The copied object in the given world.
"""
- obj = Object(self.name, self.obj_type, self.path, self.description, self.get_pose(),
+ obj = Object(self.name, self.obj_type, self.path, self.description, self.original_pose,
world, self.color)
return obj
diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py
index 8fd0501b9..b87e7a073 100644
--- a/src/pycram/world_reasoning.py
+++ b/src/pycram/world_reasoning.py
@@ -6,8 +6,9 @@
from .datastructures.world import World, UseProspectionWorld
from .external_interfaces.ik import try_to_reach, try_to_reach_with_grasp
from .robot_description import RobotDescription
+from .ros.logging import logdebug
from .utils import RayTestUtils
-from .world_concepts.world_object import Object
+from .world_concepts.world_object import Object, Link
from .config import world_conf as conf
@@ -36,7 +37,7 @@ def stable(obj: Object) -> bool:
def contact(
object1: Object,
object2: Object,
- return_links: bool = False) -> Union[bool, Tuple[bool, List]]:
+ return_links: bool = False) -> Union[bool, Tuple[bool, List[Tuple[Link, Link]]]]:
"""
Checks if two objects are in contact or not. If the links should be returned then the output will also contain a
list of tuples where the first element is the link name of 'object1' and the second element is the link name of
@@ -62,6 +63,56 @@ def contact(
return objects_are_in_contact
+def prospect_robot_contact(robot: Object, pose: Pose,
+ ignore_collision_with: Optional[List[Object]] = None) -> bool:
+ """
+ Check if the robot collides with any object in the world at the given pose.
+
+ :param robot: The robot object
+ :param pose: The pose to check for collision
+ :param ignore_collision_with: A list of objects to ignore collision with
+ :return: True if the robot collides with any object, False otherwise
+ """
+ with UseProspectionWorld():
+ prospection_robot = World.current_world.get_prospection_object_for_object(robot)
+ prospection_robot.set_pose(pose)
+ floor = prospection_robot.world.get_object_by_name("floor")
+ ignore_collision_with = [] if ignore_collision_with is None else ignore_collision_with
+ ignore = [o.name for o in ignore_collision_with]
+ for obj in prospection_robot.world.objects:
+ if obj.name in ([prospection_robot.name, floor.name] + ignore):
+ continue
+ in_contact, contact_links = contact(prospection_robot, obj, return_links=True)
+ if in_contact and not is_held_object(prospection_robot, obj, [links[0] for links in contact_links]):
+ logdebug(f"Robot is in contact with {obj.name} in prospection: {obj.world.is_prospection_world}"
+ f"at position {pose.position_as_list()} and z_angle {pose.z_angle}")
+ return True
+ logdebug(f"Robot is not in contact with {obj.name} in prospection: {obj.world.is_prospection_world}"
+ f"at position {pose.position_as_list()} and z_angle {pose.z_angle}")
+ return False
+
+
+def is_held_object(robot: Object, obj: Object, robot_contact_links: List[Link]) -> bool:
+ """
+ Check if the object is picked by the robot.
+
+ :param robot: The robot object
+ :param obj: The object to check if it is picked
+ :param robot_contact_links: The links of the robot that are in contact with the object
+ :return: True if the object is picked by the robot, False otherwise
+ """
+ picked_object = False
+ if obj in robot.attachments:
+ arm_chains = RobotDescription.current_robot_description.get_manipulator_chains()
+ for chain in arm_chains:
+ gripper_links = chain.end_effector.links
+ if (robot.attachments[obj].parent_link.name in gripper_links
+ and all(link.name in gripper_links for link in robot_contact_links)):
+ picked_object = True
+ continue
+ return picked_object
+
+
def get_visible_objects(
camera_pose: Pose,
front_facing_axis: Optional[List[float]] = None,
@@ -77,7 +128,7 @@ def get_visible_objects(
if front_facing_axis is None:
front_facing_axis = RobotDescription.current_robot_description.get_default_camera().front_facing_axis
- camera_frame = RobotDescription.current_robot_description.get_camera_frame()
+ camera_frame = RobotDescription.current_robot_description.get_camera_frame(World.robot.name)
world_to_cam = camera_pose.to_transform(camera_frame)
cam_to_point = Transform(list(np.multiply(front_facing_axis, 2)), [0, 0, 0, 1], camera_frame,
diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py
index c3a3b5f86..dee26a369 100755
--- a/src/pycram/worlds/bullet_world.py
+++ b/src/pycram/worlds/bullet_world.py
@@ -8,7 +8,7 @@
import numpy as np
import pycram_bullet as p
from geometry_msgs.msg import Point
-from typing_extensions import List, Optional, Dict, Any
+from typing_extensions import List, Optional, Dict, Any, Callable
from pycrap import Floor
from ..datastructures.dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape, \
@@ -18,7 +18,7 @@
from ..datastructures.world import World
from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription
from ..object_descriptors.urdf import ObjectDescription
-from ..ros.logging import loginfo
+from ..ros.logging import logwarn, loginfo
from ..validation.goal_validator import (validate_multiple_joint_positions, validate_joint_position,
validate_object_pose, validate_multiple_object_poses)
from ..world_concepts.constraints import Constraint
@@ -36,7 +36,8 @@ class is the main interface to the Bullet Physics Engine and should be used to s
manipulate the Bullet World.
"""
- def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: bool = False):
+ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: bool = False,
+ use_multiverse_for_real_world_simulation: bool = False):
"""
Creates a new simulation, the type decides of the simulation should be a rendered window or just run in the
background. There can only be one rendered simulation.
@@ -44,9 +45,13 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo
:param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default is "GUI"
:param is_prospection_world: For internal usage, decides if this BulletWorld should be used as a shadow world.
+ :param use_multiverse_for_real_world_simulation: Whether to use the Multiverse for real world simulation.
"""
super().__init__(mode=mode, is_prospection_world=is_prospection_world)
+ if use_multiverse_for_real_world_simulation:
+ self.add_multiverse_resources()
+
# This disables file caching from PyBullet, since this would also cache
# files that can not be loaded
p.setPhysicsEngineParameter(enableFileCaching=0)
@@ -62,6 +67,17 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo
_ = Object("floor", Floor, "plane.urdf",
world=self)
+ def add_multiverse_resources(self):
+ """
+ Add the Multiverse resources to the start of the data directories of the BulletWorld such they are searched
+ first for the resources because the pycram objects need to have the same description as the Multiverse objects.
+ """
+ try:
+ from .multiverse import Multiverse
+ Multiverse.make_sure_multiverse_resources_are_added(self.conf.clear_cache_at_start)
+ except ImportError:
+ logwarn("Multiverse is not installed, please install it to use the Multiverse.")
+
def _init_world(self, mode: WorldMode):
self._gui_thread: Gui = Gui(self, mode)
self._gui_thread.start()
@@ -136,7 +152,7 @@ def add_constraint(self, constraint: Constraint) -> int:
def remove_constraint(self, constraint_id):
p.removeConstraint(constraint_id, physicsClientId=self.id)
- def get_joint_position(self, joint: ObjectDescription.Joint) -> float:
+ def _get_joint_position(self, joint: ObjectDescription.Joint) -> float:
return p.getJointState(joint.object_id, joint.id, physicsClientId=self.id)[0]
def get_object_joint_names(self, obj: Object) -> List[str]:
@@ -217,17 +233,17 @@ def parse_points_list_to_args(self, point: List) -> Dict:
"lateral_friction_2": LateralFriction(point[12], point[13])}
@validate_multiple_joint_positions
- def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool:
+ def _set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool:
for joint, joint_position in joint_positions.items():
self.reset_joint_position(joint, joint_position)
return True
@validate_joint_position
- def reset_joint_position(self, joint: Joint, joint_position: float) -> bool:
+ def _reset_joint_position(self, joint: Joint, joint_position: float) -> bool:
p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id)
return True
- def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]:
+ def _get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]:
return {joint.name: self.get_joint_position(joint) for joint in joints}
@validate_multiple_object_poses
@@ -245,7 +261,11 @@ def _set_object_pose_by_id(self, obj_id: int, pose: Pose) -> bool:
physicsClientId=self.id)
return True
- def step(self):
+ def step(self, func: Optional[Callable[[], None]] = None, step_seconds: Optional[float] = None) -> None:
+ if func is not None:
+ logwarn("The BulletWorld step function does not support a function argument.")
+ if step_seconds is not None:
+ logwarn("The BulletWorld step function does not support a step_seconds argument.")
p.stepSimulation(physicsClientId=self.id)
def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]:
diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py
index 6cbeb0d78..5e17c4899 100644
--- a/src/pycram/worlds/multiverse.py
+++ b/src/pycram/worlds/multiverse.py
@@ -1,23 +1,29 @@
import logging
+import os
+import shutil
+from pathlib import Path
from time import sleep
import numpy as np
from tf.transformations import quaternion_matrix
-from typing_extensions import List, Dict, Optional, Union, Tuple
+from typing_extensions import List, Dict, Optional, Union, Tuple, Callable, Type
+import pycrap
+from pycrap import PhysicalObject
from .multiverse_communication.client_manager import MultiverseClientManager
from .multiverse_communication.clients import MultiverseController, MultiverseReader, MultiverseWriter, MultiverseAPI
from ..config.multiverse_conf import MultiverseConfig
-from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint
-from ..datastructures.enums import WorldMode, JointType, ObjectType, MultiverseBodyProperty, MultiverseJointPosition, \
+from ..datastructures.dataclasses import Color, ContactPointsList, ContactPoint
+from ..datastructures.enums import WorldMode, JointType, MultiverseBodyProperty, MultiverseJointPosition, \
MultiverseJointCMD
from ..datastructures.pose import Pose
from ..datastructures.world import World
from ..description import Link, Joint
-from ..object_descriptors.mjcf import ObjectDescription as MJCF
+from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription
+from ..object_descriptors.mjcf import ObjectDescription as MJCF, PrimitiveObjectFactory
from ..robot_description import RobotDescription
-from ..ros.logging import logwarn, logerr
-from ..utils import RayTestUtils, wxyz_to_xyzw, xyzw_to_wxyz
+from ..ros.logging import logwarn
+from ..utils import RayTestUtils, wxyz_to_xyzw, xyzw_to_wxyz, adjust_camera_pose_based_on_target
from ..validation.goal_validator import validate_object_pose, validate_multiple_joint_positions, \
validate_joint_position, validate_multiple_object_poses
from ..world_concepts.constraints import Constraint
@@ -44,55 +50,41 @@ class Multiverse(World):
A flag to check if the multiverse resources have been added.
"""
- simulation: Optional[str] = None
- """
- The simulation name to be used in the Multiverse world (this is the name defined in
- the multiverse configuration file).
- """
-
Object.extension_to_description_type[MJCF.get_file_extension()] = MJCF
"""
Add the MJCF description extension to the extension to description type mapping for the objects.
"""
- def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT,
- is_prospection: Optional[bool] = False,
- simulation_name: str = "pycram_test",
+ def __init__(self, is_prospection_world: Optional[bool] = False,
clear_cache: bool = False):
"""
Initialize the Multiverse Socket and the PyCram World.
- :param mode: The mode of the world (DIRECT or GUI).
- :param is_prospection: Whether the world is prospection or not.
- :param simulation_name: The name of the simulation.
+ :param is_prospection_world: Whether the world is prospection or not.
:param clear_cache: Whether to clear the cache or not.
"""
self.latest_save_id: Optional[int] = None
self.saved_simulator_states: Dict = {}
- self._make_sure_multiverse_resources_are_added(clear_cache=clear_cache)
-
- if Multiverse.simulation is None:
- if simulation_name is None:
- logging.error("Simulation name not provided")
- raise ValueError("Simulation name not provided")
- Multiverse.simulation = simulation_name
+ self.make_sure_multiverse_resources_are_added(clear_cache=clear_cache)
- self.simulation = (self.conf.prospection_world_prefix if is_prospection else "") + Multiverse.simulation
+ self.simulation = self.conf.prospection_world_prefix if is_prospection_world else "belief_state"
self.client_manager = MultiverseClientManager(self.conf.simulation_wait_time_factor)
- self._init_clients(is_prospection=is_prospection)
+ self._init_clients(is_prospection=is_prospection_world)
- World.__init__(self, mode, is_prospection)
+ World.__init__(self, WorldMode.DIRECT, is_prospection_world)
self._init_constraint_and_object_id_name_map_collections()
self.ray_test_utils = RayTestUtils(self.ray_test_batch, self.object_id_to_name)
+ self.is_paused: bool = False
+
if not self.is_prospection_world:
self._spawn_floor()
if self.conf.use_static_mode:
- self.api_requester.pause_simulation()
+ self.pause_simulation()
def _init_clients(self, is_prospection: bool = False):
"""
@@ -124,18 +116,19 @@ def _init_constraint_and_object_id_name_map_collections(self):
def _init_world(self, mode: WorldMode):
pass
- def _make_sure_multiverse_resources_are_added(self, clear_cache: bool = False):
+ @classmethod
+ def make_sure_multiverse_resources_are_added(cls, clear_cache: bool = False) -> None:
"""
Add the multiverse resources to the pycram world resources, and change the data directory and cache manager.
:param clear_cache: Whether to clear the cache or not.
"""
- if not self.added_multiverse_resources:
+ if not cls.added_multiverse_resources:
if clear_cache:
World.cache_manager.clear_cache()
- World.add_resource_path(self.conf.resources_path, prepend=True)
- World.change_cache_dir_path(self.conf.resources_path)
- self.added_multiverse_resources = True
+ World.add_resource_path(cls.conf.resources_path, prepend=True)
+ World.change_cache_dir_path(cls.conf.resources_path)
+ cls.added_multiverse_resources = True
def remove_multiverse_resources(self):
"""
@@ -150,9 +143,31 @@ def _spawn_floor(self):
"""
Spawn the plane in the simulator.
"""
- self.floor = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf",
+ self.floor = Object("floor", pycrap.Floor, "plane.urdf",
world=self)
+ def pause_simulation(self) -> None:
+ """
+ Pause the simulation.
+ """
+ self.api_requester.pause_simulation()
+ self.is_paused = True
+
+ def unpause_simulation(self) -> None:
+ """
+ Unpause the simulation.
+ """
+ self.api_requester.unpause_simulation()
+ self.is_paused = False
+
+ def load_generic_object_and_get_id(self, description: GenericObjectDescription,
+ pose: Optional[Pose] = None) -> int:
+ save_path = os.path.join(self.cache_manager.cache_dir, description.name + ".xml")
+ object_factory = PrimitiveObjectFactory(description.name, description.links[0].geometry, save_path)
+ object_factory.build_shape()
+ object_factory.export_to_mjcf(save_path)
+ return self.load_object_and_get_id(description.name, pose, pycrap.PhysicalObject)
+
def get_images_for_target(self, target_pose: Pose,
cam_pose: Pose,
size: int = 256,
@@ -163,8 +178,9 @@ def get_images_for_target(self, target_pose: Pose,
Uses ray test to get the images for the target object. (target_pose is currently not used)
"""
camera_description = RobotDescription.current_robot_description.get_default_camera()
- camera_frame = RobotDescription.current_robot_description.get_camera_frame()
- return self.ray_test_utils.get_images_for_target(cam_pose, camera_description, camera_frame,
+ camera_frame = RobotDescription.current_robot_description.get_camera_frame(World.robot.name)
+ adjusted_cam_pose = adjust_camera_pose_based_on_target(cam_pose, target_pose, camera_description)
+ return self.ray_test_utils.get_images_for_target(adjusted_cam_pose, camera_description, camera_frame,
size, camera_min_distance, camera_max_distance, plot)
@staticmethod
@@ -188,13 +204,14 @@ def spawn_robot_with_controller(self, name: str, pose: Pose) -> None:
for joint_name, actuator_name in self.robot_joint_actuators.items()
}
self.joint_controller.init_controller(actuator_joint_commands)
- self.writer.spawn_robot_with_actuators(name, pose.position_as_list(),
- xyzw_to_wxyz(pose.orientation_as_list()),
- actuator_joint_commands)
+ self.writer.spawn_robot_with_actuators(name, actuator_joint_commands)
+ if not pose.almost_equal(Pose()):
+ goal = self.robot.get_mobile_base_joint_goal(pose)
+ self.set_multiple_joint_positions(goal)
def load_object_and_get_id(self, name: Optional[str] = None,
pose: Optional[Pose] = None,
- obj_type: Optional[ObjectType] = None) -> int:
+ obj_type: Optional[Type[PhysicalObject]] = None) -> int:
"""
Spawn the object in the simulator and return the object id. Object name has to be unique and has to be same as
the name of the object in the description file.
@@ -208,12 +225,13 @@ def load_object_and_get_id(self, name: Optional[str] = None,
# Do not spawn objects with type environment as they should be already present in the simulator through the
# multiverse description file (.muv file).
- if not obj_type == ObjectType.ENVIRONMENT:
+ obj_type = pycrap.PhysicalObject if obj_type is None else obj_type
+ if not (issubclass(obj_type, pycrap.Location) or issubclass(obj_type, pycrap.Floor)):
self.spawn_object(name, obj_type, pose)
return self._update_object_id_name_maps_and_get_latest_id(name)
- def spawn_object(self, name: str, object_type: ObjectType, pose: Pose) -> None:
+ def spawn_object(self, name: str, object_type: Type[PhysicalObject], pose: Pose) -> None:
"""
Spawn the object in the simulator.
@@ -221,11 +239,24 @@ def spawn_object(self, name: str, object_type: ObjectType, pose: Pose) -> None:
:param object_type: The type of the object.
:param pose: The pose of the object.
"""
- if object_type == ObjectType.ROBOT and self.conf.use_controller:
- self.spawn_robot_with_controller(name, pose)
+ if issubclass(object_type, pycrap.Robot):
+ if self.conf.use_controller:
+ self.spawn_robot_with_controller(name, pose)
+ else:
+ self.spawn_robot(name, pose)
else:
self._set_body_pose(name, pose)
+ def spawn_robot(self, name: str, pose: Pose) -> None:
+ """
+ Spawn the robot in the simulator.
+
+ :param name: The name of the robot.
+ :param pose: The pose of the robot.
+ """
+ self._set_body_pose(name, Pose())
+ self.robot.set_mobile_robot_pose(pose)
+
def _update_object_id_name_maps_and_get_latest_id(self, name: str) -> int:
"""
Update the object id name maps and return the latest object id.
@@ -257,8 +288,8 @@ def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[fl
return self.reader.get_multiple_body_orientations([link.name for link in links])
@validate_joint_position
- def reset_joint_position(self, joint: Joint, joint_position: float) -> bool:
- if self.conf.use_controller and self.joint_has_actuator(joint):
+ def _reset_joint_position(self, joint: Joint, joint_position: float) -> bool:
+ if not self.is_paused and self.conf.use_controller and self.joint_has_actuator(joint):
self._reset_joint_position_using_controller(joint, joint_position)
else:
self._set_multiple_joint_positions_without_controller({joint: joint_position})
@@ -278,7 +309,7 @@ def _reset_joint_position_using_controller(self, joint: Joint, joint_position: f
return True
@validate_multiple_joint_positions
- def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool:
+ def _set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool:
"""
Set the positions of multiple joints in the simulator. Also check if the joint is controlled by an actuator
and use the controller to set the joint position if the joint is controlled.
@@ -288,7 +319,7 @@ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> b
errors, but not necessarily that the joint positions are set to the specified values).
"""
- if self.conf.use_controller:
+ if not self.is_paused and self.conf.use_controller:
controlled_joints = self.get_controlled_joints(list(joint_positions.keys()))
if len(controlled_joints) > 0:
controlled_joint_positions = {joint: joint_positions[joint] for joint in controlled_joints}
@@ -332,13 +363,13 @@ def _set_multiple_joint_positions_using_controller(self, joint_positions: Dict[J
self.joint_controller.send_multiple_body_data_to_server(controlled_joints_data)
return True
- def get_joint_position(self, joint: Joint) -> Optional[float]:
+ def _get_joint_position(self, joint: Joint) -> Optional[float]:
joint_position_name = self.get_joint_position_name(joint)
data = self.reader.get_body_data(joint.name, [joint_position_name])
if data is not None:
return data[joint_position_name.value][0]
- def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str, float]]:
+ def _get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str, float]]:
joint_names = [joint.name for joint in joints]
data = self.reader.get_multiple_body_data(joint_names, {joint.name: [self.get_joint_position_name(joint)]
for joint in joints})
@@ -361,7 +392,7 @@ def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]:
return self._get_multiple_body_poses([link.name for link in links])
def get_object_pose(self, obj: Object) -> Pose:
- if obj.has_type_environment():
+ if obj.is_an_environment:
return Pose()
return self._get_body_pose(obj.name)
@@ -373,17 +404,17 @@ def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]:
:param objects: The list of objects.
:return: The dictionary of object names and poses.
"""
- non_env_objects = [obj for obj in objects if not obj.has_type_environment()]
+ non_env_objects = [obj for obj in objects if not obj.is_an_environment]
all_poses = self._get_multiple_body_poses([obj.name for obj in non_env_objects])
- all_poses.update({obj.name: Pose() for obj in objects if obj.has_type_environment()})
+ all_poses.update({obj.name: Pose() for obj in objects if obj.is_an_environment})
return all_poses
@validate_object_pose
def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool:
- if obj.has_type_environment():
+ if obj.is_an_environment:
return False
- if (obj.obj_type == ObjectType.ROBOT and
+ if (obj.is_a_robot and
RobotDescription.current_robot_description.virtual_mobile_base_joints is not None):
obj.set_mobile_robot_pose(pose)
else:
@@ -399,12 +430,13 @@ def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None
:param objects: The dictionary of objects and poses.
"""
for obj in objects.keys():
- if (obj.obj_type == ObjectType.ROBOT and
+ if (obj.is_a_robot and
RobotDescription.current_robot_description.virtual_mobile_base_joints is not None):
obj.set_mobile_robot_pose(objects[obj])
- objects = {obj: pose for obj, pose in objects.items() if obj.obj_type not in [ObjectType.ENVIRONMENT,
- ObjectType.ROBOT]}
- self._set_multiple_body_poses({obj.name: pose for obj, pose in objects.items()})
+ objects = {obj: pose for obj, pose in objects.items()
+ if not obj.is_a_robot and not obj.is_an_environment}
+ if len(objects) > 0:
+ self._set_multiple_body_poses({obj.name: pose for obj, pose in objects.items()})
def _set_body_pose(self, body_name: str, pose: Pose) -> None:
"""
@@ -477,7 +509,7 @@ def _remove_visual_object(self, obj_id: int) -> bool:
return False
def remove_object_from_simulator(self, obj: Object) -> bool:
- if obj.obj_type != ObjectType.ENVIRONMENT:
+ if not obj.is_an_environment:
self.writer.remove_body(obj.name)
return True
logwarn("Cannot remove environment objects")
@@ -512,36 +544,52 @@ def remove_constraint(self, constraint_id) -> None:
def perform_collision_detection(self) -> None:
...
- def get_object_contact_points(self, obj: Object) -> ContactPointsList:
+ def get_object_contact_points(self, obj: Object, ignore_attached_objects: bool = True) -> ContactPointsList:
"""
Note: Currently Multiverse only gets one contact point per contact objects.
- """
- multiverse_contact_points = self.api_requester.get_contact_points(obj)
+
+ :param obj: The object.
+ :param ignore_attached_objects: Whether to ignore the attached objects or not.
+ :return: The contact points of the object.
+ """
+ multiverse_contact_points = self.api_requester.get_contact_points(obj.name)
+ if ignore_attached_objects and len(obj.attachments) > 0:
+ attached_objects_link_names = []
+ for att_obj in obj.attachments.keys():
+ attached_objects_link_names.extend(att_obj.link_names)
+ multiverse_contact_points = [mcp for mcp in multiverse_contact_points if
+ mcp.body_1 not in attached_objects_link_names]
contact_points = ContactPointsList([])
- body_link = None
- for point in multiverse_contact_points:
- if point.body_name == "world":
- point.body_name = "floor"
- body_object = self.get_object_by_name(point.body_name)
- if body_object is None:
- for obj in self.objects:
- for link in obj.links.values():
- if link.name == point.body_name:
- body_link = link
- break
- else:
- body_link = body_object.root_link
- if body_link is None:
- logging.error(f"Body link not found: {point.body_name}")
- raise ValueError(f"Body link not found: {point.body_name}")
- contact_points.append(ContactPoint(obj.root_link, body_link))
- contact_points[-1].force_x_in_world_frame = point.contact_force[0]
- contact_points[-1].force_y_in_world_frame = point.contact_force[1]
- contact_points[-1].force_z_in_world_frame = point.contact_force[2]
- contact_points[-1].normal_on_b = point.contact_force[2]
- contact_points[-1].normal_force = point.contact_force[2]
+ for mcp in multiverse_contact_points:
+ body_object, body_link = self.get_object_with_body_name(mcp.body_2)
+ obj_link = obj.links[mcp.body_1] if mcp.body_1 in obj.links.keys() else obj.root_link
+ contact_points.append(ContactPoint(obj_link, body_link))
+ contact_points[-1].normal_on_b = mcp.normal
+ contact_points[-1].position_on_b = mcp.position
return contact_points
+ def get_object_with_body_name(self, body_name: str) -> Tuple[Optional[Object], Optional[Link]]:
+ """
+ Get the object with the body name in the simulator, the body name can be the name of the object or the link.
+
+ :param body_name: The name of the body.
+ :return: The object if found otherwise None, and the link if the body name is a link.
+ """
+ if body_name == "world":
+ body_name = "floor"
+ body_object = self.get_object_by_name(body_name)
+ body_link = None
+ if body_object is None:
+ for obj in self.objects:
+ for link in obj.links.values():
+ if link.name == body_name:
+ body_link = link
+ body_object = obj
+ break
+ else:
+ body_link = body_object.root_link
+ return body_object, body_link
+
@staticmethod
def _get_normal_force_on_object_from_contact_force(obj: Object, contact_force: List[float]) -> float:
"""
@@ -599,14 +647,19 @@ def ray_test_batch(self, from_positions: List[List[float]],
else:
return results
- def step(self):
+ def step(self, func: Optional[Callable[[], None]] = None, step_seconds: Optional[float] = None) -> None:
"""
Perform a simulation step in the simulator, this is useful when use_static_mode is True.
+
+ :param func: A function to be called after the simulation step.
+ :param step_seconds: The number of seconds to step the simulation.
"""
if self.conf.use_static_mode:
- self.api_requester.unpause_simulation()
- sleep(self.simulation_time_step)
- self.api_requester.pause_simulation()
+ self.unpause_simulation()
+ if func is not None:
+ func()
+ sleep(self.simulation_time_step if step_seconds is None else step_seconds)
+ self.pause_simulation()
def save_physics_simulator_state(self, state_id: Optional[int] = None, use_same_id: bool = False) -> int:
if state_id is None:
@@ -617,7 +670,8 @@ def save_physics_simulator_state(self, state_id: Optional[int] = None, use_same_
return state_id
def remove_physics_simulator_state(self, state_id: int) -> None:
- self.saved_simulator_states.pop(state_id)
+ save_path = self.saved_simulator_states.pop(state_id)
+ shutil.rmtree(Path(save_path).parent)
def restore_physics_simulator_state(self, state_id: int) -> None:
self.api_requester.load(self.saved_simulator_states[state_id])
@@ -633,14 +687,6 @@ def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]:
logwarn("get_colors_of_object_links is not implemented in Multiverse")
return {}
- def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox:
- logerr("get_object_axis_aligned_bounding_box for multi-link objects is not implemented in Multiverse")
- raise NotImplementedError
-
- def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox:
- logerr("get_link_axis_aligned_bounding_box is not implemented in Multiverse")
- raise NotImplementedError
-
def set_realtime(self, real_time: bool) -> None:
logwarn("set_realtime is not implemented as an API in Multiverse, it is configured in the"
"multiverse configuration file (.muv file) as rtf_required where a value of 1 means real-time")
diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py
index b10959a90..e02ee92aa 100644
--- a/src/pycram/worlds/multiverse_communication/clients.py
+++ b/src/pycram/worlds/multiverse_communication/clients.py
@@ -2,17 +2,21 @@
import logging
import os
import threading
+from threading import RLock
+
from time import time, sleep
+from geometry_msgs.msg import Point
from typing_extensions import List, Dict, Tuple, Optional, Callable, Union
from .socket import MultiverseSocket, MultiverseMetaData
from ...config.multiverse_conf import MultiverseConfig as Conf
-from ...datastructures.dataclasses import RayResult, MultiverseContactPoint
+from ...datastructures.dataclasses import RayResult, MultiverseContactPoint, \
+ AxisAlignedBoundingBox
from ...datastructures.enums import (MultiverseAPIName as API, MultiverseBodyProperty as BodyProperty,
MultiverseProperty as Property)
from ...datastructures.pose import Pose
-from ...ros.logging import logwarn
+from ...failures import FailedAPIResponse
from ...utils import wxyz_to_xyzw
from ...world_concepts.constraints import Constraint
from ...world_concepts.world_object import Object, Link
@@ -32,9 +36,9 @@ def __init__(self, name: str, port: int, is_prospection_world: bool = False,
increase or decrease the wait time for the simulation.
"""
meta_data = MultiverseMetaData()
- meta_data.simulation_name = (Conf.prospection_world_prefix if is_prospection_world else "") + name
- meta_data.world_name = ((Conf.prospection_world_prefix if is_prospection_world else "")
- + meta_data.world_name)
+ meta_data.simulation_name = ((Conf.prospection_world_prefix if is_prospection_world else "belief_state")
+ + "_" + name)
+ meta_data.world_name = Conf.prospection_world_prefix if is_prospection_world else "belief_state"
self.is_prospection_world = is_prospection_world
super().__init__(port=str(port), meta_data=meta_data)
self.simulation_wait_time_factor = simulation_wait_time_factor
@@ -320,21 +324,20 @@ def __init__(self, name: str, port: int, simulation: Optional[str] = None,
"""
super().__init__(name, port, is_prospection_world, simulation_wait_time_factor=simulation_wait_time_factor)
self.simulation = simulation
+ self.lock = RLock()
- def spawn_robot_with_actuators(self, robot_name: str, position: List[float], orientation: List[float],
+ def spawn_robot_with_actuators(self, robot_name: str,
actuator_joint_commands: Optional[Dict[str, List[str]]] = None) -> None:
"""
Spawn the robot with controlled actuators in the simulation.
:param robot_name: The name of the robot.
- :param position: The position of the robot.
- :param orientation: The orientation of the robot.
:param actuator_joint_commands: A dictionary mapping actuator names to joint command names.
"""
send_meta_data = {robot_name: [BodyProperty.POSITION.value, BodyProperty.ORIENTATION.value,
BodyProperty.RELATIVE_VELOCITY.value]}
relative_velocity = [0.0] * 6
- data = [self.sim_time, *position, *orientation, *relative_velocity]
+ data = [self.sim_time] + [0.0] * 3 + [1, 0, 0, 0] + relative_velocity
self.send_data_to_server(data, send_meta_data=send_meta_data, receive_meta_data=actuator_joint_commands)
def _reset_request_meta_data(self, set_simulation_name: bool = True):
@@ -440,9 +443,12 @@ def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[Property,
body_names = list(response_meta_data["send"].keys())
flattened_data = [value for body_name in body_names for data in body_data[body_name].values()
for value in data]
+ self.lock.acquire()
self.send_data = [self.sim_time, *flattened_data]
self.send_and_receive_data()
- return self.response_meta_data
+ response_meta_data = self.response_meta_data
+ self.lock.release()
+ return response_meta_data
def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict:
"""
@@ -451,10 +457,13 @@ def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict:
:param send_meta_data: The metadata to be sent.
:return: The response from the server.
"""
+ self.lock.acquire()
self._reset_request_meta_data()
self.request_meta_data["send"] = send_meta_data
self.send_and_receive_meta_data()
- return self.response_meta_data
+ response_meta_data = self.response_meta_data
+ self.lock.release()
+ return response_meta_data
def send_data_to_server(self, data: List,
send_meta_data: Optional[Dict] = None,
@@ -469,6 +478,7 @@ def send_data_to_server(self, data: List,
:param set_simulation_name: Whether to set the simulation name to the value of self.simulation.
:return: The response from the server.
"""
+ self.lock.acquire()
self._reset_request_meta_data(set_simulation_name=set_simulation_name)
if send_meta_data:
self.request_meta_data["send"] = send_meta_data
@@ -477,7 +487,9 @@ def send_data_to_server(self, data: List,
self.send_and_receive_meta_data()
self.send_data = data
self.send_and_receive_data()
- return self.response_meta_data
+ response_meta_data = self.response_meta_data
+ self.lock.release()
+ return response_meta_data
class MultiverseController(MultiverseWriter):
@@ -507,7 +519,7 @@ class MultiverseAPI(MultiverseClient):
"""
The wait time for the API request in seconds.
"""
- APIs_THAT_NEED_WAIT_TIME: List[API] = [API.ATTACH]
+ APIs_THAT_NEED_WAIT_TIME: List[API] = [API.ATTACH, API.DETACH]
def __init__(self, name: str, port: int, simulation: str, is_prospection_world: bool = False,
simulation_wait_time_factor: float = 1.0):
@@ -526,6 +538,31 @@ def __init__(self, name: str, port: int, simulation: str, is_prospection_world:
self.simulation = simulation
self.wait: bool = False # Whether to wait after sending the API request.
+ def get_body_bounding_box(self, body_name: str,
+ with_children: bool = False) -> Union[AxisAlignedBoundingBox, List[AxisAlignedBoundingBox]]:
+ """
+ Get the body bounding box from the multiverse server, they are with respect to the body's frame.
+ """
+ bounding_boxes_data = self._get_bounding_box(body_name, with_children)
+ bounding_boxes = []
+ for bounding_box in bounding_boxes_data:
+ origin = Point(bounding_box[0], bounding_box[1], bounding_box[2])
+ size = Point(bounding_box[3], bounding_box[4], bounding_box[5])
+ bounding_boxes.append(AxisAlignedBoundingBox.from_origin_and_half_extents(origin, size))
+ if with_children:
+ return bounding_boxes
+ return bounding_boxes[0]
+
+ def _get_bounding_box(self, body_name: str, with_children: bool = False) -> List[List[float]]:
+ """
+ Get the body bounding box from the multiverse server.
+ """
+ params = [body_name]
+ if with_children:
+ params.append("with_children")
+ response = self._request_single_api_callback(API.GET_BOUNDING_BOX, *params)[0]
+ return [list(map(float, bounding_box.split())) for bounding_box in response]
+
def save(self, save_name: str, save_directory: Optional[str] = None) -> str:
"""
Save the current state of the simulation.
@@ -557,7 +594,7 @@ def get_save_path(save_name: str, save_directory: Optional[str] = None) -> str:
:param save_directory: The save directory.
:return: The save path.
"""
- return save_name if save_directory is None else os.path.join(save_directory, save_name)
+ return save_name if not save_directory else os.path.join(save_directory, save_name)
def attach(self, constraint: Constraint) -> None:
"""
@@ -666,18 +703,26 @@ def check_object_exists(self, obj: Object) -> bool:
"""
return self._request_single_api_callback(API.EXIST, obj.name)[0] == 'yes'
- def get_contact_points(self, obj: Object) -> List[MultiverseContactPoint]:
+ def get_resultant_force_and_torque_on_object(self, obj: Object) -> Tuple[List[float], List[float]]:
"""
- Request the contact points of an object, this includes the object names and the contact forces and torques.
+ Get the resultant force and torque on the object.
:param obj: The object.
- :return: The contact points of the object as a list of MultiverseContactPoint.
+ :return: The resultant force and torque on the object.
+ """
+ effort = self._request_single_api_callback(API.GET_CONSTRAINT_EFFORT, obj.name)
+ return self._parse_constraint_effort(effort)
+
+ def get_contact_points_between_bodies(self, body_1_name: str, body_2_name: str) -> List[MultiverseContactPoint]:
"""
- api_response_data = self._get_contact_points(obj.name)
- body_names = api_response_data[API.GET_CONTACT_BODIES]
- contact_efforts = self._parse_constraint_effort(api_response_data[API.GET_CONSTRAINT_EFFORT])
- return [MultiverseContactPoint(body_names[i], contact_efforts[:3], contact_efforts[3:])
- for i in range(len(body_names))]
+ Request the contact points between two bodies.
+
+ :param body_1_name: The name of the first body.
+ :param body_2_name: The name of the second body.
+ :return: The contact points between the bodies as a list of MultiverseContactPoint.
+ """
+ points = self._request_single_api_callback(API.GET_CONTACT_POINTS, body_1_name, body_2_name)
+ return self._parse_contact_points(body_1_name, body_2_name, points)
def get_objects_intersected_with_rays(self, from_positions: List[List[float]],
to_positions: List[List[float]]) -> List[RayResult]:
@@ -733,7 +778,7 @@ def list_of_positions_to_string(positions: List[List[float]]) -> str:
return " ".join([f"{position[0]} {position[1]} {position[2]}" for position in positions])
@staticmethod
- def _parse_constraint_effort(contact_effort: List[str]) -> List[float]:
+ def _parse_constraint_effort(contact_effort: List[str]) -> Tuple[List[float], List[float]]:
"""
Parse the contact effort of an object.
@@ -741,21 +786,41 @@ def _parse_constraint_effort(contact_effort: List[str]) -> List[float]:
:return: The contact effort of the object as a list of floats.
"""
contact_effort = contact_effort[0].split()
- if 'failed' in contact_effort:
- logwarn("Failed to get contact effort")
- return [0.0] * 6
- return list(map(float, contact_effort))
+ contact_effort = list(map(float, contact_effort))
+ forces, torques = contact_effort[:3], contact_effort[3:]
+ return forces, torques
+
+ @staticmethod
+ def _parse_contact_points(body_1, body_2, contact_points: List[str]) -> List[MultiverseContactPoint]:
+ """
+ Parse the contact points of an object.
+
+ :param body_1: The name of the first body.
+ :param body_2: The name of the second body.
+ :param contact_points: The contact points of the object as a list of strings.
+ :return: The contact positions, and normal vectors as a list of MultiverseContactPoint.
+ """
+ contact_point_data = [list(map(float, contact_point.split())) for contact_point in contact_points]
+ return [MultiverseContactPoint(body_1, body_2, point[:3], point[3:]) for point in contact_point_data]
- def _get_contact_points(self, object_name) -> Dict[API, List]:
+ def get_contact_points(self, body_name: str) -> List[MultiverseContactPoint]:
"""
- Request the contact points of an object.
+ Get the contact points of a body from the multiverse server.
- :param object_name: The name of the object.
- :return: The contact points api response as a dictionary.
+ :param body_name: The name of the body.
"""
- return self._request_apis_callbacks({API.GET_CONTACT_BODIES: [object_name],
- API.GET_CONSTRAINT_EFFORT: [object_name]
- })
+ contact_data = self._request_single_api_callback(API.GET_CONTACT_BODIES_AND_POINTS,
+ body_name, "with_children")
+ contact_points = []
+ for contact_point in contact_data:
+ contact_point_data = list(contact_point.split())
+ position_and_normal = list(map(float, contact_point_data[2:]))
+ for i in range(0, len(position_and_normal), 6):
+ contact_points.append(MultiverseContactPoint(contact_point_data[0],
+ contact_point_data[1],
+ position_and_normal[i:i + 3],
+ position_and_normal[i + 3:i + 6]))
+ return contact_points
def pause_simulation(self) -> None:
"""
@@ -790,12 +855,27 @@ def _request_apis_callbacks(self, api_data: Dict[API, List]) -> Dict[API, List[s
for api_name, params in api_data.items():
self._add_api_request(api_name.value, *params)
self._send_api_request()
- responses = self._get_all_apis_responses()
if self.wait:
sleep(self.API_REQUEST_WAIT_TIME.total_seconds() * self.simulation_wait_time_factor)
self.wait = False
+ responses = self._get_all_apis_responses()
+ self.validate_apis_response(api_data, responses)
return responses
+ @staticmethod
+ def validate_apis_response(api_data: Dict[API, List], responses: Dict[API, List[str]]):
+ """
+ Validate the responses from the multiverse server and raise error if an api request failed.
+
+ :param api_data: The data of the api request which has the api name and the arguments.
+ :param responses: The responses of the given api requests.
+ :raises FailedAPIResponse: when one of the responses reports that the request failed.
+ """
+ for api_name, response in responses.items():
+ for val in response:
+ if 'failed' in val:
+ raise FailedAPIResponse(response, api_name, api_data[api_name])
+
def _get_all_apis_responses(self) -> Dict[API, List[str]]:
"""
Get all the API responses from the server.
diff --git a/src/pycrap/agent.py b/src/pycrap/agent.py
index 26b666689..803f709c1 100644
--- a/src/pycrap/agent.py
+++ b/src/pycrap/agent.py
@@ -1,14 +1,72 @@
+from . import BodyPart
from .base import PhysicalObject
+
+class AgentPart(BodyPart):
+ """
+ An agent part is a part of an agent's body.
+ """
+
+
+class HumanPart(BodyPart):
+ """
+ A human part is a part of a human's body.
+ """
+
+
+class RobotPart(AgentPart):
+ """
+ A robot part is a part of a robot's body, could be a link or a set of links or a joint, etc.
+ """
+
+
class Agent(PhysicalObject):
"""
An agent is an entity that acts.
"""
+ has_part = [AgentPart]
+
class Robot(Agent):
"""
A robot is a machine that can carry out a complex series of actions automatically.
"""
+ has_part = [RobotPart]
+
class Human(Agent):
- ...
\ No newline at end of file
+ """
+ A human is a physical and biological agent
+ """
+ has_part = [HumanPart]
+
+
+class PrehensileEffector(AgentPart):
+ """
+ A prehensile effector is a part of an agent's body that is used for grasping.
+ """
+
+
+class Hand(PrehensileEffector):
+ """
+ A hand is a prehensile effector that is used for grasping, it has a palm, fingers, and a thumb.
+ """
+
+
+class HumanHand(HumanPart, Hand):
+ """
+ A human hand is a prehensile that is used for grasping.
+ """
+
+
+class EndEffector(RobotPart, PrehensileEffector):
+ """
+ An end effector is a device or tool that is connected to the end of a robot arm,
+ that is designed to interact with and manipulate the environment.
+ """
+
+
+class Gripper(EndEffector):
+ """
+ A gripper is a device that can grasp and hold objects.
+ """
diff --git a/src/pycrap/objects.py b/src/pycrap/objects.py
index db3c997d5..ab49ed20c 100644
--- a/src/pycrap/objects.py
+++ b/src/pycrap/objects.py
@@ -1,6 +1,13 @@
from .base import PhysicalObject
+class BodyPart(PhysicalObject):
+ """
+ A body part is a part of an object's body.
+ """
+ ...
+
+
class Container(PhysicalObject):
"""
Any object that can contain other objects.
@@ -41,6 +48,12 @@ class Milk(Food):
"""
+class Bread(Food):
+ """
+ A type of food prepared from a dough of flour and water.
+ """
+
+
class Cutlery(PhysicalObject):
"""
Any implement, tool, or container used for serving or eating food.
diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py
index e9774138c..f749a7b01 100644
--- a/test/test_bullet_world.py
+++ b/test/test_bullet_world.py
@@ -228,7 +228,7 @@ def test_remove_all_text(self):
class BulletWorldTestGUI(BulletWorldGUITestCase):
def test_add_vis_axis(self):
time.sleep(10)
- self.world.add_vis_axis(self.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_frame()))
+ self.world.add_vis_axis(self.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_link()))
self.assertTrue(len(self.world.vis_axis) == 1)
self.world.remove_vis_axis()
self.assertTrue(len(self.world.vis_axis) == 0)
diff --git a/test/test_costmaps.py b/test/test_costmaps.py
index 32452844a..0c0eef03c 100644
--- a/test/test_costmaps.py
+++ b/test/test_costmaps.py
@@ -61,6 +61,21 @@ def test_visualize(self):
origin=Pose([0, 0, 0], [0, 0, 0, 1]))
o.visualize()
+ def test_merge_costmap(self):
+ o = OccupancyCostmap(0.2, from_ros=False, size=200, resolution=0.02,
+ origin=Pose([0, 0, 0], [0, 0, 0, 1]))
+ o2 = OccupancyCostmap(0.2, from_ros=False, size=200, resolution=0.02,
+ origin=Pose([0, 0, 0], [0, 0, 0, 1]))
+ o3 = o + o2
+ self.assertTrue(np.all(o.map == o3.map))
+ o2.map[100:120, 100:120] = 0
+ o3 = o + o2
+ self.assertTrue(np.all(o3.map[100:120, 100:120] == 0))
+ self.assertTrue(np.all(o3.map[0:100, 0:100] == o.map[0:100, 0:100]))
+ o2.map = np.zeros_like(o2.map)
+ o3 = o + o2
+ self.assertTrue(np.all(o3.map == o2.map))
+
class SemanticCostmapTestCase(BulletWorldTestCase):
@@ -90,6 +105,7 @@ def test_iterate(self):
self.assertTrue(costmap.valid_area.contains([sample.position.x, sample.position.y]))
+
class ProbabilisticCostmapTestCase(BulletWorldTestCase):
origin = Pose([1.5, 1, 0], [0, 0, 0, 1])
diff --git a/test/test_designator/test_location_designator.py b/test/test_designator/test_location_designator.py
index 9a30f3563..e92803ae8 100644
--- a/test/test_designator/test_location_designator.py
+++ b/test/test_designator/test_location_designator.py
@@ -27,7 +27,8 @@ def test_reachability_pose(self):
def test_visibility(self):
object_desig = ObjectDesignatorDescription(names=["milk"])
robot_desig = ObjectDesignatorDescription(names=[RobotDescription.current_robot_description.name])
- location_desig = CostmapLocation(object_desig.resolve(), visible_for=robot_desig.resolve())
+ location_desig = CostmapLocation(object_desig.resolve(), visible_for=robot_desig.resolve(),
+ check_collision_at_start=False)
location = location_desig.resolve()
self.assertTrue(len(location.pose.position_as_list()) == 3)
self.assertTrue(len(location.pose.orientation_as_list()) == 4)
diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py
index ad390137b..8f29cf7b3 100644
--- a/test/test_goal_validator.py
+++ b/test/test_goal_validator.py
@@ -88,7 +88,7 @@ def test_single_revolute_joint_position_goal(self):
self.validate_revolute_joint_position_goal(goal_validator, JointType.REVOLUTE)
def validate_revolute_joint_position_goal(self, goal_validator, joint_type: Optional[JointType] = None):
- goal_joint_position = -np.pi / 4
+ goal_joint_position = -np.pi / 8
joint_name = 'l_shoulder_lift_joint'
if joint_type is not None:
goal_validator.register_goal(goal_joint_position, joint_type, joint_name)
diff --git a/test/test_logging.py b/test/test_logging.py
index c03459266..4bf1adec7 100644
--- a/test/test_logging.py
+++ b/test/test_logging.py
@@ -1,5 +1,5 @@
from pycram.testing import BulletWorldTestCase
-from pycram.ros.logging import set_logger_level, logwarn, logerr
+from pycram.ros.logging import set_logger_level, logwarn, logerr, logdebug
from pycram.datastructures.enums import LoggerLevel
@@ -7,8 +7,11 @@ class TestLogging(BulletWorldTestCase):
"""Testcase for the pycram logging functions."""
def test_set_logger_level(self):
- logwarn("This is a warning, it should not be printed")
+ set_logger_level(LoggerLevel.DEBUG)
+ logdebug("This is a debug message, it should be printed")
+ logwarn("This is a warning, it should be printed")
logerr("This is an error, it should be printed")
set_logger_level(LoggerLevel.ERROR)
+ logdebug("This is a debug message, it should not be printed")
logwarn("This is a warning, it should not be printed")
logerr("This is an error, it should be printed")
diff --git a/test/test_multiverse.py b/test/test_multiverse.py
index d0e75ce91..8bb3cf711 100644
--- a/test/test_multiverse.py
+++ b/test/test_multiverse.py
@@ -7,13 +7,15 @@
from tf.transformations import quaternion_from_euler, quaternion_multiply
from typing_extensions import Optional, List
-from pycram.datastructures.dataclasses import ContactPointsList, ContactPoint, AxisAlignedBoundingBox
-from pycram.datastructures.enums import ObjectType, Arms, JointType
+import pycrap
+from pycram.datastructures.dataclasses import ContactPointsList, ContactPoint, AxisAlignedBoundingBox, Color
+from pycram.datastructures.enums import Arms, JointType
from pycram.datastructures.pose import Pose
from pycram.robot_description import RobotDescriptionManager
from pycram.world_concepts.world_object import Object
from pycram.validation.error_checkers import calculate_angle_between_quaternions
from pycram.helper import get_robot_mjcf_path, parse_mjcf_actuators
+from pycram.object_descriptors.generic import ObjectDescription as GenericObjectDescription
multiverse_installed = True
try:
@@ -53,20 +55,24 @@ def tearDownClass(cls):
def tearDown(self):
self.multiverse.remove_all_objects()
+ def test_load_generic_object(self):
+ obj_desc = GenericObjectDescription('test_cube', [0, 0, 0], [0.1, 0.1, 0.1],
+ color=Color(1, 0, 0, 1))
+ obj = Object(obj_desc.name, pycrap.PhysicalObject, description=obj_desc)
+ self.assertIsInstance(obj, Object)
+ self.assertTrue(obj in self.multiverse.objects)
+ obj.set_position([1, 1, 0.1])
+ pose = obj.get_pose()
+ self.assert_poses_are_equal(pose, Pose([1, 1, 0.1], [0, 0, 0, 1]))
+
def test_save_and_restore_state(self):
milk = self.spawn_milk([1, 1, 0.1])
robot = self.spawn_robot()
- cup = self.spawn_cup([1, 2, 0.1])
- if "apartment" not in self.multiverse.get_object_names():
- apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf")
- else:
- apartment = self.multiverse.get_object_by_name("apartment")
+ apartment = self.spawn_apartment()
apartment.set_joint_position("cabinet10_drawer1_joint", 0.1)
robot.attach(milk)
- milk.attach(cup)
all_object_attachments = {obj: obj.attachments.copy() for obj in self.multiverse.objects}
state_id = self.multiverse.save_state()
- milk.detach(cup)
robot_link = robot.root_link
milk_link = milk.root_link
cid = robot_link.constraint_ids[milk_link]
@@ -84,13 +90,12 @@ def test_save_and_restore_state(self):
self.assertTrue(apartment.get_joint_position("cabinet10_drawer1_joint") == 0.1)
def test_spawn_xml_object(self):
- bread = Object("bread_1", ObjectType.GENERIC_OBJECT, "bread_1.xml", pose=Pose([1, 1, 0.1]))
+ bread = Object("bread_1", pycrap.Bread, "bread_1.xml", pose=Pose([1, 1, 0.1]))
self.assert_poses_are_equal(bread.get_pose(), Pose([1, 1, 0.1]))
def test_get_axis_aligned_bounding_box_for_one_link_object(self):
position = [1, 1, 0.1]
- milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1, 1, 0.1],
- quaternion_from_euler(np.pi/4, 0, 0).tolist()))
+ milk = self.spawn_milk(position, quaternion_from_euler(np.pi/4, 0, 0).tolist())
aabb = milk.get_axis_aligned_bounding_box()
self.assertIsInstance(aabb, AxisAlignedBoundingBox)
min_p_1, max_p_1 = aabb.get_min_max()
@@ -116,7 +121,7 @@ def test_get_axis_aligned_bounding_box_for_one_link_object(self):
self.assertAlmostEqual(max_p_1[0] + position_shift, max_p_2[0], delta=0.001)
def test_spawn_mesh_object(self):
- milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1, 1, 0.1]))
+ milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([1, 1, 0.1]))
self.assert_poses_are_equal(milk.get_pose(), Pose([1, 1, 0.1]))
self.multiverse.simulate(0.2)
contact_points = milk.contact_points()
@@ -143,7 +148,7 @@ def test_get_images_for_target(self):
camera_description = self.multiverse.robot_description.get_default_camera()
camera_link_name = camera_description.link_name
camera_pose = robot.get_link_pose(camera_link_name)
- camera_frame = self.multiverse.robot_description.get_camera_frame()
+ camera_frame = self.multiverse.robot_description.get_camera_frame(robot.name)
camera_front_facing_axis = camera_description.front_facing_axis
milk_spawn_position = np.array(camera_front_facing_axis) * 0.5
orientation = camera_pose.to_transform(camera_frame).invert().rotation_as_list()
@@ -175,7 +180,7 @@ def test_spawn_robot_with_actuators_directly_from_multiverse(self):
robot_name = "tiago_dual"
rdm = RobotDescriptionManager()
rdm.load_description(robot_name)
- self.multiverse.spawn_robot_with_controller(robot_name, Pose([-2, -2, 0.001]))
+ self.multiverse.spawn_robot_with_controller(robot_name, Pose())
def test_spawn_object(self):
milk = self.spawn_milk([1, 1, 0.1])
@@ -302,7 +307,7 @@ def step_robot_pose(self, robot, position_step, angle_step, num_steps):
def test_get_environment_pose(self):
if "apartment" not in self.multiverse.get_object_names():
- apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf")
+ apartment = Object("apartment", pycrap.Apartment, f"apartment.urdf")
else:
apartment = self.multiverse.get_object_by_name("apartment")
pose = apartment.get_pose()
@@ -351,7 +356,7 @@ def test_attach_with_robot(self):
robot = self.spawn_robot()
ee_link = self.multiverse.get_arm_tool_frame_link(Arms.RIGHT)
# Get position of milk relative to robot end effector
- robot.attach(milk, ee_link.name, coincide_the_objects=False)
+ robot.attach(milk, ee_link.name)
self.assertTrue(robot in milk.attachments)
milk_initial_pose = milk.root_link.get_pose_wrt_link(ee_link)
robot_position = 1.57
@@ -360,34 +365,42 @@ def test_attach_with_robot(self):
self.assert_poses_are_equal(milk_initial_pose, milk_pose)
def test_get_object_contact_points(self):
- for i in range(10):
+ for i in range(3):
milk = self.spawn_milk([1, 1, 0.01], [0, -0.707, 0, 0.707])
contact_points = self.multiverse.get_object_contact_points(milk)
self.assertIsInstance(contact_points, ContactPointsList)
- self.assertEqual(len(contact_points), 1)
+ self.assertTrue(len(contact_points) >= 1)
self.assertIsInstance(contact_points[0], ContactPoint)
self.assertTrue(contact_points[0].link_b.object, self.multiverse.floor)
- cup = self.spawn_cup([1, 1, 0.15])
+ cup = self.spawn_cup([1, 1, 0.12])
# This is needed because the cup is spawned in the air, so it needs to fall
# to get in contact with the milk
- self.multiverse.simulate(0.3)
+ self.multiverse.simulate(0.4)
contact_points = self.multiverse.get_object_contact_points(cup)
self.assertIsInstance(contact_points, ContactPointsList)
- self.assertEqual(len(contact_points), 1)
+ self.assertTrue(len(contact_points) >= 1)
self.assertIsInstance(contact_points[0], ContactPoint)
self.assertTrue(contact_points[0].link_b.object, milk)
self.tearDown()
+ def test_get_robot_contact_points(self):
+ robot = self.spawn_robot([0.9345829872370865, 1.9027591011850133, 0.0],
+ quaternion_from_euler(0, 0, 2.26).tolist(),
+ robot_name="pr2")
+ apartment = self.spawn_apartment()
+ contact_points = self.multiverse.get_contact_points_between_two_objects(robot, apartment)
+ self.assertTrue(len(contact_points) > 0)
+
def test_get_contact_points_between_two_objects(self):
for i in range(3):
milk = self.spawn_milk([1, 1, 0.01], [0, -0.707, 0, 0.707])
- cup = self.spawn_cup([1, 1, 0.15])
+ cup = self.spawn_cup([1, 1, 0.12])
# This is needed because the cup is spawned in the air so it needs to fall
# to get in contact with the milk
- self.multiverse.simulate(0.3)
+ self.multiverse.simulate(0.4)
contact_points = self.multiverse.get_contact_points_between_two_objects(milk, cup)
self.assertIsInstance(contact_points, ContactPointsList)
- self.assertEqual(len(contact_points), 1)
+ self.assertTrue(len(contact_points) >= 1)
self.assertIsInstance(contact_points[0], ContactPoint)
self.assertTrue(contact_points[0].link_a.object, milk)
self.assertTrue(contact_points[0].link_b.object, cup)
@@ -409,7 +422,7 @@ def test_get_rays(self):
@staticmethod
def spawn_big_bowl() -> Object:
- big_bowl = Object("big_bowl", ObjectType.GENERIC_OBJECT, "BigBowl.obj",
+ big_bowl = Object("big_bowl", pycrap.Bowl, "BigBowl.obj",
pose=Pose([2, 2, 0.1], [0, 0, 0, 1]))
return big_bowl
@@ -417,10 +430,17 @@ def spawn_big_bowl() -> Object:
def spawn_milk(position: List, orientation: Optional[List] = None, frame="map") -> Object:
if orientation is None:
orientation = [0, 0, 0, 1]
- milk = Object("milk_box", ObjectType.MILK, "milk_box.urdf",
+ milk = Object("milk_box", pycrap.Milk, "milk_box.xml",
pose=Pose(position, orientation, frame=frame))
return milk
+ def spawn_apartment(self) -> Object:
+ if "apartment" not in self.multiverse.get_object_names():
+ apartment = Object("apartment", pycrap.Apartment, f"apartment.urdf")
+ else:
+ apartment = self.multiverse.get_object_by_name("apartment")
+ return apartment
+
def spawn_robot(self, position: Optional[List[float]] = None,
orientation: Optional[List[float]] = None,
robot_name: Optional[str] = 'tiago_dual',
@@ -432,8 +452,8 @@ def spawn_robot(self, position: Optional[List[float]] = None,
if self.multiverse.robot is None or replace:
if self.multiverse.robot is not None:
self.multiverse.robot.remove()
- robot = Object(robot_name, ObjectType.ROBOT, f"{robot_name}.urdf",
- pose=Pose(position, [0, 0, 0, 1]))
+ robot = Object(robot_name, pycrap.Robot, f"{robot_name}.urdf",
+ pose=Pose(position, orientation))
else:
robot = self.multiverse.robot
robot.set_position(position)
@@ -441,7 +461,7 @@ def spawn_robot(self, position: Optional[List[float]] = None,
@staticmethod
def spawn_cup(position: List) -> Object:
- cup = Object("cup", ObjectType.GENERIC_OBJECT, "Cup.obj",
+ cup = Object("cup", pycrap.Cup, "cup.xml",
pose=Pose(position, [0, 0, 0, 1]))
return cup
diff --git a/test/test_orm/test_orm.py b/test/test_orm/test_orm.py
index 1ef6815cb..a332e900e 100644
--- a/test/test_orm/test_orm.py
+++ b/test/test_orm/test_orm.py
@@ -289,8 +289,8 @@ def test_open_and_closeAction(self):
ParkArmsActionPerformable(pycram.datastructures.enums.Arms.BOTH).perform()
NavigateActionPerformable(Pose([1.81, 1.73, 0.0],
[0.0, 0.0, 0.594, 0.804]), True).perform()
- OpenActionPerformable(handle_desig, arm=Arms.LEFT).perform()
- CloseActionPerformable(handle_desig, arm=Arms.LEFT).perform()
+ OpenActionPerformable(handle_desig, arm=Arms.LEFT, grasping_prepose_distance=0.03).perform()
+ CloseActionPerformable(handle_desig, arm=Arms.LEFT, grasping_prepose_distance=0.03).perform()
pycram.orm.base.ProcessMetaData().description = "open_and_closeAction_test"
pycram.tasktree.task_tree.root.insert(self.session)