vrep结合centerOfMassVisualization tool得到机器人质心座标

vrep结合centerOfMassVisualization tool得到机器人质心座标

步骤１

从下图找到centerOfMassVisualization tool，直接拖拽，放在你需要测量的机器人的base handle之下

例如nao机器人：

步骤2

新建一个plane, 并且创建child script

在里面增加如下的函数：

function visualizeCenterOfMass(intParams, floatParams, stringParams, bufferParam)
        local baseHandle = NAO_HANDLE    -- this is base handle, you should get it manually
        local allNonStaticShapes={}.
        if (baseHandle~=-1) then
            local allObjectsToExplore={baseHandle}
            while (#allObjectsToExplore>0) do
                local obj=allObjectsToExplore[1]
                table.remove(allObjectsToExplore,1)
                if ( (sim.boolAnd32(sim.getModelProperty(obj),sim.modelproperty_not_model)~=0) or (sim.boolAnd32(sim.getModelProperty(obj),sim.modelproperty_not_dynamic)==0) ) then
                    if (sim.getObjectType(obj)==sim.object_shape_type) then
                        local r,v=sim.getObjectInt32Parameter(obj,sim.shapeintparam_static)
                        if (v==0) then -- is the shape non-static?
                            table.insert(allNonStaticShapes,obj)
                        end
                    end
                    local index=0
                    while true do
                        local child=sim.getObjectChild(obj,index)
                        if (child==-1) then
                            break
                        end
                        table.insert(allObjectsToExplore,child)
                        index=index+1
                    end
                end
            end
        end

        local miri={0,0,0}
        local totalMass=0
        for i=1,#allNonStaticShapes,1 do
            local mass,inertia,com=sim.getShapeMassAndInertia(allNonStaticShapes[i],nil)
            miri[1]=miri[1]+mass*com[1]
            miri[2]=miri[2]+mass*com[2]
            miri[3]=miri[3]+mass*com[3]
            totalMass=totalMass+mass
        end

        if (totalMass~=0) then
            local centerOfMassOfModel={miri[1]/totalMass,miri[2]/totalMass,miri[3]/totalMass}
            --[[
            local c={centerOfMassOfModel[1],centerOfMassOfModel[2],centerOfMassOfModel[3]}
            local xx=size
            sim.addDrawingObjectItem(drawingObject,{c[1]-xx,c[2],c[3],c[1]+xx,c[2],c[3]})
            sim.addDrawingObjectItem(drawingObject,{c[1],c[2]-xx,c[3],c[1],c[2]+xx,c[3]})
            sim.addDrawingObjectItem(drawingObject,{c[1],c[2],c[3]-xx,c[1],c[2],c[3]+xx})
            --]]
            --[[
            -- Add a banner:
            local bn={centerOfMassOfModel[1],centerOfMassOfModel[2],centerOfMassOfModel[3],0,0,0}
            sim.removeBanner(bannerHandle)
            bannerHandle=sim.addBanner(string.format("Total mass=%.3f Kg",totalMass),0,sim.banner_bitmapfont+sim.banner_overlay,bn,-1,black,purple)
            -- io.write("com"..centerOfMassOfModel) -- could not print out the table
            --]]
            io.write("com"..centerOfMassOfModel[3])
            io.write("\n")
            io.write("totalMass"..totalMass)
            io.write('\n')
            return {}, centerOfMassOfModel, {tostring(totalMass)}, ''
        else
            centerOfMassOfModel={0,0,0}
            --[[
            sim.addDrawingObjectItem(drawingObject,nil)
            sim.removeBanner(bannerHandle)
            bannerHandle=sim.addBanner("Could not find dynamic objects..",0,sim.banner_bitmapfont+sim.banner_overlay,{0,0,0,0,0,0},-1,black,white)
            --]]
            return {}, centerOfMassOfModel, {tostring(totalMass)}, ''
        end
    end

然后你可以用python 2代码去读取这个值，python代码如下：

error_code, _, com, total_mass, _ = vrep.simxCallScriptFunction(self.clientID,
                                                            SCRIPT_OBJECT,
                                                            vrep.sim_scripttype_childscript,
                                                            "visualizeCenterOfMass",
                                                            [], [], [], bytearray(),
                                                            vrep.simx_opmode_blocking)
            print "com:", com, 
            print "total mass:", float(total_mass[0])

读取结果如下

仿真中，感觉nao机器人有从floor地板上面漂浮起来的感觉，所以com的z座标有点大，正常为0.25-0.26