def testBounding(self):
b = FreeCAD.BoundBox()
b.setVoid()
self.failUnless(not b.isValid(), "Bbox is not invalid")
b.add(0, 0, 0)
self.failUnless(b.isValid(), "Bbox is invalid")
self.failUnless(b.XLength == 0, "X length > 0")
self.failUnless(b.YLength == 0, "Y length > 0")
self.failUnless(b.ZLength == 0, "Z length > 0")
self.failUnless(b.Center == FreeCAD.Vector(0, 0, 0),
"Center is not at (0,0,0)")
self.failUnless(b.isInside(b.Center), "Center is not inside Bbox")
b.add(2, 2, 2)
self.failUnless(
b.isInside(
b.getIntersectionPoint(b.Center, FreeCAD.Vector(0, 1, 0))),
"Intersection point is not inside Bbox")
self.failUnless(b.intersect(b), "Bbox doesn't intersect with itself")
self.failUnless(
not b.intersected(FreeCAD.BoundBox(4, 4, 4, 6, 6, 6)).isValid(),
"Bbox should not intersect with Bbox outside")
self.failUnless(
b.intersected(FreeCAD.BoundBox(-2, -2, -2, 2, 2,
2)).Center == b.Center,
"Bbox is not a full subset")
def get_ground_bound_box(
sites: Iterable["ContainerFeature"]) -> FreeCAD.BoundBox:
boundbox = FreeCAD.BoundBox()
for site in sites:
boundbox.add(site.Shape.BoundBox)
return boundbox if boundbox.isValid() else FreeCAD.BoundBox(
0, 0, -30000, 0, 0, 0)
def boundBox(self):
x = self['status.config.axis.0.limit']
y = self['status.config.axis.1.limit']
z = self['status.config.axis.2.limit']
if x is None or y is None or z is None:
return FreeCAD.BoundBox()
return FreeCAD.BoundBox(x.min, y.min, z.min, x.max, y.max, z.max)
def boundBox(self):
'''Return a BoundBox as defined by MK's x, y and z axes limits.'''
x = self['status.config.axis.0.limit']
y = self['status.config.axis.1.limit']
z = self['status.config.axis.2.limit']
if x is None or y is None or z is None:
return FreeCAD.BoundBox()
return FreeCAD.BoundBox(x.min, y.min, z.min, x.max, y.max, z.max)
def writeGroundPlane(self,obj):
result = ""
bbox = FreeCAD.BoundBox()
for view in obj.Group:
if view.Source and hasattr(view.Source,"Shape") and hasattr(view.Source.Shape,"BoundBox"):
bbox.add(view.Source.Shape.BoundBox)
if bbox.isValid():
import Part
margin = bbox.DiagonalLength/2
p1 = FreeCAD.Vector(bbox.XMin-margin,bbox.YMin-margin,0)
p2 = FreeCAD.Vector(bbox.XMax+margin,bbox.YMin-margin,0)
p3 = FreeCAD.Vector(bbox.XMax+margin,bbox.YMax+margin,0)
p4 = FreeCAD.Vector(bbox.XMin-margin,bbox.YMax+margin,0)
# create temporary object. We do this to keep the renderers code as simple as possible:
# they only need to deal with one type of object: RenderView objects
dummy1 = FreeCAD.ActiveDocument.addObject("Part::Feature","renderdummy1")
dummy1.Shape = Part.Face(Part.makePolygon([p1,p2,p3,p4,p1]))
dummy2 = FreeCAD.ActiveDocument.addObject("App::FeaturePython","renderdummy2")
View(dummy2)
dummy2.Source = dummy1
ViewProviderView(dummy2.ViewObject)
FreeCAD.ActiveDocument.recompute()
result = self.writeObject(obj,dummy2)
# remove temp objects
FreeCAD.ActiveDocument.removeObject(dummy2.Name)
FreeCAD.ActiveDocument.removeObject(dummy1.Name)
FreeCAD.ActiveDocument.recompute()
return result
def getBounds(objs):
box = FreeCAD.BoundBox()
if len(objs) == 0:
return None
#copy across the values
b = objs[0].Shape.BoundBox
box.XMax = b.XMax
box.YMax = b.YMax
box.ZMax = b.ZMax
box.XMin = b.XMin
box.YMin = b.YMin
box.ZMin = b.ZMin
for obj in objs[1:]:
b = obj.Shape.BoundBox
#x axis comparison
box.XMin = min(box.XMin,b.XMin)
box.XMax = max(box.XMax,b.XMax)
#y axis comparison
box.YMin = min(box.YMin,b.YMin)
box.YMax = max(box.YMax,b.YMax)
#z axis comparison
box.ZMin = min(box.ZMin,b.ZMin)
box.ZMax = max(box.ZMax,b.ZMax)
return box
def getBB(self):
bb = FreeCAD.BoundBox()
if self.obj:
for o in Draft.getGroupContents(self.obj.Objects):
if hasattr(o, "Shape") and hasattr(o.Shape, "BoundBox"):
bb.add(o.Shape.BoundBox)
return bb
def makeSectionPlane(objectslist=None, name="Section"):
"""makeSectionPlane([objectslist]) : Creates a Section plane objects including the
given objects. If no object is given, the whole document will be considered."""
if not FreeCAD.ActiveDocument:
FreeCAD.Console.PrintError("No active document. Aborting\n")
return
obj = FreeCAD.ActiveDocument.addObject("App::FeaturePython", name)
obj.Label = translate("Arch", name)
_SectionPlane(obj)
if FreeCAD.GuiUp:
_ViewProviderSectionPlane(obj.ViewObject)
if objectslist:
obj.Objects = objectslist
bb = FreeCAD.BoundBox()
for o in Draft.getGroupContents(objectslist):
if hasattr(o, "Shape") and hasattr(o.Shape, "BoundBox"):
bb.add(o.Shape.BoundBox)
obj.Placement = FreeCAD.DraftWorkingPlane.getPlacement()
obj.Placement.Base = bb.Center
if FreeCAD.GuiUp:
margin = bb.XLength * 0.1
obj.ViewObject.DisplayLength = bb.XLength + margin
obj.ViewObject.DisplayHeight = bb.YLength + margin
return obj
def processWorldVol(first) :
# Check If Pure GDML workbench
# return worldVol xml element
global setup, structure
#print(dir(first)
if hasattr(first,'OutList') :
objList = first.OutList
print("Length of List : "+str(len(objList)))
if first.TypeId == "App::Part" :
print("Found App Part")
name = first.Name
# Also allow for App::Origin
print(objList[1].TypeId)
if isinstance(objList[1].Proxy,GDMLcommon) :
#if isinstance(objList[1].Proxy,GDMLBox) or \
# isinstance(objList[1].Proxy,GDMLTube) :
print("Adjust Setup")
ET.SubElement(setup,'world', {'ref':name})
return(structure)
# drop through if not handled
print("Need to create World Volume")
bbox = FreeCAD.BoundBox()
name = defineWorldBox(bbox)
ET.SubElement(setup,'world', {'ref':name})
worldVol = ET.SubElement(structure,'volume',{'name': name})
ET.SubElement(worldVol, 'solidref',{'ref': name})
print("Need to FIX !!!! To use defined gas")
ET.SubElement('worldVOL', 'materialref',{'ref': 'G4_Galactic'})
def export(exportList, filename):
"called when FreeCAD exports a file"
# process Objects
print("\nStart GDML Export 0.1")
GDMLstructure()
defineMaterials()
constructWorld()
bbox = FreeCAD.BoundBox()
defineWorldBox(exportList, bbox)
for obj in exportList:
reportObject(obj)
processObject(obj, True)
# Now append World Volume definition to stucture
# as it will contain references to volumes that need defining
# before it
structure.append(worldVOL)
#ET.ElementTree(gdml).write("test9e", 'utf-8', True)
# format & write GDML file
indent(gdml)
print("Write to GDML file")
ET.ElementTree(gdml).write(filename, 'utf-8', True)
print("GDML file written")
def boundbox_from_intersect(curves, pos, normal, doScaleXYZ):
if len(curves) == 0:
return None
plane = Part.Plane(pos, normal)
xmin = float("inf")
xmax = float("-inf")
ymin = float("inf")
ymax = float("-inf")
zmin = float("inf")
zmax = float("-inf")
found = False
for n in range(0, len(curves)):
curve = curves[n]
ipoints = []
for edge in curve.Shape.Edges:
i = plane.intersect(edge.Curve)
if i:
for p in i[0]:
vert = Part.Vertex(p)
if vert.distToShape(edge)[0] < epsilon:
ipoints.append(p)
found = True
if found == False:
return None
use_x = True
use_y = True
use_z = True
if len(ipoints) > 1:
use_x = doScaleXYZ[n][0]
use_y = doScaleXYZ[n][1]
use_z = doScaleXYZ[n][2]
for p in ipoints:
if use_x and p.X > xmax: xmax = p.X
if use_x and p.X < xmin: xmin = p.X
if use_y and p.Y > ymax: ymax = p.Y
if use_y and p.Y < ymin: ymin = p.Y
if use_z and p.Z > zmax: zmax = p.Z
if use_z and p.Z < zmin: zmin = p.Z
if xmin == float("inf") or xmax == float("-inf"):
xmin = 0
xmax = 0
if ymin == float("inf") or ymax == float("-inf"):
ymin = 0
ymax = 0
if zmin == float("inf") or zmax == float("-inf"):
zmin = 0
zmax = 0
return FreeCAD.BoundBox(xmin, ymin, zmin, xmax, ymax, zmax)
def getBoundBox(self):
bb = FreeCAD.BoundBox()
obj = self.Object
objectsToProcess = filterObjects(obj.IncludeObjects,
obj.ExcludeObjects)
for o in objectsToProcess:
if hasattr(o, "Shape") and hasattr(o.Shape, "BoundBox"):
bb.add(o.Shape.BoundBox)
return bb
def get_assembly_bounding_box(objects):
"""
Calculates overall bounding box given a set of components.
[Xmin,Ymin,Zmin,Xmax,Ymax,Zmax]
"""
bb = FreeCAD.BoundBox()
for object in objects:
if object.isDerivedFrom("Mesh::Feature"):
bb.add(object.Mesh.BoundBox)
else:
bb.add(object.Shape.BoundBox)
return bb
def GetObjectBoundBox(obj): if not (hasattr(obj,'Shape')) and obj.TypeId=='App::Part': bb=FreeCAD.BoundBox() for subobj in obj.OutList: if subobj.TypeId=='App::Part': bb2=GetObjectBoundBox(subobj) else: if subobj.TypeId!='App::Origin': bb2=subobj.Shape.BoundBox else: bb2=FreeCAD.BoundBox() bb.XMax=getMax(bb.XMax,bb2.XMax) bb.YMax=getMax(bb.YMax,bb2.YMax) bb.ZMax=getMax(bb.ZMax,bb2.ZMax) bb.XMin=getMin(bb.XMin,bb2.XMin) bb.YMin=getMin(bb.YMin,bb2.YMin) bb.ZMin=getMin(bb.ZMin,bb2.ZMin) p=[] p.append(obj.Placement.multiply(FreeCAD.Placement(FreeCAD.Vector(bb.XMin,bb.YMin,bb.ZMin),obj.Placement.Rotation))) p.append(obj.Placement.multiply(FreeCAD.Placement(FreeCAD.Vector(bb.XMax,bb.YMin,bb.ZMin),obj.Placement.Rotation))) p.append(obj.Placement.multiply(FreeCAD.Placement(FreeCAD.Vector(bb.XMax,bb.YMax,bb.ZMin),obj.Placement.Rotation))) p.append(obj.Placement.multiply(FreeCAD.Placement(FreeCAD.Vector(bb.XMin,bb.YMax,bb.ZMin),obj.Placement.Rotation))) p.append(obj.Placement.multiply(FreeCAD.Placement(FreeCAD.Vector(bb.XMin,bb.YMin,bb.ZMax),obj.Placement.Rotation))) p.append(obj.Placement.multiply(FreeCAD.Placement(FreeCAD.Vector(bb.XMax,bb.YMin,bb.ZMax),obj.Placement.Rotation))) p.append(obj.Placement.multiply(FreeCAD.Placement(FreeCAD.Vector(bb.XMax,bb.YMax,bb.ZMax),obj.Placement.Rotation))) p.append(obj.Placement.multiply(FreeCAD.Placement(FreeCAD.Vector(bb.XMin,bb.YMax,bb.ZMax),obj.Placement.Rotation))) bb=FreeCAD.BoundBox() for pp in p: bb.XMax=getMax(bb.XMax,pp.Base.x) bb.YMax=getMax(bb.YMax,pp.Base.y) bb.ZMax=getMax(bb.ZMax,pp.Base.z) bb.XMin=getMin(bb.XMin,pp.Base.x) bb.YMin=getMin(bb.YMin,pp.Base.y) bb.ZMin=getMin(bb.ZMin,pp.Base.z) return bb else: return obj.Shape.BoundBox
def calculateBoundBox(self):
xValues = [vertex['vector'][0] for vertex in self.vertices]
yValues = [vertex['vector'][1] for vertex in self.vertices]
zValues = [vertex['vector'][2] for vertex in self.vertices]
xMin = min(xValues)
yMin = min(yValues)
zMin = min(zValues)
xMax = max(xValues)
yMax = max(yValues)
zMax = max(zValues)
return FreeCAD.BoundBox(xMin, yMin, zMin, xMax, yMax, zMax)
def write_groundplane(self, renderer):
"""Generate a ground plane rendering string for the scene
For that purpose, dummy objects are temporarily added to the document
which the project belongs to, and eventually deleted
Parameters
----------
renderer: the renderer handler
Returns
-------
The rendering string for the ground plane
"""
result = ""
doc = self.fpo.Document
bbox = App.BoundBox()
for view in self.fpo.Group:
try:
bbox.add(view.Source.Shape.BoundBox)
except AttributeError:
pass
if bbox.isValid():
# Create temporary object. We do this to keep renderers codes as
# simple as possible: they only need to deal with one type of
# object: RenderView objects
margin = bbox.DiagonalLength / 2
vertices = [
App.Vector(bbox.XMin - margin, bbox.YMin - margin, 0),
App.Vector(bbox.XMax + margin, bbox.YMin - margin, 0),
App.Vector(bbox.XMax + margin, bbox.YMax + margin, 0),
App.Vector(bbox.XMin - margin, bbox.YMax + margin, 0)
]
vertices.append(vertices[0]) # Close the polyline...
dummy1 = doc.addObject("Part::Feature", "dummygroundplane1")
dummy1.Shape = Part.Face(Part.makePolygon(vertices))
dummy2 = doc.addObject("App::FeaturePython", "dummygroundplane2")
View(dummy2)
dummy2.Source = dummy1
ViewProviderView(dummy2.ViewObject)
doc.recompute()
result = renderer.get_rendering_string(dummy2)
# Remove temp objects
doc.removeObject(dummy2.Name)
doc.removeObject(dummy1.Name)
doc.recompute()
return result
def __setstate__(self, dictForJSON):
if dictForJSON:
#FreeCAD.Console.PrintMessage("Load\n"+str(dictForJSON)+"\n") #debug
self.oldDelta = dictForJSON['oldD']
bboxcoord = dictForJSON['bbox']
self.bbox = FreeCAD.BoundBox(bboxcoord[0], bboxcoord[1],
bboxcoord[2], bboxcoord[3],
bboxcoord[4], bboxcoord[5])
voxelspacedim = dictForJSON['vsDim']
self.voxelSpace = np.full(voxelspacedim, 0, np.int16)
voxelSpaceConds = (np.array(dictForJSON['vsX']),
np.array(dictForJSON['vsY']),
np.array(dictForJSON['vsZ']))
self.voxelSpace[voxelSpaceConds] = dictForJSON['vsVals']
self.Type = dictForJSON['type']
self.justLoaded = True
def __init__(self, hierarchy, info):
self.objs = [h.Assembly for h in reversed(hierarchy)]
self.assembly = resolveAssembly(info.Parent)
self.viewObject = self.assembly.Object.ViewObject
self.info = info
self.undos = None
fixed = Constraint.getFixedTransform(self.assembly.getConstraints())
fixed = fixed.get(info.Part, None)
self.fixedTransform = fixed
if fixed and fixed.Shape:
shape = fixed.Shape
else:
shape = info.Shape
rot = utils.getElementRotation(shape)
if not rot:
# in case the shape has no normal, like a vertex, just use an empty
# rotation, which means having the same rotation as the owner part.
rot = FreeCAD.Rotation()
hasBound = True
if not utils.isVertex(shape):
self.bbox = shape.BoundBox
else:
bbox = info.Object.ViewObject.getBoundingBox()
if bbox.isValid():
self.bbox = bbox
else:
logger.warn('empty bounding box of part {}'.format(
info.PartName))
self.bbox = FreeCAD.BoundBox(0, 0, 0, 5, 5, 5)
hasBound = False
pos = utils.getElementPos(shape)
if not pos:
if hasBound:
pos = self.bbox.Center
else:
pos = shape.Placement.Base
pla = FreeCAD.Placement(pos, rot)
self.offset = pla.copy()
self.offsetInv = pla.inverse()
self.draggerPlacement = info.Placement.multiply(pla)
self.trace = None
self.tracePoint = None
def CalculateBoundary(self):
Results = []
min = self.selectedObj[0].Object.Shape.BoundBox.XMin
for i in range(1, len(self.selectedObj)):
if self.selectedObj[i].Object.Shape.BoundBox.XMin < min :
min = self.selectedObj[i].Object.Shape.BoundBox.XMin
Results.append(min)
min = self.selectedObj[0].Object.Shape.BoundBox.YMin
for i in range(1, len(self.selectedObj)):
if self.selectedObj[i].Object.Shape.BoundBox.YMin < min :
min = self.selectedObj[i].Object.Shape.BoundBox.YMin
Results.append(min)
min = self.selectedObj[0].Object.Shape.BoundBox.ZMin
for i in range(1, len(self.selectedObj)):
if self.selectedObj[i].Object.Shape.BoundBox.ZMin < min :
min = self.selectedObj[i].Object.Shape.BoundBox.ZMin
Results.append(min)
max = self.selectedObj[0].Object.Shape.BoundBox.XMax
for i in range(1, len(self.selectedObj)):
if self.selectedObj[i].Object.Shape.BoundBox.XMax > max :
max = self.selectedObj[i].Object.Shape.BoundBox.XMax
Results.append(max)
max = self.selectedObj[0].Object.Shape.BoundBox.YMax
for i in range(1, len(self.selectedObj)):
if self.selectedObj[i].Object.Shape.BoundBox.YMax > max :
max = self.selectedObj[i].Object.Shape.BoundBox.YMax
Results.append(max)
max = self.selectedObj[0].Object.Shape.BoundBox.ZMax
for i in range(1, len(self.selectedObj)):
if self.selectedObj[i].Object.Shape.BoundBox.ZMax > max :
max = self.selectedObj[i].Object.Shape.BoundBox.ZMax
Results.append(max)
XMin = Results[0]
YMin = Results[1]
ZMin = Results[2]
XMax = Results[3]
YMax = Results[4]
ZMax = Results[5]
# App.BoundBox([Xmin,Ymin,Zmin,Xmax,Ymax,Zmax])
if self.NewBoundary is not None:
try:
del self.NewBoundary
except:
pass
self.NewBoundary = App.BoundBox(XMin, YMin, ZMin, XMax, YMax, ZMax)
def get_bounding_box(self):
"""Compute project bounding box.
This the bounding box of the underlying objects referenced in the
scene.
"""
bbox = App.BoundBox()
for view in self.all_views():
source = view.Source
for attr_name in ("Shape", "Mesh"):
try:
attr = getattr(source, attr_name)
except AttributeError:
pass
else:
bbox.add(attr.BoundBox)
break
return bbox
def computeContainingBBox(self):
''' Get the bounding box containing all the VHConductors in the document
Returns the global bounding box.
If there are no VHConductors, or if the VHConductors Base objects have no Shape,
the returned BoundBox is invalid (.isValid() on the returned BoundBox gives False)
'''
# create an empty bbox
gbbox = FreeCAD.BoundBox()
# get the document containing this object
doc = self.Object.Document
if doc is None:
FreeCAD.Console.PrintWarning(
translate(
"EM",
"No active document available. Cannot compute containing BBox."
))
else:
# get all the VHConductors
conds = [
obj for obj in doc.Objects
if Draft.getType(obj) == "VHConductor"
]
for obj in conds:
gbbox.add(obj.Proxy.getBBox())
# if the the old bbox or the newly computed bbox is invalid, flag the voxel space as invalid
if (not gbbox.isValid()) or (not self.bbox.isValid()):
self.Object.voxelSpaceValid = False
else:
# if we just re-loaded the model, do not flag the bbox as invalid.
# the problem is that the Shape.BoundBox of the base objects of the VHConductors
# can be different if the object actually has a visible shape or not.
# At load time, if the object is invisible, its boundbox may be different.
# However, if we knew it was valid at save time, no reason to invalidate it
if not self.justLoaded:
if (not gbbox.isInside(
self.bbox)) and (not self.bbox.isInside(gbbox)):
self.Object.voxelSpaceValid = False
else:
self.justLoaded = False
self.bbox = gbbox
return gbbox
def autogroup(self,obj,child):
"Adds an object to the group of this BuildingPart automatically"
if obj.ViewObject:
if hasattr(obj.ViewObject.Proxy,"autobbox") and obj.ViewObject.Proxy.autobbox:
if hasattr(child,"Shape") and child.Shape:
abb = obj.ViewObject.Proxy.autobbox
cbb = child.Shape.BoundBox
if abb.isValid():
if not cbb.isValid():
FreeCAD.ActiveDocument.recompute()
if not cbb.isValid():
cbb = FreeCAD.BoundBox()
for v in child.Shape.Vertexes:
print(v.Point)
cbb.add(v.Point)
if cbb.isValid() and abb.isInside(cbb):
self.addObject(obj,child)
return True
return False
def shapeBoundBox(obj):
PathLog.track(type(obj))
if list == type(obj) and obj:
bb = FreeCAD.BoundBox()
for o in obj:
bb.add(shapeBoundBox(o))
return bb
if hasattr(obj, 'Shape'):
return obj.Shape.BoundBox
if obj and 'App::Part' == obj.TypeId:
bounds = [shapeBoundBox(o) for o in obj.Group]
if bounds:
bb = bounds[0]
for b in bounds[1:]:
bb = bb.united(b)
return bb
if obj:
PathLog.error(
translate('PathStock', "Invalid base object %s - no shape found") %
obj.Name)
return None
def get_bbox(obj, debug=False):
"""Return a BoundBox from any object that has a Coin RootNode.
Normally the bounding box of an object can be taken
from its `Part::TopoShape`.
::
>>> print(obj.Shape.BoundBox)
However, for objects without a `Shape`, such as those
derived from `App::FeaturePython` like `Draft Text` and `Draft Dimension`,
the bounding box can be calculated from the `RootNode` of the viewprovider.
Parameters
----------
obj: App::DocumentObject
Any object that has a `ViewObject.RootNode`.
Returns
-------
Base::BoundBox
It returns a `BoundBox` object which has information like
minimum and maximum values of X, Y, and Z, as well as bounding box
center.
None
If there is a problem it will return `None`.
"""
_name = "get_bbox"
utils.print_header(_name, "Bounding box", debug=debug)
found, doc = utils.find_doc(App.activeDocument())
if not found:
_err(_tr("No active document. Aborting."))
return None
if isinstance(obj, str):
obj_str = obj
found, obj = utils.find_object(obj, doc)
if not found:
_msg("obj: {}".format(obj_str))
_err(_tr("Wrong input: object not in document."))
return None
if debug:
_msg("obj: {}".format(obj.Label))
if (not hasattr(obj, "ViewObject")
or not obj.ViewObject
or not hasattr(obj.ViewObject, "RootNode")):
_err(_tr("Does not have 'ViewObject.RootNode'."))
# For Draft Dimensions
# node = obj.ViewObject.Proxy.node
node = obj.ViewObject.RootNode
view = Gui.ActiveDocument.ActiveView
region = view.getViewer().getSoRenderManager().getViewportRegion()
action = coin.SoGetBoundingBoxAction(region)
node.getBoundingBox(action)
bb = action.getBoundingBox()
# xlength, ylength, zlength = bb.getSize().getValue()
xmin, ymin, zmin = bb.getMin().getValue()
xmax, ymax, zmax = bb.getMax().getValue()
return App.BoundBox(xmin, ymin, zmin, xmax, ymax, zmax)
def edgelistBB(edgelist):
# get edge list bounding box
bb = FreeCAD.BoundBox(0, 0, 0, 0, 0, 0)
for e in edgelist:
bb.add(e.BoundBox)
return bb
def makeRibs(self, obj):
pl = obj.Placement
ribs = []
curvebox = FreeCAD.BoundBox(float("-inf"), float("-inf"),
float("-inf"), float("inf"), float("inf"),
float("inf"))
for n in range(0, len(obj.Hullcurves)):
cbbx = obj.Hullcurves[n].Shape.BoundBox
if self.doScaleXYZ[n][0]:
if cbbx.XMin > curvebox.XMin: curvebox.XMin = cbbx.XMin
if cbbx.XMax < curvebox.XMax: curvebox.XMax = cbbx.XMax
if self.doScaleXYZ[n][1]:
if cbbx.YMin > curvebox.YMin: curvebox.YMin = cbbx.YMin
if cbbx.YMax < curvebox.YMax: curvebox.YMax = cbbx.YMax
if self.doScaleXYZ[n][2]:
if cbbx.ZMin > curvebox.ZMin: curvebox.ZMin = cbbx.ZMin
if cbbx.ZMax < curvebox.ZMax: curvebox.ZMax = cbbx.ZMax
if len(obj.Hullcurves) > 0:
if curvebox.XMin == float("-inf"):
curvebox.XMin = obj.Hullcurves[0].Shape.BoundBox.XMin
if curvebox.XMax == float("inf"):
curvebox.XMax = obj.Hullcurves[0].Shape.BoundBox.XMax
if curvebox.YMin == float("-inf"):
curvebox.YMin = obj.Hullcurves[0].Shape.BoundBox.YMin
if curvebox.YMax == float("inf"):
curvebox.YMax = obj.Hullcurves[0].Shape.BoundBox.YMax
if curvebox.ZMin == float("-inf"):
curvebox.ZMin = obj.Hullcurves[0].Shape.BoundBox.ZMin
if curvebox.ZMax == float("inf"):
curvebox.ZMax = obj.Hullcurves[0].Shape.BoundBox.ZMax
maxlen = 0
edgelen = []
edges = Part.__sortEdges__(obj.Path.Shape.Edges)
for edge in edges:
maxlen += edge.Length
edgelen.append(edge.Length)
for n in range(0, int(obj.Items)):
plen = obj.OffsetStart
if obj.Items > 1:
plen += (maxlen - obj.OffsetStart -
obj.OffsetEnd) * n / (float(obj.Items) - 1)
for edge in edges:
if plen > edge.Length:
plen -= edge.Length
else:
param = edge.getParameterByLength(plen)
direction = edge.tangentAt(param)
posvec = edge.valueAt(param)
normal = CurvedShapes.getNormal(obj.Base)
rotaxis = normal.cross(direction)
angle = math.degrees(normal.getAngle(direction))
#dolly = self.makeRib(obj, posvec, direction)
dolly = obj.Base.Shape.copy()
if rotaxis.Length > epsilon:
dolly.rotate(dolly.BoundBox.Center, rotaxis, angle)
dolly.Placement.Base = posvec
if dolly:
if not obj.Twist == 0 and n > 0:
dolly.rotate(posvec, direction,
obj.Twist * n / int(obj.Items))
if len(obj.Hullcurves) > 0:
if not obj.ScaleX: direction = Vector(1, 0, 0)
if not obj.ScaleY: direction = Vector(0, 1, 0)
if not obj.ScaleZ: direction = Vector(0, 0, 1)
bbox = CurvedShapes.boundbox_from_intersect(
obj.Hullcurves, posvec, direction,
self.doScaleXYZ)
if bbox:
dolly = CurvedShapes.scaleByBoundbox(
dolly, bbox, self.doScaleXYZsum, copy=True)
ribs.append(dolly)
break
if (obj.Surface or obj.Solid) and obj.Items > 1:
obj.Shape = CurvedShapes.makeSurfaceSolid(ribs, obj.Solid)
else:
obj.Shape = Part.makeCompound(ribs)
obj.Placement = pl
if self.extract:
CompoundTools.Explode.explodeCompound(obj)
obj.ViewObject.hide()
def makeRibs(self, obj, update):
# obj.LeadingEdge, obj.traillingedge, obj.Path, obj.Hullcurves=[obj.leadingedge,obj.traillingedge]
ribs = []
if update == True:
FreeCAD.Console.PrintMessage(
"MakeRibs update ------------------------------------\n")
#for i in range(0,len(obj.Ribs)) :
# direction=FreeCAD.Vector([0,0,0])
# chord=obj.Ribs[i].Chord
### angle=obj.Ribs[i].Placement.Rotation.Angle
#self.updateRib(obj, obj.Ribs[i].Chord, obj.Ribs[i].Placement.Base,direction ,obj.Ribs[i].Placement.Rotation.Angle,i)
#self.updateRib(obj, chord, posvecrib,direction, rotaxis,angle,i)
ribs = obj.Ribs
else:
FreeCAD.Console.PrintMessage(
"MakeRibs create ------------------------------------\n")
FreeCAD.Console.PrintMessage(" Number of Hullcurves : " +
str(len(obj.Hullcurves)) + "\n")
#--------------CurvesPathArray code--------------------------
curvebox = FreeCAD.BoundBox(float("-inf"), float("-inf"),
float("-inf"), float("inf"),
float("inf"), float("inf"))
for n in range(0, len(obj.Hullcurves)):
cbbx = obj.Hullcurves[n].Shape.BoundBox
if self.doScaleXYZ[n][0]:
if cbbx.XMin > curvebox.XMin: curvebox.XMin = cbbx.XMin
if cbbx.XMax < curvebox.XMax: curvebox.XMax = cbbx.XMax
if self.doScaleXYZ[n][1]:
if cbbx.YMin > curvebox.YMin: curvebox.YMin = cbbx.YMin
if cbbx.YMax < curvebox.YMax: curvebox.YMax = cbbx.YMax
if self.doScaleXYZ[n][2]:
if cbbx.ZMin > curvebox.ZMin: curvebox.ZMin = cbbx.ZMin
if cbbx.ZMax < curvebox.ZMax: curvebox.ZMax = cbbx.ZMax
if len(obj.Hullcurves) > 0:
if curvebox.XMin == float("-inf"):
curvebox.XMin = obj.Hullcurves[0].Shape.BoundBox.XMin
if curvebox.XMax == float("inf"):
curvebox.XMax = obj.Hullcurves[0].Shape.BoundBox.XMax
if curvebox.YMin == float("-inf"):
curvebox.YMin = obj.Hullcurves[0].Shape.BoundBox.YMin
if curvebox.YMax == float("inf"):
curvebox.YMax = obj.Hullcurves[0].Shape.BoundBox.YMax
if curvebox.ZMin == float("-inf"):
curvebox.ZMin = obj.Hullcurves[0].Shape.BoundBox.ZMin
if curvebox.ZMax == float("inf"):
curvebox.ZMax = obj.Hullcurves[0].Shape.BoundBox.ZMax
edges = Part.__sortEdges__(obj.Path.Shape.Edges)
leadingedge_edges = Part.__sortEdges__(
obj.LeadingEdge.Shape.Edges
) #leadingedge_edges=Part.sortEdges(obj.LeadingEdge.Shape.Edges) # deprecated !
FreeCAD.Console.PrintMessage(" Len of edges : " +
str(len(edges)) + "\n")
FreeCAD.Console.PrintMessage(" Items : " +
str(int(obj.Items)) + "\n")
maxlen = obj.LeadingEdge.Shape.Length
for n in range(0, int(obj.Items)):
FreeCAD.Console.PrintMessage(
" Rib number : --- " + str(n) + " ---\n")
plen = obj.OffsetStart
if obj.Items > 1:
plen += (maxlen - obj.OffsetStart -
obj.OffsetEnd) * n / (float(obj.Items) - 1)
for edge in edges:
if plen > edge.Length:
plen = edge.Length
if plen > edge.Length:
print(" plen > edge.Length:")
#plen -= edge.Length
else:
param = edge.getParameterByLength(plen)
direction = edge.tangentAt(param) # path direction
posvec = edge.valueAt(param) #on path
posvec_path = edge.valueAt(param)
directionleadingedge = leadingedge_edges[0].tangentAt(
param) # leadinedge direction
param_leadingedge = leadingedge_edges[
0].getParameterByLength(plen)
posvec_leadingedge = leadingedge_edges[0].valueAt(
param_leadingedge)
normal = CurvedShapes.getNormal(
obj.Base) # get the rib normal
rotaxis = normal.cross(direction)
angle = math.degrees(normal.getAngle(direction))
planEdge = Part.makePlane(edge.Length, edge.Length,
FreeCAD.Vector(0, 0, 0),
direction).Faces[0]
FreeCAD.Console.PrintMessage(
" planEdge : " + str(planEdge) +
"\n")
uv = planEdge.Surface.parameter(posvec_path)
normaledge = planEdge.normalAt(uv[0], uv[1])
FreeCAD.Console.PrintMessage(
" normaledge : --- " +
str(normaledge) + " ---\n")
planLeadingedge = Part.makePlane(
leadingedge_edges[0].Length,
leadingedge_edges[0].Length,
FreeCAD.Vector(0, 0, 0), directionleadingedge)
FreeCAD.Console.PrintMessage(
" planLeadingedge : " +
str(planLeadingedge) + "\n")
uv = planLeadingedge.Surface.parameter(
posvec_leadingedge)
normaLeadingedge = planLeadingedge.normalAt(
uv[0], uv[1])
FreeCAD.Console.PrintMessage(
" normaLeadingedge c: --- " +
str(normaLeadingedge) + " ---\n")
#l=Part.Line(FreeCAD.Vector(posvec_path),normaledge)
##ll=Part.Line(FreeCAD.Vector(posvec_leadingedge),normaLeadingedge)
#point=l.intersect(normaLeadingedge)
#p=App.Vector(1,1,1)
#posvec_path.projectToPlane(App.Vector(0,0,0),App.Vector(1,0,1))
#point =normaLeadingedge.cross() #
point = FreeCAD.Vector(
posvec_leadingedge.x, posvec_leadingedge.y,
posvec_path.z
) #math.sqrt(posvec.x*posvec.x+posvec.y*posvec.y))#posvec_path.projectToPlane(planLeadingedge)
#point=planLeadingedge.projectPoint.(FreeCAD.Vector(posvec_path))
FreeCAD.Console.PrintMessage(
" Intercsection : --- " +
str(point) + " ---\n")
#normalleadingedge=leadingedge_edges[0].normalAt(param_leadingedge)
#A=DraftGeomUtils.findIntersection(normaledge,normalleadingedge)
#posvecrib=FreeCAD.Vector(posvec_leadingedge.x, posvec_leadingedge.y,posvec_path.z)
#posvecrib=FreeCAD.Vector(posvec_path.x, posvec_path.y,posvec_path.z)
posvecrib = FreeCAD.Vector(point.x, point.y, point.z)
if len(obj.Ribs) < obj.Items:
FreeCAD.Console.PrintMessage(
" Rib normal : " +
str(normal) + "\n")
FreeCAD.Console.PrintMessage(
" Path Plen : " + str(plen) +
"\n")
FreeCAD.Console.PrintMessage(
" Path Param : " +
str(param) + "\n")
FreeCAD.Console.PrintMessage(
" Path Position : " +
str(posvec) + "\n")
FreeCAD.Console.PrintMessage(
" Path Direction : " +
str(direction) + "\n")
FreeCAD.Console.PrintMessage(
" Path doScaleXYZ : " +
str(self.doScaleXYZ) + "\n")
FreeCAD.Console.PrintMessage(
" LeadingEdge doScaleXYZ : " +
str(self.doScaleXYZ) + "\n")
FreeCAD.Console.PrintMessage(
" Leading position : " +
str(posvec_leadingedge) + "\n")
FreeCAD.Console.PrintMessage(
" Path position : " +
str(posvec_path) + "\n")
FreeCAD.Console.PrintMessage(
" Rib Angle : " +
str(angle) + "\n")
FreeCAD.Console.PrintMessage(
" Rotaxis : " +
str(rotaxis) + "\n")
FreeCAD.Console.PrintMessage(
" Rotaxis.Length : " +
str(rotaxis.Length) + "\n")
#plane = Part.Plane(posvecrib, direction)
#Part.show(plane)
#bbox = CurvedShapes.boundbox_from_intersect(obj.Hullcurves, posvecrib, direction, self.doScaleXYZ,False)
bbox = CurvedShapes.boundbox_from_intersect(
obj.Hullcurves, posvec_leadingedge, direction,
self.doScaleXYZ, False)
if bbox:
x, y, z, scalex, scaley, scalez = scaleByBoundbox2(
obj.Base.Shape, bbox, self.doScaleXYZsum)
FreeCAD.Console.PrintMessage(" scalex" +
str(scalex) +
" \n")
FreeCAD.Console.PrintMessage(" scaley" +
str(scaley) +
" \n")
FreeCAD.Console.PrintMessage(" scalez" +
str(scalez) +
" \n")
chord = scalex
rib = self.createRib(
obj, chord, posvecrib, direction, rotaxis,
angle) # posvec_leadingedge
FreeCAD.Console.PrintMessage(
" Rib.Label : " +
str(rib.Label) + "\n")
if rib:
ribs.append(rib)
else:
FreeCAD.Console.PrintMessage(
" Error : not bbox ------------\n")
if len(ribs) > 1:
FreeCAD.Console.PrintMessage(" Number of rib created : " +
str(len(ribs)) + "\n")
self.wingpshape(obj)
if update == False:
obj.Ribs = ribs
print("------------End of makeRibs function------------")
def makeRibs(self, obj):
pl = obj.Placement
ribs = []
curvebox = FreeCAD.BoundBox(float("-inf"), float("-inf"),
float("-inf"), float("inf"), float("inf"),
float("inf"))
for n in range(0, len(obj.Hullcurves)):
cbbx = obj.Hullcurves[n].Shape.BoundBox
if self.doScaleXYZ[n][0]:
if cbbx.XMin > curvebox.XMin: curvebox.XMin = cbbx.XMin
if cbbx.XMax < curvebox.XMax: curvebox.XMax = cbbx.XMax
if self.doScaleXYZ[n][1]:
if cbbx.YMin > curvebox.YMin: curvebox.YMin = cbbx.YMin
if cbbx.YMax < curvebox.YMax: curvebox.YMax = cbbx.YMax
if self.doScaleXYZ[n][2]:
if cbbx.ZMin > curvebox.ZMin: curvebox.ZMin = cbbx.ZMin
if cbbx.ZMax < curvebox.ZMax: curvebox.ZMax = cbbx.ZMax
if curvebox.XMin == float("-inf"):
curvebox.XMin = obj.Hullcurves[0].Shape.BoundBox.XMin
if curvebox.XMax == float("inf"):
curvebox.XMax = obj.Hullcurves[0].Shape.BoundBox.XMax
if curvebox.YMin == float("-inf"):
curvebox.YMin = obj.Hullcurves[0].Shape.BoundBox.YMin
if curvebox.YMax == float("inf"):
curvebox.YMax = obj.Hullcurves[0].Shape.BoundBox.YMax
if curvebox.ZMin == float("-inf"):
curvebox.ZMin = obj.Hullcurves[0].Shape.BoundBox.ZMin
if curvebox.ZMax == float("inf"):
curvebox.ZMax = obj.Hullcurves[0].Shape.BoundBox.ZMax
areavec = Vector(curvebox.XLength, curvebox.YLength, curvebox.ZLength)
deltavec = areavec.scale(
obj.Axis.x, obj.Axis.y,
obj.Axis.z) - (obj.OffsetStart + obj.OffsetEnd) * obj.Axis
sections = int(obj.Items)
startvec = Vector(curvebox.XMin, curvebox.YMin, curvebox.ZMin)
if obj.Axis.x < 0: startvec.x = curvebox.XMax
if obj.Axis.y < 0: startvec.y = curvebox.YMax
if obj.Axis.z < 0: startvec.z = curvebox.ZMax
pos0 = startvec + (obj.OffsetStart * obj.Axis)
if (not hasattr(obj, "Positions") or len(obj.Positions) == 0):
for x in range(0, sections):
if sections > 1:
d = CurvedShapes.distribute(x / (sections - 1),
obj.Distribution,
obj.DistributionReverse)
posvec = pos0 + (deltavec * d)
else:
posvec = pos0
dolly = self.makeRib(obj, posvec)
if dolly:
if not obj.Twist == 0:
dolly.rotate(
dolly.BoundBox.Center, obj.Axis,
obj.Twist * posvec.Length / areavec.Length)
ribs.append(dolly)
else:
for p in obj.Positions:
posvec = pos0 + (deltavec * p)
dolly = self.makeRib(obj, posvec)
if dolly:
if not obj.Twist == 0:
dolly.rotate(
dolly.BoundBox.Center, obj.Axis,
obj.Twist * posvec.Length / areavec.Length)
ribs.append(dolly)
if (obj.Surface or obj.Solid) and obj.Items > 1:
obj.Shape = CurvedShapes.makeSurfaceSolid(ribs, obj.Solid)
else:
obj.Shape = Part.makeCompound(ribs)
obj.Placement = pl
if self.extract:
CompoundTools.Explode.explodeCompound(obj)
obj.ViewObject.hide()
def __init__(self, moveInfo, element, moveElement):
hierarchy = moveInfo.HierarchyList
info = moveInfo.ElementInfo
self.objs = [
h.Assembly.getLinkedObject(True) for h in reversed(hierarchy)
]
self.assembly = resolveAssembly(info.Parent)
self.viewObject = self.assembly.Object.ViewObject
self.info = info
self.element = element
self.undos = None
self.trace = None
self.tracePoint = None
self.moveElement = moveElement
self.sels = []
view = self.viewObject.Document.ActiveView
shape = None
if hasattr(view, 'addObjectOnTop'):
self.view = view
else:
self.view = None
if element:
if self.view:
self.sels.append((moveInfo.SelObj, moveInfo.SelSubname))
view.addObjectOnTop(*self.sels[0])
logger.debug('group on top {}.{}', moveInfo.SelObj.Name,
moveInfo.SelSubname)
shape = element.getSubObject('')
# whether to move element itself or its owner part
if moveElement:
self.bbox = shape.BoundBox
# Place the dragger at element's current (maybe offseted) shape
# center point in assembly coordinate
self.draggerPlacement = utils.getElementPlacement(shape)
return
# if we are not moving the element, but its owner part, transform
# the element shape to part's coordinate space
shape.Placement = shape.Placement.multiply(
info.Placement.inverse())
if self.view:
sub = moveInfo.SelSubname[:-len(info.SubnameRef)]
if isinstance(info.Part, tuple):
sub += '2.{}.{}.'.format(info.Part[0].Name, info.Part[1])
else:
sub += '2.{}.'.format(info.Part.Name)
self.sels.append((moveInfo.SelObj, sub))
logger.debug('group on top {}.{}', moveInfo.SelObj.Name, sub)
view.addObjectOnTop(*self.sels[-1])
fixed = Constraint.getFixedTransform(self.assembly.getConstraints())
fixed = fixed.get(info.Part, None)
self.fixedTransform = fixed
if not shape:
if fixed and fixed.Shape:
shape = fixed.Shape
else:
shape = info.Shape
rot = utils.getElementRotation(shape)
if not rot:
# in case the shape has no normal, like a vertex, just use an empty
# rotation, which means having the same rotation as the owner part.
rot = FreeCAD.Rotation()
hasBound = True
if not utils.isVertex(shape):
self.bbox = shape.BoundBox
else:
bbox = info.Object.ViewObject.getBoundingBox()
if bbox.isValid():
self.bbox = bbox
else:
logger.warn('empty bounding box of part {}', info.PartName)
self.bbox = FreeCAD.BoundBox(0, 0, 0, 5, 5, 5)
hasBound = False
pos = utils.getElementPos(shape)
if not pos:
if hasBound:
pos = self.bbox.Center
else:
pos = shape.Placement.Base
pla = FreeCAD.Placement(pos, rot)
self.offset = pla.copy()
self.offsetInv = pla.inverse()
self.draggerPlacement = info.Placement.multiply(pla)
def export_gazebo_model(assembly_file, model_dir, configs={}):
doc = FreeCAD.open(assembly_file)
robot_name = configs.get('name', doc.Label)
scale = configs.get('scale', 0.001)
scale_vec = FreeCAD.Vector([scale] * 3)
density = configs.get('density', 1000)
export_mesh = configs.get('export', True)
assembly_dir = os.path.split(doc.FileName)[0]
bounding_box = FreeCAD.BoundBox()
for obj in doc.findObjects('Part::Feature'):
bounding_box.add(obj.Shape.BoundBox)
bounding_box.scale(*scale_vec)
global_pose_base = FreeCAD.Vector(bounding_box.XLength / 2,
bounding_box.YLength / 2,
bounding_box.ZLength / 2)
global_pose_base -= bounding_box.Center
global_pose = FreeCAD.Placement()
global_pose.Base = global_pose_base
model = Model(name=robot_name, pose=global_pose)
model.self_collide = configs.get('self_collide', False)
model.sdf_version = '1.5'
joint_limits = configs.get('joints_limits', {})
joint_dynamics = configs.get('joints_dynamics', {})
constraints = []
for obj in doc.Objects:
if a2plib.isA2pPart(obj):
name = obj.Label
shape = obj.Shape
mass = shape.Mass * scale**3 * density
com = shape.CenterOfMass * scale
inr = shape.MatrixOfInertia
inr.scale(*scale_vec * (scale**4) * density)
placement = shape.Placement
placement.Base.scale(*scale_vec)
part_file = os.path.join(assembly_dir, obj.sourceFile)
part_file = os.path.normpath(part_file)
mesh_file = os.path.join(model_dir, 'meshes',
os.path.relpath(part_file, assembly_dir))
mesh_file = os.path.splitext(mesh_file)[0] + '.dae'
mesh_dir = os.path.split(mesh_file)[0]
if export_mesh:
os.makedirs(mesh_dir, exist_ok=True)
export(doc, [obj], mesh_file, scale=scale, offset=com * -1)
pose = placement.copy()
pose.Base = com
pose_rpy = pose.copy()
pose_rpy.Base = (np.zeros(3))
inertia = Inertia(inertia=np.array(inr.A)[[0, 1, 2, 5, 6, 10]])
inertial = Inertial(pose=pose_rpy, mass=mass, inertia=inertia)
package = configs.get('ros_package', robot_name)
mesh_uri = os.path.join(package,
os.path.relpath(mesh_file, model_dir))
mesh_uri = os.path.normpath(mesh_uri)
visual = Visual(name=name + '_visual', mesh=mesh_uri)
collision = Collision(name=name + '_collision', mesh=mesh_uri)
link = Link(name=name,
pose=pose,
inertial=inertial,
visual=visual,
collision=collision)
model.links.append(link)
elif a2plib.isA2pConstraint(obj):
parent = doc.getObject(obj.Object1)
child = doc.getObject(obj.Object2)
if sorted([parent.Label, child.Label]) in constraints:
continue
if obj.Type == 'axial' and not obj.lockRotation:
pose = a2plib.getPos(parent, obj.SubElement1)
pose = pose - child.Shape.CenterOfMass
pose.scale(*scale_vec)
joint_pose = FreeCAD.Placement()
joint_pose.Base = pose
axis_pose = a2plib.getAxis(parent, obj.SubElement1)
axis = Axis(pose=axis_pose,
lower_limit=joint_limits.get('lower', -90),
upper_limit=joint_limits.get('upper', 90),
effort_limit=joint_limits.get('effort', 10),
velocity_limit=joint_limits.get('velocity', 10),
friction=joint_dynamics.get('friction', 0),
damping=joint_dynamics.get('damping', 0))
joint = Joint(name=parent.Label + '_' + child.Label,
pose=joint_pose,
parent=parent.Label,
child=child.Label,
type='revolute',
axis=axis)
model.joints.append(joint)
constraints.append(sorted([parent.Label, child.Label]))
os.makedirs(os.path.join(model_dir, 'models'), exist_ok=True)
with open(os.path.join(model_dir, 'models', robot_name + '.sdf'),
'w') as sdf_file:
sdf_file.write(model.to_xml_string('sdf'))
if not configs.get('sdf_only', None):
with open(os.path.join(model_dir, 'models', robot_name + '.urdf'),
'w') as urdf_file:
urdf_file.write(model.to_xml_string('urdf'))
actuators = ET.Element('robot', name=robot_name)
gazebo = ET.SubElement(actuators, 'gazebo')
plugin = ET.SubElement(gazebo, 'plugin')
plugin.set('filename', 'libgazebo_ros_control.so')
plugin.set('name', 'gazebo_ros_control')
namespace = ET.SubElement(plugin, 'robotNamespace')
namespace.text = '/' + robot_name
simtype = ET.SubElement(plugin, 'robotSimType')
simtype.text = 'gazebo_ros_control/DefaultRobotHWSim'
tr_configs = configs.get('transmission', {})
jt_configs = configs.get('joints_config')
pid = configs.get('joints_pid')
joint_names = [joint.name for joint in model.joints]
for joint in joint_names:
transmission = ET.SubElement(actuators, 'transmission', name=joint)
tr_type = ET.SubElement(transmission, 'type')
tr_type.text = tr_configs.get(
'type', 'transmission_interface/SimpleTransmission')
actuator = ET.SubElement(transmission, 'actuator', name=joint)
hw_interface = ET.SubElement(actuator, 'hardwareInterface')
hw_interface.text = tr_configs.get(
'hardware_interface',
'hardware_interface/PositionJointInterface')
reduction = ET.SubElement(actuator, 'mechanicalReduction')
reduction.text = '1'
tr_joint = ET.SubElement(transmission, 'joint', name=joint)
hw_interface = ET.SubElement(tr_joint, 'hardwareInterface')
hw_interface.text = tr_configs.get(
'hardware_interface',
'hardware_interface/PositionJointInterface')
with open(
os.path.join(model_dir, 'models',
robot_name + '_actuators.urdf'),
'w') as actuators_file:
actuators_file.write(
parseString(ET.tostring(actuators)).toprettyxml(indent=' ' *
2))
control_configs = {}
control_configs[robot_name] = {
'joint_state_controller': {
'type': 'joint_state_controller/JointStateController',
'publish_rate': 50,
}
}
if jt_configs.get('groupped', False):
for joint in joint_names:
control_configs[robot_name][joint + '_controller'] = {
'type':
jt_configs.get(
'type',
'position_controllers/JointGroupPositionController'),
'joint':
joint,
'pid':
pid.copy()
}
else:
control_configs[robot_name]['joints_controller'] = {
'type':
jt_configs.get(
'type',
'position_controllers/JointGroupPositionController'),
'publish_rate':
50,
'joints':
joint_names
}
control_configs[robot_name]['gazebo_ros_control/pid_gains'] = {}
for joint in joint_names:
control_configs[robot_name]['gazebo_ros_control/pid_gains'][
joint] = pid.copy()
os.makedirs(os.path.join(model_dir, 'config'), exist_ok=True)
with open(
os.path.join(model_dir, 'config',
robot_name + '_controll.yaml'),
'w') as control_configs_file:
yaml.dump_all([control_configs],
control_configs_file,
sort_keys=False)
