├── .gitignore ├── Makefile ├── NOTES.md ├── README.md ├── TODO.md ├── deploy └── prod_requirements.txt ├── example_data └── ep0.pickle.gz ├── rostools ├── set_position.py └── setup-display.sh └── src └── rosbridge ├── CMakeLists.txt ├── fetch_proxy.py ├── launch └── fetch_proxy.launch ├── package.xml ├── server.py ├── test_pillow.py ├── test_wand.py ├── timeseq.py └── zmq_serialize.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | /.dockerimage 3 | /secrets/ 4 | .gitfiles 5 | .gitsitchy 6 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | 2 | 3 | default : 4 | 5 | include ../kluster/make-utils.inc 6 | 7 | force : 8 | 9 | 10 | # Run these on the robot 11 | show-battery-state: 12 | rostopic echo -n 1 /robot_state/charger 13 | 14 | reset-breakers: 15 | rosservice call /arm_breaker false && rosservice call /arm_breaker true 16 | rosservice call /base_breaker false && rosservice call /base_breaker true 17 | rosservice call /gripper_breaker false && rosservice call /gripper_breaker true 18 | -------------------------------------------------------------------------------- /NOTES.md: -------------------------------------------------------------------------------- 1 | 2 | # Fetch 3 | 4 | 5 | ## ROS Topics 6 | * Subscribe to `head_camera/depth_registered/points`, of type [sensor_msgs/PointCloud2](http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html) 7 | - Note: `head_camera/depth/*` not available in simulator. 8 | * Subscribe to `base_scan` of type [sensor_msgs/LaserScan](http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html) 9 | * Subscribe to `imu` of type [sensor_msgs/Imu](http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html) 10 | 11 | * Write to `arm_with_torso_controller/follow_joint_trajectory` of type [control_msgs/FollowJointTrajectory](http://docs.ros.org/api/control_msgs/html/action/FollowJointTrajectory.html) 12 | - Or maybe a higher level interface through Moveit. 13 | * Write to `cmd_vel` of type [geometry_msgs/Twist](http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html) 14 | * Write to `head_controller/point_head` of type [control_msgs/PointHead](http://docs.ros.org/api/control_msgs/html/action/PointHead.html) 15 | * Write to `gripper_controller/gripper_action` of type [control_msgs/GripperCommand](http://docs.ros.org/api/control_msgs/html/action/GripperCommand.html) 16 | - turn a single grip strength number into position and effort, where <=0 means open and >0 means closed with corresponding effort 17 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | **Status:** Archive (code is provided as-is, no updates expected) 2 | 3 | # rosbridge 4 | **Warning: abandoned in Oct 2016, when we changed to a different system. It might be a good starting point for using a Fetch robot from Gym, but nobody at OpenAI is maintaining it** 5 | 6 | A service implementing a bridge from Gym to ROS robots. Currently supports the [Fetch Research Robot](http://docs.fetchrobotics.com/) 7 | 8 | - Runs as a [ROS](http://www.ros.org) node (ie, start it with `roslaunch rosbridge fetch_proxy.launch`. 9 | 10 | - Listens on a [ZMQ](http://api.zeromq.org) socket, which an [OpenAI Gym](http://gym.openai.com) `ProxyClient` connects to. 11 | 12 | - Converts the action space to ROS commands, and the ROS sensors to an observation space. 13 | 14 | ## Fetch Research robot Environments 15 | * See [Docs](http://docs.fetchrobotics.com/), especially the [API](http://docs.fetchrobotics.com/api_overview.html) 16 | 17 | ### FetchRobot-v0: Action and observation space 18 | * Action is a tuple with 19 | - an 8x1 vector of joint torques, in range [-1 .. +1] corresponding to the following joints: 20 | - shoulder_pan_joint 21 | - shoulder_lift_joint 22 | - upperarm_roll_joint 23 | - elbow_flex_joint 24 | - forearm_roll_joint 25 | - wrist_flex_joint 26 | - wrist_roll_joint 27 | - l_gripper_finger_joint 28 | * Observation is a tuple of 29 | - An 8x1 vector of joint angles in range [-4 .. +4] 30 | - An 8x1 vector of joint velocities in range [-4 .. +4] 31 | - A 480x640 array of floats representing distance in meters 32 | 33 | ### FetchRobotRGB-v0: Action and observation space 34 | * Action: (same as FetchRobot-v0) 35 | * Observation is a tuple of 36 | - An 8x1 vector of joint angles in range [-4 .. +4] representing radians 37 | - An 8x1 vector of joint velocities in range [-4 .. +4] representing radians/sec 38 | - A 480x640x3 array of uint8s representing RGB pixel values 39 | 40 | 41 | ### Installation on a Fetch: 42 | See https://github.com/openai/fetch-config for installation scripts 43 | -------------------------------------------------------------------------------- /TODO.md: -------------------------------------------------------------------------------- 1 | # Rosbridge TODO 2 | 3 | 4 | ### WIP 5 | * Set fetch_proxy to run as an upstart service 6 | - add make targets to install 7 | * Install gym robotics branch on 20.fetch 8 | * Set up home directories with the right environment 9 | * Get rid of docker deploy for rosbridge 10 | * Gripper 11 | 12 | ### Architecture issues 13 | * Show sketch of operation around #robotics. 14 | - also for Dex4 15 | * How to do logging & replay? Leave up to agent? 16 | * How to allocate robot among users? 17 | - Lock server. (Also for dex4, darwin, etc. Need mask to select particular robots. And UI to reserve). 18 | 19 | ### Functionality issues 20 | 21 | 22 | ### Tidying 23 | -------------------------------------------------------------------------------- /deploy/prod_requirements.txt: -------------------------------------------------------------------------------- 1 | --index-url https://pypi.python.org/simple/ 2 | imageio 3 | numpy>=1.10.4 4 | scipy 5 | jupyter 6 | readchar 7 | tabulate 8 | nose>=1.3.7 9 | Pillow 10 | python-gflags 11 | ujson 12 | matplotlib 13 | pygtail>=0.6.1 14 | requests 15 | PyYAML>=3.11 16 | watchdog>=0.8.3 17 | rethinkdb 18 | redis 19 | wand 20 | PyOpenGL>=3.1.0 21 | wsaccel 22 | ws4py 23 | cherrypy 24 | -------------------------------------------------------------------------------- /example_data/ep0.pickle.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openai/rosbridge/983313dc67b6e2aa7434b569a8afc99c619be833/example_data/ep0.pickle.gz -------------------------------------------------------------------------------- /rostools/set_position.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | """ 3 | A proxy to allow a Gym program to connect to a Fetch robot. 4 | 5 | This program connects to the robot using ROS and both observes and controls the robot. 6 | 7 | It runs continuously, accepting connections over zmq from a GymProxyClient, controlling 8 | the robot for the duration of a session, and then accepting more connections. 9 | 10 | It needs to have low latency both to the robot and the gym program. It works to run it on the robot, 11 | but it may also work to run it on a nearby computer. 12 | 13 | The Fetch robot is decribed at http://docs.fetchrobotics.com/robot_hardware.html 14 | 15 | """ 16 | import math, random, time, logging, re, base64, argparse, collections, sys, os, pdb, threading 17 | import numpy as np 18 | import gym 19 | from gym.spaces import Box, Tuple 20 | import gym.envs.proxy.server as server 21 | import rospy 22 | import actionlib 23 | from rospy.numpy_msg import numpy_msg 24 | from moveit_msgs.msg import MoveItErrorCodes 25 | from moveit_python import MoveGroupInterface, PlanningSceneInterface 26 | from sensor_msgs.msg import LaserScan, JointState, Image 27 | from fetch_driver_msgs.msg import GripperState #as GripperState 28 | from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory 29 | from control_msgs.msg import PointHeadAction, PointHeadGoal, FollowJointTrajectoryAction, FollowJointTrajectoryGoal 30 | 31 | 32 | logger = logging.getLogger(__name__) 33 | logger.setLevel(logging.INFO) 34 | 35 | 36 | def main(): 37 | arm_move_group = MoveGroupInterface("arm", "base_link", plan_only = True) 38 | arm_trajectory_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction) 39 | arm_trajectory_client.wait_for_server() 40 | 41 | argi = 0 42 | while argi < len(sys.argv): 43 | arg = sys.argv[argi] 44 | argi += 1 45 | if arg == 'arm': 46 | print('Contacting move_group for arm...', file=sys.stderr) 47 | arm_move_group = MoveGroupInterface("arm", "base_link", plan_only = True) 48 | arm_trajectory_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction) 49 | arm_trajectory_client.wait_for_server() 50 | 51 | joints = [ 52 | 'shoulder_pan_joint', 53 | 'shoulder_lift_joint', 54 | 'upperarm_roll_joint', 55 | 'elbow_flex_joint', 56 | 'forearm_roll_joint', 57 | 'wrist_flex_joint', 58 | 'wrist_roll_joint', 59 | ] 60 | pose = [float(a) for a in sys.argv[argi : argi+len(joints)]] 61 | argi += len(joints) 62 | result = self.arm_move_group.moveToJointPosition(joints, pose, plan_only=True) 63 | if result.error_code.val == MoveItErrorCodes.SUCCESS: 64 | follow_goal = FollowJointTrajectoryGoal() 65 | follow_goal.trajectory = result.planned_trajectory.joint_trajectory 66 | print('sending trajectory to arm...', file=sys.stderr) 67 | self.arm_trajectory_client.send_goal(follow_goal) 68 | result = self.arm_trajectory_client.wait_for_result() 69 | print('arm followed trajectory %s' % result, file=sys.stderr) 70 | else: 71 | logger.warn('moveToJointPosition returned %s', result) 72 | return 73 | else: 74 | print('Unknown joint group %s' % arg, file=sys.stderr) 75 | return 76 | 77 | 78 | if __name__ == '__main__': 79 | logger.info('Creating FetchRobotGymEnv node. argv=%s', sys.argv) 80 | rospy.init_node('set_position') # After this, logging goes to ~/.ros/log/set_position.log 81 | main() 82 | -------------------------------------------------------------------------------- /rostools/setup-display.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | xrandr --addmode VIRTUAL1 1280x1024_60.00 4 | xrandr --newmode "1280x1024_60.00" 109.00 1280 1368 1496 1712 1024 1027 1034 1063 -hsync +vsync 5 | xrandr --addmode VIRTUAL1 1280x1024_60.00 6 | xrandr --output VIRTUAL1 --mode "1280x1024_60.00" 7 | -------------------------------------------------------------------------------- /src/rosbridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rosbridge) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | control_msgs 9 | roscpp 10 | rospy 11 | std_msgs 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ################################################ 24 | ## Declare ROS messages, services and actions ## 25 | ################################################ 26 | 27 | ## To declare and build messages, services or actions from within this 28 | ## package, follow these steps: 29 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 30 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 31 | ## * In the file package.xml: 32 | ## * add a build_depend tag for "message_generation" 33 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 34 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 35 | ## but can be declared for certainty nonetheless: 36 | ## * add a run_depend tag for "message_runtime" 37 | ## * In this file (CMakeLists.txt): 38 | ## * add "message_generation" and every package in MSG_DEP_SET to 39 | ## find_package(catkin REQUIRED COMPONENTS ...) 40 | ## * add "message_runtime" and every package in MSG_DEP_SET to 41 | ## catkin_package(CATKIN_DEPENDS ...) 42 | ## * uncomment the add_*_files sections below as needed 43 | ## and list every .msg/.srv/.action file to be processed 44 | ## * uncomment the generate_messages entry below 45 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 46 | 47 | ## Generate messages in the 'msg' folder 48 | # add_message_files( 49 | # FILES 50 | # Message1.msg 51 | # Message2.msg 52 | # ) 53 | 54 | ## Generate services in the 'srv' folder 55 | # add_service_files( 56 | # FILES 57 | # Service1.srv 58 | # Service2.srv 59 | # ) 60 | 61 | ## Generate actions in the 'action' folder 62 | # add_action_files( 63 | # FILES 64 | # Action1.action 65 | # Action2.action 66 | # ) 67 | 68 | ## Generate added messages and services with any dependencies listed here 69 | # generate_messages( 70 | # DEPENDENCIES 71 | # control_msgs# std_msgs 72 | # ) 73 | 74 | ################################################ 75 | ## Declare ROS dynamic reconfigure parameters ## 76 | ################################################ 77 | 78 | ## To declare and build dynamic reconfigure parameters within this 79 | ## package, follow these steps: 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 82 | ## * In this file (CMakeLists.txt): 83 | ## * add "dynamic_reconfigure" to 84 | ## find_package(catkin REQUIRED COMPONENTS ...) 85 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 86 | ## and list every .cfg file to be processed 87 | 88 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 89 | # generate_dynamic_reconfigure_options( 90 | # cfg/DynReconf1.cfg 91 | # cfg/DynReconf2.cfg 92 | # ) 93 | 94 | ################################### 95 | ## catkin specific configuration ## 96 | ################################### 97 | ## The catkin_package macro generates cmake config files for your package 98 | ## Declare things to be passed to dependent projects 99 | ## INCLUDE_DIRS: uncomment this if you package contains header files 100 | ## LIBRARIES: libraries you create in this project that dependent projects also need 101 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 102 | ## DEPENDS: system dependencies of this project that dependent projects also need 103 | catkin_package( 104 | # INCLUDE_DIRS include 105 | # LIBRARIES rosbridge 106 | # CATKIN_DEPENDS control_msgs roscpp rospy std_msgs 107 | # DEPENDS system_lib 108 | ) 109 | 110 | ########### 111 | ## Build ## 112 | ########### 113 | 114 | ## Specify additional locations of header files 115 | ## Your package locations should be listed before other locations 116 | # include_directories(include) 117 | include_directories( 118 | ${catkin_INCLUDE_DIRS} 119 | ) 120 | 121 | ## Declare a C++ library 122 | # add_library(rosbridge 123 | # src/${PROJECT_NAME}/rosbridge.cpp 124 | # ) 125 | 126 | ## Add cmake target dependencies of the library 127 | ## as an example, code may need to be generated before libraries 128 | ## either from message generation or dynamic reconfigure 129 | # add_dependencies(rosbridge ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 130 | 131 | ## Declare a C++ executable 132 | # add_executable(rosbridge_node src/rosbridge_node.cpp) 133 | 134 | ## Add cmake target dependencies of the executable 135 | ## same as for the library above 136 | # add_dependencies(rosbridge_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 137 | 138 | ## Specify libraries to link a library or executable target against 139 | # target_link_libraries(rosbridge_node 140 | # ${catkin_LIBRARIES} 141 | # ) 142 | 143 | ############# 144 | ## Install ## 145 | ############# 146 | 147 | # all install targets should use catkin DESTINATION variables 148 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 149 | 150 | ## Mark executable scripts (Python etc.) for installation 151 | ## in contrast to setup.py, you can choose the destination 152 | # install(PROGRAMS 153 | # scripts/my_python_script 154 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 155 | # ) 156 | 157 | ## Mark executables and/or libraries for installation 158 | # install(TARGETS rosbridge rosbridge_node 159 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 160 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 161 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 162 | # ) 163 | 164 | ## Mark cpp header files for installation 165 | # install(DIRECTORY include/${PROJECT_NAME}/ 166 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 167 | # FILES_MATCHING PATTERN "*.h" 168 | # PATTERN ".svn" EXCLUDE 169 | # ) 170 | 171 | ## Mark other files for installation (e.g. launch and bag files, etc.) 172 | # install(FILES 173 | # # myfile1 174 | # # myfile2 175 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 176 | # ) 177 | 178 | ############# 179 | ## Testing ## 180 | ############# 181 | 182 | ## Add gtest based cpp test target and link libraries 183 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_rosbridge.cpp) 184 | # if(TARGET ${PROJECT_NAME}-test) 185 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 186 | # endif() 187 | 188 | ## Add folders to be run by python nosetests 189 | # catkin_add_nosetests(test) 190 | -------------------------------------------------------------------------------- /src/rosbridge/fetch_proxy.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | """ 3 | A proxy to allow a Gym program to connect to a Fetch robot. 4 | 5 | This program connects to the robot using ROS and both observes and controls the robot. 6 | 7 | It runs continuously, accepting connections over zmq from a GymProxyClient, controlling 8 | the robot for the duration of a session, and then accepting more connections. 9 | 10 | It needs to have low latency both to the robot and the gym program. It works to run it on the robot, 11 | but it may also work to run it on a nearby computer. 12 | 13 | The Fetch robot is decribed at http://docs.fetchrobotics.com/robot_hardware.html 14 | 15 | """ 16 | import math, random, time, logging, re, base64, argparse, collections, sys, os, pdb, threading, Queue 17 | import numpy as np 18 | from cStringIO import StringIO 19 | import gym 20 | import server 21 | from gym.spaces import Box, Tuple 22 | import rospy 23 | import actionlib 24 | from rospy.numpy_msg import numpy_msg 25 | from moveit_msgs.msg import MoveItErrorCodes 26 | from moveit_python import MoveGroupInterface, PlanningSceneInterface 27 | from sensor_msgs.msg import LaserScan, JointState, Image, Joy 28 | from fetch_driver_msgs.msg import GripperState 29 | from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory 30 | from control_msgs.msg import PointHeadAction, PointHeadGoal, FollowJointTrajectoryAction, FollowJointTrajectoryGoal, GripperCommandGoal, GripperCommandAction 31 | from geometry_msgs.msg import Twist 32 | from timeseq import TimeseqWriter 33 | import PIL.Image 34 | 35 | logger = logging.getLogger(__name__) 36 | logger.setLevel(logging.INFO) 37 | 38 | fetch_api = None 39 | 40 | class FetchRobotApi: 41 | def __init__(self): 42 | 43 | self.moving_mode = False 44 | self.waldo_mode = False 45 | self.prev_joy_buttons = 0 46 | 47 | # See http://docs.fetchrobotics.com/robot_hardware.html#naming-conventions 48 | self.joint_names = [ 49 | #'torso_lift_joint', 50 | #'bellows_joint', 51 | #'head_pan_joint', 52 | #'head_tilt_joint', 53 | 'shoulder_pan_joint', 54 | 'shoulder_lift_joint', 55 | 'upperarm_roll_joint', 56 | 'elbow_flex_joint', 57 | 'forearm_roll_joint', 58 | 'wrist_flex_joint', 59 | 'wrist_roll_joint', 60 | 'l_gripper_finger_joint', 61 | #'r_gripper_finger_joint', 62 | ] 63 | self.joint_name_map = dict([(name, index) for index, name in enumerate(self.joint_names)]) 64 | self.cur_joint_pos = np.zeros([len(self.joint_names)]) 65 | self.cur_joint_vel = np.zeros([len(self.joint_names)]) 66 | self.cur_joint_effort = np.zeros([len(self.joint_names)]) 67 | 68 | self.cur_base_scan = np.zeros([662], dtype=np.float32) 69 | self.cur_head_camera_depth_image = np.zeros([480,640], dtype=np.float32) 70 | self.cur_head_camera_rgb_image = np.zeros([480,640,3], dtype=np.uint8) 71 | 72 | self.timeseq = None 73 | self.timeseq_mutex = threading.Lock() 74 | self.image_queue = Queue.Queue() 75 | self.image_compress_thread = threading.Thread(target=self.image_compress_main) 76 | self.image_compress_thread.daemon = True 77 | self.image_compress_thread.start() 78 | 79 | logger.info('Subscribing...') 80 | self.subs = [] 81 | if 1: self.subs.append(rospy.Subscriber('/joint_states', JointState, self.joint_states_cb)) 82 | if 0: self.subs.append(rospy.Subscriber('/base_scan', LaserScan, self.base_scan_cb)) 83 | if 1: self.subs.append(rospy.Subscriber('/head_camera/depth/image', numpy_msg(Image), self.head_camera_depth_image_cb)) 84 | if 1: self.subs.append(rospy.Subscriber('/head_camera/rgb/image_raw', numpy_msg(Image), self.head_camera_rgb_image_raw_cb)) 85 | if 1: self.subs.append(rospy.Subscriber('/spacenav/joy', Joy, self.spacenav_joy_cb)) 86 | logger.info('Subscribed') 87 | 88 | self.arm_effort_pub = rospy.Publisher('/arm_controller/weightless_torque/command', JointTrajectory, queue_size=2) 89 | #self.head_goal_pub = rospy.Publisher('/head_controller/point_head/goal', PointHeadActionGoal, queue_size=2) 90 | self.gripper_client = actionlib.SimpleActionClient('gripper_controller/gripper_action', GripperCommandAction) 91 | 92 | self.arm_cartesian_twist_pub = rospy.Publisher('/arm_controller/cartesian_twist/command', Twist, queue_size=2) 93 | 94 | 95 | self.head_point_client = actionlib.SimpleActionClient('head_controller/point_head', PointHeadAction) 96 | 97 | self.arm_move_group = MoveGroupInterface("arm", "base_link", plan_only = True) 98 | self.arm_trajectory_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction) 99 | self.arm_trajectory_client.wait_for_server() 100 | 101 | 102 | if 0: 103 | logger.info('Creating MoveGroupInterface...') 104 | self.move_group = MoveGroupInterface('arm_with_torso', 'base_link', plan_only = True) 105 | logger.info('Created MoveGroupInterface') 106 | if 0: 107 | logger.info('Creating PlanningSceneInterface...') 108 | self.planning_scene = PlanningSceneInterface('base_link') 109 | self.planning_scene.removeCollisionObject('my_front_ground') 110 | self.planning_scene.removeCollisionObject('my_back_ground') 111 | self.planning_scene.removeCollisionObject('my_right_ground') 112 | self.planning_scene.removeCollisionObject('my_left_ground') 113 | self.planning_scene.addCube('my_front_ground', 2, 1.1, 0.0, -1.0) 114 | self.planning_scene.addCube('my_back_ground', 2, -1.2, 0.0, -1.0) 115 | self.planning_scene.addCube('my_left_ground', 2, 0.0, 1.2, -1.0) 116 | self.planning_scene.addCube('my_right_ground', 2, 0.0, -1.2, -1.0) 117 | 118 | 119 | logger.warn('FetchRobotGymEnv ROS node running') 120 | 121 | self.head_point_client.wait_for_server() 122 | logger.warn('FetchRobotGymEnv ROS node connected') 123 | 124 | def start_timeseq(self, script): 125 | timeseq = TimeseqWriter.create(script) 126 | timeseq.add_channel('robot_joints', 'FetchRobotJoints') 127 | timeseq.add_channel('gripper_joints', 'FetchGripperJoints') 128 | 129 | timeseq.add_schema('FetchRobotJoints', 130 | ('torso_lift_joint', 'JointState'), 131 | ('head_pan_joint', 'JointState'), 132 | ('head_tilt_joint', 'JointState'), 133 | ('shoulder_pan_joint', 'JointState'), 134 | ('shoulder_lift_joint', 'JointState'), 135 | ('upperarm_roll_joint', 'JointState'), 136 | ('elbow_flex_joint', 'JointState'), 137 | ('forearm_roll_joint', 'JointState'), 138 | ('wrist_flex_joint', 'JointState'), 139 | ('wrist_roll_joint', 'JointState'), 140 | ) 141 | timeseq.add_schema('FetchGripperJoints', 142 | ('l_gripper_finger_joint', 'JointState'), 143 | ) 144 | timeseq.add_schema('JointState', 145 | ('pos', 'double'), 146 | ('vel', 'double'), 147 | ('effort', 'double'), 148 | ) 149 | timeseq.add_channel('arm_action', 'FetchArmWeightlessTorqueAction') 150 | timeseq.add_schema('FetchArmWeightlessTorqueAction', 151 | ('shoulder_pan_joint', 'WeightlessTorqueAction'), 152 | ('shoulder_lift_joint', 'WeightlessTorqueAction'), 153 | ('upperarm_roll_joint', 'WeightlessTorqueAction'), 154 | ('elbow_flex_joint', 'WeightlessTorqueAction'), 155 | ('forearm_roll_joint', 'WeightlessTorqueAction'), 156 | ('wrist_flex_joint', 'WeightlessTorqueAction'), 157 | ('wrist_roll_joint', 'WeightlessTorqueAction'), 158 | ) 159 | timeseq.add_schema('WeightlessTorqueAction', 160 | ('action', 'double'), 161 | ('effort', 'double'), 162 | ) 163 | 164 | timeseq.add_channel('gripper_action', 'FetchGripperAction') 165 | timeseq.add_schema('FetchGripperAction', 166 | ('action', 'double'), 167 | ('effort', 'double'), 168 | ('pos', 'double'), 169 | ) 170 | 171 | timeseq.add_channel('head_camera_rgb', 'VideoFrame') 172 | timeseq.add_schema('VideoFrame', 173 | ('url', 'string'), 174 | ) 175 | with self.timeseq_mutex: 176 | if self.timeseq: 177 | self.timeseq.close() 178 | self.timeseq = timeseq 179 | 180 | def close_timeseq(self): 181 | with self.timeseq_mutex: 182 | if self.timeseq is not None: 183 | self.timeseq.close() 184 | threading.Thread(target=self.timeseq.upload_s3).start() 185 | self.timeseq = None 186 | 187 | def start_axis_video(self): 188 | cameras = rospy.get_param('/axis_cameras') 189 | if cameras and len(cameras): 190 | with self.timeseq_mutex: 191 | if self.timeseq: 192 | for name, info in cameras.items(): 193 | logging.info('Camera %s: %s', name, repr(info)) 194 | self.timeseq.start_axis_video(timeseq_name = name, 195 | auth_header=info.get('auth_header'), 196 | daemon_endpoint=info.get('daemon_endpoint'), 197 | ipaddr=info.get('ipaddr'), 198 | local_link_prefix=info.get('local_link_prefix'), 199 | remote_traces_dir=info.get('remote_traces_dir'), 200 | resolution=info.get('resolution')) 201 | 202 | def stop_axis_video(self): 203 | with self.timeseq_mutex: 204 | if self.timeseq: 205 | self.timeseq.stop_axis_video() 206 | 207 | 208 | def base_scan_cb(self, data): 209 | # fmin replaces nans with 15 210 | self.cur_base_scan = np.fmin(np.array(data.ranges), 15.0) 211 | 212 | 213 | def head_camera_depth_image_cb(self, data): 214 | shape = [data.height, data.width] 215 | dtype = np.dtype(np.float32) 216 | npdata = np.fromstring(data.data, dtype=dtype).reshape(shape) 217 | npdata.strides = (data.step, dtype.itemsize) 218 | self.cur_head_camera_depth_image = np.fmin(npdata, 5.0) 219 | 220 | def head_camera_rgb_image_raw_cb(self, data): 221 | shape = [data.height, data.width, 3] 222 | dtype = np.dtype(np.uint8) 223 | npdata = np.fromstring(data.data, dtype=dtype).reshape(shape) 224 | npdata.strides = (data.step, dtype.itemsize*3, 1) 225 | self.cur_head_camera_rgb_image = npdata 226 | 227 | if self.timeseq: 228 | self.image_queue.put(('head_camera_rgb', data)) 229 | 230 | 231 | def joint_states_cb(self, data): 232 | """ 233 | Handle a joint_states message. Messages can have subsets of the joints, hopefully non-overlapping. 234 | """ 235 | t0 = time.time() 236 | for i in range(len(data.name)): 237 | name = data.name[i] 238 | jni = self.joint_name_map.get(name, -1) 239 | if jni >= 0: 240 | self.cur_joint_pos[jni] = data.position[i] 241 | self.cur_joint_vel[jni] = data.velocity[i] 242 | self.cur_joint_effort[jni] = data.effort[i] 243 | 244 | with self.timeseq_mutex: 245 | if self.timeseq: 246 | channel, channel_type = ((data.name[0] == 'l_wheel_joint' and ('robot_joints', 'FetchRobotJoints')) or 247 | (data.name[0] == 'l_gripper_finger_joint' and ('gripper_joints', 'FetchGripperJoints')) or 248 | (None, None)) 249 | if channel: 250 | state = dict([(jn, { 251 | '__type': 'JointState', 252 | 'pos': data.position[i], 253 | 'vel': data.velocity[i], 254 | 'effort': data.effort[i], 255 | }) for i, jn in enumerate(data.name)]) 256 | state['__type'] = channel_type 257 | state['rxtime'] = t0 258 | self.timeseq.add(data.header.stamp.to_sec(), channel, state) 259 | if 0: 260 | t1 = time.time() 261 | print '%0.6f+%0.6f stamp=%0.6f (age %0.6f) from %s' % (t0, t1-t0, data.header.stamp.to_sec(), data.header.stamp.to_sec()-t0, channel) 262 | 263 | 264 | def image_compress_main(self): 265 | while True: 266 | channel, data = self.image_queue.get() 267 | if 0: print 'len(data.data)=%d data.width=%d data.height=%d' % (len(data.data), data.width, data.height) 268 | im = PIL.Image.frombytes('RGB', (data.width, data.height), data.data, 'raw') 269 | jpeg_writer = StringIO() 270 | im.save(jpeg_writer, 'jpeg') 271 | im_url = 'data:image/jpeg;base64,' + base64.b64encode(jpeg_writer.getvalue()) 272 | with self.timeseq_mutex: 273 | if self.timeseq: 274 | self.timeseq.add(data.header.stamp.to_sec(), channel, { 275 | '__type': 'VideoFrame', 276 | 'url': im_url, 277 | }) 278 | 279 | def move_to_start(self): 280 | 281 | self.moving_mode = True 282 | # Look down 283 | goal = PointHeadGoal() 284 | goal.target.header.stamp = rospy.Time.now() 285 | goal.target.header.frame_id = '/base_link' 286 | goal.target.point.x = 1.5 287 | goal.target.point.y = 0.0 288 | goal.target.point.z = -0.2 289 | goal.min_duration = rospy.Duration(0.5) 290 | if 0: logger.info('Point head to %s...', goal); 291 | self.head_point_client.send_goal(goal) 292 | if 0: logger.info('Point head sent') 293 | 294 | goal = GripperCommandGoal() 295 | goal.command.max_effort = 60 296 | goal.command.position = 0.1 297 | self.gripper_client.send_goal(goal) 298 | 299 | joints = [ 300 | 'shoulder_pan_joint', 301 | 'shoulder_lift_joint', 302 | 'upperarm_roll_joint', 303 | 'elbow_flex_joint', 304 | 'forearm_roll_joint', 305 | 'wrist_flex_joint', 306 | 'wrist_roll_joint', 307 | ] 308 | pose = [0.0, +0.8, 0.0, -0.8, 0.0, 0.0, 0.0] 309 | result = self.arm_move_group.moveToJointPosition(joints, pose, plan_only=True) 310 | if result.error_code.val == MoveItErrorCodes.SUCCESS: 311 | if 0: logger.info('Got trajectory %s', result.planned_trajectory) 312 | follow_goal = FollowJointTrajectoryGoal() 313 | follow_goal.trajectory = result.planned_trajectory.joint_trajectory 314 | logger.info('sending trajectory to arm...') 315 | self.arm_trajectory_client.send_goal(follow_goal) 316 | result = self.arm_trajectory_client.wait_for_result() 317 | logger.info('arm followed trajectory %s', result) 318 | else: 319 | logger.warn('moveToJointPosition returned %s', result) 320 | return 321 | 322 | result = self.head_point_client.wait_for_result() 323 | logger.info('head followed trajectory %s', result) 324 | 325 | logger.info('sending empty arm goal') 326 | empty_goal = FollowJointTrajectoryGoal() 327 | self.arm_trajectory_client.send_goal(empty_goal) 328 | 329 | logger.info('Point head done') 330 | self.moving_mode = False 331 | 332 | 333 | def set_arm_action(self, action): 334 | arm_joints = [ 335 | ('shoulder_pan_joint', 1.57, 33.82), 336 | ('shoulder_lift_joint', 1.57, 131.76), 337 | ('upperarm_roll_joint', 1.57, 76.94), 338 | ('elbow_flex_joint', 1.57, 66.18), 339 | ('forearm_roll_joint', 1.57, 29.35), 340 | ('wrist_flex_joint', 2.26, 25.70), 341 | ('wrist_roll_joint', 2.26, 7.36), 342 | ] 343 | arm_efforts = [min(1.0, max(-1.0, action[self.joint_name_map.get(name)])) * torque_scale * 0.75 for name, vel_scale, torque_scale in arm_joints] 344 | arm_joint_names = [name for name, vel_scale, torque_scale in arm_joints] 345 | if 1: 346 | arm_joint_names.append('gravity_compensation') 347 | arm_efforts.append(1.0) 348 | arm_msg = JointTrajectory(joint_names=arm_joint_names, points=[JointTrajectoryPoint(effort = arm_efforts)]) 349 | self.arm_effort_pub.publish(arm_msg) 350 | 351 | with self.timeseq_mutex: 352 | if self.timeseq: 353 | state = dict([(jn, { 354 | '__type': 'WeightlessTorqueAction', 355 | 'action': action[self.joint_name_map.get(jn)], 356 | 'effort': arm_efforts[i], 357 | }) for i, jn in enumerate(arm_joint_names)]) 358 | state['__type'] = 'FetchArmWeightlessTorqueAction' 359 | self.timeseq.add(time.time(), 'arm_action', state) 360 | 361 | def set_gripper_action(self, action): 362 | 363 | grip = action[self.joint_name_map.get('l_gripper_finger_joint')] 364 | 365 | goal = GripperCommandGoal() 366 | if grip > 0: 367 | goal.command.max_effort = 60*min(1.0, grip) 368 | goal.command.position = 0.0 369 | else: 370 | goal.command.max_effort = 60 371 | goal.command.position = 0.1 372 | 373 | self.gripper_client.send_goal(goal) 374 | 375 | with self.timeseq_mutex: 376 | if self.timeseq: 377 | state = { 378 | '__type': 'FetchGripperAction', 379 | 'action': grip, 380 | 'effort': goal.command.max_effort, 381 | 'pos': goal.command.position, 382 | } 383 | self.timeseq.add(time.time(), 'gripper_action', state) 384 | 385 | def set_waldo_action(self, joy): 386 | twist = Twist() 387 | twist.linear.x = joy.axes[0] 388 | twist.linear.y = joy.axes[1] 389 | twist.linear.z = joy.axes[2] 390 | twist.angular.x = +3.0*joy.axes[3] 391 | twist.angular.y = +3.0*joy.axes[4] 392 | twist.angular.z = +2.0*joy.axes[5] 393 | self.arm_cartesian_twist_pub.publish(twist) 394 | 395 | if joy.buttons[1] and not self.prev_joy_buttons[1]: 396 | goal = GripperCommandGoal() 397 | goal.command.max_effort = 60 398 | goal.command.position = 0.0 399 | self.gripper_client.send_goal(goal) 400 | elif not joy.buttons[1] and self.prev_joy_buttons[1]: 401 | goal = GripperCommandGoal() 402 | goal.command.max_effort = 60 403 | goal.command.position = 0.1 404 | self.gripper_client.send_goal(goal) 405 | 406 | def spacenav_joy_cb(self, joy): 407 | if 0: logger.info('joy %s', str(joy)) 408 | if joy.buttons[0] and not self.prev_joy_buttons[0]: 409 | if self.waldo_mode: 410 | self.stop_waldo_mode() 411 | elif not self.moving_mode: 412 | self.start_waldo_mode() 413 | 414 | if self.waldo_mode and not self.moving_mode: 415 | self.set_waldo_action(joy) 416 | self.prev_joy_buttons = joy.buttons 417 | 418 | def start_waldo_mode(self): 419 | logger.info('Start waldo mode'); 420 | self.waldo_mode = True 421 | self.start_timeseq('fetchwaldo') 422 | self.start_axis_video() 423 | 424 | def stop_waldo_mode(self): 425 | logger.info('Stop waldo mode'); 426 | self.waldo_mode = False 427 | timeseq_url = None 428 | if self.timeseq: 429 | timeseq_url = self.timeseq.get_url() 430 | logger.info('save_logs url=%s', timeseq_url) 431 | self.stop_axis_video() 432 | self.close_timeseq() 433 | 434 | mts_thread = threading.Thread(target=self.move_to_start) 435 | mts_thread.daemon = True 436 | mts_thread.start() 437 | 438 | class FetchRobotGymEnv: 439 | def __init__(self, api, obs_joints=True, obs_lidar=False, obs_head_depth=True, obs_head_rgb=False, act_torques=True): 440 | self.api = api 441 | self.obs_joints = obs_joints 442 | self.obs_lidar = obs_lidar 443 | self.obs_head_depth = obs_head_depth 444 | self.obs_head_rgb = obs_head_rgb 445 | self.act_torques = act_torques 446 | 447 | obsparts = [] 448 | if self.obs_joints: 449 | obsparts.append(Box(-4, +4, self.api.cur_joint_pos.shape)) 450 | obsparts.append(Box(-4, +4, self.api.cur_joint_vel.shape)) 451 | if self.obs_lidar: 452 | obsparts.append(Box(0, +16.0, self.api.cur_base_scan.shape)) 453 | if self.obs_head_depth: 454 | obsparts.append(Box(0, 5.0, self.api.cur_head_camera_depth_image.shape)) 455 | if self.obs_head_rgb: 456 | obsparts.append(Box(0, 255, self.api.cur_head_camera_rgb_image.shape)) 457 | self.observation_space = Tuple(obsparts) 458 | 459 | actparts = [] 460 | if self.act_torques: 461 | actparts.append(Box(-1.0, +1.0, [len(self.api.joint_names)])) 462 | self.action_space = Tuple(actparts) 463 | 464 | self.reward_range = [-1, +1] 465 | self.tickrate = rospy.Rate(10) 466 | 467 | self.api.start_timeseq('fetch') 468 | self.api.start_axis_video() 469 | 470 | def _get_obs(self): 471 | obsparts = [] 472 | if self.obs_joints: 473 | obsparts.append(self.api.cur_joint_pos) 474 | obsparts.append(self.api.cur_joint_vel) 475 | if self.obs_lidar: 476 | obsparts.append(self.api.cur_base_scan) 477 | if self.obs_head_depth: 478 | obsparts.append(self.api.cur_head_camera_depth_image) 479 | if self.obs_head_rgb: 480 | obsparts.append(self.api.cur_head_camera_rgb_image) 481 | return tuple(obsparts) 482 | 483 | def reset(self): 484 | self.api.move_to_start() 485 | return self._get_obs() 486 | 487 | def step(self, action): 488 | # Wait here for 100mS since the last time 489 | t0 = time.time() 490 | self.tickrate.sleep() 491 | t1 = time.time() 492 | if 0: logger.info('sleep %s %s', t1-t0, t1) 493 | 494 | self.api.set_arm_action(action[0]) 495 | self.api.set_gripper_action(action[0]) 496 | 497 | obs = self._get_obs() 498 | reward = 0.0 499 | done = False 500 | info = {} 501 | return obs, reward, done, info 502 | 503 | def render(self, mode='human', close=False): 504 | # WRITEME: output this image to an avi encoder, return a compact URL reference 505 | return self.api.cur_head_camera_rgb_image 506 | 507 | def save_logs(self): 508 | timeseq_url = None 509 | if self.api.timeseq: 510 | timeseq_url = self.api.timeseq.get_url() 511 | logger.info('save_logs url=%s', timeseq_url) 512 | self.api.stop_axis_video() 513 | self.api.close_timeseq() 514 | return timeseq_url 515 | 516 | def close(self): 517 | self.api.move_to_start() 518 | 519 | 520 | 521 | def make_env(name): 522 | if name == 'FetchRobot-v0': 523 | return FetchRobotGymEnv(fetch_api) 524 | elif name == 'FetchRobotRGB-v0': 525 | return FetchRobotGymEnv(fetch_api, obs_head_depth=False, obs_head_rgb=True) 526 | else: 527 | raise Exception('Unknown env name %s' % name) 528 | 529 | if __name__ == '__main__': 530 | logger.info('Creating FetchRobotGymEnv node. argv=%s', sys.argv) 531 | rospy.init_node('FetchRobotGymEnv') # After this, logging goes to ~/.ros/log/FetchRobotGymEnv.log 532 | logger.info('Starting API') 533 | fetch_api = FetchRobotApi() 534 | fetch_api.move_to_start() 535 | if 1: 536 | logger.info('Starting Zmq') 537 | zmqs = server.GymProxyZmqServer('tcp://0.0.0.0:6911', make_env) 538 | logger.info('Creating Zmq thread') 539 | zmqt = threading.Thread(target=zmqs.run_main) 540 | zmqt.daemon = True 541 | logger.info('Starting Zmq thread') 542 | zmqt.start() 543 | logger.info('Spinning...') 544 | rospy.spin() 545 | -------------------------------------------------------------------------------- /src/rosbridge/launch/fetch_proxy.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /src/rosbridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rosbridge 4 | 0.0.0 5 | The rosbridge package 6 | 7 | root 8 | 9 | 10 | 11 | 12 | 13 | TODO 14 | 15 | 16 | 17 | 18 | 19 | http://gym.openai.com 20 | 21 | 22 | 23 | 24 | 25 | Trevor Blackwell 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | catkin 40 | control_msgs 41 | roscpp 42 | rospy 43 | std_msgs 44 | control_msgs 45 | roscpp 46 | rospy 47 | std_msgs 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /src/rosbridge/server.py: -------------------------------------------------------------------------------- 1 | """ 2 | Defines a class GymProxyZmqServer that listens on a zmq port, creates an environment as requested over the port, 3 | and accepts step & reset calls on that environment. 4 | Usage: 5 | def make_env(env_name): 6 | if env_name == 'MyRemoteEnv': 7 | return MyRemoteEnv(...) 8 | s = GymProxyZmqServer(url, make_env) 9 | s.run_main() 10 | As a special bonus, if you run this module directly: 11 | python gym/envs/proxy/server.py tcp://127.0.0.1:6911 12 | it will serve all the Gym environments over zmq at tcp://127.0.0.1:6911 13 | """ 14 | import math, random, time, logging, re, base64, argparse, collections, sys, os, traceback, threading 15 | import numpy as np 16 | import ujson 17 | import zmq, zmq.utils.monitor 18 | import gym 19 | import zmq_serialize 20 | 21 | logger = logging.getLogger(__name__) 22 | logger.setLevel(logging.INFO) 23 | 24 | class GymProxyZmqServer(object): 25 | def __init__(self, url, make_env): 26 | self.url = url 27 | self.make_env = make_env 28 | self.env = None 29 | self.env_name = None 30 | self.session_id = None 31 | self.session_last_use = 0.0 32 | self.session_owner = None 33 | self.log_msgs = [] 34 | 35 | self.ctx = zmq.Context() 36 | self.sock = self.ctx.socket(zmq.REP) 37 | self.sock.bind(self.url) 38 | self.monitor_sock = self.sock.get_monitor_socket() 39 | self.rpc_lock = threading.Lock() 40 | self.rpc_rd = threading.Condition(self.rpc_lock) 41 | self.monitor_thr = threading.Thread(target = self.run_monitor) 42 | self.monitor_thr.daemon = True 43 | self.monitor_thr.start() 44 | 45 | self.timeout_thr = threading.Thread(target = self.run_timeout) 46 | self.timeout_thr.daemon = True 47 | self.timeout_thr.start() 48 | 49 | def run_main(self): 50 | logger.info('zmq gym server running on %s', self.url) 51 | while True: 52 | rx = self.sock.recv_multipart(flags=0, copy=True, track=False) 53 | logger.debug('%s > %s', self.url, rx[0]) 54 | rpc = zmq_serialize.load_msg(rx) 55 | with self.rpc_lock: 56 | self.handle_rpc(rpc) 57 | 58 | def run_monitor(self): 59 | logger.info('zmq gym server listening on monitoring socket') 60 | while True: 61 | ev = zmq.utils.monitor.recv_monitor_message(self.monitor_sock) 62 | logger.debug('Monitor Event %s', ev) 63 | with self.rpc_lock: 64 | if ev['event'] == zmq.EVENT_DISCONNECTED: 65 | logger.info('zmq disconnect') 66 | elif ev['event'] == zmq.EVENT_ACCEPTED: 67 | logger.info('zmq accept') 68 | 69 | def run_timeout(self): 70 | while True: 71 | with self.rpc_lock: 72 | if self.session_id is not None and time.time() - self.session_last_use > 3.0: 73 | self.close_env() 74 | self.session_last_use = 0.0 75 | self.session_id = None 76 | self.session_owner = None 77 | time.sleep(1) 78 | 79 | def close_env(self): 80 | if self.env is not None and hasattr(self.env, 'save_logs'): 81 | log_url = self.env.save_logs() 82 | if log_url is not None: 83 | self.log_to_client('See high-res traces at %s' % log_url) 84 | self.env_name = None 85 | if self.env is not None: 86 | self.env.close() 87 | self.env = None 88 | self.session_id = None 89 | self.session_owner = None 90 | self.session_last_use = 0.0 91 | logger.info('GymProxyZmqServer closed') 92 | 93 | def log_to_client(self, msg): 94 | self.log_msgs.append(msg) 95 | 96 | def handle_rpc(self, rpc): 97 | rpc_method = rpc.get('method', None) 98 | rpc_params = rpc.get('params', None) 99 | 100 | def reply(rpc_result, rpc_error=None): 101 | tx = zmq_serialize.dump_msg({ 102 | 'result': rpc_result, 103 | 'error': rpc_error, 104 | 'log_msgs': self.log_msgs, 105 | }) 106 | del self.log_msgs[:] 107 | logger.debug('%s < %s', self.url, tx[0]) 108 | self.sock.send_multipart(tx, flags=0, copy=False, track=False) 109 | 110 | try: 111 | if rpc_method == 'step': 112 | reply(self.handle_step(rpc_params)) 113 | elif rpc_method == 'reset': 114 | reply(self.handle_reset(rpc_params)) 115 | elif rpc_method == 'setup': 116 | reply(self.handle_setup(rpc_params)) 117 | elif rpc_method == 'close': 118 | reply(self.handle_close(rpc_params)) 119 | elif rpc_method == 'render': 120 | reply(self.handle_render(rpc_params)) 121 | else: 122 | raise Exception('unknown method %s' % rpc_method) 123 | except: 124 | ex_type, ex_value, ex_tb = sys.exc_info() 125 | traceback.print_exception(ex_type, ex_value, ex_tb) 126 | reply(None, str(ex_type) + ': ' + str(ex_value)) 127 | 128 | def check_session(self, params): 129 | session_id = params['session_id'] 130 | if session_id == self.session_id: 131 | self.session_last_use = time.time() 132 | else: 133 | raise Exception('Wrong session id. Expected %s, got %s' % (str(self.session_id), str(session_id))) 134 | 135 | def handle_reset(self, params): 136 | self.check_session(params) 137 | obs = self.env.reset() 138 | return { 139 | 'obs': obs, 140 | 'session_id': self.session_id, 141 | } 142 | 143 | def handle_step(self, params): 144 | self.check_session(params) 145 | action = params['action'] 146 | obs, reward, done, info = self.env.step(action) 147 | return { 148 | 'obs': obs, 149 | 'reward': reward, 150 | 'done': done, 151 | 'info': info, 152 | 'session_id': self.session_id, 153 | } 154 | 155 | def handle_setup(self, params): 156 | if self.session_id is not None: 157 | raise Exception('Robot in use by %s' % str(self.session_owner)) 158 | 159 | self.env = self.make_env(params['env_name']) 160 | if self.env is None: 161 | raise Exception('No such environment') 162 | self.env_name = params['env_name'] 163 | self.session_id = zmq_serialize.mk_random_cookie() 164 | self.session_last_use = time.time() 165 | self.session_owner = params.get('session_owner', 'unknown') 166 | logger.info('Creating env %s. session_id=%s owner=%s', self.env_name, self.session_id, self.session_owner) 167 | 168 | return { 169 | 'observation_space': self.env.observation_space, 170 | 'action_space' : self.env.action_space, 171 | 'reward_range': self.env.reward_range, 172 | 'session_id': self.session_id, 173 | } 174 | 175 | def handle_close(self, params): 176 | if self.session_id is not None: 177 | self.check_session(params) 178 | prev_session_id = self.session_id 179 | self.close_env() 180 | return { 181 | 'session_id': prev_session_id, 182 | } 183 | 184 | def handle_render(self, params): 185 | self.check_session(params) 186 | mode = params['mode'] 187 | close = params['close'] 188 | img = self.env.render(mode, close) 189 | return { 190 | 'img': img, 191 | 'session_id': self.session_id, 192 | } 193 | 194 | 195 | if __name__ == '__main__': 196 | server_url = sys.argv[1] 197 | zmqs = GymProxyZmqServer(server_url, gym.make) 198 | zmqs.run_main() 199 | -------------------------------------------------------------------------------- /src/rosbridge/test_pillow.py: -------------------------------------------------------------------------------- 1 | import time 2 | import PIL.Image 3 | 4 | def test_loader(prefix): 5 | print 'Prefix=%s...' % prefix 6 | t0 = time.time() 7 | data = prefix + ('\0' * (640*480*3-len(prefix))) 8 | 9 | im = PIL.Image.frombytes('RGB', (640, 480), data, 'raw') 10 | t1 = time.time() 11 | print 'Prefix=%s: ok %s (%0.6f)' % (prefix, im.size, t1-t0) 12 | 13 | test_loader('abc') 14 | test_loader('%!PS') 15 | -------------------------------------------------------------------------------- /src/rosbridge/test_wand.py: -------------------------------------------------------------------------------- 1 | import wand.image 2 | 3 | def test_loader(prefix): 4 | print 'Prefix=%s...' % prefix 5 | data = prefix + ('\0' * (640*480*3-len(prefix))) 6 | 7 | im = wand.image.Image(blob=data, width=640, height=480, format='rgb', depth=8) 8 | 9 | print 'Prefix=%s: ok' % prefix 10 | 11 | test_loader('abc') 12 | test_loader('%!PS') 13 | -------------------------------------------------------------------------------- /src/rosbridge/timeseq.py: -------------------------------------------------------------------------------- 1 | # 2 | # A Python trace generator & importer. 3 | # Doesn't necessarily support all the features of the C++/nodejs version 4 | # 5 | import ujson, os, time, random, math, gzip, collections, copy, logging, gzip, zmq, threading 6 | from os import path 7 | 8 | logger = logging.getLogger(__name__) 9 | logger.setLevel(logging.INFO) 10 | 11 | TIMESEQ_CHUNK = 10.0 12 | 13 | def chunk_of(sample_ts): 14 | return math.floor(sample_ts / TIMESEQ_CHUNK) * TIMESEQ_CHUNK 15 | 16 | def get_chunks(begin_ts, end_ts): 17 | ret = [] 18 | ts = chunk_of(begin_ts) 19 | while ts < end_ts: 20 | ret.append(ts) 21 | ts += TIMESEQ_CHUNK 22 | return ret 23 | 24 | def scan_trace(dir, channels, sample_cb): 25 | manifest_fn = path.join(dir, 'manifest.json') 26 | with open(manifest_fn, 'rb') as f: 27 | manifest = ujson.load(f) 28 | if 0: print manifest 29 | 30 | begin_ts = manifest['beginTs'] 31 | end_ts = manifest['endTs'] 32 | print 'range', begin_ts, end_ts 33 | 34 | timeseq_infos = manifest['timeseqInfos'] 35 | cur_samples = {} 36 | # There's some thought to saving memory here. Only load a chunk (of all timeseqs) at a time 37 | for chunk in get_chunks(begin_ts, end_ts): 38 | all_chunk_data = {} 39 | all_chunk_indexes = {} 40 | for tsi in timeseq_infos: 41 | ts_name = tsi['name']; 42 | if ('*' not in channels) and (ts_name not in channels): continue 43 | chunk_fn = path.join(dir, 'chunk_%s_%d.json.gz' % (ts_name, chunk)) 44 | try: 45 | with gzip.open(chunk_fn, 'rb') as f: 46 | print 'Reading', chunk_fn, '...' 47 | all_chunk_data[ts_name] = ujson.load(f) 48 | all_chunk_indexes[ts_name] = 0 49 | except: 50 | print chunk_fn, 'not found' 51 | if ts_name in all_chunk_data: 52 | del all_chunk_data[ts_name] 53 | del all_chunk_indexes[ts_name] 54 | while True: 55 | # Find the next item to change (smallest timestamp) 56 | cur_ts = end_ts + 1 57 | for ts_name in all_chunk_data: 58 | if all_chunk_indexes[ts_name] < len(all_chunk_data[ts_name]['times']): 59 | ts1 = all_chunk_data[ts_name]['times'][all_chunk_indexes[ts_name]] 60 | cur_ts = min(cur_ts, ts1) 61 | if cur_ts > end_ts: # Didn't find anything 62 | break 63 | # Now update cur_samples with all samples with matching timestamps 64 | for ts_name in all_chunk_data: 65 | if all_chunk_indexes[ts_name] < len(all_chunk_data[ts_name]['times']): 66 | ts1 = all_chunk_data[ts_name]['times'][all_chunk_indexes[ts_name]] 67 | if ts1 == cur_ts: 68 | cur_samples[ts_name] = all_chunk_data[ts_name]['samples'][all_chunk_indexes[ts_name]] 69 | all_chunk_indexes[ts_name] += 1 70 | # We copy so that customers can keep a copy that works after we mutate cur_samples again 71 | # scan_trace_deltat does this 72 | sample_cb(cur_ts, copy.copy(cur_samples)) 73 | 74 | 75 | def scan_trace_deltat(dir, channels, deltat, sample_pair_cb): 76 | queue = collections.deque() 77 | def sample_cb(ts, samples): 78 | queue.append((ts, samples)) 79 | while len(queue) > 1: 80 | deltat1 = queue[-1][0] - queue[0][0] 81 | if deltat1 < deltat: break 82 | old = queue.popleft() 83 | if deltat1 >= 0.95 * deltat: 84 | if 0: print deltat1, ts, old[0] 85 | sample_pair_cb(old[1], samples) 86 | scan_trace(dir, channels, sample_cb) 87 | 88 | 89 | 90 | def mk_rand_suffix(n): 91 | return ''.join([random.choice('abcdefghjkmnpqrstuvwxyz') for i in range(n)]) 92 | 93 | def mk_new_trace_name(script): 94 | def fmt2(v): 95 | ret = '%d' % v 96 | while len(ret) < 2: 97 | ret = '0' + ret 98 | return ret 99 | 100 | tm = time.localtime() 101 | ftime = fmt2(tm.tm_year) + '-' + fmt2(tm.tm_mon) + '-' + fmt2(tm.tm_mday) + '_' + fmt2(tm.tm_hour) + '-' + fmt2(tm.tm_min) + '-' + fmt2(tm.tm_sec) 102 | rand_suffix = mk_rand_suffix(3) 103 | 104 | return script + '_' + ftime + '_' + rand_suffix 105 | 106 | ur_dir = path.dirname(path.dirname(path.abspath(__file__))) 107 | traces_dir = path.join(ur_dir, 'traces') 108 | 109 | class TimeseqWriter(object): 110 | """ 111 | 112 | """ 113 | def __init__(self, trace_name, trace_dir): 114 | self.trace_name = trace_name 115 | self.trace_dir = trace_dir 116 | self.files_by_channel = {} 117 | self.chunks_by_channel = {} 118 | self.times_by_channel = {} 119 | self.begin_ts = None 120 | self.end_ts = 0 121 | self.schemas = {} 122 | self.trace_notes = {} 123 | self.timeseq_infos = [] 124 | self.camera_threads = [] 125 | self.is_open = True 126 | logger.info('Writing trace to %s', self.trace_dir) 127 | 128 | @classmethod 129 | def create(cls, script): 130 | trace_name = mk_new_trace_name(script) 131 | trace_dir = path.join(traces_dir, trace_name) 132 | os.mkdir(trace_dir) 133 | return cls(trace_name, trace_dir) 134 | 135 | def get_url(self): 136 | # WRITEME: use public url here 137 | return 'http://roboscope.sci.openai.org/#scope_' + self.trace_name 138 | 139 | def add_channel(self, channel_name, type): 140 | self.timeseq_infos.append({ 141 | '__type': 'TimeseqInfo', 142 | 'name': channel_name, 143 | 'type': type, 144 | }) 145 | 146 | def add_schema(self, type, *members): 147 | self.schemas[type] = { 148 | 'typename': type, 149 | 'hasArrayNature': False, 150 | 'members': [{ 151 | 'memberName': name, 152 | 'typename': member_type, 153 | } for name, member_type in members] 154 | } 155 | 156 | def add(self, ts, channel_name, sample): 157 | try: 158 | if self.is_open: 159 | if self.begin_ts is None: self.begin_ts = ts 160 | if ts > self.end_ts: self.end_ts = ts 161 | 162 | chunk = chunk_of(ts) 163 | if self.chunks_by_channel.get(channel_name, -1) != chunk: 164 | self.close_chunk(channel_name) 165 | self.open_chunk(channel_name, chunk) 166 | f = self.files_by_channel[channel_name] 167 | if len(self.times_by_channel[channel_name]) > 0: 168 | f.write(',') 169 | ujson.dump(sample, f) 170 | self.times_by_channel[channel_name].append(ts) 171 | except OverflowError: 172 | logger.info("Failed to serialize sample. Might it have numpy arrays with float32s? Please convert numpy arrays to float64") 173 | raise 174 | 175 | 176 | def open_chunk(self, channel_name, chunk): 177 | fn = path.join(self.trace_dir, 'chunk_%s_%d.json.gz' % (channel_name, int(chunk))) 178 | f = gzip.open(fn, 'wb') 179 | self.files_by_channel[channel_name] = f 180 | self.chunks_by_channel[channel_name] = chunk 181 | self.times_by_channel[channel_name] = [] 182 | 183 | f.write('{"samples":[') 184 | 185 | def close_chunk(self, channel_name): 186 | f = self.files_by_channel.get(channel_name, None) 187 | if f is not None: 188 | f.write('],"times":') 189 | ujson.dump(self.times_by_channel[channel_name], f) 190 | f.write('}') 191 | f.close() 192 | 193 | self.files_by_channel[channel_name] = None 194 | self.chunks_by_channel[channel_name] = -1 195 | self.times_by_channel[channel_name] = [] 196 | 197 | def close(self): 198 | if self.is_open: 199 | for k,v in self.files_by_channel.items(): 200 | self.close_chunk(k) 201 | 202 | manifest = { 203 | '__type': 'TraceManifest', 204 | 'beginTs': self.begin_ts, 205 | 'endTs': self.end_ts, 206 | 'traceNotes': self.trace_notes, 207 | 'timeseqInfos': self.timeseq_infos, 208 | 'schemas': self.schemas, 209 | 'hasLores': False, 210 | 'hasHires': True, 211 | } 212 | f = open(path.join(self.trace_dir, 'manifest.json'), 'wb') 213 | ujson.dump(manifest, f) 214 | f.close() 215 | logger.info('Wrote trace to %s', self.trace_dir) 216 | self.is_open = False 217 | 218 | def upload_s3(self): 219 | os.system('cd %s && node timeseq/uploadTraceS3.js %s' % (ur_dir, self.trace_dir)) 220 | 221 | def stop_axis_video(self): 222 | self.want_axis_video = False 223 | 224 | def start_axis_video(self, timeseq_name, daemon_endpoint, resolution, ipaddr, auth_header, remote_traces_dir, local_link_prefix): 225 | self.want_axis_video = True 226 | 227 | self.ctx = zmq.Context() 228 | self.sock = self.ctx.socket(zmq.DEALER) 229 | logger.info('Connecting to %s...', daemon_endpoint) 230 | self.sock.connect(daemon_endpoint) 231 | logger.info('Connected to %s', daemon_endpoint) 232 | 233 | self.add_schema('VideoFrame', 234 | ('url', 'string')) 235 | self.add_channel(timeseq_name, 'VideoFrame') 236 | 237 | def thread_main(): 238 | delta = None 239 | rpc_count = 0 240 | while True: 241 | want = self.want_axis_video 242 | req = { 243 | '__type': 'AxisCameraDaemonReq', 244 | 'traceSpec': { 245 | '__type': 'VideoTraceSpec', 246 | 'traceDir': path.join(remote_traces_dir, self.trace_name), 247 | 'traceName': self.trace_name, 248 | 'timeseqName': timeseq_name, 249 | }, 250 | 'cameraConfig': { 251 | '__type': 'AxisCameraConfig', 252 | 'ipaddr': ipaddr, 253 | 'url': '/axis-cgi/mjpg/video.cgi?compression=30&rotation=0&resolution=' + resolution, 254 | 'authHeader': auth_header, 255 | }, 256 | 'txTime': time.time(), 257 | 'recordFor': 10.0 if want else 0.0, 258 | } 259 | rpc_id = 'rpc%d' % rpc_count 260 | rpc_count += 1 261 | tx = ['record', rpc_id, ujson.dumps(req, escape_forward_slashes=False)] 262 | if 1: logger.info('camera %s < %s', daemon_endpoint, tx) 263 | self.sock.send_multipart(tx, flags=0, copy=True, track=False) 264 | 265 | rx = self.sock.recv_multipart(flags=0, copy=True, track=False) 266 | rx_time = time.time() 267 | if 1: logger.info('%s > %s %s', daemon_endpoint, rx[1], rx[2]) 268 | rpc_id2 = rx[0] 269 | rpc_err = ujson.loads(rx[1]) 270 | rpc_result = ujson.loads(rx[2]) 271 | if rpc_err: 272 | logger.info('%s > error %s', daemon_endpoint, rpc_err) 273 | break 274 | 275 | min_delta = rpc_result['txTime'] - rx_time 276 | max_delta = rpc_result['txTime'] - rpc_result['reqTxTime'] 277 | if delta is None: 278 | delta = (min_delta + max_delta) * 0.5 279 | delta = max(min_delta, min(max_delta, delta)) 280 | if 0: logger.info("timing %0.6f %+0.6f %+0.6f min_delta=%+0.6f max_delta=%+0.6f delta=%+0.6f", 281 | rpc_result['reqTxTime'], rpc_result['txTime'], rx_time, min_delta, max_delta, delta) 282 | 283 | rep_times = rpc_result['times'] 284 | rep_samples = rpc_result['samples'] 285 | for ts, sample in zip(rep_times, rep_samples): 286 | self.add(ts - delta, timeseq_name, sample) 287 | rep_chunks = rpc_result['chunks'] 288 | for chunk_ts in rep_chunks: 289 | chunk_fn = 'chunk_%s_%.0f.video' % (timeseq_name, chunk_ts) 290 | chunk_path = path.join(self.trace_dir, chunk_fn) 291 | src_file = path.join(local_link_prefix, self.trace_name, chunk_fn) 292 | logger.info('Create symlink(%s, %s) llp=%s rtd=%s', src_file, chunk_path, local_link_prefix, remote_traces_dir) 293 | os.symlink(src_file, chunk_path) 294 | 295 | if not want: break 296 | time.sleep(0.1) 297 | 298 | 299 | thr = threading.Thread(target = thread_main) 300 | thr.daemon = True 301 | thr.start() 302 | self.camera_threads.append(thr) 303 | -------------------------------------------------------------------------------- /src/rosbridge/zmq_serialize.py: -------------------------------------------------------------------------------- 1 | """ 2 | Helpers to serialize Gym data types over zmq. 3 | """ 4 | import math, logging, random 5 | import numpy as np 6 | import ujson 7 | import gym, gym.spaces 8 | 9 | logger = logging.getLogger(__name__) 10 | logger.setLevel(logging.INFO) 11 | 12 | def mk_random_cookie(): 13 | return ''.join([random.choice('123456789ABCDEFGHJKLMNPQRSTUVWXYZabcdefghijkmnopqrstuvwxyz') for x in range(12)]) 14 | 15 | def dump_msg(msg): 16 | """ 17 | Turn o into a list of binary strings suitable for sending over zmq. 18 | This is less general than Pickle, but it's: 19 | - High-performance 20 | - Language-neutral. Should be straightforward to read & write from JS or C++ 21 | - Supports Gym objects like subtypes of gym.Space 22 | - Can be handed untrusted data without creating a remote execution risk 23 | It returns a list parts, where: 24 | - parts[0] is a JSON-formatted representation of msg, with some replacements as detailed below. 25 | - parts[1...] encode bulk data structures like numpy arrays in binary. 26 | The following replacements are made: 27 | - numpy arrays are replaced by a description (element type & shape) and reference to the binary 28 | data stored in parts 29 | - Numeric inf is sent as {"__type":"number","value":"inf"} 30 | - Spaces (Box, Tuple, Discrete) are send encoded something like {"__type":"Box","low"=...,"high"=...} 31 | - tuples and objects are wrapped with a {"__type":"tuple", ...} and {"__type":"object", ...} 32 | """ 33 | parts = [None] 34 | msg1 = _dump_msg1(msg, parts) 35 | parts[0] = ujson.dumps(msg1, escape_forward_slashes=False) 36 | return parts 37 | 38 | def _dump_msg1(o, parts): 39 | if isinstance(o, dict): 40 | return dict([(k, _dump_msg1(v, parts)) for k,v in o.items()]) 41 | elif isinstance(o, list): 42 | return [_dump_msg1(v, parts) for v in o] 43 | elif isinstance(o, np.ndarray): 44 | if str(o.dtype) == 'object': 45 | return dict(__type='ndarray', dtype=str(o.dtype), shape=o.shape, flat=[_dump_msg1(x, parts) for x in o.flat]) 46 | else: 47 | partno = len(parts) 48 | parts.append(o.tobytes()) 49 | return dict(__type='ndarray', dtype=str(o.dtype), shape=o.shape, partno=partno) 50 | elif isinstance(o, gym.Space): 51 | if isinstance(o, gym.spaces.Box): 52 | return dict(__type='Box', low=_dump_msg1(o.low, parts), high=_dump_msg1(o.high, parts)) 53 | elif isinstance(o, gym.spaces.Tuple): 54 | return dict(__type='Tuple', spaces=_dump_msg1(o.spaces, parts)) 55 | elif isinstance(o, gym.spaces.Discrete): 56 | return dict(__type='Discrete', n=_dump_msg1(o.n, parts)) 57 | else: 58 | raise Exception('Unknown space %s' % str(o)) 59 | elif isinstance(o, float): 60 | if np.isposinf(o): 61 | return dict(__type='number', value='+inf') 62 | elif np.isneginf(o): 63 | return dict(__type='number', value='-inf') 64 | elif np.isnan(o): 65 | return dict(__type='number', value='nan') 66 | else: 67 | return o 68 | elif isinstance(o, tuple): 69 | return dict(__type='tuple', elems=[_dump_msg1(v, parts) for v in o]) 70 | else: 71 | return o 72 | 73 | def load_msg(parts): 74 | """ 75 | Take a list of binary strings received over zmq, and turn it into an object by reversing the transformations 76 | of dump_msg 77 | """ 78 | msg1 = ujson.loads(parts[0]) 79 | msg = _load_msg1(msg1, parts) 80 | return msg 81 | 82 | def _load_msg1(o, parts): 83 | if isinstance(o, dict): 84 | t = o.get('__type', None) 85 | if t is not None: 86 | if t == 'ndarray': 87 | if o['dtype'] == 'object': 88 | return np.array(o['flat']).reshape(o['shape']) 89 | else: 90 | return np.frombuffer(parts[o['partno']], dtype=o['dtype']).reshape(o['shape']) 91 | elif t == 'Box': 92 | return gym.spaces.Box(low=_load_msg1(o['low'], parts), high=_load_msg1(o['high'], parts)) 93 | elif t == 'Tuple': 94 | return gym.spaces.Tuple(spaces=_load_msg1(o['spaces'], parts)) 95 | elif t == 'Discrete': 96 | return gym.spaces.Discrete(n=_load_msg1(o['n'], parts)) 97 | elif t == 'tuple': 98 | return tuple(_load_msg1(o['elems'], parts)) 99 | elif t == 'number': 100 | if o['value'] == '+inf': 101 | return np.inf 102 | elif o['value'] == '-inf': 103 | return -np.inf 104 | elif o['value'] == 'nan': 105 | return np.nan 106 | else: 107 | raise Exception('Unknown value %s' % o['value']) 108 | else: 109 | logger.warn('Unimplemented object to reconstruct %s', t) 110 | return o 111 | else: 112 | return dict([(k, _load_msg1(v, parts)) for k,v in o.items()]) 113 | elif isinstance(o, list): 114 | return [_load_msg1(v, parts) for v in o] 115 | else: 116 | return o 117 | --------------------------------------------------------------------------------