def check_kin_feasibility(s0Id, s1Id):
res = fullBody.isReachableFromState(s0Id, s1Id, True)
if not res[0]:
print "Error : isReachable returned False !"
raise Exception('Error : isReachable returned False !')
r(configs[s0Id])
c0 = np.array(fullBody.getCenterOfMass())
c1 = np.array(res[1])
c2 = np.array(res[2])
r(configs[s1Id])
c3 = np.array(fullBody.getCenterOfMass())
s0 = State(fullBody, sId=s0Id)
s1 = State(fullBody, sId=s1Id)
s0_orig = State(s0.fullBody,
q=s0.q(),
limbsIncontact=s0.getLimbsInContact())
s1_orig = State(s1.fullBody,
q=s1.q(),
limbsIncontact=s1.getLimbsInContact())
moving_limb = s0.contactsVariations(s1)
smid, successMid = StateHelper.removeContact(s0_orig, moving_limb[0])
if not successMid:
return False
successFeasible = True
print "check first part"
successFeasible = successFeasible and check_traj(s0_orig, c0, c1)
print "check mid part"
successFeasible = successFeasible and check_traj(smid, c1, c2)
print "check last part"
successFeasible = successFeasible and check_traj(s1_orig, c2, c3, True)
return successFeasible
def projectMidFeet(fullBody, q, limbs):
fullBody.setCurrentConfig(q)
s = State(fullBody, q=q, limbsIncontact=limbs)
com = np.array(fullBody.getJointPosition(rfoot)[0:3])
com += np.array(fullBody.getJointPosition(lfoot)[0:3])
com /= 2.
com[2] = fullBody.getCenterOfMass()[2]
successProj = s.projectToCOM(com.tolist())
if successProj and fullBody.isConfigValid(
s.q())[0] and fullBody.isConfigBalanced(s.q(), limbs, 5):
return s
else:
return None
def setupSpidey():
switch_context(1)
q_0 = fullBody.getCurrentConfig()
r(q_0)
q_init = [
1.8644820749752133, -0.05907835662004786, 0.36244227429714687,
0.796622257271051, -0.01123057286487888, -0.04124810131291858,
0.6029638858104035, -0.03547067445226691, 0.6709217094864277,
0.26125134036900377, 0.018790826902884056, 1.0472,
-0.12716727062306263, -0.4194108294469009, -0.5282737768672232,
0.6483375025026266, -0.36743071075802436, -1.3354344044074755,
0.8731280507393882, -0.6564341232652954, -0.14124478581057368,
0.0349066, 1.0782285414916801, -1.2054219129402555, 0.6375403038065977
]
#~ q_init[0:7] = tp.q_init[0:7]
#~ q_init[1] += 1.05
#~ q_0[2] = 1.1
s1 = State(fullBody, q=q_init, limbsIncontact=['r1', 'r3', 'l3'])
#~ q_goal = fullBody.generateContacts(s1.q(), [0,0,1])
#~ s1.setQ(q_goal)
q0 = s1.q()[:]
r(q0)
return s1
def setupHrp2():
switch_context(0)
q_init = [
0.8728886120451924, -0.8101285717720692, 0.9831109373101212,
0.9907480839443233, 0.04566101822960869, 6.080932806073498e-05,
-0.12780180701818292, 0.0, 0.0, 0.700898687394426, 0.2643416012373336,
1.0472, -0.04797431922382627, -0.013276945338995495,
-0.1474914458541713, -0.002729397895803521, 0.0417439495415805,
-0.0959524864077709, 1.0472, -0.7688894346658527, -0.12266497430814384,
-0.12242722827560858, 0.0031594766930237773, 0.0443584738891719,
0.015217658052039982, 0.6567108816211705, 0.3519940697164601,
-0.3096847583571286, 0.01661099687285411, 0.6843606576274044,
-0.33788734650437907, 0.3649203666768749, -0.3603776588496829,
0.1551833621146152, -0.001045294421723865, 0.23087743645539907,
0.28930745214380776
]
r(q_init)
#~ q_init[1] = -1.05
s1 = State(fullBody, q=q_init, limbsIncontact=[rLegId, lLegId])
#~ s1 = State(fullBody,q=q_init, limbsIncontact = [])
q0 = s1.q()[:]
r(q0)
fullBody.setRefConfig(q0)
return s1
def setupHrp2():
switch_context(0)
q_init = [
1.465951314830228, -0.9176187491454572, 0.556996332024304,
0.8949373222781749, 0.004991393277543945, 0.09328595707567328,
0.43630265344046976, -0.03547067445226664, 0.6709217094864276,
0.2612513403690041, 0.018790826902884042, 1.0472, -0.12716727062306263,
0.014121758070711707, 0.0349066, -0.005336299638361007,
0.09555844824139348, -0.15100592820137693, 1.0472, -0.6564341232652954,
-0.14124478581057368, 0.0349066, 0.006442792051943824,
0.0976416359755596, 0.048557325328327426, 0.025298798438044116,
0.5088603837394758, 0.09277155854142789, 0.06056704285485918,
-0.40397983120647746, -0.3442592958713712, -0.5709648671805531,
-0.5813136986745363, -0.8406045693532259, 0.5508411880503161,
0.06566162355557537, 0.5817029063967701
]
r(q_init)
#~ q_init[1] = -1.05
s1 = State(fullBody, q=q_init, limbsIncontact=[rLegId, lLegId])
#~ s1 = State(fullBody,q=q_init, limbsIncontact = [])
q0 = s1.q()[:]
r(q0)
fullBody.setRefConfig(q0)
return s1
def setupSpidey():
switch_context(1)
q_0 = fullBody.getCurrentConfig()
r(q_0)
q_init = [
1.5118243482119733, 0.1515578829873285, 0.48976931477040697,
0.9915264175846067, -0.018363722321990766, -0.068500987098437,
-0.10883819045195667, 0.5302771431871597, 0.5441203380575321,
0.2782134865083353, -0.06010765345021368, -0.8900451738361533,
0.276589550853067, -0.34163470046205424, -0.2710174858398316,
0.06526434276718414, -0.43672572993852576, -0.9562437680837305,
0.9084315210259988, -0.6088478466994447, -0.13563212555983598,
0.0349066, 0.9658270566778959, -0.6362233126122913, 1.5
]
#~ q_init[0:7] = tp.q_init[0:7]
#~ q_init[1] += 1.05
#~ q_0[2] = 1.1
s1 = State(fullBody, q=q_init, limbsIncontact=['r1', 'r3', 'l3', 'l2'])
#~ q_goal = fullBody.generateContacts(s1.q(), [0,0,1])
#~ s1.setQ(q_goal)
q0 = s1.q()[:]
r(q0)
return s1
def generateConfigFromPhase(fb, phase, projectCOM=False):
fb.usePosturalTaskContactCreation(False)
effectorsInContact = phase.effectorsInContact()
contacts = [
] # contacts should contains the limb names, not the effector names
list_effector = list(fb.dict_limb_joint.values())
for eeName in effectorsInContact:
contacts += [
list(fb.dict_limb_joint.keys())[list_effector.index(eeName)]
]
#q = phase.q_init.tolist() # should be the correct config for the previous phase, if used only from high level helper methods
q = fb.referenceConfig[::] + [0] * 6 # FIXME : more generic !
root = computeCenterOfSupportPolygonFromPhase(
phase, fb.DEFAULT_COM_HEIGHT).tolist()
q[0:2] = root[0:2]
q[2] += root[2] - fb.DEFAULT_COM_HEIGHT
quat = Quaternion(phase.root_t.evaluateAsSE3(phase.timeInitial).rotation)
q[3:7] = [quat.x, quat.y, quat.z, quat.w]
# create state in fullBody :
state = State(fb, q=q, limbsIncontact=contacts)
# check if q is consistent with the contact placement in the phase :
fb.setCurrentConfig(q)
for limbId in contacts:
eeName = fb.dict_limb_joint[limbId]
placement_fb = SE3FromConfig(fb.getJointPosition(eeName))
placement_phase = phase.contactPatch(eeName).placement
if placement_fb != placement_phase: # add a threshold instead of 0 ? how ?
# need to project the new contact :
placement = phase.contactPatch(eeName).placement
p = placement.translation.tolist()
n = computeContactNormal(placement).tolist()
state, success = StateHelper.addNewContact(state, limbId, p, n,
1000)
if not success:
print(
"Cannot project the configuration to contact, for effector : ",
eeName)
return state.q()
if projectCOM:
success = projectCoMInSupportPolygon(state)
if not success:
print(
"cannot project com to the middle of the support polygon."
)
phase.q_init = np.array(state.q())
return state.q()
def genFlat(init=False):
q = fullBody.shootRandomConfig()
if init:
q = fullBody.referenceConfig[::]
q[0:7] = zeroConf
fullBody.setCurrentConfig(q)
#~ v(q)
positions = [fullBody.getJointPosition(foot)[:3] for foot in effectors]
s = State(fullBody, q=q, limbsIncontact=limbIds)
succ = True
for effId, pos in zip(limbIds, positions):
s, succ = StateHelper.addNewContact(s,
effId,
pos, [0., 0., 1.],
num_max_sample=0)
if not succ:
break
# ~ posrf = fullBody.getJointPosition(rfoot)[:3]
# ~ poslf = fullBody.getJointPosition(lfoot)[:3]
# ~ print ("limbsIds ", limbIds)
# ~ s = State(fullBody, q = q, limbsIncontact = limbIds)
# ~ s, succ = StateHelper.addNewContact(s, rLegId, posrf, [0.,0.,1.], num_max_sample = 0)
# ~ if succ:
# ~ s, succ = StateHelper.addNewContact(s, lLegId, poslf, [0.,0.,1.], num_max_sample = 0)
if succ:
# ~ succ = fullBody.isConfigValid(q)[0] and norm (array(posrf[:2]) - array(poslf[:2]) ) >= 0.3
succ = fullBody.isConfigValid(q)[0]
#assert that in static equilibrium
if succ:
fullBody.setCurrentConfig(q)
succ = staticEq(positions, fullBody.getCenterOfMass())
if not succ:
v(q)
if succ:
succ = not pointInsideHull(positions)
if not succ:
print("************* contacts crossing", not succ)
v(q)
#~ if succ and norm (array(posrf[:2]) - array(poslf[:2]) ) <= 0.1:
# ~ if succ and norm (array(posrf) - array(poslf) ) <= 0.1:
v(s.q())
return s.q(), succ, s, positions
def sampleRandomTranstionFromState(fullBody, s0, limbsInContact, movingLimb,
z):
it = 0
success = False
n = [0, 0, 1]
vz = np.matrix(n).T
while not success and it < 10000:
it += 1
# sample a random position for movingLimb and try to project s0 to this position
qr = fullBody.shootRandomConfig()
q1 = s0.q()[::]
q1[limb_ids[movingLimb][0]:limb_ids[movingLimb][1]] = qr[
limb_ids[movingLimb][0]:limb_ids[movingLimb][1]]
s1 = State(fullBody, q=q1, limbsIncontact=limbsInContact)
fullBody.setCurrentConfig(s1.q())
p = fullBody.getJointPosition(
fullBody.dict_limb_joint[movingLimb])[0:3]
p[0] = random.uniform(eff_x_range[0], eff_x_range[1])
p[1] = random.uniform(eff_y_range[0], eff_y_range[1])
p[2] = z
s1, success = StateHelper.addNewContact(s1, movingLimb, p, n)
if success:
"""
# force root orientation : (align current z axis with vertical)
quat_1 = Quaternion(s1.q()[6],s1.q()[3],s1.q()[4],s1.q()[5])
v_1 = quat_1.matrix() * vz
align = Quaternion.FromTwoVectors(v_1,vz)
rot = align * quat_1
q_root = s1.q()[0:7]
q_root[3:7] = rot.coeffs().T.tolist()[0]
"""
root = SE3.Identity()
root.translation = np.matrix(s1.q()[0:3]).T
root = sampleRotationAlongZ(root)
success = s1.projectToRoot(se3ToXYZQUATtuple(root))
# check if new state is in static equilibrium
if success:
# check stability
success = fullBody.isStateBalanced(s1.sId, 3)
if success:
success = projectInKinConstraints(fullBody, s1)
# check if transition is feasible according to CROC
if success:
#success = fullBody.isReachableFromState(s0.sId,s1.sId) or (len(fullBody.isDynamicallyReachableFromState(s0.sId,s1.sId, numPointsPerPhases=0)) > 0)
success = fullBody.isReachableFromState(s0.sId, s1.sId)
return success, s1
def setupSpidey():
switch_context(1)
q_0 = fullBody.getCurrentConfig(); r(q_0)
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
q_init[1] += 1.05
#~ q_0[2] = 1.1
s1 = State(fullBody,q=q_init, limbsIncontact = [rLegId, lLegId])
q0 = s1.q()[:]
r(q0)
return s1
def setupHrp2():
switch_context(0)
q_init = fullBody.getCurrentConfig()
r(q_init)
#~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
#~ q_init [0:3] = [-0.05, -0.82, 0.50];
q_init = [
-0.05,
-0.82,
0.65,
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)
#~ q_init[1] = -1.05
s1 = State(fullBody, q=q_init, limbsIncontact=[rLegId, lLegId])
#~ s1 = State(fullBody,q=q_init, limbsIncontact = [])
q0 = s1.q()[:]
r(q0)
fullBody.setRefConfig(q0)
return s1
def check_contact_plan(ps, r, pp, fullBody, idBegin, idEnd):
fullBody.client.basic.robot.setExtraConfigSpaceBounds(
[-0, 0, -0, 0, -0, 0, 0, 0, 0, 0, 0, 0])
fullBody.setStaticStability(False)
validPlan = True
for id_state in range(idBegin, idEnd - 1):
print("#### check for transition between state " + str(id_state) +
" and " + str(id_state + 1))
s0 = State(fullBody, sId=id_state)
s1 = State(fullBody, sId=id_state + 1)
# make a copy of each state because they are modified by the projection
s0_ = State(fullBody, q=s0.q(), limbsIncontact=s0.getLimbsInContact())
s1_ = State(fullBody, q=s1.q(), limbsIncontact=s1.getLimbsInContact())
valid = check_one_transition(ps, fullBody, s0_, s1_, r, pp)
validPlan = validPlan and valid
global curves_initGuess
global timings_initGuess
return validPlan, curves_initGuess, timings_initGuess
def generateConfigFromPhase(fb, phase, projectCOM=False):
fb.usePosturalTaskContactCreation(False)
contacts = getActiveContactLimbs(phase, fb)
#q = phase.reference_configurations[0].T.tolist()[0] # should be the correct config for the previous phase, if used only from high level helper methods
q = fb.referenceConfig[::] + [0] * 6 # FIXME : more generic !
root = computeCenterOfSupportPolygonFromPhase(phase, fb).T.tolist()[0]
q[0:2] = root[0:2]
q[2] += root[2] - fb.DEFAULT_COM_HEIGHT
quat = Quaternion(
rootOrientationFromFeetPlacement(phase, None)[0].rotation)
q[3:7] = [quat.x, quat.y, quat.z, quat.w]
# create state in fullBody :
state = State(fb, q=q, limbsIncontact=contacts)
# check if q is consistent with the contact placement in the phase :
fb.setCurrentConfig(q)
for limbId in contacts:
eeName = fb.dict_limb_joint[limbId]
placement_fb = SE3FromConfig(fb.getJointPosition(eeName))
placement_phase = JointPlacementForEffector(phase, eeName, fb)
if placement_fb != placement_phase: # add a threshold instead of 0 ? how ?
# need to project the new contact :
placement = getContactPlacement(phase, eeName, fb)
p = placement.translation.T.tolist()[0]
n = computeContactNormal(placement).T.tolist()[0]
state, success = StateHelper.addNewContact(state, limbId, p, n,
1000)
if not success:
print "Cannot project the configuration to contact, for effector : ", eeName
return state.q()
if projectCOM:
success = projectCoMInSupportPolygon(state)
if not success:
print "cannot project com to the middle of the support polygon."
phase.reference_configurations[0] = np.matrix(state.q()).T
return state.q()
def setupHrp2():
switch_context(0)
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)
#~ q_init[1] = -1.05
s1 = State(fullBody,q=q_init, limbsIncontact = [rLegId, lLegId])
q0 = s1.q()[:]
r(q0)
return s1
#~ d(0.07);e(0.01)
i = 0
#~ d(0.09); e(0.01);
configs = d(0.1)
e()
states = planToStates(fullBody, configs)[:-1]
s = State(fullBody,
q=states[-1].q(),
limbsIncontact=states[-1].getLimbsInContact())
x = s.getCenterOfMass()
x[0] -= 0.05
x[2] -= 0.1
fullBody.projectStateToCOM(s.sId, x, 10)
r(s.q())
states += [s]
#~ lc()
#~ le = min(38, len(states)-10)
lc()
#~ path = []
#~ all_paths = [[],[]]
#~ states = list(reversed(states))
#~ for i in range(0,4):
#~ print i
#~ onepath(i,0,nopt=1,effector=False,mu=1);
#~
#~
#~ onepath2(states [2:-2],nopt=0,mu=1,effector=False)
pids = []
pids += [fullBody.isDynamicallyReachableFromState(si.sId,sE.sId)]
pids += [fullBody.isDynamicallyReachableFromState(sE.sId,sfe.sId)]
for pid in pids :
if pid > 0:
print "success"
pp.displayPath(pid,color=r.color.blue)
r.client.gui.setVisibility('path_'+str(pid)+'_root','ALWAYS_ON_TOP')
else:
print "fail."
"""
configs = []
configs += [si.q()]
configs += [smid.q()]
configs += [smid2.q()]
configs += [sf2.q()]
from planning.config import *
from generate_contact_sequence import *
beginState = si.sId
endState = sf2.sId
cs = generateContactSequence(fullBody,configs,beginState, endState,r)
moveSphere('s',v,pLHand)
"""
state, success = StateHelper.addNewContact(state,
fb.rArmId,
pRHand,
n,
lockOtherJoints=True)
assert success, "Unable to create contact for right hand"
state, success = StateHelper.addNewContact(state,
fb.lArmId,
pLHand,
n,
lockOtherJoints=True)
assert success, "Unable to create contact for right hand"
q_ref = state.q()
fb.setReferenceConfig(q_ref)
fb.setPostureWeights(fb.postureWeights_straff[::] + [0] * 6)
v(q_ref)
addPhaseFromConfig(fb, v, cs, q_ref,
[fb.rLegId, fb.lLegId, fb.rArmId, fb.lArmId])
num_steps = 1
step_height = 0.22
step_width = 0.207
gait = [fb.rhand, fb.lhand, fb.rfoot, fb.lfoot]
for i in range(num_steps):
for eeName in gait:
0.008020752595769589,
0.0008315553354996808,
0,
0,
0,
0,
0,
0]
s2 = State(fullBody,q=q2,limbsIncontact=[fullBody.lLegId,fullBody.rLegId,fullBody.rArmId])
v(s2.q())
com = fullBody.getCenterOfMass()
comF = [-0.24796187, 0.74366786, 0.81803711]
com=comF[::]
com[0] = -0.2165
moveSphere("com",v,com)
s2.projectToCOM(com)
s3 = State(fullBody,q=s2.q(),limbsIncontact=[fullBody.lLegId,fullBody.rLegId,fullBody.rArmId])
s3.projectToCOM(com,10000)
v(s3.q())
"""
configs = [s0.q(), s1.q(), s2.q()]
#configs = [s0.q(),s2.q()]
v(configs[-1])
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, removeContact, addNewContactIfReachable, projectToFeasibleCom s1 = State(fullBody, q=q_init, limbsIncontact=[rLegId, lLegId, rarmId]) 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()
def test_contacts_6d(self):
with ServerManager('hpp-rbprm-server'):
fullbody = Robot()
fullbody.client.robot.setDimensionExtraConfigSpace(6)
fullbody.setJointBounds("root_joint", [-10, 10, -10, 10, -10, 10])
fullbody.client.robot.setExtraConfigSpaceBounds(
[-10, 10, -10, 10, -10, 10, -10, 10, -10, 10, -10, 10])
fullbody.loadAllLimbs("static", nbSamples=10000)
q = fullbody.referenceConfig[::] + [0] * 6
fullbody.setCurrentConfig(q)
com = fullbody.getCenterOfMass()
contacts = [fullbody.rLegId, fullbody.lLegId]
state = State(fullbody, q=q, limbsIncontact=contacts)
self.assertTrue(state.isBalanced())
self.assertTrue(state.isValid()[0])
self.assertTrue(state.isLimbInContact(fullbody.rLegId))
self.assertTrue(state.isLimbInContact(fullbody.lLegId))
self.assertEqual(com, state.getCenterOfMass())
# remove rf contact :
state1, success = StateHelper.removeContact(state, fullbody.rLegId)
self.assertTrue(success)
self.assertFalse(state1.isLimbInContact(fullbody.rLegId))
self.assertTrue(state1.isLimbInContact(fullbody.lLegId))
self.assertFalse(state1.isBalanced())
self.assertEqual(com, state1.getCenterOfMass())
# create a new contact :
n = [0, 0, 1]
p = [0.15, -0.085, 0.002]
state2, success = StateHelper.addNewContact(
state1, fullbody.rLegId, p, n)
self.assertTrue(success)
self.assertTrue(state2.isLimbInContact(fullbody.rLegId))
self.assertTrue(state2.isLimbInContact(fullbody.lLegId))
self.assertTrue(state2.isBalanced())
p2, n2 = state2.getCenterOfContactForLimb(fullbody.rLegId)
self.assertEqual(n, n2)
for i in range(3):
self.assertAlmostEqual(p[i], p2[i], 2)
# try to replace a contact :
p = [0.2, 0.085, 0.002]
state3, success = StateHelper.addNewContact(
state2, fullbody.lLegId, p, n)
self.assertTrue(success)
self.assertTrue(state3.isLimbInContact(fullbody.rLegId))
self.assertTrue(state3.isLimbInContact(fullbody.lLegId))
self.assertTrue(state3.isBalanced())
# try com projection:
com_target = com[::]
com_target[2] -= 0.08
success = state.projectToCOM(com_target)
self.assertTrue(success)
fullbody.setCurrentConfig(state.q())
com_state = fullbody.getCenterOfMass()
self.assertEqual(com_state, state.getCenterOfMass())
for i in range(3):
self.assertAlmostEqual(com_state[i], com_target[i], 4)
# try lockOtherJoints:
p = [0.1, -0.085, 0.002]
state4, success = StateHelper.addNewContact(state,
fullbody.rLegId,
p,
n,
lockOtherJoints=True)
self.assertTrue(success)
self.assertTrue(state4.isLimbInContact(fullbody.rLegId))
self.assertTrue(state4.isLimbInContact(fullbody.lLegId))
self.assertTrue(state4.isBalanced())
for i in range(7, 13):
self.assertAlmostEqual(state.q()[i], state4.q()[i])
for i in range(19, len(q)):
self.assertAlmostEqual(state.q()[i], state4.q()[i])
# try with a rotation
rot = [0.259, 0, 0, 0.966]
state5, success = StateHelper.addNewContact(state,
fullbody.rLegId,
p,
n,
rotation=rot)
self.assertTrue(success)
self.assertTrue(state5.isLimbInContact(fullbody.rLegId))
self.assertTrue(state5.isLimbInContact(fullbody.lLegId))
self.assertTrue(state5.isBalanced())
fullbody.setCurrentConfig(state5.q())
rf_pose = fullbody.getJointPosition(fullbody.rfoot)
for i in range(4):
self.assertAlmostEqual(rf_pose[i + 3], rot[i], 3)
else:
global fails
fails += 1
# print(fullBody.isConfigValid(q)[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")
# f1.close()
s = State(fullBody,
q=fullBody.referenceConfig,
limbsIncontact=[fullBody.limbs_names[0]])
v(s.q())
#~ printRootPosition(rLegId, rfoot, nbSamples)
#~ printRootPosition(lLegId, lfoot, nbSamples)
#~ printRootPosition(rarmId, rHand, nbSamples)
#~ printRootPosition(larmId, lHand, nbSamples)
printFootPositionRelativeToOther(6000)
print("successes ", success)
print("fails ", fails)
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from plot_polytopes import plot_hull
# ~ for effector, comData, pointsData in zip(effectors, compoints, points):
# ~ for effector, limbId, comData, pointsData in zip(effectors[:1],limbIds[1:], compoints[:1], points[:1]):
def setupSpidey():
switch_context(1)
q_0 = fullBody.getCurrentConfig()
r(q_0)
#~ q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
#~ q_init = [1.4497476605315172,
#~ 0.586723585687863,
#~ 0.7007986764501943,
#~ 0.9992040068259673,
#~ 0.009915872351221816,
#~ -0.011224843288091112,
#~ 0.0369733838268027,
#~ -0.21440808741905482,
#~ 0.5819742125714021,
#~ -0.9540663648074178,
#~ -0.444827228611522,
#~ -0.2814234791010941,
#~ 0.6601172395940408,
#~ 0.05057907671648097,
#~ 0.03165298930985974,
#~ -0.8105513007687147,
#~ 0.23456431240474232,
#~ -0.6906832652645104,
#~ 1.042653827624378]
#~ q_init = [1.403034485826222,
#~ -0.0680461187483179,
#~ 0.698067114148141,
#~ 0.9994021773454228,
#~ 0.029333888870596826,
#~ -0.01699029633207977,
#~ 0.006792695452013932,
#~ -0.2670125225627798,
#~ 0.5760223974412482,
#~ -0.6323579390738086,
#~ -0.3824808801598628,
#~ 0.12703791928011066,
#~ 0.41203223392262833,
#~ 0.3545295626633363,
#~ 0.4930851685230413,
#~ -0.7792461682735826,
#~ 0.1068537396039267,
#~ -0.7497836830029866,
#~ 1.1890550989396227]
#~ q_init = [1.7248956090657142,
#~ 0.10332894313987395,
#~ 0.7135677069780713,
#~ 0.8053782375870717,
#~ 0.009162336020200251,
#~ 0.017984134378282467,
#~ -0.5924175190948179,
#~ -0.09343758894532768,
#~ -0.13260962085227604,
#~ -0.7334459269711523,
#~ -0.11305498904738602,
#~ -0.1883955962566395,
#~ 0.8346448329401047,
#~ 0.27875376841185046,
#~ 0.654114442736956,
#~ -0.495495198017057,
#~ -0.22902533402342157,
#~ -0.0733460650991134,
#~ 0.6485831393133032]
q_init = [
2.0073705139562783, 0.10455451701731842, 0.6636982709906831,
0.8052290989552832, -0.010905503215389124, -0.028433849502961527,
-0.5921812935222833, 0.32502766737262234, 0.15429143500131443,
-0.8306825282286409, 0.3321441608555586, 0.00475050546222314,
0.9719582647982088, -0.14368646477113056, 0.8209795520480985,
-0.349065850399, 0.32573656479055596, -0.7194630454730782,
0.349065850399
]
#~ q_init[0:7] = tp.q_init[0:7]
#~ q_init[1] += 1.05
#~ q_0[2] = 1.1
s1 = State(fullBody,
q=q_init,
limbsIncontact=[rLegId, lLegId, rarmId, larmId])
#~ q_goal = fullBody.generateContacts(s1.q(), [0,0,1])
#~ s1.setQ(q_goal)
q0 = s1.q()[:]
r(q0)
return s1
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, removeContact, addNewContactIfReachable, projectToFeasibleCom 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()
