Hi,
I’m trying to interpolate a Joint in C#, but i can’t find the methodes for interpolating in the IServoController Interface.
Any available code or information how to achieve this would be very appreciated.
1 Like
using Caliburn.Micro;
using System.ComponentModel.Composition;
using VisualComponents.Create3D;
using VisualComponents.UX.Shared;
namespace Plugin.JointedInterpolatedMotion
{
[Export(typeof(IDockableScreen))]
public class InterpolateViewModel: DockableScreen
{
public BindableCollection<double> TargetA { get; set; }
public BindableCollection<double> TargetB { get; set; }
private IConsoleViewModel console;
private IRenderService render;
private Matrix MatrixA;
private Matrix MatrixB;
[ImportingConstructor]
public InterpolateViewModel([Import]IConsoleViewModel Console,
[Import]IRenderService Render)
{
TargetA = new BindableCollection<double>();
TargetB = new BindableCollection<double>();
console = Console;
render = Render;
}
private IRobot getRobot()
{
var robot = IoC.Get<ITeachContext>().ActiveRobot;
return robot;
}
public void GetPoseA()
{
if (getRobot() != null)
{
TargetA.Clear();
MatrixA = getRobot().RobotController.CreateTarget().TargetMatrix;
TargetA.Add(MatrixA.Px);
TargetA.Add(MatrixA.Py);
TargetA.Add(MatrixA.Pz);
TargetA.Add(MatrixA.GetWPR().X);
TargetA.Add(MatrixA.GetWPR().Y);
TargetA.Add(MatrixA.GetWPR().Z);
}
}
public void GetPoseB()
{
if (getRobot() != null)
{
TargetB.Clear();
MatrixB = getRobot().RobotController.CreateTarget().TargetMatrix;
TargetB.Add(MatrixB.Px);
TargetB.Add(MatrixB.Py);
TargetB.Add(MatrixB.Pz);
TargetB.Add(MatrixB.GetWPR().X);
TargetB.Add(MatrixB.GetWPR().Y);
TargetB.Add(MatrixB.GetWPR().Z);
}
}
public void InterpolateAB()
{
var motionPath = getRobot().RobotController.CreateMotionInterpolator();
var motionTarget = getRobot().RobotController.CreateTarget();
// Initial Pose
double[] jointValues = new double[] { 0.0, 0.0, 90.0, 0.0, 0.0, 0.0 };
motionTarget.SetAllJointValues(jointValues);
// Target A
motionTarget.TargetMatrix = MatrixA;
motionTarget.CurrentConfiguration = 0;
motionTarget.MotionType = MotionType.Joint;
motionPath.AddTarget(motionTarget);
// Target B
motionTarget.TargetMatrix = MatrixB;
motionTarget.CurrentConfiguration = 0;
motionTarget.MotionType = MotionType.Joint;
motionPath.AddTarget(motionTarget);
motionTarget.ConfigurationMode = ConfigurationMode.Fixed;
var startTime = motionPath.GetCycleTimeAt(0);
var endTime = motionPath.GetCycleTimeAt(motionPath.Targets.Count - 1);
var configWarning = motionTarget.GetConfigurationWarnings(motionTarget.CurrentConfiguration);
for (double time = 0; time < endTime; time += 0.1)
{
motionPath.Interpolate(time, ref motionTarget);
UpdateRobotPositionOnScreenRender(time, startTime, endTime, motionTarget);
console.AppendMessage("configwarning: " + configWarning.ToString());
}
}
private void UpdateRobotPositionOnScreenRender(double time, double startTime, double endTime, IMotionTarget motionTarget)
{
var motionTester = getRobot().RobotController.GetMotionTester();
motionTester.CurrentTarget = motionTarget;
if (time == startTime)
{
console.AppendMessage("Start Time: "+ time.ToString());
console.AppendMessage(motionTarget.ConfigurationMode.ToString());
}
if (time < endTime)
{
console.AppendMessage("Time: " + time.ToString());
console.AppendMessage(motionTarget.ConfigurationMode.ToString());
}
}
}
}
Hope the above code helped.
4 Likes
Hello everyone,
I was testing the above-mentioned code for my own application and found some strange behavior for ptp-motions.
I have two statements defined as shown in the attached images: One linear and one ptp-motion with set jointValues. When I want to go from linear statement to ptp using ptp-motionType, the robot actually does a 360 degree turn with its first axis, although it should not be the case, given the set joint values.
Does anyone know why the Intpolator shows this behavior? If yes, how can I tell the jointTurn/jointvalues target for a ptp-motion interpolation? I tried it with motionTarget.SetAllJointTurns(), but it didn’t work for me.
This behavior shows with manually created statements as well as statements created with .net API.
public bool InterpolateAB(Matrix startPosition, Matrix endPosition, MotionType motionType, IRobotFrame tool1, IRobotFrame tool2, int configuration = -1)
{
IMessageService ms = IoC.Get<IMessageService>();
IApplication app = IoC.Get<IApplication>();
ISimWorld world = app.World;
ISimulation sim = app.Simulation;
ISimulationService sims = IoC.Get<ISimulationService>();
IRobot robot = IoC.Get<ITeachContext>().ActiveRobot;
IRobotExecutor executor = robot.Executor;
IProgram robotProgram = executor.Program;
var mainroutine = robotProgram.MainRoutine;
IRobotController robCont = robot.RobotController;
Matrix Einheitsmatrix = new Matrix();
Einheitsmatrix.SetP(new Vector3 { W = 1, X = 0, Y = 0, Z = 0 });
Einheitsmatrix.SetWPR(new Vector3 { W = 1, X = 0, Y = 0, Z = 0 });
var motionPath = Find_Robot().GetRobot().RobotController.CreateMotionInterpolator();
var motionTarget = Find_Robot().GetRobot().RobotController.CreateTarget();
var motionTargetEnde = Find_Robot().GetRobot().RobotController.CreateTarget();
var motionTargetInterpolate = Find_Robot().GetRobot().RobotController.CreateTarget();
motionTarget.TargetMode = TargetMode.RobotRoot;
motionTargetEnde.TargetMode = TargetMode.RobotRoot;
// Target A
motionTarget.TargetMatrix = startPosition;
motionTarget.CurrentConfiguration = robCont.CurrentConfiguration;
motionTarget.MotionType = motionType;
motionTarget.BaseMatrix = Einheitsmatrix;
motionTarget.ToolMatrix = tool1.TransformationInReference;
motionTarget.JointTurnMode = JointTurnMode.NearestWithinLimits;
motionTarget.TargetMode = TargetMode.RobotRoot;
motionTarget.TargetMatrix = startPosition;
motionPath.AddTarget(motionTarget);
// Target B
motionTargetEnde.TargetMatrix = endPosition;
if (configuration == -1)
{
motionTargetEnde.CurrentConfiguration = 1;
}
else
{
motionTargetEnde.CurrentConfiguration = configuration;
}
motionTargetEnde.MotionType = motionType;
motionTargetEnde.BaseMatrix = Einheitsmatrix;
motionTargetEnde.ToolMatrix = tool2.TransformationInReference;
motionTargetEnde.JointTurnMode = JointTurnMode.Fixed;
List<int> jointTurns = new List<int>();
jointTurns.Add(0);
jointTurns.Add(0);
jointTurns.Add(0);
jointTurns.Add(0);
jointTurns.Add(0);
jointTurns.Add(0);
motionTargetEnde.SetAllJointTurns(jointTurns); // seems to have no effect
int[] jTurns = motionTargetEnde.GetAllJointTurns();
int i = 0;
foreach (int turn in jTurns)
{
ms.AppendMessage($"Turn {i}: {turn}", MessageLevel.Warning);
i++;
}
motionTargetEnde.UseJointValues = true; // seems to have no effect
motionTargetEnde.ConfigurationMode = ConfigurationMode.FollowFrame;
motionTargetEnde.TargetMatrix = endPosition;
motionPath.AddTarget(motionTargetEnde);
motionTarget.ConfigurationMode = ConfigurationMode.Fixed;
double startTime = motionPath.GetCycleTimeAt(0);
double endTime = motionPath.GetCycleTimeAt(motionPath.Targets.Count - 1);
bool collisions = false;
ConfigurationWarnings configWarning = motionTarget.GetConfigurationWarnings(motionTarget.CurrentConfiguration);
for (double time = 0; time < endTime; time += 0.1)
{
_ = motionPath.Interpolate(time, ref motionTargetInterpolate);
if (time == 0)
{
collisions = UpdateRobotPositionOnScreenRender(time, startTime, endTime, motionTargetInterpolate, tool2);
}
else
{
collisions = UpdateRobotPositionOnScreenRender(time, startTime, endTime, motionTargetInterpolate, tool2);
}
ms.AppendMessage("configwarning: " + configWarning.ToString(), MessageLevel.Warning);
if (collisions) { break; }
}
motionTarget.Dispose();
motionTargetEnde.Dispose();
motionTargetInterpolate.Dispose();
motionPath.Dispose();
return collisions;
}
private bool UpdateRobotPositionOnScreenRender(double time, double startTime, double endTime, IMotionTarget motionTarget, IRobotFrame tool)
{
IApplication app = IoC.Get<IApplication>();
ISimWorld world = app.World;
ISimulation sim = app.Simulation;
ISimulationService sims = IoC.Get<ISimulationService>();
IMessageService ms = IoC.Get<IMessageService>();
IMotionTester motionTester = Find_Robot().GetRobot().RobotController.GetMotionTester();
Matrix mtx = motionTarget.TargetMatrix;
motionTester.CurrentTarget = motionTarget;
IRobot robot = IoC.Get<ITeachContext>().ActiveRobot;
IRobotExecutor executor = robot.Executor;
IProgram robotProgram = executor.Program;
IRoutine mainroutine = robotProgram.MainRoutine;
IRobotController robCont = robot.RobotController;
IRoutine routine_temp = robotProgram.Routines.ToList().Find(item => item.Name == "temp");
if (routine_temp == null)
{
routine_temp = robotProgram.AddRoutine("temp");
}
ILinearMotionStatement statement = (ILinearMotionStatement)Create_Statement(routine_temp, "VC_STATEMENT_LINMOTION", tool, motionTarget.TargetMatrix.GetP(), motionTarget.TargetMatrix.GetWPR(), 0, false, false);
_ = MoveRobotToStatementPosition(statement, robCont.CurrentConfiguration);
bool collisions = CollisionCheck(false, true);
// Output
ms.AppendMessage("_____________________________________ " + time.ToString(), MessageLevel.Warning);
if (time == startTime)
{
ms.AppendMessage("Start Time: " + time.ToString(), MessageLevel.Warning);
}
else if (time < endTime)
{
ms.AppendMessage("Time: " + time.ToString(), MessageLevel.Warning);
}
ms.AppendMessage(motionTarget.ConfigurationMode.ToString(), MessageLevel.Warning);
IoC.Get<IRenderService>().RequestRender();
motionTester.Dispose();
return collisions;
}
Best regards,
Joshua
Motion targets and interpolators can be tricky to control. I don’t see a clear error in your code that could cause turns to behave like that. SetAllJointTurns() should work and you set target matrix after that which is also correct. Some of the other properties that you set on the target might throw it off somehow. Could you test the function below to convert PTP statement into motion target and see if it makes a difference? I tested this snippet on 4.4 and for me it seemed to return correct joint positions for PTP when I had either default turns or some non-zero turns programmed into the PTP.
public void PtpStatementToMotionTarget(IJointMotionStatement stmt, ref IMotionTarget mt)
{
mt.MotionType = MotionType.Joint;
mt.SetAllJointValues(stmt.Positions[0].JointValues); // This controls joint turns
if (stmt.Base != null)
{
mt.BaseName = stmt.Base.Name;
}
else
{
mt.BaseName = "";
}
if (stmt.Tool != null)
{
mt.ToolName = stmt.Tool.Name;
}
else
{
mt.ToolName = "";
}
mt.ExternalTcp = stmt.ExternalTcp;
mt.CurrentConfiguration = stmt.Configuration;
mt.JointSpeed = stmt.JointSpeed * 100.0;
mt.AccuracyMethod = stmt.AccuracyMethod;
mt.AccuracyValue = stmt.AccuracyValue;
mt.TargetMatrix = stmt.Positions[0].TransformationInReference;
}
-k
4 Likes