From df97d14a77067d236522ee51faf9dcb2cff1cd62 Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@axle.laas.fr>
Date: Mon, 15 May 2017 23:29:28 +0200
Subject: [PATCH] finally fixed disable effector collision, new script to
generate com positions for hrp2
---
.../demos/siggraph_asia/plane_hrp2_interp.py | 78 ++++++++-----------
.../demos/siggraph_asia/plane_hrp2_path.py | 2 +
.../admissibleComPositionsFromEffector.py | 20 ++---
src/rbprmbuilder.impl.cc | 1 -
4 files changed, 46 insertions(+), 55 deletions(-)
diff --git a/script/scenarios/demos/siggraph_asia/plane_hrp2_interp.py b/script/scenarios/demos/siggraph_asia/plane_hrp2_interp.py
index f17ac85..b0b4cf9 100644
--- a/script/scenarios/demos/siggraph_asia/plane_hrp2_interp.py
+++ b/script/scenarios/demos/siggraph_asia/plane_hrp2_interp.py
@@ -41,43 +41,29 @@ lLegOffset = [0,0,-0.105]
lLegNormal = [0,0,1]
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)
-
-
#~ AFTER loading obstacles
-larmId = '4Larm'
+larmId = 'hrp2_larm_rom'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
-lArmOffset = [-0.05,-0.050,-0.050]
-lArmNormal = [1,0,0]
-lArmOffset = [0,0,-0.1]
+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", True)
+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, 0.05)
-rKneeId = '0RKnee'
-rLeg = 'RLEG_JOINT0'
-rKnee = 'RLEG_JOINT3'
-rLegOffset = [0.105,0.055,0.017]
-rLegNormal = [-1,0,0]
-rLegx = 0.05; rLegy = 0.05
-#~ fullBody.addLimb(rKneeId, rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 10000, 0.01)
-#~
-lKneeId = '1LKnee'
-lLeg = 'LLEG_JOINT0'
-lKnee = 'LLEG_JOINT3'
-lLegOffset = [0.105,0.055,0.017]
-lLegNormal = [-1,0,0]
-lLegx = 0.05; lLegy = 0.05
-#~ fullBody.addLimb(lKneeId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 10000, 0.01)
- #~
+
+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", True)
+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)
@@ -288,14 +274,14 @@ def play_all_paths_qs():
if i % 2 == 0 :
pp(pid)
-def test(stateid = 1, path = False, use_rand = False, just_one_curve = False, num_optim = 0) :
+def test(stateid = 1, path = False, use_rand = False, just_one_curve = False, num_optim = 0, effector = False) :
com_1 = __get_com(fullBody, configs[stateid])
com_2 = __get_com(fullBody, configs[stateid+1])
- data = gen_sequence_data_from_state(fullBody,stateid,configs, mu = 0.8)
+ data = gen_sequence_data_from_state(fullBody,stateid,configs, mu = 0.3)
c_bounds_1 = get_com_constraint(fullBody, stateid, configs[stateid], limbsCOMConstraints, interm = False)
c_bounds_mid = get_com_constraint(fullBody, stateid, configs[stateid], limbsCOMConstraints, interm = True)
c_bounds_2 = get_com_constraint(fullBody, stateid, configs[stateid+1], limbsCOMConstraints, interm = False)
- success, c_mid_1, c_mid_2 = solve_quasi_static(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand, mu = 0.8)
+ success, c_mid_1, c_mid_2 = solve_quasi_static(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand, mu = 0.3)
#~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand)
#~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2])
@@ -317,13 +303,13 @@ def test(stateid = 1, path = False, use_rand = False, just_one_curve = False, nu
success_proj1 = project_com_colfree(fullBody, stateid , asarray((com_interm1).transpose()).tolist()[0])
success_proj2 = project_com_colfree(fullBody, stateid+1, asarray((com_interm2).transpose()).tolist()[0])
- if not success_proj1:
- print "proj 1 failed"
- return False, c_mid_1, c_mid_2, paths_ids
-
- if not success_proj2:
- print "proj 2 failed"
- return False, c_mid_1, c_mid_2, paths_ids
+ #~ if not success_proj1:
+ #~ print "proj 1 failed"
+ #~ return False, c_mid_1, c_mid_2, paths_ids
+ #~
+ #~ if not success_proj2:
+ #~ print "proj 2 failed"
+ #~ return False, c_mid_1, c_mid_2, paths_ids
print "p0", p0
#~ pp.displayPath(p0+1)
@@ -332,7 +318,11 @@ def test(stateid = 1, path = False, use_rand = False, just_one_curve = False, nu
pp.displayPath(p0+1)
#~ pp.displayPath(p0+2)
pp.displayPath(p0+3)
- paths_ids = [int(el) for el in fullBody.comRRTFromPos(stateid,p0+1,p0+2,p0+3,num_optim)]
+ if(effector):
+ paths_ids = [int(el) for el in fullBody.effectorRRT(stateid,p0+1,p0+2,p0+3,num_optim)]
+ else:
+ paths_ids = [int(el) for el in fullBody.comRRTFromPos(stateid,p0+1,p0+2,p0+3,num_optim)]
+
else:
print "just all curve"
bezier_0, curve = __Bezier([com_1,c_mid_1[0].tolist()] , end_acc = c_mid_1[1].tolist() , end_vel = [0.,0.,0.])
@@ -360,13 +350,13 @@ def test(stateid = 1, path = False, use_rand = False, just_one_curve = False, nu
#~ pp(29),pp(9),pp(17)
from hpp.corbaserver.rbprm.tools.path_to_trajectory import *
-def gen(start = 0, len_con = 10, num_optim = 0, ine_curve =True, s = 1.):
+def gen(start = 0, len_con = 1, num_optim = 0, ine_curve =True, s = 1., effector = False):
n_fail = 0;
for i in range (start, start+len_con):
- if not test(i, True, False, ine_curve,num_optim):
+ if not test(i, True, False, ine_curve,num_optim)[0]:
found = False
for j in range(10):
- found = test(i, True, True, ine_curve, num_optim)
+ found = test(i, True, True, ine_curve, num_optim)[0]
if found:
break
if not found:
diff --git a/script/scenarios/demos/siggraph_asia/plane_hrp2_path.py b/script/scenarios/demos/siggraph_asia/plane_hrp2_path.py
index b1a84ca..1a96d0e 100644
--- a/script/scenarios/demos/siggraph_asia/plane_hrp2_path.py
+++ b/script/scenarios/demos/siggraph_asia/plane_hrp2_path.py
@@ -33,6 +33,8 @@ rbprmBuilder.setJointBounds ("base_joint_xyz", [0,3, -1, 1, 0, 2.2])
#~ rbprmBuilder.setFilter(['hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
+rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hrp2_larm_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)
diff --git a/script/tools/admissibleComPositionsFromEffector.py b/script/tools/admissibleComPositionsFromEffector.py
index 750a0df..368b310 100644
--- a/script/tools/admissibleComPositionsFromEffector.py
+++ b/script/tools/admissibleComPositionsFromEffector.py
@@ -31,32 +31,32 @@ rootName = 'base_joint_xyz'
rLegId = 'RL'
rLeg = 'RLEG_JOINT0'
rfoot = 'RLEG_JOINT5'
-rLegOffset = [0,-0.105,0,]
-rLegNormal = [0,1,0]
+rLegOffset = [0,0,-0.105]
+rLegNormal = [0,0,1]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "manipulability", 0.1)
lLegId = 'LL'
lLeg = 'LLEG_JOINT0'
-lfoot = 'LLEG_JOINT5'
-lLegOffset = [0,-0.105,0]
-lLegNormal = [0,1,0]
-lLegx = 0.09; lLegy = 0.05
+lfoot = 'LLEG_JOINT5'
+lLegx = 0.09; lLegy = 0.05
+lLegOffset = [0,0,-0.105]
+lLegNormal = [0,0,1]
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "manipulability", 0.1)
rarmId = 'RA'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
-rArmOffset = [-0.05,-0.050,-0.050]
-rArmNormal = [1,0,0]
+rArmOffset = [0,0,-0.1075]
+rArmNormal = [0,0,1]
rArmx = 0.024; rArmy = 0.024
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "manipulability", 0.05)
larmId = 'LA'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
-lArmOffset = [-0.05,-0.050,-0.050]
-lArmNormal = [1,0,0]
+lArmOffset = [0,0,-0.1075]
+lArmNormal = [0,0,1]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "manipulability", 0.05)
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 9fb446f..3038ff1 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -1684,7 +1684,6 @@ assert(s2 == s1 +1);
{
bool success(false);
core::Configuration_t res = rbprm::interpolation::projectOnCom(fulllBody, problem,state,targetCom, success);
- std::cout << "no com " << targetCom << std::endl;
if(!success)
{
throw std::runtime_error("could not project state on COM constraint");
--
GitLab