diff --git a/script/scenarios/memmo/talos_platform.py b/script/scenarios/memmo/talos_platform.py new file mode 100644 index 0000000000000000000000000000000000000000..875ee14032d5a2e7ab7c31a032b216a10c9366fb --- /dev/null +++ b/script/scenarios/memmo/talos_platform.py @@ -0,0 +1,185 @@ +from hpp.corbaserver.rbprm.talos import Robot +from hpp.gepetto import Viewer +from tools.display_tools import * +import time +print "Plan guide trajectory ..." +import talos_platform_path as tp + +pId = 0 +""" +print "Done." +import time +statusFilename = tp.statusFilename + +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 () + +# Set the bounds for the root +fullBody.setJointBounds ("root_joint", tp.rootBounds) +fullBody.setConstrainedJointsBounds() + +# add the 6 extraDof for velocity and acceleration (see *_path.py script) +fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof) +fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0]) +ps = tp.ProblemSolver( fullBody ) +ps.setParameter("Kinodynamic/velocityBound",tp.vMax) +ps.setParameter("Kinodynamic/accelerationBound",tp.aMax) +#load the viewer +try : + v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True) +except Exception: + print "No viewer started !" + class FakeViewer(): + def __init__(self): + return + def __call__(self,q): + return + def addLandmark(self,a,b): + return + v = FakeViewer() + + +# load a reference configuration +q_ref = fullBody.referenceConfig[::]+[0]*6 +#q_ref = fullBody.referenceConfig_legsApart[::]+[0]*6 +q_init = q_ref[::] +fullBody.setReferenceConfig(q_ref) + +""" +if abs(tp.q_goal[1]) <= abs(tp.q_goal[0]) : + fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6) + heuristicR = "fixedStep08" + heuristicL = "fixedStep08" + print "Use weight for straight walk" + fullBody.usePosturalTaskContactCreation(True) +else : + fullBody.setPostureWeights(fullBody.postureWeights_straff[::]+[0]*6) + print "Use weight for straff walk" + if tp.q_goal[1] < 0 : + print "start with right leg" + heuristicL = "static" + heuristicR = "fixedStep06" + else: + print "start with left leg" + heuristicR = "static" + heuristicL = "fixedStep06" +""" + +fullBody.setCurrentConfig (q_init) + +print "Generate limb DB ..." +tStart = time.time() +# generate databases : + +nbSamples = 100000 +fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep06", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.85) +fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True) +fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep06", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.85) +fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True) + + +tGenerate = time.time() - tStart +print "Done." +print "Databases generated in : "+str(tGenerate)+" s" + +#define initial and final configurations : +configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace() + +q_init[0:7] = tp.ps.configAtParam(pId,0)[0:7] # use this to get the correct orientation +q_goal = q_init[::]; q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7] +vel_init = tp.ps.configAtParam(pId,0)[tp.indexECS:tp.indexECS+3] +acc_init = tp.ps.configAtParam(pId,0)[tp.indexECS+3:tp.indexECS+6] +vel_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[tp.indexECS:tp.indexECS+3] +acc_goal = [0,0,0] + +robTreshold = 3 +# copy extraconfig for start and init configurations +q_init[configSize:configSize+3] = vel_init[::] +q_init[configSize+3:configSize+6] = acc_init[::] +q_goal[configSize:configSize+3] = vel_goal[::] +q_goal[configSize+3:configSize+6] = [0,0,0] + + +q_init[2] = q_ref[2] +q_goal[2] = q_ref[2] + + +fullBody.setStaticStability(True) +fullBody.setCurrentConfig (q_init) +v(q_init) + +fullBody.setCurrentConfig (q_goal) +v(q_goal) + +v.addLandmark('talos/base_link',0.3) +v(q_init) +#fullBody.setReferenceConfig(fullBody.referenceConfig_legsApart[::]+[0]*6) + + +# specify the full body configurations as start and goal state of the problem + +if q_goal[1] < 0: # goal on the right side of the circle, start motion with right leg first + fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lLegId]) + fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lLegId]) +else : + fullBody.setStartState(q_init,[fullBody.lLegId,fullBody.rLegId]) + fullBody.setEndState(q_goal,[fullBody.lLegId,fullBody.rLegId]) + +print "Generate contact plan ..." +tStart = time.time() +configs = fullBody.interpolate(0.005,pathId=pId,robustnessTreshold = 3, filterStates = True,quasiStatic=True) +tInterpolateConfigs = time.time() - tStart +print "Done." +print "Contact plan generated in : "+str(tInterpolateConfigs)+" s" +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 +if len(configs) > 5 : + cg_too_many_states = True + cg_success = False + print "Discarded contact sequence because it was too long." +else: + cg_too_many_states = False + +f = open(statusFilename,"a") +f.write("cg_success: "+str(cg_success)+"\n") +f.write("cg_reach_goal: "+str(cg_reach_goal)+"\n") +f.write("cg_too_many_states: "+str(cg_too_many_states)+"\n") +f.close() + +if (not cg_success) or cg_too_many_states or (not cg_reach_goal): + import sys + sys.exit(1) + +# put back original bounds for wholebody methods +fullBody.setJointBounds('leg_left_6_joint',joint6L_bounds_prev) +fullBody.setJointBounds('leg_left_2_joint',joint2L_bounds_prev) +fullBody.setJointBounds('leg_right_6_joint',joint6R_bounds_prev) +fullBody.setJointBounds('leg_right_2_joint',joint2R_bounds_prev) + diff --git a/script/scenarios/memmo/talos_platform_path.py b/script/scenarios/memmo/talos_platform_path.py new file mode 100644 index 0000000000000000000000000000000000000000..bef10cb240fe05ed297b09b1ca6d55ed7e546696 --- /dev/null +++ b/script/scenarios/memmo/talos_platform_path.py @@ -0,0 +1,125 @@ +from hpp.corbaserver.rbprm.talos_abstract import Robot +from hpp.gepetto import Viewer +from hpp.corbaserver import ProblemSolver +import numpy as np +import time +statusFilename = "infos.log" + + +vMax = 0.5# linear velocity bound for the root +aMax = 0.5# linear acceleration bound for the root +extraDof = 6 +mu=0.5# coefficient of friction +# Creating an instance of the helper class, and loading the robot +# Creating an instance of the helper class, and loading the robot +rbprmBuilder = Robot () +# Define bounds for the root : bounding box of the scenario +rootBounds = [0.2,3.9, 0.2, 2.2, 0.95, 1.05] +rbprmBuilder.setJointBounds ("root_joint", rootBounds) + +# The following lines set constraint on the valid configurations: +# a configuration is valid only if all limbs can create a contact with the corresponding afforcances type +rbprmBuilder.setFilter([])#'talos_lleg_rom','talos_rleg_rom']) +rbprmBuilder.setAffordanceFilter('talos_lleg_rom', ['Support',]) +rbprmBuilder.setAffordanceFilter('talos_rleg_rom', ['Support']) +# We also bound the rotations of the torso. (z, y, x) +rbprmBuilder.boundSO3([-1.7,1.7,-0.1,0.1,-0.1,0.1]) +# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root +rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof) +# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis) +rbprmBuilder.client.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,-aMax,aMax,-aMax,aMax,0,0]) +indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.robot.getDimensionExtraConfigSpace() + +# Creating an instance of HPP problem solver +ps = ProblemSolver( rbprmBuilder ) +# define parameters used by various methods : +ps.setParameter("Kinodynamic/velocityBound",vMax) +ps.setParameter("Kinodynamic/accelerationBound",aMax) +ps.setParameter("DynamicPlanner/sizeFootX",0.2) +ps.setParameter("DynamicPlanner/sizeFootY",0.12) +ps.setParameter("DynamicPlanner/friction",0.5) +# sample only configuration with null velocity and acceleration : +ps.setParameter("ConfigurationShooter/sampleExtraDOF",False) + +# initialize the viewer : +from hpp.gepetto import ViewerFactory +vf = ViewerFactory (ps) + +# load the module to analyse the environnement and compute the possible contact surfaces +from hpp.corbaserver.affordance.affordance import AffordanceTool +afftool = AffordanceTool () +afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) +afftool.loadObstacleModel ("hpp_environments", "multicontact/plateforme_not_flat", "planning", vf, reduceSizes=[0.13,0,0]) +try : + v = vf.createViewer(displayArrows = True) +except Exception: + print "No viewer started !" + class FakeViewer(): + def __init__(self): + return + def __call__(self,q): + return + v = FakeViewer() + +#afftool.visualiseAffordances('Support', v, v.color.lightBrown) + +v.addLandmark(v.sceneName,1) +q_init = rbprmBuilder.getCurrentConfig (); +q_init[0:3] = [0.3,1.15,0.98] +q_init[3:7] = [0,0,0,1] + +# sample random position on a circle of radius 2m +""" +radius = 0.3 +import random +random.seed() +alpha = random.uniform(0.,2.*np.pi) +print "Test on a circle, alpha = ",alpha +q_goal = q_init[::] +q_goal [0:3] = [radius*np.sin(alpha), -radius*np.cos(alpha), 1.] +""" +q_goal = q_init[::] +q_goal [0:2] = [3.3,2.2] + +print "initial root position : ",q_init[0:3] +print "final root position : ",q_goal[0:3] +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") +ps.selectPathValidation("RbprmPathValidation",0.05) +# Choosing kinodynamic methods : +ps.selectSteeringMethod("RBPRMKinodynamic") +ps.selectDistance("Kinodynamic") +ps.selectPathPlanner("DynamicPlanner") + +# Solve the planning problem : +t = ps.solve () +print "Guide planning time : ",t + +try : + # display solution : + from hpp.gepetto import PathPlayer + pp = PathPlayer (v) + pp.dt=0.1 + pp.displayVelocityPath(0) + #v.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP") + pp.dt=0.01 + #pp(0) +except Exception: + pass + +# move the robot out of the view before computing the contacts +q_far = q_init[::] +q_far[2] = -2 +v(q_far) +