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 test_generalized_gravity(self):
model = self.model
tau = pin.computeGeneralizedGravity(model, self.data, self.q)
data2 = model.createData()
tau_ref = pin.rnea(model, data2, self.q, self.v * 0, self.a * 0)
self.assertApprox(tau, tau_ref)
def get_gravity(self):
return np.copy(
pin.computeGeneralizedGravity(self._model, self._data, self._q))
#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")
###
def calc(self, q):
g = pin.computeGeneralizedGravity(self.rmodel, self.rdata, q)
taugrav = -pin.aba(self.rmodel, self.rdata, q, self.v0, self.v0)
return np.dot(taugrav, g)
def residual(self, q):
return pin.computeGeneralizedGravity(self.rmodel, self.rdata, q)
# 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) # compute using generalized velocities and system mass matrix # .....
def residual(self, q):
return pin.computeGeneralizedGravity(robot.model, robot.data, q)
ftot_pyb += forces_pyb[ct_id]['f']
# print('o_f_tot: ', o_f_tot)
# mg = robot.data.mass[0] * robot.model.gravity.linear
# print('m*g: ', mg)
# print('mg+o_f_tot: ', mg+o_f_tot)
# print('ftot_pyb')
# print(ftot_pyb)
################
################
# dynamic equation
Mq = pin.crba(model, data, q)
Cqdq = pin.computeCoriolisMatrix(model, data, q, v)
gq = pin.computeGeneralizedGravity(model, data, q)
q_static = q.copy()
q_static[:7] = np.array([0] * 6 + [1])
gq_static = pin.computeGeneralizedGravity(model, data, q_static)
# Compare gravity in world frame with total forces
print('ftot_pyb - gq_static: ', ftot_pyb - gq_static[:3]) # OK
print('o_f_tot - gq_static: ', o_f_tot - gq_static[:3]) # NOTOK
Mq_a = Mq[6:, :]
Cqdq_a = Cqdq[6:, :]
gq_a = gq[6:]
# another test on the dynamics equation (without free flyer 6 first rows)
assert (np.allclose(Mq_a @ dv + Cqdq_a @ v + gq_a,