def simulateCallback(self, frame):
global ddth_des_flat
global stepsPerFrame
global wcfg
global vpWorld
# reload(tf)
# motionModel.update(motion[frame])
self.frame = frame
print("main:frame : ", frame)
motionModel.update(motion[0])
self.timeIndex = 0
self.setTimeStamp()
# IK solver
'''
solver.clear()
solver.setInitPose(motion[0])
cVpBodyIds, cPositions, cPositionsLocal, cVelocities = vpWorld.getContactPoints(bodyIDsToCheck)
if len(cVpBodyIds) > 1:
solver.addConstraints(cVpBodyIds[1], cPositionsLocal[1], np.array((0., 0., 0.)), None, (False, True, False, False))
solver.addConstraints(cVpBodyIds[3], cPositionsLocal[3], np.array((0., 0., 0.)), None, (False, True, False, False))
solver.addConstraints(cVpBodyIds[5], cPositionsLocal[5], np.array((0., 0., 0.)), None, (False, True, False, False))
# solver.solve(controlModel, np.array((0., .15 + .05*math.sin(frame/10.), 0.)))
'''
# constant setting
# (Kt, damp, stepsPerFrame, simulSpeedInv) = viewer.objectInfoWnd.getVals()
getVal = viewer.objectInfoWnd.getVal
Kt = getVal('PD gain')
damp = getVal('Joint Damping')
stepsPerFrame = getVal('steps per frame')
simulSpeedInv = getVal('1/simul speed')
wcfg.timeStep = 1 / (30. * simulSpeedInv * stepsPerFrame)
vpWorld.SetTimeStep(wcfg.timeStep)
# Dt = 2. * (Kt**.5)
# Dt = 2. * (Kt**.5)/20.
Dt = 0.
# controlModel.SetJointsDamping(damp)
controlModel.SetJointsDamping(1.)
wLCP = math.pow(2., getVal('LCP weight'))
wForce = math.pow(2., getVal('force weight'))
wTorque = math.pow(2., getVal('tau weight'))
# tracking
th_r = motion.getDOFPositions(0)
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(0)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(0)
weightMapTuple = config['weightMapTuple']
weightMapTuple = None
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r,
Kt, Dt, weightMapTuple)
ddth_c = controlModel.getDOFAccelerations()
ype.flatten(ddth_des, ddth_des_flat)
desForceFrameBegin = getVal('des force begin')
desForceDuration = getVal('des force dur') * simulSpeedInv
desForceFrame = [
desForceFrameBegin, desForceFrameBegin + desForceDuration
]
desForceRelFrame = float(frame - desForceFrame[0]) / desForceDuration
desNormalForceMin = getVal('normal des force min')
desNormalForceMax = getVal('normal des force max')
desNormalForce = desNormalForceMin
if desForceFrame[0] <= frame <= desForceFrame[1]:
desNormalForce = desNormalForceMin * \
(1 - desForceRelFrame) + desNormalForceMax * desForceRelFrame
totalForce = np.array([0., desNormalForce, 0., 0., 0., 0.])
# totalForce = np.array([-desNormalForce, 34.3, 0., 0., 0., 0.])
# totalForce = np.array([0., 34.3, desNormalForce, 0., 0., 0.])
# totalForce = np.array([50., 150.])
torques = None
ddth_des_flat[0:6] = [0.] * 6
self.setTimeStamp()
simulContactForces = np.zeros(3)
cBodyIDs = None
cPositions = None
cPositionLocals = None
cForces = None
cBodyIDsControl = None
cPositionsControl = None
cPositionLocalsControl = None
cForcesControl = None
# if desForceFrame[0] <= frame <= desForceFrame[1]:
if True:
# totalForceImpulse = stepsPerFrame * totalForce
cBodyIDsControl, cPositionsControl, cPositionLocalsControl, cForcesControl, torques \
= hls.calcLCPbasicControl(
motion, vpWorld, controlModel, bodyIDsToCheck, 1., totalForce, [wLCP, wTorque, wForce], ddth_des_flat)
# if cForces is not None:
# print "control: ", sum(cForces)
sumControlForce = np.array([0.] * 6)
if cForcesControl is not None:
sumControlForce = np.hstack(
(sum(cForcesControl), np.array([0., 0., 0.])))
timeStamp = None
torque_None = False
if not (desForceFrame[0] <= frame <= desForceFrame[1]) or (torques is
None):
torque_None = True
torques = ddth_des_flat
elif np.linalg.norm(sumControlForce -
totalForce) > np.linalg.norm(totalForce):
print("control failed!")
torque_None = True
torques = ddth_des_flat
else:
torques *= 1.
for i in range(int(stepsPerFrame)):
if i % 5 == 0:
cBodyIDs, cPositions, cPositionLocals, cForces, timeStamp \
= hls.calcLCPForces(motion, vpWorld, controlModel, bodyIDsToCheck, 1., torques, solver='qp')
if i % 5 == 0 and len(cBodyIDs) > 0:
# apply contact forces
if False and not torque_None:
vpWorld.applyPenaltyForce(cBodyIDs, cPositionLocals,
cForcesControl)
simulContactForces += sum(cForcesControl)
else:
vpWorld.applyPenaltyForce(cBodyIDs, cPositionLocals,
cForces)
simulContactForces += sum(cForces)
# simulContactForces += sum(cForces)
ype.nested(torques, torques_nested)
controlModel.setDOFTorques(torques_nested[1:])
vpWorld.step()
self.setTimeStamp()
# rendering expected force
del rd_cForcesControl[:]
del rd_cPositionsControl[:]
if cBodyIDsControl is not None:
# print cBodyIDsControl
for i in range(len(cBodyIDsControl)):
# print expected force
rd_cForcesControl.append(cForcesControl[i].copy() / 50.)
rd_cPositionsControl.append(cPositionsControl[i].copy())
# rendering sum of expected force
del rd_ForceControl[:]
del rd_Position[:]
if cForcesControl is not None:
# print expected force
rd_ForceControl.append(sum(cForcesControl) / 50.)
rd_Position.append(np.array([0., 0., 0.1]))
# graph expected force
if cForcesControl is not None:
sumForce = sum(cForcesControl)
if sumForce[1] > 10000:
sumForce[1] = 10000
viewer.cForceWnd.insertData('expForce', frame, sumForce[1])
else:
viewer.cForceWnd.insertData('expForce', frame, 0.)
# rendering calculated forces
del rd_cForces[:]
del rd_cPositions[:]
for i in range(len(cBodyIDs)):
# print calculated force
rd_cForces.append(cForces[i].copy() / 50.)
rd_cPositions.append(cPositions[i].copy())
# rendering joint position
del rd_jointPos[:]
for i in range(motion[0].skeleton.getJointNum()):
rd_jointPos.append(motion[frame].getJointPositionGlobal(i))
# rendering desired force
del rd_ForceDes[:]
del rd_PositionDes[:]
# rd_ForceDes.append(totalForce/50.)
rd_ForceDes.append(totalForce[1] * np.array([0., 1., 0.]) / 50.)
rd_PositionDes.append(np.array([0., 0., 0.]))
# if self.cForces is not None:
# rd_ForceDes.append(sum(self.cForces)[1]/50. * [0., 1., 0.])
# rd_PositionDes.append(np.array([0., 0., -0.1]))
# graph calculated force
if cForces is not None:
sumForce = sum(cForces)
# viewer.cForceWnd.insertData('realForce', frame, sumForce[1])
viewer.cForceWnd.insertData('realForce', frame,
simulContactForces[1] / stepsPerFrame)
else:
viewer.cForceWnd.insertData('realForce', frame, 0.)
# viewer.cForceWnd.insertData('realForce', frame, simulContactForces[1]/stepsPerFrame)
# graph desired force
if desForceFrame[0] <= frame <= desForceFrame[1]:
viewer.cForceWnd.insertData('desForceMin', frame, totalForce[1])
# viewer.cForceWnd.insertData('desForceMin', frame, totalForce[1] * 1.0)
# viewer.cForceWnd.insertData('desForceMax', frame, totalForce[1] * 1.1)
else:
viewer.cForceWnd.insertData('desForceMin', frame, 0.)
viewer.cForceWnd.insertData('desForceMax', frame, 0.)
self.setTimeStamp()
def simulateCallback(frame):
# print(frame)
# print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0']))
if True:
if frame == 0:
foot_viewer.check_all_seg()
if motionFile == 'wd2_tiptoe_zygote.bvh':
setParamVal('com Y offset', -0.01)
elif frame == start_frame:
foot_viewer.check_tilt_left_all()
# elif frame == start_frame+10:
setParamVal('left tilt angle', -0.2)
setParamVal('right tilt angle', 0.2)
elif frame == start_frame + 90:
setParamVal('left tilt angle', 0.)
setParamVal('right tilt angle', 0.)
foot_viewer.check_all_seg()
# 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))
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*0.8))
motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle*0.8))
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*0.8))
motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle*0.8))
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*0.8))
motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle*0.8))
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*0.8))
motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle*0.8))
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()
# footCenter = np.array([1.54049, 0., -0.29345])
# 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))]))
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))]
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_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.])
# 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))]
# 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))]
# set up soft constraint
ee_idx = motion[frame].skeleton.getJointIndex('Spine')
ee_ori = controlModel.getBodyOrientationGlobal(ee_idx)
ee_ang = controlModel.getBodyAngVelocityGlobal(ee_idx)
J_ee = Jsys[6*ee_idx+3:6*ee_idx+6, :]
dJ_ee = dJsys[6*ee_idx+3:6*ee_idx+6]
a_ee = 20. * mm.logSO3(mm.getSO3FromVectors(np.dot(ee_ori, mm.unitZ()), mm.unitY())) - 1. * ee_ang
# 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)
# mot.addEndEffectorTerms(problem, totalDOF, 1., J_ee, dJ_ee, dth, a_ee)
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())
# 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)):
control_model_renderer.geom_colors[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 * 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)
# 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)