diff --git a/ros_pybullet_interface/CMakeLists.txt b/ros_pybullet_interface/CMakeLists.txt index 0b8731c5..7b7908c1 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,8 @@ add_service_files( CalculateInverseKinematics.srv GetDebugVisualizerCamera.srv AddPybulletObject.srv + GetObjectDynamics.srv + ChangeObjectDynamics.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..27adc586 --- /dev/null +++ b/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 diff --git a/ros_pybullet_interface/scripts/ros_pybullet_interface_node.py b/ros_pybullet_interface/scripts/ros_pybullet_interface_node.py index 80a8e9f1..4df010cf 100755 --- a/ros_pybullet_interface/scripts/ros_pybullet_interface_node.py +++ b/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): diff --git a/ros_pybullet_interface/src/rpbi/pybullet_object.py b/ros_pybullet_interface/src/rpbi/pybullet_object.py index fc447bf4..fec8fc1b 100644 --- a/ros_pybullet_interface/src/rpbi/pybullet_object.py +++ b/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: 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 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