diff --git a/script/scenarios/memmo/talos_circle_oriented_path.py b/script/scenarios/memmo/talos_circle_oriented_path.py index eb03e0b0dca77c488460c361897b050e8f776686..b01213ff311b9f4d38e1d59314e0a23839f99e3d 100644 --- a/script/scenarios/memmo/talos_circle_oriented_path.py +++ b/script/scenarios/memmo/talos_circle_oriented_path.py @@ -4,7 +4,7 @@ from hpp.corbaserver import ProblemSolver import numpy as np from pinocchio import Quaternion import time -statusFilename = "infos.log" +statusFilename = "/res/infos.log" vMax = 0.5# linear velocity bound for the root diff --git a/script/scenarios/memmo/talos_circle_path.py b/script/scenarios/memmo/talos_circle_path.py index 40256d4a4b9fcd10d795ff1e0ed60b5f6af6f1a3..d9bc5e191f3836334f3350580b543fc2e8263af7 100644 --- a/script/scenarios/memmo/talos_circle_path.py +++ b/script/scenarios/memmo/talos_circle_path.py @@ -3,7 +3,7 @@ from hpp.gepetto import Viewer from hpp.corbaserver import ProblemSolver import numpy as np import time -statusFilename = "infos.log" +statusFilename = "/res/infos.log" vMax = 0.5# linear velocity bound for the root diff --git a/script/scenarios/memmo/talos_moveEffector_flat.py b/script/scenarios/memmo/talos_moveEffector_flat.py index 2d7aaf1ed9a668c68a4007175880106273315761..e0675554d7af96b78ccd74ba812e9925f016fb47 100644 --- a/script/scenarios/memmo/talos_moveEffector_flat.py +++ b/script/scenarios/memmo/talos_moveEffector_flat.py @@ -6,7 +6,7 @@ from hpp.corbaserver import ProblemSolver import os import random import time -statusFilename = "infos.log" +statusFilename = "/res/infos.log" diff --git a/script/scenarios/memmo/talos_moveEffector_stairs_m10.py b/script/scenarios/memmo/talos_moveEffector_stairs_m10.py index e1c5348e2cd3ab10ad24a3cf2498936765b28825..22fe5e38cf43a75e01e32bb902554ac9ead79fd2 100644 --- a/script/scenarios/memmo/talos_moveEffector_stairs_m10.py +++ b/script/scenarios/memmo/talos_moveEffector_stairs_m10.py @@ -6,7 +6,7 @@ from hpp.corbaserver import ProblemSolver import os import random import time -statusFilename = "infos.log" +statusFilename = "/res/infos.log" diff --git a/script/scenarios/memmo/talos_moveEffector_stairs_m15.py b/script/scenarios/memmo/talos_moveEffector_stairs_m15.py index d6e0eac434b0132d23b377e613b47803c9722f41..a8ecf5032593be71ae447c71777b5773ce5fbadf 100644 --- a/script/scenarios/memmo/talos_moveEffector_stairs_m15.py +++ b/script/scenarios/memmo/talos_moveEffector_stairs_m15.py @@ -6,7 +6,7 @@ from hpp.corbaserver import ProblemSolver import os import random import time -statusFilename = "infos.log" +statusFilename = "/res/infos.log" diff --git a/script/scenarios/memmo/talos_moveEffector_stairs_p10.py b/script/scenarios/memmo/talos_moveEffector_stairs_p10.py index 6df22ff2e06038f4b595bd276bcdb6f7ab29847d..7a095c6032269257ff34527422843f6e3576b208 100644 --- a/script/scenarios/memmo/talos_moveEffector_stairs_p10.py +++ b/script/scenarios/memmo/talos_moveEffector_stairs_p10.py @@ -6,7 +6,7 @@ from hpp.corbaserver import ProblemSolver import os import random import time -statusFilename = "infos.log" +statusFilename = "/res/infos.log" diff --git a/script/scenarios/memmo/talos_moveEffector_stairs_p15.py b/script/scenarios/memmo/talos_moveEffector_stairs_p15.py index 1ffb355d9600f32b6aea3ff0f82c6abf54f62c63..ba98fc28e7646115427b6e863331f11e557c33ad 100644 --- a/script/scenarios/memmo/talos_moveEffector_stairs_p15.py +++ b/script/scenarios/memmo/talos_moveEffector_stairs_p15.py @@ -6,7 +6,7 @@ from hpp.corbaserver import ProblemSolver import os import random import time -statusFilename = "infos.log" +statusFilename = "/res/infos.log" diff --git a/script/scenarios/memmo/talos_platform_random.py b/script/scenarios/memmo/talos_platform_random.py index 2064252363d72a4c755e74263b0662dcc7dbfdfe..42b2905497b36dfcf7339ff49dd8612dbda35d7b 100644 --- a/script/scenarios/memmo/talos_platform_random.py +++ b/script/scenarios/memmo/talos_platform_random.py @@ -5,8 +5,19 @@ import time print "Plan guide trajectory ..." import scenarios.memmo.talos_platform_random_path as tp #Robot.urdfSuffix += "_safeFeet" +statusFilename = tp.statusFilename pId = 0 - +f = open(statusFilename,"a") +if tp.ps.numberPaths() > 0 : + print "Path planning OK." + f.write("Planning_success: True"+"\n") + f.close() +else : + print "Error during path planning" + f.write("Planning_success: False"+"\n") + f.close() + import sys + sys.exit(1) fullBody = Robot () @@ -87,7 +98,7 @@ q_goal[configSize+3:configSize+6] = [0,0,0] fullBody.setStaticStability(True) -v.addLandmark('talos/base_link',0.3) +#v.addLandmark('talos/base_link',0.3) # FOR EASY SCENARIOS ? q_init[2]=q_ref[2] @@ -122,7 +133,30 @@ print "number of configs :", len(configs) #raw_input("Press Enter to display the contact sequence ...") #displayContactSequence(v,configs) +if len(configs) < 2 : + cg_success = False + print "Error during contact generation." +else: + cg_success = True + print "Contact generation Done." +if abs(configs[-1][0] - tp.q_goal[0]) < 0.01 and abs(configs[-1][1]- tp.q_goal[1]) < 0.01 and (len(fullBody.getContactsVariations(len(configs)-2,len(configs)-1))==1): + print "Contact generation successful." + cg_reach_goal = True +else: + print "Contact generation failed to reach the goal." + cg_reach_goal = False + +f = open(statusFilename,"a") +f.write("cg_success: "+str(cg_success)+"\n") +f.write("cg_reach_goal: "+str(cg_reach_goal)+"\n") +f.close() + +if (not cg_success): + import sys + sys.exit(1) + + # put back original bounds for wholebody methods fullBody.resetJointsBounds() -displayContactSequence(v,configs) +#displayContactSequence(v,configs) diff --git a/script/scenarios/memmo/talos_platform_random_path.py b/script/scenarios/memmo/talos_platform_random_path.py index 24018452e4a875f85a2537d68694b1c552d324f2..5434dbdc3fe3c007e50dab3287c65e52e4021a3a 100644 --- a/script/scenarios/memmo/talos_platform_random_path.py +++ b/script/scenarios/memmo/talos_platform_random_path.py @@ -5,7 +5,8 @@ from pinocchio import Quaternion import numpy as np import time import math -statusFilename = "infos.log" +statusFilename = "/res/infos.log" + vInit = 0.05# initial / final velocity to fix the direction vGoal = 0.01 @@ -119,12 +120,11 @@ ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) # write problem in files : -""" f = open(statusFilename,"w") f.write("q_init= "+str(q_init)+"\n") f.write("q_goal= "+str(q_goal)+"\n") f.close() -""" + # Choosing RBPRM shooter and path validation methods. ps.selectConfigurationShooter("RbprmShooter")