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 removeDummyLinksAndStaticBaseFlag(linkList, jointList, toolSlot):
"""Remove the dummy links (links without masses) and return true in case a dummy link should
set the base of the robot as static. """
staticBase = False
linkIndex = 0
childList = []
for joint in jointList:
childList.append(joint.child)
while linkIndex < len(linkList):
link = linkList[linkIndex]
# We want to skip links between the robot root and the static environment.
if isRootLink(link.name, childList):
linkIndex += 1
continue
# This link will not have a 'physics' field and is not used to have a toolSlot or a static base -> remove it
if link.inertia.mass is None and not link.collision and link.name != toolSlot:
parentJointIndex = None
childJointIndex = None
index = -1
for joint in jointList:
index += 1
if joint.parent == link.name:
childJointIndex = index
elif joint.child == link.name:
parentJointIndex = index
if parentJointIndex is not None:
if childJointIndex is not None:
jointList[parentJointIndex].child = jointList[
childJointIndex].child
jointList[parentJointIndex].position = combineTranslations(
jointList[parentJointIndex].position,
rotateVector(jointList[childJointIndex].position,
jointList[parentJointIndex].rotation))
jointList[parentJointIndex].rotation = combineRotations(
jointList[childJointIndex].rotation,
jointList[parentJointIndex].rotation)
jointList[parentJointIndex].name = jointList[
parentJointIndex].parent + "-" + jointList[
parentJointIndex].child
jointList.remove(jointList[childJointIndex])
else:
# Special case for dummy non-root links used to fix the base of the robot
parentLink = jointList[parentJointIndex].parent
if isRootLink(parentLink, childList):
# Ensure the parent link does not have physics, if it does, it should be kept as-is
# since some sensors require the parent to have physics
for l in linkList:
if l.name == parentLink and l.inertia.mass is None:
staticBase = True
jointList.remove(jointList[parentJointIndex])
# This link can be removed
linkList.remove(link)
else:
linkIndex += 1
childList.clear()
return staticBase