dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId)-0.01)[tp.indexECS:tp.indexECS+3]
acc_goal = [0,0,0]
robTreshold = 3
# copy extraconfig for start and init configurations
q_init[configSize:configSize+3] = dir_init[::]
q_init[configSize+3:configSize+6] = acc_init[::]
q_goal[configSize:configSize+3] = dir_goal[::]
q_goal[configSize+3:configSize+6] = [0,0,0]
# FIXME : test
q_init[2] = q_ref[2]+0.011
q_goal[2] = q_ref[2]+0.011
fullBody.setStaticStability(True)
# Randomly generating a contact configuration at q_init
fullBody.setCurrentConfig (q_init)
r(q_init)
#q_init = fullBody.generateContacts(q_init,dir_init,acc_init,robTreshold)
r(q_init)
# Randomly generating a contact configuration at q_end
fullBody.setCurrentConfig (q_goal)
#q_goal = fullBody.generateContacts(q_goal, dir_goal,acc_goal,robTreshold)
r(q_goal)
# specifying the full body configurations as start and goal state of the problem
r.addLandmark('hrp2_14/BODY',0.3)
r(q_init)
q_goal = fullBody.getCurrentConfig() q_goal[0:7] = tp.ps.configAtParam(pathId, tp.ps.pathLength(pathId))[0:7] dir_init = tp.ps.configAtParam(pathId, 0.01)[7:10] acc_init = tp.ps.configAtParam(pathId, 0.01)[10:13] dir_goal = tp.ps.configAtParam(pathId, tp.ps.pathLength(pathId))[7:10] acc_goal = tp.ps.configAtParam(pathId, tp.ps.pathLength(pathId))[10:13] configSize = fullBody.getConfigSize( ) - fullBody.client.basic.robot.getDimensionExtraConfigSpace() # copy extraconfig for start and init configurations q_init[configSize:configSize + 3] = dir_init[::] q_init[configSize + 3:configSize + 6] = acc_init[::] q_goal[configSize:configSize + 3] = dir_goal[::] q_goal[configSize + 3:configSize + 6] = acc_goal[::] fullBody.setStaticStability(False) # Randomly generating a contact configuration at q_init fullBody.setCurrentConfig(q_init) q_init = fullBody.generateContacts(q_init, dir_init, acc_init, 2) # Randomly generating a contact configuration at q_end fullBody.setCurrentConfig(q_goal) q_goal = fullBody.generateContacts(q_goal, dir_goal, acc_goal, 2) # specifying the full body configurations as start and goal state of the problem fullBody.setStartState(q_init, [larmId, rLegId, rarmId, lLegId]) fullBody.setEndState(q_goal, [larmId, rLegId, rarmId, lLegId]) r(q_init) # computing the contact sequence
pId, tp.ps.pathLength(pId))[tp.indexECS:tp.indexECS + 3]
acc_goal = [0, 0, 0]
robTreshold = 1
# copy extraconfig for start and init configurations
q_init[configSize:configSize + 3] = dir_init[::]
q_init[configSize + 3:configSize + 6] = acc_init[::]
q_goal[configSize:configSize + 3] = dir_goal[::]
q_goal[configSize + 3:configSize + 6] = [0, 0, 0]
# FIXME : test
q_init[2] = q_init[2] + 0.1
q_goal[2] = q_goal[2] + 0.1
# Randomly generating a contact configuration at q_init
fullBody.setStaticStability(True)
fullBody.setCurrentConfig(q_init)
r(q_init)
q_init = fullBody.generateContacts(q_init, dir_init, acc_init, robTreshold)
r(q_init)
# Randomly generating a contact configuration at q_end
fullBody.setCurrentConfig(q_goal)
q_goal = fullBody.generateContacts(q_goal, dir_goal, acc_goal, robTreshold)
r(q_goal)
# specifying the full body configurations as start and goal state of the problem
r.addLandmark('hrp2_14/BODY', 0.3)
r(q_init)
fullBody.setStartState(q_init, [rLegId, lLegId])
