public VectorOfDoubleVector(VectorOfDoubleVector other) : this(CoolPropPINVOKE.new_VectorOfDoubleVector__SWIG_1(VectorOfDoubleVector.getCPtr(other)), true)
{
if (CoolPropPINVOKE.SWIGPendingException.Pending)
{
throw CoolPropPINVOKE.SWIGPendingException.Retrieve();
}
}
public VectorOfDoubleVectorEnumerator(VectorOfDoubleVector collection)
{
collectionRef = collection;
currentIndex = -1;
currentObject = null;
currentSize = collectionRef.Count;
}
public void SetRange(int index, VectorOfDoubleVector values)
{
CoolPropPINVOKE.VectorOfDoubleVector_SetRange(swigCPtr, index, VectorOfDoubleVector.getCPtr(values));
if (CoolPropPINVOKE.SWIGPendingException.Pending)
{
throw CoolPropPINVOKE.SWIGPendingException.Retrieve();
}
}
public static VectorOfDoubleVector PropsSImulti(StringVector Outputs, string Name1, DoubleVector Prop1, string Name2, DoubleVector Prop2, string backend, StringVector fluids, DoubleVector fractions)
{
VectorOfDoubleVector ret = new VectorOfDoubleVector(CoolPropPINVOKE.PropsSImulti(StringVector.getCPtr(Outputs), Name1, DoubleVector.getCPtr(Prop1), Name2, DoubleVector.getCPtr(Prop2), backend, StringVector.getCPtr(fluids), DoubleVector.getCPtr(fractions)), true);
if (CoolPropPINVOKE.SWIGPendingException.Pending)
{
throw CoolPropPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public static VectorOfDoubleVector Repeat(DoubleVector value, int count)
{
global::System.IntPtr cPtr = CoolPropPINVOKE.VectorOfDoubleVector_Repeat(DoubleVector.getCPtr(value), count);
VectorOfDoubleVector ret = (cPtr == global::System.IntPtr.Zero) ? null : new VectorOfDoubleVector(cPtr, true);
if (CoolPropPINVOKE.SWIGPendingException.Pending)
{
throw CoolPropPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public VectorOfDoubleVector GetRange(int index, int count)
{
global::System.IntPtr cPtr = CoolPropPINVOKE.VectorOfDoubleVector_GetRange(swigCPtr, index, count);
VectorOfDoubleVector ret = (cPtr == global::System.IntPtr.Zero) ? null : new VectorOfDoubleVector(cPtr, true);
if (CoolPropPINVOKE.SWIGPendingException.Pending)
{
throw CoolPropPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(VectorOfDoubleVector obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
/// <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();
}
}
