// Converts the PIDs to actual references to the components and proceeds recursively through the child links
public static void loadSWComponents(ModelDoc2 model, link Link)
{
Link.SWMainComponent = loadSWComponent(model, Link.SWMainComponentPID);
Link.SWcomponents = loadSWComponents(model, Link.SWComponentPIDs);
foreach (link Child in Link.Children)
{
loadSWComponents(model, Child);
}
}
public static int getCount(link Link)
{
int count = 1;
foreach (link child in Link.Children)
{
count += getCount(child);
}
return(count);
}
//Converts a LinkNode into a Link
public link createLinkFromLinkNode(LinkNode node)
{
link Link = node.Link;
Link.Children.Clear();
foreach (LinkNode child in node.Nodes)
{
link childLink = createLinkFromLinkNode(child);
Link.Children.Add(childLink); // Recreates the children of each embedded link
}
return(Link);
}
//From the link, this method fills the property boxes on the Link Properties page
public void fillLinkPropertyBoxes(link Link)
{
fillBlank(linkBoxes);
if (!Link.isFixedFrame)
{
Link.Collision.Origin.fillBoxes(textBox_collision_origin_x,
textBox_collision_origin_y,
textBox_collision_origin_z,
textBox_collision_origin_roll,
textBox_collision_origin_pitch,
textBox_collision_origin_yaw,
"G5");
Link.Visual.Origin.fillBoxes(textBox_visual_origin_x,
textBox_visual_origin_y,
textBox_visual_origin_z,
textBox_visual_origin_roll,
textBox_visual_origin_pitch,
textBox_visual_origin_yaw,
"G5");
Link.Inertial.Origin.fillBoxes(textBox_inertial_origin_x,
textBox_inertial_origin_y,
textBox_inertial_origin_z,
textBox_inertial_origin_roll,
textBox_inertial_origin_pitch,
textBox_inertial_origin_yaw,
"G5");
Link.Inertial.Mass.fillBoxes(textBox_mass, "G5");
Link.Inertial.Inertia.fillBoxes(textBox_ixx,
textBox_ixy,
textBox_ixz,
textBox_iyy,
textBox_iyz,
textBox_izz,
"G5");
Link.Visual.Material.fillBoxes(comboBox_materials, "G5");
textBox_texture.Text = Link.Visual.Material.Texture.wFilename;
Link.Visual.Material.Color.fillBoxes(domainUpDown_red,
domainUpDown_green,
domainUpDown_blue,
domainUpDown_alpha,
"G5");
radioButton_fine.Checked = Link.STLQualityFine;
radioButton_course.Checked = !Link.STLQualityFine;
}
}
//From the link, this method fills the property boxes on the Link Properties page
public void fillLinkPropertyBoxes(link Link)
{
fillBlank(linkBoxes);
if (!Link.isFixedFrame)
{
Link.Collision.Origin.fillBoxes(textBox_collision_origin_x,
textBox_collision_origin_y,
textBox_collision_origin_z,
textBox_collision_origin_roll,
textBox_collision_origin_pitch,
textBox_collision_origin_yaw,
"G5");
Link.Visual.Origin.fillBoxes(textBox_visual_origin_x,
textBox_visual_origin_y,
textBox_visual_origin_z,
textBox_visual_origin_roll,
textBox_visual_origin_pitch,
textBox_visual_origin_yaw,
"G5");
Link.Inertial.Origin.fillBoxes(textBox_inertial_origin_x,
textBox_inertial_origin_y,
textBox_inertial_origin_z,
textBox_inertial_origin_roll,
textBox_inertial_origin_pitch,
textBox_inertial_origin_yaw,
"G5");
Link.Inertial.Mass.fillBoxes(textBox_mass, "G5");
Link.Inertial.Inertia.fillBoxes(textBox_ixx,
textBox_ixy,
textBox_ixz,
textBox_iyy,
textBox_iyz,
textBox_izz,
"G5");
Link.Visual.Material.fillBoxes(comboBox_materials, "G5");
textBox_texture.Text = Link.Visual.Material.Texture.wFilename;
Link.Visual.Material.Color.fillBoxes(domainUpDown_red,
domainUpDown_green,
domainUpDown_blue,
domainUpDown_alpha,
"G5");
radioButton_fine.Checked = Link.STLQualityFine;
radioButton_course.Checked = !Link.STLQualityFine;
}
}
//Converts the SW component references to PIDs
public static void saveSWComponents(ModelDoc2 model, link Link)
{
model.ClearSelection2(true);
byte[] PID = saveSWComponent(model, Link.SWMainComponent);
if (PID != null)
{
Link.SWMainComponentPID = PID;
}
Link.SWComponentPIDs = saveSWComponents(model, Link.SWcomponents);
foreach (link Child in Link.Children)
{
saveSWComponents(model, Child);
}
}
// Similar to the AssemblyExportForm method. It creates a LinkNode from a Link object
public LinkNode createLinkNodeFromLink(link Link)
{
LinkNode node = new LinkNode();
node.Name = Link.name;
node.Text = Link.name;
node.Link = Link;
node.ContextMenuStrip = docMenu;
foreach (link child in Link.Children)
{
node.Nodes.Add(createLinkNodeFromLink(child));
}
node.Link.Children.Clear(); // Need to erase the children from the embedded link because they may be rearranged later.
return(node);
}
//Converts the text boxes back into values for the link
public void saveLinkDataFromPropertyBoxes(link Link)
{
if (!Link.isFixedFrame)
{
Link.Inertial.Origin.update(textBox_inertial_origin_x,
textBox_inertial_origin_y,
textBox_inertial_origin_z,
textBox_inertial_origin_roll,
textBox_inertial_origin_pitch,
textBox_inertial_origin_yaw);
Link.Visual.Origin.update(textBox_visual_origin_x,
textBox_visual_origin_y,
textBox_visual_origin_z,
textBox_visual_origin_roll,
textBox_visual_origin_pitch,
textBox_visual_origin_yaw);
Link.Collision.Origin.update(textBox_collision_origin_x,
textBox_collision_origin_y,
textBox_collision_origin_z,
textBox_collision_origin_roll,
textBox_collision_origin_pitch,
textBox_collision_origin_yaw);
Link.Inertial.Mass.update(textBox_mass);
Link.Inertial.Inertia.update(textBox_ixx,
textBox_ixy,
textBox_ixz,
textBox_iyy,
textBox_iyz,
textBox_izz);
Link.Visual.Material.name = comboBox_materials.Text;
Link.Visual.Material.Texture.wFilename = textBox_texture.Text;
Link.Visual.Material.Color.update(domainUpDown_red,
domainUpDown_green,
domainUpDown_blue,
domainUpDown_alpha);
Link.STLQualityFine = radioButton_fine.Checked;
}
}
//Populates the TreeView with the organized links from the robot
public void fillTreeViewFromRobot(robot robot)
{
tree.Nodes.Clear();
LinkNode baseNode = new LinkNode();
link baseLink = robot.BaseLink;
baseNode.Name = baseLink.name;
baseNode.Text = baseLink.name;
baseNode.Link = baseLink;
baseNode.ContextMenuStrip = docMenu;
foreach (link child in baseLink.Children)
{
baseNode.Nodes.Add(createLinkNodeFromLink(child));
}
tree.Nodes.Add(baseNode);
tree.ExpandAll();
}
//Fills either TreeView from the URDF robot
public void fillTreeViewFromRobot(robot Robot, TreeView tree)
{
LinkNode baseNode = new LinkNode();
link baseLink = Robot.BaseLink;
baseNode.Name = baseLink.name;
baseNode.Text = baseLink.name;
baseNode.Link = baseLink;
baseNode.isBaseNode = true;
baseNode.linkName = baseLink.name;
baseNode.Components = baseLink.SWcomponents;
baseNode.coordsysName = "Origin_global";
baseNode.isIncomplete = false;
foreach (link child in baseLink.Children)
{
baseNode.Nodes.Add(createLinkNodeFromLink(child));
}
tree.Nodes.Add(baseNode);
tree.ExpandAll();
}
//Recursive method for exporting each link (and writing it to the URDF)
public string exportFiles(link Link, URDFPackage package, int count)
{
progressBar.UpdateProgress(count);
progressBar.UpdateTitle("Exporting mesh: " + Link.name);
// Iterate through each child and export its files
foreach (link child in Link.Children)
{
count += 1;
if (!child.isFixedFrame)
{
string filename = exportFiles(child, package, count);
child.Visual.Geometry.Mesh.filename = filename;
child.Collision.Geometry.Mesh.filename = filename;
}
}
// Copy the texture file (if it was specified) to the textures directory
if (!Link.isFixedFrame && Link.Visual.Material.Texture.wFilename != "")
{
if (System.IO.File.Exists(Link.Visual.Material.Texture.wFilename))
{
Link.Visual.Material.Texture.filename =
package.TexturesDirectory + Path.GetFileName(Link.Visual.Material.Texture.wFilename);
string textureSavePath =
package.WindowsTexturesDirectory + Path.GetFileName(Link.Visual.Material.Texture.wFilename);
System.IO.File.Copy(Link.Visual.Material.Texture.wFilename, textureSavePath, true);
}
}
// Create the mesh filenames. SolidWorks likes to use / but that will get messy in filenames so use _ instead
string linkName = Link.name.Replace('/', '_');
string meshFileName = package.MeshesDirectory + linkName + ".STL";
string windowsMeshFileName = package.WindowsMeshesDirectory + linkName + ".STL";
// Export STL
saveSTL(Link, windowsMeshFileName);
return(meshFileName);
}
//Selects the components of a link. Helps highlight when the associated node is selected from the tree
public static void selectComponents(ModelDoc2 model, link Link, bool clearSelection, int mark = -1)
{
if (clearSelection)
{
model.ClearSelection2(true);
}
SelectionMgr manager = model.SelectionManager;
SelectData data = manager.CreateSelectData();
data.Mark = mark;
if (Link.SWComponent != null)
{
Link.SWComponent.Select4(true, data, false);
}
else
{
selectComponents(model, Link.SWcomponents, false);
}
foreach (link child in Link.Children)
{
selectComponents(model, child, false, mark);
}
}
//Selects the components of a link. Helps highlight when the associated node is selected from the tree
public static void selectComponents(ModelDoc2 model, link Link, bool clearSelection, int mark = -1)
{
if (clearSelection)
{
model.ClearSelection2(true);
}
SelectionMgr manager = model.SelectionManager;
SelectData data = manager.CreateSelectData();
data.Mark = mark;
if (Link.SWComponent != null)
{
Link.SWComponent.Select4(true, data, false);
}
else
{
selectComponents(model, Link.SWcomponents, false);
}
foreach (link child in Link.Children)
{
selectComponents(model, child, false, mark);
}
}
// This creates a Link from a Part ModelDoc. It basically just extracts the material properties and saves them to the appropriate fields.
public link createLinkFromPartModel(ModelDoc2 swModel)
{
link Link = new link();
Link.name = swModel.GetTitle();
Link.isFixedFrame = false;
Link.Visual = new visual();
Link.Inertial = new inertial();
Link.Collision = new collision();
//Get link properties from SolidWorks part
IMassProperty swMass = swModel.Extension.CreateMassProperty();
Link.Inertial.Mass.value = swMass.Mass;
double[] moment = swMass.GetMomentOfInertia((int)swMassPropertyMoment_e.swMassPropertyMomentAboutCenterOfMass); // returned as double with values [Lxx, Lxy, Lxz, Lyx, Lyy, Lyz, Lzx, Lzy, Lzz]
Link.Inertial.Inertia.setMomentMatrix(moment);
double[] centerOfMass = swMass.CenterOfMass;
Link.Inertial.Origin.xyz = centerOfMass;
Link.Inertial.Origin.rpy = new double[3] { 0, 0, 0 };
// Will this ever not be zeros?
Link.Visual.Origin.xyz = new double[3] { 0, 0, 0 };
Link.Visual.Origin.rpy = new double[3] { 0, 0, 0 };
Link.Collision.Origin.xyz = new double[3] { 0, 0, 0 };
Link.Collision.Origin.rpy = new double[3] { 0, 0, 0 };
// [ R, G, B, Ambient, Diffuse, Specular, Shininess, Transparency, Emission ]
double[] values = swModel.MaterialPropertyValues;
Link.Visual.Material.Color.Red = values[0];
Link.Visual.Material.Color.Green = values[1];
Link.Visual.Material.Color.Blue = values[2];
Link.Visual.Material.Color.Alpha = 1.0 - values[7];
Link.Visual.Material.name = "material_" + Link.name;
return Link;
}
// Converts the PIDs to actual references to the components and proceeds recursively through the child links
public static void loadSWComponents(ModelDoc2 model, link Link)
{
Link.SWMainComponent = loadSWComponent(model, Link.SWMainComponentPID);
Link.SWcomponents = loadSWComponents(model, Link.SWComponentPIDs);
foreach (link Child in Link.Children)
{
loadSWComponents(model, Child);
}
}
//Recursive method for exporting each link (and writing it to the URDF)
public string exportFiles(link Link, URDFPackage package, int count)
{
progressBar.UpdateProgress(count);
progressBar.UpdateTitle("Exporting mesh: " + Link.name);
// Iterate through each child and export its files
foreach (link child in Link.Children)
{
count += 1;
if (!child.isFixedFrame)
{
string filename = exportFiles(child, package, count);
child.Visual.Geometry.Mesh.filename = filename;
child.Collision.Geometry.Mesh.filename = filename;
}
}
// Copy the texture file (if it was specified) to the textures directory
if (!Link.isFixedFrame && Link.Visual.Material.Texture.wFilename != "")
{
if (System.IO.File.Exists(Link.Visual.Material.Texture.wFilename))
{
Link.Visual.Material.Texture.filename = package.TexturesDirectory + Path.GetFileName(Link.Visual.Material.Texture.wFilename);
string textureSavePath = package.WindowsTexturesDirectory + Path.GetFileName(Link.Visual.Material.Texture.wFilename);
System.IO.File.Copy(Link.Visual.Material.Texture.wFilename, textureSavePath, true);
}
}
// Create the mesh filenames. SolidWorks likes to use / but that will get messy in filenames so use _ instead
string linkName = Link.name.Replace('/', '_');
string meshFileName = package.MeshesDirectory + linkName + ".STL";
string windowsMeshFileName = package.WindowsMeshesDirectory + linkName + ".STL";
// Export STL
saveSTL(Link, windowsMeshFileName);
return meshFileName;
}
//Hides the components from a link
public static void hideComponents(ModelDoc2 model, link Link)
{
selectComponents(model, Link, true);
model.HideComponent2();
}
//Converts a Link to a LinkNode
public LinkNode createLinkNodeFromLink(link Link)
{
LinkNode node = new LinkNode();
node.Name = Link.name;
node.Text = Link.name;
node.Link = Link;
node.isBaseNode = false;
node.linkName = Link.name;
node.jointName = Link.Joint.name;
node.Components = Link.SWcomponents;
node.coordsysName = Link.Joint.CoordinateSystemName;
node.axisName = Link.Joint.AxisName;
node.jointType = Link.Joint.type;
node.isIncomplete = false;
foreach (link child in Link.Children)
{
node.Nodes.Add(createLinkNodeFromLink(child));
}
node.Link.Children.Clear(); // Need to erase the children from the embedded link because they may be rearranged later.
return node;
}
//Hides the components from a link
public static void hideComponents(ModelDoc2 model, link Link)
{
selectComponents(model, Link, true);
model.HideComponent2();
}
public static int getCount(link Link)
{
int count = 1;
foreach (link child in Link.Children)
{
count += getCount(child);
}
return count;
}
public void moveOrigin(link parent, link nonLocalizedChild)
{
double X_max = Double.MinValue; double Y_max = Double.MinValue; double Z_max = Double.MinValue;
double X_min = Double.MaxValue; double Y_min = Double.MaxValue; double Z_min = Double.MaxValue;
double[] points;
foreach (Component2 comp in nonLocalizedChild.SWcomponents)
{
points = comp.GetBox(false, false); // Returns box as [ XCorner1, YCorner1, ZCorner1, XCorner2, YCorner2, ZCorner2 ]
X_max = ops.max(points[0], points[3], X_max);
Y_max = ops.max(points[1], points[4], Y_max);
Z_max = ops.max(points[2], points[5], Z_max);
X_min = ops.min(points[0], points[3], X_min);
Y_min = ops.min(points[1], points[4], Y_min);
Z_min = ops.min(points[2], points[5], Z_min);
}
string coordsys = (parent.Joint == null) ? parent.CoordSysName : parent.Joint.CoordinateSystemName;
MathTransform parentTransform = getCoordinateSystemTransform(coordsys);
double[] idealOrigin = ops.closestPointOnLineToPoint(ops.getXYZ(parentTransform), nonLocalizedChild.Joint.Axis.xyz, nonLocalizedChild.Joint.Origin.xyz);
nonLocalizedChild.Joint.Origin.xyz = ops.closestPointOnLineWithinBox(X_min, X_max, Y_min, Y_max, Z_min, Z_max, nonLocalizedChild.Joint.Axis.xyz, idealOrigin);
}
//This is only used by the Part Exporter, but it localizes the link to the Origin_global coordinate system
public void localizeLink(link Link, Matrix<double> GlobalTransform)
{
Matrix<double> GlobalTransformInverse = GlobalTransform.Inverse();
Matrix<double> linkCoMTransform = ops.getTranslation(Link.Inertial.Origin.xyz);
Matrix<double> localLinkCoMTransform = GlobalTransformInverse * linkCoMTransform;
Matrix<double> linkVisualTransform = ops.getTransformation(Link.Visual.Origin.xyz, Link.Visual.Origin.rpy);
Matrix<double> localVisualTransform = GlobalTransformInverse * linkVisualTransform;
Matrix<double> linkCollisionTransform = ops.getTransformation(Link.Collision.Origin.xyz, Link.Collision.Origin.rpy);
Matrix<double> localCollisionTransform = GlobalTransformInverse * linkCollisionTransform;
// The linear array in Link.Inertial.Inertia.Moment is in row major order, but this matrix constructor uses column major order
// It's a rotation matrix, so this shouldn't matter. If it does, just transpose linkGlobalMomentInertia
// These three matrices are 3x3 as opposed to the 4x4 transformation matrices above. You're welcome for the confusion.
Matrix<double> linkGlobalMomentInertia = new DenseMatrix(3, 3, Link.Inertial.Inertia.Moment);
Matrix<double> GlobalRotMat = GlobalTransform.SubMatrix(0, 3, 0, 3);
Matrix<double> linkLocalMomentInertia = GlobalRotMat.Inverse() * linkGlobalMomentInertia;
Link.Inertial.Origin.xyz = ops.getXYZ(localLinkCoMTransform);
Link.Inertial.Origin.rpy = new double[] { 0, 0, 0 };
// Wait are you saying that even though the matrix was trasposed from column major order, you are writing it in row-major order here.
// Yes, yes I am.
double[] moment = linkLocalMomentInertia.ToRowWiseArray();
Link.Inertial.Inertia.setMomentMatrix(moment);
Link.Collision.Origin.xyz = ops.getXYZ(localCollisionTransform);
Link.Collision.Origin.rpy = ops.getRPY(localCollisionTransform);
Link.Visual.Origin.rpy = ops.getXYZ(localVisualTransform);
Link.Visual.Origin.xyz = ops.getRPY(localVisualTransform);
}
public robot()
{
BaseLink = new link();
}
//Converts the SW component references to PIDs
public static void saveSWComponents(ModelDoc2 model, link Link)
{
model.ClearSelection2(true);
byte[] PID = saveSWComponent(model, Link.SWMainComponent);
if (PID != null)
{
Link.SWMainComponentPID = PID;
}
Link.SWComponentPIDs = saveSWComponents(model, Link.SWcomponents);
foreach (link Child in Link.Children)
{
saveSWComponents(model, Child);
}
}
private void saveSTL(link Link, string windowsMeshFileName)
{
int errors = 0;
int warnings = 0;
string coordsysName = "";
coordsysName = (Link.Joint == null || Link.Joint.CoordinateSystemName == null) ? Link.CoordSysName : Link.Joint.CoordinateSystemName;
Dictionary<string, string> names = GetComponentRefGeoNames(coordsysName);
ModelDoc2 ActiveDoc = ActiveSWModel;
string ComponentName = "";
string ConfigurationName = "";
string DisplayStateName = "";
Component2 geoComponent = default(Component2);
if (names["component"].Length > 0)
{
foreach (Component2 comp in Link.SWcomponents)
{
if (comp.Name2 == names["component"])
{
geoComponent = comp;
ComponentName = comp.GetPathName();
ConfigurationName = comp.ReferencedConfiguration;
DisplayStateName = comp.ReferencedDisplayState;
bool usenamed = comp.UseNamedConfiguration;
ActiveDoc = (ModelDoc2)iSwApp.ActivateDoc3(ComponentName, false, 0, 0);
Configuration config = ActiveDoc.GetConfigurationByName(ConfigurationName);
ActiveDoc.ShowConfiguration2(ConfigurationName);
config.ApplyDisplayState(DisplayStateName);
}
break;
}
}
if (ComponentName.Length == 0)
{
Common.showComponents(ActiveSWModel, Link.SWcomponents);
}
int saveOptions = (int)swSaveAsOptions_e.swSaveAsOptions_Silent;
setLinkSpecificSTLPreferences(names["geo"], Link.STLQualityFine, ActiveDoc);
ActiveDoc.Extension.SaveAs(windowsMeshFileName, (int)swSaveAsVersion_e.swSaveAsCurrentVersion, saveOptions, null, ref errors, ref warnings);
if (ComponentName.Length > 0)
{
iSwApp.CloseDoc(ComponentName);
geoComponent.ReferencedConfiguration = ConfigurationName;
}
else
{
Common.hideComponents(ActiveSWModel, Link.SWcomponents);
}
correctSTLMesh(windowsMeshFileName);
}
//Converts the text boxes back into values for the link
public void saveLinkDataFromPropertyBoxes(link Link)
{
if (!Link.isFixedFrame)
{
Link.Inertial.Origin.update(textBox_inertial_origin_x,
textBox_inertial_origin_y,
textBox_inertial_origin_z,
textBox_inertial_origin_roll,
textBox_inertial_origin_pitch,
textBox_inertial_origin_yaw);
Link.Visual.Origin.update(textBox_visual_origin_x,
textBox_visual_origin_y,
textBox_visual_origin_z,
textBox_visual_origin_roll,
textBox_visual_origin_pitch,
textBox_visual_origin_yaw);
Link.Collision.Origin.update(textBox_collision_origin_x,
textBox_collision_origin_y,
textBox_collision_origin_z,
textBox_collision_origin_roll,
textBox_collision_origin_pitch,
textBox_collision_origin_yaw);
Link.Inertial.Mass.update(textBox_mass);
Link.Inertial.Inertia.update(textBox_ixx,
textBox_ixy,
textBox_ixz,
textBox_iyy,
textBox_iyz,
textBox_izz);
Link.Visual.Material.name = comboBox_materials.Text;
Link.Visual.Material.Texture.wFilename = textBox_texture.Text;
Link.Visual.Material.Color.update(domainUpDown_red,
domainUpDown_green,
domainUpDown_blue,
domainUpDown_alpha);
Link.STLQualityFine = radioButton_fine.Checked;
}
}
public robot()
{
BaseLink = new link();
isRequired = true;
Name = new Attribute();
Name.isRequired = true;
Name.type = "name";
}
//Used to fix components to estimate the degree of freedom.
private List<Component2> fixComponents(link parent)
{
List<Component2> componentsToUnfix = new List<Component2>();
foreach (Component2 comp in parent.SWcomponents)
{
bool isFixed = comp.IsFixed();
if (!comp.IsFixed())
{
componentsToUnfix.Add(comp);
}
}
Common.selectComponents(ActiveSWModel, parent.SWcomponents, true);
AssemblyDoc assy = (AssemblyDoc)ActiveSWModel;
assy.FixComponent();
return componentsToUnfix;
}
private void saveSTL(link Link, string windowsMeshFileName)
{
int errors = 0;
int warnings = 0;
string coordsysName = "";
coordsysName = (Link.Joint == null || Link.Joint.CoordinateSystemName == null) ? Link.CoordSysName : Link.Joint.CoordinateSystemName;
Dictionary <string, string> names = GetComponentRefGeoNames(coordsysName);
ModelDoc2 ActiveDoc = ActiveSWModel;
string ComponentName = "";
string ConfigurationName = "";
string DisplayStateName = "";
Component2 geoComponent = default(Component2);
if (names["component"].Length > 0)
{
foreach (Component2 comp in Link.SWcomponents)
{
if (comp.Name2 == names["component"])
{
geoComponent = comp;
ComponentName = comp.GetPathName();
ConfigurationName = comp.ReferencedConfiguration;
DisplayStateName = comp.ReferencedDisplayState;
bool usenamed = comp.UseNamedConfiguration;
ActiveDoc = (ModelDoc2)iSwApp.ActivateDoc3(ComponentName, false, 0, 0);
Configuration config = ActiveDoc.GetConfigurationByName(ConfigurationName);
ActiveDoc.ShowConfiguration2(ConfigurationName);
config.ApplyDisplayState(DisplayStateName);
}
break;
}
}
if (ComponentName.Length == 0)
{
Common.showComponents(ActiveSWModel, Link.SWcomponents);
}
int saveOptions = (int)swSaveAsOptions_e.swSaveAsOptions_Silent;
setLinkSpecificSTLPreferences(names["geo"], Link.STLQualityFine, ActiveDoc);
ActiveDoc.Extension.SaveAs(windowsMeshFileName, (int)swSaveAsVersion_e.swSaveAsCurrentVersion, saveOptions, null, ref errors, ref warnings);
if (ComponentName.Length > 0)
{
iSwApp.CloseDoc(ComponentName);
geoComponent.ReferencedConfiguration = ConfigurationName;
}
else
{
Common.hideComponents(ActiveSWModel, Link.SWcomponents);
}
correctSTLMesh(windowsMeshFileName);
}
// Similar to the AssemblyExportForm method. It creates a LinkNode from a Link object
public LinkNode createLinkNodeFromLink(link Link)
{
LinkNode node = new LinkNode();
node.Name = Link.name;
node.Text = Link.name;
node.Link = Link;
node.ContextMenuStrip = docMenu;
foreach (link child in Link.Children)
{
node.Nodes.Add(createLinkNodeFromLink(child));
}
node.Link.Children.Clear(); // Need to erase the children from the embedded link because they may be rearranged later.
return node;
}
//Calculates the free degree of freedom (if exists), and then determines the location of the joint, the axis of rotation/translation, and the type of joint
public void estimateGlobalJointFromComponents(AssemblyDoc assy, link parent, link child)
{
//Create the ref objects
int R1Status, R2Status, L1Status, L2Status, R1DirStatus, R2DirStatus, DOFs;
MathPoint RPoint1, RPoint2;
MathVector RDir1, RDir2, LDir1, LDir2;
// Surpress Limit Mates to properly find degrees of freedom. They don't work with the API call
List<Mate2> limitMates = new List<Mate2>();
limitMates = suppressLimitMates(child.SWMainComponent);
if (child.SWMainComponent != null)
{
// The wonderful undocumented API call I found to get the degrees of freedom in a joint.
// https://forum.solidworks.com/thread/57414
int remainingDOFs = child.SWMainComponent.GetRemainingDOFs(out R1Status, out RPoint1, out R1DirStatus, out RDir1,
out R2Status, out RPoint2, out R2DirStatus, out RDir2,
out L1Status, out LDir1,
out L2Status, out LDir2);
DOFs = remainingDOFs;
// Convert the gotten degrees of freedom to a joint type, origin and axis
child.Joint.type = "fixed";
child.Joint.Origin.xyz = ops.getXYZ(child.SWMainComponent.Transform2);
child.Joint.Origin.rpy = ops.getRPY(child.SWMainComponent.Transform2);
if (DOFs == 0 && (R1Status + L1Status > 0))
{
if (R1Status == 1)
{
child.Joint.type = "continuous";
child.Joint.Axis.xyz = RDir1.ArrayData;
child.Joint.Origin.xyz = RPoint1.ArrayData;
child.Joint.Origin.rpy = ops.getRPY(child.SWMainComponent.Transform2);
moveOrigin(parent, child);
}
else if (L1Status == 1)
{
child.Joint.type = "prismatic";
child.Joint.Axis.xyz = LDir1.ArrayData;
child.Joint.Origin.xyz = RPoint1.ArrayData;
child.Joint.Origin.rpy = ops.getRPY(child.SWMainComponent.Transform2);
moveOrigin(parent, child);
}
}
ops.threshold(child.Joint.Origin.xyz, 0.00001);
ops.threshold(child.Joint.Origin.rpy, 0.00001);
unsuppressLimitMates(limitMates);
if (limitMates.Count > 0)
{
addLimits(child.Joint, limitMates);
}
}
}
//Method which builds a single link
public link createLinkFromComponents(link parent, List<Component2> components, LinkNode node)
{
link child = new link();
child.name = node.linkName;
if (components.Count > 0)
{
child.isFixedFrame = false;
child.Visual = new visual();
child.Inertial = new inertial();
child.Collision = new collision();
child.SWMainComponent = components[0];
child.SWcomponents.AddRange(components);
}
//Get link properties from SolidWorks part
if (parent != null)
{
createJoint(parent, child, node);
}
string childCoordSysName = "";
if (child.Joint == null)
{
childCoordSysName = node.coordsysName;
}
else
{
childCoordSysName = child.Joint.CoordinateSystemName;
}
// Get the SolidWorks MathTransform that corresponds to the child coordinate system
MathTransform jointTransform = getCoordinateSystemTransform(childCoordSysName);
if (!child.isFixedFrame)
{
//selectComponents(components, true);
IMassProperty swMass = ActiveSWModel.Extension.CreateMassProperty();
swMass.SetCoordinateSystem(jointTransform);
Body2[] bodies = getBodies(components);
bool addedBodies = swMass.AddBodies(bodies);
child.Inertial.Mass.value = swMass.Mass;
double[] moment = swMass.GetMomentOfInertia((int)swMassPropertyMoment_e.swMassPropertyMomentAboutCenterOfMass); // returned as double with values [Lxx, Lxy, Lxz, Lyx, Lyy, Lyz, Lzx, Lzy, Lzz]
child.Inertial.Inertia.setMomentMatrix(moment);
double[] centerOfMass = swMass.CenterOfMass;
child.Inertial.Origin.xyz = centerOfMass;
child.Inertial.Origin.rpy = new double[3] { 0, 0, 0 };
// Will this ever not be zeros?
child.Visual.Origin.xyz = new double[3] { 0, 0, 0 };
child.Visual.Origin.rpy = new double[3] { 0, 0, 0 };
child.Collision.Origin.xyz = new double[3] { 0, 0, 0 };
child.Collision.Origin.rpy = new double[3] { 0, 0, 0 };
// [ R, G, B, Ambient, Diffuse, Specular, Shininess, Transparency, Emission ]
ModelDoc2 mainCompdoc = components[0].GetModelDoc2();
double[] values = mainCompdoc.MaterialPropertyValues;
child.Visual.Material.Color.Red = values[0];
child.Visual.Material.Color.Green = values[1];
child.Visual.Material.Color.Blue = values[2];
child.Visual.Material.Color.Alpha = 1.0 - values[7];
//child.Visual.Material.name = "material_" + child.name;
//The part model doesn't actually know where the origin is, but the component does and this is important when exporting from assembly
child.Visual.Origin.xyz = new double[] { 0, 0, 0 };
child.Visual.Origin.rpy = new double[] { 0, 0, 0 };
child.Collision.Origin.xyz = new double[] { 0, 0, 0 };
child.Collision.Origin.rpy = new double[] { 0, 0, 0 };
}
//ActiveSWModel.ClearSelection2(true);
return child;
}
//This now needs to be able to get the component, and it's associated coordinate system name.
//Then it needs to transform to the top level assembly (sounds like fun).
public void estimateGlobalJointFromRefGeometry(link parent, link child)
{
MathTransform GlobalCoordsysTransform = getCoordinateSystemTransform(child.Joint.CoordinateSystemName);
child.Joint.Origin.xyz = ops.getXYZ(GlobalCoordsysTransform);
child.Joint.Origin.rpy = ops.getRPY(GlobalCoordsysTransform);
if (child.Joint.type != "fixed")
{
estimateAxis(child.Joint);
}
}
public robot()
{
BaseLink = new link();
}
//Base method for constructing a joint from a parent link and child link.
public void createJoint(link parent, link child, LinkNode node)
{
checkRefGeometryExists(node);
string jointName = node.jointName;
string coordSysName = node.coordsysName;
string axisName = node.axisName;
string jointType = node.jointType;
List<Component2> componentsToFix = fixComponents(parent);
AssemblyDoc assy = (AssemblyDoc)ActiveSWModel;
child.Joint = new joint();
child.Joint.name = jointName;
child.Joint.Parent.name = parent.name;
child.Joint.Child.name = child.name;
if (child.isFixedFrame)
{
axisName = "";
jointType = "fixed";
child.Joint.type = jointType;
}
else if (coordSysName == "Automatically Generate" || axisName == "Automatically Generate" || jointType == "Automatically Detect")
{
// We have to estimate the joint if the user specifies automatic for either the reference coordinate system, the reference axis or the joint type.
estimateGlobalJointFromComponents(assy, parent, child);
}
if (coordSysName == "Automatically Generate")
{
child.Joint.CoordinateSystemName = "Origin_" + child.Joint.name;
ActiveSWModel.ClearSelection2(true);
int i = 2;
while (ActiveSWModel.Extension.SelectByID2(child.Joint.CoordinateSystemName, "COORDSYS", 0, 0, 0, false, 0, null, 0))
{
ActiveSWModel.ClearSelection2(true);
child.Joint.CoordinateSystemName = "Origin_" + child.Joint.name + i.ToString();
i++;
}
createRefOrigin(child.Joint);
}
else
{
child.Joint.CoordinateSystemName = coordSysName;
}
if (axisName == "Automatically Generate")
{
child.Joint.AxisName = "Axis_" + child.Joint.name;
ActiveSWModel.ClearSelection2(true);
int i = 2;
while (ActiveSWModel.Extension.SelectByID2(child.Joint.AxisName, "AXIS", 0, 0, 0, false, 0, null, 0))
{
ActiveSWModel.ClearSelection2(true);
child.Joint.AxisName = "Axis_" + child.Joint.name + i.ToString();
i++;
}
if (child.Joint.type != "fixed")
{
createRefAxis(child.Joint);
}
}
else
{
child.Joint.AxisName = axisName;
}
if (jointType != "Automatically Detect")
{
child.Joint.type = jointType;
}
estimateGlobalJointFromRefGeometry(parent, child);
coordSysName = (parent.Joint == null) ? parent.CoordSysName : parent.Joint.CoordinateSystemName;
unFixComponents(componentsToFix);
localizeJoint(child.Joint, coordSysName);
}
