/// <summary>
/// Simple function to update the size of the cylinder which visualizes the current separation distance.
/// </summary>
/// <param name="robot"></param>The robot for which the update should be made.
private void UpdateVisualizationDistance(IRobot robot)
{
if (robot != null && robot.IsValid && app.World.FindComponent("SeparationVisualization_" + robot.Name) != null)
{
ISimComponent comp = app.World.FindComponent("SeparationVisualization_" + robot.Name);
ISimNode node = robot.Component.FindNode("mountplate");
Matrix matrix = comp.TransformationInReference;
matrix.SetP(new Vector3(node.TransformationInWorld.Px, node.TransformationInWorld.Py, 201));
comp.TransformationInReference = matrix;
ICylinderFeature cylinder = (ICylinderFeature)app.World.FindComponent("SeparationVisualization_" + robot.Name).FindFeature("SeparationVisualization");
if (robotList[robot].currentSeperationDistance <= 100)
{
cylinder.GetProperty("Radius").Value = "100";
comp.GetProperty("SeparationDistance").Value = "100";
}
else
{
cylinder.GetProperty("Radius").Value =
robotList[robot].currentSeperationDistance.ToString();
comp.GetProperty("SeparationDistance").Value = robotList[robot].currentSeperationDistance;
}
cylinder.Rebuild();
}
else
{
ms.AppendMessage("UpdateVisualizationDistance: Failed to find robot component!", MessageLevel.Warning);
}
}
/// <summary>
/// Sets value of OPCUA node property to corresponding VC property
/// </summary>
private void UpdateValueToVcProperty(NodeState node)
{
Debug.Assert(System.Threading.Thread.CurrentThread == System.Windows.Application.Current.Dispatcher.Thread);
if (!UaBrowseName2VcComponentName.ContainsKey(node.BrowseName.Name))
{
_vcUtils.VcWriteWarningMsg(String.Format("Component with property {0} not found", node.BrowseName.Name));
return;
}
// Cast property OPCUA node to BaseDataVariableState type
BaseDataVariableState uaProperty = (BaseDataVariableState)node;
// Get property component as VC object
ISimComponent vcComponent = _vcUtils.GetComponent(UaBrowseName2VcComponentName[node.BrowseName.Name]);
IProperty vcProperty = vcComponent.GetProperty(node.DisplayName.ToString());
if (uaProperty.DataType == new NodeId(DataTypeIds.String))
{
if ((string)uaProperty.Value == (string)vcProperty.Value)
{
return;
}
vcProperty.Value = (string)uaProperty.Value;
}
else if (uaProperty.DataType == new NodeId(DataTypeIds.Boolean))
{
if ((bool)uaProperty.Value == (bool)vcProperty.Value)
{
return;
}
vcProperty.Value = (bool)uaProperty.Value;
}
else if (uaProperty.DataType == new NodeId(DataTypeIds.Double))
{
if ((double)uaProperty.Value == (double)vcProperty.Value)
{
return;
}
vcProperty.Value = (double)uaProperty.Value;
}
else if (uaProperty.DataType == new NodeId(DataTypeIds.Integer))
{
if ((int)uaProperty.Value == (int)vcProperty.Value)
{
return;
}
vcProperty.Value = (int)uaProperty.Value;
}
else
{
_vcUtils.VcWriteWarningMsg("OPCUA property type not supported" + uaProperty.DataType.ToString());
return;
}
}
/// <summary>
/// Returns a node tree with a component and all its signals
/// </summary>
private ComponentState CreateNodeTree(ISimComponent simComponent)
{
ComponentState componentNode = CreateComponentNode(simComponent.Name);
VcComponent vcComponent = new VcComponent(simComponent);
AddSignalNodes(componentNode, vcComponent);
AddPropertyNodes(componentNode, vcComponent);
return(componentNode);
}
/// <summary>
/// Sets value of OPCUA node property to corresponding VC property
/// </summary>
private void ua_PropertyChanged(ISystemContext context, NodeState node, NodeStateChangeMasks changes)
{
if (!UaBrowseName2VcComponentName.ContainsKey(node.BrowseName.Name))
{
_vcUtils.VcWriteWarningMsg(String.Format("Component with property {0} not found", node.BrowseName.Name));
return;
}
// Cast property OPCUA node to BaseDataVariableState type
BaseDataVariableState uaProperty = (BaseDataVariableState)node;
// Get property component as VC object
ISimComponent vcComponent = _vcUtils.GetComponent(UaBrowseName2VcComponentName[node.BrowseName.Name]);
IProperty vcProperty = vcComponent.GetProperty(node.DisplayName.ToString());
if (uaProperty.DataType == new NodeId(DataTypeIds.String))
{
if ((string)uaProperty.Value == (string)vcProperty.Value)
{
return;
}
vcProperty.Value = (string)uaProperty.Value;
}
else if (uaProperty.DataType == new NodeId(DataTypeIds.Boolean))
{
if ((bool)uaProperty.Value == (bool)vcProperty.Value)
{
return;
}
vcProperty.Value = (bool)uaProperty.Value;
}
else if (uaProperty.DataType == new NodeId(DataTypeIds.Double))
{
if ((double)uaProperty.Value == (double)vcProperty.Value)
{
return;
}
vcProperty.Value = (double)uaProperty.Value;
}
else if (uaProperty.DataType == new NodeId(DataTypeIds.Integer))
{
if ((int)uaProperty.Value == (int)vcProperty.Value)
{
return;
}
vcProperty.Value = (int)uaProperty.Value;
}
else
{
_vcUtils.VcWriteWarningMsg("OPCUA property type not supported" + uaProperty.DataType.ToString());
return;
}
}
/// <summary>
/// Sets value of OPCUA node signal to corresponding VC signal
/// </summary>
private void ua_SignalTriggered(ISystemContext context, NodeState node, NodeStateChangeMasks changes)
{
if (!UaBrowseName2VcComponentName.ContainsKey(node.BrowseName.Name))
{
_vcUtils.VcWriteWarningMsg(String.Format("Component with signal {0} not found", node.BrowseName.Name));
return;
}
// Cast signal OPCUA node to BaseDataVariableState type
BaseDataVariableState uaSignal = (BaseDataVariableState)node;
// Get signal component as VC object
ISimComponent vcComponent = _vcUtils.GetComponent(UaBrowseName2VcComponentName[node.BrowseName.Name]);
ISignal vcSignal = (ISignal)vcComponent.FindBehavior(node.DisplayName.ToString());
if (uaSignal.DataType == new NodeId(DataTypeIds.String))
{
if ((string)uaSignal.Value == (string)vcSignal.Value)
{
return;
}
vcSignal.Value = (string)uaSignal.Value;
}
else if (uaSignal.DataType == new NodeId(DataTypeIds.Boolean))
{
if ((bool)uaSignal.Value == (bool)vcSignal.Value)
{
return;
}
vcSignal.Value = (bool)uaSignal.Value;
}
else if (uaSignal.DataType == new NodeId(DataTypeIds.Double))
{
if ((double)uaSignal.Value == (double)vcSignal.Value)
{
return;
}
vcSignal.Value = (double)uaSignal.Value;
}
else if (uaSignal.DataType == new NodeId(DataTypeIds.Integer))
{
if ((int)uaSignal.Value == (int)vcSignal.Value)
{
return;
}
vcSignal.Value = (int)uaSignal.Value;
}
else
{
_vcUtils.VcWriteWarningMsg("OPCUA signal type not supported" + uaSignal.DataType.ToString());
return;
}
}
/// <summary>
/// Initialization at Simulation startup. Each time the play button is pressed, this is triggered.
/// </summary>
/// <param name="sender"></param>The source of the simulation start event.
/// <param name="e"></param>The simulation start event.
public void SimulationStarted(object sender, EventArgs e)
{
ms.AppendMessage("Simulation Started", MessageLevel.Warning);
timer = statisticsManager.CreateTimer(RegularTick, TICK_INTERVAL);
timer.StartStopTimer(true);
if (app.World.FindComponent("WorksHuman") != null)
{
human = app.World.FindComponent("WorksHuman");
if (human.GetProperty("AngleIndicatorZRotation") != null)
{
humanAngleIndicatorZRotation = human.GetProperty("AngleIndicatorZRotation");
}
}
}
private void stapleConfig(String stapleComponentName)
{
StapleSection parameterStaple = ConfigReader.readStapleConfig();
ISimComponent stapleComponent = app.Value.World.FindComponent(stapleComponentName);
if (stapleComponentName != "" && stapleComponent == null)
{
ms.AppendMessage("Failed to find staple component with name\"" + stapleComponentName + "\"! Planning of motion aborted...", MessageLevel.Warning);
return;
}
String stapleFileFolder = parameterStaple.staplePath.Path;
if (stapleComponent.GetProperty("stlID") == null)
{
ms.AppendMessage("Component with name \"" + stapleComponentName + "\" has no property \"stlID\"! Planning of motion aborted...", MessageLevel.Warning);
return;
}
String stlID = (String)stapleComponent.GetProperty("stlID").Value;
String obstacleFilePath = stapleFileFolder + stlID + ".stl";
if (!File.Exists(obstacleFilePath))
{
ms.AppendMessage("A path planning request requested stlID with \"" + stlID + "\" failed, because file \"" + obstacleFilePath + "\" does not exist...", MessageLevel.Warning);
return;
}
;
if (stapleComponent.GetProperty("StackHeight") == null)
{
ms.AppendMessage("Failed to find StackHeight property in component with name \"" + stapleComponentName + "\"! Planning of motion aborted!", MessageLevel.Warning);
return;
}
Vector3 staplePosition = stapleComponent.TransformationInWorld.GetP();
IDoubleProperty stackwidth = (IDoubleProperty)stapleComponent.GetProperty("StackWidth");
IDoubleProperty stacklength = (IDoubleProperty)stapleComponent.GetProperty("StackLength");
float translate_x = (float)staplePosition.X - (float)(stacklength.Value / 2.0);
float translate_y = (float)staplePosition.Y - (float)(stackwidth.Value / 2.0);
IDoubleProperty stackheight = (IDoubleProperty)stapleComponent.GetProperty("StackHeight");
float translate_z = (float)stackheight.Value;
// setup staple obstacle
int obstacleId = motionPlan.addObstacle(obstacleFilePath, translate_x / 1000.0f, translate_y / 1000.0f, translate_z / 1000.0f, 0.0f, 0.0f, 0.0f);
}
/// <summary>
/// Visualization of Separation Distance. A cylinder feature is added to the robot if not already present.
/// Initial values are set.
/// </summary>
/// <param name="robot"></param>The robot for which the separation distance should be visualized.
/// <param name="initialRadius"></param>The initial radius for the cylinder.
private void VisualizeSeperationDistance(IRobot robot, double initialRadius)
{
if (robot != null && robot.IsValid && app.World.FindComponent("SeparationVisualization_" + robot.Name) == null)
{
ISimComponent component = app.World.CreateComponent("SeparationVisualization_" + robot.Name);
component.CreateProperty(typeof(Double), PropertyConstraintType.NotSpecified, "SeparationDistance");
ISimNode node = robot.Component.FindNode("mountplate");
Matrix matrix = component.TransformationInReference;
matrix.SetP(new Vector3(node.TransformationInWorld.Px, node.TransformationInWorld.Py, 201));
component.TransformationInReference = matrix;
ICylinderFeature seperationVisualization = component.RootNode.RootFeature.CreateFeature <ICylinderFeature>();
// true would remove the top and bottom of the cylinder, but backfaces of the inside of the cylinder are not rendered
//seperationVisualization.GetProperty("Caps").Value = false;
seperationVisualization.GetProperty("Height").Value = "3000.0";
seperationVisualization.GetProperty("Sections").Value = "36.0";
seperationVisualization.GetProperty("Radius").Value = initialRadius.ToString();
seperationVisualization.GetProperty("Material").Value = app.FindMaterial("transp_yellow", false);
seperationVisualization.SetName("SeparationVisualization");
}
}
public VcComponent(ISimComponent inputcomponent)
{
component = inputcomponent;
}
/// <summary>
/// Sets value of VC signal to OPCUA node
/// </summary>
private void vc_SignalTriggered(object sender, SignalTriggerEventArgs e)
{
// Get OPCUA node that will get its value updated
string nodeNameParent = String.Format("{0}-{1}", e.Signal.Name, e.Signal.Node.Name);
NodeId nodeId = NodeId.Create(nodeNameParent, Namespaces.vc2opcua, uaServer.NamespaceUris);
BaseDataVariableState uaSignal = (BaseDataVariableState)nodeManager.FindPredefinedNode(nodeId, typeof(BaseDataVariableState));
if (uaSignal == null)
{
// Unsubscribe to events
e.Signal.SignalTrigger -= vc_SignalTriggered;
return;
}
if (e.Signal.Type == BehaviorType.StringSignal)
{
if ((string)uaSignal.Value == (string)e.Signal.Value)
{
return;
}
uaSignal.Value = (string)e.Signal.Value;
}
else if (e.Signal.Type == BehaviorType.ComponentSignal)
{
ISimComponent component = (ISimComponent)e.Signal.Value;
if ((string)uaSignal.Value == component.Name)
{
return;
}
uaSignal.Value = component.Name;
}
else if (e.Signal.Type == BehaviorType.BooleanSignal)
{
if ((bool)uaSignal.Value == (bool)e.Signal.Value)
{
return;
}
uaSignal.Value = (bool)e.Signal.Value;
}
else if (e.Signal.Type == BehaviorType.RealSignal)
{
if ((double)uaSignal.Value == (double)e.Signal.Value)
{
return;
}
uaSignal.Value = (double)e.Signal.Value;
}
else if (e.Signal.Type == BehaviorType.IntegerSignal)
{
if ((int)uaSignal.Value == (int)e.Signal.Value)
{
return;
}
uaSignal.Value = (int)e.Signal.Value;
}
else
{
_vcUtils.VcWriteWarningMsg("VC signal type not supported" + e.Signal.Type.ToString());
return;
}
uaSignal.Timestamp = DateTime.UtcNow;
uaSignal.ClearChangeMasks(nodeManager.context, true);
}
public override void Execute(PropertyCollection args)
{
if (args.Count < 6)
{
ms.AppendMessage("Too few arguments were passed to StartMovementActionItem. [robotName, startFrameName, goalFrameName, maxAllowedCartesianSpeed, payload, stapleComponentName]", MessageLevel.Warning);
return;
}
//TODO: Fix the hard index access or at least print out a message if input was wrong
String robotName = (String)args.GetByIndex(0).Value;
ISimComponent robotParent = app.Value.World.FindComponent(robotName);
robot = robotParent.GetRobot();
String startFrameName = (String)args.GetByIndex(1).Value;
String goalFrameName = (String)args.GetByIndex(2).Value;
int maxAllowedCartesianSpeed = (int)args.GetByIndex(3).Value;
String payload = (String)args.GetByIndex(4).Value;
//String stapleComponentName = "StapleForRightIiwa"
String stapleComponentName = (String)args.GetByIndex(5).Value;
RobotSection parameter = ConfigReader.readSection(robotName);
RobotController.getInstance().setMaxAllowedCartesianSpeed(robot, maxAllowedCartesianSpeed);
try
{
motionPlanCollectionLock.EnterWriteLock();
motionPlanCollection.Clear();
if (!motionPlanCollection.TryGetValue(robotParent, out motionPlan))
{
mpm = new MotionPlanningManager();
motionPlan = mpm.InitializeMotionPlanner(robot,
parameter.urdfFile.Path,
RobotParameters.KinStart, RobotParameters.KinEnd,
parameter.obsFile.Path);
motionPlanCollection.Add(robotParent, motionPlan);
//ms.AppendMessage("Created new motionPlan for " + robotName, MessageLevel.Warning);
}
} finally {
motionPlanCollectionLock.ExitWriteLock();
}
if (stapleComponentName != null && stapleComponentName != "")
{
stapleConfig(stapleComponentName);
}
////motionPlan.showSetupInInspector();
VectorOfDoubleVector resultMotion = mpm.planMotion(robot, motionPlan, startFrameName, goalFrameName);
if (resultMotion != null)
{
IBehavior beh = robot.Component.FindBehavior("MovementFinished");
if (beh != null && beh is IStringSignal)
{
IStringSignal movementFinished = (IStringSignal)robot.Component.FindBehavior("MovementFinished");
movementFinished.Value = ""; // empty string means no payload contained yet
MotionInterpolator inp = motionPlan.getMotionInterpolator();
inp.setMaxJointAcceleration(7.5);
inp.setMaxJointVelocity(75.0);
RobotController.getInstance().AddMotionPlan(robot, payload, motionPlan);
}
else
{
ms.AppendMessage("\"MovementFinished\" behavior was either null or not of type IStringSignal. Abort!", MessageLevel.Warning);
}
}
else
{
motionPlan.showSetupInInspector();
}
}
