def main():
CURRENT_CHECK = False
RSI = True
MOTION_ONLY = False
SKELETON_ON = False
CAMERA_TRACKING = True
PD_PLOT = False
PUSH = False
pydart.init()
env_name = 'walk'
env_name = 'walk_repeated'
# env_name = 'walk_fast'
# env_name = 'walk_sukiko'
# env_name = 'walk_u_turn'
# env_name = '1foot_contact_run'
# env_name = 'round_girl'
# env_name = 'fast_2foot_hop'
# env_name = 'slow_2foot_hop'
# env_name = 'long_broad_jump'
# env_name = 'short_broad_jump'
# env_name = 'n_kick'
# env_name = 'jump'
gain_p0 = []
gain_p1 = []
gain_p2 = []
gain_p3 = []
gain_p4 = []
MA_DUR = 5
ma_gain_p0 = []
ma_gain_p1 = []
ma_gain_p2 = []
ma_gain_p3 = []
ma_gain_p4 = []
rfoot_contact_ranges = []
lfoot_contact_ranges = []
ppo = PPO(env_name, 0, visualize_only=True)
if not MOTION_ONLY and not CURRENT_CHECK:
ppo.LoadModel('model/' + env_name + '.pt')
# ppo.LoadModel('model/' + 'param' + '.pt')
# ppo.LoadModel('model_test/' + env_name + '.pt')
# ppo.LoadModel('model_test/' + 'param' + '.pt')
elif not MOTION_ONLY and CURRENT_CHECK:
env_model_dir = []
for dir_name in sorted(os.listdir()):
if env_name in dir_name:
env_model_dir.append(dir_name)
pt_names = os.listdir(env_model_dir[-1])
pt_names.pop(pt_names.index('log.txt'))
pt_names.sort(key=lambda f: int(os.path.splitext(f)[0]))
pt_names.append('1171.pt')
ppo.LoadModel(env_model_dir[-1] + '/' + pt_names[-1])
print(env_model_dir[-1] + '/' + pt_names[-1])
ppo.env.dart_skel_file = '../data/wd2_seg_rough_terrain.xml'
ppo.env.rsi_num = 560
ppo.env.hard_reset()
ppo.env.Resets(RSI)
ppo.env.ref_skel.set_positions(
ppo.env.ref_motion.get_q(ppo.env.phase_frame))
# viewer settings
rd_contact_positions = [None]
rd_contact_forces = [None]
rd_exf = [None]
rd_exf_pos = [None]
dart_world = ppo.env.world
skel = dart_world.skeletons[1]
viewer_w, viewer_h = 960, 1080
viewer = hsv.hpSimpleViewer(rect=(0, 0, viewer_w + 300, 1 + viewer_h + 55),
viewForceWnd=False)
# viewer.doc.addRenderer('MotionModel', yr.DartRenderer(ppo.env.ref_world, (150,150,255), yr.POLYGON_FILL))
control_model_renderer = None
if not MOTION_ONLY:
control_model_renderer = yr.DartRenderer(dart_world, (255, 240, 255),
yr.POLYGON_FILL)
viewer.doc.addRenderer('controlModel', control_model_renderer)
for shape_idx in range(
ppo.env.world.skeletons[0].body(0).num_shapenodes() // 2):
control_model_renderer.geom_colors[0][0][shape_idx] = (128, 128,
128)
# viewer.doc.addRenderer('contact', yr.VectorsRenderer(rd_contact_forces, rd_contact_positions, (255,0,0)))
viewer.doc.addRenderer(
'extraForceEnable',
yr.WideArrowRenderer(rd_exf,
rd_exf_pos, (255, 0, 0),
lineWidth=.05,
fromPoint=False))
def makeEmptyBasicSkeletonTransformDict(init=None):
Ts = dict()
Ts['pelvis'] = init
Ts['spine_ribs'] = init
Ts['head'] = init
Ts['thigh_R'] = init
Ts['shin_R'] = init
Ts['foot_heel_R'] = init
Ts['foot_R'] = init
Ts['heel_R'] = init
Ts['outside_metatarsal_R'] = init
Ts['outside_phalanges_R'] = init
Ts['inside_metatarsal_R'] = init
Ts['inside_phalanges_R'] = init
Ts['upper_limb_R'] = init
Ts['lower_limb_R'] = init
Ts['thigh_L'] = init
Ts['shin_L'] = init
Ts['foot_heel_L'] = init
Ts['foot_L'] = init
Ts['heel_L'] = init
Ts['outside_metatarsal_L'] = init
Ts['outside_phalanges_L'] = init
Ts['inside_metatarsal_L'] = init
Ts['inside_phalanges_L'] = init
Ts['upper_limb_L'] = init
Ts['lower_limb_L'] = init
return Ts
skeleton_renderer = None
if SKELETON_ON:
# skeleton_renderer = yr.BasicSkeletonRenderer(makeEmptyBasicSkeletonTransformDict(np.eye(4)), offset_Y=-0.08)
# skeleton_renderer = yr.BasicSkeletonRenderer(makeEmptyBasicSkeletonTransformDict(np.eye(4)), color=(230, 230, 230), offset_draw=(0.8, -0.02, 0.))
skeleton_renderer = yr.BasicSkeletonRenderer(
makeEmptyBasicSkeletonTransformDict(np.eye(4)),
color=(230, 230, 230),
offset_draw=(0., -0.0, 0.))
viewer.doc.addRenderer('skeleton', skeleton_renderer)
def simulateCallback(frame):
state = ppo.env.GetState(0)
action_dist, v = ppo.model(torch.tensor(state.reshape(1, -1)).float())
action = action_dist.loc.detach().numpy()
value = v.detach().numpy()
if PUSH and 110 < frame < 123:
ppo.env.applied_force = np.array([0., 0., 50.])
ppo.env.applied_force_offset = np.array([-0.11, 0., 0.])
del rd_exf[:]
del rd_exf_pos[:]
rd_exf.append(ppo.env.applied_force / 100.)
rd_exf_pos.append(
ppo.env.skel.body('Spine').to_world(
ppo.env.applied_force_offset))
elif PUSH and 195 < frame < 208:
ppo.env.applied_force = np.array([0., 0., 80.])
ppo.env.applied_force_offset = np.array([-0.11, 0., 0.])
del rd_exf[:]
del rd_exf_pos[:]
rd_exf.append(ppo.env.applied_force / 100.)
rd_exf_pos.append(
ppo.env.skel.body('Spine').to_world(
ppo.env.applied_force_offset))
elif PUSH and 281 < frame < 294:
ppo.env.applied_force = np.array([0., 0., 100.])
ppo.env.applied_force_offset = np.array([-0.11, 0., 0.])
del rd_exf[:]
del rd_exf_pos[:]
rd_exf.append(ppo.env.applied_force / 100.)
rd_exf_pos.append(
ppo.env.skel.body('Spine').to_world(
ppo.env.applied_force_offset))
else:
del rd_exf[:]
del rd_exf_pos[:]
ppo.env.applied_force = np.zeros(3)
res = ppo.env.Steps(action)
# for gain plotting
gain_p0.append(res[3]['kp'][ppo.env.skel.dof_index('j_RightFoot_x')])
gain_p1.append(
res[3]['kp'][ppo.env.skel.dof_index('j_RightFoot_foot_0_0_x')])
gain_p2.append(
res[3]['kp'][ppo.env.skel.dof_index('j_RightFoot_foot_0_0_0_x')])
gain_p3.append(
res[3]['kp'][ppo.env.skel.dof_index('j_RightFoot_foot_0_1_0_x')])
gain_p4.append(
res[3]['kp'][ppo.env.skel.dof_index('j_RightFoot_foot_1_0_x')])
ma_gain_p0.append(
sum(gain_p0[-MA_DUR:]) /
MA_DUR if len(gain_p0) >= MA_DUR else sum(gain_p0) / len(gain_p0))
ma_gain_p1.append(
sum(gain_p1[-MA_DUR:]) /
MA_DUR if len(gain_p1) >= MA_DUR else sum(gain_p1) / len(gain_p1))
ma_gain_p2.append(
sum(gain_p2[-MA_DUR:]) /
MA_DUR if len(gain_p2) >= MA_DUR else sum(gain_p2) / len(gain_p2))
ma_gain_p3.append(
sum(gain_p3[-MA_DUR:]) /
MA_DUR if len(gain_p3) >= MA_DUR else sum(gain_p3) / len(gain_p3))
ma_gain_p4.append(
sum(gain_p4[-MA_DUR:]) /
MA_DUR if len(gain_p4) >= MA_DUR else sum(gain_p4) / len(gain_p4))
if any([
'RightFoot' in body.name
for body in ppo.env.world.collision_result.contacted_bodies
]):
if rfoot_contact_ranges and rfoot_contact_ranges[-1][
1] == frame - 1:
rfoot_contact_ranges[-1][1] = frame
else:
rfoot_contact_ranges.append([frame, frame])
# res = ppo.env.Steps(np.zeros_like(action))
ppo.env.world.collision_result.update()
# print(frame, ppo.env.Ref_skel.current_frame, ppo.env.world.time()*ppo.env.ref_motion.fps)
# print(frame, res[0][0])
# if res[0][0] > 0.46:
# ppo.env.continue_from_now_by_phase(0.2)
# print(frame, ' '.join(["{:0.1f}".format(400. * exp(log(400.) * rate/10.)) for rate in action[0][ppo.env.skel.ndofs-6:]]))
if res[2]:
print(frame, 'Done')
ppo.env.reset()
contacts = ppo.env.world.collision_result.contacts
# contact body rendering
if control_model_renderer is not None:
skel_idx = dart_world.skel.id
for body_idx in range(
dart_world.skeletons[skel_idx].num_bodynodes()):
for shape_idx in range(dart_world.skeletons[skel_idx].body(
body_idx).num_shapenodes()):
control_model_renderer.geom_colors[skel_idx][body_idx][
shape_idx] = control_model_renderer.totalColor
for contact in contacts:
body_idx, geom_idx = (
contact.bodynode_id1, contact.shape_id1
) if dart_world.skeletons[contact.skel_id1].body(
contact.bodynode_id1).name != 'ground' else (
contact.bodynode_id2, contact.shape_id2)
body = dart_world.skeletons[skel_idx].body(body_idx)
visual = sum(shapenode.has_visual_aspect()
for shapenode in body.shapenodes)
collision = sum(shapenode.has_collision_aspect()
for shapenode in body.shapenodes)
if visual == collision:
control_model_renderer.geom_colors[skel_idx][body_idx][
geom_idx - visual] = (255, 0, 0)
else:
control_model_renderer.geom_colors[skel_idx][body_idx][
(geom_idx - visual) // 2] = (255, 0, 0)
if PD_PLOT:
fig = plt.figure(1)
plt.clf()
fig.add_subplot(5, 1, 1)
for rfoot_contact_range in rfoot_contact_ranges:
plt.axvspan(rfoot_contact_range[0],
rfoot_contact_range[1],
facecolor='0.5',
alpha=0.3)
plt.ylabel('ankle')
plt.plot(range(len(ma_gain_p0)), ma_gain_p0)
fig.add_subplot(5, 1, 2)
for rfoot_contact_range in rfoot_contact_ranges:
plt.axvspan(rfoot_contact_range[0],
rfoot_contact_range[1],
facecolor='0.5',
alpha=0.3)
plt.ylabel('talus')
plt.plot(range(len(ma_gain_p1)), ma_gain_p1)
fig.add_subplot(5, 1, 3)
for rfoot_contact_range in rfoot_contact_ranges:
plt.axvspan(rfoot_contact_range[0],
rfoot_contact_range[1],
facecolor='0.5',
alpha=0.3)
plt.ylabel('thumb')
plt.plot(range(len(ma_gain_p2)), ma_gain_p2)
fig.add_subplot(5, 1, 4)
for rfoot_contact_range in rfoot_contact_ranges:
plt.axvspan(rfoot_contact_range[0],
rfoot_contact_range[1],
facecolor='0.5',
alpha=0.3)
plt.ylabel('phalange')
plt.plot(range(len(ma_gain_p3)), ma_gain_p3)
fig.add_subplot(5, 1, 5)
for rfoot_contact_range in rfoot_contact_ranges:
plt.axvspan(rfoot_contact_range[0],
rfoot_contact_range[1],
facecolor='0.5',
alpha=0.3)
plt.ylabel('heel')
plt.plot(range(len(ma_gain_p4)), ma_gain_p4)
plt.show()
plt.pause(0.001)
# contact rendering
contacts = ppo.env.world.collision_result.contacts
del rd_contact_forces[:]
del rd_contact_positions[:]
for contact in contacts:
rd_contact_forces.append(contact.f / 1000.)
rd_contact_positions.append(contact.p)
# render skeleton
if SKELETON_ON:
Ts = dict()
Ts['pelvis'] = skel.joint('j_Hips').get_local_transform()
Ts['thigh_R'] = skel.joint('j_RightUpLeg').get_local_transform()
Ts['shin_R'] = skel.joint('j_RightLeg').get_local_transform()
Ts['foot_R'] = skel.joint('j_RightFoot').get_local_transform()
Ts['foot_heel_R'] = skel.joint('j_RightFoot').get_local_transform()
Ts['heel_R'] = np.eye(4)
Ts['outside_metatarsal_R'] = skel.joint(
'j_RightFoot_foot_0_0').get_local_transform()
Ts['outside_phalanges_R'] = skel.joint(
'j_RightFoot_foot_0_0_0').get_local_transform()
# Ts['inside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1'])
Ts['inside_metatarsal_R'] = np.eye(4)
Ts['inside_phalanges_R'] = skel.joint(
'j_RightFoot_foot_0_1_0').get_local_transform()
Ts['spine_ribs'] = skel.joint('j_Spine').get_local_transform()
Ts['head'] = skel.joint('j_Spine1').get_local_transform()
Ts['upper_limb_R'] = skel.joint('j_RightArm').get_local_transform()
# Ts['upper_limb_R'] = np.dot(skel.joint('j_RightArm').get_local_transform(), mm.SO3ToSE3(mm.rotX(pi/6.)))
Ts['lower_limb_R'] = skel.joint(
'j_RightForeArm').get_local_transform()
# Ts['lower_limb_R'] = np.dot(skel.joint('j_RightForeArm').get_local_transform(), mm.SO3ToSE3(mm.rotY(-pi/6.)))
Ts['thigh_L'] = skel.joint('j_LeftUpLeg').get_local_transform()
Ts['shin_L'] = skel.joint('j_LeftLeg').get_local_transform()
Ts['foot_L'] = skel.joint('j_LeftFoot').get_local_transform()
Ts['foot_heel_L'] = skel.joint('j_LeftFoot').get_local_transform()
Ts['heel_L'] = np.eye(4)
Ts['outside_metatarsal_L'] = skel.joint(
'j_LeftFoot_foot_0_0').get_local_transform()
Ts['outside_phalanges_L'] = skel.joint(
'j_LeftFoot_foot_0_0_0').get_local_transform()
# Ts['inside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1'])
Ts['inside_metatarsal_L'] = np.eye(4)
Ts['inside_phalanges_L'] = skel.joint(
'j_LeftFoot_foot_0_1_0').get_local_transform()
Ts['upper_limb_L'] = skel.joint('j_LeftArm').get_local_transform()
# Ts['upper_limb_L'] = np.dot(skel.joint('j_LeftArm').get_local_transform(), mm.SO3ToSE3(mm.rotX(pi/6.)))
Ts['lower_limb_L'] = skel.joint(
'j_LeftForeArm').get_local_transform()
# Ts['lower_limb_L'] = np.dot(skel.joint('j_LeftForeArm').get_local_transform(), mm.SO3ToSE3(mm.rotY(pi/6.)))
skeleton_renderer.appendFrameState(Ts)
if MOTION_ONLY:
viewer.setMaxFrame(len(ppo.env.ref_motion) - 1)
if CAMERA_TRACKING:
cameraTargets = [None] * (viewer.getMaxFrame() + 1)
def postCallback(frame):
ppo.env.ref_skel.set_positions(ppo.env.ref_motion.get_q(frame - 1))
ppo.env.ref_motion.frame = frame - 1
if CAMERA_TRACKING:
if cameraTargets[frame] is None:
cameraTargets[frame] = ppo.env.ref_skel.body(0).com()
viewer.setCameraTarget(cameraTargets[frame])
viewer.setPostFrameCallback_Always(postCallback)
else:
viewer.setSimulateCallback(simulateCallback)
viewer.setMaxFrame(3000)
if CAMERA_TRACKING:
cameraTargets = [None] * (viewer.getMaxFrame() + 1)
def postFrameCallback_Always(frame):
if CAMERA_TRACKING:
if cameraTargets[frame] is None:
cameraTargets[frame] = ppo.env.skel.body(0).com()
viewer.setCameraTarget(cameraTargets[frame])
viewer.setPostFrameCallback_Always(postFrameCallback_Always)
viewer.startTimer(1. / 30.)
viewer.show()
Fl.run()
def main():
# np.set_printoptions(precision=4, linewidth=200)
np.set_printoptions(precision=5,
threshold=np.inf,
suppress=True,
linewidth=3000)
motionFile = 'wd2_tiptoe.bvh'
motionFile = 'wd2_tiptoe_zygote.bvh'
# motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(motionFile, SEGMENT_FOOT_RAD=0.008)
motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(
motionFile, SEGMENT_FOOT_MAG=0.01, SEGMENT_FOOT_RAD=0.008)
# motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped()
vpWorld = cvw.VpWorld(wcfg)
vpWorld.SetGlobalDamping(0.999)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
# controlModel_shadow_for_ik = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
# controlToMotionOffset = (1.5, -0.02, 0)
controlToMotionOffset = (0., 0., 0)
controlModel.translateByOffset(controlToMotionOffset)
# controlModel_shadow_for_ik.set_q(controlModel.get_q())
# controlModel_shadow_for_ik.computeJacobian(0, np.array([0., 0., 0.]))
wcfg_ik = copy.deepcopy(wcfg)
vpWorld_ik = cvw.VpWorld(wcfg_ik)
controlModel_ik = cvm.VpControlModel(vpWorld_ik, motion[0], mcfg)
vpWorld_ik.initialize()
controlModel_ik.set_q(np.zeros_like(controlModel.get_q()))
controlModel_q = np.zeros_like(controlModel.get_q())
controlModel_q[4] = controlModel_q[4] + 1.2 - 0.24
controlModel.set_q(controlModel_q)
print(
controlModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex('RightFoot_foot_1_0')))
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
foot_dofs = []
left_foot_dofs = []
right_foot_dofs = []
foot_seg_dofs = []
left_foot_seg_dofs = []
right_foot_seg_dofs = []
# for joint_idx in range(motion[0].skeleton.getJointNum()):
for joint_idx in range(controlModel.getJointNum()):
joint_name = controlModel.index2name(joint_idx)
# joint_name = motion[0].skeleton.getJointName(joint_idx)
if 'Foot' in joint_name:
foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx)
foot_dofs.extend(foot_dofs_temp)
if 'Left' in joint_name:
left_foot_dofs.extend(foot_dofs_temp)
elif 'Right' in joint_name:
right_foot_dofs.extend(foot_dofs_temp)
if 'foot' in joint_name:
foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx)
foot_seg_dofs.extend(foot_dofs_temp)
if 'Left' in joint_name:
left_foot_seg_dofs.extend(foot_dofs_temp)
elif 'Right' in joint_name:
right_foot_seg_dofs.extend(foot_dofs_temp)
# parameter
Kt = config['Kt']
Dt = config['Dt'] # tracking gain
Kl = config['Kl']
Dl = config['Dl'] # linear balance gain
Kh = config['Kh']
Dh = config['Dh'] # angular balance gain
Ks = config['Ks']
Ds = config['Ds'] # penalty force spring gain
Bt = config['Bt']
Bl = config['Bl']
Bh = config['Bh']
selectedBody = motion[0].skeleton.getJointIndex(config['end'])
constBody = motion[0].skeleton.getJointIndex('RightFoot')
supL = motion[0].skeleton.getJointIndex('LeftFoot')
supR = motion[0].skeleton.getJointIndex('RightFoot')
# momentum matrix
linkMasses = controlModel.getBodyMasses()
totalMass = controlModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
problem = yac.LSE(totalDOF, 12)
# a_sup = (0,0,0, 0,0,0) #ori
# a_sup = (0,0,0, 0,0,0) #L
CP_old = [mm.v3(0., 0., 0.)]
# penalty method
bodyIDsToCheck = list(range(vpWorld.getBodyNum()))
# mus = [1.]*len(bodyIDsToCheck)
mus = [.5] * len(bodyIDsToCheck)
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
# viewer
rd_footCenter = [None]
rd_footCenter_ref = [None]
rd_footCenterL = [None]
rd_footCenterR = [None]
rd_CM_plane = [None]
rd_CM = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_exf_des = [None]
rd_exfen_des = [None]
rd_root_des = [None]
rd_foot_ori = [None]
rd_foot_pos = [None]
rd_root_ori = [None]
rd_root_pos = [None]
rd_CF = [None]
rd_CF_pos = [None]
rootPos = [None]
selectedBodyId = [selectedBody]
extraForce = [None]
extraForcePos = [None]
rightFootVectorX = [None]
rightFootVectorY = [None]
rightFootVectorZ = [None]
rightFootPos = [None]
rightVectorX = [None]
rightVectorY = [None]
rightVectorZ = [None]
rightPos = [None]
def makeEmptyBasicSkeletonTransformDict(init=None):
Ts = dict()
Ts['pelvis'] = init
Ts['spine_ribs'] = init
Ts['head'] = init
Ts['thigh_R'] = init
Ts['shin_R'] = init
Ts['foot_heel_R'] = init
Ts['foot_R'] = init
Ts['heel_R'] = init
Ts['outside_metatarsal_R'] = init
Ts['outside_phalanges_R'] = init
Ts['inside_metatarsal_R'] = init
Ts['inside_phalanges_R'] = init
Ts['upper_limb_R'] = init
Ts['lower_limb_R'] = init
Ts['thigh_L'] = init
Ts['shin_L'] = init
Ts['foot_heel_L'] = init
Ts['foot_L'] = init
Ts['heel_L'] = init
Ts['outside_metatarsal_L'] = init
Ts['outside_phalanges_L'] = init
Ts['inside_metatarsal_L'] = init
Ts['inside_phalanges_L'] = init
Ts['upper_limb_L'] = init
Ts['lower_limb_L'] = init
return Ts
# viewer = ysv.SimpleViewer()
# viewer = hsv.hpSimpleViewer(rect=[0, 0, 1024, 768], viewForceWnd=False)
viewer = hsv.hpSimpleViewer(rect=[0, 0, 1920 + 300, 1 + 1080 + 55],
viewForceWnd=False)
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
motion_model_renderer = yr.VpModelRenderer(motionModel, (255, 240, 255),
yr.POLYGON_FILL)
viewer.doc.addRenderer('motionModel', motion_model_renderer)
# viewer.doc.setRendererVisible('motionModel', False)
viewer.doc.addRenderer(
'ikModel',
yr.VpModelRenderer(controlModel_ik, (150, 150, 255), yr.POLYGON_LINE))
viewer.doc.setRendererVisible('ikModel', False)
# viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
control_model_renderer = yr.VpModelRenderer(controlModel, (255, 240, 255),
yr.POLYGON_FILL)
viewer.doc.addRenderer('controlModel', control_model_renderer)
viewer.doc.setRendererVisible('controlModel', False)
skeleton_renderer = None
if SKELETON_ON:
# skeleton_renderer = yr.BasicSkeletonRenderer(makeEmptyBasicSkeletonTransformDict(np.eye(4)), offset_Y=-0.08)
# skeleton_renderer = yr.BasicSkeletonRenderer(makeEmptyBasicSkeletonTransformDict(np.eye(4)), color=(230, 230, 230), offset_draw=(0.8, -0.02, 0.))
skeleton_renderer = yr.BasicSkeletonRenderer(
makeEmptyBasicSkeletonTransformDict(np.eye(4)),
color=(230, 230, 230),
offset_draw=(0., -0.0, 0.))
viewer.doc.addRenderer('skeleton', skeleton_renderer)
viewer.doc.setRendererVisible('skeleton', False)
viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
viewer.doc.setRendererVisible('rd_footCenter', False)
viewer.doc.addRenderer('rd_footCenter_ref',
yr.PointsRenderer(rd_footCenter_ref))
viewer.doc.setRendererVisible('rd_footCenter_ref', False)
viewer.doc.addRenderer('rd_CM_plane',
yr.PointsRenderer(rd_CM_plane, (255, 255, 0)))
viewer.doc.setRendererVisible('rd_CM_plane', False)
viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0, 255, 0)))
viewer.doc.setRendererVisible('rd_CP', False)
viewer.doc.addRenderer('rd_CP_des',
yr.PointsRenderer(rd_CP_des, (255, 0, 255)))
viewer.doc.setRendererVisible('rd_CP_des', False)
viewer.doc.addRenderer(
'rd_dL_des_plane',
yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255, 255, 0)))
viewer.doc.setRendererVisible('rd_dL_des_plane', False)
viewer.doc.addRenderer('rd_dH_des',
yr.VectorsRenderer(rd_dH_des, rd_CM, (0, 255, 0)))
viewer.doc.setRendererVisible('rd_dH_des', False)
# viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001))
viewer.doc.addRenderer('rd_CF',
yr.VectorsRenderer(rd_CF, rd_CF_pos, (255, 255, 0)))
viewer.doc.setRendererVisible('rd_CF', False)
viewer.doc.addRenderer(
'rd_foot_ori',
yr.OrientationsRenderer(rd_foot_ori, rd_foot_pos, (255, 255, 0)))
viewer.doc.setRendererVisible('rd_foot_ori', False)
viewer.doc.addRenderer(
'rd_root_ori',
yr.OrientationsRenderer(rd_root_ori, rd_root_pos, (255, 255, 0)))
viewer.doc.setRendererVisible('rd_root_ori', False)
viewer.doc.addRenderer(
'extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos,
(0, 255, 0)))
viewer.doc.setRendererVisible('extraForce', False)
viewer.doc.addRenderer(
'extraForceEnable',
yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255, 0, 0)))
# viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0)))
# viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0)))
# viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255)))
# viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0)))
# viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0)))
# viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255)))
# foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300, 400, 'foot contact modifier', controlModel)
foot_viewer = None # type: FootWindow
initKt = 25.
# initKt = 60.
initKl = 100.
initKh = 100.
initBl = .1
initBh = .13
# initSupKt = 17
initSupKt = 22
initFm = 50.0
initComX = 0.
initComY = 0.
initComZ = 0.
viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt)
viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl)
viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh)
viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl)
viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh)
viewer.objectInfoWnd.add1DSlider("SupKt", 0., 300., 0.1, initSupKt)
viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 10., initFm)
viewer.objectInfoWnd.add1DSlider("com X offset", -1., 1., 0.001, initComX)
viewer.objectInfoWnd.add1DSlider("com Y offset", -1., 1., 0.001, initComY)
viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.001, initComZ)
viewer.objectInfoWnd.add1DSlider("tiptoe angle", -0.5, .5, 0.001, 0.)
viewer.objectInfoWnd.add1DSlider("left tilt angle", -0.5, .5, 0.001, 0.)
viewer.objectInfoWnd.add1DSlider("right tilt angle", -0.5, .5, 0.001, 0.)
viewer.force_on = False
def viewer_SetForceState(object):
viewer.force_on = True
def viewer_GetForceState():
return viewer.force_on
def viewer_ResetForceState():
viewer.force_on = False
viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState)
viewer_ResetForceState()
offset = 60
viewer.objectInfoWnd.begin()
viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30 + offset * 9, 40,
20, 'X')
viewer.objectInfoWnd.labelForceX.value(0)
viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30 + offset * 9, 40,
20, 'Y')
viewer.objectInfoWnd.labelForceY.value(0)
viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30 + offset * 9, 40,
20, 'Z')
viewer.objectInfoWnd.labelForceZ.value(1)
viewer.objectInfoWnd.labelForceDur = Fl_Value_Input(
220, 30 + offset * 9, 40, 20, 'Dur')
viewer.objectInfoWnd.labelForceDur.value(0.1)
viewer.objectInfoWnd.end()
# self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10)
def getParamVal(paramname):
return viewer.objectInfoWnd.getVal(paramname)
def getParamVals(paramnames):
return (getParamVal(name) for name in paramnames)
def setParamVal(paramname, val):
viewer.objectInfoWnd.setVal(paramname, val)
idDic = dict()
for i in range(motion[0].skeleton.getJointNum()):
idDic[motion[0].skeleton.getJointName(i)] = i
# extendedFootName = ['Foot_foot_0_0', 'Foot_foot_0_1', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0']
extendedFootName = [
'Foot_foot_0_0', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0'
]
lIDdic = {
'Left' + name: motion[0].skeleton.getJointIndex('Left' + name)
for name in extendedFootName
}
rIDdic = {
'Right' + name: motion[0].skeleton.getJointIndex('Right' + name)
for name in extendedFootName
}
footIdDic = lIDdic.copy()
footIdDic.update(rIDdic)
lIDlist = [
motion[0].skeleton.getJointIndex('Left' + name)
for name in extendedFootName
]
rIDlist = [
motion[0].skeleton.getJointIndex('Right' + name)
for name in extendedFootName
]
footIdlist = []
footIdlist.extend(lIDlist)
footIdlist.extend(rIDlist)
foot_left_idx = motion[0].skeleton.getJointIndex('LeftFoot')
foot_right_idx = motion[0].skeleton.getJointIndex('RightFoot')
foot_left_idx_temp = motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0')
foot_right_idx_temp = motion[0].skeleton.getJointIndex(
'RightFoot_foot_1_0')
# ik_solver = hik.numIkSolver(dartIkModel)
# ik_solver.clear()
# bodyIDsToCheck = rIDlist.copy()
joint_names = [
motion[0].skeleton.getJointName(i)
for i in range(motion[0].skeleton.getJointNum())
]
def fix_dofs(_DOFs, nested_dof_values, _mcfg, _joint_names):
fixed_nested_dof_values = list()
fixed_nested_dof_values.append(nested_dof_values[0])
for i in range(1, len(_DOFs)):
dof = _DOFs[i]
if dof == 1:
node = _mcfg.getNode(_joint_names[i])
axis = mm.unitZ()
if node.jointAxes[0] == 'X':
axis = mm.unitX()
elif node.jointAxes[0] == 'Y':
axis = mm.unitY()
fixed_nested_dof_values.append(
np.array([np.dot(nested_dof_values[i], axis)]))
else:
fixed_nested_dof_values.append(nested_dof_values[i])
return fixed_nested_dof_values
start_frame = 50
up_vec_in_each_link = dict()
for foot_id in footIdlist:
up_vec_in_each_link[
foot_id] = controlModel_ik.getBodyOrientationGlobal(foot_id)[1, :]
controlModel_ik.set_q(controlModel.get_q())
###################################
# simulate
###################################
def preFrameCallback_Always(frame):
global COLOR_ON
viewer.motionViewWnd.glWindow.camera.rotateX = math.pi / 180. * -25.
viewer.motionViewWnd.glWindow.camera.rotateY = mm.deg2Rad(45.)
viewer.motionViewWnd.glWindow.camera.distance = .5
viewer.motionViewWnd.glWindow.camera.center = np.array(
[0.04347, -0.01005, -0.31936])
if frame < start_frame:
setParamVal('com Y offset', -0.11)
viewer.doc.setRendererVisible('motionModel', False)
viewer.doc.setRendererVisible('skeleton', True)
elif frame < start_frame + 20:
viewer.doc.setRendererVisible('motionModel', True)
elif frame < start_frame + 50:
setParamVal('com X offset', (frame - start_frame - 20) * 0.003)
elif frame < start_frame + 70:
pass
elif frame < start_frame + 90:
COLOR_ON = True
elif frame < start_frame + 105:
setParamVal('com Y offset',
-0.11 + (frame - start_frame - 90) * 0.0032)
setParamVal('tiptoe angle', (frame - start_frame - 90) * 0.012)
elif frame < start_frame + 120:
pass
elif frame < start_frame + 135:
setParamVal('com Y offset',
-0.065 - (frame - start_frame - 120) * 0.0032)
setParamVal('tiptoe angle',
0.168 - (frame - start_frame - 120) * 0.012)
elif frame < start_frame + 150:
pass
elif frame < start_frame + 165:
setParamVal('com Y offset',
-0.11 + (frame - start_frame - 150) * 0.0004)
setParamVal('left tilt angle',
-(frame - start_frame - 150) * 0.012)
elif frame < start_frame + 180:
pass
elif frame < start_frame + 195:
setParamVal('com Y offset',
-0.104 - (frame - start_frame - 180) * 0.0004)
setParamVal('left tilt angle',
-0.168 + (frame - start_frame - 180) * 0.012)
# print(viewer.motionViewWnd.glWindow.camera.rotateX/math.pi * 180.)
# print(viewer.motionViewWnd.glWindow.camera.rotateY/math.pi * 180.)
# print(viewer.motionViewWnd.glWindow.camera.distance)
# print(viewer.motionViewWnd.glWindow.camera.center)
# if frame > start_frame:
# viewer.motionViewWnd.glWindow.camera.rotateY = mm.deg2Rad((frame-start_frame)*3+45.)
# else:
# viewer.motionViewWnd.glWindow.camera.rotateY = mm.deg2Rad(45.)
#
# elif frame > 340:
# viewer.doc.setRendererVisible('controlModel', False)
# viewer.doc.setRendererVisible('skeleton', True)
# else:
# if 0 <= frame % 50 < 22:
# viewer.doc.setRendererVisible('controlModel', True)
# viewer.doc.setRendererVisible('skeleton', False)
# elif 22 <= frame % 50 < 25:
# viewer.doc.setRendererVisible('controlModel', False)
# viewer.doc.setRendererVisible('skeleton', False)
# elif 25 <= frame % 50 < 47:
# viewer.doc.setRendererVisible('controlModel', False)
# viewer.doc.setRendererVisible('skeleton', True)
# elif 47 <= frame % 50 < 50:
# viewer.doc.setRendererVisible('controlModel', False)
# viewer.doc.setRendererVisible('skeleton', False)
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)
viewer.setSimulateCallback(simulateCallback)
viewer.setPreFrameCallback_Always(preFrameCallback_Always)
viewer.startTimer(1 / 30.)
# viewer.play()
viewer.show()
foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300,
500, 'foot contact modifier', controlModel)
foot_viewer.show()
foot_viewer.check_op_l.value(True)
foot_viewer.check_ip_l.value(True)
foot_viewer.check_op_r.value(True)
foot_viewer.check_ip_r.value(True)
foot_viewer.check_not_all_seg()
viewer.motionViewWnd.goToFrame(0)
Fl.run()
def main():
# np.set_printoptions(precision=4, linewidth=200)
np.set_printoptions(precision=5,
threshold=np.inf,
suppress=True,
linewidth=3000)
motionFile = 'wd2_tiptoe.bvh'
motionFile = 'wd2_tiptoe_zygote.bvh'
# motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(motionFile, SEGMENT_FOOT_RAD=0.008)
motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(
motionFile, SEGMENT_FOOT_MAG=0.01, SEGMENT_FOOT_RAD=0.008)
# motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped()
vpWorld = cvw.VpWorld(wcfg)
sphere_radius = 0.5
# vpWorld.add_sphere_bump(sphere_radius, (1.6361, -sphere_radius + 0.08, -0.3209))
# vpWorld.add_sphere_bump(sphere_radius, (1.4543, -sphere_radius + 0.08, -0.3301))
vpWorld.add_sphere_bump(sphere_radius,
(1.6361, -sphere_radius + 0.08, -0.2909))
vpWorld.add_sphere_bump(sphere_radius,
(1.4543, -sphere_radius + 0.08, -0.2901))
vpWorld.SetGlobalDamping(0.999)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
# controlModel_shadow_for_ik = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
# controlToMotionOffset = (1.5, -0.02, 0)
controlToMotionOffset = (1.5, 0., 0)
controlModel.translateByOffset(controlToMotionOffset)
# controlModel_shadow_for_ik.set_q(controlModel.get_q())
# controlModel_shadow_for_ik.computeJacobian(0, np.array([0., 0., 0.]))
wcfg_ik = copy.deepcopy(wcfg)
vpWorld_ik = cvw.VpWorld(wcfg_ik)
controlModel_ik = cvm.VpControlModel(vpWorld_ik, motion[0], mcfg)
vpWorld_ik.initialize()
controlModel_ik.set_q(np.zeros_like(controlModel.get_q()))
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
print(totalDOF)
print(controlModel.getTotalMass())
foot_dofs = []
left_foot_dofs = []
right_foot_dofs = []
foot_seg_dofs = []
left_foot_seg_dofs = []
right_foot_seg_dofs = []
# for joint_idx in range(motion[0].skeleton.getJointNum()):
for joint_idx in range(controlModel.getJointNum()):
joint_name = controlModel.index2name(joint_idx)
# joint_name = motion[0].skeleton.getJointName(joint_idx)
if 'Foot' in joint_name:
foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx)
foot_dofs.extend(foot_dofs_temp)
if 'Left' in joint_name:
left_foot_dofs.extend(foot_dofs_temp)
elif 'Right' in joint_name:
right_foot_dofs.extend(foot_dofs_temp)
if 'foot' in joint_name:
foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx)
foot_seg_dofs.extend(foot_dofs_temp)
if 'Left' in joint_name:
left_foot_seg_dofs.extend(foot_dofs_temp)
elif 'Right' in joint_name:
right_foot_seg_dofs.extend(foot_dofs_temp)
# parameter
Kt = config['Kt']
Dt = config['Dt'] # tracking gain
Kl = config['Kl']
Dl = config['Dl'] # linear balance gain
Kh = config['Kh']
Dh = config['Dh'] # angular balance gain
Ks = config['Ks']
Ds = config['Ds'] # penalty force spring gain
Bt = config['Bt']
Bl = config['Bl']
Bh = config['Bh']
# selectedBody = motion[0].skeleton.getJointIndex(config['end'])
selectedBody = motion[0].skeleton.getJointIndex('Spine')
constBody = motion[0].skeleton.getJointIndex('RightFoot')
supL = motion[0].skeleton.getJointIndex('LeftFoot')
supR = motion[0].skeleton.getJointIndex('RightFoot')
# momentum matrix
linkMasses = controlModel.getBodyMasses()
totalMass = controlModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
problem = yac.LSE(totalDOF, 12)
# a_sup = (0,0,0, 0,0,0) #ori
# a_sup = (0,0,0, 0,0,0) #L
CP_old = [mm.v3(0., 0., 0.)]
# penalty method
bodyIDsToCheck = list(range(vpWorld.getBodyNum()))
# mus = [1.]*len(bodyIDsToCheck)
mus = [2.] * len(bodyIDsToCheck)
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
# viewer
rd_footCenter = [None]
rd_footCenter_ref = [None]
rd_footCenterL = [None]
rd_footCenterR = [None]
rd_CM_plane = [None]
rd_CM = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_exf_des = [None]
rd_exfen_des = [None]
rd_root_des = [None]
rd_foot_ori = [None]
rd_foot_pos = [None]
rd_root_ori = [None]
rd_root_pos = [None]
rd_CF = [None]
rd_CF_pos = [None]
rootPos = [None]
selectedBodyId = [selectedBody]
extraForce = [None]
extraForcePos = [None]
rightFootVectorX = [None]
rightFootVectorY = [None]
rightFootVectorZ = [None]
rightFootPos = [None]
rightVectorX = [None]
rightVectorY = [None]
rightVectorZ = [None]
rightPos = [None]
def makeEmptyBasicSkeletonTransformDict(init=None):
Ts = dict()
Ts['pelvis'] = init
Ts['spine_ribs'] = init
Ts['head'] = init
Ts['thigh_R'] = init
Ts['shin_R'] = init
Ts['foot_heel_R'] = init
Ts['foot_R'] = init
Ts['heel_R'] = init
Ts['outside_metatarsal_R'] = init
Ts['outside_phalanges_R'] = init
Ts['inside_metatarsal_R'] = init
Ts['inside_phalanges_R'] = init
Ts['upper_limb_R'] = init
Ts['lower_limb_R'] = init
Ts['thigh_L'] = init
Ts['shin_L'] = init
Ts['foot_heel_L'] = init
Ts['foot_L'] = init
Ts['heel_L'] = init
Ts['outside_metatarsal_L'] = init
Ts['outside_phalanges_L'] = init
Ts['inside_metatarsal_L'] = init
Ts['inside_phalanges_L'] = init
Ts['upper_limb_L'] = init
Ts['lower_limb_L'] = init
return Ts
# viewer = ysv.SimpleViewer()
# viewer = hsv.hpSimpleViewer(rect=[0, 0, 1024, 768], viewForceWnd=False)
viewer = hsv.hpSimpleViewer(rect=[0, 0, 960 + 300, 1 + 1080 + 55],
viewForceWnd=False)
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('world', yr.VpWorldRenderer(vpWorld,
(150, 150, 150)))
viewer.doc.addRenderer(
'motionModel',
yr.VpModelRenderer(motionModel, (150, 150, 255), yr.POLYGON_FILL))
viewer.doc.setRendererVisible('motionModel', False)
viewer.doc.addRenderer(
'ikModel',
yr.VpModelRenderer(controlModel_ik, (150, 150, 255), yr.POLYGON_LINE))
viewer.doc.setRendererVisible('ikModel', False)
# viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
control_model_renderer = yr.VpModelRenderer(controlModel, (255, 240, 255),
yr.POLYGON_FILL)
viewer.doc.addRenderer('controlModel', control_model_renderer)
skeleton_renderer = None
if SKELETON_ON:
# skeleton_renderer = yr.BasicSkeletonRenderer(makeEmptyBasicSkeletonTransformDict(np.eye(4)), offset_Y=-0.08)
# skeleton_renderer = yr.BasicSkeletonRenderer(makeEmptyBasicSkeletonTransformDict(np.eye(4)), color=(230, 230, 230), offset_draw=(0.8, -0.02, 0.))
skeleton_renderer = yr.BasicSkeletonRenderer(
makeEmptyBasicSkeletonTransformDict(np.eye(4)),
color=(230, 230, 230),
offset_draw=(0., -0.0, 0.))
viewer.doc.addRenderer('skeleton', skeleton_renderer)
viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
# viewer.doc.setRendererVisible('rd_footCenter', False)
viewer.doc.addRenderer('rd_footCenter_ref',
yr.PointsRenderer(rd_footCenter_ref))
# viewer.doc.setRendererVisible('rd_footCenter_ref', False)
viewer.doc.addRenderer('rd_CM_plane',
yr.PointsRenderer(rd_CM_plane, (255, 255, 0)))
# viewer.doc.setRendererVisible('rd_CM_plane', False)
viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0, 255, 0)))
# viewer.doc.setRendererVisible('rd_CP', False)
viewer.doc.addRenderer('rd_CP_des',
yr.PointsRenderer(rd_CP_des, (255, 0, 255)))
# viewer.doc.setRendererVisible('rd_CP_des', False)
viewer.doc.addRenderer(
'rd_dL_des_plane',
yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255, 255, 0)))
# viewer.doc.setRendererVisible('rd_dL_des_plane', False)
viewer.doc.addRenderer('rd_dH_des',
yr.VectorsRenderer(rd_dH_des, rd_CM, (0, 255, 0)))
# viewer.doc.setRendererVisible('rd_dH_des', False)
# viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001))
viewer.doc.addRenderer('rd_CF',
yr.VectorsRenderer(rd_CF, rd_CF_pos, (255, 255, 0)))
# viewer.doc.setRendererVisible('rd_CF', False)
viewer.doc.addRenderer(
'rd_foot_ori',
yr.OrientationsRenderer(rd_foot_ori, rd_foot_pos, (255, 255, 0)))
viewer.doc.setRendererVisible('rd_foot_ori', False)
viewer.doc.addRenderer(
'rd_root_ori',
yr.OrientationsRenderer(rd_root_ori, rd_root_pos, (255, 255, 0)))
viewer.doc.setRendererVisible('rd_root_ori', False)
viewer.doc.addRenderer(
'extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos,
(0, 255, 0)))
viewer.doc.setRendererVisible('extraForce', False)
# viewer.doc.addRenderer('extraForceEnable', yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255,0,0)))
viewer.doc.addRenderer(
'extraForceEnable',
yr.WideArrowRenderer(rd_exfen_des,
extraForcePos, (255, 0, 0),
lineWidth=.05,
fromPoint=False))
# viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0)))
# viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0)))
# viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255)))
# viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0)))
# viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0)))
# viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255)))
# foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300, 400, 'foot contact modifier', controlModel)
foot_viewer = None # type: FootWindow
initKt = 25.
# initKt = 60.
initKl = 100.
initKh = 100.
initBl = .1
initBh = .13
# initSupKt = 17
initSupKt = 22
initFm = 50.0
initComX = 0.
initComY = 0.
initComZ = 0.
viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt)
viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl)
viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh)
viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl)
viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh)
viewer.objectInfoWnd.add1DSlider("SupKt", 0., 300., 0.1, initSupKt)
viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 1., initFm)
viewer.objectInfoWnd.add1DSlider("com X offset", -1., 1., 0.01, initComX)
viewer.objectInfoWnd.add1DSlider("com Y offset", -1., 1., 0.01, initComY)
viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.01, initComZ)
viewer.objectInfoWnd.add1DSlider("tiptoe angle", -0.5, .5, 0.001, 0.)
viewer.objectInfoWnd.add1DSlider("left tilt angle", -0.5, .5, 0.001, 0.)
viewer.objectInfoWnd.add1DSlider("right tilt angle", -0.5, .5, 0.001, 0.)
viewer.force_on = False
def viewer_SetForceState(object):
viewer.force_on = True
def viewer_GetForceState():
return viewer.force_on
def viewer_ResetForceState():
viewer.force_on = False
viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState)
viewer_ResetForceState()
offset = 60
viewer.objectInfoWnd.begin()
viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30 + offset * 9, 40,
20, 'X')
viewer.objectInfoWnd.labelForceX.value(0)
viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30 + offset * 9, 40,
20, 'Y')
viewer.objectInfoWnd.labelForceY.value(0)
viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30 + offset * 9, 40,
20, 'Z')
viewer.objectInfoWnd.labelForceZ.value(-1)
viewer.objectInfoWnd.labelForceDur = Fl_Value_Input(
220, 30 + offset * 9, 40, 20, 'Dur')
viewer.objectInfoWnd.labelForceDur.value(0.4)
viewer.objectInfoWnd.end()
# self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10)
def getParamVal(paramname):
return viewer.objectInfoWnd.getVal(paramname)
def getParamVals(paramnames):
return (getParamVal(name) for name in paramnames)
def setParamVal(paramname, val):
viewer.objectInfoWnd.setVal(paramname, val)
idDic = dict()
for i in range(motion[0].skeleton.getJointNum()):
idDic[motion[0].skeleton.getJointName(i)] = i
# extendedFootName = ['Foot_foot_0_0', 'Foot_foot_0_1', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0']
extendedFootName = [
'Foot_foot_0_0', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0'
]
lIDdic = {
'Left' + name: motion[0].skeleton.getJointIndex('Left' + name)
for name in extendedFootName
}
rIDdic = {
'Right' + name: motion[0].skeleton.getJointIndex('Right' + name)
for name in extendedFootName
}
footIdDic = lIDdic.copy()
footIdDic.update(rIDdic)
lIDlist = [
motion[0].skeleton.getJointIndex('Left' + name)
for name in extendedFootName
]
rIDlist = [
motion[0].skeleton.getJointIndex('Right' + name)
for name in extendedFootName
]
footIdlist = []
footIdlist.extend(lIDlist)
footIdlist.extend(rIDlist)
foot_left_idx = motion[0].skeleton.getJointIndex('LeftFoot')
foot_right_idx = motion[0].skeleton.getJointIndex('RightFoot')
foot_left_idx_temp = motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0')
foot_right_idx_temp = motion[0].skeleton.getJointIndex(
'RightFoot_foot_1_0')
# ik_solver = hik.numIkSolver(dartIkModel)
# ik_solver.clear()
# bodyIDsToCheck = rIDlist.copy()
joint_names = [
motion[0].skeleton.getJointName(i)
for i in range(motion[0].skeleton.getJointNum())
]
def fix_dofs(_DOFs, nested_dof_values, _mcfg, _joint_names):
fixed_nested_dof_values = list()
fixed_nested_dof_values.append(nested_dof_values[0])
for i in range(1, len(_DOFs)):
dof = _DOFs[i]
if dof == 1:
node = _mcfg.getNode(_joint_names[i])
axis = mm.unitZ()
if node.jointAxes[0] == 'X':
axis = mm.unitX()
elif node.jointAxes[0] == 'Y':
axis = mm.unitY()
fixed_nested_dof_values.append(
np.array([np.dot(nested_dof_values[i], axis)]))
else:
fixed_nested_dof_values.append(nested_dof_values[i])
return fixed_nested_dof_values
start_frame = 200
up_vec_in_each_link = dict()
for foot_id in footIdlist:
up_vec_in_each_link[
foot_id] = controlModel_ik.getBodyOrientationGlobal(foot_id)[1, :]
controlModel_ik.set_q(controlModel.get_q())
###################################
# simulate
###################################
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)
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
# viewer.play()
viewer.show()
foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300,
500, 'foot contact modifier', controlModel)
foot_viewer.show()
foot_viewer.check_all_seg()
viewer.motionViewWnd.goToFrame(0)
Fl.run()
def main():
# np.set_printoptions(precision=4, linewidth=200)
np.set_printoptions(precision=5,
threshold=np.inf,
suppress=True,
linewidth=3000)
motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped()
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
# controlModel_shadow_for_ik = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
# controlToMotionOffset = (1.5, -0.02, 0)
controlToMotionOffset = (1.5, 0, 0)
controlModel.translateByOffset(controlToMotionOffset)
# controlModel_shadow_for_ik.set_q(controlModel.get_q())
# controlModel_shadow_for_ik.computeJacobian(0, np.array([0., 0., 0.]))
wcfg_ik = copy.deepcopy(wcfg)
vpWorld_ik = cvw.VpWorld(wcfg_ik)
controlModel_ik = cvm.VpControlModel(vpWorld_ik, motion[0], mcfg)
vpWorld_ik.initialize()
controlModel_ik.set_q(controlModel.get_q())
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
foot_dofs = []
left_foot_dofs = []
right_foot_dofs = []
foot_seg_dofs = []
left_foot_seg_dofs = []
right_foot_seg_dofs = []
# for joint_idx in range(motion[0].skeleton.getJointNum()):
for joint_idx in range(controlModel.getJointNum()):
joint_name = controlModel.index2name(joint_idx)
# joint_name = motion[0].skeleton.getJointName(joint_idx)
if 'Foot' in joint_name:
foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx)
foot_dofs.extend(foot_dofs_temp)
if 'Left' in joint_name:
left_foot_dofs.extend(foot_dofs_temp)
elif 'Right' in joint_name:
right_foot_dofs.extend(foot_dofs_temp)
if 'foot' in joint_name:
foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx)
foot_seg_dofs.extend(foot_dofs_temp)
if 'Left' in joint_name:
left_foot_seg_dofs.extend(foot_dofs_temp)
elif 'Right' in joint_name:
right_foot_seg_dofs.extend(foot_dofs_temp)
# parameter
Kt = config['Kt']
Dt = config['Dt'] # tracking gain
Kl = config['Kl']
Dl = config['Dl'] # linear balance gain
Kh = config['Kh']
Dh = config['Dh'] # angular balance gain
Ks = config['Ks']
Ds = config['Ds'] # penalty force spring gain
Bt = config['Bt']
Bl = config['Bl']
Bh = config['Bh']
supL = motion[0].skeleton.getJointIndex(config['supLink1'])
supR = motion[0].skeleton.getJointIndex(config['supLink2'])
selectedBody = motion[0].skeleton.getJointIndex(config['end'])
constBody = motion[0].skeleton.getJointIndex('RightFoot')
# jacobian
# JsupL = yjc.makeEmptyJacobian(DOFs, 1)
# dJsupL = JsupL.copy()
# JsupPreL = JsupL.copy()
#
# JsupR = yjc.makeEmptyJacobian(DOFs, 1)
# dJsupR = JsupR.copy()
# JsupPreR = JsupR.copy()
Jconst = yjc.makeEmptyJacobian(DOFs, 1)
dJconst = Jconst.copy()
JconstPre = Jconst.copy()
Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum())
dJsys_temp = Jsys.copy()
JsysPre = Jsys.copy()
constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)]
allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton)
# momentum matrix
linkMasses = controlModel.getBodyMasses()
totalMass = controlModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
problem = yac.LSE(totalDOF, 12)
# a_sup = (0,0,0, 0,0,0) #ori
# a_sup = (0,0,0, 0,0,0) #L
CP_old = [mm.v3(0., 0., 0.)]
# penalty method
bodyIDsToCheck = list(range(vpWorld.getBodyNum()))
# mus = [1.]*len(bodyIDsToCheck)
mus = [.5] * len(bodyIDsToCheck)
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
# viewer
rd_footCenter = [None]
rd_footCenter_ref = [None]
rd_footCenterL = [None]
rd_footCenterR = [None]
rd_CM_plane = [None]
rd_CM = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_exf_des = [None]
rd_exfen_des = [None]
rd_root_des = [None]
rd_root_ori = [None]
rd_root_pos = [None]
rd_CF = [None]
rd_CF_pos = [None]
rootPos = [None]
selectedBodyId = [selectedBody]
extraForce = [None]
extraForcePos = [None]
rightFootVectorX = [None]
rightFootVectorY = [None]
rightFootVectorZ = [None]
rightFootPos = [None]
rightVectorX = [None]
rightVectorY = [None]
rightVectorZ = [None]
rightPos = [None]
def makeEmptyBasicSkeletonTransformDict(init=None):
Ts = dict()
Ts['pelvis'] = init
Ts['spine_ribs'] = init
Ts['head'] = init
Ts['thigh_R'] = init
Ts['shin_R'] = init
Ts['foot_R'] = init
Ts['upper_limb_R'] = init
Ts['lower_limb_R'] = init
Ts['thigh_L'] = init
Ts['shin_L'] = init
Ts['foot_L'] = init
Ts['upper_limb_L'] = init
Ts['lower_limb_L'] = init
return Ts
# viewer = ysv.SimpleViewer()
viewer = hsv.hpSimpleViewer(rect=[0, 0, 1024, 768], viewForceWnd=False)
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'motionModel',
yr.VpModelRenderer(motionModel, (150, 150, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'ikModel',
yr.VpModelRenderer(controlModel_ik, (150, 150, 255), yr.POLYGON_LINE))
# viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
control_model_renderer = yr.VpModelRenderer(controlModel, (255, 240, 255),
yr.POLYGON_FILL)
viewer.doc.addRenderer('controlModel', control_model_renderer)
skeleton_renderer = None
if SKELETON_ON:
skeleton_renderer = yr.BasicSkeletonRenderer(
makeEmptyBasicSkeletonTransformDict(np.eye(4)), offset_Y=-0.08)
viewer.doc.addRenderer('skeleton', skeleton_renderer)
viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
viewer.doc.addRenderer('rd_footCenter_ref',
yr.PointsRenderer(rd_footCenter_ref))
viewer.doc.addRenderer('rd_CM_plane',
yr.PointsRenderer(rd_CM_plane, (255, 255, 0)))
viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0, 255, 0)))
viewer.doc.addRenderer('rd_CP_des',
yr.PointsRenderer(rd_CP_des, (255, 0, 255)))
viewer.doc.addRenderer(
'rd_dL_des_plane',
yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255, 255, 0)))
viewer.doc.addRenderer('rd_dH_des',
yr.VectorsRenderer(rd_dH_des, rd_CM, (0, 255, 0)))
# viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001))
viewer.doc.addRenderer('rd_CF',
yr.VectorsRenderer(rd_CF, rd_CF_pos, (255, 255, 0)))
viewer.doc.addRenderer(
'rd_root_ori',
yr.OrientationsRenderer(rd_root_ori, rd_root_pos, (255, 255, 0)))
viewer.doc.addRenderer(
'extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos,
(0, 255, 0)))
viewer.doc.addRenderer(
'extraForceEnable',
yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255, 0, 0)))
# viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0)))
# viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0)))
# viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255)))
# viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0)))
# viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0)))
# viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255)))
# foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300, 400, 'foot contact modifier', controlModel)
foot_viewer = None # type: FootWindow
# success!!
# initKt = 50
# initKl = 10.1
# initKh = 3.1
# initBl = .1
# initBh = .1
# initSupKt = 21.6
# initFm = 100.0
# success!! -- 2015.2.12. double stance
# initKt = 50
# initKl = 37.1
# initKh = 41.8
# initBl = .1
# initBh = .13
# initSupKt = 21.6
# initFm = 165.0
# single stance
# initKt = 25
# initKl = 80.1
# initKh = 10.8
# initBl = .1
# initBh = .13
# initSupKt = 21.6
# initFm = 50.0
# single stance -> double stance
# initKt = 25
# initKl = 60.
# initKh = 20.
# initBl = .1
# initBh = .13
# initSupKt = 21.6
# initFm = 50.0
initKt = 25
initKl = 100.
initKh = 100.
initBl = .1
initBh = .13
initSupKt = 17
initFm = 50.0
initComX = 0.
initComY = 0.
initComZ = 0.
viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt)
viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl)
viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh)
viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl)
viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh)
viewer.objectInfoWnd.add1DSlider("SupKt", 0., 300., 0.1, initSupKt)
viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 10., initFm)
viewer.objectInfoWnd.add1DSlider("com X offset", -1., 1., 0.01, initComX)
viewer.objectInfoWnd.add1DSlider("com Y offset", -1., 1., 0.01, initComY)
viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.01, initComZ)
viewer.force_on = False
def viewer_SetForceState(object):
viewer.force_on = True
def viewer_GetForceState():
return viewer.force_on
def viewer_ResetForceState():
viewer.force_on = False
viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState)
viewer_ResetForceState()
offset = 60
viewer.objectInfoWnd.begin()
viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30 + offset * 9, 40,
20, 'X')
viewer.objectInfoWnd.labelForceX.value(0)
viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30 + offset * 9, 40,
20, 'Y')
viewer.objectInfoWnd.labelForceY.value(0)
viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30 + offset * 9, 40,
20, 'Z')
viewer.objectInfoWnd.labelForceZ.value(1)
viewer.objectInfoWnd.labelForceDur = Fl_Value_Input(
220, 30 + offset * 9, 40, 20, 'Dur')
viewer.objectInfoWnd.labelForceDur.value(0.1)
viewer.objectInfoWnd.end()
# self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10)
def getParamVal(paramname):
return viewer.objectInfoWnd.getVal(paramname)
def getParamVals(paramnames):
return (getParamVal(name) for name in paramnames)
extendedFootName = [
'Foot_foot_0_0', 'Foot_foot_0_1', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0',
'Foot_foot_1_0'
]
lIDdic = {
'Left' + name: motion[0].skeleton.getJointIndex('Left' + name)
for name in extendedFootName
}
rIDdic = {
'Right' + name: motion[0].skeleton.getJointIndex('Right' + name)
for name in extendedFootName
}
footIdDic = lIDdic.copy()
footIdDic.update(rIDdic)
lIDlist = [
motion[0].skeleton.getJointIndex('Left' + name)
for name in extendedFootName
]
rIDlist = [
motion[0].skeleton.getJointIndex('Right' + name)
for name in extendedFootName
]
footIdlist = []
footIdlist.extend(lIDlist)
footIdlist.extend(rIDlist)
foot_left_idx = motion[0].skeleton.getJointIndex('LeftFoot')
foot_right_idx = motion[0].skeleton.getJointIndex('RightFoot')
foot_left_idx_temp = motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0')
foot_right_idx_temp = motion[0].skeleton.getJointIndex(
'RightFoot_foot_1_0')
def get_jacobianbase_and_masks(skeleton, DOFs, joint_idx):
J = yjc.makeEmptyJacobian(DOFs, 1)
joint_masks = [yjc.getLinkJointMask(skeleton, joint_idx)]
return J, joint_masks
# ik_solver = hik.numIkSolver(dartIkModel)
# ik_solver.clear()
###################################
# simulate
###################################
def simulateCallback(frame):
# print(frame)
# print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0']))
if False and viewer_GetForceState():
# print('force on, frame: ', frame)
motion[frame].mulJointOrientationLocal(
footIdDic['LeftFoot_foot_0_0_0'],
mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6))
motion[frame].mulJointOrientationLocal(
footIdDic['LeftFoot_foot_0_1_0'],
mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6))
motion[frame].mulJointOrientationLocal(
footIdDic['RightFoot_foot_0_0_0'],
mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6))
motion[frame].mulJointOrientationLocal(
footIdDic['RightFoot_foot_0_1_0'],
mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6))
# print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0']))
motionModel.update(motion[frame])
controlModel_ik.set_q(controlModel.get_q())
global g_initFlag
global forceShowTime
global JsysPre
global JsupPreL
global JsupPreR
global JconstPre
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
# Kt, Kl, Kh, Bl, Bh, kt_sup = viewer.GetParam()
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2 * (Kt**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
dt_sup = 2 * (kt_sup**.5)
doubleTosingleOffset = 0.15
singleTodoubleOffset = 0.30
# doubleTosingleOffset = 0.09
doubleTosingleVelOffset = 0.0
# tracking
th_r = motion.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r,
Kt, Dt)
ype.flatten(ddth_des, ddth_des_flat)
# ddth_des_flat = Kt * (motion.get_q(frame) - np.array(controlModel.get_q())) - Dt * np.array(controlModel.get_dq())
ype.flatten(dth, dth_flat)
# dth_flat = np.array(controlModel.get_dq())
#################################################
# jacobian
#################################################
contact_des_ids = list() # desired contact segments
if foot_viewer.check_om_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0'))
if foot_viewer.check_op_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0'))
if foot_viewer.check_im_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1'))
if foot_viewer.check_ip_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0'))
if foot_viewer.check_h_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0'))
if foot_viewer.check_om_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_0'))
if foot_viewer.check_op_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0'))
if foot_viewer.check_im_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_1'))
if foot_viewer.check_ip_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0'))
if foot_viewer.check_h_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_1_0'))
contact_ids = list() # temp idx for balancing
contact_ids.extend(contact_des_ids)
contact_joint_ori = list(
map(controlModel.getJointOrientationGlobal, contact_ids))
contact_joint_pos = list(
map(controlModel.getJointPositionGlobal, contact_ids))
contact_body_ori = list(
map(controlModel.getBodyOrientationGlobal, contact_ids))
contact_body_pos = list(
map(controlModel.getBodyPositionGlobal, contact_ids))
contact_body_vel = list(
map(controlModel.getBodyVelocityGlobal, contact_ids))
contact_body_angvel = list(
map(controlModel.getBodyAngVelocityGlobal, contact_ids))
ref_joint_ori = list(
map(motion[frame].getJointOrientationGlobal, contact_ids))
ref_joint_pos = list(
map(motion[frame].getJointPositionGlobal, contact_ids))
ref_joint_vel = [
motion.getJointVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_joint_angvel = [
motion.getJointAngVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_body_ori = list(
map(motionModel.getBodyOrientationGlobal, contact_ids))
ref_body_pos = list(map(motionModel.getBodyPositionGlobal,
contact_ids))
# ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids))
ref_body_angvel = [
motion.getJointAngVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_body_vel = [
ref_joint_vel[i] +
np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i])
for i in range(len(ref_joint_vel))
]
J_contacts = [
yjc.makeEmptyJacobian(DOFs, 1) for i in range(len(contact_ids))
]
dJ_contacts = [
yjc.makeEmptyJacobian(DOFs, 1) for i in range(len(contact_ids))
]
joint_masks = [
yjc.getLinkJointMask(motion[0].skeleton, joint_idx)
for joint_idx in contact_ids
]
# caution!! body orientation and joint orientation of foot are totally different!!
footOriL = controlModel.getJointOrientationGlobal(supL)
footOriR = controlModel.getJointOrientationGlobal(supR)
# desire footCenter[1] = 0.041135
# desire footCenter[1] = 0.0197
footCenterL = controlModel.getBodyPositionGlobal(supL)
footCenterR = controlModel.getBodyPositionGlobal(supR)
footBodyOriL = controlModel.getBodyOrientationGlobal(supL)
footBodyOriR = controlModel.getBodyOrientationGlobal(supR)
footBodyVelL = controlModel.getBodyVelocityGlobal(supL)
footBodyVelR = controlModel.getBodyVelocityGlobal(supR)
footBodyAngVelL = controlModel.getBodyAngVelocityGlobal(supL)
footBodyAngVelR = controlModel.getBodyAngVelocityGlobal(supR)
refFootL = motionModel.getBodyPositionGlobal(supL)
refFootR = motionModel.getBodyPositionGlobal(supR)
refFootVelL = motionModel.getBodyVelocityGlobal(supL)
refFootVelR = motionModel.getBodyVelocityGlobal(supR)
refFootAngVelL = motionModel.getBodyAngVelocityGlobal(supL)
refFootAngVelR = motionModel.getBodyAngVelocityGlobal(supR)
refFootJointVelR = motion.getJointVelocityGlobal(supR, frame)
refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootJointR = motion.getJointPositionGlobal(supR, frame)
refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR,
(refFootR - refFootJointR))
refFootJointVelL = motion.getJointVelocityGlobal(supL, frame)
refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
refFootJointL = motion.getJointPositionGlobal(supL, frame)
refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL,
(refFootL - refFootJointL))
is_contact = [1] * len(contact_ids)
contactR = 1
contactL = 1
if refFootVelR[1] < 0 and refFootVelR[1] / 30. + refFootR[
1] > singleTodoubleOffset:
contactR = 0
if refFootVelL[1] < 0 and refFootVelL[1] / 30. + refFootL[
1] > singleTodoubleOffset:
contactL = 0
if refFootVelR[1] > 0 and refFootVelR[1] / 30. + refFootR[
1] > doubleTosingleOffset:
contactR = 0
if refFootVelL[1] > 0 and refFootVelL[1] / 30. + refFootL[
1] > doubleTosingleOffset:
contactL = 0
# contactR = 1
contMotionOffset = th[0][0] - th_r[0][0]
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
CM = yrp.getCM(linkPositions, linkMasses, totalMass)
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
CM_plane = copy.copy(CM)
CM_plane[1] = 0.
dCM_plane = copy.copy(dCM)
dCM_plane[1] = 0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
# calculate contact state
# if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08:
if g_initFlag == 1:
# contact state
# 0: flying 1: right only 2: left only 3: double
# if contact == 2 and refFootR[1] < doubleTosingleOffset:
if contact == 2 and contactR == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
# elif contact == 3 and refFootL[1] < doubleTosingleOffset:
elif contact == 1 and contactL == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
# elif contact == 3 and refFootR[1] > doubleTosingleOffset:
elif contact == 3 and contactR == 0:
contact = 2
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
# elif contact == 3 and refFootL[1] > doubleTosingleOffset:
elif contact == 3 and contactL == 0:
contact = 1
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
else:
contact = 0
# if refFootR[1] < doubleTosingleOffset:
if contactR == 1:
contact += 1
# if refFootL[1] < doubleTosingleOffset:
if contactL == 1:
contact += 2
# initialization
if g_initFlag == 0:
# JsysPre = Jsys.copy()
JconstPre = Jconst.copy()
softConstPoint = footCenterR.copy()
# yjc.computeJacobian2(JsysPre, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks)
# yjc.computeJacobian2(JconstPre, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks)
footCenter = footCenterL + (footCenterR - footCenterL) / 2.0
footCenter[1] = 0.
preFootCenter = footCenter.copy()
# footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL)
# footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR)
if refFootR[1] < doubleTosingleOffset:
contact += 1
if refFootL[1] < doubleTosingleOffset:
contact += 2
g_initFlag = 1
# calculate jacobian
Jsys, dJsys = controlModel.computeCom_J_dJdq()
for i in range(len(J_contacts)):
J_contacts[i] = Jsys[6 * contact_ids[i]:6 * contact_ids[i] + 6, :]
dJ_contacts[i] = dJsys[6 * contact_ids[i]:6 * contact_ids[i] + 6]
# calculate footCenter
footCenter = sum(contact_body_pos) / len(contact_body_pos) if len(contact_body_pos) > 0 \
else .5 * (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(supR))
# if len(contact_body_pos) > 2:
# hull = ConvexHull(contact_body_pos)
footCenter_ref = sum(ref_body_pos) / len(ref_body_pos) if len(ref_body_pos) > 0 \
else .5 * (motionModel.getBodyPositionGlobal(supL) + motionModel.getBodyPositionGlobal(supR))
footCenter_ref = footCenter_ref + contMotionOffset
# if len(ref_body_pos) > 2:
# hull = ConvexHull(ref_body_pos)
footCenter_ref[1] = 0.
# footCenter = footCenterL + (footCenterR - footCenterL)/2.0
# if refFootR[1] >doubleTosingleOffset:
# if refFootR[1] > doubleTosingleOffset or footCenterR[1] > 0.08:
# if contact == 1 or footCenterR[1] > 0.08:
# if contact == 2 or footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
footCenter = footCenterL.copy()
# elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
footCenter = footCenterR.copy()
footCenter[1] = 0.
footCenter[0] = footCenter[0] + getParamVal('com X offset')
if contactChangeCount > 0 and contactChangeType == 'StoD':
# change footcenter gradually
footCenter = preFootCenter + (
maxContactChangeCount - contactChangeCount) * (
footCenter - preFootCenter) / maxContactChangeCount
preFootCenter = footCenter.copy()
# linear momentum
# TODO:
# We should consider dCM_ref, shouldn't we?
# add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel!
# to do that, set joint velocities to vpModel
CM_ref_plane = footCenter
# CM_ref_plane = footCenter_ref
dL_des_plane = Kl * totalMass * (CM_ref_plane -
CM_plane) - Dl * totalMass * dCM_plane
# dL_des_plane[1] = 0.
# print('dCM_plane : ', np.linalg.norm(dCM_plane))
# angular momentum
CP_ref = footCenter
# CP_ref = footCenter_ref
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
# bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0]) / (1 / 30.)
CP_old[0] = CP
if CP is not None and dCP is not None:
ddCP_des = Kh * (CP_ref - CP) - Dh * dCP
dCP_des = dCP + ddCP_des * (1 / 30.)
CP_des = CP + dCP_des * (1 / 30.)
# CP_des = footCenter
CP_des = CP + dCP * (1 / 30.) + .5 * ddCP_des * ((1 / 30.)**2)
dH_des = np.cross(
(CP_des - CM),
(dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
if contactChangeCount > 0: # and contactChangeType == 'DtoS':
dH_des *= (maxContactChangeCount -
contactChangeCount) / maxContactChangeCount
else:
dH_des = None
# convex hull
contact_pos_2d = np.asarray([
np.array([contactPosition[0], contactPosition[2]])
for contactPosition in contactPositions
])
p = np.array([CM_plane[0], CM_plane[2]])
# hull = None # type: Delaunay
# if contact_pos_2d.shape[0] > 0:
# hull = Delaunay(contact_pos_2d)
# print(hull.find_simplex(p) >= 0)
# set up equality constraint
# TODO:
# logSO3 is just q'', not acceleration.
# To make a_oris acceleration, q'' -> a will be needed
# body_ddqs = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori]))
body_ddqs = list(
map(mm.logSO3, [
np.dot(
contact_body_ori[i].T,
np.dot(
ref_body_ori[i],
mm.getSO3FromVectors(
np.dot(ref_body_ori[i], mm.unitY()), mm.unitY())))
for i in range(len(contact_body_ori))
]))
body_qs = list(map(mm.logSO3, contact_body_ori))
body_angs = [
np.dot(contact_body_ori[i], contact_body_angvel[i])
for i in range(len(contact_body_ori))
]
body_dqs = [
mm.vel2qd(body_angs[i], body_qs[i]) for i in range(len(body_angs))
]
a_oris = [
np.dot(contact_body_ori[i],
mm.qdd2accel(body_ddqs[i], body_dqs[i], body_qs[i]))
for i in range(len(contact_body_ori))
]
# body_ddq = body_ddqs[0]
# body_ori = contact_body_ori[0]
# body_ang = np.dot(body_ori.T, contact_body_angvel[0])
#
# body_q = mm.logSO3(body_ori)
# body_dq = mm.vel2qd(body_ang, body_q)
# a_ori = np.dot(body_ori, mm.qdd2accel(body_ddq, body_dq, body_q))
# a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori]))
a_sups = [
np.append(
kt_sup *
(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) +
dt_sup * (ref_body_vel[i] - contact_body_vel[i]),
kt_sup * a_oris[i] + dt_sup *
(ref_body_angvel[i] - contact_body_angvel[i]))
for i in range(len(a_oris))
]
# momentum matrix
RS = np.dot(P, Jsys)
R, S = np.vsplit(RS, 2)
# rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat)
rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsys)
r_bias, s_bias = np.hsplit(rs, 2)
#######################################################
# optimization
#######################################################
# if contact == 2 and footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
config['weightMap']['RightUpLeg'] = .8
config['weightMap']['RightLeg'] = .8
config['weightMap']['RightFoot'] = .8
else:
config['weightMap']['RightUpLeg'] = .1
config['weightMap']['RightLeg'] = .25
config['weightMap']['RightFoot'] = .2
# if contact == 1 and footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
config['weightMap']['LeftUpLeg'] = .8
config['weightMap']['LeftLeg'] = .8
config['weightMap']['LeftFoot'] = .8
else:
config['weightMap']['LeftUpLeg'] = .1
config['weightMap']['LeftLeg'] = .25
config['weightMap']['LeftFoot'] = .2
w = mot.getTrackingWeight(DOFs, motion[0].skeleton,
config['weightMap'])
# if contact == 2:
# mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
if dH_des is not None:
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
# mot.setConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup)
# mot.addConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup)
# if contact & 1 and contactChangeCount == 0:
if True:
for c_idx in range(len(contact_ids)):
# mot.addConstraint(problem, totalDOF, J_contacts[c_idx], dJ_contacts[c_idx], dth_flat, a_sups[c_idx])
mot.addConstraint2(problem, totalDOF, J_contacts[c_idx],
dJ_contacts[c_idx], dth_flat,
a_sups[c_idx])
if contactChangeCount > 0:
contactChangeCount = contactChangeCount - 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
r = problem.solve()
problem.clear()
ddth_sol_flat = np.asarray(r['x'])
# ddth_sol_flat[foot_seg_dofs] = np.array(ddth_des_flat)[foot_seg_dofs]
ype.nested(ddth_sol_flat, ddth_sol)
rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
for i in range(stepsPerFrame):
# apply penalty force
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
# bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
controlModel.setDOFAccelerations(ddth_sol)
# controlModel.setDOFAccelerations(ddth_des)
# controlModel.set_ddq(ddth_sol_flat)
# controlModel.set_ddq(ddth_des_flat)
controlModel.solveHybridDynamics()
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([
viewer.objectInfoWnd.labelForceX.value(),
viewer.objectInfoWnd.labelForceY.value(),
viewer.objectInfoWnd.labelForceZ.value()
])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += wcfg.timeStep
vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
vpWorld.step()
controlModel_ik.set_q(controlModel.get_q())
if foot_viewer is not None:
foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info(
frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds)
foot_viewer.foot_pressure_gl_window.goToFrame(frame)
# rendering
for foot_seg_id in footIdlist:
control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255)
for contact_id in contact_ids:
control_model_renderer.body_colors[contact_id] = (255, 0, 0)
rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0]))
rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0]))
rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0, .1]))
rightFootPos[0] = footCenterL
rightVectorX[0] = np.dot(footBodyOriL, np.array([.1, 0, 0]))
rightVectorY[0] = np.dot(footBodyOriL, np.array([0, .1, 0]))
rightVectorZ[0] = np.dot(footBodyOriL, np.array([0, 0, .1]))
rightPos[0] = footCenterL + np.array([.1, 0, 0])
rd_footCenter[0] = footCenter
rd_footCenter_ref[0] = footCenter_ref
rd_footCenterL[0] = footCenterL
rd_footCenterR[0] = footCenterR
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
rd_CM_plane[0][1] = 0.
if CP is not None and dCP is not None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des
rd_dL_des_plane[0] = [
dL_des_plane[0] / 100, dL_des_plane[1] / 100,
dL_des_plane[2] / 100
]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity)
rd_root_des[0] = rootPos[0]
rd_root_ori[0] = controlModel.getBodyOrientationGlobal(0)
rd_root_pos[0] = controlModel.getBodyPositionGlobal(0)
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append(contactForces[i] / 400)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exf_des[0] = [0, 0, 0]
else:
rd_exf_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exfen_des[0] = [0, 0, 0]
extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody)
# render contact_ids
# render skeleton
if SKELETON_ON:
Ts = dict()
Ts['pelvis'] = controlModel.getJointTransform(0)
Ts['thigh_R'] = controlModel.getJointTransform(1)
Ts['shin_R'] = controlModel.getJointTransform(2)
Ts['foot_R'] = controlModel.getJointTransform(3)
Ts['spine_ribs'] = controlModel.getJointTransform(9)
Ts['head'] = controlModel.getJointTransform(10)
Ts['upper_limb_R'] = controlModel.getJointTransform(13)
Ts['lower_limb_R'] = controlModel.getJointTransform(14)
Ts['thigh_L'] = controlModel.getJointTransform(15)
Ts['shin_L'] = controlModel.getJointTransform(16)
Ts['foot_L'] = controlModel.getJointTransform(17)
Ts['upper_limb_L'] = controlModel.getJointTransform(11)
Ts['lower_limb_L'] = controlModel.getJointTransform(12)
skeleton_renderer.appendFrameState(Ts)
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
# viewer.play()
viewer.show()
foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300,
500, 'foot contact modifier', controlModel)
foot_viewer.show()
foot_viewer.check_op_l.value(True)
foot_viewer.check_om_l.value(True)
foot_viewer.check_h_l.value(True)
foot_viewer.check_op_r.value(True)
foot_viewer.check_om_r.value(True)
foot_viewer.check_h_r.value(True)
Fl.run()
