Skip to content

Add frame_id to wrench publisher #6

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 5 commits into
base: main
Choose a base branch
from
Open

Add frame_id to wrench publisher #6

Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
6dc0be0
Add frame_id to wrench publisher
joaomoura24 Jan 9, 2023
2ca7f01
Invert force reading sign
joaomoura24 Jan 17, 2023
6858e3b
Add service to zero measured wrench
joaomoura24 Jan 18, 2023
173bec2
Use average to zero wrench
joaomoura24 Jan 18, 2023
bfe24f7
Change wrench zeroing service name
joaomoura24 Jan 18, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
53 changes: 41 additions & 12 deletions ros_pybullet_interface/src/rpbi/pybullet_robot_joints.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:

Expand Down Expand Up @@ -38,6 +39,13 @@ def __init__(self, pb_obj, jointIndex):
self.parentFrameOrn = self.info[15]
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.]

# 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,
Expand Down Expand Up @@ -75,21 +83,43 @@ 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}/Zero', 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):
msg = WrenchStamped()
msg.header.stamp = self.pb_obj.node.time_now()
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)
def publish_wrench(self, joint_reaction_forces, frame_id):

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.setzero = True
return []

class Joints(list):

Expand Down Expand Up @@ -128,7 +158,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
Expand Down Expand Up @@ -270,7 +299,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)
Copy link
Member

@cmower cmower Jan 9, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Would it be worth making this modifiable in the config (i.e. user can set the frame_id in yaml) ? Also, doesn't it make more sense to use the jointName rather than the link?

Copy link
Member Author

@joaomoura24 joaomoura24 Jan 9, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Regarding the first question, that was my first though when going into the code, but it becomes a bit complicated for the case you have multiple sensors in one robot, in which case you would have to associate the two lists and think about the ordering. Using the link associated with the ft sensor enabled joint should work well because in the urdf you can always make sure that the fixed joint you want to measure has as parent the correct link in which the forces are measured

Regarding the second question, no, the frame_id is a name of a link

Copy link
Member

@cmower cmower Jan 10, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Regarding the first question, that was my first though when going into the code, but it becomes a bit complicated for the case you have multiple sensors in one robot, in which case you would have to associate the two lists and think about the ordering. Using the link associated with the ft sensor enabled joint should work well because in the urdf you can always make sure that the fixed joint you want to measure has as parent the correct link in which the forces are measured

Ok, sounds good.

Regarding the second question, no, the frame_id is a name of a link

I'm not sure. The user specifies a joint in the yaml config when enabling the FT sensor and the reading is retrieved from pybullet using getJointState. So the reading is associated with a joint, not the link. Furthermore, there is the parent and child link. So, is the link name you're passing the parent or child, and why that choice?

Copy link
Member Author

@joaomoura24 joaomoura24 Jan 10, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The joints are always defined with respect to a frame/link. So even though the reading corresponds to a joint, it has to have a frame_id which is the respective link. Now, if that link is the parent or child, that's a question of decision. I think the norm is to have the parent link as the reference.

Copy link
Member Author

@joaomoura24 joaomoura24 Jan 25, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I checked the sign of the reading force and fixed that.
I also added an additional service to calibrate (zeroing) the force sensor in ros-pybullet, so we have the same procedure on the pybullet and on the robot.
From my side it should be ready to merge.
Check if there are any conflicts.


# Publish joint state message
self.pb_obj.pubs['joint_state'].publish(self.pack_joint_state_msg(joint_states))
Expand Down