// Use this for initialization
void Start()
{
initializeJoints();
RoboDK RDK = new RoboDK();
ROBOT = RDK.ItemUserPick("Select a robot", RoboDK.ITEM_TYPE_ROBOT);
RDK.setRunMode(RoboDK.RUNMODE_SIMULATE);
ROBOT.MoveJ(home_joints);
Mat frame = ROBOT.PoseFrame();
//Mat tool = Variables.ROBOT.PoseTool();
Mat pose_ref = ROBOT.Pose();
target_pose = ROBOT.Pose();
xyz_ref = target_pose.Pos();
ROBOT.MoveJ(pose_ref);
ROBOT.setPoseFrame(frame);
//Variables.ROBOT.setPoseTool(tool);
ROBOT.setSpeed(500);
ROBOT.setZoneData(5);
//Setting inial right controller 3D position
VR_Init_Pos[0] = Right_Controller.transform.position.z;
VR_Init_Pos[1] = Right_Controller.transform.position.x;
VR_Init_Pos[2] = Right_Controller.transform.position.y;
//Debug.Log(VR_Init_Pos[0] + " " + VR_Init_Pos[1] + " " + VR_Init_Pos[2]);
}
/// <summary>
/// 生成机器人程序
/// </summary>
/// <param name="strFileAddress"></param>
public void MakeRobotProgramHandler(string strFileAddress)
{
if (!this.CheckRoboDK())
{
return;
}
if (!this.CheckRobot())
{
return;
}
if (this.CheckIsRobotBusy())
{
return;
}
lock (this._thisLock)
{
RoboDK.Item itemProgram = this._rdkPlatform.getItem(this._strProgramName, RoboDK.ITEM_TYPE_PROGRAM);
if (itemProgram.Valid())
{
itemProgram.MakeProgram(strFileAddress);
this.WriteLogHandler("生成实际机器人程序.");
}
}
}
private void SensorActive(object obj)
{
RoboDK RDK = new RoboDK("127.0.0.1");
RoboDK.Item SENSOR = RDK.GetItem("Sensor SICK WL4S", RoboDK.ITEM_TYPE_OBJECT);
RoboDK.Item program = roboDK.GetItem("MoveConveyor");
program.RunCode();
while (true)
{
RoboDK.Item item = GetEndedPart();
if (item != null)
{
if (SENSOR.Collision(item) > 0)
{
Console.WriteLine("检测到碰撞");
program.Stop();
RDK.SetParam("SENSOR", "1");
// 等待东西拿走再继续
while (RDK.GetParam("SENSOR") == "1")
{
Thread.Sleep(10);
}
program.RunCode();
}
}
Thread.Sleep(20);
}
}
/// <summary>
/// 停止RoboDK中的程序
/// </summary>
public void StopRoboDKProgramHandler( )
{
if (!this.CheckRoboDK())
{
return;
}
if (!this.CheckRobot())
{
return;
}
if (!this.CheckIsRobotBusy())
{
return;
} // 若机器人停止中，则不执行停止操作
lock (this._thisLock)
{
RoboDK.Item itemProgram = this._rdkPlatform.getItem(this._strProgramName, RoboDK.ITEM_TYPE_PROGRAM);
if (itemProgram.Valid())
{
itemProgram.Stop();
this.WriteLogHandler("停止RoboDK机器人程序.");
}
//this._rdkItemRobot.Stop();
}
}
/// <summary>
/// 运行RoboDK中的程序
/// </summary>
public void RunRoboDKProgramHandler( )
{
if (!this.CheckRoboDK())
{
return;
}
if (!this.CheckRobot())
{
return;
}
if (this.CheckIsRobotBusy())
{
this.WriteLogHandler("机器人正在运动,无法进行其他操作.", 1); return;
}
lock (this._thisLock)
{
RoboDK.Item itemProgram = this._rdkPlatform.getItem(this._strProgramName, RoboDK.ITEM_TYPE_PROGRAM);
if (itemProgram.Valid())
{
this.WriteLogHandler("运行RoboDK机器人程序.");
itemProgram.setRunType(RoboDK.PROGRAM_RUN_ON_SIMULATOR); // force to run on robot
itemProgram.RunProgram();
}
else
{
this.WriteLogHandler("不存在" + this._strProgramName + "这个程序.", 1);
}
}
}
/// <summary>
/// 驱动机器人运动
/// </summary>
private void DriveRobotMove( )
{
if (this._queueRobotMoveMessage.Count != 0)
{
double[] dblJoints = this._queueRobotMoveMessage.Dequeue();
// 检查关节角度值是否在限定范围内，若不是，则使其在限定范围内
for (int i = 0; i < 6; i++)
{
if (dblJoints[i] > this._dblRobotUpperLimits[i])
{
dblJoints[i] = this._dblRobotUpperLimits[i];
}
if (dblJoints[i] < this._dblRobotLowerLimits[i])
{
dblJoints[i] = this._dblRobotLowerLimits[i];
}
}
lock (this._thisLock)
{
try
{
this._rdkPlatform.setSimulationSpeed(1000); //控制机器人的运动速度
this._rdkItemRobot.MoveJ(dblJoints, BLOCKING_MOVE);
}
catch
{
this._rdkItemRobot = null; // 一旦在传输的过程中出现问题，都需要重新选择机器人模型，都将机器人对象置为null
this.WriteLogHandler("选中的机器人模型无效或者不存在，请重新选择.", 1);
}
}
}
}
private void AddPart(RoboDK.Item item)
{
lock (parts_lock)
{
ConveyorParts.Add(item);
}
}
private void button3_Click(object sender, EventArgs e)
{
// y平移测试
roboDK.GetItem("Part 1").Copy();
RoboDK.Item part = roboDK.GetItem("PalletA").Paste();
part.SetName("Part 1000");
part.Scale(new double[] { 2, 2, 2 });
part.SetPose(part.Pose().Translate(0, 0, 400));
}
private void PrepareSimulation()
{
RoboDK RDK = new RoboDK("127.0.0.1");
RoboDK.Item frame_pallet = RDK.GetItem("PalletA", RoboDK.ITEM_TYPE_FRAME);
// 获取参数
string SizeBox = RDK.GetParam("SizeBox"); // 150,200,150 箱子大小
string SizePallet = RDK.GetParam("SizePallet"); // 5,3,3 横纵列
float[] SizeBoxXyz = SizeBox.Split(',').Select((m) => float.Parse(m.Trim())).ToArray(); // [150,200,150]
int[] SizePalletXyz = SizePallet.Split(',').Select((m) => int.Parse(m.Trim())).ToArray(); // [5,3,3]
float SizeBoxZ = SizeBoxXyz[2]; // 箱子的高度很重要，用来计算位置和碰撞检测的
RDK.Render(false);
CleanUp(RDK.GetItemList(RoboDK.ITEM_TYPE_OBJECT), "Part ");
CleanUp(RDK.GetItemList(RoboDK.ITEM_TYPE_TOOL), "TCP ");
RDK.GetItem("box100mm").Copy();
Random random = new Random();
for (int i = 0; i < SizePalletXyz[0]; i++)
{
for (int j = 0; j < SizePalletXyz[1]; j++)
{
for (int k = 0; k < SizePalletXyz[2]; k++)
{
// 计算位置
float location_x = SizeBoxXyz[0] / 2 + i * SizeBoxXyz[0];
float location_y = SizeBoxXyz[1] / 2 + j * SizeBoxXyz[1];
float location_z = SizeBoxXyz[2] / 2 + k * SizeBoxXyz[2];
// 复制新的对象
RoboDK.Item newPart = frame_pallet.Paste();
newPart.Scale(new double[] { SizeBoxXyz[0] / 100, SizeBoxXyz[1] / 100, SizeBoxXyz[2] / 100 });
newPart.SetName("Part " + i.ToString() + j.ToString() + k.ToString());
newPart.SetPose(Matrix.Transl(location_x, location_y, location_z));
newPart.SetVisible(true, 0);
newPart.SetColor(new double[] { random.NextDouble(), random.NextDouble(), random.NextDouble(), 1 });
}
}
}
RDK.SetParam("SENSOR", "0");
RDK.Render(true);
RDK.Disconnect();
PartsClear();
}
private RoboDK.Item GetStartedPart()
{
lock (parts_lock)
{
if (ConveyorParts.Count > 0)
{
RoboDK.Item item = ConveyorParts[ConveyorParts.Count - 1];
return(item);
}
else
{
return(null);
}
}
}
/// <summary>
/// 关闭所有变量，使它们都invalidition
/// </summary>
private void CloseAllVariable( )
{
this._rdkItemRobot = null;
this._rdkPlatform = null;
this._thrdUpdateRobotParameter = null;
this._thisLock = null;
this._thrdDriveRobotMoveHandler = null;
this._queueRobotMoveMessage = null;
this._bolIsDriveRbtMoveThrdAlive = false;
this._autoEvent = null;
this._bolIsUpdateRbtParThrdAlive = false;
this._bolIsThreadSuspend = false;
}
private RoboDK.Item GetPartAndRemove()
{
lock (parts_lock)
{
if (ConveyorParts.Count > 0)
{
RoboDK.Item item = ConveyorParts[0];
ConveyorParts.RemoveAt(0);
return(item);
}
else
{
return(null);
}
}
}
/// <summary>
/// 删除目标点和程序
/// </summary>
/// <param name="intDeleteNum">目标点的个数</param>
/// <returns>是否删除成功</returns>
private bool DeleteTargetPointAndProgram(ref int intDeleteNum)
{
bool bolIsSuccess = false;
if (this._rdkItemRobot == null)
{
return(bolIsSuccess);
}
//this._rdkItemRobot.Delete();
lock (this._thisLock)
{
RoboDK.Item station = this._rdkPlatform.getItem("", RoboDK.ITEM_TYPE_STATION);
RoboDK.Item[] station_childs = station.Childs();
this.WriteLogHandler("开始删除RoboDK中的目标点.");
int sum = station_childs.Length;
for (int i = 0; i < sum; i++)
{
// 删除所有PROGRAM
if (station_childs[i].Type() == RoboDK.ITEM_TYPE_PROGRAM)
{
station_childs[i].Delete();
}
// 删除FRAME下面所有TARGET
else if (station_childs[i].Type() == RoboDK.ITEM_TYPE_FRAME)
{
RoboDK.Item[] frame_childs = station_childs[i].Childs();
int intLength = frame_childs.Length;
intDeleteNum = intLength;
for (int j = 0; j < intLength; j++)
{
if (frame_childs[j].Type() == RoboDK.ITEM_TYPE_TARGET)
{
frame_childs[j].Delete();
// 更新添加目标点的状态
int intState = (int)(50 * (j + 1) / intLength);
this.UpdateAddTargetPointStateHandler(intState);
}
}
}
}
}
this.WriteLogHandler("结束删除RoboDK中的目标点.");
bolIsSuccess = true;
return(bolIsSuccess);
}
/// <summary>
/// 检查是否和RoboDK的连接状态，true为连接，false为断开
/// </summary>
/// <returns>连接状态</returns>
private bool CheckRoboDK( )
{
lock (this._thisLock)
{
bool ok = this._rdkPlatform.Connected();
if (!ok)
{
ok = this._rdkPlatform.Connect();
this._rdkItemRobot = null; // 每次重连或者重启软件之后，都将机器人对象置null，需重新选取机器人
this.WriteLogHandler("已重启或重连RoboDK.");
}
if (!ok)
{
this.WriteLogHandler("未找到 RoboDK. 请先启动RoboDK.", 1);
}
return(ok);
}
}
/// <summary>
/// 关闭工作站
/// </summary>
private void CloseAllStations( )
{
if (this._rdkPlatform == null)
{
return;
}
lock (this._thisLock)
{
// get the first station available:
RoboDK.Item station = this._rdkPlatform.getItem("", RoboDK.ITEM_TYPE_STATION);
// check if the station has any items (object, robots, reference frames, ...)
RoboDK.Item[] station_childs = station.Childs();
while (station_childs.Length > 0)
{
this._rdkPlatform.CloseStation();
station = this._rdkPlatform.getItem("", RoboDK.ITEM_TYPE_STATION);
station_childs = station.Childs();
}
}
}
/// <summary>
/// 选中机器人对象
/// </summary>
/// <returns>是否选中机器人</returns>
private bool SelectRobotItem( )
{
lock (this._thisLock)
{
if (!this.CheckRoboDK())
{
return(false);
}
this._rdkItemRobot = this._rdkPlatform.ItemUserPick("Select a robot", RoboDK.ITEM_TYPE_ROBOT); // select robot among available robots
if (this._rdkItemRobot.Valid())
{
this.WriteLogHandler("已选中机器人: " + this._rdkItemRobot.Name() + ".");
return(true);
}
else
{
//this.WriteLogHandler("没有机器人可以选取.");
return(false);
}
}
}
/// <summary>
/// 向RoboDK中添加关节目标节点
/// </summary>
/// <param name="lstJoints">关节角度</param>
/// <returns>是否添加成功</returns>
private bool AddJointTarget(List <double[]> lstJoints, int intDeleteNum)
{
bool bolIsSuccess = false;
RoboDK.Item itemProgram = null;
lock (this._thisLock)
{
this._rdkPlatform.setSimulationSpeed(5); //控制机器人的运动速度
itemProgram = this._rdkPlatform.AddProgram(this._strProgramName, this._rdkItemRobot);
this.WriteLogHandler("开始向RoboDK中添加目标点.");
this.WriteLogHandler("正在向RoboDK中添加目标点...");
}
int intLength = lstJoints.Count;
for (int i = 0; i < intLength; i++)
{
lock (this._thisLock)
{
RoboDK.Item jointTarget = this._rdkPlatform.AddTarget("Target " + i.ToString(), null, this._rdkItemRobot);
jointTarget.setAsJointTarget();
jointTarget.setJoints(lstJoints[i]);
itemProgram.addMoveJ(jointTarget);
}
// 更新添加目标点的状态
int intState = 0;
if (intDeleteNum > 2)
{
intState = (int)(50 * (i + 1) / intLength + 50);
}
else
{
intState = (int)(100 * (i + 1) / intLength);
}
this.UpdateAddTargetPointStateHandler(intState);
}
this.WriteLogHandler("结束向RoboDK中添加目标点.");
bolIsSuccess = true;
return(bolIsSuccess);
}
/// <summary>
/// 获取机器人相关的参数
/// </summary>
private void GetRobotParameters( )
{
if (!this.CheckRobot())
{
return;
}
try
{
lock (this._thisLock)
{
// 获取机器人关节坐标系的角度
double[] dblJoints = this._rdkItemRobot.Joints();
// 获取机器人直角坐标系的角度
Mat locMatPose = this._rdkItemRobot.SolveFK(dblJoints);
double[] dblPoses = locMatPose.ToXYZRPW();
// RoboDK中机器人的运动速度
double dblRobotMoveSpeed = this._rdkPlatform.SimulationSpeed();
this._rdkItemRobot.JointLimits(ref this._dblRobotLowerLimits, ref this._dblRobotUpperLimits);
// 机器人参数整合在字典中，一起传出去
Dictionary <string, object> dicData = RobotParametersIntegrity(dblJoints, dblPoses, dblRobotMoveSpeed);
// 更新机器人参数
if (this.UpdateRobotParameter != null)
{
this.UpdateRobotParameter(dicData);
}
}
}
catch
{
this._rdkItemRobot = null; // 一旦在传输的过程中出现问题，都需要重新选择机器人模型，都将机器人对象置为null
this.WriteLogHandler("选中的机器人模型无效或者不存在，请重新选择.", 1);
}
}
/// <summary>
/// 删除机器人模型
/// </summary>
private void DeleteAllRobotModel( )
{
if (this._rdkItemRobot == null)
{
return;
}
//this._rdkItemRobot.Delete();
lock (this._thisLock)
{
RoboDK.Item station = this._rdkPlatform.getItem("", RoboDK.ITEM_TYPE_STATION);
RoboDK.Item[] station_childs = station.Childs();
while (station_childs.Length > 0)
{
int sum = station_childs.Length;
for (int i = 0; i < sum; i++)
{
station_childs[i].Delete();
}
station = this._rdkPlatform.getItem("", RoboDK.ITEM_TYPE_STATION);
station_childs = station.Childs();
}
}
}
private void button11_Click(object sender, EventArgs e)
{
RoboDK.Item robot = roboDK.GetItem("UR10 A", RoboDK.ITEM_TYPE_ROBOT);
RoboDK.Item taget = roboDK.GetItem("Put Conveyor", RoboDK.ITEM_TYPE_TARGET);
robot.MoveJ(taget);
}
private void RobotBStoreParts(object obj)
{
RoboDK RDK = new RoboDK("127.0.0.1");
int APPROACH = 100;
RoboDK.Item robot = RDK.GetItem("UR10 B", RoboDK.ITEM_TYPE_ROBOT);
RoboDK.Item tool = RDK.GetItem("GripperB", RoboDK.ITEM_TYPE_TOOL);
RoboDK.Item frame_pallet = RDK.GetItem("PalletB", RoboDK.ITEM_TYPE_FRAME);
RoboDK.Item frame_conv = RDK.GetItem("ConveyorReference", RoboDK.ITEM_TYPE_FRAME);
RoboDK.Item frame_conv_moving = RDK.GetItem("MovingRef", RoboDK.ITEM_TYPE_FRAME);
// 获取目标点
RoboDK.Item target_pallet_safe = RDK.GetItem("PalletApproachB", RoboDK.ITEM_TYPE_TARGET);
RoboDK.Item target_conv_safe = RDK.GetItem("ConvApproachB", RoboDK.ITEM_TYPE_TARGET);
RoboDK.Item target_conv = RDK.GetItem("Get Conveyor", RoboDK.ITEM_TYPE_TARGET);
// 获取参数
string SizeBox = RDK.GetParam("SizeBox"); // 150,200,150 箱子大小
string SizePallet = RDK.GetParam("SizePallet"); // 5,3,3 横纵列
float[] SizeBoxXyz = SizeBox.Split(',').Select((m) => float.Parse(m.Trim())).ToArray(); // [150,200,150]
int[] SizePalletXyz = SizePallet.Split(',').Select((m) => int.Parse(m.Trim())).ToArray(); // [5,3,3]
float SizeBoxZ = SizeBoxXyz[2]; // 箱子的高度很重要，用来计算位置和碰撞检测的
Matrix tool_xyzrpw = tool.PoseTool().Translate(0, 0, SizeBoxZ / 2);
RoboDK.Item tool_tcp = robot.AddTool(tool_xyzrpw, "TCP B");
robot.SetPoseTool(tool_tcp);
for (int k = 0; k < SizePalletXyz[2]; k++)
{
for (int j = 0; j < SizePalletXyz[1]; j++)
{
for (int i = 0; i < SizePalletXyz[0]; i++)
{
// 计算位置
float location_x = SizeBoxXyz[0] / 2 + i * SizeBoxXyz[0];
float location_y = SizeBoxXyz[1] / 2 + j * SizeBoxXyz[1];
float location_z = SizeBoxXyz[2] / 2 + k * SizeBoxXyz[2];
robot.SetPoseFrame(frame_conv);
Matrix target_conv_pose = target_conv.Pose().Translate(0, 0, -SizeBoxZ / 2);
Matrix target_conv_app = target_conv_pose.Translate(0, 0, -APPROACH);
robot.MoveJ(target_conv_safe);
robot.MoveJ(target_conv_app);
// 等待传感器信号
while (RDK.GetParam("SENSOR") == "0")
{
Thread.Sleep(10);
}
Thread.Sleep(10);
//robot.MoveL(target_conv_pose);
RoboDK.Item item = new RoboDK.Item(RDK, GetPartAndRemove().GetItemPtr());
robot.MoveL(target_conv_pose);
item.SetParentStatic(tool);
RDK.SetParam("SENSOR", "0");
Matrix matOffect = item.Pose();
robot.MoveL(target_conv_app);
robot.MoveJ(target_conv_safe);
robot.SetPoseFrame(frame_pallet);
robot.MoveJ(target_pallet_safe);
Matrix target_i = Matrix.Transl(location_x, location_y, location_z).RotX(Math.PI);
robot.MoveJ(target_i.Translate(0, 0, -APPROACH - SizeBoxZ));
robot.MoveJ(target_i);
item.SetParentStatic(frame_pallet);
robot.MoveJ(target_i.Translate(0, 0, -APPROACH - SizeBoxZ));
robot.MoveJ(target_pallet_safe);
}
}
}
}
private void ConvertorControl(object obj)
{
RoboDK RDK = new RoboDK("127.0.0.1");
RoboDK.Item Convertor = RDK.GetItem("Conveyor Belt");
}
private void button14_Click(object sender, EventArgs e)
{
// 停止皮带
RoboDK.Item program = roboDK.GetItem("MoveConveyor");
program.Stop();
}
private void button13_Click(object sender, EventArgs e)
{
RoboDK.Item robot = roboDK.GetItem("UR10 A", RoboDK.ITEM_TYPE_ROBOT);
RoboDK.Item taget = roboDK.GetItem("PalletApproachA", RoboDK.ITEM_TYPE_TARGET);
robot.MoveJ(taget);
}
private void button12_Click(object sender, EventArgs e)
{
// 启动皮带
RoboDK.Item program = roboDK.GetItem("MoveConveyor");
program.RunCode();
}
private void RobotAGetParts(object obj)
{
RoboDK RDK = new RoboDK("127.0.0.1");
int APPROACH = 100;
RoboDK.Item robot = RDK.GetItem("UR10 A", RoboDK.ITEM_TYPE_ROBOT);
RoboDK.Item tool = RDK.GetItem("GripperA", RoboDK.ITEM_TYPE_TOOL);
RoboDK.Item frame_pallet = RDK.GetItem("PalletA", RoboDK.ITEM_TYPE_FRAME);
RoboDK.Item frame_conv = RDK.GetItem("ConveyorReference", RoboDK.ITEM_TYPE_FRAME);
RoboDK.Item frame_conv_moving = RDK.GetItem("MovingRef", RoboDK.ITEM_TYPE_FRAME);
// 获取目标点
RoboDK.Item target_pallet_safe = RDK.GetItem("PalletApproachA", RoboDK.ITEM_TYPE_TARGET);
RoboDK.Item target_conv_safe = RDK.GetItem("ConvApproachA", RoboDK.ITEM_TYPE_TARGET);
RoboDK.Item target_conv = RDK.GetItem("Put Conveyor", RoboDK.ITEM_TYPE_TARGET);
// 获取参数
string SizeBox = RDK.GetParam("SizeBox"); // 150,200,150 箱子大小
string SizePallet = RDK.GetParam("SizePallet"); // 5,3,3 横纵列
//RoboDK.Item SENSOR2 = RDK.GetItem("Sensor SICK WL4S2", RoboDK.ITEM_TYPE_OBJECT);
//RoboDK.Item SENSOR3 = RDK.GetItem("Sensor SICK WL4S3", RoboDK.ITEM_TYPE_OBJECT);
float[] SizeBoxXyz = SizeBox.Split(',').Select((m) => float.Parse(m.Trim())).ToArray(); // [150,200,150]
int[] SizePalletXyz = SizePallet.Split(',').Select((m) => int.Parse(m.Trim())).ToArray(); // [5,3,3]
float SizeBoxZ = SizeBoxXyz[2]; // 箱子的高度很重要，用来计算位置和碰撞检测的
Matrix tool_xyzrpw = tool.PoseTool().Translate(0, 0, SizeBoxZ / 2);
RoboDK.Item tool_tcp = robot.AddTool(tool_xyzrpw, "TCP A");
robot.SetPoseTool(tool_tcp);
for (int k = SizePalletXyz[2] - 1; k >= 0; k--)
{
for (int i = SizePalletXyz[0] - 1; i >= 0; i--)
{
for (int j = SizePalletXyz[1] - 1; j >= 0; j--)
{
// 计算位置
float location_x = SizeBoxXyz[0] / 2 + i * SizeBoxXyz[0];
float location_y = SizeBoxXyz[1] / 2 + j * SizeBoxXyz[1];
float location_z = SizeBoxXyz[2] / 2 + k * SizeBoxXyz[2];
// 从传送带抓取
robot.SetPoseFrame(frame_pallet);
robot.MoveJ(target_pallet_safe); // 移动到上方的一个安全点
Matrix target_i = Matrix.Transl(location_x, location_y, location_z).RotX(Math.PI);
robot.MoveJ(target_i.Translate(0, 0, -APPROACH - SizeBoxZ));
robot.MoveJ(target_i);
RoboDK.Item item = RDK.GetItem("Part " + i.ToString() + j.ToString() + k.ToString());
item.SetParentStatic(tool);
robot.MoveJ(target_i.Translate(0, 0, -APPROACH - SizeBoxZ));
robot.MoveJ(target_pallet_safe);
// 放置到传送带上去
robot.SetPoseFrame(frame_conv);
Matrix target_conv_pose = target_conv.Pose().Translate(0, 0, -SizeBoxZ / 2);
Matrix target_conv_app = target_conv_pose.Translate(0, 0, -APPROACH);
robot.MoveJ(target_conv_safe);
// 移动到上方之前，需要先判断是否有物品阻挡
//RoboDK.Item itemStarted = GetStartedPart();
//if (itemStarted != null)
//{
// while (true)
// {
// if (SENSOR2.Collision(itemStarted) > 0 ||
// SENSOR3.Collision(itemStarted) > 0)
// {
// continue;
// }
// else
// {
// break;
// }
// }
//}
robot.MoveJ(target_conv_app);
robot.MoveL(target_conv_pose);
AddPart(item);
item.SetParentStatic(frame_conv_moving);
robot.MoveL(target_conv_app);
robot.MoveJ(target_conv_safe);
}
}
}
}
private void button1_Click(object sender, EventArgs e)
{
RoboDK.Item item = roboDK.GetItem("part1");
item.SetColor(new double[] { random.NextDouble(), random.NextDouble(), random.NextDouble(), 1 });
}
private void button1_Click(object sender, EventArgs e)
{
RoboDK.Item part = roboDK.GetItem("Part 1");
part.SetPose(part.Pose().Translate(-100, 0, 0));
}