Skip to content

Commit dcc0bee

Browse files
Rufus31415Rufus31415
committed
Implement demo for forward and inverse kinematics
1 parent 6d65043 commit dcc0bee

File tree

8 files changed

+739
-19
lines changed

8 files changed

+739
-19
lines changed

‎UnderAutomation.UniversalRobots.Showcase.Forms/Components/KinematicsControl.Designer.cs‎

Lines changed: 438 additions & 0 deletions
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

‎UnderAutomation.UniversalRobots.Showcase.Forms/Components/KinematicsControl.cs‎

Lines changed: 145 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,145 @@
1+
using UnderAutomation.UniversalRobots;
2+
using UnderAutomation.UniversalRobots.Common;
3+
using UnderAutomation.UniversalRobots.Kinematics;
4+
5+
public partial class KinematicsControl : UserControl, IUserControl
6+
{
7+
UR _ur;
8+
public KinematicsControl(UR ur)
9+
{
10+
_ur = ur;
11+
12+
InitializeComponent();
13+
14+
cbModele.Items.Clear();
15+
cbModele.Items.Add("");
16+
foreach (var model in Enum.GetNames<RobotModelsExtended>())
17+
{
18+
cbModele.Items.Add(model.ToString());
19+
}
20+
cbModele.SelectedIndex = 0;
21+
22+
gridJointsFK.SelectedObject = new double[] { 0, 0, 0, 0, 0, 0 };
23+
gridDH.SelectedObject = new CustomUrDhParameters(0, 0, 0, 0, 0, 0);
24+
gridCartesianIK.SelectedObject = new Pose(0, 0, 0, 0, 0, 0);
25+
26+
lstIKResults.Items.Clear();
27+
}
28+
29+
#region IUserControl
30+
public string Title => "Forward & Invert Kinematics";
31+
32+
public bool FeatureEnabled => true;
33+
34+
public void PeriodicUpdate()
35+
{
36+
btnImportDH.Enabled = cbModele.SelectedIndex > 0;
37+
BtnCopyIKSelectedResult.Enabled = lstIKResults.SelectedItems.Count > 0;
38+
39+
var jointData = _ur.PrimaryInterface.JointData;
40+
if (jointData is null) return;
41+
42+
gridCurrentJoints.SelectedObject = new double[] {
43+
jointData.Base.Position,
44+
jointData.Shoulder.Position,
45+
jointData.Elbow.Position,
46+
jointData.Wrist1.Position,
47+
_ur.PrimaryInterface.JointData.Wrist2.Position,
48+
jointData.Wrist3.Position,
49+
};
50+
51+
var configurationData = _ur.PrimaryInterface.ConfigurationData;
52+
if (configurationData is null) return;
53+
54+
gridCurrentDh.SelectedObject = new CustomUrDhParameters(configurationData);
55+
56+
var cartesianInfo = _ur.PrimaryInterface.CartesianInfo;
57+
if (cartesianInfo is null) return;
58+
59+
gridCurrentCartesian.SelectedObject = cartesianInfo.AsPose();
60+
}
61+
62+
public void OnClose() { }
63+
64+
public void OnOpen() { }
65+
#endregion
66+
67+
68+
private void btnCopyCurrentJoints_Click(object sender, EventArgs e)
69+
{
70+
var joints = gridCurrentJoints.SelectedObject as double[];
71+
var currentDh = gridCurrentDh.SelectedObject as IUrDhParameters;
72+
if (joints is null || currentDh is null) return;
73+
74+
gridJointsFK.SelectedObject = joints.ToArray();
75+
gridDH.SelectedObject = new CustomUrDhParameters(currentDh);
76+
}
77+
78+
private void btnCopyCurrentCartesian_Click(object sender, EventArgs e)
79+
{
80+
var pose = gridCurrentCartesian.SelectedObject as Pose;
81+
var currentDh = gridCurrentDh.SelectedObject as IUrDhParameters;
82+
if (pose is null || currentDh is null) return;
83+
gridCartesianIK.SelectedObject = new Pose(pose);
84+
gridDH.SelectedObject = new CustomUrDhParameters(currentDh);
85+
}
86+
87+
private void btnImportDH_Click(object sender, EventArgs e)
88+
{
89+
gridDH.SelectedObject = new CustomUrDhParameters(KinematicsUtils.GetDhParametersFromModel(Enum.Parse<RobotModelsExtended>(cbModele.SelectedItem.ToString())));
90+
}
91+
92+
private void btnCopyFKResult_Click(object sender, EventArgs e)
93+
{
94+
var result = gridFKResult.SelectedObject as Pose;
95+
if (result is null) return;
96+
gridCartesianIK.SelectedObject = new Pose(result);
97+
}
98+
99+
private void BtnCopyIKSelectedResult_Click(object sender, EventArgs e)
100+
{
101+
if (lstIKResults.SelectedItems.Count == 0) return;
102+
103+
var joints = lstIKResults.SelectedItems[0].Tag as double[];
104+
105+
if (joints is null) return;
106+
107+
gridJointsFK.SelectedObject = joints.ToArray();
108+
}
109+
110+
private void btnForwardKinematics_Click(object sender, EventArgs e)
111+
{
112+
var joints = gridJointsFK.SelectedObject as double[];
113+
var dh = gridDH.SelectedObject as IUrDhParameters;
114+
115+
if (joints is null || dh is null) return;
116+
117+
var fkResult = KinematicsUtils.ForwardKinematics(joints, dh);
118+
119+
gridFKResult.SelectedObject = Pose.From4x4MatrixToRotationVector(fkResult.ToolTransform);
120+
}
121+
122+
private void btnInvertKinematics_Click(object sender, EventArgs e)
123+
{
124+
var pose = gridCartesianIK.SelectedObject as Pose;
125+
var dh = gridDH.SelectedObject as IUrDhParameters;
126+
127+
if (pose is null || dh is null) return;
128+
129+
var matrix = pose.FromRotationVectorTo4x4Matrix();
130+
131+
var results = KinematicsUtils.InverseKinematics(matrix, dh);
132+
133+
lstIKResults.Items.Clear();
134+
for (int i = 0; i < results.Length; i++)
135+
{
136+
var result = results[i];
137+
var item = new ListViewItem();
138+
item.Text = (i + 1).ToString();
139+
foreach (var value in result) item.SubItems.Add(value.ToString("F4"));
140+
item.Tag = result;
141+
142+
lstIKResults.Items.Add(item);
143+
}
144+
}
145+
}

‎UnderAutomation.UniversalRobots.Showcase.Forms/Components/KinematicsControl.resx‎

Lines changed: 120 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,120 @@
1+
<?xml version="1.0" encoding="utf-8"?>
2+
<root>
3+
<!--
4+
Microsoft ResX Schema
5+
6+
Version 2.0
7+
8+
The primary goals of this format is to allow a simple XML format
9+
that is mostly human readable. The generation and parsing of the
10+
various data types are done through the TypeConverter classes
11+
associated with the data types.
12+
13+
Example:
14+
15+
... ado.net/XML headers & schema ...
16+
<resheader name="resmimetype">text/microsoft-resx</resheader>
17+
<resheader name="version">2.0</resheader>
18+
<resheader name="reader">System.Resources.ResXResourceReader, System.Windows.Forms, ...</resheader>
19+
<resheader name="writer">System.Resources.ResXResourceWriter, System.Windows.Forms, ...</resheader>
20+
<data name="Name1"><value>this is my long string</value><comment>this is a comment</comment></data>
21+
<data name="Color1" type="System.Drawing.Color, System.Drawing">Blue</data>
22+
<data name="Bitmap1" mimetype="application/x-microsoft.net.object.binary.base64">
23+
<value>[base64 mime encoded serialized .NET Framework object]</value>
24+
</data>
25+
<data name="Icon1" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">
26+
<value>[base64 mime encoded string representing a byte array form of the .NET Framework object]</value>
27+
<comment>This is a comment</comment>
28+
</data>
29+
30+
There are any number of "resheader" rows that contain simple
31+
name/value pairs.
32+
33+
Each data row contains a name, and value. The row also contains a
34+
type or mimetype. Type corresponds to a .NET class that support
35+
text/value conversion through the TypeConverter architecture.
36+
Classes that don't support this are serialized and stored with the
37+
mimetype set.
38+
39+
The mimetype is used for serialized objects, and tells the
40+
ResXResourceReader how to depersist the object. This is currently not
41+
extensible. For a given mimetype the value must be set accordingly:
42+
43+
Note - application/x-microsoft.net.object.binary.base64 is the format
44+
that the ResXResourceWriter will generate, however the reader can
45+
read any of the formats listed below.
46+
47+
mimetype: application/x-microsoft.net.object.binary.base64
48+
value : The object must be serialized with
49+
: System.Runtime.Serialization.Formatters.Binary.BinaryFormatter
50+
: and then encoded with base64 encoding.
51+
52+
mimetype: application/x-microsoft.net.object.soap.base64
53+
value : The object must be serialized with
54+
: System.Runtime.Serialization.Formatters.Soap.SoapFormatter
55+
: and then encoded with base64 encoding.
56+
57+
mimetype: application/x-microsoft.net.object.bytearray.base64
58+
value : The object must be serialized into a byte array
59+
: using a System.ComponentModel.TypeConverter
60+
: and then encoded with base64 encoding.
61+
-->
62+
<xsd:schema id="root" xmlns="" xmlns:xsd="http://www.w3.org/2001/XMLSchema" xmlns:msdata="urn:schemas-microsoft-com:xml-msdata">
63+
<xsd:import namespace="http://www.w3.org/XML/1998/namespace" />
64+
<xsd:element name="root" msdata:IsDataSet="true">
65+
<xsd:complexType>
66+
<xsd:choice maxOccurs="unbounded">
67+
<xsd:element name="metadata">
68+
<xsd:complexType>
69+
<xsd:sequence>
70+
<xsd:element name="value" type="xsd:string" minOccurs="0" />
71+
</xsd:sequence>
72+
<xsd:attribute name="name" use="required" type="xsd:string" />
73+
<xsd:attribute name="type" type="xsd:string" />
74+
<xsd:attribute name="mimetype" type="xsd:string" />
75+
<xsd:attribute ref="xml:space" />
76+
</xsd:complexType>
77+
</xsd:element>
78+
<xsd:element name="assembly">
79+
<xsd:complexType>
80+
<xsd:attribute name="alias" type="xsd:string" />
81+
<xsd:attribute name="name" type="xsd:string" />
82+
</xsd:complexType>
83+
</xsd:element>
84+
<xsd:element name="data">
85+
<xsd:complexType>
86+
<xsd:sequence>
87+
<xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
88+
<xsd:element name="comment" type="xsd:string" minOccurs="0" msdata:Ordinal="2" />
89+
</xsd:sequence>
90+
<xsd:attribute name="name" type="xsd:string" use="required" msdata:Ordinal="1" />
91+
<xsd:attribute name="type" type="xsd:string" msdata:Ordinal="3" />
92+
<xsd:attribute name="mimetype" type="xsd:string" msdata:Ordinal="4" />
93+
<xsd:attribute ref="xml:space" />
94+
</xsd:complexType>
95+
</xsd:element>
96+
<xsd:element name="resheader">
97+
<xsd:complexType>
98+
<xsd:sequence>
99+
<xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
100+
</xsd:sequence>
101+
<xsd:attribute name="name" type="xsd:string" use="required" />
102+
</xsd:complexType>
103+
</xsd:element>
104+
</xsd:choice>
105+
</xsd:complexType>
106+
</xsd:element>
107+
</xsd:schema>
108+
<resheader name="resmimetype">
109+
<value>text/microsoft-resx</value>
110+
</resheader>
111+
<resheader name="version">
112+
<value>2.0</value>
113+
</resheader>
114+
<resheader name="reader">
115+
<value>System.Resources.ResXResourceReader, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
116+
</resheader>
117+
<resheader name="writer">
118+
<value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
119+
</resheader>
120+
</root>

‎UnderAutomation.UniversalRobots.Showcase.Forms/MainForm.cs‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,7 @@ public MainForm()
3939
AddNode(new SocketControl(_ur));
4040
AddNode(new SftpControl(_ur));
4141
AddNode(new SshControl(_ur));
42+
AddNode(new KinematicsControl(_ur));
4243
AddNode(new ToolsControl());
4344
AddNode(new ArchiveControl(_ur));
4445
AddNode(new LicenseControl());

‎UnderAutomation.UniversalRobots.Showcase.Forms/Properties/Resources.Designer.cs‎

Lines changed: 10 additions & 0 deletions
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

‎UnderAutomation.UniversalRobots.Showcase.Forms/Properties/Resources.resx‎

Lines changed: 22 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -112,21 +112,18 @@
112112
<value>2.0</value>
113113
</resheader>
114114
<resheader name="reader">
115-
<value>System.Resources.ResXResourceReader, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
115+
<value>System.Resources.ResXResourceReader, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
116116
</resheader>
117117
<resheader name="writer">
118-
<value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
118+
<value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
119119
</resheader>
120-
<assembly alias="System.Windows.Forms" name="System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089" />
121-
<data name="delete-bin-5-line" type="System.Resources.ResXFileRef, System.Windows.Forms">
122-
<value>..\Resources\delete-bin-5-line.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
120+
<assembly alias="System.Windows.Forms" name="System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089" />
121+
<data name="ua" type="System.Resources.ResXFileRef, System.Windows.Forms">
122+
<value>..\Resources\ua.ico;System.Drawing.Icon, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
123123
</data>
124124
<data name="socket_sample" type="System.Resources.ResXFileRef, System.Windows.Forms">
125125
<value>..\Resources\socket_sample.JPG;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
126126
</data>
127-
<data name="refresh-line" type="System.Resources.ResXFileRef, System.Windows.Forms">
128-
<value>..\Resources\refresh-line.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
129-
</data>
130127
<data name="xml_rpc_sample" type="System.Resources.ResXFileRef, System.Windows.Forms">
131128
<value>..\Resources\xml_rpc_sample.jpg;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
132129
</data>
@@ -160,31 +157,37 @@
160157
<data name="warning" type="System.Resources.ResXFileRef, System.Windows.Forms">
161158
<value>..\Resources\warning.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
162159
</data>
163-
<data name="error" type="System.Resources.ResXFileRef, System.Windows.Forms">
164-
<value>..\Resources\error.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
160+
<data name="refresh-line" type="System.Resources.ResXFileRef, System.Windows.Forms">
161+
<value>..\Resources\refresh-line.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
165162
</data>
166-
<data name="folder-add-line" type="System.Resources.ResXFileRef, System.Windows.Forms">
167-
<value>..\Resources\folder-add-line.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
163+
<data name="delete-bin-5-line" type="System.Resources.ResXFileRef, System.Windows.Forms">
164+
<value>..\Resources\delete-bin-5-line.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
165+
</data>
166+
<data name="program" type="System.Resources.ResXFileRef, System.Windows.Forms">
167+
<value>..\Resources\program.urp;System.Byte[], mscorlib, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
168168
</data>
169169
<data name="upload-2-line" type="System.Resources.ResXFileRef, System.Windows.Forms">
170170
<value>..\Resources\upload-2-line.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
171171
</data>
172-
<data name="ua" type="System.Resources.ResXFileRef, System.Windows.Forms">
173-
<value>..\Resources\ua.ico;System.Drawing.Icon, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
172+
<data name="folder-add-line" type="System.Resources.ResXFileRef, System.Windows.Forms">
173+
<value>..\Resources\folder-add-line.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
174174
</data>
175-
<data name="program" type="System.Resources.ResXFileRef, System.Windows.Forms">
176-
<value>..\Resources\program.urp;System.Byte[], mscorlib, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
175+
<data name="pause" type="System.Resources.ResXFileRef, System.Windows.Forms">
176+
<value>..\Resources\pause.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
177+
</data>
178+
<data name="error" type="System.Resources.ResXFileRef, System.Windows.Forms">
179+
<value>..\Resources\error.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
177180
</data>
178181
<data name="UnderAutomation132" type="System.Resources.ResXFileRef, System.Windows.Forms">
179182
<value>..\Resources\UnderAutomation132.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
180183
</data>
181184
<data name="folder-shared-line" type="System.Resources.ResXFileRef, System.Windows.Forms">
182185
<value>..\Resources\folder-shared-line.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
183186
</data>
184-
<data name="pause" type="System.Resources.ResXFileRef, System.Windows.Forms">
185-
<value>..\Resources\pause.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
186-
</data>
187187
<data name="play" type="System.Resources.ResXFileRef, System.Windows.Forms">
188188
<value>..\Resources\play.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
189189
</data>
190+
<data name="ik_diagram_background.drawio" type="System.Resources.ResXFileRef, System.Windows.Forms">
191+
<value>..\Resources\ik_diagram_background.drawio.png;System.Drawing.Bitmap, System.Drawing, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
192+
</data>
190193
</root>

‎UnderAutomation.UniversalRobots.Showcase.Forms/Resources/ik_diagram_background.drawio.png‎

86.6 KB
Loading

‎UnderAutomation.UniversalRobots.Showcase.Forms/UnderAutomation.UniversalRobots.Showcase.Forms.csproj‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,9 @@
3737

3838

3939
<ItemGroup>
40+
<Compile Update="Components\KinematicsControl.cs">
41+
<SubType>UserControl</SubType>
42+
</Compile>
4043
<Compile Update="Properties\Resources.Designer.cs">
4144
<DesignTime>True</DesignTime>
4245
<AutoGen>True</AutoGen>

0 commit comments

Comments
 (0)