from hpp.corbaserver import ProblemSolver
from math import pi
parser = ArgumentParser()
parser.add_argument('-N', default=20, type=int)
parser.add_argument('--display', action='store_true')
parser.add_argument('--run', action='store_true')
args = parser.parse_args()
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix = '_capsule'
#Robot.urdfSuffix = ''
#Robot.srdfSuffix= ''
robot = Robot('hrp2_14')
robot.setJointBounds("root_joint",
[-3, 3, -3, 3, 0, 1, -1, 1, -1, 1, -1, 1, -1, 1])
cl = robot.client
q0 = robot.getInitialConfig()
# Add constraints
ps = ProblemSolver(robot)
ps.addPartialCom('hrp2_14', ['root_joint'])
robot.createSlidingStabilityConstraint("balance/", 'hrp2_14', robot.leftAnkle,
robot.rightAnkle, q0)
ps.setNumericalConstraints("balance", [
"balance/relative-com",
"balance/relative-pose",
"balance/pose-left-foot",
])
robot = Robot ('hrp2')
p = ProblemSolver (robot)
wcl = WSClient ()
q0 = robot.getInitialConfig ()
constraintName = "balance"
wcl.problem.addStaticStabilityConstraints (constraintName, q0, robot.leftAnkle, robot.rightAnkle)
balanceConstraints = [constraintName + "/relative-com",
constraintName + "/relative-orientation",
constraintName + "/relative-position",
constraintName + "/orientation-left-foot",
constraintName + "/position-left-foot"]
for a in ["x","y","z"]:
robot.setJointBounds ("base_joint_"+a, [-4,4])
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)
Robot.urdfSuffix = '_reduced'
Robot.srdfSuffix = ''
# Define which problems are to be solved
walk_forward=True
walk_sideway=True
walk_oblique=True
walk_various=True
# Load HRP2 and a screwgun {{{3
robot = Robot ('hrp2')
ps = ProblemSolver (robot)
#~ ps.selectPathPlanner ("DiffusingPlanner")
# ps.addPathOptimizer ("SmallSteps")
vf = ViewerFactory (ps)
robot.setJointBounds ("base_joint_xyz", [-4,4,-4,4,-4,4])
#~ vf.loadObstacleModel ("hpp_tutorial", "box", "box")
#~ ps.moveObstacle ("box/base_link_0", [0.5,0.415,0.7,1,0,0,0])
wcl = WSClient ()
ps.setErrorThreshold (1e-3)
ps.setMaxIterations (60)
# 3}}}
# Define configurations {{{3
half_sitting = q = robot.getInitialConfig ()
q_init = half_sitting [::]
print (q_init)
#/usr/bin/env python
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.wholebody_step.client import Client as WsClient, Problem
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix= '_capsule'
robot = Robot ('hrp2_14')
robot.setJointBounds ("base_joint_xyz", (-3, 3, -3, 3, 0, 1))
cl = robot.client
q0 = robot.getInitialConfig ()
# Add constraints
wcl = WsClient ()
wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle,
robot.rightAnkle, "",
Problem.SLIDING)
ps = ProblemSolver (robot)
ps.setNumericalConstraints ("balance", ["balance/relative-com",
"balance/relative-orientation",
"balance/relative-position",
"balance/orientation-left-foot",
"balance/position-left-foot"])
# lock hands in closed position
lockedjoints = robot.leftHandClosed ()
for name, value in lockedjoints.iteritems ():
#/usr/bin/env python
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.wholebody_step.client import Client as WsClient, Problem
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix = '_capsule'
robot = Robot('hrp2_14')
robot.setJointBounds("base_joint_xyz", (-3, 3, -3, 3, 0, 1))
cl = robot.client
q0 = robot.getInitialConfig()
# Add constraints
wcl = WsClient()
wcl.problem.addStaticStabilityConstraints("balance", q0, robot.leftAnkle,
robot.rightAnkle, "",
Problem.SLIDING)
ps = ProblemSolver(robot)
ps.setNumericalConstraints("balance", [
"balance/relative-com", "balance/relative-orientation",
"balance/relative-position", "balance/orientation-left-foot",
"balance/position-left-foot"
])
# lock hands in closed position
lockedjoints = robot.leftHandClosed()
for name, value in lockedjoints.iteritems():
#/usr/bin/env python
# Script which goes with gravity.launch, to simulate Hrp2 in the space with a spaceship from a movie and an emu character from Nasa.gov. See gravity_description package.
from hpp.gepetto import Viewer, PathPlayer
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver import Client
#Robot.urdfSuffix = '_capsule'
#Robot.srdfSuffix= '_capsule'
robot = Robot ('hrp2_14')
#robot.setJointBounds('base_joint_xyz', [-5, 10, -10, 10, -5, 5])
robot.setJointBounds('base_joint_xyz', [-3, 10, -4, 4, -3, 5])
ps = ProblemSolver (robot)
cl = robot.client
r = Viewer (ps)
pp = PathPlayer (cl, r)
#r.loadObstacleModel ("gravity_description","emu","emu")
r.loadObstacleModel ("gravity_description","gravity_decor","gravity_decor")
# Difficult init config
q1 = [1.45, 1.05, -0.8, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, 1.0, -1.0, -0.85, 0.0, -0.65, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.9, 0.0, -0.6, -0.3, 0.7, -0.4, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.1, -0.15, -0.1, 0.3, -0.418879, 0.0, 0.0, 0.3, -0.8, 0.3, 0.0, 0.0]
q2 = [6.55, -2.91, 1.605, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.2, 1.0, -0.4, -1.0, 0.0, -0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.5, -0.2, 0.1, -0.3, 0.1, 0.1, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0]
q2hard = [7.60, -2.41, 0.545, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.8, 0.0, -0.4, -0.55, 0.0, -0.6, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -2.8, 0.0, 0.1, -0.2, -0.1, 0.4, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.1, 1.2, -0.4, 0.2, -0.3, 0.0, -0.4, 0.2, 0.7, 0.0]
robot.isConfigValid(q1)
robot.isConfigValid(q2)
# qf should be invalid
#/usr/bin/env python
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.wholebody_step.client import Client as WsClient, Problem
from math import pi
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix= '_capsule'
#Robot.urdfSuffix = ''
#Robot.srdfSuffix= ''
robot = Robot ('hrp2_14')
robot.setJointBounds ("root_joint", [-3, 3, -3, 3, 0, 1,-1,1,-1,1,-1,1,-1,1])
cl = robot.client
q0 = robot.getInitialConfig()
# Add constraints
wcl = WsClient ()
wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle, robot.rightAnkle,"",Problem.SLIDING)
ps = ProblemSolver (robot)
ps.setNumericalConstraints ("balance", ["balance/relative-com",
"balance/relative-orientation",
"balance/relative-position",
"balance/orientation-left-foot",
"balance/position-left-foot"
])
# lock hands in closed position
#/usr/bin/env python
from hpp.gepetto import Viewer, PathPlayer
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.wholebody_step.client import Client as WsClient
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix= '_capsule'
robot = Robot ('hrp2_14')
robot.setJointBounds ("base_joint_xyz", [-3, 3, -3, 3, 0, 1])
ps = ProblemSolver (robot)
cl = robot.client
r = Viewer (ps)
q0 = robot.getInitialConfig ()
r (q0)
# Add constraints
wcl = WsClient ()
wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle,
robot.rightAnkle)
ps.setNumericalConstraints ("balance", ["balance/relative-com",
"balance/relative-orientation",
"balance/relative-position",
"balance/orientation-left-foot",
"balance/position-left-foot"])
#/usr/bin/env python
from hpp.gepetto import Viewer, PathPlayer
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.wholebody_step.client import Client as WsClient
from hpp.corbaserver.wholebody_step import Problem
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix= '_capsule'
robot = Robot ('hrp2_14')
robot.setJointBounds ("base_joint_xyz", [-3, 3, -3, 3, 0, 1])
ps = ProblemSolver (robot)
cl = robot.client
r = Viewer (ps)
q0 = robot.getInitialConfig ()
r (q0)
# Add constraints
wcl = WsClient ()
wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle,
robot.rightAnkle, "",
Problem.SLIDING)
ps.setNumericalConstraints ("balance", ["balance/relative-com",
"balance/relative-orientation",
"balance/relative-position",
"balance/orientation-left-foot",
#/usr/bin/env python
from hpp.gepetto import Viewer, PathPlayer
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.wholebody_step.client import Client as WsClient
import time
#Robot.urdfSuffix = '_capsule'
#Robot.srdfSuffix= '_capsule'
robot = Robot('hrp2_14') #46 DOF
#robot.setJointBounds ('base_joint_xyz', [-2.5, 2.5, -2.5, 2.5, 0.62, 0.68])
robot.setJointBounds('base_joint_xyz', [-3, 3, -3, 3, 0, 1])
ps = ProblemSolver(robot)
cl = robot.client
Viewer.withFloor = True
r = Viewer(ps)
#r.loadObstacleModel ("room_description","room","room")
#r.loadObstacleModel ("room_description","walls","walls")
pp = PathPlayer(cl, r)
q0 = robot.getInitialConfig()
r(q0)
# Add constraints
wcl = WsClient()
wcl.problem.addStaticStabilityConstraints("balance", q0, robot.leftAnkle,
robot.rightAnkle)
ps.setNumericalConstraints("balance", [
"balance/relative-com", "balance/relative-orientation",
Problem.SLIDING)
balanceConstraints = [constraintName + "/relative-com",
constraintName + "/relative-orientation",
constraintName + "/relative-position",
constraintName + "/orientation-left-foot",
constraintName + "/position-left-foot"]
c2name = "stability"
wcl.problem.addStabilityConstraints (c2name, q0, robot.leftAnkle,
robot.rightAnkle, "", Problem.ALIGNED_COM)
b2C = [ c2name + "/com-between-feet",
c2name + "/orientation-right",
c2name + "/orientation-left",
c2name + "/position-right",
c2name + "/position-left"]
robot.setJointBounds ("base_joint_xyz", [-4,4,-4,4,-4,4])
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)
p = ProblemSolver(robot)
wcl = WSClient()
q0 = robot.getInitialConfig()
constraintName = "balance"
wcl.problem.addStaticStabilityConstraints(constraintName, q0, robot.leftAnkle,
robot.rightAnkle)
balanceConstraints = [
constraintName + "/relative-com", constraintName + "/relative-orientation",
constraintName + "/relative-position",
constraintName + "/orientation-left-foot",
constraintName + "/position-left-foot"
]
for a in ["x", "y", "z"]:
robot.setJointBounds("base_joint_" + a, [-4, 4])
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])
#/usr/bin/env python
from hpp.gepetto import Viewer, PathPlayer
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.wholebody_step.client import Client as WsClient
import time
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix= '_capsule'
robot = Robot ('hrp2_14')
robot.setJointBounds ('base_joint_xyz', [-4, 4, -4, 4, 0.6, 0.7])
ps = ProblemSolver (robot)
cl = robot.client
Viewer.withFloor = True
r = Viewer (ps)
r.loadObstacleModel ("room_description","room","room")
r.loadObstacleModel ("room_description","walls","walls")
pp = PathPlayer (cl, r)
q0 = robot.getInitialConfig ()
r (q0)
q1 = [1, 2.2, 0.64, 0.707, 0, 0, 0.707, 0.0, 0.0, 0.0, 0.0, 0.261799, 0.17453, 0.0, -0.523599, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0]
q2 = [0.5, -2.0, 0.64, 0.707, 0, 0, -0.707, 0.0, 0.0, 0.0, 0.0, -1.85, 0.2, -0.7, -0.7, 0.3, 0.3, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.5, -0.2, 0.0, -0.3, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0]
r(q1)
# Add constraints
wcl = WsClient ()
wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle,
#/usr/bin/env python
from hpp.gepetto import Viewer, PathPlayer
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.wholebody_step.client import Client as WsClient
import time
#Robot.urdfSuffix = '_capsule'
#Robot.srdfSuffix= '_capsule'
robot = Robot ('hrp2_14') #46 DOF
#robot.setJointBounds ('base_joint_xyz', [-2.5, 2.5, -2.5, 2.5, 0.62, 0.68])
robot.setJointBounds ('base_joint_xyz', [-3, 3, -3, 3, 0, 1])
ps = ProblemSolver (robot)
cl = robot.client
Viewer.withFloor = True
r = Viewer (ps)
#r.loadObstacleModel ("room_description","room","room")
#r.loadObstacleModel ("room_description","walls","walls")
pp = PathPlayer (cl, r)
q0 = robot.getInitialConfig ()
r (q0)
# Add constraints
wcl = WsClient ()
wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle,
robot.rightAnkle)
ps.setNumericalConstraints ("balance", ["balance/relative-com",
"balance/relative-orientation",
from hpp_ros import PathPlayer
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver import Client
import time
import sys
sys.path.append('/local/mcampana/devel/hpp/src/test-hpp/script')
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix= '_capsule'
robot = Robot ('hrp2_14')
cl = robot.client
ps = ProblemSolver (robot)
r = ScenePublisher (robot)
p = PathPlayer (cl, r)
robot.setJointBounds('base_joint_xyz', [-2.1, -0.1, -0.3, 0.6, 0.505, 0.905])
q1 = [-0.2, 0.1, 0.705, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.4, 0, -1.2, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0]
q2 = [-0.2, 0.1, 0.705, 1, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, 1.0, 0, -1.4, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0]
# lock hands in closed position
lockedDofs = robot.leftHandClosed ()
for name, value in lockedDofs.iteritems ():
ps.lockDof (name, value, 0, 0)
lockedDofs = robot.rightHandClosed ()
for name, value in lockedDofs.iteritems ():
ps.lockDof (name, value, 0, 0)
ps.setInitialConfig (q1)
