V-rep学习笔记：Geometric Constraint Solver（几何约束求解）

　　The geometric constraint solver is slower and less precise at solving kinematic problems, but might be easier and more intuitive to use. Moreover, it allows interacting with a mechanism in a more flexible way than the inverse kinematics calculation module. Following figures illustrate the geometric constraint solver applied to a complicated mechanism:

[Geometric constraint solver handling a complicated mechanism: (1) initial situation, (2) during simulation, (3) interaction with the mechanism]

　　V-REP's geometric constraint solver functionality operates in a similar way as the kinematics calculation module, with the difference that the solver will try to automatically identify kinematic chains, and handle them in an appropriate way (automatic constraint adjustments, loop closures, etc.). Typically, the user has to tell the solver:

which objects (dummies) should coincide (in order to close a loop for instance)
what mechanism has to be handled by the geometric constraint solver
which additional constraints should be applied to the mechanism

Specifying closure constraints

　　Closure constraints can be seen as constraints that require two object's configurations (position and orientation) to be identical. The idea is illustrated in following figures:

[The geometric constraint solver closure constraint]

[Closing action of the geometric constraint solver (1) intermediate situation, (2) final situation]

　　GCS会试图将closure pair的位置以及姿态重合，因此机构要有足够的自由度。对于平面机构（如铰链四杆机构）来说使用GCS保持机构闭合就不合适，可以考虑用IK,tip-target来构成闭环。The geometric constraint solver will try to overlap the position and orientation of the two objects while trying to keep the mechanism coherent (i.e. by only adjusting the joints in IK mode in the mechanism to reach that overlap situation). The objects that the geometric constraint solver uses to specify closure constraints are dummies. To this end, two dummies need to be marked as closure pair. This can be adjusted in the dummy properties by selecting the opposite dummy as Linked dummy and specifying GCS, overlap constraint for the Link type. Following figure illustrates two linked dummies specifying a geometric constraint solver closure constraint:

[Two linked dummies specifying a closure constraint]

Specifying the mechanism to be solved

　　All objects that can be reached from a given object by following a path that can go will be considered as the same mechanism：

from one object to its parent object.
from one object to its child objects.
from one dummy to its linked dummy (however only links of type GCS, overlap constraint)

　　即按照上面3种方式从运动链（elements）上某一点开始探索，能够到达的节点都属于同一个机构。如果两个运动链没有公共的节点，但是由类型为GCS,overlap constraint的dummy相连，则这两个运动链仍属于同一个机构。Two object trees that don't share any common parent objects (referred hereafter as elements), can also be part of the same mechanism if two linked dummies join them. This is illustrated in following figure:

[One mechanism composed by 3 elements (object trees)]

　　The last parent object of the object that is chosen as the starting point of the mechanism exploration (path exploration) is referred to as the base object of the mechanism. When the geometric constraint solver tries to solve a mechanism, it will try to do so by keeping the base object of the mechanism in place（求解几何约束时选作base的object会固定住不动）. This is important to remember. To specify a mechanism to be solved, select an object parented with the base object of the mechanism and in the geometric constraint solver dialog click insert new object. One same mechanism can only be registered once for solving. Mechanisms that do not include at least one joint in IK mode will not be handled by the solver.

Specifying additional constraints

　　Additional positional constraints can be specified for a mechanism. This can be done with two linked dummies that form a tip-target pair (specify GCS, tip/GCS, target as Link type in the dummy properties). Following figures illustrate two linked dummies, where one is marked as target:

[The geometric constraint solver position constraint]

　　注意GCS, tip/GCS, target与GCS, overlap constraint的区别是：GCS, target不会被当做机构的一部分，解算时target不动tip移动到与target重合；而GCS, overlap constraint类型的dummy则属于同一个机构，解算时会同时移动直到重合。The dummy marked as target is not considered as part of the mechanism and therefore will not move during geometric constraint resolution, while the other dummy will try to reach the same position as the dummy marked as target.

[Position constraint action of the geometric constraint solver (1) intermediate situation, (2) final situation]

　　使用GCS（几何约束求解器），可以用鼠标拖动场景中的物体进行直观地控制。During simulation, a mechanism that was previously registered to be solved with the geometric constraint solver can be manipulated in a flexible way with the mouse, when the mechanism navigation mode is selected. That mode can be enabled via following API call: simSetNavigationMode. Simply click and drag any shape object that is part of the mechanism, and the solver will try to take into account that additional position constraint.

[Mechanism with two additional position constraints]

　　It is good practice to build your mechanism as a tree structure (i.e. where all objects in the mechanism have at least one common parent object) and have linked dummies be in charge of closing certain tree branches. By doing so you reduce the mechanism's complexity, you simplify the mechanism's scene hierarchy representation, and you will be able to handle the mechanism as a model.

　　下面是一个简单的例子，用鼠标选中门并按住左键拖动，可以看到门能跟着鼠标运动。修改Model browser/Models/infrastructure/doors/manual door中的场景和代码，添加一个控制球和一组Dummy（类型分别设置为GCS, tip/GCS, target），然后在Calculation Modules的Geometric Constraint Solver选项页中将根节点door设为机构的base 。最后将旋转关节设为Inverse kinematics模式，进行仿真：

　　代码如下：

if (sim_call_type==sim_childscriptcall_initialization) then 

    jointHandle=simGetObjectHandle('_doorJoint')
    sphereHandle=simGetObjectHandle('manipulationSphere')

    -- Retrieves the navigation and selection mode for the mouse
    initialMode=simGetNavigationMode() 

    -- Sets the navigation and selection mode for the mouse. 
    simSetNavigationMode(sim_navigation_objectshift+
                            sim_navigation_clickselection+
                            sim_navigation_ctrlselection+
                            sim_navigation_shiftselection+
                            sim_navigation_camerazoomwheel+
                            sim_navigation_camerarotaterightbutton)

    tipDummyHandle=simGetObjectHandle('tip')

    -- sim_intparam_prox_sensor_select_down: Allows to enable message or callback generation when a shape 
    -- was clicked (down-click) in the scene. The click is a "geometric" click. This value is set to zero 
    -- at simulation start and simulation stop.
    -- (-1: enabled for all visible shapes in the scene)
    simSetInt32Parameter(sim_intparam_prox_sensor_select_down, -1)  

    simDisplayDialog("info","You can click and drag the door",sim_dlgstyle_ok,false)

end 



if (sim_call_type==sim_childscriptcall_sensing) then

    -- auxiliaryData[1]: objectID
    -- auxiliaryData2[1] ~ auxiliaryData2[3]: coordinates of clicked point
    local message,data,data2 = simGetSimulatorMessage()

    while message>-1 do  -- (-1 if no message is available or in case of an error)
        if message == sim_message_prox_sensor_select_down then

            -- move manipulation sphere according the position of clicked point
            simSetObjectPosition(sphereHandle,-1,{data2[1],data2[2],data2[3]}) 

            simSetObjectPosition(tipDummyHandle,-1,{data2[1],data2[2],data2[3]})
            simSetObjectParent(tipDummyHandle, data[1], true)

            -- select the manipulation sphere to move
            simRemoveObjectFromSelection(sim_handle_all)
            simAddObjectToSelection(sim_handle_single,sphereHandle)
        end
        message,data,data2 = simGetSimulatorMessage()
    end
end



if (sim_call_type==sim_childscriptcall_cleanup) then 
    simRemoveObjectFromSelection(sim_handle_all) -- removes all objects from the selection
    simSetInt32Parameter(sim_intparam_prox_sensor_select_down, 0) -- 0: disabled
    simSetNavigationMode(initialMode)
end

View Code

　　下面是一个修改自V-REP_PRO_EDU\scenes\constraintSolverExample.ttt的例子，用鼠标选中机构上任意杆件进行拖拽，可以看到机构能跟着一起运动而不散开：

Geometric constraint solver operation

　　When building a mechanism that will be solved by the geometric constraint solver, make sure that the mechanism is coherent and that constraints are not impossible (i.e. that there are enough degrees of freedom for the mechanism to work). Following figures show an example of a mechanism and its resolution: