Add get object dynamics srv

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(

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

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):
 

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:
 
 

ros_pybullet_interface/srv/GetObjectDynamics.srv

@@ -0,0 +1,5 @@
+string object_name
+---
+bool success
+string message
+ros_pybullet_interface/ObjectDynamics object_dynamics