Skip to content

Commit

Permalink
Merge pull request #45 from robotology/feat/addFrameFTSensor
Browse files Browse the repository at this point in the history 
Add frameName and frame parameters in FT sensor
  • Loading branch information
@Nicogene
Nicogene authored Nov 24, 2020
2 parents 386e2c9 + 64d7358 commit f6055ab
Show file tree
Hide file tree
Showing 2 changed files with 37 additions and 8 deletions.
2 changes: 2 additions & 0 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -212,6 +212,8 @@ this script will output two different elements for each sensor:
| `sensorName` | String | jointName | Name of the sensor, to be used in the output URDF file |
| `exportFrameInURDF` | Bool | False | If true, export a fake URDF link whose frame is coincident with the sensor frame (as if the sensor frame was added to the `exportedFrames` array). |
| `exportedFrameName` | String | sensorName | Name of the URDF link exported by the `exportFrameInURDF` option |
| `frameName` | String | empty | Name of the frame in which the sensor measure is expressed. |
| `frame` | String | empty | The value of this element may be one of: child, parent, or sensor. It is the frame in which the forces and torques should be expressed. The values parent and child refer to the parent or child links of the joint. The value sensor means the measurement is rotated by the rotation component of the `<pose>` of this sensor. The translation component of the pose has no effect on the measurement. |
| `sensorBlobs` | String | empty | Array of strings (possibly on multiple lines) represeting complex XML blobs that will be included as child of the `<sensor>` element of type "force_torque" |

Note that for now the FT sensors sensor frame is required to be coincident with child link frame, due
Expand Down
43 changes: 35 additions & 8 deletions simmechanics_to_urdf/firstgen.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -269,21 +269,38 @@ def addSensors(self):
# for the ft sensors, load the sensors as described in the YAML without further check
for ftSens in self.forceTorqueSensors:
referenceJoint = ftSens["jointName"];
frame = ftSens.get("frame");
frameName = ftSens.get("frameName");
if ('sensorName' not in ftSens.keys()):
sensorName = referenceJoint
else:
sensorName = ftSens["sensorName"];
sensorBlobs = ftSens.get("sensorBlobs");

if (frameName is None):
# If frame is not specified, the sensor frame is the link frame
offset = [0.0, 0.0, 0.0];
rot = quaternion_from_matrix(numpy.identity(4));
else:
# The default referenceLink is the sensorLink itself
sensorJoint = self.joints[referenceJoint];
for key in self.linkNameDisplayName2fid.keys():
# Since frameName is univoque in the map
if (frameName in key):
referenceLink = key[0];
# Get user added frame
sensor_frame_fid = self.linkNameDisplayName2fid[(referenceLink, frameName)];
(offset, rot) = self.tfman.get("X" + referenceLink, sensor_frame_fid)
pose = toGazeboPose(offset, rot);
# Add sensor in Gazebo format
ft_gazebo_el = generatorGazeboSensors.getURDFForceTorque(referenceJoint, sensorName,
ftSens["directionChildToParent"],
sensorBlobs)
sensorBlobs, frame, pose)
self.urdf_xml.append(ft_gazebo_el);

urdf_origin_el = toURDFOriginXMLElement(offset, rot);
# Add sensor in URDF format
ft_sensor_el = generatorURDFSensors.getURDFForceTorque(referenceJoint, sensorName,
ftSens["directionChildToParent"]);
ftSens["directionChildToParent"], frame, urdf_origin_el);
self.urdf_xml.append(ft_sensor_el);

# for the other sensors, we rely on pose given by a USERADDED frame
Expand Down Expand Up @@ -1839,21 +1856,25 @@ class URDFSensorsGenerator:
def __init__(self):
self.dummy = ""

def getURDFForceTorque(self, jointName, sensorName, directionChildToParent):
def getURDFForceTorque(self, jointName, sensorName, directionChildToParent, frame, origin_el):
sensor_el = lxml.etree.Element("sensor")
sensor_el.set("name", sensorName);
sensor_el.set("type", "force_torque");
parent_el = lxml.etree.SubElement(sensor_el, "parent");
parent_el.set("joint", jointName);
force_torque_el = lxml.etree.SubElement(sensor_el, "force_torque")
frame_el = lxml.etree.SubElement(force_torque_el, "frame")
frame_el.text = "child";
if (frame is not None):
assert(frame == "child" or frame == "parent" or frame == "sensor")
frame_el.text = frame;
else:
frame_el.text = "child";
measure_direction_el = lxml.etree.SubElement(force_torque_el, "measure_direction")
if (directionChildToParent):
measure_direction_el.text = "child_to_parent"
else:
measure_direction_el.text = "parent_to_child"

sensor_el.append(origin_el);
return sensor_el;

def getURDFSensor(self, linkName, sensorType, sensorName, origin_el):
Expand All @@ -1871,7 +1892,7 @@ class URDFGazeboSensorsGenerator:
def __init__(self):
self.dummy = ""

def getURDFForceTorque(self, jointName, sensorName, directionChildToParent, sensorBlobs, updateRate=100):
def getURDFForceTorque(self, jointName, sensorName, directionChildToParent, sensorBlobs, frame, pose, updateRate=100):
gazebo_el = lxml.etree.Element("gazebo", reference=jointName)
sensor_el = lxml.etree.SubElement(gazebo_el, "sensor")
sensor_el.set("name", sensorName);
Expand All @@ -1882,7 +1903,13 @@ def getURDFForceTorque(self, jointName, sensorName, directionChildToParent, sens
sensor_el.set("type", "force_torque");
force_torque_el = lxml.etree.SubElement(sensor_el, "force_torque")
frame_el = lxml.etree.SubElement(force_torque_el, "frame")
frame_el.text = "child";
if (frame is not None):
assert(frame == "child" or frame == "parent" or frame == "sensor")
frame_el.text = frame;
else:
frame_el.text = "child";
pose_el = lxml.etree.SubElement(sensor_el, "pose");
pose_el.text = pose;
measure_direction_el = lxml.etree.SubElement(force_torque_el, "measure_direction")
if (directionChildToParent):
measure_direction_el.text = "child_to_parent"
Expand Down

0 comments on commit f6055ab

Please sign in to comment.