def iden_model(model, data, N, nq, nv, q, v, a): """This function calculates joint torques and generates the joint torque regressor. Note: a flag IsFrictioncld to include in dynamic model Input: model, data: model and data structure of robot from Pinocchio q, v, a: joint's position, velocity, acceleration N : number of samples nq: length of q Output: tau: vector of joint torque W : joint torque regressor""" tau = np.empty(nq*N) W = np.empty([N*nq, 10*nq]) for i in range(N): tau_temp = pin.rnea(model, data, q[i,:], v[i,:], a[i,:]) W_temp = pin.computeJointTorqueRegressor(model, data, q[i,:], v[i,:], a[i,:]) for j in range(nq): tau[j*N + i] = tau_temp[j] W[j*N + i, :] = W_temp[j,:] if isFrictionincld: W = np.c_[W,np.zeros([N*nq,2*nq])] for i in range(N): for j in range(nq): tau[j*N + i] = tau[j*N + i] + v[i,j]*fv + np.sign(v[i,j])*fc W[j*N + i, 10*nq+2*j] = v[i,j] W[j*N + i, 10*nq+2*j + 1] = np.sign(v[i,j]) return tau, W
def test_joint_torque_regressor(self):
model = pin.buildSampleModelHumanoidRandom()
model.lowerPositionLimit[:7] = -1.
model.upperPositionLimit[:7] = 1.
data = model.createData()
data_ref = model.createData()
q = pin.randomConfiguration(model)
v = pin.utils.rand(model.nv)
a = pin.utils.rand(model.nv)
pin.rnea(model,data_ref,q,v,a)
params = zero(10*(model.njoints-1))
for i in range(1, model.njoints):
params[(i-1)*10:i*10] = model.inertias[i].toDynamicParameters()
pin.computeJointTorqueRegressor(model,data,q,v,a)
tau_regressor = data.jointTorqueRegressor.dot(params)
self.assertApprox(tau_regressor, data_ref.tau)
def compute_regressor_matrix(self, angle, velocity, acceleration):
joint_torque_regressor = \
pinocchio.computeJointTorqueRegressor(self.model, self.data,
to_matrix(angle),
to_matrix(velocity),
to_matrix(acceleration))
viscous_friction_torque_regressor = to_diagonal_matrix(velocity)
static_friction_torque_regressor = to_diagonal_matrix(
np.sign(velocity))
regressor_matrix = np.concatenate([
joint_torque_regressor, viscous_friction_torque_regressor,
static_friction_torque_regressor
],
axis=1)
return regressor_matrix
def iden_model_fext(model, data, N, nq, nv, njoints, q, v, a):
"""This function calculates joint torques and generates the joint torque regressor.
Note: a flag IsFrictioncld to include in dynamic model
Input: model, data: model and data structure of robot from Pinocchio
q, v, a: joint's position, velocity, acceleration
N : number of samples
nq: length of q
Output: tau: vector of joint torque
W : joint torque regressor"""
tau = np.empty(6 * N)
W = np.empty([N * 6, 10 * (njoints - 1)])
for i in range(N):
tau_temp = pin.rnea(model, data, q[i, :], v[i, :], a[i, :])
W_temp = pin.computeJointTorqueRegressor(model, data, q[i, :], v[i, :],
a[i, :])
for j in range(6):
tau[j * N + i] = tau_temp[j]
W[j * N + i, :] = W_temp[j, :]
return tau, W
