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
joaomoura24 wants to merge 5 commits into
base: main
Choose a base branch
from
Open

Add frame_id to wrench publisher #6

Changes from 1 commit
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
5 changes: 3 additions & 2 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 @@ -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]
Expand Down Expand Up @@ -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)
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