reentering planner

script/scenarios/sandbox/anymal_one_step/anymal_circle_one_step.py

+from hpp.corbaserver.rbprm.anymal import Robot
+from hpp.gepetto import Viewer
+from tools.display_tools import *
+import time
+print "Plan guide trajectory ..."
+import anymal_circle_one_step_path as tp
+print "Done."
+import time
+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 ()
+root_bounds =  tp.root_bounds
+root_bounds[-1] = 0.6
+root_bounds[-2] = 0.3
+# Set the bounds for the root
+fullBody.setJointBounds ("root_joint",  root_bounds)
+## reduce bounds on joints along x, to put conservative condition on the contact generation for sideway steps
+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,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)
+fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
+fullBody.usePosturalTaskContactCreation(True)
+
+"""
+if abs(tp.q_goal[1]) <= abs(tp.q_goal[0]) : 
+  heuristicR = "fixedStep08"
+  heuristicL = "fixedStep08"
+  print "Use weight for straight walk"
+  fullBody.usePosturalTaskContactCreation(True)
+else :
+  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, heuristicR, 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, heuristicL, 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.85)
+fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
+"""
+#~ fullBody.loadAllLimbs("fixedStep04","ReferenceConfiguration")
+fullBody.loadAllLimbs("static","ReferenceConfiguration")
+
+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('anymal/base_0',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
+
+normals = [[0.,0.,1.] for _ in range(4)]
+if q_goal[1] < 0: # goal on the right side of the circle, start motion with right leg first
+  fullBody.setStartState(q_init,[fullBody.rArmId,fullBody.rLegId,fullBody.lArmId,fullBody.lLegId],normals)
+  fullBody.setEndState(q_goal,[fullBody.rArmId,fullBody.rLegId,fullBody.lArmId,fullBody.lLegId],normals)
+else :
+  fullBody.setStartState(q_init,[fullBody.lArmId,fullBody.lLegId,fullBody.rArmId,fullBody.rLegId],normals)
+  fullBody.setEndState(q_goal,[fullBody.lArmId,fullBody.lLegId,fullBody.rArmId,fullBody.rLegId],normals)
+
+print "Generate contact plan ..."
+tStart = time.time()
+configs = fullBody.interpolate(0.002,pathId=pId,robustnessTreshold = 1, filterStates = True,quasiStatic=True)
+tInterpolateConfigs = time.time() - tStart
+print "Done."
+print "Contact plan generated in : "+str(tInterpolateConfigs)+" s"
+print "number of configs :", len(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) > 10 :
+  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.resetJointsBounds()
+
+
+
+
+
+import hpp.corbaserver.rbprm.fewstepsplanner as sp
+
+states = sp.interpolateState(fullBody, 0.002,pathId=pId,robustnessTreshold = 1, filterStates = True,quasiStatic=True)
+
+fsp = sp.FewStepPlanner(tp.cl,tp.ps,tp.rbprmBuilder, fullBody, pathPlayer = tp.pp)
+
+
+
+#~ print "names ", fsp.rbprmBuilder.getAllJointNames()
+#~ print "names ", tp.rbprmBuilder.getAllJointNames()
+
+n_goal = tp.q_goal[:7]
+n_goal[0] += 1
+
+
+
+goal_state = states[-1].q()[:]
+goal_state[:7] = n_goal
+fullBody.setStartState(states[-1].q(),[fullBody.rArmId,fullBody.rLegId,fullBody.lArmId,fullBody.lLegId],normals)
+fullBody.setEndState(goal_state, [fullBody.rArmId,fullBody.rLegId,fullBody.lArmId,fullBody.lLegId],normals)
+
+pId= fsp.guidePath(tp.q_goal[:7],n_goal)
+states2 = sp.interpolateState(fullBody, 0.002,pathId=pId,robustnessTreshold = 1, filterStates = True,quasiStatic=True, erasePreviousStates = False)
+
+
+goal_state[6] = -1
+
+fullBody.setStartState(states[-1].q(),[fullBody.rArmId,fullBody.rLegId,fullBody.lArmId,fullBody.lLegId],normals)
+fullBody.setEndState(goal_state, [fullBody.rArmId,fullBody.rLegId,fullBody.lArmId,fullBody.lLegId],normals)
+
+pId= fsp.guidePath(tp.q_goal[:7],n_goal)
+states3 = sp.interpolateState(fullBody, 0.001,pathId=pId,robustnessTreshold = 1, filterStates = True,quasiStatic=True, erasePreviousStates = False)
+

script/scenarios/sandbox/anymal_one_step/anymal_circle_one_step_path.py

+from hpp.corbaserver.rbprm.anymal_abstract import Robot
+from hpp.gepetto import Viewer
+from hpp.corbaserver import ProblemSolver
+import numpy as np
+import time
+#~ statusFilename = "/res/infos.log"
+statusFilename = "/tmp/infos.log"
+
+
+vMax = 0.3# linear velocity bound for the root
+aMax = 1.# 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
+root_bounds =  [-2,2, -2, 2, 0.4, 0.5]
+rbprmBuilder.setJointBounds ("root_joint", root_bounds)
+
+# 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(rbprmBuilder.urdfNameRom)
+for rom in rbprmBuilder.urdfNameRom :
+    rbprmBuilder.setAffordanceFilter(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.01)
+ps.setParameter("DynamicPlanner/sizeFootY",0.01)
+ps.setParameter("DynamicPlanner/friction",mu)
+# 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/ground", "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)
+
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init[0:3] = [0,0,0.465]
+q_init[3:7] = [0,0,0,1]
+
+# sample random position on a circle of radius 2m
+
+radius = 0.15
+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), 0.465]
+
+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
+
+pId = ps.numberPaths() - 1
+
+from hpp.corbaserver import Client
+ #~ #DEMO code to play root path and final contact plan
+cl = Client()
+cl.problem.selectProblem("rbprm_path")
+rbprmBuilder2 = Robot ("toto")
+ps2 = ProblemSolver( rbprmBuilder2 )
+cl.problem.selectProblem("default")
+cl.problem.movePathToProblem(pId,"rbprm_path",rbprmBuilder.getAllJointNames()[1:])
+r2 = Viewer (ps2, viewerClient=v.client)
+r2(q_far)
+
+#~ v(q_far)
+

script/scenarios/sandbox/anymal_one_step/run.sh

+#!/bin/bash         
+
+gepetto-gui &
+hpp-rbprm-server &
+ipython -i --no-confirm-exit ./$1
+
+pkill -f  'gepetto-gui'
+pkill -f  'hpp-rbprm-server'

src/hpp/corbaserver/rbprm/fewstepsplanner.py

+#!/usr/bin/env python
+# Copyright (c) 2014 CNRS
+# Author: Steve Tonneau
+#
+# This file is part of hpp-rbprm-corba.
+# hpp-rbprm-corba is free software: you can redistribute it
+# and/or modify it under the terms of the GNU Lesser General Public
+# License as published by the Free Software Foundation, either version
+# 3 of the License, or (at your option) any later version.
+#
+# hpp-manipulation-corba is distributed in the hope that it will be
+# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# hpp-manipulation-corba.  If not, see
+# <http://www.gnu.org/licenses/>.
+
+from hpp.corbaserver.rbprm import Client as RbprmClient
+from hpp.corbaserver import Client as BasicClient
+#from hpp.corbaserver.rbprm.tools.com_constraints import *
+from numpy import array
+
+from hpp.corbaserver.rbprm import rbprmstate
+from hpp.corbaserver.rbprm.rbprmstate import State
+
+def interpolateState(fullBody, stepsize, pathId = 1, robustnessTreshold = 0, filterStates = False, testReachability = True, quasiStatic = False, erasePreviousStates = False):
+  if(filterStates):
+        filt = 1
+  else:
+        filt = 0
+  configs = fullBody.clientRbprm.rbprm.interpolate(stepsize, pathId, robustnessTreshold, filt, testReachability, quasiStatic, erasePreviousStates)
+  firstStateId = fullBody.clientRbprm.rbprm.getNumStates() - len(configs)
+  return [ State(fullBody, i) for i in range(firstStateId, firstStateId + len(configs)) ]
+          
+
+def guidePath(problemSolver, fromPos, toPos):
+  ps = problemSolver
+  ps.setInitialConfig (fromPos)
+  ps.addGoalConfig(toPos)
+  ps.solve ()
+  return ps.numberPaths() - 1
+  
+
+class FewStepPlanner(object):
+  def __init__ (self, client, problemSolver, rbprmBuilder, fullBody, planContext="rbprm_path", fullBodyContext="default", pathPlayer = None ):
+    self.fullBody =  fullBody
+    self.rbprmBuilder =  rbprmBuilder
+    self.client   =  client
+    self.planContext   =  planContext
+    self.fullBodyContext   =  fullBodyContext
+    self.problemSolver   =  problemSolver
+    self.pathPlayer   =  pathPlayer
+    
+  def setPlanningContext(self):
+    self.client.problem.selectProblem(self.planContext) 
+    
+  def setFullBodyContext(self):
+    self.client.problem.selectProblem(self.fullBodyContext ) 
+    
+  def setCurrentContext(self,context):
+    return self.client.problem.selectProblem(context) 
+    
+  def currentContext(self):
+    return self.client.problem.getSelected("problem")[0]
+    
+  def _actInContext(self, context,f,*args):
+    oldContext = self.currentContext()
+    self.setCurrentContext(context)
+    res = f(*args)
+    self.setCurrentContext(oldContext)
+    return res
+    
+  def guidePath(self, fromPos, toPos):
+    pId =  self._actInContext(self.planContext,guidePath,self.problemSolver, fromPos, toPos)
+    self.setPlanningContext()
+    names =  self.rbprmBuilder.getAllJointNames()[1:]
+    self.pathPlayer(pId)
+    self.client.problem.movePathToProblem(pId,self.fullBodyContext, names)
+    self.setFullBodyContext()
+    return pId
+       
+              

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -259,7 +259,7 @@ class FullBody (Robot):
           
      ## Get the number of existing states in the client 
      #
-     def getNumStates(self, limbName):
+     def getNumStates(self):
           return self.clientRbprm.rbprm.getNumStates()
           
      ## Get the number of octreeNodes