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
class IrreducibleConfiguration ():
def __init__ (self, pv):
print "load robot model..."
self.robot_interface = Robot ()
self.robot_interface.setTranslationBounds (-3, 3, -3, 3, 0, 1)
print "load client model..."
self.cl = self.robot_interface.client
self.robot = self.cl.robot
self.scene_publisher = ScenePublisher (self.robot_interface.jointNames [4:])
self.volume = pv[0]
self.hull = pv[1]
self.q = pv[2]
self.robot.setCurrentConfig(self.q)
def display(self):
r = rospy.Rate(1)
r.sleep()
self.scene_publisher.oid = 0
self.scene_publisher.addPolygon(self.hull, 0.02)
self.scene_publisher.addWallAroundHole(self.hull)
self.scene_publisher.publishObjects()
self.scene_publisher(self.q)
r.sleep()
r.sleep()
def __init__ (self, pv):
print "load robot model..."
self.robot_interface = Robot ()
self.robot_interface.setTranslationBounds (-3, 3, -3, 3, 0, 1)
print "load client model..."
self.cl = self.robot_interface.client
self.robot = self.cl.robot
self.scene_publisher = ScenePublisher (self.robot_interface.jointNames [4:])
self.volume = pv[0]
self.hull = pv[1]
self.q = pv[2]
self.robot.setCurrentConfig(self.q)
#/usr/bin/env python
from hpp_corbaserver.hpp import Configuration
from hpp_ros import ScenePublisher
from hpp.tools import PathPlayer
from hrp2 import Robot
from hpp.corbaserver import Client
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix = '_capsule'
robot = Robot()
robot.setTranslationBounds(-10, 10, -10, 10, -10, 10)
cl = robot.client
r = ScenePublisher(robot.jointNames[4:])
q_obs = [0, 0, 0, 0, 0, 0,
1] # for the obstacle situation (here, the spaceStation)
q1 = [
0.05, 0.01, 0.705, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, 0.8, -1.0,
-1.0, 0.0, 0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532,
-0.174532, 1.0, -0.8, 0.1, -0.523599, 0.1, -0.3, 0.174532, -0.174532,
0.174532, -0.174532, 0.174532, -0.174532, 0.6, 0.1, -0.453786, 0.872665,
-0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.8, 0.0
]
r(q1, q_obs)
q2 = [
5.05, 4.01, 1.705, 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,
#/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
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
import time
from hpp.corbaserver import Client
from hpp_corbaserver.hpp import Configuration
from hpp_ros import ScenePublisher
from hpp.tools import PathPlayer
from math import pi
cl = Client()
cl.robot.loadRobotModel('hrp2_14', '', '', '')
jn = cl.robot.getJointNames()
r = ScenePublisher(jn[6:])
q = cl.robot.getCurrentConfig()
r(q)
q1 = q[::]
q2 = q[::]
q1[3] = pi / 3
q1[4] = 3 * pi / 2
q2[5] = pi - 1e-3
cl.problem.directPath(q1, q2)
pathId = cl.problem.countPaths() - 1
length = cl.problem.pathLength(pathId)
p = PathPlayer(cl, r, pathId)
#/usr/bin/env python
import time
import sys
from hpp.corbaserver.motion_prior.client import Client as MPClient
from hpp.corbaserver.wholebody_step.client import Client as WsClient
import rospy
from hrp2 import Robot
from hpp_ros import ScenePublisher, PathPlayer
robot = Robot ()
robot.setTranslationBounds (-0.5, 0.5, -3, 3, 0, 1)
client = robot.client
publisher = ScenePublisher(robot)
pathplayer = PathPlayer (client, publisher)
if len(sys.argv) < 2:
print " 2014 LAAS CNRS "
print "---------------------------------------------"
print "usage: traj-replay.py <TRAJECTORY-FILE-NAME> <FILE2> ..."
sys.exit()
import re
for i in range(1,len(sys.argv)):
m = re.search('([0-9]+)-([0-9]+)\.', sys.argv[i])
minutes = str(int(m.group(2))*60/100)
print minutes
posText = [2,1,0]
publisher.addText(">>Replaying Trajectory\nPlanning Time: "+m.group(1)+"h"+minutes+"m", posText)
publisher.publishObjects()
time.sleep(1)
publisher.publishObjects()
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')
#robot.setJointBounds('base_joint_xyz', [-5, 10, -10, 10, -5, 5])
robot.setJointBounds('base_joint_xyz', [-3, 10, -4, 4, -3, 5])
cl = robot.client
ps = ProblemSolver (robot)
r = ScenePublisher (robot)
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]
# q3 -> q2 ok
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]
q2to4 = [7.60, -2.41, 0.545, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.7, 0.17453, 0.0, -0.523599, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, -2.8, 0.0, 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]
# Difficult end config, q24passage-> q4 ok, q2 -> q4 pb
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
from hrp2 import Robot
from math import pi
#################################################################
robot_interface = Robot ()
robot_interface.setTranslationBounds (-3, 3, -3, 3, 0, 1)
cl = robot_interface.client
robot = robot_interface.client.robot
#################################################################
# publish half sitting position in rviz
#################################################################
r = ScenePublisher (robot_interface.jointNames [4:])
q0 = robot_interface.getInitialConfig ()
q1 = [0.0, 0.0, 0.705, 1.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]
robot.setCurrentConfig(q1)
r(q0)
time.sleep(0.2)
print robot.getNumberDof()
print robot.getNumberDof()
capsulePos = robot.computeVolume()
#access 3 elements at a time (x,y,z)
scale = 0.5
for i in range(0,len(capsulePos),3):
r.addSphere(scale*capsulePos[i], scale*capsulePos[i+1], scale*capsulePos[i+2])
#/usr/bin/env python
from hpp_ros import ScenePublisher
from hpp.tools import PathPlayer
from hpp.corbaserver.hrp2 import Robot
import time
from hpp.corbaserver.wholebody_step.client import Client as WsClient
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix = '_capsule'
robot = Robot ('hrp2_14')
robot.setTranslationBounds (-4, 4, -4, 4, 0, 1)
cl = robot.client
r = ScenePublisher (robot.jointNames [4:])
q0 = robot.getInitialConfig ()
r(q0)
# Display furnitures in RViz
k=2.3
kk=k-1 # 1.3
r.addObject('lamp','lamp_base')
lamp_pos= (0-0.3,-k,0,1,0,0,0) # -0.3 2.3 0
r.moveObject('lamp',lamp_pos)
r.addObject('armchair','armchair_base')
armchair_pos= (kk,kk,0,0.707,0,0,-0.707) # 1.3 1.3 0
r.moveObject('armchair',armchair_pos)
r.addObject('pc','pc_base')
pc_pos= (k,-k+0.4,1.04,0.707,0,0,-0.707) # 2.3 -0.9 1.04
r.moveObject('pc',pc_pos)
r.addObject('desk','desk_base')
desk_pos= (k,-k,0,0.707,0,0,-0.707) # 2.3 -2.3 0