Jogging functionality through the API - Printable Version

Code:
private double[] GetMaxReachableTranslationPose(TcpDirection direction)
{
Mat currentPoseMat = _robotItem!.Pose();
var currentPose = currentPoseMat.Pos();

while (true)
{
switch (direction)
{
case TcpDirection.MoveXUp:
currentPose[0] += 1;
break;
case TcpDirection.MoveXDown:
currentPose[0] -= 1;
break;
case TcpDirection.MoveYUp:
currentPose[1] += 1;
break;
case TcpDirection.MoveYDown:
currentPose[1] -= 1;
break;
case TcpDirection.MoveZUp:
currentPose[2] += 1;
break;
case TcpDirection.MoveZDown:
currentPose[2] -= 1;
break;
}

Mat try_pose = new Mat(currentPoseMat);
try_pose.setPos(currentPose);
//Solve the inverse kinematics for the new pose
double[]? newJoints = _robotItem.SolveIK(try_pose);

//If the new pose is not reachable, break the loop
if (newJoints == null)
break;

currentPoseMat.setPos(currentPose);

}
//Return the maximum reachable pose
var result = ToMeterRadian(currentPoseMat.ToTxyzRxyz());
return result;
}


private double[] GetMaxReachableRotationPose(TcpDirection direction)
{
double angleRadians = Math.PI / 180; // increment by 1 degree each time
Mat rotationMatrix = new Mat(4, 4);
双maxRotationRadians =数学。π* 10/11;//maximum rotation is one full revolution
double totalRotation = 0; // variable to track the total rotation applied
Mat currentPoseMat = _robotItem!.Pose();

while (totalRotation <= maxRotationRadians)
{
switch (direction)
{
case TcpDirection.MoveXUp:
rotationMatrix = Mat.rotx(angleRadians);
break;
case TcpDirection.MoveXDown:
rotationMatrix = Mat.rotx(-angleRadians);
break;
case TcpDirection.MoveYUp:
rotationMatrix = Mat.roty (angleRadians);
break;
case TcpDirection.MoveYDown:
rotationMatrix = Mat.roty(-angleRadians);
break;
case TcpDirection.MoveZUp:
rotationMatrix = Mat.rotz(angleRadians);
break;
case TcpDirection.MoveZDown:
rotationMatrix = Mat.rotz(-angleRadians);
break;
}

//Calculate the target pose
Mat targetPose = currentPoseMat * rotationMatrix;

//Solve the inverse kinematics for the new pose
double[]? newJoints = _robotItem.SolveIK(targetPose);

//If the new pose is not reachable, break the loop
if (newJoints == null)
break;

currentPoseMat = targetPose;
totalRotation += angleRadians; // increment the total rotation
}

//Return the maximum reachable pose
var result = ToMeterRadian(currentPoseMat.ToTxyzRxyz());
return result;
}


public void MoveTcp(TcpDirection direction, double velocityMps, double accelerationMps2)
{
AssertConnected("MoveTcp");
AssertBrakeReleased();
AssertLinearVelocityAccelerationWithinRange(velocityMps, accelerationMps2);

var max_reach = GetMaxReachableTranslationPose(direction);
try
{
MoveLToPose(max_reach, velocityMps, accelerationMps2, false);
}
catch(Exception ex)
{
throw new RobotException(this, -1, ex.Message);
}
}


public void RotationTCP(TcpDirection direction, double velocityMps, double accelerationMps2)
{
AssertConnected("RotationTCP");
AssertBrakeReleased();
AssertLinearVelocityAccelerationWithinRange(velocityMps, accelerationMps2);

var max_reach = GetMaxReachableRotationPose(direction);
try
{
MoveLToPose(max_reach, velocityMps, accelerationMps2, false);
}
catch (Exception ex)
{
throw new RobotException(this, -1, ex.Message);
}
}

public void MoveLToTarget(double[] target, double velocityMps, double accelerationMps2, bool blocking)
{
AssertConnected("MoveJToTarget");
AssertBrakeReleased();
AssertValidTarget(target);
AssertLinearVelocityAccelerationWithinRange(velocityMps, accelerationMps2);

var velocity_mm_s = velocityMps * 1e3;
var acceleration_mm_s2 = accelerationMps2 * 1e3;
target = ToDegree(target);

_robotItem!.SetSpeed(velocity_mm_s, acceleration_mm_s2, -1, -1);
_robotItem!.MoveL(target, blocking);
}