Regular API function

simFindMpPath (DEPRECATED)

Description DEPRECATED. See the OMPL library based path/motion planning functionality instead.

Searches for a collision-free path leading a serial manipulator from a start configuration to a goal configuration. The function uses V-REP's motion planning functionality. The first time this function is called on the given motion planning object, a preprocessing stage will prepare a calculation structure. This might take a few seconds depending mainly on the number of phase1 nodes. By default, a found path will not be simplified, and you should call simSimplifyMpPath on it afterwards (or use a bit in the options parameter).See also simGetConfigForTipPose, simSimplifyMpPath, simGenerateIkPath, simGetMpConfigTransition and simCheckIkGroup.
C synopsis simFloat* simFindMpPath(simInt motionPlanningObjectHandle,const simFloat* startConfig,const simFloat* goalConfig,simInt options,simFloat stepSize,simInt* outputConfigsCnt,simInt maxTimeInMs,simFloat* reserved,const simInt* auxIntParams,const simFloat* auxFloatParams)
C parameters
motionPlanningObjectHandle: the handle of a motion planning object. Refer to simGetMotionPlanningHandle.
startConfig: the start or initial configuration of the robot (i.e. its joint positions). Should contain x values where x is the number of DoFs of the specified motion planning task.
goalConfig: the goal configuration of the robot (i.e. its joint positions). Should contain x values where x is the number of DoFs of the specified motion planning task. You can use simGetConfigForTipPose if the goal configuration is not known.
options: bit-coded:
bit0: if set (1), then the searched collision-free nodes (organized in a search tree) will be visualized in red. Only the search tree having root at the start configuration will be visualized.
bit1: if set (2), then the searched collision-free nodes (organized in a search tree) will be visualized in blue. Only the search tree having root at the goal configuration will be visualized.
bit2: if set (4), then the found path will be visualized in yellow.
bit3: if set (8), then some information will be output to the console.
bit4: if set (16), then robot self-interferences will be ignored and calculations can drastically be sped-up.
bit5: if set (32), then robot-environment interferences will be ignored and calculations can drastically be sped-up.
bit6: if set (64), then the found path will be directly simplified. This can take quite some time and is not part of the maximum calculation time specified. See also simSimplifyMpPath.
bit7: not used, keep unset.
bit8: if set (256), then the returned Cartesian space distances will ignore the orientational distance component.
stepSize: the maximum configuration space distance between individual collision-free phase2 nodes. A distance calculation will use the weight specified for each joint in the motion planning properties.
outputConfigsCnt: a pointer to an integer receiving the number of returned configurations. If a single configuration is returned, this means that the specified start and goal configurations are coincident.
maxTimeInMs: the maximum time in milliseconds after which the search operation is aborted. Specify 0 for an infinite time.
reserved: reserved. Keep NULL.
auxIntParams: reserved. Keep NULL.
auxFloatParams: reserved. Keep NULL.
C return value
NULL in case of an error, or when the search failed. Otherwise a buffer of float values that the user is in charge of releasing with simReleaseBuffer. The returned buffer contains:
the found path (x*n values): n configurations with each x values (x is the number of DoFs of the specified motion planning task). The configurations will include the start and the goal configuration, except when start and goal are coincident, in which case a single configuration is returned.
the configuration space distances (n values): for each returned configuration, a distance to the start configuration (following the path). The last of the n values represents the length of the found path in the configuration space. The distance is calculated using the weight specified for each joint in the motion planning properties.
the end-effector positions (3*n values): for each returned configuration, the position of the corresponding end-effector (x, y, z).
the end-effector quaternions (4*n values): for each returned configuration, the quaternion of the corresponding end-effector (x, y, z, w).
the Cartesian space distances (n values): for each returned configuration, a distance to the start pose (following the path). The last of the n values represents the length of the found path in the Cartesian space. The distance is calculated using the Cartesian space metric specified in the motion planning properties.
Lua synopsis table path,table confSpaceDist,table tipPositions,table tipQuaternions,table cartesianSpaceDist=simFindMpPath(number motionPlanningObjectHandle,table startConfig,table goalConfig,number options,number stepSize,number maxTimeInMs=0,table auxIntParams=nil,table auxFloatParams=nil)
Lua parameters
Similar as C-function
Lua return values
Similar as C-function

All regular API functions on one page