fix again singularities near beginning and end of trajectory

Author: quangounet

src/TOPP.cpp

@@ -1822,7 +1822,10 @@ int ComputeProfiles(Constraints& constraints, dReal sdbeg, dReal sdend){
 
         /////////////////  Integrate from start /////////////////////
         ret = IntegrateForward(constraints,0,sdbeg,constraints.integrationtimestep,resprofile,1e5,testaboveexistingprofiles,testmvc,zlajpah);
-        if(ret == INT_BOTTOM || resprofile.nsteps < 10) {
+        if(ret != INT_BOTTOM && resprofile.nsteps>2) {
+            constraints.resprofileslist.push_back(resprofile);
+        }
+        if(ret == INT_BOTTOM || resprofile.nsteps < 5) {
             // Integration failed. However, if qd(0) = 0,  there was probably a singularity, and one can try different values for sdot
             std::vector<dReal> qd(constraints.trajectory.dimension);
             constraints.trajectory.Evald(0,qd);
@@ -1845,11 +1848,6 @@ int ComputeProfiles(Constraints& constraints, dReal sdbeg, dReal sdend){
                 }
             }
         }
-        else{
-            if(resprofile.nsteps>1) {
-                constraints.resprofileslist.push_back(resprofile);
-            }
-        }
         // Now if it still fails, shouganai
         if(ret==INT_BOTTOM) {
             message = "Start profile hit 0";
@@ -1861,7 +1859,10 @@ int ComputeProfiles(Constraints& constraints, dReal sdbeg, dReal sdend){
 
         /////////////////  Integrate from end /////////////////////
         ret = IntegrateBackward(constraints,constraints.trajectory.duration,sdend,constraints.integrationtimestep,resprofile,1e5,testaboveexistingprofiles,testmvc);
-        if(ret == INT_BOTTOM || resprofile.nsteps < 10) {
+        if(ret != INT_BOTTOM && resprofile.nsteps>2) {
+            constraints.resprofileslist.push_back(resprofile);
+        }
+        if(ret == INT_BOTTOM || resprofile.nsteps < 5) {
             // Integration failed. However, if qd(send) = 0, there was probably a singularity, and one can try different values for sdot
             dReal send = constraints.trajectory.duration;
             std::vector<dReal> qd(constraints.trajectory.dimension);
@@ -1889,11 +1890,6 @@ int ComputeProfiles(Constraints& constraints, dReal sdbeg, dReal sdend){
 
             }
         }
-        else{
-            if(resprofile.nsteps>1) {
-                constraints.resprofileslist.push_back(resprofile);
-            }
-        }
         if(ret==INT_BOTTOM) {
         }
         // Now if it still fails, shouganai

tests/robust_torque.py

@@ -103,9 +103,9 @@
     else:
         nfail += 1
         print ">>>>>>>>>>>>>>>>>>> TOPP could not retime ", nfail
-        TOPPpy.PlotProfiles(profileslist,switchpointslist,4)
-        TOPPopenravepy.PlotTorques(robot,traj0,traj1,0.01,taumin,taumax,3)
-        raw_input()
+        #TOPPpy.PlotProfiles(profileslist,switchpointslist,4)
+        #TOPPopenravepy.PlotTorques(robot,traj0,traj1,0.01,taumin,taumax,3)
+        #raw_input()
 
 
 print "\nNumber of failures:", nfail