def get_nonlinear_forces(self, q=None, qdot=None):
if q is None:
q = self.q
if qdot is None:
qdot = self.qdot
c_plus_g = np.zeros(self.qdot_size)
rbdl.NonlinearEffects(self.model, q, qdot, c_plus_g)
return g
def send_command(self, tau):
tau = np.squeeze(np.asarray(tau))
rbdl.CompositeRigidBodyAlgorithm(self.robot, self.q, self.M)
rbdl.NonlinearEffects(self.robot, self.q, self.dq, self.b)
self.ddq = np.linalg.inv(self.M).dot(tau-self.b)
self.dq = self.dq + self.dt*self.ddq
self.q = self.q + self.dt*self.dq
def compute_gravity_compensation(self):
"""
Return the torques to perform gravity compensation.
Returns:
np.array[float[N]]: torques to perform gravity compensation.
"""
tau = np.zeros(self.num_dofs)
rbdl.NonlinearEffects(self.model, self._q, self.zeros, tau)
return tau
def send_command(self, tau):
rbdl.CompositeRigidBodyAlgorithm(self.robot, self.q, self.M)
rbdl.NonlinearEffects(self.robot, self.q, self.dq, self.b)
try:
ddq = np.linalg.inv(self.M).dot(tau - self.b)
except np.linalg.LinAlgError:
ddq = self.M.T.dot(
np.linalg.inv(
np.dot(self.M, self.M.T) +
0.9**2 * np.eye(self.q.size))).dot(tau - self.b)
self.q = self.q + self.dt * self.dq
self.dq = self.dq + self.dt * ddq
def test_NonlinearEffectsConsistency(self):
""" Checks whether NonlinearEffects is consistent with InverseDynamics """
q = np.random.rand(self.model.q_size)
qdot = np.random.rand(self.model.q_size)
nle_id = np.random.rand(self.model.q_size)
rbdl.InverseDynamics(self.model, q, qdot,
np.zeros(self.model.qdot_size), nle_id)
nle = np.zeros((self.model.q_size))
rbdl.NonlinearEffects(self.model, q, qdot, nle)
assert_almost_equal(nle_id, nle)
def test_CompositeRigidBodyAlgorithm_NonlinearEffects(self):
self.q = np.random.rand(self.model.q_size)
self.qdot = np.random.rand(self.model.q_size)
self.qddot = np.random.rand(self.model.q_size)
tau_nonlin = np.zeros(self.model.q_size)
H = np.zeros((self.model.q_size, self.model.q_size))
rbdl.InverseDynamics(self.model, self.q, self.qdot, self.qddot,
self.tau)
rbdl.CompositeRigidBodyAlgorithm(self.model, self.q, H)
rbdl.NonlinearEffects(self.model, self.q, self.qdot, tau_nonlin)
tau_comp = H.dot(self.qddot) + tau_nonlin
assert_almost_equal(tau_comp, self.tau)
def compute_nonlinear_term(self):
r"""
Computes the non-linear terms :math:`N(q, \dot{q})` in the dynamic equation of motion for a rigid-body system,
given by:
.. math:: \tau = H(q) \ddot{q} + N(q, \dot{q})
where ":math:`\tau` is the vector of applied forces, :math:`H` is the joint space inertia matrix,
:math:`N(q, \dot{q})` is the vector of force terms that account for the Coriolis and centrifugal forces,
gravity, and any other forces acting on the system other than those in :math:`\tau`." [1]
Returns:
np.array[float[N]]: non-linear force terms.
References:
- [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008
"""
tau = np.zeros(self.num_dofs)
rbdl.NonlinearEffects(self.model, self._q, self._dq, tau)
return tau
n_itr = 1000
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
q_kdl[j] = q[j]
q_np[j] = q[j]
qdot[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
qdot_kdl[j] = qdot[j]
qdot_np[j] = qdot[j]
zeros_pb[j] = 0.
rbdl.NonlinearEffects(gantry_rbdl, q_np, qdot_np, C_rbdl)
kdl.ChainDynParam(gantry_kdl, gravity_kdl).JntToGravity(q_kdl, g_kdl)
kdl.ChainDynParam(gantry_kdl, gravity_kdl).JntToCoriolis(q_kdl, qdot_kdl, C_kdl)
pb.setGravity(0, 0, 0)
C_pb = pb.calculateInverseDynamics(gantry_pb, q, qdot, zeros_pb)
pb.setGravity(0, 0, -9.81)
g_pb = pb.calculateInverseDynamics(gantry_pb, q, zeros_pb, zeros_pb)
g_u2c = g_sym(q)
C_u2c = C_sym(q, qdot)
for tau_idx in range(n_joints):
error_kdl_rbdl[tau_idx] += np.absolute(((list2np(g_kdl[tau_idx]) + list2np(C_kdl[tau_idx])) - C_rbdl[tau_idx]))
error_kdl_u2c[tau_idx] += np.absolute((list2np(C_kdl[tau_idx]) - u2c2np(C_u2c[tau_idx])))
error_rbdl_u2c[tau_idx] += np.absolute(((u2c2np(g_u2c[tau_idx]) + u2c2np(C_u2c)[tau_idx]) - C_rbdl[tau_idx]))
n_itr = 1000
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
q_kdl[j] = q[j]
q_np[j] = q[j]
qdot[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
qdot_kdl[j] = qdot[j]
qdot_np[j] = qdot[j]
zeros_pb[j] = 0.
rbdl.NonlinearEffects(snake_rbdl, q_np, qdot_np, C_rbdl)
kdl.ChainDynParam(snake_chain, gravity_kdl).JntToGravity(q_kdl, g_kdl)
kdl.ChainDynParam(snake_chain,
gravity_kdl).JntToCoriolis(q_kdl, qdot_kdl, C_kdl)
pb.setGravity(0, 0, 0)
C_pb = pb.calculateInverseDynamics(snake_pb, q, qdot, zeros_pb)
pb.setGravity(0, 0, -9.81)
g_pb = pb.calculateInverseDynamics(snake_pb, q, zeros_pb, zeros_pb)
g_u2c = g_sym(q)
C_u2c = C_sym(q, qdot)
for tau_idx in range(n_joints):
error_kdl_rbdl[tau_idx] += np.absolute(
((list2np(g_kdl[tau_idx]) + list2np(C_kdl[tau_idx])) -
C_rbdl[tau_idx]))
while not rospy.is_shutdown():
# Leer valores del simulador
q = legRobot.read_joint_positions()
dq = legRobot.read_joint_velocities()
# Posicion actual del efector final
x = fkine(q)[0:3,3]
# Tiempo actual
jstate.header.stamp = rospy.Time.now()
# ____________________________________________________________
# Control dinamico
# ____________________________________________________________
rbdl.InverseDynamics(leg_model, q, zeros, zeros, g)
rbdl.CompositeRigidBodyAlgorithm(leg_model,q,M)
rbdl.NonlinearEffects(leg_model,q,dq,c)
u = M.dot( ddqdes + Kd.dot(dqdes - dq) + Kp.dot(qdes-q) ) + c
if np.linalg.norm(qdes-q)< 0.01:
break
# Simulacion del robot
legRobot.send_command(u)
# Log values
fxcurrent.write(str(x[0])+' '+str(x[1]) +' '+str(x[2])+'\n')
fxdesired.write(str(xdes[0])+' '+str(xdes[1])+' '+str(xdes[2])+'\n')
fq.write(str(q[0])+" "+str(q[1])+" "+str(q[2])+" "+str(q[3])+" "+ str(q[4])+" "+str(q[5])+"\n")
# Publicacion del mensaje
jstate.position = q
str(q[3]) + ',' + str(q[4]) + ',' + str(q[5]) + ',' + str(q[6]) +
'\n ')
fqdes.write(
str(t) + ',' + str(qdes[0]) + ',' + str(qdes[1]) + ',' + str(qdes[2]) +
',' + str(qdes[3]) + ',' + str(qdes[4]) + ',' + str(qdes[5]) + ',' +
str(qdes[6]) + '\n ')
# ----------------------------
# Control dinamico (COMPLETAR)
# ----------------------------
M = np.zeros((ndof, ndof))
b = np.zeros((ndof))
rbdl.CompositeRigidBodyAlgorithm(modelo, q, M)
#print(M.shape)
rbdl.NonlinearEffects(modelo, q, dq, b)
Kp = np.diag([15.5, 10., 5.0, 3.5, 2.9, 2.8, 2.4])
Kd = np.diag([7., 6., 3., 2.4, 2.6, 2.3, 2.3])
y = ddqdes - ddq + Kd.dot(dqdes - dq) + Kp.dot(qdes - q)
u = M.dot(y) + b
print(M)
# Simulacion del robot
robot.send_command(u)
# Publicacion del mensaje
jstate.position = q
pub.publish(jstate)
bmarker_deseado.xyz(xdes)
bmarker_actual.xyz(x)
def update_gravity_forces(self, g, q=None):
if q is None:
q = self.q
rbdl.NonlinearEffects(self.model, q, self.qdot * 0, g)
def get_gravity_forces(self, q=None):
if q is None:
q = self.q
g = np.zeros(self.qdot_size)
rbdl.NonlinearEffects(self.model, q, self.qdot * 0, g)
return g
def update_nonlinear_forces(self, c_plus_g, q=None, qdot=None):
if q is None:
q = self.q
if qdot is None:
qdot = self.qdot
rbdl.NonlinearEffects(self.model, q, qdot, c_plus_g)
n_itr = 1000
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
q_kdl[j] = q[j]
q_np[j] = q[j]
qdot[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
qdot_kdl[j] = qdot[j]
qdot_np[j] = qdot[j]
zeros_pb[j] = 0.
rbdl.NonlinearEffects(ur5_rbdl, q_np, qdot_np, C_rbdl)
kdl.ChainDynParam(ur5_kdl, gravity_kdl).JntToGravity(q_kdl, g_kdl)
kdl.ChainDynParam(ur5_kdl, gravity_kdl).JntToCoriolis(q_kdl, qdot_kdl, C_kdl)
pb.setGravity(0, 0, 0)
C_pb = pb.calculateInverseDynamics(ur5_pb, q, qdot, zeros_pb)
pb.setGravity(0, 0, -9.81)
g_pb = pb.calculateInverseDynamics(ur5_pb, q, zeros_pb, zeros_pb)
g_u2c = g_sym(q)
C_u2c = C_sym(q, qdot)
for tau_idx in range(n_joints):
error_kdl_rbdl[tau_idx] += np.absolute(((list2np(g_kdl[tau_idx]) + list2np(C_kdl[tau_idx])) - C_rbdl[tau_idx]))
error_kdl_u2c[tau_idx] += np.absolute((list2np(C_kdl[tau_idx]) - u2c2np(C_u2c[tau_idx])))
error_rbdl_u2c[tau_idx] += np.absolute(((u2c2np(g_u2c[tau_idx]) + u2c2np(C_u2c)[tau_idx]) - C_rbdl[tau_idx]))
# Posicion actual del efector final
x = fkine(q)[0:3, 3]
# Tiempo actual (necesario como indicador para ROS)
jstate.header.stamp = rospy.Time.now()
# Almacenamiento de datos
# ----------------------------
# Control dinamico (COMPLETAR)
# ----------------------------
#VECTOR MASA INERCIAL
rbdl.CompositeRigidBodyAlgorithm(modelo, q, M)
#VECTOR GRAVEDAD
rbdl.NonlinearEffects(modelo, q, zeros, g)
#VECTOR CORIOLISIS
rbdl.InverseDynamics(modelo, q, dq, zeros, c)
c = c - g
#VECTOR f, CONCATENAMOS VECTOR q Y dq
f = np.hstack([q, dq])
#VALOR DE y
y = (ddqdes - ddq) + Kd.dot(dqdes - f[6:12]) + Kp.dot(qdes - f[0:6])
#LEY DE CONTROL
u = M.dot(y) + c + g
#SE ASIGNA NUEVO VECTOR ddq
