simOMPL.compute doesn't reach the goalConfigs

Typically: "How do I... ", "How can I... " questions
Post Reply
wozxfdha
Posts: 51
Joined: 10 Jun 2021, 09:40

simOMPL.compute doesn't reach the goalConfigs

Post by wozxfdha »

Hi,

I use simOMPL to conduct motion planning for UR5, my code is as follows:

Code: Select all

    t=simOMPL.createTask('t')
    simOMPL.setAlgorithm(t,simOMPL.Algorithm.RRT)
    local j1_space=simOMPL.createStateSpace('j1_space',simOMPL.StateSpaceType.joint_position,simJoints[1],{-360*math.pi/180},{360*math.pi/180},1)
    local j2_space=simOMPL.createStateSpace('j2_space',simOMPL.StateSpaceType.joint_position,simJoints[2],{-360*math.pi/180},{360*math.pi/180},2)
    local j3_space=simOMPL.createStateSpace('j3_space',simOMPL.StateSpaceType.joint_position,simJoints[3],{-360*math.pi/180},{360*math.pi/180},3)
    local j4_space=simOMPL.createStateSpace('j4_space',simOMPL.StateSpaceType.joint_position,simJoints[4],{-360*math.pi/180},{360*math.pi/180},0)
    local j5_space=simOMPL.createStateSpace('j5_space',simOMPL.StateSpaceType.joint_position,simJoints[5],{-360*math.pi/180},{360*math.pi/180},0)
    local j6_space=simOMPL.createStateSpace('j6_space',simOMPL.StateSpaceType.joint_position,simJoints[6],{-360*math.pi/180},{360*math.pi/180},0)
    simOMPL.setStateSpace(t,{j1_space,j2_space,j3_space,j4_space,j5_space,j6_space})
    simOMPL.setCollisionPairs(t,collisionPairs)
    
    startConfig=getConfig()
    simOMPL.setStartState(t,startConfig)
    
    simOMPL.setGoalStates(t,goalConfigs)
    
    simOMPL.setup(t)
    
    local solved,path=simOMPL.compute(t,4,-1,path_point_num)
    
    print(goalConfigs)
    print(path)
    print('xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx')

However, I print the goalConfigs and the path computed by simOMPL.compute(), I find that the path doesn't match any goalConfigs.

Code: Select all

goalConfigs:
{{-6.2612330629155, -5.4728734567359, 0.15129820093459, 2.219667326582, -2.6540505434568, -1.4972567019452}, {0.02193394176395, -5.4729241218041, -6.1317291885129, 2.2195464852, -2.6540495419243, -1.4972958581045}, {0.021291701955442, 0.39836015365958, -5.455716333466, 5.0967995677525, -0.48810724437033, 1.6438040578276}, {-6.2612295246791, -5.4728921374898, 0.1513725160727, -4.0635679768942, -2.6539223380903, 4.785915390685}, {0.02128826040026, -5.8848186534424, 0.82746963235491, -1.1863944984409, -0.4881029453909, -4.6393859916285}}

path:
{0, 0, 0, 0, 0, 0, -0.021732879802585, -0.027102991938591, -0.053091429173946, 0.19018742442131, -0.18649208545685, 0.12686192989349, -0.043465759605169, -0.054205983877182, -0.10618285834789, 0.38037484884262, -0.3729841709137, 0.25372385978699, -0.065198637545109, -0.081308975815773, -0.15927428007126, 0.57056224346161, -0.55947625637054, 0.38058578968048, -0.086931519210339, -0.10841196775436, -0.21236571669579, 0.76074969768524, -0.74596834182739, 0.50744771957397, -0.10866440087557, -0.13551495969296, -0.26545715332031, 0.95093709230423, -0.93246042728424, 0.63430964946747, -0.13039727509022, -0.16261795163155, -0.31854856014252, 1.1411244869232, -1.1189525127411, 0.76117157936096, -0.15213015675545, -0.18972094357014, -0.37163999676704, 1.3313119411469, -1.3054445981979, 0.88803350925446, -0.17386303842068, -0.21682393550873, -0.42473143339157, 1.5214993953705, -1.4919366836548, 1.0148954391479, -0.19559592008591, -0.24392691254616, -0.47782284021378, 1.7116867303848, -1.6784287691116, 1.1417573690414, -0.21732880175114, -0.27102991938591, -0.53091430664062, 1.9018741846085, -1.8649208545685, 1.2686192989349, -0.23906166851521, -0.29813289642334, -0.58400571346283, 2.0920615196228, -2.0514130592346, 1.3954812288284, -0.26079455018044, -0.32523590326309, -0.63709712028503, 2.2822489738464, -2.2379050254822, 1.5223431587219, -0.28252744674683, -0.35233888030052, -0.69018858671188, 2.4724364280701, -2.4243972301483, 1.6492050886154, -0.30426031351089, -0.37944188714027, -0.74327999353409, 2.6626238822937, -2.6108891963959, 1.7760670185089, -0.32599318027496, -0.4065448641777, -0.79637140035629, 2.8528113365173, -2.797381401062, 1.9029289484024, -0.34772607684135, -0.43364787101746, -0.84946286678314, 3.042998790741, -2.9838733673096, 2.0297908782959, -0.37511238455772, -0.43198081851006, -0.93383496999741, 3.3257534503937, -3.0896751880646, 2.2050774097443, -0.40249869227409, -0.43031379580498, -1.0182070732117, 3.6085081100464, -3.195476770401, 2.3803639411926, -0.42988502979279, -0.42864674329758, -1.1025792360306, 3.8912627696991, -3.3012783527374, 2.555650472641, -0.36874824762344, -0.33146825432777, -1.1538721323013, 4.0355100631714, -3.0198850631714, 2.6182777881622, -0.3076114654541, -0.23428976535797, -1.2051650285721, 4.1797575950623, -2.7384917736053, 2.6809048652649, -0.24647471308708, -0.13711127638817, -1.2564579248428, 4.3240051269531, -2.4570982456207, 2.7435319423676, -0.18533793091774, -0.039932791143656, -1.3077508211136, 4.4682521820068, -2.1757049560547, 2.8061590194702, -0.12420116364956, 0.057245697826147, -1.359043598175, 4.6124997138977, -1.8943115472794, 2.8687860965729, -0.063064388930798, 0.15442419052124, -1.4103364944458, 4.7567472457886, -1.612918138504, 2.9314134120941, -0.0019276202656329, 0.25160267949104, -1.4616293907166, 4.9009943008423, -1.3315247297287, 2.9940404891968, 0.059209149330854, 0.34878116846085, -1.5129222869873, 5.0452418327332, -1.0501313209534, 3.0566675662994, 0.12034592032433, 0.44595965743065, -1.5642151832581, 5.189489364624, -0.76873797178268, 3.1192946434021, 0.18148268759251, 0.54313814640045, -1.6155080795288, 5.3337364196777, -0.48734459280968, 3.1819217205048}


Could you give me some advice?

Thanks.

coppelia
Site Admin
Posts: 10339
Joined: 14 Dec 2012, 00:25

Re: simOMPL.compute doesn't reach the goalConfigs

Post by coppelia »

Hello,

this very probably means that the planner didn't succeed to find a full path.

Cheers

wozxfdha
Posts: 51
Joined: 10 Jun 2021, 09:40

Re: simOMPL.compute doesn't reach the goalConfigs

Post by wozxfdha »

So what should I do to deal with this situation? The planner solved it but not totally...

coppelia
Site Admin
Posts: 10339
Joined: 14 Dec 2012, 00:25

Re: simOMPL.compute doesn't reach the goalConfigs

Post by coppelia »

use a different planner, allow more calculation time, and/or use a multi-query planner such as PRM

Cheers

fferri
Posts: 1193
Joined: 09 Sep 2013, 19:28

Re: simOMPL.compute doesn't reach the goalConfigs

Post by fferri »

You can check if solution has been found, and which type (exact, or approximate) with:
  • simOMPL.hasApproximateSolution
  • simOMPL.hasExactSolution
If solution is not found (or is approximate) maybe not enough time has been given to the planner, or is a very difficult to find solution.

wozxfdha
Posts: 51
Joined: 10 Jun 2021, 09:40

Re: simOMPL.compute doesn't reach the goalConfigs

Post by wozxfdha »

Thanks, that is what I need.

Post Reply