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