diff --git a/cognitive_node_interfaces/CMakeLists.txt b/cognitive_node_interfaces/CMakeLists.txt
index 676c2a8..0231900 100644
--- a/cognitive_node_interfaces/CMakeLists.txt
+++ b/cognitive_node_interfaces/CMakeLists.txt
@@ -19,7 +19,6 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/PerceptionParameters.msg"
"srv/AddPoint.srv"
"srv/AddNeighbor.srv"
- "srv/CalculateClosestPosition.srv"
"srv/DeleteNeighbor.srv"
"srv/Evaluate.srv"
"srv/Execute.srv"
@@ -30,8 +29,6 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"srv/IsCompatible.srv"
"srv/IsReached.srv"
"srv/IsSatisfied.srv"
- "srv/ObjectPickableWithTwoHands.srv"
- "srv/ObjectTooFar.srv"
"srv/Predict.srv"
"srv/SetActivation.srv"
"srv/SetActivationTopic.srv"
diff --git a/cognitive_node_interfaces/srv/ObjectPickableWithTwoHands.srv b/cognitive_node_interfaces/srv/ObjectPickableWithTwoHands.srv
deleted file mode 100644
index 2a51fda..0000000
--- a/cognitive_node_interfaces/srv/ObjectPickableWithTwoHands.srv
+++ /dev/null
@@ -1,3 +0,0 @@
-Perception perception
----
-bool pickable
\ No newline at end of file
diff --git a/simulators/package.xml b/simulators/package.xml
new file mode 100644
index 0000000..757de00
--- /dev/null
+++ b/simulators/package.xml
@@ -0,0 +1,18 @@
+
+
+
+ simulators
+ 0.0.0
+ TODO: Package description
+ sergio
+ TODO: License declaration
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+
+ ament_python
+
+
diff --git a/simulators/resource/simulators b/simulators/resource/simulators
new file mode 100644
index 0000000..e69de29
diff --git a/simulators/setup.cfg b/simulators/setup.cfg
new file mode 100644
index 0000000..488d3c9
--- /dev/null
+++ b/simulators/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/simulators
+[install]
+install_scripts=$base/lib/simulators
diff --git a/simulators/setup.py b/simulators/setup.py
new file mode 100644
index 0000000..1021334
--- /dev/null
+++ b/simulators/setup.py
@@ -0,0 +1,25 @@
+from setuptools import find_packages, setup
+
+package_name = 'simulators'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(exclude=['test']),
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='sergio',
+ maintainer_email='sergio.martinez3@udc.es',
+ description='TODO: Package description',
+ license='TODO: License declaration',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ ],
+ },
+)
diff --git a/simulators/simulators/__init__.py b/simulators/simulators/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/simulators/simulators/simulator.py b/simulators/simulators/simulator.py
new file mode 100644
index 0000000..dad4664
--- /dev/null
+++ b/simulators/simulators/simulator.py
@@ -0,0 +1,569 @@
+from enum import Enum
+import importlib
+import os.path
+import math
+import yaml
+import yamlloader
+import numpy
+from rclpy.node import Node
+
+from simulators_interfaces.srv import CalculateClosestPosition, ObjectPickableWithTwoHands, ObjectTooFar
+class World(Enum):
+ """Worlds to be simulated."""
+ GRIPPER_AND_LOW_FRICTION = 1
+ NO_GRIPPER_AND_HIGH_FRICTION = 2
+ GRIPPER_AND_LOW_FRICTION_TWO_BOXES = 3
+ KITCHEN = 4
+ GRIPPER_AND_LOW_FRICTION_SHORT_ARM = 5
+ GRIPPER_AND_LOW_FRICTION_DAMAGED_SERVO = 6
+ GRIPPER_AND_LOW_FRICTION_OBSTACLE = 7
+
+class Item(Enum):
+ """Types of objects."""
+
+ CYLINDER = 1
+ BOX = 2
+ SKILLET = 3
+ GRAPES = 4
+ APPLE = 5
+ TOMATO = 6
+ ORANGE = 7
+ CARROT = 8
+ LETTUCE = 9
+ PINEAPPLE = 10
+ WATERMELON = 11
+ EGGPLANT = 12
+ BANANA = 13
+
+class LTMSim(Node):
+ def __init__(self):
+ """Init attributes when a new object is created."""
+ self.rng = None
+ self.ident = None
+ self.last_reset_iteration = 0
+ self.world = None
+ self.base_messages = {}
+ self.perceptions = {}
+ self.publishers = {}
+ self.catched_object = None
+
+ self.normal_inner = numpy.poly1d(
+ numpy.polyfit([0.0, 0.3925, 0.785, 1.1775, 1.57], [0.45, 0.47, 0.525, 0.65, 0.9], 3)
+ )
+ self.normal_outer = numpy.poly1d(
+ numpy.polyfit([0.0, 0.3925, 0.785, 1.1775, 1.57], [1.15, 1.25, 1.325, 1.375, 1.375], 3)
+ )
+ self.short_outer = numpy.poly1d(
+ numpy.polyfit(
+ [0.0, 0.3925, 0.785, 1.1775, 1.57],
+ [1.15 * 0.8, 1.25 * 0.8, 1.325 * 0.8, 1.375 * 0.8, 1.375 * 0.8],
+ 3,
+ )
+ )
+
+ self.calculate_closest_position_service = self.create_service(
+ CalculateClosestPosition,
+ "simulator/calculate_closes_position",
+ self.calculate_closest_position_callback
+ )
+
+ self.object_pickable_with_two_hands_service = self.create_service(
+ ObjectPickableWithTwoHands,
+ "simulator/object_pickable_with_two_hands",
+ self.object_pickable_with_two_hands_callback
+ )
+
+ self.object_too_far_service = self.create_service(
+ ObjectTooFar,
+ "simulator/object_too_far",
+ self.object_too_far_callback
+ )
+
+ def calculate_closes_position_callback(self, request, response):
+ ang = request.angle
+ dist_near, ang_near = self.calculate_closest_position(ang)
+ response.dist_near = dist_near
+ response.ang_near = ang_near
+ return response
+
+ def calculate_closest_position(self, ang):
+ """Calculate the closest feasible position for an object taking into account the angle."""
+ dist = self.normal_inner(abs(ang))
+ y_coord = dist * math.sin(ang)
+ x_coord = dist * math.cos(ang)
+ if y_coord < -0.38:
+ y_coord = -0.38
+ x_coord = 0.35
+ elif y_coord > 0.38:
+ y_coord = 0.38
+ x_coord = 0.35
+ new_dist = 0.01 + numpy.linalg.norm([y_coord, x_coord])
+ new_ang = numpy.arctan2(y_coord, x_coord)
+ return new_dist, new_ang
+
+ def object_pickable_with_two_hands_callback(self, request, response):
+ dist = request.distance
+ ang = request.angle
+ pickable = self.object_pickable_with_two_hands(dist, ang)
+ response.pickable = pickable
+ return response
+
+ @staticmethod
+ def object_pickable_with_two_hands(dist, ang):
+ """Return True if the object is in a place where it can be picked with two hands."""
+ return abs(ang) <= 0.3925 and 0.46 <= dist <= 0.75
+
+ def object_too_far_callback(self, request, response):
+ dist = request.distance
+ ang = request.angle
+ too_far = self.object_too_far(dist, ang)
+ response.too_far = too_far
+ return response
+
+
+ def object_too_far(self, dist, ang):
+ """Return True if the object is out of range of the robot."""
+ if self.world.name == World.GRIPPER_AND_LOW_FRICTION_SHORT_ARM.name:
+ too_far = dist > self.short_outer(abs(ang))
+ elif self.world.name == World.GRIPPER_AND_LOW_FRICTION_OBSTACLE.name:
+ if abs(ang) < 0.17:
+ too_far = dist > self.short_outer(abs(ang))
+ else:
+ too_far = dist > self.normal_outer(abs(ang))
+ elif self.world.name == World.GRIPPER_AND_LOW_FRICTION_DAMAGED_SERVO.name:
+ too_far = (dist > self.normal_outer(abs(ang))) or (
+ ang > (0.8 * numpy.arctan2(1.07, 0.37))
+ )
+ else:
+ too_far = dist > self.normal_outer(abs(ang))
+ return too_far
+
+ def object_too_close(self, dist, ang):
+ """Return True if the object is too close to the robot to be caught."""
+ return dist < self.normal_inner(abs(ang))
+
+ @staticmethod
+ def object_outside_table(dist, ang):
+ """Return True if the object is outside the table. This is used in some scripts..."""
+ object_y = numpy.sin(ang) * dist
+ object_x = numpy.cos(ang) * dist
+ return not (-1.07 <= object_y <= 1.07 and 0.37 <= object_x <= 1.27)
+
+ @staticmethod
+ def object_is_small(rad):
+ """Return True if the ball is small, False if it is big. Right now, small is 0.03 and big 0.07."""
+ return rad <= 0.05
+
+ @staticmethod
+ def send_object_twohandsreachable(dist):
+ """Calculate the coordinates of the object when moving it to a place where it can be picked with two hands."""
+ x_coord = 0
+ y_coord = dist
+ if y_coord < 0.46:
+ y_coord = 0.46
+ elif y_coord > 0.75:
+ y_coord = 0.75
+ new_dist = numpy.linalg.norm([y_coord, x_coord])
+ return new_dist, 0
+
+ def send_object_outofreach(self, ang):
+ """Calculate the coordinates of the object when moving it out of reach."""
+ dist = self.normal_outer(abs(ang))
+ y_coord = dist * math.sin(ang)
+ x_coord = dist * math.cos(ang)
+ if y_coord < -1.07:
+ y_coord = -1.07
+ x_coord = 0.84
+ elif y_coord > 1.07:
+ y_coord = 1.07
+ x_coord = 0.84
+ new_dist = 0.01 + numpy.linalg.norm([y_coord, x_coord])
+ new_ang = numpy.arctan2(y_coord, x_coord)
+ return new_dist, new_ang
+
+ def object_in_close_box(self, dist, ang):
+ """Check if there is an object inside of a box."""
+ inside = False
+ for box in self.perceptions["boxes"].data:
+ if not self.object_too_far(box.distance, box.angle):
+ inside = (abs(box.distance - dist) < 0.05) and (abs(box.angle - ang) < 0.05)
+ if inside:
+ break
+ return inside
+
+ def object_in_far_box(self, dist, ang):
+ """Check if there is an object inside of a box."""
+ inside = False
+ for box in self.perceptions["boxes"].data:
+ if self.object_too_far(box.distance, box.angle):
+ inside = (abs(box.distance - dist) < 0.05) and (abs(box.angle - ang) < 0.05)
+ if inside:
+ break
+ return inside
+
+ def object_with_robot(self, dist, ang):
+ """Check if there is an object adjacent to the robot."""
+ together = False
+ if (not self.perceptions["ball_in_left_hand"].data) and (
+ not self.perceptions["ball_in_right_hand"].data
+ ):
+ dist_near, ang_near = self.calculate_closest_position(ang)
+ together = (abs(dist - dist_near) < 0.05) and (abs(ang - ang_near) < 0.05)
+ return together
+
+ def avoid_reward_by_chance(self, distance, angle):
+ """Avoid a reward situation obtained by chance."""
+ # This is necessary so sweep never puts the object close to the robot or colliding with a box.
+ # This is not realistic, it needs to be improved.
+ while (
+ self.object_with_robot(distance, angle)
+ or self.object_in_close_box(distance, angle)
+ or self.object_in_far_box(distance, angle)
+ ):
+ if distance > 0.65:
+ distance -= 0.10
+ else:
+ distance += 0.10
+ return distance
+
+ def reward_ball_in_box(self):
+ """Reward for object in box goal."""
+ self.perceptions["ball_in_box"].data = False
+ for cylinder in self.perceptions["cylinders"].data:
+ for box in self.perceptions["boxes"].data:
+ if (cylinder.distance == box.distance) and (cylinder.angle == box.angle):
+ self.perceptions["ball_in_box"].data = True
+ return True
+ return False
+
+
+ def reward_ball_with_robot(self):
+ """Reward for object with robot goal."""
+ self.perceptions["ball_with_robot"].data = False
+ for cylinder in self.perceptions["cylinders"].data:
+ dist, ang = self.calculate_closest_position(cylinder.angle)
+ if (cylinder.distance == dist) and (cylinder.angle == ang):
+ self.perceptions["ball_with_robot"].data = True
+ return True
+ return False
+
+ def reward_clean_area(self):
+ """Reward for cleaning the table goal."""
+ self.perceptions["clean_area"].data = False
+ for cylinder in self.perceptions["cylinders"].data:
+ for box in self.perceptions["boxes"].data:
+ if (
+ (cylinder.distance == box.distance)
+ and (cylinder.angle == box.angle)
+ and self.object_too_far(box.distance, box.angle)
+ ):
+ self.perceptions["clean_area"].data = True
+ return True
+ return False
+
+ def update_reward_sensor(self):
+ """Update goal sensors' values."""
+ for sensor in self.perceptions:
+ reward_method = getattr(self, "reward_" + sensor, None)
+ if callable(reward_method):
+ reward_method()
+
+
+ def random_position(self, in_valid=True, out_valid=True):
+ """Return a random position in the table."""
+ valid = False
+ while not valid:
+ object_y = self.rng.uniform(low=-1.07, high=1.07)
+ object_x = self.rng.uniform(low=0.37, high=1.27)
+ distance = numpy.linalg.norm([object_y, object_x])
+ angle = numpy.arctan2(object_y, object_x)
+ valid = not self.object_too_close(distance, angle)
+ # TODO: Ugly hacks to test curriculum learning
+ if self.last_reset_iteration >= 6000 and self.last_reset_iteration < 8000:
+ # if self.world == World.GRIPPER_AND_LOW_FRICTION_SHORT_ARM:
+ # max_distance = self.normal_outer(abs(angle))
+ # min_distance = self.short_outer(abs(angle))
+ # distance = self.rng.uniform(low=min_distance, high=max_distance)
+ if self.world == World.GRIPPER_AND_LOW_FRICTION_DAMAGED_SERVO:
+ max_angle = numpy.arctan2(1.07, 0.37)
+ min_angle = 0.8 * numpy.arctan2(1.07, 0.37)
+ if angle < min_angle:
+ angle = self.rng.uniform(low=min_angle, high=max_angle)
+ elif self.world == World.GRIPPER_AND_LOW_FRICTION_OBSTACLE:
+ angle = self.rng.uniform(low=-0.17, high=0.17)
+ max_distance = self.normal_outer(abs(angle))
+ min_distance = self.short_outer(abs(angle))
+ distance = self.rng.uniform(low=min_distance, high=max_distance)
+ if not in_valid:
+ valid = valid and self.object_too_far(distance, angle)
+ if not out_valid:
+ valid = valid and not self.object_too_far(distance, angle)
+ if valid:
+ for box in self.perceptions["boxes"].data:
+ if (abs(box.distance - distance) < 0.15) and (abs(box.angle - angle) < 0.15):
+ valid = False
+ break
+ if valid:
+ for cylinder in self.perceptions["cylinders"].data:
+ if (abs(cylinder.distance - distance) < 0.15) and (
+ abs(cylinder.angle - angle) < 0.15
+ ):
+ valid = False
+ break
+ return distance, angle
+
+
+ def random_perceptions(self):
+ """Randomize the state of the environment."""
+ # Objects
+ self.catched_object = None
+ if self.world in [
+ World.GRIPPER_AND_LOW_FRICTION,
+ World.NO_GRIPPER_AND_HIGH_FRICTION,
+ World.GRIPPER_AND_LOW_FRICTION_SHORT_ARM,
+ World.GRIPPER_AND_LOW_FRICTION_OBSTACLE,
+ World.GRIPPER_AND_LOW_FRICTION_DAMAGED_SERVO,
+ ]:
+ self.perceptions["boxes"].data = []
+ distance, angle = self.random_position(in_valid=True, out_valid=True)
+ self.perceptions["boxes"].data.append(self.base_messages["boxes"]())
+ self.perceptions["boxes"].data[0].distance = distance
+ self.perceptions["boxes"].data[0].angle = angle
+ self.perceptions["boxes"].data[0].diameter = 0.12
+ # self.perceptions["boxes"].data[0].id = Item.box.value
+ self.perceptions["cylinders"].data = []
+ distance, angle = self.random_position(in_valid=True, out_valid=True)
+ self.perceptions["cylinders"].data.append(self.base_messages["cylinders"]())
+ self.perceptions["cylinders"].data[0].distance = distance
+ self.perceptions["cylinders"].data[0].angle = angle
+ if self.rng.uniform() > 0.5:
+ self.perceptions["cylinders"].data[0].diameter = 0.03
+ else:
+ self.perceptions["cylinders"].data[0].diameter = 0.07
+ # self.perceptions["cylinders"].data[0].id = Item.cylinder.value
+ self.perceptions["ball_in_left_hand"].data = False
+ self.perceptions["ball_in_right_hand"].data = False
+ object_distance = self.perceptions["cylinders"].data[0].distance
+ object_angle = self.perceptions["cylinders"].data[0].angle
+ if (
+ (World.GRIPPER_AND_LOW_FRICTION.name in self.world.name)
+ and self.object_is_small(object_distance)
+ and (not self.object_too_far(object_distance, object_angle))
+ and (self.rng.uniform() > 0.5)
+ ):
+ self.catched_object = self.perceptions["cylinders"].data[0]
+ if object_angle > 0:
+ self.perceptions["ball_in_left_hand"].data = True
+ self.perceptions["ball_in_right_hand"].data = False
+ else:
+ self.perceptions["ball_in_left_hand"].data = False
+ self.perceptions["ball_in_right_hand"].data = True
+ if self.object_pickable_with_two_hands(distance, angle) and (self.rng.uniform() > 0.5):
+ self.catched_object = self.perceptions["cylinders"].data[0]
+ self.perceptions["ball_in_left_hand"].data = True
+ self.perceptions["ball_in_right_hand"].data = True
+ elif self.world == World.GRIPPER_AND_LOW_FRICTION_TWO_BOXES:
+ self.perceptions["boxes"].data = []
+ distance, angle = self.random_position(in_valid=True, out_valid=False)
+ self.perceptions["boxes"].data.append(self.base_messages["boxes"]())
+ self.perceptions["boxes"].data[0].distance = distance
+ self.perceptions["boxes"].data[0].angle = angle
+ self.perceptions["boxes"].data[0].diameter = 0.12
+ # self.perceptions["boxes"].data[0].id = Item.box.value
+ distance, angle = self.random_position(in_valid=False, out_valid=True)
+ self.perceptions["boxes"].data.append(self.base_messages["boxes"]())
+ self.perceptions["boxes"].data[1].distance = distance
+ self.perceptions["boxes"].data[1].angle = angle
+ self.perceptions["boxes"].data[1].diameter = 0.12
+ # self.perceptions["boxes"].data[1].id = Item.box.value
+ self.perceptions["cylinders"].data = []
+ distance, angle = self.random_position(in_valid=True, out_valid=False)
+ self.perceptions["cylinders"].data.append(self.base_messages["cylinders"]())
+ self.perceptions["cylinders"].data[0].distance = distance
+ self.perceptions["cylinders"].data[0].angle = angle
+ if self.rng.uniform() > 0.5:
+ self.perceptions["cylinders"].data[0].diameter = 0.03
+ else:
+ self.perceptions["cylinders"].data[0].diameter = 0.07
+ # self.perceptions["cylinders"].data[0].id = Item.cylinder.value
+ self.perceptions["ball_in_left_hand"].data = False
+ self.perceptions["ball_in_right_hand"].data = False
+ elif self.world == World.KITCHEN:
+ self.perceptions["boxes"].data = []
+ self.perceptions["boxes"].data.append(self.base_messages["boxes"]())
+ self.perceptions["boxes"].data[0].distance = 0.605
+ self.perceptions["boxes"].data[0].angle = 0.0
+ self.perceptions["boxes"].data[0].diameter = 0.12
+ # self.perceptions["boxes"].data[0].id = Item.skillet.value
+ self.perceptions["cylinders"].data = []
+ distance, angle = self.random_position(in_valid=True, out_valid=True)
+ self.perceptions["cylinders"].data.append(self.base_messages["cylinders"]())
+ self.perceptions["cylinders"].data[0].distance = distance
+ self.perceptions["cylinders"].data[0].angle = angle
+ self.perceptions["cylinders"].data[0].diameter = 0.03
+ # self.perceptions["cylinders"].data[0].id = Item.carrot.value
+ distance, angle = self.random_position(in_valid=True, out_valid=True)
+ self.perceptions["cylinders"].data.append(self.base_messages["cylinders"]())
+ self.perceptions["cylinders"].data[1].distance = distance
+ self.perceptions["cylinders"].data[1].angle = angle
+ self.perceptions["cylinders"].data[1].diameter = 0.03
+ # self.perceptions["cylinders"].data[1].id = Item.eggplant.value
+ distance, angle = self.random_position(in_valid=True, out_valid=True)
+ self.perceptions["cylinders"].data.append(self.base_messages["cylinders"]())
+ self.perceptions["cylinders"].data[2].distance = distance
+ self.perceptions["cylinders"].data[2].angle = angle
+ self.perceptions["cylinders"].data[2].diameter = 0.03
+ # self.perceptions["cylinders"].data[2].id = Item.orange.value
+ self.perceptions["ball_in_left_hand"].data = False
+ self.perceptions["ball_in_right_hand"].data = False
+ else:
+ self.get_logger().info("Unknown world received in simulator!")
+ # Goal sensors
+ self.update_reward_sensor()
+
+ def grasp_object_policy(self):
+ """Grasp an object with a gripper."""
+ if not self.catched_object:
+ for cylinder in self.perceptions["cylinders"].data:
+ if (
+ World.GRIPPER_AND_LOW_FRICTION.name in self.world.name
+ and (
+ not self.object_too_far(cylinder.distance, cylinder.angle)
+ )
+ and self.object_is_small(cylinder.diameter)
+ ):
+ if cylinder.angle > 0.0:
+ self.perceptions["ball_in_left_hand"].data = True
+ else:
+ self.perceptions["ball_in_right_hand"].data = True
+ self.catched_object = cylinder
+ break
+
+ def grasp_with_two_hands_policy(self):
+ """Grasp an object using both arms."""
+ if not self.catched_object:
+ for cylinder in self.perceptions["cylinders"].data:
+ if (self.object_pickable_with_two_hands(cylinder.distance, cylinder.angle)) and (
+ (World.NO_GRIPPER_AND_HIGH_FRICTION.name in self.world.name)
+ or (not self.object_is_small(cylinder.diameter))
+ ):
+ self.perceptions["ball_in_left_hand"].data = True
+ self.perceptions["ball_in_right_hand"].data = True
+ self.catched_object = cylinder
+ break
+
+
+
+ def change_hands_policy(self):
+ """Exchange an object from one hand to the other one."""
+ if self.perceptions["ball_in_left_hand"].data and (
+ not self.perceptions["ball_in_right_hand"].data
+ ):
+ self.perceptions["ball_in_left_hand"].data = False
+ self.perceptions["ball_in_right_hand"].data = True
+ self.catched_object.angle = -self.catched_object.angle
+ self.catched_object.distance = self.avoid_reward_by_chance(
+ self.catched_object.distance, self.catched_object.angle
+ )
+ elif (not self.perceptions["ball_in_left_hand"].data) and self.perceptions[
+ "ball_in_right_hand"
+ ].data:
+ self.perceptions["ball_in_left_hand"].data = True
+ self.perceptions["ball_in_right_hand"].data = False
+ self.catched_object.angle = -self.catched_object.angle
+ self.catched_object.distance = self.avoid_reward_by_chance(
+ self.catched_object.distance, self.catched_object.angle
+ )
+
+ def sweep_object_policy(self):
+ """Sweep an object to the front of the robot."""
+ if not self.catched_object:
+ for cylinder in self.perceptions["cylinders"].data:
+ if not self.object_too_far(cylinder.distance, cylinder.angle):
+ sign = numpy.sign(cylinder.angle)
+ (
+ cylinder.distance,
+ cylinder.angle,
+ ) = self.send_object_twohandsreachable(cylinder.distance)
+ if (
+ World.GRIPPER_AND_LOW_FRICTION.name in self.world.name
+ ) and self.object_is_small(cylinder.diameter):
+ cylinder.angle = -0.4 * sign
+ cylinder.distance = self.avoid_reward_by_chance(
+ cylinder.distance, cylinder.angle
+ )
+ break
+
+ def put_object_in_box_policy(self):
+ """Put an object into the box."""
+ if self.catched_object:
+ for box in self.perceptions["boxes"].data:
+ if (not self.object_too_far(box.distance, box.angle)) and (
+ ((box.angle > 0.0) and self.perceptions["ball_in_left_hand"].data)
+ or ((box.angle <= 0.0) and self.perceptions["ball_in_right_hand"].data)
+ ):
+ self.catched_object.distance = box.distance
+ self.catched_object.angle = box.angle
+ self.perceptions["ball_in_left_hand"].data = False
+ self.perceptions["ball_in_right_hand"].data = False
+ self.catched_object = None
+ break
+
+ def put_object_with_robot_policy(self):
+ """Put an object as close to the robot as possible."""
+ if self.catched_object:
+ (
+ self.catched_object.distance,
+ self.catched_object.angle,
+ ) = self.calculate_closest_position(self.catched_object.angle)
+ # Box and cylinder collide.
+ # This is not realistic, it needs to be improved.
+ if self.object_in_close_box(self.catched_object.distance, self.catched_object.angle):
+ self.catched_object.angle += 0.10
+ self.perceptions["ball_in_left_hand"].data = False
+ self.perceptions["ball_in_right_hand"].data = False
+ self.catched_object = None
+
+ def throw_policy(self):
+ """Throw an object."""
+ if self.catched_object:
+ for box in self.perceptions["boxes"].data:
+ if self.object_too_far(box.distance, box.angle) and (
+ (box.angle > 0.0 and self.perceptions["ball_in_left_hand"].data)
+ or (box.angle <= 0.0 and self.perceptions["ball_in_right_hand"].data)
+ ):
+ self.catched_object.distance = box.distance
+ self.catched_object.angle = box.angle
+ # else:
+ # self.catched_object.distance, self.catched_object.angle = self.send_object_outofreach(
+ # self.catched_object.angle, self.world.name
+ # )
+ self.perceptions["ball_in_left_hand"].data = False
+ self.perceptions["ball_in_right_hand"].data = False
+ self.catched_object = None
+ break
+
+ def ask_nicely_policy(self):
+ """Ask someone to bring the object closer to us."""
+ if not self.catched_object:
+ for cylinder in self.perceptions["cylinders"].data:
+ if self.object_too_far(cylinder.distance, cylinder.angle):
+ self.get_logger().info("Object too far in " + self.world.name)
+ if self.world == World.GRIPPER_AND_LOW_FRICTION_SHORT_ARM:
+ distance = self.short_outer(abs(cylinder.angle))
+ elif self.world == World.GRIPPER_AND_LOW_FRICTION_DAMAGED_SERVO:
+ max_allowed_angle = 0.8 * numpy.arctan2(1.07, 0.37)
+ if cylinder.angle > max_allowed_angle:
+ cylinder.angle = max_allowed_angle - 0.1
+ distance = self.normal_outer(abs(cylinder.angle))
+ elif self.world == World.GRIPPER_AND_LOW_FRICTION_OBSTACLE:
+ if abs(cylinder.angle) < 0.17:
+ distance = self.short_outer(abs(cylinder.angle))
+ else:
+ distance = self.normal_outer(abs(cylinder.angle))
+ else:
+ distance = self.normal_outer(abs(cylinder.angle))
+ cylinder.distance = self.avoid_reward_by_chance(distance - 0.1, cylinder.angle)
+ break
+
+
\ No newline at end of file
diff --git a/simulators/test/test_copyright.py b/simulators/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/simulators/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/simulators/test/test_flake8.py b/simulators/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/simulators/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/simulators/test/test_pep257.py b/simulators/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/simulators/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
diff --git a/simulators_interfaces/CMakeLists.txt b/simulators_interfaces/CMakeLists.txt
new file mode 100644
index 0000000..64c5ce9
--- /dev/null
+++ b/simulators_interfaces/CMakeLists.txt
@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.8)
+project(simulators_interfaces)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package( REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+ "srv/CalculateClosestPosition.srv"
+ "srv/ObjectPickableWithTwoHands.srv"
+ "srv/ObjectTooFar.srv"
+)
+
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ # the following line skips the linter which checks for copyrights
+ # comment the line when a copyright and license is added to all source files
+ set(ament_cmake_copyright_FOUND TRUE)
+ # the following line skips cpplint (only works in a git repo)
+ # comment the line when this package is in a git repo and when
+ # a copyright and license is added to all source files
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/simulators_interfaces/package.xml b/simulators_interfaces/package.xml
new file mode 100644
index 0000000..67efdb2
--- /dev/null
+++ b/simulators_interfaces/package.xml
@@ -0,0 +1,22 @@
+
+
+
+ simulators_interfaces
+ 0.0.0
+ TODO: Package description
+ sergio
+ TODO: License declaration
+
+ ament_cmake
+ rosidl_default_generators
+
+ rosidl_default_runtime
+ rosidl_interface_packages
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
diff --git a/cognitive_node_interfaces/srv/CalculateClosestPosition.srv b/simulators_interfaces/srv/CalculateClosestPosition.srv
similarity index 100%
rename from cognitive_node_interfaces/srv/CalculateClosestPosition.srv
rename to simulators_interfaces/srv/CalculateClosestPosition.srv
diff --git a/simulators_interfaces/srv/ObjectPickableWithTwoHands.srv b/simulators_interfaces/srv/ObjectPickableWithTwoHands.srv
new file mode 100644
index 0000000..9dc891b
--- /dev/null
+++ b/simulators_interfaces/srv/ObjectPickableWithTwoHands.srv
@@ -0,0 +1,4 @@
+float32 distance
+float32 angle
+---
+bool pickable
\ No newline at end of file
diff --git a/cognitive_node_interfaces/srv/ObjectTooFar.srv b/simulators_interfaces/srv/ObjectTooFar.srv
similarity index 100%
rename from cognitive_node_interfaces/srv/ObjectTooFar.srv
rename to simulators_interfaces/srv/ObjectTooFar.srv