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