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