def dyn_value(self, t, q, v):
#(hg_ref, vhg_ref, ahg_ref) = self._ref_traj(t)
vhg_ref = np.matrix([0., 0., 0., 0., 0., 0.]).T
model = self.robot.model
data = self.robot.data
JMom = se3.ccrba(model, data, q, v)
hg_prv = data.hg.vector.copy()[self._mask,:]
#self.p_error = data.hg.vector.copy()[self._mask,:] - vhg_ref[self._mask,:]
#self.v_error = self.robot.fext[self._mask,:] - vhg_ref[self._mask,:]
#self.v_error = self.robot.fext[self._mask,:]
#self.v_error = data.hg.vector.copy()[self._mask,:] - vhg_ref[self._mask,:]
#self.a_des = -self.kv*self.v_error #+model.gravity.vector[self._mask,:]
self.a_des = self.kv*self.robot.fext[self._mask,:]#vhg_ref #+model.gravity.vector[self._mask,:]
#self.drift = 0 * self.a_des
#self.a_des =
#***********************
p_com = data.com[0]
cXi = SE3.Identity()
oXi = self.robot.data.oMi[1]
cXi.rotation = oXi.rotation
cXi.translation = oXi.translation - p_com
m_gravity = model.gravity.copy()
model.gravity.setZero()
b = se3.nle(model,data,q,v)
model.gravity = m_gravity
f_ff = se3.Force(b[:6])
f_com = cXi.act(f_ff)
hg_drift = f_com.angular
self.drift=f_com.np[self._mask,:]
#************************
#print JMom.copy()[self._mask,:].shape
#print self.__gain_matrix.shape
self._jacobian = JMom.copy()[self._mask,:] * self.__gain_matrix
return self._jacobian, self.drift, self.a_des
def _getBiais(self,q,v):
model = self.robot.model
data = self.robot.data
p_com = self.robot.com(q)
oXi = data.oMi[1]
cXi = se3.SE3.Identity()
cXi.rotation = oXi.rotation
cXi.translation = oXi.translation - p_com
#cXi.translation = p_com
m_gravity = model.gravity.copy()
model.gravity.setZero()
b = se3.nle(model,data,q,v)
model.gravity = m_gravity
f_ff = se3.Force(b[:6])
f_com = cXi.act(f_ff)
return f_com.np
def bias(self, q, v, update_kinematics=True):
if (update_kinematics):
return se3.nle(self.model, self.data, q, v)
return self.data.nle
def biais(self, q, v):
''' the coriolis, centrifugal and gravitational effects '''
return se3.nle(self.model, self.data, q, v)
'Check dCRBA = \t\t\t',
max([
norm(Mp[:, :, diff] - dM[:, :, diff])
for diff in range(robot.model.nv)
]))
# --- vRNEA ---
# --- vRNEA ---
# --- vRNEA ---
vrnea = VRNEA(robot)
Q = vrnea(q)
# --- Compute C from rnea, for future check
robot.model.gravity = se3.Motion.Zero()
rnea0 = lambda q, vq: se3.nle(robot.model, robot.data, q, vq)
vq1 = vq * 0
C = np.zeros([
robot.model.nv,
] * 3)
for i in range(robot.model.nv):
vq1 *= 0
vq1[i] = 1
C[:, i, i] = rnea0(q, vq1).T
for i in range(robot.model.nv):
for j in range(robot.model.nv):
if i == j: continue
vq1 *= 0
vq1[i] = vq1[j] = 1.0
C[:, i, j] = (rnea0(q, vq1).T - C[:, i, i] - C[:, j, j]) / 2
dM /= eps
print('Check dCRBA = \t\t\t', max([ norm(Mp[:,:,diff]-dM[:,:,diff]) for diff in range(robot.model.nv) ]))
# --- vRNEA ---
# --- vRNEA ---
# --- vRNEA ---
vrnea = VRNEA(robot)
Q = vrnea(q)
# --- Compute C from rnea, for future check
robot.model.gravity = pin.Motion.Zero()
rnea0 = lambda q,vq: pin.nle(robot.model,robot.data,q,vq)
vq1 = vq*0
C = np.zeros([robot.model.nv,]*3)
for i in range(robot.model.nv):
vq1 *= 0; vq1[i] = 1
C[:,i,i] = rnea0(q,vq1).T
for i in range(robot.model.nv):
for j in range(robot.model.nv):
if i==j: continue
vq1 *= 0
vq1[i] = vq1[j] = 1.0
C[:,i,j] = (rnea0(q,vq1).T-C[:,i,i]-C[:,j,j]) /2
print("Check d/dv rnea = \t\t",norm(quad(Q,vq)-rnea0(q,vq)))
print("Check C = Q+Q.T = \t\t", norm((Q+Q.transpose(0,2,1))/2-C))
