/// <summary>
/// Creates a new joint editor proptery manager page that allows the user to edit joint properties.
/// </summary>
/// <param name="robot">Active robot model</param>
/// <param name="joint">Joint to be edited</param>
/// <param name="document">Active assembly document</param>
/// <param name="swApp">Solidworks application environment</param>
public JointPMPage(RobotModel robot, Joint joint, AssemblyDoc document, SldWorks swApp)
{
//Validate parameters
if (robot == null)
throw new ProgramErrorException("Tried to create a JointPMPage with a null robot model.");
if (document == null)
throw new ProgramErrorException("Tried to create a JointPMPage with a null assembly document.");
if (swApp == null)
throw new ProgramErrorException("Tried to create a JointPMPage with a null solidworks application interface.");
//Initialize fields
this.robot = robot;
this.swApp = swApp;
currentJoint = joint;
mathUtil = ((IMathUtility)swApp.GetMathUtility());
//AssemblyDoc inherits ModelDoc2 but the relationship dosn't carry through the COM interface
//Having two fields prevents having to cast half of the time
modelDoc = (ModelDoc2)document;
assemblyDoc = document;
//Setup the page, its controls and their visual layout
SetupPage();
SelectionObservers = new List<SelectionObserver>();
ButtonObservers = new List<ButtonObserver>();
CheckboxObservers = new List<CheckboxObserver>();
}
/// <summary>
/// Sets the current joint that is linked to this page
/// </summary>
/// <param name="j">joint to be used</param>
public void setJoint(Joint j)
{
currentJoint = j;
jointType.SelectedIndex = j.Type;
if ((currentJoint.Type == (int)Joint.JointType.Revolute
|| currentJoint.Type == (int)Joint.JointType.Revolute2
|| currentJoint.Type == (int)Joint.JointType.Gearbox) &&
(!currentJoint.UseCustomMovementLimits && !currentJoint.Axes[0].IsContinuous))
{
currentJoint.UseCustomMovementLimits = true;
}
NameLabel2.Text = j.Name;
if (j.Axes[0] != null)
{
DampingTextBox.Text = j.Axes[0].Damping.ToString();
FrictionTextBox.Text = j.Axes[0].Friction.ToString();
}
if (j.Axes[1] != null)
{
Damping2TextBox.Text = j.Axes[1].Damping.ToString();
Friction2TextBox.Text = j.Axes[1].Friction.ToString();
}
MotionCheckbox.Checked = currentJoint.UseCustomMovementLimits;
jointType_SelectedIndexChanged(null, null);
}
public JointAxis(SldWorks swApp, AssemblyDoc asm, StorageModel swData, string path, Joint current, int index, RobotModel robot)
{
this.swApp = swApp;
this.asmDoc = asm;
this.modelDoc = (ModelDoc2)asm;
this.swData = swData;
this.path = path;
this.owner = current;
this.robot = robot;
this.AxisIndex = index;
if (EffortLimit == 0)
{
this.EffortLimit = 1;
}
if (this.VelocityLimit == 0)
{
this.VelocityLimit = 1;
}
if (Axis != null)
{
CalcAxisVectors();
//CalcLimits(null);
}
if (index == 2)
{
IsContinuous = true;
}
}
public JointSpecificProperties(Joint j)
{
InitializeComponent();
joint = j;
JointTypeCombobox.Items.AddRange(JointFactory.GetTypesList());
RefreshBoxes();
}
public Revolute2Joint(Joint joint, string path)
: base(joint, path,
new IJointAxis[] { new RotationalJointAxis(path + "/axes/axis1", joint), new RotationalJointAxis(path + "/axes/axis2", joint) })
{
((RotationalJointAxis)Axes[1]).IsContinuous = true;
((RotationalJointAxis)Axes[0]).ArrowColor = RotationalJointAxis.DefaultArrow1Color;
((RotationalJointAxis)Axes[1]).ArrowColor = RotationalJointAxis.DefaultArrow2Color;
}
/// <summary>
/// Constructs a new double axis joint
/// </summary>
/// <param name="joint">The JOint that this joint specific is in</param>
/// <param name="path">The path to store data to</param>
/// <param name="axes">2 long array of the axes in this joint</param>
public DoubleAxisJoint(Joint joint, string path, IJointAxis[] axes)
: base(joint, path)
{
this.Axes = axes;
foreach (IJointAxis axis in Axes)
{
axis.CalcOriginHandler = new ReCalcOrigin(UpdateOriginPoint);
}
AxisSelBoxIDs = new int[2];
}
public RotationalJointAxis(string path, Joint current)
: base(path, current)
{
FlipVectors = new DoubleStorageArray(swData, path + "/FlipVectors");
if (FlipVectors.Count == 0)
{
for (int i = 0; i < 4; i++)
FlipVectors.AddItem(0);
}
}
/// <summary>
/// Sets the joint this attachment affects
/// </summary>
/// <param name="j">Joint that this attachment affects</param>
public void SetJoint(Joint j)
{
if (j.Parent.Equals(ChildLink))
{
ParentLinkID = j.Child.Id;
Joint = j;
}
else if (j.Child.Equals(ChildLink))
{
ParentLinkID = j.Parent.Id;
Joint = j;
}
}
/// <summary>
/// Creates a new Specific Joint object
/// </summary>
/// <param name="joint">The joint that this joint is in</param>
/// <param name="path">The storage path for this joint</param>
protected JointSpecifics(Joint joint, string path)
{
this.robot = RobotInfo.Robot;
this.joint = joint;
this.swApp = RobotInfo.SwApp;
this.modelDoc = RobotInfo.ModelDoc;
this.swData = RobotInfo.SwData;
this.path = path;
OriginValues = new OriginPoint();
if(OriginPt != null)
OriginValues.Point = GetPointFromEntity(OriginPt);
}
/// <summary>
/// Sets the current joint that is linked to this page
/// </summary>
/// <param name="j">joint to be used</param>
public void setJoint(Joint j)
{
currentJoint = j;
jointType.SelectedIndex = j.Type;
DampingTextBox.Text = j.Damping.ToString();
FrictionTextBox.Text = j.Friction.ToString();
MimicCheckbox.Checked = currentJoint.IsMimic;
MimicGroupBox.Enabled = MimicCheckbox.Checked;
MotionCheckbox.Checked = currentJoint.UseCustomMovementLimits;
jointType_SelectedIndexChanged(null, null);
}
/// <summary>
/// Gets a new jointSpecifics object
/// </summary>
/// <param name="type">String name of the joint type</param>
/// <param name="path">The path to store the new joint at</param>
/// <param name="joint">The joint that this specifics corresponds to</param>
/// <returns>The newly created jointSpecifics object</returns>
public static JointSpecifics GetSpecificJoint(string type, string path, Joint joint)
{
if (PluginSettings.DebugMode)
{
if (!jointTypes.Keys.Contains(type))
type = GetTypesList()[0];
}
Type t = jointTypes[type];
Type[] paramTypes = { typeof(Joint),typeof(string)};
object[] parameters = { joint, path };
JointSpecifics tempObj = (JointSpecifics) t.GetConstructor(paramTypes).Invoke(parameters);
tempObj.UpdateOriginPoint();
return tempObj;
}
/// <summary>
/// Constructs a new IJointAxis
/// </summary>
/// <param name="path">Path to this joint's storage</param>
/// <param name="current">The joint that this axis is contained in</param>
public IJointAxis(string path, Joint current)
{
this.swApp = RobotInfo.SwApp;
this.modelDoc = RobotInfo.ModelDoc;
this.swData = RobotInfo.SwData;
this.path = path;
this.owner = current;
this.robot = RobotInfo.Robot;
if (EffortLimit == 0)
{
this.EffortLimit = 1;
}
if (Axis != null)
{
CalcAxisVector();
}
mathUtil = (MathUtility)swApp.GetMathUtility();
}
/// <summary>
/// Writes a joint to the .URDF file
/// </summary>
/// <param name="joint"> The joint to be written </param>
/// <param name="writer"></param>
private void WriteJoint(Joint joint, XmlWriter writer)
{
//Joint Data
writer.WriteStartElement("joint");
writer.WriteAttributeString("name", joint.Name);
writer.WriteAttributeString("type", Joint.JointTypes[joint.Type]);
log.WriteMessage("Writing joint " + joint.Name+ " to URDF");
//Joint origin
writer.WriteStartElement("origin");
if (joint.Owner.Parent.ParentConnection != null)
{
writer.WriteAttributeString("xyz", (joint.OriginX - joint.Owner.Parent.ParentConnection.OriginX) + " " + (joint.OriginY - joint.Owner.Parent.ParentConnection.OriginY) + " " + (joint.OriginZ - joint.Owner.Parent.ParentConnection.OriginZ));
writer.WriteAttributeString("rpy", joint.OriginR + " " + joint.OriginP + " " + joint.OriginW);
}
else
{
writer.WriteAttributeString("xyz", joint.OriginX + " " + joint.OriginY + " " + joint.OriginZ);
writer.WriteAttributeString("rpy", joint.OriginR + " " + joint.OriginP + " " + joint.OriginW);
}
writer.WriteEndElement();
//Parent Link
writer.WriteStartElement("parent");
writer.WriteAttributeString("link", joint.Owner.Parent.Name);
writer.WriteEndElement();
//Child Link
writer.WriteStartElement("child");
writer.WriteAttributeString("link", joint.Owner.Name);
writer.WriteEndElement();
//Axis
writer.WriteStartElement("axis");
writer.WriteAttributeString("xyz", joint.AxisX.ToString() + " " + joint.AxisY.ToString() + " " + joint.AxisZ);
writer.WriteEndElement();
//dynamics
writer.WriteStartElement("dynamics");
writer.WriteAttributeString("damping", joint.Damping.ToString());
writer.WriteAttributeString("friction", joint.Friction.ToString());
writer.WriteEndElement();
//Limits if needed
if (joint.Type == 3 || joint.Type == 4)
{
writer.WriteStartElement("limit");
writer.WriteAttributeString("upper", joint.UpperLimit.ToString());
writer.WriteAttributeString("lower", joint.LowerLimit.ToString());
writer.WriteAttributeString("effort", joint.EffortLimit.ToString());
writer.WriteAttributeString("velocity", joint.VelocityLimit.ToString());
writer.WriteEndElement();
}
//mimic if needed
if (joint.IsMimic)
{
writer.WriteStartElement("mimic");
writer.WriteAttributeString("joint", joint.MimicJoint);
writer.WriteAttributeString("multiplier", joint.MimicMultiplier.ToString());
writer.WriteAttributeString("offset", joint.MimicOffset.ToString());
writer.WriteEndElement();
}
writer.WriteEndElement();
log.WriteMessage("Finished writing joint " + joint.Name + " to URDF");
}
/// <summary>
/// Writes a joint to the .SDF file
/// </summary>
/// <param name="joint"> The joint to be written </param>
/// <param name="writer"></param>
private void WriteJoint(Joint joint, XmlWriter writer)
{
joint.WriteSDF(log, writer);
}
/// <summary>
/// Creates a new Universal joint
/// </summary>
/// <param name="joint">joint that contains this joint specifics</param>
/// <param name="path">The path to store this joint at</param>
public UniversalJoint(Joint joint, string path)
: base(joint, path)
{
((RotationalJointAxis)Axes[0]).IsContinuous = true;
}
/// <summary>
/// Constructs a new gearbox joint
/// </summary>
/// <param name="joint">The joint that contains this jointSpecific</param>
/// <param name="path">The path to store this joint at</param>
public GearboxJoint(Joint joint, string path)
: base(joint, path)
{
((RotationalJointAxis)Axis1).IsContinuous = true;
}
/// <summary>
/// Constructs a new single axis joint
/// </summary>
/// <param name="joint">The joint that this joint is in</param>
/// <param name="path">The storage path for this joint</param>
/// <param name="axis">The IJointAxis that defines this joint's DOF</param>
public SingleAxisJoint(Joint joint, string path, IJointAxis axis)
: base(joint,path)
{
this.Axis1 = axis;
axis.CalcOriginHandler = new ReCalcOrigin(UpdateOriginPoint);
}
/// <summary>
/// Constructs a new Screw Joint axis
/// </summary>
/// <param name="path">The path to store this axis at</param>
/// <param name="current">The joint that this axis is contained in</param>
public ScrewJointAxis(string path, Joint current)
: base(path, current)
{
}
/// <summary>
/// Writes an attachment of a Joint to the .SDF file
/// </summary>
/// <param name="att"> Attachment to be written </param>
/// <param name="parentJoint"> The joint that contains this attachment </param>
/// <param name="writer"> The writer that will be used to write the URDF</param>
private void WriteAttachmentSDF(Attachment att, Joint parentJoint, XmlWriter writer)
{
att.WriteElements(log, writer);
log.WriteMessage("Finished writing attachment " + att.Name + " to SDF.");
}
/// <summary>
/// Constructs a new Prismatic joint
/// </summary>
/// <param name="joint">The joint that this joint is in</param>
/// <param name="path">The storage path for this joint</param>
public PrismaticJoint(Joint joint, string path)
: base(joint, path,new LinearJointAxis(path+"/axes/axis1",joint))
{
}
/// <summary>
/// Constructs a new Screw joint
/// </summary>
/// <param name="joint">The joint that this joint is in</param>
/// <param name="path">The storage path for this joint</param>
public ScrewJoint(Joint joint, string path)
: base(joint, path, new ScrewJointAxis(path+ "/axes/axis1",joint))
{
}
/// <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);
}
/// <summary>
/// Removes the selected joint from the link. Should only be called from the Joint class. Call joint.Delete() instead to delete a joint
/// </summary>
/// <param name="j">Joint to be removed</param>
public void RemoveJoint(Joint j)
{
///Remove all attachments from this joint
if (ParentJoints.Contains(j))
{
ParentJoints.Remove(j);
parentJointNums.RemoveItem(Convert.ToInt32(j.path.Substring(j.path.Length - 1)));
}
else if (ChildJoints.Contains(j))
{
ChildJoints.Remove(j);
}
if (ParentJoints.Count == 0 || !ConnectedToBaseLink())
this.Delete();
}
/// <summary>
/// Creates a new Axial Joint
/// </summary>
/// <param name="joint">The joint that this joint is in</param>
/// <param name="path">The storage path for this joint</param>
public AxialJoint(Joint joint, string path)
: base(joint,path)
{
this.matUtil = (MathUtility)swApp.GetMathUtility();
}
/// <summary>
/// Constructs a new Prismatic joint
/// </summary>
/// <param name="joint">The joint that this joint is in</param>
/// <param name="path">The storage path for this joint</param>
public RevoluteJoint(Joint joint, string path)
: base(joint, path,new RotationalJointAxis(path+"/axes/axis1",joint))
{
((RotationalJointAxis)Axis1).ArrowColor = RotationalJointAxis.DefaultArrow1Color;
}
/// <summary>
/// draws the node on the graph panel
/// </summary>
/// <param name="g">The Graphics object that will be used for drawing</param>
/// <param name="topCenter"></param>
/// <param name="selectionAreas">Dictionary of all selection areas</param>
/// <param name="dropZones">Dictionary of all attachment drop zones</param>
public void DrawNode(System.Drawing.Graphics g, int xOffset,int yOffset, Dictionary<Rectangle, ISelectable> selectionAreas, Dictionary<Rectangle, Link> dropZones)
{
selectedOutline.DashStyle = DashStyle.Dash;
Font font = new Font("Times New Roman", 12);
StringFormat formater = new StringFormat();
formater.Alignment = StringAlignment.Center;
formater.LineAlignment = StringAlignment.Center;
//Draw this link
Point topCenter = new Point (XPos-xOffset,GraphLevel*linkBoxHeight + (GraphLevel+1)*VerticalSpacing-yOffset);
Point topLeft = new Point((int)(topCenter.X-GetWidth()/2),topCenter.Y);
Pen linkPen, jointPen;
if (StoredLink == null)//draw temporary node
{
g.DrawLine(connectingLines,topCenter, new Point(topCenter.X,topCenter.Y+linkBoxHeight));
Point lineStartPoint = topCenter;
Point lineEndPoint;
GraphNode parent = ParentNodes.ElementAt(0);
if (parent.IsFakeNode)
lineEndPoint = new Point(parent.XPos - xOffset, topCenter.Y - VerticalSpacing);
else
lineEndPoint = new Point(parent.XPos - xOffset, topCenter.Y - VerticalSpacing + 4);
if (parent.FlippedNodes.Contains(this) && !parent.IsFakeNode)
{
connectingLines.CustomEndCap = endCap;
}
g.DrawLine(connectingLines, lineStartPoint, lineEndPoint);
connectingLines.EndCap = LineCap.NoAnchor;
return;
}
Rectangle link;
int linkWidth = MinLinkBoxWidth;
if (MinJointBoxWidth * ParentNodes.Count > linkWidth)
{
linkWidth = MinJointBoxWidth * ParentNodes.Count;
}
link = new Rectangle((int)(topCenter.X - GetWidth() / 2), topCenter.Y, linkWidth, linkBoxHeight + 4);
dropZones.Add(link, StoredLink);
linkPen = new Pen(Color.FromArgb(StoredLink.ColorRed, StoredLink.ColorGreen, StoredLink.ColorBlue), 4); //linkOutline;
if (StoredLink.Selected)
linkPen.DashStyle = selectedOutline.DashStyle;
if (ParentNodes.Count > 0)
{
link = new Rectangle((int)(topCenter.X - GetWidth() / 2), (int)topCenter.Y + jointNameHeight + 4, linkWidth, linkNameHeight);
int jointWidth = linkWidth / ParentNodes.Count;
List<GraphNode> ordererdNodes = new List<GraphNode>();
foreach (GraphNode node in ParentNodes)
{
bool inserted = false;
for (int i = 0; i < ordererdNodes.Count; i++)
{
if (node.LevelIndex < ordererdNodes[i].LevelIndex)
{
ordererdNodes.Insert(i, node);
inserted = true;
break;
}
}
if (!inserted)
{
ordererdNodes.Add(node);
}
}
Joint[] joints = new Joint[ordererdNodes.Count];
for (int i = 0; i < ordererdNodes.Count; i++)
{
Link parent = ordererdNodes[i].TraceNodes(true).StoredLink;
joints[i] = StoredLink.GetJointFromLink(parent);
}
StoredLink.UpperJoints = joints;
for (int i = 0; i < joints.Length; i++)
{
Rectangle jointRect = new Rectangle(topLeft.X + jointWidth * i, topLeft.Y, jointWidth, jointNameHeight);
if (joints[i].Selected)
jointPen = selectedOutline;
else
jointPen = jointOutline;
selectionAreas.Add(jointRect, joints[i]);
g.DrawRectangle(jointPen, jointRect);
g.DrawString(joints[i].Type, font, Brushes.Black, jointRect, formater);
Point lineStartPoint = new Point((int)(topLeft.X + jointWidth * (i + .5)), topLeft.Y);
Point lineEndPoint;
if (ordererdNodes[i].IsFakeNode)
lineEndPoint = new Point(ordererdNodes[i].XPos-xOffset, topCenter.Y - VerticalSpacing);
else
lineEndPoint = new Point(ordererdNodes[i].XPos-xOffset, topCenter.Y - VerticalSpacing+4);
if (!ordererdNodes[i].FlippedNodes.Contains(this))
{
connectingLines.CustomStartCap = endCap;
}
else if (!ordererdNodes[i].IsFakeNode)
{
connectingLines.CustomEndCap = endCap;
}
g.DrawLine(connectingLines, lineStartPoint, lineEndPoint);
connectingLines.StartCap = LineCap.NoAnchor;
connectingLines.EndCap = LineCap.NoAnchor;
}
}
g.DrawRectangle(linkPen, link);
g.DrawString(this.StoredLink.Name, font, Brushes.Black, link, formater);
selectionAreas.Add(link, StoredLink);
Rectangle att;
for (int l = 0; l < StoredLink.GetNumAttachments(); l++)
{
att = new Rectangle((int)(2 + link.X + linkWidth + attachmentBoxDim * (int)(l / numVerticalAttBoxes)), (int)topCenter.Y + attachmentBoxDim * (int)(l % numVerticalAttBoxes), attachmentBoxDim, attachmentBoxDim);
System.Diagnostics.Debug.WriteLine(StoredLink.GetAttachment(l));
StoredLink.GetAttachment(l).DrawOnGraph(g, att, StoredLink.GetAttachment(l).Selected);
selectionAreas.Add(att, StoredLink.GetAttachment(l));
}
}
/// <summary>
/// Constructs a new Linear Axis
/// </summary>
/// <param name="path">Path to this joint's storage</param>
/// <param name="current">The joint that this axis is contained in</param>
public LinearJointAxis( string path, Joint current)
: base(path, current)
{
}
/// <summary>
/// Adds to this link's list of parent joints and joint ids and incremenets nextJointNum
/// </summary>
/// <param name="parent">parent link of the new joint</param>
/// <returns>newly created joint</returns>
public Joint AddParentJoint(Link parent)
{
RobotInfo.WriteToLogFile("Creating new joint (Link)");
Joint newJoint = new Joint(path + "/joint/" + nextJointNum, parent, this);
RobotInfo.WriteToLogFile("Adding joint to parent list (Link)");
ParentJoints.Add(newJoint);
parentJointNums.AddItem(nextJointNum);
nextJointNum++;
return newJoint;
}
/// <summary>
/// Creates a new ball joint
/// </summary>
/// <param name="joint">The joint that contains this</param>
/// <param name="path">the storage path for this jointspecifics</param>
public BallJoint(Joint joint, string path)
: base(joint, path)
{
}
