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
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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)
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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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)]
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
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..3866f44 100755
--- a/examples/demo/eager_demo/src/demo_3_webots.py
+++ b/examples/demo/eager_demo/src/demo_3_webots.py
@@ -35,16 +35,21 @@
engine = WebotsEngine(gui=True)
# 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 +61,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 +71,7 @@
env.render()
obs = env.reset()
- for i in range(25):
+ 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_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()
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