机器人仿真软件 V-REP学习记录(三) -- Demo(7、8)笔记

第二类案例：7，8

预习–案例

Demo7- fkAndIkResolutionForParal-lelMechanisms

1. 功能：正运动学控制 & 逆运动学控制
2. 说明

• 功能1：正向–>拖动滑条控制两个关节移动
• 功能2：逆向–>逆运动按轨迹移动到指定点

3. 演示
4. 代码：

1 初始化函数
function sysCall_init()
    --获取UI handle
    ui=simGetUIHandle('UI') 
    fkUi=simGetUIHandle('fkUi')
    
    --获取IK机械臂组的handle
    ikGroup=sim.getIkGroupHandle('ik')
    --获取足尖的handle
    tipDummy=sim.getObjectHandle('tip')
    --获电机的handle
    motorJoint1=sim.getObjectHandle('fk_motor1')
    motorJoint2=sim.getObjectHandle('fk_motor2')
    
    -- First set the motor joint into ik mode:
    -- set 0 is used (if set 1 the joint operates in hybrid mode)
    sim.setJointMode(motorJoint1,sim.jointmode_ik,0)
    sim.setJointMode(motorJoint2,sim.jointmode_ik,0)
    
    -- Make sure the IK element that brings the tip onto the target is disabled:
    --参数：（模块组，模块尖端，约束）
    sim.setIkElementProperties(ikGroup,tipDummy,0)
    
    -- Now close the mechanism (if it was open):计算一次
    sim.handleIkGroup(ikGroup)
    
    -- Read the current position of the motor joint:
    --绘制滑动条
    local angle=sim.getJointPosition(motorJoint1)
    simSetUISlider(fkUi,1,1000*(angle*180/math.pi+160)/320)
    angle=sim.getJointPosition(motorJoint2)
    simSetUISlider(fkUi,2,1000*(angle*180/math.pi+160)/320)
    
end
2 执行函数
function sysCall_actuation()
	******第一部分之逆运动模式******
	local a=simGetUIEventButton(ui)
	 if a==1 then
        -- 将电机设置为 ik mode:
        sim.setJointMode(motorJoint1,sim.jointmode_ik,0)
        sim.setJointMode(motorJoint2,sim.jointmode_ik,0)
        -- 确定模块组的尖端可以移动被使能
        sim.setIkElementProperties(ikGroup,tipDummy,sim.ik_x_constraint+sim.ik_y_constraint)
        -- Compute:
        sim.handleIkGroup(ikGroup)
        -- 更新滑条位置
        local angle=sim.getJointPosition(motorJoint1)
        simSetUISlider(fkUi,1,1000*(angle*180/math.pi+160)/320)
        angle=sim.getJointPosition(motorJoint2)
        simSetUISlider(fkUi,2,1000*(angle*180/math.pi+160)/320)
    end
	*****第二部分之正运动学******
	a=simGetUIEventButton(fkUi)
    	if a>=1 then
        -- 设置电机为 passive mode:
        sim.setJointMode(motorJoint1,sim.jointmode_passive,0)
        sim.setJointMode(motorJoint2,sim.jointmode_passive,0)
        -- 拉动滑动条设置电机角度:
        local angle=math.pi*(-160+320*simGetUISlider(fkUi,1)/1000)/180
        sim.setJointPosition(motorJoint1,angle)
        angle=math.pi*(-160+320*simGetUISlider(fkUi,2)/1000)/180
        sim.setJointPosition(motorJoint2,angle)
        -- 确定模块组的尖端可以移动被失能
        sim.setIkElementProperties(ikGroup,tipDummy,0)
        -- Compute:
        sim.handleIkGroup(ikGroup)
    end
end

5. 小结：这部分基本体现了正逆运动学的操作方式，技术点有

• 制作滑动条及其调用方式
• 正逆运动学操作
  问题有：
• 控制按键是怎么来的，没找到方法

Demo8 - computingJointAnglesForRandomPoses（从自由姿态计算关节角）

1. 组成：该案例由3部分组成

• 4个旋转坐标
• LBR4P机械臂
• IRB4600机械臂

1 – 旋转坐标

1. 功能：两电机组成的坐标，正弦震荡，
2. 演示：
3. 代码：

function sysCall_init()
        
    h=sim.getObjectAssociatedWithScript(sim.handle_self)
    amplitude=0.6 -- 振幅
    speed=1	  -- 角速度

end
function sysCall_actuation()
    sim.setJointPosition(h,math.sin(sim.getSimulationTime()*speed)*amplitude)
end

4. 小结及疑问：演示了电机的控制方式

• 问题1：显示的坐标是如何建立的
• 解释1：结构：基座假体点–>框架假体–>3个组合坐标轴（以后用到可以直接复制过去，链接在显示的地方即可）

2 – LBR4P机械臂

1. 功能：使机械臂执行末端依次到达在空间4个target点
2. 演示：
3. 代码：

*初始化
function sysCall_init()
    --关节电机标号容器
    jh={-1,-1,-1,-1,-1,-1,-1}
    for i=1,7,1 do
        jh[i]=sim.getObjectHandle('j'..i)
    end
    ikGroup=sim.getIkGroupHandle('ik')
    ikTarget=sim.getObjectHandle('target')
    --目标点标号容器targets={sim.getObjectHandle('testTarget5'),sim.getObjectHandle('testTarget6'),sim.getObjectHandle('testTarg）et7'),sim.getObjectHandle('testTarget8')}
    cnt1=0 -- 目标计数器
    cnt2=0 -- 延时计数器

end
*子函数
applyJoints=function(jointHandles,joints)
    for i=1,#jointHandles,1 do 		--print(#jointHandles)是7
        sim.setJointPosition(jointHandles[i],joints[i])
    end
end
*执行函数
function sysCall_actuation()	   
    --切换目标点
   s
    --得到目标点的转移矩阵(返回12个值，第四行0001不返回)--详细参见DH转移矩阵
    local m=sim.getObjectMatrix(dummyHandle,-1)
    --设置目标点的转移矩阵
    sim.setObjectMatrix(ikTarget,-1,m)
    --得到到达目标点电机应当旋转的角度，返回7个电机的值，每次的解都不唯一
    --参数含义（机械臂，关节电机，解算最小距离阈值，解算失败后多少ms退出）
    state=sim.getConfigForTipPose(ikGroup,jh,0.5,100)
    
    if state then
        applyJoints(jh,state)
    end
    --计算重复次数，可做到在一个点的延时
    cnt2=cnt2+1
    if cnt2>9 then
        cnt2=0
        --切换下一个点
        cnt1=cnt1+1
        if cnt1>3 then
            cnt1=0
        end
    end
end

4. 小结：形象的体现了逆运动学解的不唯一，给出了一个新的电机赋值方法，做四足可以参照。

3 - IRB4600机械臂

1. 功能：和LBR4P完成同样的效果，只是IRB的关节是6个
2. 说明：移植到其他文件单独查看时，要注意两点(图片蓝色的地方)

• 要将testTarget1~5也粘贴进来，不然代码运行将因为没有目标点而报错

---