This repository has been archived by the owner on Dec 22, 2024. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 9
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #47 from pFernbach/devel
addNewContact with rotation, add tools/scenario scripts
- Loading branch information
Showing
21 changed files
with
753 additions
and
37 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
146 changes: 146 additions & 0 deletions
146
script/scenarios/sandbox/ANYmal/anymal_modular_palet_low.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,146 @@ | ||
| from hpp.corbaserver.rbprm.anymal import Robot | ||
| from hpp.gepetto import Viewer | ||
| from tools.display_tools import * | ||
| import time | ||
| print "Plan guide trajectory ..." | ||
| import scenarios.sandbox.ANYmal.anymal_modular_palet_low_path as tp | ||
|
|
||
| #Robot.urdfSuffix += "_safeFeet" | ||
| statusFilename = tp.statusFilename | ||
| pId = tp.pid | ||
| 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 | ||
| rootBounds = tp.rootBounds[::] | ||
|
|
||
| rootBounds[0] -= 0.2 | ||
| rootBounds[1] += 0.2 | ||
| rootBounds[2] -= 0.5 | ||
| rootBounds[3] += 0.5 | ||
| rootBounds[4] -= 0.2 | ||
| rootBounds[5] += 0.2 | ||
| fullBody.setJointBounds ("root_joint", rootBounds) | ||
| fullBody.setVeryConstrainedJointsBounds() | ||
|
|
||
| # 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,-tp.vMax,tp.vMax,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,-tp.aMaxZ,tp.aMaxZ]) | ||
| ps = tp.ProblemSolver( fullBody ) | ||
| ps.setParameter("Kinodynamic/velocityBound",tp.vMax) | ||
| ps.setParameter("Kinodynamic/accelerationBound",tp.aMax) | ||
| ps.setParameter("FullBody/frictionCoefficient",tp.mu) | ||
| #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_init = q_ref[::] | ||
| fullBody.setReferenceConfig(q_ref) | ||
| fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6) | ||
| #fullBody.usePosturalTaskContactCreation(True) | ||
|
|
||
| fullBody.setCurrentConfig (q_init) | ||
|
|
||
| print "Generate limb DB ..." | ||
| tStart = time.time() | ||
| # generate databases : | ||
|
|
||
| fullBody.loadAllLimbs("static","ReferenceConfiguration",nbSamples=100000,disableEffectorCollision=False) | ||
| tGenerate = time.time() - tStart | ||
| print "Done." | ||
| print "Databases generated in : "+str(tGenerate)+" s" | ||
| v.addLandmark('anymal/base_0',0.2) | ||
|
|
||
| #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] | ||
| vel_init = [0,0,0] | ||
| robTreshold = 5 | ||
| # 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] | ||
|
|
||
| normals = [[0.,0.,1.] for _ in range(4)] | ||
| fullBody.setStaticStability(True) | ||
| fullBody.setCurrentConfig (q_init) | ||
| v(q_init) | ||
| fullBody.setStartState(q_init,Robot.limbs_names,normals) | ||
| fullBody.setEndState(q_goal,Robot.limbs_names,normals) | ||
|
|
||
| fullBody.setCurrentConfig (q_goal) | ||
| v(q_goal) | ||
|
|
||
|
|
||
| print "Generate contact plan ..." | ||
| tStart = time.time() | ||
| configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=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 | ||
|
|
||
| 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) | ||
|
|
||
| #beginId = 2 | ||
|
|
||
| # put back original bounds for wholebody methods | ||
| fullBody.resetJointsBounds() | ||
| #displayContactSequence(v,configs) |
136 changes: 136 additions & 0 deletions
136
script/scenarios/sandbox/ANYmal/anymal_modular_palet_low_path.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,136 @@ | ||
| from hpp.corbaserver.rbprm.anymal_abstract import Robot | ||
| Robot.urdfName += "_large" | ||
| from hpp.gepetto import Viewer | ||
| from hpp.corbaserver import ProblemSolver | ||
| from pinocchio import Quaternion | ||
| import numpy as np | ||
| import time | ||
| import math | ||
| statusFilename = "infos.log" | ||
|
|
||
| Z_VALUE = 0.465 | ||
| Y_VALUE = -0.1 | ||
| vInit = 0.3 | ||
| vMax = 0.5# linear velocity bound for the root | ||
| aMax = 0.5# linear acceleration bound for the root | ||
| aMaxZ = 5. | ||
| extraDof = 6 | ||
| mu=0.3# 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 = [-1.7,1.7, Y_VALUE-0.001, Y_VALUE+0.001, Z_VALUE-0.001, Z_VALUE+0.231] | ||
| 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(Robot.urdfNameRom) | ||
| for rom in rbprmBuilder.urdfNameRom : | ||
| rbprmBuilder.setAffordanceFilter(rom, ['Support']) | ||
|
|
||
| # We also bound the rotations of the torso. (z, y, x) | ||
| rbprmBuilder.boundSO3([-3.14,3.14,-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,-vMax,vMax,-aMax,aMax,-aMax,aMax,-aMaxZ,aMaxZ]) | ||
| 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("Kinodynamic/verticalAccelerationBound",aMaxZ) | ||
| ps.setParameter("Kinodynamic/forceAllOrientation",True) | ||
| ps.setParameter("DynamicPlanner/sizeFootX",0.2) | ||
| ps.setParameter("DynamicPlanner/sizeFootY",0.12) | ||
| ps.setParameter("DynamicPlanner/friction",mu) | ||
| # sample only configuration with null velocity and acceleration : | ||
| ps.setParameter("ConfigurationShooter/sampleExtraDOF",False) | ||
| ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",100) | ||
|
|
||
| # 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.setMinimumArea('Support',0.03) | ||
| afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low_collision", "planning", vf, reduceSizes=[0.1,0,0]) | ||
| #afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low", "planning", vf) | ||
| 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[2] = Z_VALUE | ||
| q_init[0:2] = [-1.6,Y_VALUE] | ||
| q_init[-6] = vInit | ||
| v(q_init) | ||
|
|
||
| q_goal=q_init[::] | ||
| q_goal[0] = 1.6 | ||
|
|
||
|
|
||
| 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") | ||
| ps.addPathOptimizer ("RandomShortcutDynamic") | ||
|
|
||
|
|
||
| # Solve the planning problem : | ||
| t = ps.solve() | ||
| print "Guide planning done in "+str(t)+", optimizing trajectory ..." | ||
| pid = ps.numberPaths()-1 | ||
|
|
||
| for i in range(20): | ||
| ps.optimizePath(pid) | ||
| pid = ps.numberPaths()-1 | ||
|
|
||
| try : | ||
| # display solution : | ||
| from hpp.gepetto import PathPlayer | ||
| pp = PathPlayer (v) | ||
| pp.dt=0.1 | ||
| pp.displayPath(pid)#pp.displayVelocityPath(0) # | ||
| v.client.gui.setVisibility("path_"+str(pid)+"_root","ALWAYS_ON_TOP") | ||
| pp.dt=0.01 | ||
| pp(pid) | ||
| 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) | ||
|
|
Oops, something went wrong.