V-rep学习笔记：转动关节2

　　Torque or force mode: in this mode, the joint is simulated by the dynamics module, if and only if it is dynamically enabled. When dynamically enabled, a joint can be free or controlled in Force/torque, in velocity or in position.

　　The differentiation comes from the fact that a joint that operates in force/torque mode will be handled by the physics engine. And the physics engine will perform by default 10 times more calculation steps than the simulation loop: the simulation loop runs at 20Hz (in simulation time), while the physics engine runs at 200Hz (also in simulation time). That default behaviour can entirely be configured if required。If the joint is not in force/torque mode: if the joint is not in force/torque mode, then you can directly (and instantaneously) set its position via the simSetJointPosition API function.

　　建立如下图所示的旋转模型：

　　在转动关节的Joint Dynamic Properties对话框中勾选Control loop enable选项可以进行关节的位置控制（默认使用内置的PID控制器）。Target position为设定的目标位置（角度），Upper velocity limit为关节运动过程中允许的最大角速度，如果超过这个角速度则被限制为该值。

　　PID控制器将比例、积分、微分通过线性组合构成控制量，动态方程为：

　　PID控制器的离散化表达式为：

$$u(k)=K_pe(k)+K_iTe(k)+\frac{K_d}{T}[e(k)-e(k-1)]$$

　　式中，T为采样周期，k为采样序号，K_p为比例系数，K_i为积分系数，K_d为微分系数

　　比例控制能提高系统的动态响应速度，迅速反应误差，从而减小误差，但比例控制不能消除稳态误差。$K_p$的加大，会引起系统的不稳定；积分控制的作用是消除稳态误差，因为系统只要存在误差，积分作用就不断地积累，输出控制量，直到偏差为零，积分作用才会停止。但积分作用太强会使系统超调加大，甚至使系统出现振荡。微分作用可以在产生误差之前一发现有产生误差的趋势就开始调节，提前进行控制，所以及时性更好，可以最大限度地减少动态误差。但微分作用只能作为比例和积分控制的一种补充，不能起主导作用，微分作用太强也会使系统出现不稳定，微分作用只能在P和I调好后再由小往大调，一点一点试着加上去。

　　下面设10°为期望位置，PID参数设为0.1, 2, 0，仿真1s关节角度曲线如下图所示：

　　可以试着改变参数看看不同的效果，下图是使用默认参数0.1,0,0，即只使用比例控制的结果。可以看出增大积分项确实会引起超调

　　将比例系数设为1后结果如下图，可以看出到达目标位置的时间比K_p=0.1时要快很多，可以看出增大比例系数可以加快响应速度。

---

　　如果勾选Custom control选项，关节就会按照joint control callback script中用户编写的控制规律进行控制。This allows the user to write very specific joint controllers, that will be executed at each physics engine simulation step.

　　callback script脚本通常用于一些底层控制，VREP中分为3种：

• the contact callback script: the contact callback script lets you handle in a customized way each dynamic contact generated by the physics engine when two respondable objects collide.
• joint control callback scripts: a joint control callback script lets you write customized low-level controllers for joints that are dynamically enabled.
• the general callback script: each scene can have a general callback script that will be in charge of handling multi-purpose callback calls.

　　这些脚本有如下特点：

• a callback script always runs non-threaded.
• a callback script might be called more than once for each simulation step.
• a callback script handles low-level control items that the user wishes to customize. They should execute as quickly as possible, to avoid slowing down a simulation (this is particularly true for the contact callback script).

　　Joint control callback scripts, which are simulation scripts, can be enabled via the joint dynamics properties dialog. When enabled for a given joint (that must be dynamically enabled), then the physics engine will call the callback script with appropriate arguments, allowing the user to customize the control loop of the related joint in order to write low-level control algorithms for specific joints. The joint control callback script might be called quite often, normally 10 times per simulation step for a given joint (remember that the physics engine time step, by default, is 10 times smaller that the simulation time step). For that reason, keep things simple, in order to avoid slowing down the simulation. Following represents a simple PID joint control callback script:

　　打开关节控制脚本，默认进行PID控制，我们可以根据需要进行修改。代码中pidCumulativeErrorForIntegralParam变量用于存储积分项的累加值，如果是第一次调用控制脚本（init值为true）则将其初始化为0；dynStepSize与上面公式中的采样周期T意义相同；用于计算控制量的误差errorValue=targetPos-currentPos，那么与PID参数相乘后得到的控制量ctrl为位置量，如果要控制速度来使关节达到目标位置，则应施加的速度控制量为：velocityToApply=ctrl/dynStepSize

-- This example script performs a PID control
-- Be very careful when calling V-REP API functions from here:
-- 1. This routine gets called often, so it might slow down simulation (this is called at each dynamic simulation step, by default 10x more often than a child script)
-- 2. Some API functions are not meant to be called from here

-- Following data is handed over from V-REP:
init,revolute,cyclic,jointHandle,passCnt,totalPasses,currentPos,targetPos,errorValue,effort,dynStepSize,lowLimit,highLimit,targetVel,maxForceTorque,velUpperLimit=...

-- init: true when this callback is called for the first time (if the joint is dynamically reset during the simulation, this might be true more often)
-- revolute: true if the joint is revolute
-- cyclic: true if the joint is revolute and cyclic (i.e. no lower/upper limits)
-- passCnt: the current dynamics calculation pass. 0-9 by default. See next item for details.
-- 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)
-- currentPos: the current position of the joint
-- targetPos: the desired position of the joint
-- errorValue: targetPos-currentPos (with revolute cyclic joints we take the shortest cyclic distance)
-- 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
-- dynStepSize: the step size used for the dynamics calculations (by default 5ms)
-- lowLimit: the joint lower limit
-- highLimit: the joint upper limit
-- targetVel: the joint target velocity (as set in the user interface)
-- maxForceTorque: the joint maximum force/torque (as set in the user interface)
-- velUpperLimit: the joint velocity upper limit (as set in the user interface)

-- The control happens here:
-- 1. PID parameter def:
if not PID_P then
    PID_P=0.1
    PID_I=
    PID_D=
end
-- 2. Clear some values when the dynamic joint calls this the first time (this can happen several times, if the joint is reset dynamically):
if init then
    pidCumulativeErrorForIntegralParam=
end
-- 3. Proportional part:
ctrl=errorValue*PID_P
-- 4. Integral part:
if PID_I~= then
    pidCumulativeErrorForIntegralParam=pidCumulativeErrorForIntegralParam+errorValue*dynStepSize
else
    pidCumulativeErrorForIntegralParam=
end
ctrl=ctrl+pidCumulativeErrorForIntegralParam*PID_I
-- 5. Derivative part:
if not init then
    ctrl=ctrl+(errorValue-pidLastErrorForDerivativeParam)*PID_D/dynStepSize
end
pidLastErrorForDerivativeParam=errorValue
-- 6. Calculate the velocity needed to reach the position in one dynamic time step:
velocityToApply=ctrl/dynStepSize
if (velocityToApply>velUpperLimit) then
    velocityToApply=velUpperLimit
end
if (velocityToApply<-velUpperLimit) then
    velocityToApply=-velUpperLimit
end
forceOrTorqueToApply=maxForceTorque

-- Following data must be returned to V-REP:
return forceOrTorqueToApply,velocityToApply

-- forceOrTorqueToApply: the maximum force/torque that the joint will be able to exert
-- velocityToApply: the velocity to apply to the joint.

　　注意Lua函数可以接受可变数目的参数，在函数参数列表中使用三点（...）表示函数有可变的参数。