Coppeliasim关节控制

Torque or force mode

自己理解可能存在错误
在该模式下，关节依靠动力学模块实现仿真，根据多体动力学计算要获得目标速度/位置对应的力矩和速度，实现相应运动。

• Motor disabled
  自由运动，相当于铰链

• Motor enabled and Control loop disabled
  在该情况下，关节可以使用两种控制方式

  • 速度控制，在满足力矩限制的条件下，实现目标速度。在该情况下，需要设置两个参数，分别为目标速度和最大力矩，关节在会在最大力矩下，尝试达到期望的目标速度。但是当关节达到目标速度后，关节力矩就不一定是最大力矩了
    
    可以通过上面的gui直接设置目标速度，也可以在脚本中调用sim.setJointTargetVelocity设置，注意在此模式下脚本中的sim.setJointTargetPosition接口无作用。在给定最大力矩的情况下，每个控制周期内设置需要的关节速度，可以模拟步进电机的控制模式（在最大力矩比较大的时候，获得设定速度的时间很短，所以可近似速度控制，但当最大力矩不够大时，达到目标速度还是需要一定的周期的）

  • 力矩控制，编程时通过调用sim.setJointTargetVelocity和sim.setJointMaxForce来实现关节力矩设置，这里采用的原理是coppeliasim会尝试以最大的力矩去实现设定的速度，所以当速度设定很大时，在一个周期达不到，此时相应的关节力矩便是设定的关节最大力矩值。需要注意，力矩的设置值必须为正，若要实现相反方向的力矩，只需要设置负的速度即可

• Motor enabled and Control loop enabled
  这里只介绍PID控制，在该种控制模式下实现位置控制，可以在gui中直接设置目标位置，也可以在脚本中调用sim.setJointTargetPosition设置，注意在此模式下脚本中的sim.setJointTargetVelocity接口无作用。在该模式下，可以在对应关节添加非线程子脚本的sysCall_jointCallback设计自己的控制算法，默认的算法是通过速度PID实现的位置控制，具体代码如下。在这种情况下，我们可以很简单的实现位置控制，只需要调用sim.setJointTargetPosition即可，力矩和速度会根据控制算法自动计算。
  注意
  这里的控制算法不单单指的是PID控制算法，而是包含逆动力学前馈加PID反馈的位置控制算法。对于如下图所示的系统
  
  针对上述的系统，一般的力控pid根据当前位置和目标位置计算位置误差，输出对应的力矩，由于系统重力作用，单纯的pd控制会存在稳态误差（可以在Motor enabled and Control loop disabled 模式下验证），而在该控制模式下，不会存在误差，且响应很快。

function sysCall_jointCallback(inData)
    -- This function gets called often, so it might slow down the simulation
    --     (this is called at each dynamic simulation step, by default 10x more often than a child script)
    -- We have:
    -- inData.first : whether this is the first call from the physics engine, since the joint
    --                was initialized (or re-initialized) in it.
    -- inData.revolute : whether the joint associated with this script is revolute or prismatic
    -- inData.cyclic : whether the joint associated with this script is cyclic or not
    -- inData.handle : the handle of the joint associated with this script
    -- inData.lowLimit : the lower limit of the joint associated with this script (if the joint is not cyclic)
    -- inData.highLimit : the higher limit of the joint associated with this script (if the joint is not cyclic)
    -- inData.passCnt : the current dynamics calculation pass. 1-10 by default. See next item for details.
    -- inData.totalPasses : the number of dynamics calculation passes for each "regular" simulation pass.
    --                      10 by default (i.e. 10*5ms=50ms which is the default simulation time step)
    -- inData.currentPos : the current position of the joint
    -- inData.targetPos : the desired position of the joint
    -- inData.errorValue : targetPos-currentPos (with revolute cyclic joints we take the shortest cyclic distance)
    -- inData.effort : the last force or torque that acted on this joint along/around its axis. With Bullet,
    --                 torques from joint limits are not taken into account
    -- inData.dynStepSize : the step size used for the dynamics calculations (by default 5ms)
    -- inData.targetVel : the joint target velocity (as set in the user interface)
    -- inData.maxForce : the joint maximum force/torque (as set in the user interface)
    -- inData.velUpperLimit : the joint velocity upper limit (as set in the user interface)
    --
    -- Make sure that the joint is dynamically enabled, is in force/torque mode, motor enabled and
    -- control loop enabled, otherwise this function won't be called

    if inData.first then
        PID_P=0.1
        PID_I=0
        PID_D=0
        pidCumulativeErrorForIntegralParam=0
    end
    
    -- The control happens here:
    -- 1. Proportional part:
    local ctrl=inData.errorValue*PID_P
    
    -- 2. Integral part:
    if PID_I~=0 then
        pidCumulativeErrorForIntegralParam=pidCumulativeErrorForIntegralParam+inData.errorValue*inData.dynStepSize
    else
        pidCumulativeErrorForIntegralParam=0
    end
    ctrl=ctrl+pidCumulativeErrorForIntegralParam*PID_I
    
    -- 3. Derivative part:
    if not inData.first then
        ctrl=ctrl+(inData.errorValue-pidLastErrorForDerivativeParam)*PID_D/inData.dynStepSize
    end
    pidLastErrorForDerivativeParam=inData.errorValue
    
    -- 4. Calculate the velocity needed to reach the position in one dynamic time step:
    local maxVelocity=ctrl/inData.dynStepSize -- max. velocity allowed.
    if (maxVelocity>inData.velUpperLimit) then
        maxVelocity=inData.velUpperLimit
    end
    if (maxVelocity<-inData.velUpperLimit) then
        maxVelocity=-inData.velUpperLimit
    end
    local forceOrTorqueToApply=inData.maxForce -- the maximum force/torque that the joint will be able to exert

    -- 5. Following data must be returned to CoppeliaSim:
    firstPass=false
    local outData={}
    outData.velocity=maxVelocity
    outData.force=forceOrTorqueToApply
    print(outData.velocity, outData.force)
    return outData
end

可以利用UR5进一步佐证位置控制模式为前馈加反馈（由于在仿真软件中模型参数均为已知，逆动力学很准确，理论上只用前馈的效果也很好，但将p参数设置为0时，系统并不会运动），设置关节2运动到目标位置，记录关节位置、速度和力矩，如下图

在达到位置后关节仍存在力矩抵抗重力矩，这仅靠p控制是不可能实现的（无误差）。注意，这里记录的力矩和运动方向是相反的，应该是关节受到连杆的作用力矩。