protected override void SolveInstance(IGH_DataAccess DA)
{
Manipulator manipulator = null;
if (!DA.GetData(0, ref manipulator))
{
manipulator = Manipulator.Default;
AddRuntimeMessage(GH_RuntimeMessageLevel.Warning, "No robot system defined, using default");
} // Set Robot.
if (!DA.GetData(1, ref c_Target))
{
return; // Get the target.
}
c_Robot = (Manipulator)manipulator.Duplicate();
c_Target = (Target)c_Target.Duplicate();
if (c_Target.Tool != null)
{
c_Tool = c_Target.Tool; // Get the tool from the target.
}
Manipulator.ManipulatorPose pose = new Manipulator.ManipulatorPose(c_Robot, c_Target);
// Handle errors
List <string> log = new List <string>();
if (pose.OverHeadSig)
{
log.Add("Close to overhead singularity.");
}
if (pose.WristSing)
{
log.Add("Close to wrist singularity.");
}
if (pose.OutOfReach)
{
log.Add("Target out of range.");
}
if (pose.OutOfRoation)
{
log.Add("Joint out of range.");
}
if (c_Pose != null)
{
c_Robot.SetPose(c_Pose, checkValidity: true);
}
c_Robot.SetPose(pose, checkValidity: true);
if (c_Robot.CurrentPose.IsValid)
{
c_Pose = c_Robot.CurrentPose;
}
Plane flange = (c_Robot.CurrentPose.IsValid)? c_Robot.CurrentPose.Flange: Plane.Unset;
double[] selectedAngles = c_Robot.CurrentPose.Angles;
// Set output
DA.SetData("Flange", flange);
DA.SetDataList("Angles", selectedAngles);
DA.SetDataList("Log", log);
if (c_PoseOut)
{
DA.SetData("Robot Pose", c_Robot.CurrentPose);
}
// Update and display data
c_Robot.UpdatePose();
c_Robot.GetBoundingBox(Transform.Identity);
c_Tool.UpdatePose(c_Robot);
c_Tool.GetBoundingBox(Transform.Identity);
}