/// <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>
/// 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>
/// 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>
/// Calculates the mass and inertia values in the link and stores them
/// </summary>
/// <param name="log">The logger to write to. If null then nothing will be written</param>
/// <param name="changeConfig">default: config changed inside method, false declares config already changed outside</param>
/// <param name="overrideCustomInertial">default: if custom inertial declared, allows exception</param>
public void CalcInertia(ProgressLogger log, bool changeConfig = true, bool overrideCustomInertial = false)
{
if (UseCustomInertial && !overrideCustomInertial)
return;
string matDB = "";
string config = modelDoc.ConfigurationManager.ActiveConfiguration.Name;
bool compsMissingMat = false;
List<Body2> bodies = new List<Body2>();
object obj;
ModelConfiguration physConfig = PhysicalModel;
if (changeConfig)
modelDoc.ShowConfiguration(physConfig.swConfiguration);
foreach (modelComponent M in physConfig.LinkComponents)
{
if (M.Component == null)
continue;
Component2 c = M.Component;
if (c == null)
continue;
//modelDoc.ShowConfiguration2(M.ConfigName);
if (c.GetSuppression() != (int)swComponentSuppressionState_e.swComponentSuppressed)
{
//if (((object[])c.GetChildren()).Length > 0)
if (c.IGetChildrenCount() > 0)
{
GetSubBodies(bodies, c);
}
else
{
if (log != null)
{
string s = ((IPartDoc)c.GetModelDoc2()).GetMaterialPropertyName2(config, out matDB);
if (String.IsNullOrEmpty(s))
{
compsMissingMat = true;
}
}
if (c.GetBodies3((int)swBodyType_e.swSolidBody, out obj) != null)
{
foreach (Body2 b in c.GetBodies3((int)swBodyType_e.swSolidBody, out obj))
{
bodies.Add(b);
}
}
}
}
}
if (compsMissingMat && log!=null)
log.WriteWarning("Some components in link " + Name + " have no material assigned");
if (bodies.Count > 0)
{
MassProperty massProps = ((ModelDocExtension)modelDoc.Extension).CreateMassProperty();
massProps.AddBodies(bodies.ToArray());
ComX = massProps.CenterOfMass[0];
ComY = massProps.CenterOfMass[1];
ComZ = massProps.CenterOfMass[2];
Mass = massProps.Mass;
double[] moments = massProps.GetMomentOfInertia((int)swMassPropertyMoment_e.swMassPropertyMomentAboutCenterOfMass);
MomentIxx = moments[0];
MomentIxy = moments[1];
MomentIxz = moments[2];
MomentIyy = moments[4];
MomentIyz = moments[5];
MomentIzz = moments[8];
}
else
{
if (log != null)
log.WriteError("No solid bodies in link " + Name);
Mass = 0;
ComX = 0;
ComY = 0;
ComZ = 0;
MomentIxx = 0;
MomentIxy = 0;
MomentIxz = 0;
MomentIyy = 0;
MomentIyz = 0;
MomentIzz = 0;
}
OriginX = ComX;
OriginY = ComY;
OriginZ = ComZ;
OriginR = robot.OriginR;
OriginP = robot.OriginP;
OriginW = robot.OriginW;
if(changeConfig)
modelDoc.ShowConfiguration2(config);
}
/// <summary>
/// Verifyies the joint for export
/// </summary>
/// <param name="axisName">THe name of the axis, should be in form "jointName Axis x"</param>
/// <param name="log"></param>
public virtual void Verify(String axisName, ProgressLogger log)
{
CalcAxisVector();
RobotInfo.WriteToLogFile("Axis Vector Calculated");
CalcLimits(-1,null);
RobotInfo.WriteToLogFile("Limits Calculated");
if (Axis == null)
log.WriteError("No axis defined in joint " + axisName);
if (Friction == 0)
log.WriteWarning("No friction in joint " + axisName);
if (Damping == 0)
log.WriteWarning("No damping in joint " + axisName);
}
/// <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>
/// Verifies that the axis is ready for export
/// </summary>
/// <returns>Returns true if succesfully verified</returns>
public void Verify(Joint joint, ProgressLogger log)
{
CalcAxisVectors();
if (Axis == null)
log.WriteError("No axis " + AxisIndex + " defined in joint " + joint.Name);
if (!IsContinuous && LowerLimit == UpperLimit)
log.WriteWarning("No movement defined in joint " + joint.Name + " axis" + AxisIndex);
if (Friction == 0)
log.WriteWarning("No friction in joint " + joint.Name + " axis" + AxisIndex);
if (Damping == 0)
log.WriteError("No damping in joint " + joint.Name + " axis" + AxisIndex);
}
protected override bool VerifySpecifics(ProgressLogger logger)
{
if (GearboxRatio == 0)
logger.WriteError("No gear ratio defined in joint " + joint.Name);
return base.VerifySpecifics(logger);
}
/// <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);
}
}