diff --git a/BurtSharp.Sandbox/BurtActivityTest/BurtActivityTest.cs b/BurtSharp.Sandbox/BurtActivityTest/BurtActivityTest.cs new file mode 100644 index 0000000000000000000000000000000000000000..0835a969bb87c37b0906e649b977bafea3d89a9b --- /dev/null +++ b/BurtSharp.Sandbox/BurtActivityTest/BurtActivityTest.cs @@ -0,0 +1,79 @@ +using System; +using System.Threading; +using BurtSharp.Control; +using MathNet.Numerics.LinearAlgebra; + +namespace BurtSharp.Sandbox { + + class BurtActivityTest { + + public BurtActivityTest() { + + // This part of the code is meant to mimic the initialization of the + // RobotControllerSwitchable that happens inside the RobotConnection + // class in BurtSharpUnity at the start of activities. + _transCoordsRobotToUnity = Matrix<float>.Build.DenseOfArray(new float[,] { + { 0, -1, 0, 0 }, // Unity x is robot -y, no translation + { 0, 0, 1, 0 }, // Unity y is robot z, no translation + { 1, 0, 0, 0 }, // Unity z is robot x, no translation + { 0, 0, 0, 1 } + }); + _transCoordsUnityToRobot = _transCoordsRobotToUnity.Inverse(); + + _robot = new RobotControllerSwitchable(null, false, openConn: false, + autoUnsubscribe: false) { + HasDisturbanceObserver = true, + PrintDebug = false + }; + _robot.OpenConnection(false, subscribeToServerUpdate: false); + _robot.SetTransform(_transCoordsRobotToUnity); + _robot.SetTeneoTransform(_teneoDirection); + _robot.StartControlLoop(); + + // Set up keyboard callbacks + _keyboardManager = new KeyboardManager(); + _keyboardManager.AddKeyPressCallback("e", _robot.Enable); + _keyboardManager.AddKeyPressCallback("d", _robot.Disable); + _keyboardManager.SetQuitKey("q"); + + // Display most recent info and check for new key presses. + PrintUsage(); + bool isRunning = true; + while (isRunning) { + isRunning = _keyboardManager.ReadKeyPress(); + Thread.Sleep(50); + } + + // Always call Dispose() before exiting the program to properly + // disconnect from the robot. + _robot.Dispose(); + Environment.Exit(0); + } + + + private KeyboardManager _keyboardManager; + private RobotControllerSwitchable _robot; + + // Transformation matrices from robot coordinate frame to Unity coordinate + // frame, defined below in Awake(). + private Matrix<float> _transCoordsRobotToUnity; + private Matrix<float> _transCoordsUnityToRobot; + + // Teneo direction is mirrored so that clockwise is positive from the user perspective + private float _teneoDirection = -1; + + /// <summary> + /// Prints the usage instructions. + /// </summary> + private void PrintUsage() { + Console.WriteLine("\nBurt Activity Test:"); + Console.WriteLine(" [E]nable"); + Console.WriteLine(" [D]isable"); + Console.WriteLine(" [Q]uit"); + } + + public static void Main(string[] args) { + _ = new BurtActivityTest(); + } + } +} diff --git a/BurtSharp.Sandbox/BurtActivityTest/BurtActivityTest.csproj b/BurtSharp.Sandbox/BurtActivityTest/BurtActivityTest.csproj new file mode 100644 index 0000000000000000000000000000000000000000..fea6843a455841107d5a6b4c055239e8e39ae9ee --- /dev/null +++ b/BurtSharp.Sandbox/BurtActivityTest/BurtActivityTest.csproj @@ -0,0 +1,65 @@ +<?xml version="1.0" encoding="utf-8"?> +<Project DefaultTargets="Build" ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> + <PropertyGroup> + <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration> + <Platform Condition=" '$(Platform)' == '' ">x86</Platform> + <ProductVersion>8.0.30703</ProductVersion> + <SchemaVersion>2.0</SchemaVersion> + <ProjectGuid>{631483F0-7E88-4428-B113-1F105FB265F4}</ProjectGuid> + <OutputType>Exe</OutputType> + <RootNamespace>BurtActivityTest</RootNamespace> + <AssemblyName>BurtActivityTest</AssemblyName> + <TargetFrameworkVersion>v4.7</TargetFrameworkVersion> + </PropertyGroup> + <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|x86' "> + <DebugSymbols>true</DebugSymbols> + <DebugType>full</DebugType> + <Optimize>false</Optimize> + <OutputPath>bin\Debug</OutputPath> + <DefineConstants>DEBUG;</DefineConstants> + <ErrorReport>prompt</ErrorReport> + <WarningLevel>4</WarningLevel> + <ExternalConsole>true</ExternalConsole> + <PlatformTarget>x86</PlatformTarget> + </PropertyGroup> + <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|x86' "> + <Optimize>true</Optimize> + <OutputPath>bin\Release</OutputPath> + <ErrorReport>prompt</ErrorReport> + <WarningLevel>4</WarningLevel> + <ExternalConsole>true</ExternalConsole> + <PlatformTarget>x86</PlatformTarget> + </PropertyGroup> + <ItemGroup> + <Reference Include="System" /> + <Reference Include="MathNet.Numerics"> + <HintPath>..\..\dll\MathNet.Numerics.dll</HintPath> + </Reference> + <Reference Include="coapsharp-barrett"> + <HintPath>..\..\dll\coapsharp-barrett.dll</HintPath> + </Reference> + <Reference Include="RSG.Promise"> + <HintPath>..\..\dll\RSG.Promise.1.2.0.0\lib\net35\RSG.Promise.dll</HintPath> + </Reference> + <Reference Include="MsgPack"> + <HintPath>..\..\dll\MsgPack.dll</HintPath> + </Reference> + <Reference Include="UnityEngine"> + <HintPath>..\..\dll\UnityEngine.dll</HintPath> + </Reference> + <Reference Include="Newtonsoft.Json"> + <HintPath>..\..\dll\Newtonsoft.Json.dll</HintPath> + </Reference> + </ItemGroup> + <ItemGroup> + <Compile Include="BurtActivityTest.cs" /> + <Compile Include="Properties\AssemblyInfo.cs" /> + </ItemGroup> + <ItemGroup> + <ProjectReference Include="..\..\BurtSharp\BurtSharp.csproj"> + <Project>{F43DBD53-0249-4151-B1C0-3CD52ABD4D66}</Project> + <Name>BurtSharp</Name> + </ProjectReference> + </ItemGroup> + <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" /> +</Project> \ No newline at end of file diff --git a/BurtSharp.Sandbox/BurtActivityTest/Properties/AssemblyInfo.cs b/BurtSharp.Sandbox/BurtActivityTest/Properties/AssemblyInfo.cs new file mode 100644 index 0000000000000000000000000000000000000000..28dd378afd20941b75577523d5ddffdc6d5240be --- /dev/null +++ b/BurtSharp.Sandbox/BurtActivityTest/Properties/AssemblyInfo.cs @@ -0,0 +1,26 @@ +using System.Reflection; +using System.Runtime.CompilerServices; + +// Information about this assembly is defined by the following attributes. +// Change them to the values specific to your project. + +[assembly: AssemblyTitle("BurtActivityTest")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("")] +[assembly: AssemblyCopyright("${AuthorCopyright}")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// The assembly version has the format "{Major}.{Minor}.{Build}.{Revision}". +// The form "{Major}.{Minor}.*" will automatically update the build and revision, +// and "{Major}.{Minor}.{Build}.*" will update just the revision. + +[assembly: AssemblyVersion("1.0.*")] + +// The following attributes are used to specify the signing key for the assembly, +// if desired. See the Mono documentation for more information about signing. + +//[assembly: AssemblyDelaySign(false)] +//[assembly: AssemblyKeyFile("")] diff --git a/BurtSharp/Control/RobotAccelerationEstimator.cs b/BurtSharp/Control/RobotAccelerationEstimator.cs index 66b7a38cea8d12e8ee519ce19d7f1ae60214b626..ef35a4fa7d586958d7385ac6bbda23e27295c0bc 100644 --- a/BurtSharp/Control/RobotAccelerationEstimator.cs +++ b/BurtSharp/Control/RobotAccelerationEstimator.cs @@ -13,7 +13,7 @@ namespace BurtSharp.Control { public float FilterCutoff_Hz { get { return _filterDesignAcc.CutoffFrequency_Hz; } set { - _filterDesignAcc.CutoffFrequency_Hz = value; + _filterDesignAcc.CutoffFrequency_Hz = System.Math.Max(0, value); Reset(); } } diff --git a/BurtSharp/Control/RobotController.cs b/BurtSharp/Control/RobotController.cs index 88a436197076213ca342b3b805331f75c1ae71c7..df3fafc58e798ffb3a2d65f2fc54322f9fbede29 100644 --- a/BurtSharp/Control/RobotController.cs +++ b/BurtSharp/Control/RobotController.cs @@ -1115,12 +1115,19 @@ namespace BurtSharp.Control { if (IsOpen()) { // Get control parameters from robot needed to start control loop. - if (!_dynCfgManual) { + if (!_dynCfgManual && _client.HasUfeFeatures) { GetRobotDynamicsConfig(); } if (HasDynamics) { - _dynamics = new Dynamics(_dynCfg, _dynFwdKinCfg, _dynFwdKinDof, - _dynDof); + if (!_client.HasUfeFeatures) { + HasDynamics = false; + SystemLogger.Warning("HasDynamics property has been" + + "automatically set to false because firmware version does" + + "not support this feature."); + } else { + _dynamics = new Dynamics(_dynCfg, _dynFwdKinCfg, _dynFwdKinDof, + _dynDof); + } } if (HasDisturbanceObserver) { if (!_client.HasUfeFeatures) { diff --git a/BurtSharp/Properties/AssemblyInfo.cs b/BurtSharp/Properties/AssemblyInfo.cs index 72429f77af7f1ccfc63d042ee72b828447b59e97..01a5e341d67cdac7521ae735f132c8480c06aa91 100644 --- a/BurtSharp/Properties/AssemblyInfo.cs +++ b/BurtSharp/Properties/AssemblyInfo.cs @@ -14,7 +14,7 @@ using System.Runtime.CompilerServices; // The assembly version has the format "{Major}.{Minor}.{Build}.{Revision}". // The form "{Major}.{Minor}.*" will automatically update the build and revision, // and "{Major}.{Minor}.{Build}.*" will update just the revision. -[assembly: AssemblyVersion ("7.12.3")] +[assembly: AssemblyVersion ("7.12.4")] // The following attributes are used to specify the signing key for the assembly, // if desired. See the Mono documentation for more information about signing. //[assembly: AssemblyDelaySign(false)] diff --git a/CHANGELOG.md b/CHANGELOG.md index 2366d8dbf429e51ee21d9a035bac68032d545f2e..1e716b2dcfb2c91504b81ab7af874a1770cfe6c3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,13 @@ All notable changes to this project will be documented in this file. This project adheres to [Semantic Versioning](http://semver.org/). +## [dev-7.12.4] + +### Changed + +- Fixed bug in RobotController that made it incompatible with previous versions + of firmware that did not have dynamics parameters. + ## [dev-7.12.3] ### Changed @@ -976,3 +983,4 @@ This project adheres to [Semantic Versioning](http://semver.org/). [dev-7.12.1]: https://git.barrett.com/burt/software/burt-sharp/-/tags/dev-7.12.1 [dev-7.12.2]: https://git.barrett.com/burt/software/burt-sharp/-/tags/dev-7.12.2 [dev-7.12.3]: https://git.barrett.com/burt/software/burt-sharp/-/tags/dev-7.12.3 +[dev-7.12.4]: https://git.barrett.com/burt/software/burt-sharp/-/tags/dev-7.12.4 diff --git a/burt-sharp.sln b/burt-sharp.sln index 6b9710fa63923a0973c4ae7d956ebc71bc767869..e9a9d64f68b135ab3456193096500b4cd2192dec 100644 --- a/burt-sharp.sln +++ b/burt-sharp.sln @@ -115,6 +115,8 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "DebugCoapMessage", "BurtSha EndProject Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "SoftHoldTest", "BurtSharp.Sandbox\SoftHoldTest\SoftHoldTest.csproj", "{50BD7276-F7AF-40D5-B831-DE078EBDE84E}" EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "BurtActivityTest", "BurtSharp.Sandbox\BurtActivityTest\BurtActivityTest.csproj", "{631483F0-7E88-4428-B113-1F105FB265F4}" +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x86 = Debug|x86 @@ -452,6 +454,12 @@ Global {50BD7276-F7AF-40D5-B831-DE078EBDE84E}.Release|x86.Build.0 = Release|x86 {50BD7276-F7AF-40D5-B831-DE078EBDE84E}.Default|Any CPU.ActiveCfg = Debug|x86 {50BD7276-F7AF-40D5-B831-DE078EBDE84E}.Default|Any CPU.Build.0 = Debug|x86 + {631483F0-7E88-4428-B113-1F105FB265F4}.Debug|x86.ActiveCfg = Debug|x86 + {631483F0-7E88-4428-B113-1F105FB265F4}.Debug|x86.Build.0 = Debug|x86 + {631483F0-7E88-4428-B113-1F105FB265F4}.Release|x86.ActiveCfg = Release|x86 + {631483F0-7E88-4428-B113-1F105FB265F4}.Release|x86.Build.0 = Release|x86 + {631483F0-7E88-4428-B113-1F105FB265F4}.Default|Any CPU.ActiveCfg = Debug|x86 + {631483F0-7E88-4428-B113-1F105FB265F4}.Default|Any CPU.Build.0 = Debug|x86 EndGlobalSection GlobalSection(NestedProjects) = preSolution {B0F8FC1E-7D8D-4C4B-AC7A-59DEBDA4604A} = {6E619A31-7149-4E82-988A-188D9D113343} @@ -505,6 +513,7 @@ Global {92552B7D-1DBA-4C83-9AE3-AEF3619F89C2} = {75A3BFAD-B2CC-4723-B358-427803148B17} {C4630DB4-D544-42D9-A7EB-BEF00FECE9D1} = {75A3BFAD-B2CC-4723-B358-427803148B17} {50BD7276-F7AF-40D5-B831-DE078EBDE84E} = {75A3BFAD-B2CC-4723-B358-427803148B17} + {631483F0-7E88-4428-B113-1F105FB265F4} = {75A3BFAD-B2CC-4723-B358-427803148B17} EndGlobalSection GlobalSection(MonoDevelopProperties) = preSolution Policies = $0