/// <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(); } }