balance_constraints_success_statistics.py 1.43 KB
Newer Older
1
2
3
4
5
6
##
## hpp-wholebody-step-server should be launched
##

from hpp.corbaserver import Client, ProblemSolver
from hpp.corbaserver.wholebody_step import Client as WSClient
7
from hpp.corbaserver.wholebody_step import Problem
8
9
10
11
12
13
14
15
16
17
18
from hpp.corbaserver.hrp2 import Robot

Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix = '_capsule'

robot = Robot ('hrp2')
p = ProblemSolver (robot)
wcl = WSClient ()
q0 = robot.getInitialConfig ()

constraintName = "balance"
19
20
21
wcl.problem.addStaticStabilityConstraints (constraintName, q0, robot.leftAnkle,
                                           robot.rightAnkle, "",
                                           Problem.SLIDING)
22
23
24
25
26
27
balanceConstraints = [constraintName + "/relative-com",
                      constraintName + "/relative-orientation",
                      constraintName + "/relative-position",
                      constraintName + "/orientation-left-foot",
                      constraintName + "/position-left-foot"]

28
robot.setJointBounds ("base_joint_xyz", [-4,4,-4,4,-4,4])
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43

def testConstraint (constraints, nbIter = 100):
  success = 0
  p.resetConstraints ()
  p.setNumericalConstraints ('test', constraints)
  for i in range (nbIter):
    res = p.applyConstraints (robot.shootRandomConfig ())
    if res[0]:
      success = success + 1
  print "Constraint ", constraints, " succeeds ", success * 100 / nbIter, " %"

for constraint in balanceConstraints:
  testConstraint ([constraint])

testConstraint (balanceConstraints)