def __init__ (self):
print "load robot model..."
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix= '_capsule'
robot = Robot ('hrp2_14')
self.robot_interface = Robot ()
self.robot_interface.setTranslationBounds (-3, 3, -3, 3, 0, 1)
print "load client model..."
self.cl = self.robot_interface.client
print "load mpc model..."
self.mpc = MPClient()
self.precomputation = self.mpc.precomputation
self.robot = self.cl.robot
print "load scene model..."
self.scene_publisher = ScenePublisher (self.robot_interface.jointNames [4:])
self.q0 = self.robot_interface.getInitialConfig ()
self.q = self.q0
self.distanceToProjection = 1
self.vol_cvx_hull = []
cnames = self.precomputation.addNaturalConstraints ( "natural-constraints" , self.q, "LLEG_JOINT5", "RLEG_JOINT5")
self.cl.problem.setNumericalConstraints ("natural-constraints", cnames)
print cnames
from hpp.corbaserver.hrp2 import Robot
from hpp.gepetto import ViewerFactory
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
#/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 = ''
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
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",
class ProjectedVolume ():
def __init__ (self):
print "load robot model..."
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix= '_capsule'
robot = Robot ('hrp2_14')
self.robot_interface = Robot ()
self.robot_interface.setTranslationBounds (-3, 3, -3, 3, 0, 1)
print "load client model..."
self.cl = self.robot_interface.client
print "load mpc model..."
self.mpc = MPClient()
self.precomputation = self.mpc.precomputation
self.robot = self.cl.robot
print "load scene model..."
self.scene_publisher = ScenePublisher (self.robot_interface.jointNames [4:])
self.q0 = self.robot_interface.getInitialConfig ()
self.q = self.q0
self.distanceToProjection = 1
self.vol_cvx_hull = []
cnames = self.precomputation.addNaturalConstraints ( "natural-constraints" , self.q, "LLEG_JOINT5", "RLEG_JOINT5")
self.cl.problem.setNumericalConstraints ("natural-constraints", cnames)
print cnames
def displayRandomConvexHull(self):
self.setRandomConfig()
self.projectConfigurationUntilIrreducible()
self.displayRobot()
self.displayConvexHullOfProjectedCapsules()
def projectOnConstraintsManifold(self, q_in):
status, qproj, residual = self.cl.problem.applyConstraints (q_in)
if status==False or residual > 0.0001:
print "[WARNING] Projection Error on constraints manifold: ",residual," successful: ", status
return qproj, status
def setConfig(self, q_in):
self.q = q_in
self.robot.setCurrentConfig(self.q)
def setRandomConfig(self):
self.q_old = self.q
q_new = self.robot.getRandomConfig()
#do not change com
q_new[0]=-0.1
q_new[1]=0
q_new[2]=0.7
#90d rotation around z-axis
q_new[3]=1
q_new[4]=0
q_new[5]=0
q_new[6]=1
#q_new, status= self.projectOnConstraintsManifold(q_new)
#while status==False:
# q_new = self.robot.getRandomConfig()
# q_new, status= self.projectOnConstraintsManifold(q_new)
self.q_old = q_new
self.q = q_new
self.setConfig(self.q)
def displayConvexHullOfProjectedCapsules(self):
self.hull = self.precomputation.getConvexHullCapsules()
self.hull = zip(*[iter(self.hull)]*3)
print self.precomputation.getVolume()
r = rospy.Rate(1)
r.sleep()
self.scene_publisher.oid = 0
#self.scene_publisher.addPolygonFilled(self.hull)
self.scene_publisher.addPolygon(self.hull, 0.02)
self.scene_publisher.addWallAroundHole(self.hull)
self.scene_publisher.publishObjects()
r.sleep()
def projectConfigurationUntilIrreducibleConstraint(self):
self.precomputation.setCurrentConfiguration(self.q)
self.q = self.precomputation.projectUntilIrreducibleConstraint()
self.setConfig(self.q)
# def projectConfigurationUntilIrreducible(self):
# self.precomputation.setCurrentConfiguration(self.q)
# for i in range(1,20):
# self.q_new = self.precomputation.projectUntilIrreducibleOneStep()
# self.q_new = self.projectOnConstraintsManifold(self.q_new)
# self.setConfig(self.q_new)
# def projectConfigurationUntilIrreducibleOneStep(self):
# self.precomputation.setCurrentConfiguration(self.q)
# self.q_grad = self.precomputation.getGradient()
# self.q_grad_raw = self.q_grad
# self.q_grad.insert(3,0)
# self.q_grad[0]=0
# self.q_grad[1]=0
# self.q_grad[2]=0
# #make sure that quaternions are not changed
# self.q_grad[3]=0
# self.q_grad[4]=0
# self.q_grad[5]=0
# self.q_grad[6]=0
# self.q_new = [x-0.1*y for x,y in zip(self.q,self.q_grad)]
# #print self.q_new[0:8]
# self.q_new = self.projectOnConstraintsManifold(self.q_new)
# self.setConfig(self.q_new)
def displayRobot(self):
r = rospy.Rate(1)
r.sleep()
self.scene_publisher(self.q)
#/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
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')
#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)
# Difficult init config
q3 = [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]
q4 = [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]
# Load obstacles in HPP
cl.obstacle.loadObstacleModel('gravity_description','gravity_decor','') # do not move (position in urdf)
##
## hpp-wholebody-step-server should be launched
##
from hpp.corbaserver import Client, ProblemSolver
from hpp.corbaserver.wholebody_step import Client as WSClient
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"
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)
#/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_corbaserver.hpp import Configuration
from hpp_ros import ScenePublisher
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import Client
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix = '_capsule'
robot = Robot ('hrp2_14')
cl = robot.client
r = ScenePublisher (robot.jointNames [4:])
q0 = robot.getInitialConfig ()
r(q0)
#/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",
##
## hpp-wholebody-step-server should be launched
##
from hpp.corbaserver import Client, ProblemSolver
from hpp.corbaserver.wholebody_step import Client as WSClient
from hpp.corbaserver.wholebody_step import Problem
from hpp.corbaserver.hrp2 import Robot
from hpp.gepetto import ViewerFactory
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix = '_capsule'
robot = Robot ('hrp2')
p = ProblemSolver (robot)
vf = ViewerFactory (p)
wcl = WSClient ()
q0 = robot.getInitialConfig ()
constraintName = "balance"
wcl.problem.addStaticStabilityConstraints (constraintName, q0, robot.leftAnkle,
robot.rightAnkle, "",
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,
#/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.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():
from hpp.corbaserver.hrp2 import Robot
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",
#/usr/bin/env python
from hpp_ros import ScenePublisher
from hpp.tools import PathPlayer
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver.wholebody_step.client import Client as WsClient
import time
import sys
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix= '_capsule'
robot = Robot ('hrp2_14')
robot.setTranslationBounds (-3, 3, -3, 3, 0, 1)
cl = robot.client
r = ScenePublisher (robot.jointNames [4:])
q0 = robot.getInitialConfig ()
r (q0)
# Add constraints
wcl = WsClient ()
var1 = float(sys.argv[2])
var2 = float(sys.argv[3])
var3 = float(sys.argv[4])
print 'vars=', var1, var2, var3
point1=[0,0,0]
#/usr/bin/env python
from hpp_ros import ScenePublisher, 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.setTranslationBounds(-3, 3, -3, 3, 0, 1)
cl = robot.client
r = ScenePublisher(robot)
q0 = robot.getInitialConfig()
r(q0)
# Add constraints
wcl = WsClient()
wcl.problem.addStaticStabilityConstraints("balance", q0, robot.leftAnkle,
robot.rightAnkle)
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
# Script which goes with gravity.launch, to test Potential Method with
# HRP2 alone.
from hpp_ros import ScenePublisher
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)
