diff --git a/README.md b/README.md index e1fd0e5..dc871dd 100644 --- a/README.md +++ b/README.md @@ -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 `` 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 `` element of type "force_torque" | Note that for now the FT sensors sensor frame is required to be coincident with child link frame, due diff --git a/simmechanics_to_urdf/firstgen.py b/simmechanics_to_urdf/firstgen.py index f4b6a79..c14f2fc 100755 --- a/simmechanics_to_urdf/firstgen.py +++ b/simmechanics_to_urdf/firstgen.py @@ -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 @@ -1839,7 +1856,7 @@ 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"); @@ -1847,13 +1864,17 @@ def getURDFForceTorque(self, jointName, sensorName, directionChildToParent): 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): @@ -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); @@ -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"