[Mohamed El-Hamzawy]

Hello RoboDK Community,

目前我从事一个项目和我来一个cross a bit of a roadblock. I'm trying to create jogging functionality using the API, but I'm finding it a bit tricky.
What I'm aiming for are two main functions:

1. MoveTcp(Direction, velocity, reference_frame, acceleration)RotateTcp(Direction, velocity, acceleration, reference_frame)

These should allow for smooth, linear robot movement at a specified speed and acceleration in a given direction. The key point here is that I'm looking for a continuous motion, not one made up of incremental steps.

I've tried creating a list of waypoints and adding a program with some rounding to create this effect, but unfortunately, this approach proved to be slower than necessary for my use case.

One workaround I've found helpful for the MoveTcp function was to calculate the furthest reachable point along a line in a particular direction, achieved by iterating and using SolveIK to ensure a non-null return. While this method seems to work well for MoveTcp, it doesn't guarantee the movement direction for RotateTcp due to the inherent rotational aspect.

我真正y appreciate it if anyone could provide some advice or point me towards any resources that could help me overcome this hurdle. I'm sure I'm not the first person to encounter this, and any suggestions would be welcome.

[Mohamed El-Hamzawy]

Nothing?

[Albert]

I understand you achieved what you were trying to do but it is a bit slow.

Does it happen in simulation more or when you are connected to the robot? Can you share the code you are using? We may be able to help you better.

[Mohamed El-Hamzawy]

First of all, thanks for the reply and apologies for my late response. So currently I am using Robodk in simulation only. Here is my current implementation for this. For translation, this implementation though is clearly not the best, serves the purpose pretty well. But for the rotation around tcp is where the problems arise. In this particular case, I needed to use MoveL function so that the TCP is fixed. Some times in rotation this does not follow the speed expected instead it jumps to the final position. Further more I am not able to move to the final rotation position because if I just MoveL like this, it can go to the final position from either direction. This is why I had to implement this hacky solution

here is my current code:

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);
double maxRotationRadians = Math.PI * 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);
}

[Albert]

I recommend you to implement a solution like this one to translate and rotate around a static coordinate system:
//www.sinclairbody.com/forum/Thread-Rotate-a...7#pid15087

You should not need to keep a count of totalRotation if you move the pose by increments. Each time you rotate it will apply the rotation to the pose and you should not need this variable.