def getIntegralDOF(th, d_th, dt):
new_th = [None] * len(th)
v_r0 = d_th[0][0:3]
v_r1 = d_th[0][3:6]
new_th0_l = th[0][0] + [v_r0[0] * dt, v_r0[1] * dt, v_r0[2] * dt]
new_th0_a = np.dot(th[0][1],
mm.exp([v_r1[0] * dt, v_r1[1] * dt, v_r1[2] * dt]))
new_th[0] = [None] * 2
new_th[0][0] = new_th0_l
new_th[0][1] = new_th0_a
for i in range(1, len(th)):
new_th[i] = np.dot(
th[i], mm.exp([d_th[i][0] * dt, d_th[i][1] * dt, d_th[i][2] * dt]))
return new_th
def extendByIntegration(motion, extendLength, preserveJoints=[], finiteDiff=1):
lastFrame = len(motion) - 1
p = motion.getJointPositionGlobal(0, lastFrame)
v = motion.getJointVelocityGlobal(0, lastFrame - finiteDiff, lastFrame)
a = motion.getJointAccelerationGlobal(0, lastFrame - finiteDiff, lastFrame)
ap = motion.getJointOrientationsLocal(lastFrame)
av = motion.getJointAngVelocitiesLocal(lastFrame - finiteDiff, lastFrame)
aa = motion.getJointAngAccelerationsLocal(lastFrame - finiteDiff,
lastFrame)
t = 1 / motion.fps
# integration
extended = ym.JointMotion(
[motion[0].getTPose() for i in range(extendLength)])
for i in range(extendLength):
p += v * t
v += a * t
ap = map(lambda R0, dR: np.dot(R0, mm.exp(t * dR)), ap, av)
av = map(lambda V0, dV: V0 + t * dV, av, aa)
extended[i].rootPos = p.copy()
extended.setJointOrientationsLocal(i, ap)
# preserve joint orientations
preserveJointOrientations = [
motion[-1].getJointOrientationGlobal(footJoint)
for footJoint in preserveJoints
]
for extendedPosture in extended:
for i in range(len(preserveJoints)):
extendedPosture.setJointOrientationGlobal(
preserveJoints[i], preserveJointOrientations[i])
return extended
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 transToSE3(trans):
v = Vec3(trans[0], trans[1], trans[2])
length = mm.length(trans[3:])
if length < LIE_EPS:
return SE3(v)
R = mm.exp(trans[3:] / length, length)
return SE3(R[0, 0], R[1, 0], R[2, 0], R[1, 0], R[1, 1], R[1, 2], R[2, 0],
R[2, 1], R[2, 2], trans[0], trans[1], trans[2])
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 addJointSO3FromBvhJoint(self,
jointPosture,
bvhJoint,
channelValues,
scale=1.0):
localR = mm.I_SO3()
local_t = mm.O_Vec3()
for channel in bvhJoint.channels:
if channel.channelType == 'XPOSITION':
# jointPosture.rootPos[0] = channelValues[channel.channelIndex]*scale
local_t[0] = channelValues[channel.channelIndex] * scale
elif channel.channelType == 'YPOSITION':
# jointPosture.rootPos[1] = channelValues[channel.channelIndex]*scale
local_t[1] = channelValues[channel.channelIndex] * scale
elif channel.channelType == 'ZPOSITION':
# jointPosture.rootPos[2] = channelValues[channel.channelIndex]*scale
local_t[2] = channelValues[channel.channelIndex] * scale
elif channel.channelType == 'XROTATION':
localR = numpy.dot(
localR,
mm.exp(mm.s2v((1, 0, 0)),
mm.deg2Rad(channelValues[channel.channelIndex])))
elif channel.channelType == 'YROTATION':
localR = numpy.dot(
localR,
mm.exp(mm.s2v((0, 1, 0)),
mm.deg2Rad(channelValues[channel.channelIndex])))
elif channel.channelType == 'ZROTATION':
localR = numpy.dot(
localR,
mm.exp(mm.s2v((0, 0, 1)),
mm.deg2Rad(channelValues[channel.channelIndex])))
# jointPosture.setLocalR(bvhJoint.name, localR)
jointPosture.setLocalR(
jointPosture.skeleton.getElementIndex(bvhJoint.name), localR)
jointPosture.setLocal_t(
jointPosture.skeleton.getElementIndex(bvhJoint.name), local_t)
for i in range(len(bvhJoint.children)):
self.addJointSO3FromBvhJoint(jointPosture, bvhJoint.children[i],
channelValues, scale)
def get_th_dart(skel):
ls = []
pyV = np.asarray(skel.q[3:6])
pyR = mm.exp(np.asarray(skel.q[:3]))
ls.append([pyV, pyR])
for i in range(1, len(skel.joints)):
joint = skel.joints[i]
if joint.num_dofs() > 0:
ls.append(joint.get_local_transform()[:3, :3])
return ls
def makeFourContactPos(posture,
jointNameOrIdx,
isLeftFoot=True,
isOutside=True):
"""
:type posture: ym.JointPosture
:type jointNameOrIdx: str | int
:return: np.array, np.array, np.array
"""
idx = jointNameOrIdx
if type(jointNameOrIdx) == str:
idx = posture.skeleton.getJointIndex(jointNameOrIdx)
insideOffset = SEGMENT_FOOT_MAG * np.array((0., 0., 2.5))
outsideOffset = SEGMENT_FOOT_MAG * np.array((1.2, 0., 2.5))
if isLeftFoot ^ isOutside:
# if it is not outside phalange,
outsideOffset[0] = -1.2 * SEGMENT_FOOT_MAG
origin = posture.getJointPositionGlobal(idx)
inside = posture.getJointPositionGlobal(idx, insideOffset)
outside = posture.getJointPositionGlobal(idx, outsideOffset)
length = SEGMENT_FOOT_MAG * 2.5
RotVec1_tmp1 = inside - origin
RotVec1_tmp2 = inside - origin
RotVec1_tmp2[1] = 0.
RotVec1 = np.cross(RotVec1_tmp1, RotVec1_tmp2)
angle1_1 = math.acos((origin[1] - SEGMENT_FOOT_RAD) / length)
angle1_2 = math.acos((origin[1] - inside[1]) / length)
footRot1 = mm.exp(RotVec1, angle1_1 - angle1_2)
footOri1 = posture.getJointOrientationGlobal(idx)
posture.setJointOrientationGlobal(idx, np.dot(footRot1, footOri1))
inside_new = posture.getJointPositionGlobal(idx, insideOffset)
outside_new_tmp = posture.getJointPositionGlobal(idx, outsideOffset)
# RotVec2 = inside_new - origin
width = np.linalg.norm(outside - inside)
widthVec_tmp = np.cross(RotVec1_tmp1, np.array((0., 1., 0.))) if isLeftFoot ^ isOutside \
else np.cross(np.array((0., 1., 0.)), RotVec1_tmp1)
widthVec = width * widthVec_tmp / np.linalg.norm(widthVec_tmp)
outside_new = inside_new + widthVec
footRot2 = mm.getSO3FromVectors(outside_new_tmp - inside_new, widthVec)
footOri2 = posture.getJointOrientationGlobal(idx)
# print footRot2, footOri2
posture.setJointOrientationGlobal(idx, np.dot(footRot2, footOri2))
return
def continue_from_now_by_time(self, t, prev_t=0.):
# self.phase_frame = frame
# t = frame /self.ref_motion.fps
motion_q = self.ref_motion.get_q_by_time(t)
motion_ori = mm.exp(motion_q[:3])
attach_pos = np.zeros(3)
attach_ori = np.zeros(3)
motion_prev_q = self.ref_motion.get_q_by_time(prev_t)
motion_prev_ori = mm.exp(motion_prev_q[:3])
# attach current controlled character
skel_pelvis_offset = self.skel.joint(
0).position_in_world_frame() - motion_q[3:6]
# attach current motion
# skel_pelvis_offset = motion_prev_q[3:6] - motion_q[3:6]
skel_pelvis_offset[1] = 0.
self.ref_motion.translateByOffset(skel_pelvis_offset)
# attach current controlled character
skel_pelvis_x = self.skel.joint(0).orientation_in_world_frame()[:3, 0]
# attach current motion
# skel_pelvis_x = motion_prev_ori[:3, 0]
skel_pelvis_x[1] = 0.
motion_pelvis_x = motion_ori[:3, 0]
motion_pelvis_x[1] = 0.
# attach current controlled character
self.ref_motion.rotateTrajectory(
mm.getSO3FromVectors(motion_pelvis_x, skel_pelvis_x),
fixedPos=self.skel.joint(0).position_in_world_frame())
# attach current motion
# self.ref_motion.rotateTrajectory(mm.getSO3FromVectors(motion_pelvis_x, skel_pelvis_x), fixedPos=motion_prev_q[3:6])
self.time_offset = -self.world.time() + t
def step(self, _action):
"""Run one timestep of the environment's dynamics.
Accepts an action and returns a tuple (observation, reward, done, info).
# Arguments
action (object): An action provided by the environment.
# Returns
observation (object): Agent's observation of the current environment.
reward (float) : Amount of reward returned after previous action.
done (boolean): Whether the episode has ended, in which case further step() calls will return undefined results.
info (dict): Contains auxiliary diagnostic information (helpful for debugging, and sometimes learning).
"""
action = np.hstack((np.zeros(6), _action[:self.skel.ndofs - 6] / 10.))
Kp_joint = np.asarray([0.0] + [self.Kp] * self.skel.getJointNum())
Kd_joint = np.asarray([0.0] + [self.Kd] * self.skel.getJointNum())
for joint_idx in range(len(self.foot_joint)):
Kp_joint[1 + joint_idx] = self.Kd * exp(
log(self.Kp) * _action[self.skel.ndofs - 6 + joint_idx] / 10.)
Kd_joint[1 + joint_idx] = self.Kp * exp(
log(self.Kd) * _action[self.skel.ndofs - 6 + joint_idx] / 20.)
DOFs = self.skel.getDOFs()
action_nested = ype.makeNestedList(DOFs)
th_r = self.ref_skel.getDOFPositions()
ype.nested(action, action_nested)
for i in range(1, len(th_r)):
th_r[i] = np.dot(th_r[i], mm.exp(action_nested[i]))
th = self.skel.getDOFPositions()
dth = self.skel.getDOFVelocities()
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, None, dth, None,
Kp_joint, Kd_joint)
for i in range(self.step_per_frame):
bodyIDs, contactPositions, contactPositionLocals, contactForces = \
self.world.calcPenaltyForce(self.bodyIDsToCheck, self.mus, self.Ks, self.Ds)
self.world.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
self.skel.setDOFAccelerations(ddth_des)
self.world.step()
self.update_ref_skel(False)
return tuple([self.state(), self.reward(), self.is_done(), dict()])
def readTrcFile(trcFilePath, scale=1.0):
f = open(trcFilePath)
fileLines = f.readlines()
pointMotion = ym.Motion()
i = 0
while i != len(fileLines):
splited = fileLines[i].split()
boneNames = []
if i == 2:
dataRate = float(splited[0])
numFrames = int(splited[2])
numMarkers = int(splited[3])
# print numFrames, numMarkers
elif i == 3:
markerNames = [name.split(':')[1] for name in splited[2:]]
skeleton = ym.PointSkeleton()
for name in markerNames:
skeleton.addElement(None, name)
# print markerNames
elif i > 5:
markerPositions = splited[2:]
# print markerPositions
# print 'i', i
pointPosture = ym.PointPosture(skeleton)
for m in range(numMarkers):
point = numpy.array([
float(markerPositions[m * 3]),
float(markerPositions[m * 3 + 1]),
float(markerPositions[m * 3 + 2])
])
point = numpy.dot(
mm.exp(numpy.array([1, 0, 0]), -math.pi / 2.),
point) * scale
# pointPosture.addPoint(markerNames[m], point)
pointPosture.setPosition(m, point)
# print 'm', m
# print markerNames[m], (markerPositions[m*3],markerPositions[m*3+1],markerPositions[m*3+2])
pointMotion.append(pointPosture)
i += 1
f.close()
pointMotion.fps = dataRate
return pointMotion
def set_action(self, _action):
action = np.hstack((np.zeros(6), _action/10.))
th_action = ype.makeNestedList(self.skel.getDOFs())
ype.nested(action, th_action)
th_r = self.ref_motion.getDOFPositions(self.phase_frame)
th_des = [th_r[0]]
for i in range(1, len(th_r)):
th_des.append(np.dot(th_r[i], mm.exp(th_action[i])))
th = self.skel.getDOFPositions()
dth_r = self.ref_motion.getDOFVelocities(self.phase_frame)
dth = self.skel.getDOFVelocities()
ddth_r = self.ref_motion.getDOFAccelerations(self.phase_frame)
ddth_des = yct.getDesiredDOFAccelerations(th_des, th, dth_r, dth, ddth_r, self.Kp, self.Kd)
bodyIDsToCheck = list(range(self.world.getBodyNum()))
mus = [.5]*len(bodyIDsToCheck)
return ddth_des, bodyIDsToCheck, mus
def preFrameCallback_Always(frame):
# print(mm.rad2Deg(math.pi/6.*math.sin((frame-30)*math.pi/180.)))
if frame <= start_frame:
vpWorld.set_plane(0, mm.unitY(), np.zeros(3))
if frame > start_frame:
if math.sin((frame - start_frame) / 360. * math.pi) > 0.:
if frame < start_frame + 50:
setParamVal(
'com Z offset', 0.02 *
math.sin(2. * (frame - start_frame) / 360. * math.pi))
else:
setParamVal('com Z offset', 0.0)
if math.sin((frame - start_frame) / 360. * math.pi) > 0.:
foot_viewer.check_not_all_seg()
foot_viewer.check_tiptoe_all()
setParamVal(
'tiptoe angle',
mm.deg2Rad(10.) * math.sin(
(frame - start_frame) / 360. * math.pi))
# foot_viewer.check_h_l.value(False)
# foot_viewer.check_h_r.value(False)
else:
foot_viewer.check_all_seg()
# foot_viewer.check_tiptoe_all()
# foot_viewer.check_h_l.value(True)
# foot_viewer.check_h_r.value(True)
vpWorld.set_plane(
0,
np.dot(
mm.exp(
-mm.unitX(),
mm.deg2Rad(10.) * math.sin(
(frame - start_frame) / 360. * math.pi)),
mm.unitY()), np.zeros(3))
plane_list = vpWorld.get_plane_list()
plane_normal = plane_list[0][0]
plane_origin = plane_list[0][1]
viewer.motionViewWnd.glWindow.pOnPlaneshadow = plane_origin + plane_normal * 0.001
viewer.motionViewWnd.glWindow.normalshadow = plane_normal
def step(self, _action):
"""Run one timestep of the environment's dynamics.
Accepts an action and returns a tuple (observation, reward, done, info).
# Arguments
action (object): An action provided by the environment.
# Returns
observation (object): Agent's observation of the current environment.
reward (float) : Amount of reward returned after previous action.
done (boolean): Whether the episode has ended, in which case further step() calls will return undefined results.
info (dict): Contains auxiliary diagnostic information (helpful for debugging, and sometimes learning).
"""
action = np.hstack((np.zeros(6), _action/10.))
th_action = ype.makeNestedList(self.skel.getDOFs())
ype.nested(action, th_action)
th_r = self.ref_motion.getDOFPositions(self.phase_frame)
th_des = [th_r[0]]
for i in range(1, len(th_r)):
th_des.append(np.dot(th_r[i], mm.exp(th_action[i])))
th = self.skel.getDOFPositions()
dth_r = self.ref_motion.getDOFVelocities(self.phase_frame)
dth = self.skel.getDOFVelocities()
ddth_r = self.ref_motion.getDOFAccelerations(self.phase_frame)
ddth_des = yct.getDesiredDOFAccelerations(th_des, th, dth_r, dth, ddth_r, self.Kp, self.Kd)
bodyIDsToCheck = list(range(self.world.getBodyNum()))
mus = [.5]*len(bodyIDsToCheck)
for i in range(self.step_per_frame):
bodyIDs, contactPositions, contactPositionLocals, contactForces = self.world.calcPenaltyForce(bodyIDsToCheck, mus, self.Ks, self.Ds)
self.world.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces)
self.skel.setDOFAccelerations(ddth_des)
self.skel.solveHybridDynamics()
self.world.step()
self.update_ref_skel(False)
return tuple([self.state(), self.reward(), self.is_done(), dict()])
def step(self, target):
target = self.config.x_normal.normalize_l(target)
m = self.model
feed_dict = {m.x: [[target]], m.prev_y: self.current_y}
if self.state is not None:
feed_dict[m.initial_state] = self.state
# x : target x, target y => 2
# # y : foot contact=2, root transform(rotation, tx, ty)=3, root_height, joint pos=3*13=39 => 45
# y : foot contact=2, root transform(rotation, tx, ty)=3, root_height, joint pos=3*19=57 => 63
output, self.state, self.current_y = self.sess.run(
[m.generated, m.final_state, m.final_y], feed_dict)
output = output[0][0]
output = self.config.y_normal.de_normalize_l(output)
angles = copy(output[63:])
output = output[:63]
contacts = copy(output[:2])
output = output[2:]
# move root
self.pose = self.pose.transform(output)
root_orientation = mm.getSO3FromVectors(
(self.pose.d[0], 0., self.pose.d[1]), -mm.unitZ())
points = [[0, output[3], 0]]
output = output[4:]
for i in range(len(output) // 3):
points.append(output[i * 3:(i + 1) * 3])
point_offset = mm.seq2Vec3([0., -0.05, 0.])
for i in range(len(points)):
points[i] = mm.seq2Vec3(self.pose.global_point_3d(
points[i])) / 100. + point_offset
orientations = list()
for i in range(len(angles) // 3):
orientations.append(mm.exp(angles[i * 3:(i + 1) * 3]))
return contacts, points, angles, orientations, root_orientation
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 create_any_motion(motion):
#motion
yme.removeJoint(motion, 'Head', False)
#yme.removeJoint(motion, 'HEad', False)
yme.removeJoint(motion, 'RightShoulder', False)
yme.removeJoint(motion, 'LeftShoulder1', False)
yme.removeJoint(motion, 'RightToes_Effector', False)
yme.removeJoint(motion, 'LeftToes_Effector', False)
yme.removeJoint(motion, 'RightHand_Effector', False)
yme.removeJoint(motion, 'LeftHand_Effector', False)
yme.offsetJointLocal(motion, 'RightArm', (.03, -.05, 0), False)
yme.offsetJointLocal(motion, 'LeftArm', (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False)
#yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False)
#yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False)
yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1, -0.5, 0), -.45),
False)
yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1, 0.5, 0), -.45),
False)
yme.updateGlobalT(motion)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode('Hips')
node.length = .2
node.width = .25
node = mcfg.getNode('Spine1')
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode('Spine')
node.width = .22
node = mcfg.getNode('RightFoot')
node.length = .25
#node.length = .2
#node.width = .12
#node.width = .2
node.width = .15
node.mass = 2.
#node.mass = 1.
node = mcfg.getNode('LeftFoot')
node.length = .25
#node.length = .2
#node.width = .12
node.width = .15
#node.width = .2
node.mass = 2.
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 60
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
# parameter
config = {}
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':.2, 'Hips':0.5,\
'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3}
return mcfg, wcfg, stepsPerFrame, config
def create_walking_biped():
#motion
motionName = 'wd2_WalkForwardNormal00.bvh'
#motionName = '../motions/wd2_WalkForwardNormal00.bvh'
#motionName = '../motions/wd2_WalkForwardFast00.bvh'
#motionName = 'wd2_jump.bvh'
#motionName = 'wd2_stand.bvh'
motion = yf.readBvhFile(motionName, .01)
yme.removeJoint(motion, 'Head', False)
#yme.removeJoint(motion, 'HEad', False)
yme.removeJoint(motion, 'RightShoulder', False)
yme.removeJoint(motion, 'LeftShoulder1', False)
yme.removeJoint(motion, 'RightToes_Effector', False)
yme.removeJoint(motion, 'LeftToes_Effector', False)
yme.removeJoint(motion, 'RightHand_Effector', False)
yme.removeJoint(motion, 'LeftHand_Effector', False)
yme.offsetJointLocal(motion, 'RightArm', (.03, -.05, 0), False)
yme.offsetJointLocal(motion, 'LeftArm', (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False)
#yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False)
#yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False)
yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1, -0.5, 0), -.45),
False)
yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1, 0.5, 0), -.45),
False)
yme.updateGlobalT(motion)
motion.translateByOffset((0, 0.0, 0))
#motion = motion[40:-58]
#motion[0:0] = [motion[0]]*20
#motion.extend([motion[-1]]*5000)
motion = motion[40:]
#motion[0:0] = [motion[0]]*50
#motion.extend([motion[-1]]*100)
#motion.extend([motion[-1]]*100)
#motion = motion[30:151]
#motion = motion[30:]
#motion[5:5] = [motion[5]]*30
#motion[0:0] = [motion[0]]*100
#motion.extend([motion[-1]]*5000)
#motion = motion[40:41]
#motion[0:0] = [motion[0]]*5000
#motion = motion[56:-248]
#motion = motion[-249:-248]
#motion[0:0] = [motion[0]]*10
#motion.extend([motion[-1]]*5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode('Hips')
node.length = .2
node.width = .25
node = mcfg.getNode('Spine1')
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode('Spine')
node.width = .22
node = mcfg.getNode('RightFoot')
node.length = .25
#node.length = .2
node.width = .12
#node.width = .2
node.mass = 2.
#node.mass = 1.
node = mcfg.getNode('LeftFoot')
node.length = .25
#node.length = .2
node.width = .12
#node.width = .2
node.mass = 2.
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 60
#stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#wcfg.timeStep = (1/1000.)
# parameter
config = {}
'''
config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain
config['Kl'] = 2.5; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain
config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain
config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 2.5
config['Bh'] = 1.
'''
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':.2, 'Hips':0.5,\
#'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3}
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1., 'Spine1':1., 'RightFoot':.5, 'LeftFoot':.5, 'Hips':1.5,\
#'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.}
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1., 'Spine1':1., 'RightFoot':1.0, 'LeftFoot':1.0, 'Hips':1.5,\
#'RightUpLeg':2., 'RightLeg':2., 'LeftUpLeg':2., 'LeftLeg':2.}
config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':.2, 'Hips':0.5,\
'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3}
#success!!
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':.5, 'Spine1':.5, 'RightFoot':1., 'LeftFoot':1., 'Hips':0.5,\
#'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.}
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\
#'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5}
config['supLink'] = 'LeftFoot'
config['supLink1'] = 'LeftFoot'
config['supLink2'] = 'RightFoot'
#config['end'] = 'Hips'
config['end'] = 'Spine1'
return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped_zygote_two_seg():
#motion
motionName = 'wd2_tiptoe_zygote.bvh'
#motionName = 'wd2_jump.bvh'
#motionName = 'wd2_stand.bvh'
motion = yf.readBvhFile(motionName, .01)
# yme.removeJoint(motion, 'Head', False)
# yme.removeJoint(motion, 'Head', False)
# yme.removeJoint(motion, 'RightShoulder', False)
# yme.removeJoint(motion, 'LeftShoulder1', False)
# yme.removeJoint(motion, 'RightToes_Effector', False)
# yme.removeJoint(motion, 'LeftToes_Effector', False)
# yme.removeJoint(motion, 'RightHand_Effector', False)
# yme.removeJoint(motion, 'LeftHand_Effector', False)
# yme.offsetJointLocal(motion, 'RightArm', (.03,-.05,0), False)
# yme.offsetJointLocal(motion, 'LeftArm', (-.03,-.05,0), False)
yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False)
#yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False)
#yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False)
# yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.5,0), -.6), False)
# yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.5,0), -.6), False)
yme.removeJoint(motion, 'RightFoot_Effector', False)
yme.removeJoint(motion, 'LeftFoot_Effector', False)
yme.addJoint(motion, 'LeftFoot', 'LeftToes', [0., 0., 0.12], False)
yme.addJoint(motion, 'LeftToes', 'LeftToes_Effector', [0., 0., 0.07],
False)
yme.addJoint(motion, 'RightFoot', 'RightToes', [0., 0., 0.12], False)
yme.addJoint(motion, 'RightToes', 'RightToes_Effector', [0., 0., 0.07],
False)
yme.updateGlobalT(motion)
# motion.translateByOffset((0, -0.07, 0))
# motion.translateByOffset((0, -0.06, 0))
motion.extend([motion[-1]] * 300)
del motion[:270]
for i in range(2000):
motion.data.insert(0, copy.deepcopy(motion[0]))
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
# node = mcfg.getNode('Hips')
# node.length = .2
# node.width = .25
node = mcfg.getNode('Hips')
node.length = 4. / 27.
node.width = .25
node = mcfg.getNode('Spine1')
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode('Spine')
node.width = .22
node = mcfg.getNode('RightFoot')
node.length = .177
# node.length = .18
#node.length = .2
#node.width = .15
node.width = .1
node.mass = .8
node.offset = (0., 0., -0.02)
node = mcfg.getNode('LeftFoot')
node.length = .177
# node.length = .18
#node.length = .2
#node.width = .15
node.width = .1
node.mass = .8
node.offset = (0., 0., -0.02)
node = mcfg.getNode('RightToes')
node.length = .053
# node.length = .18
#node.length = .2
#node.width = .15
node.width = .1
node.mass = .218
# node.offset = (0,0,0.1)
node = mcfg.getNode('LeftToes')
node.length = .053
# node.length = .18
#node.length = .2
#node.width = .15
node.width = .1
node.mass = .218
# node.offset = (0,0,0.1)
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 60
#stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#wcfg.timeStep = (1/1000.)
# parameter
config = {}
'''
config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain
config['Kl'] = 2.5; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain
config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain
config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 2.5
config['Bh'] = 1.
'''
config['Kt'] = 200
config['Dt'] = 2. * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2. * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2. * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2. * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1., 'Spine1':1., 'RightFoot':.5, 'LeftFoot':.5, 'Hips':1.5,\
#'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.}
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1., 'Spine1':1., 'RightFoot':1.0, 'LeftFoot':1.0, 'Hips':1.5,\
#'RightUpLeg':2., 'RightLeg':2., 'LeftUpLeg':2., 'LeftLeg':2.}
config['weightMap'] = {
'RightArm': .2,
'RightForeArm': .2,
'LeftArm': .2,
'LeftForeArm': .2,
'Spine': .6,
'Spine1': .6,
'RightFoot': .2,
'LeftFoot': .2,
'Hips': 0.5,
'RightUpLeg': .1,
'RightLeg': .3,
'LeftUpLeg': .1,
'LeftLeg': .3,
'RightToes': .2,
'LeftToes': .2
}
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':1., 'Hips':0.5,\
#'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.5, 'LeftLeg':1.5}
#success!!
'''
config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':.5, 'Spine1':.5, 'RightFoot':1., 'LeftFoot':1., 'Hips':0.5,\
'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.}
'''
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\
#'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5}
config['supLink'] = 'LeftFoot'
config['supLink1'] = 'LeftFoot'
config['supLink2'] = 'RightFoot'
#config['end'] = 'Hips'
config['end'] = 'Spine1'
return motion, mcfg, wcfg, stepsPerFrame, config
config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':.2, 'Hips':0.5,\
'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3}
return mcfg, wcfg, stepsPerFrame, config
#===============================================================================
# biped config
#===============================================================================
# motion, mesh config
g_motionDirConfigMap = {}
g_motionDirConfigMap['../Data/woody2/Motion/Physics2/'] = \
{'footRot': mm.exp(mm.v3(1,0,0), .05), 'yOffset': .0, 'scale':1.,\
'rootRot': mm.I_SO3()}
g_motionDirConfigMap['../Data/woody2/Motion/Balancing/'] = \
{'footRot': mm.exp(mm.v3(1,-.5,0), -.6), 'yOffset': .0, 'scale':1.,\
'rootRot': mm.exp(mm.v3(1,0,0), .01)}
g_motionDirConfigMap['../Data/woody2/Motion/VideoMotion/'] = \
{'footRot': mm.exp(mm.v3(1,0,0), -.05), 'yOffset': .01, 'scale':2.53999905501,\
'rootRot': mm.exp(mm.v3(1,0,0), .0)}
g_motionDirConfigMap['../Data/woody2/Motion/Samsung/'] = \
{'footRot': mm.exp(mm.v3(1,0,0), -.03), 'yOffset': .0, 'scale':2.53999905501,\
'rootRot': mm.exp(mm.v3(1,0,0), .03)}
#===============================================================================
# # reloadable config
#===============================================================================
def ik_analytic(posture, joint_name_or_index, new_position):
if isinstance(joint_name_or_index, int):
joint = joint_name_or_index
else:
joint = posture.skeleton.getJointIndex(joint_name_or_index)
# joint_parent = posture.body.joint_parent[joint]
# joint_parent_parent = posture.body.joint_parent[joint_parent]
joint_parent = posture.skeleton.getParentJointIndex(joint)
joint_parent_parent = posture.skeleton.getParentJointIndex(joint_parent)
# B = posture.get_position(joint)
# C = posture.get_position(joint_parent)
# A = posture.get_position(joint_parent_parent)
B = posture.getJointPositionGlobal(joint)
C = posture.getJointPositionGlobal(joint_parent)
A = posture.getJointPositionGlobal(joint_parent_parent)
L = B - A
N = B - C
M = C - A
# l = mathlib.length(L);
# n = mathlib.length(N);
# m = mathlib.length(M);
l = mm.length(L)
n = mm.length(N)
m = mm.length(M)
# a = mathlib.ACOS((l*l + n*n - m*m) / (2*l*n))
# b = mathlib.ACOS((l*l + m*m - n*n) / (2*l*m))
a = mm.ACOS((l * l + n * n - m * m) / (2 * l * n))
b = mm.ACOS((l * l + m * m - n * n) / (2 * l * m))
B_new = new_position
L_new = B_new - A
# l_ = mathlib.length(L_new)
l_ = mm.length(L_new)
# a_ = mathlib.ACOS((l_*l_ + n*n - m*m) / (2*l_*n))
# b_ = mathlib.ACOS((l_*l_ + m*m - n*n) / (2*l_*m))
a_ = mm.ACOS((l_ * l_ + n * n - m * m) / (2 * l_ * n))
b_ = mm.ACOS((l_ * l_ + m * m - n * n) / (2 * l_ * m))
# rotate joint in plane
# rotV = mathlib.normalize(numpy.cross(M, L))
rotV = mm.normalize2(np.cross(M, L))
rotb = b - b_
rota = a_ - a - rotb
# posture.rotate_global_orientation(joint_parent_parent, mathlib.exp(rotV, rotb))
# posture.rotate_global_orientation(joint_parent, mathlib.exp(rotV * rota))
posture.mulJointOrientationGlobal(joint_parent_parent, mm.exp(rotV, rotb))
posture.mulJointOrientationGlobal(joint_parent, mm.exp(rotV * rota))
# rotate plane
# rotV2 = mathlib.normalize(numpy.cross(L, L_new))
# l_new = mathlib.length(L_new)
# l_diff = mathlib.length(L_new - L)
# rot2 = mathlib.ACOS((l_new * l_new + l * l - l_diff * l_diff) / (2 * l_new * l))
# posture.rotate_global_orientation(joint_parent_parent, mathlib.exp(rotV2, rot2))
rotV2 = mm.normalize2(np.cross(L, L_new))
l_new = mm.length(L_new)
l_diff = mm.length(L_new - L)
rot2 = mm.ACOS((l_new * l_new + l * l - l_diff * l_diff) / (2 * l_new * l))
posture.mulJointOrientationGlobal(joint_parent_parent, mm.exp(rotV2, rot2))
return posture
def simulateCallback(frame):
# print(frame)
# print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0']))
# hfi.footAdjust(motion[frame], idDic, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=.015, baseHeight=0.02)
if abs(getParamVal('tiptoe angle')) > 0.001:
tiptoe_angle = getParamVal('tiptoe angle')
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_0_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_0_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
# motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95))
# motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95))
# motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle))
# motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle))
if getParamVal('left tilt angle') > 0.001:
left_tilt_angle = getParamVal('left tilt angle')
if motion[0].skeleton.getJointIndex(
'LeftFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
else:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1_0'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot'], mm.exp(mm.unitZ(),
math.pi * left_tilt_angle))
elif getParamVal('left tilt angle') < -0.001:
left_tilt_angle = getParamVal('left tilt angle')
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_0'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
if motion[0].skeleton.getJointIndex(
'LeftFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1'],
mm.exp(mm.unitZ(), math.pi * left_tilt_angle))
else:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1_0'],
mm.exp(mm.unitZ(), math.pi * left_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot'], mm.exp(mm.unitZ(),
math.pi * left_tilt_angle))
if getParamVal('right tilt angle') > 0.001:
right_tilt_angle = getParamVal('right tilt angle')
if motion[0].skeleton.getJointIndex(
'RightFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
else:
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1_0'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot'],
mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
elif getParamVal('right tilt angle') < -0.001:
right_tilt_angle = getParamVal('right tilt angle')
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_0'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
if motion[0].skeleton.getJointIndex(
'RightFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1'],
mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
# else:
# motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot'],
mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
motionModel.update(motion[frame])
motionModel.translateByOffset(
np.array([
getParamVal('com X offset'),
getParamVal('com Y offset'),
getParamVal('com Z offset')
]))
controlModel_ik.set_q(controlModel.get_q())
global g_initFlag
global forceShowTime
global JsysPre
global JsupPreL
global JsupPreR
global JconstPre
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2 * (Kt**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
dt_sup = 2 * (kt_sup**.5)
# tracking
th_r = motion.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r,
Kt, Dt)
# ype.flatten(fix_dofs(DOFs, ddth_des, mcfg, joint_names), ddth_des_flat)
# ype.flatten(fix_dofs(DOFs, dth, mcfg, joint_names), dth_flat)
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
#################################################
# jacobian
#################################################
contact_des_ids = list() # desired contact segments
if foot_viewer.check_om_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0'))
if foot_viewer.check_op_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0'))
if foot_viewer.check_im_l is not None and foot_viewer.check_im_l.value(
):
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1'))
if foot_viewer.check_ip_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0'))
if foot_viewer.check_h_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0'))
if foot_viewer.check_om_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_0'))
if foot_viewer.check_op_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0'))
if foot_viewer.check_im_r is not None and foot_viewer.check_im_r.value(
):
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_1'))
if foot_viewer.check_ip_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0'))
if foot_viewer.check_h_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_1_0'))
contact_ids = list() # temp idx for balancing
contact_ids.extend(contact_des_ids)
contact_joint_ori = list(
map(controlModel.getJointOrientationGlobal, contact_ids))
contact_joint_pos = list(
map(controlModel.getJointPositionGlobal, contact_ids))
contact_body_ori = list(
map(controlModel.getBodyOrientationGlobal, contact_ids))
contact_body_pos = list(
map(controlModel.getBodyPositionGlobal, contact_ids))
contact_body_vel = list(
map(controlModel.getBodyVelocityGlobal, contact_ids))
contact_body_angvel = list(
map(controlModel.getBodyAngVelocityGlobal, contact_ids))
ref_joint_ori = list(
map(motion[frame].getJointOrientationGlobal, contact_ids))
ref_joint_pos = list(
map(motion[frame].getJointPositionGlobal, contact_ids))
ref_joint_vel = [
motion.getJointVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_joint_angvel = [
motion.getJointAngVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_body_ori = list(
map(motionModel.getBodyOrientationGlobal, contact_ids))
ref_body_pos = list(map(motionModel.getBodyPositionGlobal,
contact_ids))
# ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids))
ref_body_angvel = [
motion.getJointAngVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_body_vel = [
ref_joint_vel[i] +
np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i])
for i in range(len(ref_joint_vel))
]
is_contact = [1] * len(contact_ids)
contact_right = len(set(contact_des_ids).intersection(rIDlist)) > 0
contact_left = len(set(contact_des_ids).intersection(lIDlist)) > 0
contMotionOffset = th[0][0] - th_r[0][0]
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
CM = yrp.getCM(linkPositions, linkMasses, totalMass)
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
CM_plane = copy.copy(CM)
CM_plane[1] = 0.
dCM_plane = copy.copy(dCM)
dCM_plane[1] = 0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
# calculate jacobian
Jsys, dJsys = controlModel.computeCom_J_dJdq()
J_contacts = [] # type: list[np.ndarray]
dJ_contacts = [] # type: list[np.ndarray]
for contact_id in contact_ids:
J_contacts.append(Jsys[6 * contact_id:6 * contact_id + 6, :])
dJ_contacts.append(dJsys[6 * contact_id:6 * contact_id + 6])
# calculate footCenter
footCenter = sum(contact_body_pos) / len(contact_body_pos) if len(contact_body_pos) > 0 \
else .5 * (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(supR))
footCenter[1] = 0.
# if len(contact_body_pos) > 2:
# hull = ConvexHull(contact_body_pos)
footCenter_ref = sum(ref_body_pos) / len(ref_body_pos) if len(ref_body_pos) > 0 \
else .5 * (motionModel.getBodyPositionGlobal(supL) + motionModel.getBodyPositionGlobal(supR))
footCenter_ref = footCenter_ref + contMotionOffset
# if len(ref_body_pos) > 2:
# hull = ConvexHull(ref_body_pos)
footCenter_ref[1] = 0.
# footCenter[0] = footCenter[0] + getParamVal('com X offset')
# footCenter[1] = footCenter[0] + getParamVal('com Y offset')
# footCenter[2] = footCenter[2] + getParamVal('com Z offset')
# initialization
if g_initFlag == 0:
preFootCenter[0] = footCenter.copy()
g_initFlag = 1
# if contactChangeCount == 0 and np.linalg.norm(footCenter - preFootCenter[0]) > 0.01:
# contactChangeCount += 30
if contactChangeCount > 0:
# change footcenter gradually
footCenter = preFootCenter[0] + (
maxContactChangeCount - contactChangeCount) * (
footCenter - preFootCenter[0]) / maxContactChangeCount
else:
preFootCenter[0] = footCenter.copy()
# linear momentum
# TODO:
# We should consider dCM_ref, shouldn't we?
# add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel!
# to do that, set joint velocities to vpModel
CM_ref_plane = footCenter
# CM_ref_plane = footCenter_ref
CM_ref = footCenter + np.array([
getParamVal('com X offset'),
motionModel.getCOM()[1] + getParamVal('com Y offset'),
getParamVal('com Z offset')
])
dL_des_plane = Kl * totalMass * (CM_ref - CM) - Dl * totalMass * dCM
# dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane
# dL_des_plane[1] = 0.
# print('dCM_plane : ', np.linalg.norm(dCM_plane))
# angular momentum
CP_ref = footCenter
# CP_ref = footCenter_ref
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0]) / (1 / 30.)
CP_old[0] = CP
if CP is not None and dCP is not None:
ddCP_des = Kh * (CP_ref - CP) - Dh * dCP
dCP_des = dCP + ddCP_des * (1 / 30.)
CP_des = CP + dCP_des * (1 / 30.)
# CP_des = footCenter
CP_des = CP + dCP * (1 / 30.) + .5 * ddCP_des * ((1 / 30.)**2)
dH_des = np.cross(
(CP_des - CM),
(dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
if contactChangeCount > 0: # and contactChangeType == 'DtoS':
dH_des *= (maxContactChangeCount -
contactChangeCount) / maxContactChangeCount
else:
dH_des = None
# convex hull
contact_pos_2d = np.asarray([
np.array([contactPosition[0], contactPosition[2]])
for contactPosition in contactPositions
])
p = np.array([CM_plane[0], CM_plane[2]])
# hull = None # type: Delaunay
# if contact_pos_2d.shape[0] > 0:
# hull = Delaunay(contact_pos_2d)
# print(hull.find_simplex(p) >= 0)
# set up equality constraint
# TODO:
# logSO3 is just q'', not acceleration.
# To make a_oris acceleration, q'' -> a will be needed
# body_ddqs = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori]))
# body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], mm.unitY()), mm.unitY()))) for i in range(len(contact_body_ori))]))
# body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()))) for i in range(len(contact_body_ori))]))
a_oris = list(
map(mm.logSO3, [
np.dot(
contact_body_ori[i].T,
np.dot(
ref_body_ori[i],
mm.getSO3FromVectors(
np.dot(ref_body_ori[i],
up_vec_in_each_link[contact_ids[i]]),
mm.unitY()))) for i in range(len(contact_body_ori))
]))
a_oris = list(
map(mm.logSO3, [
np.dot(
np.dot(
ref_body_ori[i],
mm.getSO3FromVectors(
np.dot(ref_body_ori[i],
up_vec_in_each_link[contact_ids[i]]),
mm.unitY())), contact_body_ori[i].T)
for i in range(len(contact_body_ori))
]))
body_qs = list(map(mm.logSO3, contact_body_ori))
body_angs = [
np.dot(contact_body_ori[i], contact_body_angvel[i])
for i in range(len(contact_body_ori))
]
body_dqs = [
mm.vel2qd(body_angs[i], body_qs[i]) for i in range(len(body_angs))
]
# a_oris = [np.dot(contact_body_ori[i], mm.qdd2accel(body_ddqs[i], body_dqs[i], body_qs[i])) for i in range(len(contact_body_ori))]
# body_ddq = body_ddqs[0]
# body_ori = contact_body_ori[0]
# body_ang = np.dot(body_ori.T, contact_body_angvel[0])
#
# body_q = mm.logSO3(body_ori)
# body_dq = mm.vel2qd(body_ang, body_q)
# a_ori = np.dot(body_ori, mm.qdd2accel(body_ddq, body_dq, body_q))
KT_SUP = np.diag([kt_sup / 10., kt_sup, kt_sup / 10.])
# KT_SUP = np.diag([kt_sup, kt_sup, kt_sup])
# a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori]))
# a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(contact_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()) for i in range(len(contact_body_ori))]))
# a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) + dt_sup*(ref_body_vel[i] - contact_body_vel[i]),
# kt_sup*a_oris[i]+dt_sup*(ref_body_angvel[i]-contact_body_angvel[i])) for i in range(len(a_oris))]
# a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) - dt_sup * contact_body_vel[i],
# kt_sup*a_oris[i] - dt_sup * contact_body_angvel[i]) for i in range(len(a_oris))]
a_sups = [
np.append(
np.dot(KT_SUP,
(ref_body_pos[i] - contact_body_pos[i] +
contMotionOffset)) - dt_sup * contact_body_vel[i],
kt_sup * a_oris[i] - dt_sup * contact_body_angvel[i])
for i in range(len(a_oris))
]
# for i in range(len(a_sups)):
# a_sups[i][1] = -kt_sup * contact_body_pos[i][1] - dt_sup * contact_body_vel[i][1]
# momentum matrix
RS = np.dot(P, Jsys)
R, S = np.vsplit(RS, 2)
# rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat)
rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsys)
r_bias, s_bias = np.hsplit(rs, 2)
#######################################################
# optimization
#######################################################
# if contact == 2 and footCenterR[1] > doubleTosingleOffset/2:
if contact_left and not contact_right:
config['weightMap']['RightUpLeg'] = .8
config['weightMap']['RightLeg'] = .8
config['weightMap']['RightFoot'] = .8
else:
config['weightMap']['RightUpLeg'] = .1
config['weightMap']['RightLeg'] = .25
config['weightMap']['RightFoot'] = .2
# if contact == 1 and footCenterL[1] > doubleTosingleOffset/2:
if contact_right and not contact_left:
config['weightMap']['LeftUpLeg'] = .8
config['weightMap']['LeftLeg'] = .8
config['weightMap']['LeftFoot'] = .8
else:
config['weightMap']['LeftUpLeg'] = .1
config['weightMap']['LeftLeg'] = .25
config['weightMap']['LeftFoot'] = .2
w = mot.getTrackingWeight(DOFs, motion[0].skeleton,
config['weightMap'])
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
if dH_des is not None:
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
if True:
for c_idx in range(len(contact_ids)):
mot.addConstraint2(problem, totalDOF, J_contacts[c_idx],
dJ_contacts[c_idx], dth_flat,
a_sups[c_idx])
if contactChangeCount > 0:
contactChangeCount = contactChangeCount - 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
r = problem.solve()
problem.clear()
ddth_sol_flat = np.asarray(r['x'])
# ddth_sol_flat[foot_seg_dofs] = np.array(ddth_des_flat)[foot_seg_dofs]
ype.nested(ddth_sol_flat, ddth_sol)
rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
for i in range(stepsPerFrame):
# apply penalty force
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
# bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
controlModel.setDOFAccelerations(ddth_sol)
# controlModel.setDOFAccelerations(ddth_des)
# controlModel.set_ddq(ddth_sol_flat)
# controlModel.set_ddq(ddth_des_flat)
controlModel.solveHybridDynamics()
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([
viewer.objectInfoWnd.labelForceX.value(),
viewer.objectInfoWnd.labelForceY.value(),
viewer.objectInfoWnd.labelForceZ.value()
])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += wcfg.timeStep
vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
vpWorld.step()
controlModel_ik.set_q(controlModel.get_q())
if foot_viewer is not None:
foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info(
frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds)
foot_viewer.foot_pressure_gl_window.goToFrame(frame)
# rendering
for foot_seg_id in footIdlist:
control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255)
for contact_id in contact_ids:
control_model_renderer.body_colors[contact_id] = (255, 0, 0)
rd_footCenter[0] = footCenter
rd_footCenter_ref[0] = footCenter_ref
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
rd_CM_plane[0][1] = 0.
if CP is not None and dCP is not None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des
rd_dL_des_plane[0] = [
dL_des_plane[0] / 100, dL_des_plane[1] / 100,
dL_des_plane[2] / 100
]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity)
del rd_foot_ori[:]
del rd_foot_pos[:]
# for seg_foot_id in footIdlist:
# rd_foot_ori.append(controlModel.getJointOrientationGlobal(seg_foot_id))
# rd_foot_pos.append(controlModel.getJointPositionGlobal(seg_foot_id))
rd_foot_ori.append(controlModel.getJointOrientationGlobal(supL))
rd_foot_ori.append(controlModel.getJointOrientationGlobal(supR))
rd_foot_pos.append(controlModel.getJointPositionGlobal(supL))
rd_foot_pos.append(controlModel.getJointPositionGlobal(supR))
rd_root_des[0] = rootPos[0]
rd_root_ori[0] = controlModel.getBodyOrientationGlobal(0)
rd_root_pos[0] = controlModel.getBodyPositionGlobal(0)
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append(contactForces[i] / 400)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exf_des[0] = [0, 0, 0]
else:
rd_exf_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exfen_des[0] = [0, 0, 0]
# extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody)
extraForcePos[0] = controlModel.getBodyPositionGlobal(
selectedBody) - 0.1 * np.array([
viewer.objectInfoWnd.labelForceX.value(), 0.,
viewer.objectInfoWnd.labelForceZ.value()
])
# render contact_ids
# render skeleton
if SKELETON_ON:
Ts = dict()
Ts['pelvis'] = controlModel.getJointTransform(idDic['Hips'])
Ts['thigh_R'] = controlModel.getJointTransform(idDic['RightUpLeg'])
Ts['shin_R'] = controlModel.getJointTransform(idDic['RightLeg'])
Ts['foot_R'] = controlModel.getJointTransform(idDic['RightFoot'])
Ts['foot_heel_R'] = controlModel.getJointTransform(
idDic['RightFoot'])
Ts['heel_R'] = np.eye(4)
Ts['outside_metatarsal_R'] = controlModel.getJointTransform(
idDic['RightFoot_foot_0_0'])
Ts['outside_phalanges_R'] = controlModel.getJointTransform(
idDic['RightFoot_foot_0_0_0'])
# Ts['inside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1'])
Ts['inside_metatarsal_R'] = np.eye(4)
Ts['inside_phalanges_R'] = controlModel.getJointTransform(
idDic['RightFoot_foot_0_1_0'])
Ts['spine_ribs'] = controlModel.getJointTransform(idDic['Spine'])
Ts['head'] = controlModel.getJointTransform(idDic['Spine1'])
Ts['upper_limb_R'] = controlModel.getJointTransform(
idDic['RightArm'])
Ts['lower_limb_R'] = controlModel.getJointTransform(
idDic['RightForeArm'])
Ts['thigh_L'] = controlModel.getJointTransform(idDic['LeftUpLeg'])
Ts['shin_L'] = controlModel.getJointTransform(idDic['LeftLeg'])
Ts['foot_L'] = controlModel.getJointTransform(idDic['LeftFoot'])
Ts['foot_heel_L'] = controlModel.getJointTransform(
idDic['LeftFoot'])
Ts['heel_L'] = np.eye(4)
Ts['outside_metatarsal_L'] = controlModel.getJointTransform(
idDic['LeftFoot_foot_0_0'])
Ts['outside_phalanges_L'] = controlModel.getJointTransform(
idDic['LeftFoot_foot_0_0_0'])
# Ts['inside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1'])
Ts['inside_metatarsal_L'] = np.eye(4)
Ts['inside_phalanges_L'] = controlModel.getJointTransform(
idDic['LeftFoot_foot_0_1_0'])
Ts['upper_limb_L'] = controlModel.getJointTransform(
idDic['LeftArm'])
Ts['lower_limb_L'] = controlModel.getJointTransform(
idDic['LeftForeArm'])
skeleton_renderer.appendFrameState(Ts)
def 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 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()
yme.removeJoint(motion, 'LHipJoint', False)
yme.removeJoint(motion, 'RHipJoint', False)
yme.removeJoint(motion, 'LowerBack', False)
yme.removeJoint(motion, 'LeftToeBase', False)
yme.removeJoint(motion, 'RightToeBase', False)
yme.removeJoint(motion, 'Neck', False)
yme.removeJoint(motion, 'Head', False)
yme.removeJoint(motion, 'LeftHandIndex1', False)
yme.removeJoint(motion, 'RightHandIndex1', False)
yme.removeJoint(motion, 'LeftFingerBase', False)
yme.removeJoint(motion, 'RightFingerBase', False)
yme.removeJoint(motion, 'LThumb_Effector', False)
yme.removeJoint(motion, 'RThumb_Effector', False)
yme.removeJoint(motion, 'LThumb', False)
yme.removeJoint(motion, 'RThumb', False)
yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(.8, -0.0, -0.4),
-.5), False)
yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(.8, 0.0, 0.4), -.5),
False)
motion.updateGlobalT()
motion.translateByOffset((0, -0.08, 0))
massMap = {}
massMap = massMap.fromkeys([
'Head', 'Head_Effector', 'Hips', 'LeftArm', 'LeftFoot', 'LeftForeArm',
'LeftHand', 'LeftHand_Effector', 'LeftLeg', 'LeftShoulder',
'LeftToeBase', 'LeftToeBase_Effector', 'LeftUpLeg', 'RightArm',
'RightFoot', 'RightForeArm', 'RightHand', 'RightHand_Effector',
'RightLeg', 'RightShoulder', 'RightToeBase', 'RightToeBase_Effector',
'RightUpLeg', 'Spine', 'Spine1', 'Neck1'
], 0.)
def create_biped(motionName='wd2_n_kick.bvh',
SEGMENT_FOOT=True,
SEGMENT_FOOT_MAG=.03,
SEGMENT_FOOT_RAD=None):
"""
:param motionName: motion file name
:param SEGMENT_FOOT: whether segment foot is
:param SEGMENT_FOOT_MAG:
:return:
"""
# :rtype: ym.JointMotion, ypc.ModelConfig, ypc.WorldConfig, int, dict[str, float|dict[str, float]], float
if SEGMENT_FOOT_RAD is None:
SEGMENT_FOOT_RAD = SEGMENT_FOOT_MAG * .5
SEGMENT_FOOT_SEPARATE = False
SEGMENT_FOOT_OUTSIDE_JOINT_FIRST = True
SEGMENT_FOOT_ARC = True
SEGMENT_BETWEEN_SPACE = 1.2
SEGMENT_METATARSAL_LEN = 2.5
SEGMENT_THIRD_PHA_LEN = 1.8
SEGMENT_FOURTH_PHA_RATIO = 5. / 6.
SEGMENT_HEEL_LEN = 1.2
# motion
# motionName = 'wd2_n_kick.bvh'
# motionName = 'wd2_tiptoe.bvh'
# motionName = 'wd2_n_kick_zygote.bvh'
# motionName = 'wd2_jump.bvh'
# motionName = 'wd2_stand.bvh'
bvh = yf.readBvhFileAsBvh(motionName)
bvh.set_scale(.01)
if SEGMENT_FOOT:
# partBvhFilePath = '../PyCommon/modules/samples/simpleJump_long_test2.bvh'
current_path = os.path.dirname(os.path.abspath(__file__))
partBvhFilePath = current_path + '/../../PyCommon/modules/samples/'
if SEGMENT_FOOT_SEPARATE:
partBvhFilePath = partBvhFilePath + 'simpleJump_long_test5.bvh'
elif SEGMENT_FOOT_OUTSIDE_JOINT_FIRST:
# partBvhFilePath = partBvhFilePath + 'simpleJump_long_test3.bvh'
# partBvhFilePath = partBvhFilePath + 'foot_model_01.bvh'
partBvhFilePath = partBvhFilePath + 'foot_model_01.bvh'
else:
partBvhFilePath = partBvhFilePath + 'simpleJump_long_test4.bvh'
partBvh = yf.readBvhFileAsBvh(partBvhFilePath)
partSkeleton = partBvh.toJointSkeleton(1., False)
SEGMENT_BETWEEN_SPACE = partSkeleton.getOffset(
partSkeleton.getElementIndex('foot_0_1'))[0]
SEGMENT_METATARSAL_LEN = partSkeleton.getOffset(
partSkeleton.getElementIndex('foot_0_1_0'))[2]
SEGMENT_THIRD_PHA_LEN = partSkeleton.getOffset(
partSkeleton.getElementIndex('foot_0_0_0_Effector'))[2]
SEGMENT_HEEL_LEN = abs(
partSkeleton.getOffset(
partSkeleton.getElementIndex('foot_1_0_Effector'))[2])
bvh.replaceJointFromBvh('RightFoot', partBvh, SEGMENT_FOOT_MAG)
partBvh = yf.readBvhFileAsBvh(partBvhFilePath)
partBvh.mirror('YZ')
bvh.replaceJointFromBvh('LeftFoot', partBvh, SEGMENT_FOOT_MAG)
motion = bvh.toJointMotion(1., False) # type: ym.JointMotion
# motion.translateByOffset((0., 0.15, 0.))
# motion.translateByOffset((0., -0.12, 0.))
# motion.rotateByOffset(mm.rotZ(math.pi*1./18.))
# motion = yf.readBvhFile(motionName, .01)
# yme.offsetJointLocal(motion, 'RightArm', (.03,-.05,0), False)
# yme.offsetJointLocal(motion, 'LeftArm', (-.03,-.05,0), False)
# yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False)
# yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False)
# yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False)
# yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.5,0), -.6), False)
# yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.5,0), -.6), False)
# yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.1), False)
# yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.1), False)
# yme.removeJoint(motion, 'RightFoot_foot_1_1')
# yme.removeJoint(motion, 'RightFoot_foot_1_2')
# yme.removeJoint(motion, 'LeftFoot_foot_1_1')
# yme.removeJoint(motion, 'LeftFoot_foot_1_2')
if motionName == 'wd2_n_kick.bvh' or motionName == 'wd2_n_kick_zygote.bvh':
yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01),
False)
yme.updateGlobalT(motion)
motion.translateByOffset((0, 0.04, 0))
for i in range(2000):
motion.data.insert(0, copy.deepcopy(motion[0]))
motion.extend([motion[-1]] * 300)
elif motionName == 'wd2_tiptoe.bvh' or motionName == 'wd2_tiptoe_zygote.bvh':
yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01),
False)
yme.rotateJointLocal(motion, 'LeftFoot',
mm.exp(mm.v3(1., 0., 0.), -.1), False)
yme.rotateJointLocal(motion, 'RightFoot',
mm.exp(mm.v3(1., 0., 0.), -.1), False)
yme.updateGlobalT(motion)
motion.translateByOffset((0, 0.06, 0))
# if motionName == 'wd2_tiptoe.bvh':
# motion.translateByOffset((0, 0.06, 0))
# else:
# motion.translateByOffset((0, -0.03, 0))
del motion[:270]
for i in range(2000):
motion.data.insert(0, copy.deepcopy(motion[0]))
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 60
# stepsPerFrame = 30
frame_rate = 30
wcfg.timeStep = 1. / (frame_rate * stepsPerFrame)
# wcfg.timeStep = (1/30.)/(stepsPerFrame)
# wcfg.timeStep = (1/1000.)
# width : x axis on body frame
# height: y axis on body frame
# length: z axis on body frame
node = mcfg.getNode('Hips')
node.length = 4. / 27.
node.width = .25
# node.height = .2
# node.width = .25
node = mcfg.getNode('Spine1')
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode('Spine')
node.width = .22
node = mcfg.getNode('RightFoot')
node.length = .25
#node.length = .2
#node.width = .15
node.width = .2
node.mass = 2.
node = mcfg.getNode('LeftFoot')
node.length = .25
#node.length = .2
#node.width = .15
node.width = .2
node.mass = 2.
def capsulize(node_name):
node_capsule = mcfg.getNode(node_name)
node_capsule.geom = 'MyFoot4'
node_capsule.width = 0.01
node_capsule.density = 200.
# node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., math.pi/4., 0.])], ypc.CapsuleMaterial(1000., .02, .2))
# node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., math.pi/4., 0.])], ypc.CapsuleMaterial(1000., .02, .1))
# node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., 0., 0.])], ypc.CapsuleMaterial(1000., .01, -1))
# node.addGeom('MyFoot4', None, ypc.CapsuleMaterial(1000., .02, .1))
# capsulize('RightFoot')
# capsulize('LeftFoot')
if SEGMENT_FOOT:
node = mcfg.getNode('RightFoot')
node.density = 200.
node.geom = 'MyFoot5'
node.width = 0.01
node.jointType = 'B'
node = mcfg.getNode('LeftFoot')
node.density = 200.
node.geom = 'MyFoot5'
node.width = 0.01
node.jointType = 'B'
# bird foot
# capsulize('RightFoot_foot_0_0')
# capsulize('RightFoot_foot_0_1')
# capsulize('RightFoot_foot_1_0')
# capsulize('RightFoot_foot_1_1')
# capsulize('RightFoot_foot_2_0')
# capsulize('RightFoot_foot_2_1')
# capsulize('LeftFoot_foot_0_0')
# capsulize('LeftFoot_foot_0_1')
# capsulize('LeftFoot_foot_1_0')
# capsulize('LeftFoot_foot_1_1')
# capsulize('LeftFoot_foot_2_0')
# capsulize('LeftFoot_foot_2_1')
# human foot
if SEGMENT_FOOT:
footJointType = 'B'
capsulDensity = 400.
if SEGMENT_FOOT_SEPARATE:
# RightFoot_foot_0_0 : outside metatarsals
capsulize('RightFoot_foot_0_0')
node = mcfg.getNode('RightFoot_foot_0_0')
# node.addGeom('MyFoot3', [SEGMENT_FOOT_MAG*np.array([0., 0., 2.5*0.25]), mm.exp([0., 0., 0.])],
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([0., 0., 0.]),
mm.exp([0., 0., 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([0. - 1.2, 0., 0.]),
mm.exp([0., 0., 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
# RightFoot_foot_0_0_0 : outside phalanges
capsulize('RightFoot_foot_0_0_0')
node = mcfg.getNode('RightFoot_foot_0_0_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# RightFoot_foot_0_1 : inside metatarsals
capsulize('RightFoot_foot_0_1')
node = mcfg.getNode('RightFoot_foot_0_1')
node.addGeom(
'MyFoot3',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# RightFoot_foot_0_1_0 : inside phalanges
capsulize('RightFoot_foot_0_1_0')
node = mcfg.getNode('RightFoot_foot_0_1_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# RightFoot_foot_1_0 : center heel
capsulize('RightFoot_foot_1_0')
node = mcfg.getNode('RightFoot_foot_1_0')
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([-.6, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([+.6, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0')
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([0., 0., 0.]),
mm.exp([0., 0., 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([0. + 1.2, 0., 0.]),
mm.exp([0., 0., 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_1')
node = mcfg.getNode('LeftFoot_foot_0_1')
node.addGeom(
'MyFoot3',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_1_0')
node = mcfg.getNode('LeftFoot_foot_0_1_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
capsulize('LeftFoot_foot_1_0')
node = mcfg.getNode('LeftFoot_foot_1_0')
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([-.6, 0., .0]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([+.6, 0., .0]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
elif SEGMENT_FOOT_OUTSIDE_JOINT_FIRST and not SEGMENT_FOOT_ARC:
# RightFoot_foot_0_0 : outside metatarsals
capsulize('RightFoot_foot_0_0')
node = mcfg.getNode('RightFoot_foot_0_0')
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([-0.3, 0., 2.5 * 0.25]),
mm.exp([0., -math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([-0.3 - 1.2, 0., 2.5 * 0.25]),
mm.exp([0., -math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
# node.jointType = footJointType
node.jointType = 'B'
# RightFoot_foot_0_0_0 : outside phalanges
capsulize('RightFoot_foot_0_0_0')
node = mcfg.getNode('RightFoot_foot_0_0_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['X']
# RightFoot_foot_0_1 : inside metatarsals
capsulize('RightFoot_foot_0_1')
node = mcfg.getNode('RightFoot_foot_0_1')
node.addGeom(
'MyFoot3',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['Z']
# RightFoot_foot_0_1_0 : inside phalanges
capsulize('RightFoot_foot_0_1_0')
node = mcfg.getNode('RightFoot_foot_0_1_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['X']
# RightFoot_foot_1_0 : center heel
capsulize('RightFoot_foot_1_0')
node = mcfg.getNode('RightFoot_foot_1_0')
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([-.6, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([+.6, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
# node.jointType = footJointType
node.jointType = 'B'
# left foot
# outside metatarsals
capsulize('LeftFoot_foot_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0')
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([0.3, 0., 2.5 * 0.25]),
mm.exp([0., math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([0.3 + 1.2, 0., 2.5 * 0.25]),
mm.exp([0., math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
# node.jointType = footJointType
node.jointType = 'B'
capsulize('LeftFoot_foot_0_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['X']
capsulize('LeftFoot_foot_0_1')
node = mcfg.getNode('LeftFoot_foot_0_1')
node.addGeom(
'MyFoot3',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['Z']
capsulize('LeftFoot_foot_0_1_0')
node = mcfg.getNode('LeftFoot_foot_0_1_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['X']
capsulize('LeftFoot_foot_1_0')
node = mcfg.getNode('LeftFoot_foot_1_0')
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([-.6, 0., .0]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([+.6, 0., .0]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
# node.jointType = footJointType
node.jointType = 'B'
elif SEGMENT_FOOT_OUTSIDE_JOINT_FIRST and SEGMENT_FOOT_ARC:
FIRST_METATARSAL_ANGLE = mm.deg2Rad(30.)
SECOND_METATARSAL_ANGLE = mm.deg2Rad(20.)
THIRD_METATARSAL_ANGLE = mm.deg2Rad(15.)
# RightFoot_foot_0_0 : outside metatarsals
capsulize('RightFoot_foot_0_0')
node = mcfg.getNode('RightFoot_foot_0_0')
node.bone_dir_child = 'RightFoot_foot_0_0_0'
# third
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([
0., .5 * SEGMENT_METATARSAL_LEN *
math.tan(THIRD_METATARSAL_ANGLE), 0.
]),
mm.exp(THIRD_METATARSAL_ANGLE * mm.unitX())
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN /
math.cos(THIRD_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD))
# fourth
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG *
np.array([-SEGMENT_BETWEEN_SPACE, 0., 0.]),
mm.exp([0., 0., 0.])
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
# node.jointType = footJointType
node.jointType = 'B'
capsulize('LeftFoot_foot_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0')
node.bone_dir_child = 'LeftFoot_foot_0_0_0'
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([
0., .5 * SEGMENT_METATARSAL_LEN *
math.tan(THIRD_METATARSAL_ANGLE), 0.
]),
mm.exp(THIRD_METATARSAL_ANGLE * mm.unitX())
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN /
math.cos(THIRD_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG *
np.array([SEGMENT_BETWEEN_SPACE, 0., 0.]),
mm.exp([0., 0., 0.])
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
# node.jointType = footJointType
node.jointType = 'B'
# RightFoot_foot_0_0_0 : outside phalanges
SEGMENT_FOURTH_PHA_OFFSET = .5 * SEGMENT_THIRD_PHA_LEN * (
1. - SEGMENT_FOURTH_PHA_RATIO)
capsulize('RightFoot_foot_0_0_0')
node = mcfg.getNode('RightFoot_foot_0_0_0')
# third
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([0., 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
# fourth
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG * np.array([
-SEGMENT_BETWEEN_SPACE, 0., -SEGMENT_FOURTH_PHA_OFFSET
]),
mm.exp([0.] * 3)
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_THIRD_PHA_LEN *
SEGMENT_FOURTH_PHA_RATIO + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['X']
capsulize('LeftFoot_foot_0_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0_0')
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([0., 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG * np.array([
SEGMENT_BETWEEN_SPACE, 0., -SEGMENT_FOURTH_PHA_OFFSET
]),
mm.exp([0.] * 3)
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_THIRD_PHA_LEN *
SEGMENT_FOURTH_PHA_RATIO + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['X']
# RightFoot_foot_0_1 : inside metatarsals
capsulize('RightFoot_foot_0_1')
node = mcfg.getNode('RightFoot_foot_0_1')
# second
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([
0., .5 * SEGMENT_METATARSAL_LEN *
math.tan(SECOND_METATARSAL_ANGLE), 0.
]),
mm.exp(SECOND_METATARSAL_ANGLE * mm.unitX())
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN /
math.cos(SECOND_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD))
# first
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([
SEGMENT_BETWEEN_SPACE, .5 * SEGMENT_METATARSAL_LEN *
math.tan(FIRST_METATARSAL_ANGLE), 0.
]),
mm.exp(FIRST_METATARSAL_ANGLE * mm.unitX())
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN /
math.cos(FIRST_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['Z']
capsulize('LeftFoot_foot_0_1')
node = mcfg.getNode('LeftFoot_foot_0_1')
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([
0., .5 * SEGMENT_METATARSAL_LEN *
math.tan(SECOND_METATARSAL_ANGLE), 0.
]),
mm.exp(SECOND_METATARSAL_ANGLE * mm.unitX())
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN /
math.cos(SECOND_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([
-SEGMENT_BETWEEN_SPACE, .5 * SEGMENT_METATARSAL_LEN *
math.tan(FIRST_METATARSAL_ANGLE), 0.
]),
mm.exp(FIRST_METATARSAL_ANGLE * mm.unitX())
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN /
math.cos(FIRST_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['Z']
# RightFoot_foot_0_1_0 : inside phalanges
capsulize('RightFoot_foot_0_1_0')
node = mcfg.getNode('RightFoot_foot_0_1_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG *
np.array([SEGMENT_BETWEEN_SPACE, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['X']
capsulize('LeftFoot_foot_0_1_0')
node = mcfg.getNode('LeftFoot_foot_0_1_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG *
np.array([-SEGMENT_BETWEEN_SPACE, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['X']
# RightFoot_foot_1_0 : center heel
capsulize('RightFoot_foot_1_0')
node = mcfg.getNode('RightFoot_foot_1_0')
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG *
np.array([-SEGMENT_BETWEEN_SPACE / 2., 0., 0.]),
mm.exp([0.] * 3)
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN +
2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG *
np.array([+SEGMENT_BETWEEN_SPACE / 2., 0., 0.]),
mm.exp([0.] * 3)
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN +
2. * SEGMENT_FOOT_RAD))
# node.jointType = footJointType
node.jointType = 'B'
capsulize('LeftFoot_foot_1_0')
node = mcfg.getNode('LeftFoot_foot_1_0')
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG *
np.array([-SEGMENT_BETWEEN_SPACE / 2., 0., .0]),
mm.exp([0.] * 3)
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN +
2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG *
np.array([+SEGMENT_BETWEEN_SPACE / 2., 0., .0]),
mm.exp([0.] * 3)
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN +
2. * SEGMENT_FOOT_RAD))
# node.jointType = footJointType
node.jointType = 'B'
else: # SEGMENT_FOOT_INSIDE_FIRST
# TODO:
# adjust transformation of geometries
# RightFoot_foot_0_1 : inside metatarsals
capsulize('RightFoot_foot_0_1')
node = mcfg.getNode('RightFoot_foot_0_1')
node.addGeom(
'MyFoot3',
[np.array([0.] * 3),
mm.exp([0., math.atan2(1.2, 2.5), 0.])],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0., math.atan2(1.2, 2.5), 0.])
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# RightFoot_foot_0_1_0 : inside phalanges
capsulize('RightFoot_foot_0_1_0')
node = mcfg.getNode('RightFoot_foot_0_1_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# RightFoot_foot_0_0 : outside metatarsals
capsulize('RightFoot_foot_0_0')
node = mcfg.getNode('RightFoot_foot_0_0')
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([-0.3, 0., 2.5 * 0.25]),
mm.exp([0., -math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([-0.3 - 1.2, 0., 2.5 * 0.25]),
mm.exp([0., -math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
# RightFoot_foot_0_0_0 : outside phalanges
capsulize('RightFoot_foot_0_0_0')
node = mcfg.getNode('RightFoot_foot_0_0_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# RightFoot_foot_1_0 : center heel
capsulize('RightFoot_foot_1_0')
node = mcfg.getNode('RightFoot_foot_1_0')
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([-.6, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([+.6, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_1')
node = mcfg.getNode('LeftFoot_foot_0_1')
node.addGeom(
'MyFoot3',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_1_0')
node = mcfg.getNode('LeftFoot_foot_0_1_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0')
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([0.3, 0., 2.5 * 0.25]),
mm.exp([0., math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([0.3 + 1.2, 0., 2.5 * 0.25]),
mm.exp([0., math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
capsulize('LeftFoot_foot_1_0')
node = mcfg.getNode('LeftFoot_foot_1_0')
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([-.6, 0., .0]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([+.6, 0., .0]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
# parameter
config = {}
'''
config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain
config['Kl'] = 2.5; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain
config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain
config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 2.5
config['Bh'] = 1.
'''
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 15000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1., 'Spine1':1., 'RightFoot':.5, 'LeftFoot':.5, 'Hips':1.5,\
#'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.}
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1., 'Spine1':1., 'RightFoot':1.0, 'LeftFoot':1.0, 'Hips':1.5,\
#'RightUpLeg':2., 'RightLeg':2., 'LeftUpLeg':2., 'LeftLeg':2.}
config['weightMap'] = {
'RightArm': .2,
'RightForeArm': .2,
'LeftArm': .2,
'LeftForeArm': .2,
'Spine': .6,
'Spine1': .6,
'RightFoot': .2,
'LeftFoot': .2,
'Hips': 0.5,
'RightUpLeg': .1,
'RightLeg': .3,
'LeftUpLeg': .1,
'LeftLeg': .3
}
if SEGMENT_FOOT:
segfoot_weight = 10.
# segfoot_weight = .1
config['weightMap'] = {
'RightArm': .2,
'RightForeArm': .2,
'LeftArm': .2,
'LeftForeArm': .2,
'Spine': .6,
'Spine1': .6,
'RightFoot': .2,
'LeftFoot': .2,
'Hips': 0.5,
'RightUpLeg': .1,
'RightLeg': .3,
'LeftUpLeg': .1,
'LeftLeg': .3,
'RightFoot_foot_0_0': segfoot_weight,
'RightFoot_foot_0_1': segfoot_weight,
'RightFoot_foot_1_0': segfoot_weight,
'RightFoot_foot_1_1': segfoot_weight,
'RightFoot_foot_1_2': segfoot_weight,
'RightFoot_foot_0_0_0': segfoot_weight,
'RightFoot_foot_0_1_0': segfoot_weight,
'LeftFoot_foot_0_0': segfoot_weight,
'LeftFoot_foot_0_1': segfoot_weight,
'LeftFoot_foot_1_0': segfoot_weight,
'LeftFoot_foot_1_1': segfoot_weight,
'LeftFoot_foot_1_2': segfoot_weight,
'LeftFoot_foot_0_0_0': segfoot_weight,
'LeftFoot_foot_0_1_0': segfoot_weight
}
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':1., 'Hips':0.5,\
#'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.5, 'LeftLeg':1.5}
#success!!
'''
config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':.5, 'Spine1':.5, 'RightFoot':1., 'LeftFoot':1., 'Hips':0.5,\
'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.}
'''
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\
#'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5}
config['supLink'] = 'LeftFoot'
config['supLink1'] = 'LeftFoot'
config['supLink2'] = 'RightFoot'
#config['end'] = 'Hips'
config['end'] = 'Spine1'
return motion, mcfg, wcfg, stepsPerFrame, config, frame_rate
def simulateCallback(frame):
global COLOR_ON
# print(frame)
# print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0']))
if False:
if frame == 200:
if motionFile == 'wd2_tiptoe.bvh':
setParamVal('tiptoe angle', 0.3)
if motionFile == 'wd2_tiptoe_zygote.bvh':
setParamVal('tiptoe angle', 0.3)
# elif 210 < frame < 240:
# if motionFile == 'wd2_tiptoe_zygote.bvh':
# setParamVal('com Y offset', 0.01/30. * (frame-110))
elif frame == 400:
setParamVal('com Y offset', 0.)
setParamVal('tiptoe angle', 0.)
elif frame == 430:
foot_viewer.check_all_seg()
# setParamVal('SupKt', 30.)
# elif frame == 400:
# setParamVal('SupKt', 17.)
# hfi.footAdjust(motion[frame], idDic, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=.015, baseHeight=0.02)
if abs(getParamVal('tiptoe angle')) > 0.001:
tiptoe_angle = getParamVal('tiptoe angle')
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_0_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_0_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
# motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95))
# motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle))
if getParamVal('left tilt angle') > 0.001:
left_tilt_angle = getParamVal('left tilt angle')
if motion[0].skeleton.getJointIndex(
'LeftFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
else:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1_0'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_1_0'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot'], mm.exp(mm.unitZ(),
math.pi * left_tilt_angle))
elif getParamVal('left tilt angle') < -0.001:
left_tilt_angle = getParamVal('left tilt angle')
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_0'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
if motion[0].skeleton.getJointIndex(
'LeftFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1'],
mm.exp(mm.unitZ(), math.pi * left_tilt_angle))
# else:
# motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_1_0'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot'], mm.exp(mm.unitZ(),
math.pi * left_tilt_angle))
if getParamVal('right tilt angle') > 0.001:
right_tilt_angle = getParamVal('right tilt angle')
if motion[0].skeleton.getJointIndex(
'RightFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
else:
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1_0'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_1_0'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot'],
mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
elif getParamVal('right tilt angle') < -0.001:
right_tilt_angle = getParamVal('right tilt angle')
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_0'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
if motion[0].skeleton.getJointIndex(
'RightFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1'],
mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
# else:
# motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_1_0'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot'],
mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
motionModel.update(motion[frame])
motionModel.translateByOffset(
np.array([
getParamVal('com X offset'),
getParamVal('com Y offset'),
getParamVal('com Z offset')
]))
controlModel.update(motion[frame])
# controlModel_ik.set_q(controlModel.get_q())
# controlModel_ik.set_q(controlModel.get_q())
controlModel_ik.update(motion[frame])
controlModel_ik.translateByOffset(
np.array([
-getParamVal('com X offset') * 2.,
getParamVal('com Y offset'),
getParamVal('com Z offset')
]))
global g_initFlag
global forceShowTime
global JsysPre
global JsupPreL
global JsupPreR
global JconstPre
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2 * (Kt**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
dt_sup = 2 * (kt_sup**.5)
# tracking
th_r = motion.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r,
Kt, Dt)
# ype.flatten(fix_dofs(DOFs, ddth_des, mcfg, joint_names), ddth_des_flat)
# ype.flatten(fix_dofs(DOFs, dth, mcfg, joint_names), dth_flat)
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
#################################################
# jacobian
#################################################
contact_des_ids = list() # desired contact segments
if foot_viewer.check_om_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0'))
if foot_viewer.check_op_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0'))
if foot_viewer.check_im_l is not None and foot_viewer.check_im_l.value(
):
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1'))
if foot_viewer.check_ip_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0'))
if foot_viewer.check_h_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0'))
if foot_viewer.check_om_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_0'))
if foot_viewer.check_op_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0'))
if foot_viewer.check_im_r is not None and foot_viewer.check_im_r.value(
):
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_1'))
if foot_viewer.check_ip_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0'))
if foot_viewer.check_h_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_1_0'))
contact_ids = list() # temp idx for balancing
contact_ids.extend(contact_des_ids)
contact_joint_ori = list(
map(controlModel.getJointOrientationGlobal, contact_ids))
contact_joint_pos = list(
map(controlModel.getJointPositionGlobal, contact_ids))
contact_body_ori = list(
map(controlModel.getBodyOrientationGlobal, contact_ids))
contact_body_pos = list(
map(controlModel.getBodyPositionGlobal, contact_ids))
contact_body_vel = list(
map(controlModel.getBodyVelocityGlobal, contact_ids))
contact_body_angvel = list(
map(controlModel.getBodyAngVelocityGlobal, contact_ids))
ref_joint_ori = list(
map(motion[frame].getJointOrientationGlobal, contact_ids))
ref_joint_pos = list(
map(motion[frame].getJointPositionGlobal, contact_ids))
ref_joint_vel = [
motion.getJointVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_joint_angvel = [
motion.getJointAngVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_body_ori = list(
map(motionModel.getBodyOrientationGlobal, contact_ids))
ref_body_pos = list(map(motionModel.getBodyPositionGlobal,
contact_ids))
# ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids))
ref_body_angvel = [
motion.getJointAngVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_body_vel = [
ref_joint_vel[i] +
np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i])
for i in range(len(ref_joint_vel))
]
is_contact = [1] * len(contact_ids)
contact_right = len(set(contact_des_ids).intersection(rIDlist)) > 0
contact_left = len(set(contact_des_ids).intersection(lIDlist)) > 0
contMotionOffset = th[0][0] - th_r[0][0]
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
CM = yrp.getCM(linkPositions, linkMasses, totalMass)
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
CM_plane = copy.copy(CM)
CM_plane[1] = 0.
dCM_plane = copy.copy(dCM)
dCM_plane[1] = 0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
if foot_viewer is not None:
foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info(
frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds)
foot_viewer.foot_pressure_gl_window.goToFrame(frame)
# rendering
for foot_seg_id in footIdlist:
motion_model_renderer.body_colors[foot_seg_id] = (255, 240, 255)
for contact_id in contact_ids:
motion_model_renderer.body_colors[contact_id] = (255, 0, 0)
pallete2 = list()
pallete2.append((244, 198, 61))
pallete2.append((4, 105, 113))
pallete2.append((234, 219, 196))
pallete2.append((216, 1, 6))
pallete2.append((230, 230, 230))
pallete = pallete2
color = dict()
color['RightFoot'] = pallete[0]
color['RightFoot_foot_1_0'] = pallete[4]
color['RightFoot_foot_0_0'] = pallete[1]
color['RightFoot_foot_0_0_0'] = pallete[2]
color['RightFoot_foot_0_1_0'] = pallete[3]
color['LeftFoot'] = pallete[0]
color['LeftFoot_foot_1_0'] = pallete[4]
color['LeftFoot_foot_0_0'] = pallete[1]
color['LeftFoot_foot_0_0_0'] = pallete[2]
color['LeftFoot_foot_0_1_0'] = pallete[3]
if COLOR_ON:
for color_key in color.keys():
motion_model_renderer.body_colors[
idDic[color_key]] = color[color_key]
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
rd_CM_plane[0][1] = 0.
del rd_foot_ori[:]
del rd_foot_pos[:]
# for seg_foot_id in footIdlist:
# rd_foot_ori.append(controlModel.getJointOrientationGlobal(seg_foot_id))
# rd_foot_pos.append(controlModel.getJointPositionGlobal(seg_foot_id))
rd_foot_ori.append(controlModel.getJointOrientationGlobal(supL))
rd_foot_ori.append(controlModel.getJointOrientationGlobal(supR))
rd_foot_pos.append(controlModel.getJointPositionGlobal(supL))
rd_foot_pos.append(controlModel.getJointPositionGlobal(supR))
rd_root_des[0] = rootPos[0]
rd_root_ori[0] = controlModel.getBodyOrientationGlobal(0)
rd_root_pos[0] = controlModel.getBodyPositionGlobal(0)
del rd_CF[:]
del rd_CF_pos[:]
# render contact_ids
# render skeleton
if SKELETON_ON:
Ts = dict()
Ts['pelvis'] = controlModel_ik.getJointTransform(idDic['Hips'])
Ts['thigh_R'] = controlModel_ik.getJointTransform(
idDic['RightUpLeg'])
Ts['shin_R'] = controlModel_ik.getJointTransform(idDic['RightLeg'])
Ts['foot_R'] = controlModel_ik.getJointTransform(
idDic['RightFoot'])
Ts['foot_heel_R'] = controlModel_ik.getJointTransform(
idDic['RightFoot'])
Ts['heel_R'] = np.eye(4)
Ts['outside_metatarsal_R'] = controlModel_ik.getJointTransform(
idDic['RightFoot_foot_0_0'])
Ts['outside_phalanges_R'] = controlModel_ik.getJointTransform(
idDic['RightFoot_foot_0_0_0'])
# Ts['inside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1'])
Ts['inside_metatarsal_R'] = np.eye(4)
Ts['inside_phalanges_R'] = controlModel_ik.getJointTransform(
idDic['RightFoot_foot_0_1_0'])
Ts['spine_ribs'] = controlModel_ik.getJointTransform(
idDic['Spine'])
Ts['head'] = controlModel_ik.getJointTransform(idDic['Spine1'])
Ts['upper_limb_R'] = controlModel_ik.getJointTransform(
idDic['RightArm'])
Ts['lower_limb_R'] = controlModel_ik.getJointTransform(
idDic['RightForeArm'])
Ts['thigh_L'] = controlModel_ik.getJointTransform(
idDic['LeftUpLeg'])
Ts['shin_L'] = controlModel_ik.getJointTransform(idDic['LeftLeg'])
Ts['foot_L'] = controlModel_ik.getJointTransform(idDic['LeftFoot'])
Ts['foot_heel_L'] = controlModel_ik.getJointTransform(
idDic['LeftFoot'])
Ts['heel_L'] = np.eye(4)
Ts['outside_metatarsal_L'] = controlModel_ik.getJointTransform(
idDic['LeftFoot_foot_0_0'])
Ts['outside_phalanges_L'] = controlModel_ik.getJointTransform(
idDic['LeftFoot_foot_0_0_0'])
# Ts['inside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1'])
Ts['inside_metatarsal_L'] = np.eye(4)
Ts['inside_phalanges_L'] = controlModel_ik.getJointTransform(
idDic['LeftFoot_foot_0_1_0'])
Ts['upper_limb_L'] = controlModel_ik.getJointTransform(
idDic['LeftArm'])
Ts['lower_limb_L'] = controlModel_ik.getJointTransform(
idDic['LeftForeArm'])
color = dict()
color['foot_R'] = pallete[0]
color['heel_R'] = pallete[4]
color['outside_metatarsal_R'] = pallete[1]
color['outside_phalanges_R'] = pallete[2]
color['inside_phalanges_R'] = pallete[3]
color['foot_L'] = pallete[0]
color['heel_L'] = pallete[4]
color['outside_metatarsal_L'] = pallete[1]
color['outside_phalanges_L'] = pallete[2]
color['inside_phalanges_L'] = pallete[3]
if COLOR_ON:
skeleton_renderer.appendFrameState(Ts, color)
else:
skeleton_renderer.appendFrameState(Ts)
def buildMcfg():
massMap = buildMassMap()
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
totalMass = 0.
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
# totalMass += node.mass
node = mcfg.getNode('Hips')
node.length = .2
node.width = .25
node = mcfg.getNode('Spine1')
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode('Spine')
node.width = .22
node = mcfg.getNode('RightFoot')
node.length = .25
# node.length = .27
# node.offset = (0,0,0.01)
node.width = .1
node.geom = 'MyFoot1'
node = mcfg.getNode('LeftFoot')
node.length = .25
# node.length = .27
# node.offset = (0,0,0.01)
node.width = .1
node.geom = 'MyFoot1'
def capsulize(node_name):
node = mcfg.getNode(node_name)
node.geom = 'MyFoot4'
node.width = 0.01
# node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., math.pi/4., 0.])], ypc.CapsuleMaterial(1000., .02, .2))
# node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., math.pi/4., 0.])], ypc.CapsuleMaterial(1000., .02, .1))
# node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., 0., 0.])], ypc.CapsuleMaterial(1000., .01, -1))
# node.addGeom('MyFoot4', None, ypc.CapsuleMaterial(1000., .02, .1))
# capsulize('RightFoot')
# capsulize('LeftFoot')
'''
node = mcfg.getNode('RightFoot')
node.density = 200.
node.geom = 'MyFoot5'
node.width = 0.01
# node.jointType = 'U'
node = mcfg.getNode('LeftFoot')
node.density = 200.
node.geom = 'MyFoot5'
node.width = 0.01
# node.jointType = 'U'
'''
# bird foot
# capsulize('RightFoot_foot_0_0')
# capsulize('RightFoot_foot_0_1')
# capsulize('RightFoot_foot_1_0')
# capsulize('RightFoot_foot_1_1')
# capsulize('RightFoot_foot_2_0')
# capsulize('RightFoot_foot_2_1')
# capsulize('LeftFoot_foot_0_0')
# capsulize('LeftFoot_foot_0_1')
# capsulize('LeftFoot_foot_1_0')
# capsulize('LeftFoot_foot_1_1')
# capsulize('LeftFoot_foot_2_0')
# capsulize('LeftFoot_foot_2_1')
# human foot
capsulize('RightFoot_foot_0_0')
node = mcfg.getNode('RightFoot_foot_0_0')
node.addGeom('MyFoot3', [
0.02 * np.array([-0.3, 0., 2.5 * 0.25]),
mm.exp([0., -math.atan2(1.2, 2.5), 0.])
], ypc.CapsuleMaterial(400., .01, 0.02 * 2.5 + 0.02))
node.addGeom('MyFoot3', [
0.02 * np.array([-0.3 - 1.2, 0., 2.5 * 0.25]),
mm.exp([0., -math.atan2(1.2, 2.5), 0.])
], ypc.CapsuleMaterial(400., .01, 0.02 * 2.5 + 0.02))
# node.addGeom('MyFoot4', [0.02*np.array([-1.2, 0., 0.]), mm.exp([0., 0., 0.])], ypc.CapsuleMaterial(1000., .01, -1))
node.jointType = 'R'
capsulize('RightFoot_foot_0_0_0')
node = mcfg.getNode('RightFoot_foot_0_0_0')
node.addGeom('MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(400., .01, -1))
node.addGeom('MyFoot4',
[0.02 * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1))
node.jointType = 'R'
capsulize('RightFoot_foot_0_1')
node = mcfg.getNode('RightFoot_foot_0_1')
node.addGeom('MyFoot3',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(400., .01, -1))
node.addGeom('MyFoot3',
[0.02 * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1))
node.jointType = 'R'
capsulize('RightFoot_foot_0_1_0')
node = mcfg.getNode('RightFoot_foot_0_1_0')
node.addGeom('MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(400., .01, -1))
node.addGeom('MyFoot4',
[0.02 * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1))
node.jointType = 'R'
capsulize('RightFoot_foot_1_0')
node = mcfg.getNode('RightFoot_foot_1_0')
node.addGeom('MyFoot3',
[0.02 * np.array([0., 0., .7]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(400., .01, 0.02 * 2.0 + 0.02))
# node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(1000., .01, -1))
node.jointType = 'R'
capsulize('RightFoot_foot_1_1')
node = mcfg.getNode('RightFoot_foot_1_1')
node.addGeom('MyFoot3',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(400., .01, -1))
node.jointType = 'R'
capsulize('RightFoot_foot_1_2')
node = mcfg.getNode('RightFoot_foot_1_2')
node.addGeom('MyFoot3',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(400., .01, -1))
node.jointType = 'R'
capsulize('LeftFoot_foot_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0')
node.addGeom('MyFoot3', [
0.02 * np.array([0.3, 0., 2.5 * 0.25]),
mm.exp([0., math.atan2(1.2, 2.5), 0.])
], ypc.CapsuleMaterial(400., .01, 0.02 * 2.5 + 0.02))
node.addGeom('MyFoot3', [
0.02 * np.array([0.3 + 1.2, 0., 2.5 * 0.25]),
mm.exp([0., math.atan2(1.2, 2.5), 0.])
], ypc.CapsuleMaterial(400., .01, 0.02 * 2.5 + 0.02))
# node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(1000., .01, -1))
node.jointType = 'R'
capsulize('LeftFoot_foot_0_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0_0')
node.addGeom('MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(400., .01, -1))
node.addGeom('MyFoot4',
[0.02 * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1))
node.jointType = 'R'
capsulize('LeftFoot_foot_0_1')
node = mcfg.getNode('LeftFoot_foot_0_1')
node.addGeom('MyFoot3',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(400., .01, -1))
node.addGeom('MyFoot3',
[0.02 * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1))
node.jointType = 'R'
capsulize('LeftFoot_foot_0_1_0')
node = mcfg.getNode('LeftFoot_foot_0_1_0')
node.addGeom('MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(400., .01, -1))
node.addGeom('MyFoot4',
[0.02 * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)], ypc.CapsuleMaterial(400., .01, -1))
node.jointType = 'R'
capsulize('LeftFoot_foot_1_0')
node = mcfg.getNode('LeftFoot_foot_1_0')
node.addGeom('MyFoot3',
[0.02 * np.array([0., 0., .7]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(400., .01, 0.02 * 2.0 + 0.02))
# node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(1000., .01, -1))
node.jointType = 'R'
capsulize('LeftFoot_foot_1_1')
node = mcfg.getNode('LeftFoot_foot_1_1')
node.addGeom('MyFoot3',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(400., .01, -1))
node.jointType = 'R'
capsulize('LeftFoot_foot_1_2')
node = mcfg.getNode('LeftFoot_foot_1_2')
node.addGeom('MyFoot3',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(400., .01, -1))
node.jointType = 'R'
return mcfg
