V-rep 逆运动学（IK）8-computingJointAnglesForRandomPoses 代码自学

V-rep 逆运动学（IK）8-computingJointAnglesForRandomPoses 代码自学

白色机器人（即通过封装好的simExtIK插件进行逆运动学计算）的代码
代码功能：用代码实现dummy的创建、ik组和元素的创建及参数设置
（//注释部分的代码不能放在vrep里直接运行，这里只做解释使用）

function sysCall_init()
	//一共六个关节，通过句柄获得
	//这里的IRB4600_joint1、……、IRB4600_joint6与场景层次结构里的名称一一对应
	//把这些句柄分别赋给simJointHandles[1]……simJointHandles[6]
    simJointHandles={}
    for i=1,6,1 do
        simJointHandles[i]=sim.getObjectHandle('IRB4600_joint'..i)
    end
    //机器人末端的Dummy赋给simTip，机器人的基座赋给simBase
    simTip=sim.getObjectHandle('IRB4600_IkTip')
    simBase=sim.getObjectHandle('IRB4600')
    //规划的机器人末端目标位置一共有四个Dummy，分别是testTarget1……testTarget4
    //把这些末端位置句柄分别赋给target[1]…target[4]
    targets={sim.getObjectHandle('testTarget1'),sim.getObjectHandle('testTarget2'),sim.getObjectHandle('testTarget3'),sim.getObjectHandle('testTarget4')}
    //机器人的目标位置，初始化为0，cnt1在0-3之间循环
    cnt1=0
    cnt2=0
 //
    //现在基于机器人的运动学在IK插件环境中建立一个运动学链和2个IK组(无阻尼和阻尼)
    ikJointHandles={}
    //创建一个IK环境ikEnv
    ikEnv=simIK.createEnvironment()
    //在IK环境中创建一个Dummy，命名为ikBase
    local ikBase=simIK.createDummy(ikEnv) 
    //把ikBase设置成与CoppeliaSim中的simBase对应的相同姿势
    simIK.setObjectMatrix(ikEnv,ikBase,-1,sim.getObjectMatrix(simBase,-1))
    //建立父基座的Dummy，当前与ikBase相同
    local parent=ikBase
    //循环所有simJointHandles关节，simJointHandles[1]-simJointHandles[6]
    for i=1,#simJointHandles,1 do
    	//在IK环境中创建一个旋转关节ikJointHandles[i]
        ikJointHandles[i]=simIK.createJoint(ikEnv,simIK.jointtype_revolute)
        //把该关节ikJointHandles[i]设置成IK模式
        simIK.setJointMode(ikEnv,ikJointHandles[i],simIK.jointmode_ik) 
        //获得关节simJointHandles[i]的关节限制
        local cyclic,interv=sim.getJointInterval(simJointHandles[i])
        //为ikJointHandles[i]设置上述获得的与关节simJointHandles[i]相同的关节限制
        simIK.setJointInterval(ikEnv,ikJointHandles[i],cyclic,interv) 
        //为ikJointHandles[i]设置与关节simJointHandles[i]相同的关节位置
        simIK.setJointPosition(ikEnv,ikJointHandles[i],sim.getJointPosition(simJointHandles[i])) 
        //为ikJointHandles[i]设置与关节simJointHandles[i]相同的关节姿态
        simIK.setObjectMatrix(ikEnv,ikJointHandles[i],-1,sim.getObjectMatrix(simJointHandles[i],-1)) 
        //设置相应的父元素（第一个关节的父节点为基座）
        simIK.setObjectParent(ikEnv,ikJointHandles[i],parent,true) 
        //更新相应的父元素（第二个关节的父节点为第一个关节）
        parent=ikJointHandles[i]
    end
    //在IK环境中创建末端tip Dummy，命名为ikTip
    ikTip=simIK.createDummy(ikEnv)
     //把ikTip设置成与CoppeliaSim中的simTip对应的相同姿势
    simIK.setObjectMatrix(ikEnv,ikTip,-1,sim.getObjectMatrix(simTip,-1)) 
    //把ikTip连接到运动链的parent上，此时parent为ikJointHandles[6]
    simIK.setObjectParent(ikEnv,ikTip,parent,true) 
    //在IK环境中创建目标Dummy，命名为ikTarget
    ikTarget=simIK.createDummy(ikEnv) 
    //将ikTarget设置成与CoppeliaSim中的simTip对应的相同姿势
    simIK.setObjectMatrix(ikEnv,ikTarget,-1,sim.getObjectMatrix(simTip,-1)) 
    //连接两个Dummy,ikTip和ikTarget
    simIK.setLinkedDummy(ikEnv,ikTip,ikTarget) 
    //创建一个IK组，命名为ikGroup
    ikGroup=simIK.createIkGroup(ikEnv)
    //把ikGroup的分辨率（计算方法）设置为undamped，即对应于界面设置中的Pseudo inverse，参数0为Damping的值，参数3为maxIterations的值
    simIK.setIkGroupCalculation(ikEnv,ikGroup,simIK.method_pseudo_inverse,0,3) 
    //向ikGroup中添加一个IK元素---ikTip
    local ikElementHandle=simIK.addIkElement(ikEnv,ikGroup,ikTip)
    //指定IK元素ikTip的基座为ikBase
    simIK.setIkElementBase(ikEnv,ikGroup,ikElementHandle,ikBase) 
    //指定IK元素ikTip的约束条件
    //其中，最后一项值是约束条件
    //simIK.constraint_x, simIK.constraint_y, simIK.constraint_z, simIK.constraint_alpha_beta, simIK.constraint_gamma 
    //其中，simIK.constraint_gamma只能在simIK.constraint_alpha_beta设置时设置 
    //为了方便起见，还有
    //simIK.constraint_position=simIK.constraint_x|simIK.constraint_y|simIK.constraint_z
    //simIK.constraint_orientation=simIK.constraint_alpha_beta|simIK.constraint_gamma
    //simIK.constraint_pose=simIK.constraint_position|simIK.constraint_orientation.
    simIK.setIkElementConstraints(ikEnv,ikGroup,ikElementHandle,simIK.constraint_pose) 
    //设置IK元素ikTip所需的精度，最后一项第一个值是linear精度，第二个值是angular精度
    simIK.setIkElementPrecision(ikEnv,ikGroup,ikElementHandle,{0.00005,0.1*math.pi/180})
end

//函数applyJoints，将joints的每个值设置为jointHandles的关节位置
applyJoints=function(jointHandles,joints)
    for i=1,#jointHandles,1 do
        sim.setJointPosition(jointHandles[i],joints[i])
    end
end

//在这里你可以检查碰撞和其他测试。如果配置有效，返回true
function configurationValidationCallback(config)
    sim.addLog(sim.verbosity_scriptinfos,"Hello from validation callback")
    return true
end

function sysCall_actuation()
	//定义当前的目标dummyHandle为targets[cnt1+1]
    local dummyHandle=targets[cnt1+1]
    //获得dummyHandle的姿态m
    local m=sim.getObjectMatrix(dummyHandle,-1)
    //设置ikTarget的姿态为m
    simIK.setObjectMatrix(ikEnv,ikTarget,-1,m)
    
    //搜索一个姿态配置，最大100毫秒
    local startTime=sim.getSystemTimeInMs(-1)
    while sim.getSystemTimeInMs(startTime)<100 do
        local validationCB=''
        //如果你需要对找到的配置执行额外的检查，请取消下面的注释:
        --local validationCB='configurationValidationCallback@IRB4600'
        //搜索与目标Dummy位置/方向在空间中匹配的manipulator配置。 搜索是随机的。如果manipulator的某些旋转关节的范围超过360度，则应为每个返回的配置调用 simIK.getAlternateConfigs，以便生成一些等效的姿势但替代配置。 IK 环境保持不变。
        //0.65：一个距离，指示何时计算IK，以便尝试将尖端对准目标
        //10000：函数返回时间的上限，以秒为单位。
        //{1，1，1，0.1}：一个表的4个值表明一个度量用来计算姿势距离：distance=sqrt((dx*metric[1])^2+(dy*metric[2])^2+(dz*metric[3])^2+(angle*metric[4])^2)
        local state=simIK.getConfigForTipPose(ikEnv,ikGroup,ikJointHandles,0.65,10000,{1,1,1,0.1},validationCB)
        if state then
            applyJoints(simJointHandles,state)
            break
        end
    end
    
    cnt2=cnt2+1
    if cnt2>20 then
        cnt2=0
        cnt1=cnt1+1
        if cnt1>3 then
            cnt1=0
        end
    end
end

function sysCall_cleanup()
    simIK.eraseEnvironment(ikEnv)
end