def pre(self,q):
robot = self.robot
self.H = hessian(robot,q)
self.J = robot.data.J.copy()
pin.crba(robot.model,robot.data,q)
self.Y =[ (robot.data.oMi[i]*robot.data.Ycrb[i]).matrix() for i in range(0,robot.model.njoints) ]
self.dM = np.zeros([robot.model.nv,]*3)
def pre(self, q):
robot = self.robot
self.H = hessian(robot, q)
self.J = robot.data.J.copy()
se3.crba(robot.model, robot.data, q)
self.Y = [(robot.data.oMi[i] * robot.data.Ycrb[i]).matrix()
for i in range(0, robot.model.njoints)]
self.dM = np.zeros([
robot.model.nv,
] * 3)
def test_impulseDynamics_rd(self):
data_no_q = self.model.createData()
vnext = pin.impulseDynamics(self.model, self.data, self.q, self.v0,
self.J, r_coeff, inv_damping)
self.assertLess(np.linalg.norm(vnext), self.tolerance)
pin.crba(self.model, data_no_q, self.q)
vnext_no_q = pin.impulseDynamics(self.model, data_no_q, self.v0,
self.J, r_coeff, inv_damping)
self.assertLess(np.linalg.norm(vnext_no_q), self.tolerance)
self.assertApprox(vnext, vnext_no_q)
def test_impulseDynamics_no_q(self):
data4 = self.data
data5 = self.model.createData()
data6 = self.model.createData()
data7_deprecated = self.model.createData()
pin.crba(self.model, data4, self.q)
pin.crba(self.model, data5, self.q)
pin.crba(self.model, data6, self.q)
pin.crba(self.model, data7_deprecated, self.q)
vnext4 = pin.impulseDynamics(self.model, data4, self.v, self.J)
vnext5 = pin.impulseDynamics(self.model, data5, self.v, self.J,
r_coeff)
vnext6 = pin.impulseDynamics(self.model, data6, self.v, self.J,
r_coeff, inv_damping)
with warnings.catch_warnings(record=True) as warning_list:
vnext7_deprecated = pin.impulseDynamics(self.model,
data7_deprecated, self.q,
self.v, self.J, r_coeff,
False)
self.assertTrue(
any(item.category == pin.DeprecatedWarning
for item in warning_list))
self.assertTrue((vnext4 == vnext5).all())
self.assertTrue((vnext4 == vnext6).all())
self.assertTrue((vnext5 == vnext6).all())
self.assertTrue((vnext7_deprecated == vnext6).all())
def compute_contact_forces(self,
qa,
dqa,
o_R_b,
b_v_ob,
b_omg_ob,
_,
tauj,
world_frame=True):
o_quat_b = pin.Quaternion(o_R_b).coeffs()
q = np.concatenate(
[np.array([0, 0, 0]), o_quat_b,
qa]) # robot anywhere with orientation estimated from IMU
vq = np.concatenate([b_v_ob, b_omg_ob, dqa])
# vq = np.hstack([np.zeros(3), b_omg_ob, dqa])
C = pin.computeCoriolisMatrix(self.rm, self.rd, q, vq)
g = pin.computeGeneralizedGravity(self.rm, self.rd, q)
M = pin.crba(self.rm, self.rd, q)
p = M @ vq
tauj = np.concatenate([np.zeros(6), tauj])
self.int = self.int + (tauj + C.T @ vq - g + self.r) * self.dt
self.r = self.Ki * (p - self.int - self.p0)
return taucf_to_oforces(self.robot, q, self.nv, o_R_b, self.r,
self.contact_frame_id_lst)
def Kid(q_, J_=None):
pinocchio.computeJointJacobians(model, data, q_)
J = pinocchio.getJointJacobian(model, data, model.joints[-1].id, pinocchio.ReferenceFrame.LOCAL).copy()
if J_ is not None:
J_[:, :] = J
M = pinocchio.crba(model, data, q_).copy()
return np.bmat([[M, J.T], [J, zero([6, 6])]])
def test_computeMinverse(self):
model = self.model
Minv = pin.computeMinverse(model, self.data, self.q)
data2 = model.createData()
M = pin.crba(model, data2, self.q)
self.assertApprox(np.linalg.inv(M), Minv)
def MvJtf(q, vnext, v, f):
M = pinocchio.crba(rmodel, rdata, q)
pinocchio.computeJointJacobians(rmodel, rdata, q)
pinocchio.forwardKinematics(rmodel, rdata, q)
pinocchio.updateFramePlacements(rmodel, rdata)
J = pinocchio.getFrameJacobian(rmodel, rdata, CONTACTFRAME,
pinocchio.ReferenceFrame.LOCAL)
return M * (vnext - v) - J.T * f
def talker():
rospy.init_node("tocabi_pinocchio", anonymous = False)
rospy.Subscriber("/tocabi/pinocchio/jointstates", JointState, callback)
global model
pub = rospy.Publisher("/tocabi/pinocchio", model, queue_size=1)
modelmsg.CMM = [0 for i in range(33*6)]
modelmsg.COR = [0 for i in range(33*33)]
modelmsg.M = [0 for i in range(33*33)]
modelmsg.g = [0 for i in range(33)]
rate = rospy.Rate(1000) #10hz
model = pinocchio.buildModelFromUrdf("/home/jhk/catkin_ws/src/dyros_tocabi/tocabi_description/robots/dyros_tocabi.urdf",pinocchio.JointModelFreeFlyer())
data = model.createData()
global start
q = pinocchio.randomConfiguration(model)
qdot = pinocchio.utils.zero(model.nv)
qdot_t = pinocchio.utils.zero(model.nv)
qddot_t = pinocchio.utils.zero(model.nv)
while not rospy.is_shutdown():
if start:
global qmsg
for i in range(0, 39):
q[i] = qmsg.position[i]
for j in range(0, 38):
qdot[j] = qmsg.velocity[j]
qdot_t[j] = 0.0
qddot_t[j] = 0.0
CMM = pinocchio.computeCentroidalMap(model, data, q)
pinocchio.crba(model, data, q)
pinocchio.computeCoriolisMatrix(model, data, q, qdot)
pinocchio.rnea(model, data, q, qdot_t, qddot_t)
CMM_slice = CMM[:,6:39]
COR_slice = data.C[:,6:39]
M_slice = data.M[:,6:39]
g_slice = data.tau[6:39]
modelmsg.CMM = CMM_slice.flatten()
modelmsg.COR = COR_slice.flatten()
modelmsg.M = M_slice.flatten()
modelmsg.g = g_slice.flatten()
def cid(q_, v_, tau_):
pinocchio.computeJointJacobians(model, data, q_)
J6 = pinocchio.getJointJacobian(model, data, model.joints[-1].id, pinocchio.ReferenceFrame.LOCAL).copy()
J = J6[:3, :]
b = pinocchio.rnea(model, data, q_, v_, zero(model.nv)).copy()
M = pinocchio.crba(model, data, q_).copy()
pinocchio.forwardKinematics(model, data, q_, v_, zero(model.nv))
gamma = data.a[-1].linear + cross(data.v[-1].angular, data.v[-1].linear)
K = np.bmat([[M, J.T], [J, zero([3, 3])]])
r = np.concatenate([tau_ - b, -gamma])
return inv(K) * r
def init_viewer(enable_viewer):
# loadSolo(False) to load solo12
# loadSolo(True) to load solo8
solo = robots_loader.loadSolo(False)
if enable_viewer:
solo.initDisplay(loadModel=True)
if ('viewer' in solo.viz.__dict__):
solo.viewer.gui.addFloor('world/floor')
solo.viewer.gui.setRefreshIsSynchronous(False)
"""offset = np.zeros((19, 1))
offset[5, 0] = 0.7071067811865475
offset[6, 0] = 0.7071067811865475 - 1.0
temp = solo.q0 + offset"""
solo.display(solo.q0)
pin.centerOfMass(solo.model, solo.data, solo.q0, np.zeros((18, 1)))
pin.updateFramePlacements(solo.model, solo.data)
pin.crba(solo.model, solo.data, solo.q0)
return solo
def imp_ctrl(self, q, force_dir, joint_id, vel, pos, v, K_xy, K_z):
q = self.trafo_q(q)
v = self.trafo_q(v)
m = 1.0
d_xy = np.sqrt(4 * m * 500)
d_z = np.sqrt(4 * m * 500)
M = np.eye(3) * m
M_inv = np.linalg.pinv(M)
D = np.eye(3)
D[0, 0] = D[1, 1] = d_xy
D[2, 2] = d_z
K = np.eye(3)
K[0, 0] = K_xy
K[1, 1] = K_xy
K[2, 2] = K_z
F = 0.01 #0.05
M_joint = pin.crba(self.model, self.data, q)
pos_gain = np.matmul(K, pos)
damp_gain = np.matmul(D, vel)
# damp_gain = 0.0
force_gain = F * force_dir
acc = np.matmul(M_inv, (force_gain - pos_gain - damp_gain))
# TODO: I think it is sufficient if this is calculated only once:
full_J_var = pin.computeJointJacobiansTimeVariation(
self.model, self.data, q, v)
J_var_frame = pin.getJointJacobianTimeVariation(
self.model, self.data, joint_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[:3, :]
dyn_comp = np.matmul(J_var_frame, v)
acc = acc - dyn_comp
# TODO: I think it is sufficient if this is calculated only once:
full_JAC = pin.computeJointJacobians(self.model, self.data, q)
J_NEW = pin.getJointJacobian(
self.model, self.data, joint_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[:3, :]
J = J_NEW
J_inv = np.matmul(np.linalg.pinv(np.matmul(J.T, J)), J.T)
# print (J_inv)
torque = np.matmul(M_joint, np.matmul(J_inv, acc))
return self.inv_trafo_q(torque)
def init_robot(q_init, enable_viewer):
"""Load the solo model and initialize the Gepetto viewer if it is enabled
Args:
q_init (array): the default position of the robot actuators
enable_viewer (bool): if the Gepetto viewer is enabled or not
"""
# Load robot model and data
# Initialisation of the Gepetto viewer
print(enable_viewer)
solo = load('solo12', display=enable_viewer)
q = solo.q0.reshape((-1, 1))
q[7:, 0] = q_init
"""if enable_viewer:
solo.initViewer(loadModel=True)
if ('viewer' in solo.viz.__dict__):
solo.viewer.gui.addFloor('world/floor')
solo.viewer.gui.setRefreshIsSynchronous(False)"""
if enable_viewer:
solo.display(q)
print("PASS")
# Initialisation of model quantities
pin.centerOfMass(solo.model, solo.data, q, np.zeros((18, 1)))
pin.updateFramePlacements(solo.model, solo.data)
pin.crba(solo.model, solo.data, solo.q0)
# Initialisation of the position of footsteps
fsteps_init = np.zeros((3, 4))
indexes = [10, 18, 26, 34]
for i in range(4):
fsteps_init[:, i] = solo.data.oMf[indexes[i]].translation
h_init = (solo.data.oMf[1].translation -
solo.data.oMf[indexes[0]].translation)[2]
fsteps_init[2, :] = 0.0
return solo, fsteps_init, h_init
def taskspace_control(self):
while self.engage:
# Reset for the Integral History
if not np.array_equal(self.desired_position,
self.prev_DesiredPosition):
self.log = []
# Read the states
self.joint_positions = self.simulator.get_state()[0]
self.joint_velocities = self.simulator.get_state()[1]
y, v = self.FK_Solver(self.joint_positions, self.joint_velocities)
# Control's Offline Term Estimates
M = pin.crba(self.model, self.data, self.joint_positions)
N = pin.nonLinearEffects(self.model, self.data,
self.joint_positions,
self.joint_velocities)
tau = (self.Lp * (self.desired_position - y) + self.Ld *
(self.desired_velocity - v) + self.Li * np.sum(self.log))
# Build Online Control Terms
J = self.Jacobian_Solver(self.joint_positions)
DJ = J # Naive Method as Pinocchio for DJ is not stable.
JM = M @ linalg.pinv(J)
# Compute & Send Torques
self.TAU = (JM @ tau) + N - 2 * (JM @ DJ @ self.joint_velocities)
self.clipped_TAU = np.clip(self.TAU, -10, 10)
self.simulator.send_joint_torque(self.clipped_TAU)
self.simulator.step()
# PID: Integral Anti-Windup
error = self.desired_position - y
if not np.array_equal(self.TAU, self.clipped_TAU):
sign_tau = np.sign(self.TAU)
sign_error = np.sign(error)
for i in range(sign_tau.shape[0]):
if sign_tau[i] == sign_error[i]:
self.Li = 0
break
else:
self.Li = self.Li_copy
self.log.append(error)
# Store the Desired Position
self.prev_DesiredPosition = self.desired_position
sleep(self.freq)
def eval_f(tau, user_data = None):
assert len(tau) == nvar
tempRobot = robot
#vectorNorm = agnp.linalg.norm(tau)
vectorNorm = 0
# tauNorm = 0
# for tk in tau:
# tauNorm += tk**2
# tauNorm = agnp.sqrt(tauNorm)
q = q0.copy()
vq = vq0.copy()
aq = aq0.copy()
qNorm = agnp.empty([timeSteps, 1, robot.nq, 1])
t = 0
while (t < timeSteps):
tau_k = tau[t*robot.nv:(t*robot.nv)+robot.nv].copy()
tau_k = np.asmatrix(tau_k).T
b_k = se3.rnea(tempRobot.model, tempRobot.data, q, vq, aq)
if (math.isnan(b_k[0])):
print
print("t")
print(t)
print
print("tau")
print(tau)
print
print("q")
print(q)
print
print("vq")
print(vq)
time.sleep(100)
exit()
M_k = se3.crba(tempRobot.model, tempRobot.data, q)
aq = np.linalg.inv(M_k)*(tau_k - b_k)
vq += dt*aq
q = se3.integrate(tempRobot.model, q, vq*dt)
qNorm[t, 0] = q.copy()
t += 1
#qNorm = np.linalg.norm(qRef - qNorm)
qNorm = agnp.linalg.norm(qNorm-qRef)
#return vectorNorm + qNorm
return vectorNorm + qNorm
def joint_control(self):
while self.engage:
# Reset for the Integral History
if not np.array_equal(self.desired_position,
self.prev_DesiredPosition):
self.log = []
# Read the states
self.joint_positions = self.simulator.get_state()[0]
self.joint_velocities = self.simulator.get_state()[1]
# Control's Offline Term Estimates
M = pin.crba(self.model, self.data, self.joint_positions)
N = pin.nonLinearEffects(self.model, self.data,
self.joint_positions,
self.joint_velocities)
tau = (self.Kp * (self.desired_position - self.joint_positions) +
self.Kd * (self.desired_velocity - self.joint_velocities) +
self.Ki * np.sum(self.log))
self.TAU = M @ tau + N
self.clipped_TAU = np.clip(self.TAU, -10, 10)
self.simulator.send_joint_torque(self.clipped_TAU)
self.simulator.step()
# PID: Integral Anti-Windup
error = self.desired_position - self.joint_positions
if not np.array_equal(self.TAU, self.clipped_TAU):
sign_tau = np.sign(self.TAU)
sign_error = np.sign(error)
for i in range(sign_tau.shape[0]):
if sign_tau[i] == sign_error[i]:
self.Ki = 0
break
else:
self.Ki = self.Ki_copy
self.log.append(error)
# Store the Desired Position
self.prev_DesiredPosition = self.desired_position
sleep(self.freq)
def test_forwardDynamics_default(self):
data_no_q = self.model.createData()
self.model.gravity = pin.Motion.Zero()
ddq = pin.forwardDynamics(self.model, self.data, self.q, self.v0,
self.tau0, self.J, self.gamma)
self.assertLess(np.linalg.norm(ddq), self.tolerance)
KKT_inverse = pin.getKKTContactDynamicMatrixInverse(
self.model, self.data, self.J)
M = pin.crba(self.model, self.model.createData(), self.q)
self.assertApprox(
M,
np.linalg.inv(KKT_inverse)[:self.model.nv, :self.model.nv])
pin.computeAllTerms(self.model, data_no_q, self.q, self.v0)
ddq_no_q = pin.forwardDynamics(self.model, data_no_q, self.tau0,
self.J, self.gamma)
self.assertLess(np.linalg.norm(ddq_no_q), self.tolerance)
self.assertApprox(ddq, ddq_no_q)
data = pinmodel.createData()
###
#qdd = np.array(([0]*(model.num_joints+6)))
###
print("\n\n\nYOHHHHHHH")
q_pin = q[model.mask_q_pyb_to_pin]
qd_pin = qd[model.mask_qd_pyb_to_pin]
qdd_pin = qdd[model.mask_qd_pyb_to_pin]
# q_pin[2]=1
# qd_pin[2]=1
# qdd_pin[2]=-9.81
print("shape q_pin:", q_pin.shape)
print("Q_PIN\n", np.round(q_pin, 5))
print("QD_PIN\n", np.round(qd_pin, 5))
print("QDD_PIN\n", np.round(qdd_pin, 5))
pin.crba(pinmodel, data, q_pin)
pin.computeCoriolisMatrix(pinmodel, data, q_pin, qd_pin)
datag = pin.computeGeneralizedGravity(pinmodel, data, q_pin)
pin.rnea(pinmodel, data, q_pin, qd_pin, qdd_pin)
print("M\n", np.round(data.M, 5))
print("C\n", np.round(data.C, 5))
print("g\n", np.round(datag, 5))
print("TAU_PIN\n", np.round(data.tau, 4))
print("\n")
indices = list(range(model.num_joints))
joint_states = p.getJointStates(model.Id, indices)
joint_states_np = np.array(joint_states)
tau_real_np = joint_states_np[:, 3]
print("TAU REAL SHAPE: ", tau_real_np.shape)
print("TAU REAL: ", tau_real_np)
print("\n\n\n")
da_dq = data.ddq_dq.copy()
assertNumDiff(
da_dq, da_dqn, NUMDIFF_MODIFIER *
h) # threshold was 3e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
# Check ABA versus RNEA derivatives (with forces)
assertNumDiff(
inv(data.M) * dtau_dq, -da_dq, NUMDIFF_MODIFIER *
h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
# Check ABA versus RNEA + forces (no derivatives)
del a
for i, f in enumerate(fs[:-1]):
fs[i] = f * 0
f = fs[-1].vector
M = pinocchio.crba(model, data, q).copy()
pinocchio.computeJointJacobians(model, data, q)
J = pinocchio.getJointJacobian(model, data, model.joints[-1].id,
pinocchio.ReferenceFrame.LOCAL).copy()
b = pinocchio.rnea(model, data, q, v, zero(model.nv)).copy()
a = pinocchio.aba(model, data, q, v, tau, fs).copy()
assert (absmax(a - (inv(M) * (tau - b + J.T * f))) < 1e-6)
tau = pinocchio.rnea(model, data, q, v, a, fs)
assert (absmax(tau - (M * a + b - J.T * f)) < 1e-6)
# ------------------------------------------------
# ------------------------------------------------
# ------------------------------------------------
# Checking linear acceleration (i.e. ahat = a.linear + w x v = [a + v x (vlinear,0)].linear )
# --- dCRBA ---
dcrba = DCRBA(robot)
dcrba.pre(q)
Mp = dcrba()
# --- Validate dM/dq by finite diff
dM = np.zeros([
robot.model.nv,
] * 3)
eps = 1e-6
dq = zero(robot.model.nv)
for diff in range(robot.model.nv):
dM[:, :, diff] = -se3.crba(robot.model, robot.data, q)
dq *= 0
dq[diff] = eps
qdq = se3.integrate(robot.model, q, dq)
dM[:, :, diff] += se3.crba(robot.model, robot.data, qdq)
dM /= eps
print(
'Check dCRBA = \t\t\t',
max([
norm(Mp[:, :, diff] - dM[:, :, diff])
for diff in range(robot.model.nv)
]))
# --- dCRBA ---
# --- dCRBA ---
# --- dCRBA ---
dcrba = DCRBA(robot)
dcrba.pre(q)
Mp = dcrba()
# --- Validate dM/dq by finite diff
dM = np.zeros([robot.model.nv,]*3)
eps = 1e-6
dq = zero(robot.model.nv)
for diff in range(robot.model.nv):
dM[:,:,diff] = -pin.crba(robot.model,robot.data,q)
dq *=0; dq[diff] = eps
qdq = pin.integrate(robot.model,q,dq)
dM[:,:,diff] += pin.crba(robot.model,robot.data,qdq)
dM /= eps
print('Check dCRBA = \t\t\t', max([ norm(Mp[:,:,diff]-dM[:,:,diff]) for diff in range(robot.model.nv) ]))
# --- vRNEA ---
# --- vRNEA ---
# --- vRNEA ---
def get_mass_matrix(self):
return np.copy(pin.crba(self._model, self._data, self._q))
def step(self,
inner_step,
u,
dt=None,
use_exponential_integrator=True,
dt_force_pred=None,
ndt_force_pred=None,
update_expm=True):
if dt is None:
dt = self.dt
if self.first_iter:
self.compute_forces()
self.first_iter = False
# se3.forwardKinematics(self.model, self.data, self.q, self.v, zero(self.model.nv))
# se3.computeJointJacobians(self.model, self.data)
# se3.updateFramePlacements(self.model, self.data)
se3.crba(self.model, self.data, self.q)
se3.nonLinearEffects(self.model, self.data, self.q, self.v)
i = 0
for c in self.contacts:
if (c.active):
self.Jc[3 * i:3 * i + 3, :] = c.getJacobianWorldFrame()
c.f_inner[:, inner_step] = self.f[3 * i:3 * i + 3]
i += 1
# array containing the forces predicting during the time step (meaningful only for exponential integrator)
if (dt_force_pred is not None):
f_pred = np.empty((self.nk, ndt_force_pred)) * nan
for c in self.contacts:
c.f_pred = zero((3, ndt_force_pred))
c.f_avg = zero((3, ndt_force_pred))
c.f_avg2 = zero((3, ndt_force_pred))
else:
f_pred = None
f_pred_int = None
if (not use_exponential_integrator):
self.dv = self.forward_dyn(self.S.T @ u + self.Jc.T @ self.f)
v_mean = self.v + 0.5 * dt * self.dv
self.v += self.dv * dt
self.q = se3.integrate(self.model, self.q, v_mean * dt)
else:
x0, dv_bar, JMinv = self.compute_exponential_LDS(u, update_expm)
# Sanity check on contact point Jacobian and dJv
# self.compute_dJv_finite_difference()
# USE THE VALUE COMPUTED WITH FINITE DIFFERENCING FOR NOW
# self.dJv = np.copy(self.debug_dJv_fd)
# self.a[self.nk:] = JMinv@(self.S.T@u-h) + self.dJv
self.logToFile(x0)
if (update_expm):
if (self.unilateral_contacts == 'QP'):
self.int_exp_A = compute_integral_expm(self.A, dt)
self.dp0_qp = self.solve_friction_QP(x0, dt)
x0[self.nk:] -= self.dp0_qp
int_x = self.expMatHelper.compute_integral_x_T(
self.A, self.a, x0, dt, self.max_mat_mult)
# store int_x because it may be needed to compute int2_x without updating expm in next iteration
self.int_x_prev = int_x
else:
int_x = self.expMatHelper.compute_next_integral()
# compute average force
self.f_avg = self.D @ int_x / dt
# project average forces in friction cones
self.f_avg_pre_projection = np.copy(self.f_avg)
i = 0
for c in self.contacts:
if (c.active):
if (self.unilateral_contacts == 'projection'):
self.f_avg[3 * i:3 * i + 3] = c.project_force_in_cone(
self.f_avg[3 * i:3 * i + 3])
c.f_avg[:, inner_step] = self.f_avg[3 * i:3 * i + 3]
i += 1
dv_mean = dv_bar + JMinv.T @ self.f_avg
if (self.use_second_integral):
if (update_expm):
int2_x = self.expMatHelper.compute_double_integral_x_T(
self.A, self.a, x0, dt, self.max_mat_mult)
else:
int2_x = self.expMatHelper.compute_next_double_integral()
int2_x -= dt * self.int_x_prev
self.int_x_prev += int_x
self.f_avg2 = self.D @ int2_x / (0.5 * dt * dt)
# project average forces in friction cones
self.f_avg2_pre_projection = np.copy(self.f_avg2)
i = 0
for c in self.contacts:
if (c.active):
if (self.unilateral_contacts == 'projection'):
self.f_avg2[3 * i:3 * i +
3] = c.project_force_in_cone(
self.f_avg2[3 * i:3 * i + 3])
c.f_avg2[:, inner_step] = self.f_avg2[3 * i:3 * i + 3]
i += 1
v_mean = self.v + 0.5 * dt * (dv_bar + JMinv.T @ self.f_avg2)
else:
v_mean = self.v + 0.5 * dt * dv_mean
if (dt_force_pred is not None):
# predict intermediate forces using linear dynamical system (i.e. matrix exponential)
n = self.A.shape[0]
C = np.zeros((n + 1, n + 1))
C[0:n, 0:n] = self.A
C[0:n, n] = self.a
z = np.concatenate((x0, [1.0]))
e_TC = expm(dt_force_pred / ndt_force_pred * C)
for i in range(ndt_force_pred):
f_pred[:, i] = self.D @ z[:n]
z = e_TC @ z
i = 0
for c in self.contacts:
if (c.active):
c.f_pred = f_pred[3 * i:3 * i + 3, :]
i += 1
# predict also what forces we would get by integrating with the force prediction
int_x = self.expMatHelper.compute_integral_x_T(
self.A,
self.a,
x0,
dt_force_pred,
self.max_mat_mult,
store=False)
int2_x = self.expMatHelper.compute_double_integral_x_T(
self.A,
self.a,
x0,
dt_force_pred,
self.max_mat_mult,
store=False)
D_int_x = self.D @ int_x
D_int2_x = self.D @ int2_x
v_mean_pred = self.v + 0.5 * dt_force_pred * dv_bar + JMinv.T @ D_int2_x / dt_force_pred
dv_mean_pred = dv_bar + JMinv.T @ D_int_x / dt_force_pred
v_pred = self.v + dt_force_pred * dv_mean_pred
q_pred = se3.integrate(self.model, self.q,
v_mean_pred * dt_force_pred)
se3.forwardKinematics(self.model, self.data, q_pred, v_pred)
se3.updateFramePlacements(self.model, self.data)
f_pred_int = np.zeros(self.nk)
# comment these lines because they were messing up the anchor point updates
# for (i,c) in enumerate(self.contacts):
# f_pred_int[3*i:3*i+3] = c.compute_force(self.unilateral_contacts)
# DEBUG: predict forces integrating contact point dynamics while updating robot dynamics M and h
# t = dt_force_pred/ndt_force_pred
# f_pred[:, 0] = K_p0 + self.D @ x0
# x = np.copy(x0)
# q, v = np.copy(self.q), np.copy(self.v)
# for i in range(1,ndt_force_pred):
# # integrate robot state
# dv = np.linalg.solve(M, self.S.T@u - h + self.Jc.T@f_pred[:,i-1])
# v_tmp = v + 0.5*dt*dv
# v += dv*dt
# q = se3.integrate(self.model, q, v_tmp*dt)
# # update kinematics
# se3.forwardKinematics(self.model, self.data, q, v, np.zeros(self.model.nv))
# se3.computeJointJacobians(self.model, self.data)
# se3.updateFramePlacements(self.model, self.data)
# ii = 0
# for c in self.contacts:
# J = c.getJacobianWorldFrame()
# self.Jc[ii:ii+3, :] = J
# self.p[i:i+3] = c.p
# self.dp[i:i+3] = c.v
# self.dJv[i:i+3] = c.dJv
# ii += 3
# x0 = np.concatenate((self.p, self.dp))
# # update M and h
# se3.crba(self.model, self.data, q)
# se3.nonLinearEffects(self.model, self.data, q, v)
# M = self.data.M
# h = self.data.nle
# # recompute A and a
# JMinv = np.linalg.solve(M, self.Jc.T).T
# self.Upsilon = [email protected]
# self.a[self.nk:] = JMinv@(self.S.T@u - h) + self.dJv + self.Upsilon@K_p0
# self.A[self.nk:, :self.nk] = [email protected]
# self.A[self.nk:, self.nk:] = [email protected]
# # integrate LDS
# dx = self.A @ x + self.a
# x += t * dx
# f_pred[:, i] = f_pred[:,i-1] + t*self.D @ dx
# DEBUG: predict forces integrating contact point dynamics with Euler
# t = dt_force_pred/ndt_force_pred
# f_pred[:, 0] = K_p0 + self.D @ x0
# x = np.copy(x0)
# for i in range(1,ndt_force_pred):
# dx = self.A @ x + self.a
# x += t * dx
# f_pred[:, i] = f_pred[:,i-1] + t*self.D @ dx
# DEBUG: predict forces assuming constant contact point acceleration (works really bad)
# df_debug = self.D @ (self.A @ x0 + self.a)
# dv = np.linalg.solve(M, self.S.T@u - h + [email protected])
# ddp = self.Jc @ dv + self.dJv
# df = -self.K @ self.dp - self.B @ ddp
# if(norm(df - df_debug)>1e-6):
# print("Error:", norm(df - df_debug))
# f0 = K_p0 + self.D @ x0
# t = 0.0
# for i in range(ndt_force_pred):
# f_pred[:, i] = f0 + t*df
# t += dt_force_pred/ndt_force_pred
# DEBUG: predict forces assuming constant contact point velocity (works reasonably well)
# df = -self.K @ self.dp
# f0 = K_p0 + self.D @ x0
# t = 0.0
# for i in range(ndt_force_pred):
# f_pred[:, i] = f0 + t*df
# t += dt_force_pred/ndt_force_pred
self.v += dt * dv_mean
self.q = se3.integrate(self.model, self.q, v_mean * dt)
self.dv = dv_mean
# compute forces at the end so that user has access to updated forces
self.compute_forces()
self.t += dt
return self.q, self.v, f_pred, f_pred_int
def update(self, solo, qmes12, vmes12):
"""Update the quantities of the Interface based on the last measurements from the simulation
Args:
solo (object): Pinocchio wrapper for the quadruped
qmes12 (19x1 array): the position/orientation of the trunk and angular position of actuators
vmes12 (18x1 array): the linear/angular velocity of the trunk and angular velocity of actuators
"""
# Rotation matrix from the world frame to the base frame
self.oRb = pin.Quaternion(qmes12[3:7]).matrix()
# Linear and angular velocity in base frame
self.vmes12_base = vmes12.copy()
self.vmes12_base[0:3,
0:1] = self.oRb.transpose() @ self.vmes12_base[0:3,
0:1]
self.vmes12_base[3:6,
0:1] = self.oRb.transpose() @ self.vmes12_base[3:6,
0:1]
"""# Update Kinematics (required or automatically done by other functions?)
pin.forwardKinematics(solo.model, solo.data, qmes12, self.vmes12_base)
self.mean_feet_z = solo.data.oMf[self.indexes[0]].translation[2, 0]
for i in self.indexes[1:]:
self.mean_feet_z = np.min((self.mean_feet_z, solo.data.oMf[i].translation[2, 0]))
qmes12[2, 0] -= self.mean_feet_z"""
# Update Kinematics (required or automatically done by other functions?)
pin.forwardKinematics(solo.model, solo.data, qmes12, self.vmes12_base)
pin.framesForwardKinematics(solo.model, solo.data, qmes12)
# Get center of mass from Pinocchio
pin.centerOfMass(solo.model, solo.data, qmes12, self.vmes12_base)
# Update position/orientation of frames
pin.updateFramePlacements(solo.model, solo.data)
pin.ccrba(solo.model, solo.data, qmes12, self.vmes12_base)
# Update minimum height of feet
# TODO: Rename mean_feet_z into min_feet_z
self.mean_feet_z = solo.data.oMf[self.indexes[0]].translation[2, 0]
"""for i in self.indexes:
self.mean_feet_z += solo.data.oMf[i].translation[2, 0]
self.mean_feet_z *= 0.25"""
for i in self.indexes[1:]:
self.mean_feet_z = np.min(
(self.mean_feet_z, solo.data.oMf[i].translation[2, 0]))
self.mean_feet_z = 0.0
# Store position, linear velocity and angular velocity in global frame
self.oC = solo.data.com[0]
self.oV = solo.data.vcom[0]
self.oW = vmes12[3:6]
pin.crba(solo.model, solo.data, qmes12)
# Get SE3 object from world frame to base frame
self.oMb = pin.SE3(pin.Quaternion(qmes12[3:7]), self.oC)
self.RPY = pin.rpy.matrixToRpy(self.oMb.rotation)
# Get SE3 object from world frame to local frame
self.oMl = pin.SE3(
pin.utils.rotate('z', self.RPY[2, 0]),
np.array([qmes12[0, 0], qmes12[1, 0], self.mean_feet_z]))
# Get position, linear velocity and angular velocity in local frame
self.lC = self.oMl.inverse() * self.oC
self.lV = self.oMl.rotation.transpose() @ self.oV
self.lW = self.oMl.rotation.transpose() @ self.oW
# Pos, vel and acc of feet
for i, j in enumerate(self.indexes):
# Position of feet in local frame
self.o_feet[:, i:(i + 1)] = solo.data.oMf[j].translation
self.l_feet[:, i:(
i + 1)] = self.oMl.inverse() * solo.data.oMf[j].translation
# getFrameVelocity output is in the frame of the foot so a transform is required
self.ov_feet[:, i:(
i + 1)] = solo.data.oMf[j].rotation @ pin.getFrameVelocity(
solo.model, solo.data, j).vector[0:3, 0:1]
self.lv_feet[:, i:(
i + 1
)] = self.oMl.rotation.transpose() @ self.ov_feet[:, i:(i + 1)]
# getFrameAcceleration output is in the frame of the foot so a transform is required
self.oa_feet[:, i:(
i + 1)] = solo.data.oMf[j].rotation @ pin.getFrameAcceleration(
solo.model, solo.data, j).vector[0:3, 0:1]
self.la_feet[:, i:(
i + 1
)] = self.oMl.rotation.transpose() @ self.oa_feet[:, i:(i + 1)]
# Orientation of the base in local frame
# Base and local frames have the same yaw orientation in world frame
self.abg[0:2] = self.RPY[0:2]
# Position of shoulders in world frame
for i in range(4):
self.o_shoulders[:, i:(i + 1)] = self.oMl * self.l_shoulders[:, i]
return 0
model.q[0]: valq[0, 0],
model.q[1]: valq[1, 0]
}
import pinocchio as se3
from pinocchio.utils import *
from pendulum import Pendulum
#env = Pendulum(2,length=.1) # Continuous pendulum
env = Pendulum(2, length=.5, mass=3.0, armature=10., withDisplay=False)
print 'Statistic assert of model correctness....'
for i in range(100):
q = rand(2)
vq = rand(2)
bv = se3.rnea(env.model, env.data, q, vq, zero(2))
Mv = se3.crba(env.model, env.data, q)
assert ((b.subs(values(q, vq)) - bv).norm() < 1e-6)
assert ((M.subs(values(q, vq)) - Mv).norm() < 1e-6)
print '\t\t\t\t\t... ok'
b = b.subs({c: p / 2})
b.simplify()
Mi = (M + A).inv()
Mi = Mi.subs({c: p / 2})
Mi.simplify()
_, denom = fraction(Mi[0, 0])
denom.simplify()
Mi = denom * Mi
Mi.simplify()
def mass(self, q, update_kinematics=True):
if (update_kinematics):
return se3.crba(self.model, self.data, q)
return self.data.M
def update_quantities(robot: jiminy.Model,
position: np.ndarray,
velocity: Optional[np.ndarray] = None,
acceleration: Optional[np.ndarray] = None,
update_physics: bool = True,
update_com: bool = True,
update_energy: bool = True,
update_jacobian: bool = False,
update_collisions: bool = True,
use_theoretical_model: bool = True) -> None:
"""Compute all quantities using position, velocity and acceleration
configurations.
Run multiple algorithms to compute all quantities which can be known with
the model position, velocity and acceleration.
This includes:
- body spatial transforms,
- body spatial velocities,
- body spatial drifts,
- body transform acceleration,
- body transform jacobians,
- center-of-mass position,
- center-of-mass velocity,
- center-of-mass drift,
- center-of-mass acceleration,
- center-of-mass jacobian,
- articular inertia matrix,
- non-linear effects (Coriolis + gravity)
- collisions and distances
.. note::
Computation results are stored internally in the robot, and can
be retrieved with associated getters.
.. warning::
This function modifies the internal robot data.
.. warning::
It does not called overloaded pinocchio methods provided by
`jiminy_py.core` but the original pinocchio methods instead. As a
result, it does not take into account the rotor inertias / armatures.
One is responsible of calling the appropriate methods manually instead
of this one if necessary. This behavior is expected to change in the
future.
:param robot: Jiminy robot.
:param position: Robot position vector.
:param velocity: Robot velocity vector.
:param acceleration: Robot acceleration vector.
:param update_physics: Whether or not to compute the non-linear effects and
internal/external forces.
Optional: True by default.
:param update_com: Whether or not to compute the COM of the robot AND each
link individually. The global COM is the first index.
Optional: False by default.
:param update_energy: Whether or not to compute the energy of the robot.
Optional: False by default
:param update_jacobian: Whether or not to compute the jacobians.
Optional: False by default.
:param use_theoretical_model: Whether the state corresponds to the
theoretical model when updating and fetching
the robot's state.
Optional: True by default.
"""
if use_theoretical_model:
pnc_model = robot.pinocchio_model_th
pnc_data = robot.pinocchio_data_th
else:
pnc_model = robot.pinocchio_model
pnc_data = robot.pinocchio_data
if (update_physics and update_com and
update_energy and update_jacobian and
velocity is not None and acceleration is None):
pin.computeAllTerms(pnc_model, pnc_data, position, velocity)
else:
if velocity is None:
pin.forwardKinematics(pnc_model, pnc_data, position)
elif acceleration is None:
pin.forwardKinematics(pnc_model, pnc_data, position, velocity)
else:
pin.forwardKinematics(
pnc_model, pnc_data, position, velocity, acceleration)
if update_com:
if velocity is None:
pin.centerOfMass(pnc_model, pnc_data, position)
elif acceleration is None:
pin.centerOfMass(pnc_model, pnc_data, position, velocity)
else:
pin.centerOfMass(
pnc_model, pnc_data, position, velocity, acceleration)
if update_jacobian:
if update_com:
pin.jacobianCenterOfMass(pnc_model, pnc_data)
pin.computeJointJacobians(pnc_model, pnc_data)
if update_physics:
if velocity is not None:
pin.nonLinearEffects(pnc_model, pnc_data, position, velocity)
pin.crba(pnc_model, pnc_data, position)
if update_energy:
if velocity is not None:
pin.computeKineticEnergy(pnc_model, pnc_data)
pin.computePotentialEnergy(pnc_model, pnc_data)
pin.updateFramePlacements(pnc_model, pnc_data)
if update_collisions:
pin.updateGeometryPlacements(
pnc_model, pnc_data, robot.collision_model, robot.collision_data)
pin.computeCollisions(
robot.collision_model, robot.collision_data,
stop_at_first_collision=False)
pin.computeDistances(robot.collision_model, robot.collision_data)
for dist_req in robot.collision_data.distanceResults:
if np.linalg.norm(dist_req.normal) < 1e-6:
pin.computeDistances(
robot.collision_model, robot.collision_data)
break
if READ_FROM_TXT:
rmodel_dist.loadFromText(TXT_FILE)
else:
for iner in rmodel_dist.inertias:
iner.lever += SCALE * (np.random.rand(3) - 0.5)
rdata_dist = rmodel_dist.createData()
# CoM
c = pin.centerOfMass(rmodel, rdata, r.q0)
c_dist = pin.centerOfMass(rmodel_dist, rdata_dist, r.q0)
print('c_dist - c')
print(c_dist - c)
# Mass matrix
M = pin.crba(rmodel, rdata, r.q0)
M_dist = pin.crba(rmodel_dist, rdata_dist, r.q0)
print('M_dist - M')
print(M_dist[:6, :6] - M[:6, :6])
# save the result as a txt file:
rmodel_dist.saveToText(TXT_FILE)
if PLOT_DISPARITY:
# funny plots
# number of configuration to sample
N = 5000
biases = np.zeros((N, 3))
for i in range(N):
def update_quantities(jiminy_model,
position,
velocity=None,
acceleration=None,
update_physics=True,
update_com=False,
update_energy=False,
update_jacobian=False,
use_theoretical_model=True):
"""
@brief Compute all quantities using position, velocity and acceleration
configurations.
@details Run multiple algorithms to compute all quantities
which can be known with the model position, velocity
and acceleration configuration.
This includes:
- body spatial transforms,
- body spatial velocities,
- body spatial drifts,
- body transform acceleration,
- body transform jacobians,
- center-of-mass position,
- center-of-mass velocity,
- center-of-mass drift,
- center-of-mass acceleration,
(- center-of-mass jacobian : No Python binding available so far),
- articular inertia matrix,
- non-linear effects (Coriolis + gravity)
Computation results are stored in internal
data and can be retrieved with associated getters.
@note This function modifies internal data.
@param jiminy_model The Jiminy model
@param position Joint position vector
@param velocity Joint velocity vector
@param acceleration Joint acceleration vector
@pre model_.nq == positionIn.size()
@pre model_.nv == velocityIn.size()
@pre model_.nv == accelerationIn.size()
"""
if use_theoretical_model:
pnc_model = jiminy_model.pinocchio_model_th
pnc_data = jiminy_model.pinocchio_data_th
else:
pnc_model = jiminy_model.pinocchio_model
pnc_data = jiminy_model.pinocchio_data
if (update_physics and update_com and \
update_energy and update_jacobian and \
velocity is not None):
pnc.computeAllTerms(pnc_model, pnc_data, position, velocity)
else:
if update_physics:
if velocity is not None:
pnc.nonLinearEffects(pnc_model, pnc_data, position, velocity)
pnc.crba(pnc_model, pnc_data, position)
if update_jacobian:
# if update_com:
# pnc.getJacobianComFromCrba(pnc_model, pnc_data)
pnc.computeJointJacobians(pnc_model, pnc_data)
if update_com:
if velocity is None:
pnc.centerOfMass(pnc_model, pnc_data, position)
elif acceleration is None:
pnc.centerOfMass(pnc_model, pnc_data, position, velocity)
else:
pnc.centerOfMass(pnc_model, pnc_data, position, velocity,
acceleration)
else:
if velocity is None:
pnc.forwardKinematics(pnc_model, pnc_data, position)
elif acceleration is None:
pnc.forwardKinematics(pnc_model, pnc_data, position, velocity)
else:
pnc.forwardKinematics(pnc_model, pnc_data, position, velocity,
acceleration)
pnc.framesForwardKinematics(pnc_model, pnc_data, position)
if update_energy:
if velocity is not None:
pnc.kineticEnergy(pnc_model, pnc_data, position, velocity,
False)
pnc.potentialEnergy(pnc_model, pnc_data, position, False)
np.set_printoptions(threshold=np.inf) # Loading a robot model model = example_robot_data.loadHyQ().model data = model.createData() #start configuration v = np.array([0.0 , 0.0 , 0.0, 0.0, 0.0, 0.0, #underactuated 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) #actuated q = example_robot_data.loadHyQ().q0 # Update the joint and frame placements pin.forwardKinematics(model,data,q,v) pin.updateFramePlacements(model,data) M = pin.crba(model, data, q) H = pin.nonLinearEffects(model, data, q, v) G = pin.computeGeneralizedGravity(model,data, q) #EXERCISE 1: Compute the com of the robot (in WF) #..... # compare the result by using native pinocchio function com_test = pin.centerOfMass(model, data, q, v) print "Com Position (pinocchio): ", com_test # EXERCIZE 2: Compute robot kinetic energy #get a random generalized velocity v = rand(model.nv) # Update the joint and frame placements pin.forwardKinematics(model,data,q,v)
def Mdv(q, vnext, v):
M = pinocchio.crba(rmodel, rdata, q)
return M * (vnext - v)
# --- dCRBA ---
dcrba = DCRBA(robot)
dcrba.pre(q)
Mp = dcrba()
# --- Validate dM/dq by finite diff
dM = np.zeros([
robot.model.nv,
] * 3)
eps = 1e-6
dq = zero(robot.model.nv)
for diff in range(robot.model.nv):
dM[:, :, diff] = -pin.crba(robot.model, robot.data, q)
dq *= 0
dq[diff] = eps
qdq = pin.integrate(robot.model, q, dq)
dM[:, :, diff] += pin.crba(robot.model, robot.data, qdq)
dM /= eps
print(
'Check dCRBA = \t\t\t',
max([
norm(Mp[:, :, diff] - dM[:, :, diff])
for diff in range(robot.model.nv)
]))