Vrep之configuration space之间和pose之间距离的计算方法

1.configuration  space中的distance

      metric说明：定义了在configuration space中distance是怎么计算的
    the configuration space metric: this metric defines how distances are calculated in the configuration space of the manipulator, in order to identify a configuration node that is closest to what we are looking for. The metric corresponds to assigning a weight to each joint in the manipulator. Larger values will count for larger distances in the configuration space: if the first joint of a manipulator rotates by 1 degree, the end-effector displacement will usually be larger than if the last joint of the same manipulator rotates by the same amount. To take this into account, one would normally give larger weights to the base joints.

 metric={0.5,1,1,0.5,0.1,0.2,0.1}  给每一个关节值的差分配权重。

计算方法：

getConfigConfigDistance=function(config1,config2)
        -- Returns the distance (in configuration space) between two configurations
	-- 返回两个 configurations 之间的距离 
	-- 这个求距离的方法叫做2范数（求元素的平方和再开方）
	-- 求这个距离d的方法应该是将两个configuration之间对应的每一个关节值相减得到的值乘以一个权重metric然后平方，再求平方和，最最后再开方
    local d=0
    for i=1,#jh,1 do
        local dx=(config1[i]-config2[i])*metric[i]
        d=d+dx*dx   
    end
    return math.sqrt(d)
end

2. pose 与 pose 之间距离的计算方法

  具体的函数说明看官方文档中的 sim.getConfigForTipPose( )

 distances计算方法（求2范数）:

        -- Distance between two poses is calculated using a metric.
      -- Metric (input): an array to 4 values indicating a metric used to compute pose-pose
-- Distances: distance=sqrt((dx*metric[0])^2+(dy*metric[1])^2+(dz*metric[2])^2+(angle*metric[3])^2). 
-- Can be NULL for a default metric of {1.0,1.0,1.0,0.1}.  

       sim.getConfigForTipPose( ) 说明:

       Searches for a manipulator configuration that matches a given end-effector  position/orientation in space. 

       搜索是随机的（猜测是随机采样）getConfigForTipPose找到一个符合给定的end-effector 位置和姿态的                                       configuration 

       举例说明：

       c=sim.getConfigForTipPose(ikGroup,jh,0.65,10,nil,collisionPairs)

-- ikGroup 是逆运动解算对的handle    
-- 0.65(thresholdDist)是一个距离(或者说一个度量)  a distance indicating when IK should be computed in order              to try to bring the tip onto the target
     Distance between two poses is calculated using a metric.
-- 10 以毫秒为单位的最大时间，之后搜索将中止
-- metric (input): an array to 4 values indicating a metric used to compute pose-pose
-- distances: distance=sqrt((dx*metric[0])^2+(dy*metric[1])^2+(dz*metric[2])^2+(angle*metric[3])^2). 
-- Can be NULL for a default metric of {1.0,1.0,1.0,0.1}.

参考：

1.Motion planning setup

http://www.coppeliarobotics.com/helpFiles/en/oldMotionPlanningSetup.htm

2.http://www.coppeliarobotics.com/helpFiles/en/regularApi/simGetConfigForTipPose.htm