def compute_desired_accel(self, qpos_err, qvel_err, k_p, k_d):
dt = self.model.opt.timestep
nv = self.model.nv
M = np.zeros(nv * nv)
mjf.mj_fullM(self.model, M, self.data.qM)
M.resize(self.model.nv, self.model.nv)
C = self.data.qfrc_bias.copy()
K_p = np.diag(k_p)
K_d = np.diag(k_d)
q_accel = cho_solve(cho_factor(M + K_d * dt,
overwrite_a=True,
check_finite=False),
-C[:, None] - K_p.dot(qpos_err[:, None]) -
K_d.dot(qvel_err[:, None]),
overwrite_b=True,
check_finite=False)
return q_accel.squeeze()
def compute_desired_accel(self, qpos_err, qvel_err):
dt = self.model.opt.timestep
nv = self.model.nv
M = np.zeros(nv * nv)
mjf.mj_fullM(self.model, M, self.data.qM)
M.resize(self.model.nv, self.model.nv)
C = self.data.qfrc_bias.copy()
k_p = np.zeros(nv)
k_d = np.zeros(nv)
k_p[6:] = self.cfg.jkp
k_d[6:] = self.cfg.jkd
K_p = np.diag(k_p)
K_d = np.diag(k_d)
q_accel = cho_solve(
cho_factor(M + K_d * dt), -C[:, None] -
K_p.dot(qpos_err[:, None]) - K_d.dot(qvel_err[:, None]))
return q_accel.squeeze()
m.forward()
q = np.random.uniform(-1., 1., (N, m.model.nq))
qdot = np.random.uniform(-1., 1., (N, m.model.nq))
sim_state = m.get_state()
t1 = time.time()
for i in tqdm(range(M)):
for j in range(N):
for k in range(m.model.nq):
sim_state.qpos[k] = q[j, k]
sim_state.qvel[k] = qdot[j, k]
m.set_state(sim_state)
m.forward()
M_ = np.zeros(m.model.nv * m.model.nv)
functions.mj_fullM(m.model, M_, m.data.qM)
t2 = time.time()
print("Mujoco: ", t2 - t1)
# ==============================================================================
# tf_rbdl
# ==============================================================================
ic = initial_config_from_mjcf(xml_path, ee_list, verbose=False)
mass_matrix(tf.convert_to_tensor(q, tf.float32), ic['pidlist'], ic['Mlist'],
ic['Glist'], ic['Slist'])
t1 = time.time()
for i in tqdm(range(M)):
mass_matrix(tf.convert_to_tensor(q, tf.float32), ic['pidlist'],
ic['Mlist'], ic['Glist'], ic['Slist'])
t2 = time.time()
print("tf_rbdl with GPU: ", t2 - t1)
def _get_qM(self, dynsim, J):
""" Joint mass matrix for current pose """
qM = np.zeros(self.n_joints * self.n_joints + 100)
mjlib.mj_fullM(dynsim.model, qM, dynsim.data.qM)
return qM[0:49].reshape(self.n_joints, self.n_joints)
v_intial = sim.data.site_xvelp[1]
v_desired = np.array([0, 0, 0])
a_desired = np.array([0, 0, 0])
a_intial = np.array([0, 0, 0])
dt = sim.model.opt.timestep
#sim.data.get_site_jacp('target', jacp=target_jacp)
# Should be unchanged after steps (zero action)
graph = []
for _ in range(100000):
F[:3] = np.dot(K, x_desired - x_intial) + np.dot(
C, v_desired - v_intial) + np.dot(A, a_desired - a_intial)
H = np.zeros(sim.model.nv * sim.model.nv)
functions.mj_fullM(sim.model, H, sim.data.qM)
sim.data.get_site_jacp('target', jacp=target_jacp)
sim.data.get_site_jacr('target', jacr=target_jacr)
J_L = target_jacp.reshape((3, sim.model.nv))
J_A = target_jacr.reshape((3, sim.model.nv))
J = np.concatenate((J_L, J_A), axis=0)
H_L = np.dot(
np.linalg.pinv(J_L.T),
np.dot(H.reshape(sim.model.nv, sim.model.nv), np.linalg.pinv(J_L)))
H_all = np.dot(
np.linalg.pinv(J.T),
np.dot(H.reshape(sim.model.nv, sim.model.nv), np.linalg.pinv(J)))
#F_a=np.dot(A,0.3-sim.data.qacc)
#action = np.dot(J_L.T, np.dot(H_L, F[:3]))+sim.data.qfrc_bias
action = sim.data.qfrc_bias + np.dot(H.reshape(3, 3), np.dot(J_L.T, F[:3]))
def general_usage(xml_path, ee_list, N):
# ==========================================================================
# Quantaties
# ==========================================================================
ee_SE3_mujoco, ee_jac_mujoco, M_mujoco, qfrc_bias_mujoco = dict(), dict(
), [], []
ee_SE3_tf_rbdl, ee_jac_tf_rbdl, M_tf_rbdl, qfrc_bias_tf_rbdl = dict(
), dict(), None, None
for ee in ee_list:
ee_SE3_mujoco[ee], ee_jac_mujoco[ee], ee_SE3_tf_rbdl[
ee], ee_jac_tf_rbdl[ee] = [], [], None, None
# ==========================================================================
# Mujoco
# ==========================================================================
mujoco_model = load_model_from_path(xml_path)
m = MjSim(mujoco_model)
m.forward()
sim_state = m.get_state()
q = np.random.uniform(-1., 1., (N, m.model.nq))
qdot = np.random.uniform(-1., 1., (N, m.model.nq))
for i in tqdm(range(N)):
for j in range(m.model.nq):
sim_state.qpos[j] = q[i, j]
sim_state.qvel[j] = qdot[i, j]
m.set_state(sim_state)
m.forward()
for ee in ee_list:
T_ee = np.eye(4)
T_ee[0:3, 0:3] = m.data.get_body_xmat(ee)
T_ee[0:3, 3] = m.data.get_body_xpos(ee)
ee_SE3_mujoco[ee].append(T_ee)
J_ee = np.zeros((6, m.model.nq))
jacp = np.zeros(m.model.nv * 3)
jacr = np.zeros(m.model.nv * 3)
functions.mj_jac(m.model, m.data, jacp, jacr,
m.data.get_body_xpos(ee),
m.model.body_name2id(ee))
J_ee[0:3, :] = jacr.reshape(3, m.model.nv)
J_ee[3:6, :] = jacp.reshape(3, m.model.nv)
ee_jac_mujoco[ee].append(J_ee)
M_ = np.zeros(m.model.nv * m.model.nv)
functions.mj_fullM(m.model, M_, m.data.qM)
M_mujoco.append(M_.reshape(m.model.nq, m.model.nq))
qfrc_bias_mujoco.append(np.copy(m.data.qfrc_bias))
# ==========================================================================
# tf_rbdl
# ==========================================================================
ic = initial_config_from_mjcf(xml_path, ee_list, verbose=True)
for ee in ee_list:
ee_SE3_tf_rbdl[ee] = fk(ic['init_ee_SE3'][ee], ic['Blist'][ee],
ic['Bidlist'][ee],
tf.convert_to_tensor(q, tf.float32))
ee_local_jac_tf_rbdl = jacobian(ic['Blist'][ee], ic['Bidlist'][ee],
tf.convert_to_tensor(q, tf.float32))
R = ee_SE3_tf_rbdl[ee][:, 0:3, 0:3]
RR = tf.concat([
tf.concat([R, tf.zeros((N, 3, 3))], 2),
tf.concat([tf.zeros((N, 3, 3)), R], 2)
], 1)
ee_jac_tf_rbdl[ee] = tf.matmul(RR, ee_local_jac_tf_rbdl)
M_tf_rbdl = mass_matrix(
tf.convert_to_tensor(q, tf.float32), ic['pidlist'], ic['Mlist'],
ic['Glist'], ic['Slist']) + tf.eye(m.model.nq) * ic['joint_armature']
cori = coriolis_forces(tf.convert_to_tensor(q, tf.float32),
tf.convert_to_tensor(qdot,
tf.float32), ic['pidlist'],
ic['Mlist'], ic['Glist'], ic['Slist'])
grav = gravity_forces(tf.convert_to_tensor(q, tf.float32), ic['g'],
ic['pidlist'], ic['Mlist'], ic['Glist'], ic['Slist'])
qfrc_bias_tf_rbdl = cori + grav
# ==========================================================================
# Comparing
# ==========================================================================
# print("="*80)
# print("q")
# print(q)
# print("qdot")
# print(qdot)
# print("="*80)
# for i in range(N):
# for ee in ee_list:
# print("-"*80)
# print("{} SE3".format(ee))
# print("Mujoco\n{}\ntf_rbdl\n{}\nComparison\n{}".format(ee_SE3_mujoco[ee][i], ee_SE3_tf_rbdl[ee][i].numpy(), np.isclose(ee_SE3_mujoco[ee][i], ee_SE3_tf_rbdl[ee][i].numpy(),atol=1e-5)))
# print("-"*80)
# print("{} Jacobian".format(ee))
# print("Mujoco\n{}\ntf_rbdl\n{}\nComparison\n{}".format(ee_jac_mujoco[ee][i], ee_jac_tf_rbdl[ee][i].numpy(), np.isclose(ee_jac_mujoco[ee][i], ee_jac_tf_rbdl[ee][i].numpy(),atol=1e-5)))
# print("-"*80)
# print("Mass Matrix")
# print("Mujoco\n{}\ntf_rbdl\n{}\nComparison\n{}".format(M_mujoco[i], M_tf_rbdl[i].numpy(), np.isclose(M_mujoco[i], M_tf_rbdl[i].numpy(),atol=1e-5)))
# print("-"*80)
# print("Bias frc")
# print("Mujoco\n{}\ntf_rbdl\n{}\nComparison\n{}".format(qfrc_bias_mujoco[i], qfrc_bias_tf_rbdl[i].numpy(), np.isclose(qfrc_bias_mujoco[i], qfrc_bias_tf_rbdl[i].numpy(),atol=1e-5)))
# print("="*80)
for i in range(N):
for ee in ee_list:
assert np.allclose(
ee_SE3_mujoco[ee][i],
ee_SE3_tf_rbdl[ee][i].numpy(),
atol=1e-04), "Mujoco\n{}\ntf_rbdl\n{}\nComparison\n{}".format(
ee_SE3_mujoco[ee][i], ee_SE3_tf_rbdl[ee][i].numpy(),
np.isclose(ee_SE3_mujoco[ee][i],
ee_SE3_tf_rbdl[ee][i].numpy()))
# print("[{} SE3] PASSED]".format(ee))
assert np.allclose(
ee_jac_mujoco[ee][i],
ee_jac_tf_rbdl[ee][i].numpy(),
atol=1e-04), "Mujoco\n{}\ntf_rbdl\n{}\nComparison\n{}".format(
ee_jac_mujoco[ee][i], ee_jac_tf_rbdl[ee][i].numpy(),
np.isclose(ee_jac_mujoco[ee][i],
ee_jac_tf_rbdl[ee][i].numpy()))
# print("[{} Jacobian] PASSED]".format(ee))
assert np.allclose(
M_mujoco[i], M_tf_rbdl[i].numpy(),
atol=1e-04), "Mujoco\n{}\ntf_rbdl\n{}\nComparison\n{}".format(
M_mujoco[i], M_tf_rbdl[i].numpy(),
np.isclose(M_mujoco[i], M_tf_rbdl[i].numpy()))
# print("[Mass Matrix] PASSED]")
assert np.allclose(
qfrc_bias_mujoco[i], qfrc_bias_tf_rbdl[i].numpy(),
atol=1e-04), "Mujoco\n{}\ntf_rbdl\n{}\nComparison\n{}".format(
qfrc_bias_mujoco[i], qfrc_bias_tf_rbdl[i].numpy(),
np.isclose(qfrc_bias_mujoco[i], qfrc_bias_tf_rbdl[i].numpy()))
# print("[Coriolis + Gravity] PASSED]")
print("[PASSED]")