Regular API function

simGetMpConfigTransition (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, by trying to follow a predefined path in the Cartesian space. This is useful for redundant manipulators, but also for safely driving non-redundant manipulators via IK through a singular 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. See also simGetConfigForTipPose, simFindMpPath, simGenerateIkPath and simCheckIkGroup.
C synopsis simFloat* simGetMpConfigTransition(simInt motionPlanningObjectHandle,const simFloat* startConfig,const simFloat* goalConfig,simInt options,const simInt* select,simFloat calcStepSize,simFloat maxOutStepSize,simInt wayPointCnt,const simFloat* wayPoints,simInt* outputConfigsCnt,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 (1): reserved. Keep unset.
bit1 (2): reserved. Keep unset.
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 (64): reserved. Keep unset.
bit7 (128): reserved. Keep unset.
bit8: if set (256), then the returned Cartesian space distances will ignore the orientational distance component.
bit9: if set (512), then the specified waypoints will be followed by the end-effector in the Cartesian space. For that to happen, IK will be used too. If no waypoints are specified, then the end-effector will link the start to the goal configuration via a straight path in the Cartesian space.
select: an optional array that describes the behaviour of specific joints during the operation, when bit9 of options is set. Can be NULL, in which case all joints are treated equally. The first value in the array indicates how many joint behaviour descriptions will follow. Then, joint behaviour descriptions are appended, with 2 values per joint:
value 1: a joint handle.
value 2: a value indicating how the joint will be handled during the path search operation:
0: the joint will be fixed, i.e. perform an exact interpolation between the start and goal configuration.
1-7: the weight of the joint during the IK operations: 1=weight is 0.5, 2=weight is 0.25, 3=weight is 0.1, 4=weight is 0.05, 5=weight is 0.025, 6=weight is 0.01, and 7=weight is 0.001
A good strategy for redundant manipulators with redundancy level n, is to fix n joints, if resolution is not successful by keeping the select argument NULL.
calcStepSize: the maximum configuration space distance between individual collision-free phase2 nodes, during the interpolation phase. A distance calculation will use the weight specified for each joint in the motion planning properties.
maxOutCalcStepSize: the maximum configuration space distance between individual collision-free phase2 nodes, after the IK calculation phase. Keep this distance larger than calcStepSize. A distance calculation will use the weight specified for each joint in the motion planning properties.
wayPointCnt: the number of provided waypoints. Providing waypoints is optional, and only makes sense if bit9 of options is set. If no waypoints are provided, this should be 0. Otherwise this should be at least 2.
wayPoints: an array that contains optional waypoints. If no waypoints are provided, this should be NULL. For n waypoints, this array should contain n*7 values: for each waypoint, provide the x,y,z coordinates, and the quaternion values (qx,qy,qz,qw).
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.
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 confSpaceLengths,table_3 tipPositions,table_4 tipQuaternions,table cartesianSpaceLengths=simGetMpConfigTransition(number motionPlanningObjectHandle,table startConfig,table goalConfig,number options,table select,number calcStepSize,number maxOutStepSize,table wayPoints=nil)
Lua parameters
Similar as C-function, except that the select table (if not nil) should not contain the first element that is required in the C-function
Lua return values
Similar as C-function

All regular API functions on one page