UR5 IK group中遇到的问题

目前遇到的问题应该是说在shapes是static，joints是inverse kinematic mode的情况下。

0.1redundantRobot的例子里，直接拖动target，机械臂会进行相应的逆解。直接在 thread child scirpt写函数改变target的位置，机械臂也会进行相应的逆解。

0.2但是在UR5 的例子里，直接拖动target，机械臂不会进行相应的逆解。直接在 thread child scirpt写函数改变target的位置，机械臂也不会进行相应的逆解。在non-thread child scirpt里写函数改变 target的位置，机械臂倒是会进行相应的逆解。

具体介绍：

1.1joint  inverse  kinematic模式机械臂会掉下来 。但是勾选hybrid  operation 就不会出现这种现现象。但是拖动target并不能使机器人进行相应的逆解。写thread child scirpt函数也不能控制。这个配置和函数都和redundanrobot这个例子一模一样但是没有复现一样的效果。

Hybrid operation: when the joint is in passive mode, inverse kinematics mode or dependent mode, it can optionally also be operated in a hybrid fashion: hybrid operation allows the joint to operate in a regular way, but additionally, just before dynamics calculations, the current joint position will be copied to the target joint position, and then, during dynamics calculations, the joint will be handled as a motor in position control (if and only if it is dynamically enabled). Refer to the joint types and operation section for details.

发生的现象：

修改后：

1.2如果取消其它shape 的dynamic属性。机器人也不会掉下来。但是拖动target并不能使机器人进行相应的逆解。写thread child scirpt函数也不能控制。这个配置和函数都和redundanrobot这个例子一模一样但是没有复现一样的效果。

论坛有解释，但是没有解决我的问题：

it really depends what you want to do exactly. If you do not need dynamics, then you should set all joints into IK mode (non-hybrid), and turn all shapes into static and non-respondable shapes. This way you will have a purely kinematics model of your robot.（这样机器人就是一个纯运动学的模型了，所有的shape是static并且non-respondable）

1.3 使用  8-computingJointAnglesForRandomPoses里non-thread child scirpt的方法，改变target的pose，机械臂会进行相应的逆运动学解算。

但是仍然无法像redundantRobot的例子里那样，直接拖动target就可以，在函数里直接改变它的target的pose，机械臂也会直接进行相应的逆解。

代码如下：

function sysCall_init()
        
    jh={-1,-1,-1,-1,-1,-1}
    for i=1,6,1 do
        jh[i]=sim.getObjectHandle('UR5_joint'..i)
    end
    ikGroup=sim.getIkGroupHandle('UR5')
    ikTarget=sim.getObjectHandle('target')
    targets={sim.getObjectHandle('target'),sim.getObjectHandle('target2'),sim.getObjectHandle('target'),sim.getObjectHandle('target')}
    cnt1=0
    cnt2=0
    
end
applyJoints=function(jointHandles,joints)
    for i=1,#jointHandles,1 do   --#:get the lenth of  jointHandles
        sim.setJointPosition(jointHandles[i],joints[i])
    end
end


function sysCall_actuation()

 -- local dummyHandle=targets[cnt1+1]
    local dummyHandle=targets[1]
    local m=sim.getObjectMatrix(dummyHandle,-1)
    print("m:") 
    --m[4]  = 0.13
    m[8]  = -0.70
    m[12] =0.90
    
    print(m[4],m[8],m[12])
    sim.setObjectMatrix(ikTarget,-1,m)
    state=sim.getConfigForTipPose(ikGroup,jh,0.65,100)
    print("state:")
    print(state)
   -- sim.pauseSimulation()

  
    if state then
        applyJoints(jh,state)
    end


end

2.   8-computingJointAnglesForRandomPoses  

 

2.1 其中 IRB4600机器人的shapes 是static，joints是inverse kinematic mode。

但是拖动target并不能使机器人进行相应的逆解。写脚本直接改变target的位置，机器人也不能进行相应的逆解。

function sysCall_init()
    -- do some initialization here:
 
   target=sim.getObjectHandle("IRB4600_IkTarget")
   m={0.3,0,0.5}  
   sim.setObjectPosition(target,-1,m) 
 end

2.2 其中的LBR4p_base的joints 模式 是passive

3.UR5机器人joints是inverse kinematic mode

（在文件夹中搜索：UR5使用函数改变targetpose但是不能拖动target）

在原脚本里  设置了两个target，但是并不能到达，两个不同的target。

function sysCall_init()
        
    jh={-1,-1,-1,-1,-1,-1}
    for i=1,6,1 do
        jh[i]=sim.getObjectHandle('IRB4600_joint'..i)
    end
    ikGroup=sim.getIkGroupHandle('IRB4600')
    ikTarget=sim.getObjectHandle('IRB4600_IkTarget')
    targets={sim.getObjectHandle('testTarget1'),sim.getObjectHandle('testTarget2'),sim.getObjectHandle('testTarget3'),sim.getObjectHandle('testTarget4')}
    cnt1=0
    cnt2=0
    
end
applyJoints=function(jointHandles,joints)
    for i=1,#jointHandles,1 do   --#:get the lenth of  jointHandles
        sim.setJointPosition(jointHandles[i],joints[i])
    end
end


function sysCall_actuation()

   -- local dummyHandle=targets[cnt1+1]
    local dummyHandle=targets[1]
    local m=sim.getObjectMatrix(dummyHandle,-1)
    print("m:") 
    m[4]  = 0.13
    m[8]  = -1.35
    m[12] = 1.2
    
    print(m[4],m[8],m[12])
    sim.setObjectMatrix(ikTarget,-1,m)
    state=sim.getConfigForTipPose(ikGroup,jh,0.65,100)
    print("state")
    print(state)
   -- sim.pauseSimulation()
   -- sim.wait(0.1)  
  
    if state then
        applyJoints(jh,state)
    end
------------------------------------------
--以为是没有延时，命令无法得到执行，加了这段代码，但是证明没有什么用

    cnt2=cnt2+1
    if cnt2>30 then
        cnt2=0
        cnt1=cnt1+1
        if cnt1>3 then
            cnt1=0
        end
    end    
  
---------------------------------------

    local dummyHandle1=targets[3]
    local m1=sim.getObjectMatrix(dummyHandle1,-1)
    print("m1:")
    print(m1[4],m1[8],m1[12])
    
    sim.setObjectMatrix(ikTarget,-1,m1)
    state1=sim.getConfigForTipPose(ikGroup,jh,0.65,100)
    --print("state")
    --print(state)
    --sim.pauseSimulation()
    --sim.wait(0.1)
   print("state1")
    print(state1)
    if state then
        applyJoints(jh,state1)
    end

end

但是写成如下的形式却是可以的：

function sysCall_init()
        
    jh={-1,-1,-1,-1,-1,-1}
    for i=1,6,1 do
        jh[i]=sim.getObjectHandle('UR5_joint'..i)
    end
    ikGroup=sim.getIkGroupHandle('UR5')
    ikTarget=sim.getObjectHandle('target')
    targets={sim.getObjectHandle('target1'),sim.getObjectHandle('target2'),sim.getObjectHandle('target3'),sim.getObjectHandle('target4')}
    cnt1=0
    cnt2=0
    
end
applyJoints=function(jointHandles,joints)
    for i=1,#jointHandles,1 do   --#:get the lenth of  jointHandles
        sim.setJointPosition(jointHandles[i],joints[i])
    end
end


function sysCall_actuation()

    --local  dummyHandle=targets[cnt1+1]
    local dummyHandle=targets[cnt1+1]
    local m=sim.getObjectMatrix(dummyHandle,-1)

    sim.setObjectMatrix(ikTarget,-1,m)
    state=sim.getConfigForTipPose(ikGroup,jh,0.65,100)
 
    if state then
        applyJoints(jh,state)
    end
------------------------------------------
--使用这段代码改变target
     cnt2=cnt2+1
     if cnt2>20 then
        cnt2=0
        cnt1=cnt1+1
        print(cnt1) 
        print(m[4],m[8],m[12])
        if cnt1>3 then
            cnt1=0
        end
    end
    
end

5.为什么有的demo里joint的模式都不相同  有的是passive  有的是force  inverse  

在verp中关于IK的例子，许多都是纯逆运动学模式，指的是它的joints是inverse kinematic mode，shapes是 static and non-respondable。

例如：4-twoIkGroupsWithEachOneIkElement-resolutionOrderIsRelevant

 

You will have to remember that the dynamically enabled part of the scene will be controlled by the phyiscs engine. If you want t apply an instantaneous change, you will have to first remove those objects from the physics engine (which happens with simResetDynamicObject). Those objects will then automatically be added to the physics engine again in nxt simulation step. So if you need to reset several objects at once, make sure this happens in the same simulation pass (in a non-threaded child script, or in a threaded child script where you temporarily disable the automatics thread switches with simSetThreadAutomaticSwitch).

In your case you need to make sure that not only the joint is reset, but also the object tree that might be attached to it.