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 get_inertia_matrix(self):
"""
Computes the joint space inertia matrix.
Returns:
np.array[float[N,N]]: joint space inertia matrix (where `N` is the number of DoFs).
"""
H = np.zeros(self.num_dofs, self.num_dofs)
rbdl.CompositeRigidBodyAlgorithm(self.model, self._q, H, update_kinematics=False)
return H
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 get_M(q_):
q_ = np.asarray(q_)
q = np.zeros(7)
M = np.zeros([7, 7])
q[0] = q_[0]
q[1] = q_[1]
q[2] = q_[2]
q[3] = q_[3]
q[4] = q_[4]
q[5] = q_[5]
q[6] = q_[6]
rbdl.CompositeRigidBodyAlgorithm(model, q, M, True)
return M
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 get_M(
q_
): # This function computes the joint space inertia matrix from a given model and the generalized state vector
q_ = np.asarray(q_)
q = np.zeros(7)
M = np.zeros([7, 7])
q[0] = q_[0]
q[1] = q_[1]
q[2] = q_[2]
q[3] = q_[3]
q[4] = q_[4]
q[5] = q_[5]
q[6] = q_[6]
rbdl.CompositeRigidBodyAlgorithm(model, q, M, True)
return M
#!/usr/bin/python
#
# rbdl mass matrix
import rbdl
import numpy as np
import csv
np.set_printoptions(precision=6, suppress=True)
mdl = rbdl.loadModel(
b'/home/shjliu/workspace/EPR/bullet3-new/20200214/bullet3/examples/pybullet/gym/pybullet_data/franka_panda/panda.urdf'
)
mass_matrix = np.zeros((9, 9), dtype=np.double)
#neutral_joint_angles= [0., -0., 0., -0, 0., 0, 0.]
neutral_joint_angles = [0., -0.3135, 0., -2.515, 0., 2.226, 0.87]
rbdl.CompositeRigidBodyAlgorithm(
mdl, np.asarray(neutral_joint_angles, dtype=np.double), mass_matrix)
print("mass_matrix=")
print(mass_matrix)
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
fqact.write(
str(t) + ',' + str(q[0]) + ',' + str(q[1]) + ',' + str(q[2]) + ',' +
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)
def update_inertia_matrix(self, M, q=None, update_kinematics=True):
if q is None:
q = self.q
rbdl.CompositeRigidBodyAlgorithm(self.model, q, M, update_kinematics)
def get_inertia_matrix(self, q=None, update_kinematics=True):
if q is None:
q = self.q
M = np.zeros((self.qdot_size, self.qdot_size))
rbdl.CompositeRigidBodyAlgorithm(self.model, q, M, update_kinematics)
return M
def u2c2np(asd):
return cs.Function("temp",[],[asd])()["o0"].toarray()
def list2np(asd):
return np.asarray(asd)
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]
rbdl.CompositeRigidBodyAlgorithm(snake_rbdl, q_np, M_rbdl)
kdl.ChainDynParam(snake_chain, gravity_kdl).JntToMass(q_kdl, M_kdl)
M_pb = pb.calculateMassMatrix(snake_pb, q)
M_u2c = M_sym(q)
for row_idx in range(n_joints):
for col_idx in range(n_joints):
error_kdl_u2c[row_idx][col_idx] += np.absolute(M_kdl[row_idx,col_idx] - u2c2np(M_u2c[row_idx, col_idx]))
error_rbdl_u2c[row_idx][col_idx] += np.absolute((M_rbdl[row_idx,col_idx]) - u2c2np(M_u2c[row_idx, col_idx]))
error_pb_u2c[row_idx][col_idx] += np.absolute(list2np(M_pb[row_idx][col_idx]) - u2c2np(M_u2c[row_idx, col_idx]))
error_kdl_rbdl[row_idx][col_idx] += np.absolute((M_rbdl[row_idx,col_idx]) - list2np(M_kdl[row_idx, col_idx]))
error_pb_kdl[row_idx][col_idx] += np.absolute(list2np(M_pb[row_idx][col_idx]) - list2np(M_kdl[row_idx, col_idx]))
error_pb_rbdl[row_idx][col_idx] += np.absolute((M_rbdl[row_idx,col_idx]) - list2np(M_pb[row_idx][col_idx]))
return cs.Function("temp", [], [asd])()["o0"].toarray()
def list2np(asd):
return np.asarray(asd)
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]
rbdl.CompositeRigidBodyAlgorithm(gantry_rbdl, q_np, M_rbdl)
kdl.ChainDynParam(gantry_chain, gravity_kdl).JntToMass(q_kdl, M_kdl)
M_pb = pb.calculateMassMatrix(gantry_pb, q)
M_u2c = M_sym(q)
for row_idx in range(n_joints):
for col_idx in range(n_joints):
error_kdl_u2c[row_idx][col_idx] += np.absolute(
M_kdl[row_idx, col_idx] - u2c2np(M_u2c[row_idx, col_idx]))
error_rbdl_u2c[row_idx][col_idx] += np.absolute(
(M_rbdl[row_idx, col_idx]) - u2c2np(M_u2c[row_idx, col_idx]))
error_pb_u2c[row_idx][col_idx] += np.absolute(
list2np(M_pb[row_idx][col_idx]) -
u2c2np(M_u2c[row_idx, col_idx]))
error_kdl_rbdl[row_idx][col_idx] += np.absolute(
(M_rbdl[row_idx, col_idx]) - list2np(M_kdl[row_idx, col_idx]))
return cs.Function("temp", [], [asd])()["o0"].toarray()
def list2np(asd):
return np.asarray(asd)
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]
rbdl.CompositeRigidBodyAlgorithm(ur5_rbdl, q_np, M_rbdl)
kdl.ChainDynParam(ur5_chain, gravity_kdl).JntToMass(q_kdl, M_kdl)
M_pb = pb.calculateMassMatrix(ur5_pb, q)
M_u2c = M_sym(q)
for row_idx in range(n_joints):
for col_idx in range(n_joints):
error_kdl_u2c[row_idx][col_idx] += np.absolute(
M_kdl[row_idx, col_idx] - u2c2np(M_u2c[row_idx, col_idx]))
error_rbdl_u2c[row_idx][col_idx] += np.absolute(
(M_rbdl[row_idx, col_idx]) - u2c2np(M_u2c[row_idx, col_idx]))
error_pb_u2c[row_idx][col_idx] += np.absolute(
list2np(M_pb[row_idx][col_idx]) -
u2c2np(M_u2c[row_idx, col_idx]))
error_kdl_rbdl[row_idx][col_idx] += np.absolute(
(M_rbdl[row_idx, col_idx]) - list2np(M_kdl[row_idx, col_idx]))
