public override void Execute(PropertyCollection args)
{
ms.AppendMessage("Executing RegisterRobot...", MessageLevel.Warning);
//TODO: Fix the hard index access or at least print out a message if input was wrong
String robotName = (String)args.GetByIndex(0).Value;
IRobot robot = app.Value.World.FindComponent(robotName).GetRobot();
if (robot != null)
{
if (RobotController.getInstance().RegisterRobot(robot))
{
ms.AppendMessage("Robot registered at RobotController instance.", MessageLevel.Warning);
}
else
{
ms.AppendMessage("Robot Registration failed!", MessageLevel.Warning);
}
}
}
/// <summary>
/// Calculates safety separation distance and allowed velocity, when SSM_Data object is received.
/// </summary>
/// <param name="data"> Information regarding the robot and a sensed human </param>
public void Handle(SSM_Data data)
{
double separationDistance = separationCalculator
.GetSeparationDistance(data.GetHumanVelocity(), data.GetRobotVelocity());
double allowedVelocity = speedCalculator
.GetAllowedVelocity(data.GetBodyPart(), data.GetHumanVelocity(), data.GetContactArea());
string message = "Distance: " + separationDistance + "| Velocity: " + allowedVelocity;
IMessageService ms = IoC.Get <IMessageService>();
ms.AppendMessage(message, MessageLevel.Warning);
}
/// <summary>
/// Plugin Initialization. Only called once at program start.
/// </summary>
public void Initialize()
{
if (!(IoC.Get <IPluginManager>().queryPlugin(PluginCategory.Controller)))
{
return;
}
ConfigReader.init();
this.app.Simulation.SimulationStarted += SimulationStarted;
this.app.Simulation.SimulationStopped += SimulationStopped;
this.app.Simulation.SimulationReset += SimulationReset;
//IoC.Get<ISimulationService>().PropertyChanged += JointConfigurationChanged;
ms = IoC.Get <IMessageService>();
statisticsManager = this.app.StatisticsManager;
statisticsManager.IsEnabled = true;
statisticsManager.StatisticsInterval = TICK_INTERVAL;
// Must be at least Warning, Info level is not printed
ms.AppendMessage("DynamicRobotControl Plugin started initialization...", MessageLevel.Warning);
app.World.ComponentAdded += World_ComponentAdded;
app.World.ComponentRemoving += World_ComponentRemoving;
//IoC.Get<ISimulationService>().PropertyChanged += SimulationPropertyChanged;
}
public void VcWriteWarningMsg(string message)
{
_ms.AppendMessage("[vc2opcua] " + message, MessageLevel.Warning);
}
/// <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);
}
}
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();
}
}
public StartMovementActionItem() : base("StartMovement")
{
ms = IoC.Get <IMessageService>();
ms.AppendMessage("Constructor of StartMovement Action Item called", MessageLevel.Warning);
}
public RegisterRobotActionItem() : base("RegisterRobot")
{
ms = IoC.Get <IMessageService>();
ms.AppendMessage("Constructor of RegisterRobot Action Item called", MessageLevel.Warning);
}
