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()
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)
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()
pathplayer.fromFile(sys.argv[i])
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])
r.publishObjects()
print "finished"
#################################################################