From 9d1dc485dbbc65af901577c84721f870540aa491 Mon Sep 17 00:00:00 2001 From: ohwi Date: Sat, 13 Jul 2019 01:48:58 +0900 Subject: [PATCH] Add mass randomization and test --- scripts/envs/open_manipulator/ros_interface.py | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/scripts/envs/open_manipulator/ros_interface.py b/scripts/envs/open_manipulator/ros_interface.py index 0918c75..747eb52 100755 --- a/scripts/envs/open_manipulator/ros_interface.py +++ b/scripts/envs/open_manipulator/ros_interface.py @@ -388,8 +388,6 @@ class OpenManipulatorRosGazeboInterface(OpenManipulatorRosBaseInterface): self.default_settings = {} for link in self.link_names: default = self.get_link_properties(GetLinkPropertiesRequest(link)) - print(type(default)) - print(default.items()) self.default_settings[link] = default def reset_gazebo_world(self, block_pose=None): @@ -403,6 +401,9 @@ class OpenManipulatorRosGazeboInterface(OpenManipulatorRosBaseInterface): time.sleep(0.5) self.set_target_block(block_pose) + self.mass_randomization() + for link in self.link_names: + print(self.get_link_properties(GetLinkPropertiesRequest(link))) def set_target_block(self, block_pose=None): """Set target block Gazebo model""" @@ -485,8 +486,13 @@ class OpenManipulatorRosGazeboInterface(OpenManipulatorRosBaseInterface): for link in self.link_names: request = SetLinkPropertiesRequest() default = self.default_settings[link] - for k, v in default.items(): - pass + # copy values + for slot in request.__slots__: + value = getattr(default, slot) + setattr(request, slot, value) + # randomize mass + request.mass *= np.random.uniform(0.9, 1.1) + self.set_link_properties(request) class OpenManipulatorRosRealInterface(OpenManipulatorRosBaseInterface):