four_link_robot

Comprehensive project modelling and controlling four link planar robot.

Project consist of:

  • Octave files deriving the kinematics and dynalic matrices
  • ROS nodes
    • four_link_description
    • four_link_control
    • four_link_brain
    • test_simulation

Dependencies

For running ROS nodes, Octave scripts and Jupyter notebooks you will need python numpy and sympy and matplotlib

pip install numpy
pip install matplotlib==2.0.2
pip install --user sympy==1.5.1

ROS simulation

Copy the ros nodes inside your catkin workspace source directory:

cd ~/example_catkin_ws/
mkdir src && cd src

Then you need to copy the ros nodes in this directory:

cp  ~/example_path_to_four_link_robot/ROS_nodes/* .

Additionally for the inverse kinematics example you will need to use python rqt so ,ake sure to install:

sudo apt-get install ros-melodic-rqt ros-melodic-rqt-common-plugins

And get the rqt_ez_publisher ros node in your src directory

git clone https://github.com/OTL/rqt_ez_publisher.git

Now you are ready to build the workspace

cd ~/example_catkin_ws/
catkin_make

The final step is adding the directory to the environmental variables if it is not done before

source ~/example_catkin_ws/devel/setup.sh

Direct kinematics simulation with manipulability

Open terminal and run:

roslaunch four_link_description rviz_ik_gui.launch

To visualize the manipulability ellipsoids, in the additional terminal run:

rosrun four_link_brain four_link_manipulability.py

To visualize the force polytopes in a new terminal run:

rosrun four_link_brain four_link_force_polytope.py

Each time you open a new terminal you might need to redo the command
source ~/example_catkin_ws/devel/setup.sh

Inverse kinematics simulation with manipulability

Open terminal and run:

roslaunch four_link_description rviz_dk_gui.launch

To visualize the manipulability ellipsoids, in the additional terminal run:

rosrun four_link_brain four_link_manipulability.py

To visualize robot apparent (equivalent) mass in y and z direction, in a new terminal run:

rosrun four_link_brain four_link_equivalent_mass.py

Each time you open a new terminal you might need to redo the command
source ~/example_catkin_ws/devel/setup.sh

Full dynamics simulation using Gazebo

Open terminal and first run the empty robot world.

roslaunch test_simulation my_robot_world.launch

Then open a new terminal and spawn the robot to the world.

roslaunch four_link_description spawn.launch

Once when the robot is spawned we can start the control strategy.

Gravity compensation example

For the gravity compensation control we will apply to the robot joints exactly the negative gravity matrix torque = G(q). Therefore we will control each joint torque directly, to enable the joint torque control interface run:

roslaunch four_link_control four_link_torque_control.launch

And finally, run the gravity compensation algorithm:

rosrun four_link_brain gravity_compensation.py

Octave scripts

This repository contains the derivation of the dynamical model of the robot as well as the direct kinematics and jacobian matrix.

To calculate the jacobian matrix run:

four_link_jacobian

To calculate the potential and kinetic energies or the robot and to construct symbolic mass M, coriolis C and gravity G matrices using lagrange method

four_link_dynamics

Then in order to evaluate for example the mass matrix for specific joint position you can simply run:

# add the physical parameters
M_ = subs(M, {l0 l1 l2 l3 l4 r1 r2 r3 r4 m1 m2 m3 m4},{ 0.05 0.5 0.5 0.5 0.7 0.25 0.25 0.25 0.25 0.5 0.5 0.5 0.5 })
# add the actual joint positions
M_ = eval(subs(M_, {q1 q2 q3 q4},{0  0 pi/10 0}))

Or for example gravity vector

# add the physical parameters
G_ = subs(G, {g, l0 l1 l2 l3 l4 r1 r2 r3 r4 m1 m2 m3 m4},{ 9.81 0.05 0.5 0.5 0.5 0.7 0.25 0.25 0.25 0.25 0.5 0.5 0.5 0.5 })
# add the actual joint positions
G_ = eval(subs(G_, {q1 q2 q3 q4},{0  0 pi/10 0}))

To evaluate achievable forces and see the joint torque limits projected into the force space run:

four_link_analysis

To display the vertexes of force polytopes by the algorithm from paper:

Chiacchio, Pasquale, Yann Bouffard-Vercelli, and Francois Pierrot. "Evaluation of force capabilities for redundant manipulators." Proceedings of IEEE International Conference on Robotics and Automation. Vol. 4. IEEE, 1996.

four_link_polytope_pierrot # implementing paper

Or the algorithm explained in:https://www.overleaf.com/read/rqrbvtfyhhnm

four_link_polytope_auctus

Finally to add the ellipsoid of force to the figure run:

four_link_ellipsoid

results matching ""

    No results matching ""