The ABR Control library is a python package for the control and path planning of robotic arms in real or simulated environments.

Overview

https://imgur.com/4qIqbRn.jpg

ABR Control

The ABR Control library is a python package for the control and path planning of robotic arms in real or simulated environments. ABR Control provides API's for the Mujoco, CoppeliaSim (formerly known as VREP), and Pygame simulation environments, and arm configuration files for one, two, and three-joint models, as well as the UR5 and Kinova Jaco 2 arms. Users can also easily extend the package to run with custom arm configurations. ABR Control auto-generates efficient C code for generating the control signals, or uses Mujoco's internal functions to carry out the calculations.

ABR also provides an interface and config available for controlling a real Jaco 2 at the ABR_Jaco2 repository.

Installation

The ABR Control library depends on NumPy, SymPy, SciPy, CloudPickle, Cython, SetupTools, Nengo, and Matplotlib. We recommend using Anaconda. Note that installing in a clean environment will require compiling of the dependent libraries, and will take a few minutes.

To install ABR Control, clone this repository and run:

sudo apt-get install g++
sudo apt-get install python-dev
sudo apt-get install libfreetype6-dev
conda activate your_environment
python setup.py install
python setup.py develop

ABR Control is tested to work on Python 3.6+, Python 2 is not supported.

Optional Installation

Mujoco

If you would like to use the Mujoco API you will need to install a forked version of mujoco-py with hooks for exitting out of simulations with the ESC key. To use the mujoco API, make sure you are in your anaconda environment and run:

git clone https://github.com/studywolf/mujoco-py.git
cd mujoco-py
pip install -e .
pip install glfw>=1.8.3
pip install requests

Pygame

If you would like to use the Pygame API, from your anaconda environment run:

pip install pygame

Vrep

We support Vrep <=4.0. You will need to download Vrep and follow the installation instructions.

PyDMPs

Some of the path planners work through the use of dynamic movement primitives (DMPs). DMPs allow for a stable, generalizable, and easily extensible way of representing complex trajectories. Path planners making use of DMP are appended with 'dmp' in their filename and will require the installation of the pydmps repository. To install, from your Anaconda environment run:

pip install pydmps

Usage

The ABR Control repo is comprised of three parts: 1) arms, 2) controllers, and 3) interfaces.

1a) Arms: Using CoppeliaSim, Pygame, or a real arm

All of the required information about an arm model is kept in that arm's config file. To use the ABR Control library with a new arm, the user must provide the transformation matrices (written using SymPy expressions) from the robot's origin reference frame to each link's centre-of-mass (COM) and joints. These are specified sequentially, e.g. origin -> link0 COM, link0 COM -> joint0, joint0 -> link1 COM, etc. The arm config file and any simulation code is kept in a folder named the same as the arm in the abr_control/arms/ directory.

The ABR Control configuration base class uses the SymPy transform matrices to provide functions that will calculate the transforms, Jacobian, Jacobian derivative, inertia matrices, gravity forces, and centripetal and Coriolis effects for each joint and COM. These can be accessed:

from abr_control.arms import jaco2

robot_config = jaco2.Config()
# calculate the following given the arm state at joint_angles
robot_config.Tx('joint3', q=joint_angles)  # the (x, y, z) position of joint3
robot_config.M(q=joint_angles)  # calculate the inertia matrix in joint space
robot_config.J('EE', q=joint_angles)  # the Jacobian of the end-effector

By default, the use_cython parameter is set to True to allow for real-time control by generating optimized Cython code for each of the robot configuration functions. This can take a little bit of time to generate these functions, but they are saved in ~.cache/abr_control/arm_name/saved_functions where they will be loaded from for future runs. Note that a hash is saved for the config, so if any changes are made the functions will be regenerated during the next use. The cython optimization can be turned off on instantiation:

from abr_control.arms import ur5

robot_config = ur5.Config(use_cython=False)

Below are results from running the operational space controller with different controllers with use_cython=True and False.

docs/examples/timing.png

1b) Arms: Using Mujoco

When using Mujoco the process is a bit different. Mujoco handles the calculation of all the kinematics and dynamics functions, and only requires an xml config be made describing the kinematic chain. The Mujoco API page describes this in detail.

Detailed models can be created by importing 3D modeling stl files and using the mesh object type in the tag. An example of this is the abr_control/arms/jaco2/jaco2.xml. For users building their own models, you may specify the location of the xml with the folder parameter. For more details, please refer to the Mujoco documentation linked above and use the xml files in this repository as examples.

2) Controllers

Controllers make use of the robot configuration files to generate control signals that accomplish a given task (for most controllers this is reaching a target). The ABR Control library provides implementations of several primary controllers, including operational space, generalized coordinates (joint) space, sliding, and floating control.

When using an operational space controller, it is possible to also pass in secondary controllers to operate in the null space of the operational space controller. These secondary controllers can be set up to achieve secondary goals such as avoiding joint limits and obstacles, damping movement, or maintaining a configuration near a specified resting state.

In the path_planners folder there are several path planners that can be used in conjunction with the controllers. There are filters, linear and second order, which can be used to trace a path from the current position to the target without suddenly warping and causing large spikes in generated torque. The inverse kinematics planner takes in a target for the end-effector and returns a joint angle trajectory to follow. An arc path planner is also provided that creates an arcing path which can be useful when the arm has to reach over itself. This can help prevent self-collisions and odd arm configurations.

Each path planner also has the ability to generate a trajectory for end-effector orientation with the path_plannner.generate_orientation_path() function. This uses spherical linear interpolation (SLERP) to generate a set of orientations from a start to a target quaternion. The time profile will match that of the path planner instantiated (ie: a linear path planner will have a linear step in orientation over time, with a constant change in orientation, whereas a second order path planner will have a bell shaped profile with the largest steps occurring during the middle of the movement, with an acceleration and deceleration at the start and end, respectively.) In addition to filters, there is an example path planner using the dynamic movement primitives trajectory generation system.

Finally, there is an implementation of nonlinear adaptive control in the signals folder, as well as examples in Mujoco, PyGame, and CoppeliaSim showing how this class can be used to overcome unexpected forces acting on the arm.

3) Interfaces

For communications to and from the system under control, an interface API is used. The functions available in each class vary depending on the specific system, but must provide connect, disconnect, send_forces and get_feedback methods.

Putting everything together

A control loop using these three files looks like:

from abr_control.arms import jaco2
from abr_control.controllers import OSC
from abr_control.interfaces import CoppeliaSim

robot_config = jaco2.Config()
interface = CoppeliaSim(robot_config)
interface.connect()

ctrlr = OSC(robot_config, kp=20,
            # control (x, y, z) out of [x, y, z, alpha, beta, gamma]
            ctrlr_dof=[True, True, True, False, False, False])

target_xyz = [.2, .2, .5]  # in metres
target_orientation = [0, 0, 0]  # Euler angles, relevant when controlled
for ii in range(1000):
    feedback = interface.get_feedback()  # returns a dictionary with q, dq
    u = ctrlr.generate(
        q=feedback['q'],
        dq=feedback['dq'],
        target=np.hstack([target_xyz, target_orientation]))
    interface.send_forces(u)  # send forces and step CoppeliaSim sim forward

interface.disconnect()

NOTE that when using the Mujoco interface it is necessary to instantiate and connect the interface before instantiating the controller. Some parameters only get parsed from the xml once the arm config is linked to the mujoco interface, which happens upon connection.

Examples

The ABR Control repo comes with several examples that demonstrate the use of the different interfaces and controllers.

By default all of the PyGame examples run with the three-link MapleSim arm. You can also run the examples using the two-link Python arm by changing the import statement at the top of the example scripts.

To run the CoppeliaSim examples, have the most recent CoppeliaSim version open. By default, the CoppeliaSim examples all run with the UR5 or Jaco2 arm model. To change this, change which arm folder is imported at the top of the example script. The first time you run an example you will be promted to download the arm model. Simply select yes to download the file and the simulation will start once the download completes.

To run the Mujoco examples, you will be promted to download any mesh or texture files, if they are used in the xml config, similarly to the CoppeliaSim arm model. Once the download completes the simulation will start. If you are using the forked Mujoco-Py repository (See Optional Installation section) you can exit the simulation with the ESC key and pause with the spacebar.

Comments
  • xyz_offset argument with problems

    xyz_offset argument with problems

    I've added an ur5 arm on a mobile robot. Because of that i need to constantly update its base position. But when I use the OSC.generate and robot_config.Tx with the specific xyz offset. It returns a different value that is supposed to be. For example:

    base = interface.get_xyz("UR5_link0_visible")
    result: Base is:  [-9.712021827697754, 1.791999101638794, 0.16733066737651825]
    
    start = robot_config.Tx("EE", feedback["q"], x= np.array(base))
    result: start is:  [-0.33954067  1.79179413 -8.71078803]
    
    opened by quartzolit 7
  • Moved scaling function from abr_analyze

    Moved scaling function from abr_analyze

    • function for scaling inputs for spherical conversion using the nengolib function moved from abr_analyze.network_utils to dynamics_adaptation
    • moved test for this function as well to avoid rewriting it
    opened by p3jawors 7
  • Added orientation control to OSC

    Added orientation control to OSC

    Added the ability to specify target Euler angles for the end-effector to the operational space controller. It now accepts an additional parameter, ctrlr_dof which is a 6D list specifying which dimensions of [x, y, z, alpha, beta, gamma] you would like to control. NOTE: if the arm under control has fewer DOF than the number of DOF specified for control you will get poor behaviour (usually oscillation between several DOF targets).

    Also changed target_pos -> target in all the controllers, and the input target and target_vel for the OSC must now be 6D vectors.

    Cleaned up the examples, reducing code (removing generation calls outside of main control loop) and removing unused tracking code, updated comments, added two more examples for both the PyGame and VREP folders (orientation_control_osc and position_and_orientation_control_osc), renamed reaching_* -> position_control_*.

    Also I removed the commented code from the OSC null space controller and removed use_dJ as an option.

    opened by studywolf 7
  • error occurs when calculation of 'C'

    error occurs when calculation of 'C'

    When I run file ' position_control_sliding.py' to test, an error occurs as follows: 'UnicodeDecodeError: 'utf-8' codec can't decode byte 0xc3 in position 908: invalid continuation byte', which is in file base_config.py Line 769 in _calc_C. I compare the calculation of J, M, G, it seems that only calculation C is nor right.

    opened by vanusy 6
  • WIP: Mujoco interface through mujoco_py

    WIP: Mujoco interface through mujoco_py

    Basic interface and example script set up

    • need to resolve the forward kinematics calculations (set up differently than VREP sim)
    • ideally reading from ur5.xml file to get the transformation matrices
    opened by studywolf 6
  • How to create manipulator config files

    How to create manipulator config files

    Hello, I have a new robot config information with urdf format file and D-H parameters, I wish to know how to convert it into config files of this project? The variables in config.py is confusing, can you tell me how to calculate them?

    opened by fhln 5
  • Gauss pp

    Gauss pp

    Overhaul of path planners. We now have one master path planner that takes in a position and velocity profile. This allows up to separate the shape of the path we're taking, and the velocity throughout that path. Examples have been updated, and a new section for path_planner examples has been added.

    opened by p3jawors 4
  • What is the effect of dt in your expressions?

    What is the effect of dt in your expressions?

    I can see on your examples you set the dt value in order to connect and set time step of the simulation. But when I designedly increase the dt value, higher is the joint torque value (u) and consequently the control fails at higher dt value. However, no step time is used on your equations, besides when you set up the connection to CoppeliaSim.

    My main objective by asking this is to use your repository via ROS, and controls UR5 robot arm at CoppeliaSim using ROS interface. By doing this it is not possible to force a time step, and both (CoppeliaSim and ROS) will run asynchronously.

    Is that a way to control according to dt, or I need a powerful computer to run both as fast as possible, in order to indirectly generate a lower dt value? (Also thinking in real life robot arm control)

    opened by quartzolit 4
  • thanks + pointer to other robot control library

    thanks + pointer to other robot control library

    @studywolf I've been a reader of your blog for quite some time and first I wanted to thank you for all the detailed and informative posts you've written. I saw your recent post about the motion control library. https://studywolf.wordpress.com/tag/vrep/

    I wanted to point you to another set of libraries that go together which I've been using and have found to be quite good for similar purposes: https://github.com/jrl-umi3218/Tasks https://github.com/jrl-umi3218/SpaceVecAlg https://github.com/jrl-umi3218/RBDyn https://github.com/jrl-umi3218/Eigen3ToPython

    This is really just an FYI which incorporates other interesting control approaches and also has been integrated with v-rep in various projects. Hopefully this is more useful or interesting than disruptive, but feel free to close. Thanks! :-)

    opened by ahundt 4
  • MujocoConfig Error

    MujocoConfig Error

    AttributeError: type object 'MujocoConfig' has no attribute 'Config'

    from abr_control.arms.mujoco_config import MujocoConfig as arm
    
    # 'ur5', 'onelink', 'twolink', and 'threelink' also available as arm_model's
    arm_model = 'jaco2'
    robot_config = arm.Config(arm_model)
    
    opened by jeanbuntu 3
  • ask for  reference about the controllers

    ask for reference about the controllers

    I have seen there are several different controllers in the "abr_control/abr_control/controllers/", such as OSC and sliding et al. Could you give me some references related to your controllers? Many thanks for your kindly help :-D

    opened by vanusy 3
  • AttributeError: 'MjViewer' object has no attribute 'exit'

    AttributeError: 'MjViewer' object has no attribute 'exit'

    Hello everyone, I met a problem recently when I run the bar control example of jaco2 for Mujoco platform. When I run the program "position_joint_control.py", all my environment variables and configuration are right. Howver, on Pycharm, the terminal shows the problem as below:

    To download mujoco stl files, you need to "pip install requests"
    Creating window glfw
    MuJoCo session created
    
    Simulation starting...
    
    Traceback (most recent call last):
      File "/home/shine/abr_control/docs/examples/Mujoco/position_joint_control.py", line 43, in <module>
        if interface.viewer.exit:
    AttributeError: 'MjViewer' object has no attribute 'exit'
    
    MuJoCO session closed...
    Simulation terminated...
    

    The simulation windows seems crash out and I didn't see anything on that. Does anyone meet the same problem? Thanks!

    Additionally, I see someone came up with the same question a few years ago, I followed the suggestions but it seems doesn't work for current Mujoco or mujoco-py. That makes me feel confused.

    opened by JackTony123 1
  • Add a warning when the joint state is ignored by mujoco_config

    Add a warning when the joint state is ignored by mujoco_config

    Unless use_sim_state=False, the joint state q that's passed in is ignored. If this is ever not the same as the state of the arm simulation a warning should be printed to alert the user.

    opened by studywolf 0
Owner
Applied Brain Research
Applied Brain Research
Playing diabolo with two robot arms in ROS + Gazebo

Playing diabolo with robots This repository holds the ROS packages for playing diabolo with two UR5e robot arms on ROS Melodic (Ubuntu 18.04). Read ou

null 23 Dec 18, 2022
Example Python code for building RPi-controlled robotic systems

RPi Example Code Example Python code for building RPi-controlled robotic systems These python files have been compiled / developed by the Neurobionics

Elliott Rouse 2 Feb 4, 2022
FERM: A Framework for Efficient Robotic Manipulation

Framework for Efficient Robotic Manipulation FERM is a framework that enables robots to learn tasks within an hour of real time training.

Ruihan (Philip) Zhao 111 Dec 31, 2022
Python implementation of ZMP Preview Control approach for biped robot control.

ZMP Preview Control This is the Python implementation of ZMP Preview Control app

Chaobin 24 Dec 19, 2022
Isaac Gym Environments for Legged Robots

Isaac Gym Environments for Legged Robots This repository provides the environment used to train ANYmal (and other robots) to walk on rough terrain usi

Robotic Systems Lab - Legged Robotics at ETH Zürich 372 Jan 8, 2023
Tool to create 3D printable terrain with integrated path/road part files (Single material 3d printer)

BACKGROUND This has been an ongoing project of mine for a few months now. I run trails a lot and original the goal was to create a function to combine

null 9 Apr 26, 2022
It is a program that displays the current temperature of the GPU and CPU in real time and stores the temperature history.

HWLogger It is a program that displays the current temperature of the GPU and CPU in real time and stores the temperature history. Sample Usage Run HW

Xeros 0 Apr 5, 2022
A module for cross-platform control of the mouse and keyboard in python that is simple to install and use.

PyUserInput PyUserInput is a group project so we've moved the project over to a group organization: https://github.com/PyUserInput/PyUserInput . That

Paul Barton 1k Dec 27, 2022
This application works with serial communication. Use a simple gui to send and receive serial data from arduino and control leds and motor direction

This application works with serial communication. Use a simple gui to send and receive serial data from arduino and control leds and motor direction

ThyagoKZKR 2 Jul 18, 2022
I made this so I can control my Tapo L510 light bulb and Govee H6159 light strip using the PyP100 module and the Govee public API

TAPO-And-Govee-Controller I made this so I can control my Tapo L510 light bulb and Govee H6159 light strip using the PyP100 module and the Govee publi

James Westhead 0 Nov 23, 2021
Hotplugger: Real USB Port Passthrough for VFIO/QEMU!

Hotplugger: Real USB Port Passthrough for VFIO/QEMU! Welcome to Hotplugger! This app, as the name might tell you, is a combination of some scripts (py

DARKGuy (Alemar) 66 Nov 24, 2022
LedFx is a network based LED effect controller with support for advanced real-time audio effects

Welcome to LedFx ✨ -Making music come alive! LedFx website: https://ledfx.app/ What is LedFx? What LedFx offers is the ability to take audio input, an

null 786 Jan 2, 2023
Uses the Duke Energy Gateway to import near real time energy usage into Home Assistant

Duke Energy Gateway This is a custom integration for Home Assistant. It pulls near-real-time energy usage from Duke Energy via the Duke Energy Gateway

Michael Meli 28 Dec 23, 2022
2021 Real Robot Challenge Phase2 attemp

Real_Robot_Challenge_Phase2_AE_attemp We(team name:thriftysnipe) are the first place winner of Phase1 in 2021 Real Robot Challenge. Please see this pa

Qiang Wang 2 Nov 15, 2021
NYCT-GTFS - Real-time NYC subway data parsing for humans

NYCT-GTFS - Real-time NYC subway data parsing for humans This python library provides a human-friendly, native python interface for dealing with the N

Andrew Dickinson 37 Dec 27, 2022
Real-time Coastal Monitoring at the University of Hawaii at Manoa

Coastal Monitoring at the University of Manoa Source code for Beaglebone/RPi-based data loggers, shore internet gateways, and web server. Software dev

Stanley Lio 7 Dec 7, 2021
Python module for the qwiic serial control motor driver

Qwiic_SCMD_Py Python module for the qwiic motor driver This python package is a port of the existing SparkFun Serial Controlled Motor Driver Arduino L

SparkFun Electronics 6 Dec 6, 2022
Alternative firmware for ESP8266 with easy configuration using webUI, OTA updates, automation using timers or rules, expandability and entirely local control over MQTT, HTTP, Serial or KNX. Full documentation at

Alternative firmware for ESP8266/ESP32 based devices with easy configuration using webUI, OTA updates, automation using timers or rules, expandability

Theo Arends 59 Dec 26, 2022
A simple small scale electric car was build which can be driven by remote control and features a fully autonomous parking procedure.

personal-autonomous-parking-car-raspberry A simple electric car model was build using Raspbery pi. The car has remote control and autonomous operation

Kostas Ziovas 2 Jan 26, 2022