From 6113c9f045468da34ba1759db07a9dae52028b61 Mon Sep 17 00:00:00 2001 From: rickstaa Date: Tue, 26 Oct 2021 16:57:19 +0200 Subject: [PATCH] fix: add gravity compensation hotfix This commit adds a temporary gravity compensation hotfix for the controllering the `franka_gazebo` simulation (see https://github.com/frankaemika/franka_ros/issues/160). Can be removed if https://github.com/frankaemika/franka_ros/pull/177 is merged. --- .../src/panda_gazebo/core/control_server.py | 37 ++++++++++++++++++- 1 file changed, 35 insertions(+), 2 deletions(-) diff --git a/panda_gazebo/src/panda_gazebo/core/control_server.py b/panda_gazebo/src/panda_gazebo/core/control_server.py index b685c022..9c106bc9 100644 --- a/panda_gazebo/src/panda_gazebo/core/control_server.py +++ b/panda_gazebo/src/panda_gazebo/core/control_server.py @@ -93,6 +93,8 @@ ARM_TRAJ_CONTROLLERS = ["panda_arm_controller"] HAND_CONTROLLERS = ["franka_gripper"] +USE_GRAVITY_COMPENSATION_FIX = True + class PandaControlServer(object): """Controller server used to send control commands to the simulated Panda Robot. @@ -357,6 +359,22 @@ def __init__( # noqa: C901 ) self._arm_joint_traj_as.start() + # Add a effort publisher + # FIXME: Can be removed if https://github.com/frankaemika/franka_ros/issues/160 + # is fixed + if USE_GRAVITY_COMPENSATION_FIX: + controlled_joints_dict = self._get_controlled_joints(control_type="effort") + self._arm_joint_efforts_setpoint_msg = [ + Float64(val) + for key, val in dict( + zip(self.joint_states.name, self.joint_states.effort) + ).items() + if key in controlled_joints_dict["arm"] + ] + self._effort_command_lock = False + PUBLISH_RATE = 1000 + rospy.Timer(rospy.Duration(1.0 / PUBLISH_RATE), self._effort_publisher_cb) + ################################################ # Panda control member functions ############### ################################################ @@ -1886,7 +1904,9 @@ def _arm_set_joint_efforts_cb(self, set_joint_efforts_req): # noqa: C901 # Save setpoint and publish request self.arm_joint_efforts_setpoint = [command.data for command in control_pub_msgs] + self._arm_joint_efforts_setpoint_msg = control_pub_msgs rospy.logdebug("Publishing Panda arm joint efforts control message.") + self._effort_command_lock = True self._arm_joint_effort_pub.publish(control_pub_msgs) # Wait till control is finished or timeout has been reached @@ -1895,6 +1915,7 @@ def _arm_set_joint_efforts_cb(self, set_joint_efforts_req): # noqa: C901 control_type="effort", ) + self._effort_command_lock = False resp.success = True resp.message = "Everything went OK" return resp @@ -1907,7 +1928,7 @@ def _set_gripper_width_cb(self, set_gripper_width_req): Service request message specifying the gripper width for the robot hand. Returns: - :obj:`panda_gazebo.srv.SetGripperWidthReponse`: Service response. + :obj:`panda_gazebo.srv.SetGripperWidthResponse`: Service response. """ # Check if gripper width is within boundaries if set_gripper_width_req.width < 0.0 or set_gripper_width_req.width > 0.08: @@ -1970,6 +1991,18 @@ def _get_controlled_joints_cb(self, get_controlled_joints_req): resp.success = False return resp + def _effort_publisher_cb(self, event=None): + """Publishes the joint effort commands at a given rate. This is done since + there is a bug in the gravity compensation see + https://github.com/frankaemika/franka_ros/issues/160. As a result this method + can be removed if this issue is fixed. + + Args: + event (:obj:`rospy.TimerEvent`): The timer event object. + """ + if not self._effort_command_lock: + self._arm_joint_effort_pub.publish(self._arm_joint_efforts_setpoint_msg) + ################################################ # Action server callback functions ############# ################################################ @@ -1978,7 +2011,7 @@ def _arm_joint_traj_execute_cb(self, goal): server. Args: - Goal (:obj:`panda_gazebo.msg.FollowJointTrajectoryGoal`): Goal execution + goal (:obj:`panda_gazebo.msg.FollowJointTrajectoryGoal`): Goal execution action server goal message. """ # Check if joint goal.joint_names contains duplicates