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

stairs working

parent ee0805a5
......@@ -97,6 +97,7 @@ install(FILES
data/urdf/siggraph_asia/scale_spidey.urdf
data/urdf/siggraph_asia/grasp.urdf
data/urdf/siggraph_asia/plane_hole.urdf
data/urdf/siggraph_asia/stairs_lower.urdf
data/urdf/siggraph_asia/wall.urdf
data/spiderman/urdf/SpidermanLFootSphere.urdf
data/spiderman/urdf/SpidermanLHandSphere.urdf
......@@ -142,6 +143,7 @@ install(FILES
data/srdf/siggraph_asia/grasp.srdf
data/srdf/siggraph_asia/wall.srdf
data/srdf/siggraph_asia/plane_hole.srdf
data/srdf/siggraph_asia/stairs_lower.srdf
data/spiderman/srdf/SpidermanLFootSphere.srdf
data/spiderman/srdf/SpidermanLHandSphere.srdf
data/spiderman/srdf/SpidermanRFootSphere.srdf
......@@ -192,6 +194,7 @@ install(FILES
data/meshes/siggraph_asia/grasp.stl
data/meshes/siggraph_asia/plane_hole.stl
data/meshes/siggraph_asia/wall.stl
data/meshes/siggraph_asia/stairs_lower.stl
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
)
......
<?xml version="1.0"?>
<robot name="stairs_lower">
</robot>
<robot name="stairs_lower">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/stairs_lower.stl"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/stairs_lower.stl"/>
</geometry>
</collision>
</link>
</robot>
......@@ -123,6 +123,12 @@ module hpp
/// \param com target com
double projectStateToCOM(in unsigned short stateId, in floatSeq com) raises (Error);
/// Project a state into a COM
///
/// \param stateId target state
/// \param com target com
double setConfigAtState(in unsigned short stateId, in floatSeq config) raises (Error);
/// Create a state and push it to the state array
///
/// \param q configuration
......
......@@ -307,7 +307,7 @@ def gen_several_states_partial(start = 0, len_con = 1, num_optim = 0, ine_curve
com_vel = init_vel[:]
com_acc = init_acc[:]
print "going from, to ", com_1, "->", com_2
print "going from, to ", start, "->", start + len_con
#~ print "going from, to ", start, "->", start + len_con
allpoints = [com_1]
all_partitions = []
n_fail = 0;
......@@ -336,7 +336,7 @@ def gen_several_states_partial(start = 0, len_con = 1, num_optim = 0, ine_curve
step = (1./ len_con)
idx = step * (i - start)
all_partitions += [idx +0.2*step,idx+0.8*step,idx+step]
print all_partitions
print "[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[", all_partitions
allpoints+=[com_2]
bezier_0, curve = __Bezier(allpoints, init_acc = com_acc, init_vel = com_vel)
all_partitions = [0.] + all_partitions[:-3]
......@@ -344,12 +344,7 @@ def gen_several_states_partial(start = 0, len_con = 1, num_optim = 0, ine_curve
com_acc = curve.derivate(all_partitions[-1],2)
com_vel = flatten(asarray(com_vel).transpose().tolist())
com_acc = flatten(asarray(com_acc).transpose().tolist())
print "at", all_partitions[-1]
print "com_vel", com_vel
print "com_acc", com_acc
p0 = fullBody.generateCurveTrajParts(bezier_0,all_partitions) + 1
print "n_fail ", n_fail
print "generating super curve"
print all_partitions
#~ ppl.displayPath(p0-1)
ppl.displayPath(p0)
......@@ -357,24 +352,23 @@ def gen_several_states_partial(start = 0, len_con = 1, num_optim = 0, ine_curve
ppl.displayPath(p0+2)
#~ ppl.displayPath(p0)
# now we need to project all states to the new com positions
print "WTF ", len(all_partitions)
for k in range(3, len(all_partitions),3):
print "k ", k
print all_partitions[k]
new_com = flatten(asarray(curve(all_partitions[k]).transpose()).tolist())
print "curve end ", curve(1.)
ok = False
#~ try:
sid = start+k/3
print "for state", sid
print "before project to new com ", new_com
print "before previous com", __get_com(fullBody, fullBody.getConfigAtState(sid))
#~ new_com[0]+=0.02
ok = fullBody.projectStateToCOM(sid, new_com)
print "projection", ok
#~ print "projection", ok
if ok:
q1 = fullBody.getConfigAtState(sid)
ok = fullBody.isConfigValid(q1)[0]
print "is config valud", ok
#~ print "is config valud", ok
#~ except:
#~ print "hpperr"
#~ break
......@@ -382,10 +376,16 @@ def gen_several_states_partial(start = 0, len_con = 1, num_optim = 0, ine_curve
print "faield to project"
return [], com_vel, com_acc
j = 0;
print "WTF2"
#~ print "WTF2"
if path:
for i in range(p0,p0+len_con*3-3,3):
paths_ids = [int(el) for el in fullBody.comRRTFromPos(start+j,i,i+1,i+2,num_optim)]
try:
#~ print "FOR STATE ", start+j
#~ print "USING PATHS", i
paths_ids = [int(el) for el in fullBody.comRRTFromPos(start+j,i,i+1,i+2,num_optim)]
except:
print "COULD NOT SOLVE COMRRT"
return [], com_vel, com_acc
j += 1
global allpaths
allpaths += paths_ids[:-1]
......
......@@ -44,9 +44,9 @@ 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", 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, "manipulability", 0.1, "_6_DOF")
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF")
rarmId = 'hrp2_rarm_rom'
......@@ -57,8 +57,8 @@ 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.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)
......
......@@ -518,3 +518,709 @@ path computation 42
path computation 22
path computation 23
path computation 22
path computation 14
path computation 18
path computation 16
path computation 8
path computation 12
path computation 4
path computation 4
path computation 9
path computation 8
path computation 12
path computation 16
path computation 8
path computation 14
path computation 10
path computation 13
path computation 13
path computation 7
path computation 11
path computation 4
path computation 4
path computation 11
path computation 7
path computation 12
path computation 7
path computation 9
path computation 4
path computation 4
path computation 9
path computation 7
path computation 4
path computation 12
path computation 9
path computation 13
path computation 6
path computation 13
path computation 5
path computation 8
path computation 10
path computation 16
path computation 12
path computation 14
path computation 10
path computation 11
path computation 13
path computation 14
path computation 10
path computation 16
path computation 4
path computation 6
path computation 10
path computation 4
path computation 6
path computation 10
path computation 12
path computation 9
path computation 12
path computation 11
path computation 9
path computation 9
path computation 8
path computation 7
path computation 7
path computation 17
path computation 7
path computation 7
path computation 8
path computation 7
path computation 9
path computation 7
path computation 11
path computation 8
path computation 10
path computation 14
path computation 16
path computation 7
path computation 12
path computation 15
path computation 12
path computation 11
path computation 15
path computation 15
path computation 11
path computation 11
path computation 11
path computation 11
path computation 15
path computation 16
path computation 16
path computation 14
path computation 4
path computation 14
path computation 9
path computation 7
path computation 5
path computation 3
path computation 11
path computation 20
path computation 19
path computation 7
path computation 20
path computation 4
path computation 11
path computation 8
path computation 30
path computation 22
path computation 15
path computation 25
path computation 21
path computation 23
path computation 22
path computation 32
path computation 21
path computation 30
path computation 27
path computation 16
path computation 22
path computation 22
path computation 5
path computation 7
path computation 9
path computation 4
path computation 13
path computation 9
path computation 2
path computation 2
path computation 2
path computation 5
path computation 8
path computation 12
path computation 86
path computation 114
path computation 222
path computation 140
path computation 261
path computation 98
path computation 194
path computation 80
path computation 285
path computation 138
path computation 122
path computation 124
path computation 266
path computation 113
path computation 48
path computation 131
path computation 195
path computation 151
path computation 109
path computation 10
path computation 17
path computation 7
path computation 8
path computation 14
path computation 23
path computation 10
path computation 10
path computation 14
path computation 11
path computation 14
path computation 12
path computation 15
path computation 21
path computation 10
path computation 9
path computation 16
path computation 10
path computation 15
path computation 38
path computation 37
path computation 24
path computation 27
path computation 33
path computation 3
path computation 6
path computation 7
path computation 10
path computation 9
path computation 6
path computation 7
path computation 4
path computation 3
path computation 5
path computation 3
path computation 13
path computation 14
path computation 33
path computation 16
path computation 23
path computation 18
path computation 22
path computation 22
path computation 18
path computation 16
path computation 24
path computation 25
path computation 32
path computation 402
path computation 383
path computation 299
path computation 63
path computation 156
path computation 25
path computation 14
path computation 17
path computation 21
path computation 14
path computation 20
path computation 24
path computation 27
path computation 21
path computation 23
path computation 21
path computation 22
path computation 13
path computation 6
path computation 8
path computation 6
path computation 11
path computation 6
path computation 7
path computation 11
path computation 10
path computation 9
path computation 8
path computation 7
path computation 7
path computation 3
path computation 5
path computation 5
path computation 5
path computation 5
path computation 10
path computation 15
path computation 10
path computation 5
path computation 6
path computation 12
path computation 13
path computation 11
path computation 6
path computation 9
path computation 5
path computation 8
path computation 15
path computation 5
path computation 8
path computation 11
path computation 9
path computation 11
path computation 13
path computation 12
path computation 12
path computation 5
path computation 3
path computation 12
path computation 7
path computation 7
path computation 5
path computation 8
path computation 9
path computation 8
path computation 5
path computation 7
path computation 4
path computation 10
path computation 8
path computation 4
path computation 12
path computation 12
path computation 6
path computation 8
path computation 8
path computation 5
path computation 6
path computation 9
path computation 3
path computation 12
path computation 6
path computation 6
path computation 3
path computation 5
path computation 7
path computation 7
path computation 7
path computation 7
path computation 11
path computation 9
path computation 9
path computation 3
path computation 3
path computation 12
path computation 4
path computation 6
path computation 5
path computation 10
path computation 6
path computation 3
path computation 3
path computation 4
path computation 6
path computation 3
path computation 3
path computation 6
path computation 11
path computation 7
path computation 6
path computation 7
path computation 5
path computation 5
path computation 6
path computation 5
path computation 13
path computation 9
path computation 15
path computation 12
path computation 7
path computation 5
path computation 7
path computation 3
path computation 5
path computation 9
path computation 10
path computation 11
path computation 3
path computation 3
path computation 10
path computation 6
path computation 8
path computation 3
path computation 3
path computation 5
path computation 12
path computation 9
path computation 7
path computation 3
path computation 3
path computation 9
path computation 7
path computation 3
path computation 11
path computation 6
path computation 7
path computation 12
path computation 8
path computation 12
path computation 7
path computation 9
path computation 9
path computation 4
path computation 7
path computation 3
path computation 5
path computation 5
path computation 7
path computation 7
path computation 12
path computation 11
path computation 13
path computation 9
path computation 13
path computation 3
path computation 5
path computation 5
path computation 6
path computation 6
path computation 4
path computation 12
path computation 11
path computation 5
path computation 8
path computation 10
path computation 13
path computation 8
path computation 5
path computation 5
path computation 12
path computation 8
path computation 3
path computation 11
path computation 8
path computation 13
path computation 8
path computation 9
path computation 9
path computation 5
path computation 5
path computation 7
path computation 5
path computation 8
path computation 10
path computation 5
path computation 6
path computation 9
path computation 9
path computation 5
path computation 3
path computation 10
path computation 8
path computation 3
path computation 3
path computation 6
path computation 6
path computation 9
path computation 9
path computation 5
path computation 3
path computation 13
path computation 8
path computation 3
path computation 8
path computation 3
path computation 11
path computation 7
path computation 5
path computation 6
path computation 8
path computation 10
path computation 7
path computation 7
path computation 12
path computation 8
path computation 6
path computation 5
path computation 4
path computation 8
path computation 9
path computation 6
path computation 6
path computation 8
path computation 24
path computation 3
path computation 3
path computation 4
path computation 5
path computation 13
path computation 9
path computation 7
path computation 3
path computation 9
path computation 3
path computation 6
path computation 13
path computation 4
path computation 7
path computation 9
path computation 12
path computation 3
path computation 11
path computation 11
path computation 5
path computation 9
path computation 7
path computation 6
path computation 3