基于CoppeliaSim和KUKA youBot的移动机械臂抓取仿真(五):youBot智能抓取四层汉诺塔仿真

用钢铁意志，成就不平凡人生。

上期我们学习了KUKA youBot机械臂轨迹规划https://blog.csdn.net/m0_71721954/article/details/131485182

这期我们学习youBot智能抓取四层汉诺塔仿真

完整附件见最后链接

通过上述对KUKA youBot底盘和机械臂运动学分析与仿真，我们可以进一步理解和掌握移动机械臂的运动规划和控制。移动机械臂的运动规划涉及两个主要方面，移动平台的运动规划和机械臂轨迹规划。移动平台的运动规划建立在自主定位和建图的基础上，在包含动态和静态障碍物的环境中，寻找避开障碍物的最优路径，使机器人能够尽快到达目标点。机械臂的运动规划涉及关节空间轨迹规划、笛卡尔空间轨迹规划和避障路径规划等方面。在运动规划过程中，需要基于机械臂的正逆运动学，计算每个关节的角度，以使机械臂能够避开障碍物并确保末端执行器到达目标点。

5.1 场景搭建

我们搭建的场景是四层汉诺塔，第一层为一个红色的方块，第二层为两个黄色的方块，第三层为三个绿色的方块，第四层为4个蓝色的方块。通过 KUKA youBot机器人移动抓取，从而实现汉诺塔从START位置移动到GOAL位置。我们搭建的四层汉诺塔场景，是一个经典的编程问题。

我们需要在场景中添加 KUKA youBot 机器人和汉诺塔场景，并设置好各个方块的起始位置和姿态信息。接着，可以通过编写 Lua 脚本或使用 V-REP 提供的 API 来控制机器人按照预设的路径移动，并抓取汉诺塔上的方块。我们建立场景时候直接可以在CoppeliaSim自带的KUKA youBot 机器人抓取三层汉诺塔基础上添加底层四个物块就可以

5.2汉诺塔求解算法

在上述我们分析了单个物块的抓取过程，下面我们针对四层汉诺塔的移动进行分析，四层汉诺塔的移动可以通过递归方式来实现。

汉诺塔问题是经典的递归问题，具体而言，我们可以将移动四层汉诺塔的问题分解为三个步骤：

1. 将前三层汉诺塔从 START 位置移动到 BUFFER 位置，需要借助 GOAL 位置。

2. 将第四层汉诺塔从 START 位置直接移动到 GOAL 位置。

3. 将前三层汉诺塔从 BUFFER 位置移动到 GOAL 位置，需要借助 START 位置。

在三个步骤中，第一步和第三步都是移动前三层汉诺塔，可以通过递归算法来实现。对于第二步，由于只有一个方块需要移动，因此可以直接控制机器人将该方块从 START 位置移动到 GOAL 位置。

5.3 主要代码解释

这个脚本控制着Youbot机器人的任务。它是多线程的。在接下来的部分中，有许多函数定义，用于简化对机器人的控制。在代码中，机器人的手臂控制有以下几种模式：

1. 正向运动学模式（setFkMode()）：在这种模式下，机器人的手臂位置由正向运动学计算确定。

2. 仅位置的逆向运动学模式（setIkMode(false)）：在这种模式下，机器人的手臂通过逆向运动学计算确定位置。

3. 位置和朝向的逆向运动学模式（保持夹爪垂直）（setIkMode(true)）：在这种模式下，机器人的手臂通过逆向运动学计算确定位置和朝向。为了保持夹爪垂直，朝向的计算也会被应用。

所有的逆向运动学任务都在IK属性对话框中定义。有四个任务：

- "youBotUndamped_group"：仅处理位置的逆向运动学。解算不阻尼。

- "youBotDamped_group"：仅处理位置的逆向运动学。解算阻尼。当前一个IK任务失败时，例如由于超出范围时，这个任务很有用。

- "youBotUndamped_group"：处理位置和朝向的逆向运动学。解算不阻尼。

- "youBotDamped_group"：处理位置和朝向的逆向运动学。解算阻尼。当前一个IK任务失败时，例如由于超出范围时，这个任务很有用。

根据需要启用/禁用上述四个任务，可以使用"sim.setExplicitHandling"函数来完成。

逆向运动学模式

setIkMode=function(withOrientation)

    sim.setThreadAutomaticSwitch(false) -- Don't get interrupted for this function

    if (ikMode==false) then

      sim.setObjectPosition(gripperTarget,-1,sim.getObjectPosition(gripperTip,-1))

    end

    if (withOrientation) then

        sim.setExplicitHandling(ikWithOrientation1,0)

        sim.setExplicitHandling(ikWithOrientation2,0)

    else

        sim.setExplicitHandling(ik1,0)

        sim.setExplicitHandling(ik2,0)

    end

    for i=1,5,1 do

        sim.setJointMode(armJoints[i],sim.jointmode_ik,1)

    end

    ikMode=true

    sim.setThreadAutomaticSwitch(true)

end

正向运动学模式

setFkMode=function()

    sim.setThreadAutomaticSwitch(false) -- Don't get interrupted for this function

    sim.setExplicitHandling(ik1,1)

    sim.setExplicitHandling(ik2,1)

    sim.setExplicitHandling(ikWithOrientation1,1)

    sim.setExplicitHandling(ikWithOrientation2,1)

    for i=1,5,1 do

        sim.setJointMode(armJoints[i],sim.jointmode_force,0)

    end

    ikMode=false

    sim.setThreadAutomaticSwitch(true)

end

打开夹爪程序

openGripper=function()

    sim.tubeWrite(gripperCommunicationTube,sim.packInt32Table({1}))

    sim.wait(0.8)

end

关闭夹爪程序

closeGripper=function()

    sim.tubeWrite(gripperCommunicationTube,sim.packInt32Table({0}))

    sim.wait(2)

end

setGripperTargetMovingWithVehicle是CoppeliaSim中的一个函数，用于设置夹爪目标相对于机器人的运动。在Youbot机器人模型中，夹爪可以与机器人一起移动或独立移动。通过调用setGripperTargetMovingWithVehicle函数，并将参数设置为true，可以使夹爪随机器人一起移动。这意味着当机器人移动时，夹爪会以相同的速度和方向跟随机器人的运动。这在某些情况下很有用，比如机器人在搬运物体时，夹爪需要保持与物体的相对位置关系。

setGripperTargetMovingWithVehicle=function()

    sim.setObjectParent(gripperTarget,vehicleReference,true)

end

setGripperTargetFixedToWorld=function()

    sim.setObjectParent(gripperTarget,-1,true)

end

waitToReachVehicleTargetPositionAndOrientation是CoppeliaSim中的一个函数，用于等待机器人达到目标位置和朝向。在Youbot机器人模型中，可以使用waitToReachVehicleTargetPositionAndOrientation函数来确保机器人在执行移动和旋转操作后到达目标位置和朝向之前暂停其他操作。

waitToReachVehicleTargetPositionAndOrientation=function()

    repeat

        sim.switchThread() -- don't waste your time waiting!

        p1=sim.getObjectPosition(vehicleTarget,-1)

        p2=sim.getObjectPosition(vehicleReference,-1)

        p={p2[1]-p1[1],p2[2]-p1[2]}

        pError=math.sqrt(p[1]*p[1]+p[2]*p[2])

        oError=math.abs(sim.getObjectOrientation(vehicleReference,vehicleTarget)[3])

    until (pError<0.001)and(oError<0.1*math.pi/180)

end

getBoxAdjustedMatrixAndFacingAngle是一个自定义函数，用于获取调整后的盒子（box）对象的矩阵和朝向角度。

getBoxAdjustedMatrixAndFacingAngle=function(boxHandle)

    p2=sim.getObjectPosition(boxHandle,-1)

    p1=sim.getObjectPosition(vehicleReference,-1)

    p={p2[1]-p1[1],p2[2]-p1[2],p2[3]-p1[3]}

    pl=math.sqrt(p[1]*p[1]+p[2]*p[2]+p[3]*p[3])

    p[1]=p[1]/pl

    p[2]=p[2]/pl

    p[3]=p[3]/pl

    m=sim.getObjectMatrix(boxHandle,-1)

    matchingScore=0

    for i=1,3,1 do

        v={m[0+i],m[4+i],m[8+i]}

        score=v[1]*p[1]+v[2]*p[2]+v[3]*p[3]

        if (math.abs(score)>matchingScore) then

            s=1

            if (score<0) then s=-1 end

            matchingScore=math.abs(score)

            bestMatch={v[1]*s,v[2]*s,v[3]*s}

        end

    end

    angle=math.atan2(bestMatch[2],bestMatch[1])

    m=sim.buildMatrix(p2,{0,0,angle})

    return m, angle-math.pi/2

end

pickupBoxFromPlace是一个自定义函数，用于从指定位置拾取一个盒子。

pickupBoxFromPlace=function(boxHandle,pickupConf)

    local m,angle=getBoxAdjustedMatrixAndFacingAngle(boxHandle)

    sim.setObjectPosition(vehicleTarget,-1,{m[4]-m[1]*dist1,m[8]-m[5]*dist1,0})

    sim.setObjectOrientation(vehicleTarget,-1,{0,0,angle})

    setFkMode()

sim.rmlMoveToJointPositions(armJoints,-1,nil,nil,fkSpeed,fkAccel,fkJerk,pickupConf,nil)

    waitToReachVehicleTargetPositionAndOrientation()

    setIkMode(true)

    setGripperTargetFixedToWorld()

    local p=sim.getObjectPosition(gripperTarget,-1)

    p[1]=m[4]

    p[2]=m[8]

sim.rmlMoveToPosition(gripperTarget,-1,-1,nil,nil,ikSpeed,ikAccel,ikJerk,p,nil,nil)

    openGripper()

    p[3]=m[12]

 sim.rmlMoveToPosition(gripperTarget,-1,-1,nil,nil,ikSpeed,ikAccel,ikJerk,p,nil,nil)

    closeGripper()

    p[3]=p[3]+0.1

 sim.rmlMoveToPosition(gripperTarget,-1,-1,nil,nil,ikSpeed,ikAccel,ikJerk,p,nil,nil)

    setGripperTargetMovingWithVehicle()    

    setFkMode()

end

dropToPlatform是一个自定义函数，用于将物体放置到指定的平台上。

dropToPlatform=function(platform)

    setFkMode() sim.rmlMoveToJointPositions(armJoints,-1,nil,nil,fkSpeed,{0.3,0.3,0.3,0.3,0.3},fkJerk,platform,nil)

    openGripper()

end

pickupFromPlatformAndReorient是一个自定义函数，用于从平台上拾取盒子并重新调整朝向。

pickupFromPlatformAndReorient=function(boxHandle)

    setFkMode()

    sim.rmlMoveToJointPositions(armJoints,-1,nil,nil,fkSpeed,fkAccel,fkJerk,platformIntermediateDrop,nil)

    setIkMode(false)

    local p=sim.getObjectPosition(boxHandle,-1)

    sim.rmlMoveToPosition(gripperTarget,-1,-1,nil,nil,ikSpeed,ikAccel,ikJerk,p,nil,nil)

    closeGripper()

    setFkMode()

end

dropToPlace是一个自定义函数，用于将物体放置到指定位置。

dropToPlace=function(placeHandle,shift,verticalPos,startConf,noVerticalArmForUpMovement)

    local m,angle=getBoxAdjustedMatrixAndFacingAngle(placeHandle)

    m[4]=m[4]+m[2]*shift

    m[8]=m[8]+m[6]*shift

    sim.setObjectPosition(vehicleTarget,-1,{m[4]-m[1]*dist1,m[8]-m[5]*dist1,0})

    sim.setObjectOrientation(vehicleTarget,-1,{0,0,angle})

    setFkMode()

sim.rmlMoveToJointPositions(armJoints,-1,nil,nil,fkSpeed,fkAccel,fkJerk,startConf,nil)

    waitToReachVehicleTargetPositionAndOrientation()

    setIkMode(true)

    setGripperTargetFixedToWorld()

    local p=sim.getObjectPosition(gripperTarget,-1)

    p[1]=m[4]

    p[2]=m[8]

sim.rmlMoveToPosition(gripperTarget,-1,-1,nil,nil,ikSpeed,ikAccel,ikJerk,p,nil,nil)

    p[3]=verticalPos

 sim.rmlMoveToPosition(gripperTarget,-1,-1,nil,nil,ikSpeed,ikAccel,ikJerk,p,nil,nil)

    openGripper()

    if (noVerticalArmForUpMovement) then

        setIkMode(false)

    end

    p[3]=p[3]+0.1

sim.rmlMoveToPosition(gripperTarget,-1,-1,nil,nil,ikSpeed,ikAccel,ikJerk,p,nil,nil)

    setGripperTargetMovingWithVehicle()    

    setFkMode()

end