def calculate_dynamics(self, qdd):
tau = np.asarray([0.0] * self._joint_num)
rbdl.InverseDynamics(self.rbdl_model, self.q[0:2], self.qd[0:2],
qdd[0:2], tau)
return tau
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
qdot[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
qddot[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
q_np[j] = q[j]
qdot_np[j] = qdot[j]
qddot_np[j] = qddot[j]
rbdl.InverseDynamics(gantry_rbdl, q_np, qdot_np, qddot_np, id_rbdl)
id_pb = pb.calculateInverseDynamics(gantry_pb, q, qdot, qddot)
id_u2c = id_sym(q, qdot, qddot)
for tau_idx in range(n_joints):
error_pb_u2c[tau_idx] += np.absolute(list2np(id_pb[tau_idx]) - u2c2np(id_u2c[tau_idx]))
error_rbdl_pb[tau_idx] += np.absolute(id_rbdl[tau_idx] - list2np(id_pb[tau_idx]))
error_rbdl_u2c[tau_idx] += np.absolute(id_rbdl[tau_idx] - u2c2np(id_u2c)[tau_idx])
sum_error_rbdl_pb = 0
sum_error_rbdl_u2c = 0
sum_error_pb_u2c = 0
for err in range(n_joints):
sum_error_rbdl_u2c += error_rbdl_u2c[err]
def grav(self, q):
tau = np.asarray([0.0] * self._joint_num)
qd = qdd = np.asarray([0.0] * self._joint_num)
rbdl.InverseDynamics(self.rbdl_model, q, qd, qdd, tau)
return tau
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]
zeros_pb[j] = 0.
rbdl.InverseDynamics(urmodel, q_np, zeros_rbdl, zeros_rbdl, g_rbdl)
kdl.ChainDynParam(ur_chain, gravity_kdl).JntToGravity(q_kdl, g_kdl)
g_pb = pb.calculateInverseDynamics(pbmodel, q, zeros_pb, zeros_pb)
g_u2c = g_sym(q)
#print g_u2c
for tau_idx in range(n_joints):
error_kdl_rbdl[tau_idx] += np.absolute((list2np(g_kdl[tau_idx]) - g_rbdl[tau_idx]))
error_kdl_u2c[tau_idx] += np.absolute((list2np(g_kdl[tau_idx]) - u2c2np(g_u2c[tau_idx])))
error_rbdl_u2c[tau_idx] += np.absolute((u2c2np(g_u2c[tau_idx]) - g_rbdl[tau_idx]))
error_pb_u2c[tau_idx] += np.absolute((u2c2np(g_u2c[tau_idx]) - list2np(g_pb[tau_idx])))
error_pb_kdl[tau_idx] += np.absolute((list2np(g_kdl[tau_idx]) - list2np(g_pb[tau_idx])))
error_pb_rbdl[tau_idx] += np.absolute(g_rbdl[tau_idx] - list2np(g_pb[tau_idx]))
sum_error_kdl_rbdl = 0
else:
taus_traj = np.zeros((N, robot_model.qdot_size))
print("Moving to the initial configuration of trajectory with torque control.")
raw_input("Press a key to continue...")
for ii in range(N):
if load_torques:
print("Reproducing previous torques!")
des_cmd.effort = taus_traj[ii, joints_to_move]
des_cmd.stiffness = np.zeros_like(tau[joints_to_move])
des_cmd.damping = np.zeros_like(tau[joints_to_move])
else:
des_cmd.position = joint_traj[ii, joints_to_move]
des_cmd.stiffness = default_joint_stiffness[joints_to_move]
des_cmd.damping = default_joint_damping[joints_to_move]
rbdl.InverseDynamics(robot_model, joint_traj[ii, :], joint_traj_dots[ii, :], joint_traj_ddots[ii, :], tau)
#taus_traj[ii, :] = joint_effort_state
#print(joint_traj[ii, joints_to_move] - joint_pos_state[joints_to_move])
#rbdl.NonlinearEffects(robot_model, joint_pos_state, joint_vel_state, g)
#rbdl.NonlinearEffects(robot_model, joint_pos_state, joint_vel_state*0, g)
print(repr(joint_traj_ddots[ii, joints_to_move]))
print(repr(joint_traj_ddots[ii, joints_to_move]/(freq*freq)))
print("##")
print(repr(joint_traj_dots[ii, joints_to_move]))
print(repr(joint_vel_state[joints_to_move]))
print("--")
#a = joint_traj_ddots[ii, :] + \
# default_joint_damping*0 * (joint_traj_dots[ii, :] - joint_vel_state) + \
# default_joint_stiffness*0.0 * (joint_traj[ii, :] - joint_pos_state)
pd_tau = Kp_tau.dot(joint_traj[ii, :] - joint_pos_state) + \
Kd_tau.dot(joint_traj_dots[ii, :] - joint_vel_state)
print("Size of q:", np.size(q))
#
# Giving an arbitrary location described in the local frame and printing it's
# location wrt the world frame
# COM_L1 = np.array([0.0, 0.0, 0.0])
# COM_L1_base = rbdl.CalcBodyToBaseCoordinates (model, q, body_1, COM_L1)
# print 'COM_L1_base: ', COM_L1_base
# Giving an arbitrary location described in the local frame and printing it's
# location wrt the world frame
q[1] = 3.14 / 2 #0#-105*3.14/180# 3.14
COM_L3 = np.array([0.0, -1.0, 0.0])
COM_L3_base = rbdl.CalcBodyToBaseCoordinates(model, q, body_3, COM_L3)
print 'Point Location wrt base: ', COM_L3_base
rbdl.InverseDynamics(model, q, qdot, qddot, tau)
print 'G: ', tau
def get_G(q_):
q_ = np.asarray(q_)
# print "Commanded q is :", q_[1]*180/3.1457
q = np.zeros(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]
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]
zeros_pb[j] = 0.
rbdl.InverseDynamics(snake_rbdl, q_np, zeros_rbdl, zeros_rbdl, g_rbdl)
kdl.ChainDynParam(snake_chain, gravity_kdl).JntToGravity(q_kdl, g_kdl)
g_pb = pb.calculateInverseDynamics(snake_pb, q, zeros_pb, zeros_pb)
g_u2c = g_sym(q)
for tau_idx in range(n_joints):
error_kdl_rbdl[tau_idx] += np.absolute(
(list2np(g_kdl[tau_idx]) - g_rbdl[tau_idx]))
error_kdl_u2c[tau_idx] += np.absolute(
(list2np(g_kdl[tau_idx]) - u2c2np(g_u2c[tau_idx])))
error_rbdl_u2c[tau_idx] += np.absolute(
(u2c2np(g_u2c[tau_idx]) - g_rbdl[tau_idx]))
error_pb_u2c[tau_idx] += np.absolute(
(u2c2np(g_u2c[tau_idx]) - list2np(g_pb[tau_idx])))
error_pb_kdl[tau_idx] += np.absolute(
(list2np(g_kdl[tau_idx]) - list2np(g_pb[tau_idx])))
def calculate_torque(self):
rbdl.InverseDynamics(self.model, self.q, self.qd, self.qdd,
self.torque)
# Configuracion articular q = np.array([0.5, 0.2, 0.3, 0.8, 0.5, 0.6]) # Velocidad articular dq = np.array([0.8, 0.7, 0.8, 0.6, 0.9, 1.0]) # Aceleracion articular ddq = np.array([0.2, 0.5, 0.4, 0.3, 1.0, 0.5]) # Arrays numpy zeros = np.zeros(ndof) # Vector de ceros tau = np.zeros(ndof) # Para torque g = np.zeros(ndof) # Para la gravedad c = np.zeros(ndof) # Para el vector de Coriolis+centrifuga M = np.zeros([ndof, ndof]) # Para la matriz de inercia e = np.eye(6) # Vector identidad # Torque dada la configuracion del robot rbdl.InverseDynamics(modelo, q, dq, ddq, tau) # Parte 1: Calcular vector de gravedad, vector de Coriolis/centrifuga, # y matriz M usando solamente InverseDynamics # Parte 2: Calcular M y los efectos no lineales b usando las funciones # CompositeRigidBodyAlgorithm y NonlinearEffects. Almacenar los resultados # en los arreglos llamados M2 y b2 b2 = np.zeros(ndof) # Para efectos no lineales M2 = np.zeros([ndof, ndof]) # Para matriz de inercia # Parte 2: Verificacion de valores # Parte 3: Verificacion de la expresion de la dinamica
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
ddq = np.linalg.inv(M).dot(u - g - c) ## ace
#SE OPTIENE dq anterior
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
qdot[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
qddot[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
q_np[j] = q[j]
qdot_np[j] = qdot[j]
qddot_np[j] = qddot[j]
rbdl.InverseDynamics(snake_rbdl, q_np, qdot_np, qddot_np, id_rbdl)
id_pb = pb.calculateInverseDynamics(snake_pb, q, qdot, qddot)
id_u2c = id_sym(q, qdot, qddot)
for tau_idx in range(n_joints):
error_pb_u2c[tau_idx] += np.absolute(
list2np(id_pb[tau_idx]) - u2c2np(id_u2c[tau_idx]))
error_rbdl_pb[tau_idx] += np.absolute(id_rbdl[tau_idx] -
list2np(id_pb[tau_idx]))
error_rbdl_u2c[tau_idx] += np.absolute(id_rbdl[tau_idx] -
u2c2np(id_u2c)[tau_idx])
sum_error_rbdl_pb = 0
sum_error_rbdl_u2c = 0
sum_error_pb_u2c = 0
str(t) + ' ' + str(xdes[0, 0]) + ' ' + str(xdes[1, 0]) + ' ' +
str(xdes[2, 0]) + '\n')
fqact.write(
str(t) + ' ' + str(q[0]) + ' ' + str(q[1]) + ' ' + str(q[2]) + ' ' +
str(q[3]) + ' ' + str(q[4]) + ' ' + str(q[5]) + '\n ')
fqdes.write(
str(t) + ' ' + str(qdes[0]) + ' ' + str(qdes[1]) + ' ' + str(qdes[2]) +
' ' + str(qdes[3]) + ' ' + str(qdes[4]) + ' ' + str(qdes[5]) + '\n ')
# ----------------------------
# Control dinamico (COMPLETAR)
# ----------------------------
y = ddqdes + Kd.dot(dqdes - dq) + Kp.dot(qdes - q)
rbdl.InverseDynamics(modelo, q, dq, y, u)
# Simulacion del robot
robot.send_command(u)
# Publicacion del mensaje
jstate.position = q
pub.publish(jstate)
bmarker_deseado.xyz(xdes)
bmarker_actual.xyz(x)
t = t + dt
# Esperar hasta la siguiente iteracion
rate.sleep()
fqact.close()
fqdes.close()
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
qdot[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
qddot[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
q_np[j] = q[j]
qdot_np[j] = qdot[j]
qddot_np[j] = qddot[j]
rbdl.InverseDynamics(ur5_rbdl, q_np, qdot_np, qddot_np, id_rbdl)
id_pb = pb.calculateInverseDynamics(ur5_pb, q, qdot, qddot)
id_u2c = id_sym(q, qdot, qddot)
for tau_idx in range(n_joints):
error_pb_u2c[tau_idx] += np.absolute(list2np(id_pb[tau_idx]) - u2c2np(id_u2c[tau_idx]))
error_rbdl_pb[tau_idx] += np.absolute(id_rbdl[tau_idx] - list2np(id_pb[tau_idx]))
error_rbdl_u2c[tau_idx] += np.absolute(id_rbdl[tau_idx] - u2c2np(id_u2c)[tau_idx])
sum_error_rbdl_pb = 0
sum_error_rbdl_u2c = 0
sum_error_pb_u2c = 0
for err in range(n_joints):
sum_error_rbdl_u2c += error_rbdl_u2c[err]
fxact.write(
str(t) + ' ' + str(x[0]) + ' ' + str(x[1]) + ' ' + str(x[2]) + '\n')
fxdes.write(
str(t) + ' ' + str(xdes[0]) + ' ' + str(xdes[1]) + ' ' + str(xdes[2]) +
'\n')
fqact.write(
str(t) + ' ' + str(q[0]) + ' ' + str(q[1]) + ' ' + str(q[2]) + ' ' +
str(q[3]) + ' ' + str(q[4]) + ' ' + str(q[5]) + '\n ')
fqdes.write(
str(t) + ' ' + str(qdes[0]) + ' ' + str(qdes[1]) + ' ' + str(qdes[2]) +
' ' + str(qdes[3]) + ' ' + str(qdes[4]) + ' ' + str(qdes[5]) + '\n ')
# ----------------------------
# Control dinamico
# ----------------------------
rbdl.InverseDynamics(modelo, q, np.zeros(ndof), np.zeros(ndof), g)
ST = Kp.dot(qdes - q)
TT = -Kd.dot(dq)
u = g + ST + TT # Ley de control
if np.linalg.norm(qdes - q) < 0.01:
break
# Simulacion del robot
robot.send_command(u)
# Publicacion del mensaje
jstate.position = q
pub.publish(jstate)
bmarker_deseado.xyz(xdes)
bmarker_actual.xyz(x)
t = t + dt
