From 9ea29499ff23ce60878945a6cce6185b592dd2f9 Mon Sep 17 00:00:00 2001 From: jelledouwe Date: Wed, 15 Sep 2021 15:43:53 +0200 Subject: [PATCH 1/5] 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/5] 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/5] 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/5] 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/5] 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: