/// <summary>
/// Writes a Link to the .SDF file
/// </summary>
/// <param name="link"> The Link to be written </param>
/// <param name="writer"> The writer to use to write the Link </param>
private void WriteLink(Link link, XmlWriter writer)
{
//Link data
String linkName = link.Name.Replace(" ", "_");
writer.WriteStartElement("link");
writer.WriteAttributeString("name", linkName);
log.WriteMessage("Writing link " + linkName + " to SDF.");
{
//gravity, self_collide, kinematic,
writeSDFElement(writer, "gravity", "1"); // 1 true
writeSDFElement(writer, "self_collide", link.SelfCollide ? "1" : "0"); // 0 false
double[] transPoint = link.GetTransformedCoordinate();
string linkpose =transPoint[0] + " " + transPoint[1] + " " + transPoint[2] + " " + link.OriginR + " " + link.OriginP + " " + link.OriginW;
writeSDFElement(writer, "pose", linkpose);
//if baselink true
if (link.isBaseLink)
writeSDFElement(writer, "must_be_base_link", "1");
else
writeSDFElement(writer, "must_be_base_link", "0");
writer.WriteStartElement("velocity_decay");
writeSDFElement(writer, "linear", link.LinearDamping.ToString());
writeSDFElement(writer, "angular", link.AngularDamping.ToString());
writer.WriteEndElement();
//Inertial data
writer.WriteStartElement("inertial");
{
//mass data
writeSDFElement(writer, "mass", link.Mass.ToString());
//pose data
string link_xyzrpy = (link.ComX-link.OriginX) + " " + (link.ComY-link.OriginY) + " " + (link.ComZ-link.OriginZ) + " 0 0 0";
/* link_xyzrpy = (link.ParentConnection != null) ?
((link.ComX - link.ParentConnection.OriginX) + " " + (link.ComY - link.ParentConnection.OriginY) + " " + (link.ComZ - link.ParentConnection.OriginZ)) :
(link.ComX + " " + link.ComY + " " + link.ComZ);
link_xyzrpy += link.OriginR + " " + link.OriginP + " " + link.OriginW;
*/ writeSDFElement(writer, "pose", link_xyzrpy);
//Moment data
writer.WriteStartElement("inertia");
writeSDFElement(writer, "ixx", link.MomentIxx.ToString());
writeSDFElement(writer, "ixy", link.MomentIxy.ToString());
writeSDFElement(writer, "ixz", link.MomentIxz.ToString());
writeSDFElement(writer, "iyy", link.MomentIyy.ToString());
writeSDFElement(writer, "iyz", link.MomentIyz.ToString());
writeSDFElement(writer, "izz", link.MomentIzz.ToString());
writer.WriteEndElement();
}
writer.WriteEndElement();
//Collision Data
if (!link.CollisionModel.EmptyModel)
{
writer.WriteStartElement("collision");
string collision = linkName + "_collision"; //////////multiple visuals????
writer.WriteAttributeString("name", collision);
{
//origin (pose)
string collisionpose = -link.OriginX + " " + -link.OriginY + " " + -link.OriginZ + " 0 0 0";
/* collisionpose = (link.ParentConnection != null) ?
(-link.ParentConnection.OriginX + " " + -link.ParentConnection.OriginY + " " + -link.ParentConnection.OriginZ) :
(link.OriginXcollision + " " + link.OriginYcollision + " " + link.OriginZcollision);
collisionpose += link.OriginRcollision + " " + link.OriginPcollision + " " + link.OriginWcollision;
*/
writeSDFElement(writer, "pose", collisionpose);
//mesh (collision geometry)
writer.WriteStartElement("geometry");
writer.WriteStartElement("mesh");
writeSDFElement(writer, "scale", "1 1 1");
string visualuri = "model://" + robotName + "/meshes/" + linkName + "_col.STL";
writeSDFElement(writer, "uri", visualuri);
writer.WriteEndElement();
writer.WriteEndElement();
//surface(s)
writer.WriteStartElement("surface");
{
writer.WriteStartElement("contact");
writer.WriteStartElement("ode");
if (link.Kp > 0)
writeSDFElement(writer, "kp", link.Kp.ToString());
if (link.Kd > 0)
writeSDFElement(writer, "kd", link.Kd.ToString());
//writeSDFElement(writer, "max_vel", );
//writeSDFElement(writer, "max_depth", link.Kp.ToString());
writer.WriteEndElement();
writer.WriteEndElement();
writer.WriteStartElement("friction");
writer.WriteStartElement("ode");
writeSDFElement(writer, "mu", link.Mu1.ToString());
writeSDFElement(writer, "mu2", link.Mu2.ToString());
//writeSDFElement(writer, "max_vel", );
//writeSDFElement(writer, "max_depth", link.Kp.ToString());
writer.WriteEndElement();
writer.WriteEndElement();
}
writer.WriteEndElement();
}
writer.WriteEndElement();
}
//Visual Data
writer.WriteStartElement("visual");
string visualname = linkName + "_visual"; //////////multiple visuals????
writer.WriteAttributeString("name", visualname);
{
//Visual pose
string visualpose = -link.OriginX + " " + -link.OriginY + " " + -link.OriginZ + " 0 0 0"; ;
/* visualpose = ( (link.ParentConnection != null) ? //check for base link as the baselink will not have an offset)
(-link.ParentConnection.OriginX + " " + -link.ParentConnection.OriginY + " " + -link.ParentConnection.OriginZ) :
(link.OriginXvisual + " " + link.OriginYvisual + " " + link.OriginZvisual));
visualpose += link.OriginRvisual + " " + link.OriginPvisual + " " + link.OriginWvisual;
*/
writeSDFElement(writer, "pose", visualpose);
//Color (material)
writer.WriteStartElement("material");
//writer.WriteStartElement("script");
//writeSDFElement(writer, "name", "Gazebo/" + link.color.ToString("X6"));
//writeSDFElement(writer, "uri", "__default__");
//writer.WriteEndElement();
writeSDFElement(writer, "ambient", link.ColorRed / 255.0 + " " + link.ColorBlue / 255.0 + " " + link.ColorGreen / 255.0 + " 1");
writeSDFElement(writer, "diffuse", link.ColorRed / 255.0 + " " + link.ColorBlue / 255.0 + " " + link.ColorGreen / 255.0 + " 1");
writer.WriteEndElement();
//Mesh (visual)
writer.WriteStartElement("geometry");
writer.WriteStartElement("mesh");
writeSDFElement(writer, "scale", "1 1 1");
string visualuri = "model://" + robotName + "/meshes/" + linkName + ".STL";
writeSDFElement(writer, "uri", visualuri);
writer.WriteEndElement();
writer.WriteEndElement();
}
writer.WriteEndElement();
foreach (Attachment att in link.GetAttachmentsAsArray())
{
att.WriteSensor(log, writer);
}
}
writer.WriteEndElement();
log.WriteMessage("Finished writing link " + link.Name + " to SDF.");
}
