Skip to content

Commit

Permalink
Merge pull request #42 from robotology/feat/addSensorTransform
Browse files Browse the repository at this point in the history 
Add additionalTransform parameter to exportedFrames
  • Loading branch information
@Nicogene
Nicogene authored Nov 5, 2020
2 parents 1bd48b2 + dd10964 commit 386e2c9
Show file tree
Hide file tree
Showing 2 changed files with 95 additions and 23 deletions.
43 changes: 22 additions & 21 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ The parameter accepted by the script are documented in the following.
##### Naming Parameters
| Attribute name | Type | Default Value | Description |
|:----------------:|:---------:|:------------:|:-------------:|
| `robotName` | String | model name set in the file PhysicalModelingXMLFile | Used for setting the model name, i.e. the parameter `<robot name="...">` in the `URDF` model file. |
| `robotName` | String | model name set in the file PhysicalModelingXMLFile | Used for setting the model name, i.e. the parameter `<robot name="...">` in the `URDF` model file. |
| `rename` | Map | {} (Empty Map) | Structure mapping the SimMechanics XML names to the desired URDF names. |


Expand All @@ -108,25 +108,26 @@ The parameter accepted by the script are documented in the following.
##### Frame Parameters
| Attribute name | Type | Default Value | Description |
|:----------------:|:---------:|:------------:|:-------------:|
| `linkFrames` | Array | empty | Structure mapping the link names to the displayName of their desired frame. Unfortunatly in URDF the link frame origin placement is not free, but it is constrained to be placed on the parent joint axis, hence this option is for now reserved to the root link and to links connected to their parent by a fixed joint |
| `linkFrames` | Array | empty | Structure mapping the link names to the displayName of their desired frame. Unfortunatly in URDF the link frame origin placement is not free, but it is constrained to be placed on the parent joint axis, hence this option is for now reserved to the root link and to links connected to their parent by a fixed joint |
| `exportAllUseradded` | Boolean | False | If true, export all SimMechanics frame tagged with `USERADDED` in the output URDF as fake links i.e. fake link with zero mass connected to a link with a fixed joint.. |
| `exportedFrames` | Array | empty | Array of `displayName` of UserAdded frames to export. This are exported as fixed URDF frames, i.e. fake link with zero mass connected to a link with a fixed joint. |

###### Link Frames Parameters (keys of elements of `linkFrames`)
| Attribute name | Type | Default Value | Description |
|:----------------:|:---------:|:-----------:|:-------------:|
| `linkName` | String | Mandatory | name of the link for which we want to set the frame |
| `frameName` | String | Mandatory | `displayName` of the frame that we want to use as link frame. This frame should be attached to the `frameReferenceLink` link. The default value for `frameReferenceLink` is `linkName` |
| `linkName` | String | Mandatory | name of the link for which we want to set the frame |
| `frameName` | String | Mandatory | `displayName` of the frame that we want to use as link frame. This frame should be attached to the `frameReferenceLink` link. The default value for `frameReferenceLink` is `linkName` |
| `frameReferenceLink` | String | empty | `frameReferenceLink` at which the frame is attached. If `frameReferenceLink` is empty, it will default to `linkName` |



###### Exported Frame Parameters (keys of elements of `exportedFrames`)
| Attribute name | Type | Default Value | Description |
|:----------------:|:---------:|:------------:|:-------------:|
| `frameName` | String | Mandatory | `displayName` of the frame in which the sensor measure is expressed. The selected frame should be attached to the `referenceLink` link, but `referenceLink` can be be omitted if the frameName is unique in the model. |
| `frameName` | String | Mandatory | `displayName` of the frame in which the sensor measure is expressed. The selected frame should be attached to the `referenceLink` link, but `referenceLink` can be be omitted if the frameName is unique in the model. |
| `frameReferenceLink` | String | empty | `frameReferenceLink` at which the frame is attached. If `referenceLink` is empty, it will be selected the first USERADDED frame with the specified `frameName` |
| `exportedFrameName` | String | sensorName | Name of the URDF link exported by the `exportedFrames` option |
| `exportedFrameName` | String | sensorName | Name of the URDF link exported by the `exportedFrames` option |
| `additionalTransformation` | List | Empty | Additional transformation applied to the exported frame, it is expressed as [x, y, z, r, p, y] according to the semantics and units of the [SDF convention](http://sdformat.org/tutorials?tut=specify_pose&cat=specification&) for expressing poses. If the unmodified transformation of the additionalFrame is indicated as linkFrame_H_additionalFrameOld, this parameter specifies the additionalFrameOld_H_additionalFrame transform, and the final transform exported in the URDF is computed as linkFrame_H_additionalFrame = linkFrame_H_additionalFrameOld*additionalFrameOld_H_additionalFrame . If not specified it is assume to be the `[0, 0, 0, 0, 0, 0]` element and the specified frame is exported in the URDF unmodified. |

##### Mesh Parameters
| Attribute name | Type | Default Value | Description |
Expand All @@ -141,7 +142,7 @@ The parameter accepted by the script are documented in the following.
###### Assigned collision geometries (keys of elements of `assignedCollisionGeometry`)
| Attribute name | Type | Default Value | Description |
|:----------------:|:---------:|:------------:|:-------------:|
| `linkName` | String | Mandatory | Name of the link for which the collision geometry is specified. |
| `linkName` | String | Mandatory | Name of the link for which the collision geometry is specified. |
| `geometricShape` | Dictionary | Mandatory | This dictionary contains the parameters used to define the type and the position of the geometric shape. In particular we have: <ul><li>shape: geometric shape type. Supported "box", "cylinder", "sphere". </li><li>type dependent geometric shape parameters. Refer to [SDF Geometry](http://sdformat.org/spec?elem=geometry). </li><li>origin: String defining the pose of the geometric shape with respect to the `linkFrame`. </li></ul> |

~~~
Expand All @@ -163,18 +164,18 @@ assignedCollisionGeometry:
##### Inertia parameters
Parameters related to the inertia parameters of a link

| Attribute name | Type | Default Value | Description |
| Attribute name | Type | Default Value | Description |
|:----------------:|:---------:|:------------:|:-------------:|
| `assignedMasses` | Map | {} (Empty Map) | If a link is in this map, the mass found in the SimMechanics file is substituted with the one passed through this map. Furthermore, the inertia matrix present in the SimMechanics file is scaled accounting for the new mass (i.e. multiplied by new_mass/old_mass). The mass is expressed in Kg. |
| `assignedInertias` | Array | empty | Structure for redefining the inertia tensor (at the COM) for a given link. |
| `assignedMasses` | Map | {} (Empty Map) | If a link is in this map, the mass found in the SimMechanics file is substituted with the one passed through this map. Furthermore, the inertia matrix present in the SimMechanics file is scaled accounting for the new mass (i.e. multiplied by new_mass/old_mass). The mass is expressed in Kg. |
| `assignedInertias` | Array | empty | Structure for redefining the inertia tensor (at the COM) for a given link. |

###### Assigned Inertias parameters (elements of `assignedInertias` parameters)
| Attribute name | Type | Default Value | Description |
|:----------------:|:---------:|:-----------:|:-------------:|
| `linkName` | String | Mandatory | name of the link for which we want to set the frame |
| `xx` | String | empty | If defined, change the Ixx value of the inertia matrix of the link. Unit of measure: Kg*m^2 . |
| `yy` | String | empty | If defined, change the Iyy value of the inertia matrix of the link. Unit of measure: Kg*m^2 . |
| `zz` | String | empty | If defined, change the Izz value of the inertia matrix of the link. Unit of measure: Kg*m^2 . |
| `linkName` | String | Mandatory | name of the link for which we want to set the frame |
| `xx` | String | empty | If defined, change the Ixx value of the inertia matrix of the link. Unit of measure: Kg*m^2 . |
| `yy` | String | empty | If defined, change the Iyy value of the inertia matrix of the link. Unit of measure: Kg*m^2 . |
| `zz` | String | empty | If defined, change the Izz value of the inertia matrix of the link. Unit of measure: Kg*m^2 . |

~~~
assignedMasses:
Expand Down Expand Up @@ -207,11 +208,11 @@ this script will output two different elements for each sensor:
| Attribute name | Type | Default Value | Description |
|:----------------:|:---------:|:------------:|:-------------:|
| `jointName` | String | empty | Name of the Joint for which this sensor measure the ForceTorque. |
| `directionChildToParent` | Bool | True | True if the sensor measures the force excerted by the child on the parent, false otherwise |
| `directionChildToParent` | Bool | True | True if the sensor measures the force excerted by the child on the parent, false otherwise |
| `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 |
| `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" |
| `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 |
| `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
to URDF limitations.
Expand Down Expand Up @@ -268,9 +269,9 @@ are listed in the following:

| Attribute name | Required | Unit of Measure | Description |
|:--------------:|:--------:|:----------------:|:---------------:|
| joint_name | **Yes** | - | Name of the joint to which the content line is referring |
| lower_limit | No | Degrees | `lower` attribute of the `limit` child element of the URDF `joint`. **Please note that we specify this limit here in Degrees, but in the urdf it is expressed in Radians, the script will take care of internally converting this parameter.** |
| upper_limit | No | Degrees | `upper` attribute of the `limit` child element of the URDF `joint`. **Please note that we specify this limit here in Degrees, but in the urdf it is expressed in Radians, the script will take care of internally converting this parameter.** |
| joint_name | **Yes** | - | Name of the joint to which the content line is referring |
| lower_limit | No | Degrees | `lower` attribute of the `limit` child element of the URDF `joint`. **Please note that we specify this limit here in Degrees, but in the urdf it is expressed in Radians, the script will take care of internally converting this parameter.** |
| upper_limit | No | Degrees | `upper` attribute of the `limit` child element of the URDF `joint`. **Please note that we specify this limit here in Degrees, but in the urdf it is expressed in Radians, the script will take care of internally converting this parameter.** |
| velocity_limit | No | Radians/second | `velocity` attribute of the `limit` child element of the URDF `joint`. |
| effort_limit | No | Newton meters | `effort` attribute of the `limit` child element of the URDF `joint`.
| damping | No | Newton meter seconds / radians | `damping` of the `dynamics` child element of the URDF `joint`. |
Expand Down
75 changes: 73 additions & 2 deletions simmechanics_to_urdf/firstgen.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,68 @@
_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())


def euler_matrix(ai, aj, ak, axes='sxyz'):
"""Return homogeneous rotation matrix from Euler angles and axis sequence.
ai, aj, ak : Euler's roll, pitch and yaw angles
axes : One of 24 axis sequences as string or encoded tuple
>>> R = euler_matrix(1, 2, 3, 'syxz')
>>> numpy.allclose(numpy.sum(R[0]), -1.34786452)
True
>>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1))
>>> numpy.allclose(numpy.sum(R[0]), -0.383436184)
True
>>> ai, aj, ak = (4*math.pi) * (numpy.random.random(3) - 0.5)
>>> for axes in _AXES2TUPLE.keys():
... R = euler_matrix(ai, aj, ak, axes)
>>> for axes in _TUPLE2AXES.keys():
... R = euler_matrix(ai, aj, ak, axes)
"""
try:
firstaxis, parity, repetition, frame = _AXES2TUPLE[axes]
except (AttributeError, KeyError):
_TUPLE2AXES[axes] # noqa: validation
firstaxis, parity, repetition, frame = axes

i = firstaxis
j = _NEXT_AXIS[i+parity]
k = _NEXT_AXIS[i-parity+1]

if frame:
ai, ak = ak, ai
if parity:
ai, aj, ak = -ai, -aj, -ak

si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak)
ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak)
cc, cs = ci*ck, ci*sk
sc, ss = si*ck, si*sk

M = numpy.identity(4)
if repetition:
M[i, i] = cj
M[i, j] = sj*si
M[i, k] = sj*ci
M[j, i] = sj*sk
M[j, j] = -cj*ss+cc
M[j, k] = -cj*cs-sc
M[k, i] = -sj*ck
M[k, j] = cj*sc+cs
M[k, k] = cj*cc-ss
else:
M[i, i] = cj*ck
M[i, j] = sj*sc-cs
M[i, k] = sj*cc+ss
M[j, i] = cj*sk
M[j, j] = sj*ss+cc
M[j, k] = sj*cs-sc
M[k, i] = -sj
M[k, j] = cj*si
M[k, k] = cj*ci
return M

def euler_from_matrix(matrix, axes='sxyz'):
"""Return Euler angles from rotation matrix for specified axis sequence.
Expand Down Expand Up @@ -249,12 +311,13 @@ def addSensors(self):
sensor_frame_fid = self.linkNameDisplayName2fid[(referenceLink, frameName)];
(offset, rot) = self.tfman.get("X" + sensorLink, sensor_frame_fid)


if (sensorName is None):
sensorName = sensorLink + "_" + frameName;

# Add sensors in Gazebo format
pose = toGazeboPose(offset, rot);


# sys.stderr.write("Processing link " + link['uid'] + "\n")

gazeboSensorType = self.sensorTypeUrdf2sdf.get(sensorType, sensorType);
Expand Down Expand Up @@ -602,7 +665,15 @@ def parseFrames(self, frames, parent_link):
if (map_key in self.exportedFramesMap.keys()):
if ("exportedFrameName" in self.exportedFramesMap[map_key].keys()):
useradded_frame_name = self.exportedFramesMap[map_key]["exportedFrameName"];

if ("additionalTransformation" in self.exportedFramesMap[map_key].keys()):
addTransform = self.exportedFramesMap[map_key]["additionalTransformation"];

if (addTransform is not None):
assert(len(addTransform) is 6)
sensorOriginal_R_sensorModifed = euler_matrix(addTransform[3], addTransform[4], addTransform[5])
link_R_sensorOriginal = quaternion_matrix(quat)
offset = numpy.add(offset, numpy.matmul(link_R_sensorOriginal[0:3,0:3], addTransform[0:3]))
quat = quaternion_from_matrix(numpy.matmul(link_R_sensorOriginal, sensorOriginal_R_sensorModifed))
fid = useradded_frame_name + "CS1"
extraframe = {'parentlink': parent_link, 'framename': useradded_frame_name}
self.extraframes = self.extraframes + [extraframe]
Expand Down

0 comments on commit 386e2c9

Please sign in to comment.