From 260f28091778e2d6c12f32084dae7012e47199cd Mon Sep 17 00:00:00 2001 From: Joao Moura Date: Wed, 25 Jan 2023 16:17:48 +0000 Subject: [PATCH 1/3] Add get object dynamics srv --- ros_pybullet_interface/CMakeLists.txt | 2 + ros_pybullet_interface/msg/ObjectDynamics.msg | 12 +++++ .../scripts/ros_pybullet_interface_node.py | 46 +++++++++++++++++++ .../src/rpbi/pybullet_object.py | 23 +++++++++- .../srv/GetObjectDynamics.srv | 5 ++ 5 files changed, 87 insertions(+), 1 deletion(-) create mode 100644 ros_pybullet_interface/msg/ObjectDynamics.msg create mode 100644 ros_pybullet_interface/srv/GetObjectDynamics.srv diff --git a/ros_pybullet_interface/CMakeLists.txt b/ros_pybullet_interface/CMakeLists.txt index 0b8731c5..a7d72db0 100644 --- a/ros_pybullet_interface/CMakeLists.txt +++ b/ros_pybullet_interface/CMakeLists.txt @@ -17,6 +17,7 @@ add_message_files( JointInfo.msg CalculateInverseKinematicsProblem.msg PybulletObject.msg + ObjectDynamics.msg KeyboardEvent.msg MouseEvent.msg ) @@ -29,6 +30,7 @@ add_service_files( CalculateInverseKinematics.srv GetDebugVisualizerCamera.srv AddPybulletObject.srv + GetObjectDynamics.srv ) generate_messages( diff --git a/ros_pybullet_interface/msg/ObjectDynamics.msg b/ros_pybullet_interface/msg/ObjectDynamics.msg new file mode 100644 index 00000000..35cb38e4 --- /dev/null +++ b/ros_pybullet_interface/msg/ObjectDynamics.msg @@ -0,0 +1,12 @@ +float64 mass +float64 lateral_friction +float64[] local_inertia_diagonal +float64[] local_inertia_pos +float64[] local_inertia_orn +float64 restitution +float64 rolling_friction +float64 spinning_friction +float64 contact_damping +float64 contact_stiffness +uint64 body_type +float64 collision_margin diff --git a/ros_pybullet_interface/scripts/ros_pybullet_interface_node.py b/ros_pybullet_interface/scripts/ros_pybullet_interface_node.py index 80a8e9f1..709db9b4 100755 --- a/ros_pybullet_interface/scripts/ros_pybullet_interface_node.py +++ b/ros_pybullet_interface/scripts/ros_pybullet_interface_node.py @@ -15,7 +15,9 @@ from rpbi.pybullet_urdf import PybulletURDF from ros_pybullet_interface.msg import PybulletObject +from ros_pybullet_interface.msg import ObjectDynamics from ros_pybullet_interface.srv import AddPybulletObject, AddPybulletObjectResponse +from ros_pybullet_interface.srv import GetObjectDynamics, GetObjectDynamicsResponse from custom_ros_tools.config import load_config, load_configs from cob_srvs.srv import SetString, SetStringResponse @@ -81,6 +83,7 @@ def add_list(filenames, object_type): # Start services self.Service('rpbi/add_pybullet_object', AddPybulletObject, self.service_add_pybullet_object) self.Service('rpbi/remove_pybullet_object', SetString, self.service_remove_pybullet_object) + self.Service('rpbi/get_pybullet_object_dynamics', GetObjectDynamics, self.service_get_pybullet_object_dynamics) # Start pybullet if self.pybullet_instance.start_pybullet_after_initialization: @@ -172,6 +175,49 @@ def service_add_pybullet_object(self, req): message = 'failed to add pybullet object, neither filename of config was given in request!' return AddPybulletObjectResponse(success=success, message=message) + def service_get_pybullet_object_dynamics(self, req): + + success = True + message = 'got pybullet object dynamics' + + # Get object + if req.object_name in self.pybullet_objects: + object = self.pybullet_objects[req.object_name] + else: + success = False + message = f"did not recognize object name" + self.logerr(message) + return GetObjectDynamicsResponse(success=success, message=message, object_dynamics=None) + + # Get object type + if isinstance(object, PybulletCollisionObject): + object_type = PybulletCollisionObject + elif isinstance(object, PybulletDynamicObject): + object_type = PybulletDynamicObject + else: + success = False + message = f"did not recognize object type" + self.logerr(message) + return GetObjectDynamicsResponse(success=success, message=message, object_dynamics=None) + + object_dynamics = object.get_dynamics() + + object_dynamics_msg = ObjectDynamics() + object_dynamics_msg.mass = object_dynamics['mass'] + object_dynamics_msg.lateral_friction = object_dynamics['lateral friction'] + object_dynamics_msg.local_inertia_diagonal = object_dynamics['local inertia diagonal'] + object_dynamics_msg.local_inertia_pos = object_dynamics['local inertia pos'] + object_dynamics_msg.local_inertia_orn = object_dynamics['local inertia orn'] + object_dynamics_msg.restitution = object_dynamics['restitution'] + object_dynamics_msg.rolling_friction = object_dynamics['rolling friction'] + object_dynamics_msg.spinning_friction = object_dynamics['spinning friction'] + object_dynamics_msg.contact_damping = object_dynamics['contact damping'] + object_dynamics_msg.contact_stiffness = object_dynamics['contact stiffness'] + object_dynamics_msg.body_type = object_dynamics['body type'] + object_dynamics_msg.collision_margin = object_dynamics['collision margin'] + + return GetObjectDynamicsResponse(success=success, message=message, object_dynamics=object_dynamics_msg) + def service_remove_pybullet_object(self, req): diff --git a/ros_pybullet_interface/src/rpbi/pybullet_object.py b/ros_pybullet_interface/src/rpbi/pybullet_object.py index fc447bf4..3a9fa695 100644 --- a/ros_pybullet_interface/src/rpbi/pybullet_object.py +++ b/ros_pybullet_interface/src/rpbi/pybullet_object.py @@ -68,6 +68,28 @@ def change_dynamics(self, config, link_index=-1): config['activationState'] = getattr(self.pb, config['activationState']) self.pb.changeDynamics(**config) + def get_dynamics(self, link_index=-1): + """Exposes changeDynamics.""" + config = {} + config['bodyUniqueId'] = self.body_unique_id + config['linkIndex'] = link_index + dynamics_info = self.pb.getDynamicsInfo(**config) + dynamics_dict = { + "mass": dynamics_info[0], + "lateral friction": dynamics_info[1], + "local inertia diagonal": dynamics_info[2], + "local inertia pos": dynamics_info[3], + "local inertia orn": dynamics_info[4], + "restitution": dynamics_info[5], + "rolling friction": dynamics_info[6], + "spinning friction": dynamics_info[7], + "contact damping": dynamics_info[8], + "contact stiffness": dynamics_info[9], + "body type": dynamics_info[10], + "collision margin": dynamics_info[11] + } + return dynamics_dict + def destroy(self): """Removes the object from Pybullet and closes any communication with ROS.""" @@ -85,7 +107,6 @@ def destroy(self): # Remove object from pybullet self.pb.removeBody(self.body_unique_id) - class PybulletObjectArray: diff --git a/ros_pybullet_interface/srv/GetObjectDynamics.srv b/ros_pybullet_interface/srv/GetObjectDynamics.srv new file mode 100644 index 00000000..9e4d7501 --- /dev/null +++ b/ros_pybullet_interface/srv/GetObjectDynamics.srv @@ -0,0 +1,5 @@ +string object_name +--- +bool success +string message +ros_pybullet_interface/ObjectDynamics object_dynamics \ No newline at end of file From 1448653c20ba07e3bd826233758374bf058f20fc Mon Sep 17 00:00:00 2001 From: Joao Moura Date: Wed, 25 Jan 2023 16:43:37 +0000 Subject: [PATCH 2/3] Add change object dynamics srv --- ros_pybullet_interface/CMakeLists.txt | 1 + .../scripts/ros_pybullet_interface_node.py | 45 +++++++++++++++++++ .../srv/ChangeObjectDynamics.srv | 5 +++ 3 files changed, 51 insertions(+) create mode 100644 ros_pybullet_interface/srv/ChangeObjectDynamics.srv diff --git a/ros_pybullet_interface/CMakeLists.txt b/ros_pybullet_interface/CMakeLists.txt index a7d72db0..7b7908c1 100644 --- a/ros_pybullet_interface/CMakeLists.txt +++ b/ros_pybullet_interface/CMakeLists.txt @@ -31,6 +31,7 @@ add_service_files( GetDebugVisualizerCamera.srv AddPybulletObject.srv GetObjectDynamics.srv + ChangeObjectDynamics.srv ) generate_messages( diff --git a/ros_pybullet_interface/scripts/ros_pybullet_interface_node.py b/ros_pybullet_interface/scripts/ros_pybullet_interface_node.py index 709db9b4..1ef94f08 100755 --- a/ros_pybullet_interface/scripts/ros_pybullet_interface_node.py +++ b/ros_pybullet_interface/scripts/ros_pybullet_interface_node.py @@ -18,6 +18,7 @@ from ros_pybullet_interface.msg import ObjectDynamics from ros_pybullet_interface.srv import AddPybulletObject, AddPybulletObjectResponse from ros_pybullet_interface.srv import GetObjectDynamics, GetObjectDynamicsResponse +from ros_pybullet_interface.srv import ChangeObjectDynamics, ChangeObjectDynamicsResponse from custom_ros_tools.config import load_config, load_configs from cob_srvs.srv import SetString, SetStringResponse @@ -84,6 +85,7 @@ def add_list(filenames, object_type): self.Service('rpbi/add_pybullet_object', AddPybulletObject, self.service_add_pybullet_object) self.Service('rpbi/remove_pybullet_object', SetString, self.service_remove_pybullet_object) self.Service('rpbi/get_pybullet_object_dynamics', GetObjectDynamics, self.service_get_pybullet_object_dynamics) + self.Service('rpbi/change_pybullet_object_dynamics', ChangeObjectDynamics, self.service_change_pybullet_object_dynamics) # Start pybullet if self.pybullet_instance.start_pybullet_after_initialization: @@ -219,6 +221,49 @@ def service_get_pybullet_object_dynamics(self, req): return GetObjectDynamicsResponse(success=success, message=message, object_dynamics=object_dynamics_msg) + def service_change_pybullet_object_dynamics(self, req): + + success = True + message = 'changed pybullet object dynamics' + + # Get object + if req.object_name in self.pybullet_objects: + object = self.pybullet_objects[req.object_name] + else: + success = False + message = f"did not recognize object name" + self.logerr(message) + return ChangeObjectDynamicsResponse(success=success, message=message) + + # Get object type + if isinstance(object, PybulletCollisionObject): + object_type = PybulletCollisionObject + elif isinstance(object, PybulletDynamicObject): + object_type = PybulletDynamicObject + else: + success = False + message = f"did not recognize object type" + self.logerr(message) + return ChangeObjectDynamicsResponse(success=success, message=message) + + object_dynamics_msg = req.object_dynamics + object_dynamics = {} + + object_dynamics['mass'] = object_dynamics_msg.mass + object_dynamics['lateralFriction'] = object_dynamics_msg.lateral_friction + object_dynamics['localInertiaDiagonal'] = object_dynamics_msg.local_inertia_diagonal + object_dynamics['restitution'] = object_dynamics_msg.restitution + object_dynamics['rollingFriction'] = object_dynamics_msg.rolling_friction + object_dynamics['spinningFriction'] = object_dynamics_msg.spinning_friction + object_dynamics['contactDamping'] = object_dynamics_msg.contact_damping + object_dynamics['contactStiffness'] = object_dynamics_msg.contact_stiffness + object_dynamics['collisionMargin'] = object_dynamics_msg.collision_margin + + object.change_dynamics(object_dynamics) + + return ChangeObjectDynamicsResponse(success=success, message=message) + + def service_remove_pybullet_object(self, req): success = True diff --git a/ros_pybullet_interface/srv/ChangeObjectDynamics.srv b/ros_pybullet_interface/srv/ChangeObjectDynamics.srv new file mode 100644 index 00000000..5f57c97d --- /dev/null +++ b/ros_pybullet_interface/srv/ChangeObjectDynamics.srv @@ -0,0 +1,5 @@ +string object_name +ros_pybullet_interface/ObjectDynamics object_dynamics +--- +bool success +string message \ No newline at end of file From 5d3dbf37f754888fd8d771ab82c3e0b849efb35f Mon Sep 17 00:00:00 2001 From: Joao Moura Date: Wed, 25 Jan 2023 17:03:38 +0000 Subject: [PATCH 3/3] Simplify object dyn msg --- ros_pybullet_interface/msg/ObjectDynamics.msg | 3 --- .../scripts/ros_pybullet_interface_node.py | 17 +++++++---------- .../src/rpbi/pybullet_object.py | 17 +++++++---------- 3 files changed, 14 insertions(+), 23 deletions(-) diff --git a/ros_pybullet_interface/msg/ObjectDynamics.msg b/ros_pybullet_interface/msg/ObjectDynamics.msg index 35cb38e4..27adc586 100644 --- a/ros_pybullet_interface/msg/ObjectDynamics.msg +++ b/ros_pybullet_interface/msg/ObjectDynamics.msg @@ -1,12 +1,9 @@ float64 mass float64 lateral_friction float64[] local_inertia_diagonal -float64[] local_inertia_pos -float64[] local_inertia_orn float64 restitution float64 rolling_friction float64 spinning_friction float64 contact_damping float64 contact_stiffness -uint64 body_type float64 collision_margin diff --git a/ros_pybullet_interface/scripts/ros_pybullet_interface_node.py b/ros_pybullet_interface/scripts/ros_pybullet_interface_node.py index 1ef94f08..4df010cf 100755 --- a/ros_pybullet_interface/scripts/ros_pybullet_interface_node.py +++ b/ros_pybullet_interface/scripts/ros_pybullet_interface_node.py @@ -206,17 +206,14 @@ def service_get_pybullet_object_dynamics(self, req): object_dynamics_msg = ObjectDynamics() object_dynamics_msg.mass = object_dynamics['mass'] - object_dynamics_msg.lateral_friction = object_dynamics['lateral friction'] - object_dynamics_msg.local_inertia_diagonal = object_dynamics['local inertia diagonal'] - object_dynamics_msg.local_inertia_pos = object_dynamics['local inertia pos'] - object_dynamics_msg.local_inertia_orn = object_dynamics['local inertia orn'] + object_dynamics_msg.lateral_friction = object_dynamics['lateralFriction'] + object_dynamics_msg.local_inertia_diagonal = object_dynamics['localInertiaDiagonal'] object_dynamics_msg.restitution = object_dynamics['restitution'] - object_dynamics_msg.rolling_friction = object_dynamics['rolling friction'] - object_dynamics_msg.spinning_friction = object_dynamics['spinning friction'] - object_dynamics_msg.contact_damping = object_dynamics['contact damping'] - object_dynamics_msg.contact_stiffness = object_dynamics['contact stiffness'] - object_dynamics_msg.body_type = object_dynamics['body type'] - object_dynamics_msg.collision_margin = object_dynamics['collision margin'] + object_dynamics_msg.rolling_friction = object_dynamics['rollingFriction'] + object_dynamics_msg.spinning_friction = object_dynamics['spinningFriction'] + object_dynamics_msg.contact_damping = object_dynamics['contactDamping'] + object_dynamics_msg.contact_stiffness = object_dynamics['contactStiffness'] + object_dynamics_msg.collision_margin = object_dynamics['collisionMargin'] return GetObjectDynamicsResponse(success=success, message=message, object_dynamics=object_dynamics_msg) diff --git a/ros_pybullet_interface/src/rpbi/pybullet_object.py b/ros_pybullet_interface/src/rpbi/pybullet_object.py index 3a9fa695..fec8fc1b 100644 --- a/ros_pybullet_interface/src/rpbi/pybullet_object.py +++ b/ros_pybullet_interface/src/rpbi/pybullet_object.py @@ -76,17 +76,14 @@ def get_dynamics(self, link_index=-1): dynamics_info = self.pb.getDynamicsInfo(**config) dynamics_dict = { "mass": dynamics_info[0], - "lateral friction": dynamics_info[1], - "local inertia diagonal": dynamics_info[2], - "local inertia pos": dynamics_info[3], - "local inertia orn": dynamics_info[4], + "lateralFriction": dynamics_info[1], + "localInertiaDiagonal": dynamics_info[2], "restitution": dynamics_info[5], - "rolling friction": dynamics_info[6], - "spinning friction": dynamics_info[7], - "contact damping": dynamics_info[8], - "contact stiffness": dynamics_info[9], - "body type": dynamics_info[10], - "collision margin": dynamics_info[11] + "rollingFriction": dynamics_info[6], + "spinningFriction": dynamics_info[7], + "contactDamping": dynamics_info[8], + "contactStiffness": dynamics_info[9], + "collisionMargin": dynamics_info[11] } return dynamics_dict