4.2 Robot Motion Control and Status
4.2.1 Getting Robot Parameters
4.2.1.1 Getting OVC Overall Velocity Coefficient
| Method Name | Motion.GetOVC() |
|---|---|
| Description | Gets the current robot's OVC (Overall Velocity Control) global velocity ratio, with a range of 0~1. |
| Request Parameters | None |
| Return Value | double: Global velocity ratio value StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
4.2.1.2 Getting OAC Overall Acceleration Coefficient
| Method Name | Motion.GetOAC() |
|---|---|
| Description | Gets the current robot's OAC (Overall Acceleration Control) global acceleration ratio, with a range of 0~1.2. |
| Request Parameters | None |
| Return Value | double: Global acceleration ratio value StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
4.2.1.3 Getting the Current TF
| Method Name | Motion.GetTF() |
|---|---|
| Description | Gets the current TF (Tool Frame) tool coordinate system index used by the robot, with a range of 0~10. |
| Request Parameters | None |
| Return Value | int: TF tool coordinate system index StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
4.2.1.4 Getting the Current UF
| Method Name | Motion.GetUF() |
|---|---|
| Description | Gets the current UF (User Frame) user coordinate system index used by the robot, with a range of 0~10. |
| Request Parameters | None |
| Return Value | int: UF user coordinate system index StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
4.2.1.5 Getting the Current TCS Teaching Coordinate System
| Method Name | Motion.GetTCS() |
|---|---|
| Description | Gets the current TCS (Teach Coordinate System) teaching coordinate system type used by the robot, see TCSType for details. |
| Request Parameters | None |
| Return Value | TCSType: TCS teaching coordinate system type enum value StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
Example Code
cs
using Agilebot.IR;
using Agilebot.IR.Types;
public class GetMotionParameters
{
public static StatusCode Run(string controllerIP, bool useLocalProxy = true)
{
// [ZH] 初始化捷勃特机器人
// [EN] Initialize the Agilebot robot
Arm controller = new Arm(controllerIP, useLocalProxy);
// [ZH] 连接捷勃特机器人
// [EN] Connect to the Agilebot robot
StatusCode code = controller.ConnectSync();
Console.WriteLine(code != StatusCode.OK ? code.GetDescription() : "连接成功/Successfully connected.");
if (code != StatusCode.OK)
{
return code;
}
try
{
// [ZH] 获取 OVC 全局速度比率
// [EN] Get OVC global speed ratio
double ovc;
(ovc, code) = controller.Motion.GetOVC();
if (code == StatusCode.OK)
{
Console.WriteLine($"OVC = {ovc}");
}
else
{
Console.WriteLine($"获取OVC失败: {code.GetDescription()}");
}
// [ZH] 获取 OAC 全局加速度比率
// [EN] Get OAC global acceleration ratio
double oac;
(oac, code) = controller.Motion.GetOAC();
if (code == StatusCode.OK)
{
Console.WriteLine($"OAC = {oac}");
}
else
{
Console.WriteLine($"获取OAC失败: {code.GetDescription()}");
}
// [ZH] 获取当前使用的 TF
// [EN] Get current TF (Tool Frame)
int tf;
(tf, code) = controller.Motion.GetTF();
if (code == StatusCode.OK)
{
Console.WriteLine($"TF = {tf}");
}
else
{
Console.WriteLine($"获取TF失败: {code.GetDescription()}");
}
// [ZH] 获取当前使用的 UF
// [EN] Get current UF (User Frame)
int uf;
(uf, code) = controller.Motion.GetUF();
if (code == StatusCode.OK)
{
Console.WriteLine($"UF = {uf}");
}
else
{
Console.WriteLine($"获取UF失败: {code.GetDescription()}");
}
// [ZH] 获取当前使用的 TCS 示教坐标系
// [EN] Get current TCS teaching coordinate system
TCSType tcs;
(tcs, code) = controller.Motion.GetTCS();
if (code == StatusCode.OK)
{
Console.WriteLine($"TCSType = {tcs}");
}
else
{
Console.WriteLine($"获取TCS失败: {code.GetDescription()}");
}
// [ZH] 获取机器人软限位
// [EN] Get robot soft limits
List<List<double>> softLimit;
(softLimit, code) = controller.Motion.GetUserSoftLimit();
if (code == StatusCode.OK)
{
Console.WriteLine("软限位信息:");
for (int i = 0; i < softLimit.Count; i++)
{
Console.WriteLine($"轴{i + 1}: 下限={softLimit[i][0]}, 上限={softLimit[i][1]}");
}
}
else
{
Console.WriteLine($"获取软限位失败: {code.GetDescription()}");
}
}
catch (Exception ex)
{
Console.WriteLine($"执行过程中发生异常/Exception occurred during execution: {ex.Message}");
code = StatusCode.OtherReason;
}
finally
{
// [ZH] 关闭连接
// [EN] Close the connection
StatusCode disconnectCode = controller.Disconnect();
Console.WriteLine(
disconnectCode != StatusCode.OK ? disconnectCode.GetDescription() : "Successfully disconnected."
);
}
return code;
}
}4.2.2 Setting Robot Parameters
4.2.2.1 Setting OVC Overall Velocity Coefficient
| Method Name | Motion.SetOVC( double value ) |
|---|---|
| Description | Sets the current robot's OVC (Overall Velocity Control) global velocity ratio. |
| Request Parameters | value : double velocity ratio, range 0~1 |
| Return Value | StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
4.2.2.2 Setting OAC Overall Acceleration Coefficient
| Method Name | Motion.SetOAC( double value ) |
|---|---|
| Description | Sets the current robot's OAC (Overall Acceleration Control) global acceleration ratio. |
| Request Parameters | value : double acceleration ratio, range 0~1.2 |
| Return Value | StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
4.2.2.3 Setting the Current TF Tool Coordinate System Index
| Method Name | Motion.SetTF( int value ) |
|---|---|
| Description | Sets the current TF (Tool Frame) tool coordinate system index. |
| Request Parameters | value : int TF index, range 0~10 |
| Return Value | StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
4.2.2.4 Setting the Current UF User Coordinate System Index
| Method Name | Motion.SetUF( int value ) |
|---|---|
| Description | Sets the current UF (User Frame) user coordinate system index. |
| Request Parameters | value : int UF index, range 0~10 |
| Return Value | StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
4.2.2.5 Setting the Current TCS Teaching Coordinate System
| Method Name | Motion.SetTCS( TCSType value ) |
|---|---|
| Description | Sets the current TCS (Teach Coordinate System) teaching coordinate system. |
| Request Parameters | value : TCSType TCS teaching coordinate system type |
| Return Value | StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
Example Code
cs
using Agilebot.IR;
using Agilebot.IR.Types;
public class SetMotionParameters
{
public static StatusCode Run(string controllerIP, bool useLocalProxy = true)
{
// [ZH] 初始化捷勃特机器人
// [EN] Initialize the Agilebot robot
Arm controller = new Arm(controllerIP, useLocalProxy);
// [ZH] 连接捷勃特机器人
// [EN] Connect to the Agilebot robot
StatusCode code = controller.ConnectSync();
Console.WriteLine(code != StatusCode.OK ? code.GetDescription() : "连接成功/Successfully connected.");
if (code != StatusCode.OK)
{
return code;
}
try
{
// [ZH] 设置 OVC 全局速度比率
// [EN] Set OVC global speed ratio
code = controller.Motion.SetOVC(0.5);
if (code == StatusCode.OK)
{
Console.WriteLine("设置OVC成功");
}
else
{
Console.WriteLine($"设置OVC失败: {code.GetDescription()}");
}
// [ZH] 设置 OAC 全局加速度比率
// [EN] Set OAC global acceleration ratio
code = controller.Motion.SetOAC(0.8);
if (code == StatusCode.OK)
{
Console.WriteLine("设置OAC成功");
}
else
{
Console.WriteLine($"设置OAC失败: {code.GetDescription()}");
}
// [ZH] 设置当前使用的 TF 用户坐标系编号
// [EN] Set current TF (Tool Frame) user coordinate system number
code = controller.Motion.SetTF(2);
if (code == StatusCode.OK)
{
Console.WriteLine("设置TF成功");
}
else
{
Console.WriteLine($"设置TF失败: {code.GetDescription()}");
}
// [ZH] 设置当前使用的 UF 工具坐标系编号
// [EN] Set current UF (User Frame) tool coordinate system number
code = controller.Motion.SetUF(1);
if (code == StatusCode.OK)
{
Console.WriteLine("设置UF成功");
}
else
{
Console.WriteLine($"设置UF失败: {code.GetDescription()}");
}
// [ZH] 设置当前使用的 TCS 示教坐标系
// [EN] Set current TCS teaching coordinate system
code = controller.Motion.SetTCS(TCSType.TOOL);
if (code == StatusCode.OK)
{
Console.WriteLine("设置TCS成功");
}
else
{
Console.WriteLine($"设置TCS失败: {code.GetDescription()}");
}
// [ZH] 设置UDP位置控制的相关参数
// [EN] Set UDP position control related parameters
code = controller.Motion.SetPositionTrajectoryParams(10, 20, 10, 10);
if (code == StatusCode.OK)
{
Console.WriteLine("设置位置控制参数成功");
}
else
{
Console.WriteLine($"设置位置控制参数失败: {code.GetDescription()}");
}
}
catch (Exception ex)
{
Console.WriteLine($"执行过程中发生异常/Exception occurred during execution: {ex.Message}");
code = StatusCode.OtherReason;
}
finally
{
// [ZH] 关闭连接
// [EN] Close the connection
StatusCode disconnectCode = controller.Disconnect();
Console.WriteLine(
disconnectCode != StatusCode.OK ? disconnectCode.GetDescription() : "Successfully disconnected."
);
}
return code;
}
}4.2.3 Converting Cartesian Position to Joint Values
| Method Name | Motion.ConvertCartToJoint( MotionPose pose , int ufIndex = 0, int tfIndex = 0 ) |
|---|---|
| Description | Converts pose data from Cartesian coordinates to joint coordinates. |
| Request Parameters | pose : MotionPose Robot pose data ufIndex : int User coordinate system index, default is 0 tfIndex : int Tool coordinate system index, default is 0 |
| Return Value | MotionPose: Converted robot pose data StatusCode: Conversion operation execution result |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
Example Code
cs
using Agilebot.IR;
using Agilebot.IR.Motion;
using Agilebot.IR.Types;
public class ConvertCartToJoint
{
public static StatusCode Run(string controllerIP, bool useLocalProxy = true)
{
// [ZH] 初始化捷勃特机器人
// [EN] Initialize the Agilebot robot
Arm controller = new Arm(controllerIP, useLocalProxy);
// [ZH] 连接捷勃特机器人
// [EN] Connect to the Agilebot robot
StatusCode code = controller.ConnectSync();
Console.WriteLine(code != StatusCode.OK ? code.GetDescription() : "连接成功/Successfully connected.");
if (code != StatusCode.OK)
{
return code;
}
try
{
// [ZH] 创建笛卡尔位姿
// [EN] Create Cartesian pose
MotionPose motionPose = new MotionPose();
motionPose.Pt = PoseType.Cart;
motionPose.CartData.Position = new Position
{
X = 300,
Y = 300,
Z = 300,
A = 0,
B = 0,
C = 0,
};
motionPose.CartData.Posture = new Posture
{
WristFlip = 1,
ArmUpDown = 1,
ArmBackFront = 1,
ArmLeftRight = 1,
TurnCircle = new List<int>(9) { 0, 0, 0, 0, 0, 0, 0, 0, 0 },
};
// [ZH] 将笛卡尔点位转换成关节值点位
// [EN] Convert Cartesian pose to joint pose
MotionPose convertPose;
(convertPose, code) = controller.Motion.ConvertCartToJoint(motionPose);
if (code == StatusCode.OK)
{
Console.WriteLine("笛卡尔转关节成功:");
Console.WriteLine(
$"关节值: J1={convertPose.Joint.J1}, J2={convertPose.Joint.J2}, J3={convertPose.Joint.J3}, J4={convertPose.Joint.J4}, J5={convertPose.Joint.J5}, J6={convertPose.Joint.J6}"
);
}
else
{
Console.WriteLine($"笛卡尔转关节失败: {code.GetDescription()}");
}
}
catch (Exception ex)
{
Console.WriteLine($"执行过程中发生异常/Exception occurred during execution: {ex.Message}");
code = StatusCode.OtherReason;
}
finally
{
// [ZH] 关闭连接
// [EN] Close the connection
StatusCode disconnectCode = controller.Disconnect();
Console.WriteLine(
disconnectCode != StatusCode.OK ? disconnectCode.GetDescription() : "Successfully disconnected."
);
}
return code;
}
}4.2.4 Converting Joint Values to Cartesian Position
| Method Name | Motion.ConvertJointToCart( MotionPose pose , int ufIndex = 0, int tfIndex = 0 ) |
|---|---|
| Description | Converts pose data from joint coordinates to Cartesian coordinates. |
| Request Parameters | pose : MotionPose Robot pose data ufIndex : int User coordinate system index, default is 0 tfIndex : int Tool coordinate system index, default is 0 |
| Return Value | MotionPose: Converted robot pose data StatusCode: Conversion operation execution result |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
Example Code
cs
using Agilebot.IR;
using Agilebot.IR.Motion;
using Agilebot.IR.Types;
public class ConvertJointToCart
{
public static StatusCode Run(string controllerIP, bool useLocalProxy = true)
{
// [ZH] 初始化捷勃特机器人
// [EN] Initialize the Agilebot robot
Arm controller = new Arm(controllerIP, useLocalProxy);
// [ZH] 连接捷勃特机器人
// [EN] Connect to the Agilebot robot
StatusCode code = controller.ConnectSync();
Console.WriteLine(code != StatusCode.OK ? code.GetDescription() : "连接成功/Successfully connected.");
if (code != StatusCode.OK)
{
return code;
}
try
{
// [ZH] 创建关节位姿
// [EN] Create joint pose
MotionPose motionPose = new MotionPose();
motionPose.Pt = PoseType.Joint;
motionPose.Joint = new Joint
{
J1 = 0,
J2 = 0,
J3 = 60,
J4 = 60,
J5 = 0,
J6 = 0,
};
// [ZH] 将关节值点位转换成笛卡尔点位
// [EN] Convert joint pose to Cartesian pose
MotionPose convertPose;
(convertPose, code) = controller.Motion.ConvertJointToCart(motionPose);
if (code == StatusCode.OK)
{
Console.WriteLine("关节转笛卡尔成功:");
Console.WriteLine(
$"位置: X={convertPose.CartData.Position.X}, Y={convertPose.CartData.Position.Y}, Z={convertPose.CartData.Position.Z}"
);
Console.WriteLine(
$"姿态: A={convertPose.CartData.Position.A}, B={convertPose.CartData.Position.B}, C={convertPose.CartData.Position.C}"
);
}
else
{
Console.WriteLine($"关节转笛卡尔失败: {code.GetDescription()}");
}
}
catch (Exception ex)
{
Console.WriteLine($"执行过程中发生异常/Exception occurred during execution: {ex.Message}");
code = StatusCode.OtherReason;
}
finally
{
// [ZH] 关闭连接
// [EN] Close the connection
StatusCode disconnectCode = controller.Disconnect();
Console.WriteLine(
disconnectCode != StatusCode.OK ? disconnectCode.GetDescription() : "Successfully disconnected."
);
}
return code;
}
}4.2.5 Moving the Robot End Effector to a Specified Position
| Method Name | Motion.MoveJoint( MotionPose pose , double vel = 1, double acc = 1 ) |
|---|---|
| Description | Moves the robot end effector to a specified position, using the fastest path (joint motion). |
| Request Parameters | pose : MotionPose Target position coordinates in Cartesian space or joint coordinate system vel : double Motion speed, range 0~1, representing multiple of maximum speed acc : double Acceleration, range 0~1.2, representing multiple of maximum acceleration |
| Return Value | StatusCode: Motion command execution result |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
Example Code
cs
using Agilebot.IR;
using Agilebot.IR.Motion;
using Agilebot.IR.Types;
public class MoveJoint
{
public static StatusCode Run(string controllerIP, bool useLocalProxy = true)
{
// [ZH] 初始化捷勃特机器人
// [EN] Initialize the Agilebot robot
Arm controller = new Arm(controllerIP, useLocalProxy);
// [ZH] 连接捷勃特机器人
// [EN] Connect to the Agilebot robot
StatusCode code = controller.ConnectSync();
Console.WriteLine(code != StatusCode.OK ? code.GetDescription() : "连接成功/Successfully connected.");
if (code != StatusCode.OK)
{
return code;
}
try
{
// [ZH] 创建关节位姿
// [EN] Create joint pose
MotionPose motionPose = new MotionPose();
motionPose.Pt = PoseType.Joint;
motionPose.Joint = new Joint
{
J1 = 10,
J2 = 30,
J3 = 30,
J4 = 0,
J5 = 0,
J6 = 0,
};
// [ZH] 让机器人末端移动到指定的位置
// [EN] Move robot end to specified position
code = controller.Motion.MoveJoint(motionPose, 0.5, 0.8);
if (code == StatusCode.OK)
{
Console.WriteLine("关节运动请求成功");
}
else
{
Console.WriteLine($"关节运动失败: {code.GetDescription()}");
}
}
catch (Exception ex)
{
Console.WriteLine($"执行过程中发生异常/Exception occurred during execution: {ex.Message}");
code = StatusCode.OtherReason;
}
finally
{
// [ZH] 关闭连接
// [EN] Close the connection
StatusCode disconnectCode = controller.Disconnect();
Console.WriteLine(
disconnectCode != StatusCode.OK ? disconnectCode.GetDescription() : "Successfully disconnected."
);
}
return code;
}
}4.2.6 Moving the Robot End Effector Along a Straight Line to a Specified Position
| Method Name | Motion.MoveLine( MotionPose pose , double vel = 100, double acc = 1 ) |
|---|---|
| Description | Moves the robot end effector along a straight line to a specified position, using a linear path between two points. |
| Request Parameters | pose : MotionPose Target position coordinates in Cartesian space or joint coordinate system vel : double Motion speed, range 0~5000mm/s, representing robot end effector movement speed acc : double Acceleration, range 0~1.2, representing multiple of maximum acceleration |
| Return Value | StatusCode: Motion command execution result |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
Example Code
cs
using Agilebot.IR;
using Agilebot.IR.Motion;
using Agilebot.IR.Types;
public class MoveLine
{
public static StatusCode Run(string controllerIP, bool useLocalProxy = true)
{
// [ZH] 初始化捷勃特机器人
// [EN] Initialize the Agilebot robot
Arm controller = new Arm(controllerIP, useLocalProxy);
// [ZH] 连接捷勃特机器人
// [EN] Connect to the Agilebot robot
StatusCode code = controller.ConnectSync();
Console.WriteLine(code != StatusCode.OK ? code.GetDescription() : "连接成功/Successfully connected.");
if (code != StatusCode.OK)
{
return code;
}
try
{
// [ZH] 创建关节位姿
// [EN] Create joint pose
MotionPose motionPose = new MotionPose();
motionPose.Pt = PoseType.Joint;
motionPose.Joint = new Joint
{
J1 = 20,
J2 = 40,
J3 = 40,
J4 = 5,
J5 = 5,
J6 = 5,
};
// [ZH] 让机器人末端沿直线移动到指定的位置
// [EN] Move robot end in straight line to specified position
code = controller.Motion.MoveLine(motionPose, 100, 1.0);
if (code == StatusCode.OK)
{
Console.WriteLine("直线运动请求成功");
}
else
{
Console.WriteLine($"直线运动失败: {code.GetDescription()}");
}
}
catch (Exception ex)
{
Console.WriteLine($"执行过程中发生异常/Exception occurred during execution: {ex.Message}");
code = StatusCode.OtherReason;
}
finally
{
// [ZH] 关闭连接
// [EN] Close the connection
StatusCode disconnectCode = controller.Disconnect();
Console.WriteLine(
disconnectCode != StatusCode.OK ? disconnectCode.GetDescription() : "Successfully disconnected."
);
}
return code;
}
}4.2.7 Moving the Robot End Effector Along an Arc to a Specified Position
| Method Name | Motion.MoveCircle( MotionPose pose1 , MotionPose pose2 , double vel = 100, double acc = 1 ) |
|---|---|
| Description | Moves the robot end effector along an arc to a specified position. |
| Request Parameters | pose1 : MotionPose Robot motion intermediate pose pose2 : MotionPose Robot motion final pose vel : double Motion speed, range 0~5000mm/s, representing robot end effector movement speed acc : double Acceleration, range 0~1.2, representing multiple of maximum acceleration |
| Return Value | StatusCode: Motion command execution result |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
Example Code
cs
using Agilebot.IR;
using Agilebot.IR.Motion;
using Agilebot.IR.Types;
public class MoveCircle
{
public static StatusCode Run(string controllerIP, bool useLocalProxy = true)
{
// [ZH] 初始化捷勃特机器人
// [EN] Initialize the Agilebot robot
Arm controller = new Arm(controllerIP, useLocalProxy);
// [ZH] 连接捷勃特机器人
// [EN] Connect to the Agilebot robot
StatusCode code = controller.ConnectSync();
Console.WriteLine(code != StatusCode.OK ? code.GetDescription() : "连接成功/Successfully connected.");
if (code != StatusCode.OK)
{
return code;
}
try
{
// [ZH] 创建第一个位姿(途径点)
// [EN] Create first pose (waypoint)
MotionPose motionPose1 = new MotionPose();
motionPose1.Pt = PoseType.Joint;
motionPose1.Joint = new Joint
{
J1 = 0,
J2 = 0,
J3 = 60,
J4 = 60,
J5 = 0,
J6 = 0,
};
// [ZH] 创建第二个位姿(终点)
// [EN] Create second pose (endpoint)
MotionPose motionPose2 = new MotionPose();
motionPose2.Pt = PoseType.Joint;
motionPose2.Joint = new Joint
{
J1 = 0,
J2 = 30,
J3 = 70,
J4 = 40,
J5 = 0,
J6 = 0,
};
// [ZH] 让机器人末端沿弧线移动到指定的位置
// [EN] Move robot end in arc to specified position
code = controller.Motion.MoveCircle(motionPose1, motionPose2, 100, 1.0);
if (code == StatusCode.OK)
{
Console.WriteLine("弧线运动请求成功");
}
else
{
Console.WriteLine($"弧线运动失败: {code.GetDescription()}");
}
}
catch (Exception ex)
{
Console.WriteLine($"执行过程中发生异常/Exception occurred during execution: {ex.Message}");
code = StatusCode.OtherReason;
}
finally
{
// [ZH] 关闭连接
// [EN] Close the connection
StatusCode disconnectCode = controller.Disconnect();
Console.WriteLine(
disconnectCode != StatusCode.OK ? disconnectCode.GetDescription() : "Successfully disconnected."
);
}
return code;
}
}4.2.8 Getting the Current Pose of the Robot
| Method Name | Motion.GetCurrentPose( PoseType pt , int ufIndex = 0, int tfIndex = 0 ) |
|---|---|
| Description | Gets the current pose of the robot, which can be pose information in Cartesian space or joint coordinate system. |
| Request Parameters | pt : PoseType Pose type ufIndex : int When using PoseType.CART, user coordinate system index must be provided, default is 0 tfIndex : int When using PoseType.CART, tool coordinate system index must be provided, default is 0 |
| Return Value | MotionPose: Robot pose data StatusCode: Get operation execution result |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
Example Code
cs
using Agilebot.IR;
using Agilebot.IR.Motion;
using Agilebot.IR.Types;
public class GetCurrentPose
{
public static StatusCode Run(string controllerIP, bool useLocalProxy = true)
{
// [ZH] 初始化捷勃特机器人
// [EN] Initialize the Agilebot robot
Arm controller = new Arm(controllerIP, useLocalProxy);
// [ZH] 连接捷勃特机器人
// [EN] Connect to the Agilebot robot
StatusCode code = controller.ConnectSync();
Console.WriteLine(code != StatusCode.OK ? code.GetDescription() : "连接成功/Successfully connected.");
if (code != StatusCode.OK)
{
return code;
}
try
{
// [ZH] 获取机器人的当前位姿(笛卡尔坐标)
// [EN] Get robot current pose (Cartesian coordinates)
MotionPose cartPose;
(cartPose, code) = controller.Motion.GetCurrentPose(PoseType.Cart, 0, 0);
if (code == StatusCode.OK)
{
Console.WriteLine("当前笛卡尔位姿:");
Console.WriteLine(
$"位置: X={cartPose.CartData.Position.X}, Y={cartPose.CartData.Position.Y}, Z={cartPose.CartData.Position.Z}"
);
Console.WriteLine(
$"姿态: A={cartPose.CartData.Position.A}, B={cartPose.CartData.Position.B}, C={cartPose.CartData.Position.C}"
);
}
else
{
Console.WriteLine($"获取笛卡尔位姿失败: {code.GetDescription()}");
}
// [ZH] 获取机器人的当前位姿(关节坐标)
// [EN] Get robot current pose (joint coordinates)
MotionPose jointPose;
(jointPose, code) = controller.Motion.GetCurrentPose(PoseType.Joint, 0, 0);
if (code == StatusCode.OK)
{
Console.WriteLine("当前关节位姿:");
Console.WriteLine(
$"关节值: J1={jointPose.Joint.J1}, J2={jointPose.Joint.J2}, J3={jointPose.Joint.J3}, J4={jointPose.Joint.J4}, J5={jointPose.Joint.J5}, J6={jointPose.Joint.J6}"
);
}
else
{
Console.WriteLine($"获取关节位姿失败: {code.GetDescription()}");
}
}
catch (Exception ex)
{
Console.WriteLine($"执行过程中发生异常/Exception occurred during execution: {ex.Message}");
code = StatusCode.OtherReason;
}
finally
{
// [ZH] 关闭连接
// [EN] Close the connection
StatusCode disconnectCode = controller.Disconnect();
Console.WriteLine(
disconnectCode != StatusCode.OK ? disconnectCode.GetDescription() : "Successfully disconnected."
);
}
return code;
}
}4.2.9 Getting the Robot's DH Parameters
| Method Name | Motion.GetDHParam() |
|---|---|
| Description | Gets the robot's DH (Denavit-Hartenberg) parameters. |
| Request Parameters | None |
| Return Value | List<DHparam>: DH parameter list StatusCode: Get operation execution result |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): Not supported |
Example Code
cs
using Agilebot.IR;
using Agilebot.IR.Types;
public class GetDHParam
{
public static StatusCode Run(string controllerIP, bool useLocalProxy = true)
{
// [ZH] 初始化捷勃特机器人
// [EN] Initialize the Agilebot robot
Arm controller = new Arm(controllerIP, useLocalProxy);
// [ZH] 连接捷勃特机器人
// [EN] Connect to the Agilebot robot
StatusCode code = controller.ConnectSync();
Console.WriteLine(code != StatusCode.OK ? code.GetDescription() : "连接成功/Successfully connected.");
if (code != StatusCode.OK)
{
return code;
}
try
{
// [ZH] 获取机器人的DH参数
// [EN] Get robot DH parameters
List<DHparam> dhParamsList;
(dhParamsList, code) = controller.Motion.GetDHParam(1);
if (code == StatusCode.OK)
{
Console.WriteLine("获取DH参数成功:");
for (int i = 0; i < dhParamsList.Count; i++)
{
var dh = dhParamsList[i];
Console.WriteLine($"轴{i + 1}: Alpha={dh.alpha}, A={dh.a}, D={dh.d}, Offset={dh.offset}");
}
}
else
{
Console.WriteLine($"获取DH参数失败: {code.GetDescription()}");
}
}
catch (Exception ex)
{
Console.WriteLine($"执行过程中发生异常/Exception occurred during execution: {ex.Message}");
code = StatusCode.OtherReason;
}
finally
{
// [ZH] 关闭连接
// [EN] Close the connection
StatusCode disconnectCode = controller.Disconnect();
Console.WriteLine(
disconnectCode != StatusCode.OK ? disconnectCode.GetDescription() : "Successfully disconnected."
);
}
return code;
}
}4.2.10 Setting the Robot's DH Parameters
| Method Name | Motion.SetDHParam( List<DHparam> dHparams ) |
|---|---|
| Description | Sets the robot's DH (Denavit-Hartenberg) parameters. |
| Request Parameters | dHparams : List<DHparam> DH parameter list |
| Return Value | StatusCode: Set operation execution result |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): Not supported |
Example Code
cs
using Agilebot.IR;
using Agilebot.IR.Types;
public class SetDHParam
{
public static StatusCode Run(string controllerIP, bool useLocalProxy = true)
{
// [ZH] 初始化捷勃特机器人
// [EN] Initialize the Agilebot robot
Arm controller = new Arm(controllerIP, useLocalProxy);
// [ZH] 连接捷勃特机器人
// [EN] Connect to the Agilebot robot
StatusCode code = controller.ConnectSync();
Console.WriteLine(code != StatusCode.OK ? code.GetDescription() : "连接成功/Successfully connected.");
if (code != StatusCode.OK)
{
return code;
}
try
{
// [ZH] 先获取当前的DH参数
// [EN] First get current DH parameters
List<DHparam> dhParamsList;
(dhParamsList, code) = controller.Motion.GetDHParam(1);
if (code != StatusCode.OK)
{
Console.WriteLine($"获取DH参数失败: {code.GetDescription()}");
return code;
}
Console.WriteLine("获取DH参数成功,准备设置相同的参数...");
// [ZH] 设置DH参数(这里设置为相同的参数作为示例)
// [EN] Set DH parameters (set same parameters as example)
code = controller.Motion.SetDHParam(dhParamsList);
if (code == StatusCode.OK)
{
Console.WriteLine("设置DH参数成功");
}
else
{
Console.WriteLine($"设置DH参数失败: {code.GetDescription()}");
}
}
catch (Exception ex)
{
Console.WriteLine($"执行过程中发生异常/Exception occurred during execution: {ex.Message}");
code = StatusCode.OtherReason;
}
finally
{
// [ZH] 关闭连接
// [EN] Close the connection
StatusCode disconnectCode = controller.Disconnect();
Console.WriteLine(
disconnectCode != StatusCode.OK ? disconnectCode.GetDescription() : "Successfully disconnected."
);
}
return code;
}
}4.2.11 Getting the Robot Axis Lock Status
| Method Name | Motion.GetDragSet() |
|---|---|
| Description | Gets the current robot axis lock status, which only applies to teaching movements. |
| Request Parameters | None |
| Return Value | DragStatus: Axis lock status, True indicates the axis is movable, False indicates it is locked StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): Not supported |
4.2.12 Setting the Robot Axis Lock Status
| Method Name | Motion.SetDragSet( DragStatus dragStatus ) |
|---|---|
| Description | Sets the current robot axis lock status, which only applies to teaching movements. |
| Request Parameters | dragStatus : DragStatus Axis lock status, default is all True: unlocked state |
| Return Value | StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): Not supported |
4.2.13 Enabling Drag Teaching for the Robot
| Method Name | Motion.EnableDrag( bool dragState ) |
|---|---|
| Description | Enables or disables drag teaching for the robot. |
| Request Parameters | dragState : bool The drag state of the robot, true indicates entering drag mode, false indicates exiting drag mode |
| Return Value | StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): Not supported |
Example Code
cs
using Agilebot.IR;
using Agilebot.IR.Motion;
using Agilebot.IR.Types;
public class DragControl
{
public static StatusCode Run(string controllerIP, bool useLocalProxy = true)
{
// [ZH] 初始化捷勃特机器人
// [EN] Initialize the Agilebot robot
Arm controller = new Arm(controllerIP, useLocalProxy);
// [ZH] 连接捷勃特机器人
// [EN] Connect to the Agilebot robot
StatusCode code = controller.ConnectSync();
Console.WriteLine(code != StatusCode.OK ? code.GetDescription() : "连接成功/Successfully connected.");
if (code != StatusCode.OK)
{
return code;
}
try
{
// [ZH] 获取当前机器人的轴锁定状态
// [EN] Get current robot axis lock status
DragStatus dragStatus;
(dragStatus, code) = controller.Motion.GetDragSet();
if (code == StatusCode.OK)
{
Console.WriteLine("获取轴锁定状态成功:");
Console.WriteLine(
$"X轴: {dragStatus.CartStatus.X}, Y轴: {dragStatus.CartStatus.Y}, Z轴: {dragStatus.CartStatus.Z}"
);
Console.WriteLine($"连续拖动: {dragStatus.IsContinuousDrag}");
}
else
{
Console.WriteLine($"获取轴锁定状态失败: {code.GetDescription()}");
}
// [ZH] 修改当前机器人的轴锁定状态
// [EN] Modify current robot axis lock status
if (code == StatusCode.OK)
{
dragStatus.CartStatus.X = false;
dragStatus.IsContinuousDrag = true;
code = controller.Motion.SetDragSet(dragStatus);
if (code == StatusCode.OK)
{
Console.WriteLine("设置轴锁定状态成功");
}
else
{
Console.WriteLine($"设置轴锁定状态失败: {code.GetDescription()}");
}
}
// [ZH] 启动拖动(注意:实际使用中需要谨慎)
// [EN] Enable drag (Note: use with caution in practice)
if (code == StatusCode.OK)
{
Console.WriteLine("注意:启动拖动功能,请确保安全!");
code = controller.Motion.EnableDrag(true);
if (code == StatusCode.OK)
{
Console.WriteLine("启动拖动成功");
// [ZH] 等待一段时间后停止拖动
// [EN] Wait for a while then stop drag
Console.WriteLine("等待3秒后停止拖动...");
Thread.Sleep(3000);
code = controller.Motion.EnableDrag(false);
if (code == StatusCode.OK)
{
Console.WriteLine("停止拖动成功");
}
else
{
Console.WriteLine($"停止拖动失败: {code.GetDescription()}");
}
}
else
{
Console.WriteLine($"启动拖动失败: {code.GetDescription()}");
}
}
}
catch (Exception ex)
{
Console.WriteLine($"执行过程中发生异常/Exception occurred during execution: {ex.Message}");
code = StatusCode.OtherReason;
}
finally
{
// [ZH] 关闭连接
// [EN] Close the connection
StatusCode disconnectCode = controller.Disconnect();
Console.WriteLine(
disconnectCode != StatusCode.OK ? disconnectCode.GetDescription() : "Successfully disconnected."
);
}
return code;
}
}4.2.14 Entering Real-Time Position Control Mode
| Method Name | Motion.EnterPositionControl() |
|---|---|
| Description | Enters real-time position control mode, allowing precise position control of the robot. |
| Request Parameters | None |
| Return Value | StatusCode: Result of function execution |
| Note | After entering real-time control mode, control commands must be sent via UDP. |
| Compatible robot software version | Collaborative (Copper): v7.5.2.0+ Industrial (Bronze): Not supported |
4.2.15 Exiting Real-Time Position Control Mode
| Method Name | Motion.ExitPositionControl() |
|---|---|
| Description | Exits real-time position control mode, returning to the default robot control state. |
| Request Parameters | None |
| Return Value | StatusCode: Result of function execution |
| Note | After exiting, the robot will no longer accept real-time control commands. |
| Compatible robot software version | Collaborative (Copper): v7.5.2.0+ Industrial (Bronze): Not supported |
4.2.16 Setting Subscription Parameters
| Method Name | Motion.SetUDPFeedbackParams( bool flag , string ip , int interval , int feedbackType , List<int> DOList = null ) |
|---|---|
| Description | Configures the subscription parameters for the robot to push data to a specified IP address. |
| Request Parameters | flag : bool Whether to enable UDP data pushing; ip : string IP address of the recipient; interval : int Interval for sending data (unit: milliseconds); feedbackType : int Feedback data format (0: XML format); DOList : List<int> List of DO signals to be obtained (up to ten, optional) |
| Return Value | StatusCode: Result of function execution |
| Note | The parameter settings are only effective when the UDP data pushing function is enabled. |
| Compatible robot software version | Collaborative (Copper): v7.5.2.0+ Industrial (Bronze): Not supported |
Example Code
cs
using Agilebot.IR;
using Agilebot.IR.Motion;
using Agilebot.IR.Types;
public class PositionControl
{
public static StatusCode Run(string controllerIP, bool useLocalProxy = true)
{
// [ZH] 初始化捷勃特机器人
// [EN] Initialize the Agilebot robot
Arm controller = new Arm(controllerIP, useLocalProxy);
// [ZH] 连接捷勃特机器人
// [EN] Connect to the Agilebot robot
StatusCode code = controller.ConnectSync();
Console.WriteLine(code != StatusCode.OK ? code.GetDescription() : "连接成功/Successfully connected.");
if (code != StatusCode.OK)
{
return code;
}
try
{
// [ZH] 设置UDP反馈参数
// [EN] Set UDP feedback parameters
code = controller.Motion.SetUDPFeedbackParams(true, "192.168.1.1", 10, 0);
if (code == StatusCode.OK)
{
Console.WriteLine("设置UDP反馈参数成功");
}
else
{
Console.WriteLine($"设置UDP反馈参数失败: {code.GetDescription()}");
}
// [ZH] 进入实时位置控制模式
// [EN] Enter real-time position control mode
code = controller.Motion.EnterPositionControl();
if (code == StatusCode.OK)
{
Console.WriteLine("进入实时位置控制模式成功");
// [ZH] 在此可以插入发送UDP数据控制机器人的代码
// [EN] Insert UDP data control code here
Console.WriteLine("注意:在实时位置控制模式下,需要通过UDP发送控制指令");
Console.WriteLine("等待2秒...");
Thread.Sleep(2000);
// [ZH] 退出实时位置控制模式
// [EN] Exit real-time position control mode
code = controller.Motion.ExitPositionControl();
if (code == StatusCode.OK)
{
Console.WriteLine("退出实时位置控制模式成功");
}
else
{
Console.WriteLine($"退出实时位置控制模式失败: {code.GetDescription()}");
}
}
else
{
Console.WriteLine($"进入实时位置控制模式失败: {code.GetDescription()}");
}
}
catch (Exception ex)
{
Console.WriteLine($"执行过程中发生异常/Exception occurred during execution: {ex.Message}");
code = StatusCode.OtherReason;
}
finally
{
// [ZH] 关闭连接
// [EN] Close the connection
StatusCode disconnectCode = controller.Disconnect();
Console.WriteLine(
disconnectCode != StatusCode.OK ? disconnectCode.GetDescription() : "Successfully disconnected."
);
}
return code;
}
}Data Push Description
| Name | Field | Description |
|---|---|---|
| RIst: Cartesian Position | X | Value in the X direction in the tool coordinate system, unit is millimeters |
| Y | Value in the Y direction in the tool coordinate system, unit is millimeters | |
| Z | Value in the Z direction in the tool coordinate system, unit is millimeters | |
| A | Rotation around the X axis in the tool coordinate system, unit is degrees | |
| B | Rotation around the Y axis in the tool coordinate system, unit is degrees | |
| C | Rotation around the Z axis in the tool coordinate system, unit is degrees | |
| AIPos: Joint Position | A1-A6 | Values of the six joints, unit is degrees |
| EIPos: Additional Axis Data | EIPos | Additional axis data |
| WristBtnState: Wrist Button State | Button State | 1 = Button pressed, 0 = Button released |
| DragModel | Drag button state | |
| RcordJoint | Teach record button state | |
| PauseResume | Pause/resume button state | |
| Digout: DO Output | Digout | State of digital output (DO) |
| ProgramStatus: Program Status | ProgId | Program ID |
| Status | Interpreter execution status: 0 = INTERPRETER_IDLE 1 = INTERPRETER_EXECUTE 2 = INTERPRETER_PAUSED | |
| Xpath | Program segment return value, format is program name: line number | |
| IPOC: Timestamp | IPOC | Timestamp |
4.2.17 Getting the Robot's Soft Limits
| Method Name | Motion.GetUserSoftLimit() |
|---|---|
| Description | Gets the current soft limits of the robot. |
| Request Parameters | None |
| Return Value | List<List<double>>: Robot's soft limits, the first layer of the list represents each axis, and the second layer represents the lower and upper limit values of each axis StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
4.2.18 Specifying UDP Position Control Parameters
| Method Name | Motion.SetPositionTrajectoryParams( int maxTimeoutCount , int timeout , double wristElbowThreshold , double shoulderThreshold ) |
|---|---|
| Description | Specifies the parameters related to UDP position control. |
| Request Parameters | maxTimeoutCount : Maximum number of timeouts; timeout : Timeout period (i.e., send interval, default is 20ms); wristElbowThreshold : Threshold for wrist/elbow approaching singularity; shoulderThreshold : Threshold for approaching shoulder singularity; |
| Return Value | StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
4.2.19 Payload-Related Interfaces
4.2.19.1 Getting the Current Active Payload
| Method Name | Motion.Payload.GetCurrentPayload() |
|---|---|
| Description | Gets the currently active payload information. |
| Request Parameters | None |
| Return Value | int: Index of the currently active payload StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
4.2.19.2 Getting the Corresponding Payload
| Method Name | Motion.Payload.GetPayloadById( int index ) |
|---|---|
| Description | Gets the corresponding payload. |
| Request Parameters | index : Payload index |
| Return Value | StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
4.2.19.3 Activating the Corresponding Payload
| Method Name | Motion.Payload.SetCurrentPayload( int index ) |
|---|---|
| Description | Activates the corresponding payload. |
| Request Parameters | index : Payload index |
| Return Value | StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
| Note | The payload ID must exist in the current device. |
4.2.19.4 Getting All Payload Information
| Method Name | Motion.Payload.GetAllPayloadInfo() |
|---|---|
| Description | Gets detailed information of all payloads. |
| Request Parameters | None |
| Return Value | Dictionary<uint, string>: Returns a dictionary of payload information StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
4.2.19.5 Adding a Payload
| Method Name | Motion.Payload.AddPayload( PayloadInfo payload ) |
|---|---|
| Description | Adds a new payload. |
| Request Parameters | payload : PayloadInfo Payload object |
| Return Value | StatusCode: Result of function execution |
| Note | The new payload ID must not exist in the current device and must be between 1 and 10. |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
4.2.19.6 Deleting a Specified Payload
| Method Name | Motion.Payload.DeletePayload( int index ) |
|---|---|
| Description | Deletes the payload with the specified index. |
| Request Parameters | index : int Payload index |
| Return Value | StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
| Note | Note: The currently active payload cannot be deleted. If you want to delete the active payload, please activate another payload first and then delete the current one. |
4.2.19.7 Updating a Specified Payload
| Method Name | Motion.Payload.UpdatePayload( PayloadInfo payload ) |
|---|---|
| Description | Updates the information of the specified payload. |
| Request Parameters | payload : PayloadInfo Payload object |
| Return Value | StatusCode: Result of function execution |
| Note | The payload ID must exist in the current device. |
| Compatible robot software version | Collaborative (Copper): v7.5.0.0+ Industrial (Bronze): v7.5.0.0+ |
Example Code
cs
using Agilebot.IR;
using Agilebot.IR.Motion;
public class PayloadControl
{
public static StatusCode Run(string controllerIP, bool useLocalProxy = true)
{
// [ZH] 初始化捷勃特机器人
// [EN] Initialize the Agilebot robot
Arm controller = new Arm(controllerIP, useLocalProxy);
// [ZH] 连接捷勃特机器人
// [EN] Connect to the Agilebot robot
StatusCode code = controller.ConnectSync();
Console.WriteLine(code != StatusCode.OK ? code.GetDescription() : "连接成功/Successfully connected.");
if (code != StatusCode.OK)
{
return code;
}
try
{
// [ZH] 获取负载列表
// [EN] Get payload list
Dictionary<int, string> payloadList;
(payloadList, code) = controller.Motion.Payload.GetAllPayloadInfo();
if (code == StatusCode.OK)
{
Console.WriteLine("获取负载列表成功:");
foreach (var p in payloadList)
{
Console.WriteLine($"负载ID: {p.Key}, 描述: {p.Value}");
}
}
else
{
Console.WriteLine($"获取负载列表失败: {code.GetDescription()}");
}
// [ZH] 获取当前激活的负载
// [EN] Get current active payload
int currentPayload;
(currentPayload, code) = controller.Motion.Payload.GetCurrentPayload();
if (code == StatusCode.OK)
{
Console.WriteLine($"当前激活的负载ID: {currentPayload}");
}
else
{
Console.WriteLine($"获取当前负载失败: {code.GetDescription()}");
}
// [ZH] 添加新负载
// [EN] Add new payload
PayloadInfo payload = new()
{
Id = 3,
Comment = "测试负载",
Weight = 1.0,
MassCenter = new()
{
X = 1,
Y = 2,
Z = 3,
},
InertiaMoment = new()
{
LX = 10,
LY = 20,
LZ = 30,
},
};
code = controller.Motion.Payload.AddPayload(payload);
if (code == StatusCode.OK)
{
Console.WriteLine("添加负载成功");
}
else
{
Console.WriteLine($"添加负载失败: {code.GetDescription()}");
}
// [ZH] 设置当前激活的负载
// [EN] Set current active payload
if (code == StatusCode.OK)
{
code = controller.Motion.Payload.SetCurrentPayload(3);
if (code == StatusCode.OK)
{
Console.WriteLine("设置当前负载成功");
}
else
{
Console.WriteLine($"设置当前负载失败: {code.GetDescription()}");
}
}
}
catch (Exception ex)
{
Console.WriteLine($"执行过程中发生异常/Exception occurred during execution: {ex.Message}");
code = StatusCode.OtherReason;
}
finally
{
// [ZH] 关闭连接
// [EN] Close the connection
StatusCode disconnectCode = controller.Disconnect();
Console.WriteLine(
disconnectCode != StatusCode.OK ? disconnectCode.GetDescription() : "Successfully disconnected."
);
}
return code;
}
}4.2.19.8 Checking if Axis 3 is Horizontal
| Method Name | Motion.Payload.CheckAxisThreeHorizontal() |
|---|---|
| Description | Checks if Axis 3 is horizontal. |
| Request Parameters | None |
| Return Value | double: The horizontal angle of Axis 3 StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.2.0+ Industrial (Bronze): Not supported |
| Note | The horizontal angle must be between -1 and 1 to perform payload identification. |
4.2.19.9 Getting the Payload Identification State
| Method Name | Motion.Payload.GetPayloadIdentifyState() |
|---|---|
| Description | Gets the state of payload identification. |
| Request Parameters | None |
| Return Value | PayloadIdentifyState: Payload identification state StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.2.0+ Industrial (Bronze): Not supported |
4.2.19.10 Starting Payload Identification
| Method Name | Motion.Payload.StartPayloadIdentify( double weight , double angle ) |
|---|---|
| Description | Starts payload identification. |
| Request Parameters | weight : double Payload weight (use -1 for unknown weight) angle : double Allowed rotation angle of Axis 6 (30-90 degrees) |
| Return Value | StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.2.0+ Industrial (Bronze): Not supported |
| Note | You must enter the payload identification state before starting payload identification. |
4.2.19.11 Getting the Payload Identification Result
| Method Name | Motion.Payload.PayloadIdentifyResult() |
|---|---|
| Description | Gets the result of payload identification. |
| Request Parameters | None |
| Return Value | PayloadInfo: Payload identification result StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.2.0+ Industrial (Bronze): Not supported |
4.2.19.12 Starting Interference Check for Payload Identification
| Method Name | Motion.Payload.InterferenceCheckForPayloadIdentify( double weight , double angle ) |
|---|---|
| Description | Starts interference check for payload identification to check for potential collisions. |
| Request Parameters | weight : double Payload weight (use -1 for unknown weight) angle : double Allowed rotation angle of Axis 6 (30-90 degrees) |
| Return Value | StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.2.0+ Industrial (Bronze): Not supported |
4.2.19.13 Entering Payload Identification State
| Method Name | Motion.Payload.PayloadIdentifyStart() |
|---|---|
| Description | Enters the payload identification state. |
| Request Parameters | None |
| Return Value | StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.2.0+ Industrial (Bronze): Not supported |
4.2.19.14 Exiting Payload Identification State
| Method Name | Motion.Payload.PayloadIdentifyDone() |
|---|---|
| Description | Exits the payload identification state. |
| Request Parameters | None |
| Return Value | StatusCode: Result of function execution |
| Compatible robot software version | Collaborative (Copper): v7.5.2.0+ Industrial (Bronze): Not supported |
4.2.19.15 Full Payload Identification Process
| Method Name | Motion.Payload.PayloadIdentify( double weight = -1, double angle = 90 ) |
|---|---|
| Description | Complete payload identification process, including all the interfaces mentioned above. For general payload identification, this interface is sufficient. |
| Request Parameters | weight : double Payload weight (use -1 for unknown weight) angle : double Allowed rotation angle of Axis 6 (30-90 degrees) |
| Return Value | PayloadInfo: Payload identification result StatusCode: Result of function execution |
| Note | The returned payload can be added to the robot or saved to an existing payload in the robot. The full process steps are: 1. Enter payload identification state 2. Start payload identification 3. Get payload identification result 4. Exit payload identification state |
| Compatible robot software version | Collaborative (Copper): v7.5.2.0+ Industrial (Bronze): Not supported |
Example Code
cs
using Agilebot.IR;
using Agilebot.IR.Motion;
using Agilebot.IR.Types;
public class PayloadIdentify
{
public static StatusCode Run(string controllerIP, bool useLocalProxy = true)
{
// [ZH] 初始化捷勃特机器人
// [EN] Initialize the Agilebot robot
Arm controller = new Arm(controllerIP, useLocalProxy);
// [ZH] 连接捷勃特机器人
// [EN] Connect to the Agilebot robot
StatusCode code = controller.ConnectSync();
Console.WriteLine(code != StatusCode.OK ? code.GetDescription() : "连接成功/Successfully connected.");
if (code != StatusCode.OK)
{
return code;
}
try
{
// [ZH] 获取机器人模式
// [EN] Get robot mode
(UserOpMode opMode, StatusCode opCode) = controller.GetOpMode();
if (opCode == StatusCode.OK)
{
Console.WriteLine($"当前机器人模式/Current robot mode: {opMode}");
if (opMode != UserOpMode.AUTO)
{
Console.WriteLine(
$"负载测定执行必须在机器人自动模式下/Payload identification execution must be in automatic mode"
);
return StatusCode.OtherReason;
}
}
else
{
Console.WriteLine($"获取机器人模式失败/Failed to get robot mode: {opCode.GetDescription()}");
}
// [ZH] 检测3轴是否水平
// [EN] Check if axis 3 is horizontal
double horizontalAngle;
(horizontalAngle, code) = controller.Motion.Payload.CheckAxisThreeHorizontal();
if (code == StatusCode.OK)
{
Console.WriteLine($"3轴水平角度: {horizontalAngle}");
if (Math.Abs(horizontalAngle) > 1)
{
Console.WriteLine("警告:3轴水平角度超出范围(-1~1),无法进行负载测定");
return StatusCode.OtherReason;
}
}
else
{
Console.WriteLine($"检测3轴水平失败: {code.GetDescription()}");
}
// [ZH] 获取负载测定状态
// [EN] Get payload identification state
PayloadIdentifyState identifyState;
(identifyState, code) = controller.Motion.Payload.GetPayloadIdentifyState();
if (code == StatusCode.OK)
{
Console.WriteLine($"负载测定状态: {identifyState}");
}
else
{
Console.WriteLine($"获取负载测定状态失败: {code.GetDescription()}");
}
// [ZH] 执行完整的负载测定流程
// [EN] Execute complete payload identification process
PayloadInfo payload;
(payload, code) = controller.Motion.Payload.PayloadIdentify(-1, 90);
if (code == StatusCode.OK)
{
Console.WriteLine("负载测定成功:");
Console.WriteLine($"负载重量: {payload.Weight}");
Console.WriteLine(
$"质心位置: X={payload.MassCenter.X}, Y={payload.MassCenter.Y}, Z={payload.MassCenter.Z}"
);
Console.WriteLine(
$"惯性矩: LX={payload.InertiaMoment.LX}, LY={payload.InertiaMoment.LY}, LZ={payload.InertiaMoment.LZ}"
);
// [ZH] 保存负载到机器人中
// [EN] Save payload to robot
payload.Id = 6;
code = controller.Motion.Payload.AddPayload(payload);
if (code == StatusCode.OK)
{
Console.WriteLine("保存负载到机器人成功");
}
else
{
Console.WriteLine($"保存负载失败: {code.GetDescription()}");
}
}
else
{
Console.WriteLine($"负载测定失败: {code.GetDescription()}");
}
}
catch (Exception ex)
{
Console.WriteLine($"执行过程中发生异常/Exception occurred during execution: {ex.Message}");
code = StatusCode.OtherReason;
}
finally
{
// [ZH] 关闭连接
// [EN] Close the connection
StatusCode disconnectCode = controller.Disconnect();
Console.WriteLine(
disconnectCode != StatusCode.OK ? disconnectCode.GetDescription() : "Successfully disconnected."
);
}
return code;
}
}