def writeLinkPhysics(robotFile, link, level):
"""Write a Webots Physics node."""
indent = ' '
robotFile.write((level + 1) * indent + 'physics Physics {\n')
robotFile.write((level + 2) * indent + 'density -1\n')
robotFile.write((level + 2) * indent + 'mass %lf\n' % link.inertia.mass)
robotFile.write((level + 2) * indent + 'centerOfMass [ %lf %lf %lf ]\n' %
(link.inertia.position[0], link.inertia.position[1],
link.inertia.position[2]))
if link.inertia.ixx > 0.0 and link.inertia.iyy > 0.0 and link.inertia.izz > 0.0:
i = link.inertia
inertiaMatrix = [
i.ixx, i.ixy, i.ixz, i.ixy, i.iyy, i.iyz, i.ixz, i.iyz, i.izz
]
if link.inertia.rotation[-1] != 0.0:
rotationMatrix = matrixFromRotation(link.inertia.rotation)
I_mat = np.array(inertiaMatrix).reshape(3, 3)
R = np.array(rotationMatrix).reshape(3, 3)
R_t = np.transpose(R)
# calculate the rotated inertiaMatrix with R_t * I * R. For reference, check the link below
# https://www.euclideanspace.com/physics/dynamics/inertia/rotation/index.htm
inertiaMatrix = np.dot(np.dot(R_t, I_mat), R).reshape(9)
if (inertiaMatrix[0] != 1.0 or inertiaMatrix[4] != 1.0
or inertiaMatrix[8] != 1.0 or inertiaMatrix[1] != 0.0
or inertiaMatrix[2] != 0.0 or inertiaMatrix[5] != 0.0):
robotFile.write((level + 2) * indent + 'inertiaMatrix [\n')
# principals moments of inertia (diagonal)
robotFile.write(
(level + 3) * indent + '%e %e %e\n' %
(inertiaMatrix[0], inertiaMatrix[4], inertiaMatrix[8]))
# products of inertia
robotFile.write(
(level + 3) * indent + '%e %e %e\n' %
(inertiaMatrix[1], inertiaMatrix[2], inertiaMatrix[5]))
robotFile.write((level + 2) * indent + ']\n')
robotFile.write((level + 1) * indent + '}\n')
def URDFJoint(proto, joint, level, parentList, childList, linkList, jointList,
sensorList, boxCollision, normal):
"""Write a Joint iteratively."""
indent = ' '
if not joint.axis:
joint.axis = [1, 0, 0]
axis = joint.axis
endpointRotation = joint.rotation
endpointPosition = joint.position
if joint.rotation[3] != 0.0 and axis:
axis = rotateVector(axis, joint.rotation)
if joint.type == 'revolute' or joint.type == 'continuous':
proto.write(level * indent + 'HingeJoint {\n')
proto.write((level + 1) * indent + 'jointParameters HingeJointParameters {\n')
if joint.limit.lower > 0.0:
# if 0 is not in the range, set the position to be the middle of the range
position = joint.limit.lower
if joint.limit.upper >= joint.limit.lower:
position = (joint.limit.upper - joint.limit.lower) / 2.0 + joint.limit.lower
proto.write((level + 2) * indent + 'position %lf \n' % position)
mat1 = matrixFromRotation(endpointRotation)
mat2 = matrixFromRotation([axis[0], axis[1], axis[2], position])
mat3 = multiplyMatrix(mat2, mat1)
endpointRotation = rotationFromMatrix(mat3)
proto.write((level + 2) * indent + 'axis %lf %lf %lf\n' % (axis[0], axis[1], axis[2]))
proto.write((level + 2) * indent + 'anchor %lf %lf %lf\n' % (joint.position[0], joint.position[1], joint.position[2]))
proto.write((level + 2) * indent + 'dampingConstant ' + str(joint.dynamics.damping) + '\n')
proto.write((level + 2) * indent + 'staticFriction ' + str(joint.dynamics.friction) + '\n')
proto.write((level + 1) * indent + '}\n')
proto.write((level + 1) * indent + 'device [\n')
proto.write((level + 2) * indent + 'RotationalMotor {\n')
elif joint.type == 'prismatic':
proto.write(level * indent + 'SliderJoint {\n')
proto.write((level + 1) * indent + 'jointParameters JointParameters {\n')
if joint.limit.lower > 0.0:
# if 0 is not in the range, set the position to be the middle of the range
position = joint.limit.lower
if joint.limit.upper >= joint.limit.lower:
position = (joint.limit.upper - joint.limit.lower) / 2.0 + joint.limit.lower
proto.write((level + 2) * indent + 'position %lf \n' % position)
length = math.sqrt(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2])
if length > 0:
endpointPosition[0] += axis[0] / length * position
endpointPosition[0] += axis[1] / length * position
endpointPosition[0] += axis[2] / length * position
proto.write((level + 2) * indent + 'axis %lf %lf %lf\n' % (axis[0], axis[1], axis[2]))
proto.write((level + 2) * indent + 'dampingConstant ' + str(joint.dynamics.damping) + '\n')
proto.write((level + 2) * indent + 'staticFriction ' + str(joint.dynamics.friction) + '\n')
proto.write((level + 1) * indent + '}\n')
proto.write((level + 1) * indent + 'device [\n')
proto.write((level + 2) * indent + 'LinearMotor {\n')
elif joint.type == 'fixed':
for childLink in linkList:
if childLink.name == joint.child:
URDFLink(proto, childLink, level, parentList, childList,
linkList, jointList, sensorList, joint.position, joint.rotation,
boxCollision, normal)
return
elif joint.type == 'floating' or joint.type == 'planar':
print(joint.type + ' is not a supported joint type in Webots')
return
proto.write((level + 3) * indent + 'name "' + joint.name + '"\n')
if joint.limit.velocity != 0.0:
proto.write((level + 3) * indent + 'maxVelocity ' + str(joint.limit.velocity) + '\n')
if joint.limit.lower != 0.0:
proto.write((level + 3) * indent + 'minPosition ' + str(joint.limit.lower) + '\n')
if joint.limit.upper != 0.0:
proto.write((level + 3) * indent + 'maxPosition ' + str(joint.limit.upper) + '\n')
if joint.limit.effort != 0.0:
if joint.type == 'prismatic':
proto.write((level + 3) * indent + 'maxForce ' + str(joint.limit.effort) + '\n')
else:
proto.write((level + 3) * indent + 'maxTorque ' + str(joint.limit.effort) + '\n')
proto.write((level + 2) * indent + '}\n')
proto.write((level + 2) * indent + 'PositionSensor {\n')
proto.write((level + 3) * indent + 'name "' + joint.name + '_sensor"\n')
proto.write((level + 2) * indent + '}\n')
proto.write((level + 1) * indent + ']\n')
proto.write((level + 1) * indent + 'endPoint')
found_link = False
for childLink in linkList:
if childLink.name == joint.child:
URDFLink(proto, childLink, level + 1, parentList, childList,
linkList, jointList, sensorList, endpointPosition, endpointRotation,
boxCollision, normal, endpoint=True)
assert(not found_link)
found_link = True
# case that non-existing link cited, set dummy flag
if not found_link and joint.child:
URDFLink(proto, joint.child, level + 1, parentList, childList,
linkList, jointList, sensorList, endpointPosition, endpointRotation,
boxCollision, normal, dummy=True)
print('warning: link ' + joint.child + ' is dummy!')
proto.write(level * indent + '}\n')
def URDFLink(proto, link, level, parentList, childList, linkList, jointList, sensorList,
jointPosition=[0.0, 0.0, 0.0], jointRotation=[1.0, 0.0, 0.0, 0.0],
boxCollision=False, normal=False, dummy=False, robot=False, endpoint=False):
"""Write a link iteratively."""
indent = ' '
haveChild = False
if robot:
proto.write(level * indent + 'Robot {\n')
proto.write((level + 1) * indent + 'translation IS translation\n')
proto.write((level + 1) * indent + 'rotation IS rotation\n')
proto.write((level + 1) * indent + 'controller IS controller\n')
proto.write((level + 1) * indent + 'controllerArgs IS controllerArgs\n')
proto.write((level + 1) * indent + 'customData IS customData\n')
proto.write((level + 1) * indent + 'supervisor IS supervisor\n')
proto.write((level + 1) * indent + 'synchronization IS synchronization\n')
proto.write((level + 1) * indent + 'selfCollision IS selfCollision\n')
else:
if link.forceSensor:
proto.write((' ' if endpoint else level * indent) + 'TouchSensor {\n')
proto.write((level + 1) * indent + 'type "force-3d"\n')
else:
proto.write((' ' if endpoint else level * indent) + 'Solid {\n')
proto.write((level + 1) * indent + 'translation %lf %lf %lf\n' % (jointPosition[0],
jointPosition[1],
jointPosition[2]))
proto.write((level + 1) * indent + 'rotation %lf %lf %lf %lf\n' % (jointRotation[0],
jointRotation[1],
jointRotation[2],
jointRotation[3]))
if not dummy: # dummy: case when link not defined but referenced (e.g. Atlas robot)
# 1: export Shapes
if link.visual:
if not haveChild:
haveChild = True
proto.write((level + 1) * indent + 'children [\n')
URDFShape(proto, link, level + 2, normal)
# 2: export Sensors
for sensor in sensorList:
if sensor.parentLink == link.name:
if not haveChild:
haveChild = True
proto.write((level + 1) * indent + 'children [\n')
sensor.export(proto, level + 2)
# 3: export Joints
for joint in jointList:
if joint.parent == link.name:
if not haveChild:
haveChild = True
proto.write((level + 1) * indent + 'children [\n')
URDFJoint(proto, joint, level + 2, parentList, childList,
linkList, jointList, sensorList, boxCollision, normal)
# 4: export ToolSlot if specified
if link.name == toolSlot:
if not haveChild:
proto.write((level + 1) * indent + 'children [\n')
proto.write((level + 2) * indent + 'Group {\n')
proto.write((level + 3) * indent + 'children IS toolSlot\n')
proto.write((level + 2) * indent + '}\n')
proto.write((level + 1) * indent + ']\n')
# add dummy physics and bounding object, so tools don't fall off
if link.inertia.mass is None:
proto.write((level + 1) * indent + 'physics Physics {\n')
proto.write((level + 1) * indent + '}\n')
proto.write((level + 1) * indent + 'boundingObject Box {\n')
proto.write((level + 2) * indent + 'size 0.01 0.01 0.01\n')
proto.write((level + 1) * indent + '}\n')
elif haveChild:
proto.write((level + 1) * indent + ']\n')
if level == 1:
proto.write((level + 1) * indent + 'name IS name \n')
else:
proto.write((level + 1) * indent + 'name "' + link.name + '"\n')
if link.collision:
URDFBoundingObject(proto, link, level + 1, boxCollision)
if link.inertia.mass is not None:
if level == 1 and staticBase:
proto.write((level + 1) * indent + '%{ if fields.staticBase.value == false then }%\n')
proto.write((level + 1) * indent + 'physics Physics {\n')
proto.write((level + 2) * indent + 'density -1\n')
proto.write((level + 2) * indent + 'mass %lf\n' % link.inertia.mass)
if link.inertia.position != [0.0, 0.0, 0.0]:
proto.write((level + 2) * indent + 'centerOfMass [ %lf %lf %lf ]\n' % (link.inertia.position[0],
link.inertia.position[1],
link.inertia.position[2]))
if link.inertia.ixx > 0.0 and link.inertia.iyy > 0.0 and link.inertia.izz > 0.0:
i = link.inertia
inertiaMatrix = [i.ixx, i.ixy, i.ixz, i.ixy, i.iyy, i.iyz, i.ixz, i.iyz, i.izz]
if link.inertia.rotation[-1] != 0.0:
rotationMatrix = matrixFromRotation(link.inertia.rotation)
I_mat = np.array(inertiaMatrix).reshape(3, 3)
R = np.array(rotationMatrix).reshape(3, 3)
R_t = np.transpose(R)
# calculate the rotated inertiaMatrix with R_t * I * R. For reference, check the link below
# https://www.euclideanspace.com/physics/dynamics/inertia/rotation/index.htm
inertiaMatrix = np.dot(np.dot(R_t, I_mat), R).reshape(9)
if (inertiaMatrix[0] != 1.0 or inertiaMatrix[4] != 1.0 or inertiaMatrix[8] != 1.0 or
inertiaMatrix[1] != 0.0 or inertiaMatrix[2] != 0.0 or inertiaMatrix[5] != 0.0):
proto.write((level + 2) * indent + 'inertiaMatrix [\n')
# principals moments of inertia (diagonal)
proto.write((level + 3) * indent + '%e %e %e\n' % (inertiaMatrix[0], inertiaMatrix[4], inertiaMatrix[8]))
# products of inertia
proto.write((level + 3) * indent + '%e %e %e\n' % (inertiaMatrix[1], inertiaMatrix[2], inertiaMatrix[5]))
proto.write((level + 2) * indent + ']\n')
proto.write((level + 1) * indent + '}\n')
if level == 1 and staticBase:
proto.write((level + 1) * indent + '%{ end }%\n')
elif link.collision:
if level == 1 and staticBase:
proto.write((level + 1) * indent + '%{ if fields.staticBase.value == false then }%\n')
proto.write((level + 1) * indent + 'physics Physics {\n')
proto.write((level + 1) * indent + '}\n')
if level == 1 and staticBase:
proto.write((level + 1) * indent + '%{ end }%\n')
proto.write(level * indent + '}\n')