print "number of configs :", len(configsFull)
"""
q_init[0] += 0.05
createSphere('s', r)
n = [0, 0, 1]
p = [0, 0.1, 0]
sf = State(fullBody, q=q_goal, limbsIncontact=[lLegId, rLegId])
si = State(fullBody, q=q_init, limbsIncontact=[lLegId, rLegId])
n = [0.0, -0.42261828000211843, 0.9063077785212101]
p = [0.775, 0.23, -0.02]
moveSphere('s', r, p)
smid, success = StateHelper.addNewContact(si, lLegId, p, n)
assert (success)
smid2, success = StateHelper.addNewContact(sf, lLegId, p, n)
assert (success)
r(smid.q())
sf2 = State(fullBody, q=q_goal, limbsIncontact=[lLegId, rLegId])
"""
com = fullBody.getCenterOfMass()
com[1] = 0
"""
import disp_bezier
pp.dt = 0.0001
pids = []
curves = []
timings = []
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)
q_0 = fullBody.getCurrentConfig(); q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.ps.configAtParam(0,0.01)[0:7] # use this to get the correct orientation q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.ps.configAtParam(0,tp.ps.pathLength(0))[0:7] dir_init = tp.ps.configAtParam(0,0.01)[7:10] acc_init = tp.ps.configAtParam(0,0.01)[10:13] dir_goal = tp.ps.configAtParam(0,tp.ps.pathLength(0))[7:10] acc_goal = tp.ps.configAtParam(0,tp.ps.pathLength(0))[10:13] configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace() fullBody.setStaticStability(True) # Randomly generating a contact configuration at q_init fullBody.setCurrentConfig (q_init) ; r(q_init) s_init = StateHelper.generateStateInContact(fullBody,q_init,dir_init,acc_init) q_init = s_init.q() r(q_init) # Randomly generating a contact configuration at q_end fullBody.setCurrentConfig (q_goal) s_goal = StateHelper.generateStateInContact(fullBody,q_goal, dir_goal,acc_goal) q_goal = s_goal.q() # 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[::] # specifying the full body configurations as start and goal state of the problem fullBody.setStartStateId(s_init.sId)
def gen_state(s,
pId,
num_max_sample=0,
first=False,
normal=lp.Z_AXIS,
newContact=True,
projectCOM=True):
#~ pId = 6
phase = pb["phaseData"][pId]
moving = phase["moving"]
movingID = fullBody.lLegId
if moving == lp.RF:
movingID = fullBody.rLegId
print("# gen state for phase Id = ", pId)
if USE_ORIENTATION:
if pId < len(pb["phaseData"]) - 1:
if phase["moving"] == lp.RF:
rot = quatConfigFromMatrix(
pb["phaseData"][pId + 1]["rootOrientation"]
) # rotation of the root, from the guide
else:
rot = quatConfigFromMatrix(
pb["phaseData"][pId]["rootOrientation"]
) # rotation of the root, from the guide
#quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"])
#quat1 = Quaternion(pb["phaseData"][pId+1]["rootOrientation"])
#rot = quatConfigFromMatrix((quat0.slerp(0.5,quat1)).matrix())
else:
rot = quatConfigFromMatrix(
phase["rootOrientation"]
) # rotation of the root, from the guide
else:
rot = [0, 0, 0, 1]
#rot = quatConfigFromMatrix(phase["rootOrientation"]) # rotation of the root, from the guide
#print "current config q=",s.q()
#print "move limb ",movingID
pos = allfeetpos[pId]
print("Try to add contact for " + movingID + " pos = " +
str(pos.tolist() + rot))
disp.moveSphere('c', v, pos.tolist() + rot)
if newContact:
sres, succ = StateHelper.addNewContact(s,
movingID,
pos.tolist(),
normal.tolist(),
num_max_sample=num_max_sample,
rotation=rot)
#sres, succ = StateHelper.removeContact(s,movingID)
#assert succ
#succ = sres.projectToRoot(s.q()[0:3]+rot)
#sres, succ = StateHelper.addNewContact(s, movingID, pos.tolist(), normal.tolist(), num_max_sample= num_max_sample)
else:
sres, succ = StateHelper.cloneState(s)
if not succ:
print("Cannot project config q = ", sres.q())
print("To new contact position for " + movingID + " = " +
str(pos.tolist() + rot) + " ; n = " + str(normal.tolist()))
raise RuntimeError("Cannot project feet to new contact position"
) # try something else ??
if projectCOM:
#print "config before projecting to com q1=",sres.q()
successCOM = projectCoMInSupportPolygon(sres)
if not successCOM:
# is it really an issue ?
print(
"Unable to project CoM in the center of the support polygone")
v(sres.q())
return sres
from display_tools import *
createSphere('target', v, size=0.05, color=v.color.red)
v.client.gui.setVisibility('target', 'ON')
moveSphere('target', v, [0, 0, 0.5])
# create contact :
fullBody.setStartState(q_init, [lLegId, rLegId])
q_ref[0:3] = q_init[0:3]
sref = State(fullBody, q=q_ref, limbsIncontact=[lLegId, rLegId])
s0 = State(fullBody, q=q_init, limbsIncontact=[lLegId, rLegId])
createSphere('s', v)
p0 = [-0.25, 0.5, 0.75]
#p1 = [-0,0.45,0.8]
p = [0.1, 0.5, 0.75]
moveSphere('s', v, p)
s0_bis, success = StateHelper.addNewContact(sref, rArmId, p0, [0, 0, 1])
#s0_bis2,success = StateHelper.addNewContact(s0_bis,rArmId,p1,[0,0,1])
s1, success = StateHelper.addNewContact(s0_bis, rArmId, p, [0, 0, 1])
assert (success)
v(s1.q())
#project com
v(q_init)
com_i = fullBody.getCenterOfMass()
com_i[2] -= 0.03
com_i[0] += 0.06
createSphere("com", v)
moveSphere("com", v, com_i)
s1.projectToCOM(com_i)
v(s1.q())
s1_feet = State(fullBody, q=s1.q(), limbsIncontact=[lLegId, rLegId])