def __init__(self,
model_path,
frame_skip,
observation_size,
action_bounds,
dt=0.002):
pydart.init()
print('pydart initialization OK')
if model_path.startswith("/"):
fullpath = model_path
else:
fullpath = os.path.join(os.path.dirname(__file__), "assets",
model_path)
if not path.exists(fullpath):
raise IOError("File %s does not exist" % fullpath)
self.dart_world = pydart.World(dt, fullpath)
self.robot_skeleton = self.dart_world.skeletons[
-1] # assume that the skeleton of interest is always the last one
for jt in range(0, len(self.robot_skeleton.joints)):
if self.robot_skeleton.joints[jt].has_position_limit(0):
self.robot_skeleton.joints[jt].set_position_limit_enforced(
True)
self.frame_skip = frame_skip
self.viewer = None
observation, _reward, done, _info = self._step(
np.zeros(len(action_bounds[0])))
assert not done
self.obs_dim = observation_size
self.act_dim = len(action_bounds[0])
self.action_space = spaces.Box(action_bounds[1], action_bounds[0])
self.track_skeleton_id = -1 # track the last skeleton's com by default
high = np.inf * np.ones(self.obs_dim)
low = -high
self.observation_space = spaces.Dict({
"observation":
spaces.Box(low, high),
"mass":
spaces.Box(np.array([0]), np.array([10]))
})
# self.observation_space = spaces.Box(low, high)
self._seed()
# self.viewer = None
self.metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second': int(np.round(1.0 / self.dt))
}
def main():
MOTION_ONLY = False
pydart.init()
env_name = 'multi'
ppo = PPO(env_name, 1, visualize_only=True)
if not MOTION_ONLY:
ppo.LoadModel('model/' + env_name + '.pt')
ppo.env.specify_motion_num(1)
ppo.env.Resets(False)
# viewer settings
rd_contact_positions = [None]
rd_contact_forces = [None]
dart_world = ppo.env.world
viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False)
viewer.doc.addRenderer(
'MotionModel',
yr.DartRenderer(ppo.env.ref_world, (150, 150, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'controlModel',
yr.DartRenderer(dart_world, (255, 240, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'contact',
yr.VectorsRenderer(rd_contact_forces, rd_contact_positions,
(255, 0, 0)))
def preCallback(frame):
ppo.env.ref_skel.set_positions(ppo.env.ref_motion.get_q(frame))
def simulateCallback(frame):
state = ppo.env.GetState(0)
action_dist, _ = ppo.model(torch.tensor(state.reshape(1, -1)).float())
action = action_dist.loc.detach().numpy()
res = ppo.env.Steps(action)
# 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)
if MOTION_ONLY:
viewer.setPreFrameCallback_Always(preCallback)
else:
viewer.setSimulateCallback(simulateCallback)
viewer.setMaxFrame(len(ppo.env.ref_motion) - 1)
viewer.startTimer(1 / ppo.env.ref_motion.fps)
viewer.show()
Fl.run()
def main():
fGraph = build_graph()
def get_info(x):
temp = x.split()
return tuple([int(temp[0]), int(temp[1]), float(temp[2])])
motion_idx = [None, None]
with open('mg.txt', 'r') as f:
s = f.read().splitlines(False)
total_num = int(s[0])
motion_info = list(map(get_info, s[1:]))
pydart.init()
world = [
pydart.World(1. / 1200., "../DartDeep/data/woody_with_ground_v2.xml")
for _ in range(2)
]
# viewer settings
rd_contact_positions = [None]
rd_contact_forces = [None]
viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False)
viewer.doc.addRenderer('motionModel[0]',
yr.DartRenderer(world[0], (255, 240, 255)))
viewer.doc.addRenderer('motionModel[1]', yr.DartRenderer(world[1]))
motion_state = [0]
def callback_0(_):
motion_state[0] = 0
def callback_1(_):
motion_state[0] = 1
def callback_2(_):
motion_state[0] = 2
viewer.objectInfoWnd.addBtn('0', callback_0)
viewer.objectInfoWnd.addBtn('1', callback_1)
viewer.objectInfoWnd.addBtn('2', callback_2)
def postCallback(frame):
for i in range(2):
world[i].skeletons[1].set_positions(
fGraph.motion_frames.get_q(motion_info[frame][i]))
print(frame, motion_info[frame])
viewer.setPreFrameCallback_Always(postCallback)
viewer.setMaxFrame(total_num - 1)
viewer.startTimer(1. / 30.)
viewer.show()
Fl.run()
def __init__(self, folder):
pydart.init()
self.world = pydart.World(1./1200., "../data/cmu_with_ground.xml")
self.model = self.world.skeletons[1]
self.ik = DartIk(self.model)
self.controller = RNNController(folder)
self.all_angles = [[] for i in range(93)]
viewer = SimpleViewer(rect=[0, 0, 1280+300, 720+1+55], viewForceWnd=False)
self.viewer = viewer
# viewer.record(True)
viewer.record(False)
viewer.setMaxFrame(10000)
self.isFirst = True
self.lines = None
viewer.motionViewWnd.glWindow.set_mouse_pick(True)
def callback_btn(ptr):
self.controller.reset()
viewer.objectInfoWnd.addBtn('reset', callback_btn)
self.rc = yr.RenderContext()
self.rd_target_position = [None]
self.rd_frames = [None]
viewer.doc.addRenderer('contact', yr.PointsRenderer(self.rd_target_position, (0, 255, 0), save_state=False))
viewer.doc.addRenderer('MotionModel', yr.DartRenderer(self.world, (150,150,255), yr.POLYGON_FILL, save_state=False))
viewer.doc.addRenderer('rd_frames', yr.FramesRenderer(self.rd_frames))
def extraDrawCallback():
self.rd_target_position[0] = self.viewer.motionViewWnd.glWindow.pickPoint
self.step_model()
del self.rd_frames[:]
self.rd_frames.append(self.model.body(0).world_transform())
# for i in range(3):
# print(self.model.body(0).world_transform()[:3, i])
viewer.setExtraDrawCallback(extraDrawCallback)
viewer.startTimer(1. / 30.)
viewer.show()
# viewer.play()
Fl.run()
def main():
from time import strftime
pydart.init()
args = common_arg_parser().parse_args()
logger.configure(dir='./log'+strftime("%Y%m%d%H%M")+'/')
model, env = train(num_timesteps=10000000, seed=args.seed)
logger.log("Running trained model")
obs = np.zeros((env.num_envs,) + env.observation_space.shape)
obs[:] = env.reset()
while True:
actions = model.step(obs)
res = env.step(actions)
obs[:] = res[0]
done = res[2]
if done[0]:
break
def __init__(self, runtime, hyperParam=None):
pydart.init()
#print('pydart initialization OK')
self.runtime = runtime
self.world = trainingWorldR650.trainingWorldR650(step=0.1,
setGravity=False)
#print('pydart create_world OK')
self.skel = self.world.skeletons[
-1] #print(world.skeletons[-1]): [Skeleton(2): KR5sixxR650WP_description]
self.hyperParams = random.random((5, 3))
self.fitness = -1
def main():
from fltk import Fl
from PyCommon.modules.GUI import hpSimpleViewer as hsv
from PyCommon.modules.Renderer import ysRenderer as yr
pydart.init()
args = mujoco_arg_parser().parse_args()
logger.configure(dir="./run/")
model, env = load(args.env, num_timesteps=10000000, seed=args.seed)
logger.log("Running trained model")
obs = np.zeros((env.num_envs, ) + env.observation_space.shape)
obs[:] = env.reset()
env.venv.envs[0].env.rsi = False
dart_world = env.venv.envs[0].env.world
viewer = hsv.hpSimpleViewer(viewForceWnd=False)
viewer.doc.addRenderer(
'controlModel',
yr.DartRenderer(dart_world, (150, 150, 255), yr.POLYGON_FILL))
def simulateCallback(frame):
actions = model.step(obs)
res = env.step(actions[0])
obs[:] = res[0]
done = res[2]
# if done[0]:
# break
# env.render()
viewer.setSimulateCallback(simulateCallback)
viewer.setMaxFrame(3000)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
class DampingController(object):
""" Add damping force to the skeleton """
def __init__(self, skel):
self.skel = skel
def compute(self):
damping = -0.01 * self.skel.dq
damping[1::3] *= 0.1
return damping
if __name__ == '__main__':
import pydart2 as pydart
pydart.init(verbose=True)
print('pydart initialization OK')
world = pydart.World(0.0002, './data/skel/chain.skel')
print('pydart create_world OK')
skel = world.skeletons[0]
skel.q = (np.random.rand(skel.ndofs) - 0.5)
print('init pose = %s' % skel.q)
skel.controller = DampingController(skel)
pydart.gui.viewer.launch(world)
# # Or, you can manually create the window...
# win = pydart.gui.viewer.PydartWindow(world)
# win.camera_event(1)
import tensorflow as tf
from DartDeep.dart_env_v2_1 import HpDartEnv
from DartDeep.tf.ppo_tf import *
from DartDeep.tf.worker import Worker
import pydart2
tf.reset_default_graph()
global_episodes = tf.Variable(0,
dtype=tf.int32,
name='global_episodes',
trainable=False)
summary_writer = tf.summary.FileWriter('./summary_log/' + 'Walking')
pydart2.init()
env = HpDartEnv()
chief = Worker('Walking',
env,
summary_writer,
global_episodes,
visualize=False,
gamma=0.95,
batch_size=128,
a_lr=5e-5,
c_lr=1e-2)
with tf.Session() as sess:
saver = tf.train.Saver(max_to_keep=5)
sess.run(tf.global_variables_initializer())
chief.ppo.load_model(sess, saver)
chief.process(sess, saver)
def init_world(self):
pydart.init(verbose=False)
print('pydart initialization OK')
self.world = MyWorld(self.dt)
def main():
# np.set_printoptions(precision=4, linewidth=200)
np.set_printoptions(precision=4, linewidth=1000, threshold=np.inf)
#motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5()
motion, mcfg, wcfg, stepsPerFrame, config, frame_rate= mit.create_biped()
#motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped()
frame_step_size = 1./frame_rate
pydart.init()
dartModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON)
dartMotionModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON)
# wcfg.lockingVel = 0.01
time_step = dartModel.world.time_step()
# dartModel.initializeForwardDynamics()
# dartModel.initializeHybridDynamics()
#controlToMotionOffset = (1.5, -0.02, 0)
controlToMotionOffset = (1.5, 0, 0)
# dartModel.translateByOffset(controlToMotionOffset)
totalDOF = dartModel.getTotalDOF()
DOFs = dartModel.getDOFs()
# 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']
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
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')
# momentum matrix
linkMasses = dartModel.getBodyMasses()
print([body.name for body in dartModel.skeleton.bodynodes])
print(linkMasses)
totalMass = dartModel.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
a_supL = (0,0,0, 0,0,0)
a_supR = (0,0,0, 0,0,0)
a_sup_2 = (0,0,0, 0,0,0, 0,0,0, 0,0,0)
CP_old = [mm.v3(0.,0.,0.)]
CP_des = [None]
dCP_des = [np.zeros(3)]
# penalty method
# bodyIDsToCheck = range(dartModel.getBodyNum())
bodyIDsToCheck = [supL, supR]
#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_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_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]
viewer = hsv.hpSimpleViewer(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.DartModelRenderer(dartMotionModel, (150,150,255), yr.POLYGON_FILL))
viewer.doc.addRenderer('controlModel', yr.DartRenderer(dartModel.world, (255,240,255), yr.POLYGON_FILL))
viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
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,0,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)))
#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 = 11.
# initKh = 22.
initKl = 100.
initKh = 100.
initBl = .1
initBh = .13
initSupKt = 17.
# initSupKt = 2.5
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., 100., 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)
pdcontroller = PDController(dartModel, dartModel.skeleton, wcfg.timeStep, Kt, Dt)
def getParamVal(paramname):
return viewer.objectInfoWnd.getVal(paramname)
def getParamVals(paramnames):
return (getParamVal(name) for name in paramnames)
ik_solver = hikd.numIkSolver(dartMotionModel)
body_num = dartModel.getBodyNum()
# dJsys = np.zeros((6*body_num, totalDOF))
# dJsupL = np.zeros((6, totalDOF))
# dJsupR = np.zeros((6, totalDOF))
# Jpre = [np.zeros((6*body_num, totalDOF)), np.zeros((6, totalDOF)), np.zeros((6, totalDOF))]
extendedFootName = ['Foot']
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}
lIDlist = [motion[0].skeleton.getJointIndex('Left'+name) for name in extendedFootName]
rIDlist = [motion[0].skeleton.getJointIndex('Right'+name) for name in extendedFootName]
bodyIDs, contactPositions, contactPositionLocals, contactForces = [], [], [], []
###################################
# simulate
###################################
def simulateCallback(frame):
# print()
# print(dartModel.getJointVelocityGlobal(0))
# print(dartModel.getDOFVelocities()[0])
# print(dartModel.get_dq()[:6])
dartMotionModel.update(motion[frame])
global g_initFlag
global forceShowTime
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
# print('contactstate:', contact, contactChangeCount)
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2.*(Kt**.5)
Dl = (Kl**.5)
Dh = (Kh**.5)
dt_sup = 2.*(kt_sup**.5)
# Dt = .2*(Kt**.5)
# Dl = .2*(Kl**.5)
# Dh = .2*(Kh**.5)
# dt_sup = .2*(kt_sup**.5)
pdcontroller.setKpKd(Kt, Dt)
footHeight = dartModel.getBody(supL).shapenodes[0].shape.size()[1]/2.
doubleTosingleOffset = 0.15
singleTodoubleOffset = 0.30
#doubleTosingleOffset = 0.09
doubleTosingleVelOffset = 0.0
com_offset_x, com_offset_y, com_offset_z = getParamVals(['com X offset', 'com Y offset', 'com Z offset'])
footOffset = np.array((com_offset_x, com_offset_y, com_offset_z))
des_com = dartMotionModel.getCOM() + footOffset
footCenterL = dartMotionModel.getBodyPositionGlobal(supL)
footCenterR = dartMotionModel.getBodyPositionGlobal(supR)
footBodyOriL = dartMotionModel.getBodyOrientationGlobal(supL)
footBodyOriR = dartMotionModel.getBodyOrientationGlobal(supR)
torso_pos = dartMotionModel.getBodyPositionGlobal(4)
torso_ori = dartMotionModel.getBodyOrientationGlobal(4)
# tracking
# th_r = motion.getDOFPositions(frame)
th_r = dartMotionModel.getDOFPositions()
th = dartModel.getDOFPositions()
th_r_flat = dartMotionModel.get_q()
# dth_r = motion.getDOFVelocities(frame)
dth = dartModel.getDOFVelocities()
# ddth_r = motion.getDOFAccelerations(frame)
# ddth_des = yct.getDesiredDOFAccelerations(th_r, th, None, dth, None, Kt, Dt)
dth_flat = dartModel.get_dq()
# dth_flat = np.concatenate(dth)
# ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q())
# ddth_des_flat = pdcontroller.compute(th_r)
ddth_des_flat = pdcontroller.compute_flat(th_r_flat)
# ype.flatten(ddth_des, ddth_des_flat)
# ype.flatten(dth, dth_flat)
#################################################
# jacobian
#################################################
contact_des_ids = [dartModel.skeleton.bodynode_index("LeftFoot")]
contact_ids = list() # temp idx for balancing
contact_ids.extend(contact_des_ids)
contact_joint_ori = list(map(dartModel.getJointOrientationGlobal, contact_ids))
contact_joint_pos = list(map(dartModel.getJointPositionGlobal, contact_ids))
contact_body_ori = list(map(dartModel.getBodyOrientationGlobal, contact_ids))
contact_body_pos = list(map(dartModel.getBodyPositionGlobal, contact_ids))
contact_body_vel = list(map(dartModel.getBodyVelocityGlobal, contact_ids))
contact_body_angvel = list(map(dartModel.getBodyAngVelocityGlobal, contact_ids))
ref_joint_ori = list(map(motion[frame].getJointOrientationGlobal, contact_ids))
ref_joint_pos = list(map(motion[frame].getJointPositionGlobal, contact_ids))
ref_joint_vel = [motion.getJointVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids]
ref_joint_angvel = [motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids]
ref_body_ori = list(map(dartMotionModel.getBodyOrientationGlobal, contact_ids))
ref_body_pos = list(map(dartMotionModel.getBodyPositionGlobal, contact_ids))
for idx in range(len(ref_body_pos)):
ref_body_pos[idx] = dartModel.skeleton.body("RightFoot").shapenodes[0].shape.size()[1]/2.
# ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids))
ref_body_angvel = [motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids]
ref_body_vel = [ref_joint_vel[i] + np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i])
for i in range(len(ref_joint_vel))]
is_contact = [1] * len(contact_ids)
contact_right = len(set(contact_des_ids).intersection(rIDlist)) > 0
contact_left = len(set(contact_des_ids).intersection(lIDlist)) > 0
footOriL = dartModel.getJointOrientationGlobal(supL)
footOriR = dartModel.getJointOrientationGlobal(supR)
footCenterL = dartModel.getBodyPositionGlobal(supL)
footCenterR = dartModel.getBodyPositionGlobal(supR)
footBodyOriL = dartModel.getBodyOrientationGlobal(supL)
footBodyOriR = dartModel.getBodyOrientationGlobal(supR)
footBodyVelL = dartModel.getBodyVelocityGlobal(supL)
footBodyVelR = dartModel.getBodyVelocityGlobal(supR)
footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL)
footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR)
refFootL = dartMotionModel.getBodyPositionGlobal(supL)
refFootR = dartMotionModel.getBodyPositionGlobal(supR)
# refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
# refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootAngVelL = np.zeros(3)
refFootAngVelR = np.zeros(3)
refFootJointVelR = motion.getJointVelocityGlobal(supR, frame)
refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootJointR = motion.getJointPositionGlobal(supR, frame)
# refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR-refFootJointR))
refFootVelR = np.zeros(3)
refFootJointVelL = motion.getJointVelocityGlobal(supL, frame)
refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
refFootJointL = motion.getJointPositionGlobal(supL, frame)
# refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL-refFootJointL))
refFootVelL = np.zeros(3)
contactR = 1
contactL = 1
# contMotionOffset = th[0][0] - th_r[0][0]
contMotionOffset = dartModel.getBodyPositionGlobal(0) - dartMotionModel.getBodyPositionGlobal(0)
contMotionOffset = np.zeros(3)
linkPositions = dartModel.getBodyPositionsGlobal()
linkVelocities = dartModel.getBodyVelocitiesGlobal()
linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal()
linkInertias = dartModel.getBodyInertiasGlobal()
CM = dartModel.skeleton.com()
dCM = dartModel.skeleton.com_velocity()
CM_plane = copy.copy(CM)
CM_plane[1]=0.
dCM_plane = copy.copy(dCM)
dCM_plane[1]=0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias)
#calculate jacobian
body_num = dartModel.getBodyNum()
Jsys = np.zeros((6*body_num, totalDOF))
dJsys = np.zeros((6*body_num, totalDOF))
Jsys_, dJsysdq = compute_J_dJdq(dartModel.skeleton)
# dJsys = np.zeros((6*body_num, totalDOF))
for i in range(dartModel.getBodyNum()):
Jsys[6*i:6*i+6, :] = dartModel.getBody(i).world_jacobian()[range(-3, 3), :]
dJsys[6*i:6*i+6, :] = dartModel.getBody(i).world_jacobian_classic_deriv()[range(-3, 3), :]
dJsysdq = np.dot(dJsys, dartModel.skeleton.dq)
# print(Jsys_ - Jsys)
# print(Jsys_.dot(dth_flat))
# print(Jsys.dot(dth_flat))
# print(dartModel.getBody(0).world_linear_velocity())
# print(np.dot(Jsys[:3, :3], Jsys[0:3, 3:6].T))
print('dq', np.asarray(dartModel.skeleton.dq)[6:9])
print('joint vel', dartModel.skeleton.joint(1).velocity())
# print('bjoint', mm.exp(dartModel.skeleton.q[6:9]).dot(get_bjoint_jacobian(dartModel.skeleton.q[6:9]).dot(np.asarray(dartModel.skeleton.dq)[6:9])))
# print('not bjoint', mm.exp(dartModel.skeleton.q[6:9]).dot(np.asarray(dartModel.skeleton.dq)[6:9]))
print('frombody', dartModel.getJointOrientationGlobal(1).T.dot(dartModel.getJointAngVelocityGlobal(1) - dartModel.getJointAngVelocityGlobal(0)))
print('ddq', np.asarray(dartModel.skeleton.ddq)[6:9])
# print('bjoint', mm.exp(dartModel.skeleton.q[6:9]).dot(get_bjoint_jacobian(dartModel.skeleton.q[6:9]).dot(np.asarray(dartModel.skeleton.dq)[6:9])))
# print('not bjoint', mm.exp(dartModel.skeleton.q[6:9]).dot(np.asarray(dartModel.skeleton.dq)[6:9]))
bodybody = dartModel.skeleton.body(1)
joint_trans = dartModel.skeleton.joint(1).get_world_frame_after_transform()
joint_pos = bodybody.to_local(joint_trans[:3, 3])
print('com spati', dartModel.getJointOrientationGlobal(1).T.dot(
dartModel.skeleton.body(1).world_angular_acceleration() - dartModel.skeleton.body(0).world_angular_acceleration()))
J_contacts = [] # type: list[np.ndarray]
dJ_contacts = [] # type: list[np.ndarray]
for contact_id in contact_ids:
J_contacts.append(Jsys[6*contact_id:6*contact_id + 6, :])
dJ_contacts.append(dJsysdq[6*contact_id:6*contact_id + 6])
#calculate footCenter
footCenter = .5 * (footCenterL + footCenterR) + footOffset
if contact == 2:
footCenter = footCenterL.copy() + footOffset
#elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
footCenter = footCenterR.copy() + footOffset
footCenter[1] = 0.
if contactChangeCount > 0 and contactChangeType == 'StoD':
# change footcenter gradually
footCenter = preFootCenter + (maxContactChangeCount - contactChangeCount)*(footCenter-preFootCenter)/maxContactChangeCount
preFootCenter = footCenter.copy()
# linear momentum
# CM_ref_plane = footCenter
# dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane
# dL_des_plane[1] = 0.
CM_ref_plane = footCenter
CM_ref_plane[1] = dartMotionModel.skeleton.com()[1]
dL_des_plane = Kl*totalMass*(CM_ref_plane - CM) - Dl*totalMass*dCM
# dL_des_plane[1] = 0.
# angular momentum
CP_ref = footCenter
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0])/frame_step_size
CP_old[0] = CP
CP_des[0] = None
if CP is not None and dCP is not None:
ddCP_des = Kh*(CP_ref - CP) - Dh*(dCP)
CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des*(frame_step_size**2)
dH_des = mm.cross(CP_des[0] - CM, dL_des_plane + totalMass*mm.s2v(wcfg.gravity))
else:
dH_des = None
# set up equality constraint
a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(contact_body_ori[i], mm.unitY()), mm.unitY()) for i in range(len(contact_body_ori))]))
KT_SUP = np.diag([kt_sup/10., kt_sup, kt_sup/10.])
a_sups = [np.append(np.dot(KT_SUP, (ref_body_pos[i] - contact_body_pos[i] + contMotionOffset)) - dt_sup * contact_body_vel[i],
kt_sup*a_oris[i] - dt_sup * contact_body_angvel[i]) for i in range(len(a_oris))]
# print(a_sups)
# print(np.asarray(dartModel.skeleton.dq)[0:3])
# print(dartModel.getJointAngVelocityGlobal(0))
# print(dartModel.getJointAngVelocityLocal(0))
# momentum matrix
RS = np.dot(P, Jsys)
R, S = np.vsplit(RS, 2)
# rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat)
rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsysdq)
r_bias, s_bias = np.hsplit(rs, 2)
#######################################################
# optimization
#######################################################
#if contact == 2 and footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
config['weightMap']['RightUpLeg'] = .8
config['weightMap']['RightLeg'] = .8
config['weightMap']['RightFoot'] = .8
else:
config['weightMap']['RightUpLeg'] = .1
config['weightMap']['RightLeg'] = .25
config['weightMap']['RightFoot'] = .2
#if contact == 1 and footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
config['weightMap']['LeftUpLeg'] = .8
config['weightMap']['LeftLeg'] = .8
config['weightMap']['LeftFoot'] = .8
else:
config['weightMap']['LeftUpLeg'] = .1
config['weightMap']['LeftLeg'] = .25
config['weightMap']['LeftFoot'] = .2
# print('vel2', np.dot(dartModel.getJointOrientationGlobal(0).T, dartModel.skeleton.body(0).world_linear_velocity()))
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
#if contact == 2:
#mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
if dH_des is not None:
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
if True:
for c_idx in range(len(contact_ids)):
mot.addConstraint2(problem, totalDOF, J_contacts[c_idx], dJ_contacts[c_idx], a_sups[c_idx])
if contactChangeCount > 0:
contactChangeCount -= 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
r = problem.solve()
problem.clear()
ddth_sol_flat = np.asarray(r['x'])
# ype.nested(r['x'], ddth_sol)
# ddth_sol[:6] = np.zeros(6)
rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
inv_h = 1./time_step
_bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = [], [], [], []
for i in range(stepsPerFrame):
# apply penalty force
_ddq, _tau, _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = hqp.calc_QP(dartModel.skeleton, ddth_sol_flat, inv_h)
# _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck,mus, Ks, Ds)
dartModel.applyPenaltyForce(_bodyIDs, _contactPositionLocals, _contactForces)
dartModel.skeleton.set_forces(_tau)
# dartModel.setDOFAccelerations(ddth_sol)
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value()])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += wcfg.timeStep
dartModel.applyPenaltyForce(selectedBodyId, localPos, extraForce)
dartModel.step()
del bodyIDs[:]
del contactPositions[:]
del contactPositionLocals[:]
del contactForces[:]
bodyIDs.extend(_bodyIDs)
contactPositions.extend(_contactPositions)
contactPositionLocals.extend(_contactPositionLocals)
contactForces.extend(_contactForces)
# rendering
rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0]))
rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0]))
rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0,.1]))
rightFootPos[0] = footCenterL
rightVectorX[0] = np.dot(footBodyOriL, np.array([.1,0,0]))
rightVectorY[0] = np.dot(footBodyOriL, np.array([0,.1,0]))
rightVectorZ[0] = np.dot(footBodyOriL, np.array([0,0,.1]))
rightPos[0] = footCenterL + np.array([.1,0,0])
rd_footCenter[0] = footCenter
rd_footCenterL[0] = footCenterL
rd_footCenterR[0] = footCenterR
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
rd_CM_plane[0][1] = 0.
if CP is not None and dCP is not None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des[0]
rd_dL_des_plane[0] = [dL_des_plane[0]/100, dL_des_plane[1]/100, dL_des_plane[2]/100]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
rd_root_des[0] = rootPos[0]
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append( contactForces[i]/100)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100]
rd_exf_des[0] = [0,0,0]
else:
rd_exf_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100]
rd_exfen_des[0] = [0,0,0]
extraForcePos[0] = dartModel.getBodyPositionGlobal(selectedBody)
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def main2():
import PyCommon.modules.Motion.ysMotionAnalysis as yma
pydart.init()
world = [
pydart.World(1. / 1200., "../DartDeep/data/woody_with_ground_v2.xml")
for _ in range(2)
]
motion_files = ['wd2_jump0.bvh']
motion_files = ['woody_walk_normal.bvh']
motion_files = ['walk_left_90degree.bvh']
motion = yf.readBvhFileAsBvh(motion_files[0]).toPmLinearMotion(1., False)
# 70, 112, 156 right heel strike
mg = hmg.MotionGraph()
mg.add_motion(motion)
mg.build()
total_num = len(mg.transition_processed)
# viewer settings
rd_contact_positions = [None]
rd_contact_forces = [None]
viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False)
viewer.doc.addRenderer('motionModel[0]',
yr.DartRenderer(world[0], (255, 240, 255)))
viewer.doc.addRenderer('motionModel[1]', yr.DartRenderer(world[1]))
motion_state = [0]
def callback_0(_):
motion_state[0] = 0
def callback_1(_):
motion_state[0] = 1
def callback_2(_):
motion_state[0] = 2
viewer.objectInfoWnd.addBtn('0', callback_0)
viewer.objectInfoWnd.addBtn('1', callback_1)
viewer.objectInfoWnd.addBtn('2', callback_2)
hRef = 0.1
vRef = 0.4
lc = yma.getElementContactStates(motion, 'LeftFoot', hRef, vRef)
rc = yma.getElementContactStates(motion, 'RightFoot', hRef, vRef)
lindex = motion[0].skeleton.getElementIndex('LeftFoot')
rindex = motion[0].skeleton.getElementIndex('RightFoot')
def postCallback(frame):
transition = mg.transition_processed[frame]
world[0].skeletons[1].set_positions(
motion.get_q(transition.motion_from_idx))
world[1].skeletons[1].set_positions(
motion.get_q(transition.motion_to_idx))
print(frame, transition.motion_from_idx, transition.motion_to_idx,
transition.dist)
print(motion.getPosition(lindex))
print(lc[transition.motion_from_idx], rc[transition.motion_from_idx])
print(lc[transition.motion_to_idx], rc[transition.motion_to_idx])
viewer.setPreFrameCallback_Always(postCallback)
viewer.setMaxFrame(total_num - 1)
viewer.startTimer(1. / 30.)
viewer.show()
all_node_set = set()
dot = Digraph()
for i in range(len(mg.transition_processed)):
all_node_set.add(mg.transition_processed[i].motion_from_idx)
all_node_set.add(mg.transition_processed[i].motion_to_idx)
all_node_list = list(all_node_set)
all_node_list.sort()
for i in all_node_list:
dot.node(str(i))
all_edge_set = set()
for i in range(len(all_node_list) - 1):
all_edge_set.add((all_node_list[i], all_node_list[i + 1]))
for i in range(len(mg.transition_processed)):
all_edge_set.add((mg.transition_processed[i].motion_from_idx,
mg.transition_processed[i].motion_to_idx))
for i in list(all_edge_set):
dot.edge(str(i[0]), str(i[1]))
# print(dot.source)
dot.render('test', view=True)
dot.pipe()
Fl.run()
def main(fm=50., fv=np.array([0., 0., 50.]), ts=2., te=2.1):
np.set_printoptions(precision=4, linewidth=200)
# np.set_printoptions(precision=4, linewidth=1000, threshold=np.inf)
motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped()
frame_step_size = 1. / frame_rate
pydart.init()
dartModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON)
dartMotionModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON)
# wcfg.lockingVel = 0.01
dartModel.initializeHybridDynamics()
#controlToMotionOffset = (1.5, -0.02, 0)
controlToMotionOffset = (1.5, 0, 0)
dartModel.translateByOffset(controlToMotionOffset)
totalDOF = dartModel.getTotalDOF()
DOFs = dartModel.getDOFs()
# parameter
Kt = 25.
Dt = 5.
Ks = config['Ks']
Ds = config['Ds']
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
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')
# momentum matrix
linkMasses = dartModel.getBodyMasses()
totalMass = dartModel.getTotalMass()
dartModel.world.time()
# penalty method
# bodyIDsToCheck = range(dartModel.getBodyNum())
bodyIDsToCheck = [supL, supR]
#mus = [1.]*len(bodyIDsToCheck)
mus = [.5] * len(bodyIDsToCheck)
# viewer
selectedBodyId = [selectedBody]
extraForce = [None]
extraForcePos = [None]
pdcontroller = PDController(dartModel, dartModel.skeleton, wcfg.timeStep,
Kt, Dt)
###################################
#simulate
###################################
def simulateCallback(frame):
dartMotionModel.update(motion[frame])
# tracking
# th_r = motion.getDOFPositions(frame)
th_r = dartMotionModel.getDOFPositions()
th = dartModel.getDOFPositions()
th_r_flat = dartMotionModel.get_q()
dth_flat = dartModel.get_dq()
ddth_des_flat = pdcontroller.compute_flat(th_r_flat)
localPos = [[0, 0, 0]]
for i in range(stepsPerFrame):
# apply penalty force
if not DART_CONTACT_ON:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
dartModel.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
# dartModel.skeleton.set_accelerations(ddth_sol)
dartModel.skeleton.set_accelerations(ddth_des_flat)
# dartModel.skeleton.set_forces(np.zeros(totalDOF))
extraForce[0] = fm * fv
if ts <= dartModel.world.time() <= te:
dartModel.applyPenaltyForce(selectedBodyId, localPos,
extraForce)
dartModel.step()
if DART_CONTACT_ON:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info(
)
else:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
def __init__(self):
joint_names = [
'pelvis', 'leftThigh', 'rightThigh', 'spine', 'leftCalf',
'rightCalf', 'spine1', 'leftFoot', 'rightFoot', 'spine2', 'neck',
'leftShoulder', 'rightShoulder', 'head', 'leftUpperArm',
'rightUpperArm', 'leftForeArm', 'rightForeArm', 'leftHand',
'rightHand'
]
red_parent_ref = [
-1, 0, 0, 0, 1, 2, 3, 4, 5, 6, 9, 9, 9, 10, 11, 12, 14, 15, 16, 17
]
radii = [
0.12769461096148343, 0.09182758062402535, 0.09183365264505983,
0.128195873394974, 0.054201506359127107, 0.054204782651247056,
0.11441021953758712, 0.05134816175061338, 0.051367681280828095,
0.12840909729388708, 0.08568376456832136, 0.09205466746236975,
0.09207195048860886, 0.09417674072728775, 0.04450030029080607,
0.04453954596015603, 0.03869968571382823, 0.03875571491372652,
0.02922814811888941, 0.0297973538063161
]
radii_med = [
0.1443921929584865, 0.09879753554393726, 0.09880958937889255,
0.14038170079741627, 0.05892499163491047, 0.05891232648197925,
0.13581042138687494, 0.04932037409824811, 0.049343780055406926,
0.1396929637576165, 0.0861958797422231, 0.09078340056685348,
0.09077927107473645, 0.09547074699086751, 0.05319511539915989,
0.0532042033919998, 0.041963113363422504, 0.04198389621382932,
0.02922814811888941, 0.0297973538063161
]
lengths = [
0.1331743261250832, 0.25341930101960214, 0.2534154318813515,
0.0726371962432468, 0.49113402834862235, 0.4911362895591663,
0.11898002646639905, 0.1501895966849679, 0.15021745070394746,
0.01754202138831551, 0.13059433732982026, 0.020496847079791814,
0.020512735422737065, 0.12692070216147233, 0.2900605373659263,
0.29364024694729224, 0.28099209587280943, 0.2945346732747498,
0.1390784801895961, 0.13718088501877865
]
lengths_med = [
0.11221012660894233, 0.21838745393591003, 0.21838652247014215,
0.04009886721901377, 0.4404043065827791, 0.4404005018107258,
0.03352836109003122, 0.14624878028636093, 0.14626475496405048,
0.09201591904274159, 0.10279722790210173, 0.03407651369086422,
0.03408094322130216, 0.11318665800279451, 0.25693470642127425,
0.2624116078945138, 0.26616023158348917, 0.2692486717111876,
0.1390784801895961, 0.13718088501877865
]
initial_rots = [[0., 0., 1.57079633], [0., 0., 3.14159265],
[0., 0., 3.14159265], [0., 0., 1.57079633],
[0., 0., 3.14159265], [0., 0., 3.14159265],
[0., 0., 1.57079633], [1.57079633, 0., 0.],
[1.57079633, 0., 0.], [0., 0., 1.57079633],
[0., 0., 0.], [0., 0., -1.57079633],
[0., 0., 1.57079633], [0., 0., 0.],
[0., 0., -1.57079633], [0., 0., 1.57079633],
[0., 0., -1.57079633], [0., 0., 1.57079633],
[0., 0., -1.57079633], [0., 0., 1.57079633]]
cap_offsets = [
[0.0, -0.04, 0.0],
[0.011169376037587928, -0.15284656946574843, 0.012903663900451865],
[
-0.008170654460734582, -0.14484365810893893,
0.009715733426505214
],
[-0.004528416367088686, 0.013605244090683206, 0.04478019087771228],
[
-0.010037273572433072, -0.23605176764955693,
-0.016171902309391355
],
[0.01309094764543177, -0.2308922510404945, -0.016202019999160024],
[
-0.00591252018941266, -0.01317568744365618,
-0.002010005222645302
],
[0.01094751596360935, -0.03146133114580548, 0.07573397320647444],
[
-0.012013303281514093, -0.033379105586368994,
0.07790374457555227
],
[
-0.0035521415314855153, 0.036178626564513906,
-0.0033071152353626437
],
[0.009242849296194584, 0.03218511724478738, 0.020464375843508388],
[0.07173393305388479, 0.03415988444216093, -0.021207023426983884],
[-0.07011977756584435, 0.03500467757275714, -0.01431098898502919],
[
0.0017877181471058897, -0.003604552239007318,
-0.013366571797805468
],
[0.1670164481950132, -0.01047385968513933, -0.00955946729325003],
[
-0.17489038653427352, -0.012404766368258179,
-0.01493462681539015
],
[0.13686313856829088, 0.012178408710815582, 0.009027286895816854],
[-0.14869936653600047, 0.012743380671612937, 0.010315014780206785],
[0.07295133648723572, 0.003332175704076991, 0.00477568727285764],
[
-0.06900674634442043, 0.0053072850629911975,
0.005648618042937342
]
]
joint_locs = [
[0.048575008136347936, 0.024119849828711848, 0.0],
[0.05603581582384387, -0.08435470924277266, -0.02125171045905388],
[
-0.056734287288310335, -0.09246840627063796,
-0.019865012003098714
],
[0.00478469354216688, 0.12540467056158064, -0.029426528753042178],
[0.05438855330810292, -0.43938729822764777, 0.010090597202908305],
[-0.05687942203278569, -0.4329298092709282, -0.002264318594297732],
[0.004335110280848386, 0.1424258678301017, 0.032243119346393],
[
-0.013017916423372161, -0.47630985208241616,
-0.03525983240121225
],
[0.01879643192824705, -0.47063802124032617, -0.031908165182390605],
[
-0.0037562386956846213, 0.057957653859279396,
-0.0026084214232746233
],
[
-0.012891345804552689, 0.23382254793308366,
-0.024546227595192645
],
[0.06810633105445495, 0.12066967354888963, -0.025617194319459552],
[-0.08312624481554046, 0.1213346306423565, -0.02646990522515696],
[0.010160433355839844, 0.11431142528985228, 0.06561347872079941],
[0.11340919931011699, 0.04872322320654052, -0.014010241744952277],
[
-0.10906087070470047, 0.050948984849806256,
-0.0034195487756392123
],
[0.2909446510941778, -0.020225692509760418, -0.016255943269017036],
[-0.2940904813191163, -0.024448930412666936, -0.02096928534590224],
[0.281254400830233, 0.011761597131565299, -0.011719547351094269],
[-0.2957180548455181, 0.014828733728343285, -0.00917540965007943],
[0.08686301274055253, -0.010243204490181151, -0.01588782879746012],
[
-0.09127255488333685, -0.008439567905351025,
-0.010547811064669982
]
]
########################################## SET UP DART ###########################################
pydart.init(verbose=True)
self.world = pydart.World(0.002, "EMPTY") #0.003, .0002
self.world.set_gravity([0, 0, GRAVITY]) #([0, 0, -9.81])
self.world.set_collision_detector(detector_type=2)
################## CREATE SKELETON FOR HUMAN, ADD TO WORLD, AND SET PARAMETERS ###################
self.world.add_empty_skeleton(_skel_name="human")
for count in range(NUM_CAPSULES):
cap_rad = radii[count]
cap_len = lengths[count]
cap_init_rot = initial_rots[count]
cap_offset = cap_offsets[count]
joint_loc = joint_locs[count]
if count == 0:
self.world.add_capsule(parent=int(red_parent_ref[count]),
radius=cap_rad,
length=cap_len,
cap_rot=cap_init_rot,
cap_offset=cap_offset,
joint_loc=joint_loc,
joint_type="FREE",
joint_name=joint_names[count])
elif count == 4 or count == 5:
self.world.add_capsule(parent=int(red_parent_ref[count]),
radius=cap_rad,
length=cap_len,
cap_rot=cap_init_rot,
cap_offset=cap_offset,
joint_loc=joint_loc,
joint_type="REVOLUTE_X",
joint_name=joint_names[count])
elif count == 16 or count == 17:
self.world.add_capsule(parent=int(red_parent_ref[count]),
radius=cap_rad,
length=cap_len,
cap_rot=cap_init_rot,
cap_offset=cap_offset,
joint_loc=joint_loc,
joint_type="REVOLUTE_Y",
joint_name=joint_names[count])
else:
self.world.add_capsule(parent=int(red_parent_ref[count]),
radius=cap_rad,
length=cap_len,
cap_rot=cap_init_rot,
cap_offset=cap_offset,
joint_loc=joint_loc,
joint_type="BALL",
joint_name=joint_names[count])
skel = self.world.add_built_skeleton(_skel_id=0, _skel_name="human")
skel.set_self_collision_check(True)
skel.set_adjacent_body_check(False)
skel.set_collision_filter(True)
#set parameters
#[-1.32726466 0.31681432 0.04725983 - 1.29570571 - 0.60504953 - 0.43173041]
skel = self.assign_initial_joint_angles(skel)
skel = self.assign_joint_rest_and_stiffness(skel)
skel = self.assign_joint_limits_and_damping(skel)
skel = self.assign_body_masses_and_inertia(skel, initial_rots, radii,
lengths, radii_med,
lengths_med)
#self.world.test_filter()
########################### CREATE SKELETON FOR FLOOR AND ADD TO WORLD ###########################
self.world.add_empty_skeleton(_skel_name="floor")
self.world.add_weld_box(
width=10.0,
length=10.0,
height=0.2,
joint_loc=[
0.0, 0.0, -STARTING_HEIGHT / DART_TO_FLEX_CONV / 2 - 0.05 - 2.0
],
box_rot=[0.0, 0.0, 0.0],
joint_name="floor") # -0.05
skel_floor = self.world.add_built_skeleton(_skel_id=1,
_skel_name="floor")
#now setup the open GL window
self.title = "GLUT Window"
self.window_size = (1280, 720)
self.scene = OpenGLScene(*self.window_size)
self.mouseLastPos = None
self.is_simulating = False
self.is_animating = False
self.frame_index = 0
self.capture_index = 0
self.force_application_count = 0
self.count = 0
def __init__(self,
render,
STARTING_HEIGHT,
input_flex_radius,
input_flex_length,
input_flex_mass,
shiftSIDE=0.0,
shiftUD=0.0):
model_path = '/home/henry/git/SMPL_python_v.1.0.0/smpl/models/basicModel_f_lbs_10_207_0_v1.0.0.pkl'
m = load_model(model_path)
regs = np.load(
'/home/henry/git/smplify_public/code/models/regressors_locked_normalized_male.npz'
)
length_regs = regs['betas2lens']
rad_regs = regs['betas2rads']
betas = m.betas
capsules_median = get_capsules(m, betas * 0, length_regs, rad_regs)
capsules = get_capsules(m, betas, length_regs, rad_regs)
joint_names = joint2name
initial_rots = rots0
self.num_steps = 10000
self.render_dart = render
self.ct = 0
self.num_dart_steps = 4
self.has_reset_velocity1 = False
self.has_reset_velocity2 = False
joint_ref = list(m.kintree_table[1]) #joints
parent_ref = list(m.kintree_table[0]) #parent of each joint
parent_ref[0] = -1
self.capsules = capsules
pydart.init(verbose=True)
print('pydart initialization OK')
self.world = pydart.World(
0.0103 / 4, "EMPTY"
) #0.002 is what works well. 0.0103 is when the velocity aligns. thus flex is 0.0103/0.0020 = 5.15x more fast than dart
self.world.set_gravity([0, 0, GRAVITY]) #([0, 0, -9.81])
self.world.set_collision_detector(detector_type=2)
self.world.add_empty_skeleton(_skel_name="human")
self.force_dir_list_prev = [[], [], [], [], [], [], [], [], [], [], [],
[], [], [], [], [], [], [], [], []]
self.pmat_idx_list_prev = [[], [], [], [], [], [], [], [], [], [], [],
[], [], [], [], [], [], [], [], []]
self.force_loc_list_prev = [[], [], [], [], [], [], [], [], [], [], [],
[], [], [], [], [], [], [], [], []]
joint_root_loc = np.asarray(np.transpose(capsules[0].t)[0])
self.step_num = 0
joint_locs = []
capsule_locs = []
joint_locs_abs = []
joint_locs_trans_abs = []
capsule_locs_abs = []
mJ = np.asarray(m.J)
mJ_transformed = np.asarray(m.J_transformed)
shift = [shiftSIDE, shiftUD, 0.0]
red_joint_ref = joint_ref[0:20] #joints
red_parent_ref = parent_ref[0:20] #parent of each joint
red_parent_ref[10] = 9 #fix neck
red_parent_ref[11] = 9 #fix l inner shoulder
red_parent_ref[13] = 10 #fix head
red_parent_ref[14] = 11 #fix l outer shoulder
red_parent_ref[15] = 12 #fix r outer shoulder
red_parent_ref[16] = 14 #fix l elbow
red_parent_ref[17] = 15 #fix r elbow
head_ref = [10, 13]
leg_cap_ref = [1, 2, 4, 5]
foot_ref = [7, 8]
l_arm_ref = [11, 14, 16, 18]
r_arm_ref = [12, 15, 17, 19]
self.red_joint_ref = [joint_ref[0]]
self.red_parent_ref = red_parent_ref
#make lists of the locations of the joint locations and the smplify capsule initial ends
for i in range(np.shape(mJ)[0]):
if i == 0:
joint_locs.append(list(mJ[0, :] - mJ[0, :] + shift))
joint_locs_abs.append(list(mJ[0, :] - mJ[0, :]))
joint_locs_trans_abs.append(
list(mJ_transformed[0, :] - mJ_transformed[0, :]))
if i < 20:
capsule_locs.append(
list(
np.asarray(np.transpose(capsules[i].t)[0]) -
joint_root_loc))
capsule_locs_abs.append(
list(
np.asarray(np.transpose(capsules[i].t)[0]) -
joint_root_loc))
print(capsule_locs_abs, "caps locs abs")
else:
joint_locs.append(list(mJ[i, :] - mJ[parent_ref[i], :]))
joint_locs_abs.append(list(mJ[i, :] - mJ[0, :]))
joint_locs_trans_abs.append(
list(mJ_transformed[i, :] - mJ_transformed[0, :]))
if i < 20:
capsule_locs.append(
list(
np.asarray(np.transpose(capsules[i].t)[0]) -
np.asarray(
np.transpose(capsules[red_parent_ref[i]].t)[0])
))
capsule_locs_abs.append(
list(
np.asarray(np.transpose(capsules[i].t)[0]) -
joint_root_loc))
capsule_locs_abs[i][0] += np.abs(
float(capsules[0].length[0])) / 2
if i in [
1, 2
]: #shift over the legs relative to where the pelvis mid capsule is
capsule_locs[i][0] += np.abs(
float(capsules[0].length[0])) / 2
if i in [
3, 6, 9
]: #shift over the torso segments relative to their length and their parents length to match the mid capsule
capsule_locs[i][0] -= (
np.abs(float(capsules[i].length[0])) -
np.abs(float(
capsules[red_parent_ref[i]].length[0]))) / 2
if i in [
10, 11, 12
]: #shift over the inner shoulders and neck to match the middle of the top spine capsule
capsule_locs[i][0] += np.abs(
float(capsules[red_parent_ref[i]].length[0])) / 2
if i in [
3, 6, 9
]: #shift over everything in the abs list to match the root
capsule_locs_abs[i][0] -= np.abs(
float(capsules[i].length[0])) / 2
del (joint_locs[10])
del (joint_locs[10])
del (joint_locs_abs[10])
del (joint_locs_abs[10])
self.joint_locs = joint_locs
count = 0
root_joint_type = "FREE"
self.cap_offsets = []
self.cap_init_rots = []
lowest_points = []
for capsule in capsules:
print "************* Capsule No.", count, joint_names[
count], " joint ref: ", red_joint_ref[
count], " parent_ref: ", red_parent_ref[
count], " ****************"
cap_rad = input_flex_radius
cap_len = input_flex_length
cap_init_rot = list(np.asarray(initial_rots[count]))
joint_loc = joint_locs[count]
joint_loc_abs = joint_locs_abs[count]
capsule_loc = capsule_locs[count]
capsule_loc_abs = capsule_locs_abs[count]
cap_offset = [0., 0., 0.]
if count in leg_cap_ref:
cap_offset[1] = -cap_len / 2
if count in foot_ref: cap_offset[2] = cap_len / 2
if count in l_arm_ref: cap_offset[0] = cap_len / 2
if count in r_arm_ref: cap_offset[0] = -cap_len / 2
#if count in head_ref: cap_offset[1] = cap_len/2
cap_offset[0] += capsule_loc_abs[0] - joint_loc_abs[0]
cap_offset[1] += capsule_loc_abs[1] - joint_loc_abs[1] - .04
cap_offset[2] += capsule_loc_abs[2] - joint_loc_abs[2]
self.cap_offsets.append(np.asarray(cap_offset))
self.cap_init_rots.append(np.asarray(cap_init_rot))
if count == 0:
self.world.add_capsule(parent=int(red_parent_ref[count]),
radius=cap_rad,
length=cap_len,
cap_rot=cap_init_rot,
cap_offset=cap_offset,
joint_loc=joint_loc,
joint_type=root_joint_type,
joint_name=joint_names[count])
elif count == 4 or count == 5:
self.world.add_capsule(parent=int(red_parent_ref[count]),
radius=cap_rad,
length=cap_len,
cap_rot=cap_init_rot,
cap_offset=cap_offset,
joint_loc=joint_loc,
joint_type="REVOLUTE_X",
joint_name=joint_names[count])
elif count == 16 or count == 17:
self.world.add_capsule(parent=int(red_parent_ref[count]),
radius=cap_rad,
length=cap_len,
cap_rot=cap_init_rot,
cap_offset=cap_offset,
joint_loc=joint_loc,
joint_type="REVOLUTE_Y",
joint_name=joint_names[count])
else:
self.world.add_capsule(parent=int(red_parent_ref[count]),
radius=cap_rad,
length=cap_len,
cap_rot=cap_init_rot,
cap_offset=cap_offset,
joint_loc=joint_loc,
joint_type="BALL",
joint_name=joint_names[count])
lowest_points.append(
np.asarray(joint_locs_trans_abs)[count, 2] -
np.abs(float(input_flex_radius)))
count += 1
break
#print "pelvis cap",
#print np.asarray(joint_locs_trans_abs)[:, 2]
self.STARTING_HEIGHT = STARTING_HEIGHT - 0.0508
#add a floor-STARTING_HEIGHT / DART_TO_FLEX_CONV
self.world.add_weld_box(
width=10.0,
length=10.0,
height=0.2,
joint_loc=[
0.0, 0.0, -self.STARTING_HEIGHT / DART_TO_FLEX_CONV / 2 - 0.05
],
box_rot=[0.0, 0.0, 0.0],
joint_name="floor") #-0.05
skel = self.world.add_built_skeleton(_skel_id=0, _skel_name="human")
skel.set_self_collision_check(True)
#weight the capsules appropriately
volume = []
volume_median = []
for body_ct in range(NUM_CAPSULES):
#give the capsules a weight propertional to their volume
cap_rad = input_flex_radius
cap_len = input_flex_length
cap_rad_median = np.abs(float(capsules_median[body_ct].rad[0]))
cap_len_median = np.abs(float(capsules_median[body_ct].length[0]))
volume.append(np.pi * np.square(cap_rad) *
(cap_rad * 4 / 3 + cap_len))
volume_median.append(np.pi * np.square(cap_rad_median) *
(cap_rad_median * 4 / 3 + cap_len_median))
skel.bodynodes[0].set_mass(input_flex_mass)
body_mass = 0.0
#set the mass moment of inertia matrices
for body_ct in range(NUM_CAPSULES):
radius = input_flex_radius
length = input_flex_length
radius2 = radius * radius
length2 = length * length
mass = skel.bodynodes[body_ct].m
cap_init_rot = list(np.asarray(initial_rots[body_ct]))
volumeCylinder = np.pi * radius2 * length
volumeSphere = np.pi * radius * radius * radius * 4 / 3
density = mass / (volumeCylinder + volumeSphere)
massCylinder = density * volumeCylinder
massSphere = density * volumeSphere
Ixx = massCylinder * (length2 / 12.0 +
radius2 / 4.0) + massSphere * (
length2 + (3.0 / 8.0) * length * radius +
(2.0 / 5.0) * radius2)
Izz = massCylinder * (radius2 / 2.0) + massSphere * (
(2.0 / 5.0) * radius2)
RotMatInit = LibDartSkel().eulerAnglesToRotationMatrix(
[np.pi / 2, 0.0, 0.0])
RotMat = LibDartSkel().eulerAnglesToRotationMatrix(cap_init_rot)
I = np.matmul(np.matmul(RotMatInit, RotMat),
np.asarray([Ixx, Izz, Ixx]))
Ixx = np.abs(I[0])
Iyy = np.abs(I[1])
Izz = np.abs(I[2])
#print body_ct, I
skel.bodynodes[body_ct].set_inertia_entries(Ixx, Iyy, Izz)
body_mass += skel.bodynodes[body_ct].m
break
print "Body mass is: ", body_mass, "kg"
self.body_node = 9 #need to solve for the body node that corresponds to a force using flex.
self.force = np.asarray([0.0, 100.0, 100.0])
self.offset_from_centroid = np.asarray([-0.15, 0.0, 0.0])
self.pmat_red_all = np.load(
"/home/henry/git/volumetric_pose_gen/data/pmat_red.npy")
self.force_dir_red_dart_all = np.load(
"/home/henry/git/volumetric_pose_gen/data/force_dir_red.npy")
for element in range(len(self.force_dir_red_dart_all)):
self.force_dir_red_dart_all[element] = (np.multiply(
np.asarray(self.force_dir_red_dart_all[element]),
np.expand_dims(np.asarray(self.pmat_red_all[element]),
axis=1)))
self.force_loc_red_dart_all = np.load(
"/home/henry/git/volumetric_pose_gen/data/force_loc_red.npy"
).tolist()
self.nearest_capsule_list_all = np.load(
"/home/henry/git/volumetric_pose_gen/data/nearest_capsule.npy"
).tolist()
print('init pose = %s' % skel.q)
skel.controller = DampingController(skel)
#now setup the open GL window
self.title = "GLUT Window"
self.window_size = (1280, 720)
self.scene = OpenGLScene(*self.window_size)
self.mouseLastPos = None
self.is_simulating = False
self.is_animating = False
self.frame_index = 0
self.capture_index = 0
self.force_application_count = 0
self.count = 0
self.zi = []
self.b = []
self.a = []
for i in range(60):
b, a = signal.butter(1, 0.05, analog=False)
self.b.append(b)
self.a.append(a)
self.zi.append(signal.lfilter_zi(self.b[-1], self.a[-1]))
self.ziF = []
self.bF = []
self.aF = []
for i in range(3):
b, a = signal.butter(1, 0.05, analog=False)
self.bF.append(b)
self.aF.append(a)
self.ziF.append(signal.lfilter_zi(self.bF[-1], self.aF[-1]))
import pydart2 as pydart
if __name__ == '__main__':
print('Hello, PyDART!')
pydart.init()
print('pydart initialization OK')
world = pydart.World(0.0005, './data/skel/cubes.skel')
print('pydart create_world OK')
while world.t < 2.0:
if world.nframes % 100 == 0:
skel = world.skeletons[-1]
print("%.4fs: The last cube COM = %s" % (world.t, str(skel.C)))
world.step()
def main():
np.set_printoptions(precision=4, linewidth=200)
# np.set_printoptions(precision=4, linewidth=1000, threshold=np.inf)
pydart.init()
dartModel = cdm.DartModel(None, None, None, None, 'cart_pole_blade.skel')
dartMotionModel = cdm.DartModel(None, None, None, None,
'cart_pole_blade.skel')
footIdlist = list(
dartMotionModel.skeleton.body('h_' + name).index_in_skeleton()
for name in ['blade_left', 'blade_right'])
up_vec_in_each_link = dict()
for foot_id in footIdlist:
up_vec_in_each_link[
foot_id] = dartMotionModel.getBodyOrientationGlobal(foot_id)[1, :]
pelvis_pos = dartMotionModel.skeleton.dof_indices(
(["j_pelvis_pos_x", 'j_pelvis_pos_y', 'j_pelvis_pos_z']))
pelvis_x = dartMotionModel.skeleton.dof_indices((["j_pelvis_rot_x"]))
pelvis = dartMotionModel.skeleton.dof_indices(
(["j_pelvis_rot_y", "j_pelvis_rot_z"]))
upper_body = dartMotionModel.skeleton.dof_indices(
["j_abdomen_1", "j_abdomen_2"])
right_leg = dartMotionModel.skeleton.dof_indices([
"j_thigh_right_x", "j_thigh_right_y", "j_thigh_right_z", "j_shin_right"
])
left_leg = dartMotionModel.skeleton.dof_indices(
["j_thigh_left_x", "j_thigh_left_y", "j_thigh_left_z", "j_shin_left"])
arms = dartMotionModel.skeleton.dof_indices(
["j_bicep_left_x", "j_bicep_right_x"])
foot = dartMotionModel.skeleton.dof_indices(
["j_heel_left_1", "j_heel_left_2", "j_heel_right_1", "j_heel_right_2"])
leg_y = dartMotionModel.skeleton.dof_indices(
["j_thigh_right_y", "j_thigh_left_y"])
INIT_ANGLE = 0.09
s0q = np.zeros(dartMotionModel.skeleton.ndofs)
s0q[pelvis_pos] = 0., .92, 0.
# s0q[pelvis] = 0., -0.
# s0q[upper_body] = 0.3, -0.
s0q[right_leg] = -0., -0., 0.9, -1.5
s0q[left_leg] = -INIT_ANGLE, 0., 0.0, 0.0
s0q[foot] = 0., INIT_ANGLE, 0., 0.
# s0q[right_leg] = -0., -0., 0.2, -.4
# s0q[left_leg] = -0., 0., 0.2, -.4
# s0q[foot] = 0.2, 0., 0.2, 0.
# s0q[leg_y] = -0.785, 0.785
s0q[arms] = 1.5, -1.5
dartModel.set_q(s0q)
dartMotionModel.set_q(s0q)
frame_step_size = 1. / 40.
stepsPerFrame = 25
time_step = dartModel.world.time_step()
# wcfg.lockingVel = 0.01
# dartModel.initializeHybridDynamics()
#controlToMotionOffset = (1.5, -0.02, 0)
controlToMotionOffset = (0, 0, 2.0)
dartModel.translateByOffset(controlToMotionOffset)
totalDOF = dartModel.getTotalDOF()
DOFs = dartModel.getDOFs()
# parameter
Kt = 25.
Dt = 2. * (Kt**.5)
Kl = 100.
Dl = 2. * (Kt**.5)
Kh = 100.
Dh = 2. * (Kt**.5)
Ks = 20000.
Ds = 2. * (Kt**.5)
Bt = 1.
Bl = 0.1
Bh = 0.13
supL = dartModel.skeleton.body('h_blade_left').index_in_skeleton()
supR = dartModel.skeleton.body('h_blade_right').index_in_skeleton()
selectedBody = dartModel.skeleton.body('h_head').index_in_skeleton()
# momentum matrix
linkMasses = dartModel.getBodyMasses()
print([body.name for body in dartModel.skeleton.bodynodes])
print(linkMasses)
totalMass = dartModel.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
a_supL = (0, 0, 0, 0, 0, 0)
a_supR = (0, 0, 0, 0, 0, 0)
a_sup_2 = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
CP_old = [mm.v3(0., 0., 0.)]
CP_des = [None]
dCP_des = [np.zeros(3)]
# penalty method
# bodyIDsToCheck = range(dartModel.getBodyNum())
bodyIDsToCheck = [supL, supR]
#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)
config = dict()
config['weightMap'] = {
'j_scapula_left': .2,
'j_bicep_left': .2,
'j_forearm_left': .2,
'j_hand_left': .2,
'j_scapula_right': .2,
'j_bicep_right': .2,
'j_forearm_right': .2,
'j_hand_right': .2,
'j_abdomen': .6,
'j_spine': .6,
'j_head': .6,
'j_heel_right': .2,
'j_heel_left': .2,
'j_pelvis': 0.5,
'j_thigh_left': 5.,
'j_shin_left': .5,
'j_thigh_right': 5.,
'j_shin_right': .5
}
# viewer
rd_footCenter = [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_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]
# viewer = ysv.SimpleViewer()
viewer = hsv.hpSimpleViewer(viewForceWnd=False)
viewer.setMaxFrame(1000)
#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', cvr.VpModelRenderer(motionModel, (150,150,255), yr.POLYGON_FILL))
# viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
#viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'motionModel',
yr.DartRenderer(dartMotionModel.world, (255, 240, 255),
yr.POLYGON_FILL))
viewer.doc.addRenderer(
'controlModel',
yr.DartRenderer(dartModel.world, (150, 150, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
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, 0, 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)))
#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 = 11.
# initKh = 22.
initKl = 100.
initKh = 100.
initBl = .1
initBh = .13
initSupKt = 17.
# initSupKt = 2.5
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., 100., 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)
pdcontroller = PDController(dartModel, dartModel.skeleton,
dartModel.world.time_step(), Kt, Dt)
def getParamVal(paramname):
return viewer.objectInfoWnd.getVal(paramname)
def getParamVals(paramnames):
return (getParamVal(name) for name in paramnames)
ik_solver = hikd.numIkSolver(dartMotionModel)
body_num = dartModel.getBodyNum()
# dJsys = np.zeros((6*body_num, totalDOF))
# dJsupL = np.zeros((6, totalDOF))
# dJsupR = np.zeros((6, totalDOF))
# Jpre = [np.zeros((6*body_num, totalDOF)), np.zeros((6, totalDOF)), np.zeros((6, totalDOF))]
###################################
#simulate
###################################
bodyIDs, contactPositions, contactPositionLocals, contactForces = [], [], [], []
def simulateCallback(frame):
# print()
# print(dartModel.getJointVelocityGlobal(0))
# print(dartModel.getDOFVelocities()[0])
# print(dartModel.get_dq()[:6])
# dartMotionModel.update(motion[frame])
global g_initFlag
global forceShowTime
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
# print('contactstate:', contact, contactChangeCount)
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2. * (Kt**.5)
Dl = (Kl**.5)
Dh = (Kh**.5)
dt_sup = 2. * (kt_sup**.5)
# Dt = .2*(Kt**.5)
# Dl = .2*(Kl**.5)
# Dh = .2*(Kh**.5)
# dt_sup = .2*(kt_sup**.5)
pdcontroller.setKpKd(Kt, Dt)
footHeight = dartModel.getBody(supL).shapenodes[0].shape.size()[1] / 2.
doubleTosingleOffset = 0.15
singleTodoubleOffset = 0.30
com_offset_x, com_offset_y, com_offset_z = getParamVals(
['com X offset', 'com Y offset', 'com Z offset'])
footOffset = np.array((com_offset_x, com_offset_y, com_offset_z))
# tracking
# th_r = motion.getDOFPositions(frame)
th_r = dartMotionModel.getDOFPositions()
th = dartModel.getDOFPositions()
th_r_flat = dartMotionModel.get_q()
# dth_r = motion.getDOFVelocities(frame)
# dth = dartModel.getDOFVelocities()
# ddth_r = motion.getDOFAccelerations(frame)
# ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
dth_flat = dartModel.get_dq()
# dth_flat = np.concatenate(dth)
# ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q())
# ddth_des_flat = pdcontroller.compute(th_r)
ddth_des_flat = pdcontroller.compute_flat(th_r_flat)
# ype.flatten(ddth_des, ddth_des_flat)
# ype.flatten(dth, dth_flat)
print(dartModel.skeleton.get_spd_tau(th_r_flat, Kt, Dt))
#################################################
# jacobian
#################################################
footOriL = dartModel.getJointOrientationGlobal(supL)
footOriR = dartModel.getJointOrientationGlobal(supR)
footCenterL = dartModel.getBodyPositionGlobal(supL)
footCenterR = dartModel.getBodyPositionGlobal(supR)
footBodyOriL = dartModel.getBodyOrientationGlobal(supL)
footBodyOriR = dartModel.getBodyOrientationGlobal(supR)
footBodyVelL = dartModel.getBodyVelocityGlobal(supL)
footBodyVelR = dartModel.getBodyVelocityGlobal(supR)
footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL)
footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR)
refFootL = dartMotionModel.getBodyPositionGlobal(supL)
refFootR = dartMotionModel.getBodyPositionGlobal(supR)
# refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
# refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootAngVelL = np.zeros(3)
refFootAngVelR = np.zeros(3)
refFootVelR = np.zeros(3)
refFootVelL = np.zeros(3)
contactR = 1
contactL = 1
if refFootVelR[1] < 0 and refFootVelR[1] * frame_step_size + refFootR[
1] > singleTodoubleOffset:
contactR = 0
if refFootVelL[1] < 0 and refFootVelL[1] * frame_step_size + refFootL[
1] > singleTodoubleOffset:
contactL = 0
if refFootVelR[1] > 0 and refFootVelR[1] * frame_step_size + refFootR[
1] > doubleTosingleOffset:
contactR = 0
if refFootVelL[1] > 0 and refFootVelL[1] * frame_step_size + refFootL[
1] > doubleTosingleOffset:
contactL = 0
# contactR = 0
# contMotionOffset = th[0][0] - th_r[0][0]
# contMotionOffset = dartModel.getBodyPositionGlobal(0) - dartMotionModel.getBodyPositionGlobal(0)
contMotionOffset = controlToMotionOffset
linkPositions = dartModel.getBodyPositionsGlobal()
linkVelocities = dartModel.getBodyVelocitiesGlobal()
linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal()
linkInertias = dartModel.getBodyInertiasGlobal()
CM = dartModel.skeleton.com()
dCM = dartModel.skeleton.com_velocity()
CM_plane = copy.copy(CM)
CM_plane[1] = 0.
dCM_plane = copy.copy(dCM)
dCM_plane[1] = 0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
#calculate contact state
#if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08:
if g_initFlag == 1:
#contact state
# 0: flying 1: right only 2: left only 3: double
#if contact == 2 and refFootR[1] < doubleTosingleOffset:
if contact == 2 and contactR == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
#elif contact == 3 and refFootL[1] < doubleTosingleOffset:
elif contact == 1 and contactL == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
#elif contact == 3 and refFootR[1] > doubleTosingleOffset:
elif contact == 3 and contactR == 0:
contact = 2
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
#elif contact == 3 and refFootL[1] > doubleTosingleOffset:
elif contact == 3 and contactL == 0:
contact = 1
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
else:
contact = 0
#if refFootR[1] < doubleTosingleOffset:
if contactR == 1:
contact += 1
#if refFootL[1] < doubleTosingleOffset:
if contactL == 1:
contact += 2
#initialization
if g_initFlag == 0:
softConstPoint = footCenterR.copy()
footCenter = footCenterL + (footCenterR - footCenterL) / 2.0
footCenter[1] = 0.
preFootCenter = footCenter.copy()
#footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL)
#footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR)
# if refFootR[1] < doubleTosingleOffset:
# contact +=1
# if refFootL[1] < doubleTosingleOffset:
# contact +=2
if refFootR[1] < footHeight:
contact += 1
if refFootL[1] < footHeight:
contact += 2
g_initFlag = 1
contact = 2
# contact = 1 + 2
# calculate jacobian
body_num = dartModel.getBodyNum()
Jsys = np.zeros((6 * body_num, totalDOF))
dJsys = np.zeros((6 * body_num, totalDOF))
for i in range(dartModel.getBodyNum()):
Jsys[6 * i:6 * i +
6, :] = dartModel.getBody(i).world_jacobian()[range(-3, 3), :]
dJsys[6 * i:6 * i + 6, :] = dartModel.getBody(
i).world_jacobian_classic_deriv()[range(-3, 3), :]
JsupL = dartModel.getBody(supL).world_jacobian()[range(-3, 3), :]
dJsupL = dartModel.getBody(supL).world_jacobian_classic_deriv()[
range(-3, 3), :]
JsupR = dartModel.getBody(supR).world_jacobian()[range(-3, 3), :]
dJsupR = dartModel.getBody(supR).world_jacobian_classic_deriv()[
range(-3, 3), :]
# calculate footCenter
footCenter = .5 * (footCenterL + footCenterR) + footOffset
if contact == 2:
footCenter = footCenterL.copy() + footOffset
if contact == 1:
footCenter = footCenterR.copy() + footOffset
footCenter[1] = 0.
footCenter[0] += 0.02
preFootCenter = footCenter.copy()
# linear momentum
# CM_ref_plane = footCenter.copy()
# CM_ref_plane += np.array([0., 0.9, 0.])
# dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane
# dL_des_plane[1] = 0.
kl = np.diagflat([Kl * 5., Kl, Kl * 5.])
dl = np.diagflat([2.2 * Dl, Dl, 2.2 * Dl])
CM_ref = footCenter.copy()
CM_ref[1] = dartMotionModel.getCOM()[1] - 0.1
# CM_ref += np.array((0., com_offset_y, 0.))
# dL_des_plane = Kl*totalMass*(CM_ref - CM) - Dl*totalMass*dCM
dL_des_plane = kl.dot(totalMass *
(CM_ref - CM)) - dl.dot(totalMass * dCM)
# angular momentum
CP_ref = footCenter
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0]) / frame_step_size
CP_old[0] = CP
CP_des[0] = None
# if CP_des[0] is None:
# CP_des[0] = footCenter
if CP is not None and dCP is not None:
ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP)
CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des * (
frame_step_size**2)
dH_des = np.cross(
CP_des[0] - CM,
dL_des_plane - totalMass * mm.s2v(dartModel.world.gravity()))
# dH_des = np.cross(footCenter - CM, dL_des_plane - totalMass*mm.s2v(dartModel.world.gravity()))
# H = np.dot(P, np.dot(Jsys, dth_flat))
# dH_des = -Kh * H[3:]
else:
dH_des = None
# set up equality constraint
a_oriL = mm.logSO3(
mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0, 1, 0])),
np.array([0, 1, 0])))
a_oriR = mm.logSO3(
mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0, 1, 0])),
np.array([0, 1, 0])))
footErrorL = refFootL.copy()
footErrorL[1] = dartModel.getBody(
supL).shapenodes[0].shape.size()[1] / 2.
footErrorL += -footCenterL + contMotionOffset
footErrorR = refFootR.copy()
footErrorR[1] = dartModel.getBody(
supR).shapenodes[0].shape.size()[1] / 2.
footErrorR += -footCenterR + contMotionOffset
a_supL = np.append(
kt_sup * footErrorL + dt_sup * (refFootVelL - footBodyVelL),
kt_sup * a_oriL + dt_sup * (refFootAngVelL - footBodyAngVelL))
a_supR = np.append(
kt_sup * footErrorR + dt_sup * (refFootVelR - footBodyVelR),
kt_sup * a_oriR + dt_sup * (refFootAngVelR - footBodyAngVelR))
# momentum matrix
RS = np.dot(P, Jsys)
R, S = np.vsplit(RS, 2)
rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat)
r_bias, s_bias = np.hsplit(rs, 2)
#######################################################
# optimization
#######################################################
if LEG_FLEXIBLE:
if contact == 2:
config['weightMap']['j_thigh_right'] = .8
config['weightMap']['j_shin_right'] = .8
config['weightMap']['j_heel_right'] = .8
else:
config['weightMap']['j_thigh_right'] = .1
config['weightMap']['j_shin_right'] = .25
config['weightMap']['j_heel_right'] = .2
if contact == 1:
config['weightMap']['j_thigh_left'] = .8
config['weightMap']['j_shin_left'] = .8
config['weightMap']['j_heel_left'] = .8
else:
config['weightMap']['j_thigh_left'] = .1
config['weightMap']['j_shin_left'] = .25
config['weightMap']['j_heel_left'] = .2
w = mot.getTrackingWeightDart(DOFs, dartModel.skeleton,
config['weightMap'])
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
if dH_des is not None:
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
if contact & 1:
mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat,
a_supR)
if contact & 2:
mot.addConstraint(problem, totalDOF, JsupL, dJsupL, dth_flat,
a_supL)
if contactChangeCount > 0:
contactChangeCount -= 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
r = problem.solve()
problem.clear()
# ype.nested(r['x'], ddth_sol)
ddth_sol = np.asarray(r['x'])
# ddth_sol[:6] = np.zeros(6)
if dH_des is None:
ddth_sol = ddth_des_flat
rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
inv_h = 1. / time_step
_bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = [], [], [], []
for iii in range(stepsPerFrame):
_ddq, _tau, _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = hqp.calc_QP(
dartModel.skeleton, ddth_sol, inv_h)
# _ddq, _tau, _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = hqp.calc_QP(dartModel.skeleton, ddth_des_flat, inv_h)
# print(frame, i, tau)
dartModel.applyPenaltyForce(_bodyIDs, _contactPositionLocals,
_contactForces)
dartModel.skeleton.set_forces(_tau)
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([
viewer.objectInfoWnd.labelForceX.value(),
viewer.objectInfoWnd.labelForceY.value(),
viewer.objectInfoWnd.labelForceZ.value()
])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += time_step
dartModel.applyPenaltyForce(selectedBodyId, localPos,
extraForce)
dartModel.step()
del bodyIDs[:]
del contactPositions[:]
del contactPositions[:]
del contactPositionLocals[:]
del contactForces[:]
bodyIDs.extend(_bodyIDs)
contactPositions.extend(_contactPositions)
contactPositionLocals.extend(_contactPositionLocals)
contactForces.extend(_contactForces)
# rendering
rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0]))
rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0]))
rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0, .1]))
rightFootPos[0] = footCenterL
rightVectorX[0] = np.dot(footBodyOriL, np.array([.1, 0, 0]))
rightVectorY[0] = np.dot(footBodyOriL, np.array([0, .1, 0]))
rightVectorZ[0] = np.dot(footBodyOriL, np.array([0, 0, .1]))
rightPos[0] = footCenterL + np.array([.1, 0, 0])
rd_footCenter[0] = footCenter
rd_footCenterL[0] = footCenterL
rd_footCenterR[0] = footCenterR
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
rd_CM_plane[0][1] = 0.
if CP is not None and dCP is not None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des[0]
rd_dL_des_plane[0] = [
dL_des_plane[0] / 100, dL_des_plane[1] / 100,
dL_des_plane[2] / 100
]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(
dartModel.world.gravity())
rd_root_des[0] = rootPos[0]
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append(contactForces[i] / 100)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exf_des[0] = [0, 0, 0]
else:
rd_exf_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exfen_des[0] = [0, 0, 0]
extraForcePos[0] = dartModel.getBodyPositionGlobal(selectedBody)
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def setUp(self):
pydart.init(verbose=False)
def main():
MOTION_ONLY = False
pydart.init()
env_name = 'multi'
ppo = PPO_MULTI(env_name, 0, visualize_only=True)
if not MOTION_ONLY:
ppo.LoadModel('model/param.pt')
ppo.env.specify_motion_num(5)
ppo.env.Resets(False)
# viewer settings
rd_contact_positions = [None]
rd_contact_forces = [None]
rd_targets = [None]
dart_world = ppo.env.world
viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False)
# viewer = hsv.hpSimpleViewer(rect=[0, 0, 960+300, 1+1080+55], viewForceWnd=False)
viewer.doc.addRenderer(
'MotionModel',
yr.DartRenderer(ppo.env.ref_world, (150, 150, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'controlModel',
yr.DartRenderer(dart_world, (255, 240, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'contact',
yr.VectorsRenderer(rd_contact_forces, rd_contact_positions,
(255, 0, 0)))
viewer.doc.addRenderer('targets',
yr.PointsRenderer(rd_targets, color=(0, 0, 0)))
viewer.setMaxFrame(3000)
cameraTargets = [None] * (viewer.getMaxFrame() + 1)
def preCallback(frame):
ppo.env.ref_skel.set_positions(ppo.env.ref_motion.get_q(frame))
def simulateCallback(frame):
state = ppo.env.GetState(0)
action_dist, _ = ppo.model(state.reshape(1, -1))
action = action_dist.loc.detach().numpy()
res = ppo.env.Steps(action)
del rd_targets[:]
rd_targets.extend(ppo.env.goals_in_world_frame)
# res = ppo.env.step_after_training(action[0])
if res[2]:
print(frame, 'Done')
ppo.env.reset()
# 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)
def postFrameCallback_Always(frame):
if cameraTargets[frame] is None:
cameraTargets[frame] = np.asarray(
dart_world.skel.body(0).to_world())
viewer.setCameraTarget(cameraTargets[frame])
if MOTION_ONLY:
viewer.setPreFrameCallback_Always(preCallback)
else:
viewer.setSimulateCallback(simulateCallback)
viewer.setPostFrameCallback_Always(postFrameCallback_Always)
viewer.startTimer(1 / ppo.env.ref_motion.fps)
viewer.show()
Fl.run()
def setUp(self):
pydart.init(verbose=False)
def main():
np.set_printoptions(precision=4, linewidth=200)
# np.set_printoptions(precision=4, linewidth=1000, threshold=np.inf)
pydart.init()
dartModel = cdm.DartModel(None, None, None, None, 'cart_pole_blade.skel')
dartMotionModel = cdm.DartModel(None, None, None, None,
'cart_pole_blade.skel')
footIdlist = list(
dartMotionModel.skeleton.body('h_' + name).index_in_skeleton()
for name in ['blade_left', 'blade_right'])
up_vec_in_each_link = dict()
for foot_id in footIdlist:
up_vec_in_each_link[
foot_id] = dartMotionModel.getBodyOrientationGlobal(foot_id)[1, :]
pelvis_pos = dartMotionModel.skeleton.dof_indices(
(["j_pelvis_pos_x", 'j_pelvis_pos_y', 'j_pelvis_pos_z']))
pelvis_x = dartMotionModel.skeleton.dof_indices((["j_pelvis_rot_x"]))
pelvis = dartMotionModel.skeleton.dof_indices(
(["j_pelvis_rot_y", "j_pelvis_rot_z"]))
upper_body = dartMotionModel.skeleton.dof_indices(
["j_abdomen_1", "j_abdomen_2"])
right_leg = dartMotionModel.skeleton.dof_indices([
"j_thigh_right_x", "j_thigh_right_y", "j_thigh_right_z", "j_shin_right"
])
left_leg = dartMotionModel.skeleton.dof_indices(
["j_thigh_left_x", "j_thigh_left_y", "j_thigh_left_z", "j_shin_left"])
arms = dartMotionModel.skeleton.dof_indices(
["j_bicep_left_x", "j_bicep_right_x"])
foot = dartMotionModel.skeleton.dof_indices(
["j_heel_left_1", "j_heel_left_2", "j_heel_right_1", "j_heel_right_2"])
leg_y = dartMotionModel.skeleton.dof_indices(
["j_thigh_right_y", "j_thigh_left_y"])
s0q = np.zeros(dartMotionModel.skeleton.ndofs)
s0q[pelvis_pos] = 0., .95, 0.
# s0q[pelvis] = 0., -0.
# s0q[upper_body] = 0.3, -0.
s0q[right_leg] = -0., -0., 0.9, -1.5
# s0q[left_leg] = 0., 0., 0.0, -0.1
# s0q[leg_y] = -0.785, 0.785
s0q[arms] = 1.5, -1.5
dartModel.set_q(s0q)
dartMotionModel.set_q(s0q)
frame_step_size = 1. / 40.
stepsPerFrame = 25
time_step = dartModel.world.time_step()
# wcfg.lockingVel = 0.01
# dartModel.initializeHybridDynamics()
#controlToMotionOffset = (1.5, -0.02, 0)
controlToMotionOffset = (0, 0, 2.0)
dartModel.translateByOffset(controlToMotionOffset)
totalDOF = dartModel.getTotalDOF()
DOFs = dartModel.getDOFs()
# parameter
Kt = 25.
Dt = 2. * (Kt**.5)
Kl = 100.
Dl = 2. * (Kt**.5)
Kh = 100.
Dh = 2. * (Kt**.5)
Ks = 20000.
Ds = 2. * (Kt**.5)
Bt = 1.
Bl = 0.1
Bh = 0.13
supL = dartModel.skeleton.body('h_blade_left').index_in_skeleton()
supR = dartModel.skeleton.body('h_blade_right').index_in_skeleton()
selectedBody = dartModel.skeleton.body('h_head').index_in_skeleton()
# momentum matrix
linkMasses = dartModel.getBodyMasses()
print([body.name for body in dartModel.skeleton.bodynodes])
print(linkMasses)
totalMass = dartModel.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
a_supL = (0, 0, 0, 0, 0, 0)
a_supR = (0, 0, 0, 0, 0, 0)
a_sup_2 = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
CP_old = [mm.v3(0., 0., 0.)]
CP_des = [None]
dCP_des = [np.zeros(3)]
# penalty method
# bodyIDsToCheck = range(dartModel.getBodyNum())
bodyIDsToCheck = [supL, supR]
#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)
config = dict()
config['weightMap'] = {
'j_scapula_left': .2,
'j_bicep_left': .2,
'j_forearm_left': .2,
'j_hand_left': .2,
'j_scapula_right': .2,
'j_bicep_right': .2,
'j_forearm_right': .2,
'j_hand_right': .2,
'j_abdomen': .6,
'j_spine': .6,
'j_head': .6,
'j_heel_right': .2,
'j_heel_left': .2,
'j_pelvis': 0.5,
'j_thigh_left': .1,
'j_shin_left': .3,
'j_thigh_right': .1,
'j_shin_right': .3
}
# viewer
rd_footCenter = [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_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]
# viewer = ysv.SimpleViewer()
viewer = hsv.hpSimpleViewer(viewForceWnd=False)
viewer.setMaxFrame(1000)
#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', cvr.VpModelRenderer(motionModel, (150,150,255), yr.POLYGON_FILL))
# viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
#viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'motionModel',
yr.DartRenderer(dartMotionModel.world, (255, 240, 255),
yr.POLYGON_FILL))
viewer.doc.addRenderer(
'controlModel',
yr.DartRenderer(dartModel.world, (150, 150, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
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, 0, 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)))
#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 = 11.
# initKh = 22.
initKl = 100.
initKh = 100.
initBl = .1
initBh = .13
initSupKt = 17.
# initSupKt = 2.5
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., 100., 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)
pdcontroller = PDController(dartModel, dartModel.skeleton,
dartModel.world.time_step(), Kt, Dt)
def getParamVal(paramname):
return viewer.objectInfoWnd.getVal(paramname)
def getParamVals(paramnames):
return (getParamVal(name) for name in paramnames)
ik_solver = hikd.numIkSolver(dartMotionModel)
body_num = dartModel.getBodyNum()
# dJsys = np.zeros((6*body_num, totalDOF))
# dJsupL = np.zeros((6, totalDOF))
# dJsupR = np.zeros((6, totalDOF))
# Jpre = [np.zeros((6*body_num, totalDOF)), np.zeros((6, totalDOF)), np.zeros((6, totalDOF))]
l_idx = [dartModel.skeleton.body("h_blade_left").index_in_skeleton()]
r_idx = [dartModel.skeleton.body("h_blade_right").index_in_skeleton()]
up_vec_in_each_link = {supL: mm.unitY(), supR: mm.unitY()}
mbc = DartMomentumBalanceController(dartModel.skeleton,
dartMotionModel.skeleton,
config['weightMap'],
up_vec_in_each_link)
###################################
#simulate
###################################
def simulateCallback(frame):
# print()
# print(dartModel.getJointVelocityGlobal(0))
# print(dartModel.getDOFVelocities()[0])
# print(dartModel.get_dq()[:6])
# dartMotionModel.update(motion[frame])
global g_initFlag
global forceShowTime
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
# print('contactstate:', contact, contactChangeCount)
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
mbc.set_parameters(Kt, Kl, Kh, Bl, Bh, kt_sup)
pdcontroller.setKpKd(Kt, Dt)
footHeight = dartModel.getBody(supL).shapenodes[0].shape.size()[1] / 2.
doubleTosingleOffset = 0.15
singleTodoubleOffset = 0.30
com_offset_x, com_offset_y, com_offset_z = getParamVals(
['com X offset', 'com Y offset', 'com Z offset'])
footOffset = np.array((com_offset_x, com_offset_y, com_offset_z))
# tracking
th_r_flat = dartMotionModel.get_q()
dth_flat = dartModel.get_dq()
ddth_des_flat = pdcontroller.compute_flat(th_r_flat)
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
ddth_sol = mbc.solve(
ddth_des_flat,
# [dartModel.skeleton.body('h_blade_left').index_in_skeleton(), dartModel.skeleton.body('h_blade_right').index_in_skeleton()],
[dartModel.skeleton.body('h_blade_left').index_in_skeleton()],
footOffset + np.array([0.9]),
r_idx,
l_idx,
CP,
None)
localPos = np.zeros(3)
inv_h = 1. / time_step
for i in range(stepsPerFrame):
ddq, tau, bodyIDs, contactPositions, contactPositionLocals, contactForces = hqp.calc_QP(
dartModel.skeleton, ddth_sol, inv_h)
# ddq, tau, bodyIDs, contactPositions, contactPositionLocals, contactForces = hqp.calc_QP(dartModel.skeleton, ddth_des_flat, inv_h)
# print(frame, i, tau)
dartModel.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
dartModel.skeleton.set_forces(tau)
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([
viewer.objectInfoWnd.labelForceX.value(),
viewer.objectInfoWnd.labelForceY.value(),
viewer.objectInfoWnd.labelForceZ.value()
])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += time_step
dartModel.applyPenaltyForce(selectedBodyId, localPos,
extraForce)
dartModel.step()
# rendering
rd_CM[0] = dartModel.skeleton.com()
rd_CM_plane[0] = dartModel.skeleton.com().copy()
rd_CM_plane[0][1] = 0.
rd_root_des[0] = rootPos[0]
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append(contactForces[i] / 100)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exf_des[0] = [0, 0, 0]
else:
rd_exf_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exfen_des[0] = [0, 0, 0]
extraForcePos[0] = dartModel.getBodyPositionGlobal(selectedBody)
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
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 init_dart(self):
pydart.init()
def main():
np.set_printoptions(precision=4, linewidth=200)
# np.set_printoptions(precision=4, linewidth=1000, threshold=np.inf)
#motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5()
motion, mcfg, wcfg, stepsPerFrame, config, frame_rate= mit.create_biped()
#motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped()
frame_step_size = 1./frame_rate
pydart.init()
dartModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON)
dartMotionModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON)
# wcfg.lockingVel = 0.01
# dartModel.initializeHybridDynamics()
if not DART_CONTACT_ON:
dartModel.initContactPoint()
#controlToMotionOffset = (1.5, -0.02, 0)
# controlToMotionOffset = (1.5, 0, 0)
# dartModel.translateByOffset(controlToMotionOffset)
totalDOF = dartModel.getTotalDOF()
DOFs = dartModel.getDOFs()
# 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']
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
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')
# momentum matrix
linkMasses = dartModel.getBodyMasses()
totalMass = dartModel.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
a_supL = (0,0,0, 0,0,0)
a_supR = (0,0,0, 0,0,0)
a_sup_2 = (0,0,0, 0,0,0, 0,0,0, 0,0,0)
CP_old = [mm.v3(0.,0.,0.)]
CP_des = [None]
dCP_des = [np.zeros(3)]
# penalty method
# bodyIDsToCheck = range(dartModel.getBodyNum())
bodyIDsToCheck = [supL, supR]
#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_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_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]
# viewer = ysv.SimpleViewer()
viewer = hsv.hpSimpleViewer(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', cvr.VpModelRenderer(motionModel, (150,150,255), yr.POLYGON_FILL))
# viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
#viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL))
# viewer.doc.addRenderer('motionModel', yr.DartModelRenderer(dartMotionModel, (150,150,255), yr.POLYGON_FILL))
# viewer.doc.addRenderer('controlModel', yr.DartModelRenderer(dartModel, (255,240,255), yr.POLYGON_FILL))
viewer.doc.addRenderer('controlModel', yr.DartRenderer(dartModel.world, (150,150,255), yr.POLYGON_FILL))
viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
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,0,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)))
#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 = 11.
# initKh = 22.
initKl = 100.
initKh = 100.
initBl = .1
initBh = .13
initSupKt = 17.
# initSupKt = 2.5
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., 100., 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.reset = False
def viewer_reset(object):
viewer.reset = True
viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState)
viewer.objectInfoWnd.addBtn('reset', viewer_reset)
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)
pdcontroller = PDController(dartModel, dartModel.skeleton, wcfg.timeStep, Kt, Dt, config['PDweightMap'])
def getParamVal(paramname):
return viewer.objectInfoWnd.getVal(paramname)
def getParamVals(paramnames):
return (getParamVal(name) for name in paramnames)
ik_solver = hikd.numIkSolver(dartMotionModel)
body_num = dartModel.getBodyNum()
# dJsys = np.zeros((6*body_num, totalDOF))
# dJsupL = np.zeros((6, totalDOF))
# dJsupR = np.zeros((6, totalDOF))
# Jpre = [np.zeros((6*body_num, totalDOF)), np.zeros((6, totalDOF)), np.zeros((6, totalDOF))]
###################################
#simulate
###################################
def simulateCallback(frame):
print(frame)
# print()
# print(dartModel.getJointVelocityGlobal(0))
# print(dartModel.getDOFVelocities()[0])
# print(dartModel.get_dq()[:6])
dartMotionModel.update(motion[frame])
global g_initFlag
global forceShowTime
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
# print('contactstate:', contact, contactChangeCount)
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
# Dt = 2.*(Kt**.5)
Dt = Kt/100.
Dl = (Kl**.5)
Dh = (Kh**.5)
dt_sup = 2.*(kt_sup**.5)
# Dt = .2*(Kt**.5)
# Dl = .2*(Kl**.5)
# Dh = .2*(Kh**.5)
# dt_sup = .2*(kt_sup**.5)
pdcontroller.setKpKd(Kt, Dt)
footHeight = dartModel.getBody(supL).shapenodes[0].shape.size()[1]/2.
doubleTosingleOffset = 0.15
singleTodoubleOffset = 0.30
#doubleTosingleOffset = 0.09
doubleTosingleVelOffset = 0.0
com_offset_x, com_offset_y, com_offset_z = getParamVals(['com X offset', 'com Y offset', 'com Z offset'])
footOffset = np.array((com_offset_x, com_offset_y, com_offset_z))
des_com = dartMotionModel.getCOM() + footOffset
footCenterL = dartMotionModel.getBodyPositionGlobal(supL)
footCenterR = dartMotionModel.getBodyPositionGlobal(supR)
footBodyOriL = dartMotionModel.getBodyOrientationGlobal(supL)
footBodyOriR = dartMotionModel.getBodyOrientationGlobal(supR)
torso_pos = dartMotionModel.getBodyPositionGlobal(4)
torso_ori = dartMotionModel.getBodyOrientationGlobal(4)
# ik_solver.setInitPose(motion[frame])
# ik_solver.addConstraints(supL, np.zeros(3), footCenterL, footBodyOriL, (True, True, True, True))
# ik_solver.addConstraints(supR, np.zeros(3), footCenterR, footBodyOriR, (True, True, True, True))
# ik_solver.addConstraints(4, np.zeros(3), torso_pos, torso_ori, (False, False, False, True))
# ik_solver.solve(des_com)
# ik_solver.clear()
# tracking
# th_r = motion.getDOFPositions(frame)
th_r = dartMotionModel.getDOFPositions()
th = dartModel.getDOFPositions()
th_r_flat = dartMotionModel.get_q()
# dth_r = motion.getDOFVelocities(frame)
# dth = dartModel.getDOFVelocities()
# ddth_r = motion.getDOFAccelerations(frame)
# ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
dth_flat = dartModel.get_dq()
# dth_flat = np.concatenate(dth)
# ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q())
# ddth_des_flat = pdcontroller.compute(th_r)
ddth_des_flat = pdcontroller.compute_flat(th_r_flat)
# ype.flatten(ddth_des, ddth_des_flat)
# ype.flatten(dth, dth_flat)
#################################################
# jacobian
#################################################
footOriL = dartModel.getJointOrientationGlobal(supL)
footOriR = dartModel.getJointOrientationGlobal(supR)
footCenterL = dartModel.getBodyPositionGlobal(supL)
footCenterR = dartModel.getBodyPositionGlobal(supR)
footBodyOriL = dartModel.getBodyOrientationGlobal(supL)
footBodyOriR = dartModel.getBodyOrientationGlobal(supR)
footBodyVelL = dartModel.getBodyVelocityGlobal(supL)
footBodyVelR = dartModel.getBodyVelocityGlobal(supR)
footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL)
footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR)
refFootL = dartMotionModel.getBodyPositionGlobal(supL)
refFootR = dartMotionModel.getBodyPositionGlobal(supR)
# refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
# refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootAngVelL = np.zeros(3)
refFootAngVelR = np.zeros(3)
refFootJointVelR = motion.getJointVelocityGlobal(supR, frame)
refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootJointR = motion.getJointPositionGlobal(supR, frame)
# refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR-refFootJointR))
refFootVelR = np.zeros(3)
refFootJointVelL = motion.getJointVelocityGlobal(supL, frame)
refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
refFootJointL = motion.getJointPositionGlobal(supL, frame)
# refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL-refFootJointL))
refFootVelL = np.zeros(3)
contactR = 1
contactL = 1
if refFootVelR[1] < 0 and refFootVelR[1]*frame_step_size + refFootR[1] > singleTodoubleOffset:
contactR = 0
if refFootVelL[1] < 0 and refFootVelL[1]*frame_step_size + refFootL[1] > singleTodoubleOffset:
contactL = 0
if refFootVelR[1] > 0 and refFootVelR[1]*frame_step_size + refFootR[1] > doubleTosingleOffset:
contactR = 0
if refFootVelL[1] > 0 and refFootVelL[1]*frame_step_size + refFootL[1] > doubleTosingleOffset:
contactL = 0
# contactR = 0
# contMotionOffset = th[0][0] - th_r[0][0]
contMotionOffset = dartModel.getBodyPositionGlobal(0) - dartMotionModel.getBodyPositionGlobal(0)
linkPositions = dartModel.getBodyPositionsGlobal()
linkVelocities = dartModel.getBodyVelocitiesGlobal()
linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal()
linkInertias = dartModel.getBodyInertiasGlobal()
CM = dartModel.skeleton.com()
dCM = dartModel.skeleton.com_velocity()
CM_plane = copy.copy(CM)
CM_plane[1]=0.
dCM_plane = copy.copy(dCM)
dCM_plane[1]=0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias)
#calculate contact state
#if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08:
if g_initFlag == 1:
#contact state
# 0: flying 1: right only 2: left only 3: double
#if contact == 2 and refFootR[1] < doubleTosingleOffset:
if contact == 2 and contactR==1:
contact = 3
maxContactChangeCount+=30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
#elif contact == 3 and refFootL[1] < doubleTosingleOffset:
elif contact == 1 and contactL==1:
contact = 3
maxContactChangeCount+=30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
#elif contact == 3 and refFootR[1] > doubleTosingleOffset:
elif contact == 3 and contactR == 0:
contact = 2
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
#elif contact == 3 and refFootL[1] > doubleTosingleOffset:
elif contact == 3 and contactL == 0:
contact = 1
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
else:
contact = 0
#if refFootR[1] < doubleTosingleOffset:
if contactR == 1:
contact +=1
#if refFootL[1] < doubleTosingleOffset:
if contactL == 1:
contact +=2
#initialization
if g_initFlag == 0:
softConstPoint = footCenterR.copy()
footCenter = footCenterL + (footCenterR - footCenterL)/2.0
footCenter[1] = 0.
preFootCenter = footCenter.copy()
#footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL)
#footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR)
# if refFootR[1] < doubleTosingleOffset:
# contact +=1
# if refFootL[1] < doubleTosingleOffset:
# contact +=2
if refFootR[1] < footHeight:
contact +=1
if refFootL[1] < footHeight:
contact +=2
g_initFlag = 1
#calculate jacobian
body_num = dartModel.getBodyNum()
Jsys = np.zeros((6*body_num, totalDOF))
dJsys = np.zeros((6*body_num, totalDOF))
for i in range(dartModel.getBodyNum()):
# body_i_jacobian = dartModel.getBody(i).world_jacobian()[range(-3, 3), :]
# body_i_jacobian_deriv = dartModel.getBody(i).world_jacobian_classic_deriv()[range(-3, 3), :]
# Jsys[6*i:6*i+6, :] = body_i_jacobian
# dJsys[6*i:6*i+6, :] = body_i_jacobian_deriv
Jsys[6*i:6*i+6, :] = dartModel.getBody(i).world_jacobian()[range(-3, 3), :]
dJsys[6*i:6*i+6, :] = dartModel.getBody(i).world_jacobian_classic_deriv()[range(-3, 3), :]
# dJsys = (Jsys - Jpre[0])/frame_step_size
# Jpre[0] = Jsys.copy()
JsupL = dartModel.getBody(supL).world_jacobian()[range(-3, 3), :]
dJsupL = dartModel.getBody(supL).world_jacobian_classic_deriv()[range(-3, 3), :]
# dJsupL = np.zeros_like(JsupL)
# dJsupL = (JsupL - Jpre[1])/frame_step_size
# Jpre[1] = JsupL.copy()
JsupR = dartModel.getBody(supR).world_jacobian()[range(-3, 3), :]
dJsupR = dartModel.getBody(supR).world_jacobian_classic_deriv()[range(-3, 3), :]
# dJsupR = np.zeros_like(JsupR)
# dJsupR = (JsupR - Jpre[2])/frame_step_size
# Jpre[2] = JsupR.copy()
#calculate footCenter
footCenter = .5 * (footCenterL + footCenterR) + footOffset
#if refFootR[1] >doubleTosingleOffset:
#if refFootR[1] > doubleTosingleOffset or footCenterR[1] > 0.08:
#if contact == 1 or footCenterR[1] > 0.08:
#if contact == 2 or footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
footCenter = footCenterL.copy() + footOffset
#elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
footCenter = footCenterR.copy() + footOffset
footCenter[1] = 0.
if contactChangeCount > 0 and contactChangeType == 'StoD':
#change footcenter gradually
footCenter = preFootCenter + (maxContactChangeCount - contactChangeCount)*(footCenter-preFootCenter)/maxContactChangeCount
preFootCenter = footCenter.copy()
# linear momentum
#TODO:
# We should consider dCM_ref, shouldn't we?
# add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel!
# todo that, set joint velocities to vpModel
CM_ref_plane = footCenter
dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane
dL_des_plane[1] = 0.
# CM_ref = footCenter.copy()
# CM_ref[1] = dartMotionModel.getCOM()[1]
# CM_ref += np.array((0., com_offset_y, 0.))
# dL_des_plane = Kl*totalMass*(CM_ref - CM) - Dl*totalMass*dCM
# angular momentum
CP_ref = footCenter
bodyIDs, contactPositions, contactPositionLocals, contactForces = [], [], [], []
if DART_CONTACT_ON:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info()
else:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
#bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0])/frame_step_size
CP_old[0] = CP
CP_des[0] = None
# if CP_des[0] is None:
# CP_des[0] = footCenter
if CP is not None and dCP is not None:
ddCP_des = Kh*(CP_ref - CP) - Dh*(dCP)
CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des*(frame_step_size**2)
# dCP_des[0] += ddCP_des * frame_step_size
# CP_des[0] += dCP_des[0] * frame_step_size + .5 * ddCP_des*(frame_step_size ** 2)
dH_des = np.cross(CP_des[0] - CM, dL_des_plane + totalMass*mm.s2v(wcfg.gravity))
if contactChangeCount > 0:# and contactChangeType == 'DtoS':
#dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount*10)
dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount)
#dH_des *= (contactChangeCount)/(maxContactChangeCount)*.9+.1
else:
dH_des = None
# H = np.dot(P, np.dot(Jsys, dth_flat))
# dH_des = -Kh* H[3:]
# soft point constraint
#softConstPoint = refFootR.copy()
##softConstPoint[0] += 0.2
#Ksc = 50
#Dsc = 2*(Ksc**.5)
#Bsc = 1.
#P_des = softConstPoint
#P_cur = controlModel.getBodyPositionGlobal(constBody)
#dP_des = [0, 0, 0]
#dP_cur = controlModel.getBodyVelocityGlobal(constBody)
#ddP_des1 = Ksc*(P_des - P_cur) + Dsc*(dP_des - dP_cur)
#r = P_des - P_cur
#I = np.vstack(([1,0,0],[0,1,0],[0,0,1]))
#Z = np.hstack((I, mm.getCrossMatrixForm(-r)))
#yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks)
#dJconst = (Jconst - Jconst)/(1/30.)
#JconstPre = Jconst.copy()
##yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False)
#JL, JA = np.vsplit(Jconst, 2)
#Q1 = np.dot(Z, Jconst)
#q1 = np.dot(JA, dth_flat)
#q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r))
#q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2
#set up equality constraint
a_oriL = mm.logSO3(mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0,1,0])), np.array([0,1,0])))
a_oriR = mm.logSO3(mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0,1,0])), np.array([0,1,0])))
footErrorL = refFootL.copy()
footErrorL[1] = dartModel.getBody(supL).shapenodes[0].shape.size()[1]/2.
footErrorL += -footCenterL + contMotionOffset
footErrorR = refFootR.copy()
footErrorR[1] = dartModel.getBody(supR).shapenodes[0].shape.size()[1]/2.
footErrorR += -footCenterR + contMotionOffset
a_supL = np.append(kt_sup*footErrorL + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL))
a_supR = np.append(kt_sup*footErrorR + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR))
if contactChangeCount > 0 and contactChangeType == 'DtoS':
a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), 4*kt_sup*a_oriL+2*dt_sup*(refFootAngVelL-footBodyAngVelL))
a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), 4*kt_sup*a_oriR+2*dt_sup*(refFootAngVelR-footBodyAngVelR))
elif contactChangeCount > 0 and contactChangeType == 'StoD':
linkt = (13.*contactChangeCount)/(maxContactChangeCount)+1.
lindt = 2*(linkt**.5)
angkt = (13.*contactChangeCount)/(maxContactChangeCount)+1.
angdt = 2*(angkt**.5)
a_supL = np.append(linkt*kt_sup*(refFootL - footCenterL + contMotionOffset) + lindt*dt_sup*(refFootVelL - footBodyVelL), angkt*kt_sup*a_oriL+angdt*dt_sup*(refFootAngVelL-footBodyAngVelL))
a_supR = np.append(linkt*kt_sup*(refFootR - footCenterR + contMotionOffset) + lindt*dt_sup*(refFootVelR - footBodyVelR), angkt*kt_sup*a_oriR+angdt*dt_sup*(refFootAngVelR-footBodyAngVelR))
# momentum matrix
RS = np.dot(P, Jsys)
R, S = np.vsplit(RS, 2)
rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat)
r_bias, s_bias = np.hsplit(rs, 2)
#######################################################
# optimization
#######################################################
#if contact == 2 and footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
config['weightMap']['RightUpLeg'] = .8
config['weightMap']['RightLeg'] = .8
config['weightMap']['RightFoot'] = .8
else:
config['weightMap']['RightUpLeg'] = .1
config['weightMap']['RightLeg'] = .25
config['weightMap']['RightFoot'] = .2
#if contact == 1 and footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
config['weightMap']['LeftUpLeg'] = .8
config['weightMap']['LeftLeg'] = .8
config['weightMap']['LeftFoot'] = .8
else:
config['weightMap']['LeftUpLeg'] = .1
config['weightMap']['LeftLeg'] = .25
config['weightMap']['LeftFoot'] = .2
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
#if contact == 2:
#mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
if dH_des is not None:
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
#mot.setConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup)
#mot.addConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup)
#if contact & 1 and contactChangeCount == 0:
if contact & 1:
#if refFootR[1] < doubleTosingleOffset:
mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_supR)
if contact & 2:
#if refFootL[1] < doubleTosingleOffset:
mot.addConstraint(problem, totalDOF, JsupL, dJsupL, dth_flat, a_supL)
if contactChangeCount >0:
contactChangeCount -= 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
r = problem.solve()
problem.clear()
# ype.nested(r['x'], ddth_sol)
ddth_sol = np.asarray(r['x'])
# ddth_sol[:6] = np.zeros(6)
rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
for i in range(stepsPerFrame):
# apply penalty force
if not DART_CONTACT_ON:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
dartModel.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces)
#bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
# dartModel.skeleton.set_accelerations(ddth_sol)
# dartModel.skeleton.set_accelerations(ddth_des_flat)
# dartModel.skeleton.set_forces(np.zeros(totalDOF))
# ddth_des_flat[:6] = np.zeros(6)
th_r_flat = dartMotionModel.get_q()
ddth_des_flat = pdcontroller.compute_flat(th_r_flat)
dartModel.skeleton.set_forces(ddth_des_flat)
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value()])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += wcfg.timeStep
dartModel.applyPenaltyForce(selectedBodyId, localPos, extraForce)
dartModel.step()
if DART_CONTACT_ON:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info()
else:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
if viewer.reset:
viewer.reset = False
dartModel.reset()
# print(dartModel.getCOM())
# rendering
rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0]))
rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0]))
rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0,.1]))
rightFootPos[0] = footCenterL
rightVectorX[0] = np.dot(footBodyOriL, np.array([.1,0,0]))
rightVectorY[0] = np.dot(footBodyOriL, np.array([0,.1,0]))
rightVectorZ[0] = np.dot(footBodyOriL, np.array([0,0,.1]))
rightPos[0] = footCenterL + np.array([.1,0,0])
rd_footCenter[0] = footCenter
rd_footCenterL[0] = footCenterL
rd_footCenterR[0] = footCenterR
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
rd_CM_plane[0][1] = 0.
if CP is not None and dCP is not None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des[0]
rd_dL_des_plane[0] = [dL_des_plane[0]/100, dL_des_plane[1]/100, dL_des_plane[2]/100]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
rd_root_des[0] = rootPos[0]
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append( contactForces[i]/100)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100]
rd_exf_des[0] = [0,0,0]
else:
rd_exf_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100]
rd_exfen_des[0] = [0,0,0]
extraForcePos[0] = dartModel.getBodyPositionGlobal(selectedBody)
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def main():
MOTION_ONLY = False
pydart.init()
env_name = 'walk'
ppo = PPO(env_name, 1, visualize_only=True)
if not MOTION_ONLY:
ppo.LoadModel('model/' + env_name + '.pt')
ppo.env.Resets(False)
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]
dart_world = ppo.env.world
viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False)
viewer.doc.addRenderer(
'MotionModel',
yr.DartRenderer(ppo.env.ref_world, (150, 150, 255), yr.POLYGON_FILL))
if not MOTION_ONLY:
viewer.doc.addRenderer(
'controlModel',
yr.DartRenderer(dart_world, (255, 240, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'contact',
yr.VectorsRenderer(rd_contact_forces, rd_contact_positions,
(255, 0, 0)))
def postCallback(frame):
ppo.env.ref_skel.set_positions(ppo.env.ref_motion.get_q(frame))
def simulateCallback(frame):
state = ppo.env.GetState(0)
action_dist, _ = ppo.model(torch.tensor(state.reshape(1, -1)).float())
action = action_dist.loc.detach().numpy()
res = ppo.env.Steps(action)
# res = ppo.env.Steps(np.zeros_like(action))
# 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)
if res[2]:
print(frame, 'Done')
ppo.env.reset()
# 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)
if MOTION_ONLY:
viewer.setPostFrameCallback_Always(postCallback)
viewer.setMaxFrame(len(ppo.env.ref_motion) - 1)
else:
viewer.setSimulateCallback(simulateCallback)
viewer.setMaxFrame(3000)
viewer.startTimer(1. / 30.)
viewer.show()
Fl.run()
import os
from gym import error, spaces
from gym.utils import seeding
import numpy as np
from os import path
import gym
import six
from gym.envs.dart.static_window import *
try:
import pydart2 as pydart
from pydart2.gui.trackball import Trackball
pydart.init()
except ImportError as e:
raise error.DependencyNotInstalled(
"{}. (HINT: you need to install pydart2.)".format(e))
class DartEnv(gym.Env):
"""Superclass for all Dart environments.
"""
def __init__(self, model_paths, frame_skip, observation_size, action_bounds, \
dt=0.002, obs_type="parameter", action_type="continuous", visualize=True, disableViewer=False,\
screen_width=80, screen_height=45):
assert obs_type in ('parameter', 'image')
assert action_type in ("continuous", "discrete")
print('pydart initialization OK')
def main():
MOTION_ONLY = False
pydart.init()
env_name = 'walk'
ppo = PPO(env_name, 0, visualize_only=True)
if not MOTION_ONLY:
ppo.LoadModel('model/param.pt')
# ppo.LoadModel('walk_model_10240127/'+'181'+'.pt')
ppo.generate_rnn_motion()
ppo.envs_send_rnn_motion()
ppo.env.Resets(False)
# ppo.replace_motion_num = ppo.rnn_len
# 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_target_position = [None]
rd_com = [None]
dart_world = ppo.env.world
viewer = hsv.hpSimpleViewer(rect=[0, 0, 1280 + 300, 720 + 1 + 55],
viewForceWnd=False)
viewer.doc.addRenderer(
'MotionModel',
yr.DartRenderer(ppo.env.ref_world, (150, 150, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer('target',
yr.PointsRenderer(rd_target_position, (0, 255, 0)))
viewer.doc.addRenderer('motion',
yr.JointMotionRenderer(ppo.env.ref_motion))
if not MOTION_ONLY:
viewer.doc.addRenderer(
'controlModel',
yr.DartRenderer(dart_world, (255, 240, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'contact',
yr.VectorsRenderer(rd_contact_forces, rd_contact_positions,
(255, 0, 0)))
viewer.doc.addRenderer('CM_plane',
yr.PointsRenderer(rd_com, (0, 0, 255)))
last_frame = [0]
q = ppo.env.ref_skel.q
def simulateCallback(frame):
ppo.env.ref_motion.frame = frame - last_frame[0]
del rd_target_position[:]
del rd_com[:]
rd_target_position.append(
ppo.env.goals_in_world_frame[ppo.env.phase_frame])
com = ppo.env.skel.com()
com[1] = 0.
rd_com.append(com)
state = ppo.env.GetState(0)
action_dist, _ = ppo.model(torch.tensor(state.reshape(1, -1)).float())
action = action_dist.loc.detach().numpy()
# res = ppo.env.Steps(np.zeros_like(action))
res = ppo.env.Steps(action)
# res = [False, False, False]
# print(res[1])
# res = ppo.env.Steps(np.zeros_like(action))
# 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(ppo.env.goal)
if res[2]:
print(frame, 'Done')
last_frame[0] = frame
ppo.generate_rnn_motion()
ppo.envs_send_rnn_motion()
ppo.env.reset()
# 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)
# viewer.setPreFrameCallback_Always(preCallback)
viewer.setSimulateCallback(simulateCallback)
viewer.setMaxFrame(3000)
viewer.startTimer(1. / 30.)
viewer.show()
Fl.run()
