Commit 992b7430 authored by t steve's avatar t steve
Browse files

state method to create contact + retrieve centroids of affodance

parent a4ef5775
......@@ -565,6 +565,16 @@ module hpp
/// \return (success,NState) whether the creation was successful, as well as the new state
short addNewContact(in unsigned short stateId, in string limbName, in floatSeq position, in floatSeq normal) raises (Error);
/// Computes the closest projection matrix that will bring a limb's effector
/// from its current configuration to a specified location
/// \param limbname name of the considered limb
/// \param configuration considered configuration of the robot
/// \param p target position
/// \param n target normal
/// \return 7D Transform of effector in contact (position + quaternion)
floatSeq computeTargetTransform(in string limbname, in floatSeq configuration, in floatSeq p, in floatSeq n) raises (Error);
}; // interface Robot
}; // module rbprm
}; // module corbaserver
......
This diff is collapsed.
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import time
packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "hrp2_14"
urdfSuffix = "_reduced"
srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,0,-0.105]
rLegNormal = [0,0,1]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.1)
lLegId = 'hrp2_lleg_rom'
lLeg = 'LLEG_JOINT0'
lLegx = 0.09; lLegy = 0.05
lLegOffset = [0,0,-0.105]
lLegNormal = [0,0,1]
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.1)
#~ AFTER loading obstacles
larmId = 'hrp2_larm_rom'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [0,0,-0.1075]
lArmNormal = [0,0,1]
lArmx = 0.024; lArmy = 0.024
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF", False,grasp = True)
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF", False)
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF")
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF")
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [0,0,-0.1075]
rArmNormal = [0,0,1]
rArmx = 0.024; rArmy = 0.024
#disabling collision for hook
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF", False,grasp = True)
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF", False)
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF")
fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},
lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'},
rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : rHand},
larmId : {'file': "hrp2/LA_com.ineq", 'effector' : lHand} }
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import time
from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
from hpp.gepetto import PathPlayer
def act(i, numOptim = 0, use_window = 0, friction = 0.5, optim_effectors = True, verbose = False, draw = False):
return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.4, optim_effectors = optim_effectors, time_scale = 20., useCOMConstraints = True, use_window = use_window,
verbose = verbose, draw = draw)
def play(frame_rate = 1./24.):
play_traj(fullBody,pp,frame_rate)
def saveAll(name):
saveAllData(fullBody, r, name)
def initConfig():
r.client.gui.setVisibility("hrp2_14", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
r(q_init)
def endConfig():
r.client.gui.setVisibility("hrp2_14", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
r(q_goal)
def rootPath():
tp.cl.problem.selectProblem("rbprm_path")
r.client.gui.setVisibility("hrp2_14", "OFF")
tp.r.client.gui.setVisibility("toto", "OFF")
r.client.gui.setVisibility("hyq", "OFF")
r.client.gui.setVisibility("hrp2_trunk_flexible", "ON")
tp.pp(0)
r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
r.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
def genPlan(stepsize=0.1, rob = 2, filt = True):
r.client.gui.setVisibility("hrp2_14", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
global configs
start = time.clock()
configs = fullBody.interpolate(stepsize, 1, rob, filt)
end = time.clock()
print "Contact plan generated in " + str(end-start) + "seconds"
def contactPlan(step = 0.5, rob = 2):
r.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
for i in range(0,len(configs)):
r(configs[i]);
time.sleep(step)
def a():
print "initial configuration"
initConfig()
def b():
print "end configuration"
endConfig()
def c():
print "displaying root path"
rootPath()
def d(step=0.1, filt = True):
print "computing contact plan"
genPlan(step, filt = filt)
return configs
def e(step = 0.5, rob = 2, qs=None):
if(qs != None):
global configs
configs = qs[:]
print "displaying contact plan"
contactPlan(step, rob)
r = None
tp = None
pp = None
fullBody = None
def init_plan_execute(robot, viewer, pathplanner, pplayer):
global r
global tp
global pp
global fullBody
r = viewer
tp = pathplanner
pp = pplayer
fullBody = robot
#!/bin/bash
gepetto-viewer-server &
hpp-rbprm-server &
ipython -i --no-confirm-exit ./$1
pkill -f 'gepetto-viewer-server'
pkill -f 'hpp-rbprm-server'
#!/bin/bash
gepetto-viewer-server &
ipython -i --no-confirm-exit ./$1
pkill -f 'gepetto-viewer-server'
from twister_geom import *
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
from hpp.gepetto import PathPlayer
import twister_path as path_planner
import hrp2_model as model
#~ import hrp2_model_grasp as model
from hrp2_model import *
import time
ps = path_planner.ProblemSolver( model.fullBody )
r = path_planner.Viewer (ps, viewerClient=path_planner.r.client)
pp = PathPlayer (fullBody.client.basic, r)
fullBody = model.fullBody
fullBody.setJointBounds ("base_joint_xyz", [-2,2.5, -2, 2, 0, 2.2])
from plan_execute import a, b, c, d, e, init_plan_execute
init_plan_execute(model.fullBody, r, path_planner, pp)
q_0 = fullBody.getCurrentConfig();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
q_init = fullBody.getCurrentConfig(); q_init[0:7] = path_planner.q_init[0:7]
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = path_planner.q_goal[0:7]
fullBody.setCurrentConfig (q_init)
q_init = [
0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6
0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10
0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM 11-17
0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # RARM 18-24
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36
]; r (q_init)
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
fullBody.setStartState(q_init,[rLegId,lLegId])#,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId])
i = 0;
configs = []
#~ d(0.1); e(0.01)
print "confgs ", configs
qs = configs
fb = fullBody
ttp = path_planner
from bezier_traj import *
init_bezier_traj(fb, r, pp, qs, limbsCOMConstraints)
#~ AFTER loading obstacles
configs = qs
fullBody = fb
tp = ttp
from hpp.corbaserver.rbprm.rbprmstate import State
from hpp.corbaserver.rbprm.state_alg import addNewContact, isContactReachable, closestTransform
s1 = State(fullBody,q=q_init, limbsIncontact = [rLegId, lLegId])
q0 = s1.q()[:]
def test(p, n):
global s1
t0 = time.clock()
a, success = addNewContact(s1,rLegId,p, n)
print "projecttion successfull ? ", success
t1 = time.clock()
val, com = isContactReachable(s1, rLegId,p, n, limbsCOMConstraints)
t2 = time.clock()
print 'is reachable ? ', val, com
if(val):
com[2]+=0.1
if(success > 0):
print 'a > 0'
t3 = time.clock()
q = fullBody.projectStateToCOM(a.sId, com.tolist())
t4= time.clock()
r(a.q())
else:
print "using s1 ", s1.sId
t3 = time.clock()
q = fullBody.projectStateToCOM(s1.sId, com.tolist())
t4= time.clock()
a, succ = addNewContact(s1,rLegId,p, n)
print "success projection ??", succ
if (succ):
r(a.q())
print "time to addnc", t2 - t1
print "projectcom succesfull ?", q
print "time to check reachable", t3- t2
print "time to project", t4- t3
p = [-0.1,-0.91499999,0.00]
n = [1,0.,0.]
test(p,n)
p = [-0.1,-0.91499999,0.00]
n = [0,0.,1.]
test(p,n)
p = [-0.1,-0.91499999,0.10]
n = [0,0.,1.]
test(p,n)
p = [-0.1,-0.91499999,0.50]
n = [0,0.,1.]
test(p,n)
p = [-0.1,-1.1, 0.30]
n = [0,0.,1.]
test(p,n)
from numpy import array, cross
from numpy.linalg import norm
def flat(pts):
return [item for sublist in pts for item in sublist]
__EPS = 1e-5
def __filter_points(points):
res = []
for el in points:
el_arr = array(el)
add = True
for al in res:
if(norm(al - el_arr) < __EPS):
add = False
break
if add:
res += [array(el)]
return res
def __normal(points):
p1 = array(points[0])
p2 = array(points[1])
p3 = array(points[2])
normal = cross((p2 - p1),(p3 - p1))
normal /= norm(normal)
return normal
def __centroid(points):
print "points", type(points[0])
return sum(points) / len(points)
def __centroid_list(list_points):
return [[__centroid(__filter_points(flat(pts))).tolist(), __normal(pts[0]) ] for pts in list_points]
def computeAffordanceCentroids(afftool, affordances=["Support","Lean"]):
all_points = []
for _, el in enumerate(affordances):
all_points += afftool.getAffordancePoints(el)
return __centroid_list(all_points)
b_id = 0
def draw_centroid(gui, winId, pt, scene="n_name", color = [1,1,1,0.3]):
p = pt[0]
n = pt[0] + 0.03 * pt[1]
resolution = 0.01
global b_id
boxname = scene+"/"+str(b_id)
boxnameN = scene+"/"+str(b_id)+"n"
b_id += 1
gui.addBox(boxname,resolution,resolution,resolution, color)
gui.addBox(boxnameN,resolution,resolution,resolution, color)
gui.applyConfiguration(boxname,[p[0],p[1],p[2],1,0,0,0])
gui.applyConfiguration(boxnameN,[n[0],n[1],n[2],1,0,0,0])
gui.addSceneToWindow(scene,winId)
gui.refresh()
def draw_centroids(gui, winId, pts_lists, scene="n_name", color = [1,0,0,1]):
gui.createScene(scene)
for _, pt in enumerate(pts_lists):
draw_centroid(gui, winId, pt, scene=scene, color = color)
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.gepetto import Viewer
from hpp.corbaserver import Client
from hpp.corbaserver.robot import Robot as Parent
class Robot (Parent):
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
# URDF file describing the trunk of the robot HyQ
urdfName = 'hrp2_trunk_flexible'
urdfSuffix = ""
srdfSuffix = ""
def __init__ (self, robotName, load = True):
Parent.__init__ (self, robotName, self.rootJointType, load)
self.tf_root = "base_footprint"
self.client.basic = Client ()
self.load = load
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hrp2_trunk_flexible'
urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2.5, -1, 1, 0, 2.2])
rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Support','Lean'])
rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support','Lean'])
#~ rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support','Lean'])
rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support'])
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.5)
#~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
rbprmBuilder.boundSO3([-0.,0,-1,1,-1,1])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [0, -0.82, 0.648702]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_init [0:3] = [0.1, -0.82, 0.648702]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
q_goal = q_init [::]
q_goal [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
#~ q_goal [0:3] = [1.49, -0.65, 1.25]; r (q_goal)
q_goal [0:3] = [2.1, -0.82, 1.0]; r (q_goal)
#~ ps.addPathOptimizer("GradientBased")
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel (packageName, "stairs_lower", "planning", r)
#~ afftool.analyseAll()
#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
#~ afftool.visualiseAffordances('Lean', r, [0, 0, 0.9])
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
#~ ps.solve ()
t = ps.solve ()
print t;
if isinstance(t, list):
t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
f = open('log.txt', 'a')
f.write("path computation " + str(t) + "\n")
f.close()
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~
#~ pp (2)
#~ pp (0)
#~ pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.01, "stair_bauzil_hrp2_path.txt")
cl = Client()
cl.problem.selectProblem("rbprm_path")
rbprmBuilder2 = Robot ("toto")
ps2 = ProblemSolver( rbprmBuilder2 )
cl.problem.selectProblem("default")
cl.problem.movePathToProblem(1,"rbprm_path",rbprmBuilder.getAllJointNames())
r2 = Viewer (ps2, viewerClient=r.client)
r.client.gui.setVisibility("toto", "OFF")
#~ r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
#~ r2(q_far)
This diff is collapsed.
......@@ -18,6 +18,7 @@
from hpp.corbaserver.rbprm import Client as RbprmClient
from hpp.corbaserver import Client as BasicClient
from hpp.corbaserver.rbprm.tools.com_constraints import *
from numpy import array
......@@ -27,12 +28,20 @@ from numpy import array
# trunk of the robot, one for the range of motion
class State (object):
## Constructor
def __init__ (self, fullBody, sId, isIntermediate = False):
def __init__ (self, fullBody, sId=-1, isIntermediate = False, q = None, limbsIncontact = []):
assert ((sId > -1 and len(limbsIncontact) == 0) or sId == -1), "state created from existing id can't specify limbs in contact"
self.cl = fullBody.client.rbprm.rbprm
self.sId = sId
self.isIntermediate = isIntermediate
if(sId == -1):
print "limbsIncontact ", limbsIncontact
self.sId = self.cl.createState(q, limbsIncontact)
self.isIntermediate = False
else:
self.sId = sId
self.isIntermediate = isIntermediate
self.fullBody = fullBody
self.H_h = None
self.__last_used_friction = None
## assert for case where functions can't be used with intermediate state
def _cni(self):
assert not self.isIntermediate, "method can't be called with intermediate state"
......@@ -102,10 +111,10 @@ class State (object):
## Get position of center of mass
def getCenterOfMass (self):
q0 = fullBody.client.basic.robot.getCurrentConfig()
fullBody.client.basic.robot.setCurrentConfig(self.q())
c = fullBody.client.basic.robot.getComPosition()
fullBody.client.basic.robot.setCurrentConfig(q0)
q0 = self.fullBody.client.basic.robot.getCurrentConfig()
self.fullBody.client.basic.robot.setCurrentConfig(self.q())
c = self.fullBody.client.basic.robot.getComPosition()
self.fullBody.client.basic.robot.setCurrentConfig(q0)
return c
......@@ -115,9 +124,14 @@ class State (object):
# \return world position of the limb end effector given the current robot configuration.
# array of size 4, where each entry is the position of a corner of the contact surface
def getContactCone(self, friction):
if(self.isIntermediate):
rawdata = self.cl.getContactIntermediateCone(self.sId,friction)
else: