/// <summary>
/// Initializes a new instance of the Robot class with attached external axes.
/// </summary>
/// <param name="name"> The name. </param>
/// <param name="meshes"> The base and links meshes defined in the world coorindate space. </param>
/// <param name="internalAxisPlanes"> The internal axes planes defined in the world coorindate space. </param>
/// <param name="internalAxisLimits"> The internal axes limit. </param>
/// <param name="basePlane"> The position and orientation of the robot base in the world coordinate space. </param>
/// <param name="mountingFrame"> The tool mounting frame definied in the world coordinate space. </param>
/// <param name="tool"> The Robot Tool. </param>
/// <param name="externalAxes"> The attached external axes. </param>
public Robot(string name, List <Mesh> meshes, List <Plane> internalAxisPlanes, List <Interval> internalAxisLimits, Plane basePlane, Plane mountingFrame, RobotTool tool, List <ExternalAxis> externalAxes)
{
// Robot related fields
_name = name;
_meshes = meshes;
_internalAxisPlanes = internalAxisPlanes;
_internalAxisLimits = internalAxisLimits;
_basePlane = basePlane;
_mountingFrame = mountingFrame;
// Tool related fields
_tool = tool.Duplicate(); // Make a deep copy since we transform it later
_meshes.Add(GetAttachedToolMesh());
CalculateAttachedToolPlane();
// External axis related fields
_externalAxes = externalAxes;
_externalAxisPlanes = new List <Plane>();
_externalAxisLimits = new List <Interval>();
UpdateExternalAxisFields();
// Transform Robot Tool to Mounting Frame
Transform trans = Transform.PlaneToPlane(_tool.AttachmentPlane, _mountingFrame);
_tool.Transform(trans);
// Set kinematics
_inverseKinematics = new InverseKinematics(new RobotTarget("init", Plane.WorldXY), this);
_forwardKinematics = new ForwardKinematics(this);
}
/// <summary>
/// This is the method that actually does the work.
/// </summary>
/// <param name="DA">The DA object can be used to retrieve data from input parameters and
/// to store data in output parameters.</param>
protected override void SolveInstance(IGH_DataAccess DA)
{
// Input variables
Robot robot = null;
Movement Movement = null;
// Catch the input data
if (!DA.GetData(0, ref robot))
{
return;
}
if (!DA.GetData(1, ref Movement))
{
return;
}
// Calculate the robot pose
_inverseKinematics = new InverseKinematics(Movement, robot);
_inverseKinematics.Calculate();
// Check the values
for (int i = 0; i < _inverseKinematics.ErrorText.Count; i++)
{
AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, _inverseKinematics.ErrorText[i]);
}
// Set forward kinematics (for visualization)
_forwardKinematics = new ForwardKinematics(robot);
// Output
DA.SetData(0, _inverseKinematics.RobotJointPosition);
DA.SetData(1, _inverseKinematics.ExternalJointPosition);
}
private static Point3 GetWristPoint(double shoulderYaw, double shoulderPitch, double shoulderRoll, double elbowPitch)
{
Matrix elbowTranslationMatrix = ForwardKinematics.GetElbowTranslationMatrixLeft(shoulderPosition, upperArmLength, shoulderYaw, shoulderPitch);
Point3 point = ForwardKinematics.GetWristPointLeft(elbowTranslationMatrix, shoulderRoll, elbowPitch, lowerArmLength);
return(point);
}
/// <summary>
/// This is the method that actually does the work.
/// </summary>
/// <param name="DA">The DA object can be used to retrieve data from input parameters and
/// to store data in output parameters.</param>
protected override void SolveInstance(IGH_DataAccess DA)
{
// Warning that this component is OBSOLETE
AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, "This component is OBSOLETE and will be removed " +
"in the future. Remove this component from your canvas and replace it by picking the new component " +
"from the ribbon.");
// Input variables
Robot robot = null;
List <double> internalAxisValues = new List <double>();
List <double> externalAxisValues = new List <double>();
// Catch input data
if (!DA.GetData(0, ref robot))
{
return;
}
if (!DA.GetDataList(1, internalAxisValues))
{
return;
}
if (!DA.GetDataList(2, externalAxisValues))
{
return;
}
// Add up missing internal axisValues
for (int i = internalAxisValues.Count; i < 6; i++)
{
internalAxisValues.Add(0);
}
// Add up missing external axisValues
for (int i = externalAxisValues.Count; i < 6; i++)
{
externalAxisValues.Add(0);
}
// Calcuate the robot pose
_fk = new ForwardKinematics(robot, new RobotJointPosition(internalAxisValues), new ExternalJointPosition(externalAxisValues), _hideMesh);
_fk.Calculate();
// Check the values
for (int i = 0; i < _fk.ErrorText.Count; i++)
{
AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, _fk.ErrorText[i]);
}
// Fill data tree with meshes
if (_hideMesh == false)
{
_meshes = GetPosedMeshesDataTree(_fk);
}
// Output
DA.SetDataTree(0, _meshes);
DA.SetData(1, _fk.TCPPlane); // Outputs the TCP as a plane
DA.SetDataList(2, _fk.PosedExternalAxisPlanes); // Outputs the External Axis Planes
}
/// <summary>
/// This is the method that actually does the work.
/// </summary>
/// <param name="DA">The DA object can be used to retrieve data from input parameters and
/// to store data in output parameters.</param>
protected override void SolveInstance(IGH_DataAccess DA)
{
// Warning that this component is OBSOLETE
AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, "This component is OBSOLETE and will be removed " +
"in the future. Remove this component from your canvas and replace it by picking the new component " +
"from the ribbon.");
// Input variables
GH_Robot robotGoo = null;
List <double> internalAxisValues = new List <double>();
List <double> externalAxisValues = new List <double>();
// Catch input data
if (!DA.GetData(0, ref robotGoo))
{
return;
}
if (!DA.GetDataList(1, internalAxisValues))
{
return;
}
if (!DA.GetDataList(2, externalAxisValues))
{
return;
}
// Add up missing internal axisValues
for (int i = internalAxisValues.Count; i < 6; i++)
{
internalAxisValues.Add(0);
}
// Add up missing external axisValues
for (int i = externalAxisValues.Count; i < 6; i++)
{
externalAxisValues.Add(0);
}
// Calcuate the robot pose
ForwardKinematics forwardKinematics = new ForwardKinematics(robotGoo.Value, new RobotJointPosition(internalAxisValues), new ExternalJointPosition(externalAxisValues));
forwardKinematics.Calculate();
// Check the values
for (int i = 0; i < forwardKinematics.ErrorText.Count; i++)
{
AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, forwardKinematics.ErrorText[i]);
}
// Create data tree for output of all posed meshes
GH_Structure <GH_Mesh> meshes = GetPosedMeshesDataTree(forwardKinematics);
// Output
_fk = forwardKinematics;
DA.SetDataTree(0, meshes);
DA.SetData(1, forwardKinematics.TCPPlane); // Outputs the TCP as a plane
DA.SetDataList(2, forwardKinematics.PosedExternalAxisPlanes); // Outputs the External Axis Planes
}
void Start()
{
gripper = GameObject.Find("Gripper");
vidTex = video.GetComponent <VideoPlayer>();
FKscr = gameObject.GetComponent <ForwardKinematics>();
rend = gameObject.GetComponent <Renderer>();
vidTex.time = (long)(tSafeComplReplay1 * vidTex.length);
}
private static void AssertProjectionsMatchPointsLeft(Point3 elbow, Point3 wrist)
{
// Make sure the lengths of each segment are the expected lengths.
Assert.AreEqual(1, Vector3.FromPoints(shoulderL, elbow).Magnitude);
Assert.AreEqual(1, Vector3.FromPoints(elbow, wrist).Magnitude);
InverseKinematics ik = InverseKinematics.GetInverseKinematicsLeft(neck, spine, shoulderL, shoulderR, elbow, wrist, inRadians: true);
ForwardKinematics fk = ForwardKinematics.GetForwardKinematicsLeft(shoulderL, ik);
Assert.AreEqual(elbow, fk.Elbow);
Assert.AreEqual(wrist, fk.Wrist);
}
/// <summary>
/// Returns the position hit from the current configuration.
/// </summary>
public DenseMatrix GetForwardTransformationMatrix()
{
var result = DenseMatrix.CreateIdentity(4);
for (int i = 0; i < DhParameterCollection.Count; i++)
{
var currentDhParameter = DhParameterCollection[i];
var currentMatrix = ForwardKinematics.GetDenseMatrixForDhParameter(currentDhParameter);
result *= currentMatrix;
}
return(result);
}
//binds the objects in the object hierarchy to a pose, either current or base
void setPose(bool boundToCurrent)
{
for (int jointID = 0; jointID < animData.poseBase.Length; jointID++)
{
animationTransformData tData = new animationTransformData(0);
if (boundToCurrent)
{
KeyFrame currentKey = animData.poseBase[jointID].keyFrames[currentKeyFrame];
tData.setTransformIndividual(currentKey.keyPosition, currentKey.keyQRotation, currentKey.scale);
}
ForwardKinematics.setData(gameObjectHierarchy, animData, tData, jointID);
}
}
public void Origin()
{
double shoulderYaw = 0;
double shoulderPitch = 0;
Point3 point = ForwardKinematics.GetElbowPointLeft(shoulderPosition, upperArmLength, shoulderYaw, shoulderPitch);
Point3 expectedPoint = new Point3(
shoulderPosition.X - upperArmLength,
shoulderPosition.Y,
shoulderPosition.Z
);
Assert.AreEqual(expectedPoint, point);
}
public void PointingUp()
{
double shoulderYaw = -_90Deg;
double shoulderPitch = _90Deg;
Point3 point = ForwardKinematics.GetElbowPointRight(shoulderPosition, upperArmLength, shoulderYaw, shoulderPitch);
Point3 expectedPoint = new Point3(
shoulderPosition.X,
shoulderPosition.Y + upperArmLength,
shoulderPosition.Z
);
Assert.AreEqual(expectedPoint, point);
}
public void PointingForward()
{
// Yaw will go from 90 to 270, making 180 the mid point.
double shoulderYaw = 2 * _90Deg;
double shoulderPitch = -_90Deg;
Point3 point = ForwardKinematics.GetElbowPointLeft(shoulderPosition, upperArmLength, shoulderYaw, shoulderPitch);
Point3 expectedPoint = new Point3(
shoulderPosition.X,
shoulderPosition.Y,
shoulderPosition.Z - upperArmLength
);
Assert.AreEqual(expectedPoint, point);
}
private void Initialize()
{
serviceConverter = new StewartPlatformFKConverter();
serviceInitializer = new ServiceInitializerFK(sliders);
serviceInitializer.InitializeSliders();
serviceRotation = ScriptableObject.CreateInstance <ServiceRotationFK>();
serviceRotation.Init(topLegs, bottomLegs, finalLegs, children, sliders);
serviceRotation.RotateLegs();
forwardKinematics = ScriptableObject.CreateInstance <ForwardKinematics>();
forwardKinematics.Init(serviceConverter, children, childrenFinal, bottomLegs, endEffector, sliders,
StewartPlatformFKUtils.A_Matrix, StewartPlatformFKUtils.B_Matrix);
forwardKinematics.SetupVariables();
}
private List <Interval> _externalAxisLimits; // The external axis limit
#endregion
#region (de)serialization
/// <summary>
/// Protected constructor needed for deserialization of the object.
/// </summary>
/// <param name="info"> The SerializationInfo to extract the data from. </param>
/// <param name="context"> The context of this deserialization. </param>
protected Robot(SerializationInfo info, StreamingContext context)
{
_name = (string)info.GetValue("Name", typeof(string));
_meshes = (List <Mesh>)info.GetValue("Meshes", typeof(List <Mesh>));
_internalAxisPlanes = (List <Plane>)info.GetValue("Internal Axis Planes", typeof(List <Plane>));
_internalAxisLimits = (List <Interval>)info.GetValue("Internal Axis Limits", typeof(List <Interval>));
_basePlane = (Plane)info.GetValue("Base Plane", typeof(Plane));
_mountingFrame = (Plane)info.GetValue("Mounting Frame", typeof(Plane));
_tool = (RobotTool)info.GetValue("RobotTool", typeof(RobotTool));
_toolPlane = (Plane)info.GetValue("Tool Plane", typeof(Plane));
_externalAxes = (List <ExternalAxis>)info.GetValue("External Axis", typeof(List <ExternalAxis>));
_externalAxisPlanes = (List <Plane>)info.GetValue("External Axis Planes", typeof(List <Plane>));
_externalAxisLimits = (List <Interval>)info.GetValue("External Axis Limits", typeof(List <Interval>));
_inverseKinematics = new InverseKinematics(new RobotTarget("init", Plane.WorldXY), this);
_forwardKinematics = new ForwardKinematics(this);
}
// Update is called once per frame
void Update()
{
int jointCount = animationObjectHierData.ObjectHierarchy.Count;
currentKeyFrame++;
if (currentKeyFrame > maxFrame)
{
currentKeyFrame = 0;
}
blendPoseData poseDataResult = blendTreeData.useBlendTree(blendTreeData, currentKeyFrame);
for (int i = 0; i < jointCount; i++)
{
ForwardKinematics.setData(animationObjectHierData, animData, poseDataResult.getPoseData(i), i);
}
}
public void CheckDHParameterValuesWrist()
{
List <TransformationMatrix> matrices = new List <TransformationMatrix>();
DenseMatrix resMatrix = DenseMatrix.CreateIdentity(4);
var terminalMatrix = new TransformationMatrix();
resMatrix = ForwardKinematics.GetTerminalFrameFor(_dhParam, new List <double>()
{
0, -Math.PI / 2, 0, Math.PI / 2, Math.PI / 2, 0
});
terminalMatrix.DenseMatrix = resMatrix;
Debug.Print("\n" + resMatrix.ToString());
Assert.IsTrue(terminalMatrix.Matrix3D.OffsetX.DoubleEquals(1550) &&
terminalMatrix.Matrix3D.OffsetY.DoubleEquals(-240) &&
terminalMatrix.Matrix3D.OffsetZ.DoubleEquals(1784));
}
/// <summary>
/// This is the method that actually does the work.
/// </summary>
/// <param name="DA">The DA object can be used to retrieve data from input parameters and
/// to store data in output parameters.</param>
protected override void SolveInstance(IGH_DataAccess DA)
{
// Input variables
Robot robot = null;
RobotJointPosition robotJointPosition = new RobotJointPosition();
ExternalJointPosition externalJointPosition = new ExternalJointPosition();
// Catch input data
if (!DA.GetData(0, ref robot))
{
return;
}
if (!DA.GetData(1, ref robotJointPosition))
{
robotJointPosition = new RobotJointPosition();
}
if (!DA.GetData(2, ref externalJointPosition))
{
externalJointPosition = new ExternalJointPosition();
}
// Calcuate the robot pose
_fk = new ForwardKinematics(robot, robotJointPosition, externalJointPosition, _hideMesh);
_fk.Calculate();
// Check the values
for (int i = 0; i < _fk.ErrorText.Count; i++)
{
AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, _fk.ErrorText[i]);
}
// Fill data tree with meshes
if (_hideMesh == false)
{
_meshes = GetPosedMeshesDataTree(_fk);
}
// Output
DA.SetDataTree(0, _meshes);
DA.SetData(1, _fk.TCPPlane);
DA.SetDataList(2, _fk.PosedExternalAxisPlanes);
}
/// <summary>
/// Transform the posed meshes rom the forward kinematics to a datatree
/// </summary>
/// <param name="forwardKinematics"> The forward kinematics the posed meshes will be extracted from. </param>
/// <returns> The data tree structure with all the posed meshes. </returns>
public GH_Structure <GH_Mesh> GetPosedMeshesDataTree(ForwardKinematics forwardKinematics)
{
// Create data tree for output of alle posed meshes
GH_Structure <GH_Mesh> meshes = new GH_Structure <GH_Mesh>();
{
// Robot pose meshes
List <Mesh> posedInternalAxisMeshes = forwardKinematics.PosedInternalAxisMeshes;
// Data tree path
GH_Path path = new GH_Path(0);
// Save the posed meshes
for (int i = 0; i < posedInternalAxisMeshes.Count; i++)
{
meshes.Append(new GH_Mesh(posedInternalAxisMeshes[i]), path);
}
// Extenal axis meshes
List <List <Mesh> > posedExternalAxisMeshes = forwardKinematics.PosedExternalAxisMeshes;
// Loop over all the external axes
for (int i = 0; i < posedExternalAxisMeshes.Count; i++)
{
// Data tree path
path = new GH_Path(i + 1);
// Save the posed meshes
for (int j = 0; j < posedExternalAxisMeshes[i].Count; j++)
{
meshes.Append(new GH_Mesh(posedExternalAxisMeshes[i][j]), path);
}
}
}
// Return the data tree stucture
return(meshes);
}
/// <summary>
/// Initializes a new instance of the Robot class by duplicating an existing Robot instance.
/// </summary>
/// <param name="robot"> The Robot instance to duplicate. </param>
public Robot(Robot robot)
{
// Robot related fields
_name = robot.Name;
_meshes = robot.Meshes.ConvertAll(mesh => mesh.DuplicateMesh()); // This includes the tool mesh
_internalAxisPlanes = new List <Plane>(robot.InternalAxisPlanes);
_internalAxisLimits = new List <Interval>(robot.InternalAxisLimits);
_basePlane = new Plane(robot.BasePlane);
_mountingFrame = new Plane(robot.MountingFrame);
// Tool related fields
_tool = robot.Tool.Duplicate();
_toolPlane = new Plane(robot.ToolPlane);
// External axis related fields
_externalAxes = new List <ExternalAxis>(robot.ExternalAxes); //TODO: make deep copy
_externalAxisPlanes = new List <Plane>(robot.ExternalAxisPlanes);
_externalAxisLimits = new List <Interval>(robot.ExternalAxisLimits);
// Kinematics
_inverseKinematics = new InverseKinematics(robot.InverseKinematics.Movement.Duplicate(), this);
_forwardKinematics = new ForwardKinematics(this, robot.ForwardKinematics.HideMesh);
}
/// <summary>
/// This is the method that actually does the work.
/// </summary>
/// <param name="DA">The DA object can be used to retrieve data from input parameters and
/// to store data in output parameters.</param>
protected override void SolveInstance(IGH_DataAccess DA)
{
// Warning that this component is OBSOLETE
AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, "This component is OBSOLETE and will be removed " +
"in the future. Remove this component from your canvas and replace it by picking the new component " +
"from the ribbon.");
// Input variables
List <RobotComponents.Actions.Action> actions = new List <RobotComponents.Actions.Action>();
int interpolations = 0;
double interpolationSlider = 0;
bool update = false;
// Catch the input data
if (!DA.GetData(0, ref _robot))
{
return;
}
if (!DA.GetDataList(1, actions))
{
return;
}
if (!DA.GetData(2, ref interpolations))
{
return;
}
if (!DA.GetData(3, ref interpolationSlider))
{
return;
}
if (!DA.GetData(4, ref update))
{
return;
}
// Update the path
if (update == true || _lastInterpolations != interpolations)
{
// Create forward kinematics for mesh display
_forwardKinematics = new ForwardKinematics(_robot);
// Create the path generator
_pathGenerator = new PathGenerator(_robot);
// Re-calculate the path
_pathGenerator.Calculate(actions, interpolations);
// Get all the targets
_planes.Clear();
_planes = _pathGenerator.Planes;
// Get the new path curve
_paths.Clear();
_paths = _pathGenerator.Paths;
// Clear the lists with the internal and external axis values
_robotJointPositions.Clear();
_externalJointPositions.Clear();
_robotJointPositions = _pathGenerator.RobotJointPositions;
_externalJointPositions = _pathGenerator.ExternalJointPositions;
// Store the number of interpolations that are used, to check if this value is changed.
_lastInterpolations = interpolations;
// Raise warnings
if (_pathGenerator.ErrorText.Count != 0)
{
_raiseWarnings = true;
}
else
{
_raiseWarnings = false;
}
}
// Get the index number of the current target
int index = (int)(((_planes.Count - 1) * interpolationSlider));
// Create list with external axis values
List <double> externalAxisValues = _externalJointPositions[index].ToList();
externalAxisValues.RemoveAll(val => val == 9e9);
// Calcualte foward kinematics
_forwardKinematics.RobotJointPosition = _robotJointPositions[index];
_forwardKinematics.ExternalJointPosition = _externalJointPositions[index];
_forwardKinematics.HideMesh = !_previewMesh;
_forwardKinematics.Calculate();
// Show error messages
if (_raiseWarnings == true)
{
AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, "Only axis values of absolute joint movements are checked.");
for (int i = 0; i < _pathGenerator.ErrorText.Count; i++)
{
AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, _pathGenerator.ErrorText[i]);
if (i == 30)
{
break;
}
}
}
// Output
DA.SetData(0, _forwardKinematics.TCPPlane);
DA.SetDataList(1, _forwardKinematics.PosedExternalAxisPlanes);
DA.SetDataList(2, _forwardKinematics.RobotJointPosition.ToList());
DA.SetDataList(3, externalAxisValues);
if (_previewCurve == true)
{
DA.SetDataList(4, _paths);
}
else
{
DA.SetDataList(4, null);
}
}
/// <summary>
/// This is the method that actually does the work.
/// </summary>
/// <param name="DA">The DA object can be used to retrieve data from input parameters and
/// to store data in output parameters.</param>
protected override void SolveInstance(IGH_DataAccess DA)
{
// Input variables
List <RobotComponents.Actions.Action> actions = new List <RobotComponents.Actions.Action>();
int interpolations = 0;
double interpolationSlider = 0;
bool update = false;
// Catch the input data
if (!DA.GetData(0, ref _robot))
{
return;
}
if (!DA.GetDataList(1, actions))
{
return;
}
if (!DA.GetData(2, ref interpolations))
{
return;
}
if (!DA.GetData(3, ref interpolationSlider))
{
return;
}
if (!DA.GetData(4, ref update))
{
return;
}
// Update the path
if (update == true || _lastInterpolations != interpolations)
{
// Create forward kinematics for mesh display
_forwardKinematics = new ForwardKinematics(_robot);
// Create the path generator
_pathGenerator = new PathGenerator(_robot);
// Re-calculate the path
_pathGenerator.Calculate(actions, interpolations);
// Get all the targets
_planes.Clear();
_planes = _pathGenerator.Planes;
// Get the new path curve
_paths.Clear();
_paths = _pathGenerator.Paths;
// Clear the lists
_robotJointPositions.Clear();
_externalJointPositions.Clear();
// Get new lists
_robotJointPositions = _pathGenerator.RobotJointPositions;
_externalJointPositions = _pathGenerator.ExternalJointPositions;
// Store the number of interpolations that are used, to check if this value is changed.
_lastInterpolations = interpolations;
}
// Get the index number of the current target
int index = (int)(((_planes.Count - 1) * interpolationSlider));
// Calculate foward kinematics
_forwardKinematics.HideMesh = !_previewMesh;
_forwardKinematics.Calculate(_robotJointPositions[index], _externalJointPositions[index]);
// Show error messages
if (_pathGenerator.ErrorText.Count != 0)
{
for (int i = 0; i < _pathGenerator.ErrorText.Count; i++)
{
AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, _pathGenerator.ErrorText[i]);
if (i == 30)
{
break;
}
}
}
// Output
DA.SetData(0, _forwardKinematics.TCPPlane);
DA.SetDataList(1, _forwardKinematics.PosedExternalAxisPlanes);
DA.SetData(2, _forwardKinematics.RobotJointPosition);
DA.SetData(3, _forwardKinematics.ExternalJointPosition);
if (_previewCurve == true)
{
DA.SetDataList(4, _paths);
}
else
{
DA.SetDataList(4, null);
}
}
/// <summary>
/// This is the method that actually does the work.
/// </summary>
/// <param name="DA">The DA object can be used to retrieve data from input parameters and
/// to store data in output parameters.</param>
protected override void SolveInstance(IGH_DataAccess DA)
{
// Input variables
List <RobotComponents.Actions.Action> actions = new List <RobotComponents.Actions.Action>();
int interpolations = 0;
double interpolationSlider = 0;
bool update = false;
// Catch the input data
if (!DA.GetData(0, ref _robot))
{
return;
}
if (!DA.GetDataList(1, actions))
{
return;
}
if (!DA.GetData(2, ref interpolations))
{
return;
}
if (!DA.GetData(3, ref interpolationSlider))
{
return;
}
if (!DA.GetData(4, ref update))
{
return;
}
// Update the path
if (update == true | _calculated == false)
{
// Create forward kinematics for mesh display
_forwardKinematics = new ForwardKinematics(_robot);
// Create the path generator
_pathGenerator = new PathGenerator(_robot);
// Re-calculate the path
_pathGenerator.Calculate(actions, interpolations);
// Makes sure that there is always a calculated solution
_calculated = true;
}
// Get the index number of the current target
int index = (int)(((_pathGenerator.Planes.Count - 1) * interpolationSlider));
// Calculate foward kinematics
_forwardKinematics.HideMesh = !_previewMesh;
_forwardKinematics.Calculate(_pathGenerator.RobotJointPositions[index], _pathGenerator.ExternalJointPositions[index]);
// Show error messages
if (_pathGenerator.ErrorText.Count != 0)
{
for (int i = 0; i < _pathGenerator.ErrorText.Count; i++)
{
AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, _pathGenerator.ErrorText[i]);
if (i == 30)
{
break;
}
}
}
// Output Parameters
int ind;
if (Params.Output.Any(x => x.NickName.Equality(outputParameters[0].NickName)))
{
ind = Params.Output.FindIndex(x => x.NickName.Equality(outputParameters[0].NickName));
DA.SetData(ind, _pathGenerator.Planes[index]);
}
if (Params.Output.Any(x => x.NickName.Equality(outputParameters[1].NickName)))
{
ind = Params.Output.FindIndex(x => x.NickName.Equality(outputParameters[1].NickName));
DA.SetDataList(ind, _pathGenerator.Planes);
}
if (Params.Output.Any(x => x.NickName.Equality(outputParameters[2].NickName)))
{
ind = Params.Output.FindIndex(x => x.NickName.Equality(outputParameters[2].NickName));
DA.SetData(ind, _pathGenerator.RobotJointPositions[index]);
}
if (Params.Output.Any(x => x.NickName.Equality(outputParameters[3].NickName)))
{
ind = Params.Output.FindIndex(x => x.NickName.Equality(outputParameters[3].NickName));
DA.SetDataList(ind, _pathGenerator.RobotJointPositions);
}
if (Params.Output.Any(x => x.NickName.Equality(outputParameters[4].NickName)))
{
ind = Params.Output.FindIndex(x => x.NickName.Equality(outputParameters[4].NickName));
DA.SetDataList(ind, _forwardKinematics.PosedExternalAxisPlanes);
}
if (Params.Output.Any(x => x.NickName.Equality(outputParameters[5].NickName)))
{
ind = Params.Output.FindIndex(x => x.NickName.Equality(outputParameters[5].NickName));
DA.SetData(ind, _pathGenerator.ExternalJointPositions[index]);
}
if (Params.Output.Any(x => x.NickName.Equality(outputParameters[6].NickName)))
{
ind = Params.Output.FindIndex(x => x.NickName.Equality(outputParameters[6].NickName));
DA.SetDataList(ind, _pathGenerator.ExternalJointPositions);
}
if (Params.Output.Any(x => x.NickName.Equality(outputParameters[7].NickName)))
{
ind = Params.Output.FindIndex(x => x.NickName.Equality(outputParameters[7].NickName));
DA.SetDataList(ind, _pathGenerator.ErrorText);
}
DA.SetDataList(outputParameters[8].Name, _pathGenerator.Paths);
}