Add set 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,8 @@ add_service_files(
   CalculateInverseKinematics.srv
   GetDebugVisualizerCamera.srv
   AddPybulletObject.srv
+  GetObjectDynamics.srv
+  ChangeObjectDynamics.srv
 )
 
 generate_messages(

ros_pybullet_interface/msg/ObjectDynamics.msg

@@ -0,0 +1,9 @@
+float64 mass
+float64 lateral_friction
+float64[] local_inertia_diagonal
+float64 restitution
+float64 rolling_friction
+float64 spinning_friction
+float64 contact_damping
+float64 contact_stiffness
+float64 collision_margin

ros_pybullet_interface/scripts/ros_pybullet_interface_node.py

@@ -15,7 +15,10 @@
 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 ros_pybullet_interface.srv import ChangeObjectDynamics, ChangeObjectDynamicsResponse
 
 from custom_ros_tools.config import load_config, load_configs
 from cob_srvs.srv import SetString, SetStringResponse
@@ -81,6 +84,8 @@ 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)
+        self.Service('rpbi/change_pybullet_object_dynamics', ChangeObjectDynamics, self.service_change_pybullet_object_dynamics)
 
         # Start pybullet
         if self.pybullet_instance.start_pybullet_after_initialization:
@@ -172,6 +177,89 @@ 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['lateralFriction']
+        object_dynamics_msg.local_inertia_diagonal = object_dynamics['localInertiaDiagonal']
+        object_dynamics_msg.restitution = object_dynamics['restitution']
+        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)
+
+
+    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):
 

ros_pybullet_interface/src/rpbi/pybullet_object.py

@@ -68,6 +68,25 @@ 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],
+            "lateralFriction": dynamics_info[1],
+            "localInertiaDiagonal": dynamics_info[2],
+            "restitution": dynamics_info[5],
+            "rollingFriction": dynamics_info[6],
+            "spinningFriction": dynamics_info[7],
+            "contactDamping": dynamics_info[8],
+            "contactStiffness": dynamics_info[9],
+            "collisionMargin": dynamics_info[11]
+        }
+        return dynamics_dict
+
 
     def destroy(self):
         """Removes the object from Pybullet and closes any communication with ROS."""
@@ -85,7 +104,6 @@ def destroy(self):
         # Remove object from pybullet
         self.pb.removeBody(self.body_unique_id)
 
-
 class PybulletObjectArray:
 
 

ros_pybullet_interface/srv/ChangeObjectDynamics.srv

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

ros_pybullet_interface/srv/GetObjectDynamics.srv

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