Commit e229ff30 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

fixed rectangle contact computation

parent d0a4efa5
......@@ -126,6 +126,13 @@ module hpp
/// and the selected sample
floatSeq getSamplePosition(in string sampleName, in unsigned short sampleId) raises (Error);
/// Get the end effector position for a given configuration, assuming z normal
/// \param limbName name of the limb from which to retrieve a sample
/// \param dofArray configuration of the robot
/// \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
floatSeqSeq getEffectorPosition(in string limbName, in floatSeq dofArray) raises (Error);
/// Get the end effector position of a given limb configuration
/// \param limbName name of the limb from which to retrieve a sample
/// \return number of samples generated for the limb
......
......@@ -16,8 +16,11 @@ rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,2, -1.5, 1, 0.5, 0.9])
rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom','hrp2_larm_rom'])
#~ rbprmBuilder.setNormalFilter('hrp2_larm_rom', [0,0,1], 0.4)
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.4)
rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.6)
rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.6)
rbprmBuilder.setAffordanceFilter('3Rarm', ['Support','Lean'])
rbprmBuilder.setAffordanceFilter('4Larm', ['Support','Lean'])
rbprmBuilder.setAffordanceFilter('0rLeg', ['Support'])
rbprmBuilder.setAffordanceFilter('1lLeg', ['Support'])
rbprmBuilder.boundSO3([-1.5,1.5,0,3,-0.0,0.0])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
......@@ -42,9 +45,14 @@ ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
r.loadObstacleModel (packageName, "car", "planning")
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel (packageName, "car", "planning", r)
#~ afftool.analyseAll()
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
afftool.visualiseAffordances('Lean', r, [0, 0, 0.9])
t = ps.solve ()
print (t)
if isinstance(t, list):
......@@ -64,6 +72,9 @@ pp = PathPlayer (rbprmBuilder.client.basic, r)
rob = rbprmBuilder.client.basic.robot
r(q_init)
q_loin = q_init[::]
q_loin[0:3] = [100,100,100]
r (q_loin)
#~ configs = []
......
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
from hpp import Error as hpperr
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)
fullBody.setJointBounds ("base_joint_xyz", [-0.135,2, -1, 1, 0, 2.2])
ps = ProblemSolver( fullBody )
r = Viewer (ps)
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.1)
lLegId = '1lLeg'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.105,0]
lLegNormal = [0,1,0]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.1)
rarmId = '3Rarm'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [0,0,-0.1]
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.05, "_6_DOF", 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} }
#~ AFTER loading obstacles
larmId = '4Larm'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [-0.05,-0.050,-0.050]
lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.05, "_6_DOF", True)
#~
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
def draw_cp(cid, limb, config):
global r
posetc = fullBody.getEffectorPosition(limb, config)
print "pos ", posetc[0]
scene = "qds"+limb+str(cid)
r.client.gui.createScene(scene)
for i in range(4):
pos = posetc[2*i]
r.client.gui.addBox(scene+"/b"+str(i),0.01,0.01,0.01, [1,0,0,1])
r.client.gui.applyConfiguration(scene+"/b"+str(i),pos+[1,0,0,0])
r.client.gui.refresh()
r.client.gui.addSceneToWindow(scene,0)
def fill_contact_points(limbs, config, config_pinocchio):
res = {}
res["q"] = config_pinocchio[:]
res["contact_points"] = {}
res["P"] = []
res["N"] = []
for limb in limbs:
posetc = fullBody.getEffectorPosition(limb, config)
res["contact_points"][limb] = {}
res["contact_points"][limb]["P"] = [p for i, p in enumerate (posetc) if (i%2 == 0)]
res["P"] += [p for i, p in enumerate (posetc) if (i%2 == 0)]
res["contact_points"][limb]["N"] = [n for i, n in enumerate (posetc) if (i%2 == 1)]
res["N"] += [n for i, n in enumerate (posetc) if (i%2 == 1)]
return res
q_0 = fullBody.getCurrentConfig();
#~ fullBody.getSampleConfig()
qs = []; qs_gepetto = []; states = []
limbs = [lLegId,rLegId]
for _ in range(1000):
q = fullBody.generateGroundContact(limbs)
q_gep = q[:]
quat_end = q[4:7]
q[6] = q[3]
q[3:6] = quat_end
qs.append(q)
qs_gepetto.append(q_gep)
states.append(fill_contact_points(limbs,q_gep,q))
from pickle import dump
f1=open("configs_feet_on_ground_static_eq", 'w+')
dump(states, f1)
f1.close()
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
from hpp import Error as hpperr
packageName = "hyq_description"
meshPackageName = "hyq_description"
rootJointType = "freeflyer"
# Information to retrieve urdf and srdf files.
urdfName = "hyq"
urdfSuffix = ""
srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-0.135,2, -1, 1, 0, 2.2])
# This time we load the full body model of HyQ
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
# Setting a number of sample configurations used
nbSamples = 20000
ps = ProblemSolver( fullBody )
r = Viewer (ps)
rootName = 'base_joint_xyz'
# Creating limbs
# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
cType = "_3_DOF"
# string identifying the limb
rLegId = 'rfleg'
# First joint of the limb, as in urdf file
rLeg = 'rf_haa_joint'
# Last joint of the limb, as in urdf file
rfoot = 'rf_foot_joint'
# Specifying the distance between last joint and contact surface
offset = [0.,-0.021,0.]
# Specifying the contact surface direction when the limb is in rest pose
normal = [0,1,0]
# Specifying the rectangular contact surface length
legx = 0.02; legy = 0.02
# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.1, cType)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
lfoot = 'lh_foot_joint'
fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
larmId = 'lfleg'
larm = 'lf_haa_joint'
lHand = 'lf_foot_joint'
fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
q_0 = fullBody.getCurrentConfig();
r(q_0)
def draw_cp(cid, limb, config):
global r
posetc = fullBody.getEffectorPosition(limb, config)
print "pos ", posetc[0]
scene = "qds"+limb+str(cid)
r.client.gui.createScene(scene)
for i in range(4):
pos = posetc[2*i]
r.client.gui.addBox(scene+"/b"+str(i),0.01,0.01,0.01, [1,0,0,1])
r.client.gui.applyConfiguration(scene+"/b"+str(i),pos+[1,0,0,0])
r.client.gui.refresh()
r.client.gui.addSceneToWindow(scene,0)
def fill_contact_points(limbs, config, config_pinocchio):
res = {}
res["q"] = config_pinocchio[:]
res["contact_points"] = {}
res["P"] = []
res["N"] = []
for limb in limbs:
posetc = fullBody.getEffectorPosition(limb, config)
res["contact_points"][limb] = {}
res["contact_points"][limb]["P"] = [p for i, p in enumerate (posetc) if (i%2 == 0)]
res["P"] += [p for i, p in enumerate (posetc) if (i%2 == 0)]
res["contact_points"][limb]["N"] = [n for i, n in enumerate (posetc) if (i%2 == 1)]
res["N"] += [n for i, n in enumerate (posetc) if (i%2 == 1)]
return res
q_0 = fullBody.getCurrentConfig();
#~ fullBody.getSampleConfig()
qs = []; qs_gepetto = []; states = []
limbs = [lLegId,rLegId]
for i in range(2):
q = fullBody.generateGroundContact(limbs)
q_gep = q[:]
quat_end = q[4:7]
q[6] = q[3]
q[3:6] = quat_end
qs.append(q)
qs_gepetto.append(q_gep)
states.append(fill_contact_points(limbs,q_gep,q))
from pickle import dump
f1=open("configs_feet_on_ground_static_eq", 'w+')
dump(qs, f1)
f1.close()
......@@ -17,6 +17,7 @@ srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-6,5, -4, 4, 0.6, 2])
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
......@@ -76,16 +77,100 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init)
configs = fullBody.interpolate(0.1)
configs = fullBody.interpolate(0.1,1,5)
r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
#~ fullBody.exportAll(r, configs, 'obstacle_hyq_robust_10');
i = 0;
r (configs[i]); i=i+1; i-1
#~ q0 = configs[2]
#~ q0 = fullBody.generateContacts(q0, [0,0,1])
#~ r(q0)
c = fullBody.getContactSamplesIds("rfleg",q_init, [0,0,1])
#~ r(fullBody.getSample("rfleg",int(c[i]))); i = i+1
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
from hpp import Error as hpperr
def displayComPath(pathId,color=[0.,0.75,0.15,0.9]) :
pathPos=[]
length = pp.end*pp.client.problem.pathLength (pathId)
t = pp.start*pp.client.problem.pathLength (pathId)
while t < length :
q = pp.client.problem.configAtParam (pathId, t)
pp.publisher.robot.setCurrentConfig(q)
q = pp.publisher.robot.getCenterOfMass()
pathPos = pathPos + [q[:3]]
t += pp.dt
nameCurve = "path_"+str(pathId)+"_com"
pp.publisher.client.gui.addCurve(nameCurve,pathPos,color)
pp.publisher.client.gui.addToGroup(nameCurve,pp.publisher.sceneName)
pp.publisher.client.gui.refresh()
def displayIn(pp, pathId,length):
t = 0
while t < length :
start = time.time()
q = pp.client.problem.configAtParam (pathId, t)
pp.publisher.robotConfig = q
pp.publisher.publishRobots ()
t += (pp.dt * pp.speed)
elapsed = time.time() - start
if elapsed < pp.dt :
time.sleep(pp.dt-elapsed)
res = []
import sys
numerror = 0
def act(i, optim):
try:
pid = solve_com_RRT(fullBody, configs, i, True, 0.5, 0.2, False, optim, False, True)
displayComPath(pid, [0.9,0.,0.15,0.9])
#~ pp(pid)
global res
res = res + [pid]
global numerror
except hpperr as e:
print "failed at id " + str(i) , e.strerror
numerror+=1
except ValueError as e:
print "failed at id " + str(i) , e
numerror+=1
except IndexError as e:
print "failed at id " + str(i) , e
numerror+=1
except Exception as e:
print e
numerror+=1
except:
print "smthg wrong"
return
def displayInSave(pp, pathId, configs):
length = pp.end*pp.client.problem.pathLength (pathId)
t = pp.start*pp.client.problem.pathLength (pathId)
while t < length :
q = pp.client.problem.configAtParam (pathId, t)
configs.append(q)
t += (pp.dt * pp.speed)
pp.setSpeed(2)
respath = []
for p in res:
print p
displayInSave(pp,p, respath)
#~ for p in res:
#~ displayIn(pp,p,1.5)
import time
#~ fullBody.exportAll(r, respath, 'groun_crouch_hyq_full_bridge_05');
#~ fullBody.exportAll(r, respath, 'groun_crouch_hyq_full_hole_05');
......@@ -14,10 +14,10 @@ rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,5, -4, 4, 0.6, 2])
rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
rbprmBuilder.setNormalFilter('hyq_lhleg_rom', [0,0,1], 0.9)
rbprmBuilder.setNormalFilter('hyq_rfleg_rom', [0,0,1], 0.9)
rbprmBuilder.setNormalFilter('hyq_lfleg_rom', [0,0,1], 0.9)
rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
rbprmBuilder.boundSO3([-0.1,0.1,-1,1,-1,1])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
......@@ -37,9 +37,16 @@ ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.loadObstacleModel (packageName, "groundcrouch", "planning", r)
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
# Choosing RBPRM shooter and path validation methods.
# Note that the standard RRT algorithm is used.
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
r.loadObstacleModel (packageName, "groundcrouch", "planning")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
#~ r.loadObstacleModel (packageName, "groundcrouch", "planning")
#~ ps.solve ()
t = ps.solve ()
......@@ -58,3 +65,7 @@ pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (0)
#~ pp (1)
r (q_init)
q_far = q_init [::]
q_far [0:3] = [-2, -3, 0.63];
r(q_far)
#Importing helper class for RBPRM
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import quaternion as quat
packageName = "hyq_description"
meshPackageName = "hyq_description"
rootJointType = "freeflyer"
# Information to retrieve urdf and srdf files.
urdfName = "hyq"
urdfSuffix = ""
srdfSuffix = ""
# This time we load the full body model of HyQ
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-20,20, -20, 20, -20, 20])
# Setting a number of sample configurations used
nbSamples = 1
rootName = 'base_joint_xyz'
# Creating limbs
# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
cType = "_3_DOF"
# string identifying the limb
rLegId = 'rfleg'
# First joint of the limb, as in urdf file
rLeg = 'rf_haa_joint'
# Last joint of the limb, as in urdf file
rfoot = 'rf_foot_joint'
# Specifying the distance between last joint and contact surface
offset = [0.,-0.021,0.]
# Specifying the contact surface direction when the limb is in rest pose
normal = [0,1,0]
# Specifying the rectangular contact surface length
legx = 0.02; legy = 0.02
# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.1, cType)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
lfoot = 'lh_foot_joint'
fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
larmId = 'lfleg'
larm = 'lf_haa_joint'
lHand = 'lf_foot_joint'
fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
#make sure this is 0
q_0 = fullBody.getCurrentConfig ()
zeroConf = [0,0,0, 1, 0, 0, 0]
q_0[0:7] = zeroConf
fullBody.setCurrentConfig (q_0)
effectors = [rfoot, lfoot, lHand, rHand]
limbIds = [rLegId, lLegId, larmId, rarmId ]
import numpy as np
#~ effectorName = rfoot
#~ limbId = rLegId
#~ q = fullBody.getSample(limbId, 1)
#~ fullBody.setCurrentConfig(q) #setConfiguration matching sample
#~ qEffector = fullBody.getJointPosition(effectorName)
#~ q0 = quat.Quaternion(qEffector[3:7])
#~ rot = q0.toRotationMatrix() #compute rotation matrix world -> local
#~ p = qEffector[0:3] #(0,0,0) coordinate expressed in effector fram
#~ rm=np.zeros((4,4))
#~ for i in range(0,3):
#~ for j in range(0,3):
#~ rm[i,j] = rot[i,j]
#~ for i in range(0,3):
#~ rm[i,3] = qEffector[i]
#~ rm[3,3] = 1
#~ invrm = np.linalg.inv(rm)
#~ p = invrm.dot([0,0,0,1])
points = [[],[],[],[]]
def printComPosition(nbConfigs):
num_invalid = 0
for i in range(0,nbConfigs):
q = fullBody.shootRandomConfig()
q[0:7] = zeroConf
fullBody.setCurrentConfig(q) #setConfiguration matching sample
com = fullBody.getCenterOfMass()
for x in range(0,3):
q[x] = -com[x]
fullBody.setCurrentConfig(q)
#~ print ("final com" + str(com))
#~ print ("final com" + str(fullBody.getCenterOfMass()))
if(fullBody.isConfigValid(q)[0]):
for j in range(0,len(effectors)):
effectorName = effectors[j]
limbId = limbIds[j]
qEffector = fullBody.getJointPosition(effectorName)
q0 = quat.Quaternion(qEffector[3:7])
rot = q0.toRotationMatrix() #compute rotation matrix world -> local
p = qEffector[0:3] #(0,0,0) coordinate expressed in effector fram
rm=np.zeros((4,4))
for k in range(0,3):
for l in range(0,3):
rm[k,l] = rot[k,l]
for m in range(0,3):
rm[m,3] = qEffector[m]
rm[3,3] = 1
invrm = np.linalg.inv(rm)
p = invrm.dot([0,0,0,1])
points[j].append(p)
#~ print (points[j])
else:
num_invalid +=1
for j in range(0,len(limbIds)):
f1=open('./'+str(limbIds[j])+'_com.erom', 'w+')
for p in points[j]:
f1.write(str(p[0]) + "," + str(p[1]) + "," + str(p[2]) + "\n")