diff --git a/launch/ik_and_description.launch b/launch/ik_and_description.launch
index b4fc410ac..0ae6d3098 100644
--- a/launch/ik_and_description.launch
+++ b/launch/ik_and_description.launch
@@ -38,6 +38,13 @@
textfile="$(find pycram)/resources/hsrb.urdf"/>
+
+
+
+
+
+
diff --git a/resources/stretch.urdf b/resources/stretch.urdf
new file mode 100644
index 000000000..044ee54e0
--- /dev/null
+++ b/resources/stretch.urdf
@@ -0,0 +1,952 @@
+
+
+
+
+
+
+
+
+ name="link_gripper">
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0
+ 0
+ 1.0
+ 1.0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/pycram/designators/actions/actions.py b/src/pycram/designators/actions/actions.py
index 71e781afc..7ce0372ba 100644
--- a/src/pycram/designators/actions/actions.py
+++ b/src/pycram/designators/actions/actions.py
@@ -278,12 +278,12 @@ def perform(self) -> None:
# Perform the motion with the prepose and open gripper
BulletWorld.current_bullet_world.add_vis_axis(prepose)
- MoveTCPMotion(prepose, self.arm).perform()
+ MoveTCPMotion(prepose, self.arm, allow_gripper_collision=True).perform()
MoveGripperMotion(motion="open", gripper=self.arm).perform()
# Perform the motion with the adjusted pose -> actual grasp and close gripper
BulletWorld.current_bullet_world.add_vis_axis(adjusted_oTm)
- MoveTCPMotion(adjusted_oTm, self.arm).perform()
+ MoveTCPMotion(adjusted_oTm, self.arm, allow_gripper_collision=True).perform()
adjusted_oTm.pose.position.z += 0.03
MoveGripperMotion(motion="close", gripper=self.arm).perform()
tool_frame = robot_description.get_tool_frame(self.arm)
@@ -291,7 +291,7 @@ def perform(self) -> None:
# Lift object
BulletWorld.current_bullet_world.add_vis_axis(adjusted_oTm)
- MoveTCPMotion(adjusted_oTm, self.arm).perform()
+ MoveTCPMotion(adjusted_oTm, self.arm, allow_gripper_collision=True).perform()
# Remove the vis axis from the world
BulletWorld.current_bullet_world.remove_vis_axis()
diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py
index 1edb9d17d..72a95237b 100644
--- a/src/pycram/designators/location_designator.py
+++ b/src/pycram/designators/location_designator.py
@@ -11,8 +11,7 @@
from ..enums import JointType
from ..helper import transform
from ..plan_failures import EnvironmentManipulationImpossible
-from ..pose_generator_and_validator import pose_generator, visibility_validator, reachability_validator, \
- generate_orientation
+from ..pose_generator_and_validator import PoseGenerator, visibility_validator, reachability_validator
from ..robot_description import ManipulatorDescription
from ..pose import Pose
@@ -168,7 +167,7 @@ def __iter__(self):
ground_pose = Pose(target_pose.position_as_list())
ground_pose.position.z = 0
- occupancy = OccupancyCostmap(0.4, False, 200, 0.02, ground_pose)
+ occupancy = OccupancyCostmap(0.32, False, 200, 0.02, ground_pose)
final_map = occupancy
if self.reachable_for:
@@ -183,8 +182,7 @@ def __iter__(self):
test_robot = BulletWorld.current_bullet_world.get_shadow_object(robot_object)
with Use_shadow_world():
-
- for maybe_pose in pose_generator(final_map, number_of_samples=600):
+ for maybe_pose in PoseGenerator(final_map, number_of_samples=600):
res = True
arms = None
if self.visible_for:
@@ -276,8 +274,8 @@ def __iter__(self) -> Location:
self.handle.name)
with Use_shadow_world():
- for maybe_pose in pose_generator(final_map, number_of_samples=600,
- orientation_generator=lambda p, o: 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)):
hand_links = []
for name, chain in robot_description.chains.items():
@@ -340,6 +338,6 @@ def __iter__(self):
if self.for_object:
min, max = self.for_object.bullet_world_object.get_AABB()
height_offset = (max[2] - min[2]) / 2
- for maybe_pose in pose_generator(sem_costmap):
+ for maybe_pose in PoseGenerator(sem_costmap):
maybe_pose.position.z += height_offset
yield self.Location(maybe_pose)
diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py
index e332c3e07..5e3f91606 100644
--- a/src/pycram/external_interfaces/robokudo.py
+++ b/src/pycram/external_interfaces/robokudo.py
@@ -39,7 +39,7 @@ def wrapper(*args, **kwargs):
rospy.logwarn("RoboKudo is not running, could not initialize RoboKudo interface")
return
- func(*args, **kwargs)
+ return func(*args, **kwargs)
return wrapper
diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py
index f959fd126..20495072e 100644
--- a/src/pycram/pose_generator_and_validator.py
+++ b/src/pycram/pose_generator_and_validator.py
@@ -7,6 +7,7 @@
from .bullet_world_reasoning import contact
from .costmaps import Costmap
from .pose import Pose, Transform
+from .robot_description import ManipulatorDescription
from .robot_descriptions import robot_description
from .external_interfaces.ik import request_ik
from .plan_failures import IKError
@@ -14,65 +15,77 @@
from typing import Type, Tuple, List, Union, Dict, Iterable
-def pose_generator(costmap: Costmap, number_of_samples=100, orientation_generator=None) -> Iterable:
- """
- A generator that crates pose candidates from a given costmap. The generator
- selects the highest 100 values and returns the corresponding positions.
- Orientations are calculated such that the Robot faces the center of the costmap.
-
- :param costmap: The costmap from which poses should be sampled.
- :param number_of_samples: The number of samples from the costmap that should be returned at max
- :param orientation_generator: function that generates an orientation given a position and the origin of the costmap
- :Yield: A tuple of position and orientation
- """
- if not orientation_generator:
- orientation_generator = generate_orientation
-
- # Determines how many positions should be sampled from the costmap
- if number_of_samples == -1:
- number_of_samples = costmap.map.flatten().shape[0]
- indices = np.argpartition(costmap.map.flatten(), -number_of_samples)[-number_of_samples:]
- indices = np.dstack(np.unravel_index(indices, costmap.map.shape)).reshape(number_of_samples, 2)
-
- height = costmap.map.shape[0]
- width = costmap.map.shape[1]
- center = np.array([height // 2, width // 2])
- for ind in indices:
- if costmap.map[ind[0]][ind[1]] == 0:
- continue
- # The position is calculated by creating a vector from the 2D position in the costmap (given by x and y)
- # and the center of the costmap (since this is the origin). This vector is then turned into a transformation
- # and muiltiplied with the transformation of the origin.
- vector_to_origin = (center - ind) * costmap.resolution
- point_to_origin = Transform([*vector_to_origin, 0], frame="point", child_frame="origin")
- origin_to_map = costmap.origin.to_transform("origin").invert()
- point_to_map = point_to_origin * origin_to_map
- map_to_point = point_to_map.invert()
-
- orientation = orientation_generator(map_to_point.translation_as_list(), costmap.origin)
- yield Pose(map_to_point.translation_as_list(), orientation)
-
-
-def height_generator() -> float:
- pass
-
-
-def generate_orientation(position: List[float], origin: Pose) -> List[float]:
- """
- This method generates the orientation for a given position in a costmap. The
- orientation is calculated such that the robot faces the origin of the costmap.
- This generation is done by simply calculating the arctan between the position,
- in the costmap, and the origin of the costmap.
-
- :param position: The position in the costmap. This position is already converted
- to the world coordinate frame.
- :param origin: The origin of the costmap. This is also the point which the
- robot should face.
- :return: A quaternion of the calculated orientation
- """
- angle = np.arctan2(position[1]-origin.position.y, position[0]-origin.position.x) + np.pi
- quaternion = list(tf.transformations.quaternion_from_euler(0, 0, angle, axes="sxyz"))
- return quaternion
+class PoseGenerator:
+ current_orientation_generator = None
+
+ def __init__(self, costmap: Costmap, number_of_samples=100, orientation_generator=None):
+ """
+ :param costmap: The costmap from which poses should be sampled.
+ :param number_of_samples: The number of samples from the costmap that should be returned at max
+ :param orientation_generator: function that generates an orientation given a position and the origin of the costmap
+ """
+
+ if not PoseGenerator.current_orientation_generator:
+ PoseGenerator.current_orientation_generator = PoseGenerator.generate_orientation
+
+ self.costmap = costmap
+ self.number_of_samples = number_of_samples
+ self.orientation_generator = orientation_generator if orientation_generator else PoseGenerator.current_orientation_generator
+
+ def __iter__(self) -> Iterable:
+ """
+ A generator that crates pose candidates from a given costmap. The generator
+ selects the highest 100 values and returns the corresponding positions.
+ Orientations are calculated such that the Robot faces the center of the costmap.
+
+ :Yield: A tuple of position and orientation
+ """
+
+ # Determines how many positions should be sampled from the costmap
+ if self.number_of_samples == -1:
+ 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)
+
+ height = self.costmap.map.shape[0]
+ width = self.costmap.map.shape[1]
+ center = np.array([height // 2, width // 2])
+ for ind in indices:
+ if self.costmap.map[ind[0]][ind[1]] == 0:
+ continue
+ # The position is calculated by creating a vector from the 2D position in the costmap (given by x and y)
+ # and the center of the costmap (since this is the origin). This vector is then turned into a transformation
+ # and muiltiplied with the transformation of the origin.
+ vector_to_origin = (center - ind) * self.costmap.resolution
+ point_to_origin = Transform([*vector_to_origin, 0], frame="point", child_frame="origin")
+ origin_to_map = self.costmap.origin.to_transform("origin").invert()
+ point_to_map = point_to_origin * origin_to_map
+ map_to_point = point_to_map.invert()
+
+ orientation = self.orientation_generator(map_to_point.translation_as_list(), self.costmap.origin)
+ yield Pose(map_to_point.translation_as_list(), orientation)
+
+ @staticmethod
+ def height_generator() -> float:
+ pass
+
+ @staticmethod
+ def generate_orientation(position: List[float], origin: Pose) -> List[float]:
+ """
+ This method generates the orientation for a given position in a costmap. The
+ orientation is calculated such that the robot faces the origin of the costmap.
+ This generation is done by simply calculating the arctan between the position,
+ in the costmap, and the origin of the costmap.
+
+ :param position: The position in the costmap. This position is already converted
+ to the world coordinate frame.
+ :param origin: The origin of the costmap. This is also the point which the
+ robot should face.
+ :return: A quaternion of the calculated orientation
+ """
+ angle = np.arctan2(position[1] - origin.position.y, position[0] - origin.position.x) + np.pi
+ quaternion = list(tf.transformations.quaternion_from_euler(0, 0, angle, axes="sxyz"))
+ return quaternion
def visibility_validator(pose: Pose,
@@ -134,14 +147,9 @@ def reachability_validator(pose: Pose,
target = target.get_pose()
robot.set_pose(pose)
+ manipulator_descs = list(
+ filter(lambda chain: isinstance(chain[1], ManipulatorDescription), robot_description.chains.items()))
- left_gripper = robot_description.get_tool_frame('left')
- right_gripper = robot_description.get_tool_frame('right')
-
- # left_joints = robot_description._safely_access_chains('left').joints
- left_joints = robot_description.chains['left'].joints
- # right_joints = robot_description._safely_access_chains('right').joints
- right_joints = robot_description.chains['right'].joints
# TODO Make orientation adhere to grasping orientation
res = False
arms = []
@@ -151,57 +159,30 @@ def reachability_validator(pose: Pose,
if robot in allowed_collision.keys():
allowed_robot_links = allowed_collision[robot]
- joint_state_before_ik=robot._current_joint_states
- try:
- # resp = request_ik(base_link, end_effector, target_diff, robot, left_joints)
- resp = request_ik(target, robot, left_joints, left_gripper)
+ for chain_name, chain in manipulator_descs:
+ joint_state_before_ik = robot._current_joint_states
+ try:
+ resp = request_ik(target, robot, chain.joints, chain.tool_frame)
- _apply_ik(robot, resp, left_joints)
-
- for obj in BulletWorld.current_bullet_world.objects:
- if obj.name == "floor":
- continue
- in_contact, contact_links = contact(robot, obj, return_links=True)
- allowed_links = allowed_collision[obj] if obj in allowed_collision.keys() else []
+ _apply_ik(robot, resp, chain.joints)
- if in_contact:
- for link in contact_links:
-
- if link[0] in allowed_robot_links or link[1] in allowed_links:
- in_contact = False
-
- if not in_contact:
- arms.append("left")
- res = True
- except IKError:
- pass
- finally:
- robot.set_joint_states(joint_state_before_ik)
+ for obj in BulletWorld.current_bullet_world.objects:
+ if obj.name == "floor":
+ continue
+ in_contact, contact_links = contact(robot, obj, return_links=True)
+ allowed_links = allowed_collision[obj] if obj in allowed_collision.keys() else []
- try:
- # resp = request_ik(base_link, end_effector, target_diff, robot, right_joints)
- resp = request_ik(target, robot, right_joints, right_gripper)
+ if in_contact:
+ for link in contact_links:
- _apply_ik(robot, resp, right_joints)
-
- for obj in BulletWorld.current_bullet_world.objects:
- if obj.name == "floor":
- continue
- in_contact, contact_links = contact(robot, obj, return_links=True)
- allowed_links = allowed_collision[obj] if obj in allowed_collision.keys() else []
-
- if in_contact:
- for link in contact_links:
-
- if link[0] in allowed_robot_links or link[1] in allowed_links:
- in_contact = False
-
- if not in_contact:
- arms.append("right")
- res = True
- except IKError:
- pass
- finally:
- robot.set_joint_states(joint_state_before_ik)
+ if link[0] in allowed_robot_links or link[1] in allowed_links:
+ in_contact = False
+ if not in_contact:
+ arms.append(chain_name)
+ res = True
+ except IKError:
+ pass
+ finally:
+ robot.set_joint_states(joint_state_before_ik)
return res, arms
diff --git a/src/pycram/process_module.py b/src/pycram/process_module.py
index 16cf4f651..6df951b95 100644
--- a/src/pycram/process_module.py
+++ b/src/pycram/process_module.py
@@ -78,6 +78,7 @@ class RealRobot:
"""
def __init__(self):
self.pre: str = ""
+ self.pre_delay: bool = False
def __enter__(self):
"""
@@ -86,6 +87,8 @@ def __enter__(self):
"""
self.pre = ProcessModuleManager.execution_type
ProcessModuleManager.execution_type = "real"
+ self.pre_delay = ProcessModule.execution_delay
+ ProcessModule.execution_delay = False
def __exit__(self, type, value, traceback):
"""
@@ -93,6 +96,7 @@ def __exit__(self, type, value, traceback):
used one.
"""
ProcessModuleManager.execution_type = self.pre
+ ProcessModule.execution_delay = self.pre_delay
def __call__(self):
return self
diff --git a/src/pycram/process_modules/__init__.py b/src/pycram/process_modules/__init__.py
index 8349b3ec5..30f5906a3 100644
--- a/src/pycram/process_modules/__init__.py
+++ b/src/pycram/process_modules/__init__.py
@@ -3,9 +3,11 @@
from .donbot_process_modules import DonbotManager
from .hsr_process_modules import HSRBManager
from .default_process_modules import DefaultManager
+from .stretch_process_modules import StretchManager
Pr2Manager()
BoxyManager()
DonbotManager()
HSRBManager()
DefaultManager()
+StretchManager()
diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py
index 77cc067eb..e49427485 100644
--- a/src/pycram/process_modules/pr2_process_modules.py
+++ b/src/pycram/process_modules/pr2_process_modules.py
@@ -291,7 +291,8 @@ def _execute(self, designator: MoveTCPMotion) -> Any:
if designator.allow_gripper_collision:
giskard.allow_gripper_collision(designator.arm)
giskard.achieve_cartesian_goal(pose_in_map, robot_description.get_tool_frame(designator.arm),
- robot_description.base_link)
+ "torso_lift_link")
+ #robot_description.base_link)
class Pr2MoveArmJointsReal(ProcessModule):
diff --git a/src/pycram/process_modules/stretch_process_modules.py b/src/pycram/process_modules/stretch_process_modules.py
new file mode 100644
index 000000000..1dc972859
--- /dev/null
+++ b/src/pycram/process_modules/stretch_process_modules.py
@@ -0,0 +1,126 @@
+from threading import Lock
+from typing import Any
+
+from ..helper import _apply_ik
+from .default_process_modules import *
+
+
+class StretchNavigate(DefaultNavigation):
+ pass
+
+
+class StretchMoveHead(ProcessModule):
+ def _execute(self, designator) -> Any:
+ target = designator.target
+ robot = BulletWorld.robot
+
+ local_transformer = LocalTransformer()
+ pose_in_pan = local_transformer.transform_pose(target, robot.get_link_tf_frame("link_head_pan"))
+ pose_in_tilt = local_transformer.transform_pose(target, robot.get_link_tf_frame("link_head_tilt"))
+
+ new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x)
+ new_tilt = np.arctan2(-pose_in_tilt.position.y, pose_in_tilt.position.z ** 2 + pose_in_tilt.position.x ** 2) * -1
+
+ current_pan = robot.get_joint_state("joint_head_pan")
+ current_tilt = robot.get_joint_state("joint_head_tilt")
+
+ robot.set_joint_state("joint_head_pan", new_pan + current_pan)
+ robot.set_joint_state("joint_head_tilt", new_tilt + current_tilt)
+
+
+class StretchMoveGripper(DefaultMoveGripper):
+ pass
+
+
+class StretchDetecting(DefaultDetecting):
+ pass
+
+
+class StretchMoveTCP(DefaultMoveTCP):
+ pass
+
+
+class StretchMoveArmJoints(DefaultMoveArmJoints):
+ pass
+
+
+class StretchMoveJoints(DefaultMoveJoints):
+ pass
+
+
+class StretchWorldStateDetecting(DefaultWorldStateDetecting):
+ pass
+
+
+class StretchOpen(ProcessModule):
+ def _execute(self, designator) -> Any:
+ pass
+
+
+class StretchClose(ProcessModule):
+ def _execute(self, designator) -> Any:
+ pass
+
+
+def _move_arm_tcp(target: Pose, robot: Object, arm: str) -> None:
+ gripper = robot_description.get_tool_frame(arm)
+
+ joints = robot_description.chains[arm].joints
+
+ inv = request_ik(target, robot, joints, gripper)
+ _apply_ik(robot, inv, joints)
+
+
+class StretchManager(ProcessModuleManager):
+ def __init__(self):
+ super().__init__("stretch")
+ self._navigate_lock = Lock()
+ self._looking_lock = Lock()
+ self._detecting_lock = Lock()
+ self._move_tcp_lock = Lock()
+ self._move_arm_joints_lock = Lock()
+ self._world_state_detecting_lock = Lock()
+ self._move_joints_lock = Lock()
+ self._move_gripper_lock = Lock()
+ self._open_lock = Lock()
+ self._close_lock = Lock()
+
+ def navigate(self):
+ if ProcessModuleManager.execution_type == "simulated":
+ return StretchNavigate(self._navigate_lock)
+ def looking(self):
+ if ProcessModuleManager.execution_type == "simulated":
+ return StretchMoveHead(self._looking_lock)
+
+ def detecting(self):
+ if ProcessModuleManager.execution_type == "simulated":
+ return StretchDetecting(self._detecting_lock)
+
+ def move_tcp(self):
+ if ProcessModuleManager.execution_type == "simulated":
+ return StretchMoveTCP(self._move_tcp_lock)
+
+ def move_arm_joints(self):
+ if ProcessModuleManager.execution_type == "simulated":
+ return StretchMoveArmJoints(self._move_arm_joints_lock)
+
+ def world_state_detecting(self):
+ if ProcessModuleManager.execution_type == "simulated" or ProcessModuleManager.execution_type == "real":
+ return StretchWorldStateDetecting(self._world_state_detecting_lock)
+
+ def move_joints(self):
+ if ProcessModuleManager.execution_type == "simulated":
+ return StretchMoveJoints(self._move_joints_lock)
+
+ def move_gripper(self):
+ if ProcessModuleManager.execution_type == "simulated":
+ return StretchMoveGripper(self._move_gripper_lock)
+
+ def open(self):
+ if ProcessModuleManager.execution_type == "simulated":
+ return StretchOpen(self._open_lock)
+
+ def close(self):
+ if ProcessModuleManager.execution_type == "simulated":
+ return StretchClose(self._close_lock)
+
diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py
index d8df6f5b8..18059f436 100644
--- a/src/pycram/robot_description.py
+++ b/src/pycram/robot_description.py
@@ -48,7 +48,7 @@ def add_static_joint_chains(self, static_joint_states: Dict[str, List[float]]) -
"""
for configuration, joint_states in static_joint_states.items():
if not self.add_static_joint_chain(configuration, joint_states):
- logger.error("(robot_description) Could not add all static_joint_states for the chain %s.", self.name)
+ rospy.logerr("(robot_description) Could not add all static_joint_states for the chain %s.", self.name)
break
def add_static_joint_chain(self, configuration: str, static_joint_states: List[float]) -> Union[None, bool]:
@@ -68,10 +68,10 @@ def add_static_joint_chain(self, configuration: str, static_joint_states: List[f
self.static_joint_states[configuration] = static_joint_states
return True
else:
- logger.warning("(robot_description) The chain %s already has static joint values "
+ rospy.logwarn("(robot_description) The chain %s already has static joint values "
"for the config %s.", self.name, configuration)
else:
- logger.error("(robot_description) The number of the joint values does not equal the amount of the joints"
+ rospy.logerr("(robot_description) The number of the joint values does not equal the amount of the joints"
"for the chain %s.", self.name)
def get_static_joint_chain(self, configuration: str) -> Union[None, Dict[str, float]]:
@@ -85,7 +85,7 @@ def get_static_joint_chain(self, configuration: str) -> Union[None, Dict[str, fl
try:
joint_values = self.static_joint_states[configuration]
except KeyError:
- logger.error("(robot_description) Configuration %s is unknown for the chain %s.", configuration, self.name)
+ rospy.logerr("(robot_description) Configuration %s is unknown for the chain %s.", configuration, self.name)
return None
return dict(zip(self.joints, joint_values))
@@ -260,8 +260,8 @@ def __init__(self, name: str,
rospack = rospkg.RosPack()
filename = rospack.get_path('pycram') + '/resources/' + name + '.urdf'
with open(filename) as f:
- with utils.suppress_stdout_stderr():
- self.robot_urdf = URDF.from_xml_string(f.read())
+ # with utils.suppress_stdout_stderr():
+ self.robot_urdf = URDF.from_xml_string(f.read())
def _safely_access_chains(self, chain_name: str, verbose: Optional[bool] = True) -> Union[None, ChainDescription]:
"""
@@ -272,7 +272,7 @@ def _safely_access_chains(self, chain_name: str, verbose: Optional[bool] = True)
chain_description = self.chains[chain_name]
except KeyError:
if verbose:
- logger.warning("(robot_description) Chain name %s is unknown.", chain_name)
+ rospy.logwarn("(robot_description) Chain name %s is unknown.", chain_name)
return None
return chain_description
@@ -295,10 +295,10 @@ def _get_chain_description(self, chain_name: str,
if type(chain_description) is description_type:
return chain_description
else:
- logger.error("(robot_description) The chain %s is not of type %s, but of type %s.",
+ rospy.logerr("(robot_description) The chain %s is not of type %s, but of type %s.",
chain_name, description_type, type(chain_description))
else:
- logger.warning("(robot_description) Only subclasses of ChainDescription are allowed.")
+ rospy.logwarn("(robot_description) Only subclasses of ChainDescription are allowed.")
def get_tool_frame(self, manipulator_name: str) -> Union[None, str]:
"""
@@ -310,7 +310,7 @@ def get_tool_frame(self, manipulator_name: str) -> Union[None, str]:
if manipulator_description:
return manipulator_description.tool_frame
else:
- logger.error("(robot_description) Could not get the tool frame of the manipulator %s.", manipulator_name)
+ rospy.logerr("(robot_description) Could not get the tool frame of the manipulator %s.", manipulator_name)
def get_static_joint_chain(self, chain_name: str, configuration: str) -> Union[None, Dict[str, float]]:
"""
@@ -322,7 +322,7 @@ def get_static_joint_chain(self, chain_name: str, configuration: str) -> Union[N
if chain_description:
return chain_description.get_static_joint_chain(configuration)
else:
- logger.error("(robot_description) Could not get static joint chain called %s of the chain %s.",
+ rospy.logerr("(robot_description) Could not get static joint chain called %s of the chain %s.",
configuration, chain_name)
def get_static_tf(self, base_link: str, target_link: str):
@@ -338,11 +338,11 @@ def add_chain(self, name: str, chain_description: ChainDescription) -> Union[Non
"""
if issubclass(type(chain_description), ChainDescription):
if self._safely_access_chains(name, verbose=False):
- logger.warning("(robot_description) Replacing the chain description of the name %s.", name)
+ rospy.logwarn("(robot_description) Replacing the chain description of the name %s.", name)
self.chains[name] = chain_description
return True
else:
- logger.error("(robot_description) Given chain_description object is no subclass of ChainDescription.")
+ rospy.logerr("(robot_description) Given chain_description object is no subclass of ChainDescription.")
def add_chains(self, chains_dict: Dict[str, ChainDescription]) -> None:
"""
@@ -354,7 +354,7 @@ def add_chains(self, chains_dict: Dict[str, ChainDescription]) -> None:
"""
for name, chain in chains_dict.items():
if not self.add_chain(name, chain):
- logger.error("(robot_description) Could not add the chain object of name %s.", name)
+ rospy.logerr("(robot_description) Could not add the chain object of name %s.", name)
break
def add_camera(self, name: str, camera_description: CameraDescription) -> Union[None, bool]:
@@ -369,11 +369,11 @@ def add_camera(self, name: str, camera_description: CameraDescription) -> Union[
except KeyError:
found = False
if found:
- logger.warning("(robot_description) Replacing the camera description of the name %s.", name)
+ rospy.logwarn("(robot_description) Replacing the camera description of the name %s.", name)
self.cameras[name] = camera_description
return True
else:
- logger.error("(robot_description) Given camera_description object is not of type CameraDescription.")
+ rospy.logerr("(robot_description) Given camera_description object is not of type CameraDescription.")
def add_cameras(self, cameras_dict: Dict[str, CameraDescription]) -> None:
"""
@@ -385,7 +385,7 @@ def add_cameras(self, cameras_dict: Dict[str, CameraDescription]) -> None:
"""
for name, camera in cameras_dict.items():
if not self.add_camera(name, camera):
- logger.error("(robot_description) Could not add the camera object of name %s.", name)
+ rospy.logerr("(robot_description) Could not add the camera object of name %s.", name)
break
def get_camera_frame(self, camera_name: str) -> Union[None, str]:
@@ -397,7 +397,7 @@ def get_camera_frame(self, camera_name: str) -> Union[None, str]:
try:
camera_description = self.cameras[camera_name]
except KeyError:
- logger.error("(robot_description) Camera name %s is unknown.", camera_name)
+ rospy.logerr("(robot_description) Camera name %s is unknown.", camera_name)
return None
return camera_description.frame
@@ -426,7 +426,7 @@ def add_static_joint_chains(self, chain_name: str, static_joint_states: Dict[str
"""
for configuration, joint_states in static_joint_states.items():
if not self.add_static_joint_chain(chain_name, configuration, joint_states):
- logger.error("(robot_description) Could not add the static joint chain called %s for chain %s.",
+ rospy.logerr("(robot_description) Could not add the static joint chain called %s for chain %s.",
configuration, chain_name)
break
@@ -456,7 +456,7 @@ def add_static_gripper_chains(self, manipulator_name: str, static_joint_states:
"""
for configuration, joint_states in static_joint_states.items():
if not self.add_static_gripper_chain(manipulator_name, configuration, joint_states):
- logger.error("(robot_description) Could not add static gripper chain called %s for manipulator chain %s.",
+ rospy.logerr("(robot_description) Could not add static gripper chain called %s for manipulator chain %s.",
configuration, manipulator_name)
break
diff --git a/src/pycram/robot_descriptions/__init__.py b/src/pycram/robot_descriptions/__init__.py
index 897e86d4d..3385fab74 100644
--- a/src/pycram/robot_descriptions/__init__.py
+++ b/src/pycram/robot_descriptions/__init__.py
@@ -9,6 +9,7 @@
from .pr2_description import PR2Description
from .ur5_description import UR5Description
from .tiago_description import TiagoDescription
+from .stretch_description import StretchDescription
from .. import utils
from ..robot_description import RobotDescription
@@ -61,13 +62,15 @@ def update_robot_description(robot_name=None, from_ros=None):
description = UR5Description
elif "tiago_dual" in robot:
description = TiagoDescription
+ elif "stretch" in robot:
+ description = StretchDescription
else:
logger.error("(robot-description) The given robot name %s has no description class.", robot_name)
return None
return InitializedRobotDescription(description)
-with utils.suppress_stdout_stderr():
- update_robot_description(from_ros=True) # "ur5_robotiq")# # todo: put in ros init
+# with utils.suppress_stdout_stderr():
+update_robot_description(from_ros=True) # "ur5_robotiq")# # todo: put in ros init
robot_description = InitializedRobotDescription.i
diff --git a/src/pycram/robot_descriptions/stretch_description.py b/src/pycram/robot_descriptions/stretch_description.py
new file mode 100644
index 000000000..9bdf63717
--- /dev/null
+++ b/src/pycram/robot_descriptions/stretch_description.py
@@ -0,0 +1,68 @@
+from ..robot_description import *
+import numpy as np
+import tf
+
+
+class StretchDescription(RobotDescription):
+
+ def __init__(self):
+ super().__init__("stretch", "base_link", "base_link", "link_lift", "joint_lift")
+
+ realsense_color = CameraDescription('camera_color_optical_frame', horizontal_angle=1.047, vertical_angle=0.785,
+ minimal_height=1.322, maximal_height=1.322)
+
+ realsense_depth = CameraDescription('camera_depth_optical_frame', horizontal_angle=1.047, vertical_angle=0.785,
+ minimal_height=1.307, maximal_height=1.307)
+
+ realsense_infra1 = CameraDescription('camera_infra1_optical_frame', horizontal_angle=1.047,
+ vertical_angle=0.785,
+ minimal_height=1.307, maximal_height=1.307)
+ realsense_infra2 = CameraDescription('camera_infra2_optical_frame', horizontal_angle=1.047,
+ vertical_angle=0.785,
+ minimal_height=1.257, maximal_height=1.257)
+ self.add_cameras(
+ {'color': realsense_color, 'depth': realsense_depth, 'infra1': realsense_infra1,
+ 'infra2': realsense_infra2})
+
+ self.front_facing_axis = [0, 0, 1]
+
+ neck = ChainDescription("neck", ["joint_head_pan", "joint_head_tilt"],
+ ["link_head_pan", "link_head_tilt"])
+ self.add_chain("neck", neck)
+
+ arm_joints = ['joint_lift','joint_arm_l3', 'joint_arm_l2', 'joint_arm_l1', 'joint_arm_l0', "joint_wrist_yaw"]
+ arm_links = ['link_lift', 'link_arm_l3', 'link_arm_l2', 'link_arm_l1', 'link_arm_l0', "link_wrist_yaw"]
+
+ arm_chain_desc = ChainDescription("arm", arm_joints, arm_links)
+ arm_inter_desc = InteractionDescription(arm_chain_desc, "link_wrist_yaw")
+
+ gripper_links = ['link_gripper_finger_left', 'link_gripper_fingertip_left',
+ 'link_gripper_finger_right', 'link_gripper_fingertip_right', 'link_grasp_center']
+ gripper_joints = ['joint_gripper_finger_left', 'joint_gripper_finger_right']
+ arm_gripper_desc = GripperDescription("arm", gripper_links, gripper_joints,
+ gripper_meter_to_jnt_multiplier=5.0,
+ gripper_minimal_position=0.013,
+ gripper_convergence_delta=0.005)
+
+ arm_desc = ManipulatorDescription(arm_inter_desc, tool_frame="link_grasp_center", gripper_description=arm_gripper_desc)
+ self.add_chains({"arm": arm_desc, "right": arm_desc, "left": arm_desc})
+
+ arm_park = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+ self.add_static_joint_chain("arm", "park", arm_park)
+
+ gripper_confs = {"open": [0.59, 0.59], "close": [0.0, 0.0]}
+ self.add_static_gripper_chains("arm", gripper_confs)
+
+ self.grasps = GraspingDescription({"front": [0, 0, 0, 1],
+ "left": [0, 0, -1, 1],
+ "right": [0, 0, 1, 1],
+ "top": [0, 1, 0, 1]})
+
+ def get_camera_frame(self, name="color"):
+ return super().get_camera_frame(name)
+
+ @staticmethod
+ def stretch_orientation_generator(position, origin):
+ angle = np.arctan2(position[1] - origin.position.y, position[0] - origin.position.x) + np.pi
+ quaternion = list(tf.transformations.quaternion_from_euler(0, 0, angle + np.pi / 2, axes="sxyz"))
+ return quaternion