From 9c7330b54a1ede7628e87efdc9b67cacc6c6eee6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Nicol=C3=B2?= Date: Thu, 19 Nov 2020 16:52:28 +0100 Subject: [PATCH 1/2] Add frame and frameName to forceTorqueSensors --- README.md | 2 ++ simmechanics_to_urdf/firstgen.py | 43 ++++++++++++++++++++++++++------ 2 files changed, 37 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index e1fd0e5..7abe279 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 | `displayName` of the frame in which the sensor measure is expressed. | +| `frame` | String | empty | It is the frame in which the forces and torques should be expressed. If empty, the child frame is used. | | `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" From 64d7358bede2242b3cdbe14c0a40aedda3a309ff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Nicol=C3=B2?= Date: Mon, 23 Nov 2020 15:45:22 +0100 Subject: [PATCH 2/2] Update README --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 7abe279..dc871dd 100644 --- a/README.md +++ b/README.md @@ -212,8 +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 | `displayName` of the frame in which the sensor measure is expressed. | -| `frame` | String | empty | It is the frame in which the forces and torques should be expressed. If empty, the child frame is used. | +| `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