def stepQP(self, model, timestep):
totalDOF = self.totalDOF
# optimization
x = self.qp.solve()
ype.nested(np.array(x[:totalDOF].T).flatten(), self.ddth_sol)
test_ddq = np.array(x[:totalDOF].T).flatten()
test_tau = np.array(x[totalDOF:2*totalDOF].T).flatten()
test_lambda = np.array(x[2*totalDOF:].T).flatten()
# integrate
if self.ddth_sol != None:
model.stepKinematics(timestep, self.ddth_sol)
self.ddth_des_flat = None
self.dth_flat = None
self.ddth_sol = None
#np.dot(Vc_tmp.T, np.array(test_lambda[4*i:4*i+4]))
if test_lambda != []:
contactForces = []
for i in range(len(test_lambda)/4):
contactForces.append( np.dot(self.Vc_tmp.T, test_lambda[4*i:4*i+4]))
return x, contactForces
else:
return x, None
else:
print("setup QP first!")
return None
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
def simulateCallback(frame):
print "main:frame : ", 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)
# tracking
th_r_ori = motion.getDOFPositions(frame)
th_r = copy.copy(th_r_ori)
############################
#Reference motion modulation
dCM_k = 10.
linkVelocities = controlModel.getBodyVelocitiesGlobal()
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
dCM_plane = copy.copy(dCM)
dCM_plane[1] = 0.
global leftHipTimer
if viewer.objectInfoWnd.onLeftHip:
leftHipTimer = 60
viewer.objectInfoWnd.onLeftHip = False
if leftHipTimer > 0:
viewer.objectInfoWnd.comOffsetX.value(
0.08 * 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()
lcpBodyIDs, lcpContactPositions, lcpContactPositionLocals, lcpContactForces = hls.calcLCPForces(
motion, vpWorld, controlModel, bodyIDsToCheck, 1., 4, None)
# bodyIDs : IDs for Virtual Physics, not VpModel !!!
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if (CP is not None):
CP[1] = 0.
for i in range(controlModel.getBodyNum()):
controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0, 255)
contactFlagFootL = [0] * footPartNum
contactFlagFootR = [0] * footPartNum
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):
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]
#dL_des_plane[1] = 0.
#print 'dL_des_plane', dL_des_plane
# angular momentum
CP_ref = footCenter + footCenterOffset
CP_ref[1] = 0.
timeStep = 30.
if (CP_old[0] is None) or (CP is None):
dCP = None
else:
dCP = (CP - CP_old[0]) * timeStep
CP_old[0] = CP
if (CP is not None) and (dCP is not None):
ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP)
CP_des = CP + dCP * (1 / timeStep) + .5 * ddCP_des * (
(1 / timeStep)**2)
#print 'dCP: ', dCP
#print 'ddCP_des: ', ddCP_des
#print 'CP_des: ', CP_des
#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 is 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]
elif mit.MOTION == mit.WALK:
frame_index = [10000, 60]
elif mit.MOTION == mit.TIPTOE:
frame_index = [1000000, 1000000]
#frame_index = [10000, 165]
else:
frame_index = [1000000, 1000000]
#MOTION = TAEKWONDO
#frame_index = [135, 100]
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 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)
#mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
if flagContact == True:
if stage != MOTION_TRACKING + 10:
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R,
r_bias)
#mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
# using || dH ||^2 instead
mot.addAnotherTerms(problem, totalDOF, Bh, S,
-(s_bias + Kh * np.dot(S, dth_flat)))
a_sup_2 = None
Jsup_2 = None
dJsup_2 = None
##############################
# Hard constraint
Kk2 = Kk * 4.0
Dk2 = 2 * (Kk2**.5)
ankleW = 0
ankleOffset = ankleW * curRelCMVec[2]
metatarW = 0
metatarOffset = metatarW * curRelCMVec[2]
##############################
##############################
# Additional constraint
if stage != MOTION_TRACKING and frame > 5:
# ankle strategy
idx = 0 #LEFT/RIGHT_TOES
if mit.FOOT_PART_NUM == 1:
yOffset = 0.03
else:
yOffset = 0.069
#yOffset = 0.06
# ankleOffset = (footCenter - CM_plane)*4.
ankleOffset = footCenterOffset * 10.
ankleOffset[1] = 0.
#ankleOffset[2] = 0.
ankleOffset[2] = ankleOffset[2] * 20.
ankleOffsetL = ankleOffset.copy()
ankleOffsetR = ankleOffset.copy()
#ankleOffset= np.array((0,0,0))
if footCenterOffset[0] > 0.0:
ankleOffsetL[0] = 0.
else:
ankleOffsetR[0] = 0.
# print 'ankleOffset=', ankleOffset
desLinearAccL, desPosL = getDesFootLinearAcc(
motionModel, controlModel, indexFootL[idx], ModelOffset,
CM_ref, CM, Kk, Dk, yOffset) #0.076) #0.14)
desLinearAccR, desPosR = getDesFootLinearAcc(
motionModel, controlModel, indexFootR[idx], ModelOffset,
CM_ref, CM, Kk, Dk, yOffset)
ax = [0, 0, -1]
aaa = getBodyGlobalOri(controlModel, motion, 'RightFoot')
#print np.dot(aaa, ax)
if mit.FOOT_PART_NUM == 1:
ax = [0, 1, 0]
desAngularAccL = getDesFootAngularAcc(
motionModel, controlModel, indexFootL[idx], Kk, Dk, ax,
mm.normalize([0, 1, 0] + ankleOffsetL))
desAngularAccR = getDesFootAngularAcc(
motionModel, controlModel, indexFootR[idx], Kk, Dk, ax,
mm.normalize([0, 1, 0] + ankleOffsetR))
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]))
#mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2)
#mot.addConstraint(problem, totalDOF, Jsup_2[:1], dJsup_2[:1], dth_flat, a_sup_2[:1])
#mot.addConstraint(problem, totalDOF, Jsup_2[2:], dJsup_2[2:], dth_flat, a_sup_2[2:])
#mot.addConstraint(problem, totalDOF, Jsup_2[3:], dJsup_2[3:], dth_flat, a_sup_2[3:])
mot.addAnotherTerms(problem, totalDOF,
viewer.objectInfoWnd.Bc.value(), Jsup_2[3:],
a_sup_2[3:] - np.dot(dJsup_2[3:], dth_flat))
#mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2, a_sup_2 - np.dot(dJsup_2, dth_flat))
#mot.addAnotherTerms(problem, totalDOF, 1.*viewer.objectInfoWnd.Bc.value(), Jsup_2[0:1], a_sup_2[0:1] - np.dot(dJsup_2[0:1] , dth_flat))
#mot.addAnotherTerms(problem, totalDOF, 1.*viewer.objectInfoWnd.Bc.value(), Jsup_2[2:], a_sup_2[2:] - np.dot(dJsup_2[2:] , dth_flat))
desCOMOffset = 0.0
rd_DesPosL[0] = desPosL.copy()
rd_DesPosR[0] = desPosR.copy()
if stage == STATIC_BALANCING and frame > 10: # and False:
del rd_desPoints[:]
# foot strategy
#Kk2 = Kk * 2.5
#Kk2 = Kk * .2
#Dk2 = 2*(Kk2**.5)
desForePosL = [0, 0, 0]
desForePosR = [0, 0, 0]
desRearPosL = [0, 0, 0]
desRearPosR = [0, 0, 0]
footPartPos = []
footPartPos.append(
controlModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex('LeftCalcaneus_1')))
footPartPos.append(
controlModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex('LeftPhalange_1')))
footPartPos.append(
controlModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex('RightCalcaneus_1')))
footPartPos.append(
controlModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex('RightPhalange_1')))
for i in range(1, footPartNum):
contactFlagFootL[i] = 1
contactFlagFootR[i] = 1
SupPts = np.vstack(
(np.array((footPartPos[0][0], footPartPos[1][0],
footPartPos[2][0], footPartPos[3][0])),
np.array(
(footPartPos[0][2], footPartPos[1][2], footPartPos[2][2],
footPartPos[3][2])), np.array((1., 1., 1., 1.))))
coordWidthLen = 2.
coordLengthLen = 1.5
SupUV = np.vstack(
(np.array((-coordWidthLen, -coordWidthLen, coordWidthLen,
coordWidthLen)),
np.array((-coordLengthLen, coordLengthLen, -coordLengthLen,
coordLengthLen)), np.array((1., 1., 1., 1.))))
SupMap = np.dot(np.dot(SupUV, SupUV.T),
np.linalg.inv(np.dot(SupPts, SupUV.T)))
#print SupMap
desFootCenter = footCenter + footCenterOffset
footCenterPts = np.array((desFootCenter[0], desFootCenter[2], 1))
#print np.dot(SupMap, footCenterPts)
#print np.dot(getBodyGlobalOri(controlModel, motion, 'LeftMetatarsal_1'), np.array((0,1,0)))
CM_plane_2D = np.array((CM[0], CM[2], 1))
# CM_plane_UV = np.dot(SupMap, CM_plane_2D)
CM_plane_UV = np.dot(SupMap, footCenterPts)
# print CM_plane_UV
# for i in range(1, footPartNum):
if CM_plane_UV[1] > .5:
# com is in front
for i in range(1, 5):
contactFlagFootL[i] = 0
contactFlagFootR[i] = 0
elif CM_plane_UV[1] < -.5:
# com is back
for i in range(3, footPartNum):
contactFlagFootL[i] = 0
contactFlagFootR[i] = 0
else:
# com is in middle position
for i in range(3, 5):
contactFlagFootL[i] = 0
contactFlagFootR[i] = 0
contactFlagFoot = contactFlagFootL
if CM_plane_UV[0] < 0.:
contactFlagFoot = contactFlagFootR
# CM_plane_UV[0] = -CM_plane_UV[0]
if abs(CM_plane_UV[0]) > 1.:
for j in range(0, 3):
contactFlagFoot[2 * j + 2] = 0
# print 'footL : ',contactFlagFootL
# print 'footR : ',contactFlagFootR
for i in range(1, footPartNum):
axis = [0, 0, 1]
if i == 1 or i == 2:
axis = [0, 0, -1]
desAng = [0, 0, 1]
if i == 1 or i == 2:
desAng = [0, 0, -1]
desY = 0.029
if contactFlagFootL[i] == 1:
desLinearAccL, desForePosL = getDesFootLinearAcc(
motionModel, controlModel, indexFootL[i], ModelOffset,
CM_ref, CM, Kk2, Dk2, desY)
desAngularAccL = getDesFootAngularAcc(
motionModel, controlModel, indexFootL[i], Kk2, Dk2,
axis, desAng)
a_sup_2 = np.hstack((desLinearAccL, desAngularAccL))
Jsup_2 = jFootL[i].copy()
dJsup_2 = dJFootL[i].copy()
mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2,
dth_flat, a_sup_2)
#mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2, a_sup_2 - np.dot(dJsup_2, dth_flat))
#mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2[3:], a_sup_2[3:] - np.dot(dJsup_2[3:] , dth_flat))
rd_desPoints.append(desForePosL.copy())
if contactFlagFootR[i] == 1:
desLinearAccR, desForePosR = getDesFootLinearAcc(
motionModel, controlModel, indexFootR[i], ModelOffset,
CM_ref, CM, Kk2, Dk2, desY)
desAngularAccR = getDesFootAngularAcc(
motionModel, controlModel, indexFootR[i], Kk2, Dk2,
axis, desAng)
a_sup_2 = np.hstack((desLinearAccR, desAngularAccR))
Jsup_2 = jFootR[i].copy()
dJsup_2 = dJFootR[i].copy()
mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2,
dth_flat, a_sup_2)
#mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2, a_sup_2 - np.dot(dJsup_2, dth_flat))
#mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2[3:], a_sup_2[3:] - np.dot(dJsup_2[3:], dth_flat))
rd_desPoints.append(desForePosR.copy())
rd_DesForePosL[0] = desForePosL
rd_DesForePosR[0] = desForePosR
rd_DesRearPosL[0] = desRearPosL
rd_DesRearPosR[0] = desRearPosR
##############################
#if Jsup_2 is not 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()
#print frame
#Ashape = np.shape(problem.A)
#if len(Ashape) >0 :
# for i in range(0, Ashape[0]):
# print problem.A[i]
#print problem.A[]
#print problem.b
#print r
problem.clear()
#print r['x']
ype.nested(r['x'], ddth_sol)
#print ddth_sol
rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
###########################################
##Jacobian Transpose control
# COM Position control
#fCom = Wcp*(pHatComDes - pHatCom) + Wcv*(vComDes - vCom) + Wcm*(footCenter_plane - CM_plane)
w1 = 10 #10.1
w2 = 1 #1#2*(w1**.5)
if frame > 100:
w1 = 10.1 #10.1
w2 = 1
footToCMVec = CM - footCenter
desCMPos = [footCenter[0], mm.length(footToCMVec), footCenter[2]]
#print("desCMPos", desCMPos)
#print("CM", CM)
fCom = w1 * (desCMPos - CM) + w2 * (-dCM)
#print("fCom", fCom)
#fCom[0] = 0.
#fCom[1] = 0
#fCom[2] = 0
rd_virtualForce[0] = fCom.copy()
#hipPos = controlModel.getBodyPositionGlobal(rootB)
headPos = controlModel.getBodyPositionGlobal(selectedBody)
hipPos = controlModel.getBodyPositionGlobal(rootB)
yjc.computeJacobian2(Jcom, DOFs, jointPositions, jointAxeses,
[headPos], comUpperJointMasks)
#yjc.computeJacobianDerivative2(dJcom, DOFs, jointPositions, jointAxeses, linkAngVelocities, [CM], comUpperJointMasks, False)
JcomT = Jcom.T
TauJT = np.dot(JcomT, fCom)
# Angular Momentum
Hc = ymt.getAngularMomentum(CM, linkInertias, linkAngVelocities,
linkPositions, linkMasses, linkVelocities)
Href = ymt.getAngularMomentum(CM_ref, linkInertias_ref,
linkAngVelocities_ref, linkPositions_ref,
linkMasses, linkVelocities_ref)
Wam = .05
Tam = Wam * (Href - Hc)
#print("Tam", Tam)
yjc.computeAngJacobian2(JcomAng, DOFs, jointPositions, jointAxeses,
[headPos], comUpperJointMasks)
TauAM = np.dot(JcomAng.T, Tam)
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)
#print frame, bodyIDs, contactPositions, contactPositionLocals, contactForces
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
#print ddth_sol
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 is not None) and (dCP is not 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
del rd_contactForces[:]
del rd_contactPositions[:]
if CP is not None:
for i in range(len(lcpBodyIDs)):
rd_contactForces.append(lcpContactForces[i].copy() / 200.)
rd_contactPositions.append(lcpContactPositions[i].copy())
timeReport[6] += time.time() - curTime
def simulateCallback(frame):
global g_initFlag
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()
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
'''
if stage == POWERFUL_BALANCING:
#if stage != MOTION_TRACKING:
footCenterL = controlModel.getBodyPositionGlobal(supL)
footCenterR = controlModel.getBodyPositionGlobal(supR)
else:
footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1])
footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1])
'''
if footPartNum == 1:
footCenterL = controlModel.getBodyPositionGlobal(supL)
footCenterR = controlModel.getBodyPositionGlobal(supR)
else:
if stage == POWERFUL_BALANCING:
footCenterL = controlModel.getBodyPositionGlobal(supL)
footCenterR = controlModel.getBodyPositionGlobal(supR)
else:
footCenterL = (
controlModel.getBodyPositionGlobal(supL) +
controlModel.getBodyPositionGlobal(indexFootL[1])) / 2.0
footCenterR = (
controlModel.getBodyPositionGlobal(supR) +
controlModel.getBodyPositionGlobal(indexFootR[1])) / 2.0
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.
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)
if g_initFlag == 0:
softConstPoint = controlModel.getBodyPositionGlobal(constBody)
softConstPoint[1] -= .3
g_initFlag = 1
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)
contactFlagFootL = [0] * footPartNum
contactFlagFootR = [0] * footPartNum
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.computeJacobian2(jFootL[j], DOFs, jointPositions,
jointAxeses, [positionFootL[j]],
jointMasksFootL[j])
yjc.computeJacobianDerivative2(
dJFootL[j], DOFs, jointPositions, jointAxeses,
linkAngVelocities, [positionFootL[j]],
jointMasksFootL[j], False)
break
for j in range(len(indexFootR)):
if index == indexFootR[j]:
contactFlagFootR[j] = 1
if j != 0:
yjc.computeJacobian2(jFootR[j], DOFs, jointPositions,
jointAxeses, [positionFootR[j]],
jointMasksFootR[j])
yjc.computeJacobianDerivative2(
dJFootR[j], DOFs, jointPositions, jointAxeses,
linkAngVelocities, [positionFootR[j]],
jointMasksFootR[j], False)
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 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
footCenter[1] = 0.
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_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]
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 + 10:
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)
desLinearAccR, desPosR = getDesFootLinearAcc(
motionModel, controlModel, indexFootR[idx], ModelOffset,
CM_ref, CM, Kk2, Dk2)
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]))
rd_DesPosL[0] = desPosL.copy()
rd_DesPosR[0] = desPosR.copy()
else:
if footPartNum != 1:
idx = 1
else:
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]))
##############################
##############################
# Additional constraint
if stage != MOTION_TRACKING:
Kk2 = Kk * 1.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 contactFlagFootL[i] == 1:
desLinearAccL, desForePosL = getDesFootLinearAcc(
motionModel, controlModel, indexFootL[i], ModelOffset,
CM_ref, CM, Kk2, Dk2)
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)
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]
def simulateCallback(self, frame):
global ddth_des_flat
global stepsPerFrame
global wcfg
global vpWorld
# reload(tf)
motionModel.update(motion[0])
self.frame = frame
print("main:frame : ", frame)
# motionModel.update(motion[0])
self.timeIndex = 0
self.setTimeStamp()
# IK solver
'''
solver.clear()
solver.setInitPose(motion[0])
cVpBodyIds, cPositions, cPositionsLocal, cVelocities = vpWorld.getContactPoints(bodyIDsToCheck)
if len(cVpBodyIds) > 1:
solver.addConstraints(cVpBodyIds[1], cPositionsLocal[1], np.array((0., 0., 0.)), None, (False, True, False, False))
solver.addConstraints(cVpBodyIds[3], cPositionsLocal[3], np.array((0., 0., 0.)), None, (False, True, False, False))
solver.addConstraints(cVpBodyIds[5], cPositionsLocal[5], np.array((0., 0., 0.)), None, (False, True, False, False))
# solver.solve(controlModel, np.array((0., .15 + .05*math.sin(frame/10.), 0.)))
'''
# constant setting
# (Kt, damp, stepsPerFrame, simulSpeedInv) = viewer.objectInfoWnd.getVals()
getVal = viewer.objectInfoWnd.getVal
Kt = getVal('PD gain')
damp = getVal('Joint Damping')
stepsPerFrame = getVal('steps per frame')
simulSpeedInv = getVal('1/simul speed')
wcfg.timeStep = 1 / (30. * simulSpeedInv * stepsPerFrame)
vpWorld.SetTimeStep(wcfg.timeStep)
Dt = 2. * (Kt**.5)/20.
# Dt = 0.
controlModel.SetJointsDamping(damp)
# controlModel.SetJointsDamping(1.)
wLCP = math.pow(2., getVal('LCP weight'))
wForce = math.pow(2., getVal('force weight'))
wTorque = math.pow(2., getVal('tau weight'))
# tracking
th_r = motion.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(frame)
weightMapTuple = config['weightMapTuple']
weightMapTuple = None
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt, weightMapTuple)
ddth_c = controlModel.getDOFAccelerations()
ype.flatten(ddth_des, ddth_des_flat)
desForceFrameBegin = getVal('des force begin')
desForceDuration = getVal('des force dur') * simulSpeedInv
desForceFrame = [
desForceFrameBegin, desForceFrameBegin + desForceDuration]
desForceRelFrame = float(frame - desForceFrame[0]) / desForceDuration
desNormalForceMin = getVal('normal des force min')
desNormalForceMax = getVal('normal des force max')
desNormalForce = desNormalForceMin
if desForceFrame[0] <= frame <= desForceFrame[1]:
desNormalForce = desNormalForceMin * \
(1 - desForceRelFrame) + desNormalForceMax * desForceRelFrame
totalForce = np.array([0., desNormalForce, 0., 0., 0., 0.])
# totalForce = np.array([-desNormalForce, 34.3, 0., 0., 0., 0.])
# totalForce = np.array([0., 34.3, desNormalForce, 0., 0., 0.])
# totalForce = np.array([50., 150.])
torques = None
ddth_des_flat[0:6] = [0.] * 6
self.setTimeStamp()
simulContactForces = np.zeros(3)
cBodyIDs = None
cPositions = None
cPositionLocals = None
cForces = None
cBodyIDsControl = None
cPositionsControl = None
cPositionLocalsControl = None
cForcesControl = None
# if desForceFrame[0] <= frame <= desForceFrame[1]:
if True:
# totalForceImpulse = stepsPerFrame * totalForce
cBodyIDsControl, cPositionsControl, cPositionLocalsControl, cForcesControl, torques \
= hls.calcLCPbasicControl(
motion, vpWorld, controlModel, bodyIDsToCheck, 1., totalForce, [wLCP, wTorque, wForce], ddth_des_flat)
# if cForces is not None:
# print "control: ", sum(cForces)
sumControlForce = np.array([0.]*6)
if cForcesControl is not None:
sumControlForce = np.hstack((sum(cForcesControl), np.array([0., 0., 0.])))
timeStamp = None
torque_None = False
if not (desForceFrame[0] <= frame <= desForceFrame[1]) or (torques is None):
torque_None = True
torques = ddth_des_flat
elif np.linalg.norm(sumControlForce - totalForce) > np.linalg.norm(totalForce):
print("control failed!")
torque_None = True
torques = ddth_des_flat
else:
torques *= 1.
for i in range(int(stepsPerFrame)):
if i % 5 == 0:
cBodyIDs, cPositions, cPositionLocals, cForces, timeStamp \
= hls.calcLCPForces(motion, vpWorld, controlModel, bodyIDsToCheck, 1., torques, solver='qp')
if i % 5 == 0 and len(cBodyIDs) > 0:
# apply contact forces
if False and not torque_None:
vpWorld.applyPenaltyForce(cBodyIDs, cPositionLocals, cForcesControl)
simulContactForces += sum(cForcesControl)
else:
vpWorld.applyPenaltyForce(cBodyIDs, cPositionLocals, cForces)
simulContactForces += sum(cForces)
# simulContactForces += sum(cForces)
ype.nested(torques, torques_nested)
controlModel.setDOFTorques(torques_nested[1:])
vpWorld.step()
self.setTimeStamp()
# rendering expected force
del rd_cForcesControl[:]
del rd_cPositionsControl[:]
if cBodyIDsControl is not None:
# print cBodyIDsControl
for i in range(len(cBodyIDsControl)):
# print expected force
rd_cForcesControl.append(cForcesControl[i].copy() /50.)
rd_cPositionsControl.append(cPositionsControl[i].copy())
# rendering sum of expected force
del rd_ForceControl[:]
del rd_Position[:]
if cForcesControl is not None:
# print expected force
rd_ForceControl.append(sum(cForcesControl) /50.)
rd_Position.append(np.array([0., 0., 0.1]))
# graph expected force
if cForcesControl is not None:
sumForce = sum(cForcesControl)
if sumForce[1] > 10000:
sumForce[1] = 10000
viewer.cForceWnd.insertData('expForce', frame, sumForce[1])
else:
viewer.cForceWnd.insertData('expForce', frame, 0.)
# rendering calculated forces
del rd_cForces[:]
del rd_cPositions[:]
for i in range(len(cBodyIDs)):
# print calculated force
rd_cForces.append(cForces[i].copy() / 50.)
rd_cPositions.append(cPositions[i].copy())
# rendering joint position
del rd_jointPos[:]
for i in range(motion[0].skeleton.getJointNum()):
rd_jointPos.append(motion[frame].getJointPositionGlobal(i))
# rendering desired force
del rd_ForceDes[:]
del rd_PositionDes[:]
# rd_ForceDes.append(totalForce/50.)
rd_ForceDes.append(totalForce[1] * np.array([0., 1., 0.]) / 50.)
rd_PositionDes.append(np.array([0., 0., 0.]))
# if self.cForces is not None:
# rd_ForceDes.append(sum(self.cForces)[1]/50. * [0., 1., 0.])
# rd_PositionDes.append(np.array([0., 0., -0.1]))
# graph calculated force
if cForces is not None:
sumForce = sum(cForces)
# viewer.cForceWnd.insertData('realForce', frame, sumForce[1])
viewer.cForceWnd.insertData('realForce', frame, simulContactForces[1]/stepsPerFrame)
else:
viewer.cForceWnd.insertData('realForce', frame, 0.)
# viewer.cForceWnd.insertData('realForce', frame, simulContactForces[1]/stepsPerFrame)
# graph desired force
if desForceFrame[0] <= frame <= desForceFrame[1]:
viewer.cForceWnd.insertData('desForceMin', frame, totalForce[1])
# viewer.cForceWnd.insertData('desForceMin', frame, totalForce[1] * 1.0)
# viewer.cForceWnd.insertData('desForceMax', frame, totalForce[1] * 1.1)
else:
viewer.cForceWnd.insertData('desForceMin', frame, 0.)
viewer.cForceWnd.insertData('desForceMax', frame, 0.)
self.setTimeStamp()
def simulateCallback(frame):
global g_initFlag
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()
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.
foreFootCenterL = controlModel.getBodyPositionGlobal(foreSupL)
foreFootCenterR = controlModel.getBodyPositionGlobal(foreSupR)
rearFootCenterL = controlModel.getBodyPositionGlobal(rearSupL)
rearFootCenterR = controlModel.getBodyPositionGlobal(rearSupR)
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)
#dJsys = (Jsys - JsysPre)/(1/30.)
#JsysPre = Jsys
yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions,
jointAxeses, linkAngVelocities,
linkPositions, allLinkJointMasks)
if g_initFlag == 0:
softConstPoint = controlModel.getBodyPositionGlobal(constBody)
softConstPoint[1] -= .3
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)
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)
flagForeSupLContact = 0
flagForeSupRContact = 0
flagRearSupLContact = 0
flagRearSupLContact = 0
for i in range(len(bodyIDs)):
controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105)
index = controlModel.id2index(bodyIDs[i])
if index == foreSupL:
flagForeSupLContact = 1
yjc.computeJacobian2(JforeSupL, DOFs, jointPositions,
jointAxeses, [foreFootCenterL],
foreSupLJointMasks)
yjc.computeJacobianDerivative2(dJforeSupL, DOFs,
jointPositions, jointAxeses,
linkAngVelocities,
[foreFootCenterL],
foreSupLJointMasks, False)
elif index == foreSupR:
flagForeSupRContact = 1
yjc.computeJacobian2(JforeSupR, DOFs, jointPositions,
jointAxeses, [foreFootCenterR],
foreSupRJointMasks)
yjc.computeJacobianDerivative2(dJforeSupR, DOFs,
jointPositions, jointAxeses,
linkAngVelocities,
[foreFootCenterR],
foreSupRJointMasks, False)
elif index == rearSupL:
flagRearSupLContact = 1
yjc.computeJacobian2(JrearSupL, DOFs, jointPositions,
jointAxeses, [rearFootCenterL],
rearSupLJointMasks)
yjc.computeJacobianDerivative2(dJrearSupL, DOFs,
jointPositions, jointAxeses,
linkAngVelocities,
[rearFootCenterL],
rearSupLJointMasks, False)
elif index == rearSupR:
flagRearSupRContact = 1
yjc.computeJacobian2(JrearSupR, DOFs, jointPositions,
jointAxeses, [rearFootCenterR],
rearSupRJointMasks)
yjc.computeJacobianDerivative2(dJrearSupR, DOFs,
jointPositions, jointAxeses,
linkAngVelocities,
[rearFootCenterR],
rearSupRJointMasks, False)
#
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
CMP = yrp.getCMP(contactForces, CM)
r = [0, 0, 0]
if CP != None and np.any(np.isnan(CMP)) != True:
r = CP - CMP
# 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_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]
elif mit.MOTION == mit.TAEKWONDO:
frame_index = [130, 100]
else:
frame_index = [1000000, 1000000]
#MOTION = TAEKWONDO
#frame_index = [135, 100]
if frame > frame_index[0]:
stage = POWERFUL_BALANCING
Kk = Kk * 2
Dk = 2 * (Kk**.5)
elif frame > frame_index[1]:
stage = MOTION_TRACKING
trackingW = w
if stage == MOTION_TRACKING:
trackingW = w2
# 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)
##############################
# Hard constraint
desLinearAccL = getDesFootLinearAcc(motionModel, controlModel, supL,
ModelOffset, CM_ref, CM, Kk, Dk)
desLinearAccR = getDesFootLinearAcc(motionModel, controlModel, supR,
ModelOffset, CM_ref, CM, Kk, Dk)
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, supL,
Kk, Dk)
desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, supR,
Kk, Dk)
'''
desLinearAccL = [0,0,0]
desAngularAccL = [0,0,0]
desLinearAccR = [0,0,0]
desAngularAccR = [0,0,0]
refPos = motionModel.getBodyPositionGlobal(supL)
curPos = controlModel.getBodyPositionGlobal(supL)
refRootPos = motionModel.getBodyPositionGlobal(rootB)
curRootPos = controlModel.getBodyPositionGlobal(rootB)
refVecL = refPos - CM_ref
if stage == MOTION_TRACKING:
refPos = CM + refVecL
refPos[1] += 0.05
refPos[0] -= 0.05
elif stage == POWERFUL_BALANCING:
refPos = copy.copy(curPos)
refPos[1] = 0
else:
refPos[0] += ModelOffset[0]
rd_root[0] = curRootPos
refVel = motionModel.getBodyVelocityGlobal(supL)
rd_footCenterL[0] = copy.copy(curPos)
#rd_footCenterL[0][2] += 0.2
curVel = controlModel.getBodyVelocityGlobal(supL)
#refAcc = (0,0,0)
refAcc = motionModel.getBodyAccelerationGlobal(supL)
if stage != MOTION_TRACKING:
refPos[1] = 0
if refPos[1] < 0.0 :
refPos[1] = 0.0
rd_DesPosL[0] = refPos
desLinearAccL = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk)
refPos = motionModel.getBodyPositionGlobal(supR)
curPos = controlModel.getBodyPositionGlobal(supR)
refVecR = refPos - CM_ref
if stage == MOTION_TRACKING:
refPos = CM + refVecR
refPos[1] += 0.05
refPos[0] -= 0.05
elif stage == POWERFUL_BALANCING:
refPos = copy.copy(curPos)
refPos[1] = 0
else :
refPos[0] += ModelOffset[0]
refVel = motionModel.getBodyVelocityGlobal(supR)
curVel = controlModel.getBodyVelocityGlobal(supR)
refAcc = motionModel.getBodyAccelerationGlobal(supR)
if stage != MOTION_TRACKING:
refPos[1] = 0
if refPos[1] < 0.0 :
refPos[1] = 0.0
rd_DesPosR[0] = refPos
desLinearAccR = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk)
curAng = [controlModel.getBodyOrientationGlobal(supL)]
refAngVel = motionModel.getBodyAngVelocityGlobal(supL)
curAngVel = controlModel.getBodyAngVelocityGlobal(supL)
refAngAcc = (0,0,0)
curAngY = np.dot(curAng, np.array([0,1,0]))
refAngY = np.array([0,1,0])
if stage == DYNAMIC_BALANCING:
refAng = [motionModel.getBodyOrientationGlobal(supL)]
refAngY2 = np.dot(refAng, np.array([0,1,0]))
refAngY = refAngY2[0]
rd_footL_vec[0] = refAngY
rd_footR_vec[0] = curAngY[0]
aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], refAngY))
desAngularAccL = [Kk*aL + Dk*(refAngVel-curAngVel)]
curAng = [controlModel.getBodyOrientationGlobal(supR)]
refAngVel = motionModel.getBodyAngVelocityGlobal(supR)
curAngVel = controlModel.getBodyAngVelocityGlobal(supR)
refAngAcc = (0,0,0)
curAngY = np.dot(curAng, np.array([0,1,0]))
refAngY = np.array([0,1,0])
if stage == DYNAMIC_BALANCING:
refAng = [motionModel.getBodyOrientationGlobal(supR)]
refAngY2 = np.dot(refAng, np.array([0,1,0]))
refAngY = refAngY2[0]
aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], refAngY))
desAngularAccR = [Kk*aL + Dk*(refAngVel-curAngVel)]
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]]
'''
a_sup_2 = np.hstack((np.hstack((desLinearAccL, desAngularAccL)),
np.hstack((desLinearAccR, desAngularAccR))))
Jsup_2 = np.vstack((JsupL, JsupR))
dJsup_2 = np.vstack((dJsupL, dJsupR))
mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat,
a_sup_2)
##############################
##############################
# Additional constraint
desLinearAccL = [0, 0, 0]
desAngularAccL = [0, 0, 0]
desLinearAccR = [0, 0, 0]
desAngularAccR = [0, 0, 0]
refPos = motionModel.getBodyPositionGlobal(supL)
curPos = controlModel.getBodyPositionGlobal(supL)
refVecL = refPos - CM_ref
if stage == MOTION_TRACKING:
refPos = CM + refVecL
refPos[1] += 0.05
refPos[0] -= 0.05
elif stage == POWERFUL_BALANCING:
refPos = copy.copy(curPos)
refPos[1] = 0
else:
refPos[0] += ModelOffset[0]
refVel = motionModel.getBodyVelocityGlobal(supL)
curVel = controlModel.getBodyVelocityGlobal(supL)
#refAcc = (0,0,0)
refAcc = motionModel.getBodyAccelerationGlobal(supL)
if stage != MOTION_TRACKING:
refPos[1] = 0
if refPos[1] < 0.0:
refPos[1] = 0.0
desLinearAccL = yct.getDesiredAcceleration(refPos, curPos, refVel,
curVel, refAcc, Kk, Dk)
curAng = [controlModel.getBodyOrientationGlobal(supL)]
refAngVel = motionModel.getBodyAngVelocityGlobal(supL)
curAngVel = controlModel.getBodyAngVelocityGlobal(supL)
refAngAcc = (0, 0, 0)
curAngY = np.dot(curAng, np.array([0, 1, 0]))
refAngY = np.array([0, 1, 0])
if stage == DYNAMIC_BALANCING:
refAng = [motionModel.getBodyOrientationGlobal(supL)]
refAngY2 = np.dot(refAng, np.array([0, 1, 0]))
refAngY = refAngY2[0]
rd_footL_vec[0] = refAngY
rd_footR_vec[0] = curAngY[0]
aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], refAngY))
desAngularAccL = [Kk * aL + Dk * (refAngVel - curAngVel)]
##############################
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)
if flagForeSupLContact == 1:
controlModel.setJointAngAccelerationLocal(
foreSupL, (2.1, 0, 0))
if flagForeSupRContact == 1:
controlModel.setJointAngAccelerationLocal(
foreSupR, (2.1, 0, 0))
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]
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[3])
p4 = controlModel.getBodyPositionGlobal(indexFootR[3])
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 = .4
desFCL = (controlModel.getBodyPositionGlobal(indexFootL[1])*r + controlModel.getBodyPositionGlobal(indexFootL[3])*(1.0-r))#controlModel.getBodyPositionGlobal(indexFootL[1])
desFCR = (controlModel.getBodyPositionGlobal(indexFootR[1])*r + controlModel.getBodyPositionGlobal(indexFootR[3])*(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
#if stage == MOTION_TRACKING:
# 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)*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])
print("totalNormalForce=", totalNormalForce[1])
print("F_Diff=", (totalMass*mm.s2v(wcfg.gravity))[1]+totalNormalForce[1])
# linear momentum
CM_ref_plane = footCenter
dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane
print("CM_Diff=",mm.length(CM_ref_plane - CM_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)))
print("CP_Diff=",mm.length(CP_des - CP))
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, [P_cur], 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, [P_cur], 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]
elif mit.MOTION == mit.WALK:
frame_index = [10000, 60]
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 checkAll(contactFlagFootR, 0) != 1 :
if stage == MOTION_TRACKING:
trackingW = w2
#stage = POWERFUL_BALANCING
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+10:
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
if stage != MOTION_TRACKING:
desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.14)#0.076)
desLinearAccR, desPosR = getDesFootLinearAcc(motionModel, controlModel, indexFootR[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.14)
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2, [0,0,-1], [0,1,1.5])
desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[idx], Kk2, Dk2, [0,0,-1], [0,1,1.5])
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]))
else:
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)
a_sup_2 = np.hstack((desLinearAccL, desAngularAccL))
Jsup_2 = jFootL[idx]
dJsup_2 = dJFootL[idx]
rd_DesPosL[0] = desPosL.copy()
rd_DesPosR[0] = desPosR.copy()
else:
if footPartNum != 5:
idx = 0
desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.045)
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2)
a_sup_2 = np.hstack(( desLinearAccL, desAngularAccL))
Jsup_2 = (jFootL[idx])
dJsup_2 = (dJFootL[idx])
'''
idx = 4
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2)
a_sup_2 = np.hstack(( a_sup_2, desAngularAccL))
Jsup_2 = np.vstack(( Jsup_2, jAngFootL[idx]))
dJsup_2 = np.vstack(( dJsup_2, dJAngFootL[idx]))
'''
'''
idx = 1
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2)
a_sup_2 = np.hstack(( a_sup_2, desAngularAccL))
Jsup_2 = np.vstack(( Jsup_2, jAngFootL[idx]))
dJsup_2 = np.vstack(( dJsup_2, dJAngFootL[idx]))
'''
else:
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]))
##############################
##############################
# Additional constraint
if stage != MOTION_TRACKING+10:
#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 stage != MOTION_TRACKING:
axis = [0,1,0]
desAng = [0,1,0]
desY = 0.04
if i == 1 or i == 2:
desAng = [0,1,1.2]
desY = 0.076
if contactFlagFootL[i] == 1:
desLinearAccL, desForePosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[i], ModelOffset, CM_ref, CM, Kk2, Dk2, desY)
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[i], Kk2, Dk2, axis, desAng)
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, desY)
desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[i], Kk2, Dk2, axis, desAng)
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]]
rd_Joint[0] = controlModel.getJointPositionGlobal(motion[0].skeleton.getJointIndex('LeftMetatarsal_1'))
rd_Joint2[0] = controlModel.getJointPositionGlobal(motion[0].skeleton.getJointIndex('LeftMetatarsal_3'))
rd_Joint3[0] = controlModel.getJointPositionGlobal(motion[0].skeleton.getJointIndex('LeftPhalange_1'))
rd_Joint4[0] = controlModel.getJointPositionGlobal(motion[0].skeleton.getJointIndex('LeftPhalange_3'))
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
#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]
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]
def simulateCallback(self, frame):
global ddth_des_flat
global stepsPerFrame
global wcfg
global vpWorld
# reload(tf)
self.frame = frame
print "main:frame : ", frame
# motionModel.update(motion[0])
self.timeIndex = 0
self.setTimeStamp()
# constant setting
# (Kt, damp, stepsPerFrame, simulSpeedInv) = viewer.objectInfoWnd.getVals()
getVal = viewer.objectInfoWnd.getVal
Kt = getVal('PD gain')
damp = getVal('Joint Damping')
stepsPerFrame = getVal('steps per frame')
simulSpeedInv = getVal('1/simul speed')
wcfg.timeStep = 1 / (30. * simulSpeedInv * stepsPerFrame)
vpWorld.SetTimeStep(wcfg.timeStep)
# Dt = 2. * (Kt**.5)
Dt = 0.
controlModel.SetJointsDamping(damp)
wForce = math.pow(2., getVal('force weight'))
wTorque = math.pow(2., getVal('tau weight'))
# tracking
th_r = motion.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(frame)
# config['weightMapTuple'])
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)
desForceFrameBegin = getVal('des force begin')
desForceDuration = getVal('des force dur') * simulSpeedInv
desForceFrame = [
desForceFrameBegin, desForceFrameBegin + desForceDuration
]
desForceRelFrame = float(frame - desForceFrame[0]) / desForceDuration
desNormalForceMin = getVal('normal des force min')
desNormalForceMax = getVal('normal des force max')
desNormalForce = desNormalForceMin
if desForceFrame[0] <= frame <= desForceFrame[1]:
desNormalForce = desNormalForceMin * \
(1 - desForceRelFrame) + desNormalForceMax * desForceRelFrame
totalForce = np.array([0., desNormalForce, 0., 0., 0., 0.])
# totalForce = np.array([0., 720., 0., 0., 0., 0.])
# totalForce = np.array([50., 150.])
torques = None
ddth_des_flat[0:6] = [0.] * 6
self.setTimeStamp()
simulContactForces = np.zeros(3)
torque_None = True
for i in range(int(stepsPerFrame)):
torques = None
torque_None = True
cBodyIDs = []
cPositions = []
cPositionLocals = []
cForces = []
cBodyIDsControl = []
cPositionsControl = []
cPositionLocalsControl = []
cForcesControl = []
if desForceFrame[0] <= frame <= desForceFrame[1]:
if True:
# totalForceImpulse = stepsPerFrame * totalForce
cBodyIDs, cPositions, cPositionLocals, cForcesControl, torques \
= hls.calcLCPbasicControl(
motion, vpWorld, controlModel, bodyIDsToCheck, 1., totalForce, wForce, wTorque, ddth_des_flat)
# if cForces is not None:
# print "control: ", sum(cForces)
if torques is not None:
# print torques[:6]
torque_None = False
# cForcesControl = cForces.copy()
# cBodyIDsControl = cBodyIDs.copy()
# cPositionsControl = cPositions.copy()
# cPositionLocalsControl = cPositionLocals.copy()
else:
torques = ddth_des_flat
cBodyIDs, cPositions, cPositionLocals, cForces, timeStamp \
= hls.calcLCPForces(motion, vpWorld, controlModel, bodyIDsToCheck, 1., torques, solver='qp')
# if (not torque_None) and cForces is not None:
# print "calcul: ", sum(cForces)
if len(cBodyIDs) > 0:
# apply contact forces
if False and not torque_None:
vpWorld.applyPenaltyForce(cBodyIDs, cPositionLocals,
cForcesControl)
simulContactForces += sum(cForcesControl)
else:
vpWorld.applyPenaltyForce(cBodyIDs, cPositionLocals,
cForces)
simulContactForces += sum(cForces)
# simulContactForces += sum(cForces)
ype.nested(torques, torques_nested)
controlModel.setDOFTorques(torques_nested[1:])
vpWorld.step()
self.setTimeStamp()
# print ddth_des_flat
# print torques
print simulContactForces / stepsPerFrame
self.cBodyIDs, self.cPositions, self.cPositionLocals, self.cForces, torques \
= hls.calcLCPbasicControl(motion, vpWorld, controlModel, bodyIDsToCheck, 1., totalForce, wForce, wTorque, ddth_des_flat)
del rd_cForcesControl[:]
del rd_cPositionsControl[:]
for i in range(len(self.cBodyIDs)):
# print expected force
rd_cForcesControl.append(self.cForces[i].copy() / 50.)
rd_cPositionsControl.append(self.cPositions[i].copy())
del rd_ForceControl[:]
del rd_Position[:]
if self.cForces is not None:
# print expected force
rd_ForceControl.append(sum(self.cForces) / 50.)
rd_Position.append(np.array([0., 0., 0.1]))
# graph
if self.cForces is not None:
sumForce = sum(self.cForces)
viewer.cForceWnd.insertData('expForce', frame, sumForce[1])
else:
viewer.cForceWnd.insertData('expForce', frame, 0.)
self.cBodyIDs, self.cPositions, self.cPositionLocals, self.cForces, tmptmp \
= hls.calcLCPForces(motion, vpWorld, controlModel, bodyIDsToCheck, 1., torques, solver='qp')
del rd_cForces[:]
del rd_cPositions[:]
for i in range(len(self.cBodyIDs)):
# print calculated force
rd_cForces.append(self.cForces[i].copy() / 50.)
rd_cPositions.append(self.cPositions[i].copy())
del rd_jointPos[:]
for i in range(motion[0].skeleton.getJointNum()):
rd_jointPos.append(motion[frame].getJointPositionGlobal(i))
del rd_ForceDes[:]
del rd_PositionDes[:]
# rd_ForceDes.append(totalForce/50.)
rd_ForceDes.append(totalForce[1] * np.array([0., 1., 0.]) / 50.)
rd_PositionDes.append(np.array([0., 0., 0.]))
# if self.cForces is not None:
# rd_ForceDes.append(sum(self.cForces)[1]/50. * [0., 1., 0.])
# rd_PositionDes.append(np.array([0., 0., -0.1]))
# graph
if self.cForces is not None:
sumForce = sum(self.cForces)
# viewer.cForceWnd.insertData('realForce', frame, sumForce[1])
viewer.cForceWnd.insertData('realForce', frame,
simulContactForces[1] / stepsPerFrame)
else:
viewer.cForceWnd.insertData('realForce', frame, 0.)
viewer.cForceWnd.insertData('realForce', frame,
simulContactForces[1] / stepsPerFrame)
if desForceFrame[0] <= frame <= desForceFrame[1]:
viewer.cForceWnd.insertData('desForceMin', frame,
totalForce[1] * 1.)
# viewer.cForceWnd.insertData('desForceMin', frame, totalForce[1] * .9)
# viewer.cForceWnd.insertData('desForceMax', frame, totalForce[1] * 1.1)
else:
viewer.cForceWnd.insertData('desForceMin', frame, 0.)
viewer.cForceWnd.insertData('desForceMax', frame, 0.)
self.setTimeStamp()
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
