From 6dc0be00935de45cc966fcec46d4eef7ef7c2b3f Mon Sep 17 00:00:00 2001 From: Joao Moura Date: Mon, 9 Jan 2023 19:19:32 +0000 Subject: [PATCH 1/5] Add frame_id to wrench publisher --- ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py b/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py index 5e41073b..c2737f9d 100644 --- a/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py +++ b/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py @@ -80,9 +80,10 @@ def enable_ft_sensor(self): def ft_sensor_enabled(self): return self.ft_pub_key is not None - def publish_wrench(self, joint_reaction_forces): + def publish_wrench(self, joint_reaction_forces, frame_id): msg = WrenchStamped() msg.header.stamp = self.pb_obj.node.time_now() + msg.header.frame_id = frame_id msg.wrench.force.x = joint_reaction_forces[0] msg.wrench.force.y = joint_reaction_forces[1] msg.wrench.force.z = joint_reaction_forces[2] @@ -270,7 +271,7 @@ def _publish_joint_state(self, event): # Publish ft sensor states for joint, joint_state in zip(self, joint_states): if joint.ft_sensor_enabled: - joint.publish_wrench(joint_state[2]) + joint.publish_wrench(joint_state[2], joint.linkName) # Publish joint state message self.pb_obj.pubs['joint_state'].publish(self.pack_joint_state_msg(joint_states)) From 2ca7f016fcd10fc81940536794f0fc4850c6897f Mon Sep 17 00:00:00 2001 From: Joao Moura Date: Tue, 17 Jan 2023 15:16:17 +0000 Subject: [PATCH 2/5] Invert force reading sign --- .../src/rpbi/pybullet_robot_joints.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py b/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py index c2737f9d..6438018b 100644 --- a/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py +++ b/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py @@ -84,12 +84,12 @@ def publish_wrench(self, joint_reaction_forces, frame_id): msg = WrenchStamped() msg.header.stamp = self.pb_obj.node.time_now() msg.header.frame_id = frame_id - msg.wrench.force.x = joint_reaction_forces[0] - msg.wrench.force.y = joint_reaction_forces[1] - msg.wrench.force.z = joint_reaction_forces[2] - msg.wrench.torque.x = joint_reaction_forces[3] - msg.wrench.torque.y = joint_reaction_forces[4] - msg.wrench.torque.z = joint_reaction_forces[5] + msg.wrench.force.x = -joint_reaction_forces[0] + msg.wrench.force.y = -joint_reaction_forces[1] + msg.wrench.force.z = -joint_reaction_forces[2] + msg.wrench.torque.x = -joint_reaction_forces[3] + msg.wrench.torque.y = -joint_reaction_forces[4] + msg.wrench.torque.z = -joint_reaction_forces[5] self.pb_obj.pubs[self.ft_pub_key].publish(msg) class Joints(list): From 6858e3b8d5055d8c71fa28e15f2c7953fa9b48d9 Mon Sep 17 00:00:00 2001 From: Joao Moura Date: Wed, 18 Jan 2023 16:55:01 +0000 Subject: [PATCH 3/5] Add service to zero measured wrench --- .../src/rpbi/pybullet_robot_joints.py | 36 +++++++++++++++---- 1 file changed, 29 insertions(+), 7 deletions(-) diff --git a/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py b/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py index 6438018b..e3765866 100644 --- a/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py +++ b/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py @@ -4,6 +4,7 @@ from ros_pybullet_interface.msg import JointInfo from sensor_msgs.msg import JointState from geometry_msgs.msg import WrenchStamped +from std_srvs.srv import Empty class Joint: @@ -38,6 +39,10 @@ def __init__(self, pb_obj, jointIndex): self.parentFrameOrn = self.info[15] self.parentIndex = self.info[16] + # wrench offset + self.wrench_offset = [0., 0., 0., 0., 0., 0.] + self.wrench_measured = [0., 0., 0., 0., 0., 0.] + # Parse the joint information as a ros_pybullet_interface/JointInfo ROS message self.joint_info_msg = JointInfo( jointIndex = self.jointIndex, jointName = self.jointName, jointType = self.jointType, @@ -75,23 +80,41 @@ def enable_ft_sensor(self): self.pb_obj.pb.enableJointForceTorqueSensor(self.pb_obj.body_unique_id, self.jointIndex, enableSensor=1) self.ft_pub_key = f'{self.pb_obj.name}_{self.jointName}_ft_sensor' self.pb_obj.pubs[self.ft_pub_key] = self.pb_obj.node.Publisher(f'rpbi/{self.pb_obj.name}/{self.jointName}/ft_sensor', WrenchStamped, queue_size=10) + self.pb_obj.srvs[self.ft_pub_key] = self.pb_obj.node.Service(f'rpbi/{self.pb_obj.name}/{self.jointName}/ft_sensor', Empty, self.zero_wrench) @property def ft_sensor_enabled(self): return self.ft_pub_key is not None def publish_wrench(self, joint_reaction_forces, frame_id): + self.wrench_measured[0] = -joint_reaction_forces[0] + self.wrench_measured[1] = -joint_reaction_forces[1] + self.wrench_measured[2] = -joint_reaction_forces[2] + self.wrench_measured[3] = -joint_reaction_forces[3] + self.wrench_measured[4] = -joint_reaction_forces[4] + self.wrench_measured[5] = -joint_reaction_forces[5] + + # set and publish wrench msg = WrenchStamped() msg.header.stamp = self.pb_obj.node.time_now() msg.header.frame_id = frame_id - msg.wrench.force.x = -joint_reaction_forces[0] - msg.wrench.force.y = -joint_reaction_forces[1] - msg.wrench.force.z = -joint_reaction_forces[2] - msg.wrench.torque.x = -joint_reaction_forces[3] - msg.wrench.torque.y = -joint_reaction_forces[4] - msg.wrench.torque.z = -joint_reaction_forces[5] + msg.wrench.force.x = self.wrench_measured[0] - self.wrench_offset[0] + msg.wrench.force.y = self.wrench_measured[1] - self.wrench_offset[1] + msg.wrench.force.z = self.wrench_measured[2] - self.wrench_offset[2] + msg.wrench.torque.x = self.wrench_measured[3] - self.wrench_offset[3] + msg.wrench.torque.y = self.wrench_measured[4] - self.wrench_offset[4] + msg.wrench.torque.z = self.wrench_measured[5] - self.wrench_offset[5] self.pb_obj.pubs[self.ft_pub_key].publish(msg) + def zero_wrench(self, req): + self.wrench_offset[0] = self.wrench_measured[0] + self.wrench_offset[1] = self.wrench_measured[1] + self.wrench_offset[2] = self.wrench_measured[2] + self.wrench_offset[3] = self.wrench_measured[3] + self.wrench_offset[4] = self.wrench_measured[4] + self.wrench_offset[5] = self.wrench_measured[5] + return [] + class Joints(list): def __init__(self, pb_obj): @@ -129,7 +152,6 @@ def __init__(self, pb_obj): # NOTE: if robot is visual then this data will not be published, even if this is set in config file if not self.pb_obj.is_visual_robot: for name in self.enabled_joint_force_torque_sensors: - self[name].enable_ft_sensor() # Setup target joint state subscriber From 173bec2931fa022abf0af1e1eb44911a0640924d Mon Sep 17 00:00:00 2001 From: Joao Moura Date: Wed, 18 Jan 2023 17:25:01 +0000 Subject: [PATCH 4/5] Use average to zero wrench --- .../src/rpbi/pybullet_robot_joints.py | 54 ++++++++++--------- 1 file changed, 30 insertions(+), 24 deletions(-) diff --git a/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py b/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py index e3765866..3e13cfb5 100644 --- a/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py +++ b/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py @@ -40,6 +40,9 @@ def __init__(self, pb_obj, jointIndex): self.parentIndex = self.info[16] # wrench offset + self.setzero = False + self.setzero_cnt = 0 + self.total_setzero_cnt = 100 self.wrench_offset = [0., 0., 0., 0., 0., 0.] self.wrench_measured = [0., 0., 0., 0., 0., 0.] @@ -87,32 +90,35 @@ def ft_sensor_enabled(self): return self.ft_pub_key is not None def publish_wrench(self, joint_reaction_forces, frame_id): - self.wrench_measured[0] = -joint_reaction_forces[0] - self.wrench_measured[1] = -joint_reaction_forces[1] - self.wrench_measured[2] = -joint_reaction_forces[2] - self.wrench_measured[3] = -joint_reaction_forces[3] - self.wrench_measured[4] = -joint_reaction_forces[4] - self.wrench_measured[5] = -joint_reaction_forces[5] - - # set and publish wrench - msg = WrenchStamped() - msg.header.stamp = self.pb_obj.node.time_now() - msg.header.frame_id = frame_id - msg.wrench.force.x = self.wrench_measured[0] - self.wrench_offset[0] - msg.wrench.force.y = self.wrench_measured[1] - self.wrench_offset[1] - msg.wrench.force.z = self.wrench_measured[2] - self.wrench_offset[2] - msg.wrench.torque.x = self.wrench_measured[3] - self.wrench_offset[3] - msg.wrench.torque.y = self.wrench_measured[4] - self.wrench_offset[4] - msg.wrench.torque.z = self.wrench_measured[5] - self.wrench_offset[5] - self.pb_obj.pubs[self.ft_pub_key].publish(msg) + + wrench_measured = [-wrench for wrench in joint_reaction_forces] + + if self.setzero: + if self.setzero_cnt == 0: + self.wrench_offset = [wrench for wrench in wrench_measured] + self.setzero_cnt += 1 + elif self.setzero_cnt < self.total_setzero_cnt: + self.wrench_offset = [offset + wrench for offset,wrench in zip(self.wrench_offset,wrench_measured)] + self.setzero_cnt += 1 + else: + self.wrench_offset = [offset/self.total_setzero_cnt for offset in self.wrench_offset] + self.setzero_cnt = 0 + self.setzero = False + else: + # set and publish wrench + msg = WrenchStamped() + msg.header.stamp = self.pb_obj.node.time_now() + msg.header.frame_id = frame_id + msg.wrench.force.x = wrench_measured[0] - self.wrench_offset[0] + msg.wrench.force.y = wrench_measured[1] - self.wrench_offset[1] + msg.wrench.force.z = wrench_measured[2] - self.wrench_offset[2] + msg.wrench.torque.x = wrench_measured[3] - self.wrench_offset[3] + msg.wrench.torque.y = wrench_measured[4] - self.wrench_offset[4] + msg.wrench.torque.z = wrench_measured[5] - self.wrench_offset[5] + self.pb_obj.pubs[self.ft_pub_key].publish(msg) def zero_wrench(self, req): - self.wrench_offset[0] = self.wrench_measured[0] - self.wrench_offset[1] = self.wrench_measured[1] - self.wrench_offset[2] = self.wrench_measured[2] - self.wrench_offset[3] = self.wrench_measured[3] - self.wrench_offset[4] = self.wrench_measured[4] - self.wrench_offset[5] = self.wrench_measured[5] + self.setzero = True return [] class Joints(list): From bfe24f79e21afb38585b14176031cf15f53afaf6 Mon Sep 17 00:00:00 2001 From: Joao Moura Date: Wed, 18 Jan 2023 17:29:27 +0000 Subject: [PATCH 5/5] Change wrench zeroing service name --- ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py b/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py index 3e13cfb5..a46005f3 100644 --- a/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py +++ b/ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py @@ -83,7 +83,7 @@ def enable_ft_sensor(self): self.pb_obj.pb.enableJointForceTorqueSensor(self.pb_obj.body_unique_id, self.jointIndex, enableSensor=1) self.ft_pub_key = f'{self.pb_obj.name}_{self.jointName}_ft_sensor' self.pb_obj.pubs[self.ft_pub_key] = self.pb_obj.node.Publisher(f'rpbi/{self.pb_obj.name}/{self.jointName}/ft_sensor', WrenchStamped, queue_size=10) - self.pb_obj.srvs[self.ft_pub_key] = self.pb_obj.node.Service(f'rpbi/{self.pb_obj.name}/{self.jointName}/ft_sensor', Empty, self.zero_wrench) + self.pb_obj.srvs[self.ft_pub_key] = self.pb_obj.node.Service(f'rpbi/{self.pb_obj.name}/{self.jointName}/Zero', Empty, self.zero_wrench) @property def ft_sensor_enabled(self):