top of page
public static int TestFTLoadCompute(Robot robot)
{
DescTran tr1=new DescTran(0,0,0);
robot.SetForceSensorPayload(0);
robot.SetForceSensorPayloadCog(tr1);
int company = 24;
int device = 0;
int softversion = 0;
int bus = 1;
int index = 1;
DeviceConfig con=new DeviceConfig(company, device, softversion, bus);
robot.FT_SetConfig(con);
robot.Sleep(1000);
robot.FT_GetConfig(con);
robot.Sleep(1000);
robot.FT_Activate(0);
robot.Sleep(1000);
robot.FT_Activate(1);
robot.Sleep(1000);
robot.Sleep(1000);
robot.FT_SetZero(0);
robot.Sleep(1000);
ForceTorque ft=new ForceTorque(0,0,0,0,0,0);
robot.FT_GetForceTorqueOrigin(0, ft);
robot.FT_SetZero(1);
robot.Sleep(1000);
DescPose tcoord = new DescPose();
tcoord.tran.z = 35.0;
robot.SetToolCoord(10, tcoord, 1, 0, 0, 0);
robot.FT_PdIdenRecord(10);
robot.Sleep(1000);
List<Number> weight =new ArrayList<>();
weight=robot.FT_PdIdenCompute();
DescPose desc_p1=new DescPose(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
DescPose desc_p2=new DescPose(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
DescPose desc_p3=new DescPose(-327.622, 402.230, 320.402, -178.067, 2.127, -46.207);
robot.MoveCart(desc_p1, 0, 0, 100.0, 100.0, 100.0, -1.0, -1);
robot.Sleep(1000);
robot.FT_PdCogIdenRecord(10, 1);
robot.MoveCart(desc_p2, 0, 0, 100.0, 100.0, 100.0, -1.0, -1);
robot.Sleep(1000);
robot.FT_PdCogIdenRecord(10, 2);
robot.MoveCart(desc_p3, 0, 0, 100.0, 100.0, 100.0, -1.0, -1);
robot.Sleep(1000);
robot.FT_PdCogIdenRecord(10, 3);
robot.Sleep(1000);
DescTran cog=new DescTran(0,0,0);
robot.FT_PdCogIdenCompute(cog);
robot.CloseRPC();
return 0;
}
12.19. Collision Guard
/**
* @brief Collision guard
* @param [in] flag 0-Disable collision guard, 1-Enable collision guard
* @param [in] sensor_id Force sensor number
* @param [in] select Select which degrees of freedom to detect collision, 0-Disable, 1-Enable
* @param [in] ft Collision force/torque, fx,fy,fz,tx,ty,tz
* @param [in] max_threshold Maximum threshold
* @param [in] min_threshold Minimum threshold
* @note Force/torque detection range: (ft-min_threshold, ft+max_threshold)
* @return Error code
*/
int FT_Guard(int flag, int sensor_id, Object[] select, ForceTorque ft, Object[] max_threshold, Object[] min_threshold);
12.20. Collision Guard Example
public static void main(String[] args)
{
Robot robot = new Robot();
robot.SetReconnectParam(true,20,500);//Set reconnection times and interval
robot.LoggerInit(FrLogType.DIRECT, FrLogLevel.INFO, "D://log", 10, 10);
int rtn = robot.RPC("192.168.58.2");
if(rtn == 0)
{
System.out.println("rpc connection success");
}
else
{
System.out.println("rpc connection fail");
return ;
}
byte flag = 1;
byte sensor_id = 8;
Object[] select = { 1, 0, 0, 0, 0, 0 };//Only enable collision guard on x-axis
Object[] max_threshold = { 5.0, 0.01, 0.01, 0.01, 0.01, 0.01 };
Object[] min_threshold = { 3.0, 0.01, 0.01, 0.01, 0.01, 0.01 };
ForceTorque ft = new ForceTorque(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
DescPose desc_p1, desc_p2, desc_p3;
desc_p1 = new DescPose(-14.404,-455.283,319.847,-172.935,25.141,-68.097);
desc_p2 = new DescPose(-107.999,-599.174,285.939,153.472,12.686,-71.284);
desc_p3 = new DescPose(6.586,-704.897,309.638,178.909,-27.759,-70.479);
int rtn = robot.FT_Guard(flag, sensor_id, select, ft, max_threshold, min_threshold);
System.out.println("FT_Guard start rtn {rtn}");
robot.MoveCart(desc_p1, 0, 0, 20, 100.0f, 100.0f, -1.0f, -1);
robot.MoveCart(desc_p2, 0, 0, 20, 100.0f, 100.0f, -1.0f, -1);
robot.MoveCart(desc_p3, 0, 0, 20, 100.0f, 100.0f, -1.0f, -1);
}
12.21. Constant Force Control
/**
* @brief Constant force control
* @param [in] flag 0-Disable constant force control, 1-Enable constant force control
* @param [in] sensor_id Force sensor number
* @param [in] select Select which degrees of freedom to detect, 0-Disable, 1-Enable
* @param [in] ft Force/torque, fx,fy,fz,tx,ty,tz
* @param [in] ft_pid Force PID parameters, torque PID parameters
* @param [in] adj_sign Adaptive start/stop control, 0-Disable, 1-Enable
* @param [in] ILC_sign ILC start/stop control, 0-Stop, 1-Training, 2-Operation
* @param [in] max_dis Maximum adjustment distance in mm
* @param [in] max_ang Maximum adjustment angle in deg
* @param [in] filter_Sign Filter enable flag, 0-Disable, 1-Enable, default is disabled
* @param [in] posAdapt_sign Posture compliance enable flag, 0-Disable, 1-Enable, default is disabled
* @param [in] isNoBlock Blocking flag, 0-Blocking, 1-Non-blocking
* @return Error code
*/
int FT_Control(int flag, int sensor_id, Object[] select, ForceTorque ft, Object[] ft_pid, int adj_sign, int ILC_sign, double max_dis, double max_ang, int filter_Sign, int posAdapt_sign, int isNoBlock);
12.22. Constant Force Control
/**
* @brief Constant Force Control
* @param flag 0-Disable constant force control, 1-Enable constant force control
* @param sensor_id Force sensor ID
* @param select Collision detection for six degrees of freedom, 0-Disable, 1-Enable
* @param ft Collision force/torque, fx, fy, fz, tx, ty, tz
* @param ft_pid Force PID parameters, Torque PID parameters
* @param adj_sign Adaptive start/stop control, 0-Disable, 1-Enable
* @param ILC_sign ILC start/stop control: 0-Stop, 1-Training, 2-Operational
* @param max_dis Maximum adjustment distance, unit mm
* @param max_ang Maximum adjustment angle, unit deg
* @param M Mass parameter for rx, ry [0.1-10], default 2
* @param B Damping parameter for rx, ry [0.1-50], default 8
* @param threshold Activation threshold for rx, ry [0-10], default 0.2
* @param adjustCoeff Torque adjustment coefficient for rx, ry [0-1], default 1
* @param polishRadio Polishing radius, in mm
* @param filter_Sign Filter enable flag: 0-Off; 1-On (default off)
* @param posAdapt_sign Pose adaptation enable flag: 0-Off; 1-On (default off)
* @param isNoBlock Blocking flag: 0-Blocking; 1-Non-blocking
* @return Error code
*/
public int FT_Control(int flag, int sensor_id, int[] select, ForceTorque ft, double[] ft_pid, int adj_sign, int ILC_sign, double max_dis, double max_ang,double[] M,double[] B, double[] threshold, double[] adjustCoeff, double polishRadio,int filter_Sign, int posAdapt_sign, int isNoBlock)
12.23. Example Code for Constant Force Control with Damping
public static int TestFTControlWithAdjustCoeff(Robot robot)
{
int sensor_id = 10;
int[] select = { 0,0,1,0,0,0 };
double[] ft_pid = { 0.0008, 0.0, 0.0, 0.0, 0.0, 0.0 };
int adj_sign = 0;
int ILC_sign = 0;
double max_dis = 1000.0;
double max_ang = 20;
ForceTorque ft = new ForceTorque(0.0,0,0,0,0,0);
ExaxisPos epos=new ExaxisPos(0, 0, 0, 0);
JointPos j1=new JointPos(80.765, -98.795, 106.548, -97.734, -89.999, 94.842);
JointPos j2=new JointPos(43.067, -84.429, 92.620, -98.175, -90.011, 57.144);
DescPose desc_p1=new DescPose(5.009, -547.463, 262.053, -179.999, -0.019, 75.923);
DescPose desc_p2=new DescPose(-347.966, -547.463, 262.048, -180.000, -0.019, 75.923);
DescPose offset_pos=new DescPose(0, 0, 0, 0, 0, 0);
double[] M = { 2.0, 2.0 };
double[] B = { 15.0, 15.0 };
double[] threshold = {1.0, 1.0};
double[] adjustCoeff = {1.0, 0.8};
double polishRadio = 0.0;
int filter_Sign = 0;
int posAdapt_sign = 1;
int isNoBlock;
ft.fz = -10.0;
while(true)
{
int rtn = robot.FT_Control(1, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang, M, B, threshold, adjustCoeff, 0, 0, 1, 0);
System.out.printf("FT_Control start rtn is %d\n", rtn);
robot.MoveL(j1, desc_p1, 1, 0, 100.0, 100.0, 100.0, -1.0, 0, epos, 0, 0, offset_pos, 0,0, 0,10);
robot.MoveL(j2, desc_p2, 1, 0, 100.0, 100.0, 100.0, -1.0, 0, epos, 0, 0, offset_pos, 0,0, 0,10);
rtn = robot.FT_Control(0, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang, M, B, threshold, adjustCoeff, 0, 0, 1, 0);
System.out.printf("FT_Control end rtn is %d\n", rtn);
}
}
12.24. Rotational Insertion
/**
* @brief Rotational Insertion
* @param rcs Reference coordinate system, 0 - Tool coordinate system, 1 - Base coordinate system
* @param angVelRot Rotational angular velocity, unit deg/s
* @param ft Force/Torque threshold, fx,fy,fz,tx,ty,tz, range [0~100]
* @param max_angle Maximum rotation angle, unit deg
* @param orn Force/Torque direction, 1 - Along Z-axis direction, 2 - Around Z-axis direction
* @param max_angAcc Maximum rotational acceleration, unit deg/s^2, currently unused, default is 0
* @param rotorn Rotation direction, 1 - Clockwise, 2 - Counterclockwise
* @param strategy Processing strategy for undetected force/torque, 0 - Error; 1 - Warning, continue motion
* @return Error code
*/
public int FT_RotInsertion(int rcs, double angVelRot, double ft, double max_angle, int orn, double max_angAcc, int rotorn, int strategy)
bottom of page