/// <summary>
/// Exports the robot
/// </summary>
/// <param name="log"> Logger to be used to write messeges </param>
/// <param name="path"> File path to export robot to </param>
public override void Export(ProgressLogger log, String path)
{
this.log = log;
path += "\\" + robot.Name +"_Exported";
folderPath = path + "\\" + robot.Name;
tempFilePath = folderPath + "\\";
visualSTLPath = folderPath + "\\visualSTL\\";
collisionSTLPath = folderPath + "\\collisionSTL\\";
if (Directory.Exists(path))
Directory.Delete(path, true);
Directory.CreateDirectory(path);
Directory.CreateDirectory(folderPath);
Directory.CreateDirectory(visualSTLPath);
Directory.CreateDirectory(collisionSTLPath);
try
{
log.WriteMessage("Setting up exporter", false);
//Export STLs
log.WriteMessage("Starting STL exports", false);
ModelDoc2 ActiveDoc = (ModelDoc2)asm;
//switchs to the visual configuration and hides all components
Configuration currentConfig = ActiveDoc.ConfigurationManager.ActiveConfiguration;
ActiveDoc.ShowConfiguration2(robot.VisualConfig);
STLExporter meshMaker = new STLExporter(iSwApp, asm);
Component2[] hiddenComps = meshMaker.HideAllComponents();
//exports each Link's visual models
foreach (Link L in robot.GetLinksAsArray())
{
if (!robot.ContinueExport)
{
meshMaker.UnhideComponents(hiddenComps);
meshMaker.close();
ActiveDoc.ShowConfiguration2(currentConfig.Name);
return;
}
meshMaker.ExportLink(L, robot.VisualConfig, visualSTLPath + L.Name + ".stl", log);
}
//unhides all components, switches to collision configuration, then hides all components
meshMaker.UnhideComponents(hiddenComps);
ActiveDoc.ShowConfiguration2(robot.CollisionConfig);
hiddenComps = meshMaker.HideAllComponents();
//exports each Link's collision model
foreach (Link L in robot.GetLinksAsArray())
{
if (!robot.ContinueExport)
{
meshMaker.UnhideComponents(hiddenComps);
meshMaker.close();
ActiveDoc.ShowConfiguration2(currentConfig.Name);
return;
}
meshMaker.ExportLink(L, robot.CollisionConfig, collisionSTLPath + L.Name + ".stl", log);
}
//restores the model to its original configuration
meshMaker.UnhideComponents(hiddenComps);
ActiveDoc.ShowConfiguration(currentConfig.Name);
meshMaker.close();
log.WriteMessage("Finished exporting STL files", false);
//Make URDF
log.WriteMessage("Creating URDF file", false);
XmlWriterSettings settings = new XmlWriterSettings();
settings.Encoding = new UTF8Encoding(false);
settings.Indent = true;
settings.NewLineOnAttributes = false;
XmlWriter URDFwriter = XmlWriter.Create(tempFilePath+robot.Name+".URDF", settings);
URDFwriter.WriteStartDocument();
WriteURDF(URDFwriter);
URDFwriter.WriteEndDocument();
URDFwriter.Close();
log.WriteMessage("URDF file complete");
CreateConfigFile();
log.WriteMessage("Config file complete");
log.WriteMessage("Export Complete");
}
catch (Exception e)
{
log.WriteError("Unhandled Exception thrown when exporting robot: "+e.Message);
}
}
/// <summary>
/// Verifies that everything is good in the attachment
/// </summary>
/// <param name="log">Logger to output messages to</param>
/// <returns>true if successfully verified</returns>
public override bool Verify(ProgressLogger log)
{
log.WriteMessage("Verifying " + Name);
if (TicksPerRev == 0)
log.WriteWarning("Tick number not specified in " + Name);
if (Joint == null)
log.WriteError(Name + " must be associated with a joint.");
return true;
}
/// <summary>
/// Verifies that everything is good in the attachment
/// </summary>
/// <param name="log">Logger to output messages to</param>
/// <returns>true if successfully verified</returns>
public override bool Verify(ProgressLogger log)
{
log.WriteMessage("Verifying " + Name);
if (Multiplier == 0)
log.WriteWarning("No multiplier in " + Name);
if (Joint == null)
log.WriteError(Name + " must be associated with a joint.");
return true;
}
/// <summary>
/// Verifies that everything is good in the attachment
/// </summary>
/// <param name="log">Logger to output messages to</param>
/// <returns>true if successfully verified</returns>
public override bool Verify(ProgressLogger log)
{
log.WriteMessage("Verifying " + Name);
return true;
}
/// <summary>
/// Recalculates the mass properties for the robot and checks for issues
/// </summary>
/// <param name="log"> The ProgressLogger that messages will be printed to </param>
/// <returns> Returns true if no errors were found and the Export was not cancelled </returns>
public bool Verify(ProgressLogger log)
{
log.WriteMessage("Verifying Link Names");
modelDoc.Rebuild((int)swRebuildOptions_e.swRebuildAll);
Configuration currentConfig = modelDoc.ConfigurationManager.ActiveConfiguration;
String currentDisplayState = currentConfig.GetDisplayStates()[0];
modelDoc.ShowConfiguration2(ConfigName);
CalcAxisVectors();
CalcOrigin();
//modelDoc.ShowConfiguration(PhysicalConfig);
if (String.IsNullOrEmpty(Name) || String.IsNullOrWhiteSpace(Name))
{
log.WriteError("No name set for robot");
}
if (OriginPt == null || Direction == null || BasePlane == null)
log.WriteWarning("No pose set for robot. Defaulting to model frame");
foreach (Link l in Links.Values.ToArray())
{
l.Verify(log);
RobotInfo.WriteToLogFile("Verifying");
Application.DoEvents();
if (!ContinueExport)
break;
}
modelDoc.ShowConfiguration(currentConfig.Name);
currentConfig.ApplyDisplayState(currentDisplayState);
return ContinueExport;
}
/// <summary>
/// Exports the .stl model for one link
/// The model must already be switched to the correct configuration and all parts must be hidden before this method is called
/// </summary>
/// <param name="link">Link to be exported</param>
/// <param name="configuration">The configuration that this link should be exported from</param>
/// <param name="path">The path to save the .stl file</param>
/// <param name="log">The logger to write messages to</param>
public void ExportLink(Link link ,ModelConfiguration configuration, String path, ProgressLogger log)
{
log.WriteMessage("Exporting link " + link.Name + " as an STL. Configuration: " + ((ModelConfiguration.ModelConfigType)configuration.Type) + "; Path: " + path, false);
int errors = 0;
int warnings = 0;
ModelDoc2 ActiveDoc = (ModelDoc2)asm;
IsolateLink(link, configuration);
int saveOptions = (int)swSaveAsOptions_e.swSaveAsOptions_Silent;
ActiveDoc.Extension.SaveAs(path, (int)swSaveAsVersion_e.swSaveAsCurrentVersion, saveOptions, null, ref errors, ref warnings);
HideLink(link);
log.WriteMessage("Correcting STL header");
CorrectSTLMesh(path);
log.WriteMessage("Finished exporting STL.");
}
/// <summary>
/// recalculates the mass properties and origins of this link and checks for any issues
/// </summary>
/// <param name="log"> ProgressLogger that messages should be printed to </param>
/// <returns> Returns true if successfully verified link, otherwise returns false </returns>
public bool Verify(ProgressLogger log)
{
log.WriteMessage("Verifying link " + Name);
string currentConfig = modelDoc.ConfigurationManager.ActiveConfiguration.Name;
//switch to physical config for inertia calcs
if (!currentConfig.Equals(PhysicalModel.swConfiguration))
modelDoc.ShowConfiguration2(PhysicalModel.swConfiguration);
//recalculates center of mass and inertia
CalcInertia(log,false);
RobotInfo.WriteToLogFile("Calc Inertia Completed");
//switch to visual config for axis calcs
if (!modelDoc.ConfigurationManager.ActiveConfiguration.Name
.Equals(LinkModels[(int)ModelConfiguration.ModelConfigType.Visual].swConfiguration))
modelDoc.ShowConfiguration2(LinkModels[(int)ModelConfiguration.ModelConfigType.Visual].swConfiguration);
//checks that model components are defined
if (LinkModels[(int)ModelConfiguration.ModelConfigType.Visual].IsEmpty())
log.WriteError("No Visual models for link " + Name);
if (LinkModels[(int)ModelConfiguration.ModelConfigType.Collision].IsEmpty())
log.WriteWarning("No unique collison models for link " + Name + ". Using visual models for collisions");
foreach (Joint j in ParentJoints)
{
if (!robot.ContinueExport)
return false;
j.Verify(log);
}
RobotInfo.WriteToLogFile("All Joints Verified");
foreach (Attachment att in attachments)
{
if (!robot.ContinueExport)
return false;
att.Verify(log);
}
RobotInfo.WriteToLogFile("All Attachments Verified");
return true;
}
/// <summary>
/// Verifies that everything is good in the attachment
/// </summary>
/// <param name="log">Logger to output messages to</param>
/// <returns>true if successfully verified</returns>
public override bool Verify(ProgressLogger log)
{
log.WriteMessage("Verifying " + Name);
if (Joint != null)
{
if (Joint.jointSpecifics is SingleAxisJoint)
{
if (MinLimit == MaxLimit)
log.WriteWarning("No range of movement defined in " + Name);
else if (MinLimit > MaxLimit)
log.WriteError("Minimum value must be less than maximum value in " + Name);
//check that units of switch limits match those of joint limits
if (Joint.jointSpecifics is RevoluteJoint) // for revolute joints, convert to radians and check limit ranges
{
if (units == 0) // if degrees, convert to radians
{
MinLimit = MinLimit * Math.PI / 180;
MaxLimit = MaxLimit * Math.PI / 180;
units = 1;
}
}
if (MinLimit < ((SingleAxisJoint)Joint.jointSpecifics).Axis1.LowerLimit)
log.WriteError("Minimum limit in " + Name + " is less than miniumum limit in joint " + Joint.Name);
if (MaxLimit > ((SingleAxisJoint)Joint.jointSpecifics).Axis1.UpperLimit)
log.WriteError("Maximum limit in " + Name + " is greater than maximum limit in joint " + Joint.Name);
}
else
{
log.WriteError(Name + " must be applied to a revolute or prismatic joint");
}
}
else
{
log.WriteError(Name + " must be associated with a joint.");
}
return true;
}
/// <summary>
/// Writes the SDF file for this joint
/// </summary>
/// <param name="log">The logger to use to write messages</param>
/// <param name="writer">The writer to use to write the sdf file</param>
public override void WriteJointSDF(ProgressLogger log,XmlWriter writer)
{
base.WriteJointSDF(log, writer);
string ParentName;
string ChildName = joint.Child.Name.Replace(" ", "_");
if (referenceJoint.Parent.Equals(joint.Parent))
ParentName = referenceJoint.Child.Name.Replace(" ", "_");
else
ParentName = referenceJoint.Parent.Name.Replace(" ", "_");
string jointName = ParentName + "-" + ChildName + "_gearbox";
writer.WriteStartElement("joint");
writer.WriteAttributeString("name", jointName);
writer.WriteAttributeString("type", "gearbox");
log.WriteMessage("Writing joint " + jointName + " to SDF.");
{
SDFExporter.writeSDFElement(writer, "parent", ParentName); //Parent Link
SDFExporter.writeSDFElement(writer, "child", ChildName); //Child Link
SDFExporter.writeSDFElement(writer, "gearbox_ratio", GearboxRatio.ToString());
SDFExporter.writeSDFElement(writer, "gearbox_reference_body", joint.Parent.Name.Replace(" ", "_"));
writer.WriteStartElement("axis");
{
SDFExporter.writeSDFElement(writer, "xyz", Axis1.AxisX + " " + Axis1.AxisY + " " + Axis1.AxisZ);
}
writer.WriteEndElement();
writer.WriteStartElement("axis2");
{
SDFExporter.writeSDFElement(writer, "xyz", Axis1.AxisX + " " + Axis1.AxisY + " " + Axis1.AxisZ);
}
writer.WriteEndElement();
}
writer.WriteEndElement();
}
/// <summary>
/// Exports the robot
/// </summary>
/// <param name="log"> Logger to be used to write messeges </param>
/// <param name="path"> File path to export robot to </param>
public override void Export(ProgressLogger log, String path)
{
this.log = log;
robotName = robot.Name.Replace(" ", "_");
try
{
if (path.Equals(System.Environment.ExpandEnvironmentVariables("%HOMEDRIVE%%HOMEPATH%") + "\\wpilib\\simulation"))
{
folderPath = path + "\\models\\" + robotName;
tempFilePath = folderPath + "\\";
MeshesPath = folderPath + "\\meshes\\";
RobotPath = folderPath + "\\robots\\";
if (Directory.Exists(folderPath))
{
ClearReadOnly(new DirectoryInfo(path));
Directory.Delete(folderPath, true);
}
Directory.CreateDirectory(folderPath);
Directory.CreateDirectory(MeshesPath);
Directory.CreateDirectory(RobotPath);
folderPath = path + "\\worlds";
if(File.Exists(folderPath+"\\"+robotName+".world"))
File.Delete(folderPath+"\\"+robotName+".world");
}
else
{
path += "\\" + robotName + "_Exported";
folderPath = path + "\\" + robotName;
tempFilePath = folderPath + "\\";
MeshesPath = folderPath + "\\meshes\\";
RobotPath = folderPath + "\\robots\\";
if (Directory.Exists(path))
{
ClearReadOnly(new DirectoryInfo(path));
Directory.Delete(path, true);
}
Directory.CreateDirectory(path);
Directory.CreateDirectory(folderPath);
Directory.CreateDirectory(MeshesPath);
Directory.CreateDirectory(RobotPath);
folderPath = path;
}
}
catch (IOException e)
{
log.WriteMessage("Failed to create directory. Another application may be accesing the file");
return;
}
try
{
log.WriteMessage("Setting up exporter", false);
//Export STLs
log.WriteMessage("Starting STL exports", false);
ModelDoc2 ActiveDoc = (ModelDoc2)asm;
//switches to the visual configuration and hides all components
Configuration currentConfig = ActiveDoc.ConfigurationManager.ActiveConfiguration;
//ActiveDoc.ShowConfiguration2(robot.VisualConfig);
STLExporter meshMaker = new STLExporter(iSwApp, asm);
Dictionary<string, List<ModelConfiguration>> VisualModels = new Dictionary<string, List<ModelConfiguration>>();
Dictionary<string, List<ModelConfiguration>> CollisionModels = new Dictionary<string, List<ModelConfiguration>>();
//Stopwatch watch = new Stopwatch();
//watch.Start();
//sort configurations
foreach (Link l in robot.GetLinksAsArray())
{
if(!l.VisualModel.EmptyModel)
if (VisualModels.ContainsKey(l.VisualModel.swConfiguration))
VisualModels[l.VisualModel.swConfiguration].Add(l.VisualModel);
else
{
List<ModelConfiguration> tempList = new List<ModelConfiguration>();
tempList.Add(l.VisualModel);
VisualModels.Add(l.VisualModel.swConfiguration, tempList);
}
if(!l.CollisionModel.EmptyModel)
if (CollisionModels.ContainsKey(l.CollisionModel.swConfiguration))
CollisionModels[l.CollisionModel.swConfiguration].Add(l.CollisionModel);
else
{
List<ModelConfiguration> tempList = new List<ModelConfiguration>();
tempList.Add(l.CollisionModel);
CollisionModels.Add(l.CollisionModel.swConfiguration, tempList);
}
}
//Debug.WriteLine("Finished sorting configs=" + watch.ElapsedMilliseconds);
string[] configs = VisualModels.Keys.Union(CollisionModels.Keys).ToArray();
//Export each config
foreach (string config in configs)
{
((ModelView)ActiveDoc.ActiveView).EnableGraphicsUpdate = false;
ActiveDoc.ShowConfiguration2(config);
//Debug.WriteLine("switched config " + config +"=" + watch.ElapsedMilliseconds);
ActiveDoc.ClearSelection2(true);
Component2[] hiddenComps = meshMaker.HideAllComponents();
//Debug.WriteLine("hid components=" + watch.ElapsedMilliseconds);
if(VisualModels.ContainsKey(config))
foreach (ModelConfiguration m in VisualModels[config])
{
Application.DoEvents();
if (!robot.ContinueExport)
{
meshMaker.UnhideComponents(hiddenComps);
meshMaker.close();
ActiveDoc.ShowConfiguration2(currentConfig.Name);
((ModelView)ActiveDoc.ActiveView).EnableGraphicsUpdate = true;
return;
}
((ModelView)ActiveDoc.ActiveView).EnableGraphicsUpdate = false;
meshMaker.ExportLink(m.Owner, m, MeshesPath + m.Owner.Name.Replace(" ", "_") + ".stl", log);
//Debug.WriteLine("exported visual=" + watch.ElapsedMilliseconds);
}
if(CollisionModels.ContainsKey(config))
foreach (ModelConfiguration m in CollisionModels[config])
{
Application.DoEvents();
if (!robot.ContinueExport)
{
meshMaker.UnhideComponents(hiddenComps);
meshMaker.close();
ActiveDoc.ShowConfiguration2(currentConfig.Name);
((ModelView)ActiveDoc.ActiveView).EnableGraphicsUpdate = true;
return;
}
((ModelView)ActiveDoc.ActiveView).EnableGraphicsUpdate = false;
meshMaker.ExportLink(m.Owner, m, MeshesPath + m.Owner.Name.Replace(" ", "_") + "_col.stl", log);
// Debug.WriteLine("exported col=" + watch.ElapsedMilliseconds);
}
meshMaker.UnhideComponents(hiddenComps);
//Debug.WriteLine("unhid comps=" + watch.ElapsedMilliseconds);
}
ActiveDoc.ShowConfiguration(currentConfig.Name);
meshMaker.close();
((ModelView)ActiveDoc.ActiveView).EnableGraphicsUpdate = true;
log.WriteMessage("Finished exporting STL files", false);
//Make SDF
log.WriteMessage("Creating SDF file", false);
XmlWriterSettings settings = new XmlWriterSettings();
settings.Encoding = new UTF8Encoding(false);
settings.Indent = true;
settings.NewLineOnAttributes = false;
XmlWriter SDFwriter = XmlWriter.Create(RobotPath+robotName+".sdf", settings);
SDFwriter.WriteStartDocument();
WriteSDF(SDFwriter);
SDFwriter.WriteEndDocument();
SDFwriter.Close();
log.WriteMessage("SDF file complete");
CreateConfigFile();
log.WriteMessage("Config file complete");
log.WriteMessage("Export Complete");
}
catch (Exception e)
{
log.WriteError("Unhandled Exception thrown when exporting robot: "+e.Message);
}
}
/// <summary>
/// Writes the SDF tags for the joint
/// </summary>
/// <param name="log">The logger to write messages to</param>
/// <param name="writer">the writer to use to write the sdf</param>
public virtual void WriteJointSDF(ProgressLogger log, XmlWriter writer)
{
string ParentName;
string ChildName = joint.Child.Name.Replace(" ", "_");
ParentName = joint.Parent.Name.Replace(" ", "_");
string jointName = joint.Name.Replace(" ", "_");
writer.WriteStartElement("joint");
writer.WriteAttributeString("name", jointName);
writer.WriteAttributeString("type", GetJointTypeName());
log.WriteMessage("Writing joint " + joint.Name + " to SDF.");
{
SDFExporter.writeSDFElement(writer, "parent", ParentName); //Parent Link
SDFExporter.writeSDFElement(writer, "child", ChildName); //Child Link
string jointpose = (OriginValues.X - joint.Child.OriginX) + " " + (OriginValues.Y - joint.Child.OriginY) + " " + (OriginValues.Z - joint.Child.OriginZ) + " " + OriginValues.Roll + " " + OriginValues.Pitch + " " + OriginValues.Yaw;
SDFExporter.writeSDFElement(writer, "pose", jointpose);
WriteSDF(log, writer);
}
writer.WriteEndElement();
}