From 9ea29499ff23ce60878945a6cce6185b592dd2f9 Mon Sep 17 00:00:00 2001 From: jelledouwe Date: Wed, 15 Sep 2021 15:43:53 +0200 Subject: [PATCH 1/8] Fix rgb/bgr rendering --- eager_core/src/eager_core/render_node.py | 6 +- .../eager_robot_vx300s/config/vx300s.yaml | 2 +- eager_robots/eager_robot_vx300s/vx300s.urdf | 455 ++++++++++++++++++ 3 files changed, 460 insertions(+), 3 deletions(-) create mode 100644 eager_robots/eager_robot_vx300s/vx300s.urdf diff --git a/eager_core/src/eager_core/render_node.py b/eager_core/src/eager_core/render_node.py index 7053e23..79713b7 100755 --- a/eager_core/src/eager_core/render_node.py +++ b/eager_core/src/eager_core/render_node.py @@ -25,9 +25,11 @@ def render(self): if self.image_ros is None: return try: + cv_image = self.bridge.imgmsg_to_cv2(self.image_ros) # Related issue: https://github.com/ros-perception/vision_opencv/issues/207 - cv_image = np.frombuffer(self.image_ros.data, dtype=np.uint8).reshape(self.image_ros.height, self.image_ros.width, -1) - cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR) + # cv_image = np.frombuffer(self.image_ros.data, dtype=np.uint8).reshape(self.image_ros.height, self.image_ros.width, -1) + if not self.image_ros.encoding == 'bgra8': + cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR) except CvBridgeError as e: print(e) return diff --git a/eager_robots/eager_robot_vx300s/config/vx300s.yaml b/eager_robots/eager_robot_vx300s/config/vx300s.yaml index 32979f4..96dd27b 100644 --- a/eager_robots/eager_robot_vx300s/config/vx300s.yaml +++ b/eager_robots/eager_robot_vx300s/config/vx300s.yaml @@ -21,7 +21,7 @@ actuators: states: joint_pos: type: boxf32 - high: [3.14158, 0.628315, 1.605702, 3.14158, 2.23402, 3.14158] + high: [3.14158, 0.628315, 0.802851, 3.14158, 2.23402, 3.14158] low: [-3.14158, -0.92502, -1.76278, -3.14158, -1.86750, -3.14158] joint_vel: type: boxf32 diff --git a/eager_robots/eager_robot_vx300s/vx300s.urdf b/eager_robots/eager_robot_vx300s/vx300s.urdf new file mode 100644 index 0000000..f886de1 --- /dev/null +++ b/eager_robots/eager_robot_vx300s/vx300s.urdf @@ -0,0 +1,455 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 0e6eec5f79cf9985d6e07a71f0e3c8b2e233b7d8 Mon Sep 17 00:00:00 2001 From: jelledouwe Date: Wed, 15 Sep 2021 15:45:26 +0200 Subject: [PATCH 2/8] Automate calibration routine --- .../eager_calibration/launch/calibrate.launch | 4 + .../launch/eager_calibration.launch | 8 +- .../eager_calibration_node.py | 83 +++++++++++-------- .../eager_calibration_rqt.py | 4 + 4 files changed, 62 insertions(+), 37 deletions(-) diff --git a/examples/demo/eager_calibration/launch/calibrate.launch b/examples/demo/eager_calibration/launch/calibrate.launch index 3addd16..1a7a802 100644 --- a/examples/demo/eager_calibration/launch/calibrate.launch +++ b/examples/demo/eager_calibration/launch/calibrate.launch @@ -20,6 +20,8 @@ + + @@ -44,6 +46,8 @@ + + diff --git a/examples/demo/eager_calibration/launch/eager_calibration.launch b/examples/demo/eager_calibration/launch/eager_calibration.launch index bc44469..191f546 100644 --- a/examples/demo/eager_calibration/launch/eager_calibration.launch +++ b/examples/demo/eager_calibration/launch/eager_calibration.launch @@ -30,13 +30,15 @@ - - + + + + @@ -117,5 +119,7 @@ + + diff --git a/examples/demo/eager_calibration/src/eager_calibration/eager_calibration_node.py b/examples/demo/eager_calibration/src/eager_calibration/eager_calibration_node.py index a861af6..e15a325 100755 --- a/examples/demo/eager_calibration/src/eager_calibration/eager_calibration_node.py +++ b/examples/demo/eager_calibration/src/eager_calibration/eager_calibration_node.py @@ -44,8 +44,8 @@ def __init__(self): self.collision_height = rospy.get_param('~collision_height', 0.2) self.base_length = rospy.get_param('~base_length', 0.4) self.workspace_length = rospy.get_param('~workspace_length', 2.4) - self.velocity_scaling_factor = rospy.get_param('~velocity_scaling_factor', 0.75) - self.acceleration_scaling_factor = rospy.get_param('~acceleration_scaling_factor', 0.75) + self.velocity_scaling_factor = rospy.get_param('~velocity_scaling_factor', 0.3) + self.acceleration_scaling_factor = rospy.get_param('~acceleration_scaling_factor', 0.3) # Calibration prefix self.namespace_prefix = rospy.get_param('~namespace_prefix', 'hand_eye_calibration') @@ -53,58 +53,58 @@ def __init__(self): # Calibration poses calibration_pose_1 = rospy.get_param( '~calibration_pose_1', - [-0.0076699042692780495, 0.15800002217292786, -0.42337870597839355, - -0.0015339808305725455, 0.8390874862670898, -0.003067961661145091] + [-0.0076699042692780495, 0.2070874124765396, -0.5138835906982422, + -0.006135923322290182, 0.9618059992790222, -0.003067961661145091] ) calibration_pose_2 = rospy.get_param( '~calibration_pose_2', - [0.19634954631328583, 0.26077672839164734, -0.526155412197113, - -0.8053399324417114, 1.084524393081665, 0.5890486240386963] + [-0.0076699042692780495, 0.5813787579536438, -1.2179807424545288, + -0.006135923322290182, 1.636757493019104, -0.003067961661145091] ) calibration_pose_3 = rospy.get_param( '~calibration_pose_3', - [0.28378644585609436, 0.43104860186576843, -0.7179030179977417, - -1.118272066116333, 1.366776943206787, 0.7133010625839233] + [0.14726215600967407, 0.18867963552474976, -0.36968937516212463, + -0.7086991667747498, 0.8620972037315369, 0.5875146389007568] ) calibration_pose_4 = rospy.get_param( '~calibration_pose_4', - [0.2822524607181549, 0.46019425988197327, -0.7071651816368103, - -1.1167380809783936, 1.366776943206787, 0.5767768025398254] + [0.2899223864078522, 0.42644667625427246, -0.644271969795227, + -1.1919031143188477, 1.3299614191055298, 0.8436894416809082] ) calibration_pose_5 = rospy.get_param( '~calibration_pose_5', - [0.3129321038722992, 0.849825382232666, -1.2026410102844238, - -1.5569905042648315, 1.6889128684997559, 0.5706408619880676] + [-0.28378644585609436, 0.3834952116012573, -0.5737088322639465, + 1.0967962741851807, 1.2333205938339233, -0.22396120429039001] ) calibration_pose_6 = rospy.get_param( '~calibration_pose_6', - [0.3129321038722992, 0.8636311888694763, -1.2072429656982422, - -1.558524489402771, 1.6889128684997559, 0.9710098505020142] + [-0.2822524607181549, 0.3834952116012573, -0.575242817401886, + 1.0967962741851807, 1.2333205938339233, -0.9295923709869385] ) calibration_pose_7 = rospy.get_param( '~calibration_pose_7', - [0.31446605920791626, 0.5154175758361816, -0.6043884754180908, - -1.4419419765472412, 1.3959225416183472, 1.2624661922454834] + [-0.0015339808305725455, 0.5476311445236206, -1.1627575159072876, + -0.03374757990241051, 1.575398325920105, 0.4356505572795868] ) calibration_pose_8 = rospy.get_param( '~calibration_pose_8', - [-0.026077674701809883, 0.1733398288488388, -0.24543693661689758, - -0.8559613227844238, 0.7056311964988708, 0.6043884754180908] + [-0.0076699042692780495, 0.526155412197113, -1.2241166830062866, + 0.0, 1.5953400135040283, 0.0015339808305725455] ) calibration_pose_9 = rospy.get_param( '~calibration_pose_9', - [-0.03221359848976135, 0.1733398288488388, -0.0951068103313446, - -1.1428157091140747, 0.5276893973350525, 0.8682331442832947] + [-0.0076699042692780495, 0.526155412197113, -1.2241166830062866, + 0.0, 1.5953400135040283, 0.0015339808305725455] ) calibration_pose_10 = rospy.get_param( '~calibration_pose_10', - [0.06749515980482101, 0.2715145945549011, -0.17487381398677826, - -1.4695535898208618, 0.9157865643501282, 1.1566215753555298] + [-0.00920388475060463, 0.598252534866333, -1.1949710845947266, + -0.003067961661145091, 1.5999419689178467, -0.6258642077445984] ) calibration_pose_11 = rospy.get_param( '~calibration_pose_11', - [0.06749515980482101, 0.28071850538253784, -0.1764077991247177, - -1.4695535898208618, 0.9157865643501282, 1.402058482170105] + [-0.5108156204223633, -0.11658254265785217, 0.2070874124765396, + 0.9295923709869385, 0.6734175682067871, -0.9418642520904541] ) calibration_pose_12 = rospy.get_param( '~calibration_pose_12', @@ -112,25 +112,35 @@ def __init__(self): -0.2346990704536438, 0.8022719621658325, -0.21322333812713623] ) calibration_pose_13 = rospy.get_param( - '~calibration_pose_12', + '~calibration_pose_13', [-0.16260196268558502, 0.5414952039718628, -0.951068103313446, 0.12118448317050934, 1.1274758577346802, -0.653475821018219] ) calibration_pose_14 = rospy.get_param( - '~calibration_pose_12', + '~calibration_pose_14', [-0.23930101096630096, 0.36201947927474976, -0.49087387323379517, 0.607456386089325, 0.6657477021217346, -1.0737866163253784] ) calibration_pose_15 = rospy.get_param( - '~calibration_pose_12', + '~calibration_pose_15', [-0.31446605920791626, 0.552233099937439, -0.8881748914718628, 0.7623884677886963, 1.1397477388381958, -1.1075341701507568] ) calibration_pose_16 = rospy.get_param( - '~calibration_pose_12', + '~calibration_pose_16', [-0.2791845202445984, 0.5721748471260071, -0.9710098505020142, 0.6013205051422119, 1.1842331886291504, -0.7470486760139465] ) + calibration_pose_17 = rospy.get_param( + '~calibration_pose_17', + [-0.18867963552474976, 0.3436117172241211, -0.5583690404891968, + -0.8283496499061584, 0.8559613227844238, 0.5905826091766357] + ) + calibration_pose_18 = rospy.get_param( + '~calibration_pose_18', + [-0.41724279522895813, 0.49547579884529114, -1.0139613151550293, + 0.19788353145122528, 1.24252450466156, -0.374291330575943] + ) self.calibration_poses = np.asarray([calibration_pose_1, calibration_pose_2, calibration_pose_3, @@ -141,12 +151,14 @@ def __init__(self): calibration_pose_8, calibration_pose_9, calibration_pose_10, - calibration_pose_11, - calibration_pose_12, - calibration_pose_13, - calibration_pose_14, - calibration_pose_15, - calibration_pose_16, + # calibration_pose_11, + # calibration_pose_12, + # calibration_pose_13, + # calibration_pose_14, + # calibration_pose_15, + # calibration_pose_16, + # calibration_pose_17, + # calibration_pose_18, ]) # Initialize Moveit Commander and Scene @@ -239,6 +251,7 @@ def _goal_callback(self, req): response.success = False return response response.success = True + self.action_server.reset() return response def _check_callback(self, req): @@ -334,7 +347,7 @@ def _command_calibrate(self): return for pose in self.calibration_poses: self._move_to_joint_goal(self.manipulator_group, pose) - rospy.sleep(1.0) + rospy.sleep(5.0) try: self._take_sample_service() except rospy.ServiceException: diff --git a/examples/demo/eager_calibration_rqt/src/eager_calibration_rqt/eager_calibration_rqt.py b/examples/demo/eager_calibration_rqt/src/eager_calibration_rqt/eager_calibration_rqt.py index 7de292e..f6cc96a 100644 --- a/examples/demo/eager_calibration_rqt/src/eager_calibration_rqt/eager_calibration_rqt.py +++ b/examples/demo/eager_calibration_rqt/src/eager_calibration_rqt/eager_calibration_rqt.py @@ -39,6 +39,8 @@ def __init__(self, context): self.freehand_robot_movement = rospy.get_param('~freehand_robot_movement', True) self.robot_velocity_scaling = rospy.get_param('~robot_velocity_scaling', 0.2) self.robot_acceleration_scaling = rospy.get_param('robot_acceleration_scaling', 0.2) + self.move_group_namespace = rospy.get_param('move_group_namespace', '/vx300s') + self.move_group = rospy.get_param('move_group', 'interbotix_arm') # Process standalone plugin command-line arguments from argparse import ArgumentParser @@ -143,6 +145,8 @@ def handle_calibrate(self): "freehand_robot_movement:={}".format(self.freehand_robot_movement), "robot_velocity_scaling:={}".format(self.robot_velocity_scaling), "robot_acceleration_scaling:={}".format(self.robot_acceleration_scaling), + "move_group_namespace:={}".format(self.move_group_namespace), + "move_group:={}".format(self.move_group), ] roslaunch_args = cli_args[1:] roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0], roslaunch_args)] From 4aade2c9bf6ad12545ef5285272642134e94d570 Mon Sep 17 00:00:00 2001 From: jelledouwe Date: Wed, 15 Sep 2021 15:46:25 +0200 Subject: [PATCH 3/8] More accurate calibration --- examples/demo/eager_demo/config/calibration.yaml | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/examples/demo/eager_demo/config/calibration.yaml b/examples/demo/eager_demo/config/calibration.yaml index 3f18864..224a508 100644 --- a/examples/demo/eager_demo/config/calibration.yaml +++ b/examples/demo/eager_demo/config/calibration.yaml @@ -1,9 +1,9 @@ orientation: -- -0.15044073811242573 -- 0.03129335744680928 -- 0.9827168577615274 -- 0.10322735861778351 +- -0.14375034449039983 +- 0.0038339501395679987 +- 0.9891742484878223 +- 0.029247998457513195 position: -- 1.1114968665631024 -- -0.04466884969307203 -- 0.7231197600919643 +- 1.0881738373974 +- -0.025237391211205884 +- 0.6737925282041892 From 3f14f81b5a85ac66fb4ec293b8d823e8cff5081e Mon Sep 17 00:00:00 2001 From: jelledouwe Date: Wed, 15 Sep 2021 15:46:55 +0200 Subject: [PATCH 4/8] Update demo --- .../demo/eager_demo/src/demo_1_pybullet.py | 4 +- examples/demo/eager_demo/src/demo_2_real.py | 5 +- examples/demo/eager_demo/src/demo_3_webots.py | 18 ++-- examples/demo/eager_demo/src/demo_5_webots.py | 75 +++++++++++++ .../demo/eager_demo/src/demo_6_pybullet.py | 102 ++++++++++++++++++ examples/demo/eager_demo/src/eager_demo.py | 3 +- 6 files changed, 195 insertions(+), 12 deletions(-) create mode 100755 examples/demo/eager_demo/src/demo_5_webots.py create mode 100755 examples/demo/eager_demo/src/demo_6_pybullet.py diff --git a/examples/demo/eager_demo/src/demo_1_pybullet.py b/examples/demo/eager_demo/src/demo_1_pybullet.py index 5a37802..bf21d74 100755 --- a/examples/demo/eager_demo/src/demo_1_pybullet.py +++ b/examples/demo/eager_demo/src/demo_1_pybullet.py @@ -42,7 +42,7 @@ ) # Create environment - env = EagerEnv(name='demo_env', + env = EagerEnv(name='demo1', engine=engine, objects=[robot, cam], render_sensor=cam.sensors['camera_rgb'], @@ -51,7 +51,7 @@ env.render() obs = env.reset() # TODO: if code does not close properly, render seems to keep a thread open.... - for i in range(200): + for i in range(100): action = env.action_space.sample() obs, reward, done, info = env.step(action) if done: diff --git a/examples/demo/eager_demo/src/demo_2_real.py b/examples/demo/eager_demo/src/demo_2_real.py index b0f1048..65ff092 100755 --- a/examples/demo/eager_demo/src/demo_2_real.py +++ b/examples/demo/eager_demo/src/demo_2_real.py @@ -56,15 +56,14 @@ # Create environment env = EagerEnv(engine=engine, objects=[robot, cam], - name='demo_env', + name='demo2', render_sensor=cam.sensors['camera_rgb'], - max_steps=10, ) env = Flatten(env) env.render() obs = env.reset() - for i in range(100): + for i in range(50): action = env.action_space.sample() obs, reward, done, info = env.step(action) if done: diff --git a/examples/demo/eager_demo/src/demo_3_webots.py b/examples/demo/eager_demo/src/demo_3_webots.py index 66e0c2a..62aa08b 100755 --- a/examples/demo/eager_demo/src/demo_3_webots.py +++ b/examples/demo/eager_demo/src/demo_3_webots.py @@ -36,15 +36,21 @@ # Create robot # todo: add calibrated position & orientation - robot = Object.create('robot', 'eager_robot_ur5e', 'ur5e') + robot1 = Object.create('robot1', 'eager_robot_ur5e', 'ur5e') + robot2 = Object.create('robot2', 'eager_robot_ur5e', 'ur5e', position=[-1, 1, 0]) + # Add action preprocessing processor = SafeActionsProcessor(vel_limit=2.0, robot_type='ur5e', collision_height=0.01, ) - robot.actuators['joints'].add_preprocess( + robot1.actuators['joints'].add_preprocess( + processor=processor, + observations_from_objects=[robot1], + ) + robot2.actuators['joints'].add_preprocess( processor=processor, - observations_from_objects=[robot], + observations_from_objects=[robot2], ) # Add a camera for rendering @@ -56,8 +62,8 @@ # Create environment env = EagerEnv(engine=engine, - objects=[robot, cam], - name='demo_env', + objects=[robot1, robot2, cam], + name='demo3', render_sensor=cam.sensors['camera_right'], max_steps=10, ) @@ -66,7 +72,7 @@ env.render() obs = env.reset() - for i in range(25): + for i in range(100): action = env.action_space.sample() obs, reward, done, info = env.step(action) if done: diff --git a/examples/demo/eager_demo/src/demo_5_webots.py b/examples/demo/eager_demo/src/demo_5_webots.py new file mode 100755 index 0000000..d43465e --- /dev/null +++ b/examples/demo/eager_demo/src/demo_5_webots.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python3 + +# Copyright 2021 - present, OpenDR European Project + +# 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. + +# ROS packages required +import rospy +from eager_core.eager_env import EagerEnv +from eager_core.objects import Object +from eager_core.wrappers.flatten import Flatten +from eager_core.utils.file_utils import launch_roscore, load_yaml +from eager_bridge_webots.webots_engine import WebotsEngine # noqa: F401 + +# Required for action processor +from eager_process_safe_actions.safe_actions_processor import SafeActionsProcessor + + +if __name__ == '__main__': + roscore = launch_roscore() # First launch roscore + + rospy.init_node('eager_demo', anonymous=True, log_level=rospy.WARN) + + # Define the engine + engine = WebotsEngine() + + # Create robot + robot = Object.create('robot', 'eager_robot_ur5e', 'ur5e', fixed_base=True) + + # Add action preprocessing + processor = SafeActionsProcessor(vel_limit=0.25, + robot_type='ur5e', + collision_height=0.01, + ) + robot.actuators['joints'].add_preprocess( + processor=processor, + observations_from_objects=[robot], + ) + + # Add a camera for rendering + # First load calibrated position & orientation + calibration = load_yaml('eager_demo', 'calibration') + cam = Object.create('cam', 'eager_sensor_multisense_s21', 'dual_cam', + position=calibration['position'], + orientation=calibration['orientation'], + ) + + # Create environment + env = EagerEnv(engine=engine, + objects=[robot, cam], + name='demo5', + render_sensor=cam.sensors['camera_right'], + max_steps=10, + ) + env = Flatten(env) + + env.render() + obs = env.reset() + for i in range(500): + action = env.action_space.sample() + obs, reward, done, info = env.step(action) + if done: + obs = env.reset() + + env.close() diff --git a/examples/demo/eager_demo/src/demo_6_pybullet.py b/examples/demo/eager_demo/src/demo_6_pybullet.py new file mode 100755 index 0000000..241b0fa --- /dev/null +++ b/examples/demo/eager_demo/src/demo_6_pybullet.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python3 + +import rospy +import numpy as np + +# Import eager packages +from eager_core.utils.file_utils import launch_roscore, load_yaml +from eager_core.eager_env import EagerEnv +from eager_core.objects import Object +from eager_core.wrappers.flatten import Flatten +from eager_bridge_pybullet.pybullet_engine import PyBulletEngine # noqa: F401 +from eager_core.utils.file_utils import substitute_xml_args +from stable_baselines3 import PPO +from gym import spaces + +# Required for action processor +from eager_process_safe_actions.safe_actions_processor import SafeActionsProcessor + +class NormalizeActions(object): + """ + if and(low, high) not np.inf: scales action from [-1, 1] back to the unnormalized action + if or(low,high) np.inf: no normalization of the actions, and true action must be used""" + + def __init__(self, env): + self._env = env + low, high = env.action_space.low, env.action_space.high + self._enabled = np.logical_and(np.isfinite(low), np.isfinite(high)) + self._low = np.where(self._enabled, low, -np.ones_like(low)) + self._high = np.where(self._enabled, high, np.ones_like(low)) + + def __getattr__(self, name): + return getattr(self._env, name) + + @property + def action_space(self): + space = self._env.action_space + low = np.where(self._enabled, -np.ones_like(space.low), space.low) + high = np.where(self._enabled, np.ones_like(space.high), space.high) + return spaces.Box(low, high, dtype=space.dtype) + + def step(self, action): + # de-normalize action + # action = (action + 1) / 2 * (self._high - self._low) + self._low + action = self.denormalize_action(action) + + # apply action + obs, reward, done, info = self._env.step(action) + + # normalize applied action (in case action was above maximum) + # info['action'] = (2*info['action'] - (self._high + self._low))/(self._high - self._low) + # info['action'] = self.normalize_action(info['action']) + return obs, reward, done, info + + def denormalize_action(self, action): + return (action + 1) / 2 * (self._high - self._low) + self._low + + def normalize_action(self, action): + return (2*action - (self._high + self._low))/(self._high - self._low) + +def reward_fn(obs): + reward = -np.sum(np.power(obs['robot']['joint_pos'],2)) + return reward + +if __name__ == '__main__': + roscore = launch_roscore() # First launch roscore + + rospy.init_node('eager_demo', anonymous=True, log_level=rospy.WARN) + + # Define the engine + engine = PyBulletEngine(gui=False) + + # Create robot + robot = Object.create('robot', 'eager_robot_vx300s', 'vx300s') + # Add action preprocessing + processor = SafeActionsProcessor(robot_type='vx300s', + vel_limit=0.25, + collision_height=0.15, + duration=0.08 + ) + robot.actuators['joints'].add_preprocess( + processor=processor, + observations_from_objects=[robot], + ) + + # Create environment + env = EagerEnv(name='demo6', + engine=engine, + objects=[robot], + reward_fn=reward_fn, + max_steps=200, + ) + env = Flatten(env) + env = NormalizeActions(Flatten(env)) + + env.render() + obs = env.reset() # TODO: if code does not close properly, render seems to keep a thread open.... + model = PPO('MlpPolicy', env, verbose=1, tensorboard_log=substitute_xml_args("$(find eager_demo)/logs/demo")) + model.learn(total_timesteps=100000) + save_path = substitute_xml_args("$(find eager_demo)/models/demo.zip") + model.save(save_path) + # todo: create a env.close(): close render screen, and env.shutdown() to shutdown the environment cleanly. + env.close() diff --git a/examples/demo/eager_demo/src/eager_demo.py b/examples/demo/eager_demo/src/eager_demo.py index 4abee79..90433a3 100755 --- a/examples/demo/eager_demo/src/eager_demo.py +++ b/examples/demo/eager_demo/src/eager_demo.py @@ -18,9 +18,9 @@ roscore = launch_roscore() # First launch roscore rospy.init_node('eager_demo', anonymous=True, log_level=rospy.WARN) + rate = rospy.Rate(1 / 0.08) # Define the engine - engine = RealEngine() # Create robot @@ -39,5 +39,6 @@ obs, reward, done, info = env.step(action) if done: obs = env.reset() + rate.sleep() env.close() From 153d9644b10928134e876e233ead7ec7482726b2 Mon Sep 17 00:00:00 2001 From: jelledouwe Date: Fri, 17 Sep 2021 09:40:05 +0200 Subject: [PATCH 5/8] Add webots demo --- examples/demo/eager_demo/src/demo_3_webots.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/examples/demo/eager_demo/src/demo_3_webots.py b/examples/demo/eager_demo/src/demo_3_webots.py index 62aa08b..3866f44 100755 --- a/examples/demo/eager_demo/src/demo_3_webots.py +++ b/examples/demo/eager_demo/src/demo_3_webots.py @@ -35,7 +35,6 @@ engine = WebotsEngine(gui=True) # Create robot - # todo: add calibrated position & orientation robot1 = Object.create('robot1', 'eager_robot_ur5e', 'ur5e') robot2 = Object.create('robot2', 'eager_robot_ur5e', 'ur5e', position=[-1, 1, 0]) @@ -72,7 +71,7 @@ env.render() obs = env.reset() - for i in range(100): + for i in range(50): action = env.action_space.sample() obs, reward, done, info = env.step(action) if done: From 59e52289ca7fa2947277898439ab2d4e56815d5f Mon Sep 17 00:00:00 2001 From: jelledouwe Date: Mon, 20 Sep 2021 14:15:47 +0200 Subject: [PATCH 6/8] Add urdf and frame to Pybullet parameters --- .../eager_sensor_multisense_s21/config/dual_cam.yaml | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/eager_sensors/eager_sensor_multisense_s21/config/dual_cam.yaml b/eager_sensors/eager_sensor_multisense_s21/config/dual_cam.yaml index 566b2fe..9bc261f 100644 --- a/eager_sensors/eager_sensor_multisense_s21/config/dual_cam.yaml +++ b/eager_sensors/eager_sensor_multisense_s21/config/dual_cam.yaml @@ -3,12 +3,12 @@ sensors: type: boxu8 high: 255 low: 0 - shape: [1024, 544, 4] + shape: [544, 1024, 4] camera_left: type: boxu8 high: 255 low: 0 - shape: [1024, 544, 4] + shape: [544, 1024, 4] webots: node_type_name: MultiSenseS21 @@ -27,10 +27,12 @@ webots: - MultiSense_S21_left_camera pybullet: - urdf: + xacro: $(find eager_sensor_multisense_s21)/urdf/multisense_s21.urdf.xacro + urdf_name: multisense_s21 sensors: camera_right: type: camera_rgbd + optical_frame_link: right_camera_optical_frame names: [MultiSense_S21_right_camera] intrinsic: fov: 80.21 # (deg) @@ -44,6 +46,7 @@ pybullet: camera_left: type: camera_rgbd + optical_frame_link: left_camera_optical_frame names: [MultiSense_S21_left_camera] intrinsic: fov: 80.21 # (deg) From 3d974a091167aad7aa59a957c149aa602c2891f8 Mon Sep 17 00:00:00 2001 From: jelledouwe Date: Mon, 20 Sep 2021 14:28:14 +0200 Subject: [PATCH 7/8] Create urdf that is compatible with pybullet and gazebo --- .../launch/gazebo.launch | 2 +- .../multisense_s21.urdf | 88 +++++++++++++++ .../urdf/multisense_s21.urdf.xacro | 12 +- .../{camera.gazebo.xacro => s21.gazebo.xacro} | 103 +---------------- .../urdf/s21.urdf.xacro | 106 ++++++++++++++++++ 5 files changed, 206 insertions(+), 105 deletions(-) create mode 100644 eager_sensors/eager_sensor_multisense_s21/multisense_s21.urdf rename eager_sensors/eager_sensor_multisense_s21/urdf/{camera.gazebo.xacro => s21.gazebo.xacro} (54%) create mode 100644 eager_sensors/eager_sensor_multisense_s21/urdf/s21.urdf.xacro diff --git a/eager_sensors/eager_sensor_multisense_s21/launch/gazebo.launch b/eager_sensors/eager_sensor_multisense_s21/launch/gazebo.launch index 230a93b..39dd97e 100644 --- a/eager_sensors/eager_sensor_multisense_s21/launch/gazebo.launch +++ b/eager_sensors/eager_sensor_multisense_s21/launch/gazebo.launch @@ -7,7 +7,7 @@ + command="$(find xacro)/xacro '$(find eager_sensor_multisense_s21)/urdf/multisense_s21.urdf.xacro' gazebo:=true"/> diff --git a/eager_sensors/eager_sensor_multisense_s21/multisense_s21.urdf b/eager_sensors/eager_sensor_multisense_s21/multisense_s21.urdf new file mode 100644 index 0000000..fa99e99 --- /dev/null +++ b/eager_sensors/eager_sensor_multisense_s21/multisense_s21.urdf @@ -0,0 +1,88 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/eager_sensors/eager_sensor_multisense_s21/urdf/multisense_s21.urdf.xacro b/eager_sensors/eager_sensor_multisense_s21/urdf/multisense_s21.urdf.xacro index 3bee120..f528105 100644 --- a/eager_sensors/eager_sensor_multisense_s21/urdf/multisense_s21.urdf.xacro +++ b/eager_sensors/eager_sensor_multisense_s21/urdf/multisense_s21.urdf.xacro @@ -1,9 +1,17 @@ + + + - + - + + + + + + diff --git a/eager_sensors/eager_sensor_multisense_s21/urdf/camera.gazebo.xacro b/eager_sensors/eager_sensor_multisense_s21/urdf/s21.gazebo.xacro similarity index 54% rename from eager_sensors/eager_sensor_multisense_s21/urdf/camera.gazebo.xacro rename to eager_sensors/eager_sensor_multisense_s21/urdf/s21.gazebo.xacro index ccacc26..79a0de2 100644 --- a/eager_sensors/eager_sensor_multisense_s21/urdf/camera.gazebo.xacro +++ b/eager_sensors/eager_sensor_multisense_s21/urdf/s21.gazebo.xacro @@ -1,108 +1,7 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + Gazebo/DarkGrey diff --git a/eager_sensors/eager_sensor_multisense_s21/urdf/s21.urdf.xacro b/eager_sensors/eager_sensor_multisense_s21/urdf/s21.urdf.xacro new file mode 100644 index 0000000..e201315 --- /dev/null +++ b/eager_sensors/eager_sensor_multisense_s21/urdf/s21.urdf.xacro @@ -0,0 +1,106 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 9105f0422c8889870b08ead34cc8023ca2fd348f Mon Sep 17 00:00:00 2001 From: jelledouwe Date: Mon, 20 Sep 2021 14:29:00 +0200 Subject: [PATCH 8/8] Update examples --- examples/eager_examples/scripts/example.py | 2 +- .../scripts/example_custom_env.py | 19 ++++++++---------- .../scripts/example_multi_robot.py | 13 +++++++----- .../eager_examples/scripts/example_reset.py | 13 +++++------- .../scripts/example_safe_actions.py | 20 +++++++++---------- .../scripts/example_switch_engine.py | 2 +- 6 files changed, 32 insertions(+), 37 deletions(-) diff --git a/examples/eager_examples/scripts/example.py b/examples/eager_examples/scripts/example.py index 6ddbc8d..b714eb7 100755 --- a/examples/eager_examples/scripts/example.py +++ b/examples/eager_examples/scripts/example.py @@ -22,7 +22,7 @@ # Initialize environment robot = Object.create('ur5e1', 'eager_robot_ur5e', 'ur5e', fixed_base=False) robot2 = Object.create('ur5e2', 'eager_robot_ur5e', 'ur5e', position=[1, 0, 0], self_collision=False) - env = EagerEnv(engine=engine, objects=[robot, robot2], name='ros_env') + env = EagerEnv(engine=engine, objects=[robot, robot2], name='example_env') env = Flatten(env) check_env(env) diff --git a/examples/eager_examples/scripts/example_custom_env.py b/examples/eager_examples/scripts/example_custom_env.py index 87bcc23..9e3df99 100644 --- a/examples/eager_examples/scripts/example_custom_env.py +++ b/examples/eager_examples/scripts/example_custom_env.py @@ -5,6 +5,7 @@ from eager_core.eager_env import BaseEagerEnv from eager_core.objects import Object from eager_core.wrappers.flatten import Flatten +from eager_core.utils.file_utils import launch_node from eager_bridge_webots.webots_engine import WebotsEngine # noqa: F401 from eager_bridge_pybullet.pybullet_engine import PyBulletEngine # noqa: F401 from eager_process_safe_actions.safe_actions_processor import SafeActionsProcessor @@ -36,7 +37,10 @@ def __init__(self, engine, name="custom_env"): processor=processor, observations_from_objects=[self.ur5e], action_space=spaces.Box(low=-np.pi, high=np.pi, shape=(6,))) - self.camera = Object.create('ms21', 'eager_sensor_multisense_s21', 'dual_cam') + self.camera = Object.create('ms21', 'eager_sensor_multisense_s21', 'dual_cam', + position=[1.0, 0.0, 0.65], + orientation=[-0.1305262, 0.0, 0.9914449, 0.0], + ) self._init_nodes([self.camera, self.ur5e]) @@ -71,13 +75,8 @@ def reset(self) -> object: # Get new observations return self.ur5e.get_obs() - def render(self, mode, **kwargs): - # Use camera to render rgb images - rgbd = self.camera.sensors['camera_right'].get_obs() - return rgbd[:, :, :3] - def _get_reward(self, obs): - return -(self.ur5e.get_state(['joint_pos'])['joint_pos'][5] - 2)**2 # Je mag hier iets verzinnen Bas + return -(obs['joint_pos'][5] - 2)**2 def _is_done(self, obs): return self.steps >= self.STEPS_PER_ROLLOUT @@ -88,8 +87,8 @@ def _is_done(self, obs): rospy.init_node('ur5e_example', anonymous=True, log_level=rospy.WARN) # Engine specific parameters - # engine = WebotsEngine(world='$(find ur5e_example)/worlds/ur5e_cam.wbt') - engine = PyBulletEngine(gui=True) + engine = WebotsEngine() + # engine = PyBulletEngine() env = MyEnv(engine, name="my_env") env = Flatten(env) @@ -100,8 +99,6 @@ def _is_done(self, obs): for i in range(1000): action = env.action_space.sample() obs, reward, done, info = env.step(action) - - env.render() if done: obs = env.reset() diff --git a/examples/eager_examples/scripts/example_multi_robot.py b/examples/eager_examples/scripts/example_multi_robot.py index e5f04b3..0363f40 100755 --- a/examples/eager_examples/scripts/example_multi_robot.py +++ b/examples/eager_examples/scripts/example_multi_robot.py @@ -25,7 +25,7 @@ if __name__ == '__main__': - rospy.init_node('example_safe_actions', anonymous=True, log_level=rospy.WARN) + rospy.init_node('example_multi_robot', anonymous=True, log_level=rospy.WARN) # Define the engine engine = PyBulletEngine(gui=True) @@ -44,19 +44,22 @@ def reward_fn(obs): rwd = [] for obj in obs: if 'ur5e' in obj: - rwd.append(-(obs[obj]['joint_sensors'] ** 2).sum()) + rwd.append(-(obs[obj]['joint_pos'] ** 2).sum()) return rwd # Add a camera for rendering - cam = Object.create('ms21', 'eager_sensor_multisense_s21', 'dual_cam') + cam = Object.create('d435', 'eager_sensor_realsense', 'd435', + position=[3.0, 0.0, 0.65], + orientation=[0.0, 0.0, 1.0, 0.0], + ) objects.append(cam) # Create environment env = EagerEnv( engine=engine, objects=objects, - name='multi_env', - render_obs=cam.sensors['camera_right'].get_obs, + name='example_multi_env', + render_sensor=cam.sensors['camera_rgb'], max_steps=100, reward_fn=reward_fn) env = Flatten(env) diff --git a/examples/eager_examples/scripts/example_reset.py b/examples/eager_examples/scripts/example_reset.py index 4f3ddcc..f10ec33 100755 --- a/examples/eager_examples/scripts/example_reset.py +++ b/examples/eager_examples/scripts/example_reset.py @@ -13,16 +13,14 @@ def reset_func(env: EagerEnv): - for obj in env.objects: - if obj.name == 'can': - # WeBots cannot handle exactly 0 rotations - states = dict(position=[random(), 0, random()], orientation=[0.0000001, 0, 0, 0.9999999]) - obj.reset(states) + can_states = dict(position=[random(), 0, random()], orientation=[0.0000001, 0, 0, 0.9999999]) + states = dict(can=can_states) + return states if __name__ == '__main__': - rospy.init_node('ur5e_example', anonymous=True, log_level=rospy.WARN) + rospy.init_node('reset_example', anonymous=True, log_level=rospy.WARN) # Engine specific parameters engine = WebotsEngine(physics_step=0.01, seed=42) @@ -31,7 +29,7 @@ def reset_func(env: EagerEnv): # Initialize environment robot = Object.create('ur5e1', 'eager_robot_ur5e', 'ur5e') can = Object.create('can', 'eager_solid_other', 'can', position=[0, 0, 1]) - env = EagerEnv(engine=engine, objects=[robot, can], name='ros_env', reset_fn=reset_func, max_steps=20) + env = EagerEnv(engine=engine, objects=[robot, can], name='example_reset_env', reset_fn=reset_func, max_steps=20) env = Flatten(env) check_env(env) @@ -39,7 +37,6 @@ def reset_func(env: EagerEnv): for i in range(1000): action = env.action_space.sample() obs, reward, done, info = env.step(action) - env.render() if done: obs = env.reset() diff --git a/examples/eager_examples/scripts/example_safe_actions.py b/examples/eager_examples/scripts/example_safe_actions.py index 0742bb6..d6f0290 100755 --- a/examples/eager_examples/scripts/example_safe_actions.py +++ b/examples/eager_examples/scripts/example_safe_actions.py @@ -19,6 +19,7 @@ from eager_core.eager_env import BaseEagerEnv from eager_core.objects import Object from eager_core.wrappers.flatten import Flatten +from eager_core.utils.file_utils import launch_node from eager_bridge_webots.webots_engine import WebotsEngine # noqa: F401 from eager_bridge_pybullet.pybullet_engine import PyBulletEngine # noqa: F401 from eager_process_safe_actions.safe_actions_processor import SafeActionsProcessor @@ -44,7 +45,7 @@ def __init__(self, engine, name="my_env"): duration=0.08, checks_per_rad=15, vel_limit=3.0, - collision_height=0.1, + collision_height=0.01, ) self.ur5e.actuators['joints'].add_preprocess( processor=processor, @@ -52,7 +53,10 @@ def __init__(self, engine, name="my_env"): action_space=spaces.Box(low=-np.pi, high=np.pi, shape=(6,))) # Create a camera object - self.camera = Object.create('ms21', 'eager_sensor_multisense_s21', 'dual_cam') + self.camera = Object.create('ms21', 'eager_sensor_multisense_s21', 'dual_cam', + position=[1.0, 0.0, 0.65], + orientation=[0.0, 0.0, 1.0, 0.0], + ) # Initialize all the services of the robots self._init_nodes([self.camera, self.ur5e]) @@ -89,14 +93,9 @@ def reset(self) -> object: # Get new observations return self.ur5e.get_obs() - def render(self, mode, **kwargs): - # Use camera to render rgb images - rgbd = self.camera.sensors['camera_right'].get_obs() - return rgbd[:, :, :3] - def _get_reward(self, obs): # Quadratic reward - move to goal position [0, -np.pi/2, 0, 0, 0, 0] - return -((obs['joint_sensors'] - np.array([0, -np.pi / 2, 0, 0, 0, 0], dtype='float32')) ** 2).sum() + return -((obs['joint_pos'] - np.array([0, -np.pi / 2, 0, 0, 0, 0], dtype='float32')) ** 2).sum() def _is_done(self, obs): return self.steps >= self.STEPS_PER_ROLLOUT @@ -107,8 +106,8 @@ def _is_done(self, obs): rospy.init_node('example_safe_actions', anonymous=True, log_level=rospy.WARN) # Define the engine - # engine = WebotsEngine() - engine = PyBulletEngine(gui=True) + engine = WebotsEngine() + # engine = PyBulletEngine(gui=True) # Create environment env = MyEnv(engine, name="my_env") @@ -120,7 +119,6 @@ def _is_done(self, obs): for i in range(1000): action = env.action_space.sample() obs, reward, done, info = env.step(action) - env.render() if done: obs = env.reset() diff --git a/examples/eager_examples/scripts/example_switch_engine.py b/examples/eager_examples/scripts/example_switch_engine.py index 5c99da2..5a006e3 100755 --- a/examples/eager_examples/scripts/example_switch_engine.py +++ b/examples/eager_examples/scripts/example_switch_engine.py @@ -77,7 +77,7 @@ def render(self, mode, **kwargs): def _get_reward(self, obs): # Quadratic reward - move to goal position [0, -np.pi/2, 0, 0, 0, 0] - return -((obs['joint_sensors'] - np.array([0, -np.pi / 2, 0, 0, 0, 0], dtype='float32')) ** 2).sum() + return -((obs['joint_pos'] - np.array([0, -np.pi / 2, 0, 0, 0, 0], dtype='float32')) ** 2).sum() def _is_done(self, obs): return self.steps >= self.STEPS_PER_ROLLOUT