def simulateCallback(frame):
# print()
# print(dartModel.getJointVelocityGlobal(0))
# print(dartModel.getDOFVelocities()[0])
# print(dartModel.get_dq()[:6])
dartMotionModel.update(motion[frame])
global g_initFlag
global forceShowTime
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
# print('contactstate:', contact, contactChangeCount)
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2.*(Kt**.5)
Dl = (Kl**.5)
Dh = (Kh**.5)
dt_sup = 2.*(kt_sup**.5)
# Dt = .2*(Kt**.5)
# Dl = .2*(Kl**.5)
# Dh = .2*(Kh**.5)
# dt_sup = .2*(kt_sup**.5)
pdcontroller.setKpKd(Kt, Dt)
footHeight = dartModel.getBody(supL).shapenodes[0].shape.size()[1]/2.
doubleTosingleOffset = 0.15
singleTodoubleOffset = 0.30
#doubleTosingleOffset = 0.09
doubleTosingleVelOffset = 0.0
com_offset_x, com_offset_y, com_offset_z = getParamVals(['com X offset', 'com Y offset', 'com Z offset'])
footOffset = np.array((com_offset_x, com_offset_y, com_offset_z))
des_com = dartMotionModel.getCOM() + footOffset
footCenterL = dartMotionModel.getBodyPositionGlobal(supL)
footCenterR = dartMotionModel.getBodyPositionGlobal(supR)
footBodyOriL = dartMotionModel.getBodyOrientationGlobal(supL)
footBodyOriR = dartMotionModel.getBodyOrientationGlobal(supR)
torso_pos = dartMotionModel.getBodyPositionGlobal(4)
torso_ori = dartMotionModel.getBodyOrientationGlobal(4)
# tracking
# th_r = motion.getDOFPositions(frame)
th_r = dartMotionModel.getDOFPositions()
th = dartModel.getDOFPositions()
th_r_flat = dartMotionModel.get_q()
# dth_r = motion.getDOFVelocities(frame)
dth = dartModel.getDOFVelocities()
# ddth_r = motion.getDOFAccelerations(frame)
# ddth_des = yct.getDesiredDOFAccelerations(th_r, th, None, dth, None, Kt, Dt)
dth_flat = dartModel.get_dq()
# dth_flat = np.concatenate(dth)
# ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q())
# ddth_des_flat = pdcontroller.compute(th_r)
ddth_des_flat = pdcontroller.compute_flat(th_r_flat)
# ype.flatten(ddth_des, ddth_des_flat)
# ype.flatten(dth, dth_flat)
#################################################
# jacobian
#################################################
contact_des_ids = [dartModel.skeleton.bodynode_index("LeftFoot")]
contact_ids = list() # temp idx for balancing
contact_ids.extend(contact_des_ids)
contact_joint_ori = list(map(dartModel.getJointOrientationGlobal, contact_ids))
contact_joint_pos = list(map(dartModel.getJointPositionGlobal, contact_ids))
contact_body_ori = list(map(dartModel.getBodyOrientationGlobal, contact_ids))
contact_body_pos = list(map(dartModel.getBodyPositionGlobal, contact_ids))
contact_body_vel = list(map(dartModel.getBodyVelocityGlobal, contact_ids))
contact_body_angvel = list(map(dartModel.getBodyAngVelocityGlobal, contact_ids))
ref_joint_ori = list(map(motion[frame].getJointOrientationGlobal, contact_ids))
ref_joint_pos = list(map(motion[frame].getJointPositionGlobal, contact_ids))
ref_joint_vel = [motion.getJointVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids]
ref_joint_angvel = [motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids]
ref_body_ori = list(map(dartMotionModel.getBodyOrientationGlobal, contact_ids))
ref_body_pos = list(map(dartMotionModel.getBodyPositionGlobal, contact_ids))
for idx in range(len(ref_body_pos)):
ref_body_pos[idx] = dartModel.skeleton.body("RightFoot").shapenodes[0].shape.size()[1]/2.
# ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids))
ref_body_angvel = [motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids]
ref_body_vel = [ref_joint_vel[i] + np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i])
for i in range(len(ref_joint_vel))]
is_contact = [1] * len(contact_ids)
contact_right = len(set(contact_des_ids).intersection(rIDlist)) > 0
contact_left = len(set(contact_des_ids).intersection(lIDlist)) > 0
footOriL = dartModel.getJointOrientationGlobal(supL)
footOriR = dartModel.getJointOrientationGlobal(supR)
footCenterL = dartModel.getBodyPositionGlobal(supL)
footCenterR = dartModel.getBodyPositionGlobal(supR)
footBodyOriL = dartModel.getBodyOrientationGlobal(supL)
footBodyOriR = dartModel.getBodyOrientationGlobal(supR)
footBodyVelL = dartModel.getBodyVelocityGlobal(supL)
footBodyVelR = dartModel.getBodyVelocityGlobal(supR)
footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL)
footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR)
refFootL = dartMotionModel.getBodyPositionGlobal(supL)
refFootR = dartMotionModel.getBodyPositionGlobal(supR)
# refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
# refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootAngVelL = np.zeros(3)
refFootAngVelR = np.zeros(3)
refFootJointVelR = motion.getJointVelocityGlobal(supR, frame)
refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootJointR = motion.getJointPositionGlobal(supR, frame)
# refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR-refFootJointR))
refFootVelR = np.zeros(3)
refFootJointVelL = motion.getJointVelocityGlobal(supL, frame)
refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
refFootJointL = motion.getJointPositionGlobal(supL, frame)
# refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL-refFootJointL))
refFootVelL = np.zeros(3)
contactR = 1
contactL = 1
# contMotionOffset = th[0][0] - th_r[0][0]
contMotionOffset = dartModel.getBodyPositionGlobal(0) - dartMotionModel.getBodyPositionGlobal(0)
contMotionOffset = np.zeros(3)
linkPositions = dartModel.getBodyPositionsGlobal()
linkVelocities = dartModel.getBodyVelocitiesGlobal()
linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal()
linkInertias = dartModel.getBodyInertiasGlobal()
CM = dartModel.skeleton.com()
dCM = dartModel.skeleton.com_velocity()
CM_plane = copy.copy(CM)
CM_plane[1]=0.
dCM_plane = copy.copy(dCM)
dCM_plane[1]=0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias)
#calculate jacobian
body_num = dartModel.getBodyNum()
Jsys = np.zeros((6*body_num, totalDOF))
dJsys = np.zeros((6*body_num, totalDOF))
Jsys_, dJsysdq = compute_J_dJdq(dartModel.skeleton)
# dJsys = np.zeros((6*body_num, totalDOF))
for i in range(dartModel.getBodyNum()):
Jsys[6*i:6*i+6, :] = dartModel.getBody(i).world_jacobian()[range(-3, 3), :]
dJsys[6*i:6*i+6, :] = dartModel.getBody(i).world_jacobian_classic_deriv()[range(-3, 3), :]
dJsysdq = np.dot(dJsys, dartModel.skeleton.dq)
# print(Jsys_ - Jsys)
# print(Jsys_.dot(dth_flat))
# print(Jsys.dot(dth_flat))
# print(dartModel.getBody(0).world_linear_velocity())
# print(np.dot(Jsys[:3, :3], Jsys[0:3, 3:6].T))
print('dq', np.asarray(dartModel.skeleton.dq)[6:9])
print('joint vel', dartModel.skeleton.joint(1).velocity())
# print('bjoint', mm.exp(dartModel.skeleton.q[6:9]).dot(get_bjoint_jacobian(dartModel.skeleton.q[6:9]).dot(np.asarray(dartModel.skeleton.dq)[6:9])))
# print('not bjoint', mm.exp(dartModel.skeleton.q[6:9]).dot(np.asarray(dartModel.skeleton.dq)[6:9]))
print('frombody', dartModel.getJointOrientationGlobal(1).T.dot(dartModel.getJointAngVelocityGlobal(1) - dartModel.getJointAngVelocityGlobal(0)))
print('ddq', np.asarray(dartModel.skeleton.ddq)[6:9])
# print('bjoint', mm.exp(dartModel.skeleton.q[6:9]).dot(get_bjoint_jacobian(dartModel.skeleton.q[6:9]).dot(np.asarray(dartModel.skeleton.dq)[6:9])))
# print('not bjoint', mm.exp(dartModel.skeleton.q[6:9]).dot(np.asarray(dartModel.skeleton.dq)[6:9]))
bodybody = dartModel.skeleton.body(1)
joint_trans = dartModel.skeleton.joint(1).get_world_frame_after_transform()
joint_pos = bodybody.to_local(joint_trans[:3, 3])
print('com spati', dartModel.getJointOrientationGlobal(1).T.dot(
dartModel.skeleton.body(1).world_angular_acceleration() - dartModel.skeleton.body(0).world_angular_acceleration()))
J_contacts = [] # type: list[np.ndarray]
dJ_contacts = [] # type: list[np.ndarray]
for contact_id in contact_ids:
J_contacts.append(Jsys[6*contact_id:6*contact_id + 6, :])
dJ_contacts.append(dJsysdq[6*contact_id:6*contact_id + 6])
#calculate footCenter
footCenter = .5 * (footCenterL + footCenterR) + footOffset
if contact == 2:
footCenter = footCenterL.copy() + footOffset
#elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
footCenter = footCenterR.copy() + footOffset
footCenter[1] = 0.
if contactChangeCount > 0 and contactChangeType == 'StoD':
# change footcenter gradually
footCenter = preFootCenter + (maxContactChangeCount - contactChangeCount)*(footCenter-preFootCenter)/maxContactChangeCount
preFootCenter = footCenter.copy()
# linear momentum
# CM_ref_plane = footCenter
# dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane
# dL_des_plane[1] = 0.
CM_ref_plane = footCenter
CM_ref_plane[1] = dartMotionModel.skeleton.com()[1]
dL_des_plane = Kl*totalMass*(CM_ref_plane - CM) - Dl*totalMass*dCM
# dL_des_plane[1] = 0.
# angular momentum
CP_ref = footCenter
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0])/frame_step_size
CP_old[0] = CP
CP_des[0] = None
if CP is not None and dCP is not None:
ddCP_des = Kh*(CP_ref - CP) - Dh*(dCP)
CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des*(frame_step_size**2)
dH_des = mm.cross(CP_des[0] - CM, dL_des_plane + totalMass*mm.s2v(wcfg.gravity))
else:
dH_des = None
# set up equality constraint
a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(contact_body_ori[i], mm.unitY()), mm.unitY()) for i in range(len(contact_body_ori))]))
KT_SUP = np.diag([kt_sup/10., kt_sup, kt_sup/10.])
a_sups = [np.append(np.dot(KT_SUP, (ref_body_pos[i] - contact_body_pos[i] + contMotionOffset)) - dt_sup * contact_body_vel[i],
kt_sup*a_oris[i] - dt_sup * contact_body_angvel[i]) for i in range(len(a_oris))]
# print(a_sups)
# print(np.asarray(dartModel.skeleton.dq)[0:3])
# print(dartModel.getJointAngVelocityGlobal(0))
# print(dartModel.getJointAngVelocityLocal(0))
# 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)
rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsysdq)
r_bias, s_bias = np.hsplit(rs, 2)
#######################################################
# optimization
#######################################################
#if contact == 2 and footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
config['weightMap']['RightUpLeg'] = .8
config['weightMap']['RightLeg'] = .8
config['weightMap']['RightFoot'] = .8
else:
config['weightMap']['RightUpLeg'] = .1
config['weightMap']['RightLeg'] = .25
config['weightMap']['RightFoot'] = .2
#if contact == 1 and footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
config['weightMap']['LeftUpLeg'] = .8
config['weightMap']['LeftLeg'] = .8
config['weightMap']['LeftFoot'] = .8
else:
config['weightMap']['LeftUpLeg'] = .1
config['weightMap']['LeftLeg'] = .25
config['weightMap']['LeftFoot'] = .2
# print('vel2', np.dot(dartModel.getJointOrientationGlobal(0).T, dartModel.skeleton.body(0).world_linear_velocity()))
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
#if contact == 2:
#mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
if dH_des is not None:
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
if True:
for c_idx in range(len(contact_ids)):
mot.addConstraint2(problem, totalDOF, J_contacts[c_idx], dJ_contacts[c_idx], a_sups[c_idx])
if contactChangeCount > 0:
contactChangeCount -= 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
r = problem.solve()
problem.clear()
ddth_sol_flat = np.asarray(r['x'])
# ype.nested(r['x'], ddth_sol)
# ddth_sol[:6] = np.zeros(6)
rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
inv_h = 1./time_step
_bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = [], [], [], []
for i in range(stepsPerFrame):
# apply penalty force
_ddq, _tau, _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = hqp.calc_QP(dartModel.skeleton, ddth_sol_flat, inv_h)
# _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck,mus, Ks, Ds)
dartModel.applyPenaltyForce(_bodyIDs, _contactPositionLocals, _contactForces)
dartModel.skeleton.set_forces(_tau)
# dartModel.setDOFAccelerations(ddth_sol)
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value()])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += wcfg.timeStep
dartModel.applyPenaltyForce(selectedBodyId, localPos, extraForce)
dartModel.step()
del bodyIDs[:]
del contactPositions[:]
del contactPositionLocals[:]
del contactForces[:]
bodyIDs.extend(_bodyIDs)
contactPositions.extend(_contactPositions)
contactPositionLocals.extend(_contactPositionLocals)
contactForces.extend(_contactForces)
# rendering
rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0]))
rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0]))
rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0,.1]))
rightFootPos[0] = footCenterL
rightVectorX[0] = np.dot(footBodyOriL, np.array([.1,0,0]))
rightVectorY[0] = np.dot(footBodyOriL, np.array([0,.1,0]))
rightVectorZ[0] = np.dot(footBodyOriL, np.array([0,0,.1]))
rightPos[0] = footCenterL + np.array([.1,0,0])
rd_footCenter[0] = footCenter
rd_footCenterL[0] = footCenterL
rd_footCenterR[0] = footCenterR
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
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[0]
rd_dL_des_plane[0] = [dL_des_plane[0]/100, dL_des_plane[1]/100, dL_des_plane[2]/100]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
rd_root_des[0] = rootPos[0]
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append( contactForces[i]/100)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100]
rd_exf_des[0] = [0,0,0]
else:
rd_exf_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100]
rd_exfen_des[0] = [0,0,0]
extraForcePos[0] = dartModel.getBodyPositionGlobal(selectedBody)
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(frame):
# print(frame)
# print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0']))
# hfi.footAdjust(motion[frame], idDic, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=.015, baseHeight=0.02)
if abs(getParamVal('tiptoe angle')) > 0.001:
tiptoe_angle = getParamVal('tiptoe angle')
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_0_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_0_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
# motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95))
# motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95))
# motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle))
# motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle))
if getParamVal('left tilt angle') > 0.001:
left_tilt_angle = getParamVal('left tilt angle')
if motion[0].skeleton.getJointIndex(
'LeftFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
else:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1_0'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot'], mm.exp(mm.unitZ(),
math.pi * left_tilt_angle))
elif getParamVal('left tilt angle') < -0.001:
left_tilt_angle = getParamVal('left tilt angle')
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_0'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
if motion[0].skeleton.getJointIndex(
'LeftFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1'],
mm.exp(mm.unitZ(), math.pi * left_tilt_angle))
else:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1_0'],
mm.exp(mm.unitZ(), math.pi * left_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot'], mm.exp(mm.unitZ(),
math.pi * left_tilt_angle))
if getParamVal('right tilt angle') > 0.001:
right_tilt_angle = getParamVal('right tilt angle')
if motion[0].skeleton.getJointIndex(
'RightFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
else:
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1_0'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot'],
mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
elif getParamVal('right tilt angle') < -0.001:
right_tilt_angle = getParamVal('right tilt angle')
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_0'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
if motion[0].skeleton.getJointIndex(
'RightFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1'],
mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
# else:
# motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot'],
mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
motionModel.update(motion[frame])
motionModel.translateByOffset(
np.array([
getParamVal('com X offset'),
getParamVal('com Y offset'),
getParamVal('com Z offset')
]))
controlModel_ik.set_q(controlModel.get_q())
global g_initFlag
global forceShowTime
global JsysPre
global JsupPreL
global JsupPreR
global JconstPre
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2 * (Kt**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
dt_sup = 2 * (kt_sup**.5)
# 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)
# ype.flatten(fix_dofs(DOFs, ddth_des, mcfg, joint_names), ddth_des_flat)
# ype.flatten(fix_dofs(DOFs, dth, mcfg, joint_names), dth_flat)
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
#################################################
# jacobian
#################################################
contact_des_ids = list() # desired contact segments
if foot_viewer.check_om_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0'))
if foot_viewer.check_op_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0'))
if foot_viewer.check_im_l is not None and foot_viewer.check_im_l.value(
):
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1'))
if foot_viewer.check_ip_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0'))
if foot_viewer.check_h_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0'))
if foot_viewer.check_om_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_0'))
if foot_viewer.check_op_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0'))
if foot_viewer.check_im_r is not None and foot_viewer.check_im_r.value(
):
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_1'))
if foot_viewer.check_ip_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0'))
if foot_viewer.check_h_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_1_0'))
contact_ids = list() # temp idx for balancing
contact_ids.extend(contact_des_ids)
contact_joint_ori = list(
map(controlModel.getJointOrientationGlobal, contact_ids))
contact_joint_pos = list(
map(controlModel.getJointPositionGlobal, contact_ids))
contact_body_ori = list(
map(controlModel.getBodyOrientationGlobal, contact_ids))
contact_body_pos = list(
map(controlModel.getBodyPositionGlobal, contact_ids))
contact_body_vel = list(
map(controlModel.getBodyVelocityGlobal, contact_ids))
contact_body_angvel = list(
map(controlModel.getBodyAngVelocityGlobal, contact_ids))
ref_joint_ori = list(
map(motion[frame].getJointOrientationGlobal, contact_ids))
ref_joint_pos = list(
map(motion[frame].getJointPositionGlobal, contact_ids))
ref_joint_vel = [
motion.getJointVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_joint_angvel = [
motion.getJointAngVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_body_ori = list(
map(motionModel.getBodyOrientationGlobal, contact_ids))
ref_body_pos = list(map(motionModel.getBodyPositionGlobal,
contact_ids))
# ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids))
ref_body_angvel = [
motion.getJointAngVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_body_vel = [
ref_joint_vel[i] +
np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i])
for i in range(len(ref_joint_vel))
]
is_contact = [1] * len(contact_ids)
contact_right = len(set(contact_des_ids).intersection(rIDlist)) > 0
contact_left = len(set(contact_des_ids).intersection(lIDlist)) > 0
contMotionOffset = th[0][0] - th_r[0][0]
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
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.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
# calculate jacobian
Jsys, dJsys = controlModel.computeCom_J_dJdq()
J_contacts = [] # type: list[np.ndarray]
dJ_contacts = [] # type: list[np.ndarray]
for contact_id in contact_ids:
J_contacts.append(Jsys[6 * contact_id:6 * contact_id + 6, :])
dJ_contacts.append(dJsys[6 * contact_id:6 * contact_id + 6])
# calculate footCenter
footCenter = sum(contact_body_pos) / len(contact_body_pos) if len(contact_body_pos) > 0 \
else .5 * (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(supR))
footCenter[1] = 0.
# if len(contact_body_pos) > 2:
# hull = ConvexHull(contact_body_pos)
footCenter_ref = sum(ref_body_pos) / len(ref_body_pos) if len(ref_body_pos) > 0 \
else .5 * (motionModel.getBodyPositionGlobal(supL) + motionModel.getBodyPositionGlobal(supR))
footCenter_ref = footCenter_ref + contMotionOffset
# if len(ref_body_pos) > 2:
# hull = ConvexHull(ref_body_pos)
footCenter_ref[1] = 0.
# footCenter[0] = footCenter[0] + getParamVal('com X offset')
# footCenter[1] = footCenter[0] + getParamVal('com Y offset')
# footCenter[2] = footCenter[2] + getParamVal('com Z offset')
# initialization
if g_initFlag == 0:
preFootCenter[0] = footCenter.copy()
g_initFlag = 1
# if contactChangeCount == 0 and np.linalg.norm(footCenter - preFootCenter[0]) > 0.01:
# contactChangeCount += 30
if contactChangeCount > 0:
# change footcenter gradually
footCenter = preFootCenter[0] + (
maxContactChangeCount - contactChangeCount) * (
footCenter - preFootCenter[0]) / maxContactChangeCount
else:
preFootCenter[0] = footCenter.copy()
# linear momentum
# TODO:
# We should consider dCM_ref, shouldn't we?
# add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel!
# to do that, set joint velocities to vpModel
CM_ref_plane = footCenter
# CM_ref_plane = footCenter_ref
CM_ref = footCenter + np.array([
getParamVal('com X offset'),
motionModel.getCOM()[1] + getParamVal('com Y offset'),
getParamVal('com Z offset')
])
dL_des_plane = Kl * totalMass * (CM_ref - CM) - Dl * totalMass * dCM
# dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane
# dL_des_plane[1] = 0.
# print('dCM_plane : ', np.linalg.norm(dCM_plane))
# angular momentum
CP_ref = footCenter
# CP_ref = footCenter_ref
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0]) / (1 / 30.)
CP_old[0] = CP
if CP is not None and dCP is not None:
ddCP_des = Kh * (CP_ref - CP) - Dh * dCP
dCP_des = dCP + ddCP_des * (1 / 30.)
CP_des = CP + dCP_des * (1 / 30.)
# CP_des = footCenter
CP_des = CP + dCP * (1 / 30.) + .5 * ddCP_des * ((1 / 30.)**2)
dH_des = np.cross(
(CP_des - CM),
(dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
if contactChangeCount > 0: # and contactChangeType == 'DtoS':
dH_des *= (maxContactChangeCount -
contactChangeCount) / maxContactChangeCount
else:
dH_des = None
# convex hull
contact_pos_2d = np.asarray([
np.array([contactPosition[0], contactPosition[2]])
for contactPosition in contactPositions
])
p = np.array([CM_plane[0], CM_plane[2]])
# hull = None # type: Delaunay
# if contact_pos_2d.shape[0] > 0:
# hull = Delaunay(contact_pos_2d)
# print(hull.find_simplex(p) >= 0)
# set up equality constraint
# TODO:
# logSO3 is just q'', not acceleration.
# To make a_oris acceleration, q'' -> a will be needed
# body_ddqs = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori]))
# body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], mm.unitY()), mm.unitY()))) for i in range(len(contact_body_ori))]))
# body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()))) for i in range(len(contact_body_ori))]))
a_oris = list(
map(mm.logSO3, [
np.dot(
contact_body_ori[i].T,
np.dot(
ref_body_ori[i],
mm.getSO3FromVectors(
np.dot(ref_body_ori[i],
up_vec_in_each_link[contact_ids[i]]),
mm.unitY()))) for i in range(len(contact_body_ori))
]))
a_oris = list(
map(mm.logSO3, [
np.dot(
np.dot(
ref_body_ori[i],
mm.getSO3FromVectors(
np.dot(ref_body_ori[i],
up_vec_in_each_link[contact_ids[i]]),
mm.unitY())), contact_body_ori[i].T)
for i in range(len(contact_body_ori))
]))
body_qs = list(map(mm.logSO3, contact_body_ori))
body_angs = [
np.dot(contact_body_ori[i], contact_body_angvel[i])
for i in range(len(contact_body_ori))
]
body_dqs = [
mm.vel2qd(body_angs[i], body_qs[i]) for i in range(len(body_angs))
]
# a_oris = [np.dot(contact_body_ori[i], mm.qdd2accel(body_ddqs[i], body_dqs[i], body_qs[i])) for i in range(len(contact_body_ori))]
# body_ddq = body_ddqs[0]
# body_ori = contact_body_ori[0]
# body_ang = np.dot(body_ori.T, contact_body_angvel[0])
#
# body_q = mm.logSO3(body_ori)
# body_dq = mm.vel2qd(body_ang, body_q)
# a_ori = np.dot(body_ori, mm.qdd2accel(body_ddq, body_dq, body_q))
KT_SUP = np.diag([kt_sup / 10., kt_sup, kt_sup / 10.])
# KT_SUP = np.diag([kt_sup, kt_sup, kt_sup])
# a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori]))
# a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(contact_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()) for i in range(len(contact_body_ori))]))
# a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) + dt_sup*(ref_body_vel[i] - contact_body_vel[i]),
# kt_sup*a_oris[i]+dt_sup*(ref_body_angvel[i]-contact_body_angvel[i])) for i in range(len(a_oris))]
# a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) - dt_sup * contact_body_vel[i],
# kt_sup*a_oris[i] - dt_sup * contact_body_angvel[i]) for i in range(len(a_oris))]
a_sups = [
np.append(
np.dot(KT_SUP,
(ref_body_pos[i] - contact_body_pos[i] +
contMotionOffset)) - dt_sup * contact_body_vel[i],
kt_sup * a_oris[i] - dt_sup * contact_body_angvel[i])
for i in range(len(a_oris))
]
# for i in range(len(a_sups)):
# a_sups[i][1] = -kt_sup * contact_body_pos[i][1] - dt_sup * contact_body_vel[i][1]
# 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)
rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsys)
r_bias, s_bias = np.hsplit(rs, 2)
#######################################################
# optimization
#######################################################
# if contact == 2 and footCenterR[1] > doubleTosingleOffset/2:
if contact_left and not contact_right:
config['weightMap']['RightUpLeg'] = .8
config['weightMap']['RightLeg'] = .8
config['weightMap']['RightFoot'] = .8
else:
config['weightMap']['RightUpLeg'] = .1
config['weightMap']['RightLeg'] = .25
config['weightMap']['RightFoot'] = .2
# if contact == 1 and footCenterL[1] > doubleTosingleOffset/2:
if contact_right and not contact_left:
config['weightMap']['LeftUpLeg'] = .8
config['weightMap']['LeftLeg'] = .8
config['weightMap']['LeftFoot'] = .8
else:
config['weightMap']['LeftUpLeg'] = .1
config['weightMap']['LeftLeg'] = .25
config['weightMap']['LeftFoot'] = .2
w = mot.getTrackingWeight(DOFs, motion[0].skeleton,
config['weightMap'])
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
if dH_des is not None:
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
if True:
for c_idx in range(len(contact_ids)):
mot.addConstraint2(problem, totalDOF, J_contacts[c_idx],
dJ_contacts[c_idx], dth_flat,
a_sups[c_idx])
if contactChangeCount > 0:
contactChangeCount = contactChangeCount - 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
r = problem.solve()
problem.clear()
ddth_sol_flat = np.asarray(r['x'])
# ddth_sol_flat[foot_seg_dofs] = np.array(ddth_des_flat)[foot_seg_dofs]
ype.nested(ddth_sol_flat, 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)
# bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
controlModel.setDOFAccelerations(ddth_sol)
# controlModel.setDOFAccelerations(ddth_des)
# controlModel.set_ddq(ddth_sol_flat)
# controlModel.set_ddq(ddth_des_flat)
controlModel.solveHybridDynamics()
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([
viewer.objectInfoWnd.labelForceX.value(),
viewer.objectInfoWnd.labelForceY.value(),
viewer.objectInfoWnd.labelForceZ.value()
])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += wcfg.timeStep
vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
vpWorld.step()
controlModel_ik.set_q(controlModel.get_q())
if foot_viewer is not None:
foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info(
frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds)
foot_viewer.foot_pressure_gl_window.goToFrame(frame)
# rendering
for foot_seg_id in footIdlist:
control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255)
for contact_id in contact_ids:
control_model_renderer.body_colors[contact_id] = (255, 0, 0)
rd_footCenter[0] = footCenter
rd_footCenter_ref[0] = footCenter_ref
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
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[0] / 100, dL_des_plane[1] / 100,
dL_des_plane[2] / 100
]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity)
del rd_foot_ori[:]
del rd_foot_pos[:]
# for seg_foot_id in footIdlist:
# rd_foot_ori.append(controlModel.getJointOrientationGlobal(seg_foot_id))
# rd_foot_pos.append(controlModel.getJointPositionGlobal(seg_foot_id))
rd_foot_ori.append(controlModel.getJointOrientationGlobal(supL))
rd_foot_ori.append(controlModel.getJointOrientationGlobal(supR))
rd_foot_pos.append(controlModel.getJointPositionGlobal(supL))
rd_foot_pos.append(controlModel.getJointPositionGlobal(supR))
rd_root_des[0] = rootPos[0]
rd_root_ori[0] = controlModel.getBodyOrientationGlobal(0)
rd_root_pos[0] = controlModel.getBodyPositionGlobal(0)
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append(contactForces[i] / 400)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exf_des[0] = [0, 0, 0]
else:
rd_exf_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exfen_des[0] = [0, 0, 0]
# extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody)
extraForcePos[0] = controlModel.getBodyPositionGlobal(
selectedBody) - 0.1 * np.array([
viewer.objectInfoWnd.labelForceX.value(), 0.,
viewer.objectInfoWnd.labelForceZ.value()
])
# render contact_ids
# render skeleton
if SKELETON_ON:
Ts = dict()
Ts['pelvis'] = controlModel.getJointTransform(idDic['Hips'])
Ts['thigh_R'] = controlModel.getJointTransform(idDic['RightUpLeg'])
Ts['shin_R'] = controlModel.getJointTransform(idDic['RightLeg'])
Ts['foot_R'] = controlModel.getJointTransform(idDic['RightFoot'])
Ts['foot_heel_R'] = controlModel.getJointTransform(
idDic['RightFoot'])
Ts['heel_R'] = np.eye(4)
Ts['outside_metatarsal_R'] = controlModel.getJointTransform(
idDic['RightFoot_foot_0_0'])
Ts['outside_phalanges_R'] = controlModel.getJointTransform(
idDic['RightFoot_foot_0_0_0'])
# Ts['inside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1'])
Ts['inside_metatarsal_R'] = np.eye(4)
Ts['inside_phalanges_R'] = controlModel.getJointTransform(
idDic['RightFoot_foot_0_1_0'])
Ts['spine_ribs'] = controlModel.getJointTransform(idDic['Spine'])
Ts['head'] = controlModel.getJointTransform(idDic['Spine1'])
Ts['upper_limb_R'] = controlModel.getJointTransform(
idDic['RightArm'])
Ts['lower_limb_R'] = controlModel.getJointTransform(
idDic['RightForeArm'])
Ts['thigh_L'] = controlModel.getJointTransform(idDic['LeftUpLeg'])
Ts['shin_L'] = controlModel.getJointTransform(idDic['LeftLeg'])
Ts['foot_L'] = controlModel.getJointTransform(idDic['LeftFoot'])
Ts['foot_heel_L'] = controlModel.getJointTransform(
idDic['LeftFoot'])
Ts['heel_L'] = np.eye(4)
Ts['outside_metatarsal_L'] = controlModel.getJointTransform(
idDic['LeftFoot_foot_0_0'])
Ts['outside_phalanges_L'] = controlModel.getJointTransform(
idDic['LeftFoot_foot_0_0_0'])
# Ts['inside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1'])
Ts['inside_metatarsal_L'] = np.eye(4)
Ts['inside_phalanges_L'] = controlModel.getJointTransform(
idDic['LeftFoot_foot_0_1_0'])
Ts['upper_limb_L'] = controlModel.getJointTransform(
idDic['LeftArm'])
Ts['lower_limb_L'] = controlModel.getJointTransform(
idDic['LeftForeArm'])
skeleton_renderer.appendFrameState(Ts)
def simulateCallback(frame):
print(frame)
# print()
# print(dartModel.getJointVelocityGlobal(0))
# print(dartModel.getDOFVelocities()[0])
# print(dartModel.get_dq()[:6])
dartMotionModel.update(motion[frame])
global g_initFlag
global forceShowTime
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
# print('contactstate:', contact, contactChangeCount)
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
# Dt = 2.*(Kt**.5)
Dt = Kt/100.
Dl = (Kl**.5)
Dh = (Kh**.5)
dt_sup = 2.*(kt_sup**.5)
# Dt = .2*(Kt**.5)
# Dl = .2*(Kl**.5)
# Dh = .2*(Kh**.5)
# dt_sup = .2*(kt_sup**.5)
pdcontroller.setKpKd(Kt, Dt)
footHeight = dartModel.getBody(supL).shapenodes[0].shape.size()[1]/2.
doubleTosingleOffset = 0.15
singleTodoubleOffset = 0.30
#doubleTosingleOffset = 0.09
doubleTosingleVelOffset = 0.0
com_offset_x, com_offset_y, com_offset_z = getParamVals(['com X offset', 'com Y offset', 'com Z offset'])
footOffset = np.array((com_offset_x, com_offset_y, com_offset_z))
des_com = dartMotionModel.getCOM() + footOffset
footCenterL = dartMotionModel.getBodyPositionGlobal(supL)
footCenterR = dartMotionModel.getBodyPositionGlobal(supR)
footBodyOriL = dartMotionModel.getBodyOrientationGlobal(supL)
footBodyOriR = dartMotionModel.getBodyOrientationGlobal(supR)
torso_pos = dartMotionModel.getBodyPositionGlobal(4)
torso_ori = dartMotionModel.getBodyOrientationGlobal(4)
# ik_solver.setInitPose(motion[frame])
# ik_solver.addConstraints(supL, np.zeros(3), footCenterL, footBodyOriL, (True, True, True, True))
# ik_solver.addConstraints(supR, np.zeros(3), footCenterR, footBodyOriR, (True, True, True, True))
# ik_solver.addConstraints(4, np.zeros(3), torso_pos, torso_ori, (False, False, False, True))
# ik_solver.solve(des_com)
# ik_solver.clear()
# tracking
# th_r = motion.getDOFPositions(frame)
th_r = dartMotionModel.getDOFPositions()
th = dartModel.getDOFPositions()
th_r_flat = dartMotionModel.get_q()
# dth_r = motion.getDOFVelocities(frame)
# dth = dartModel.getDOFVelocities()
# ddth_r = motion.getDOFAccelerations(frame)
# ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
dth_flat = dartModel.get_dq()
# dth_flat = np.concatenate(dth)
# ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q())
# ddth_des_flat = pdcontroller.compute(th_r)
ddth_des_flat = pdcontroller.compute_flat(th_r_flat)
# ype.flatten(ddth_des, ddth_des_flat)
# ype.flatten(dth, dth_flat)
#################################################
# jacobian
#################################################
footOriL = dartModel.getJointOrientationGlobal(supL)
footOriR = dartModel.getJointOrientationGlobal(supR)
footCenterL = dartModel.getBodyPositionGlobal(supL)
footCenterR = dartModel.getBodyPositionGlobal(supR)
footBodyOriL = dartModel.getBodyOrientationGlobal(supL)
footBodyOriR = dartModel.getBodyOrientationGlobal(supR)
footBodyVelL = dartModel.getBodyVelocityGlobal(supL)
footBodyVelR = dartModel.getBodyVelocityGlobal(supR)
footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL)
footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR)
refFootL = dartMotionModel.getBodyPositionGlobal(supL)
refFootR = dartMotionModel.getBodyPositionGlobal(supR)
# refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
# refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootAngVelL = np.zeros(3)
refFootAngVelR = np.zeros(3)
refFootJointVelR = motion.getJointVelocityGlobal(supR, frame)
refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootJointR = motion.getJointPositionGlobal(supR, frame)
# refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR-refFootJointR))
refFootVelR = np.zeros(3)
refFootJointVelL = motion.getJointVelocityGlobal(supL, frame)
refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
refFootJointL = motion.getJointPositionGlobal(supL, frame)
# refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL-refFootJointL))
refFootVelL = np.zeros(3)
contactR = 1
contactL = 1
if refFootVelR[1] < 0 and refFootVelR[1]*frame_step_size + refFootR[1] > singleTodoubleOffset:
contactR = 0
if refFootVelL[1] < 0 and refFootVelL[1]*frame_step_size + refFootL[1] > singleTodoubleOffset:
contactL = 0
if refFootVelR[1] > 0 and refFootVelR[1]*frame_step_size + refFootR[1] > doubleTosingleOffset:
contactR = 0
if refFootVelL[1] > 0 and refFootVelL[1]*frame_step_size + refFootL[1] > doubleTosingleOffset:
contactL = 0
# contactR = 0
# contMotionOffset = th[0][0] - th_r[0][0]
contMotionOffset = dartModel.getBodyPositionGlobal(0) - dartMotionModel.getBodyPositionGlobal(0)
linkPositions = dartModel.getBodyPositionsGlobal()
linkVelocities = dartModel.getBodyVelocitiesGlobal()
linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal()
linkInertias = dartModel.getBodyInertiasGlobal()
CM = dartModel.skeleton.com()
dCM = dartModel.skeleton.com_velocity()
CM_plane = copy.copy(CM)
CM_plane[1]=0.
dCM_plane = copy.copy(dCM)
dCM_plane[1]=0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias)
#calculate contact state
#if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08:
if g_initFlag == 1:
#contact state
# 0: flying 1: right only 2: left only 3: double
#if contact == 2 and refFootR[1] < doubleTosingleOffset:
if contact == 2 and contactR==1:
contact = 3
maxContactChangeCount+=30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
#elif contact == 3 and refFootL[1] < doubleTosingleOffset:
elif contact == 1 and contactL==1:
contact = 3
maxContactChangeCount+=30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
#elif contact == 3 and refFootR[1] > doubleTosingleOffset:
elif contact == 3 and contactR == 0:
contact = 2
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
#elif contact == 3 and refFootL[1] > doubleTosingleOffset:
elif contact == 3 and contactL == 0:
contact = 1
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
else:
contact = 0
#if refFootR[1] < doubleTosingleOffset:
if contactR == 1:
contact +=1
#if refFootL[1] < doubleTosingleOffset:
if contactL == 1:
contact +=2
#initialization
if g_initFlag == 0:
softConstPoint = footCenterR.copy()
footCenter = footCenterL + (footCenterR - footCenterL)/2.0
footCenter[1] = 0.
preFootCenter = footCenter.copy()
#footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL)
#footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR)
# if refFootR[1] < doubleTosingleOffset:
# contact +=1
# if refFootL[1] < doubleTosingleOffset:
# contact +=2
if refFootR[1] < footHeight:
contact +=1
if refFootL[1] < footHeight:
contact +=2
g_initFlag = 1
#calculate jacobian
body_num = dartModel.getBodyNum()
Jsys = np.zeros((6*body_num, totalDOF))
dJsys = np.zeros((6*body_num, totalDOF))
for i in range(dartModel.getBodyNum()):
# body_i_jacobian = dartModel.getBody(i).world_jacobian()[range(-3, 3), :]
# body_i_jacobian_deriv = dartModel.getBody(i).world_jacobian_classic_deriv()[range(-3, 3), :]
# Jsys[6*i:6*i+6, :] = body_i_jacobian
# dJsys[6*i:6*i+6, :] = body_i_jacobian_deriv
Jsys[6*i:6*i+6, :] = dartModel.getBody(i).world_jacobian()[range(-3, 3), :]
dJsys[6*i:6*i+6, :] = dartModel.getBody(i).world_jacobian_classic_deriv()[range(-3, 3), :]
# dJsys = (Jsys - Jpre[0])/frame_step_size
# Jpre[0] = Jsys.copy()
JsupL = dartModel.getBody(supL).world_jacobian()[range(-3, 3), :]
dJsupL = dartModel.getBody(supL).world_jacobian_classic_deriv()[range(-3, 3), :]
# dJsupL = np.zeros_like(JsupL)
# dJsupL = (JsupL - Jpre[1])/frame_step_size
# Jpre[1] = JsupL.copy()
JsupR = dartModel.getBody(supR).world_jacobian()[range(-3, 3), :]
dJsupR = dartModel.getBody(supR).world_jacobian_classic_deriv()[range(-3, 3), :]
# dJsupR = np.zeros_like(JsupR)
# dJsupR = (JsupR - Jpre[2])/frame_step_size
# Jpre[2] = JsupR.copy()
#calculate footCenter
footCenter = .5 * (footCenterL + footCenterR) + footOffset
#if refFootR[1] >doubleTosingleOffset:
#if refFootR[1] > doubleTosingleOffset or footCenterR[1] > 0.08:
#if contact == 1 or footCenterR[1] > 0.08:
#if contact == 2 or footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
footCenter = footCenterL.copy() + footOffset
#elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
footCenter = footCenterR.copy() + footOffset
footCenter[1] = 0.
if contactChangeCount > 0 and contactChangeType == 'StoD':
#change footcenter gradually
footCenter = preFootCenter + (maxContactChangeCount - contactChangeCount)*(footCenter-preFootCenter)/maxContactChangeCount
preFootCenter = footCenter.copy()
# linear momentum
#TODO:
# We should consider dCM_ref, shouldn't we?
# add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel!
# todo that, set joint velocities to vpModel
CM_ref_plane = footCenter
dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane
dL_des_plane[1] = 0.
# CM_ref = footCenter.copy()
# CM_ref[1] = dartMotionModel.getCOM()[1]
# CM_ref += np.array((0., com_offset_y, 0.))
# dL_des_plane = Kl*totalMass*(CM_ref - CM) - Dl*totalMass*dCM
# angular momentum
CP_ref = footCenter
bodyIDs, contactPositions, contactPositionLocals, contactForces = [], [], [], []
if DART_CONTACT_ON:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info()
else:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
#bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0])/frame_step_size
CP_old[0] = CP
CP_des[0] = None
# if CP_des[0] is None:
# CP_des[0] = footCenter
if CP is not None and dCP is not None:
ddCP_des = Kh*(CP_ref - CP) - Dh*(dCP)
CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des*(frame_step_size**2)
# dCP_des[0] += ddCP_des * frame_step_size
# CP_des[0] += dCP_des[0] * frame_step_size + .5 * ddCP_des*(frame_step_size ** 2)
dH_des = np.cross(CP_des[0] - CM, dL_des_plane + totalMass*mm.s2v(wcfg.gravity))
if contactChangeCount > 0:# and contactChangeType == 'DtoS':
#dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount*10)
dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount)
#dH_des *= (contactChangeCount)/(maxContactChangeCount)*.9+.1
else:
dH_des = None
# H = np.dot(P, np.dot(Jsys, dth_flat))
# dH_des = -Kh* H[3:]
# soft point constraint
#softConstPoint = refFootR.copy()
##softConstPoint[0] += 0.2
#Ksc = 50
#Dsc = 2*(Ksc**.5)
#Bsc = 1.
#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_des - dP_cur)
#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)
#dJconst = (Jconst - Jconst)/(1/30.)
#JconstPre = Jconst.copy()
##yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False)
#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))
#q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2
#set up equality constraint
a_oriL = mm.logSO3(mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0,1,0])), np.array([0,1,0])))
a_oriR = mm.logSO3(mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0,1,0])), np.array([0,1,0])))
footErrorL = refFootL.copy()
footErrorL[1] = dartModel.getBody(supL).shapenodes[0].shape.size()[1]/2.
footErrorL += -footCenterL + contMotionOffset
footErrorR = refFootR.copy()
footErrorR[1] = dartModel.getBody(supR).shapenodes[0].shape.size()[1]/2.
footErrorR += -footCenterR + contMotionOffset
a_supL = np.append(kt_sup*footErrorL + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL))
a_supR = np.append(kt_sup*footErrorR + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR))
if contactChangeCount > 0 and contactChangeType == 'DtoS':
a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), 4*kt_sup*a_oriL+2*dt_sup*(refFootAngVelL-footBodyAngVelL))
a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), 4*kt_sup*a_oriR+2*dt_sup*(refFootAngVelR-footBodyAngVelR))
elif contactChangeCount > 0 and contactChangeType == 'StoD':
linkt = (13.*contactChangeCount)/(maxContactChangeCount)+1.
lindt = 2*(linkt**.5)
angkt = (13.*contactChangeCount)/(maxContactChangeCount)+1.
angdt = 2*(angkt**.5)
a_supL = np.append(linkt*kt_sup*(refFootL - footCenterL + contMotionOffset) + lindt*dt_sup*(refFootVelL - footBodyVelL), angkt*kt_sup*a_oriL+angdt*dt_sup*(refFootAngVelL-footBodyAngVelL))
a_supR = np.append(linkt*kt_sup*(refFootR - footCenterR + contMotionOffset) + lindt*dt_sup*(refFootVelR - footBodyVelR), angkt*kt_sup*a_oriR+angdt*dt_sup*(refFootAngVelR-footBodyAngVelR))
# 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)
#######################################################
# optimization
#######################################################
#if contact == 2 and footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
config['weightMap']['RightUpLeg'] = .8
config['weightMap']['RightLeg'] = .8
config['weightMap']['RightFoot'] = .8
else:
config['weightMap']['RightUpLeg'] = .1
config['weightMap']['RightLeg'] = .25
config['weightMap']['RightFoot'] = .2
#if contact == 1 and footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
config['weightMap']['LeftUpLeg'] = .8
config['weightMap']['LeftLeg'] = .8
config['weightMap']['LeftFoot'] = .8
else:
config['weightMap']['LeftUpLeg'] = .1
config['weightMap']['LeftLeg'] = .25
config['weightMap']['LeftFoot'] = .2
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
#if contact == 2:
#mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
if dH_des is not None:
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
#mot.setConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup)
#mot.addConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup)
#if contact & 1 and contactChangeCount == 0:
if contact & 1:
#if refFootR[1] < doubleTosingleOffset:
mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_supR)
if contact & 2:
#if refFootL[1] < doubleTosingleOffset:
mot.addConstraint(problem, totalDOF, JsupL, dJsupL, dth_flat, a_supL)
if contactChangeCount >0:
contactChangeCount -= 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
r = problem.solve()
problem.clear()
# ype.nested(r['x'], ddth_sol)
ddth_sol = np.asarray(r['x'])
# ddth_sol[:6] = np.zeros(6)
rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
for i in range(stepsPerFrame):
# apply penalty force
if not DART_CONTACT_ON:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
dartModel.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces)
#bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
# dartModel.skeleton.set_accelerations(ddth_sol)
# dartModel.skeleton.set_accelerations(ddth_des_flat)
# dartModel.skeleton.set_forces(np.zeros(totalDOF))
# ddth_des_flat[:6] = np.zeros(6)
th_r_flat = dartMotionModel.get_q()
ddth_des_flat = pdcontroller.compute_flat(th_r_flat)
dartModel.skeleton.set_forces(ddth_des_flat)
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value()])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += wcfg.timeStep
dartModel.applyPenaltyForce(selectedBodyId, localPos, extraForce)
dartModel.step()
if DART_CONTACT_ON:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info()
else:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
if viewer.reset:
viewer.reset = False
dartModel.reset()
# print(dartModel.getCOM())
# rendering
rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0]))
rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0]))
rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0,.1]))
rightFootPos[0] = footCenterL
rightVectorX[0] = np.dot(footBodyOriL, np.array([.1,0,0]))
rightVectorY[0] = np.dot(footBodyOriL, np.array([0,.1,0]))
rightVectorZ[0] = np.dot(footBodyOriL, np.array([0,0,.1]))
rightPos[0] = footCenterL + np.array([.1,0,0])
rd_footCenter[0] = footCenter
rd_footCenterL[0] = footCenterL
rd_footCenterR[0] = footCenterR
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
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[0]
rd_dL_des_plane[0] = [dL_des_plane[0]/100, dL_des_plane[1]/100, dL_des_plane[2]/100]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
rd_root_des[0] = rootPos[0]
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append( contactForces[i]/100)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100]
rd_exf_des[0] = [0,0,0]
else:
rd_exf_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100]
rd_exfen_des[0] = [0,0,0]
extraForcePos[0] = dartModel.getBodyPositionGlobal(selectedBody)
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)
# 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()
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if CP != None:
CP[1] = 0.
for i in range(len(bodyIDsToCheck)):
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] == None or CP == None:
dCP = None
else:
dCP = (CP - CP_old[0]) * 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)
#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 == 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 != None:
# mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2)
timeReport[4] += time.time() - curTime
curTime = time.time()
#print np.NAN
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()
'''
if (frame > 5):
tau = controlModel.getJointTorqueLocal(indexFootL[3])
tau2 = controlModel.getJointTorqueLocal(indexFootL[4])
tau3 = controlModel.getJointTorqueLocal(indexFootR[3])
tau4 = controlModel.getJointTorqueLocal(indexFootR[4])
torques = controlModel.getInternalJointTorquesLocal()
if (frame > 100 and frame < 110) or (frame > 165 and frame < 190):
Wcal1 = 0.05
Wcal2 = 0.05
dC = fCom[2]*Wcal1-dCM[2]*Wcal2
print("dC", dC)
torques[indexFootL[5]-1]+= (dC, 0.0, 0.0)
torques[indexFootR[5]-1]+= (dC, 0.0, 0.0)
if (frame > 50 and frame < 75) or (frame > 110 and frame <140) or (frame > 185 and frame < 220):
metatarR = [controlModel.getBodyOrientationGlobal(indexFootL[1])]
phalangeR = [controlModel.getBodyOrientationGlobal(indexFootL[3])]
metatarR2 = np.dot(metatarR, np.array([0,0,1]))
phalangeR2 = np.dot(phalangeR, np.array([0,0,1]))
metatarRZ = mm.normalize(metatarR2[0])
phalangeRZ = mm.normalize(phalangeR2[0])
lean = np.dot(metatarRZ, phalangeRZ)
Wlean = 2
dt = -0.02*(lean)*Wlean
torques[indexFootL[3]-1]+= (dt, 0.0, 0.0)
torques[indexFootL[4]-1]+= (dt, 0.0, 0.0)
torques[indexFootR[3]-1]+= (dt, 0.0, 0.0)
torques[indexFootR[4]-1]+= (dt, 0.0, 0.0)
i = 0
t = 6
while t < len(TauJT) :
torques[i] += (TauJT[t]+TauAM[t], TauJT[t+1]+TauAM[t+1], TauJT[t+2]+TauAM[t+2])
i+=1
t+=3
#totalTorques = [a + b for a, b in zip(torques, TauJT)]
#print("torques2", torques)
#print("TauJT", TauJT[16], TauJT[17], TauJT[18])
#print("torques", torques[16])
#print("totalTorques", totalTorques[16])
controlModel.setInternalJointTorquesLocal(torques)
'''
'''
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()
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
def simulateCallback(frame):
motionModel.update(motion[frame])
global g_initFlag
global forceShowTime
global JsysPre
global JsupPreL
global JsupPreR
global JsupPre
global JconstPre
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
# Kt, Kl, Kh, Bl, Bh, kt_sup = viewer.GetParam()
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2 * (Kt**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
dt_sup = 2 * (kt_sup**.5)
doubleTosingleOffset = 0.15
singleTodoubleOffset = 0.30
# doubleTosingleOffset = 0.09
doubleTosingleVelOffset = 0.0
# 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)
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
#################################################
# jacobian
#################################################
# caution!! body orientation and joint orientation of foot are totally different!!
footOriL = controlModel.getJointOrientationGlobal(supL)
footOriR = controlModel.getJointOrientationGlobal(supR)
# desire footCenter[1] = 0.041135
# desire footCenter[1] = 0.0197
footCenterL = controlModel.getBodyPositionGlobal(supL)
footCenterR = controlModel.getBodyPositionGlobal(supR)
footBodyOriL = controlModel.getBodyOrientationGlobal(supL)
footBodyOriR = controlModel.getBodyOrientationGlobal(supR)
footBodyVelL = controlModel.getBodyVelocityGlobal(supL)
footBodyVelR = controlModel.getBodyVelocityGlobal(supR)
footBodyAngVelL = controlModel.getBodyAngVelocityGlobal(supL)
footBodyAngVelR = controlModel.getBodyAngVelocityGlobal(supR)
refFootL = motionModel.getBodyPositionGlobal(supL)
refFootR = motionModel.getBodyPositionGlobal(supR)
refFootVelL = motionModel.getBodyVelocityGlobal(supL)
refFootVelR = motionModel.getBodyVelocityGlobal(supR)
refFootAngVelL = motionModel.getBodyAngVelocityGlobal(supL)
refFootAngVelR = motionModel.getBodyAngVelocityGlobal(supR)
refFootJointVelR = motion.getJointVelocityGlobal(supR, frame)
refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootJointR = motion.getJointPositionGlobal(supR, frame)
refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR,
(refFootR - refFootJointR))
refFootJointVelL = motion.getJointVelocityGlobal(supL, frame)
refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
refFootJointL = motion.getJointPositionGlobal(supL, frame)
refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL,
(refFootL - refFootJointL))
contactR = 1
contactL = 1
if refFootVelR[1] < 0 and refFootVelR[1] / 30. + refFootR[
1] > singleTodoubleOffset:
contactR = 0
if refFootVelL[1] < 0 and refFootVelL[1] / 30. + refFootL[
1] > singleTodoubleOffset:
contactL = 0
if refFootVelR[1] > 0 and refFootVelR[1] / 30. + refFootR[
1] > doubleTosingleOffset:
contactR = 0
if refFootVelL[1] > 0 and refFootVelL[1] / 30. + refFootL[
1] > doubleTosingleOffset:
contactL = 0
# if 32 < frame < 147:
# contactR = 0
contMotionOffset = th[0][0] - th_r[0][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.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
# calculate jacobian
Jsys, dJsys = controlModel.computeCom_J_dJdq()
JsupL = Jsys[6 * supL:6 * supL + 6, :]
dJsupL = dJsys[6 * supL:6 * supL + 6]
JsupR = Jsys[6 * supR:6 * supR + 6, :]
dJsupR = dJsys[6 * supR:6 * supR + 6]
# calculate contact state
# if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08:
if g_initFlag == 1:
# contact state
# 0: flying 1: right only 2: left only 3: double
# if contact == 2 and refFootR[1] < doubleTosingleOffset:
if contact == 2 and contactR == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
# elif contact == 3 and refFootL[1] < doubleTosingleOffset:
elif contact == 1 and contactL == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
# elif contact == 3 and refFootR[1] > doubleTosingleOffset:
elif contact == 3 and contactR == 0:
contact = 2
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
# elif contact == 3 and refFootL[1] > doubleTosingleOffset:
elif contact == 3 and contactL == 0:
contact = 1
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
else:
contact = 0
# if refFootR[1] < doubleTosingleOffset:
if contactR == 1:
contact += 1
# if refFootL[1] < doubleTosingleOffset:
if contactL == 1:
contact += 2
# initialization
if g_initFlag == 0:
JsysPre = Jsys.copy()
JsupPreL = JsupL.copy()
JsupPreR = JsupR.copy()
JconstPre = Jconst.copy()
softConstPoint = footCenterR.copy()
# yjc.computeJacobian2(JsysPre, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks)
# yjc.computeJacobian2(JsupPreL, DOFs, jointPositions, jointAxeses, [footCenterL], supLJointMasks)
# yjc.computeJacobian2(JsupPreR, DOFs, jointPositions, jointAxeses, [footCenterR], supRJointMasks)
# yjc.computeJacobian2(JconstPre, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks)
footCenter = footCenterL + (footCenterR - footCenterL) / 2.0
footCenter[1] = 0.
preFootCenter = footCenter.copy()
# footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL)
# footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR)
if refFootR[1] < doubleTosingleOffset:
contact += 1
if refFootL[1] < doubleTosingleOffset:
contact += 2
g_initFlag = 1
# calculate footCenter
footCenter = footCenterL + (footCenterR - footCenterL) / 2.0
# if refFootR[1] >doubleTosingleOffset:
# if refFootR[1] > doubleTosingleOffset or footCenterR[1] > 0.08:
# if contact == 1 or footCenterR[1] > 0.08:
# if contact == 2 or footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
footCenter = footCenterL.copy()
# elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
footCenter = footCenterR.copy()
footCenter[1] = 0.
if contactChangeCount > 0 and contactChangeType == 'StoD':
# change footcenter gradually
footCenter = preFootCenter + (
maxContactChangeCount - contactChangeCount) * (
footCenter - preFootCenter) / maxContactChangeCount
preFootCenter = footCenter.copy()
# linear momentum
# TODO:
# We should consider dCM_ref, shouldn't we?
# add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel!
# todo that, set joint velocities to vpModel
CM_ref_plane = footCenter
dL_des_plane = Kl * totalMass * (CM_ref_plane -
CM_plane) - Dl * totalMass * dCM_plane
# dL_des_plane[1] = 0.
# angular momentum
CP_ref = footCenter
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
# bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0]) / (1 / 30.)
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 / 30.) + .5 * ddCP_des * ((1 / 30.)**2)
dH_des = np.cross(
(CP_des - CM),
(dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
if contactChangeCount > 0: # and contactChangeType == 'DtoS':
# dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount*10)
dH_des *= (maxContactChangeCount -
contactChangeCount) / (maxContactChangeCount)
# dH_des *= (contactChangeCount)/(maxContactChangeCount)*.9+.1
else:
dH_des = None
# H = np.dot(P, np.dot(Jsys, dth_flat))
# dH_des = -Kh* H[3:]
# soft point constraint
#softConstPoint = refFootR.copy()
##softConstPoint[0] += 0.2
#Ksc = 50
#Dsc = 2*(Ksc**.5)
#Bsc = 1.
#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_des - dP_cur)
#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)
#dJconst = (Jconst - Jconst)/(1/30.)
#JconstPre = Jconst.copy()
##yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False)
#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))
#q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2
#set up equality constraint
a_oriL = mm.logSO3(
mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0, 1, 0])),
np.array([0, 1, 0])))
a_oriR = mm.logSO3(
mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0, 1, 0])),
np.array([0, 1, 0])))
#if contact == 3 and contactChangeCount < maxContactChangeCount/4 and contactChangeCount >=1:
#kt_sup = 30
#viewer.objectInfoWnd.labelSupKt.value(kt_sup)
#viewer.objectInfoWnd.sliderSupKt.value(initSupKt*10)
# a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL))
# a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR))
a_supL = np.append(
kt_sup * (refFootL - footCenterL + contMotionOffset) -
dt_sup * footBodyVelL, kt_sup * a_oriL - dt_sup * footBodyAngVelL)
a_supR = np.append(
kt_sup * (refFootR - footCenterR + contMotionOffset) -
dt_sup * footBodyVelR, kt_sup * a_oriR - dt_sup * footBodyAngVelR)
if contactChangeCount > 0 and contactChangeType == 'DtoS':
#refFootR += (footCenter-CM_plane)/2.
#refFootR[1] = 0
#pre contact value are needed
#if contact == 2:
##refFootR[0] += 0.2
##refFootR[2] -= 0.05
#offsetDropR = (footCenter-CM_plane)/2.
#refFootR += offsetDropR
#refFootR[1] = 0.
##refFootR[2] = footCenterR[2] - contMotionOffset[2]
##refFootR[0] = footCenterR[0] - contMotionOffset[0]
#refFootL[0] += 0.05
#refFootL[2] -= 0.05
#elif contact == 1:
#offsetDropL = (footCenter-CM_plane)/2.
#refFootL += offsetDropL
#refFootL[1] = 0.
#a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL))
#a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR))
#a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL))
#a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR))
a_supL = np.append(
kt_sup * (refFootL - footCenterL + contMotionOffset) + dt_sup *
(refFootVelL - footBodyVelL), 4 * kt_sup * a_oriL +
2 * dt_sup * (refFootAngVelL - footBodyAngVelL))
a_supR = np.append(
kt_sup * (refFootR - footCenterR + contMotionOffset) + dt_sup *
(refFootVelR - footBodyVelR), 4 * kt_sup * a_oriR +
2 * dt_sup * (refFootAngVelR - footBodyAngVelR))
elif contactChangeCount > 0 and contactChangeType == 'StoD':
#refFootR[0] +=0.05
#refFootR[2] +=0.05
linkt = (13. * contactChangeCount) / (maxContactChangeCount) + 1.
lindt = 2 * (linkt**.5)
angkt = (13. * contactChangeCount) / (maxContactChangeCount) + 1.
angdt = 2 * (angkt**.5)
#a_supL = np.append(4*kt_sup*(refFootL - footCenterL + contMotionOffset) + 2*dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL))
#a_supR = np.append(4*kt_sup*(refFootR - footCenterR + contMotionOffset) + 2*dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR))
a_supL = np.append(
linkt * kt_sup * (refFootL - footCenterL + contMotionOffset) +
lindt * dt_sup * (refFootVelL - footBodyVelL),
angkt * kt_sup * a_oriL + angdt * dt_sup *
(refFootAngVelL - footBodyAngVelL))
a_supR = np.append(
linkt * kt_sup * (refFootR - footCenterR + contMotionOffset) +
lindt * dt_sup * (refFootVelR - footBodyVelR),
angkt * kt_sup * a_oriR + angdt * dt_sup *
(refFootAngVelR - footBodyAngVelR))
#a_supL = np.append(16*kt_sup*(refFootL - footCenterL + contMotionOffset) + 4*dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL))
#a_supR = np.append(16*kt_sup*(refFootR - footCenterR + contMotionOffset) + 4*dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR))
#a_supL = np.append(4*kt_sup*(refFootL - footCenterL + contMotionOffset) + 2*dt_sup*(refFootVelL - footBodyVelL), 32*kt_sup*a_oriL+5.6*dt_sup*(refFootAngVelL-footBodyAngVelL))
#a_supR = np.append(4*kt_sup*(refFootR - footCenterR + contMotionOffset) + 2*dt_sup*(refFootVelR - footBodyVelR), 32*kt_sup*a_oriR+5.6*dt_sup*(refFootAngVelR-footBodyAngVelR))
#a_supL[1] = kt_sup*(refFootL[1] - footCenterL[1] + contMotionOffset[1]) + dt_sup*(refFootVelL[1] - footBodyVelL[1])
#a_supR[1] = kt_sup*(refFootR[1] - footCenterR[1] + contMotionOffset[1]) + dt_sup*(refFootVelR[1] - footBodyVelR[1])
##if contact == 2:
#if refFootR[1] <doubleTosingleOffset :
#Jsup = np.vstack((JsupL, JsupR))
#dJsup = np.vstack((dJsupL, dJsupR))
#a_sup = np.append(a_supL, a_supR)
#else:
#Jsup = JsupL.copy()
#dJsup = dJsupL.copy()
#a_sup = a_supL.copy()
# 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)
rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsys)
r_bias, s_bias = np.hsplit(rs, 2)
#######################################################
# optimization
#######################################################
#if contact == 2 and footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
config['weightMap']['RightUpLeg'] = .8
config['weightMap']['RightLeg'] = .8
config['weightMap']['RightFoot'] = .8
else:
config['weightMap']['RightUpLeg'] = .1
config['weightMap']['RightLeg'] = .25
config['weightMap']['RightFoot'] = .2
#if contact == 1 and footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
config['weightMap']['LeftUpLeg'] = .8
config['weightMap']['LeftLeg'] = .8
config['weightMap']['LeftFoot'] = .8
else:
config['weightMap']['LeftUpLeg'] = .1
config['weightMap']['LeftLeg'] = .25
config['weightMap']['LeftFoot'] = .2
w = mot.getTrackingWeight(DOFs, motion[0].skeleton,
config['weightMap'])
#if contact == 2:
#mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
if dH_des is not None:
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
#if contact & 1 and contactChangeCount == 0:
if contact & 1:
#if refFootR[1] < doubleTosingleOffset:
mot.addConstraint2(problem, totalDOF, JsupR, dJsupR, dth_flat,
a_supR)
if contact & 2:
#if refFootL[1] < doubleTosingleOffset:
mot.addConstraint2(problem, totalDOF, JsupL, dJsupL, dth_flat,
a_supL)
if contactChangeCount > 0:
contactChangeCount -= 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
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)
# print(contactForces)
#bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
controlModel.setDOFAccelerations(ddth_sol)
controlModel.solveHybridDynamics()
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([
viewer.objectInfoWnd.labelForceX.value(),
viewer.objectInfoWnd.labelForceY.value(),
viewer.objectInfoWnd.labelForceZ.value()
])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
# extraForce[0] = viewer.objectInfoWnd.labelFm.value() * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += wcfg.timeStep
vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
vpWorld.step()
# rendering
rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0]))
rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0]))
rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0, .1]))
rightFootPos[0] = footCenterL
rightVectorX[0] = np.dot(footBodyOriL, np.array([.1, 0, 0]))
rightVectorY[0] = np.dot(footBodyOriL, np.array([0, .1, 0]))
rightVectorZ[0] = np.dot(footBodyOriL, np.array([0, 0, .1]))
rightPos[0] = footCenterL + np.array([.1, 0, 0])
rd_footCenter[0] = footCenter
rd_footCenterL[0] = footCenterL
rd_footCenterR[0] = footCenterR
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
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[0] / 100, dL_des_plane[1] / 100,
dL_des_plane[2] / 100
]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity)
rd_root_des[0] = rootPos[0]
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append(contactForces[i] / 400)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exf_des[0] = [0, 0, 0]
else:
rd_exf_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exfen_des[0] = [0, 0, 0]
extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody)
def simulateCallback(frame):
global COLOR_ON
# print(frame)
# print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0']))
if False:
if frame == 200:
if motionFile == 'wd2_tiptoe.bvh':
setParamVal('tiptoe angle', 0.3)
if motionFile == 'wd2_tiptoe_zygote.bvh':
setParamVal('tiptoe angle', 0.3)
# elif 210 < frame < 240:
# if motionFile == 'wd2_tiptoe_zygote.bvh':
# setParamVal('com Y offset', 0.01/30. * (frame-110))
elif frame == 400:
setParamVal('com Y offset', 0.)
setParamVal('tiptoe angle', 0.)
elif frame == 430:
foot_viewer.check_all_seg()
# setParamVal('SupKt', 30.)
# elif frame == 400:
# setParamVal('SupKt', 17.)
# hfi.footAdjust(motion[frame], idDic, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=.015, baseHeight=0.02)
if abs(getParamVal('tiptoe angle')) > 0.001:
tiptoe_angle = getParamVal('tiptoe angle')
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_0_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_0_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
# motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95))
# motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle))
if getParamVal('left tilt angle') > 0.001:
left_tilt_angle = getParamVal('left tilt angle')
if motion[0].skeleton.getJointIndex(
'LeftFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
else:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1_0'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_1_0'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot'], mm.exp(mm.unitZ(),
math.pi * left_tilt_angle))
elif getParamVal('left tilt angle') < -0.001:
left_tilt_angle = getParamVal('left tilt angle')
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_0'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
if motion[0].skeleton.getJointIndex(
'LeftFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1'],
mm.exp(mm.unitZ(), math.pi * left_tilt_angle))
# else:
# motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_1_0'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot'], mm.exp(mm.unitZ(),
math.pi * left_tilt_angle))
if getParamVal('right tilt angle') > 0.001:
right_tilt_angle = getParamVal('right tilt angle')
if motion[0].skeleton.getJointIndex(
'RightFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
else:
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1_0'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_1_0'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot'],
mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
elif getParamVal('right tilt angle') < -0.001:
right_tilt_angle = getParamVal('right tilt angle')
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_0'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
if motion[0].skeleton.getJointIndex(
'RightFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1'],
mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
# else:
# motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_1_0'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot'],
mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
motionModel.update(motion[frame])
motionModel.translateByOffset(
np.array([
getParamVal('com X offset'),
getParamVal('com Y offset'),
getParamVal('com Z offset')
]))
controlModel.update(motion[frame])
# controlModel_ik.set_q(controlModel.get_q())
# controlModel_ik.set_q(controlModel.get_q())
controlModel_ik.update(motion[frame])
controlModel_ik.translateByOffset(
np.array([
-getParamVal('com X offset') * 2.,
getParamVal('com Y offset'),
getParamVal('com Z offset')
]))
global g_initFlag
global forceShowTime
global JsysPre
global JsupPreL
global JsupPreR
global JconstPre
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2 * (Kt**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
dt_sup = 2 * (kt_sup**.5)
# 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)
# ype.flatten(fix_dofs(DOFs, ddth_des, mcfg, joint_names), ddth_des_flat)
# ype.flatten(fix_dofs(DOFs, dth, mcfg, joint_names), dth_flat)
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
#################################################
# jacobian
#################################################
contact_des_ids = list() # desired contact segments
if foot_viewer.check_om_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0'))
if foot_viewer.check_op_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0'))
if foot_viewer.check_im_l is not None and foot_viewer.check_im_l.value(
):
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1'))
if foot_viewer.check_ip_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0'))
if foot_viewer.check_h_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0'))
if foot_viewer.check_om_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_0'))
if foot_viewer.check_op_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0'))
if foot_viewer.check_im_r is not None and foot_viewer.check_im_r.value(
):
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_1'))
if foot_viewer.check_ip_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0'))
if foot_viewer.check_h_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_1_0'))
contact_ids = list() # temp idx for balancing
contact_ids.extend(contact_des_ids)
contact_joint_ori = list(
map(controlModel.getJointOrientationGlobal, contact_ids))
contact_joint_pos = list(
map(controlModel.getJointPositionGlobal, contact_ids))
contact_body_ori = list(
map(controlModel.getBodyOrientationGlobal, contact_ids))
contact_body_pos = list(
map(controlModel.getBodyPositionGlobal, contact_ids))
contact_body_vel = list(
map(controlModel.getBodyVelocityGlobal, contact_ids))
contact_body_angvel = list(
map(controlModel.getBodyAngVelocityGlobal, contact_ids))
ref_joint_ori = list(
map(motion[frame].getJointOrientationGlobal, contact_ids))
ref_joint_pos = list(
map(motion[frame].getJointPositionGlobal, contact_ids))
ref_joint_vel = [
motion.getJointVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_joint_angvel = [
motion.getJointAngVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_body_ori = list(
map(motionModel.getBodyOrientationGlobal, contact_ids))
ref_body_pos = list(map(motionModel.getBodyPositionGlobal,
contact_ids))
# ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids))
ref_body_angvel = [
motion.getJointAngVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_body_vel = [
ref_joint_vel[i] +
np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i])
for i in range(len(ref_joint_vel))
]
is_contact = [1] * len(contact_ids)
contact_right = len(set(contact_des_ids).intersection(rIDlist)) > 0
contact_left = len(set(contact_des_ids).intersection(lIDlist)) > 0
contMotionOffset = th[0][0] - th_r[0][0]
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
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.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
if foot_viewer is not None:
foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info(
frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds)
foot_viewer.foot_pressure_gl_window.goToFrame(frame)
# rendering
for foot_seg_id in footIdlist:
motion_model_renderer.body_colors[foot_seg_id] = (255, 240, 255)
for contact_id in contact_ids:
motion_model_renderer.body_colors[contact_id] = (255, 0, 0)
pallete2 = list()
pallete2.append((244, 198, 61))
pallete2.append((4, 105, 113))
pallete2.append((234, 219, 196))
pallete2.append((216, 1, 6))
pallete2.append((230, 230, 230))
pallete = pallete2
color = dict()
color['RightFoot'] = pallete[0]
color['RightFoot_foot_1_0'] = pallete[4]
color['RightFoot_foot_0_0'] = pallete[1]
color['RightFoot_foot_0_0_0'] = pallete[2]
color['RightFoot_foot_0_1_0'] = pallete[3]
color['LeftFoot'] = pallete[0]
color['LeftFoot_foot_1_0'] = pallete[4]
color['LeftFoot_foot_0_0'] = pallete[1]
color['LeftFoot_foot_0_0_0'] = pallete[2]
color['LeftFoot_foot_0_1_0'] = pallete[3]
if COLOR_ON:
for color_key in color.keys():
motion_model_renderer.body_colors[
idDic[color_key]] = color[color_key]
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
rd_CM_plane[0][1] = 0.
del rd_foot_ori[:]
del rd_foot_pos[:]
# for seg_foot_id in footIdlist:
# rd_foot_ori.append(controlModel.getJointOrientationGlobal(seg_foot_id))
# rd_foot_pos.append(controlModel.getJointPositionGlobal(seg_foot_id))
rd_foot_ori.append(controlModel.getJointOrientationGlobal(supL))
rd_foot_ori.append(controlModel.getJointOrientationGlobal(supR))
rd_foot_pos.append(controlModel.getJointPositionGlobal(supL))
rd_foot_pos.append(controlModel.getJointPositionGlobal(supR))
rd_root_des[0] = rootPos[0]
rd_root_ori[0] = controlModel.getBodyOrientationGlobal(0)
rd_root_pos[0] = controlModel.getBodyPositionGlobal(0)
del rd_CF[:]
del rd_CF_pos[:]
# render contact_ids
# render skeleton
if SKELETON_ON:
Ts = dict()
Ts['pelvis'] = controlModel_ik.getJointTransform(idDic['Hips'])
Ts['thigh_R'] = controlModel_ik.getJointTransform(
idDic['RightUpLeg'])
Ts['shin_R'] = controlModel_ik.getJointTransform(idDic['RightLeg'])
Ts['foot_R'] = controlModel_ik.getJointTransform(
idDic['RightFoot'])
Ts['foot_heel_R'] = controlModel_ik.getJointTransform(
idDic['RightFoot'])
Ts['heel_R'] = np.eye(4)
Ts['outside_metatarsal_R'] = controlModel_ik.getJointTransform(
idDic['RightFoot_foot_0_0'])
Ts['outside_phalanges_R'] = controlModel_ik.getJointTransform(
idDic['RightFoot_foot_0_0_0'])
# Ts['inside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1'])
Ts['inside_metatarsal_R'] = np.eye(4)
Ts['inside_phalanges_R'] = controlModel_ik.getJointTransform(
idDic['RightFoot_foot_0_1_0'])
Ts['spine_ribs'] = controlModel_ik.getJointTransform(
idDic['Spine'])
Ts['head'] = controlModel_ik.getJointTransform(idDic['Spine1'])
Ts['upper_limb_R'] = controlModel_ik.getJointTransform(
idDic['RightArm'])
Ts['lower_limb_R'] = controlModel_ik.getJointTransform(
idDic['RightForeArm'])
Ts['thigh_L'] = controlModel_ik.getJointTransform(
idDic['LeftUpLeg'])
Ts['shin_L'] = controlModel_ik.getJointTransform(idDic['LeftLeg'])
Ts['foot_L'] = controlModel_ik.getJointTransform(idDic['LeftFoot'])
Ts['foot_heel_L'] = controlModel_ik.getJointTransform(
idDic['LeftFoot'])
Ts['heel_L'] = np.eye(4)
Ts['outside_metatarsal_L'] = controlModel_ik.getJointTransform(
idDic['LeftFoot_foot_0_0'])
Ts['outside_phalanges_L'] = controlModel_ik.getJointTransform(
idDic['LeftFoot_foot_0_0_0'])
# Ts['inside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1'])
Ts['inside_metatarsal_L'] = np.eye(4)
Ts['inside_phalanges_L'] = controlModel_ik.getJointTransform(
idDic['LeftFoot_foot_0_1_0'])
Ts['upper_limb_L'] = controlModel_ik.getJointTransform(
idDic['LeftArm'])
Ts['lower_limb_L'] = controlModel_ik.getJointTransform(
idDic['LeftForeArm'])
color = dict()
color['foot_R'] = pallete[0]
color['heel_R'] = pallete[4]
color['outside_metatarsal_R'] = pallete[1]
color['outside_phalanges_R'] = pallete[2]
color['inside_phalanges_R'] = pallete[3]
color['foot_L'] = pallete[0]
color['heel_L'] = pallete[4]
color['outside_metatarsal_L'] = pallete[1]
color['outside_phalanges_L'] = pallete[2]
color['inside_phalanges_L'] = pallete[3]
if COLOR_ON:
skeleton_renderer.appendFrameState(Ts, color)
else:
skeleton_renderer.appendFrameState(Ts)
def simulateCallback(frame):
# print(frame)
# print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0']))
if False and viewer_GetForceState():
# print('force on, frame: ', frame)
motion[frame].mulJointOrientationLocal(
footIdDic['LeftFoot_foot_0_0_0'],
mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6))
motion[frame].mulJointOrientationLocal(
footIdDic['LeftFoot_foot_0_1_0'],
mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6))
motion[frame].mulJointOrientationLocal(
footIdDic['RightFoot_foot_0_0_0'],
mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6))
motion[frame].mulJointOrientationLocal(
footIdDic['RightFoot_foot_0_1_0'],
mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6))
# print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0']))
motionModel.update(motion[frame])
controlModel_ik.set_q(controlModel.get_q())
global g_initFlag
global forceShowTime
global JsysPre
global JsupPreL
global JsupPreR
global JconstPre
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
# Kt, Kl, Kh, Bl, Bh, kt_sup = viewer.GetParam()
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2 * (Kt**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
dt_sup = 2 * (kt_sup**.5)
doubleTosingleOffset = 0.15
singleTodoubleOffset = 0.30
# doubleTosingleOffset = 0.09
doubleTosingleVelOffset = 0.0
# 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)
ype.flatten(ddth_des, ddth_des_flat)
# ddth_des_flat = Kt * (motion.get_q(frame) - np.array(controlModel.get_q())) - Dt * np.array(controlModel.get_dq())
ype.flatten(dth, dth_flat)
# dth_flat = np.array(controlModel.get_dq())
#################################################
# jacobian
#################################################
contact_des_ids = list() # desired contact segments
if foot_viewer.check_om_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0'))
if foot_viewer.check_op_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0'))
if foot_viewer.check_im_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1'))
if foot_viewer.check_ip_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0'))
if foot_viewer.check_h_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0'))
if foot_viewer.check_om_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_0'))
if foot_viewer.check_op_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0'))
if foot_viewer.check_im_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_1'))
if foot_viewer.check_ip_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0'))
if foot_viewer.check_h_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_1_0'))
contact_ids = list() # temp idx for balancing
contact_ids.extend(contact_des_ids)
contact_joint_ori = list(
map(controlModel.getJointOrientationGlobal, contact_ids))
contact_joint_pos = list(
map(controlModel.getJointPositionGlobal, contact_ids))
contact_body_ori = list(
map(controlModel.getBodyOrientationGlobal, contact_ids))
contact_body_pos = list(
map(controlModel.getBodyPositionGlobal, contact_ids))
contact_body_vel = list(
map(controlModel.getBodyVelocityGlobal, contact_ids))
contact_body_angvel = list(
map(controlModel.getBodyAngVelocityGlobal, contact_ids))
ref_joint_ori = list(
map(motion[frame].getJointOrientationGlobal, contact_ids))
ref_joint_pos = list(
map(motion[frame].getJointPositionGlobal, contact_ids))
ref_joint_vel = [
motion.getJointVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_joint_angvel = [
motion.getJointAngVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_body_ori = list(
map(motionModel.getBodyOrientationGlobal, contact_ids))
ref_body_pos = list(map(motionModel.getBodyPositionGlobal,
contact_ids))
# ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids))
ref_body_angvel = [
motion.getJointAngVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_body_vel = [
ref_joint_vel[i] +
np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i])
for i in range(len(ref_joint_vel))
]
J_contacts = [
yjc.makeEmptyJacobian(DOFs, 1) for i in range(len(contact_ids))
]
dJ_contacts = [
yjc.makeEmptyJacobian(DOFs, 1) for i in range(len(contact_ids))
]
joint_masks = [
yjc.getLinkJointMask(motion[0].skeleton, joint_idx)
for joint_idx in contact_ids
]
# caution!! body orientation and joint orientation of foot are totally different!!
footOriL = controlModel.getJointOrientationGlobal(supL)
footOriR = controlModel.getJointOrientationGlobal(supR)
# desire footCenter[1] = 0.041135
# desire footCenter[1] = 0.0197
footCenterL = controlModel.getBodyPositionGlobal(supL)
footCenterR = controlModel.getBodyPositionGlobal(supR)
footBodyOriL = controlModel.getBodyOrientationGlobal(supL)
footBodyOriR = controlModel.getBodyOrientationGlobal(supR)
footBodyVelL = controlModel.getBodyVelocityGlobal(supL)
footBodyVelR = controlModel.getBodyVelocityGlobal(supR)
footBodyAngVelL = controlModel.getBodyAngVelocityGlobal(supL)
footBodyAngVelR = controlModel.getBodyAngVelocityGlobal(supR)
refFootL = motionModel.getBodyPositionGlobal(supL)
refFootR = motionModel.getBodyPositionGlobal(supR)
refFootVelL = motionModel.getBodyVelocityGlobal(supL)
refFootVelR = motionModel.getBodyVelocityGlobal(supR)
refFootAngVelL = motionModel.getBodyAngVelocityGlobal(supL)
refFootAngVelR = motionModel.getBodyAngVelocityGlobal(supR)
refFootJointVelR = motion.getJointVelocityGlobal(supR, frame)
refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootJointR = motion.getJointPositionGlobal(supR, frame)
refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR,
(refFootR - refFootJointR))
refFootJointVelL = motion.getJointVelocityGlobal(supL, frame)
refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
refFootJointL = motion.getJointPositionGlobal(supL, frame)
refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL,
(refFootL - refFootJointL))
is_contact = [1] * len(contact_ids)
contactR = 1
contactL = 1
if refFootVelR[1] < 0 and refFootVelR[1] / 30. + refFootR[
1] > singleTodoubleOffset:
contactR = 0
if refFootVelL[1] < 0 and refFootVelL[1] / 30. + refFootL[
1] > singleTodoubleOffset:
contactL = 0
if refFootVelR[1] > 0 and refFootVelR[1] / 30. + refFootR[
1] > doubleTosingleOffset:
contactR = 0
if refFootVelL[1] > 0 and refFootVelL[1] / 30. + refFootL[
1] > doubleTosingleOffset:
contactL = 0
# contactR = 1
contMotionOffset = th[0][0] - th_r[0][0]
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
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.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
# calculate contact state
# if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08:
if g_initFlag == 1:
# contact state
# 0: flying 1: right only 2: left only 3: double
# if contact == 2 and refFootR[1] < doubleTosingleOffset:
if contact == 2 and contactR == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
# elif contact == 3 and refFootL[1] < doubleTosingleOffset:
elif contact == 1 and contactL == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
# elif contact == 3 and refFootR[1] > doubleTosingleOffset:
elif contact == 3 and contactR == 0:
contact = 2
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
# elif contact == 3 and refFootL[1] > doubleTosingleOffset:
elif contact == 3 and contactL == 0:
contact = 1
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
else:
contact = 0
# if refFootR[1] < doubleTosingleOffset:
if contactR == 1:
contact += 1
# if refFootL[1] < doubleTosingleOffset:
if contactL == 1:
contact += 2
# initialization
if g_initFlag == 0:
# JsysPre = Jsys.copy()
JconstPre = Jconst.copy()
softConstPoint = footCenterR.copy()
# yjc.computeJacobian2(JsysPre, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks)
# yjc.computeJacobian2(JconstPre, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks)
footCenter = footCenterL + (footCenterR - footCenterL) / 2.0
footCenter[1] = 0.
preFootCenter = footCenter.copy()
# footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL)
# footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR)
if refFootR[1] < doubleTosingleOffset:
contact += 1
if refFootL[1] < doubleTosingleOffset:
contact += 2
g_initFlag = 1
# calculate jacobian
Jsys, dJsys = controlModel.computeCom_J_dJdq()
for i in range(len(J_contacts)):
J_contacts[i] = Jsys[6 * contact_ids[i]:6 * contact_ids[i] + 6, :]
dJ_contacts[i] = dJsys[6 * contact_ids[i]:6 * contact_ids[i] + 6]
# calculate footCenter
footCenter = sum(contact_body_pos) / len(contact_body_pos) if len(contact_body_pos) > 0 \
else .5 * (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(supR))
# if len(contact_body_pos) > 2:
# hull = ConvexHull(contact_body_pos)
footCenter_ref = sum(ref_body_pos) / len(ref_body_pos) if len(ref_body_pos) > 0 \
else .5 * (motionModel.getBodyPositionGlobal(supL) + motionModel.getBodyPositionGlobal(supR))
footCenter_ref = footCenter_ref + contMotionOffset
# if len(ref_body_pos) > 2:
# hull = ConvexHull(ref_body_pos)
footCenter_ref[1] = 0.
# footCenter = footCenterL + (footCenterR - footCenterL)/2.0
# if refFootR[1] >doubleTosingleOffset:
# if refFootR[1] > doubleTosingleOffset or footCenterR[1] > 0.08:
# if contact == 1 or footCenterR[1] > 0.08:
# if contact == 2 or footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
footCenter = footCenterL.copy()
# elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
footCenter = footCenterR.copy()
footCenter[1] = 0.
footCenter[0] = footCenter[0] + getParamVal('com X offset')
if contactChangeCount > 0 and contactChangeType == 'StoD':
# change footcenter gradually
footCenter = preFootCenter + (
maxContactChangeCount - contactChangeCount) * (
footCenter - preFootCenter) / maxContactChangeCount
preFootCenter = footCenter.copy()
# linear momentum
# TODO:
# We should consider dCM_ref, shouldn't we?
# add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel!
# to do that, set joint velocities to vpModel
CM_ref_plane = footCenter
# CM_ref_plane = footCenter_ref
dL_des_plane = Kl * totalMass * (CM_ref_plane -
CM_plane) - Dl * totalMass * dCM_plane
# dL_des_plane[1] = 0.
# print('dCM_plane : ', np.linalg.norm(dCM_plane))
# angular momentum
CP_ref = footCenter
# CP_ref = footCenter_ref
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
# bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0]) / (1 / 30.)
CP_old[0] = CP
if CP is not None and dCP is not None:
ddCP_des = Kh * (CP_ref - CP) - Dh * dCP
dCP_des = dCP + ddCP_des * (1 / 30.)
CP_des = CP + dCP_des * (1 / 30.)
# CP_des = footCenter
CP_des = CP + dCP * (1 / 30.) + .5 * ddCP_des * ((1 / 30.)**2)
dH_des = np.cross(
(CP_des - CM),
(dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
if contactChangeCount > 0: # and contactChangeType == 'DtoS':
dH_des *= (maxContactChangeCount -
contactChangeCount) / maxContactChangeCount
else:
dH_des = None
# convex hull
contact_pos_2d = np.asarray([
np.array([contactPosition[0], contactPosition[2]])
for contactPosition in contactPositions
])
p = np.array([CM_plane[0], CM_plane[2]])
# hull = None # type: Delaunay
# if contact_pos_2d.shape[0] > 0:
# hull = Delaunay(contact_pos_2d)
# print(hull.find_simplex(p) >= 0)
# set up equality constraint
# TODO:
# logSO3 is just q'', not acceleration.
# To make a_oris acceleration, q'' -> a will be needed
# body_ddqs = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori]))
body_ddqs = list(
map(mm.logSO3, [
np.dot(
contact_body_ori[i].T,
np.dot(
ref_body_ori[i],
mm.getSO3FromVectors(
np.dot(ref_body_ori[i], mm.unitY()), mm.unitY())))
for i in range(len(contact_body_ori))
]))
body_qs = list(map(mm.logSO3, contact_body_ori))
body_angs = [
np.dot(contact_body_ori[i], contact_body_angvel[i])
for i in range(len(contact_body_ori))
]
body_dqs = [
mm.vel2qd(body_angs[i], body_qs[i]) for i in range(len(body_angs))
]
a_oris = [
np.dot(contact_body_ori[i],
mm.qdd2accel(body_ddqs[i], body_dqs[i], body_qs[i]))
for i in range(len(contact_body_ori))
]
# body_ddq = body_ddqs[0]
# body_ori = contact_body_ori[0]
# body_ang = np.dot(body_ori.T, contact_body_angvel[0])
#
# body_q = mm.logSO3(body_ori)
# body_dq = mm.vel2qd(body_ang, body_q)
# a_ori = np.dot(body_ori, mm.qdd2accel(body_ddq, body_dq, body_q))
# a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori]))
a_sups = [
np.append(
kt_sup *
(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) +
dt_sup * (ref_body_vel[i] - contact_body_vel[i]),
kt_sup * a_oris[i] + dt_sup *
(ref_body_angvel[i] - contact_body_angvel[i]))
for i in range(len(a_oris))
]
# 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)
rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsys)
r_bias, s_bias = np.hsplit(rs, 2)
#######################################################
# optimization
#######################################################
# if contact == 2 and footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
config['weightMap']['RightUpLeg'] = .8
config['weightMap']['RightLeg'] = .8
config['weightMap']['RightFoot'] = .8
else:
config['weightMap']['RightUpLeg'] = .1
config['weightMap']['RightLeg'] = .25
config['weightMap']['RightFoot'] = .2
# if contact == 1 and footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
config['weightMap']['LeftUpLeg'] = .8
config['weightMap']['LeftLeg'] = .8
config['weightMap']['LeftFoot'] = .8
else:
config['weightMap']['LeftUpLeg'] = .1
config['weightMap']['LeftLeg'] = .25
config['weightMap']['LeftFoot'] = .2
w = mot.getTrackingWeight(DOFs, motion[0].skeleton,
config['weightMap'])
# if contact == 2:
# mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
if dH_des is not None:
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, dJsup, dth_flat, a_sup)
# mot.addConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup)
# if contact & 1 and contactChangeCount == 0:
if True:
for c_idx in range(len(contact_ids)):
# mot.addConstraint(problem, totalDOF, J_contacts[c_idx], dJ_contacts[c_idx], dth_flat, a_sups[c_idx])
mot.addConstraint2(problem, totalDOF, J_contacts[c_idx],
dJ_contacts[c_idx], dth_flat,
a_sups[c_idx])
if contactChangeCount > 0:
contactChangeCount = contactChangeCount - 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
r = problem.solve()
problem.clear()
ddth_sol_flat = np.asarray(r['x'])
# ddth_sol_flat[foot_seg_dofs] = np.array(ddth_des_flat)[foot_seg_dofs]
ype.nested(ddth_sol_flat, 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)
# bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
controlModel.setDOFAccelerations(ddth_sol)
# controlModel.setDOFAccelerations(ddth_des)
# controlModel.set_ddq(ddth_sol_flat)
# controlModel.set_ddq(ddth_des_flat)
controlModel.solveHybridDynamics()
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([
viewer.objectInfoWnd.labelForceX.value(),
viewer.objectInfoWnd.labelForceY.value(),
viewer.objectInfoWnd.labelForceZ.value()
])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += wcfg.timeStep
vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
vpWorld.step()
controlModel_ik.set_q(controlModel.get_q())
if foot_viewer is not None:
foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info(
frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds)
foot_viewer.foot_pressure_gl_window.goToFrame(frame)
# rendering
for foot_seg_id in footIdlist:
control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255)
for contact_id in contact_ids:
control_model_renderer.body_colors[contact_id] = (255, 0, 0)
rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0]))
rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0]))
rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0, .1]))
rightFootPos[0] = footCenterL
rightVectorX[0] = np.dot(footBodyOriL, np.array([.1, 0, 0]))
rightVectorY[0] = np.dot(footBodyOriL, np.array([0, .1, 0]))
rightVectorZ[0] = np.dot(footBodyOriL, np.array([0, 0, .1]))
rightPos[0] = footCenterL + np.array([.1, 0, 0])
rd_footCenter[0] = footCenter
rd_footCenter_ref[0] = footCenter_ref
rd_footCenterL[0] = footCenterL
rd_footCenterR[0] = footCenterR
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
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[0] / 100, dL_des_plane[1] / 100,
dL_des_plane[2] / 100
]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity)
rd_root_des[0] = rootPos[0]
rd_root_ori[0] = controlModel.getBodyOrientationGlobal(0)
rd_root_pos[0] = controlModel.getBodyPositionGlobal(0)
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append(contactForces[i] / 400)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exf_des[0] = [0, 0, 0]
else:
rd_exf_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exfen_des[0] = [0, 0, 0]
extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody)
# render contact_ids
# render skeleton
if SKELETON_ON:
Ts = dict()
Ts['pelvis'] = controlModel.getJointTransform(0)
Ts['thigh_R'] = controlModel.getJointTransform(1)
Ts['shin_R'] = controlModel.getJointTransform(2)
Ts['foot_R'] = controlModel.getJointTransform(3)
Ts['spine_ribs'] = controlModel.getJointTransform(9)
Ts['head'] = controlModel.getJointTransform(10)
Ts['upper_limb_R'] = controlModel.getJointTransform(13)
Ts['lower_limb_R'] = controlModel.getJointTransform(14)
Ts['thigh_L'] = controlModel.getJointTransform(15)
Ts['shin_L'] = controlModel.getJointTransform(16)
Ts['foot_L'] = controlModel.getJointTransform(17)
Ts['upper_limb_L'] = controlModel.getJointTransform(11)
Ts['lower_limb_L'] = controlModel.getJointTransform(12)
skeleton_renderer.appendFrameState(Ts)
def simulateCallback(frame):
# print(frame)
# print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0']))
# hfi.footAdjust(motion[frame], idDic, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=.015, baseHeight=0.02)
# motionModel.update(motion[frame])
motionModel.translateByOffset(
np.array([
getParamVal('com X offset'),
getParamVal('com Y offset'),
getParamVal('com Z offset')
]))
# controlModel_ik.set_q(controlModel.get_q())
global g_initFlag
global forceShowTime
global JsysPre
global JsupPreL
global JsupPreR
global JconstPre
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2 * (Kt**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
dt_sup = 2 * (kt_sup**.5)
# tracking
# # th_r = motion.getDOFPositions(frame)
# th_r = motionModel.getDOFPositions()
# th = controlModel.getDOFPositions()
# # dth_r = motion.getDOFVelocities(frame)
# dth = controlModel.getDOFVelocities()
# # ddth_r = motion.getDOFAccelerations(frame)
# ddth_des = yct.getDesiredDOFAccelerations(th_r, th, None, dth, None, Kt, Dt)
#
# # ype.flatten(fix_dofs(DOFs, ddth_des, mcfg, joint_names), ddth_des_flat)
# # ype.flatten(fix_dofs(DOFs, dth, mcfg, joint_names), dth_flat)
# # print(ddth_des)
# ype.flatten(ddth_des, ddth_des_flat)
# ype.flatten(dth, dth_flat)
th_r_flat = motionModel.get_q()
th_flat = controlModel.get_q()
dth_flat = controlModel.get_dq()
joint_dof_info = controlModel.getJointDOFInfo()
ddth_des_flat = yct.getDesiredDOFAccelerations_flat(
th_r_flat, th_flat, None, dth_flat, None, Kt, Dt, joint_dof_info)
# print(controlModel.getCoriAndGrav())
#################################################
# jacobian
#################################################
contact_des_ids = list() # desired contact segments
contact_des_ids.append(supL)
# if foot_viewer.check_h_l.value():
# contact_des_ids.append(motion[0].skeleton.getJointIndex('LeftFoot'))
#
# if foot_viewer.check_h_r.value():
# contact_des_ids.append(motion[0].skeleton.getJointIndex('RightFoot'))
contact_ids = list() # temp idx for balancing
contact_ids.extend(contact_des_ids)
contact_joint_ori = list(
map(controlModel.getJointOrientationGlobal, contact_ids))
contact_joint_pos = list(
map(controlModel.getJointPositionGlobal, contact_ids))
contact_body_ori = list(
map(controlModel.getBodyOrientationGlobal, contact_ids))
contact_body_pos = list(
map(controlModel.getBodyPositionGlobal, contact_ids))
contact_body_vel = list(
map(controlModel.getBodyVelocityGlobal, contact_ids))
contact_body_angvel = list(
map(controlModel.getBodyAngVelocityGlobal, contact_ids))
ref_joint_ori = list(
map(motionModel.getJointOrientationGlobal, contact_ids))
ref_joint_pos = list(
map(motionModel.getJointPositionGlobal, contact_ids))
ref_joint_vel = [
motionModel.getJointVelocityGlobal(joint_idx)
for joint_idx in contact_ids
]
ref_joint_angvel = [
motionModel.getJointAngVelocityGlobal(joint_idx)
for joint_idx in contact_ids
]
ref_body_ori = list(
map(motionModel.getBodyOrientationGlobal, contact_ids))
ref_body_pos = list(map(motionModel.getBodyPositionGlobal,
contact_ids))
# ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids))
ref_body_angvel = [
motionModel.getJointAngVelocityGlobal(joint_idx)
for joint_idx in contact_ids
]
ref_body_vel = [
ref_joint_vel[i] +
np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i])
for i in range(len(ref_joint_vel))
]
is_contact = [1] * len(contact_ids)
contact_right = len(set(contact_des_ids).intersection(rIDlist)) > 0
contact_left = len(set(contact_des_ids).intersection(lIDlist)) > 0
# contMotionOffset = th_flat[0:3] - th_r_flat[0:3]
contMotionOffset = np.array((1.5, 0., 0.))
linkPositions = [
controlModel.getBodyComPositionGlobal(i)
for i in range(controlModel.getBodyNum())
]
linkVelocities = [
controlModel.getBodyComVelocityGlobal(i)
for i in range(controlModel.getBodyNum())
]
linkAngVelocities = [
controlModel.getBodyAngVelocityGlobal(i)
for i in range(controlModel.getBodyNum())
]
linkInertias = [
controlModel.getBodyInertiaGlobal(i)
for i in range(controlModel.getBodyNum())
]
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.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
# calculate jacobian
Jsys, dJsys = controlModel.computeCom_J_dJdq()
J_contacts = [] # type: list[np.ndarray]
dJ_contacts = [] # type: list[np.ndarray]
for contact_id in contact_ids:
J_contacts.append(Jsys[6 * contact_id:6 * contact_id + 6, :])
dJ_contacts.append(dJsys[6 * contact_id:6 * contact_id + 6])
# calculate footCenter
footCenter = sum(contact_body_pos) / len(contact_body_pos) if len(contact_body_pos) > 0 \
else .5 * (controlModel.getBodyComPositionGlobal(supL) + controlModel.getBodyComPositionGlobal(supR))
footCenter[1] = 0.
# if len(contact_body_pos) > 2:
# hull = ConvexHull(contact_body_pos)
footCenter_ref = sum(ref_body_pos) / len(ref_body_pos) if len(ref_body_pos) > 0 \
else .5 * (motionModel.getBodyComPositionGlobal(supL) + motionModel.getBodyComPositionGlobal(supR))
footCenter_ref = footCenter_ref + contMotionOffset
# if len(ref_body_pos) > 2:
# hull = ConvexHull(ref_body_pos)
footCenter_ref[1] = 0.
# footCenter[0] = footCenter[0] + getParamVal('com X offset')
# footCenter[1] = footCenter[0] + getParamVal('com Y offset')
# footCenter[2] = footCenter[2] + getParamVal('com Z offset')
# initialization
if g_initFlag == 0:
preFootCenter[0] = footCenter.copy()
g_initFlag = 1
# if contactChangeCount == 0 and np.linalg.norm(footCenter - preFootCenter[0]) > 0.01:
# contactChangeCount += 30
if contactChangeCount > 0:
# change footcenter gradually
footCenter = preFootCenter[0] + (
maxContactChangeCount - contactChangeCount) * (
footCenter - preFootCenter[0]) / maxContactChangeCount
else:
preFootCenter[0] = footCenter.copy()
# linear momentum
# TODO:
# We should consider dCM_ref, shouldn't we?
# add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel!
# to do that, set joint velocities to vpModel
CM_ref_plane = footCenter
# CM_ref_plane = footCenter_ref
CM_ref = footCenter + np.array([
getParamVal('com X offset'),
motionModel.getCOM()[1] + getParamVal('com Y offset'),
getParamVal('com Z offset')
])
dL_des_plane = Kl * totalMass * (CM_ref - CM) - Dl * totalMass * dCM
# dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane
# dL_des_plane[1] = 0.
# print('dCM_plane : ', np.linalg.norm(dCM_plane))
# angular momentum
CP_ref = footCenter
# CP_ref = footCenter_ref
bodyIDs, contactPositions, contactPositionLocals, contactForces = controlModel.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0]) / (1 / 30.)
CP_old[0] = CP
if CP is not None and dCP is not None:
ddCP_des = Kh * (CP_ref - CP) - Dh * dCP
dCP_des = dCP + ddCP_des * (1 / 30.)
CP_des = CP + dCP_des * (1 / 30.)
# CP_des = footCenter
CP_des = CP + dCP * (1 / 30.) + .5 * ddCP_des * ((1 / 30.)**2)
dH_des = np.cross(
(CP_des - CM),
(dL_des_plane + totalMass * controlModel.getGravity()))
if contactChangeCount > 0: # and contactChangeType == 'DtoS':
dH_des *= (maxContactChangeCount -
contactChangeCount) / maxContactChangeCount
else:
dH_des = None
# set up equality constraint
# TODO:
# logSO3 is just q'', not acceleration.
# To make a_oris acceleration, q'' -> a will be needed
# body_ddqs = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori]))
# body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], mm.unitY()), mm.unitY()))) for i in range(len(contact_body_ori))]))
# body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()))) for i in range(len(contact_body_ori))]))
a_oris = list(
map(mm.logSO3, [
np.dot(
contact_body_ori[i].T,
np.dot(
ref_body_ori[i],
mm.getSO3FromVectors(
np.dot(ref_body_ori[i],
up_vec_in_each_link[contact_ids[i]]),
mm.unitY()))) for i in range(len(contact_body_ori))
]))
a_oris = list(
map(mm.logSO3, [
np.dot(
np.dot(
ref_body_ori[i],
mm.getSO3FromVectors(
np.dot(ref_body_ori[i],
up_vec_in_each_link[contact_ids[i]]),
mm.unitY())), contact_body_ori[i].T)
for i in range(len(contact_body_ori))
]))
body_qs = list(map(mm.logSO3, contact_body_ori))
body_angs = [
np.dot(contact_body_ori[i], contact_body_angvel[i])
for i in range(len(contact_body_ori))
]
body_dqs = [
mm.vel2qd(body_angs[i], body_qs[i]) for i in range(len(body_angs))
]
# a_oris = [np.dot(contact_body_ori[i], mm.qdd2accel(body_ddqs[i], body_dqs[i], body_qs[i])) for i in range(len(contact_body_ori))]
# body_ddq = body_ddqs[0]
# body_ori = contact_body_ori[0]
# body_ang = np.dot(body_ori.T, contact_body_angvel[0])
#
# body_q = mm.logSO3(body_ori)
# body_dq = mm.vel2qd(body_ang, body_q)
# a_ori = np.dot(body_ori, mm.qdd2accel(body_ddq, body_dq, body_q))
KT_SUP = np.diag([kt_sup / 10., kt_sup, kt_sup / 10.])
# KT_SUP = np.diag([kt_sup, kt_sup, kt_sup])
# a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori]))
a_oris = list(
map(mm.logSO3, [
mm.getSO3FromVectors(
np.dot(contact_body_ori[i],
up_vec_in_each_link[contact_ids[i]]), mm.unitY())
for i in range(len(contact_body_ori))
]))
# a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) + dt_sup*(ref_body_vel[i] - contact_body_vel[i]),
# kt_sup*a_oris[i]+dt_sup*(ref_body_angvel[i]-contact_body_angvel[i])) for i in range(len(a_oris))]
# a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) - dt_sup * contact_body_vel[i],
# kt_sup*a_oris[i] - dt_sup * contact_body_angvel[i]) for i in range(len(a_oris))]
a_sups = [
np.append(
np.dot(KT_SUP,
(ref_body_pos[i] - contact_body_pos[i] +
contMotionOffset)) - dt_sup * contact_body_vel[i],
kt_sup * a_oris[i] - dt_sup * contact_body_angvel[i])
for i in range(len(a_oris))
]
# for i in range(len(a_sups)):
# a_sups[i][1] = -kt_sup * contact_body_pos[i][1] - dt_sup * contact_body_vel[i][1]
# 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)
rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsys)
r_bias, s_bias = np.hsplit(rs, 2)
#######################################################
# optimization
#######################################################
if LEG_FLEXIBLE:
if contact == 2:
config['weightMap']['h_thigh_right'] = .8
config['weightMap']['h_shin_right'] = .8
config['weightMap']['h_heel_right'] = .8
else:
config['weightMap']['h_thigh_right'] = .1
config['weightMap']['h_shin_right'] = .25
config['weightMap']['h_heel_right'] = .2
if contact == 1:
config['weightMap']['h_thigh_left'] = .8
config['weightMap']['h_shin_left'] = .8
config['weightMap']['h_heel_left'] = .8
else:
config['weightMap']['h_thigh_left'] = .1
config['weightMap']['h_shin_left'] = .25
config['weightMap']['h_heel_left'] = .2
w = mot.getTrackingWeight(DOFs, controlModel, config['weightMap'])
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
if dH_des is not None:
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
if True:
for c_idx in range(len(contact_ids)):
mot.addConstraint2(problem, totalDOF, J_contacts[c_idx],
dJ_contacts[c_idx], dth_flat,
a_sups[c_idx])
if contactChangeCount > 0:
contactChangeCount = contactChangeCount - 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
r = problem.solve()
problem.clear()
ddth_sol_flat = np.asarray(r['x'])
# ddth_sol_flat[foot_seg_dofs] = np.array(ddth_des_flat)[foot_seg_dofs]
# ype.nested(ddth_sol_flat, ddth_sol)
rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
for _ in range(stepsPerFrame):
bodyIDs, contactPositions, contactPositionLocals, contactForces = controlModel.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
controlModel.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
# apply penalty force
# controlModel.setDOFAccelerations(ddth_sol)
# controlModel.setDOFAccelerations(ddth_des)
controlModel.set_ddq(ddth_sol_flat)
# controlModel.set_ddq(ddth_des_flat)
controlModel.solveHybridDynamics()
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([
viewer.objectInfoWnd.labelForceX.value(),
viewer.objectInfoWnd.labelForceY.value(),
viewer.objectInfoWnd.labelForceZ.value()
])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += controlModel.getTimeStep()
controlModel.applyPenaltyForce(selectedBodyId, localPos,
extraForce)
controlModel.step()
# rendering
# bodyIDs, geomIDs, positionLocalsForGeom = vpWorld.getContactInfoForcePlate(bodyIDsToCheck)
# for foot_seg_id in footIdlist:
# control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255)
# control_model_renderer.geom_colors[foot_seg_id] = [(255, 240, 255)] * controlModel.getBodyGeomNum(foot_seg_id)
# for i in range(len(geomIDs)):
# if controlModel.vpid2index(bodyIDs[i]) in footIdlist:
# control_model_renderer.geom_colors[controlModel.vpid2index(bodyIDs[i])][geomIDs[i]] = (255, 0, 0)
# for foot_seg_id in footIdlist:
# control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255)
#
# for contact_id in contact_ids:
# control_model_renderer.body_colors[contact_id] = (255, 0, 0)
rd_footCenter[0] = footCenter
rd_footCenter_ref[0] = footCenter_ref
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
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[0] / 100, dL_des_plane[1] / 100,
dL_des_plane[2] / 100
]
rd_dH_des[0] = dH_des
rd_grf_des[
0] = dL_des_plane - totalMass * controlModel.getGravity()
del rd_foot_ori[:]
del rd_foot_pos[:]
# for seg_foot_id in footIdlist:
# rd_foot_ori.append(controlModel.getJointOrientationGlobal(seg_foot_id))
# rd_foot_pos.append(controlModel.getJointPositionGlobal(seg_foot_id))
rd_foot_ori.append(controlModel.getJointOrientationGlobal(supL))
rd_foot_ori.append(controlModel.getJointOrientationGlobal(supR))
rd_foot_pos.append(controlModel.getJointPositionGlobal(supL))
rd_foot_pos.append(controlModel.getJointPositionGlobal(supR))
del rd_body_ori[:]
del rd_body_pos[:]
# for body_idx in range(dartModel.getBodyNum()):
rd_root_des[0] = rootPos[0]
rd_root_ori[0] = controlModel.getBodyOrientationGlobal(0)
rd_root_pos[0] = controlModel.getBodyPositionGlobal(0)
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append(contactForces[i] / 400)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exf_des[0] = [0, 0, 0]
else:
rd_exf_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exfen_des[0] = [0, 0, 0]
# extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody)
extraForcePos[0] = controlModel.getBodyPositionGlobal(
selectedBody) - 0.1 * np.array([
viewer.objectInfoWnd.labelForceX.value(), 0.,
viewer.objectInfoWnd.labelForceZ.value()
])
def simulateCallback(frame):
# print()
# print(dartModel.getJointVelocityGlobal(0))
# print(dartModel.getDOFVelocities()[0])
# print(dartModel.get_dq()[:6])
# dartMotionModel.update(motion[frame])
global g_initFlag
global forceShowTime
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
# print('contactstate:', contact, contactChangeCount)
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2. * (Kt**.5)
Dl = (Kl**.5)
Dh = (Kh**.5)
dt_sup = 2. * (kt_sup**.5)
# Dt = .2*(Kt**.5)
# Dl = .2*(Kl**.5)
# Dh = .2*(Kh**.5)
# dt_sup = .2*(kt_sup**.5)
pdcontroller.setKpKd(Kt, Dt)
footHeight = dartModel.getBody(supL).shapenodes[0].shape.size()[1] / 2.
doubleTosingleOffset = 0.15
singleTodoubleOffset = 0.30
com_offset_x, com_offset_y, com_offset_z = getParamVals(
['com X offset', 'com Y offset', 'com Z offset'])
footOffset = np.array((com_offset_x, com_offset_y, com_offset_z))
# tracking
# th_r = motion.getDOFPositions(frame)
th_r = dartMotionModel.getDOFPositions()
th = dartModel.getDOFPositions()
th_r_flat = dartMotionModel.get_q()
# dth_r = motion.getDOFVelocities(frame)
# dth = dartModel.getDOFVelocities()
# ddth_r = motion.getDOFAccelerations(frame)
# ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
dth_flat = dartModel.get_dq()
# dth_flat = np.concatenate(dth)
# ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q())
# ddth_des_flat = pdcontroller.compute(th_r)
ddth_des_flat = pdcontroller.compute_flat(th_r_flat)
# ype.flatten(ddth_des, ddth_des_flat)
# ype.flatten(dth, dth_flat)
print(dartModel.skeleton.get_spd_tau(th_r_flat, Kt, Dt))
#################################################
# jacobian
#################################################
footOriL = dartModel.getJointOrientationGlobal(supL)
footOriR = dartModel.getJointOrientationGlobal(supR)
footCenterL = dartModel.getBodyPositionGlobal(supL)
footCenterR = dartModel.getBodyPositionGlobal(supR)
footBodyOriL = dartModel.getBodyOrientationGlobal(supL)
footBodyOriR = dartModel.getBodyOrientationGlobal(supR)
footBodyVelL = dartModel.getBodyVelocityGlobal(supL)
footBodyVelR = dartModel.getBodyVelocityGlobal(supR)
footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL)
footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR)
refFootL = dartMotionModel.getBodyPositionGlobal(supL)
refFootR = dartMotionModel.getBodyPositionGlobal(supR)
# refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
# refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootAngVelL = np.zeros(3)
refFootAngVelR = np.zeros(3)
refFootVelR = np.zeros(3)
refFootVelL = np.zeros(3)
contactR = 1
contactL = 1
if refFootVelR[1] < 0 and refFootVelR[1] * frame_step_size + refFootR[
1] > singleTodoubleOffset:
contactR = 0
if refFootVelL[1] < 0 and refFootVelL[1] * frame_step_size + refFootL[
1] > singleTodoubleOffset:
contactL = 0
if refFootVelR[1] > 0 and refFootVelR[1] * frame_step_size + refFootR[
1] > doubleTosingleOffset:
contactR = 0
if refFootVelL[1] > 0 and refFootVelL[1] * frame_step_size + refFootL[
1] > doubleTosingleOffset:
contactL = 0
# contactR = 0
# contMotionOffset = th[0][0] - th_r[0][0]
# contMotionOffset = dartModel.getBodyPositionGlobal(0) - dartMotionModel.getBodyPositionGlobal(0)
contMotionOffset = controlToMotionOffset
linkPositions = dartModel.getBodyPositionsGlobal()
linkVelocities = dartModel.getBodyVelocitiesGlobal()
linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal()
linkInertias = dartModel.getBodyInertiasGlobal()
CM = dartModel.skeleton.com()
dCM = dartModel.skeleton.com_velocity()
CM_plane = copy.copy(CM)
CM_plane[1] = 0.
dCM_plane = copy.copy(dCM)
dCM_plane[1] = 0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
#calculate contact state
#if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08:
if g_initFlag == 1:
#contact state
# 0: flying 1: right only 2: left only 3: double
#if contact == 2 and refFootR[1] < doubleTosingleOffset:
if contact == 2 and contactR == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
#elif contact == 3 and refFootL[1] < doubleTosingleOffset:
elif contact == 1 and contactL == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
#elif contact == 3 and refFootR[1] > doubleTosingleOffset:
elif contact == 3 and contactR == 0:
contact = 2
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
#elif contact == 3 and refFootL[1] > doubleTosingleOffset:
elif contact == 3 and contactL == 0:
contact = 1
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
else:
contact = 0
#if refFootR[1] < doubleTosingleOffset:
if contactR == 1:
contact += 1
#if refFootL[1] < doubleTosingleOffset:
if contactL == 1:
contact += 2
#initialization
if g_initFlag == 0:
softConstPoint = footCenterR.copy()
footCenter = footCenterL + (footCenterR - footCenterL) / 2.0
footCenter[1] = 0.
preFootCenter = footCenter.copy()
#footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL)
#footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR)
# if refFootR[1] < doubleTosingleOffset:
# contact +=1
# if refFootL[1] < doubleTosingleOffset:
# contact +=2
if refFootR[1] < footHeight:
contact += 1
if refFootL[1] < footHeight:
contact += 2
g_initFlag = 1
contact = 2
# contact = 1 + 2
# calculate jacobian
body_num = dartModel.getBodyNum()
Jsys = np.zeros((6 * body_num, totalDOF))
dJsys = np.zeros((6 * body_num, totalDOF))
for i in range(dartModel.getBodyNum()):
Jsys[6 * i:6 * i +
6, :] = dartModel.getBody(i).world_jacobian()[range(-3, 3), :]
dJsys[6 * i:6 * i + 6, :] = dartModel.getBody(
i).world_jacobian_classic_deriv()[range(-3, 3), :]
JsupL = dartModel.getBody(supL).world_jacobian()[range(-3, 3), :]
dJsupL = dartModel.getBody(supL).world_jacobian_classic_deriv()[
range(-3, 3), :]
JsupR = dartModel.getBody(supR).world_jacobian()[range(-3, 3), :]
dJsupR = dartModel.getBody(supR).world_jacobian_classic_deriv()[
range(-3, 3), :]
# calculate footCenter
footCenter = .5 * (footCenterL + footCenterR) + footOffset
if contact == 2:
footCenter = footCenterL.copy() + footOffset
if contact == 1:
footCenter = footCenterR.copy() + footOffset
footCenter[1] = 0.
footCenter[0] += 0.02
preFootCenter = footCenter.copy()
# linear momentum
# CM_ref_plane = footCenter.copy()
# CM_ref_plane += np.array([0., 0.9, 0.])
# dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane
# dL_des_plane[1] = 0.
kl = np.diagflat([Kl * 5., Kl, Kl * 5.])
dl = np.diagflat([2.2 * Dl, Dl, 2.2 * Dl])
CM_ref = footCenter.copy()
CM_ref[1] = dartMotionModel.getCOM()[1] - 0.1
# CM_ref += np.array((0., com_offset_y, 0.))
# dL_des_plane = Kl*totalMass*(CM_ref - CM) - Dl*totalMass*dCM
dL_des_plane = kl.dot(totalMass *
(CM_ref - CM)) - dl.dot(totalMass * dCM)
# angular momentum
CP_ref = footCenter
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0]) / frame_step_size
CP_old[0] = CP
CP_des[0] = None
# if CP_des[0] is None:
# CP_des[0] = footCenter
if CP is not None and dCP is not None:
ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP)
CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des * (
frame_step_size**2)
dH_des = np.cross(
CP_des[0] - CM,
dL_des_plane - totalMass * mm.s2v(dartModel.world.gravity()))
# dH_des = np.cross(footCenter - CM, dL_des_plane - totalMass*mm.s2v(dartModel.world.gravity()))
# H = np.dot(P, np.dot(Jsys, dth_flat))
# dH_des = -Kh * H[3:]
else:
dH_des = None
# set up equality constraint
a_oriL = mm.logSO3(
mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0, 1, 0])),
np.array([0, 1, 0])))
a_oriR = mm.logSO3(
mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0, 1, 0])),
np.array([0, 1, 0])))
footErrorL = refFootL.copy()
footErrorL[1] = dartModel.getBody(
supL).shapenodes[0].shape.size()[1] / 2.
footErrorL += -footCenterL + contMotionOffset
footErrorR = refFootR.copy()
footErrorR[1] = dartModel.getBody(
supR).shapenodes[0].shape.size()[1] / 2.
footErrorR += -footCenterR + contMotionOffset
a_supL = np.append(
kt_sup * footErrorL + dt_sup * (refFootVelL - footBodyVelL),
kt_sup * a_oriL + dt_sup * (refFootAngVelL - footBodyAngVelL))
a_supR = np.append(
kt_sup * footErrorR + dt_sup * (refFootVelR - footBodyVelR),
kt_sup * a_oriR + dt_sup * (refFootAngVelR - footBodyAngVelR))
# 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)
#######################################################
# optimization
#######################################################
if LEG_FLEXIBLE:
if contact == 2:
config['weightMap']['j_thigh_right'] = .8
config['weightMap']['j_shin_right'] = .8
config['weightMap']['j_heel_right'] = .8
else:
config['weightMap']['j_thigh_right'] = .1
config['weightMap']['j_shin_right'] = .25
config['weightMap']['j_heel_right'] = .2
if contact == 1:
config['weightMap']['j_thigh_left'] = .8
config['weightMap']['j_shin_left'] = .8
config['weightMap']['j_heel_left'] = .8
else:
config['weightMap']['j_thigh_left'] = .1
config['weightMap']['j_shin_left'] = .25
config['weightMap']['j_heel_left'] = .2
w = mot.getTrackingWeightDart(DOFs, dartModel.skeleton,
config['weightMap'])
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
if dH_des is not None:
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
if contact & 1:
mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat,
a_supR)
if contact & 2:
mot.addConstraint(problem, totalDOF, JsupL, dJsupL, dth_flat,
a_supL)
if contactChangeCount > 0:
contactChangeCount -= 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
r = problem.solve()
problem.clear()
# ype.nested(r['x'], ddth_sol)
ddth_sol = np.asarray(r['x'])
# ddth_sol[:6] = np.zeros(6)
if dH_des is None:
ddth_sol = ddth_des_flat
rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
inv_h = 1. / time_step
_bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = [], [], [], []
for iii in range(stepsPerFrame):
_ddq, _tau, _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = hqp.calc_QP(
dartModel.skeleton, ddth_sol, inv_h)
# _ddq, _tau, _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = hqp.calc_QP(dartModel.skeleton, ddth_des_flat, inv_h)
# print(frame, i, tau)
dartModel.applyPenaltyForce(_bodyIDs, _contactPositionLocals,
_contactForces)
dartModel.skeleton.set_forces(_tau)
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([
viewer.objectInfoWnd.labelForceX.value(),
viewer.objectInfoWnd.labelForceY.value(),
viewer.objectInfoWnd.labelForceZ.value()
])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += time_step
dartModel.applyPenaltyForce(selectedBodyId, localPos,
extraForce)
dartModel.step()
del bodyIDs[:]
del contactPositions[:]
del contactPositions[:]
del contactPositionLocals[:]
del contactForces[:]
bodyIDs.extend(_bodyIDs)
contactPositions.extend(_contactPositions)
contactPositionLocals.extend(_contactPositionLocals)
contactForces.extend(_contactForces)
# rendering
rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0]))
rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0]))
rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0, .1]))
rightFootPos[0] = footCenterL
rightVectorX[0] = np.dot(footBodyOriL, np.array([.1, 0, 0]))
rightVectorY[0] = np.dot(footBodyOriL, np.array([0, .1, 0]))
rightVectorZ[0] = np.dot(footBodyOriL, np.array([0, 0, .1]))
rightPos[0] = footCenterL + np.array([.1, 0, 0])
rd_footCenter[0] = footCenter
rd_footCenterL[0] = footCenterL
rd_footCenterR[0] = footCenterR
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
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[0]
rd_dL_des_plane[0] = [
dL_des_plane[0] / 100, dL_des_plane[1] / 100,
dL_des_plane[2] / 100
]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(
dartModel.world.gravity())
rd_root_des[0] = rootPos[0]
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append(contactForces[i] / 100)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exf_des[0] = [0, 0, 0]
else:
rd_exf_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exfen_des[0] = [0, 0, 0]
extraForcePos[0] = dartModel.getBodyPositionGlobal(selectedBody)
def simulateCallback(frame):
global g_initFlag
global forceShowTime
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2. * (Kt**.5)
Dl = 2. * (Kl**.5)
Dh = 2. * (Kh**.5)
dt_sup = 2. * (kt_sup**.5)
# Dt = .2*(Kt**.5)
# Dl = .2*(Kl**.5)
# Dh = .2*(Kh**.5)
# dt_sup = .2*(kt_sup**.5)
pdcontroller.setKpKd(Kt, Dt)
doubleTosingleOffset = 0.15
singleTodoubleOffset = 0.30
#doubleTosingleOffset = 0.09
doubleTosingleVelOffset = 0.0
# tracking
# th_r = motion.getDOFPositions(frame)
# th = dartModel.getDOFPositions()
# dth_r = motion.getDOFVelocities(frame)
# dth = dartModel.getDOFVelocities()
# ddth_r = motion.getDOFAccelerations(frame)
# ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
dth_flat = dartModel.get_dq()
# ype.flatten(ddth_des, ddth_des_flat)
# ype.flatten(dth, dth_flat)
#################################################
# jacobian
#################################################
#caution!! body orientation and joint orientation of foot are totally different!!
footOriL = dartModel.getJointOrientationGlobal(supL)
footOriR = dartModel.getJointOrientationGlobal(supR)
#desire footCenter[1] = 0.041135
#desire footCenter[1] = 0.0197
footCenterL = dartModel.getBodyPositionGlobal(supL)
footCenterR = dartModel.getBodyPositionGlobal(supR)
footBodyOriL = dartModel.getBodyOrientationGlobal(supL)
footBodyOriR = dartModel.getBodyOrientationGlobal(supR)
footBodyVelL = dartModel.getBodyVelocityGlobal(supL)
footBodyVelR = dartModel.getBodyVelocityGlobal(supR)
footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL)
footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR)
refFootL = dartMotionModel.getBodyPositionGlobal(supL)
refFootR = dartMotionModel.getBodyPositionGlobal(supR)
refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootJointVelR = motion.getJointVelocityGlobal(supR, frame)
refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootJointR = motion.getJointPositionGlobal(supR, frame)
refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR,
(refFootR - refFootJointR))
refFootJointVelL = motion.getJointVelocityGlobal(supL, frame)
refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
refFootJointL = motion.getJointPositionGlobal(supL, frame)
refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL,
(refFootL - refFootJointL))
contactR = 1
contactL = 1
if refFootVelR[1] < 0 and refFootVelR[1] * frame_step_size + refFootR[
1] > singleTodoubleOffset:
contactR = 0
if refFootVelL[1] < 0 and refFootVelL[1] * frame_step_size + refFootL[
1] > singleTodoubleOffset:
contactL = 0
if refFootVelR[1] > 0 and refFootVelR[1] * frame_step_size + refFootR[
1] > doubleTosingleOffset:
contactR = 0
if refFootVelL[1] > 0 and refFootVelL[1] * frame_step_size + refFootL[
1] > doubleTosingleOffset:
contactL = 0
# contactR = 0
# contMotionOffset = th[0][0] - th_r[0][0]
contMotionOffset = dartModel.getBodyPositionGlobal(
0) - dartMotionModel.getBodyPositionGlobal(0)
linkPositions = dartModel.getBodyPositionsGlobal()
linkVelocities = dartModel.getBodyVelocitiesGlobal()
linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal()
linkInertias = dartModel.getBodyInertiasGlobal()
CM = dartModel.skeleton.com()
dCM = dartModel.skeleton.com_velocity()
CM_plane = copy.copy(CM)
CM_plane[1] = 0.
dCM_plane = copy.copy(dCM)
dCM_plane[1] = 0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
#calculate contact state
#if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08:
if g_initFlag == 1:
#contact state
# 0: flying 1: right only 2: left only 3: double
#if contact == 2 and refFootR[1] < doubleTosingleOffset:
if contact == 2 and contactR == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
#elif contact == 3 and refFootL[1] < doubleTosingleOffset:
elif contact == 1 and contactL == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
#elif contact == 3 and refFootR[1] > doubleTosingleOffset:
elif contact == 3 and contactR == 0:
contact = 2
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
#elif contact == 3 and refFootL[1] > doubleTosingleOffset:
elif contact == 3 and contactL == 0:
contact = 1
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
else:
contact = 0
#if refFootR[1] < doubleTosingleOffset:
if contactR == 1:
contact += 1
#if refFootL[1] < doubleTosingleOffset:
if contactL == 1:
contact += 2
#initialization
if g_initFlag == 0:
softConstPoint = footCenterR.copy()
footCenter = footCenterL + (footCenterR - footCenterL) / 2.0
footCenter[1] = 0.
preFootCenter = footCenter.copy()
#footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL)
#footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR)
if refFootR[1] < doubleTosingleOffset:
contact += 1
if refFootL[1] < doubleTosingleOffset:
contact += 2
g_initFlag = 1
#calculate jacobian
body_num = dartModel.getBodyNum()
Jsys = np.zeros((6 * body_num, totalDOF))
dJsys = np.zeros((6 * body_num, totalDOF))
for i in range(dartModel.getBodyNum()):
body_i_jacobian = dartModel.getBody(i).world_jacobian()[
range(-3, 3), :]
body_i_jacobian_deriv = dartModel.getBody(
i).world_jacobian_classic_deriv()[range(-3, 3), :]
Jsys[6 * i:6 * i + 6, :] = body_i_jacobian
dJsys[6 * i:6 * i + 6, :] = body_i_jacobian_deriv
JsupL = dartModel.getBody(supL).world_jacobian()[range(-3, 3), :]
dJsupL = dartModel.getBody(supL).world_jacobian_classic_deriv()[
range(-3, 3), :]
JsupR = dartModel.getBody(supR).world_jacobian()[range(-3, 3), :]
dJsupR = dartModel.getBody(supR).world_jacobian_classic_deriv()[
range(-3, 3), :]
dartMotionModel.update(motion[frame])
# ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q())
ddth_des_flat = pdcontroller.compute(motion.getDOFPositions(frame))
#calculate footCenter
footCenter = .5 * (footCenterL + footCenterR)
#if refFootR[1] >doubleTosingleOffset:
#if refFootR[1] > doubleTosingleOffset or footCenterR[1] > 0.08:
#if contact == 1 or footCenterR[1] > 0.08:
#if contact == 2 or footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
footCenter = footCenterL.copy()
#elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
footCenter = footCenterR.copy()
footCenter[1] = 0.
if contactChangeCount > 0 and contactChangeType == 'StoD':
#change footcenter gradually
footCenter = preFootCenter + (
maxContactChangeCount - contactChangeCount) * (
footCenter - preFootCenter) / maxContactChangeCount
preFootCenter = footCenter.copy()
# foot adjustment
foot_angle_weight = 1.
foot_dCM_weight = 5.
foot_center_diff = CM_plane + dCM_plane * frame_step_size * foot_dCM_weight - footCenter
foot_center_diff_norm = np.linalg.norm(foot_center_diff)
foot_left_height = dartModel.getJointPositionGlobal(foot_left_idx)[1]
foot_right_height = dartModel.getJointPositionGlobal(foot_left_idx)[1]
foot_left_angle = foot_angle_weight * math.atan2(
foot_center_diff_norm, foot_left_height)
foot_right_angle = foot_angle_weight * math.atan2(
foot_center_diff_norm, foot_right_height)
foot_axis = np.cross(np.array((0., 1., 0.)), foot_center_diff)
foot_left_R = mm.exp(foot_axis, foot_left_angle)
foot_right_R = mm.exp(foot_axis, foot_right_angle)
# motion[frame].mulJointOrientationGlobal(foot_left_idx, foot_left_R)
# motion[frame].mulJointOrientationGlobal(foot_right_idx, foot_right_R)
# hfi.footAdjust(motion[frame], footIdDic, SEGMENT_FOOT_MAG, SEGMENT_FOOT_RAD, 0.)
# linear momentum
#TODO:
# We should consider dCM_ref, shouldn't we?
# add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel!
# to do that, set joint velocities to vpModel
CM_ref_plane = footCenter
dL_des_plane = Kl * totalMass * (CM_ref_plane -
CM_plane) - Dl * totalMass * dCM_plane
dL_des_plane[1] = 0.
# angular momentum
CP_ref = footCenter
bodyIDs, contactPositions, contactPositionLocals, contactForces = [], [], [], []
if DART_CONTACT_ON:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info(
)
else:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
#bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0]) / frame_step_size
CP_old[0] = CP
# CP_des = None
if CP_des[0] is None:
CP_des[0] = footCenter
if CP is not None and dCP is not None:
ddCP_des = Kh * (CP_ref - CP) - Dh * dCP
CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des * (
frame_step_size**2)
# dCP_des[0] += ddCP_des * frame_step_size
# CP_des[0] += dCP_des[0] * frame_step_size + .5 * ddCP_des*(frame_step_size ** 2)
dH_des = np.cross(
CP_des[0] - CM,
(dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
if contactChangeCount > 0: # and contactChangeType == 'DtoS':
#dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount*10)
dH_des *= (maxContactChangeCount -
contactChangeCount) / maxContactChangeCount
#dH_des *= (contactChangeCount)/(maxContactChangeCount)*.9+.1
else:
dH_des = None
# H = np.dot(P, np.dot(Jsys, dth_flat))
# dH_des = -Kh* H[3:]
# soft point constraint
#softConstPoint = refFootR.copy()
##softConstPoint[0] += 0.2
#Ksc = 50
#Dsc = 2*(Ksc**.5)
#Bsc = 1.
#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_des - dP_cur)
#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)
#dJconst = (Jconst - Jconst)/(1/30.)
#JconstPre = Jconst.copy()
##yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False)
#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))
#q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2
#set up equality constraint
# a_oriL = mm.logSO3(mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0,1,0])), np.array([0,1,0])))
# a_oriR = mm.logSO3(mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0,1,0])), np.array([0,1,0])))
left_foot_up_vec, right_foot_up_vec = hfi.get_foot_up_vector(
motion[frame], footIdDic, None)
a_oriL = mm.logSO3(
mm.getSO3FromVectors(left_foot_up_vec, np.array([0, 1, 0])))
a_oriR = mm.logSO3(
mm.getSO3FromVectors(right_foot_up_vec, np.array([0, 1, 0])))
#if contact == 3 and contactChangeCount < maxContactChangeCount/4 and contactChangeCount >=1:
#kt_sup = 30
#viewer.objectInfoWnd.labelSupKt.value(kt_sup)
#viewer.objectInfoWnd.sliderSupKt.value(initSupKt*10)
# a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL))
# a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR))
a_supL = np.append(
kt_sup * (refFootL - footCenterL + contMotionOffset) + dt_sup *
(refFootVelL - footBodyVelL),
kt_sup * a_oriL + dt_sup * (refFootAngVelL - footBodyAngVelL))
a_supR = np.append(
kt_sup * (refFootR - footCenterR + contMotionOffset) + dt_sup *
(refFootVelR - footBodyVelR),
kt_sup * a_oriR + dt_sup * (refFootAngVelR - footBodyAngVelR))
# a_supL[3:] = 0.
# a_supR[3:] = 0.
if contactChangeCount > 0 and contactChangeType == 'DtoS':
#refFootR += (footCenter-CM_plane)/2.
#refFootR[1] = 0
#pre contact value are needed
#if contact == 2:
##refFootR[0] += 0.2
##refFootR[2] -= 0.05
#offsetDropR = (footCenter-CM_plane)/2.
#refFootR += offsetDropR
#refFootR[1] = 0.
##refFootR[2] = footCenterR[2] - contMotionOffset[2]
##refFootR[0] = footCenterR[0] - contMotionOffset[0]
#refFootL[0] += 0.05
#refFootL[2] -= 0.05
#elif contact == 1:
#offsetDropL = (footCenter-CM_plane)/2.
#refFootL += offsetDropL
#refFootL[1] = 0.
#a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL))
#a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR))
#a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL))
#a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR))
a_supL = np.append(
kt_sup * (refFootL - footCenterL + contMotionOffset) + dt_sup *
(refFootVelL - footBodyVelL), 4 * kt_sup * a_oriL +
2 * dt_sup * (refFootAngVelL - footBodyAngVelL))
a_supR = np.append(
kt_sup * (refFootR - footCenterR + contMotionOffset) + dt_sup *
(refFootVelR - footBodyVelR), 4 * kt_sup * a_oriR +
2 * dt_sup * (refFootAngVelR - footBodyAngVelR))
elif contactChangeCount > 0 and contactChangeType == 'StoD':
#refFootR[0] +=0.05
#refFootR[2] +=0.05
linkt = (13. * contactChangeCount) / maxContactChangeCount + 1.
lindt = 2 * (linkt**.5)
angkt = (13. * contactChangeCount) / maxContactChangeCount + 1.
angdt = 2 * (angkt**.5)
#a_supL = np.append(4*kt_sup*(refFootL - footCenterL + contMotionOffset) + 2*dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL))
#a_supR = np.append(4*kt_sup*(refFootR - footCenterR + contMotionOffset) + 2*dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR))
a_supL = np.append(
linkt * kt_sup * (refFootL - footCenterL + contMotionOffset) +
lindt * dt_sup * (refFootVelL - footBodyVelL),
angkt * kt_sup * a_oriL + angdt * dt_sup *
(refFootAngVelL - footBodyAngVelL))
a_supR = np.append(
linkt * kt_sup * (refFootR - footCenterR + contMotionOffset) +
lindt * dt_sup * (refFootVelR - footBodyVelR),
angkt * kt_sup * a_oriR + angdt * dt_sup *
(refFootAngVelR - footBodyAngVelR))
#a_supL = np.append(16*kt_sup*(refFootL - footCenterL + contMotionOffset) + 4*dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL))
#a_supR = np.append(16*kt_sup*(refFootR - footCenterR + contMotionOffset) + 4*dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR))
#a_supL = np.append(4*kt_sup*(refFootL - footCenterL + contMotionOffset) + 2*dt_sup*(refFootVelL - footBodyVelL), 32*kt_sup*a_oriL+5.6*dt_sup*(refFootAngVelL-footBodyAngVelL))
#a_supR = np.append(4*kt_sup*(refFootR - footCenterR + contMotionOffset) + 2*dt_sup*(refFootVelR - footBodyVelR), 32*kt_sup*a_oriR+5.6*dt_sup*(refFootAngVelR-footBodyAngVelR))
#a_supL[1] = kt_sup*(refFootL[1] - footCenterL[1] + contMotionOffset[1]) + dt_sup*(refFootVelL[1] - footBodyVelL[1])
#a_supR[1] = kt_sup*(refFootR[1] - footCenterR[1] + contMotionOffset[1]) + dt_sup*(refFootVelR[1] - footBodyVelR[1])
##if contact == 2:
#if refFootR[1] <doubleTosingleOffset :
#Jsup = np.vstack((JsupL, JsupR))
#dJsup = np.vstack((dJsupL, dJsupR))
#a_sup = np.append(a_supL, a_supR)
#else:
#Jsup = JsupL.copy()
#dJsup = dJsupL.copy()
#a_sup = a_supL.copy()
# 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)
#######################################################
# optimization
#######################################################
#if contact == 2 and footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
config['weightMap']['RightUpLeg'] = .8
config['weightMap']['RightLeg'] = .8
config['weightMap']['RightFoot'] = .8
else:
config['weightMap']['RightUpLeg'] = .1
config['weightMap']['RightLeg'] = .25
config['weightMap']['RightFoot'] = .2
#if contact == 1 and footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
config['weightMap']['LeftUpLeg'] = .8
config['weightMap']['LeftLeg'] = .8
config['weightMap']['LeftFoot'] = .8
else:
config['weightMap']['LeftUpLeg'] = .1
config['weightMap']['LeftLeg'] = .25
config['weightMap']['LeftFoot'] = .2
w = mot.getTrackingWeight(DOFs, motion[0].skeleton,
config['weightMap'])
#if contact == 2:
#mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
if dH_des is not None:
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, dJsup, dth_flat, a_sup)
#mot.addConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup)
#if contact & 1 and contactChangeCount == 0:
if contact & 1:
#if refFootR[1] < doubleTosingleOffset:
mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat,
a_supR)
if contact & 2:
#if refFootL[1] < doubleTosingleOffset:
mot.addConstraint(problem, totalDOF, JsupL, dJsupL, dth_flat,
a_supL)
if contactChangeCount > 0:
contactChangeCount -= 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
r = problem.solve()
problem.clear()
# ype.nested(r['x'], ddth_sol)
ddth_sol = np.asarray(r['x'])
# remove foot seg effect
ddth_sol[foot_dofs] = ddth_des_flat[foot_dofs]
# ddth_sol[:] = ddth_des_flat[:]
rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
for i in range(stepsPerFrame):
# apply penalty force
if not DART_CONTACT_ON:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
dartModel.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
#bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
dartModel.skeleton.set_accelerations(ddth_sol)
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([
viewer.objectInfoWnd.labelForceX.value(),
viewer.objectInfoWnd.labelForceY.value(),
viewer.objectInfoWnd.labelForceZ.value()
])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += wcfg.timeStep
dartModel.applyPenaltyForce(selectedBodyId, localPos,
extraForce)
dartModel.step()
if DART_CONTACT_ON:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info(
)
else:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
# rendering
rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0]))
rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0]))
rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0, .1]))
rightFootPos[0] = footCenterL
rightVectorX[0] = np.dot(footBodyOriL, np.array([.1, 0, 0]))
rightVectorY[0] = np.dot(footBodyOriL, np.array([0, .1, 0]))
rightVectorZ[0] = np.dot(footBodyOriL, np.array([0, 0, .1]))
rightPos[0] = footCenterL + np.array([.1, 0, 0])
rd_footCenter[0] = footCenter
rd_footCenterL[0] = footCenterL
rd_footCenterR[0] = footCenterR
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
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[0]
rd_dL_des_plane[0] = [
dL_des_plane[0] / 100, dL_des_plane[1] / 100,
dL_des_plane[2] / 100
]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity)
rd_root_des[0] = rootPos[0]
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append(contactForces[i] / 100)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exf_des[0] = [0, 0, 0]
else:
rd_exf_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exfen_des[0] = [0, 0, 0]
extraForcePos[0] = dartModel.getBodyPositionGlobal(selectedBody)
