/// <summary>
/// Initialize a motionPlanner instance for a specific robot.
/// This instance is stored in the dictonary of the class which manages and maintains the instances of all motionPlanners of all robots.
/// </summary>
/// <param name="robot"></param>The VC robot instance for which a motionPlanner instance should be created
/// <param name="pathToRobotUrdfDescriptionFile"></param>The URDF description of the robot
/// <param name="kinChainStart"></param>The name of the first part in the kinematic chain.
/// <param name="kinChainEnd"></param>The name of the last part in the kinematic chain.
/// <param name="pathToObstacleFile"></param>The path to an obstacle model file (e.g. stl format).
public MotionPlan InitializeMotionPlanner(IRobot robot, String pathToRobotUrdfDescriptionFile, String kinChainStart, String kinChainEnd, String pathToObstacleFile)
{
MotionPlan motionPlan = new MotionPlan(pathToRobotUrdfDescriptionFile, kinChainStart, kinChainEnd);
if (!motionPlan.isValid())
{
Console.WriteLine("Failed to initialize motionPlan, unable to load robot urdf file \"" + pathToRobotUrdfDescriptionFile + "\" from " + kinChainStart + " to " + kinChainEnd + "!");
return(null);
}
MotionPlanRobotDescription description = motionPlan.getMotionPlanRobotDescription();
Vector3 wpr = robot.Component.TransformationInWorld.GetWPR();
description.setRobotPosition(robot.Component.TransformationInWorld.GetP().X / 1000,
robot.Component.TransformationInWorld.GetP().Y / 1000,
robot.Component.TransformationInWorld.GetP().Z / 1000);
description.setRobotRotation(wpr.X, wpr.Y, wpr.Z);
motionPlan.addObstacle(pathToObstacleFile);
return(motionPlan);
}
/// <summary>
/// Adds a new motion plan for a specific robot which would come from path planning.
/// This motion plan contains at least two or more (the intermediate) joint angle configuration to reach the goal position.
/// Furthermore motion interpolator and motion tester are initialized and the motion plan from path planning
/// is subdived.
/// </summary>
/// <param name="robot"></param>The robot to which the motion plan belongs and for which it should be saved.
/// <param name="motionPlan"></param>
public void AddMotionPlan(IRobot robot, String payloadOnFinishMovement, MotionPlan motionPlan)
{
try
{
robotListLock.EnterWriteLock();
try
{
robotList[robot].motionPlan = motionPlan;
robotList[robot].currentMotionStartTime = app.Simulation.Elapsed;
robotList[robot].payloadOnFinishMovement = payloadOnFinishMovement;
//ms.AppendMessage("New motion plan (" + motionPlan.getLastResult().Count + ") set for robot " + robot.Name + " starting at " + app.Simulation.Elapsed, MessageLevel.Warning);
}
catch (KeyNotFoundException e)
{
ms.AppendMessage("Motion plan could not be added, maybe robot was not registred?", MessageLevel.Warning);
ms.AppendMessage(e.ToString(), MessageLevel.Error);
}
}
finally
{
robotListLock.ExitWriteLock();
}
}
public void Post([FromBody] MotionPlan value)
{
Response.StatusCode = 501;
}
/// <summary>
/// Trigger and receive a concrete motionPlan from startFrame to goalFrame.
/// Obstacles must be added to the motionPlanner instance of the robot before.
/// </summary>
/// <param name="robot"></param>The robot for which a motion should be planned.
/// <param name="motionPlan"></param>The preconfigured motionPlan which should be used.
/// <param name="startFrame"></param>The frame where the motion should start.
/// <param name="goalFrame"></param>The frame where the motion should end.
/// <returns></returns>
public VectorOfDoubleVector planMotion(IRobot robot, MotionPlan motionPlan, String startFrame, String goalFrame)
{
MotionPlanRobotDescription description = motionPlan.getMotionPlanRobotDescription();
// we need the current position of the robot to enhance the result of the inverse kinematics
VectorOfDouble currentPositionJointAngles = new VectorOfDouble(robot.RobotController.Joints.Count);
if (robot.RobotController.Joints.Count == 7)
{
currentPositionJointAngles.Add(robot.RobotController.Joints[0].Value);
currentPositionJointAngles.Add(robot.RobotController.Joints[1].Value);
currentPositionJointAngles.Add(robot.RobotController.Joints[6].Value);
currentPositionJointAngles.Add(robot.RobotController.Joints[2].Value);
currentPositionJointAngles.Add(robot.RobotController.Joints[3].Value);
currentPositionJointAngles.Add(robot.RobotController.Joints[4].Value);
currentPositionJointAngles.Add(robot.RobotController.Joints[5].Value);
}
else
{
currentPositionJointAngles.Add(robot.RobotController.Joints[0].Value);
currentPositionJointAngles.Add(robot.RobotController.Joints[1].Value);
currentPositionJointAngles.Add(robot.RobotController.Joints[2].Value);
currentPositionJointAngles.Add(robot.RobotController.Joints[3].Value);
currentPositionJointAngles.Add(robot.RobotController.Joints[4].Value);
currentPositionJointAngles.Add(robot.RobotController.Joints[5].Value);
}
//TODO: Make those frames global?
IFeature startNode = robot.Component.FindFeature(startFrame);
if (startNode == null)
{
IoC.Get <IMessageService>().AppendMessage("Start Frame \"" + startFrame + "\" was not found.", MessageLevel.Error);
return(null);
}
IFeature goalNode = robot.Component.FindFeature(goalFrame);
if (goalNode == null)
{
IoC.Get <IMessageService>().AppendMessage("Goal Frame \"" + goalFrame + "\" was not found.", MessageLevel.Error);
return(null);
}
Matrix startPosition = robot.Component.RootNode.GetFeatureTransformationInWorld(startNode);
Matrix goalPosition = robot.Component.RootNode.GetFeatureTransformationInWorld(goalNode);
Vector3 startRotation = startPosition.GetWPR();
Vector3 goalRotation = goalPosition.GetWPR();
Vector3 robotPosition = robot.Component.TransformationInWorld.GetP();
VectorOfDouble startJointAngles = currentPositionJointAngles;
//IoC.Get<IMessageService>().AppendMessage("Goal Frame \"" + goalFrame + "\": [" + String.Format("{0:0.000}", (goalPosition.GetP().X / 1000)) + ", " + String.Format("{0:0.000}", (goalPosition.GetP().Y / 1000)) + ", " + String.Format("{0:0.000}", (goalPosition.GetP().Z / 1000)) + "] ["+ String.Format("{0:0.000}", goalRotation.X)+" "+ String.Format("{0:0.000}", goalRotation.Y)+" "+ String.Format("{0:0.000}", goalRotation.Z) + "]", MessageLevel.Warning);
VectorOfDouble goalJointAngles = description.getIK(goalPosition.GetP().X / 1000,
goalPosition.GetP().Y / 1000,
goalPosition.GetP().Z / 1000,
goalRotation.X, goalRotation.Y, goalRotation.Z, startJointAngles, 0.5, "Distance");
motionPlan.setStartPosition(startJointAngles);
motionPlan.setGoalPosition(goalJointAngles);
String startOut = "[", goalOut = "[";
for (int i = 0; i < startJointAngles.Count; i++)
{
startOut += String.Format("{0:0.000}", startJointAngles[i]) + " ";
}
for (int i = 0; i < goalJointAngles.Count; i++)
{
goalOut += String.Format("{0:0.000}", goalJointAngles[i]) + " ";
}
startOut += "]";
goalOut += "]";
motionPlan.setSolveTime(10.0);
motionPlan.setStateValidityCheckingResolution(0.01);
//motionPlan.setReportFirstExactSolution(true);
motionPlan.setPlannerByString("RRTConnect");
if (motionPlan.plan() > 0)
{
//IoC.Get<IMessageService>().AppendMessage("Found motion from " + startFrame + "@" + startOut + " to " + goalFrame + "@" + goalOut + ": ", MessageLevel.Warning);
VectorOfDoubleVector plan = motionPlan.getLastResult();
//motionPlan.showSetupInInspector();
foreach (VectorOfDouble jointConfiguration in plan)
{
String motionBuf = "[", sep = "";
foreach (double jointAngle in jointConfiguration)
{
motionBuf += sep + String.Format("{0:0.000}", jointAngle);
sep = ",";
}
//IoC.Get<IMessageService>().AppendMessage(motionBuf + "]", MessageLevel.Warning);
}
//IoC.Get<IMessageService>().AppendMessage("Found motion END", MessageLevel.Warning);
return(plan);
}
else
{
//motionPlan.showSetupInInspector();
IoC.Get <IMessageService>().AppendMessage("Failed to find motion from " + startOut + " to " + goalOut + ": " + motionPlan.getLastPlanningError(), MessageLevel.Warning);
}
return(null);
}
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();
}
}
