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)