Merge pull request #196 from eager-dev/fix-demo

Fix demo

eager_bridges/eager_bridge_gazebo/src/eager_bridge_gazebo/gazebo_bridge.py

@@ -238,7 +238,8 @@ def _init_actuators(self, topic, name, actuators):
                     action_server_name, valid_servers))
             get_action_srv = rospy.ServiceProxy(topic + "/actuators/" + actuator, get_message_from_def(space))
             set_action_srv = self._action_servers[name][actuator].act
-            self._actuator_services[name][actuator] = (get_action_srv, set_action_srv)
+            reset_action_srv = self._action_servers[name][actuator].reset
+            self._actuator_services[name][actuator] = (get_action_srv, set_action_srv, reset_action_srv)
 
     def _init_states(self, topic, name, states):
         self._state_buffer[name] = {}
@@ -397,7 +398,7 @@ def _step(self):
             self.stepped = True
         for robot in self._actuator_services:
             for actuator in self._actuator_services[robot]:
-                (get_action_srv, set_action_srv) = self._actuator_services[robot][actuator]
+                (get_action_srv, set_action_srv, reset_action_srv) = self._actuator_services[robot][actuator]
                 actions = get_action_srv()
                 set_action_srv(actions.value)
         self._step_world(self.step_request)
@@ -407,6 +408,10 @@ def _step(self):
         return True
 
     def _reset(self, req):
+        for robot in self._actuator_services:
+            for actuator in self._actuator_services[robot]:
+                (get_action_srv, set_action_srv, reset_action_srv) = self._actuator_services[robot][actuator]
+                reset_action_srv()
         # First we switch off all controllers
         for controller in self.controller_list:
             service = controller['service']
@@ -452,7 +457,6 @@ def _reset(self, req):
             self._unpause_physics()
             service(self.switch_controller_request)
             self._pause_physics()
-        # Finally, we need to step once in order to make the controllers active
         self.stepped = False
         return response
 

eager_bridges/eager_bridge_real/src/eager_bridge_real/real_bridge.py

@@ -168,7 +168,10 @@ def _init_states(self, topic, name, states):
 
     def _sensor_callback(self, data, name, sensor, attribute):
         if self._remap_publishers[name][sensor]:
-            self._remap_publishers[name][sensor].publish(data)
+            try:
+                self._remap_publishers[name][sensor].publish(data)
+            except rospy.exceptions.ROSException:
+                pass
         data_list = getattr(data, attribute)
         self._sensor_buffer[name][sensor] = data_list
 

eager_bridges/eager_bridge_real/src/eager_bridge_real/real_engine.py

@@ -11,7 +11,7 @@ class RealEngine(EngineParams):
     """
 
     def __init__(self,
-                 dt: float = 0.08):
+                 dt: float = 0.4):
         # Only define variables (locally) you wish to store on the parameter server (done in baseclass constructor).
         bridge_type = 'real'
         launch_file = '$(find eager_bridge_%s)/launch/%s.launch' % (bridge_type, bridge_type)

eager_bridges/eager_bridge_webots/src/eager_bridge_webots/webots_bridge.py

@@ -56,6 +56,7 @@ def __init__(self):
         self._state_services = []
 
         self._reset_services = dict()
+        self._remap_publishers = dict()
 
         super(WeBotsBridge, self).__init__("webots")
 
@@ -121,6 +122,7 @@ def _register_object(self, topic, name, package, object_type, args, config):
 
     def _init_sensors(self, topic, name, sensors):
         self._sensor_buffer[name] = dict()
+        self._remap_publishers[name] = dict()
         for sensor_name in sensors:
             sensor = sensors[sensor_name]
             topic_list = sensor['names']
@@ -138,6 +140,11 @@ def _init_sensors(self, topic, name, sensors):
                     if 'type' in sensor:
                         sens_type = sensor['type']
                         if sens_type == 'camera':
+                            self._remap_publishers[name][sensor_name] = None
+                            if 'remap' in sensor:
+                                if sensor['remap']:
+                                    self._remap_publishers[name][sensor_name] = rospy.Publisher(
+                                        topic + "/sensors/" + sensor_name, Image, queue_size=1)
                             assert len(topic_list) == 1  # Temp check
                             self._sensor_subscribers.append(
                                 rospy.Subscriber(
@@ -281,6 +288,11 @@ def _sensor_callback(self, data, name, sensor, pos):
         self._sensor_buffer[name][sensor][pos] = data.data
 
     def _camera_callback(self, data, name, sensor):
+        if self._remap_publishers[name][sensor]:
+            try:
+                self._remap_publishers[name][sensor].publish(data)
+            except rospy.exceptions.ROSException:
+                pass
         # TODO: Standardize image encoding
         self._sensor_buffer[name][sensor] = data.data
 

eager_core/src/eager_core/action_server.py

@@ -21,6 +21,9 @@ def __init__(self, action_server, action_type):
     def act(self, action):
         self.client.send_goal(action)
 
+    def reset(self):
+        self.client.cancel_all_goals()
+
 
 class FollowJointTrajectoryActionServer(ActionServer):
     def __init__(self, joint_names, server_name, duration=0.5):

eager_core/src/eager_core/eager_env.py

@@ -210,7 +210,7 @@ class EagerEnv(BaseEagerEnv):
     """
 
     def __init__(self, engine: EngineParams, objects: List[Object] = [], observers: List['Observer'] = [],
-                 name: str = 'ros_env', render_sensor: Sensor = None, max_steps: int = None,
+                 name: str = 'ros_env', render_sensor: Sensor = None, max_steps: int = 20,
                  reward_fn: Callable[['EagerEnv', 'OrderedDict[str, object]'], float] = None,
                  is_done_fn: Callable[['EagerEnv', 'OrderedDict[str, object]'], bool] = None,
                  reset_fn: Callable[['EagerEnv'], None] = None) -> None:
@@ -270,7 +270,7 @@ def reset(self) -> 'OrderedDict[str, object]':
         """
         self.steps = 0
         states_to_reset = self.state_space.sample()
-        done = False
+        done = states_to_reset is None
         while not done:
             reset_dict = self._reset_fn(self)
             for obj_name, object in reset_dict.items():

eager_processors/eager_process_safe_actions/config/ur5e.yaml

@@ -2,7 +2,7 @@ moveit_package: ur5_e_moveit_config
 urdf_path: '$(find ur_e_description)/urdf/ur5e_robot.urdf.xacro'
 joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
 group_name: manipulator
-sensor_name: joint_sensors
+sensor_name: joint_pos
 base_frame: base_link
 base_length: 0.4
 workspace_length: 2.4

eager_processors/eager_process_safe_actions/src/eager_process_safe_actions/safe_actions.py

@@ -151,8 +151,6 @@ def _reset(self):
     def _process_action(self, action, observation):
         current_position = observation[self.robot_name][self.sensor_name]
         safe_action = self._getSafeAction(np.asarray(action), np.asarray(current_position))
-        if safe_action is None:
-            safe_action = current_position
         return safe_action
 
     def _getSafeAction(self, goal_position, current_position):
@@ -194,8 +192,11 @@ def _getSafeAction(self, goal_position, current_position):
             gsvr.robot_state.joint_state.position = way_points[i, :]
             if not self.state_validity_service.call(gsvr).valid:
                 if i == 0:
-                    rospy.logwarn("Current state in collision!")
-                    return self.previous_position
+                    # rospy.logwarn("Current state in collision!")
+                    goal_position = current_position
+                    if self.previous_position is not None and np.max(np.abs(current_position - self.previous_position) / self.duration)  < self.vel_limit:
+                        goal_position = self.previous_position
+                    return goal_position
                 self.previous_position = current_position
                 return way_points[i-1, :]
         self.previous_position = current_position

eager_processors/eager_process_safe_actions/src/eager_process_safe_actions/safe_actions_processor.py

@@ -4,10 +4,11 @@
 class SafeActionsProcessor(EngineParams):
     def __init__(self,
                  robot_type: str,
-                 checks_per_rad: int,
-                 duration: float,
                  vel_limit: float,
-                 collision_height: float):
+                 collision_height: float,
+                 checks_per_rad: int=15,
+                 duration: float=0.4,
+                 ):
         launch_file = '$(find eager_process_safe_actions)/launch/safe_actions.launch'
 
         kwargs = locals().copy()

eager_robots/eager_robot_vx300s/config/vx300s.yaml

@@ -21,8 +21,8 @@ actuators:
 states:
   joint_pos:
     type: boxf32
-    high: [3.14158, 1.25663, 1.605702, 3.14158, 2.23402, 3.14158]
-    low: [-3.14158, -1.85004, -1.76278, -3.14158, -1.86750, -3.14158]
+    high: [3.14158, 0.628315, 1.605702, 3.14158, 2.23402, 3.14158]
+    low: [-3.14158, -0.92502, -1.76278, -3.14158, -1.86750, -3.14158]
   joint_vel:
     type: boxf32
     high: [3.14159, 3.14159, 3.14159, 3.14159, 3.14159, 3.14159]