Giters

bryansgue / Course_Robot_Arm_4DOF

Geek Repo:Geek Repo

Github PK Tool:Github PK Tool

Bienvenidos Curso de Introducción a Gemelos Digitales

Demostración del Brazo Robot de 4 grados de libertad.

Brazo Robot de 4 grados de libertad

Digital Twin

Demostración del Digital Twin:

Digital Twin

Comando importantes

Clonar repositorio:

git clone https://github.com/bryansgue/Course_Robot_Arm_4DOF

Actualizar repositorio:

git pull origin main

Forzar actualizacion de repositorio

git fetch origin main && git reset --hard origin/main

Creación de un Paquete ROS C++

Si deseas crear un paquete ROS C++ para trabajar con el brazo robótico, puedes seguir estos pasos:

  1. Crea un nuevo paquete ROS llamado brazo_robotico utilizando el comando catkin_create_pkg en el direcotrio catkin_ws/src:

    cd ~/catkin_ws/src
catkin_create_pkg brazo_robot roscpp std_msgs

  2. Navega al directorio de tu paquete recién creado:

    cd ~/catkin_ws/src/brazo_robot

  3. Crea un archivo llamado nodo_robot.cpp en el directorio src del paquete:

    touch src/nodo_robot.cpp

  4. Agrega el siguiente contenido al archivo src/nodo_robot.cpp:

    #include "ros/ros.h"
#include "std_msgs/String.h"

int main(int argc, char **argv) {
    ros::init(argc, argv, "nodo_robot");
    ros::NodeHandle nh;
    
    ros::Publisher pub = nh.advertise<std_msgs::String>("chatter", 1000);

    ros::Rate loop_rate(10);

    while (ros::ok()) {
        std_msgs::String msg;
        msg.data = "Hola desde brazo_robotico";

        pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

  5. Modifica el archivo CMakeLists.txt en el directorio de tu paquete (~/catkin_ws/src/brazo_robot/CMakeLists.txt) con el siguiente contenido:

    cmake_minimum_required(VERSION 3.0.2)
project(brazo_robot)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
)

catkin_package()

include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(nodo_robot src/nodo_robot.cpp)

target_link_libraries(nodo_robot
  ${catkin_LIBRARIES}
)
# Si deseas agregar más ejecutables, puedes hacerlo por ejemplo como sigue:
# add_executable(nodo_arm src/nodo_arm.cpp)
# target_link_libraries(nodo_arm
#   ${catkin_LIBRARIES}
# )

  6. Luego, ejecuta catkin_make para compilar tu paquete:

    cd ~/catkin_ws
catkin_make

  7. En el caso de que no se detecte los cambios en el espacio de trabajo, ejecutamos lo siguiente:

    cd ~/catkin_ws
catkin_make --only-pkg-with-deps brazo_robot

  8. Actualiza el entorno de trabajo

    source devel/setup.bash

  9. Ejecuta el nodo

     rosrun brazo_robot nodo_robot

Con estos pasos, habrás creado un paquete ROS C++ llamado brazo_robot que incluye el nodos: nodo_robot.

Modelo de una Partícula en el Espacio

Este ejemplo muestra un modelo simple de una partícula en el espacio controlada por un nodo ROS en C++.

#include <ros/ros.h>
#include <sensor_msgs/Joy.h>
#include <Eigen/Dense>
#include <iostream>


using namespace Eigen;
using namespace std;

double u1 = 0;
double u2 = 0;
double u3 = 0.0;
double u4 = 0.0;

Eigen::VectorXd u_ref(4);

void control_callback(const sensor_msgs::Joy::ConstPtr& state_msg)
{
    u1 = state_msg->axes[0];
    u2 = state_msg->axes[1];
    u3 = state_msg->axes[2];
    u4 = state_msg->axes[3];

    u_ref << u1, u2, u3, u4;

    // Imprimir el valor de u
    
}

VectorXd get_control_action()
{
    Eigen::VectorXd uf(4);
    uf << u_ref(0), u_ref(1), u_ref(2), u_ref(3);
    //std::cout << "u: " << uf.transpose() << std::endl;
    return uf;
}

void send_arm_states(const VectorXd& q, ros::Publisher& control_pub, sensor_msgs::Joy& control_msg)
{
    control_msg.header.frame_id = "Arm States";
    control_msg.header.stamp = ros::Time::now();
    control_msg.axes.clear();
    for (int i = 0; i < q.size(); ++i)
    {
        control_msg.axes.push_back(q(i));
    }
    control_pub.publish(control_msg);
}

MatrixXd f_sys(const VectorXd& x, const VectorXd& u)
{
    MatrixXd A(4, 4);
    MatrixXd B(4, 4);
    
    A << -0.7706, -0.0051, 0.2265, 0.1806,
         0.0163, -0.3537, -0.2810, 0.0157,
         -0.2056, -0.0701, -1.2446, 0.0927,
         -0.3169, 0.0471, -0.0381, -1.6991;

    B << 0.9636, -0.0825, -0.1494, -0.0196,
         0.0113, 1.0200, -0.0269, -0.0893,
         0.0338, -0.0108, 1.8970, -0.0488,
         0.1425, 0.0266, -0.1410, 2.0500;

    return A * x + B * u;
}


void main_loop(ros::Publisher& control_pub, sensor_msgs::Joy& control_msg)
{
    double t_final = 60;
    double freq = 30;
    double t_s = 1.0 / freq;
    double ros_rate = 30;

    Eigen::VectorXd q(4);
    Eigen::VectorXd q_p(4);
    Eigen::VectorXd u(4);
    
    q << 0, 0, 0, 0; // Initial condition
    u_ref << 0,0,0,0;

    std::cout << "OK, controller is running!!!!" << std::endl;

    ros::Rate rate(ros_rate);

    for (int k = 0; k < t_final * freq; ++k)
    {
        // Llamar a spinOnce para procesar los callbacks
        ros::spinOnce();
        
        // Read Real data
        u =  get_control_action();
        
        //u << 1, 0.5, 1, -0.5;

        // Model ARM [ q_p = f(q,u) ]
        q_p = f_sys(q, u);

        // Evolucion de los estados del sistema
        q = q_p * t_s + q;

        send_arm_states(q, control_pub, control_msg);

        // Imprimir el valor de u
        std::cout << "q: " << q.transpose() << std::endl;

        
        
        // Espera para cumplir con la frecuencia de publicación deseada
        rate.sleep();
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "Robot");
    ros::NodeHandle nh;

    ros::Subscriber velocity_subscriber = nh.subscribe("/control", 10, control_callback);
    ros::Publisher control_pub = nh.advertise<sensor_msgs::Joy>("/states", 10);
    sensor_msgs::Joy control_msg;

    //u_ref.setZero(); 

    main_loop(control_pub, control_msg);

    return 0;
}

Modifica el archivo CMakeLists.txt en el directorio de tu paquete (~/catkin_ws/src/brazo_robot/CMakeLists.txt) con el siguiente contenido:

cmake_minimum_required(VERSION 3.0.2)
project(brazo_robot)

find_package(catkin REQUIRED COMPONENTS
  eigen_conversions
  roscpp
  std_msgs
)

catkin_package()

include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(nodo_robot src/nodo_robot.cpp)
add_executable(nodo_arm src/nodo_arm.cpp)

target_link_libraries(nodo_robot
  ${catkin_LIBRARIES}
)
target_link_libraries(nodo_arm ${catkin_LIBRARIES} )

Funcion para graficar q y q_d

def plot_states(q, qd=None):

    plt.clf()
    
    plt.plot(q[0], label='q1')
    plt.plot(q[1], label='q2')
    plt.plot(q[2], label='q3')
    plt.plot(q[3], label='q4')
    
    if qd is not None:
        plt.plot(qd[0], '--', label='qd1')
        plt.plot(qd[1], '--', label='qd2')
        plt.plot(qd[2], '--', label='qd3')
        plt.plot(qd[3], '--', label='qd4')
    
    plt.xlabel('Time steps')
    plt.ylabel('State value')
    plt.title('System States over Time')
    
    plt.legend()
    plt.draw()
    plt.pause(0.001)

Robotics Toolbox: PETER CORKE

git clone https://github.com/petercorke/robotics-toolbox-matlab.git rtb
git clone https://github.com/petercorke/spatial-math smtb
git clone https://github.com/petercorke/toolbox-common-matlab common

Luego, dentro de MATLAB, agregue estas carpetas a su ruta:

addpath rtb common smtb

(UNA SOLA VEZ V2023a) Agregar la variable WEBOTS_HOME al archivo .bashrc para que se establezca automáticamente en cada nueva sesión de terminal:

echo 'export PYTHONPATH=/usr/local/webots/lib/controller/python:$PYTHONPATH' >> ~/.bashrc

(UNA SOLA VEZ V2023b )Agregar la variable WEBOTS_HOME al archivo .bashrc para que se establezca automáticamente en cada nueva sesión de terminal:

echo 'export WEBOTS_HOME=/usr/local/webots' >> ~/.bashrc

Recargar el archivo .bashrc para aplicar los cambios en la sesión actual:

source ~/.bashrc

Verificar que la variable WEBOTS_HOME está correctamente establecida:

echo $WEBOTS_HOME

Codigo para ejecutar controlador externo en webots

$WEBOTS_HOME/webots-controller

About

Languages

Language:MATLAB 59.8%Language:Python 33.3%Language:OpenSCAD 1.8%Language:C++ 1.7%Language:C 1.6%Language:HTML 1.0%Language:Makefile 0.8%