def main():
np.set_printoptions(precision=4, linewidth=200)
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5()
motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped()
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
#motionModel.recordVelByFiniteDiff()
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
#ModelOffset = (1.5, -0.01, 0)
ModelOffset = (1.5, 0.03, 0)
controlModel.translateByOffset(ModelOffset)
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
# parameter
Kt = config['Kt']
Dt = config['Dt'] # tracking gain
Kl = config['Kl']
Dl = config['Dl'] # linear balance gain
Kh = config['Kh']
Dh = config['Dh'] # angular balance gain
Ks = config['Ks']
Ds = config['Ds'] # penalty force spring gain
Bt = config['Bt']
Bl = config['Bl']
Bh = config['Bh']
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
w2 = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap2'])
#w_IK = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['IKweightMap'])
supL = motion[0].skeleton.getJointIndex(config['supLink'])
supR = motion[0].skeleton.getJointIndex(config['supLink2'])
rootB = motion[0].skeleton.getJointIndex(config['root'])
selectedBody = motion[0].skeleton.getJointIndex(config['end'])
#constBody = motion[0].skeleton.getJointIndex('LeftForeArm')
constBody = motion[0].skeleton.getJointIndex(config['const'])
# jacobian
Jsup = yjc.makeEmptyJacobian(DOFs, 1)
dJsup = Jsup.copy()
JsupPre = Jsup.copy()
Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum())
dJsys = Jsys.copy()
JsysPre = Jsys.copy()
Jconst = yjc.makeEmptyJacobian(DOFs, 1)
dJconst = Jconst.copy()
###############
footPartNum = config['FootPartNum']
indexFootL = [None] * footPartNum
indexFootR = [None] * footPartNum
jFootL = [None] * footPartNum
dJFootL = [None] * footPartNum
jFootR = [None] * footPartNum
dJFootR = [None] * footPartNum
jointMasksFootL = [None] * footPartNum
jointMasksFootR = [None] * footPartNum
jAngFootL = [None] * footPartNum
dJAngFootL = [None] * footPartNum
jAngFootR = [None] * footPartNum
dJAngFootR = [None] * footPartNum
for i in range(footPartNum):
jFootL[i] = yjc.makeEmptyJacobian(DOFs, 1)
dJFootL[i] = jFootL[i].copy()
jFootR[i] = yjc.makeEmptyJacobian(DOFs, 1)
dJFootR[i] = jFootR[i].copy()
jAngFootL[i] = yjc.makeEmptyJacobian(DOFs, 1, False)
dJAngFootL[i] = jAngFootL[i].copy()
jAngFootR[i] = yjc.makeEmptyJacobian(DOFs, 1, False)
dJAngFootR[i] = jAngFootR[i].copy()
indexFootL[i] = motion[0].skeleton.getJointIndex(
config['FootLPart'][i])
indexFootR[i] = motion[0].skeleton.getJointIndex(
config['FootRPart'][i])
jointMasksFootL[i] = [
yjc.getLinkJointMask(motion[0].skeleton, indexFootL[i])
]
jointMasksFootR[i] = [
yjc.getLinkJointMask(motion[0].skeleton, indexFootR[i])
]
constJointMasks = [
yjc.getLinksJointMask(motion[0].skeleton,
[indexFootL[0], indexFootR[0]])
]
#constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)]
allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton)
'''
maskArray = [foreSupLJointMasks, foreSupRJointMasks, rearSupLJointMasks, rearSupRJointMasks]
parentArray = [supL, supR, supL, supR]
effectorArray = [foreSupL, foreSupR, rearSupL, rearSupR]
for j in range(4) :
for i in range(len(foreSupLJointMasks)) :
if i == parentArray[j] or i == effectorArray[j] :
maskArray[j][0][i] = 1
else :
maskArray[j][0][i] = 0
'''
# momentum matrix
linkMasses = controlModel.getBodyMasses()
totalMass = controlModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
problem = yac.LSE(totalDOF, 6)
a_sup = (0, 0, 0, 0, 0, 0) #L
#a_sup2 = (0,0,0, 0,0,0)#R
a_sup2 = [0, 0, 0, 0, 0, 0] #R
a_sup_2 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
CP_old = [mm.v3(0., 0., 0.)]
# penalty method
bodyIDsToCheck = range(vpWorld.getBodyNum())
mus = [1.] * len(bodyIDsToCheck)
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
d_th_IK = ype.makeNestedList(DOFs)
d_th_IK_L = ype.makeNestedList(DOFs)
d_th_IK_R = ype.makeNestedList(DOFs)
dd_th_IK = ype.makeNestedList(DOFs)
dd_th_IK_flat = ype.makeFlatList(totalDOF)
d_th_IK_flat = ype.makeFlatList(totalDOF)
ddth_c_flat = ype.makeFlatList(totalDOF)
# viewer
rd_footCenter = [None]
rd_footCenter_ref = [None]
rd_footCenterL = [None]
rd_footCenterR = [None]
rd_CM_plane = [None]
rd_CM_plane_ref = [None]
rd_CM_ref = [None]
rd_CM = [None]
rd_CM_vec = [None]
rd_CM_ref_vec = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_footCenter_des = [None]
rd_exf_des = [None]
rd_root_des = [None]
rd_soft_const_vec = [None]
rd_root = [None]
rd_footL_vec = [None]
rd_footR_vec = [None]
rd_CMP = [None]
rd_DesPosL = [None]
rd_DesPosR = [None]
rd_DesForePosL = [None]
rd_DesForePosR = [None]
rd_DesRearPosL = [None]
rd_DesRearPosR = [None]
rd_contactForces = [None] * 1000
rd_contactPositions = [None] * 1000
rootPos = [None]
selectedBodyId = [selectedBody]
extraForce = [None]
applyedExtraForce = [None]
applyedExtraForce[0] = [0, 0, 0]
normalVector = [[0, 2, 0]]
viewer = ysv.SimpleViewer()
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'motionModel',
cvr.VpModelRenderer(motionModel, (150, 150, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'controlModel',
cvr.VpModelRenderer(controlModel, (255, 240, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
viewer.doc.addRenderer('rd_footCenter_des',
yr.PointsRenderer(rd_footCenter_des, (150, 0, 150)))
#viewer.doc.addRenderer('rd_footCenterL', yr.PointsRenderer(rd_footCenterL))
#viewer.doc.addRenderer('rd_footCenterR', yr.PointsRenderer(rd_footCenterR))
#viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255,255,0)))
viewer.doc.addRenderer('rd_CM',
yr.PointsRenderer(rd_CM_plane, (255, 255, 0)))
viewer.doc.addRenderer('rd_CP_des',
yr.PointsRenderer(rd_CP_des, (0, 255, 0)))
#viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255,0,255)))
# viewer.doc.addRenderer('rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255,255,0)))
# viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0,255,0)))
viewer.doc.addRenderer(
'rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP, (0, 255, 255),
.001))
viewer.doc.addRenderer(
'rd_exf_des',
yr.ForcesRenderer(rd_exf_des, rd_root_des, (0, 255, 0), .009, 0.05))
viewer.doc.addRenderer('rd_CMP', yr.PointsRenderer(rd_CMP, (0, 0, 255)))
viewer.doc.addRenderer('rd_DesPosL',
yr.PointsRenderer(rd_DesPosL, (0, 0, 255)))
viewer.doc.addRenderer('rd_DesPosR',
yr.PointsRenderer(rd_DesPosR, (0, 100, 255)))
viewer.doc.addRenderer('rd_DesForePosL',
yr.PointsRenderer(rd_DesForePosL, (150, 0, 200)))
viewer.doc.addRenderer('rd_DesForePosR',
yr.PointsRenderer(rd_DesForePosR, (150, 0, 250)))
viewer.doc.addRenderer('rd_DesRearPosL',
yr.PointsRenderer(rd_DesRearPosL, (0, 150, 200)))
viewer.doc.addRenderer('rd_DesRearPosR',
yr.PointsRenderer(rd_DesRearPosR, (0, 150, 250)))
viewer.doc.addRenderer(
'softConstraint',
yr.VectorsRenderer(rd_soft_const_vec, rd_CMP, (150, 100, 100), 3))
viewer.doc.addRenderer(
'rd_footLVec',
yr.VectorsRenderer(rd_footL_vec, rd_footCenterL, (255, 0, 0), 3))
viewer.doc.addRenderer(
'rd_footRVec',
yr.VectorsRenderer(rd_footR_vec, rd_footCenterL, (255, 255, 0), 3))
#viewer.doc.addRenderer('rd_footCenter_ref', yr.PointsRenderer(rd_footCenter_ref))
viewer.doc.addRenderer('rd_CM_plane_ref',
yr.PointsRenderer(rd_CM_plane_ref, (255, 255, 0)))
viewer.doc.addRenderer(
'rd_refNormalVec',
yr.VectorsRenderer(normalVector, rd_footCenter_ref, (255, 0, 0), 3))
viewer.doc.addRenderer(
'rd_refCMVec',
yr.VectorsRenderer(rd_CM_ref_vec, rd_footCenter_ref, (255, 0, 255), 3))
viewer.doc.addRenderer(
'rd_curNormalVec',
yr.VectorsRenderer(normalVector, rd_footCenter, (255, 0, 0), 3))
viewer.doc.addRenderer(
'rd_CMVec',
yr.VectorsRenderer(rd_CM_vec, rd_footCenter, (255, 0, 255), 3))
viewer.doc.addRenderer(
'rd_contactForces',
yr.ForcesRenderer(rd_contactForces, rd_contactPositions, (0, 255, 0),
.009, 0.009))
stage = STATIC_BALANCING
contactRendererName = []
for i in range(motion[0].skeleton.getJointNum()):
print(i, motion[0].skeleton.getJointName(i))
def simulateCallback(frame):
global g_initFlag
global forceShowFrame
global forceApplyFrame
global JsysPre
global JsupPreL
global JsupPreR
global JsupPre
global softConstPoint
global stage
global contactRendererName
motionModel.update(motion[frame])
Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, Bsc = viewer.GetParam()
Dt = 2 * (Kt**.5)
Dk = 2 * (Kk**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
Dsc = 2 * (Ksc**.5)
if Bsc == 0.0:
viewer.doc.showRenderer('softConstraint', False)
viewer.motionViewWnd.update(1, viewer.doc)
else:
viewer.doc.showRenderer('softConstraint', True)
renderer1 = viewer.doc.getRenderer('softConstraint')
renderer1.rc.setLineWidth(0.1 + Bsc * 3)
viewer.motionViewWnd.update(1, viewer.doc)
# tracking
th_r = motion.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r,
Kt, Dt)
ddth_c = controlModel.getDOFAccelerations()
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
ype.flatten(ddth_c, ddth_c_flat)
# jacobian
refFootL = motionModel.getBodyPositionGlobal(supL)
refFootR = motionModel.getBodyPositionGlobal(supR)
positionFootL = [None] * footPartNum
positionFootR = [None] * footPartNum
for i in range(footPartNum):
positionFootL[i] = controlModel.getBodyPositionGlobal(
indexFootL[i])
positionFootR[i] = controlModel.getBodyPositionGlobal(
indexFootR[i])
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
jointPositions = controlModel.getJointPositionsGlobal()
jointAxeses = controlModel.getDOFAxeses()
CM = yrp.getCM(linkPositions, linkMasses, totalMass)
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
CM_plane = copy.copy(CM)
CM_plane[1] = 0.
dCM_plane = copy.copy(dCM)
dCM_plane[1] = 0.
linkPositions_ref = motionModel.getBodyPositionsGlobal()
CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass)
CM_plane_ref = copy.copy(CM_ref)
CM_plane_ref[1] = 0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses,
linkPositions, allLinkJointMasks)
yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions,
jointAxeses, linkAngVelocities,
linkPositions, allLinkJointMasks)
yjc.computeJacobian2(jFootL[0], DOFs, jointPositions, jointAxeses,
[positionFootL[0]], jointMasksFootL[0])
yjc.computeJacobianDerivative2(dJFootL[0], DOFs, jointPositions,
jointAxeses, linkAngVelocities,
[positionFootL[0]], jointMasksFootL[0],
False)
yjc.computeJacobian2(jFootR[0], DOFs, jointPositions, jointAxeses,
[positionFootR[0]], jointMasksFootR[0])
yjc.computeJacobianDerivative2(dJFootR[0], DOFs, jointPositions,
jointAxeses, linkAngVelocities,
[positionFootR[0]], jointMasksFootR[0],
False)
yjc.computeAngJacobian2(jAngFootL[0], DOFs, jointPositions,
jointAxeses, [positionFootL[0]],
jointMasksFootL[0])
yjc.computeAngJacobianDerivative2(dJAngFootL[0], DOFs, jointPositions,
jointAxeses, linkAngVelocities,
[positionFootL[0]],
jointMasksFootL[0], False)
yjc.computeAngJacobian2(jAngFootR[0], DOFs, jointPositions,
jointAxeses, [positionFootR[0]],
jointMasksFootR[0])
yjc.computeAngJacobianDerivative2(dJAngFootR[0], DOFs, jointPositions,
jointAxeses, linkAngVelocities,
[positionFootR[0]],
jointMasksFootR[0], False)
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
for i in range(len(bodyIDsToCheck)):
controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0)
##########################################
for i in range(len(rd_contactPositions)):
rd_contactPositions[i] = [0, 0, 0]
rd_contactForces[i] = [0, 0, 0]
for i in range(len(contactPositions)):
rd_contactPositions[i] = np.copy(contactPositions[i])
rd_contactForces[i] = np.copy(contactForces[i])
'''
if len(contactPositions) > 0:
rd_contactPositions = np.copy(contactPositions)
rd_contactForces = np.copy(contactForces)
print("rd_contactPositions", rd_contactPositions)
print("contactPositions", contactPositions)
'''
'''
for i in range(len(contactRendererName)):
viewer.doc.removeRenderer(contactRendererName[i])
del contactRendererName[:]
for i in range(len(contactPositions)):
contactRendererName.append(str(i))
#viewer.doc.addRenderer(str(i), yr.PointsRenderer([contactPositions[i]], (0,255,0)))
viewer.doc.addRenderer(str(i), yr.ForcesRenderer([contactForces[i]], [contactPositions[i]], (0,255,0), .009, 0.009))
viewer.motionViewWnd.update(1, viewer.doc)
'''
##########################################
contactFlagFootL = [0] * footPartNum
contactFlagFootR = [0] * footPartNum
partialDOFIndex = [22, 22]
for i in range(len(bodyIDs)):
controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105)
index = controlModel.id2index(bodyIDs[i])
for j in range(len(indexFootL)):
if index == indexFootL[j]:
contactFlagFootL[j] = 1
if j != 0:
yjc.computePartialJacobian2(jFootL[j], DOFs,
jointPositions,
jointAxeses,
[positionFootL[j]],
jointMasksFootL[j],
partialDOFIndex)
yjc.computePartialJacobianDerivative2(
dJFootL[j], DOFs, jointPositions, jointAxeses,
linkAngVelocities, [positionFootL[j]],
jointMasksFootL[j], False, partialDOFIndex)
break
for j in range(len(indexFootR)):
if index == indexFootR[j]:
contactFlagFootR[j] = 1
if j != 0:
yjc.computePartialJacobian2(jFootR[j], DOFs,
jointPositions,
jointAxeses,
[positionFootR[j]],
jointMasksFootR[j],
partialDOFIndex)
yjc.computePartialJacobianDerivative2(
dJFootR[j], DOFs, jointPositions, jointAxeses,
linkAngVelocities, [positionFootR[j]],
jointMasksFootR[j], False, partialDOFIndex)
break
for j in range(len(indexFootL)):
yjc.computeAngJacobian2(jAngFootL[j], DOFs, jointPositions,
jointAxeses, [positionFootL[j]],
jointMasksFootL[j])
yjc.computeAngJacobianDerivative2(dJAngFootL[j], DOFs,
jointPositions, jointAxeses,
linkAngVelocities,
[positionFootL[j]],
jointMasksFootL[j], False)
yjc.computeAngJacobian2(jAngFootR[j], DOFs, jointPositions,
jointAxeses, [positionFootR[j]],
jointMasksFootR[j])
yjc.computeAngJacobianDerivative2(dJAngFootR[j], DOFs,
jointPositions, jointAxeses,
linkAngVelocities,
[positionFootR[j]],
jointMasksFootR[j], False)
'''
if footPartNum == 1:
footCenterL = controlModel.getBodyPositionGlobal(supL)
footCenterR = controlModel.getBodyPositionGlobal(supR)
else:
if ((contactFlagFootL[3] == 1 or contactFlagFootL[4] == 1) and contactFlagFootL[0] == 0) or ((contactFlagFootR[3] == 1 or contactFlagFootR[4] == 1) and contactFlagFootR[0] == 0):
r = 0.8
footCenterL = (controlModel.getBodyPositionGlobal(supL)*r + controlModel.getBodyPositionGlobal(indexFootL[1])*(1.0-r))
footCenterR = (controlModel.getBodyPositionGlobal(supR)*r + controlModel.getBodyPositionGlobal(indexFootR[1])*(1.0-r))
#footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1])
#footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1])
else :
#footCenterL = (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(indexFootL[1]))/2.0
#footCenterR = (controlModel.getBodyPositionGlobal(supR) + controlModel.getBodyPositionGlobal(indexFootR[1]))/2.0
#footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1])
#footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1])
r = 0.8
footCenterL = (controlModel.getBodyPositionGlobal(indexFootL[1])*r + controlModel.getBodyPositionGlobal(indexFootL[3])*(1.0-r))
footCenterR = (controlModel.getBodyPositionGlobal(indexFootR[1])*r + controlModel.getBodyPositionGlobal(indexFootR[3])*(1.0-r))
'''
'''
if stage == POWERFUL_BALANCING:
footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1])
footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1])
else:
footCenterL = (controlModel.getBodyPositionGlobal(indexFootL[1]) + controlModel.getBodyPositionGlobal(indexFootL[3]) )/2.0
footCenterR = (controlModel.getBodyPositionGlobal(indexFootR[1]) + controlModel.getBodyPositionGlobal(indexFootR[3]))/2.0
'''
'''
p1 = controlModel.getBodyPositionGlobal(indexFootL[0])
p2 = controlModel.getBodyPositionGlobal(indexFootR[0])
p3 = controlModel.getBodyPositionGlobal(indexFootL[4])
p4 = controlModel.getBodyPositionGlobal(indexFootR[4])
print(frame, "supL", p1[1])
print(frame, "supR", p2[1])
print(frame, "calcL", p3[1])
print(frame, "calcR", p4[1])
'''
#footCenter = footCenterL + (footCenterR - footCenterL)/2.0
#footCenter[1] = 0.
#
'''
if checkAll(contactFlagFootL, 0) == 1 and checkAll(contactFlagFootR, 0) == 1:
footCenter = footCenter
elif checkAll(contactFlagFootL, 0) == 1 :
footCenter = footCenterR
elif checkAll(contactFlagFootR, 0) == 1 :
footCenter = footCenterL
'''
if footPartNum == 1:
desFCL = (controlModel.getBodyPositionGlobal(supL))
desFCR = (controlModel.getBodyPositionGlobal(supR))
else:
r = 1.
desFCL = (controlModel.getBodyPositionGlobal(supL) * r +
controlModel.getBodyPositionGlobal(indexFootL[2]) *
(1.0 - r)
) #controlModel.getBodyPositionGlobal(indexFootL[1])
desFCR = (controlModel.getBodyPositionGlobal(supR) * r +
controlModel.getBodyPositionGlobal(indexFootR[2]) *
(1.0 - r)
) #controlModel.getBodyPositionGlobal(indexFootR[1])
desFC = desFCL + (desFCR - desFCL) / 2.0
if checkAll(contactFlagFootL, 0) == 1 and checkAll(
contactFlagFootR, 0) == 1:
desFC = desFC
elif checkAll(contactFlagFootL, 0) == 1:
desFC = desFCR
elif checkAll(contactFlagFootR, 0) == 1:
desFC = desFCL
desFC[1] = 0
rd_footCenter_des[0] = desFC.copy()
curRelCMVec = CM_plane - desFC
vecRatio = mm.length(curRelCMVec) * 0.
#print(frame, vecRatio)
footCenter = desFC - curRelCMVec * (vecRatio) #/10.0
footCenter_ref = refFootL + (refFootR - refFootL) / 2.0
#footCenter_ref[1] = 0.
footCenter[1] = 0.
vecRatio = mm.length(curRelCMVec) * 10.
softConstPointOffset = -curRelCMVec * (vecRatio) #/10.0
#print(frame, vecRatio, softConstPointOffset)
desForeSupLAcc = [0, 0, 0]
desForeSupRAcc = [0, 0, 0]
totalNormalForce = [0, 0, 0]
for i in range(len(contactForces)):
totalNormalForce[0] += contactForces[i][0]
totalNormalForce[1] += contactForces[i][1]
totalNormalForce[2] += contactForces[i][2]
# linear momentum
CM_ref_plane = footCenter
dL_des_plane = Kl * totalMass * (CM_ref_plane -
CM_plane) - Dl * totalMass * dCM_plane
# angular momentum
CP_ref = footCenter
timeStep = 30.
if CP_old[0] == None or CP == None:
dCP = None
else:
dCP = (CP - CP_old[0]) / (1 / timeStep)
CP_old[0] = CP
if CP != None and dCP != None:
ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP)
CP_des = CP + dCP * (1 / timeStep) + .5 * ddCP_des * (
(1 / timeStep)**2)
dH_des = np.cross(
(CP_des - CM),
(dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
#dH_des = np.cross((CP_des - CM_plane), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity)))
else:
dH_des = None
# momentum matrix
RS = np.dot(P, Jsys)
R, S = np.vsplit(RS, 2)
rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat)
r_bias, s_bias = np.hsplit(rs, 2)
##############################
# soft point constraint
#####################################################
P_cur = controlModel.getBodyPositionGlobal(constBody)
constBodyVec = P_cur - footCenter
softConstPoint = [
footCenter[0] + softConstPointOffset[0],
mm.length(constBodyVec), footCenter[2] + softConstPointOffset[2]
]
#####################################################
P_des = softConstPoint
#P_cur = controlModel.getBodyPositionGlobal(constBody)
dP_des = [0, 0, 0]
dP_cur = controlModel.getBodyVelocityGlobal(constBody)
ddP_des1 = Ksc * (-(P_des - P_cur)) - Dsc * (-(dP_cur - dP_des))
r = -(P_des - P_cur)
I = np.vstack(([1, 0, 0], [0, 1, 0], [0, 0, 1]))
Z = np.hstack((I, mm.getCrossMatrixForm(-r)))
yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses,
[softConstPoint], constJointMasks)
JL, JA = np.vsplit(Jconst, 2)
Q1 = np.dot(Z, Jconst)
q1 = np.dot(JA, dth_flat)
q2 = np.dot(mm.getCrossMatrixForm(q1),
np.dot(mm.getCrossMatrixForm(q1), r))
yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions,
jointAxeses, linkAngVelocities,
[softConstPoint], constJointMasks,
False)
q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2
##############################
flagContact = True
if dH_des == None or np.any(np.isnan(dH_des)) == True:
flagContact = False
viewer.doc.showRenderer('rd_grf_des', False)
viewer.motionViewWnd.update(1, viewer.doc)
else:
viewer.doc.showRenderer('rd_grf_des', True)
viewer.motionViewWnd.update(1, viewer.doc)
'''
0 : initial
1 : contact
2 : fly
3 : landing
'''
#MOTION = FORWARD_JUMP
if mit.MOTION == mit.FORWARD_JUMP:
frame_index = [136, 100]
#frame_index = [100000, 100000]
elif mit.MOTION == mit.TAEKWONDO:
frame_index = [130, 100]
#frame_index = [100000, 100000]
elif mit.MOTION == mit.TAEKWONDO2:
frame_index = [130 + 40, 100]
else:
frame_index = [1000000, 1000000]
#MOTION = TAEKWONDO
#frame_index = [135, 100]
'''
if frame > 300 :
if stage != DYNAMIC_BALANCING:
print("#", frame,"-DYNAMIC_BALANCING")
stage = DYNAMIC_BALANCING
Kk = Kk*1
Dk = 2*(Kk**.5)
'''
if frame > frame_index[0]:
if stage != POWERFUL_BALANCING:
print("#", frame, "-POWERFUL_BALANCING")
stage = POWERFUL_BALANCING
Kk = Kk * 2
Dk = 2 * (Kk**.5)
elif frame > frame_index[1]:
if stage != MOTION_TRACKING:
print("#", frame, "-MOTION_TRACKING")
stage = MOTION_TRACKING
trackingW = w
if stage == MOTION_TRACKING:
trackingW = w2
Bt = Bt * 2
# optimization
mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat)
mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1,
q_bias1)
if flagContact == True:
if stage != MOTION_TRACKING:
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R,
r_bias)
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
a_sup_2 = [None]
Jsup_2 = [None]
dJsup_2 = [None]
##############################
# Hard constraint
if stage != MOTION_TRACKING:
Kk2 = Kk * 2.0
else:
Kk2 = Kk * 1.5
Dk2 = 2 * (Kk2**.5)
'''
desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, supL, ModelOffset, CM_ref, CM, Kk2, Dk2)
desLinearAccR, desPosR = getDesFootLinearAcc(motionModel, controlModel, supR, ModelOffset, CM_ref, CM, Kk2, Dk2)
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, supL, Kk2, Dk2)
desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, supR, Kk2, Dk2)
'''
if stage != MOTION_TRACKING:
idx = 0 #LEFT/RIGHT_TOES
desLinearAccL, desPosL = getDesFootLinearAcc(
motionModel, controlModel, indexFootL[idx], ModelOffset,
CM_ref, CM, Kk2, Dk2, 0.040)
desLinearAccR, desPosR = getDesFootLinearAcc(
motionModel, controlModel, indexFootR[idx], ModelOffset,
CM_ref, CM, Kk2, Dk2, 0.040)
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel,
indexFootL[idx], Kk2, Dk2)
desAngularAccR = getDesFootAngularAcc(motionModel, controlModel,
indexFootR[idx], Kk2, Dk2)
a_sup_2 = np.hstack((np.hstack((desLinearAccL, desAngularAccL)),
np.hstack((desLinearAccR, desAngularAccR))))
Jsup_2 = np.vstack((jFootL[idx], jFootR[idx]))
dJsup_2 = np.vstack((dJFootL[idx], dJFootR[idx]))
'''
idx = 0
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2)
desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[idx], Kk2, Dk2)
a_sup_2 = np.hstack(( desAngularAccL, desAngularAccR))
Jsup_2 = np.vstack((jAngFootL[idx], jAngFootR[idx]))
dJsup_2 = np.vstack((dJAngFootL[idx], dJAngFootR[idx]))
'''
rd_DesPosL[0] = desPosL.copy()
rd_DesPosR[0] = desPosR.copy()
else:
if footPartNum == 5:
idx = 3
desAngularAccL = getDesFootAngularAcc(motionModel,
controlModel,
indexFootL[idx], Kk2,
Dk2)
desAngularAccR = getDesFootAngularAcc(motionModel,
controlModel,
indexFootR[idx], Kk2,
Dk2)
a_sup_2 = np.hstack((desAngularAccL, desAngularAccR))
Jsup_2 = np.vstack((jAngFootL[idx], jAngFootR[idx]))
dJsup_2 = np.vstack((dJAngFootL[idx], dJAngFootR[idx]))
idx = 4
desAngularAccL = getDesFootAngularAcc(motionModel,
controlModel,
indexFootL[idx], Kk2,
Dk2)
desAngularAccR = getDesFootAngularAcc(motionModel,
controlModel,
indexFootR[idx], Kk2,
Dk2)
a_sup_2 = np.hstack(
(a_sup_2, np.hstack((desAngularAccL, desAngularAccR))))
Jsup_2 = np.vstack(
(Jsup_2, np.vstack((jAngFootL[idx], jAngFootR[idx]))))
dJsup_2 = np.vstack(
(dJsup_2, np.vstack((dJAngFootL[idx], dJAngFootR[idx]))))
else:
idx = 1
desAngularAccL = getDesFootAngularAcc(motionModel,
controlModel,
indexFootL[idx], Kk2,
Dk2)
desAngularAccR = getDesFootAngularAcc(motionModel,
controlModel,
indexFootR[idx], Kk2,
Dk2)
a_sup_2 = np.hstack((desAngularAccL, desAngularAccR))
Jsup_2 = np.vstack((jAngFootL[idx], jAngFootR[idx]))
dJsup_2 = np.vstack((dJAngFootL[idx], dJAngFootR[idx]))
##############################
##############################
# Additional constraint
if stage != MOTION_TRACKING:
#Kk2 = Kk * 2.5
Kk2 = Kk * 2.5
Dk2 = 2 * (Kk2**.5)
desForePosL = [0, 0, 0]
desForePosR = [0, 0, 0]
desRearPosL = [0, 0, 0]
desRearPosR = [0, 0, 0]
for i in range(1, footPartNum):
if i == i:
if contactFlagFootL[i] == 1:
desLinearAccL, desForePosL = getDesFootLinearAcc(
motionModel, controlModel, indexFootL[i],
ModelOffset, CM_ref, CM, Kk2, Dk2, 0.04)
desAngularAccL = getDesFootAngularAcc(
motionModel, controlModel, indexFootL[i], Kk2, Dk2)
a_sup_2 = np.hstack(
(a_sup_2, np.hstack(
(desLinearAccL, desAngularAccL))))
Jsup_2 = np.vstack((Jsup_2, jFootL[i]))
dJsup_2 = np.vstack((dJsup_2, dJFootL[i]))
if contactFlagFootR[i] == 1:
desLinearAccR, desForePosR = getDesFootLinearAcc(
motionModel, controlModel, indexFootR[i],
ModelOffset, CM_ref, CM, Kk2, Dk2, 0.04)
desAngularAccR = getDesFootAngularAcc(
motionModel, controlModel, indexFootR[i], Kk2, Dk2)
a_sup_2 = np.hstack(
(a_sup_2, np.hstack(
(desLinearAccR, desAngularAccR))))
Jsup_2 = np.vstack((Jsup_2, jFootR[i]))
dJsup_2 = np.vstack((dJsup_2, dJFootR[i]))
else:
if contactFlagFootL[i] == 1:
desAngularAccL = getDesFootAngularAcc(
motionModel, controlModel, indexFootL[i], Kk2, Dk2)
a_sup_2 = np.hstack((a_sup_2, desAngularAccL))
Jsup_2 = np.vstack((Jsup_2, jAngFootL[i]))
dJsup_2 = np.vstack((dJsup_2, dJAngFootL[i]))
if contactFlagFootR[i] == 1:
desAngularAccR = getDesFootAngularAcc(
motionModel, controlModel, indexFootR[i], Kk2, Dk2)
a_sup_2 = np.hstack((a_sup_2, desAngularAccR))
Jsup_2 = np.vstack((Jsup_2, jAngFootR[i]))
dJsup_2 = np.vstack((dJsup_2, dJAngFootR[i]))
'''
for i in range(1, footPartNum) :
if contactFlagFootL[i] == 1:
desLinearAccL, desForePosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[i], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.034)
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[i], Kk2, Dk2)
a_sup_2 = np.hstack(( a_sup_2, np.hstack((desLinearAccL, desAngularAccL)) ))
Jsup_2 = np.vstack(( Jsup_2, jFootL[i] ))
dJsup_2 = np.vstack(( dJsup_2, dJFootL[i] ))
if contactFlagFootR[i] == 1:
desLinearAccR, desForePosR = getDesFootLinearAcc(motionModel, controlModel, indexFootR[i], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.034)
desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[i], Kk2, Dk2)
a_sup_2 = np.hstack(( a_sup_2, np.hstack((desLinearAccR, desAngularAccR)) ))
Jsup_2 = np.vstack(( Jsup_2, jFootR[i] ))
dJsup_2 = np.vstack(( dJsup_2, dJFootR[i] ))
'''
rd_DesForePosL[0] = desForePosL
rd_DesForePosR[0] = desForePosR
rd_DesRearPosL[0] = desRearPosL
rd_DesRearPosR[0] = desRearPosR
##############################
mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat,
a_sup_2)
r = problem.solve()
problem.clear()
ype.nested(r['x'], ddth_sol)
rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
for i in range(stepsPerFrame):
# apply penalty force
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
extraForce[0] = viewer.GetForce()
if (extraForce[0][0] != 0 or extraForce[0][1] != 0
or extraForce[0][2] != 0):
forceApplyFrame += 1
#vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
controlModel.applyBodyForceGlobal(selectedBody, extraForce[0])
applyedExtraForce[0] = extraForce[0]
if forceApplyFrame * wcfg.timeStep > 0.1:
viewer.ResetForce()
forceApplyFrame = 0
controlModel.setDOFAccelerations(ddth_sol)
controlModel.solveHybridDynamics()
'''
extraForce[0] = viewer.GetForce()
if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0) :
forceApplyFrame += 1
vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
applyedExtraForce[0] = extraForce[0]
if forceApplyFrame*wcfg.timeStep > 0.1:
viewer.ResetForce()
forceApplyFrame = 0
'''
vpWorld.step()
# rendering
rd_footCenter[0] = footCenter
rd_CM[0] = CM.copy()
rd_CM_plane[0] = CM_plane.copy()
rd_footCenter_ref[0] = footCenter_ref
rd_CM_plane_ref[0] = CM_ref.copy()
rd_CM_ref[0] = CM_ref.copy()
rd_CM_ref_vec[0] = (CM_ref - footCenter_ref) * 3.
rd_CM_vec[0] = (CM - footCenter) * 3
#rd_CM_plane[0][1] = 0.
if CP != None and dCP != None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des
rd_dL_des_plane[0] = dL_des_plane
rd_dH_des[0] = dH_des
rd_grf_des[
0] = totalNormalForce # - totalMass*mm.s2v(wcfg.gravity)#dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
rd_exf_des[0] = applyedExtraForce[0]
rd_root_des[0] = rootPos[0]
rd_CMP[0] = softConstPoint
rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal(
constBody) - softConstPoint
if (forceApplyFrame == 0):
applyedExtraForce[0] = [0, 0, 0]
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 60.)
viewer.show()
Fl.run()
def main():
np.set_printoptions(precision=4, linewidth=200)
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5()
motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped()
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
ModelOffset = (1.5, -0.02, 0)
#ModelOffset = (1.5, 1.02, 0)
#ModelOffset = (1.5, 0.02, 0)
controlModel.translateByOffset(ModelOffset)
#controlModel.translateByOffset((1.5,-0.0328,0))#(1.5,-0.02,0))
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
# parameter
Kt = config['Kt']; Dt = config['Dt'] # tracking gain
Kl = config['Kl']; Dl = config['Dl'] # linear balance gain
Kh = config['Kh']; Dh = config['Dh'] # angular balance gain
Ks = config['Ks']; Ds = config['Ds'] # penalty force spring gain
Bt = config['Bt']
Bl = config['Bl']
Bh = config['Bh']
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
w_IK = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['IKweightMap'])
supL = motion[0].skeleton.getJointIndex(config['supLink'])
supR = motion[0].skeleton.getJointIndex(config['supLink2'])
#controlModel.SetGround(supL, True)
#controlModel.SetGround(supR, True)
selectedBody = motion[0].skeleton.getJointIndex(config['end'])
#constBody = motion[0].skeleton.getJointIndex('LeftForeArm')
constBody = motion[0].skeleton.getJointIndex('Hips')
# jacobian
JsupL = yjc.makeEmptyJacobian(DOFs, 1)
dJsupL = JsupL.copy()
JsupPreL = JsupL.copy()
JsupR = yjc.makeEmptyJacobian(DOFs, 1)
dJsupR = JsupR.copy()
JsupPreR = JsupR.copy()
Jsup = yjc.makeEmptyJacobian(DOFs, 1)
dJsup = Jsup.copy()
JsupPre = Jsup.copy()
Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum())
dJsys = Jsys.copy()
JsysPre = Jsys.copy()
Jconst = yjc.makeEmptyJacobian(DOFs, 1)
dJconst = Jconst.copy()
supLJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, supL)]
supRJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, supR)]
constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)]
allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton)
# momentum matrix
linkMasses = controlModel.getBodyMasses()
totalMass = controlModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
problem = yac.LSE(totalDOF, 6)
a_sup = (0,0,0, 0,0,0) #L
#a_sup2 = (0,0,0, 0,0,0)#R
a_sup2 = [0,0,0, 0,0,0]#R
a_sup_2 = [0,0,0, 0,0,0, 0,0,0, 0,0,0]
CP_old = [mm.v3(0.,0.,0.)]
# penalty method
bodyIDsToCheck = range(vpWorld.getBodyNum())
mus = [1.]*len(bodyIDsToCheck)
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
d_th_IK = ype.makeNestedList(DOFs)
d_th_IK_L = ype.makeNestedList(DOFs)
d_th_IK_R = ype.makeNestedList(DOFs)
dd_th_IK = ype.makeNestedList(DOFs)
dd_th_IK_flat = ype.makeFlatList(totalDOF)
d_th_IK_flat = ype.makeFlatList(totalDOF)
ddth_c_flat = ype.makeFlatList(totalDOF)
# viewer
rd_footCenter = [None]
rd_footCenter_ref = [None]
rd_footCenterL = [None]
rd_footCenterR = [None]
rd_CM_plane = [None]
rd_CM_plane_ref = [None]
rd_CM = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_exf_des = [None]
rd_root_des = [None]
rd_soft_const_vec = [None]
rd_CMP = [None]
rd_DesPosL = [None]
rd_DesPosR = [None]
rootPos = [None]
selectedBodyId = [selectedBody]
extraForce = [None]
applyedExtraForce = [None]
applyedExtraForce[0] = [0,0,0]
viewer = ysv.SimpleViewer()
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('motionModel', cvr.VpModelRenderer(motionModel, (150,150,255), yr.POLYGON_FILL))
viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL))
viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
#viewer.doc.addRenderer('rd_footCenterL', yr.PointsRenderer(rd_footCenterL))
#viewer.doc.addRenderer('rd_footCenterR', yr.PointsRenderer(rd_footCenterR))
viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255,255,0)))
viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0,255,0)))
#viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255,0,255)))
# viewer.doc.addRenderer('rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255,255,0)))
# viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0,255,0)))
viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,255), .001))
viewer.doc.addRenderer('rd_exf_des', yr.ForcesRenderer(rd_exf_des, rd_root_des, (0,255,0), .009, 0.05))
#viewer.doc.addRenderer('rd_CMP', yr.PointsRenderer(rd_CMP, (0,0,255)))
viewer.doc.addRenderer('rd_DesPosL', yr.PointsRenderer(rd_DesPosL, (0,0,255)))
viewer.doc.addRenderer('rd_DesPosR', yr.PointsRenderer(rd_DesPosR, (0,100,255)))
#viewer.doc.addRenderer('softConstraint', yr.VectorsRenderer(rd_soft_const_vec, rd_CMP, (255,0,0), 3))
viewer.doc.addRenderer('rd_footCenter_ref', yr.PointsRenderer(rd_footCenter_ref))
viewer.doc.addRenderer('rd_CM_plane_ref', yr.PointsRenderer(rd_CM_plane_ref, (255,255,0)))
stage = 0
def simulateCallback(frame):
global g_initFlag
global preFootCenterL, preFootCenterR
global preFootOrientationL, preFootOrientationR
global forceShowFrame
global forceApplyFrame
global JsysPre
global JsupPreL
global JsupPreR
global JsupPre
global softConstPoint
global stage
motionModel.update(motion[frame])
Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, Bsc = viewer.GetParam()
if stage == 3:
Bsc = 0
#Kl *= 1.5
Dt = 2*(Kt**.5)
Dk = 2*(Kk**.5)
Dl = 2*(Kl**.5)
Dh = 2*(Kh**.5)
Dsc = 2*(Ksc**.5)
if Bsc == 0.0 :
viewer.doc.showRenderer('softConstraint', False)
viewer.motionViewWnd.update(1, viewer.doc)
else:
viewer.doc.showRenderer('softConstraint', True)
renderer1 = viewer.doc.getRenderer('softConstraint')
renderer1.rc.setLineWidth(0.1+Bsc*3)
viewer.motionViewWnd.update(1, viewer.doc)
# tracking
th_r = motion.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
ddth_c = controlModel.getDOFAccelerations()
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
ype.flatten(ddth_c, ddth_c_flat)
# jacobian
footCenterL = controlModel.getBodyPositionGlobal(supL)
footCenterR = controlModel.getBodyPositionGlobal(supR)
refFootL = motionModel.getBodyPositionGlobal(supL)
refFootR = motionModel.getBodyPositionGlobal(supR)
footCenter = footCenterL + (footCenterR - footCenterL)/2.0
footCenter[1] = 0.
footCenter_ref = refFootL + (refFootR - refFootL)/2.0
footCenter_ref[1] = 0.
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
jointPositions = controlModel.getJointPositionsGlobal()
jointAxeses = controlModel.getDOFAxeses()
CM = yrp.getCM(linkPositions, linkMasses, totalMass)
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
CM_plane = copy.copy(CM); CM_plane[1]=0.
dCM_plane = copy.copy(dCM); dCM_plane[1]=0.
linkPositions_ref = motionModel.getBodyPositionsGlobal()
CM_plane_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass)
CM_plane_ref[1] = 0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias)
yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks)
dJsys = (Jsys - JsysPre)/(1/30.)
JsysPre = Jsys
#yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks)
if g_initFlag == 0:
preFootCenterL = footCenterL
preFootCenterR = footCenterR
preFootCenterL[1] -= 0.02
preFootCenterR[1] -= 0.02
preFootOrientationL = controlModel.getBodyOrientationGlobal(supL)
preFootOrientationR = controlModel.getBodyOrientationGlobal(supR)
softConstPoint = controlModel.getBodyPositionGlobal(constBody)
#softConstPoint[2] += 0.3
#softConstPoint[1] -= 1.1
#softConstPoint[0] += 0.1
softConstPoint[1] -= .3
#softConstPoint[0] -= .1
#softConstPoint[1] -= 1.
#softConstPoint[0] -= .5
g_initFlag = 1
yjc.computeJacobian2(JsupL, DOFs, jointPositions, jointAxeses, [footCenterL], supLJointMasks)
dJsupL = (JsupL - JsupPreL)/(1/30.)
JsupPreL = JsupL
#yjc.computeJacobianDerivative2(dJsupL, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterL], supLJointMasks, False)
yjc.computeJacobian2(JsupR, DOFs, jointPositions, jointAxeses, [footCenterR], supRJointMasks)
dJsupR = (JsupR - JsupPreR)/(1/30.)
JsupPreR = JsupR
#yjc.computeJacobianDerivative2(dJsupR, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterR], supRJointMasks, False)
preFootCenter = preFootCenterL + (preFootCenterR - preFootCenterL)/2.0
preFootCenter[1] = 0
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
# linear momentum
CM_ref_plane = footCenter
#CM_ref_plane = preFootCenter
dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane
#print("dL_des_plane ", dL_des_plane )
#dL_des_plane[1] = 0.
# angular momentum
CP_ref = footCenter
timeStep = 30.
if CP_old[0]==None or CP==None:
dCP = None
else:
dCP = (CP - CP_old[0])/(1/timeStep)
CP_old[0] = CP
if CP!=None and dCP!=None:
ddCP_des = Kh*(CP_ref - CP) - Dh*(dCP)
CP_des = CP + dCP*(1/timeStep) + .5*ddCP_des*((1/timeStep)**2)
dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity)))
#dH_des = np.cross((CP_des - CM_plane), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity)))
#dH_des = [0, 0, 0]
else:
dH_des = None
CMP = yrp.getCMP(contactForces, CM)
r = [0,0,0]
if CP!= None and np.any(np.isnan(CMP))!=True :
r = CP - CMP
#print("r.l", mm.length(r))
#Bba = Bh*(mm.length(r))
Bba = Bh
# momentum matrix
RS = np.dot(P, Jsys)
R, S = np.vsplit(RS, 2)
rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat)
r_bias, s_bias = np.hsplit(rs, 2)
##############################
# soft point constraint
'''
cmDiff = footCenter - CM_plane
print("cmDiff", cmDiff)
if stage == 3:
softConstPoint +=
'''
P_des = softConstPoint
P_cur = controlModel.getBodyPositionGlobal(constBody)
dP_des = [0, 0, 0]
dP_cur = controlModel.getBodyVelocityGlobal(constBody)
ddP_des1 = Ksc*(P_des - P_cur) - Dsc*(dP_cur - dP_des)
r = P_des - P_cur
I = np.vstack(([1,0,0],[0,1,0],[0,0,1]))
Z = np.hstack((I, mm.getCrossMatrixForm(-r)))
yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks)
JL, JA = np.vsplit(Jconst, 2)
Q1 = np.dot(Z, Jconst)
q1 = np.dot(JA, dth_flat)
q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r))
yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False)
q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2
'''
P_des = preFootCenterR
P_cur = controlModel.getBodyPositionGlobal(supR)
P_cur[1] = 0
dP_des = [0, 0, 0]
dP_cur = controlModel.getBodyVelocityGlobal(supR)
ddP_des2 = Kp*(P_des - P_cur) - Dp*(dP_cur - dP_des)
r = P_des - P_cur
#print("r2", r)
I = np.vstack(([1,0,0],[0,1,0],[0,0,1]))
Z = np.hstack((I, mm.getCrossMatrixForm(-r)))
JL, JA = np.vsplit(JsupR, 2)
Q2 = np.dot(Z, JsupR)
q1 = np.dot(JA, dth_flat)
q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r))
q_bias2 = np.dot(np.dot(Z, dJsupR), dth_flat) + q2
'''
#print("Q1", Q1)
'''
print("ddP_des1", ddP_des1)
q_ddth1 = np.dot(Q1, ddth_c_flat)
print("q_ddth1", q_ddth1)
print("q_bias1", q_bias1)
ddp1 = q_ddth1+q_bias1
print("ddp1", ddp1)
print("diff1", ddP_des1-ddp1)
'''
'''
print("ddP_des2", ddP_des2)
q_ddth2 = np.dot(Q2, ddth_c_flat)
print("q_ddth2", q_ddth2)
print("q_bias2", q_bias2)
ddp2 = q_ddth2+q_bias2
print("ddp2", ddp2)
print("diff2", ddP_des2-ddp2)
'''
##############################
############################
# IK
'''
P_des = preFootCenterL
P_cur = controlModel.getJointPositionGlobal(supL)
r = P_des - P_cur
Q_des = preFootOrientationL
Q_cur = controlModel.getJointOrientationGlobal(supL)
rv = mm.logSO3(np.dot(Q_cur.transpose(), Q_des))
#print("rv", rv)
des_v_sup = (r[0],r[1],r[2], rv[0], rv[1], rv[2])
A_large = np.dot(JsupL.T, JsupL)
b_large = np.dot(JsupL.T, des_v_sup)
des_d_th = npl.lstsq(A_large, b_large)
ype.nested(des_d_th[0], d_th_IK_L)
P_des2 = preFootCenterR
P_cur2 = controlModel.getJointPositionGlobal(supR)
r2 = P_des2 - P_cur2
Q_des2 = preFootOrientationR
Q_cur2 = controlModel.getJointOrientationGlobal(supR)
rv2 = mm.logSO3(np.dot(Q_cur2.transpose(), Q_des2))
#print("Q_des2", Q_des2)
#print("Q_cur2", Q_cur2)
#print("rv2", rv2)
des_v_sup2 = (r2[0],r2[1],r2[2], rv2[0], rv2[1], rv[2])
A_large = np.dot(JsupR.T, JsupR)
b_large = np.dot(JsupR.T, des_v_sup2)
des_d_th = npl.lstsq(A_large, b_large)
ype.nested(des_d_th[0], d_th_IK_R)
for i in range(len(d_th_IK_L)):
for j in range(len(d_th_IK_L[i])):
d_th_IK[i][j] = d_th_IK_L[i][j] + d_th_IK_R[i][j]
th_IK = yct.getIntegralDOF(th, d_th_IK, 1/timeStep)
dd_th_IK = yct.getDesiredDOFAccelerations(th_IK, th, d_th_IK, dth, ddth_r, Kk, Dk)
ype.flatten(d_th_IK, d_th_IK_flat)
ype.flatten(dd_th_IK, dd_th_IK_flat)
'''
############################
flagContact = True
if dH_des==None or np.any(np.isnan(dH_des)) == True:
flagContact = False
'''
0 : initial
1 : contact
2 : fly
3 : landing
'''
if flagContact == False :
if stage == 1:
stage = 2
print("fly")
else:
if stage == 0:
stage = 1
print("contact")
elif stage == 2:
stage = 3
print("landing")
if stage == 3:
Bt = Bt*0.8
Bl = Bl*1
# optimization
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
#mot.addTrackingTerms(problem, totalDOF, Bk, w_IK, dd_th_IK_flat)
mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
#mot.addSoftPointConstraintTerms(problem, totalDOF, Bp, ddP_des2, Q2, q_bias2)
#mot.addConstraint(problem, totalDOF, JsupL, dJsupL, dth_flat, a_sup)
#mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_sup2)
desLinearAccL = [0,0,0]
desAngularAccL = [0,0,0]
desLinearAccR = [0,0,0]
desAngularAccR = [0,0,0]
refPos = motionModel.getBodyPositionGlobal(supL)
refPos[0] += ModelOffset[0]
refPos[1] = 0
refVel = motionModel.getBodyVelocityGlobal(supL)
curPos = controlModel.getBodyPositionGlobal(supL)
#curPos[1] = 0
curVel = controlModel.getBodyVelocityGlobal(supL)
refAcc = (0,0,0)
if stage == 3:
refPos = curPos
refPos[1] = 0
if curPos[1] < 0.0:
curPos[1] = 0
else :
curPos[1] = 0
rd_DesPosL[0] = refPos
#(p_r, p, v_r, v, a_r, Kt, Dt)
desLinearAccL = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk)
#desLinearAccL[1] = 0
refPos = motionModel.getBodyPositionGlobal(supR)
refPos[0] += ModelOffset[0]
refPos[1] = 0
refVel = motionModel.getBodyVelocityGlobal(supR)
curPos = controlModel.getBodyPositionGlobal(supR)
#curPos[1] = 0
curVel = controlModel.getBodyVelocityGlobal(supR)
if stage == 3:
refPos = curPos
refPos[1] = 0
if curPos[1] < 0.0:
curPos[1] = 0
else :
curPos[1] = 0
rd_DesPosR[0] = refPos
desLinearAccR = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk)
#desLinearAccR[1] = 0
#(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
refAng = [preFootOrientationL]
curAng = [controlModel.getBodyOrientationGlobal(supL)]
refAngVel = motionModel.getBodyAngVelocityGlobal(supL)
curAngVel = controlModel.getBodyAngVelocityGlobal(supL)
refAngAcc = (0,0,0)
#desAngularAccL = yct.getDesiredAngAccelerations(refAng, curAng, refAngVel, curAngVel, refAngAcc, Kk, Dk)
curAngY = np.dot(curAng, np.array([0,1,0]))
aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], np.array([0,1,0])))
print("curAngYL=",curAngY, "aL=", aL)
desAngularAccL = [Kk*aL + Dk*(refAngVel-curAngVel)]
refAng = [preFootOrientationR]
curAng = [controlModel.getBodyOrientationGlobal(supR)]
refAngVel = motionModel.getBodyAngVelocityGlobal(supR)
curAngVel = controlModel.getBodyAngVelocityGlobal(supR)
refAngAcc = (0,0,0)
#desAngularAccR = yct.getDesiredAngAccelerations(refAng, curAng, refAngVel, curAngVel, refAngAcc, Kk, Dk)
curAngY = np.dot(curAng, np.array([0,1,0]))
aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], np.array([0,1,0])))
desAngularAccR = [Kk*aL + Dk*(refAngVel-curAngVel)]
print("curAngYR=",curAngY, "aL=", aL)
a_sup_2 = [desLinearAccL[0], desLinearAccL[1], desLinearAccL[2], desAngularAccL[0][0], desAngularAccL[0][1], desAngularAccL[0][2],
desLinearAccR[0], desLinearAccR[1], desLinearAccR[2], desAngularAccR[0][0], desAngularAccR[0][1], desAngularAccR[0][2]]
if stage == 2 :#or stage == 3:
refAccL = motionModel.getBodyAccelerationGlobal(supL)
refAndAccL = motionModel.getBodyAngAccelerationGlobal(supL)
refAccR = motionModel.getBodyAccelerationGlobal(supR)
refAndAccR = motionModel.getBodyAngAccelerationGlobal(supR)
a_sup_2 = [refAccL[0], refAccL[1], refAccL[2], refAndAccL[0], refAndAccL[1], refAndAccL[2],
refAccR[0], refAccR[1], refAccR[2], refAndAccR[0], refAndAccR[1], refAndAccR[2]]
'''
a_sup_2 = [0,0,0, desAngularAccL[0][0], desAngularAccL[0][1], desAngularAccL[0][2],
0,0,0, desAngularAccR[0][0], desAngularAccR[0][1], desAngularAccR[0][2]]
'''
Jsup_2 = np.vstack((JsupL, JsupR))
dJsup_2 = np.vstack((dJsupL, dJsupR))
if flagContact == True:
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2)
#mot.setConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_sup2)
#mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2)
#mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, d_th_IK_flat, a_sup_2)
'''
jZ = np.dot(dJsup_2.T, dJsup_2)
lamda = 0.001
for i in range(len(jZ)):
for j in range(len(jZ[0])):
if i == j :
jZ[i][j] += lamda
jZInv = npl.pinv(jZ)
jA = np.dot(Jsup_2, np.dot(jZInv, np.dot(dJsup_2.T, -Jsup_2)))
mot.addConstraint2(problem, totalDOF, jA, a_sup_2)
'''
r = problem.solve()
problem.clear()
ype.nested(r['x'], ddth_sol)
rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
for i in range(stepsPerFrame):
# apply penalty force
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces)
controlModel.setDOFAccelerations(ddth_sol)
controlModel.solveHybridDynamics()
extraForce[0] = viewer.GetForce()
if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0) :
forceApplyFrame += 1
vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
applyedExtraForce[0] = extraForce[0]
if forceApplyFrame*wcfg.timeStep > 0.1:
viewer.ResetForce()
forceApplyFrame = 0
vpWorld.step()
# rendering
rd_footCenter[0] = footCenter
rd_footCenterL[0] = preFootCenterL
rd_footCenterR[0] = preFootCenterR
rd_CM[0] = CM
rd_CM_plane[0] = CM_plane.copy()
rd_footCenter_ref[0] = footCenter_ref
rd_CM_plane_ref[0] = CM_plane_ref.copy()
#rd_CM_plane[0][1] = 0.
if CP!=None and dCP!=None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des
rd_dL_des_plane[0] = dL_des_plane
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
rd_exf_des[0] = applyedExtraForce[0]
#print("rd_exf_des", rd_exf_des[0])
rd_root_des[0] = rootPos[0]
rd_CMP[0] = softConstPoint
rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal(constBody)-softConstPoint
#if (applyedExtraForce[0][0] != 0 or applyedExtraForce[0][1] != 0 or applyedExtraForce[0][2] != 0) :
if (forceApplyFrame == 0) :
applyedExtraForce[0] = [0, 0, 0]
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/60.)
viewer.show()
Fl.run()
'lPos.y',
[motion.getPosition(LFOOT, i)[1] for i in range(len(motion))], False)
plot.addYdata(
'rPos.y',
[motion.getPosition(RFOOT, i)[1] for i in range(len(motion))], False)
plot.addYlines('hRef', [hRef], False)
plot.addYdata('lVel.length', [
mmMath.length(motion.getVelocity(LFOOT, i)) for i in range(len(motion))
], False)
plot.addYdata('rVel.length', [
mmMath.length(motion.getVelocity(RFOOT, i)) for i in range(len(motion))
], False)
plot.addYlines('vRef', [vRef], False)
plot.showModeless()
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion', yr.JointMotionRenderer(motion, (100, 255, 100),
yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
# lSwingRenderer = yr.JointMotionRenderer(motion, (255,153,0), yr.LINK_BONE)
# lSwingRenderer.renderFrames = [i for i in range(len(motion)) if lFootStates[i]==False]
# viewer.doc.addRenderer('lSwing', lSwingRenderer)
#
# rSwingRenderer = yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE)
# rSwingRenderer.renderFrames = [i for i in range(len(motion)) if rFootStates[i]==False]
# viewer.doc.addRenderer('rSwing', rSwingRenderer)
llr = yr.JointMotionRenderer(motion, (255, 0, 0), yr.LINK_BONE)
def test_getStitchedNextMotion2():
bvhFilePath = '../samples/walk_left_90degree.bvh'
motion = yf.readBvhFile(bvhFilePath)
lFoot = motion[0].skeleton.getElementIndex('LeftFoot')
rFoot = motion[0].skeleton.getElementIndex('RightFoot')
hRef = .1
vRef = .3
lc = yma.getElementContactStates(motion, lFoot, hRef, vRef)
rc = yma.getElementContactStates(motion, rFoot, hRef, vRef)
steps = yba.getWalkingSteps(lc, rc, True)
stepMotions = yma.splitMotionIntoSegments(motion, steps)
print(steps)
earlyCycle = 3
howEarly = 10
transitionFunc = lambda x: 1. - yfg.hermite2nd(x)
print('steps[%d] :' % earlyCycle)
print(
'motion[%d]~motion[%d] => ' %
(steps[earlyCycle][0], steps[earlyCycle][1]), )
print('motion_stitched_trans_rot[%d]~motion_stitched_trans_rot[%d]' %
(steps[earlyCycle][0], steps[earlyCycle][1] - howEarly))
motion_stitched_trans = stepMotions[0].copy()
motion_stitched_trans_rot = stepMotions[0].copy()
motion_stitched_d = stepMotions[0].copy()
motion_attached = stepMotions[0].copy()
for i in range(1, len(stepMotions)):
if i == earlyCycle:
stepMotions[i] = stepMotions[i][:-howEarly]
# motion_stitched_trans += getStitchedNextMotion(stepMotions[i], motion_stitched_trans[-1], len(stepMotions[i])-1, transitionFunc, True, False)
motion_stitched_trans_rot += getStitchedNextMotion(
stepMotions[i].copy(), motion_stitched_trans_rot[-1],
len(stepMotions[i]) - 1, transitionFunc, True, True)
# d = motion_stitched_d[-1] - stepMotions[i][0]
# motion_stitched_d += getStitchedNextMotion(stepMotions[i], d, len(stepMotions[i])-1, transitionFunc, True, True)
# motion_attached += getStitchedNextMotion(stepMotions[i], motion_attached[-1], len(stepMotions[i])-1, yfg.zero, True, True)
motion_attached += getAttachedNextMotion(stepMotions[i],
motion_attached[-1],
len(stepMotions[i]) - 1,
yfg.zero, True, True)
frame0 = [mm.I_SE3()]
viewer = ysv.SimpleViewer()
viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (100,100,255), yr.LINK_LINE))
# viewer.doc.addObject('motion', motion)
# viewer.doc.addRenderer('motion_stitched_trans', yr.JointMotionRenderer(motion_stitched_trans, (255,255,0), yr.LINK_LINE))
# viewer.doc.addObject('motion_stitched_trans', motion_stitched_trans)
viewer.doc.addRenderer(
'motion_stitched_trans_rot',
yr.JointMotionRenderer(motion_stitched_trans_rot, (0, 255, 0),
yr.LINK_LINE))
viewer.doc.addObject('motion_stitched_trans_rot',
motion_stitched_trans_rot)
viewer.doc.addRenderer(
'motion_attached',
yr.JointMotionRenderer(motion_attached, (255, 255, 255),
yr.LINK_LINE))
viewer.doc.addObject('motion_attached', motion_attached)
viewer.doc.addRenderer('frame0',
yr.FramesRenderer(frame0, (255, 255, 0)))
def simulateCallback(frame):
frame0[0] = motion_stitched_trans_rot[frame].getJointFrame(0)
# frame0[0] = motion_attached[frame].getJointFrame(0)
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer((1 / 30.) / 1.4)
viewer.show()
Fl.run()
def save_simulation():
class ForceInfo:
def __init__(self, startFrame, duration, force):
self.startFrame = startFrame # frame
self.duration = duration # sec
self.force = force # Newton
self.targetBody = None
#===============================================================================
# load motion
#===============================================================================
MULTI_VIEWER = True
CAMERA_TRACKING = True
TORQUE_PLOT = False
NO_FOOT_SLIDING = True
SAVE_SIMULATION = True
SAVE_DIR = './saved_simulation/'
# global parameters
Kt = 20.
Dt = 2 * (Kt**.5)
Ks = 2000.
Ds = 2 * (Ks**.5)
K_stb_vel = .1
mu = 1.
# constaants
c_min_contact_vel = 100.
# c_min_contact_vel = 2.
c_min_contact_time = .7
c_landing_duration = .2
c_taking_duration = .3
c_swf_mid_offset = .0
c_swf_stability = .5
c_locking_vel = .05
dir = './ppmotion/'
K_swp_vel_sag = .1
K_swp_vel_cor = .4
K_swp_pos_sag = .3
K_swp_pos_cor = 0.
# K_stp_pos = 0.
# K_stb_vel = .2
K_stp_pos = 1.
filename = 'wd2_WalkSameSame01.bvh'
# filename = 'wd2_WalkSameSame01_REPEATED.bvh'
# K_swp_vel_sag = .1; K_swp_vel_cor = .4; K_swp_pos_sag = .3; K_swp_pos_cor = 0.
# K_stp_pos = 1.
# filename = 'wd2_WalkForwardSlow01.bvh'
## filename = 'wd2_WalkForwardSlow01_REPEATED.bvh' # 3 frame diff
## mu = 12.
# K_swp_vel_sag = .1; K_swp_vel_cor = .4; K_swp_pos_sag = 1.; K_swp_pos_cor = 0.
# K_stp_pos = .6
# filename = 'wd2_WalkForwardNormal00.bvh'
## filename = 'wd2_WalkForwardNormal00_REPEATED.bvh'
# K_swp_vel_sag = .1; K_swp_vel_cor = .4; K_swp_pos_sag = .3; K_swp_pos_cor = 0.
# K_stp_pos = 0.
# filename = 'wd2_WalkHandWav00.bvh'
## filename = 'wd2_WalkHandWav00_REPEATED.bvh'
# mu = 2.
# K_swp_vel_sag = .1; K_swp_vel_cor = .4; K_swp_pos_sag = .3; K_swp_pos_cor = 0.
# K_stp_pos = 0.
## filename = 'wd2_WalkForwardFast00.bvh'
### filename = 'wd2_WalkForwardFast00_REPEATED.bvh'
# mu = 2.
# K_swp_vel_sag = .1; K_swp_vel_cor = .4; K_swp_pos_sag = .3; K_swp_pos_cor = 0.
# K_stp_pos = 0.
# filename = 'wd2_WalkAzuma01.bvh'
## filename = 'wd2_WalkAzuma01_REPEATED.bvh' # 2 frame diff
# K_swp_vel_sag = .1; K_swp_vel_cor = .4; K_swp_pos_sag = 1.; K_swp_pos_cor = 0.
# K_stp_pos = 0.
# filename = 'wd2_WalkSoldier00.bvh' # K_swp_pos_sag = .0
## filename = 'wd2_WalkSoldier00_REPEATED.bvh'
# mu = 2.
# K_swp_vel_sag = .2; K_swp_vel_cor = .4; K_swp_pos_sag = .5;K_swp_pos_cor = 0.
# K_stp_pos = 0.
# filename = 'wd2_WalkForwardVFast00.bvh'
## filename = 'wd2_WalkForwardVFast00_REPEATED.bvh'
# K_swp_vel_sag = .1; K_swp_vel_cor = .4; K_swp_pos_sag = 1.; K_swp_pos_cor = 0.
# K_stp_pos = 0.
# K_stb_vel = .2
# filename = 'wd2_WalkBackward00.bvh'
### filename = 'wd2_WalkBackward00_REPEATED.bvh'
## K_swp_vel_sag = .15; K_swp_vel_cor = .35; K_swp_pos_sag = 1.; K_swp_pos_cor = 0.
## K_stp_pos = 0.
### filename = 'wd2_WalkLean00.bvh'
## filename = 'wd2_WalkLean00_REPEATED.bvh'
motion_ori = yf.readBvhFile(dir + filename)
frameTime = 1 / motion_ori.fps
if 'REPEATED' in filename:
REPEATED = True
CAMERA_TRACKING = True
else:
REPEATED = False
#===============================================================================
# options
#===============================================================================
SEGMENT_EDITING = True
STANCE_FOOT_STABILIZE = True
MATCH_STANCE_LEG = True
SWING_FOOT_PLACEMENT = True
SWING_FOOT_HEIGHT = True
SWING_FOOT_ORIENTATION = False
STANCE_FOOT_PUSH = True
STANCE_FOOT_BALANCING = True
stitch_func = lambda x: 1. - yfg.hermite2nd(x)
stf_stabilize_func = yfg.concatenate([yfg.hermite2nd, yfg.one],
[c_landing_duration])
match_stl_func = yfg.hermite2nd
# match_stl_func_y = yfg.hermite2nd
swf_placement_func = yfg.hermite2nd
swf_height_func = yfg.hermite2nd
swf_height_sine_func = yfg.sine
stf_balancing_func = yfg.concatenate([yfg.hermite2nd, yfg.one],
[c_landing_duration])
# forceInfos = [ForceInfo(70, .4, (100,0,0))]
forceInfos = []
#===============================================================================
# initialize character
#===============================================================================
mcfgfile = open(dir + 'mcfg', 'r')
mcfg = cPickle.load(mcfgfile)
mcfgfile.close()
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
wcfg.lockingVel = c_locking_vel
stepsPerFrame = 30
wcfg.timeStep = (frameTime) / stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion_ori[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion_ori[0], mcfg)
vpWorld.initialize()
print controlModel
motionModel.recordVelByFiniteDiff()
controlModel.initializeHybridDynamics()
#===============================================================================
# load segment info
#===============================================================================
skeleton = motion_ori[0].skeleton
segname = os.path.splitext(filename)[0] + '.seg'
segfile = open(dir + segname, 'r')
seginfo = cPickle.load(segfile)
segfile.close()
intervals = [info['interval'] for info in seginfo]
states = [info['state'] for info in seginfo]
temp_motion = copy.deepcopy(motion_ori)
segments = yma.splitMotionIntoSegments(temp_motion, intervals)
print len(intervals), 'segments'
for i in range(len(intervals)):
print '%dth' % i, yba.GaitState.text[states[i]], intervals[i], ',',
print
motion_seg_orig = ym.JointMotion()
motion_seg_orig += segments[0]
motion_seg = ym.JointMotion()
motion_seg += segments[0]
motion_stitch = ym.JointMotion()
motion_stitch += segments[0]
motion_stf_stabilize = ym.JointMotion()
motion_match_stl = ym.JointMotion()
motion_swf_placement = ym.JointMotion()
motion_swf_height = ym.JointMotion()
motion_swf_orientation = ym.JointMotion()
motion_stf_balancing = ym.JointMotion()
motion_stf_push = ym.JointMotion()
motion_control = ym.JointMotion()
motion_debug1 = ym.JointMotion()
motion_debug2 = ym.JointMotion()
motion_debug3 = ym.JointMotion()
P = ym.JointMotion()
P_hat = ym.JointMotion()
M_tc = ym.JointMotion()
M_hat_tc_1 = ym.JointMotion()
if SAVE_SIMULATION:
motion_simulation = ym.JointMotion()
#===============================================================================
# loop variable
#===============================================================================
seg_index = [0]
acc_offset = [0]
extended = [False]
prev_R_swp = [None]
stl_y_limit_num = [0]
stl_xz_limit_num = [0]
avg_dCM = [mm.O_Vec3()]
# avg_stf_v = [mm.O_Vec3()]
# avg_stf_av = [mm.O_Vec3()]
# stf_push_func = [yfg.zero]
step_length_cur = [0.]
step_length_tar = [0.]
step_axis = [mm.O_Vec3()]
#===============================================================================
# information
#===============================================================================
bodyIDsToCheck = range(vpWorld.getBodyNum())
mus = [mu] * len(bodyIDsToCheck)
bodyMasses = controlModel.getBodyMasses()
totalMass = controlModel.getTotalMass()
lID = controlModel.name2id('LeftFoot')
rID = controlModel.name2id('RightFoot')
lUpLeg = skeleton.getJointIndex('LeftUpLeg')
rUpLeg = skeleton.getJointIndex('RightUpLeg')
lKnee = skeleton.getJointIndex('LeftLeg')
rKnee = skeleton.getJointIndex('RightLeg')
lFoot = skeleton.getJointIndex('LeftFoot')
rFoot = skeleton.getJointIndex('RightFoot')
spine = skeleton.getJointIndex('Spine')
uppers = [
skeleton.getJointIndex(name) for name in [
'Hips', 'Spine', 'Spine1', 'LeftArm', 'LeftForeArm', 'RightArm',
'RightForeArm'
]
]
upperMass = sum([bodyMasses[i] for i in uppers])
lLegs = [
skeleton.getJointIndex(name)
for name in ['LeftUpLeg', 'LeftLeg', 'LeftFoot']
]
rLegs = [
skeleton.getJointIndex(name)
for name in ['RightUpLeg', 'RightLeg', 'RightFoot']
]
allJoints = set(range(skeleton.getJointNum()))
halfFootHeight = controlModel.getBodyShape(lFoot)[1] / 2.
for fi in forceInfos:
fi.targetBody = spine
#===========================================================================
# data collection
#===========================================================================
rhip_torques = []
rknee_torques = []
rankle_torques = []
#===============================================================================
# rendering
#===============================================================================
rd_CM = [None]
rd_CP = [None]
rd_CMP = [None]
rd_forces = [None]
rd_force_points = [None]
rd_torques = []
rd_joint_positions = []
rd_point1 = [None]
rd_point2 = [None]
rd_vec1 = [None]
rd_vecori1 = [None]
rd_vec2 = [None]
rd_vecori2 = [None]
rd_frame1 = [None]
rd_frame2 = [None]
if MULTI_VIEWER:
viewer = ymv.MultiViewer(800, 655)
# viewer = ymv.MultiViewer(1600, 1255)
viewer.setRenderers1(
[cvr.VpModelRenderer(motionModel, MOTION_COLOR, yr.POLYGON_FILL)])
viewer.setRenderers2([
cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_FILL)
])
else:
viewer = ysv.SimpleViewer()
# viewer.record(False)
viewer.doc.addRenderer(
'motionModel',
cvr.VpModelRenderer(motionModel, (0, 150, 255), yr.POLYGON_LINE))
viewer.doc.addRenderer(
'controlModel',
cvr.VpModelRenderer(controlModel, (200, 200, 200),
yr.POLYGON_LINE))
# viewer.doc.addObject('motion_ori', motion_ori)
# viewer.doc.addRenderer('motion_ori', yr.JointMotionRenderer(motion_ori, (0,100,255), yr.LINK_BONE))
# viewer.doc.addRenderer('motion_seg_orig', yr.JointMotionRenderer(motion_seg_orig, (0,100,255), yr.LINK_BONE))
# viewer.doc.addRenderer('motion_seg', yr.JointMotionRenderer(motion_seg, (0,150,255), yr.LINK_BONE))
# viewer.doc.addRenderer('motion_stitch', yr.JointMotionRenderer(motion_stitch, (0,255,200), yr.LINK_BONE))
# viewer.doc.addRenderer('motion_stf_stabilize', yr.JointMotionRenderer(motion_stf_stabilize, (255,0,0), yr.LINK_BONE))
# viewer.doc.addRenderer('motion_match_stl', yr.JointMotionRenderer(motion_match_stl, (255,200,0), yr.LINK_BONE))
# viewer.doc.addRenderer('motion_swf_placement', yr.JointMotionRenderer(motion_swf_placement, (255,100,255), yr.LINK_BONE))
# viewer.doc.addRenderer('motion_swf_height', yr.JointMotionRenderer(motion_swf_height, (50,255,255), yr.LINK_BONE))
viewer.doc.addRenderer(
'motion_swf_orientation',
yr.JointMotionRenderer(motion_swf_orientation, (255, 100, 0),
yr.LINK_BONE))
viewer.doc.addRenderer(
'motion_stf_push',
yr.JointMotionRenderer(motion_stf_push, (50, 255, 200),
yr.LINK_BONE))
# viewer.doc.addRenderer('motion_stf_balancing', yr.JointMotionRenderer(motion_stf_balancing, (255,100,255), yr.LINK_BONE))
# viewer.doc.addRenderer('motion_control', yr.JointMotionRenderer(motion_control, (255,0,0), yr.LINK_BONE))
# viewer.doc.addRenderer('motion_debug1', yr.JointMotionRenderer(motion_debug1, (0,255,0), yr.LINK_BONE))
# viewer.doc.addRenderer('motion_debug2', yr.JointMotionRenderer(motion_debug2, (255,0,255), yr.LINK_BONE))
# viewer.doc.addRenderer('motion_debug3', yr.JointMotionRenderer(motion_debug3, (255,255,0), yr.LINK_BONE))
# viewer.doc.addRenderer('M_tc', yr.JointMotionRenderer(M_tc, (255,255,0), yr.LINK_BONE))
# viewer.doc.addRenderer('P_hat', yr.JointMotionRenderer(P_hat, (255,255,0), yr.LINK_BONE))
# viewer.doc.addRenderer('P', yr.JointMotionRenderer(P, (255,255,0), yr.LINK_BONE))
# viewer.doc.addRenderer('M_hat_tc_1', yr.JointMotionRenderer(M_hat_tc_1, (255,255,0), yr.LINK_BONE))
# viewer.doc.addRenderer('rd_CM', yr.PointsRenderer(rd_CM, (255,255,0)))
# viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (255,0,0)))
# viewer.doc.addRenderer('rd_CMP', yr.PointsRenderer(rd_CMP, (0,255,0)))
# viewer.doc.addRenderer('forces', yr.ForcesRenderer(rd_forces, rd_force_points, (255,0,0), ratio=.01, fromPoint=False))
# viewer.doc.addRenderer('torques', yr.VectorsRenderer(rd_torques, rd_joint_positions, (255,0,0)))
# viewer.doc.addRenderer('rd_point1', yr.PointsRenderer(rd_point1, (0,255,0)))
# viewer.doc.addRenderer('rd_point2', yr.PointsRenderer(rd_point2, (255,0,0)))
# viewer.doc.addRenderer('rd_vec1', yr.VectorsRenderer(rd_vec1, rd_vecori1, (255,0,0)))
# viewer.doc.addRenderer('rd_vec2', yr.VectorsRenderer(rd_vec2, rd_vecori2, (0,255,0)))
# viewer.doc.addRenderer('rd_frame1', yr.FramesRenderer(rd_frame1, (0,200,200)))
# viewer.doc.addRenderer('rd_frame2', yr.FramesRenderer(rd_frame2, (200,200,0)))
# viewer.setMaxFrame(len(motion_ori)-1)
if not REPEATED:
viewer.setMaxFrame(len(motion_ori) - 1)
else:
viewer.setMaxFrame(1000)
if CAMERA_TRACKING:
if MULTI_VIEWER:
cameraTargets1 = [None] * (viewer.getMaxFrame() + 1)
cameraTargets2 = [None] * (viewer.getMaxFrame() + 1)
else:
cameraTargets = [None] * (viewer.getMaxFrame() + 1)
if TORQUE_PLOT:
rhip_torques = [0.] * viewer.getMaxFrame()
rknee_torques = [0.] * viewer.getMaxFrame()
rankle_torques = [0.] * viewer.getMaxFrame()
# pt = [0.]
def postFrameCallback_Always(frame):
# if frame==1: pt[0] = time.time()
# if frame==31: print 'elapsed time for 30 frames:', time.time()-pt[0]
if CAMERA_TRACKING:
if MULTI_VIEWER:
if cameraTargets1[frame] == None:
cameraTargets1[frame] = motionModel.getBodyPositionGlobal(
0)
# cameraTargets1[frame] = motion_ori[frame].getJointPositionGlobal(0)
viewer.setCameraTarget1(cameraTargets1[frame])
if cameraTargets2[frame] == None:
cameraTargets2[
frame] = controlModel.getJointPositionGlobal(0)
viewer.setCameraTarget2(cameraTargets2[frame])
else:
if cameraTargets[frame] == None:
cameraTargets[frame] = controlModel.getJointPositionGlobal(
0)
viewer.setCameraTarget(cameraTargets[frame])
if plot != None:
plot.updateVline(frame)
viewer.setPostFrameCallback_Always(postFrameCallback_Always)
plot = None
# plot = ymp.InteractivePlot()
if plot != None:
plot.setXlimit(0, len(motion_ori))
plot.setYlimit(0., 1.)
plot.addDataSet('zero')
plot.addDataSet('diff')
plot.addDataSet('debug1')
plot.addDataSet('debug2')
def viewer_onClose(data):
if plot != None:
plot.close()
viewer.onClose(data)
viewer.callback(viewer_onClose)
def simulateCallback(frame):
# seginfo
segIndex = seg_index[0]
curState = seginfo[segIndex]['state']
curInterval = yma.offsetInterval(acc_offset[0],
seginfo[segIndex]['interval'])
stanceLegs = seginfo[segIndex]['stanceHips']
swingLegs = seginfo[segIndex]['swingHips']
stanceFoots = seginfo[segIndex]['stanceFoots']
swingFoots = seginfo[segIndex]['swingFoots']
swingKnees = seginfo[segIndex]['swingKnees']
groundHeight = seginfo[segIndex]['ground_height']
maxStfPushFrame = seginfo[segIndex]['max_stf_push_frame']
prev_frame = frame - 1 if frame > 0 else 0
# prev_frame = frame
# information
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, upperMass, uppers)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, upperMass, uppers)
## dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, totalMass)
## CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, totalMass)
# stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], frame)
# CMr_tar = CM_tar - stf_tar
dCM_tar = motion_seg.getJointVelocityGlobal(0, prev_frame)
CM_tar = motion_seg.getJointPositionGlobal(0, prev_frame)
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, upperMass, uppers)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, upperMass, uppers)
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, totalMass)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, totalMass)
stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], prev_frame)
CMr_tar = CM_tar - stf_tar
dCM = avg_dCM[0]
CM = controlModel.getJointPositionGlobal(0)
# CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, upperMass, uppers)
# CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass)
CMreal = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses,
totalMass)
stf = controlModel.getJointPositionGlobal(stanceFoots[0])
CMr = CM - stf
diff_dCM = mm.projectionOnPlane(dCM - dCM_tar, (1, 0, 0), (0, 0, 1))
diff_dCM_axis = np.cross((0, 1, 0), diff_dCM)
rd_vec1[0] = diff_dCM
rd_vecori1[0] = CM_tar
diff_CMr = mm.projectionOnPlane(CMr - CMr_tar, (1, 0, 0), (0, 0, 1))
# rd_vec1[0] = diff_CMr; rd_vecori1[0] = stf_tar
diff_CMr_axis = np.cross((0, 1, 0), diff_CMr)
direction = mm.normalize2(
mm.projectionOnPlane(dCM_tar, (1, 0, 0), (0, 0, 1)))
# direction = mm.normalize2(mm.projectionOnPlane(dCM, (1,0,0), (0,0,1)))
directionAxis = np.cross((0, 1, 0), direction)
diff_dCM_sag, diff_dCM_cor = mm.projectionOnVector2(
diff_dCM, direction)
# rd_vec1[0] = diff_dCM_sag; rd_vecori1[0] = CM_tar
diff_dCM_sag_axis = np.cross((0, 1, 0), diff_dCM_sag)
diff_dCM_cor_axis = np.cross((0, 1, 0), diff_dCM_cor)
diff_CMr_sag, diff_CMr_cor = mm.projectionOnVector2(
diff_CMr, direction)
diff_CMr_sag_axis = np.cross((0, 1, 0), diff_CMr_sag)
diff_CMr_cor_axis = np.cross((0, 1, 0), diff_CMr_cor)
t = (frame - curInterval[0]) / float(curInterval[1] - curInterval[0])
t_raw = t
if t > 1.: t = 1.
p_root = motion_stitch[frame].getJointPositionGlobal(0)
R_root = motion_stitch[frame].getJointOrientationGlobal(0)
motion_seg_orig.goToFrame(frame)
motion_seg.goToFrame(frame)
motion_stitch.goToFrame(frame)
motion_debug1.append(motion_stitch[frame].copy())
motion_debug1.goToFrame(frame)
motion_debug2.append(motion_stitch[frame].copy())
motion_debug2.goToFrame(frame)
motion_debug3.append(motion_stitch[frame].copy())
motion_debug3.goToFrame(frame)
# paper implementation
M_tc.append(motion_stitch[prev_frame])
M_tc.goToFrame(frame)
P_hat.append(M_tc[frame].copy())
P_hat.goToFrame(frame)
p_temp = ym.JointPosture(skeleton)
p_temp.rootPos = controlModel.getJointPositionGlobal(0)
p_temp.setJointOrientationsLocal(
controlModel.getJointOrientationsLocal())
P.append(p_temp)
P.goToFrame(frame)
# stance foot stabilize
motion_stf_stabilize.append(motion_stitch[frame].copy())
motion_stf_stabilize.goToFrame(frame)
if STANCE_FOOT_STABILIZE:
for stanceFoot in stanceFoots:
R_target_foot = motion_seg[frame].getJointOrientationGlobal(
stanceFoot)
R_current_foot = motion_stf_stabilize[
frame].getJointOrientationGlobal(stanceFoot)
motion_stf_stabilize[frame].setJointOrientationGlobal(
stanceFoot,
cm.slerp(R_current_foot, R_target_foot,
stf_stabilize_func(t)))
# R_target_foot = motion_seg[frame].getJointOrientationLocal(stanceFoot)
# R_current_foot = motion_stf_stabilize[frame].getJointOrientationLocal(stanceFoot)
# motion_stf_stabilize[frame].setJointOrientationLocal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t)))
# match stance leg
motion_match_stl.append(motion_stf_stabilize[frame].copy())
motion_match_stl.goToFrame(frame)
if MATCH_STANCE_LEG:
if curState != yba.GaitState.STOP:
for i in range(len(stanceLegs)):
stanceLeg = stanceLegs[i]
stanceFoot = stanceFoots[i]
# # motion stance leg -> character stance leg as time goes
R_motion = motion_match_stl[
frame].getJointOrientationGlobal(stanceLeg)
R_character = controlModel.getJointOrientationGlobal(
stanceLeg)
motion_match_stl[frame].setJointOrientationGlobal(
stanceLeg,
cm.slerp(R_motion, R_character, match_stl_func(t)))
# t_y = match_stl_func_y(t)
# t_xz = match_stl_func(t)
#
# R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg)
# R_character = controlModel.getJointOrientationGlobal(stanceLeg)
# R = np.dot(R_character, R_motion.T)
# R_y, R_xz = mm.projectRotation((0,1,0), R)
# motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_xz, t_xz))
# motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_y, t_y))
# swing foot placement
motion_swf_placement.append(motion_match_stl[frame].copy())
motion_swf_placement.goToFrame(frame)
if SWING_FOOT_PLACEMENT:
t_swing_foot_placement = swf_placement_func(t)
if extended[0]:
R_swp_sag = prev_R_swp[0][0]
R_swp_cor = prev_R_swp[0][1]
else:
R_swp_sag = mm.I_SO3()
R_swp_cor = mm.I_SO3()
R_swp_sag = np.dot(
R_swp_sag,
mm.exp(diff_dCM_sag_axis * K_swp_vel_sag *
-t_swing_foot_placement))
R_swp_cor = np.dot(
R_swp_cor,
mm.exp(diff_dCM_cor_axis * K_swp_vel_cor *
-t_swing_foot_placement))
if np.dot(direction, diff_CMr_sag) < 0:
R_swp_sag = np.dot(
R_swp_sag,
mm.exp(diff_CMr_sag_axis * K_swp_pos_sag *
-t_swing_foot_placement))
R_swp_cor = np.dot(
R_swp_cor,
mm.exp(diff_CMr_cor_axis * K_swp_pos_cor *
-t_swing_foot_placement))
for i in range(len(swingLegs)):
swingLeg = swingLegs[i]
swingFoot = swingFoots[i]
# save swing foot global orientation
# R_swf = motion_swf_placement[frame].getJointOrientationGlobal(swingFoot)
# rotate swing leg
motion_swf_placement[frame].mulJointOrientationGlobal(
swingLeg, R_swp_sag)
motion_swf_placement[frame].mulJointOrientationGlobal(
swingLeg, R_swp_cor)
# restore swing foot global orientation
# motion_swf_placement[frame].setJointOrientationGlobal(swingFoot, R_swf)
prev_R_swp[0] = (R_swp_sag, R_swp_cor)
# swing foot height
motion_swf_height.append(motion_swf_placement[frame].copy())
motion_swf_height.goToFrame(frame)
if SWING_FOOT_HEIGHT:
for swingFoot in swingFoots:
stanceFoot = stanceFoots[0]
# save foot global orientation
R_foot = motion_swf_height[frame].getJointOrientationGlobal(
swingFoot)
R_stance_foot = motion_swf_height[
frame].getJointOrientationGlobal(stanceFoot)
height_tar = motion_swf_height[frame].getJointPositionGlobal(
swingFoot)[1] - motion_swf_height[
frame].getJointPositionGlobal(stanceFoot)[1]
# motion_debug1[frame] = motion_swf_height[frame].copy()
# rotate
motion_swf_height[frame].rotateByTarget(
controlModel.getJointOrientationGlobal(0))
# motion_debug2[frame] = motion_swf_height[frame].copy()
# motion_debug2[frame].translateByTarget(controlModel.getJointPositionGlobal(0))
height_cur = motion_swf_height[frame].getJointPositionGlobal(
swingFoot)[1] - motion_swf_height[
frame].getJointPositionGlobal(stanceFoot)[1]
offset_height = (height_tar - height_cur
) * swf_height_func(t) * c_swf_stability
offset_sine = c_swf_mid_offset * swf_height_sine_func(t)
offset = 0.
offset += offset_height
offset += offset_sine
if offset > 0.:
newPosition = motion_swf_height[
frame].getJointPositionGlobal(swingFoot)
newPosition[1] += offset
aik.ik_analytic(motion_swf_height[frame], swingFoot,
newPosition)
else:
newPosition = motion_swf_height[
frame].getJointPositionGlobal(stanceFoot)
newPosition[1] -= offset
aik.ik_analytic(motion_swf_height[frame], stanceFoot,
newPosition)
# return
# motion_debug3[frame] = motion_swf_height[frame].copy()
# motion_debug3[frame].translateByTarget(controlModel.getJointPositionGlobal(0))
motion_swf_height[frame].rotateByTarget(R_root)
# restore foot global orientation
motion_swf_height[frame].setJointOrientationGlobal(
swingFoot, R_foot)
motion_swf_height[frame].setJointOrientationGlobal(
stanceFoot, R_stance_foot)
if plot != None:
plot.addDataPoint('debug1', frame, height_tar)
plot.addDataPoint('debug2', frame, height_cur)
# plot.addDataPoint('diff', frame, diff)
# swing foot orientation
motion_swf_orientation.append(motion_swf_height[frame].copy())
motion_swf_orientation.goToFrame(frame)
if SWING_FOOT_ORIENTATION:
# swf_orientation_func = yfg.concatenate([yfg.zero, yfg.hermite2nd, yfg.one], [.25, .75])
for swingFoot in swingFoots:
R_target_foot = motion_seg[
curInterval[1]].getJointOrientationGlobal(swingFoot)
R_current_foot = motion_swf_orientation[
frame].getJointOrientationGlobal(swingFoot)
motion_swf_orientation[frame].setJointOrientationGlobal(
swingFoot,
cm.slerp(R_current_foot, R_target_foot,
swf_orientation_func(t)))
# swf_stabilize_func = yfg.concatenate([yfg.hermite2nd, yfg.one], [c_taking_duration])
# push orientation
# for swingFoot in swingFoots:
# R_target_foot = motion_seg[frame].getJointOrientationGlobal(swingFoot)
# R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot)
# motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot , swf_stabilize_func(t)))
# stance foot push
motion_stf_push.append(motion_swf_orientation[frame].copy())
motion_stf_push.goToFrame(frame)
if STANCE_FOOT_PUSH:
for swingFoot in swingFoots:
# max_t = (maxStfPushFrame)/float(curInterval[1]-curInterval[0])
# stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [max_t*2])
stf_push_func = yfg.concatenate([yfg.sine, yfg.zero],
[c_taking_duration * 2])
R_swp_sag = mm.I_SO3()
# R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_stp_vel * -stf_push_func(t)))
# if step_length_cur[0] < step_length_tar[0]:
# ratio = step_length_cur[0] / step_length_tar[0]
# R_max = maxmaxStfPushFrame
# R_zero =
R_swp_sag = np.dot(
R_swp_sag,
mm.exp((step_length_tar[0] - step_length_cur[0]) *
step_axis[0] * K_stp_pos * -stf_push_func(t)))
motion_stf_push[frame].mulJointOrientationGlobal(
swingFoot, R_swp_sag)
# stance foot balancing
motion_stf_balancing.append(motion_stf_push[frame].copy())
motion_stf_balancing.goToFrame(frame)
if STANCE_FOOT_BALANCING:
R_stb = mm.exp(diff_dCM_axis * K_stb_vel * stf_balancing_func(t))
for stanceFoot in stanceFoots:
if frame < 5: continue
motion_stf_balancing[frame].mulJointOrientationGlobal(
stanceFoot, R_stb)
# control trajectory
motion_control.append(motion_stf_balancing[frame].copy())
motion_control.goToFrame(frame)
#=======================================================================
# tracking with inverse dynamics
#=======================================================================
th_r = motion_control.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion_control.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion_control.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r,
Kt, Dt)
#=======================================================================
# simulation
#=======================================================================
CP = mm.v3(0., 0., 0.)
F = mm.v3(0., 0., 0.)
avg_dCM[0] = mm.v3(0., 0., 0.)
# external force rendering info
del rd_forces[:]
del rd_force_points[:]
for fi in forceInfos:
if fi.startFrame <= frame and frame < fi.startFrame + fi.duration * (
1 / frameTime):
rd_forces.append(fi.force)
rd_force_points.append(
controlModel.getBodyPositionGlobal(fi.targetBody))
for i in range(stepsPerFrame):
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
# apply external force
for fi in forceInfos:
if fi.startFrame <= frame and frame < fi.startFrame + fi.duration * (
1 / frameTime):
controlModel.applyBodyForceGlobal(fi.targetBody, fi.force)
controlModel.setDOFAccelerations(ddth_des)
controlModel.solveHybridDynamics()
if TORQUE_PLOT:
rhip_torques[frame] += mm.length(
controlModel.getJointTorqueLocal(rUpLeg))
rknee_torques[frame] += mm.length(
controlModel.getJointTorqueLocal(rKnee))
rankle_torques[frame] += mm.length(
controlModel.getJointTorqueLocal(rFoot))
rd_torques[:] = [
controlModel.getJointTorqueLocal(i) / 100.
for i in range(skeleton.getJointNum())
]
rd_joint_positions[:] = controlModel.getJointPositionsGlobal()
vpWorld.step()
# yvu.align2D(controlModel)
if len(contactForces) > 0:
CP += yrp.getCP(contactPositions, contactForces)
F += sum(contactForces)
avg_dCM[0] += controlModel.getJointVelocityGlobal(0)
# avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, upperMass, uppers)
# avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, totalMass)
# if len(stanceFoots)>0:
# avg_stf_v[0] += controlModel.getJointVelocityGlobal(stanceFoots[0])
# avg_stf_av[0] += controlModel.getJointAngVelocityGlobal(stanceFoots[0])
CP /= stepsPerFrame
F /= stepsPerFrame
avg_dCM[0] /= stepsPerFrame
# if len(stanceFoots)>0:
# avg_stf_v[0] /= stepsPerFrame
# avg_stf_av[0] /= stepsPerFrame
# rd_vec1[0] = avg_stf_av[0]; rd_vec1[0][0] = 0.; rd_vec1[0][2] = 0.
# rd_vecori1[0]= controlModel.getJointPositionGlobal(stanceFoots[0])
#=======================================================================
# segment editing
#=======================================================================
lastFrame = False
if SEGMENT_EDITING:
if curState == yba.GaitState.STOP:
if frame == len(motion_seg) - 1:
lastFrame = True
elif (curState == yba.GaitState.LSWING or curState
== yba.GaitState.RSWING) and t > c_min_contact_time:
swingID = lID if curState == yba.GaitState.LSWING else rID
contact = False
if swingID in bodyIDs:
minContactVel = 1000.
for i in range(len(bodyIDs)):
if bodyIDs[i] == swingID:
vel = controlModel.getBodyVelocityGlobal(
swingID, contactPositionLocals[i])
vel[1] = 0
contactVel = mm.length(vel)
if contactVel < minContactVel:
minContactVel = contactVel
if minContactVel < c_min_contact_vel: contact = True
extended[0] = False
if contact:
# print frame, 'foot touch'
lastFrame = True
acc_offset[0] += frame - curInterval[1]
elif frame == len(motion_seg) - 1:
print frame, 'extend frame', frame + 1
preserveJoints = []
# preserveJoints = [lFoot, rFoot]
# preserveJoints = [lFoot, rFoot, lKnee, rKnee]
# preserveJoints = [lFoot, rFoot, lKnee, rKnee, lUpLeg, rUpLeg]
stanceKnees = [
rKnee
] if curState == yba.GaitState.LSWING else [lKnee]
preserveJoints = [
stanceFoots[0], stanceKnees[0], stanceLegs[0]
]
diff = 3
motion_seg_orig.extend([motion_seg_orig[-1]])
motion_seg.extend(
ymt.extendByIntegration_root(motion_seg, 1, diff))
motion_stitch.extend(
ymt.extendByIntegration_constant(
motion_stitch, 1, preserveJoints, diff))
# # extend for swing foot ground speed matching & swing foot height lower
## extendedPostures = ymt.extendByIntegration(motion_stitch, 1, preserveJoints, diff)
## extendedPostures = [motion_stitch[-1]]
##
# extendFrameNum = frame - curInterval[1] + 1
# k = 1.-extendFrameNum/5.
# if k<0.: k=0.
# extendedPostures = ymt.extendByIntegrationAttenuation(motion_stitch, 1, preserveJoints, diff, k)
#
## if len(swingFoots)>0 and np.inner(dCM_tar, dCM)>0.:
## print frame, 'speed matching'
## R_swf = motion_stitch[-1].getJointOrientationGlobal(swingFoots[0])
##
## p_swf = motion_stitch[-1].getJointPositionGlobal(swingFoots[0])
## v_swf = motion_stitch.getJointVelocityGlobal(swingFoots[0], frame-diff, frame)
## a_swf = motion_stitch.getJointAccelerationGlobal(swingFoots[0], frame-diff, frame)
## p_swf += v_swf * (frameTime) + a_swf * (frameTime)*(frameTime)
## aik.ik_analytic(extendedPostures[0], swingFoots[0], p_swf)
##
## extendedPostures[0].setJointOrientationGlobal(swingFoots[0], R_swf)
#
# motion_stitch.extend(extendedPostures)
extended[0] = True
else:
if frame == len(motion_seg) - 1: lastFrame = True
if lastFrame:
if segIndex < len(segments) - 1:
print '%d (%d): end of %dth seg (%s, %s)' % (
frame, frame - curInterval[1], segIndex,
yba.GaitState.text[curState], curInterval)
if plot != None:
plot.addDataPoint('diff', frame,
(frame - curInterval[1]) * .01)
if len(stanceFoots) > 0 and len(swingFoots) > 0:
# step_cur = controlModel.getJointPositionGlobal(swingFoots[0]) - controlModel.getJointPositionGlobal(stanceFoots[0])
# step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(swingFoots[0]) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
step_cur = controlModel.getJointPositionGlobal(
0) - controlModel.getJointPositionGlobal(
stanceFoots[0])
step_tar = motion_seg[
curInterval[1]].getJointPositionGlobal(0) - motion_seg[
curInterval[1]].getJointPositionGlobal(
stanceFoots[0])
step_cur = mm.projectionOnPlane(step_cur, (1, 0, 0),
(0, 0, 1))
step_tar = mm.projectionOnPlane(step_tar, (1, 0, 0),
(0, 0, 1))
step_cur_sag, step_cur_cor = mm.projectionOnVector2(
step_cur, direction)
step_tar_sag, step_tar_cor = mm.projectionOnVector2(
step_tar, direction)
step_length_tar[0] = mm.length(step_tar_sag)
if np.inner(step_tar_sag, step_cur_sag) > 0:
step_length_cur[0] = mm.length(step_cur_sag)
else:
step_length_cur[0] = -mm.length(step_cur_sag)
step_axis[0] = directionAxis
# rd_vec1[0] = step_tar_sag
# rd_vecori1[0] = motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
# rd_vec2[0] = step_cur_sag
# rd_vecori2[0] = controlModel.getJointPositionGlobal(stanceFoots[0])
seg_index[0] += 1
curSeg = segments[seg_index[0]]
stl_y_limit_num[0] = 0
stl_xz_limit_num[0] = 0
del motion_seg_orig[frame + 1:]
motion_seg_orig.extend(
ymb.getAttachedNextMotion(curSeg, motion_seg_orig[-1],
False, False))
del motion_seg[frame + 1:]
del motion_stitch[frame + 1:]
transitionLength = len(curSeg) - 1
# motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, False))
# motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, False))
d = motion_seg[-1] - curSeg[0]
d.rootPos[1] = 0.
motion_seg.extend(
ymb.getAttachedNextMotion(curSeg, d, True, False))
if NO_FOOT_SLIDING:
if segIndex == len(segments) - 2:
Rl = motion_control[-1].getJointOrientationLocal(
lUpLeg)
Rr = motion_control[-1].getJointOrientationLocal(
rUpLeg)
Rlk = motion_control[-1].getJointOrientationLocal(
lKnee)
Rrk = motion_control[-1].getJointOrientationLocal(
rKnee)
Rlf = motion_control[-1].getJointOrientationLocal(
lFoot)
Rrf = motion_control[-1].getJointOrientationLocal(
rFoot)
for p in curSeg:
p.setJointOrientationLocal(lUpLeg, Rl, False)
p.setJointOrientationLocal(rUpLeg, Rr, False)
p.setJointOrientationLocal(lKnee, Rlk, False)
p.setJointOrientationLocal(rKnee, Rrk, False)
p.setJointOrientationLocal(lFoot, Rlf, False)
p.setJointOrientationLocal(rFoot, Rrf, False)
p.updateGlobalT()
d = motion_control[-1] - curSeg[0]
d.rootPos[1] = 0.
motion_stitch.extend(
ymb.getStitchedNextMotion(curSeg, d, transitionLength,
stitch_func, True, False))
# motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, True))
# motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, True))
else:
motion_seg_orig.append(motion_seg_orig[-1])
motion_seg.append(motion_seg[-1])
motion_stitch.append(motion_control[-1])
# rendering
motionModel.update(motion_ori[frame])
# motionModel.update(motion_seg[frame])
if SAVE_SIMULATION:
p_temp = ym.JointPosture(skeleton)
p_temp.initLocalRs()
p_temp.rootPos = controlModel.getJointPositionGlobal(0)
p_temp.setJointOrientationsLocal(
controlModel.getJointOrientationsLocal())
motion_simulation.append(p_temp)
if frame == viewer.getMaxFrame():
saveFilePath = SAVE_DIR + 'simulated_' + filename
yf.writeBvhFile(saveFilePath, motion_simulation)
print saveFilePath, 'saved'
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(frameTime / 1.4)
# viewer.startTimer(frameTime * .4)
viewer.show()
Fl.run()
def test_blendedSegment_posture_warping():
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
motion0 = yf.readBvhFile(bvhFilePath, .01)
bvhFilePath = '../samples/wd2_WalkSukiko00.bvh'
# bvhFilePath = '../samples/wd2_WalkForwardVFast00.bvh'
motion1 = yf.readBvhFile(bvhFilePath, .01)
frameTime = 1 / 30.
firstMotion = motion0[0:75]
# midMotionSmooth1 = firstMotion + getBlendedNextMotion2(motion0[75:95], motion1[120:150], firstMotion[-1], None)
# midMotionFixed1 = firstMotion + getBlendedNextMotion2(motion0[75:95], motion1[120:150], firstMotion[-1], .5)
# midMotionFixed2 = firstMotion + getBlendedNextMotion2(motion0[75:95], motion1[120:150], firstMotion[-1], 1.)
midMotionSmooth1 = firstMotion + getAttachedNextMotion(
blendSegmentSmooth(motion0[75 - 1:95], motion1[120 - 1:150]),
firstMotion[-1])
midMotionFixed1 = firstMotion + getAttachedNextMotion(
blendSegmentFixed(motion0[75 - 1:95], motion1[120 - 1:150], .5),
firstMotion[-1])
midMotionFixed2 = firstMotion + getAttachedNextMotion(
blendSegmentFixed(motion0[75 - 1:95], motion1[120 - 1:150], 1.),
firstMotion[-1])
midMotion = midMotionSmooth1
secondMotion = midMotion + getAttachedNextMotion(
motion1[150:], midMotion[-1])
# print len(midMotionSmooth1), len(midMotionSmooth2)
# print len(midMotionFixed1), len(midMotionFixed2)
viewer = ysv.SimpleViewer()
viewer.record(False)
# viewer.doc.addRenderer('motion0', yr.JointMotionRenderer(motion0, (0,0,255), yr.LINK_LINE))
# viewer.doc.addObject('motion0', motion0)
# viewer.doc.addRenderer('motion1', yr.JointMotionRenderer(motion1, (0,0,255), yr.LINK_LINE))
# viewer.doc.addObject('motion1', motion1)
viewer.doc.addRenderer(
'firstMotion',
yr.JointMotionRenderer(firstMotion, (255, 0, 0), yr.LINK_LINE))
viewer.doc.addObject('firstMotion', firstMotion)
viewer.doc.addRenderer(
'midMotionSmooth1',
yr.JointMotionRenderer(midMotionSmooth1, (255, 255, 0),
yr.LINK_LINE))
viewer.doc.addObject('midMotionSmooth1', midMotionSmooth1)
viewer.doc.addRenderer(
'midMotionFixed1',
yr.JointMotionRenderer(midMotionFixed1, (255, 0, 255),
yr.LINK_LINE))
viewer.doc.addObject('midMotionFixed1', midMotionFixed1)
viewer.doc.addRenderer(
'midMotionFixed2',
yr.JointMotionRenderer(midMotionFixed2, (0, 255, 255),
yr.LINK_LINE))
viewer.doc.addObject('midMotionFixed2', midMotionFixed2)
# viewer.doc.addRenderer('secondMotion', yr.JointMotionRenderer(secondMotion, (0,255,0), yr.LINK_LINE))
# viewer.doc.addObject('secondMotion', secondMotion)
#
viewer.startTimer(frameTime / 1.4)
viewer.show()
Fl.run()
def test_stitch():
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
motion0 = yf.readBvhFile(bvhFilePath, .01)
bvhFilePath = '../samples/wd2_WalkForwardVFast00.bvh'
motion1 = yf.readBvhFile(bvhFilePath, .01)
hRef = .1
vRef = .3
LHEEL = motion0[0].skeleton.getElementIndex('LeftFoot')
RHEEL = motion0[0].skeleton.getElementIndex('RightFoot')
lc0 = yma.getElementContactStates(motion0, LHEEL, hRef, vRef)
rc0 = yma.getElementContactStates(motion0, RHEEL, hRef, vRef)
lc1 = yma.getElementContactStates(motion1, LHEEL, hRef, vRef)
rc1 = yma.getElementContactStates(motion1, RHEEL, hRef, vRef)
intervals0, states0 = yba.getBipedGaitIntervals(lc0, rc0, 10, .1)
intervals1, states1 = yba.getBipedGaitIntervals(lc1, rc1, 10, .1)
print('wd2_WalkSameSame00')
print(intervals0)
print([yba.GaitState.text[state] for state in states0])
print('wd2_WalkForwardVFast00')
print(intervals1)
print([yba.GaitState.text[state] for state in states1])
print()
seg = 3
tlen = 10
print('motion0[:%d] + motion1[%d:]' %
(intervals0[seg][1], intervals1[seg][1]))
print('transition length', tlen)
print('stitched_0changed',
'%d~%d' % (intervals0[seg][1] - tlen, intervals0[seg][1]))
print('stitched_1changed',
'%d~%d' % (intervals0[seg][1], intervals0[seg][1] + tlen))
print()
just_added = motion0[:intervals0[seg][1]] + motion1[intervals1[seg][1]:]
print('len(just_added)', len(just_added))
stitched_0changed = stitch(motion0[:intervals0[seg][1]],
motion1[intervals1[seg][1] - 1:], tlen,
yfg.identity, True)
print('len(stitched_0changed)', len(stitched_0changed))
stitched_1changed = stitch(motion0[:intervals0[seg][1]],
motion1[intervals1[seg][1] - 1:], tlen,
yfg.identity, False)
print('len(stitched_1changed)', len(stitched_1changed))
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'just_added',
yr.JointMotionRenderer(just_added, (0, 0, 255), yr.LINK_LINE))
viewer.doc.addObject('just_added', just_added)
viewer.doc.addRenderer(
'stitched_0changed',
yr.JointMotionRenderer(stitched_0changed, (0, 255, 0),
yr.LINK_LINE))
viewer.doc.addObject('stitched_0changed', stitched_0changed)
viewer.doc.addRenderer(
'stitched_1changed',
yr.JointMotionRenderer(stitched_1changed, (255, 255, 0),
yr.LINK_LINE))
viewer.doc.addObject('stitched_1changed', stitched_1changed)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def main():
np.set_printoptions(precision=4, linewidth=200)
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5()
motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped()
mcfg_motion = mit.normal_mcfg()
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
motionModel.recordVelByFiniteDiff()
IKModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
footPartNum = config['FootPartNum']
if footPartNum > 1:
elasticity = 2000
damping = 2 * (elasticity**.5)
springBody1 = 1
springBody2 = 2
springBody1Pos = motionModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]))
springBody2Pos = motionModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]))
initialDist = mm.length(springBody1Pos - springBody2Pos) * 1.
node = mcfg.getNode(mit.LEFT_METATARSAL_1)
initialDist -= node.width #0.084
v1 = (-node.width * 0.5, 0.0, node.length * 0.4)
v2 = (node.width * 0.5, 0.0, node.length * 0.4)
#controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]), elasticity, damping, v2, v1, initialDist)
#controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootRPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootRPart'][springBody2]), elasticity, damping, v1, v2, initialDist)
elasticity = 10
damping = 2 * (elasticity**.5)
springBody1 = 3
springBody2 = 4
node = mcfg.getNode(mit.LEFT_PHALANGE_1)
springBody1Pos = motionModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]))
springBody2Pos = motionModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]))
initialDist = mm.length(springBody1Pos - springBody2Pos) * 1.
initialDist -= node.width #0.084
v1 = (-node.width * 0.5, 0.0, -node.length * 0.4)
v2 = (node.width * 0.5, 0.0, -node.length * 0.4)
#controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]), elasticity, damping, v2, v1, initialDist)
#controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootRPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootRPart'][springBody2]), elasticity, damping, v1, v2, initialDist)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
#ModelOffset = (1.5, -0.01, 0)
ModelOffset = (1.5, 0.04, 0)
controlModel.translateByOffset(ModelOffset)
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
# parameter
Kt = config['Kt']
Dt = config['Dt'] # tracking gain
Kl = config['Kl']
Dl = config['Dl'] # linear balance gain
Kh = config['Kh']
Dh = config['Dh'] # angular balance gain
Ks = config['Ks']
Ds = config['Ds'] # penalty force spring gain
Bt = config['Bt']
Bl = config['Bl']
Bh = config['Bh']
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
w2 = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap2'])
#w_IK = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['IKweightMap'])
supL = motion[0].skeleton.getJointIndex(config['supLink'])
supR = motion[0].skeleton.getJointIndex(config['supLink2'])
rootB = motion[0].skeleton.getJointIndex(config['root'])
selectedBody = motion[0].skeleton.getJointIndex(config['end'])
#constBody = motion[0].skeleton.getJointIndex('LeftForeArm')
constBody = motion[0].skeleton.getJointIndex(config['const'])
# jacobian
Jsup = yjc.makeEmptyJacobian(DOFs, 1)
dJsup = Jsup.copy()
JsupPre = Jsup.copy()
Jsys_IK = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum())
Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum())
dJsys = Jsys.copy()
JsysPre = Jsys.copy()
Jconst = yjc.makeEmptyJacobian(DOFs, 1)
dJconst = Jconst.copy()
Jcom = yjc.makeEmptyJacobian(DOFs, 1, False)
dJcom = Jcom.copy()
JcomAng = yjc.makeEmptyJacobian(DOFs, 1, False)
dJcomAng = JcomAng.copy()
###############
jFootL_IK = [None] * footPartNum
jFootR_IK = [None] * footPartNum
indexFootL = [None] * footPartNum
indexFootR = [None] * footPartNum
jFootL = [None] * footPartNum
dJFootL = [None] * footPartNum
jFootR = [None] * footPartNum
dJFootR = [None] * footPartNum
jointMasksFootL = [None] * footPartNum
jointMasksFootR = [None] * footPartNum
jAngFootL = [None] * footPartNum
dJAngFootL = [None] * footPartNum
jAngFootR = [None] * footPartNum
dJAngFootR = [None] * footPartNum
for i in range(footPartNum):
jFootL_IK[i] = yjc.makeEmptyJacobian(DOFs, 1)
jFootR_IK[i] = yjc.makeEmptyJacobian(DOFs, 1)
jFootL[i] = yjc.makeEmptyJacobian(DOFs, 1)
dJFootL[i] = jFootL[i].copy()
jFootR[i] = yjc.makeEmptyJacobian(DOFs, 1)
dJFootR[i] = jFootR[i].copy()
jAngFootL[i] = yjc.makeEmptyJacobian(DOFs, 1, False)
dJAngFootL[i] = jAngFootL[i].copy()
jAngFootR[i] = yjc.makeEmptyJacobian(DOFs, 1, False)
dJAngFootR[i] = jAngFootR[i].copy()
indexFootL[i] = motion[0].skeleton.getJointIndex(
config['FootLPart'][i])
indexFootR[i] = motion[0].skeleton.getJointIndex(
config['FootRPart'][i])
jointMasksFootL[i] = [
yjc.getLinkJointMask(motion[0].skeleton, indexFootL[i])
]
jointMasksFootR[i] = [
yjc.getLinkJointMask(motion[0].skeleton, indexFootR[i])
]
constJointMasks = [
yjc.getLinksJointMask(motion[0].skeleton,
[indexFootL[0], indexFootR[0]])
]
#constJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [indexFootL[0]])]
#constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)]
allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton)
#comLowerJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [motion[0].skeleton.getJointIndex('LeftLeg'), motion[0].skeleton.getJointIndex('RightLeg')])]
comUpperJointMasks = [
yjc.getLinkJointMask(motion[0].skeleton, selectedBody)
]
#comLowerJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [motion[0].skeleton.getJointIndex('LeftLeg'), motion[0].skeleton.getJointIndex('RightLeg')])]
comUpperJointMasks[0][0] = 0
#comUpperJointMasks[0][1] = 1
#comUpperJointMasks[0][10] = 1
comUpperJointMasks[0][2] = 1
comUpperJointMasks[0][11] = 1
#print(comUpperJointMasks)
comLowerJointMasks = [
yjc.getLinksJointMask(motion[0].skeleton, [
motion[0].skeleton.getJointIndex('LeftLeg'),
motion[0].skeleton.getJointIndex('RightLeg')
])
]
'''
maskArray = [foreSupLJointMasks, foreSupRJointMasks, rearSupLJointMasks, rearSupRJointMasks]
parentArray = [supL, supR, supL, supR]
effectorArray = [foreSupL, foreSupR, rearSupL, rearSupR]
for j in range(4) :
for i in range(len(foreSupLJointMasks)) :
if i == parentArray[j] or i == effectorArray[j] :
maskArray[j][0][i] = 1
else :
maskArray[j][0][i] = 0
'''
# momentum matrix
linkMasses = controlModel.getBodyMasses()
totalMass = controlModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
problem = yac.LSE(totalDOF, 6)
a_sup = (0, 0, 0, 0, 0, 0) #L
#a_sup2 = (0,0,0, 0,0,0)#R
a_sup2 = [0, 0, 0, 0, 0, 0] #R
a_sup_2 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
CP_old = [mm.v3(0., 0., 0.)]
# penalty method
bodyIDsToCheck = range(vpWorld.getBodyNum())
mus = [1.] * len(bodyIDsToCheck)
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
dth_IK = ype.makeNestedList(DOFs)
d_th_IK = ype.makeNestedList(DOFs)
d_th_IK_L = ype.makeNestedList(DOFs)
d_th_IK_R = ype.makeNestedList(DOFs)
dd_th_IK = ype.makeNestedList(DOFs)
dd_th_IK_flat = ype.makeFlatList(totalDOF)
d_th_IK_flat = ype.makeFlatList(totalDOF)
ddth_c_flat = ype.makeFlatList(totalDOF)
# viewer
rd_footCenter = [None]
rd_footCenter_ref = [None]
rd_footCenterL = [None]
rd_footCenterR = [None]
rd_CM_plane = [None]
rd_CM_plane_ref = [None]
rd_CM_ref = [None]
rd_CM_des = [None]
rd_CM = [None]
rd_CM_vec = [None]
rd_CM_ref_vec = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_footCenter_des = [None]
rd_exf_des = [None]
rd_root_des = [None]
rd_soft_const_vec = [None]
rd_root = [None]
rd_footL_vec = [None]
rd_footR_vec = [None]
rd_CMP = [None]
rd_DesPosL = [None]
rd_DesPosR = [None]
rd_DesForePosL = [None]
rd_DesForePosR = [None]
rd_DesRearPosL = [None]
rd_DesRearPosR = [None]
rd_Joint = [None]
rd_Joint2 = [None]
rd_Joint3 = [None]
rd_Joint4 = [None]
#rd_contactForces = [None]*10000
#rd_contactPositions = [None]*10000
rd_virtualForce = [None]
rootPos = [None]
selectedBodyId = [selectedBody]
extraForce = [None]
applyedExtraForce = [None]
applyedExtraForce[0] = [0, 0, 0]
normalVector = [[0, 2, 0]]
if MULTI_VIEWER:
viewer = ymv.MultiViewer(800, 655)
#viewer = ymv.MultiViewer(1600, 1255)
viewer.setRenderers1([
cvr.VpModelRenderer(motionModel, CHARACTER_COLOR, yr.POLYGON_FILL)
])
viewer.setRenderers2([
cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_FILL)
])
else:
viewer = ysv.SimpleViewer()
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'motionModel',
cvr.VpModelRenderer(motionModel, (100, 100, 100),
yr.POLYGON_FILL)) #(150,150,255)
viewer.doc.addRenderer(
'IKModel',
cvr.VpModelRenderer(IKModel, (180, 180, 180), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'controlModel',
cvr.VpModelRenderer(controlModel, CHARACTER_COLOR,
yr.POLYGON_FILL))
#viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
#viewer.doc.addRenderer('rd_footCenter_des', yr.PointsRenderer(rd_footCenter_des, (150,0,150)) )
#viewer.doc.addRenderer('rd_footCenterL', yr.PointsRenderer(rd_footCenterL))
#viewer.doc.addRenderer('rd_footCenterR', yr.PointsRenderer(rd_footCenterR))
viewer.doc.addRenderer('rd_CM_plane',
yr.PointsRenderer(rd_CM_plane, (255, 255, 0)))
viewer.doc.addRenderer('rd_CM',
yr.PointsRenderer(rd_CM, (255, 0, 255)))
viewer.doc.addRenderer('rd_CM_des',
yr.PointsRenderer(rd_CM_des, (64, 64, 255)))
viewer.doc.addRenderer(
'rd_CM_vec',
yr.VectorsRenderer(rd_CM_vec, rd_CM_plane, (255, 0, 0), 3))
#viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (0,255,0)))
#viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255,0,255)))
# viewer.doc.addRenderer('rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255,255,0)))
# viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0,255,0)))
#viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP, (0,255,255), .001))
viewer.doc.addRenderer(
'rd_exf_des',
yr.ForcesRenderer(rd_exf_des, rd_root_des, (0, 255, 0), .009,
0.04))
viewer.doc.addRenderer('rd_Joint',
yr.PointsRenderer(rd_Joint, (255, 0, 0)))
viewer.doc.addRenderer('rd_Joint2',
yr.PointsRenderer(rd_Joint2, (0, 255, 0)))
viewer.doc.addRenderer('rd_Joint3',
yr.PointsRenderer(rd_Joint3, (0, 0, 255)))
viewer.doc.addRenderer('rd_Joint4',
yr.PointsRenderer(rd_Joint4, (255, 255, 0)))
stage = STATIC_BALANCING
contactRendererName = []
for i in range(motion[0].skeleton.getJointNum()):
print(i, motion[0].skeleton.getJointName(i))
desCOMOffset = 0.0
pt = [0.]
timeReport = [0.] * 7
viewer.objectInfoWnd.comOffsetY.value(-0.05)
viewer.objectInfoWnd.comOffsetZ.value(0.00)
viewer.objectInfoWnd.begin()
viewer.objectInfoWnd.Bc = Fl_Value_Input(100, 450, 40, 10, 'Bc')
viewer.objectInfoWnd.Bc.value(0.1)
viewer.objectInfoWnd.end()
viewer.objectInfoWnd.labelKt.value(50)
viewer.objectInfoWnd.labelKk.value(17)
config['Phalange'] = [ motion[0].skeleton.getJointIndex('LeftPhalange_1'),\
motion[0].skeleton.getJointIndex('LeftPhalange_2'),\
motion[0].skeleton.getJointIndex('LeftPhalange_3'),\
motion[0].skeleton.getJointIndex('RightPhalange_1'),\
motion[0].skeleton.getJointIndex('RightPhalange_2'),\
motion[0].skeleton.getJointIndex('RightPhalange_3')]
config['Metatarsal'] = [motion[0].skeleton.getJointIndex('LeftMetatarsal_1'),\
motion[0].skeleton.getJointIndex('LeftMetatarsal_2'),\
motion[0].skeleton.getJointIndex('LeftMetatarsal_3'),\
motion[0].skeleton.getJointIndex('RightMetatarsal_1'),\
motion[0].skeleton.getJointIndex('RightMetatarsal_2'),\
motion[0].skeleton.getJointIndex('RightMetatarsal_3')]
config['Talus'] = [ motion[0].skeleton.getJointIndex('LeftTalus_1'),\
motion[0].skeleton.getJointIndex('LeftTalus_2'),\
motion[0].skeleton.getJointIndex('LeftTalus_3'),\
motion[0].skeleton.getJointIndex('RightTalus_1'),\
motion[0].skeleton.getJointIndex('RightTalus_2'),\
motion[0].skeleton.getJointIndex('RightTalus_3')]
config['Calcaneus'] = [ motion[0].skeleton.getJointIndex('LeftCalcaneus_1'),\
motion[0].skeleton.getJointIndex('LeftCalcaneus_2'),\
motion[0].skeleton.getJointIndex('LeftCalcaneus_3'),\
motion[0].skeleton.getJointIndex('RightCalcaneus_1'),\
motion[0].skeleton.getJointIndex('RightCalcaneus_2'),\
motion[0].skeleton.getJointIndex('RightCalcaneus_3')]
def simulateCallback(frame):
curTime = time.time()
if frame % 30 == 1: pt[0] = time.time()
global g_initFlag
global forceShowFrame
global forceApplyFrame
global JsysPre
global JsupPreL
global JsupPreR
global JsupPre
global softConstPoint
global stage
global contactRendererName
global desCOMOffset
#motionModel.update(motion[0])
Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, B_CM, B_CMSd, B_Toe = viewer.GetParam(
)
Dt = 2 * (Kt**.5)
Dk = 2 * (Kk**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
Dsc = 2 * (Ksc**.5)
'''
if Bsc == 0.0 :
viewer.doc.showRenderer('softConstraint', False)
viewer.motionViewWnd.update(1, viewer.doc)
else:
viewer.doc.showRenderer('softConstraint', True)
renderer1 = viewer.doc.getRenderer('softConstraint')
renderer1.rc.setLineWidth(0.1+Bsc*3)
viewer.motionViewWnd.update(1, viewer.doc)
'''
pose = motion[0].copy()
def solveIK(desComPos,
desIdxs,
desPos,
desOri,
cmW=10.,
posW=1.,
oriW=1.):
numItr = 100
dt = .5
threshold = 0.1
for i in range(0, numItr):
jPart_IK = []
print '----iter num', i
IKModel.update(pose)
th_r_IK = pose.getDOFPositions()
jointPositions_IK = pose.getJointPositionsGlobal()
jointAxeses_IK = pose.getDOFAxeses()
linkPositions_IK = IKModel.getBodyPositionsGlobal()
linkInertias_IK = IKModel.getBodyInertiasGlobal()
CM_IK = yrp.getCM(linkPositions_IK, linkMasses, totalMass)
print CM_IK
P_IK = ymt.getPureInertiaMatrix(TO, linkMasses,
linkPositions_IK, CM_IK,
linkInertias_IK)
yjc.computeJacobian2(Jsys_IK, DOFs, jointPositions_IK,
jointAxeses_IK, linkPositions_IK,
allLinkJointMasks)
for j in range(0, len(desIdxs)):
jPart_IK.append(Jsys_IK[6 * desIdxs[j]:6 * desIdxs[j] + 6])
J_IK, JAngCom_IK = np.vsplit(np.dot(P_IK, Jsys_IK), 2)
dv_IK = cmW * (desComPos - CM_IK)
for j in range(0, len(desIdxs)):
J_IK = np.vstack((J_IK, jPart_IK[j]))
pos_IK = IKModel.getBodyPositionGlobal(desIdxs[j])
dv_IK = np.append(dv_IK, posW * (desPos[j] - pos_IK))
ori_IK = IKModel.getBodyOrientationGlobal(desIdxs[j])
dv_IK = np.append(dv_IK,
oriW * mm.logSO3(desOri[j] * ori_IK.T))
#print dv_IK[0:3]
dth_IK_solve = npl.lstsq(J_IK, dv_IK)
dth_IK_x = dth_IK_solve[0][:totalDOF]
ype.nested(dth_IK_x, dth_IK)
#print dth_IK[0][0:3]
th_IK = yct.getIntegralDOF(th_r_IK, dth_IK, dt)
pose.setDOFPositions(th_IK)
if np.dot(dv_IK, dv_IK) < threshold:
break
linkPositions_ref = motionModel.getBodyPositionsGlobal()
CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass)
footCenterOffset = np.array([
viewer.objectInfoWnd.comOffsetX.value(),
viewer.objectInfoWnd.comOffsetY.value(),
viewer.objectInfoWnd.comOffsetZ.value()
])
#CM_IK_ref = footCenter + footCenterOffset
CM_IK_ref = CM_ref + footCenterOffset
#CM_IK_ref[1] = CM_ref[1] + footCenterOffset[1]
motion[0].skeleton.getJointIndex(config['supLink'])
#IKidxs = [indexFootL[0], indexFootR[0]]
#IKdesPos = [motionModel.getBodyPositionGlobal(indexFootL[0]), motionModel.getBodyPositionGlobal(indexFootR[0])]
#for i in range(0, 2):
# #IKdesPos[i] += ModelOffset
# IKdesPos[i][1] = 0.069
#IKori = [motionModel.getBodyOrientationGlobal(indexFootL[0]), motionModel.getBodyOrientationGlobal(indexFootR[0])]
#IKdesOri = [None]*2
#for i in range(0, 2):
# IKdesOri[i] = mm.I_SO3()
IKidxs = config['Phalange'][0:1] + config['Phalange'][3:4]
print IKidxs
IKdesPos = [None] * len(IKidxs)
IKdesOri = [None] * len(IKidxs)
for i in range(0, len(IKidxs)):
#print i
IKdesPos[i] = motionModel.getBodyPositionGlobal(IKidxs[i])
IKdesPos[i][1] = 0.03
IKdesOri[i] = mm.I_SO3()
print IKdesPos
solveIK(CM_IK_ref, IKidxs, IKdesPos, IKdesOri)
# tracking
th_r_ori = motion.getDOFPositions(frame)
th_r = copy.copy(th_r_ori)
global leftHipTimer
if viewer.objectInfoWnd.onLeftHip:
leftHipTimer = 60
viewer.objectInfoWnd.onLeftHip = False
if leftHipTimer > 0:
viewer.objectInfoWnd.comOffsetX.value(
0.14 * np.sin(2 * 3.14 * leftHipTimer / 60.))
#viewer.objectInfoWnd.comOffsetZ.value(0.04*np.cos(2*3.14*leftHipTimer/90.))
#B_Hipd = viewer.objectInfoWnd.labelLeftHip.value()
#newR1 = mm.exp(mm.v3(0.0,1.0,0.0), 3.14*0.5*B_Hipd/100.)
#idx = motion[0].skeleton.getJointIndex('LeftUpLeg')
#th_r[idx] = np.dot(th_r[idx], newR1)
#idx = motion[0].skeleton.getJointIndex('RightUpLeg')
#th_r[idx] = np.dot(th_r[idx], newR1)
leftHipTimer -= 1
timeReport[0] += time.time() - curTime
curTime = time.time()
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r,
Kt, Dt)
ddth_c = controlModel.getDOFAccelerations()
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
ype.flatten(ddth_c, ddth_c_flat)
# jacobian
refFootL = motionModel.getBodyPositionGlobal(supL)
refFootR = motionModel.getBodyPositionGlobal(supR)
positionFootL = [None] * footPartNum
positionFootR = [None] * footPartNum
for i in range(footPartNum):
positionFootL[i] = controlModel.getBodyPositionGlobal(
indexFootL[i])
positionFootR[i] = controlModel.getBodyPositionGlobal(
indexFootR[i])
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
jointPositions = controlModel.getJointPositionsGlobal()
jointAxeses = controlModel.getDOFAxeses()
CM = yrp.getCM(linkPositions, linkMasses, totalMass)
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
CM_plane = copy.copy(CM)
CM_plane[1] = 0.
dCM_plane = copy.copy(dCM)
dCM_plane[1] = 0.
linkPositions_ref = motionModel.getBodyPositionsGlobal()
linkVelocities_ref = motionModel.getBodyVelocitiesGlobal()
linkAngVelocities_ref = motionModel.getBodyAngVelocitiesGlobal()
linkInertias_ref = motionModel.getBodyInertiasGlobal()
CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass)
CM_plane_ref = copy.copy(CM_ref)
CM_plane_ref[1] = 0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
timeReport[1] += time.time() - curTime
curTime = time.time()
yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses,
linkPositions, allLinkJointMasks)
timeReport[2] += time.time() - curTime
curTime = time.time()
# yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks)
if frame > 0:
dJsys = (Jsys - JsysPre) * 30.
else:
dJsys = (Jsys - Jsys)
JsysPre = Jsys.copy()
timeReport[3] += time.time() - curTime
curTime = time.time()
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
for i in range(len(bodyIDsToCheck)):
controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0, 255)
contactFlagFootL = [0] * footPartNum
contactFlagFootR = [0] * footPartNum
partialDOFIndex = [22, 22]
for i in range(len(bodyIDs)):
controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105, 200)
index = controlModel.id2index(bodyIDs[i])
for j in range(len(indexFootL)):
if index == indexFootL[j]:
contactFlagFootL[j] = 1
for j in range(len(indexFootR)):
if index == indexFootR[j]:
contactFlagFootR[j] = 1
for j in range(0, footPartNum):
jAngFootR[j] = Jsys[6 * indexFootR[j]:6 * indexFootR[j] +
6][3:] #.copy()
jAngFootL[j] = Jsys[6 * indexFootL[j]:6 * indexFootL[j] +
6][3:] #.copy()
dJAngFootR[j] = dJsys[6 * indexFootR[j]:6 * indexFootR[j] +
6][3:] #.copy()
dJAngFootL[j] = dJsys[6 * indexFootL[j]:6 * indexFootL[j] +
6][3:] #.copy()
jFootR[j] = Jsys[6 * indexFootR[j]:6 * indexFootR[j] + 6] #.copy()
jFootL[j] = Jsys[6 * indexFootL[j]:6 * indexFootL[j] + 6] #.copy()
dJFootR[j] = dJsys[6 * indexFootR[j]:6 * indexFootR[j] +
6] #.copy()
dJFootL[j] = dJsys[6 * indexFootL[j]:6 * indexFootL[j] +
6] #.copy()
if footPartNum == 1:
desFCL = (controlModel.getBodyPositionGlobal(supL))
desFCR = (controlModel.getBodyPositionGlobal(supR))
else:
r = .5 + desCOMOffset
desFCL = (controlModel.getBodyPositionGlobal(indexFootL[0]) * r +
controlModel.getBodyPositionGlobal(indexFootL[1]) *
(1.0 - r)
) #controlModel.getBodyPositionGlobal(indexFootL[1])
desFCR = (controlModel.getBodyPositionGlobal(indexFootR[0]) * r +
controlModel.getBodyPositionGlobal(indexFootR[1]) *
(1.0 - r)
) #controlModel.getBodyPositionGlobal(indexFootR[1])
desFC = desFCL + (desFCR - desFCL) / 2.0
desFC[1] = 0
rd_footCenter_des[0] = desFC.copy()
curRelCMVec = CM_plane - desFC
vecRatio = mm.length(curRelCMVec) * 0.
#print(frame, vecRatio)
footCenter = desFC - curRelCMVec * (vecRatio) #/10.0
footCenter = (
getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') +
getBodyGlobalPos(controlModel, motion, 'LeftPhalange_1') +
getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') +
getBodyGlobalPos(controlModel, motion, 'RightPhalange_1')) / 4.
#footCenter = (getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'LeftTalus_1') + getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'RightTalus_1'))/4.
footCenter_ref = refFootL + (refFootR - refFootL) / 2.0
#footCenter_ref[1] = 0.
footCenter[1] = 0.
footCenterOffset = np.array([
viewer.objectInfoWnd.comOffsetX.value(), 0,
viewer.objectInfoWnd.comOffsetZ.value()
])
#footCenter += footCenterOffset
vecRatio = mm.length(curRelCMVec) * 0.
softConstPointOffset = -curRelCMVec * (vecRatio) #/10.0
#print(frame, vecRatio, softConstPointOffset)
desForeSupLAcc = [0, 0, 0]
desForeSupRAcc = [0, 0, 0]
totalNormalForce = [0, 0, 0]
for i in range(len(contactForces)):
totalNormalForce[0] += contactForces[i][0]
totalNormalForce[1] += contactForces[i][1]
totalNormalForce[2] += contactForces[i][2]
#print((totalMass*mm.s2v(wcfg.gravity))[1])
footCenterOffset = np.array([
viewer.objectInfoWnd.comOffsetX.value(),
viewer.objectInfoWnd.comOffsetY.value(),
viewer.objectInfoWnd.comOffsetZ.value()
])
######################
# optimization terms
######################
# linear momentum
CM_ref_plane = footCenter + footCenterOffset
dL_des_plane = Kl * totalMass * (CM_ref_plane -
CM_plane) - Dl * totalMass * dCM_plane
dL_des_plane[1] = Kl * totalMass * (CM_ref[1] + footCenterOffset[1] -
CM[1]) - Dl * totalMass * dCM[1]
# angular momentum
CP_ref = footCenter + footCenterOffset
timeStep = 30.
if CP_old[0] == None or CP == None:
dCP = None
else:
dCP = (CP - CP_old[0]) / (1 / timeStep)
CP_old[0] = CP
if CP != None and dCP != None:
ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP)
CP_des = CP + dCP * (1 / timeStep) + .5 * ddCP_des * (
(1 / timeStep)**2)
#dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity)))
dH_des = np.cross(
(CP_des - CM_plane),
(dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
else:
dH_des = None
# momentum matrix
RS = np.dot(P, Jsys)
R, S = np.vsplit(RS, 2)
rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat)
r_bias, s_bias = np.hsplit(rs, 2)
flagContact = True
if dH_des == None or np.any(np.isnan(dH_des)) == True:
flagContact = False
#viewer.doc.showRenderer('rd_grf_des', False)
#viewer.motionViewWnd.update(1, viewer.doc)
#else:
#viewer.doc.showRenderer('rd_grf_des', True)
#viewer.motionViewWnd.update(1, viewer.doc)
'''
0 : initial
1 : contact
2 : fly
3 : landing
'''
trackingW = w
#if checkAll(contactFlagFootR, 0) != 1 :
if 0: #stage == MOTION_TRACKING:
trackingW = w2
#stage = POWERFUL_BALANCING
Bt = Bt * 2
#######################
# optimization
#######################
mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat)
#if flagContact == True:
# mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
# mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
a_sup_2 = None
Jsup_2 = None
dJsup_2 = None
##############################
#if Jsup_2 != None:
# mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2)
timeReport[4] += time.time() - curTime
curTime = time.time()
r = problem.solve()
problem.clear()
ype.nested(r['x'], ddth_sol)
rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
timeReport[5] += time.time() - curTime
curTime = time.time()
for i in range(stepsPerFrame):
# apply penalty force
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
extraForce[0] = viewer.GetForce()
if (extraForce[0][0] != 0 or extraForce[0][1] != 0
or extraForce[0][2] != 0):
forceApplyFrame += 1
#vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
controlModel.applyBodyForceGlobal(selectedBody, extraForce[0])
applyedExtraForce[0] = extraForce[0]
if forceApplyFrame * wcfg.timeStep > 0.1:
viewer.ResetForce()
forceApplyFrame = 0
controlModel.setDOFAccelerations(ddth_sol)
controlModel.solveHybridDynamics()
vpWorld.step()
if frame % 30 == 0:
print 'elapsed time for 30 frames:', time.time() - pt[0]
# rendering
rd_footCenter[0] = footCenter
rd_CM[0] = CM.copy()
rd_CM_plane[0] = CM_plane.copy()
rd_footCenter_ref[0] = footCenter_ref
rd_CM_plane_ref[0] = CM_ref.copy()
rd_CM_ref[0] = CM_ref.copy()
rd_CM_ref_vec[0] = (CM_ref - footCenter_ref) * 3.
rd_CM_vec[0] = (CM - CM_plane)
rd_CM_des[0] = CM_ref_plane.copy()
rd_CM_des[0][1] = .01
#rd_CM_plane[0][1] = 0.
if CP != None and dCP != None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des
rd_dL_des_plane[0] = dL_des_plane
rd_dH_des[0] = dH_des
rd_grf_des[
0] = totalNormalForce # - totalMass*mm.s2v(wcfg.gravity)#dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
rd_exf_des[0] = applyedExtraForce[0]
rd_root_des[0] = rootPos[0]
rd_CMP[0] = softConstPoint
rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal(
constBody) - softConstPoint
#indexL = motion[0].skeleton.getJointIndex('Hips')
#indexR = motion[0].skeleton.getJointIndex('Spine1')
indexL = indexFootL[0]
indexR = indexFootR[0]
curAng = [controlModel.getBodyOrientationGlobal(indexL)]
curAngY = np.dot(curAng, np.array([0, 0, 1]))
rd_footL_vec[0] = np.copy(curAngY[0])
rd_footCenterL[0] = controlModel.getBodyPositionGlobal(indexL)
curAng = [controlModel.getBodyOrientationGlobal(indexR)]
curAngY = np.dot(curAng, np.array([0, 0, 1]))
rd_footR_vec[0] = np.copy(curAngY[0])
rd_footCenterR[0] = controlModel.getBodyPositionGlobal(indexR)
if (forceApplyFrame == 0):
applyedExtraForce[0] = [0, 0, 0]
timeReport[6] += time.time() - curTime
# print timeReport
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def test_getStitchedNextMotion_analysis():
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
motion = yf.readBvhFile(bvhFilePath, .01)
# global
part1 = motion[:50].copy()
part2 = motion[49:].copy()
R_offset_orig_global = mm.rotY(math.pi / 2)
part2.rotateTrajectory(R_offset_orig_global)
print('R_offset_orig_global =\n', R_offset_orig_global)
R_part1 = part1[-1].getJointOrientationLocal(0)
R_part2 = part2[0].getJointOrientationLocal(0)
print('R_part2 =\n', np.dot(R_offset_orig_global, R_part1))
R_offset_global = np.dot(part2[0].localRs[0], part1[-1].localRs[0].T)
print('R_offset_global =\n',
np.dot(part2[0].localRs[0], part1[-1].localRs[0].T))
part2_attached_global = part2.copy()
part2_attached_global.translateByOffset((0, 0, 1))
part2_attached_global.rotateTrajectory(R_offset_global.T)
part2_attached_global.translateByOffset((0, 0, -1))
# local
part1 = motion[:50].copy()
part2 = motion[49:].copy()
R_offset_orig_local = mm.rotY(math.pi / 2)
part2.rotateTrajectoryLocal(R_offset_orig_local)
print('R_offset_orig_local =\n', R_offset_orig_local)
R_part1 = part1[-1].getJointOrientationLocal(0)
R_part2 = part2[0].getJointOrientationLocal(0)
print('R_part2 =\n', np.dot(R_part1, R_offset_orig_local))
d = part2[0] - part1[
-1] # np.dot(part1[-1].localRs[0].T, part2[0].localRs[0])
R_offset_local = d.getJointOrientationLocal(0)
print('R_offset_local =\n', R_offset_local)
part2_attached_local = part2.copy()
part2_attached_local.translateByOffset((0, 0, 1))
part2_attached_local.rotateTrajectoryLocal(R_offset_local.T)
part2_attached_local.translateByOffset((0, 0, -1))
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion',
yr.JointMotionRenderer(motion, (100, 100, 255), yr.LINK_LINE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'part1',
yr.JointMotionRenderer(part1, (255, 100, 255), yr.LINK_LINE))
viewer.doc.addObject('part1', part1)
viewer.doc.addRenderer(
'part2',
yr.JointMotionRenderer(part2, (100, 255, 255), yr.LINK_LINE))
viewer.doc.addObject('part2', part2)
# viewer.doc.addRenderer('part1[-1]', yr.JointMotionRenderer(ym.JointMotion([part1[-1]]), (255,100,255), yr.LINK_LINE))
# viewer.doc.addRenderer('part2[0]', yr.JointMotionRenderer(ym.JointMotion([part2[0]]), (100,255,255), yr.LINK_LINE))
# viewer.doc.addRenderer('part2_attached_global', yr.JointMotionRenderer(part2_attached_global, (255,0,0), yr.LINK_LINE))
# viewer.doc.addObject('part2_attached_global', part2_attached_global)
# viewer.doc.addRenderer('part2_attached_local', yr.JointMotionRenderer(part2_attached_local, (0,255,0), yr.LINK_LINE))
# viewer.doc.addObject('part2_attached_local', part2_attached_local)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def test_rotateJoint_amc():
#=======================================================================
# bvh
#=======================================================================
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
bvh_motion = yf.readBvhFile(bvhFilePath, .01)
removeJoint(bvh_motion, 'Head')
removeJoint(bvh_motion, 'RightShoulder')
removeJoint(bvh_motion, 'LeftShoulder1')
removeJoint(bvh_motion, 'RightToes_Effector')
removeJoint(bvh_motion, 'LeftToes_Effector')
removeJoint(bvh_motion, 'RightHand_Effector')
removeJoint(bvh_motion, 'LeftHand_Effector')
offsetJointLocal(bvh_motion, 'RightArm', (.03,-.05,0))
offsetJointLocal(bvh_motion, 'LeftArm', (-.03,-.05,0))
bvh_tpose = bvh_motion[0].getTPose()
bvh_tpose.translateByTarget((0,0,0))
print bvh_tpose.skeleton
#=======================================================================
# from amc
#=======================================================================
bvhFilePath = '../samples/wd2_left_turn.bvh'
amc_motion = yf.readBvhFile(bvhFilePath, .01 * 2.53999905501)
removeJoint(amc_motion, 'RightUpLegDummy')
removeJoint(amc_motion, 'SpineDummy')
removeJoint(amc_motion, 'HEadDummy')
removeJoint(amc_motion, 'LeftShoulder1Dummy')
removeJoint(amc_motion, 'RightShoulderDummy')
removeJoint(amc_motion, 'LeftUpLegDummy')
removeJoint(amc_motion, 'Head')
removeJoint(amc_motion, 'RightShoulder')
removeJoint(amc_motion, 'LeftShoulder1')
removeJoint(amc_motion, 'RightToes_Effector')
removeJoint(amc_motion, 'LeftToes_Effector')
removeJoint(amc_motion, 'RightHand_Effector')
removeJoint(amc_motion, 'LeftHand_Effector')
offsetJointLocal(amc_motion, 'RightArm', (.03,-.05,0))
offsetJointLocal(amc_motion, 'LeftArm', (-.03,-.05,0))
amc_tpose = amc_motion[0].getTPose()
amc_tpose.translateByTarget((0,0,0))
print amc_tpose.skeleton
# edited amc
amc_motion2 = copy.deepcopy(amc_motion)
amc_tpose2 = amc_motion2[0].getTPose()
amc_tpose2.translateByTarget((0,0,0))
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('bvh_tpose', yr.JointMotionRenderer(ym.JointMotion([bvh_tpose]), (255,0,0), yr.LINK_LINE))
viewer.doc.addObject('bvh_tpose', bvh_tpose)
viewer.doc.addRenderer('amc_tpose', yr.JointMotionRenderer(ym.JointMotion([amc_tpose]), (255,255,0), yr.LINK_LINE))
viewer.doc.addObject('amc_tpose', amc_tpose)
viewer.doc.addRenderer('amc_tpose2', yr.JointMotionRenderer(ym.JointMotion([amc_tpose2]), (0,255,255), yr.LINK_LINE))
viewer.doc.addObject('amc_tpose2', amc_tpose2)
viewer.doc.addRenderer('amc_motion', yr.JointMotionRenderer(amc_motion, (0,100,255), yr.LINK_LINE))
viewer.doc.addObject('amc_motion', amc_motion)
viewer.doc.addRenderer('amc_motion2', yr.JointMotionRenderer(amc_motion2, (0,255,0), yr.LINK_LINE))
viewer.doc.addObject('amc_motion2', amc_motion2)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def test_removeJoint():
# skeleton 1
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
motion = yf.readBvhFile(bvhFilePath, .01)
print motion[0].skeleton
editedMotion = copy.deepcopy(motion)
removeJoint(editedMotion, 'Head')
removeJoint(editedMotion, 'RightShoulder')
removeJoint(editedMotion, 'LeftShoulder1')
removeJoint(editedMotion, 'RightToes_Effector')
removeJoint(editedMotion, 'LeftToes_Effector')
removeJoint(editedMotion, 'RightHand_Effector')
removeJoint(editedMotion, 'LeftHand_Effector')
offsetJointLocal(editedMotion, 'RightArm', (.03,-.05,0))
offsetJointLocal(editedMotion, 'LeftArm', (-.03,-.05,0))
print editedMotion[0].skeleton
editedTPose = editedMotion[0].getTPose()
editedTPose.translateByTarget((0,0,0))
# skeleton 2
bvhFilePath = '../samples/wd2_left_turn.bvh'
motion = yf.readBvhFile(bvhFilePath, .01 * 2.53999905501)
print motion[0].skeleton
editedMotion2 = copy.deepcopy(motion)
removeJoint(editedMotion2, 'RightUpLegDummy')
removeJoint(editedMotion2, 'SpineDummy')
removeJoint(editedMotion2, 'HEadDummy')
removeJoint(editedMotion2, 'LeftShoulder1Dummy')
removeJoint(editedMotion2, 'RightShoulderDummy')
removeJoint(editedMotion2, 'LeftUpLegDummy')
removeJoint(editedMotion2, 'Head')
removeJoint(editedMotion2, 'RightShoulder')
removeJoint(editedMotion2, 'LeftShoulder1')
removeJoint(editedMotion2, 'RightToes_Effector')
removeJoint(editedMotion2, 'LeftToes_Effector')
removeJoint(editedMotion2, 'RightHand_Effector')
removeJoint(editedMotion2, 'LeftHand_Effector')
offsetJointLocal(editedMotion2, 'RightArm', (.03,-.05,0))
offsetJointLocal(editedMotion2, 'LeftArm', (-.03,-.05,0))
print editedMotion2[0].skeleton
editedTPose2 = editedMotion2[0].getTPose()
editedTPose2.translateByTarget((0,0,0))
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_LINE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('editedMotion', yr.JointMotionRenderer(editedMotion, (0,255,0), yr.LINK_LINE))
viewer.doc.addObject('editedMotion', editedMotion)
viewer.doc.addRenderer('editedTPose', yr.JointMotionRenderer(ym.JointMotion([editedTPose]), (255,0,0), yr.LINK_LINE))
viewer.doc.addObject('editedTPose', editedTPose)
viewer.doc.addRenderer('editedTPose2', yr.JointMotionRenderer(ym.JointMotion([editedTPose2]), (255,255,0), yr.LINK_LINE))
viewer.doc.addObject('editedTPose2', editedTPose2)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def simulation_test():
Kt = 20.; Dt = 2*(Kt**.5)
Ks = 2000.; Ds = 2*(Ks**.5)
mu = 1.
dir = './icmotion_last/'
filename = 'stop_left_normal.temp'
# filename = 'left_left_normal.temp'
# filename = 'right_left_fast.temp'
motion_ori = yf.readBvhFile(dir+filename)
frameTime = 1/motion_ori.fps
motion_ori[0:0] = [motion_ori[0]]*20
mcfgfile = open(dir + 'mcfg', 'rb')
mcfg = pickle.load(mcfgfile)
mcfgfile.close()
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
# wcfg.lockingVel = c_locking_vel
stepsPerFrame = 30
wcfg.timeStep = (frameTime)/stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion_ori[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion_ori[0], mcfg)
vpWorld.initialize()
print(controlModel)
controlModel.initializeHybridDynamics()
bodyIDsToCheck = range(vpWorld.getBodyNum())
mus = [mu]*len(bodyIDsToCheck)
viewer = ysv.SimpleViewer()
# viewer.record(False)
# viewer.doc.addRenderer('motion_ori', yr.JointMotionRenderer(motion_ori, (0,100,255), yr.LINK_BONE))
viewer.doc.addObject('motion_ori', motion_ori)
viewer.doc.addRenderer('motionModel', cvr.VpModelRenderer(motionModel, (0,150,255), yr.POLYGON_LINE))
viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (200,200,200), yr.POLYGON_LINE))
def simulateCallback(frame):
th_r = motion_ori.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion_ori.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion_ori.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
for i in range(stepsPerFrame):
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces)
controlModel.setDOFAccelerations(ddth_des)
controlModel.solveHybridDynamics()
vpWorld.step()
motionModel.update(motion_ori[frame])
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(frameTime / 1.4)
viewer.show()
Fl.run()
def preprocess():
tasks = []
outputDir = './ppmotion/'
dir = '../Data/woody2/Motion/Physics2/'
# box foot
# config = {'repeat':True, 'footRot': mm.rotX(-.4), 'yOffset':0., 'halfFootHeight': 0.0444444444444, 'scale':.01, 'type':'woody2'}
# segmented foot
config = {'repeat':True, 'footRot': mm.rotX(.07), 'yOffset':0., 'halfFootHeight': 0.0944444444444, 'scale':.01, 'type':'woody2'}
paths = []
# paths.append(dir+'wd2_WalkSameSame01.bvh')
# paths.append(dir+'wd2_WalkForwardSlow01.bvh')
paths.append(dir+'wd2_WalkForwardNormal00.bvh')
# paths.append(dir+'wd2_WalkHandWav00.bvh')
# paths.append(dir+'wd2_WalkForwardFast00.bvh')
# paths.append(dir+'wd2_WalkForwardVFast00.bvh')
# paths.append(dir+'wd2_WalkLean00.bvh')
# paths.append(dir+'wd2_WalkAzuma01.bvh')
# paths.append(dir+'wd2_WalkSoldier00.bvh')
# paths.append(dir+'wd2_WalkSukiko00.bvh')
# paths.append(dir+'wd2_WalkBackward00.bvh')
# paths.append(dir+'wd2_WalkTongTong00.bvh')
tasks.append({'config':config, 'paths':paths})
##
# dir = '../Data/woody2/Motion/Balancing/'
# config = {'footRot': mm.exp(mm.v3(1,-.5,0), -.6), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01, 'type':'woody2'}
# paths = []
# paths.append(dir+'wd2_2foot_walk_turn.bvh')
# paths.append(dir+'wd2_2foot_walk_turn2.bvh')
# paths.append(dir+'wd2_slow_2foot_hop.bvh')
# paths.append(dir+'wd2_short_broad_jump.bvh')
# paths.append(dir+'wd2_long_broad_jump.bvh')
# paths.append(dir+'wd2_ffast_cancan_run.bvh')
# paths.append(dir+'wd2_fast_cancan_run.bvh')
# paths.append(dir+'wd2_fast_2foot_hop.bvh')
# paths.append(dir+'wd2_1foot_contact_run.bvh')
# paths.append(dir+'wd2_1foot_contact_run2.bvh')
## tasks.append({'config':config, 'paths':paths})
#
## dir = '../Data/woody2/Motion/Samsung/'
## config = {'footRot': mm.rotX(-.46), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
## paths = []
## paths.append(dir+'wd2_left_turn.bvh')
## paths.append(dir+'wd2_right_turn.bvh')
## paths.append(dir+'wd2_pose_inner1.bvh')
## paths.append(dir+'wd2_pose_inner2.bvh')
## tasks.append({'config':config, 'paths':paths})
#
# dir = '../Data/woody2/Motion/Picking/'
# config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01, 'type':'woody2'}
# paths = []
# paths.append(dir+'wd2_pick_walk_1.bvh')
## tasks.append({'config':config, 'paths':paths})
#
# dir = '../Data/woody2/Motion/VideoMotion/'
# config = {'footRot': mm.rotX(-.48), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
## paths = glob.glob(dir+'*.bvh')
# paths = []
# paths.append(dir+'wd2_cross_walk_90d_fast_27.bvh')
## tasks.append({'config':config, 'paths':paths})
#
# dir = '../Data/woody2/Motion/Walking/'
# config = {'footRot': mm.rotX(-.40), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
# paths = glob.glob(dir+'*.bvh')
## tasks.append({'config':config, 'paths':paths})
#
## dir = '../Data/woody2/Motion/samsung_boxing/round/'
## config = {'footRot': mm.rotX(-.65), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
## paths = glob.glob(dir+'*.bvh')
## tasks.append({'config':config, 'paths':paths})
#
## dir = '../Data/woody2/Motion/samsung_boxing/boxman/'
## config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
## paths = glob.glob(dir+'*.bvh')
## tasks.append({'config':config, 'paths':paths})
#
# dir = '../Data/woody2/Motion/motion_edit/'
# config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
# paths = glob.glob(dir+'*.bvh')
## tasks.append({'config':config, 'paths':paths})
#
## outputDir = './icmotion_test/'
## dir = './rawmotion/'
## config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':1., 'type':'woody2_new'}
## paths = glob.glob(dir+'*.temp')
## tasks.append({'config':config, 'paths':paths})
#
# outputDir = './icmotion_last/'
# dir = './lastmotion/add/'
## config = {'rootRot':mm.rotZ(.05), 'footRot': mm.rotX(-.5), 'leftFootR':mm.rotZ(-.1), \
# config = {'rootRot':mm.rotZ(.0), 'footRot': np.dot(mm.rotX(-.5), mm.rotZ(.04)), 'leftFootR':mm.rotZ(-.2), \
# 'halfFootHeight': 0.0444444444444, 'scale':1., 'type':'woody2_new'}
# paths = glob.glob(dir+'*.temp')
# tasks.append({'config':config, 'paths':paths})
# outputDir = './ppmotion_slope/'
# dir = './rawmotion_slope_extracted/'
# config = {'rootRot':mm.rotZ(.0), 'footRot': np.dot(mm.rotX(-.55), mm.rotZ(.04)), 'leftFootR':mm.rotZ(-.2), \
# 'halfFootHeight': 0.0444444444444, 'scale':1., 'type':'woody2_new'}
# paths = glob.glob(dir+'*.bvh')
# tasks.append({'config':config, 'paths':paths})
VISUALIZE = False
for task in tasks:
config = task['config']
paths = task['paths']
for path in paths:
motion = yf.readBvhFile(path)
normalizeSkeleton(motion, config)
adjustHeight(motion, config['halfFootHeight'])
additionalEdit(motion, path)
outputPath = outputDir + os.path.basename(path)
yf.writeBvhFile(outputPath, motion)
print(outputPath, 'done')
if 'repeat' in config and config['repeat']:
hRef = .1; vRef = .3
lc = yma.getElementContactStates(motion, 'LeftFoot', hRef, vRef)
interval = yba.getWalkingCycle2(motion, lc)
transitionLength = 20 if 'wd2_WalkAzuma01.bvh' in path else 10
motion = ymt.repeatCycle(motion, interval, 50, transitionLength)
outputName = os.path.splitext(os.path.basename(path))[0]+'_REPEATED.bvh'
outputPath = outputDir + outputName
yf.writeBvhFile(outputPath, motion)
print(outputPath, 'done')
if VISUALIZE:
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,100,255), yr.LINK_LINE))
viewer.doc.addObject('motion', motion)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
print('FINISHED')
def test_extendByIntegration():
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
motion_ref = yf.readBvhFile(bvhFilePath, .01)
linkStyle = yr.LINK_LINE
# bvhFilePath = '../samples/chain_2_rotate_expt_root.bvh'
# motion_ref = yf.readBvhFile(bvhFilePath, 1.)
# linkStyle = yr.LINK_WIREBOX
cut = 96
motion = motion_ref[:cut]
skeleton = motion[0].skeleton
print skeleton
lFoot = skeleton.getJointIndex('LeftFoot')
rFoot = skeleton.getJointIndex('RightFoot')
lKnee = skeleton.getJointIndex('LeftLeg')
rKnee = skeleton.getJointIndex('RightLeg')
lLeg = skeleton.getJointIndex('LeftUpLeg')
rLeg = skeleton.getJointIndex('RightUpLeg')
motion_extended_by_5_frames = copy.deepcopy(motion)
motion_extended_by_5_frames.extend(extendByIntegration(motion_extended_by_5_frames, 5))
motion_extended_by_one_frame = copy.deepcopy(motion)
for i in range(5):
motion_extended_by_one_frame.extend(extendByIntegration(motion_extended_by_one_frame, 1))
motion_extended_preserve_foot = copy.deepcopy(motion)
for i in range(5):
motion_extended_preserve_foot.extend(extendByIntegration(motion_extended_preserve_foot, 1, (lFoot, rFoot)))
motion_extended_preserve_leg = copy.deepcopy(motion)
for i in range(5):
motion_extended_preserve_leg.extend(extendByIntegration(motion_extended_preserve_leg, 1, (lFoot, rFoot, lKnee, rKnee, lLeg, rLeg)))
motion_extended_preserve_knee = copy.deepcopy(motion)
for i in range(5):
motion_extended_preserve_knee.extend(extendByIntegration(motion_extended_preserve_knee, 1, (lFoot, rFoot)))
motion_extended_ik = copy.deepcopy(motion)
for i in range(5):
motion_extended_ik.extend(extendByIntegrationIK(motion_extended_ik, 1, rFoot, (lFoot, rFoot, lKnee, rKnee, lLeg, rLeg)))
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('motion_ref', yr.JointMotionRenderer(motion_ref, (100,100,255), linkStyle))
viewer.doc.addObject('motion_ref', motion_ref)
viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (255,0,0), linkStyle))
viewer.doc.addObject('motion', motion)
# viewer.doc.addRenderer('motion_extended_by_5_frames', yr.JointMotionRenderer(motion_extended_by_5_frames, (0,255,0), linkStyle))
# viewer.doc.addObject('motion_extended_by_5_frames', motion_extended_by_5_frames)
# viewer.doc.addRenderer('motion_extended_by_one_frame', yr.JointMotionRenderer(motion_extended_by_one_frame, (255,255,0), linkStyle))
# viewer.doc.addObject('motion_extended_by_one_frame', motion_extended_by_one_frame)
# viewer.doc.addRenderer('motion_extended_preserve_foot', yr.JointMotionRenderer(motion_extended_preserve_foot, (255,0,255), linkStyle))
# viewer.doc.addObject('motion_extended_preserve_foot', motion_extended_preserve_foot)
# viewer.doc.addRenderer('motion_extended_preserve_leg', yr.JointMotionRenderer(motion_extended_preserve_leg, (0,255,255), linkStyle))
# viewer.doc.addObject('motion_extended_preserve_leg', motion_extended_preserve_leg)
viewer.doc.addRenderer('motion_extended_preserve_knee', yr.JointMotionRenderer(motion_extended_preserve_knee, (0,255,255), linkStyle))
viewer.doc.addObject('motion_extended_preserve_knee', motion_extended_preserve_knee)
viewer.doc.addRenderer('motion_extended_ik', yr.JointMotionRenderer(motion_extended_ik, (0,255,0), linkStyle))
viewer.doc.addObject('motion_extended_ik', motion_extended_ik)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def main():
np.set_printoptions(precision=4, linewidth=200)
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5()
motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped()
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
#controlModel2 = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
controlModel.translateByOffset((1.5,0,0))
#controlModel2.initializeHybridDynamics()
#controlModel2.translateByOffset((2.5,0,0))
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
# parameter
Kt = config['Kt']; Dt = config['Dt'] # tracking gain
Kl = config['Kl']; Dl = config['Dl'] # linear balance gain
Kh = config['Kh']; Dh = config['Dh'] # angular balance gain
Ks = config['Ks']; Ds = config['Ds'] # penalty force spring gain
Bt = config['Bt']
Bl = config['Bl']
Bh = config['Bh']
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
sup = motion[0].skeleton.getJointIndex(config['supLink'])
sup2 = motion[0].skeleton.getJointIndex(config['supLink2'])
# jacobian
Jsup = yjc.makeEmptyJacobian(DOFs, 1)
dJsup = Jsup.copy()
Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum())
dJsys = Jsys.copy()
supJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, sup)]
allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton)
Jsup2 = yjc.makeEmptyJacobian(DOFs, 1)
dJsup2 = Jsup2.copy()
supJointMasks2 = [yjc.getLinkJointMask(motion[0].skeleton, sup2)]
CMJointMask = supJointMasks
for iMask in range(len(CMJointMask[0])):
if supJointMasks2[0][iMask] == 1:
CMJointMask[0][iMask] = 1
Jt = yjc.makeEmptyJacobian(DOFs, 1)
# momentum matrix
linkMasses = controlModel.getBodyMasses()
totalMass = controlModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
problem = yac.LSE(totalDOF, 6)
a_sup = (0,0,0, 0,0,0)
CP_old = [mm.v3(0.,0.,0.)]
# penalty method
bodyIDsToCheck = range(vpWorld.getBodyNum())
mus = [1.]*len(bodyIDsToCheck)
##### Parameter
'''
(0, 'Hips')
(1, 'LeftUpLeg')
(2, 'LeftLeg')
(3, 'LeftFoot')
(4, 'RightUpLeg')
(5, 'RightLeg')
(6, 'RightFoot')
(7, 'Spine')
(8, 'Spine1')
(9, 'LeftArm')
(10, 'LeftForeArm')
(11, 'RightArm')
(12, 'RightForeArm')
'''
Kp = [None]*len(bodyIDsToCheck)
Kd = [None]*len(bodyIDsToCheck)
#Kp = [0, 400, 450, 400, 400, 450, 400, 300, 250, 60, 25, 60, 25]
#Kp = [0, 450, 450, 400, 450, 450, 400, 450, 300, 100, 25, 100, 25]
UpLeg = 310
Leg = 400
Foot = 310
Spine = 350
Neck = 200
Arm = 10
Hand = 10
Kp = [0, UpLeg, Leg, Foot, UpLeg, Leg, Foot, Spine, Neck, Arm, Hand, Arm, Hand]
'''
for i in range(0, len(Kp)) :
Kp[i] = Kp[i]*0.8
'''
#Kd = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
#Kd = [0, 2.0, 1.5, 1.2, 2.0, 1.5, 1.2, 2.0, 0.1, 0.1, 0.1, 0.1, 0.1]
#Kd = [0, 1.5, 2.0, 1.2, 1.5, 2.0, 1.2, 1.5, 0.4, 0.5, 0.2, 0.5, 0.2]
'''
dUpLeg = 1.7
dLeg = 1.7
dFoot = 1.2
dSpine = 1.4
dNeck = 0.9
dArm = 0.8
dHand = 0.2
'''
dUpLeg = 2*(UpLeg**.5)
dLeg = 2*(Leg**.5)
dFoot = 2*(Foot**.5)
dSpine = 2*(Spine**.5)
dNeck = 2*(Neck**.5)
dArm = 2*(Arm**.5)
dHand = 2*(Hand**.5)
Kd = [0, dUpLeg, dLeg, dFoot, dUpLeg, dLeg, dFoot, dSpine, dNeck, dArm, dHand, dArm, dHand]
for i in range(0, len(Kd)) :
Kd[i] = Kd[i]*0.022
'''
for ii in bodyIDsToCheck :
print(ii, controlModel.index2name(ii))
'''
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
# viewer
rd_footCenter = [None]
rd_footCenter1 = [None]
rd_footCenter2 = [None]
rd_CM_plane = [None]
rd_CM = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_vf = [None]
rd_contactPoint1 = [None]
rd_contactPoint2 = [None]
viewer = ysv.SimpleViewer()
# viewer.record(False)
# viewer.doc.addRenderer('control', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('motionModel', cvr.VpModelRenderer(motionModel, (150,150,255), yr.POLYGON_FILL))
viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL))
#viewer.doc.addRenderer('controlModel2', cvr.VpModelRenderer(controlModel2, (155,100,100), yr.POLYGON_FILL))
#viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
#viewer.doc.addRenderer('rd_footCenter1', yr.PointsRenderer(rd_footCenter1, (255, 0, 255)))
#viewer.doc.addRenderer('rd_footCenter2', yr.PointsRenderer(rd_footCenter2, (0, 255, 255)))
#viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255,255,0)))
#viewer.doc.addRenderer('rd_CM', yr.PointsRenderer(rd_CM, (255,255,0)))
#viewer.doc.addRenderer('rd_contactPoint', yr.PointsRenderer(rd_contactPoint1, (0,0,255)))
#viewer.doc.addRenderer('rd_contactPoint2', yr.PointsRenderer(rd_contactPoint2, (0,255,255)))
#viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (255,0,255)))
# viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255,0,255)))
# viewer.doc.addRenderer('rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255,255,0)))
# viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0,255,0)))
#viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001))
viewer.doc.addRenderer('rd_vf', yr.ForcesRenderer(rd_vf, rd_CM, (0,0,255), .005))
global stage
stage = 0
def simulateCallback(frame):
motionModel.update(motion[frame])
# tracking
th_r = motion.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
'''
th2 = controlModel2.getDOFPositions()
dth2 = controlModel2.getDOFVelocities()
ddth_des2 = yct.getDesiredDOFAccelerations(th_r, th2, dth_r, dth2, ddth_r, Kt, Dt)
'''
#ype.flatten(ddth_des, ddth_des_flat)
#ype.flatten(dth, dth_flat)
#Control
#tracking control
#print(Kt, Dt)
#Tpd = yct.getDesiredDOFTorques(th_r, th, dth_r, dth, 100.0, 0.1)#0.65, 0.031)
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
jointPositions = controlModel.getJointPositionsGlobal()
jointAxeses = controlModel.getDOFAxeses()
CM = yrp.getCM(linkPositions, linkMasses, totalMass)
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
footCenter1 = controlModel.getBodyPositionGlobal(sup)
footCenter2 = controlModel.getBodyPositionGlobal(sup2)
footCenter = (footCenter1+footCenter2)/2
'''
yjc.computeJacobian2(Jt, DOFs, jointPositions, jointAxeses, [CM], CMJointMask)
pHatCom = CM - footCenter
vCom = dCM
'''
CM_plane = copy.copy(CM); CM_plane[1]=0.
dCM_plane = copy.copy(dCM); dCM_plane[1]=0.
CM_ref_plane = footCenter
dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane
dL_des_plane[1] = 0.
CP_ref = footCenter
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0]==None or CP==None:
dCP = None
else:
dCP = (CP - CP_old[0])/(1/30.)
CP_old[0] = CP
if CP!=None and dCP!=None:
ddCP_des = Kh*(CP_ref - CP) - Dh*(dCP)
CP_des = CP + dCP*(1/30.) + .5*ddCP_des*((1/30.)**2)
dH_des = np.cross((CP_des - CM), (dL_des_plane - totalMass*mm.s2v(wcfg.gravity)))
else:
dH_des = None
################
'''
linkPositions = motionModel.getBodyPositionsGlobal()
linkVelocities = motionModel.getBodyVelocitiesGlobal()
linkAngVelocities = motionModel.getBodyAngVelocitiesGlobal()
linkInertias = motionModel.getBodyInertiasGlobal()
CM2 = yrp.getCM(linkPositions, linkMasses, totalMass)
dCM2 = yrp.getCM(linkVelocities, linkMasses, totalMass)
footCenter1 = motionModel.getBodyPositionGlobal(sup)
footCenter2 = motionModel.getBodyPositionGlobal(sup2)
footCenter = (footCenter1+footCenter2)/2
pHatComDes = CM2 - footCenter
vComDes = dCM2
Wcp = -750
Wcv = -10
fCom = Wcp*(pHatComDes - pHatCom) + Wcv*(vComDes - vCom)
'''
#print("VirtualForce", fCom)
#fCom[0] = 0.
#fCom[1] = 0.
#fCom[2] = -20.
#fCom = [0., 0., 100.]
#print("VirtualForce", fCom)
for i in range(stepsPerFrame):
# apply penalty force
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces)
controlModel.setDOFAccelerations(ddth_des)
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
jointPositions = controlModel.getJointPositionsGlobal()
jointAxeses = controlModel.getDOFAxeses()
CM = yrp.getCM(linkPositions, linkMasses, totalMass)
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
footCenter1 = controlModel.getBodyPositionGlobal(sup)
footCenter2 = controlModel.getBodyPositionGlobal(sup2)
footCenter = (footCenter1+footCenter2)/2
CM_plane = copy.copy(CM); CM_plane[1]=0.
footCenter_plane = copy.copy(footCenter); footCenter_plane[1] = 0.
yjc.computeJacobian2(Jt, DOFs, jointPositions, jointAxeses, [CM], CMJointMask)
pHatCom = CM - footCenter
vCom = dCM
linkPositions2 = motionModel.getBodyPositionsGlobal()
linkVelocities2 = motionModel.getBodyVelocitiesGlobal()
linkAngVelocities2 = motionModel.getBodyAngVelocitiesGlobal()
linkInertias2 = motionModel.getBodyInertiasGlobal()
CM2 = yrp.getCM(linkPositions2, linkMasses, totalMass)
dCM2 = yrp.getCM(linkVelocities2, linkMasses, totalMass)
footCenter1 = motionModel.getBodyPositionGlobal(sup)
footCenter2 = motionModel.getBodyPositionGlobal(sup2)
footCenter = (footCenter1+footCenter2)/2
pHatComDes = CM2 - footCenter
vComDes = dCM2
Wcp = 0
Wcv = 0
Wcm = 0
'''
0 : initial
1 : contact
2 : fly
3 : landing
'''
global stage
if len(contactForces) == 0 :
if stage == 1:
stage = 2
print("fly")
else:
if stage == 0:
stage = 1
print("contact")
elif stage == 2:
stage = 3
print("landing")
Wcp = -550
Wcv = -100
Wcm = 0
if stage == 1:
Wcp = -550
Wcv = -100
Wcm = 0
#Wcp = -550
#Wcv = -100
elif stage == 3:
#Wcp = -950
#Wcv = -300
Wcp = -950
Wcv = -300
Wcm = 0
# COM Position control
fCom = Wcp*(pHatComDes - pHatCom) + Wcv*(vComDes - vCom) + Wcm*(footCenter_plane - CM_plane)
if len(contactForces) == 0 :
fCom[0] = 0.
fCom[1] = -10. #-10
fCom[2] = 0.
'''
fCom[0] = 10.
fCom[1] = 0.
fCom[2] = 250.
'''
# Angular Momentum control
L_ref = 0
L_con = 0
#R,i
for i in range(1, controlModel.getBodyNum()):
L_ref += (linkMasses[i]*np.cross(linkPositions2[i]-[CM], linkVelocities2[i])+linkInertias2[i]*linkAngVelocities2[i])
L_con += (linkMasses[i]*np.cross(linkPositions[i]-[CM], linkVelocities[i])+linkInertias[i]*linkAngVelocities[i])
for iJoint in range(1, 7): # from 'LeftUpLeg'(1) to 'RightFoot'(6)
JpT1 = ( Jt[0][6+3*(iJoint-1)], Jt[1][6+3*(iJoint-1)], Jt[2][6+3*(iJoint-1)])
JpT2 = ( Jt[0][6+3*(iJoint-1)+1], Jt[1][6+3*(iJoint-1)+1], Jt[2][6+3*(iJoint-1)+1])
JpT3 = ( Jt[0][6+3*(iJoint-1)+2], Jt[1][6+3*(iJoint-1)+2], Jt[2][6+3*(iJoint-1)+2])
Tfi = (np.dot(JpT1,fCom), np.dot(JpT2,fCom), np.dot(JpT3,fCom))
currentT = controlModel.getJointAngAccelerationLocal(iJoint)
controlModel.setJointAngAccelerationLocal(iJoint, currentT+Tfi)
currentT = controlModel.getJointAngAccelerationLocal(0)
#print(currentT)
#JpT = ( Jt[0][0], Jt[1][0], Jt[2][0])
#Tfi = JpT*fCom
#controlModel.setJointAngAccelerationLocal(0, currentT+Tfi)
'''
if (len(contactForces) != 0) :
for iJoint in range(1, 7): # from 'LeftUpLeg'(1) to 'RightFoot'(6)
JpT = ( Jt[0][6+3*(iJoint-1)], Jt[1][6+3*(iJoint-1)], Jt[2][6+3*(iJoint-1)])
Tfi = JpT*fCom
currentT = controlModel.getJointAngAccelerationLocal(iJoint)
controlModel.setJointAngAccelerationLocal(iJoint, currentT+Tfi)
else:
print("No Contact force!!")
'''
controlModel.solveHybridDynamics()
vpWorld.step()
# rendering
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
rd_CM_plane[0][1] = 0.
if CP!=None and dCP!=None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des
rd_dL_des_plane[0] = dL_des_plane
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
rd_vf[0] = fCom
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def test_InteractivePlot():
import Resource.ysMotionLoader as yf
import Renderer.ysRenderer as yr
import GUI.ysSimpleViewer as ysv
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
jointMotion = yf.readBvhFile(bvhFilePath, .01)
jointMotion = jointMotion[0:200]
hips = jointMotion[0].skeleton.getJointIndex('Hips')
lFoot = jointMotion[0].skeleton.getJointIndex('LeftFoot')
rFoot = jointMotion[0].skeleton.getJointIndex('RightFoot')
plot = InteractivePlot()
plot.setXlimit(0, len(jointMotion))
plot.setYlimit(0, 2)
plot.addDataSet('root.y')
plot.addDataSet('leftFoot.y')
plot.addDataSet('rightFoot.y')
viewer = ysv.SimpleViewer()
viewer.doc.addRenderer(
'motion(%s)' % jointMotion.resourceName,
yr.JointMotionRenderer(jointMotion, (0, 0, 255), yr.LINK_LINE))
viewer.doc.addObject('motion(%s)' % jointMotion.resourceName,
jointMotion)
pt = [None]
updated = [False]
def preFrameCallback(frame):
if updated[0] == False:
plot.addDataPoint(
'root.y', frame,
jointMotion[frame].getJointPositionGlobal(hips)[1])
plot.addDataPoint(
'leftFoot.y', frame,
jointMotion[frame].getJointPositionGlobal(lFoot)[1])
plot.addDataPoint(
'rightFoot.y', frame,
jointMotion[frame].getJointPositionGlobal(rFoot)[1])
plot.updatePoints()
if frame == viewer.getMaxFrame():
updated[0] = True
if frame == 0:
pt[0] = time.time()
if frame == 50:
print(time.time() - pt[0])
def preFrameCallback_Always(frame):
plot.updateVline(frame)
def viewer_onClose(data):
plot.close()
viewer.onClose(data)
viewer.setPreFrameCallback(preFrameCallback)
viewer.setPreFrameCallback_Always(preFrameCallback_Always)
viewer.callback(viewer_onClose)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
