def getDesiredDOFAccelerationsExtended(th_r, th, dth, Kt, Dt):
ddth_des = [None] * len(th_r) # type: list[np.ndarray]
p_r0 = th_r[0][0]
p0 = th[0][0]
v0 = dth[0][0:3]
th_r0 = th_r[0][1]
th0 = th[0][1]
dth0 = dth[0][3:6]
a_des0 = Kt[0] * (p_r0 - p0) + Dt[0] * (-v0)
ddth_des0 = Kt[0] * (mm.logSO3(np.dot(th0.transpose(),
th_r0))) + Dt[0] * (-dth0)
ddth_des[0] = np.concatenate((a_des0, ddth_des0))
for i in range(1, len(th_r)):
if th[i].shape[0] == 3:
ddth_des[i] = Kt[i + 1] * (mm.logSO3(
np.dot(th[i].transpose(), th_r[i]))) + Dt[i + 1] * (-dth[i])
elif th[i].shape[0] > 0:
ddth_des[i] = Kt[i + 1] * (mm.logSO3(
np.dot(th[i].transpose(), th_r[i]))) + Dt[i + 1] * (-dth[i])
else:
ddth_des[i] = np.zeros(0)
return ddth_des
def compute(self, positions_hat, weightMap=None):
skel = self.skel
# invM = inv(skel.M + self.Kd * self.h)
# p = -self.Kp.dot(skel.q + skel.dq * self.h - qhat)
# d = -self.Kd.dot(skel.dq)
# qddot = invM.dot(-skel.c + p + d + skel.constraint_forces())
# tau = p + d - self.Kd.dot(qddot) * self.h
'''
# Check the balance
COP = skel.body('h_heel_left').to_world([0.05, 0, 0])
offset = skel.C[0] - COP[0]
# Adjust the target pose
k1 = 200.0 if 0.0 < offset and offset < 0.1 else 2000.0
k2 = 100.0
kd = 10.0 if 0.0 < offset and offset < 0.1 else 100.0
q_delta1 = np.array([-k1, -k2, -k1, -k2]) * offset
q_delta2 = np.ones(4) * kd * (self.preoffset - offset)
tau[np.array([17, 25, 19, 26])] += (q_delta1 + q_delta2)
self.preoffset = offset
#'''
# Make sure the first six are zero
th_r = positions_hat
ddth_des = [None] * len(th_r)
th = self.model.getDOFPositions()
dth = self.model.getDOFVelocities()
p_r0 = th_r[0][0]
p0 = th[0][0]
v0 = dth[0][:3]
th_r0 = th_r[0][1]
th0 = th[0][1]
dth0 = dth[0][3:]
kt = self.Kp[0, 0]
dt = self.Kd[0, 0]
if weightMap is not None:
kt = Kt * weightMap[0]
dt = Dt * (weightMap[0]**.5)
# dt = 0.
a_des0 = kt * (p_r0 - p0) - dt * v0
ddth_des0 = kt * (mm.logSO3(np.dot(th0.transpose(),
th_r0))) - dt * dth0
ddth_des[0] = np.concatenate((a_des0, ddth_des0))
for i in range(1, len(th_r)):
if weightMap is not None:
kt = Kt * weightMap[i]
dt = Dt * (weightMap[i]**.5)
# dt = 0.
ddth_des[i] = kt * (mm.logSO3(np.dot(th[i].transpose(),
th_r[i]))) - dt * dth[i]
# ddth_des[i] = kt*(mm.logSO3(np.dot(th[i].transpose(), th_r[i]))) + dt*( - dth[i]) #+ ddth_r[i]
return np.concatenate(ddth_des)
def __init__(self, body, des, weight):
"""
:type body: pydart.BodyNode
:param des:
:param weight:
"""
super().__init__()
self.body = body
self.des = mm.seq2Vec3(mm.logSO3(des))
self.weight = weight
self.cur = mm.seq2Vec3(mm.logSO3(self.body.world_transform()[:3, :3]))
def getDesFootAngularAcc(refModel,
controlModel,
footIndex,
Kk,
Dk,
axis=[0, 1, 0],
desAng=[0, 1, 0]):
desAngularAcc = [0, 0, 0]
curAng = [controlModel.getBodyOrientationGlobal(footIndex)]
refAngVel = refModel.getBodyAngVelocityGlobal(footIndex)
curAngVel = controlModel.getBodyAngVelocityGlobal(footIndex)
refAngAcc = (0, 0, 0)
curAngY = np.dot(curAng, np.array(axis))
refAngY = np.array(desAng)
if stage == MOTION_TRACKING + 10:
refAng = [refModel.getBodyOrientationGlobal(footIndex)]
refAngY2 = np.dot(refAng, np.array([0, 1, 0]))
refAngY = refAngY2[0]
aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], refAngY))
desAngularAcc = Kk * aL + Dk * (refAngVel - curAngVel)
return desAngularAcc
def blendSegmentSmooth(motionSegment0,
motionSegment1,
attachPosition=True,
attachOrientation=True):
motionSegment1 = motionSegment1.copy()
if attachPosition:
p_offset = motionSegment0[0].rootPos - motionSegment1[0].rootPos
motionSegment1.translateByOffset(p_offset)
if attachOrientation:
R_offset = np.dot(motionSegment0[0].localRs[0],
motionSegment1[0].localRs[0].T)
R_offset = mm.exp(
mm.projectionOnVector(mm.logSO3(R_offset),
mm.v3(0, 1, 0))) # # project on y axis
motionSegment1.rotateTrajectory(R_offset)
newMotion = ym.JointMotion([None] * (int(
(len(motionSegment0) + len(motionSegment1)) / 2.)))
# newMotion = ym.JointMotion( [None]*(int( t*len(motionSegment0) + (1-t)*len(motionSegment1)) ) )
df0 = float(len(newMotion)) / len(motionSegment0)
df1 = float(len(newMotion)) / len(motionSegment1)
for frame in range(len(newMotion)):
normalizedFrame = float(frame) / (len(newMotion) - 1)
normalizedFrame2 = yfg.H1(normalizedFrame)
normalizedFrame2 += df0 * yfg.H2(normalizedFrame)
normalizedFrame2 += df1 * yfg.H3(normalizedFrame)
posture0_at_normalizedFrame = motionSegment0.getPostureAt(
normalizedFrame2 * (len(motionSegment0) - 1))
posture1_at_normalizedFrame = motionSegment1.getPostureAt(
normalizedFrame2 * (len(motionSegment1) - 1))
newMotion[frame] = posture0_at_normalizedFrame.blendPosture(
posture1_at_normalizedFrame, normalizedFrame2)
return newMotion
def getDesiredAngAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt):
ddth_des = [None] * len(th_r)
for i in range(len(th_r)):
ddth_des[i] = Kt * (mm.logSO3(np.dot(th[i].transpose(), th_r[i]))
) + Dt * (dth_r[i] - dth[i]) + ddth_r[i]
return ddth_des
def getDesiredDOFAccelerations_flat(th_r,
th,
dth_r,
dth,
ddth_r,
Kt,
Dt,
joint_dof_info,
weightMap=None):
ddth_des_flat = np.zeros_like(th) # type: list[np.ndarray]
kt = Kt
dt = Dt
for i in range(len(joint_dof_info)):
dof_start_index, dof = joint_dof_info[i]
_th_r = th_r[dof_start_index:dof_start_index + dof]
_th = th[dof_start_index:dof_start_index + dof]
_dth = dth[dof_start_index:dof_start_index + dof]
if weightMap is not None:
kt = Kt * weightMap[i]
dt = Dt * (weightMap[i]**.5)
# dt = 0.
if dof == 0:
continue
if dof == 6:
ddth_des_flat[dof_start_index + 0:dof_start_index +
3] = kt * (_th_r[:3] - _th[:3]) + dt * (
-_dth[:3]) #+ ddth_r[i]
ddth_des_flat[dof_start_index + 3:dof_start_index + 6] = kt * (
mm.logSO3(np.dot(mm.exp(_th[3:]).T, mm.exp(
_th_r[3:])))) + dt * (-_dth[3:]) #+ ddth_r[i]
if dof == 3:
ddth_des_flat[dof_start_index + 0:dof_start_index +
3] = kt * (mm.logSO3(
np.dot(mm.exp(_th).T, mm.exp(_th_r)))) + dt * (
-_dth) #+ ddth_r[i]
else:
ddth_des_flat[dof_start_index + 0:dof_start_index +
dof] = kt * (_th_r - _th) + dt * (-_dth
) #+ ddth_r[i]
return ddth_des_flat
def getBlendedNextMotion2(nextMotionA,
nextMotionB,
prevEndPosture,
t=None,
attachPosition=True,
attachOrientation=True):
dA = prevEndPosture - nextMotionA[0]
dB = prevEndPosture - nextMotionB[0]
newNextMotionA = nextMotionA.copy()
newNextMotionB = nextMotionB.copy()
if attachPosition:
p_offset_A = dA.rootPos
p_offset_B = dB.rootPos
# d.disableTranslation()
newNextMotionA.translateByOffset(p_offset_A)
newNextMotionB.translateByOffset(p_offset_B)
if attachOrientation:
R_offset_A = dA.getJointOrientationLocal(0)
R_offset_A = mm.exp(
mm.projectionOnVector(mm.logSO3(R_offset_A),
mm.v3(0, 1, 0))) # # project on y axis
R_offset_B = dA.getJointOrientationLocal(0)
R_offset_B = mm.exp(
mm.projectionOnVector(mm.logSO3(R_offset_B),
mm.v3(0, 1, 0))) # # project on y axis
# d.disableRotations([0])
newNextMotionA.rotateTrajectory(R_offset_A)
newNextMotionB.rotateTrajectory(R_offset_B)
if t == None:
blendedNextMotion = blendSegmentSmooth(newNextMotionA, newNextMotionB)
else:
blendedNextMotion = blendSegmentFixed(newNextMotionA, newNextMotionB,
t)
# del blendedNextMotion[0]
return blendedNextMotion
def reward(self):
body_e = list(map(self.skel.body, self.idx_e))
p_e = np.asarray([body.world_transform()[:3, 3]
for body in body_e]).flatten()
p_e_ori = [body.world_transform()[:3, :3] for body in body_e]
# p_e_ori_diff = np.asarray([mm.logSO3(np.dot(p_e_ori[idx].T, self.prev_ref_p_e_ori_hat[idx])) for idx in range(len(p_e_ori))]).flatten()
q_diff = np.asarray(
self.skel.position_differences(self.prev_ref_q, self.skel.q))
dq_diff = np.asarray(
self.skel.velocity_differences(self.prev_ref_dq, self.skel.dq))
rewards = list()
# q reward
# rewards.append(exp_reward_term(self.w_p, self.exp_p,
# np.concatenate(list(q_diff[get_joint_dof_range(self.skel.joint(joint_name))] for joint_name in self.reward_joint))))
rewards.append(exp_reward_term(self.w_p, self.exp_p, q_diff))
# dq reward
# rewards.append(exp_reward_term(self.w_v, self.exp_v,
# np.concatenate(list(dq_diff[get_joint_dof_range(self.skel.joint(joint_name))] for joint_name in self.reward_joint))))
rewards.append(exp_reward_term(self.w_v, self.exp_v, dq_diff))
# end effector reward
rewards.append(
exp_reward_term(self.w_e, self.exp_e, p_e - self.prev_ref_p_e_hat))
# end effector orientation reward
# rewards.append(exp_reward_term(self.w_e_ori, self.exp_e_ori, p_e_ori_diff))
# com reward
rewards.append(
exp_reward_term(self.w_c, self.exp_c,
self.skel.com() - self.prev_ref_com))
# com_vel reward
# rewards.append(exp_reward_term(self.w_c_v, self.exp_c_v, self.skel.com_velocity() - self.prev_ref_com_vel))
# rewards.append(exp_reward_term(self.w_c_v, self.exp_c_v, self.skel.com_spatial_velocity() - self.prev_ref_com_spatial_vel))
# torso reward
torso_ori = self.skel.body('Spine').world_transform()[:3, :3]
torso_ori_diff = np.asarray(
mm.logSO3(np.dot(torso_ori.T, self.prev_ref_torso_ori)))
rewards.append(exp_reward_term(self.w_t, self.exp_t, torso_ori_diff))
# ankle y position reward
# ankle_joint_y_pos = np.asarray([joint.get_world_frame_after_transform()[1, 3] for joint in [self.skel.joint(joint_name) for joint_name in self.ankle_joint_names]]).flatten()
# rewards.append(exp_reward_term(self.w_y_ankle, self.exp_y_ankle, ankle_joint_y_pos - self.prev_ref_ankle_joint_y_pos))
return sum(rewards) / (len(self.ref_motion) + self.ref_motion.fps)
def calcDeltaq(self):
deltaq = np.zeros(self.skel.q.shape)
if self.Rs is not None:
p_r0 = self.Rs[0][0]
p0 = self.skel.q[3:6]
th_r0 = self.Rs[0][1]
th0 = mm.exp(self.skel.q[:3])
deltaq[:6] = np.hstack((mm.logSO3(np.dot(th0.transpose(),
th_r0)), p_r0 - p0))
# TODO:
# apply variety dofs
dofOffset = 6
for i in range(1, len(self.skel.joints)):
# for i in range(1, len(self.Rs)):
joint = self.skel.joints[i]
if joint.num_dofs() == 3:
deltaq[dofOffset:dofOffset + 3] = mm.logSO3(
np.dot(joint.get_local_transform()[:3, :3].transpose(),
self.Rs[i]))
elif joint.num_dofs() == 2:
targetAngle1 = math.atan2(-self.Rs[i][1, 2], self.Rs[i][2,
2])
targetAngle2 = math.atan2(-self.Rs[i][0, 1], self.Rs[i][0,
0])
deltaq[dofOffset:dofOffset + 2] = np.array(
[targetAngle1, targetAngle2])
elif joint.num_dofs() == 1:
deltaq[dofOffset] = math.atan2(self.Rs[i][2, 1],
self.Rs[i][1, 1])
dofOffset += joint.num_dofs()
# a_des0 = kt*(p_r0 - p0) + dt*(- v0) #+ a_r0
# ddth_des0 = kt*(mm.logSO3(np.dot(th0.transpose(), th_r0))) + dt*(- dth0) #+ ddth_r0
return deltaq
def renderVpBody(self, body_idx):
# glPushMatrix()
# _t = self._model.getBodyPositionGlobal(body_idx)
# glTranslatef(_t[0], _t[1], _t[2])
# print(body_idx, self._model.index2name(body_idx), self._model.getBodyShape(body_idx))
# print(self._model.index2name(body_idx), self._model.getBodyGeomsType(body_idx), self._model.getBodyGeomsSize(body_idx))
# print(self._model.index2name(body_idx), self._model.getBodyGeomsGlobalFrame(body_idx))
geom_types = self._model.getBodyGeomsType(body_idx)
print('renderVpBody: ', geom_types)
geom_sizes = self._model.getBodyGeomsSize(body_idx)
print('renderVpBody: ', geom_sizes)
geom_frames = self._model.getBodyGeomsGlobalFrame(body_idx)
print('renderVpBody: ', geom_frames)
for i in range(self._model.getBodyGeomNum(body_idx)):
glPushMatrix()
geom_type, geom_sizes, _T = geom_types[i], geom_sizes[
i], geom_frames[i]
_t = _T[:3, 3].flatten()
_r = mm.logSO3(_T[:3, :3])
# print(_t)
glTranslatef(_t[0], _t[1], _t[2])
# glMultMatrixd(_T)
# pGeom->GetShape(&type, data)
if geom_type == 'B' or geom_type == 'M':
data = .5 * geom_sizes
# data[0] *= SCALAR_1_2
# data[1] *= SCALAR_1_2
# data[2] *= SCALAR_1_2
_draw_box(data)
elif geom_type == 'C':
data = geom_sizes[i]
# data.append(pGeom.GetRadius())
# data.append(pGeom.GetHeight())
data[1] -= 2. * data[0]
_draw_capsule(data[0], data[1])
elif geom_type == 'S':
data = geom_sizes[i]
_draw_sphere(data[0])
glPopMatrix()
def reward(self):
p_e = np.asarray([self.skel.getBodyPositionGlobal(body_idx) for body_idx in self.idx_e]).flatten()
q = self.skel.getDOFPositions()
dq = self.skel.getDOFVelocities()
q_diff = np.asarray([mm.logSO3(np.dot(self.prev_ref_q[i].T, q[i])) for i in range(1, len(q))]).flatten()
dq_diff = np.asarray([self.prev_ref_dq[i] - dq[i] for i in range(1, len(dq))]).flatten()
q_reward = exp_reward_term(self.w_p, self.exp_p,
np.concatenate(list(q_diff[self.skel.getJointDOFInternalIndexes(self.skel.name2index(joint_name))] for joint_name in self.reward_joint)))
dq_reward = exp_reward_term(self.w_v, self.exp_v,
np.concatenate(list(dq_diff[self.skel.getJointDOFInternalIndexes(self.skel.name2index(joint_name))] for joint_name in self.reward_joint)))
ee_reward = exp_reward_term(self.w_e, self.exp_e, p_e - self.prev_ref_p_e_hat)
com_reward = exp_reward_term(self.w_c, self.exp_c, self.skel.getCOM() - self.prev_ref_com)
reward = q_reward + dq_reward + ee_reward + com_reward
return reward
def solve(self, desComPos, cmW=1., posW=1., oriW=1.):
"""
:type desComPos: np.array
:type cmW: float
:type posW: float
:type oriW: float
:return:
"""
numItr = 1000
dt = .05
threshold = 0.00001
totalDOF = self.model.getTotalDOF()
for i in range(numItr):
Jc_IK = self.getConstJacobian(self.model)
dv_IK = cmW * (desComPos - self.model.getCOM())
for j in range(len(self.bodyIdx)):
ori_IK = self.model.getBodyOrientationGlobal(self.bodyIdx[j])
pos_IK = self.model.getBody(self.bodyIdx[j]).to_world(
self.localPos[j])
dv_IK_tmp = posW * (self.desPos[j] - pos_IK) # type: np.array
for k in range(3):
if self.desPosMask[j][k]:
dv_IK = np.append(dv_IK, dv_IK_tmp[k])
if self.desPosMask[j][3]:
dv_IK = np.append(
dv_IK,
oriW * mm.logSO3(np.dot(self.desOri[j], ori_IK.T)))
dth_IK_solve = npl.lstsq(Jc_IK, dv_IK)
dth_IK_x = dth_IK_solve[0][:totalDOF]
self.model.skeleton.q += dt * dth_IK_x
# ype.nested(dth_IK_x, dth_IK)
# th_IK = yct.getIntegralDOF(th_r_IK, dth_IK, dt)
# self.pose.setDOFPositions(th_IK)
if np.dot(dv_IK, dv_IK) < threshold:
break
def get_deep_state(self):
R_pelvis = self.getBody(0).world_transform()[:3, :3]
com = self.getCOM()
state = [
self.getBody(i).to_world() - com for i in range(self.body_num)
]
R = [
mm.logSO3(
np.dot(R_pelvis.T,
self.getBody(i).world_transform()[:3, :3])) / math.pi
for i in range(self.body_num)
]
v = [
self.getBody(i).world_linear_velocity()
for i in range(self.body_num)
]
w = [
self.getBody(i).world_angular_velocity()
for i in range(self.body_num)
]
state.extend(R)
state.extend(v)
state.extend(w)
return np.asarray(state).flatten()
def drawSO3(SO3, origin=numpy.array([0,0,0]), color=(0,255,0), lineWidth=1.0, name=''):
glEnable(GL_LINE_STIPPLE)
glLineStipple(2, 0xFAFA)
drawVector(mmMath.logSO3(SO3), origin, color, lineWidth)
glDisable(GL_LINE_STIPPLE)
def update(self):
self.cur = mm.seq2Vec3(mm.logSO3(self.body.world_transform()[:3, :3]))
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 preprocess(SEGMENT_FOOT=False):
# dir = './icmotion_test/'
# paths = glob.glob(dir+'*.temp')
dir = './ppmotion/'
# paths = glob.glob(dir+'*.bvh')
paths = None
if SEGMENT_FOOT:
paths = glob.glob(dir+'segfoot_wd2_WalkForwardNormal00.bvh')
paths = glob.glob(dir+'segfoot_wd2_WalkForwardNormal00_REPEATED.bvh')
else:
paths = glob.glob(dir+'wd2_WalkForwardNormal00.bvh')
paths = glob.glob(dir+'wd2_WalkForwardNormal00_REPEATED.bvh')
# paths = glob.glob(dir+'wd2_WalkForwardVFast00.bvh')
'''
paths = glob.glob(dir+'wd2_WalkSameSame01.bvh')
paths = glob.glob(dir+'wd2_u-turn_1.bvh')
paths = glob.glob(dir+'wd2_cross_walk*.bvh')
paths = glob.glob(dir+'*_REPEATED.bvh')
paths = glob.glob(dir+'wd2_pick_walk_1.bvh')
paths = glob.glob(dir+'wd2_WalkSameSame01_REPEATED_FOOT.bvh')
paths = glob.glob(dir+'wd2_WalkForwardSlow01_REPEATED_FOOT.bvh')
paths = glob.glob(dir+'wd2_WalkForwardNormal01_REPEATED_FOOT.bvh')
paths = glob.glob(dir+'wd2_WalkSoldier00_REPEATED_FOOT.bvh')
'''
hRef = .1; vRef = .4
## hRef = .1; vRef = .2
# dir = './ppmotion_long/'
# paths = glob.glob(dir+'wd2_WalkBackward00_REPEATED.bvh')
# hRef = 10.; vRef = .4
# dir = './rawmotion_slope/'
# paths = glob.glob(dir+'*.bvh')
## paths = [dir+'woddy2_walk_normal_to_slope.bvh']
# hRef = 10000.; vRef = .4*100
#
# dir = './ppmotion_slope/'
# paths = glob.glob(dir+'*.bvh')
# hRef = 10000.; vRef = .2
jumpThreshold = 15; jumpBias = 1.
stopThreshold = 15; stopBias = 0.
for path in paths:
motion_ori = yf.readBvhFile(path)
# informations
skeleton = motion_ori[0].skeleton
lFoot = skeleton.getJointIndex('LeftFoot'); rFoot = skeleton.getJointIndex('RightFoot')
lHip = skeleton.getJointIndex('LeftUpLeg'); rHip = skeleton.getJointIndex('RightUpLeg')
lKnee = skeleton.getJointIndex('LeftLeg'); rKnee = skeleton.getJointIndex('RightLeg')
lFoot = skeleton.getJointIndex('LeftFoot'); rFoot = skeleton.getJointIndex('RightFoot')
mcfgfile = open(dir + 'mcfg', 'rb')
mcfg = pickle.load(mcfgfile)
mcfgfile.close()
wcfg = ypc.WorldConfig()
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion_ori[0], mcfg)
# bodyMasses = getBodyMasses()
bodyMasses = motionModel.getBodyMasses()
uppers = [skeleton.getJointIndex(name) for name in ['Hips', 'Spine', 'Spine1', 'LeftArm', 'LeftForeArm', 'RightArm', 'RightForeArm']]
upperMass = sum([bodyMasses[i] for i in uppers])
lc = yma.getElementContactStates(motion_ori, 'LeftFoot', hRef, vRef)
rc = yma.getElementContactStates(motion_ori, 'RightFoot', hRef, vRef)
# intervals, states = yba.getBipedGaitIntervals(lc, rc, jumpThreshold, jumpBias, stopThreshold, stopBias)
intervals, states = yba.getBipedGaitIntervals2(lc, rc, jumpThreshold, jumpBias, stopThreshold, stopBias)
seginfos = [{} for i in range(len(intervals))]
for i in range(len(intervals)):
start = intervals[i][0]; end = intervals[i][1]
seginfos[i]['interval'] = intervals[i]
seginfos[i]['state'] = states[i]
# print yba.GaitState.text[states[i]], intervals[i]
stanceHips = []; swingHips = []; stanceFoots = []; swingFoots = []; swingKnees = []
if states[i]==yba.GaitState.LSWING: stanceHips = [rHip]; stanceFoots = [rFoot]; swingHips = [lHip]; swingFoots = [lFoot]; swingKnees = [lKnee]
elif states[i]==yba.GaitState.RSWING: stanceHips = [lHip]; stanceFoots = [lFoot]; swingHips = [rHip]; swingFoots = [rFoot]; swingKnees = [rKnee]
elif states[i]==yba.GaitState.STOP: stanceHips = [rHip, lHip]; stanceFoots = [rFoot, lFoot]
elif states[i]==yba.GaitState.JUMP: swingHips = [rHip, lHip]; swingFoots = [rFoot, lFoot]
seginfos[i]['stanceHips'] = stanceHips
seginfos[i]['swingHips'] = swingHips
seginfos[i]['stanceFoots'] = stanceFoots
seginfos[i]['swingFoots'] = swingFoots
seginfos[i]['swingKnees'] = swingKnees
if start<end:
if SEGMENT_FOOT:
# segmented foot
seginfos[i]['ground_height'] = min([posture_seg.getJointPositionGlobal(foot)[1] - 0.05 for foot in [lFoot, rFoot] for posture_seg in motion_ori[start+1:end+1]])
else:
# box foot
seginfos[i]['ground_height'] = min([posture_seg.getJointPositionGlobal(foot)[1] for foot in [lFoot, rFoot] for posture_seg in motion_ori[start+1:end+1]])
seginfos[i]['max_stf_push_frame'] = None
if len(swingFoots) > 0:
pushes = []
for frame in range(start, int((start+end)//2) + 1):
dCM_tar = yrp.getCM(motion_ori.getJointVelocitiesGlobal(frame), bodyMasses, None, uppers)
direction = mm.normalize2(mm.projectionOnPlane(dCM_tar, (1,0,0), (0,0,1)))
directionAxis = np.cross((0,1,0), direction)
pushes.append(mm.componentOnVector(mm.logSO3(motion_ori[frame].getJointOrientationFromParentGlobal(swingFoots[0])), directionAxis))
seginfos[i]['max_stf_push_frame'] = pushes.index(max(pushes))
# write .seg
inputName = os.path.basename(path)
root = os.path.splitext(inputName)[0]
outputName = root+'.seg'
outputFile = open(dir+outputName, 'wb')
pickle.dump(seginfos, outputFile)
outputFile.close()
print(outputName, 'done')
pprint.pprint(seginfos)
print('FINISHED')
def step_model(self):
contacts, points, angles, orientations, root_orientation = self.controller.step(self.get_target())
# pairs = [[0,11,3,4],
# [0,8,10,2],
# [0,13,6,7],
# [0,9,12,5],
# [0,1]]
pairs = [[0,18,11,3,4],
[0,14,8,10,2],
[0,19,13,6,7],
[0,14,9,12,5],
[0,14,17,1]]
self.lines = []
for pair in pairs:
for i in range(len(pair)-1):
self.lines.append([points[pair[i]], points[pair[i+1]]])
# print(len(orientations))
for i in range(len(angles)):
self.all_angles[i].append(angles[i])
for j in range(len(self.model.joints)):
if j == 0:
joint = self.model.joints[j] # type: pydart.FreeJoint
joint_idx = joint_list.index(joint.name)
hip_angles = mm.logSO3(np.dot(root_orientation, orientations[joint_idx]))
# hip_angles = mm.logSO3(root_orientation)
joint.set_position(np.array([hip_angles[0], hip_angles[1], hip_angles[2], points[0][0], points[0][1], points[0][2]]))
continue
joint = self.model.joints[j] # type: pydart.BallJoint
joint_idx = joint_list.index(joint.name)
joint.set_position(angles[joint_idx*3:joint_idx*3+3])
self.ik.clean_constraints()
self.ik.add_joint_pos_const('LeftForeArm', np.asarray(points[10]))
self.ik.add_joint_pos_const('LeftHand', np.asarray(points[2]))
self.ik.add_joint_pos_const('LeftLeg', np.asarray(points[11]))
self.ik.add_joint_pos_const('LeftFoot', np.asarray(points[3]))
if contacts[0] > 0.8 and False:
body_transform = self.model.body('LeftFoot').transform()[:3, :3]
angle = math.acos(body_transform[1, 1])
body_ori = np.dot(body_transform, mm.rotX(-angle))
self.ik.add_orientation_const('LeftFoot', body_ori)
self.ik.add_joint_pos_const('RightForeArm', np.asarray(points[12]))
self.ik.add_joint_pos_const('RightHand', np.asarray(points[5]))
self.ik.add_joint_pos_const('RightLeg', np.asarray(points[13]))
self.ik.add_joint_pos_const('RightFoot', np.asarray(points[6]))
self.ik.solve()
foot_joint_ori = mm.exp(self.model.joint('LeftFoot').position())
self.model.joint('LeftFoot').set_position(mm.logSO3(np.dot(foot_joint_ori, np.dot(mm.rotX(-.6), mm.rotZ(.4)))))
foot_joint_ori = mm.exp(self.model.joint('RightFoot').position())
self.model.joint('RightFoot').set_position(mm.logSO3(np.dot(foot_joint_ori, np.dot(mm.rotX(-.6), mm.rotZ(-.4)))))
left_foot = self.model.body('LeftFoot')
if (left_foot.to_world([0.05, -0.045, 0.1125])[1] < 0. or left_foot.to_world([-0.05, -0.045, 0.1125])[1] < 0.) \
and (left_foot.to_world([0.05, -0.045, -0.1125])[1] < 0. or left_foot.to_world([-0.05, -0.045, -0.1125])[1] < 0.):
left_toe_pos1 = left_foot.to_world([0.05, -0.045, +0.1125])
left_toe_pos1[1] = 0.
left_toe_pos2 = left_foot.to_world([-0.05, -0.045, +0.1125])
left_toe_pos2[1] = 0.
left_heel_pos1 = left_foot.to_world([0.05, -0.045, -0.1125])
left_heel_pos1[1] = 0.
left_heel_pos2 = left_foot.to_world([-0.05, -0.045, -0.1125])
left_heel_pos2[1] = 0.
self.ik.clean_constraints()
self.ik.add_position_const('LeftFoot', left_toe_pos1, np.array([0.05, -0.045, +0.1125]))
self.ik.add_position_const('LeftFoot', left_toe_pos2, np.array([-0.05, -0.045, +0.1125]))
self.ik.add_position_const('LeftFoot', left_heel_pos1, np.array([0.05, -0.045, -0.1125]))
self.ik.add_position_const('LeftFoot', left_heel_pos2, np.array([-0.05, -0.045, -0.1125]))
self.ik.solve()
right_foot = self.model.body('RightFoot')
if (right_foot.to_world([0.05, -0.045, 0.1125])[1] < 0. or right_foot.to_world([-0.05, -0.045, 0.1125])[1] < 0.) \
and (right_foot.to_world([0.05, -0.045, -0.1125])[1] < 0. or right_foot.to_world([-0.05, -0.045, -0.1125])[1] < 0.):
right_toe_pos1 = right_foot.to_world([0.05, -0.045, +0.1125])
right_toe_pos1[1] = 0.
right_toe_pos2 = right_foot.to_world([-0.05, -0.045, +0.1125])
right_toe_pos2[1] = 0.
right_heel_pos1 = right_foot.to_world([0.05, -0.045, -0.1125])
right_heel_pos1[1] = 0.
right_heel_pos2 = right_foot.to_world([-0.05, -0.045, -0.1125])
right_heel_pos2[1] = 0.
self.ik.clean_constraints()
self.ik.add_position_const('RightFoot', right_toe_pos1, np.array([0.05, -0.045, +0.1125]))
self.ik.add_position_const('RightFoot', right_toe_pos2, np.array([-0.05, -0.045, +0.1125]))
self.ik.add_position_const('RightFoot', right_heel_pos1, np.array([0.05, -0.045, -0.1125]))
self.ik.add_position_const('RightFoot', right_heel_pos2, np.array([-0.05, -0.045, -0.1125]))
self.ik.solve()
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 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)
def SE32vlogSO3(T):
v = np.append(T[0:3, 3].flatten(), mm.logSO3(T[0:3, 0:3]))
return v
def get_rnn_ref_pose_step(self, reset=False):
if not reset:
self.prev_ref_q = self.ref_skel.positions()
self.prev_ref_dq = self.ref_skel.velocities()
self.prev_ref_p_e_hat = np.asarray([
body.world_transform()[:3, 3] for body in self.ref_body_e
]).flatten()
self.prev_ref_com = self.ref_skel.com()
p = self.goal_in_world_frame
target = Pose2d(
[p[0] / self.RNN_MOTION_SCALE, -p[2] / self.RNN_MOTION_SCALE])
target = self.rnn.pose.relativePose(target)
target = target.p
t_len = v_len(target)
if t_len > 80:
ratio = 80 / t_len
target[0] *= ratio
target[1] *= ratio
contacts, points, angles, orientations, root_orientation = self.rnn.step(
target)
for j in range(len(self.ref_skel.joints)):
if j == 0:
joint = self.ref_skel.joints[j] # type: pydart.FreeJoint
joint_idx = self.rnn_joint_list.index(joint.name)
hip_angles = mm.logSO3(
np.dot(root_orientation, orientations[joint_idx]))
# hip_angles = mm.logSO3(root_orientation)
joint.set_position(
np.array([
hip_angles[0], hip_angles[1], hip_angles[2],
points[0][0], points[0][1], points[0][2]
]))
continue
joint = self.ref_skel.joints[j] # type: pydart.BallJoint
joint_idx = self.rnn_joint_list.index(joint.name)
joint.set_position(angles[joint_idx * 3:joint_idx * 3 + 3])
self.ik.clean_constraints()
self.ik.add_joint_pos_const('LeftForeArm', np.asarray(points[10]))
self.ik.add_joint_pos_const('LeftHand', np.asarray(points[2]))
self.ik.add_joint_pos_const('LeftLeg', np.asarray(points[11]))
self.ik.add_joint_pos_const('LeftFoot', np.asarray(points[3]))
if contacts[0] > 0.8 and False:
body_transform = self.ref_skel.body('LeftFoot').transform()[:3, :3]
angle = acos(body_transform[1, 1])
body_ori = np.dot(body_transform, mm.rotX(-angle))
self.ik.add_orientation_const('LeftFoot', body_ori)
self.ik.add_joint_pos_const('RightForeArm', np.asarray(points[12]))
self.ik.add_joint_pos_const('RightHand', np.asarray(points[5]))
self.ik.add_joint_pos_const('RightLeg', np.asarray(points[13]))
self.ik.add_joint_pos_const('RightFoot', np.asarray(points[6]))
self.ik.solve()
foot_joint_ori = mm.exp(self.ref_skel.joint('LeftFoot').position())
self.ref_skel.joint('LeftFoot').set_position(
mm.logSO3(np.dot(foot_joint_ori, np.dot(mm.rotX(-.6),
mm.rotZ(.4)))))
foot_joint_ori = mm.exp(self.ref_skel.joint('RightFoot').position())
self.ref_skel.joint('RightFoot').set_position(
mm.logSO3(
np.dot(foot_joint_ori, np.dot(mm.rotX(-.6), mm.rotZ(-.4)))))
if not self.first:
dq = 30. * self.ref_skel.position_differences(
self.ref_skel.positions(), self.prev_ref_q)
self.ref_skel.set_velocities(dq)
if reset:
self.prev_ref_q = self.ref_skel.positions()
self.prev_ref_dq = self.ref_skel.velocities()
self.prev_ref_p_e_hat = np.asarray([
body.world_transform()[:3, 3] for body in self.ref_body_e
]).flatten()
self.prev_ref_com = self.ref_skel.com()
def get_rnn_ref_pose_step(self):
p = self.goal_in_world_frame
target = Pose2d(
[p[0] / self.RNN_MOTION_SCALE, -p[2] / self.RNN_MOTION_SCALE])
target = self.rnn.pose.relativePose(target)
target = target.p
t_len = v_len(target)
if t_len > 80:
ratio = 80 / t_len
target[0] *= ratio
target[1] *= ratio
contacts, points, angles, orientations, root_orientation = self.rnn.step(
target)
for j in range(len(self.ik_skel.joints)):
if j == 0:
joint = self.ik_skel.joints[j] # type: pydart.FreeJoint
joint_idx = self.rnn_joint_list.index(joint.name)
hip_angles = mm.logSO3(
np.dot(root_orientation, orientations[joint_idx]))
# hip_angles = mm.logSO3(root_orientation)
joint.set_position(
np.array([
hip_angles[0], hip_angles[1], hip_angles[2],
points[0][0], points[0][1], points[0][2]
]))
continue
joint = self.ik_skel.joints[j] # type: pydart.BallJoint
joint_idx = self.rnn_joint_list.index(joint.name)
joint.set_position(angles[joint_idx * 3:joint_idx * 3 + 3])
self.ik.clean_constraints()
self.ik.add_joint_pos_const('Hips', np.asarray(points[0]))
self.ik.add_joint_pos_const('LeftForeArm', np.asarray(points[10]))
self.ik.add_joint_pos_const('LeftHand', np.asarray(points[2]))
self.ik.add_joint_pos_const('LeftLeg', np.asarray(points[11]))
self.ik.add_joint_pos_const('LeftFoot', np.asarray(points[3]))
if contacts[0] > 0.8 and False:
body_transform = self.ik_skel.body('LeftFoot').transform()[:3, :3]
angle = acos(body_transform[1, 1])
body_ori = np.dot(body_transform, mm.rotX(-angle))
self.ik.add_orientation_const('LeftFoot', body_ori)
self.ik.add_joint_pos_const('RightForeArm', np.asarray(points[12]))
self.ik.add_joint_pos_const('RightHand', np.asarray(points[5]))
self.ik.add_joint_pos_const('RightLeg', np.asarray(points[13]))
self.ik.add_joint_pos_const('RightFoot', np.asarray(points[6]))
self.ik.add_joint_pos_const('Neck1', np.asarray(points[17]))
self.ik.solve()
foot_joint_ori = mm.exp(self.ik_skel.joint('LeftFoot').position())
self.ik_skel.joint('LeftFoot').set_position(
mm.logSO3(np.dot(foot_joint_ori, np.dot(mm.rotX(-.6),
mm.rotZ(.4)))))
foot_joint_ori = mm.exp(self.ik_skel.joint('RightFoot').position())
self.ik_skel.joint('RightFoot').set_position(
mm.logSO3(
np.dot(foot_joint_ori, np.dot(mm.rotX(-.6), mm.rotZ(-.4)))))
left_foot = self.ik_skel.body('LeftFoot')
if (left_foot.to_world([0.05, -0.045, 0.1125])[1] < 0. or left_foot.to_world([-0.05, -0.045, 0.1125])[1] < 0.) \
and (left_foot.to_world([0.05, -0.045, -0.1125])[1] < 0. or left_foot.to_world([-0.05, -0.045, -0.1125])[1] < 0.):
left_toe_pos1 = left_foot.to_world([0.05, -0.045, +0.1125])
left_toe_pos1[1] = 0.
left_toe_pos2 = left_foot.to_world([-0.05, -0.045, +0.1125])
left_toe_pos2[1] = 0.
left_heel_pos1 = left_foot.to_world([0.05, -0.045, -0.1125])
left_heel_pos1[1] = 0.
left_heel_pos2 = left_foot.to_world([-0.05, -0.045, -0.1125])
left_heel_pos2[1] = 0.
self.ik.clean_constraints()
self.ik.add_position_const('LeftFoot', left_toe_pos1,
np.array([0.05, -0.045, +0.1125]))
self.ik.add_position_const('LeftFoot', left_toe_pos2,
np.array([-0.05, -0.045, +0.1125]))
self.ik.add_position_const('LeftFoot', left_heel_pos1,
np.array([0.05, -0.045, -0.1125]))
self.ik.add_position_const('LeftFoot', left_heel_pos2,
np.array([-0.05, -0.045, -0.1125]))
self.ik.solve()
right_foot = self.ik_skel.body('RightFoot')
if (right_foot.to_world([0.05, -0.045, 0.1125])[1] < 0. or right_foot.to_world([-0.05, -0.045, 0.1125])[1] < 0.) \
and (right_foot.to_world([0.05, -0.045, -0.1125])[1] < 0. or right_foot.to_world([-0.05, -0.045, -0.1125])[1] < 0.):
right_toe_pos1 = right_foot.to_world([0.05, -0.045, +0.1125])
right_toe_pos1[1] = 0.
right_toe_pos2 = right_foot.to_world([-0.05, -0.045, +0.1125])
right_toe_pos2[1] = 0.
right_heel_pos1 = right_foot.to_world([0.05, -0.045, -0.1125])
right_heel_pos1[1] = 0.
right_heel_pos2 = right_foot.to_world([-0.05, -0.045, -0.1125])
right_heel_pos2[1] = 0.
self.ik.clean_constraints()
self.ik.add_position_const('RightFoot', right_toe_pos1,
np.array([0.05, -0.045, +0.1125]))
self.ik.add_position_const('RightFoot', right_toe_pos2,
np.array([-0.05, -0.045, +0.1125]))
self.ik.add_position_const('RightFoot', right_heel_pos1,
np.array([0.05, -0.045, -0.1125]))
self.ik.add_position_const('RightFoot', right_heel_pos2,
np.array([-0.05, -0.045, -0.1125]))
self.ik.solve()
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)
def getDesiredDOFAccelerations(th_r,
th,
dth_r,
dth,
ddth_r,
Kt,
Dt,
weightMap=None):
ddth_des = [None] * len(th_r) # type: list[np.ndarray]
# p_r0 = mm.T2p(th_r[0])
# p0 = mm.T2p(th[0])
# v_r0 = dth_r[0][0:3]
# v0 = dth[0][0:3]
# a_r0 = ddth_r[0][0:3]
# th_r0 = mm.T2R(th_r[0])
# th0 = mm.T2R(th[0])
# dth_r0 = dth_r[0][3:6]
# dth0 = dth[0][3:6]
# ddth_r0 = ddth_r[0][3:6]
p_r0 = th_r[0][0]
p0 = th[0][0]
# v_r0 = dth_r[0][0:3]
v0 = dth[0][0:3]
# a_r0 = ddth_r[0][0:3]
th_r0 = th_r[0][1]
th0 = th[0][1]
# dth_r0 = dth_r[0][3:6]
dth0 = dth[0][3:6]
# ddth_r0 = ddth_r[0][3:6]
kt = Kt
dt = Dt
if weightMap is not None:
kt = Kt * weightMap[0]
dt = Dt * (weightMap[0]**.5)
# dt = 0.
# a_des0 = kt*(p_r0 - p0) + dt*(v_r0 - v0) + a_r0
# ddth_des0 = kt*(mm.logSO3(np.dot(th0.transpose(), th_r0))) + dt*(dth_r0 - dth0) + ddth_r0
a_des0 = kt * (p_r0 - p0) + dt * (-v0) #+ a_r0
ddth_des0 = kt * (mm.logSO3(np.dot(th0.transpose(), th_r0))) + dt * (
-dth0) #+ ddth_r0
ddth_des[0] = np.concatenate((a_des0, ddth_des0))
for i in range(1, len(th_r)):
if weightMap is not None:
kt = Kt * weightMap[i]
dt = Dt * (weightMap[i]**.5)
# dt = 0.
# ddth_des[i] = kt*(mm.logSO3(np.dot(th[i].transpose(), th_r[i]))) + dt*(dth_r[i] - dth[i]) #+ ddth_r[i]
if th[i].shape[0] == 3:
ddth_des[i] = kt * (mm.logSO3(np.dot(
th[i].transpose(), th_r[i]))) + dt * (-dth[i]) #+ ddth_r[i]
elif th[i].shape[0] > 0:
ddth_des[i] = kt * (mm.logSO3(np.dot(
th[i].transpose(), th_r[i]))) + dt * (-dth[i]) #+ ddth_r[i]
else:
ddth_des[i] = np.zeros(0)
return ddth_des
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 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 main():
# np.set_printoptions(precision=4, linewidth=200)
np.set_printoptions(precision=5, threshold=np.inf, suppress=True, linewidth=3000)
motionFile = 'wd2_tiptoe_zygote.bvh'
# motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(motionFile, SEGMENT_FOOT_RAD=0.008)
motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(motionFile, SEGMENT_FOOT_MAG=0.01, SEGMENT_FOOT_RAD=0.008)
# motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped()
vpWorld = cvw.VpWorld(wcfg)
vpWorld.SetGlobalDamping(0.999)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
# controlModel_shadow_for_ik = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
# controlToMotionOffset = (1.5, -0.02, 0)
controlToMotionOffset = (1.5, 0., 0)
controlModel.translateByOffset(controlToMotionOffset)
# controlModel_shadow_for_ik.set_q(controlModel.get_q())
# controlModel_shadow_for_ik.computeJacobian(0, np.array([0., 0., 0.]))
wcfg_ik = copy.deepcopy(wcfg)
vpWorld_ik = cvw.VpWorld(wcfg_ik)
controlModel_ik = cvm.VpControlModel(vpWorld_ik, motion[0], mcfg)
vpWorld_ik.initialize()
controlModel_ik.set_q(np.zeros_like(controlModel.get_q()))
print('<?xml version="1.0" ?>')
print('<skel version="1.0">')
print('<world name="world 1">')
print(' <physics>')
print(' <time_step>0.001</time_step>')
print(' <gravity>0 -9.81 0</gravity>')
print(' <collision_detector>fcl_mesh</collision_detector>')
print(' </physics>')
print(' <skeleton name="grount skeleton">')
print(' <mobile>false</mobile>')
print(' <body name="ground">')
print(' <transformation>0 -0.025 0 0 0 0</transformation>')
print(' <visualization_shape name=" - visual - 0">')
print(' <transformation>0.0 0.0 0.0 0.0 0.0 0.0 </transformation>')
print(' <geometry>')
print(' <box>')
print(' <size>10000.0 0.05 10000.0</size>')
print(' </box>')
print(' </geometry>')
print(' </visualization_shape>')
print(' <collision_shape name=" - collision - 0">')
print(' <transformation>0.0 0.0 0.0 0.0 0.0 0.0 </transformation>')
print(' <geometry>')
print(' <box>')
print(' <size>100.0 0.05 100.0</size>')
print(' </box>')
print(' </geometry>')
print(' </collision_shape>')
print(' </body>')
print(' <joint name="joint 1" type="free">')
print(' <parent>world</parent>')
print(' <child>ground</child>')
print(' </joint>')
print(' </skeleton>')
print(' <skeleton name="dartModel">')
for i in range(controlModel.getJointNum()):
body_name = controlModel_ik.index2name(i)
print(' <body name="'+body_name+'">')
print(' <transformation>'+str(np.concatenate((controlModel_ik.getBodyPositionGlobal(i), mm.logSO3(controlModel_ik.getBodyOrientationGlobal(i)))))[1:-1]+'</transformation>')
print(' <inertia>')
print(' <mass>' + str(controlModel_ik.getBodyMass(i))+'</mass>')
print(' <offset>'+str(controlModel_ik.getBodyLocalCom(i))[1:-1]+'</offset>')
print(' </inertia>')
geom_types = controlModel_ik.getBodyGeomsType(i)
geom_sizes = controlModel_ik.getBodyGeomsSize(i)
geom_local_frames = controlModel_ik.getBodyGeomsLocalFrame(i)
geom_count = 0
for j in range(len(geom_types)):
if geom_types[j] == 'B':
print(' <visualization_shape name="'+body_name+' - visual shape '+str(geom_count)+'">')
print(' <transformation>'+str(np.concatenate((geom_local_frames[j][:3, 3], mm.logSO3(geom_local_frames[j][:3, :3]))))[1:-1]+'</transformation>')
print(' <geometry>')
print(' <box>')
print(' <size>'+str(geom_sizes[j])[1:-1]+'</size>')
print(' </box>')
print(' </geometry>')
print(' </visualization_shape>')
geom_count += 1
elif geom_types[j] in ('C', 'D', 'E', 'F'):
print(' <visualization_shape name="'+body_name+' - visual shape '+str(geom_count)+'">')
print(' <transformation>'+str(np.concatenate((geom_local_frames[j][:3, 3], mm.logSO3(geom_local_frames[j][:3, :3]))))[1:-1]+'</transformation>')
print(' <geometry>')
print(' <capsule>')
print(' <radius>'+str(geom_sizes[j][0])+'</radius>')
print(' <height>'+str(geom_sizes[j][1]-2.*geom_sizes[j][0])+'</height>')
print(' </capsule>')
print(' </geometry>')
print(' </visualization_shape>')
geom_count += 1
geom_count = 0
for j in range(len(geom_types)):
if geom_types[j] == 'B':
print(' <collision_shape name="'+body_name+' - collision shape '+str(geom_count)+'">')
print(' <transformation>'+str(np.concatenate((geom_local_frames[j][:3, 3], mm.logSO3(geom_local_frames[j][:3, :3]))))[1:-1]+'</transformation>')
print(' <geometry>')
print(' <box>')
print(' <size>'+str(geom_sizes[j])[1:-1]+'</size>')
print(' </box>')
print(' </geometry>')
print(' </collision_shape>')
geom_count += 1
if geom_types[j] in ('C', 'D'):
print(' <collision_shape name="'+body_name+' - collision shape '+str(geom_count)+'">')
print(' <transformation>'+str(np.concatenate((np.dot(geom_local_frames[j], mm.TransVToSE3((geom_sizes[j][1]/2.-geom_sizes[j][0])*mm.unitZ()))[:3, 3], mm.logSO3(geom_local_frames[j][:3, :3]))))[1:-1]+'</transformation>')
print(' <geometry>')
print(' <sphere>')
print(' <radius>'+str(geom_sizes[j][0])+'</radius>')
print(' </sphere>')
print(' </geometry>')
print(' </collision_shape>')
geom_count += 1
if geom_types[j] in ('C', 'F'):
print(' <collision_shape name="'+body_name+' - collision shape '+str(geom_count)+'">')
print(' <transformation>'+str(np.concatenate((np.dot(geom_local_frames[j], mm.TransVToSE3(-(geom_sizes[j][1]/2.-geom_sizes[j][0])*mm.unitZ()))[:3, 3], mm.logSO3(geom_local_frames[j][:3, :3]))))[1:-1]+'</transformation>')
print(' <geometry>')
print(' <sphere>')
print(' <radius>'+str(geom_sizes[j][0])+'</radius>')
print(' </sphere>')
print(' </geometry>')
print(' </collision_shape>')
geom_count += 1
print(' </body>')
for i in range(controlModel.getJointNum()):
name = controlModel_ik.index2name(i)
inv_bone_R, inv_bone_p = controlModel_ik.getInvBoneT(i)
if i == 0:
print(' <joint name="j_'+name+'" type="free">')
print(' <transformation>'+str(np.concatenate((inv_bone_p, mm.logSO3(inv_bone_R))))[1:-1]+'</transformation>')
print(' <parent>world</parent>')
print(' <child>'+name+'</child>')
print(' </joint>')
else:
parent_name = controlModel_ik.index2name(controlModel_ik.getParentIndex(i))
print(' <joint name="j_'+name+'" type="ball">')
print(' <transformation>'+str(np.concatenate((inv_bone_p, mm.logSO3(inv_bone_R))))[1:-1]+'</transformation>')
print(' <parent>'+parent_name+'</parent>')
print(' <child>'+name+'</child>')
print(' </joint>')
print(' </skeleton>')
print('</world>')
print('</skel>')
