Add frame_id to wrench publisher #6
Conversation
There was a problem hiding this comment.
Looking good @joaomoura24. If you could let me know about the comment.
| @@ -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) | |||
There was a problem hiding this comment.
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?
There was a problem hiding this comment.
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
There was a problem hiding this comment.
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?
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.
This is just a small change to add frame_id when publishing the wrenches.
We need this in order to visualize (in rviz) the frame where the wrench applies to.
Just check and comment, do not merge it yet.
From the visualization i think that the force signs are reversed.
I'll check that tomorrow with the robot to make sure it's correct.
What i mean is, in order to be consistent with a force sensor on a robot, it should read as the force is being externally applied.
For me it seems that it is outputting the internal force instead, which is not what we want. But let me confirm this better.