├── README.md ├── launch ├── euler.launch ├── scara.launch ├── ikkdl.launch ├── scara.urdf.xacro ├── euler.rviz ├── scara.rviz └── ikkdl.rviz ├── src ├── euler │ ├── main.cpp │ ├── QuaternionWidget.h │ ├── MainWindow.h │ ├── Interpolation.h │ ├── CMakeLists.txt │ ├── Interpolation.cpp │ ├── QuaternionWidget.cpp │ ├── EulerWidget.h │ ├── RotationControl.h │ ├── MainWindow.cpp │ ├── ui │ │ ├── quaternion.ui │ │ └── euler.ui │ ├── EulerWidget.cpp │ └── RotationControl.cpp ├── CMakeLists.txt └── ikkdl.cpp ├── notebook ├── fk.py ├── demo.launch ├── task5.4.ipynb ├── ns_demo.ipynb ├── demo.rviz ├── jsp_demo.ipynb ├── qt_controller.py ├── markers.py ├── robot_model.py └── controller.py ├── package.xml ├── CMakeLists.txt └── frames.rviz /README.md: -------------------------------------------------------------------------------- 1 | This repository comprises some example sources to illustrate fundamental techniques in ROS and robotics programming in general. Please follow the wiki for more details. 2 | -------------------------------------------------------------------------------- /launch/euler.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /src/euler/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "MainWindow.h" 3 | 4 | #include 5 | 6 | int main(int argc, char *argv[]) 7 | { 8 | ros::init(argc, argv, "euler"); 9 | 10 | QApplication app(argc, argv); 11 | MainWindow w; 12 | 13 | w.show(); 14 | return app.exec(); 15 | } 16 | -------------------------------------------------------------------------------- /launch/scara.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ## Specify additional locations of header files 2 | ## Your package locations should be listed before other locations 3 | include_directories(${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}) 4 | 5 | add_subdirectory(euler) 6 | 7 | ## Declare a C++ executable 8 | add_executable(ikkdl ikkdl.cpp) 9 | 10 | ## Add cmake target dependencies of the executable 11 | ## same as for the library above 12 | add_dependencies(ikkdl ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 13 | 14 | ## Specify libraries to link a library or executable target against 15 | target_link_libraries(ikkdl ${catkin_LIBRARIES}) 16 | -------------------------------------------------------------------------------- /src/euler/QuaternionWidget.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace Ui { 7 | class QuaternionWidget; 8 | } 9 | 10 | class QuaternionWidget : public QWidget 11 | { 12 | Q_OBJECT 13 | public: 14 | explicit QuaternionWidget(QWidget *parent = 0); 15 | 16 | const Eigen::Quaterniond& value() const; 17 | 18 | signals: 19 | /// quaternion value has changed 20 | void valueChanged(const Eigen::Quaterniond &q); 21 | 22 | public slots: 23 | void setValue(const Eigen::Quaterniond &q); 24 | 25 | private slots: 26 | void updateDisplay(); 27 | 28 | private: 29 | Eigen::Quaterniond _q; 30 | Ui::QuaternionWidget *_ui; 31 | }; 32 | -------------------------------------------------------------------------------- /src/euler/MainWindow.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | class RotationControl; 7 | 8 | class MainWindow : public QMainWindow 9 | { 10 | Q_OBJECT 11 | public: 12 | explicit MainWindow(QWidget *parent = 0); 13 | ~MainWindow(); 14 | 15 | signals: 16 | 17 | public slots: 18 | void updateFrames(); 19 | 20 | private: 21 | void setupUi(); 22 | 23 | private: 24 | boost::shared_ptr server; 25 | ros::AsyncSpinner spinner; 26 | 27 | RotationControl *frame1; 28 | RotationControl *frame2; 29 | RotationControl *frame1p2; 30 | RotationControl *frame1c2; 31 | }; 32 | -------------------------------------------------------------------------------- /src/euler/Interpolation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class Interpolation : public QTimer 8 | { 9 | Q_OBJECT 10 | public: 11 | Interpolation(const Eigen::Quaterniond &q1 = Eigen::Quaterniond::Identity(), 12 | const Eigen::Quaterniond &q2 = Eigen::Quaterniond::Identity(), 13 | QObject* parent = Q_NULLPTR); 14 | 15 | public slots: 16 | void setStart(const Eigen::Quaterniond &q1); 17 | void setEnd(const Eigen::Quaterniond &q2); 18 | 19 | signals: 20 | void valueChanged(const Eigen::Quaterniond &q); 21 | 22 | private slots: 23 | void step(); 24 | 25 | private: 26 | QMutex _mutex; 27 | Eigen::Quaterniond _q1, _q2; 28 | double _t; 29 | }; 30 | -------------------------------------------------------------------------------- /src/euler/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | QT5_WRAP_UI(UI_SOURCES 2 | ui/euler.ui 3 | ui/quaternion.ui 4 | ) 5 | 6 | ## Specify additional locations of header files 7 | ## Your package locations should be listed before other locations 8 | include_directories(${Qt5Widgets_INCLUDE_DIRS}) 9 | 10 | ## Declare a C++ executable 11 | add_executable(euler 12 | main.cpp 13 | MainWindow.cpp 14 | EulerWidget.cpp 15 | QuaternionWidget.cpp 16 | RotationControl.cpp 17 | Interpolation.cpp 18 | ${UI_SOURCES}) 19 | 20 | ## Add cmake target dependencies of the executable 21 | ## same as for the library above 22 | add_dependencies(euler ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 23 | 24 | ## Specify libraries to link a library or executable target against 25 | target_link_libraries(euler ${catkin_LIBRARIES} ${Qt5Widgets_LIBRARIES}) 26 | -------------------------------------------------------------------------------- /src/euler/Interpolation.cpp: -------------------------------------------------------------------------------- 1 | #include "Interpolation.h" 2 | #include 3 | 4 | Interpolation::Interpolation(const Eigen::Quaterniond &q1, 5 | const Eigen::Quaterniond &q2, 6 | QObject* parent) 7 | :_q1(q1), _q2(q2) 8 | { 9 | _t = 0.0; 10 | connect(this, SIGNAL(timeout()), this, SLOT(step())); 11 | } 12 | 13 | void Interpolation::setStart(const Eigen::Quaterniond &q) 14 | { 15 | QMutexLocker lock(&_mutex); 16 | _q1 = q; 17 | _t = 0; 18 | } 19 | 20 | void Interpolation::setEnd(const Eigen::Quaterniond &q) 21 | { 22 | QMutexLocker lock(&_mutex); 23 | _q2 = q; 24 | _t = 0; 25 | } 26 | 27 | void Interpolation::step() 28 | { 29 | QMutexLocker lock(&_mutex); 30 | _t += 0.05; 31 | if (_t > 1.) _t = -1.; 32 | emit valueChanged(_q1.slerp(std::fabs(_t), _q2)); 33 | } 34 | -------------------------------------------------------------------------------- /launch/ikkdl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /notebook/fk.py: -------------------------------------------------------------------------------- 1 | # Import rospy and initialize a python ROS node 2 | import rospy 3 | rospy.init_node('mynode') 4 | 5 | # Import and instantiate the robot model 6 | from robot_model import RobotModel 7 | robot = RobotModel() 8 | 9 | # Define a callback function to compute the forward-kinematics and publish a frame marker 10 | from sensor_msgs.msg import JointState 11 | from visualization_msgs.msg import MarkerArray 12 | from markers import frame 13 | marker_pub = rospy.Publisher('/marker_array', MarkerArray, queue_size=1) 14 | 15 | 16 | def publish_fk_marker(msg: JointState): 17 | T, _ = robot.fk(link='panda_link8', joints={j: v for j, v in zip(msg.name, msg.position)}) 18 | marker_pub.publish(frame(T)) 19 | 20 | 21 | # Subscribe to the /joint_states topic 22 | sub = rospy.Subscriber('/joint_states', JointState, publish_fk_marker) 23 | 24 | # Run ROS loop 25 | rospy.spin() 26 | -------------------------------------------------------------------------------- /src/euler/QuaternionWidget.cpp: -------------------------------------------------------------------------------- 1 | #include "QuaternionWidget.h" 2 | #include "ui_quaternion.h" 3 | 4 | QuaternionWidget::QuaternionWidget(QWidget *parent) : 5 | QWidget(parent), _ui(new Ui::QuaternionWidget) { 6 | qRegisterMetaType("Eigen::Quaterniond"); 7 | 8 | _ui->setupUi(this); 9 | _q = Eigen::Quaterniond::Identity(); 10 | updateDisplay(); 11 | } 12 | 13 | void QuaternionWidget::setValue(const Eigen::Quaterniond &q) { 14 | if (q.isApprox(_q)) return; 15 | _q = q; 16 | updateDisplay(); 17 | emit valueChanged(q); 18 | } 19 | 20 | const Eigen::Quaterniond &QuaternionWidget::value() const { 21 | return _q; 22 | } 23 | 24 | 25 | void QuaternionWidget::updateDisplay() { 26 | _ui->w->setText(QString::number(_q.w())); 27 | _ui->x->setText(QString::number(_q.x())); 28 | _ui->y->setText(QString::number(_q.y())); 29 | _ui->z->setText(QString::number(_q.z())); 30 | } 31 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lecture 4 | 0.1.0 5 | Example programs for my robotics lecture 6 | 7 | Robert Haschke 8 | Robert Haschke 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | eigen 15 | 16 | roscpp 17 | interactive_markers 18 | kdl_parser 19 | tf_conversions 20 | 21 | xacro 22 | rviz 23 | tf2_ros 24 | joint_state_publisher 25 | robot_state_publisher 26 | moveit_resources_panda_moveit_config 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/euler/EulerWidget.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace Ui { 7 | class EulerWidget; 8 | } 9 | 10 | class EulerWidget : public QWidget 11 | { 12 | Q_OBJECT 13 | public: 14 | enum Axis {X = 0, Y = 1, Z = 2}; 15 | 16 | explicit EulerWidget(QWidget *parent = 0); 17 | 18 | const Eigen::Quaterniond &value() const; 19 | 20 | /// retrieve indices of axes selected in GUI 21 | void getGuiAxes(uint a[3]) const; 22 | /// retrieve angles from GUI 23 | void getGuiAngles(double e[]) const; 24 | 25 | signals: 26 | /// quaternion value has changed 27 | void valueChanged(const Eigen::Quaterniond &q); 28 | /// euler axis selection changed 29 | void axesChanged(uint a1, uint a2, uint a3); 30 | 31 | public slots: 32 | void setValue(const Eigen::Quaterniond &q); 33 | void setEulerAngles(double e1, double e2, double e3, bool normalize); 34 | void setEulerAxes(uint a1, uint a2, uint a3); 35 | 36 | protected slots: 37 | void axisChanged(int axis); 38 | void angleChanged(); 39 | 40 | private slots: 41 | void updateAngles(); 42 | 43 | private: 44 | Eigen::Quaterniond _q; 45 | Ui::EulerWidget *_ui; 46 | }; 47 | -------------------------------------------------------------------------------- /notebook/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | [/target_joint_states] 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(lecture) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp interactive_markers kdl_parser tf_conversions) 5 | find_package(Qt5 REQUIRED COMPONENTS Widgets) 6 | find_package(Eigen3 REQUIRED) 7 | 8 | ## Uncomment this if the package has a setup.py. This macro ensures 9 | ## modules and global scripts declared therein get installed 10 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 11 | # catkin_python_setup() 12 | 13 | ################################### 14 | ## catkin specific configuration ## 15 | ################################### 16 | ## The catkin_package macro generates cmake config files for your package 17 | ## Declare things to be passed to dependent projects 18 | ## INCLUDE_DIRS: uncomment this if you package contains header files 19 | ## LIBRARIES: libraries you create in this project that dependent projects also need 20 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 21 | ## DEPENDS: system dependencies of this project that dependent projects also need 22 | catkin_package( 23 | # INCLUDE_DIRS include 24 | # LIBRARIES lecture 25 | CATKIN_DEPENDS roscpp 26 | # DEPENDS system_lib 27 | ) 28 | 29 | ## QT stuff 30 | # Find includes in corresponding build directories 31 | set(CMAKE_INCLUDE_CURRENT_DIR ON) 32 | # Instruct CMake to run moc automatically when needed. 33 | set(CMAKE_AUTOMOC ON) 34 | add_compile_options(${Qt5Widgets_EXECUTABLE_COMPILE_FLAGS}) 35 | 36 | add_subdirectory(src) 37 | -------------------------------------------------------------------------------- /src/euler/RotationControl.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | class QuaternionWidget; 9 | class EulerWidget; 10 | 11 | /** The RotationControl widget groups a QuaternionWidget and an EulerWidget. 12 | * It also creates an interactive marker to allow modifying the orientation in rviz. 13 | */ 14 | class RotationControl : public QGroupBox 15 | { 16 | Q_OBJECT 17 | public: 18 | explicit RotationControl(const std::string &_title, 19 | const Eigen::Vector3d &position, 20 | const QColor &color, 21 | boost::shared_ptr &_server, 22 | QWidget *parent = 0); 23 | 24 | /// get the current orientation as a quaternion 25 | const Eigen::Quaterniond& value() const; 26 | /// get the current orientation as Euler angles 27 | const Eigen::Vector3d eulerAngles() const; 28 | 29 | signals: // signals are emitted when some important event happens 30 | /// quaternion value has changed 31 | void valueChanged(const Eigen::Quaterniond &q); 32 | /// euler axis selection changed (x=0, y=1, z=2) 33 | void axesChanged(uint a1, uint a2, uint a3); 34 | 35 | public slots: // slots can connect to signals and thus get called when these are emitted 36 | /// set the orientation from the given quaternion 37 | void setValue(const Eigen::Quaterniond &q, bool update_server=true); 38 | /// set the orientation from the given Euler angles 39 | void setEulerAngles(double e1, double e2, double e3); 40 | /// specify a new set of Euler axes to be used (x=0, y=1, z=2) 41 | void setEulerAxes(uint a1, uint a2, uint a3); 42 | 43 | private: 44 | void setupUi(); 45 | void createInteractiveMarker(const Eigen::Vector3d &position, const QColor &color); 46 | void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr&); 47 | 48 | private: 49 | Eigen::Quaterniond _q; ///< current orientation value 50 | boost::shared_ptr _server; 51 | std::string _title; ///< title of the group box (and marker) 52 | geometry_msgs::Pose _pose; ///< pose of the marker, orientation kept in sync with _q 53 | 54 | QuaternionWidget *_qw; 55 | EulerWidget *_ew; 56 | }; 57 | -------------------------------------------------------------------------------- /notebook/task5.4.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "metadata": { 3 | "language_info": { 4 | "codemirror_mode": { 5 | "name": "ipython", 6 | "version": 3 7 | }, 8 | "file_extension": ".py", 9 | "mimetype": "text/x-python", 10 | "name": "python", 11 | "nbconvert_exporter": "python", 12 | "pygments_lexer": "ipython3", 13 | "version": "3.8.5" 14 | }, 15 | "orig_nbformat": 2, 16 | "kernelspec": { 17 | "name": "python385jvsc74a57bd031f2aee4e71d21fbe5cf8b01ff0e069b9275f58929596ceb00d14d90e3e16cd6", 18 | "display_name": "Python 3.8.5 64-bit ('usr')" 19 | }, 20 | "metadata": { 21 | "interpreter": { 22 | "hash": "31f2aee4e71d21fbe5cf8b01ff0e069b9275f58929596ceb00d14d90e3e16cd6" 23 | } 24 | } 25 | }, 26 | "nbformat": 4, 27 | "nbformat_minor": 2, 28 | "cells": [ 29 | { 30 | "cell_type": "code", 31 | "execution_count": null, 32 | "metadata": {}, 33 | "outputs": [], 34 | "source": [ 35 | "import rospy\n", 36 | "from visualization_msgs.msg import Marker, MarkerArray\n", 37 | "from std_msgs.msg import ColorRGBA\n", 38 | "from geometry_msgs.msg import Point, Quaternion\n", 39 | "from tf import transformations as tf\n", 40 | "import numpy as np\n", 41 | "\n", 42 | "rospy.init_node('circular')\n", 43 | "pub = rospy.Publisher('/marker_array', MarkerArray, queue_size=1)" 44 | ] 45 | }, 46 | { 47 | "cell_type": "code", 48 | "execution_count": null, 49 | "metadata": {}, 50 | "outputs": [], 51 | "source": [ 52 | "def point(theta, id, radius=1.0, scale=0.01, color=ColorRGBA(0,1,1,1), frame_id='world', ns='circle'):\n", 53 | " m = Marker()\n", 54 | " m.ns = ns\n", 55 | " m.type = Marker.SPHERE\n", 56 | " m.id = id\n", 57 | " m.scale.x = m.scale.y = m.scale.z = scale\n", 58 | " m.color = color\n", 59 | " m.header.frame_id = frame_id\n", 60 | " \n", 61 | " q = tf.quaternion_about_axis(angle=theta, axis=[0,0,1])\n", 62 | " m.pose.orientation = Quaternion(*q)\n", 63 | " m.pose.position = Point(*(tf.quaternion_matrix(q)[:3, :3].dot([1,0,0])))\n", 64 | " return m" 65 | ] 66 | }, 67 | { 68 | "cell_type": "code", 69 | "execution_count": null, 70 | "metadata": {}, 71 | "outputs": [], 72 | "source": [ 73 | "# create static array of points\n", 74 | "points = [point(theta, id=i+10) for i, theta in enumerate(np.arange(0,2*np.pi,0.05))]\n", 75 | "pub.publish(MarkerArray(points))" 76 | ] 77 | }, 78 | { 79 | "source": [ 80 | "# create moving point\n", 81 | "rate = rospy.Rate(10) # 10 Hz\n", 82 | "theta = 0\n", 83 | "color = ColorRGBA(1,0,0,1)\n", 84 | "while True:\n", 85 | " theta += 0.05\n", 86 | " pub.publish(MarkerArray([point(theta, id=9, scale=0.02, color=color, ns='point')]))\n", 87 | " rate.sleep()" 88 | ], 89 | "cell_type": "code", 90 | "metadata": {}, 91 | "execution_count": null, 92 | "outputs": [] 93 | } 94 | ] 95 | } -------------------------------------------------------------------------------- /src/euler/MainWindow.cpp: -------------------------------------------------------------------------------- 1 | // include headers declaring the used class interfaces 2 | #include "MainWindow.h" 3 | #include "RotationControl.h" 4 | #include "Interpolation.h" 5 | 6 | // these are system includes (from Qt, Eigen, ROS) 7 | #include 8 | #include 9 | #include 10 | 11 | MainWindow::MainWindow(QWidget *parent) : 12 | QMainWindow(parent), spinner(1) 13 | { 14 | server.reset(new interactive_markers::InteractiveMarkerServer("euler","",false)); 15 | setupUi(); 16 | 17 | server->applyChanges(); 18 | spinner.start(); 19 | } 20 | 21 | MainWindow::~MainWindow() 22 | { 23 | spinner.stop(); 24 | } 25 | 26 | // keep axes in sync between the two RotationControl widgets f1 and f2 27 | static void linkAxes(RotationControl *f1, RotationControl *f2) { 28 | // f1 -> f2 29 | QObject::connect(f1, SIGNAL(axesChanged(uint,uint,uint)), 30 | f2, SLOT(setEulerAxes(uint,uint,uint))); 31 | // f2 -> f1 32 | QObject::connect(f2, SIGNAL(axesChanged(uint,uint,uint)), 33 | f1, SLOT(setEulerAxes(uint,uint,uint))); 34 | } 35 | 36 | void MainWindow::setupUi() { 37 | QWidget *central = new QWidget(this); 38 | 39 | // create 4 RotationControl widgets 40 | double s = 0.5; 41 | QColor grey("grey"), red("red"), green("green"); 42 | frame1 = new RotationControl("frame 1", Eigen::Vector3d(-s,s,0), grey, server, this); 43 | frame2 = new RotationControl("frame 2", Eigen::Vector3d( s,s,0), grey, server, this); 44 | frame1p2 = new RotationControl("frame 1+2", Eigen::Vector3d(-s,-s,0), red, server, this); 45 | frame1c2 = new RotationControl("frame 1*2", Eigen::Vector3d( s,-s,0), green, server, this); 46 | 47 | // add those widgets to the vertical layout of the MainWindow's central widget 48 | QVBoxLayout *layout = new QVBoxLayout(central); 49 | layout->addWidget(frame1); 50 | layout->addWidget(frame2); 51 | layout->addWidget(frame1p2); 52 | layout->addWidget(frame1c2); 53 | 54 | this->setCentralWidget(central); 55 | 56 | // keep Euler axes in sync between all widgets 57 | linkAxes(frame1, frame2); 58 | linkAxes(frame1, frame1p2); 59 | linkAxes(frame1, frame1c2); 60 | 61 | frame1p2->setDisabled(true); 62 | frame1c2->setDisabled(true); 63 | connect(frame1, SIGNAL(valueChanged(Eigen::Quaterniond)), this, SLOT(updateFrames())); 64 | connect(frame2, SIGNAL(valueChanged(Eigen::Quaterniond)), this, SLOT(updateFrames())); 65 | 66 | RotationControl *frameI = new RotationControl("interpolated", Eigen::Vector3d(0,3*s,0), QColor("yellow"), server, this); 67 | layout->addWidget(frameI); frameI->setDisabled(true); 68 | Interpolation *timer = new Interpolation(frame1->value(), frame2->value(), this); 69 | connect(frame1, SIGNAL(valueChanged(Eigen::Quaterniond)), timer, SLOT(setStart(Eigen::Quaterniond))); 70 | connect(frame2, SIGNAL(valueChanged(Eigen::Quaterniond)), timer, SLOT(setEnd(Eigen::Quaterniond))); 71 | connect(timer, SIGNAL(valueChanged(Eigen::Quaterniond)), frameI, SLOT(setValue(Eigen::Quaterniond))); 72 | timer->start(100); 73 | } 74 | 75 | void MainWindow::updateFrames() 76 | { 77 | Eigen::Quaterniond q = frame2->value() * frame1->value(); 78 | frame1c2->setValue(q); 79 | 80 | Eigen::Vector3d e = frame1->eulerAngles() + frame2->eulerAngles(); 81 | frame1p2->setEulerAngles(e[0], e[1], e[2]); 82 | } 83 | -------------------------------------------------------------------------------- /src/euler/ui/quaternion.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | QuaternionWidget 4 | 5 | 6 | 7 | 0 8 | 0 9 | 307 10 | 35 11 | 12 | 13 | 14 | Form 15 | 16 | 17 | 18 | 0 19 | 20 | 21 | 0 22 | 23 | 24 | 0 25 | 26 | 27 | 0 28 | 29 | 30 | 31 | 32 | 33 | 0 34 | 0 35 | 36 | 37 | 38 | w: 39 | 40 | 41 | 42 | 43 | 44 | 45 | QFrame::StyledPanel 46 | 47 | 48 | QFrame::Sunken 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 0 60 | 0 61 | 62 | 63 | 64 | x: 65 | 66 | 67 | 68 | 69 | 70 | 71 | QFrame::StyledPanel 72 | 73 | 74 | QFrame::Sunken 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 0 86 | 0 87 | 88 | 89 | 90 | y: 91 | 92 | 93 | 94 | 95 | 96 | 97 | QFrame::StyledPanel 98 | 99 | 100 | QFrame::Sunken 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 0 112 | 0 113 | 114 | 115 | 116 | z: 117 | 118 | 119 | 120 | 121 | 122 | 123 | QFrame::StyledPanel 124 | 125 | 126 | QFrame::Sunken 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | -------------------------------------------------------------------------------- /launch/scara.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | -------------------------------------------------------------------------------- /src/euler/ui/euler.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | EulerWidget 4 | 5 | 6 | 7 | 0 8 | 0 9 | 345 10 | 27 11 | 12 | 13 | 14 | Form 15 | 16 | 17 | 18 | QLayout::SetDefaultConstraint 19 | 20 | 21 | 0 22 | 23 | 24 | 0 25 | 26 | 27 | 0 28 | 29 | 30 | 0 31 | 32 | 33 | 34 | 35 | 36 | 0 37 | 0 38 | 39 | 40 | 41 | 42 | x 43 | 44 | 45 | 46 | 47 | y 48 | 49 | 50 | 51 | 52 | z 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 1 61 | 62 | 63 | -360.000000000000000 64 | 65 | 66 | 360.000000000000000 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 0 75 | 0 76 | 77 | 78 | 79 | 80 | x 81 | 82 | 83 | 84 | 85 | y 86 | 87 | 88 | 89 | 90 | z 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 1 99 | 100 | 101 | -360.000000000000000 102 | 103 | 104 | 360.000000000000000 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 0 113 | 0 114 | 115 | 116 | 117 | 118 | x 119 | 120 | 121 | 122 | 123 | y 124 | 125 | 126 | 127 | 128 | z 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 1 137 | 138 | 139 | -360.000000000000000 140 | 141 | 142 | 360.000000000000000 143 | 144 | 145 | 146 | 147 | 148 | 149 | a1 150 | a2 151 | a3 152 | e1 153 | e2 154 | e3 155 | 156 | 157 | 158 | 159 | -------------------------------------------------------------------------------- /launch/euler.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 81 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 535 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.03 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Class: rviz/InteractiveMarkers 52 | Enable Transparency: true 53 | Enabled: true 54 | Name: InteractiveMarkers 55 | Show Axes: false 56 | Show Descriptions: true 57 | Show Visual Aids: false 58 | Update Topic: /euler/update 59 | Value: true 60 | Enabled: true 61 | Global Options: 62 | Background Color: 48; 48; 48 63 | Fixed Frame: world 64 | Frame Rate: 30 65 | Name: root 66 | Tools: 67 | - Class: rviz/Interact 68 | Hide Inactive Objects: true 69 | - Class: rviz/MoveCamera 70 | - Class: rviz/Select 71 | - Class: rviz/FocusCamera 72 | - Class: rviz/Measure 73 | - Class: rviz/SetInitialPose 74 | Topic: /initialpose 75 | - Class: rviz/SetGoal 76 | Topic: /move_base_simple/goal 77 | - Class: rviz/PublishPoint 78 | Single click: true 79 | Topic: /clicked_point 80 | Value: true 81 | Views: 82 | Current: 83 | Class: rviz/Orbit 84 | Distance: 10 85 | Enable Stereo Rendering: 86 | Stereo Eye Separation: 0.06 87 | Stereo Focal Distance: 1 88 | Swap Stereo Eyes: false 89 | Value: false 90 | Focal Point: 91 | X: 0 92 | Y: 0 93 | Z: 0 94 | Name: Current View 95 | Near Clip Distance: 0.01 96 | Pitch: 0.785398 97 | Target Frame: 98 | Value: Orbit (rviz) 99 | Yaw: 0.785398 100 | Saved: ~ 101 | Window Geometry: 102 | Displays: 103 | collapsed: false 104 | Height: 846 105 | Hide Left Dock: false 106 | Hide Right Dock: false 107 | QMainWindow State: 000000ff00000000fd00000004000000000000013c000002a9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000043000002a9000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000043000002a9000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000023a00fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002a900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 108 | Selection: 109 | collapsed: false 110 | Time: 111 | collapsed: false 112 | Tool Properties: 113 | collapsed: false 114 | Views: 115 | collapsed: false 116 | Width: 1200 117 | X: 60 118 | Y: 60 119 | -------------------------------------------------------------------------------- /src/euler/EulerWidget.cpp: -------------------------------------------------------------------------------- 1 | #include "EulerWidget.h" 2 | #include "ui_euler.h" 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | // ensure different axes for consecutive operations 9 | static void disableAxis(QComboBox *w, unsigned int axis) { 10 | const QStandardItemModel* model = qobject_cast(w->model()); 11 | for (unsigned int i=0; i < 3; ++i) { 12 | QStandardItem* item = model->item(i); 13 | if (i == axis) { 14 | item->setFlags(item->flags() & ~Qt::ItemIsEnabled); 15 | if (w->currentIndex() == static_cast(axis)) 16 | w->setCurrentIndex((axis + 1) % 3); 17 | } else { 18 | item->setFlags(item->flags() | Qt::ItemIsEnabled); 19 | } 20 | } 21 | } 22 | 23 | EulerWidget::EulerWidget(QWidget *parent) : 24 | QWidget(parent), _ui(new Ui::EulerWidget) 25 | { 26 | qRegisterMetaType("Eigen::Quaterniond"); 27 | 28 | _ui->setupUi(this); 29 | _ui->a1->setCurrentIndex(0); 30 | _ui->a2->setCurrentIndex(1); disableAxis(_ui->a2, 0); 31 | _ui->a3->setCurrentIndex(2); disableAxis(_ui->a3, 1); 32 | 33 | _q = Eigen::Quaterniond::Identity(); 34 | updateAngles(); 35 | 36 | // react to axis changes 37 | connect(_ui->a1, SIGNAL(currentIndexChanged(int)), 38 | this, SLOT(axisChanged(int))); 39 | connect(_ui->a2, SIGNAL(currentIndexChanged(int)), 40 | this, SLOT(axisChanged(int))); 41 | connect(_ui->a3, SIGNAL(currentIndexChanged(int)), 42 | this, SLOT(axisChanged(int))); 43 | 44 | // react to angle changes 45 | connect(_ui->e1, SIGNAL(valueChanged(double)), 46 | this, SLOT(angleChanged())); 47 | connect(_ui->e2, SIGNAL(valueChanged(double)), 48 | this, SLOT(angleChanged())); 49 | connect(_ui->e3, SIGNAL(valueChanged(double)), 50 | this, SLOT(angleChanged())); 51 | } 52 | 53 | void EulerWidget::getGuiAxes(uint a[]) const { 54 | a[0] = _ui->a1->currentIndex(); 55 | a[1] = _ui->a2->currentIndex(); 56 | a[2] = _ui->a3->currentIndex(); 57 | } 58 | 59 | void EulerWidget::getGuiAngles(double e[3]) const { 60 | e[0] = angles::from_degrees(_ui->e1->value()); 61 | e[1] = angles::from_degrees(_ui->e2->value()); 62 | e[2] = angles::from_degrees(_ui->e3->value()); 63 | } 64 | 65 | 66 | void EulerWidget::axisChanged(int axis) { 67 | bool bFirstCall = !this->signalsBlocked(); 68 | this->blockSignals(true); 69 | 70 | // ensure different axes for consecutive operations 71 | QComboBox* origin = dynamic_cast(sender()); 72 | if (origin == _ui->a1) disableAxis(_ui->a2, axis); 73 | if (origin == _ui->a2) disableAxis(_ui->a3, axis); 74 | 75 | if (bFirstCall) { 76 | updateAngles(); 77 | this->blockSignals(false); 78 | 79 | emit axesChanged(_ui->a1->currentIndex(), 80 | _ui->a2->currentIndex(), 81 | _ui->a3->currentIndex()); 82 | } 83 | } 84 | 85 | void EulerWidget::angleChanged() 86 | { 87 | double e[3]; getGuiAngles(e); 88 | setEulerAngles(e[0], e[1], e[2], false); 89 | } 90 | 91 | void EulerWidget::setEulerAngles(double e1, double e2, double e3, bool normalize) { 92 | uint a[3]; getGuiAxes(a); 93 | Eigen::Quaterniond q = 94 | Eigen::AngleAxisd(e1, Eigen::Vector3d::Unit(a[0])) * 95 | Eigen::AngleAxisd(e2, Eigen::Vector3d::Unit(a[1])) * 96 | Eigen::AngleAxisd(e3, Eigen::Vector3d::Unit(a[2])); 97 | if (normalize) 98 | setValue(q); 99 | else { 100 | // do not trigger angleChanged() again 101 | _ui->e1->blockSignals(true); 102 | _ui->e2->blockSignals(true); 103 | _ui->e3->blockSignals(true); 104 | 105 | _ui->e1->setValue(angles::to_degrees(e1)); 106 | _ui->e2->setValue(angles::to_degrees(e2)); 107 | _ui->e3->setValue(angles::to_degrees(e3)); 108 | 109 | _ui->e1->blockSignals(false); 110 | _ui->e2->blockSignals(false); 111 | _ui->e3->blockSignals(false); 112 | 113 | if (Eigen::internal::isApprox(std::abs(_q.dot(q)), 1.)) 114 | return; // don't care for q / -q 115 | _q = q; 116 | emit valueChanged(q); 117 | } 118 | } 119 | 120 | void EulerWidget::setEulerAxes(uint a1, uint a2, uint a3) 121 | { 122 | if (a1 > 2 || a2 > 2 || a3 > 2) 123 | return; 124 | if (static_cast(a1) == _ui->a1->currentIndex() && 125 | static_cast(a2) == _ui->a2->currentIndex() && 126 | static_cast(a3) == _ui->a3->currentIndex()) 127 | return; 128 | 129 | this->blockSignals(true); 130 | _ui->a3->setCurrentIndex(a3); 131 | _ui->a2->setCurrentIndex(a2); 132 | _ui->a1->setCurrentIndex(a1); 133 | this->blockSignals(false); 134 | updateAngles(); 135 | 136 | emit axesChanged(a1, a2, a3); 137 | } 138 | 139 | 140 | void EulerWidget::setValue(const Eigen::Quaterniond &q) { 141 | if (Eigen::internal::isApprox(std::abs(_q.dot(q)), 1.)) 142 | return; // don't care for q / -q 143 | 144 | _q = q; 145 | updateAngles(); 146 | emit valueChanged(q); 147 | } 148 | 149 | const Eigen::Quaterniond& EulerWidget::value() const { 150 | return _q; 151 | } 152 | 153 | 154 | void EulerWidget::updateAngles() { 155 | // ensure different axes for consecutive operations 156 | uint a[3]; getGuiAxes(a); 157 | Eigen::Vector3d e = _q.matrix().eulerAngles(a[0], a[1], a[2]); 158 | setEulerAngles(e[0], e[1], e[2], false); 159 | } 160 | -------------------------------------------------------------------------------- /notebook/ns_demo.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "### Launch the ROS demo" 8 | ] 9 | }, 10 | { 11 | "cell_type": "code", 12 | "execution_count": null, 13 | "metadata": {}, 14 | "outputs": [], 15 | "source": [ 16 | "%%bash --bg\n", 17 | "trap 'kill $(jobs -p)' EXIT\n", 18 | "xterm -e /bin/bash -l -c \"roslaunch demo.launch\"" 19 | ] 20 | }, 21 | { 22 | "cell_type": "markdown", 23 | "metadata": {}, 24 | "source": [ 25 | "Create the controller and run it periodically in a thread" 26 | ] 27 | }, 28 | { 29 | "cell_type": "code", 30 | "execution_count": null, 31 | "metadata": {}, 32 | "outputs": [], 33 | "source": [ 34 | "import rospy\n", 35 | "import numpy\n", 36 | "from threading import Thread\n", 37 | "from controller import Controller, iPoseMarker\n", 38 | "\n", 39 | "rospy.init_node('ns_demo')\n", 40 | "c = Controller()\n", 41 | "c.addMarker(iPoseMarker(c.T))" 42 | ] 43 | }, 44 | { 45 | "cell_type": "code", 46 | "execution_count": null, 47 | "metadata": {}, 48 | "outputs": [], 49 | "source": [ 50 | "class Worker(Thread):\n", 51 | " def __init__(self, *args, **kwargs): \n", 52 | " super(Worker, self).__init__(*args, **kwargs) \n", 53 | " self.stop = False\n", 54 | " self.rate = rospy.Rate(50)\n", 55 | " \n", 56 | " def run(self):\n", 57 | " c.reset() \n", 58 | " while not rospy.is_shutdown() and not self.stop:\n", 59 | " target = c.targets['pose']\n", 60 | " # position > orientation > joints\n", 61 | " q_delta = c.solve([c.position_task(target, c.T),\n", 62 | " c.orientation_task(target, c.T),\n", 63 | " c.joint_task()])\n", 64 | " # pose > joints\n", 65 | " #q_delta = self.solve([c.pose_task(target, c.T), c.joint_task()])\n", 66 | " c.actuate(q_delta)\n", 67 | "\n", 68 | " self.rate.sleep()\n", 69 | " \n", 70 | "try: # stop previous thread if existing\n", 71 | " t.stop = True\n", 72 | "except NameError:\n", 73 | " pass\n", 74 | "\n", 75 | "t = Worker()\n", 76 | "t.start()" 77 | ] 78 | }, 79 | { 80 | "cell_type": "markdown", 81 | "metadata": {}, 82 | "source": [ 83 | "Create a list of slider widgets, one for each joint, to chose the default pose" 84 | ] 85 | }, 86 | { 87 | "cell_type": "code", 88 | "execution_count": null, 89 | "metadata": {}, 90 | "outputs": [], 91 | "source": [ 92 | "import ipywidgets\n", 93 | "from ipywidgets import FloatSlider, Layout, Button, Box\n", 94 | "joint_widgets = [FloatSlider(min = j.min, max = j.max, step = (j.max-j.min) / 100, description = j.name) \\\n", 95 | " for j in c.robot.active_joints]\n", 96 | "joint_weights = [FloatSlider(min = 0, max = 1, step = 0.01, value = 1, description = j.name) \\\n", 97 | " for j in c.robot.active_joints]\n", 98 | "task_dimensions = ['x', 'y', 'z', 'rx', 'ry', 'rz']\n", 99 | "task_weights = [FloatSlider(min = 0, max = 1, step = 0.01, value = 1, description = name) \\\n", 100 | " for name in task_dimensions]" 101 | ] 102 | }, 103 | { 104 | "cell_type": "markdown", 105 | "metadata": {}, 106 | "source": [ 107 | "React to slider (value) changes by adapting the default joint pose" 108 | ] 109 | }, 110 | { 111 | "cell_type": "code", 112 | "execution_count": null, 113 | "metadata": {}, 114 | "outputs": [], 115 | "source": [ 116 | "def on_joint_preference(event):\n", 117 | " widget = event.owner\n", 118 | " c.preferred_joints[c.joint_msg.name.index(widget.description)] = widget.value\n", 119 | "\n", 120 | "for widget in joint_widgets:\n", 121 | " widget.observe(on_joint_preference, 'value')" 122 | ] 123 | }, 124 | { 125 | "cell_type": "code", 126 | "execution_count": null, 127 | "metadata": {}, 128 | "outputs": [], 129 | "source": [ 130 | "def on_joint_weight(event):\n", 131 | " widget = event.owner\n", 132 | " c.joint_weights[c.joint_msg.name.index(widget.description)] = widget.value\n", 133 | "\n", 134 | "for widget in joint_weights:\n", 135 | " widget.observe(on_joint_weight, 'value')" 136 | ] 137 | }, 138 | { 139 | "cell_type": "code", 140 | "execution_count": null, 141 | "metadata": {}, 142 | "outputs": [], 143 | "source": [ 144 | "def on_task_weight(event):\n", 145 | " widget = event.owner\n", 146 | " c.cartesian_weights[task_dimensions.index(widget.description)] = widget.value\n", 147 | "\n", 148 | "for widget in task_weights:\n", 149 | " widget.observe(on_task_weight, 'value')" 150 | ] 151 | }, 152 | { 153 | "cell_type": "markdown", 154 | "metadata": {}, 155 | "source": [ 156 | "Collect all widgets (sliders and buttons) in a form and display them" 157 | ] 158 | }, 159 | { 160 | "cell_type": "code", 161 | "execution_count": null, 162 | "metadata": { 163 | "scrolled": false 164 | }, 165 | "outputs": [], 166 | "source": [ 167 | "layout = Layout(display='flex', flex_flow='column', border='solid 2px', align_items='stretch')\n", 168 | "form = Box([Box(w, layout=layout) for w in [joint_widgets, joint_weights, task_weights]])\n", 169 | "display(form)" 170 | ] 171 | } 172 | ], 173 | "metadata": { 174 | "kernelspec": { 175 | "display_name": "Python 2", 176 | "language": "python", 177 | "name": "python2" 178 | }, 179 | "language_info": { 180 | "codemirror_mode": { 181 | "name": "ipython", 182 | "version": 2 183 | }, 184 | "file_extension": ".py", 185 | "mimetype": "text/x-python", 186 | "name": "python", 187 | "nbconvert_exporter": "python", 188 | "pygments_lexer": "ipython2", 189 | "version": "2.7.17" 190 | } 191 | }, 192 | "nbformat": 4, 193 | "nbformat_minor": 2 194 | } -------------------------------------------------------------------------------- /notebook/demo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Marker1 8 | - /Marker1/Status1 9 | - /InteractiveMarkers1 10 | Splitter Ratio: 0.566107988357544 11 | Tree Height: 770 12 | - Class: rviz/Help 13 | Name: Help 14 | - Class: rviz/Views 15 | Expanded: 16 | - /Current View1 17 | Name: Views 18 | Splitter Ratio: 0.5 19 | - Class: rviz/Selection 20 | Name: Selection 21 | Preferences: 22 | PromptSaveOnExit: true 23 | Toolbars: 24 | toolButtonStyle: 2 25 | Visualization Manager: 26 | Class: "" 27 | Displays: 28 | - Alpha: 0.5 29 | Cell Size: 1 30 | Class: rviz/Grid 31 | Color: 160; 160; 164 32 | Enabled: true 33 | Line Style: 34 | Line Width: 0.029999999329447746 35 | Value: Lines 36 | Name: Grid 37 | Normal Cell Count: 0 38 | Offset: 39 | X: 0 40 | Y: 0 41 | Z: 0 42 | Plane: XY 43 | Plane Cell Count: 10 44 | Reference Frame: 45 | Value: true 46 | - Alpha: 1 47 | Class: rviz/RobotModel 48 | Collision Enabled: false 49 | Enabled: true 50 | Links: 51 | All Links Enabled: true 52 | Expand Joint Details: false 53 | Expand Link Details: false 54 | Expand Tree: false 55 | Link Tree Style: Links in Alphabetic Order 56 | panda_hand: 57 | Alpha: 1 58 | Show Axes: false 59 | Show Trail: false 60 | Value: true 61 | panda_leftfinger: 62 | Alpha: 1 63 | Show Axes: false 64 | Show Trail: false 65 | Value: true 66 | panda_link0: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | Value: true 71 | panda_link1: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | Value: true 76 | panda_link2: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | Value: true 81 | panda_link3: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | Value: true 86 | panda_link4: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | Value: true 91 | panda_link5: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | Value: true 96 | panda_link6: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | Value: true 101 | panda_link7: 102 | Alpha: 1 103 | Show Axes: false 104 | Show Trail: false 105 | Value: true 106 | panda_link8: 107 | Alpha: 1 108 | Show Axes: false 109 | Show Trail: false 110 | panda_rightfinger: 111 | Alpha: 1 112 | Show Axes: false 113 | Show Trail: false 114 | Value: true 115 | Name: RobotModel 116 | Robot Description: robot_description 117 | TF Prefix: "" 118 | Update Interval: 0 119 | Value: true 120 | Visual Enabled: true 121 | - Class: rviz/Marker 122 | Enabled: true 123 | Marker Topic: marker 124 | Name: Marker 125 | Namespaces: 126 | trace: true 127 | Queue Size: 10 128 | Value: true 129 | - Class: rviz/InteractiveMarkers 130 | Enable Transparency: true 131 | Enabled: true 132 | Name: InteractiveMarkers 133 | Show Axes: false 134 | Show Descriptions: true 135 | Show Visual Aids: false 136 | Update Topic: /controller/update 137 | Value: true 138 | Enabled: true 139 | Global Options: 140 | Background Color: 48; 48; 48 141 | Default Light: true 142 | Fixed Frame: world 143 | Frame Rate: 30 144 | Name: root 145 | Tools: 146 | - Class: rviz/Interact 147 | Hide Inactive Objects: true 148 | - Class: rviz/MoveCamera 149 | - Class: rviz/Select 150 | - Class: rviz/Measure 151 | Value: true 152 | Views: 153 | Current: 154 | Class: rviz/XYOrbit 155 | Distance: 2.212043046951294 156 | Enable Stereo Rendering: 157 | Stereo Eye Separation: 0.05999999865889549 158 | Stereo Focal Distance: 1 159 | Swap Stereo Eyes: false 160 | Value: false 161 | Field of View: 0.7853981852531433 162 | Focal Point: 163 | X: 0.23371216654777527 164 | Y: -0.5176849365234375 165 | Z: -7.852847375033889e-06 166 | Focal Shape Fixed Size: true 167 | Focal Shape Size: 0.05000000074505806 168 | Invert Z Axis: false 169 | Name: Current View 170 | Near Clip Distance: 0.009999999776482582 171 | Pitch: 0.31479719281196594 172 | Target Frame: base_link 173 | Yaw: 1.4817930459976196 174 | Saved: ~ 175 | Window Geometry: 176 | Displays: 177 | collapsed: false 178 | Height: 921 179 | Help: 180 | collapsed: false 181 | Hide Left Dock: false 182 | Hide Right Dock: false 183 | QMainWindow State: 000000ff00000000fd0000000100000000000001ae0000033ffc0200000008fb000000100044006900730070006c006100790073010000003d0000033f000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a005600690065007700730000000321000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000257000001790000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000206000001cb0000000000000000fb0000001200530065006c0065006300740069006f006e0000000264000000810000005c00ffffff000004000000033f00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 184 | Selection: 185 | collapsed: false 186 | Views: 187 | collapsed: false 188 | Width: 1460 189 | X: 2069 190 | Y: 64 191 | -------------------------------------------------------------------------------- /launch/scara.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /RobotModel1 10 | - /TF1 11 | Splitter Ratio: 0.5 12 | Tree Height: 549 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 1 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 10 55 | Reference Frame: 56 | Value: true 57 | - Alpha: 1 58 | Class: rviz/RobotModel 59 | Collision Enabled: false 60 | Enabled: true 61 | Links: 62 | All Links Enabled: true 63 | Expand Joint Details: false 64 | Expand Link Details: false 65 | Expand Tree: false 66 | Link Tree Style: "" 67 | base_link: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | link1: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | Value: true 76 | link2: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | Value: true 81 | link3: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | Value: true 86 | link4: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | Value: true 91 | Name: RobotModel 92 | Robot Description: robot_description 93 | TF Prefix: "" 94 | Update Interval: 0 95 | Value: true 96 | Visual Enabled: true 97 | - Class: rviz/TF 98 | Enabled: false 99 | Frame Timeout: 15 100 | Frames: 101 | All Enabled: true 102 | Marker Alpha: 1 103 | Marker Scale: 0.5 104 | Name: TF 105 | Show Arrows: true 106 | Show Axes: true 107 | Show Names: true 108 | Tree: 109 | {} 110 | Update Interval: 0 111 | Value: false 112 | Enabled: true 113 | Global Options: 114 | Background Color: 48; 48; 48 115 | Default Light: true 116 | Fixed Frame: base_link 117 | Frame Rate: 30 118 | Name: root 119 | Tools: 120 | - Class: rviz/Interact 121 | Hide Inactive Objects: true 122 | - Class: rviz/MoveCamera 123 | - Class: rviz/Select 124 | - Class: rviz/FocusCamera 125 | - Class: rviz/Measure 126 | - Class: rviz/SetInitialPose 127 | Theta std deviation: 0.2617993950843811 128 | Topic: /initialpose 129 | X std deviation: 0.5 130 | Y std deviation: 0.5 131 | - Class: rviz/SetGoal 132 | Topic: /move_base_simple/goal 133 | - Class: rviz/PublishPoint 134 | Single click: true 135 | Topic: /clicked_point 136 | Value: true 137 | Views: 138 | Current: 139 | Class: rviz/Orbit 140 | Distance: 1.8336788415908813 141 | Enable Stereo Rendering: 142 | Stereo Eye Separation: 0.05999999865889549 143 | Stereo Focal Distance: 1 144 | Swap Stereo Eyes: false 145 | Value: false 146 | Field of View: 0.7853981852531433 147 | Focal Point: 148 | X: 0.4839913547039032 149 | Y: 0.09005998820066452 150 | Z: 0.22336982190608978 151 | Focal Shape Fixed Size: true 152 | Focal Shape Size: 0.05000000074505806 153 | Invert Z Axis: false 154 | Name: Current View 155 | Near Clip Distance: 0.009999999776482582 156 | Pitch: 0.31539851427078247 157 | Target Frame: 158 | Yaw: 4.8235764503479 159 | Saved: ~ 160 | Window Geometry: 161 | Displays: 162 | collapsed: false 163 | Height: 846 164 | Hide Left Dock: false 165 | Hide Right Dock: false 166 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025600fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 167 | Selection: 168 | collapsed: false 169 | Time: 170 | collapsed: false 171 | Tool Properties: 172 | collapsed: false 173 | Views: 174 | collapsed: false 175 | Width: 1200 176 | X: 1234 177 | Y: 25 178 | -------------------------------------------------------------------------------- /launch/ikkdl.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 81 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /RobotModel1 8 | - /RobotModel1/Links1 9 | Splitter Ratio: 0.5 10 | Tree Height: 535 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.03 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Alpha: 1 52 | Class: rviz/RobotModel 53 | Collision Enabled: false 54 | Enabled: true 55 | Links: 56 | All Links Enabled: true 57 | Expand Joint Details: false 58 | Expand Link Details: false 59 | Expand Tree: false 60 | Link Tree Style: Links in Alphabetic Order 61 | arm_1_link: 62 | Alpha: 1 63 | Show Axes: false 64 | Show Trail: false 65 | Value: true 66 | arm_2_link: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | Value: true 71 | arm_3_link: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | Value: true 76 | arm_4_link: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | Value: true 81 | arm_5_link: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | Value: true 86 | arm_6_link: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | Value: true 91 | arm_7_link: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | Value: true 96 | arm_base_link: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | Value: true 101 | tool_mount: 102 | Alpha: 1 103 | Show Axes: false 104 | Show Trail: false 105 | world: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | Name: RobotModel 110 | Robot Description: robot_description 111 | TF Prefix: "" 112 | Update Interval: 0 113 | Value: true 114 | Visual Enabled: true 115 | - Class: rviz/InteractiveMarkers 116 | Enable Transparency: true 117 | Enabled: true 118 | Name: InteractiveMarkers 119 | Show Axes: false 120 | Show Descriptions: true 121 | Show Visual Aids: false 122 | Update Topic: /ikkdl/update 123 | Value: true 124 | Enabled: true 125 | Global Options: 126 | Background Color: 48; 48; 48 127 | Fixed Frame: world 128 | Frame Rate: 30 129 | Name: root 130 | Tools: 131 | - Class: rviz/Interact 132 | Hide Inactive Objects: true 133 | - Class: rviz/MoveCamera 134 | - Class: rviz/Select 135 | - Class: rviz/FocusCamera 136 | - Class: rviz/Measure 137 | - Class: rviz/SetInitialPose 138 | Topic: /initialpose 139 | - Class: rviz/SetGoal 140 | Topic: /move_base_simple/goal 141 | - Class: rviz/PublishPoint 142 | Single click: true 143 | Topic: /clicked_point 144 | Value: true 145 | Views: 146 | Current: 147 | Class: rviz/Orbit 148 | Distance: 2.901 149 | Enable Stereo Rendering: 150 | Stereo Eye Separation: 0.06 151 | Stereo Focal Distance: 1 152 | Swap Stereo Eyes: false 153 | Value: false 154 | Focal Point: 155 | X: -0.431349 156 | Y: 0.242821 157 | Z: 0.937138 158 | Name: Current View 159 | Near Clip Distance: 0.01 160 | Pitch: 0.0303985 161 | Target Frame: 162 | Value: Orbit (rviz) 163 | Yaw: 5.75357 164 | Saved: ~ 165 | Window Geometry: 166 | Displays: 167 | collapsed: false 168 | Height: 846 169 | Hide Left Dock: false 170 | Hide Right Dock: false 171 | QMainWindow State: 000000ff00000000fd00000004000000000000013c000002a9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000043000002a9000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000002a9000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000023900fffffffb0000000800540069006d006501000000000000045000000000000000000000036e000002a900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 172 | Selection: 173 | collapsed: false 174 | Time: 175 | collapsed: false 176 | Tool Properties: 177 | collapsed: false 178 | Views: 179 | collapsed: false 180 | Width: 1200 181 | X: 551 182 | Y: 204 183 | -------------------------------------------------------------------------------- /notebook/jsp_demo.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "### Launch the ROS demo" 8 | ] 9 | }, 10 | { 11 | "cell_type": "code", 12 | "execution_count": null, 13 | "metadata": {}, 14 | "outputs": [ 15 | { 16 | "name": "stdout", 17 | "output_type": "stream", 18 | "text": [ 19 | "Starting job # 0 in a separate thread.\n" 20 | ] 21 | } 22 | ], 23 | "source": [ 24 | "%%bash --bg\n", 25 | "trap 'kill $(jobs -p)' EXIT\n", 26 | "xterm -e /bin/bash -l -c \"roslaunch demo.launch\"" 27 | ] 28 | }, 29 | { 30 | "cell_type": "markdown", 31 | "metadata": {}, 32 | "source": [ 33 | "### Python Code\n", 34 | "Import rospy and initialize a python ROS node" 35 | ] 36 | }, 37 | { 38 | "cell_type": "code", 39 | "execution_count": null, 40 | "metadata": {}, 41 | "outputs": [], 42 | "source": [ 43 | "import rospy\n", 44 | "rospy.init_node('mynode')" 45 | ] 46 | }, 47 | { 48 | "cell_type": "markdown", 49 | "metadata": {}, 50 | "source": [ 51 | "Import and instantiate the robot model" 52 | ] 53 | }, 54 | { 55 | "cell_type": "code", 56 | "execution_count": null, 57 | "metadata": {}, 58 | "outputs": [], 59 | "source": [ 60 | "from robot_model import RobotModel\n", 61 | "robot = RobotModel()" 62 | ] 63 | }, 64 | { 65 | "cell_type": "markdown", 66 | "metadata": {}, 67 | "source": [ 68 | "Create a publisher and `JointState` message" 69 | ] 70 | }, 71 | { 72 | "cell_type": "code", 73 | "execution_count": null, 74 | "metadata": {}, 75 | "outputs": [], 76 | "source": [ 77 | "from sensor_msgs.msg import JointState\n", 78 | "pub = rospy.Publisher('/target_joint_states', JointState, queue_size=1)\n", 79 | "msg = JointState()" 80 | ] 81 | }, 82 | { 83 | "cell_type": "markdown", 84 | "metadata": {}, 85 | "source": [ 86 | "Create a list of slider widgets, one for each joint" 87 | ] 88 | }, 89 | { 90 | "cell_type": "code", 91 | "execution_count": null, 92 | "metadata": {}, 93 | "outputs": [], 94 | "source": [ 95 | "import ipywidgets\n", 96 | "from ipywidgets import FloatSlider, Layout, Button, Box\n", 97 | "joint_widgets = [FloatSlider(min = j.min, max = j.max, step = (j.max-j.min) / 100, description = j.name) \\\n", 98 | " for j in robot.active_joints]" 99 | ] 100 | }, 101 | { 102 | "cell_type": "markdown", 103 | "metadata": {}, 104 | "source": [ 105 | "Define a callback function to compute the forward-kinematics and publish a frame marker" 106 | ] 107 | }, 108 | { 109 | "cell_type": "code", 110 | "execution_count": null, 111 | "metadata": {}, 112 | "outputs": [], 113 | "source": [ 114 | "from markers import frame, MarkerArray\n", 115 | "marker_pub = rospy.Publisher('/marker_array', MarkerArray, queue_size=1)\n", 116 | "\n", 117 | "def publish_fk_marker():\n", 118 | " T, _ = robot.fk(link='panda_link8', joints={j.description: j.value for j in joint_widgets})\n", 119 | " marker_pub.publish(frame(T))" 120 | ] 121 | }, 122 | { 123 | "cell_type": "markdown", 124 | "metadata": {}, 125 | "source": [ 126 | "React to slider (value) changes by publishing this particular joint as well as the updated FK marker" 127 | ] 128 | }, 129 | { 130 | "cell_type": "code", 131 | "execution_count": null, 132 | "metadata": {}, 133 | "outputs": [], 134 | "source": [ 135 | "def on_sent(event):\n", 136 | " widget = event.owner\n", 137 | " msg.name = [widget.description]\n", 138 | " msg.position = [widget.value]\n", 139 | " pub.publish(msg)\n", 140 | " publish_fk_marker()\n", 141 | "\n", 142 | "for widget in joint_widgets:\n", 143 | " widget.observe(on_sent, 'value')" 144 | ] 145 | }, 146 | { 147 | "cell_type": "markdown", 148 | "metadata": {}, 149 | "source": [ 150 | "Create a button to randomly generate a pose within joint limits" 151 | ] 152 | }, 153 | { 154 | "cell_type": "code", 155 | "execution_count": null, 156 | "metadata": {}, 157 | "outputs": [], 158 | "source": [ 159 | "def set_joints(values):\n", 160 | " for widget, value in values:\n", 161 | " widget.unobserve(on_sent, 'value')\n", 162 | " widget.value = value\n", 163 | " widget.observe(on_sent, 'value')\n", 164 | " msg.name, msg.position = zip(*[(widget.description, widget.value) for widget in joint_widgets])\n", 165 | " pub.publish(msg)\n", 166 | " publish_fk_marker()\n", 167 | "\n", 168 | "import random\n", 169 | "def on_randomize(randomize):\n", 170 | " set_joints([(widget, random.uniform(widget.min, widget.max)) for widget in joint_widgets])\n", 171 | "\n", 172 | "randomize = Button(description='Randomize') \n", 173 | "randomize.on_click(on_randomize)" 174 | ] 175 | }, 176 | { 177 | "cell_type": "markdown", 178 | "metadata": {}, 179 | "source": [ 180 | "Create a button to center all joints within their limits" 181 | ] 182 | }, 183 | { 184 | "cell_type": "code", 185 | "execution_count": null, 186 | "metadata": {}, 187 | "outputs": [], 188 | "source": [ 189 | "def on_center(b):\n", 190 | " set_joints([(widget, (widget.min + widget.max) / 2) for widget in joint_widgets])\n", 191 | "\n", 192 | "center = Button(description='Center')\n", 193 | "center.on_click(on_center)" 194 | ] 195 | }, 196 | { 197 | "cell_type": "markdown", 198 | "metadata": {}, 199 | "source": [ 200 | "Collect all widgets (sliders and buttons) in a form and display them" 201 | ] 202 | }, 203 | { 204 | "cell_type": "code", 205 | "execution_count": null, 206 | "metadata": { 207 | "scrolled": false 208 | }, 209 | "outputs": [ 210 | { 211 | "data": { 212 | "application/vnd.jupyter.widget-view+json": { 213 | "model_id": "5d1e77012eca487a9cac27244dba6401" 214 | } 215 | }, 216 | "metadata": {}, 217 | "output_type": "display_data" 218 | } 219 | ], 220 | "source": [ 221 | "form_items = list(joint_widgets)\n", 222 | "form_items += [Box([center, randomize])]\n", 223 | "\n", 224 | "form = Box(form_items, layout=Layout(\n", 225 | " display='flex',\n", 226 | " flex_flow='column',\n", 227 | " border='solid 2px',\n", 228 | " align_items='stretch',\n", 229 | " width='100%'\n", 230 | "))\n", 231 | "display(form)" 232 | ] 233 | }, 234 | { 235 | "cell_type": "code", 236 | "execution_count": null, 237 | "metadata": {}, 238 | "outputs": [], 239 | "source": [] 240 | } 241 | ], 242 | "metadata": { 243 | "kernelspec": { 244 | "display_name": "Python 2", 245 | "language": "python", 246 | "name": "python2" 247 | }, 248 | "language_info": { 249 | "codemirror_mode": { 250 | "name": "ipython", 251 | "version": 2 252 | }, 253 | "file_extension": ".py", 254 | "mimetype": "text/x-python", 255 | "name": "python", 256 | "nbconvert_exporter": "python", 257 | "pygments_lexer": "ipython2", 258 | "version": "2.7.15+" 259 | } 260 | }, 261 | "nbformat": 4, 262 | "nbformat_minor": 2 263 | } 264 | -------------------------------------------------------------------------------- /notebook/qt_controller.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import threading 3 | import signal 4 | import numpy 5 | import rospy 6 | from argparse import ArgumentParser 7 | from controller import Controller 8 | from robot_model import adjoint 9 | from markers import iPositionMarker, iPoseMarker, iPlaneMarker, iConeMarker, addMarker, processFeedback, sphere, box, plane, frame 10 | from interactive_markers.interactive_marker_server import InteractiveMarkerServer 11 | from geometry_msgs.msg import Vector3 12 | from tf import transformations as tf 13 | from visualization_msgs.msg import MarkerArray 14 | 15 | from python_qt_binding.QtCore import Qt 16 | from python_qt_binding.QtGui import QFontDatabase 17 | from python_qt_binding.QtWidgets import QApplication, QWidget, QSlider, QLabel, QVBoxLayout 18 | 19 | 20 | class Slider(QSlider): 21 | """QSlider variant that jumps back to zero when released""" 22 | 23 | def __init__(self): 24 | super(Slider, self).__init__(Qt.Horizontal) 25 | self.setRange(-1000, 1000) 26 | self.setValue(0) 27 | self.sliderReleased.connect(lambda: self.setValue(0)) 28 | 29 | 30 | class Gui(QWidget): 31 | 32 | def __init__(self, controller, mode): 33 | super(Gui, self).__init__() 34 | self.controller = controller 35 | self.mode = mode 36 | self.errors = QLabel() 37 | self.errors.setFont(QFontDatabase.systemFont(QFontDatabase.FixedFont)) 38 | self.sliders = [] 39 | 40 | layout = QVBoxLayout() 41 | layout.addWidget(QLabel('errors:')) 42 | layout.addWidget(self.errors) 43 | layout.addWidget(QLabel('nullspace control:')) 44 | for _ in range(controller.N): 45 | slider = Slider() 46 | self.sliders.append(slider) 47 | layout.addWidget(slider) 48 | self.setLayout(layout) 49 | 50 | def onValueChanged(self, index, value): 51 | self.ns[index] = value 52 | 53 | def showErrors(self, tasks): 54 | numpy.set_printoptions(precision=3, suppress=True) 55 | self.errors.setText('\n'.join([str(t[1]) for t in tasks])) 56 | 57 | def loop(self): 58 | rate = rospy.Rate(50) 59 | c = self.controller 60 | ims = InteractiveMarkerServer('controller') 61 | mode = self.mode 62 | if "pose" in mode: 63 | addMarker(ims, iPoseMarker(c.T), processFeedback(c.setTarget)) 64 | elif "plane" in mode: 65 | markers = [plane(), sphere()] 66 | addMarker(ims, iPlaneMarker(c.T[0:3, 3], markers=markers), 67 | processFeedback(c.setTarget)) 68 | elif "box" in mode: # constrain position to box 69 | tol = 0.5 * numpy.array([0.2, 0.1, 0.05]) # tolerance box 70 | addMarker(ims, iPositionMarker(c.T[0:3, 3], 71 | markers=[sphere(), box(size=Vector3(*(2.*tol)))]), 72 | processFeedback(c.setTarget)) 73 | if "cone" in mode: # constrain orientation to cone 74 | eef_axis = numpy.array([0, 1, 0]) # y-axis to align with cone axis 75 | T = c.T 76 | T[0:3, 3] = numpy.array([-0.25, 0, 0]) # place cone next to the robot 77 | # add cone marker aligned with current eef's y-axis 78 | cm = iConeMarker(ims, T.dot(tf.rotation_matrix(numpy.pi/2, [-1, 0, 0])), 79 | pose_cb=c.setTarget, angle_cb=c.setTarget) 80 | ims.applyChanges() 81 | 82 | ns_old = numpy.zeros((c.N, 0)) 83 | while not rospy.is_shutdown(): 84 | if "pose" in mode: # position + orientation control 85 | tasks = [c.pose_task(c.targets['pose'], c.T)] 86 | 87 | elif "plane" in mode: 88 | # move on a plane spanned by xy axes of marker 89 | T = c.targets['plane'] 90 | normal = T[0:3, 2] # plane normal from marker's z-axis 91 | tasks = [c.plane_task(normal, normal.dot(T[0:3, 3]), scale=0.1)] 92 | 93 | if "dist" in mode: 94 | tasks.append(c.distance_task(T, c.T, dist=0.2, scale=0.1)) # maintain distance 0.2 to center 95 | 96 | if "rotate" in mode: 97 | tasks.append( 98 | (normal.T.dot(c.J[3:]), 0.01) 99 | ) # rotates with constant speed about normal axis 100 | elif "rotateCenter" in mode: 101 | # rotate about center with linear velocity: w x r 102 | r = T[0:3, 3] - c.T[0:3, 3] 103 | tasks.append((c.J[:3], numpy.cross(0.1 * normal, r))) 104 | 105 | elif "box" in mode: # constrain position 106 | J, e = c.position_task(c.targets["pos"], c.T) 107 | # Constrain motion to stay within tolerance box: -tol < -e + J dq < tol 108 | lb, ub = (e-tol), (e+tol) 109 | tasks = [(J, ub, lb)] 110 | 111 | # keep gripper aligned with normal axis 112 | normal = numpy.array([0, 0, 1]) # world z-axis 113 | if "parallel" in mode: # using parallel_axes_task 114 | tasks.append(c.parallel_axes_task(numpy.array([0, 1, 0]), normal)) 115 | 116 | elif "cone" in mode: # using cone_task 117 | cone_pose = c.targets['cone_pose'] 118 | angle = c.targets['cone_angle'] 119 | tasks.append(c.cone_task(eef_axis, cone_pose[0:3, 2], threshold=numpy.cos(angle))) 120 | 121 | # publish orientation of eef_axis as 2nd cylinder of eef frame 122 | T = c.T 123 | T[:3, 3] = cone_pose[:3, 3] 124 | c.marker_pub.publish(MarkerArray(markers=frame(T, scale=0.2, radius=0.01, ns='eef orientation')[1:2])) 125 | 126 | self.showErrors(tasks) 127 | q_delta = c.solve_qp(tasks) 128 | 129 | # nullspace control 130 | N = c.nullspace.shape[1] # dimensionality of nullspace 131 | ns = numpy.array([s.value() for s in self.sliders[:N]]) # slider values 132 | # disable inactive nullspace sliders 133 | [s.setEnabled(i < N) for i, s in enumerate(self.sliders)] 134 | # align new nullspace basis to previous one (vectors might be flipped) 135 | N = min(N, ns_old.shape[1]) # minimum number of columns before and now 136 | c.nullspace[:, :N] *= numpy.sign(numpy.sum(c.nullspace[:, :N] * ns_old[:, :N], axis=0)) 137 | ns_old = c.nullspace 138 | # apply nullspace motion 139 | q_delta += c.nullspace.dot(1e-4 * ns) 140 | 141 | c.actuate(q_delta) 142 | rate.sleep() 143 | 144 | 145 | try: 146 | parser = ArgumentParser() 147 | parser.add_argument( 148 | "mode", 149 | type=str, 150 | nargs="*", 151 | default="pose", 152 | help="control mode", 153 | choices=[ 154 | "pose", 155 | "plane", 156 | "box", 157 | "dist", 158 | "rotate", 159 | "rotateCenter", 160 | "parallel", 161 | "cone", 162 | ], 163 | ) 164 | args = parser.parse_args() 165 | 166 | rospy.init_node('ik') 167 | app = QApplication(sys.argv) 168 | app.setApplicationDisplayName("Constrained-based Control Demo") 169 | gui = Gui(Controller(), args.mode) 170 | gui.show() 171 | threading.Thread(target=gui.loop).start() 172 | signal.signal(signal.SIGINT, signal.SIG_DFL) 173 | sys.exit(app.exec_()) 174 | except rospy.ROSInterruptException: 175 | pass 176 | -------------------------------------------------------------------------------- /src/euler/RotationControl.cpp: -------------------------------------------------------------------------------- 1 | #include "RotationControl.h" 2 | #include "QuaternionWidget.h" 3 | #include "EulerWidget.h" 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | // update pose's orientation from the given quaternion q 10 | // pose is passed by reference (&), so the changes are visible outside this function 11 | static void updatePose(geometry_msgs::Pose &pose, 12 | const Eigen::Quaterniond &q) 13 | { 14 | pose.orientation.w = q.w(); 15 | pose.orientation.x = q.x(); 16 | pose.orientation.y = q.y(); 17 | pose.orientation.z = q.z(); 18 | } 19 | 20 | RotationControl::RotationControl(const std::string &title, 21 | const Eigen::Vector3d &position, const QColor &color, 22 | boost::shared_ptr &server, 23 | QWidget *parent) : 24 | QGroupBox(QString::fromStdString(title), parent), _server(server), _title(title) 25 | { 26 | qRegisterMetaType("Eigen::Quaterniond"); 27 | 28 | setupUi(); 29 | createInteractiveMarker(position, color); 30 | } 31 | 32 | void RotationControl::setupUi() { 33 | _qw = new QuaternionWidget(this); 34 | _ew = new EulerWidget(this); 35 | 36 | QBoxLayout *layout = new QBoxLayout(QBoxLayout::TopToBottom, this); 37 | layout->addWidget(_qw); 38 | layout->addWidget(_ew); 39 | this->setLayout(layout); 40 | 41 | setValue(Eigen::Quaterniond::Identity()); 42 | 43 | // Keep orientations stored in EulerWidget/QuaternionWidget in sync with our own quaternion value 44 | // To this end, connect those widgets' valueChanged signals to our setValue slot 45 | connect(_qw, SIGNAL(valueChanged(Eigen::Quaterniond)), 46 | this, SLOT(setValue(Eigen::Quaterniond))); 47 | connect(_ew, SIGNAL(valueChanged(Eigen::Quaterniond)), 48 | this, SLOT(setValue(Eigen::Quaterniond))); 49 | 50 | // Forward the axesChanged signal from the EulerWidget 51 | connect(_ew, SIGNAL(axesChanged(uint,uint,uint)), 52 | this, SIGNAL(axesChanged(uint,uint,uint))); 53 | } 54 | 55 | 56 | const Eigen::Quaterniond &RotationControl::value() const { 57 | return _q; 58 | } 59 | 60 | void RotationControl::setValue(const Eigen::Quaterniond &q, bool update_server) { 61 | if (q.isApprox(_q)) 62 | return; // avoid infinite recursion 63 | _q = q; 64 | 65 | // update children widgets' orientation values, ignoring their valueChanged signals 66 | _qw->blockSignals(true); 67 | _ew->blockSignals(true); 68 | _qw->setValue(q); 69 | _ew->setValue(q); 70 | _qw->blockSignals(false); 71 | _ew->blockSignals(false); 72 | 73 | if (_server && update_server) { 74 | updatePose(_pose, q); 75 | _server->setPose(_title, _pose); 76 | _server->applyChanges(); 77 | } 78 | 79 | // emit valueChanged signal to inform listeners about the new value 80 | emit valueChanged(q); 81 | } 82 | 83 | 84 | void RotationControl::setEulerAxes(uint a1, uint a2, uint a3) { 85 | _ew->setEulerAxes(a1, a2, a3); // forward to EulerWidget 86 | } 87 | 88 | const Eigen::Vector3d RotationControl::eulerAngles() const { 89 | Eigen::Vector3d result; 90 | _ew->getGuiAngles(result.data()); 91 | return result; 92 | } 93 | 94 | void RotationControl::setEulerAngles(double e1, double e2, double e3) { 95 | _ew->setEulerAngles(e1, e2, e3, true); // forward to EulerWidget 96 | } 97 | 98 | /// create an interactive marker control allowing rotation only (with left mouse button) 99 | static visualization_msgs::InteractiveMarkerControl createViewPlaneControl() { 100 | visualization_msgs::InteractiveMarkerControl control; 101 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_3D; 102 | control.always_visible = true; 103 | control.name = "rotate"; // identify the control (for use in feedback callback) 104 | 105 | return control; 106 | } 107 | 108 | /// create a visual marker, here a box of given dimensions and color 109 | static visualization_msgs::Marker createBoxMarker(double x, double y, double z, 110 | const QColor &color) { 111 | visualization_msgs::Marker marker; 112 | 113 | marker.type = visualization_msgs::Marker::CUBE; // create a box 114 | marker.scale.x = x; // pass box dimensions via scale 115 | marker.scale.y = y; 116 | marker.scale.z = z; 117 | 118 | marker.color.r = color.redF(); // pass color components in range 0..1 119 | marker.color.g = color.greenF(); 120 | marker.color.b = color.blueF(); 121 | marker.color.a = color.alphaF(); 122 | 123 | return marker; 124 | } 125 | 126 | /// create a visual marker, here an arrow of given size (scale), direction, and color 127 | static visualization_msgs::Marker createArrowMarker(double scale, 128 | const Eigen::Vector3d &dir, 129 | const QColor &color) { 130 | visualization_msgs::Marker marker; 131 | 132 | marker.type = visualization_msgs::Marker::ARROW; 133 | marker.scale.x = scale; 134 | marker.scale.y = 0.1*scale; 135 | marker.scale.z = 0.1*scale; 136 | 137 | // set arrow's orientation from given direction 138 | updatePose(marker.pose, 139 | Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), dir)); 140 | 141 | marker.color.r = color.redF(); 142 | marker.color.g = color.greenF(); 143 | marker.color.b = color.blueF(); 144 | marker.color.a = color.alphaF(); 145 | 146 | return marker; 147 | } 148 | 149 | /// create an interactive marker located at given position 150 | void RotationControl::createInteractiveMarker(const Eigen::Vector3d &pos, 151 | const QColor &color) { 152 | if (!_server) 153 | return; // no server, no marker 154 | 155 | // initialize _pose from position and current quaternion 156 | _pose.position.x = pos[0]; 157 | _pose.position.y = pos[1]; 158 | _pose.position.z = pos[2]; 159 | updatePose(_pose, _q); 160 | 161 | // configure the interactive marker's pose, name, and size (scale) 162 | visualization_msgs::InteractiveMarker imarker; 163 | imarker.header.frame_id = "world"; 164 | imarker.header.stamp = ros::Time::now(); 165 | imarker.pose = _pose; 166 | imarker.name = _title; 167 | float s = imarker.scale = 0.2; 168 | 169 | // an interactive marker consists of one or more controls determining 170 | // the type of interaction (e.g. rotation, translation, etc.) 171 | visualization_msgs::InteractiveMarkerControl ctrl = createViewPlaneControl(); 172 | // each control can have multiple markers determining its visual appearance 173 | ctrl.markers.push_back(createBoxMarker(3*s, 2*s, 1*s, color)); 174 | // add an arrow for each axis 175 | ctrl.markers.push_back(createArrowMarker(3*s, Eigen::Vector3d::UnitX(), QColor("red"))); 176 | ctrl.markers.push_back(createArrowMarker(3*s, Eigen::Vector3d::UnitY(), QColor("green"))); 177 | ctrl.markers.push_back(createArrowMarker(3*s, Eigen::Vector3d::UnitZ(), QColor("blue"))); 178 | 179 | // add the control to the interactive marker 180 | imarker.controls.push_back(ctrl); 181 | // add the interactive marker to the server, registering processFeedback as the update callback 182 | _server->insert(imarker, boost::bind(&RotationControl::processFeedback, this, _1)); 183 | } 184 | 185 | /// callback for interactive marker updates, providing the new pose and other interaction info 186 | void RotationControl::processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) 187 | { 188 | const geometry_msgs::Quaternion &q = feedback->pose.orientation; 189 | setValue(Eigen::Quaterniond(q.w, q.x, q.y, q.z), false); 190 | } 191 | -------------------------------------------------------------------------------- /frames.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | - /TF1/Frames1 11 | - /TF1/Tree1 12 | - /TF1/Tree1/world1 13 | - /TF1/Tree1/world1/T11 14 | - /TF1/Tree1/world1/R21 15 | - /T11/publish transform1 16 | - /R11/publish transform1 17 | - /R21/publish transform1 18 | - /T21/publish transform1 19 | Splitter Ratio: 0.5 20 | Tree Height: 748 21 | - Class: rviz/Selection 22 | Name: Selection 23 | - Class: rviz/Tool Properties 24 | Expanded: 25 | - /2D Pose Estimate1 26 | - /2D Nav Goal1 27 | - /Publish Point1 28 | Name: Tool Properties 29 | Splitter Ratio: 0.5886790156364441 30 | - Class: rviz/Views 31 | Expanded: 32 | - /Current View1 33 | Name: Views 34 | Splitter Ratio: 0.5 35 | - Class: rviz/Time 36 | Experimental: false 37 | Name: Time 38 | SyncMode: 0 39 | SyncSource: "" 40 | Preferences: 41 | PromptSaveOnExit: true 42 | Toolbars: 43 | toolButtonStyle: 2 44 | Visualization Manager: 45 | Class: "" 46 | Displays: 47 | - Alpha: 0.5 48 | Cell Size: 1 49 | Class: rviz/Grid 50 | Color: 160; 160; 164 51 | Enabled: true 52 | Line Style: 53 | Line Width: 0.029999999329447746 54 | Value: Lines 55 | Name: Grid 56 | Normal Cell Count: 0 57 | Offset: 58 | X: 0 59 | Y: 0 60 | Z: 0 61 | Plane: XY 62 | Plane Cell Count: 10 63 | Reference Frame: 64 | Value: true 65 | - Class: rviz/TF 66 | Enabled: true 67 | Frame Timeout: 15 68 | Frames: 69 | All Enabled: false 70 | R1: 71 | Value: true 72 | R2: 73 | Value: false 74 | T1: 75 | Value: false 76 | T2: 77 | Value: true 78 | world: 79 | Value: true 80 | Marker Alpha: 1 81 | Marker Scale: 1 82 | Name: TF 83 | Show Arrows: false 84 | Show Axes: true 85 | Show Names: true 86 | Tree: 87 | world: 88 | R2: 89 | T2: 90 | {} 91 | T1: 92 | R1: 93 | {} 94 | Update Interval: 0 95 | Value: true 96 | - Class: agni_tf_tools/Static Transform Publisher 97 | Enabled: true 98 | Name: T1 99 | Value: true 100 | adapt transformation: false 101 | marker type: 102 | Value: none 103 | marker scale: 0.20000000298023224 104 | parent frame: world 105 | publish transform: 106 | Value: true 107 | child frame: T1 108 | rotation: 109 | axes: rpy 110 | e1: 0 111 | e2: 0 112 | e3: 0 113 | translation: 114 | X: 0 115 | Y: 0.5 116 | Z: 0 117 | - Class: agni_tf_tools/Static Transform Publisher 118 | Enabled: true 119 | Name: R1 120 | Value: true 121 | adapt transformation: false 122 | marker type: 123 | Value: none 124 | marker scale: 0.20000000298023224 125 | parent frame: T1 126 | publish transform: 127 | Value: true 128 | child frame: R1 129 | rotation: 130 | axes: xyz 131 | e1: 0 132 | e2: 0 133 | e3: -90 134 | translation: 135 | X: 0 136 | Y: 0 137 | Z: 0 138 | - Class: agni_tf_tools/Static Transform Publisher 139 | Enabled: true 140 | Name: R2 141 | Value: true 142 | adapt transformation: false 143 | marker type: 144 | Value: none 145 | marker scale: 0.20000000298023224 146 | parent frame: world 147 | publish transform: 148 | Value: true 149 | child frame: R2 150 | rotation: 151 | axes: xyz 152 | e1: 0 153 | e2: 0 154 | e3: -90 155 | translation: 156 | X: 0 157 | Y: 0 158 | Z: 0 159 | - Class: agni_tf_tools/Static Transform Publisher 160 | Enabled: true 161 | Name: T2 162 | Value: true 163 | adapt transformation: false 164 | marker type: 165 | Value: none 166 | marker scale: 0.20000000298023224 167 | parent frame: R2 168 | publish transform: 169 | Value: true 170 | child frame: T2 171 | rotation: 172 | axes: rpy 173 | e1: 0 174 | e2: 0 175 | e3: 0 176 | translation: 177 | X: 0 178 | Y: 0.5 179 | Z: 0 180 | Enabled: true 181 | Global Options: 182 | Background Color: 48; 48; 48 183 | Default Light: true 184 | Fixed Frame: world 185 | Frame Rate: 30 186 | Name: root 187 | Tools: 188 | - Class: rviz/Interact 189 | Hide Inactive Objects: true 190 | - Class: rviz/MoveCamera 191 | - Class: rviz/Select 192 | - Class: rviz/FocusCamera 193 | - Class: rviz/Measure 194 | - Class: rviz/SetInitialPose 195 | Theta std deviation: 0.2617993950843811 196 | Topic: /initialpose 197 | X std deviation: 0.5 198 | Y std deviation: 0.5 199 | - Class: rviz/SetGoal 200 | Topic: /move_base_simple/goal 201 | - Class: rviz/PublishPoint 202 | Single click: true 203 | Topic: /clicked_point 204 | Value: true 205 | Views: 206 | Current: 207 | Class: rviz/Orbit 208 | Distance: 1.9152312278747559 209 | Enable Stereo Rendering: 210 | Stereo Eye Separation: 0.05999999865889549 211 | Stereo Focal Distance: 1 212 | Swap Stereo Eyes: false 213 | Value: false 214 | Field of View: 0.7853981852531433 215 | Focal Point: 216 | X: 0.11608961224555969 217 | Y: 0.080673947930336 218 | Z: -0.13913275301456451 219 | Focal Shape Fixed Size: true 220 | Focal Shape Size: 0.05000000074505806 221 | Invert Z Axis: false 222 | Name: Current View 223 | Near Clip Distance: 0.009999999776482582 224 | Pitch: 0.785398006439209 225 | Target Frame: 226 | Yaw: 0.785398006439209 227 | Saved: ~ 228 | Window Geometry: 229 | Displays: 230 | collapsed: false 231 | Height: 1045 232 | Hide Left Dock: false 233 | Hide Right Dock: false 234 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000377fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000377000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025600fffffffb0000000800540069006d00650100000000000004500000000000000000000003540000037700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 235 | Selection: 236 | collapsed: false 237 | Time: 238 | collapsed: false 239 | Tool Properties: 240 | collapsed: false 241 | Views: 242 | collapsed: false 243 | Width: 1200 244 | X: 710 245 | Y: 25 246 | -------------------------------------------------------------------------------- /notebook/markers.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | from visualization_msgs.msg import Marker, InteractiveMarker, InteractiveMarkerControl 4 | from interactive_markers.interactive_marker_server import InteractiveMarkerFeedback 5 | from std_msgs.msg import Header, ColorRGBA 6 | from geometry_msgs.msg import Pose, Point, Quaternion, Vector3 7 | from tf import transformations as tf 8 | import numpy 9 | 10 | 11 | def sphere(radius=0.02, color=ColorRGBA(1, 0, 1, 0.5), **kwargs): 12 | """Create a sphere marker""" 13 | scale = Vector3(radius, radius, radius) 14 | return Marker(type=Marker.SPHERE, scale=scale, color=color, **kwargs) 15 | 16 | 17 | def cylinder(radius=0.02, len=0.1, color=ColorRGBA(1, 0, 0, 1), **kwargs): 18 | """Create a cylinder marker""" 19 | scale = Vector3(radius, radius, len) 20 | return Marker(type=Marker.CYLINDER, scale=scale, color=color, **kwargs) 21 | 22 | 23 | def box(size=Vector3(0.1, 0.1, 0.1), color=ColorRGBA(1, 1, 1, 0.5), **kwargs): 24 | """Create a box marker""" 25 | return Marker(type=Marker.CUBE, scale=size, color=color, **kwargs) 26 | 27 | 28 | def plane(size=1.0, color=ColorRGBA(1, 1, 1, 0.5), **kwargs): 29 | """Create a plane (a flat box)""" 30 | return box(size=Vector3(size, size, 1e-3), color=color, **kwargs) 31 | 32 | 33 | def cone(halfOpenAngle, scale=0.1, color=ColorRGBA(1, 0, 1, 0.5), **kwargs): 34 | twopi = numpy.pi * 2 35 | height = scale * numpy.cos(halfOpenAngle) 36 | radius = scale * numpy.sin(halfOpenAngle) 37 | points = [] 38 | numTriangles = 50 39 | for i in range(numTriangles): 40 | points.append(Point(0, 0, 0)) 41 | theta = twopi * i/numTriangles 42 | points.append(Point(radius * numpy.sin(theta), radius * numpy.cos(theta), height)) 43 | theta = twopi * (i+1)/numTriangles 44 | points.append(Point(radius * numpy.sin(theta), radius * numpy.cos(theta), height)) 45 | 46 | return Marker(type=Marker.TRIANGLE_LIST, points=points, color=color, scale=Vector3(1, 1, 1), **kwargs) 47 | 48 | 49 | def arrow(len=0.1, width=None, color=ColorRGBA(1, 0, 0, 1), **kwargs): 50 | """Create an arrow marker""" 51 | width = width or 0.1*len 52 | scale = Vector3(len, width, width) 53 | return Marker(type=Marker.ARROW, scale=scale, color=color, **kwargs) 54 | 55 | 56 | def frame(T, scale=0.1, radius=None, frame_id='world', ns='frame'): 57 | """Create a frame composed from three cylinders""" 58 | markers = [] 59 | p = T[0:3, 3] 60 | 61 | defaults = dict(header=Header(frame_id=frame_id), ns=ns) 62 | if radius is None: 63 | radius = scale / 10 64 | 65 | xaxis = tf.quaternion_about_axis(numpy.pi / 2., [0, 1, 0]) 66 | yaxis = tf.quaternion_about_axis(numpy.pi / 2., [-1, 0, 0]) 67 | offset = numpy.array([0, 0, scale / 2.]) 68 | 69 | m = cylinder(radius, scale, color=ColorRGBA(1, 0, 0, 1), id=0, **defaults) 70 | q = tf.quaternion_multiply(tf.quaternion_from_matrix(T), xaxis) 71 | m.pose.orientation = Quaternion(*q) 72 | m.pose.position = Point(*(p + tf.quaternion_matrix(q)[:3, :3].dot(offset))) 73 | markers.append(m) 74 | 75 | m = cylinder(radius, scale, color=ColorRGBA(0, 1, 0, 1), id=1, **defaults) 76 | q = tf.quaternion_multiply(tf.quaternion_from_matrix(T), yaxis) 77 | m.pose.orientation = Quaternion(*q) 78 | m.pose.position = Point(*(p + tf.quaternion_matrix(q)[:3, :3].dot(offset))) 79 | markers.append(m) 80 | 81 | m = cylinder(radius, scale, color=ColorRGBA(0, 0, 1, 1), id=2, **defaults) 82 | m.pose.orientation = Quaternion(*tf.quaternion_from_matrix(T)) 83 | m.pose.position = Point(*(p + T[:3, :3].dot(offset))) 84 | markers.append(m) 85 | return markers 86 | 87 | 88 | def add3DControls(im, markers, mode=InteractiveMarkerControl.MOVE_ROTATE_3D, **kwargs): 89 | # Create a control to move a (sphere) marker around with the mouse 90 | control = InteractiveMarkerControl(interaction_mode=mode, markers=markers, **kwargs) 91 | im.controls.append(control) 92 | 93 | 94 | def addArrowControls(im, dirs='xyz'): 95 | # Create arrow controls to move the marker 96 | for dir in dirs: 97 | control = InteractiveMarkerControl() 98 | control.name = 'move_' + dir 99 | control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS 100 | control.orientation.x = 1 if dir == 'x' else 0 101 | control.orientation.z = 1 if dir == 'y' else 0 102 | control.orientation.y = 1 if dir == 'z' else 0 103 | control.orientation.w = 1 104 | im.controls.append(control) 105 | 106 | 107 | def addOrientationControls(im, dirs='xyz'): 108 | # Create controls to rotate the marker 109 | for dir in dirs: 110 | control = InteractiveMarkerControl() 111 | control.name = 'rotate_' + dir 112 | control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS 113 | control.orientation.x = 1 if dir == 'x' else 0 114 | control.orientation.z = 1 if dir == 'y' else 0 115 | control.orientation.y = 1 if dir == 'z' else 0 116 | control.orientation.w = 1 117 | im.controls.append(control) 118 | 119 | 120 | def createPose(T): 121 | if T.shape != (4, 4): # if not 4x4 matrix: assume position vector 122 | Tnew = numpy.identity(4) 123 | Tnew[0:3, 3] = T 124 | T = Tnew 125 | return Pose(position=Point(*T[0:3, 3]), orientation=Quaternion(*tf.quaternion_from_matrix(T))) 126 | 127 | 128 | def addMarker(im_server, im, feedback_callback): 129 | # call feedback callback once to initialize target 130 | feedback_callback(InteractiveMarkerFeedback(marker_name=im.name, pose=im.pose)) 131 | im_server.insert(im, feedback_callback) 132 | 133 | 134 | def poseMsgToTM(pose): 135 | q = pose.orientation 136 | p = pose.position 137 | T = tf.quaternion_matrix(numpy.array([q.x, q.y, q.z, q.w])) 138 | T[0:3, 3] = numpy.array([p.x, p.y, p.z]) 139 | return T 140 | 141 | 142 | def processFeedback(pose_callback): 143 | def process_marker_feedback(feedback): 144 | pose_callback(feedback.marker_name, poseMsgToTM(feedback.pose)) 145 | return process_marker_feedback 146 | 147 | 148 | def iMarker(T, markers=[], name='pose', mode=InteractiveMarkerControl.MOVE_ROTATE_3D, **kwargs): 149 | im = InteractiveMarker(name=name, pose=createPose(T), **kwargs) 150 | im.header.frame_id = 'world' 151 | if markers: 152 | add3DControls(im, markers, mode=mode) 153 | return im 154 | 155 | 156 | def iPositionMarker(T, markers=[sphere()], name='pos', **kwargs): 157 | im = iMarker(T, markers, name, scale=0.2, description='Pos') 158 | addArrowControls(im) 159 | return im 160 | 161 | 162 | def iPoseMarker(T, markers=[sphere()], name='pose'): 163 | im = iMarker(T, markers, name, scale=0.2, description='Pose 6D') 164 | addArrowControls(im) 165 | addOrientationControls(im) 166 | return im 167 | 168 | 169 | def iPlaneMarker(pos, markers, name='plane', **kwargs): 170 | im = iMarker(pos, name=name, scale=0.2, description='Plane', **kwargs) 171 | if markers: 172 | add3DControls(im, markers) 173 | else: 174 | addArrowControls(im, dirs='z') 175 | addOrientationControls(im, dirs='xy') 176 | return im 177 | 178 | 179 | class iConeMarker: 180 | def __init__(self, ims, T, angle=.4, scale=.2, name='cone', 181 | mode=InteractiveMarkerControl.ROTATE_3D, pose_cb=None, angle_cb=None): 182 | self._server = ims 183 | self._angle = angle 184 | self._scale = scale 185 | self._name = name 186 | self.cone = iMarker(T, markers=[cone(angle, scale=scale)], name=name + '_pose', mode=mode) 187 | self.cone.controls[0].always_visible = True 188 | self.handle = iMarker(T, markers=[sphere(color=ColorRGBA(0,1,1,1))], name=name + '_angle', 189 | mode=InteractiveMarkerControl.MOVE_3D) 190 | 191 | self._pose_cb = pose_cb 192 | self._angle_cb = angle_cb 193 | if self._angle_cb: 194 | self._angle_cb(self.handle.name, self._angle) 195 | 196 | ims.insert(self.cone, self.process_pose) 197 | ims.insert(self.handle, self.process_angle) 198 | self._server.applyChanges() 199 | self.process_pose(InteractiveMarkerFeedback(marker_name=self.cone.name, pose=self.cone.pose)) 200 | 201 | def process_pose(self, feedback): 202 | T = poseMsgToTM(feedback.pose) 203 | if self._pose_cb: 204 | self._pose_cb(feedback.marker_name, T) 205 | handle_pose = tf.rotation_matrix(self._angle, [1, 0, 0]).dot(tf.translation_matrix([0, 0, self._scale])) 206 | self._server.setPose(self._name + '_angle', createPose(T.dot(handle_pose))) 207 | self._server.applyChanges() 208 | 209 | def process_angle(self, feedback): 210 | T_marker = poseMsgToTM(feedback.pose) 211 | T_cone = poseMsgToTM(self.cone.pose) 212 | v = T_marker[0:3, 3] - T_cone[0:3, 3] # vector from cone's origin to marker 213 | self._scale = numpy.linalg.norm(v) 214 | self._angle = numpy.arccos(numpy.maximum(0, T_cone[0:3,2].dot(v) / self._scale)) 215 | if self._angle_cb: 216 | self._angle_cb(feedback.marker_name, self._angle) 217 | self.cone.controls[0].markers[0].points = cone(self._angle, self._scale).points 218 | self._server.insert(self.cone) 219 | self._server.applyChanges() 220 | -------------------------------------------------------------------------------- /notebook/robot_model.py: -------------------------------------------------------------------------------- 1 | # python 2/3 compatibility: interpret print() as a function 2 | from __future__ import print_function 3 | 4 | import numpy 5 | import rospy 6 | import xml.dom.minidom 7 | from tf import transformations as tf 8 | 9 | 10 | def get_value(xml, child=None, attribute=None): 11 | """Get value of given attribute. If child arg is present, fetch first child of given name.""" 12 | if child is not None: 13 | xml = xml.getElementsByTagName(child)[0] 14 | if attribute is not None: 15 | return xml.getAttribute(attribute) 16 | 17 | 18 | def parse_vector(s, default=None): 19 | """Parse a string of shape "0.1 0.2 0.3" into a vector of floats""" 20 | if not s: 21 | return default 22 | return numpy.array([float(v) for v in s.split(" ")]) 23 | 24 | 25 | def hat(p): 26 | """Return skew-symmetric matrix for given vector p""" 27 | return numpy.array([[0, -p[2], p[1]], 28 | [p[2], 0, -p[0]], 29 | [-p[1], p[0], 0]]) 30 | 31 | 32 | def adjoint(T, inverse=False): 33 | """Return adjoint matrix for homogenous transform T (or its inverse).""" 34 | if T.shape == (4, 4): # input T is homogenous transform 35 | R = T[0:3, 0:3] 36 | p = T[0:3, 3] 37 | elif T.shape == (3, 3): # input T is rotation matrix 38 | R = T 39 | p = numpy.zeros(3) 40 | else: # input T is position vector 41 | R = numpy.identity(3) 42 | p = T 43 | if not inverse: 44 | return numpy.block([[R, hat(p).dot(R)], [numpy.zeros((3, 3)), R]]) 45 | else: 46 | return numpy.block([[R.T, R.T.dot(hat(-p))], [numpy.zeros((3, 3)), R.T]]) 47 | 48 | 49 | class Mimic: 50 | """Load (and represent) the tag of an URDF joint""" 51 | 52 | def __init__(self, tag): 53 | self.joint = tag.getAttribute("joint") 54 | if tag.hasAttribute("multiplier"): 55 | self.multiplier = float(tag.getAttribute("multiplier")) 56 | else: 57 | self.multiplier = 1.0 58 | if tag.hasAttribute("offset"): 59 | self.offset = float(tag.getAttribute("offset")) 60 | else: 61 | self.offset = 0.0 62 | 63 | 64 | class Joint: 65 | """Class representing a single URDF joint""" 66 | 67 | fixed = 0 68 | revolute = 1 69 | continuous = 1 70 | prismatic = 2 71 | floating = 3 72 | 73 | def __init__(self, arg): 74 | if isinstance(arg, xml.dom.minidom.Element): 75 | self._init_from_xml(arg) 76 | else: 77 | self._init_from_pose(arg) 78 | 79 | def _init_from_xml(self, tag): 80 | self.jtype = getattr(Joint, tag.getAttribute("type")) # map joint-type string onto enum 81 | self.active = self.jtype in [Joint.revolute, Joint.prismatic] # is the joint considered active? 82 | self.name = tag.getAttribute("name") 83 | self.parent = get_value(tag, "parent", "link") 84 | self.child = get_value(tag, "child", "link") 85 | self.T = tf.euler_matrix(*parse_vector(get_value(tag, "origin", "rpy"), default=[0, 0, 0]), axes="sxyz") 86 | self.T[0:3, 3] = parse_vector(get_value(tag, "origin", "xyz"), default=[0, 0, 0]) 87 | self.mimic = None 88 | if self.active: 89 | self.axis = parse_vector(get_value(tag, "axis", "xyz")) 90 | try: 91 | self.min = float(get_value(tag, "limit", "lower")) 92 | self.max = float(get_value(tag, "limit", "upper")) 93 | except IndexError: 94 | raise Exception("Joint %s has not limits" % self.name) 95 | 96 | mimic = tag.getElementsByTagName("mimic") 97 | self.mimic = Mimic(mimic[0]) if mimic else None 98 | 99 | def _init_from_pose(self, transform): 100 | self.jtype = Joint.fixed 101 | self.active = False 102 | self.name = transform.child_frame_id 103 | self.parent = transform.header.frame_id 104 | self.child = transform.child_frame_id 105 | q = transform.transform.rotation 106 | p = transform.transform.translation 107 | self.T = tf.quaternion_matrix(numpy.array([q.x, q.y, q.z, q.w])) 108 | self.T[0:3, 3] = numpy.array([p.x, p.y, p.z]) 109 | self.mimic = None 110 | 111 | 112 | class RobotModel: 113 | """Class representing the kinematic tree of a robot""" 114 | 115 | def __init__(self, param="robot_description"): 116 | self.links = {} # map link names to its parent joints 117 | self.joints = {} # map joint names to joint instances 118 | self.active_joints = [] # list of active, non-mimic joints 119 | 120 | description = rospy.get_param(param) # fetch URDF from ROS parameter server 121 | doc = xml.dom.minidom.parseString(description) # parse URDF string into dom 122 | robot = doc.getElementsByTagName("robot")[0] 123 | for tag in robot.getElementsByTagName("joint"): # process all tags 124 | self._add(Joint(tag)) # parse and add the joint to the kinematic tree 125 | 126 | def _add(self, joint): 127 | """Add a single joint to the kinematic tree""" 128 | self.joints[joint.name] = joint 129 | if joint.active and joint.mimic is None: 130 | self.active_joints.append(joint) 131 | if joint.mimic is not None: 132 | joint.mimic.joint = self.joints[joint.mimic.joint] # replace name with instance 133 | self.links[joint.child] = joint 134 | if joint.parent not in self.links: 135 | self.links[joint.parent] = None 136 | 137 | def fk(self, link, joints): 138 | """Compute forward kinematics up to given link using given map of joint angles""" 139 | def value(joint): 140 | """Get joint value from joints, considering mimic joints""" 141 | if joint.mimic is None: 142 | return joints[joint.name] 143 | return joint.mimic.multiplier * value(joint.mimic.joint) + joint.mimic.offset 144 | 145 | def index(joint): 146 | """Get joint index (into self.active_joint) and the velocity scaling factor""" 147 | if joint.mimic is None: 148 | return next((i for i, j in enumerate(self.active_joints) if j is joint), None), 1.0 149 | idx, scale = index(joint.mimic.joint) 150 | return idx, joint.mimic.multiplier * scale 151 | 152 | T = numpy.identity(4) 153 | J = numpy.zeros((6, len(self.active_joints))) 154 | joint = self.links[link] 155 | while joint is not None: 156 | T_offset = joint.T # fixed transform from parent to joint frame 157 | # post-multiply joint's motion transform (rotation / translation along joint axis) 158 | if joint.jtype == Joint.revolute: 159 | # transform twist from current joint frame (joint.axis) into eef frame (via T^-1) 160 | twist = adjoint(T, inverse=True).dot(numpy.block([numpy.zeros(3), joint.axis])) 161 | T_motion = tf.quaternion_matrix(tf.quaternion_about_axis(angle=value(joint), axis=joint.axis)) 162 | T_offset = T_offset.dot(T_motion) 163 | elif joint.jtype == Joint.prismatic: 164 | twist = adjoint(T, inverse=True).dot(numpy.block([joint.axis, numpy.zeros(3)])) 165 | T_motion = tf.translation_matrix(value(joint) * joint.axis) 166 | T_offset = T_offset.dot(T_motion) 167 | elif joint.jtype == Joint.fixed: 168 | pass # no action: fixed frames don't move 169 | else: 170 | raise Exception("unknown joint type: " + str(joint.jtype)) 171 | # pre-multiply joint transform with T (because traversing from eef to root) 172 | T = T_offset.dot(T) # T' = joint.T * T_motion * T 173 | 174 | # update the Jacobian 175 | idx, scale = index(joint) # find active joint index for given joint 176 | if idx is not None: # ignore fixed joints 177 | J[:, idx] += scale * twist # add twist contribution, optionally scaled by mimic joint's multiplier 178 | 179 | # climb upwards to parent joint 180 | joint = self.links[joint.parent] 181 | 182 | # As we climbed the kinematic tree from end-effector to base frame, we have 183 | # represented all Jacobian twists w.r.t. the end-effector frame. 184 | # Now transform all twists into the orientation of the base frame 185 | R = T[0:3, 0:3] 186 | Ad_R = numpy.block([[R, numpy.zeros((3, 3))], [numpy.zeros((3, 3)), R]]) 187 | return T, Ad_R.dot(J) 188 | 189 | 190 | # testing code executed when directly running this script 191 | if __name__ == "__main__": 192 | import random 193 | from markers import frame, MarkerArray 194 | from sensor_msgs.msg import JointState 195 | 196 | rospy.init_node("test_node") 197 | pub = rospy.Publisher("/target_joint_states", JointState, queue_size=10) 198 | marker_pub = rospy.Publisher("/marker_array", MarkerArray, queue_size=10) 199 | 200 | robot = RobotModel() 201 | while not rospy.is_shutdown(): 202 | joints = {j.name: random.uniform(j.min, j.max) for j in robot.active_joints} 203 | pub.publish(JointState(name=joints.keys(), position=joints.values())) 204 | 205 | T, J = robot.fk("panda_link8", joints) 206 | marker_pub.publish(frame(T)) 207 | 208 | rospy.rostime.wallsleep(1) 209 | -------------------------------------------------------------------------------- /src/ikkdl.cpp: -------------------------------------------------------------------------------- 1 | /* ============================================================ 2 | * 3 | * Copyright (C) 2015 by Robert Haschke 4 | * 5 | * This file may be licensed under the terms of the BSD license. 6 | * 7 | * The development of this software was supported by: 8 | * CITEC, "Cognitive Interaction Technology" Excellence Cluster 9 | * Bielefeld University, Germany 10 | * 11 | * ============================================================ */ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace vm = visualization_msgs; 30 | 31 | boost::shared_ptr server; 32 | vm::InteractiveMarkerFeedback marker_feedback; 33 | 34 | sensor_msgs::JointState init_message(const KDL::Chain &chain) { 35 | sensor_msgs::JointState msg; 36 | for (unsigned int i=0; i < chain.getNrOfSegments(); ++i) { 37 | KDL::Segment segment = chain.getSegment(i); 38 | KDL::Joint joint = segment.getJoint(); 39 | if (joint.getType() == KDL::Joint::None) continue; 40 | msg.name.push_back(joint.getName()); 41 | msg.position.push_back(0); 42 | } 43 | return msg; 44 | } 45 | 46 | void update_message(sensor_msgs::JointState &msg, 47 | const KDL::JntArray &joints) { 48 | msg.header.stamp = ros::Time::now(); 49 | Eigen::Map(msg.position.data(), msg.position.size()) = joints.data; 50 | } 51 | 52 | void processFeedback( const vm::InteractiveMarkerFeedbackConstPtr &feedback ) 53 | { 54 | marker_feedback = *feedback; 55 | } 56 | 57 | void operator<< (geometry_msgs::Point &pos, const Eigen::Vector3d &p) { 58 | pos.x = p[0]; 59 | pos.y = p[1]; 60 | pos.z = p[2]; 61 | } 62 | void operator<< (geometry_msgs::Quaternion &quat, const Eigen::Quaterniond &q) 63 | { 64 | quat.w = q.w(); 65 | quat.x = q.x(); 66 | quat.y = q.y(); 67 | quat.z = q.z(); 68 | } 69 | 70 | visualization_msgs::InteractiveMarker 71 | createInteractiveMarker(const std::string &name, 72 | const geometry_msgs::PoseStamped &stamped) 73 | { 74 | visualization_msgs::InteractiveMarker imarker; 75 | imarker.name = name; 76 | imarker.header = stamped.header; 77 | imarker.pose = stamped.pose; 78 | return imarker; 79 | } 80 | visualization_msgs::Marker 81 | createSphereMarker(double radius) 82 | { 83 | visualization_msgs::Marker marker; 84 | 85 | marker.type = visualization_msgs::Marker::SPHERE; 86 | marker.scale.x = radius * 2.0; 87 | marker.scale.y = radius * 2.0; 88 | marker.scale.z = radius * 2.0; 89 | 90 | return marker; 91 | } 92 | visualization_msgs::InteractiveMarkerControl 93 | createViewPlaneControl(bool position, bool orientation) 94 | { 95 | visualization_msgs::InteractiveMarkerControl control; 96 | control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING; 97 | 98 | if (position && orientation) { 99 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D; 100 | } else if (orientation) { 101 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_3D; 102 | } else 103 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_3D; 104 | 105 | control.independent_marker_orientation = true; 106 | control.always_visible = false; 107 | control.name = "move"; 108 | 109 | return control; 110 | } 111 | void addPositionControl(visualization_msgs::InteractiveMarker& imarker, 112 | const Eigen::Vector3d &axis, bool orientation_fixed) { 113 | visualization_msgs::InteractiveMarkerControl control; 114 | if (orientation_fixed) 115 | control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED; 116 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; 117 | 118 | control.orientation << Eigen::Quaterniond::FromTwoVectors( 119 | Eigen::Vector3d::UnitX(), axis); 120 | imarker.controls.push_back(control); 121 | } 122 | void addPositionControls(visualization_msgs::InteractiveMarker& imarker, 123 | unsigned int axes, bool orientation_fixed = false) 124 | { 125 | for (unsigned int i=0; i < 3; ++i) { 126 | if (!(axes & (1 << i))) continue; 127 | addPositionControl(imarker, Eigen::Vector3d::Unit(i), orientation_fixed); 128 | } 129 | } 130 | void addOrientationControl(visualization_msgs::InteractiveMarker& imarker, 131 | const Eigen::Vector3d &axis, bool orientation_fixed) { 132 | visualization_msgs::InteractiveMarkerControl control; 133 | if (orientation_fixed) 134 | control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED; 135 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; 136 | 137 | control.orientation << Eigen::Quaterniond::FromTwoVectors( 138 | Eigen::Vector3d::UnitX(), axis); 139 | imarker.controls.push_back(control); 140 | } 141 | void addOrientationControls(visualization_msgs::InteractiveMarker& imarker, 142 | unsigned int axes, bool orientation_fixed = false) 143 | { 144 | for (unsigned int i=0; i < 3; ++i) { 145 | if (!(axes & (1 << i))) continue; 146 | addOrientationControl(imarker, Eigen::Vector3d::Unit(i), orientation_fixed); 147 | } 148 | } 149 | 150 | enum AXES {X = 1, Y = 2, Z = 4, ALL = X | Y | Z}; 151 | void make6DofMarker(const geometry_msgs::PoseStamped &stamped, bool ok = true) 152 | { 153 | vm::InteractiveMarker imarker = createInteractiveMarker("6dof marker", stamped); 154 | double scale = imarker.scale = 0.2; 155 | 156 | visualization_msgs::InteractiveMarkerControl ctrl = createViewPlaneControl(true, true); 157 | visualization_msgs::Marker m = createSphereMarker(scale * 0.25); 158 | m.color.r = 0; m.color.g = 1; m.color.b = 1; m.color.a = 0.5; 159 | ctrl.markers.push_back(m); 160 | imarker.controls.push_back(ctrl); 161 | 162 | addPositionControls(imarker, AXES::ALL); 163 | addOrientationControls(imarker, AXES::ALL); 164 | 165 | server->clear(); 166 | server->insert(imarker, &processFeedback); 167 | server->applyChanges(); 168 | } 169 | 170 | 171 | static 172 | unsigned int computeDepthFromRoot(KDL::SegmentMap::const_iterator el, 173 | const KDL::SegmentMap::const_iterator &root) { 174 | unsigned int result = 0; 175 | while (el != root) { 176 | ++result; 177 | el = el->second.parent; 178 | } 179 | return result; 180 | } 181 | 182 | const std::string& findEndeffector(const KDL::Tree &tree) { 183 | const KDL::SegmentMap &segments = tree.getSegments(); 184 | unsigned int maxDepth = 0; 185 | KDL::SegmentMap::const_iterator eef = segments.end(); 186 | for (KDL::SegmentMap::const_iterator it = segments.begin(), end = segments.end(); 187 | it != end; ++it) { 188 | if (it->second.children.size() == 0) { 189 | unsigned int depth = computeDepthFromRoot(it, tree.getRootSegment()); 190 | if (depth > maxDepth || eef == segments.end()) { 191 | eef = it; 192 | maxDepth = depth; 193 | } 194 | } 195 | } 196 | return eef->first; 197 | } 198 | 199 | int main(int argc, char *argv[]) { 200 | std::string tip_frame; 201 | ros::init (argc, argv, "ikkdl"); 202 | ros::NodeHandle nh; 203 | 204 | // fetch KDL tree 205 | KDL::Tree kdl_tree; 206 | if (!kdl_parser::treeFromParam("robot_description", kdl_tree)) { 207 | ROS_ERROR("Could not initialize KDL tree"); 208 | return EXIT_FAILURE; 209 | } 210 | 211 | const std::string &eef = findEndeffector(kdl_tree); 212 | KDL::Chain kdl_chain; 213 | if (!kdl_tree.getChain(kdl_tree.getRootSegment()->first, eef, kdl_chain)) { 214 | ROS_ERROR_STREAM("Could not find chain to " << eef); 215 | return EXIT_FAILURE; 216 | } 217 | 218 | // joint state publisher 219 | auto jsp = nh.advertise("joint_states", 1); 220 | // init joint_state message 221 | auto js_msg = init_message(kdl_chain); 222 | 223 | // run interactive marker server 224 | server.reset(new interactive_markers::InteractiveMarkerServer("ikkdl","",false)); 225 | 226 | // initialize marker with end-effector pose from forward kinematics 227 | KDL::ChainFkSolverPos_recursive fk = KDL::ChainFkSolverPos_recursive(kdl_chain); 228 | KDL::JntArray kdl_joints = KDL::JntArray(kdl_chain.getNrOfJoints()); 229 | KDL::Frame kdl_pose; 230 | fk.JntToCart(kdl_joints, kdl_pose); 231 | 232 | tf::Pose tf_pose; 233 | tf::poseKDLToTF(kdl_pose, tf_pose); 234 | 235 | geometry_msgs::PoseStamped stamped; 236 | stamped.header.frame_id = kdl_tree.getRootSegment()->first; 237 | tf::poseTFToMsg(tf_pose, stamped.pose); 238 | 239 | make6DofMarker(stamped); 240 | 241 | // set initial pose 242 | marker_feedback.pose = stamped.pose; 243 | 244 | KDL::JntArray joints(kdl_chain.getNrOfJoints()); 245 | KDL::JntArray qDot(kdl_chain.getNrOfJoints()); 246 | KDL::Frame target; 247 | 248 | // KDL::ChainIkSolverVel_pinv ik(kdl_chain); 249 | KDL::JntArray relaxed_posture = KDL::JntArray(kdl_chain.getNrOfJoints()); 250 | KDL::ChainIkSolverVel_pinv_nso ik(kdl_chain,kdl_joints,relaxed_posture,0.0001); 251 | 252 | // run controller 253 | ros::Rate rate(50); // 50 hz update rate 254 | while (ros::ok()) { 255 | // TODO: compute twist that moves current end-effector pose into target pose 256 | tf::poseMsgToKDL(marker_feedback.pose,target); 257 | fk.JntToCart(joints, kdl_pose); 258 | KDL::Twist twist = 0.1 * KDL::diff(kdl_pose,target); 259 | 260 | // TODO: perform inverse velocity kinematics 261 | ik.CartToJnt(joints, twist, qDot); 262 | KDL::Add(joints, qDot, joints); 263 | 264 | // fill + publish ros joint_state message 265 | update_message(js_msg, joints); 266 | jsp.publish(js_msg); 267 | 268 | // process ros messages 269 | ros::spinOnce(); 270 | rate.sleep(); 271 | } 272 | 273 | server.reset(); 274 | 275 | return EXIT_SUCCESS; 276 | } 277 | -------------------------------------------------------------------------------- /notebook/controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import numpy 4 | import qpsolvers 5 | import rospy 6 | import random 7 | from std_msgs.msg import Header, ColorRGBA 8 | from sensor_msgs.msg import JointState 9 | from geometry_msgs.msg import TransformStamped, Transform, Pose, Quaternion, Vector3, Point 10 | from visualization_msgs.msg import Marker, MarkerArray 11 | from tf import transformations as tf 12 | from interactive_markers.interactive_marker_server import InteractiveMarkerServer 13 | from robot_model import RobotModel, Joint 14 | from markers import addMarker, processFeedback, iPoseMarker, frame 15 | 16 | 17 | def skew(w): 18 | return numpy.array([[0, -w[2], w[1]], 19 | [w[2], 0, -w[0]], 20 | [-w[1], w[0], 0]]) 21 | 22 | 23 | class Controller(object): 24 | damping = 0.1 25 | threshold = 1.0 26 | 27 | def __init__(self, pose=TransformStamped(header=Header(frame_id='panda_link8'), child_frame_id='target', 28 | transform=Transform(rotation=Quaternion(*tf.quaternion_about_axis(numpy.pi/4, [0, 0, 1])), 29 | translation=Vector3(0, 0, 0.105)))): 30 | 31 | self.robot = RobotModel() 32 | self.robot._add(Joint(pose)) # add a fixed end-effector transform 33 | self.joint_pub = rospy.Publisher('/target_joint_states', JointState, queue_size=10) 34 | self.joint_msg = JointState() 35 | self.joint_msg.name = [j.name for j in self.robot.active_joints] 36 | self.reset() 37 | self.target_link = pose.child_frame_id 38 | self.T, self.J = self.robot.fk(self.target_link, dict(zip(self.joint_msg.name, self.joint_msg.position))) 39 | self.N = self.J.shape[1] # number of (active) joints 40 | self.preferred_joints = self.joint_msg.position.copy() 41 | self.joint_weights = numpy.ones(self.N) 42 | self.cartesian_weights = numpy.ones(6) 43 | self.mins = numpy.array([j.min for j in self.robot.active_joints]) 44 | self.maxs = numpy.array([j.max for j in self.robot.active_joints]) 45 | self.prismatic = numpy.array([j.jtype == j.prismatic for j in self.robot.active_joints]) 46 | 47 | # prepare publishing eef trace 48 | self.trace_marker = Marker(type=Marker.LINE_STRIP, header=Header(frame_id='world'), 49 | ns='trace', color=ColorRGBA(0, 1, 1, 0.5)) 50 | self.trace_marker.pose.orientation.w = 1 51 | self.trace_marker.scale.x = 0.01 # line width 52 | self.marker_pub = rospy.Publisher('/marker_array', MarkerArray, queue_size=10) 53 | 54 | self.targets = dict() 55 | 56 | def setTarget(self, name, goal): 57 | self.targets[name] = goal 58 | 59 | def reset(self): 60 | self.joint_msg.position = numpy.asarray( 61 | [(j.min+j.max)/2 + 0.1*(j.max-j.min)*random.uniform(0, 1) for j in self.robot.active_joints]) 62 | 63 | def actuate(self, q_delta): 64 | """Move robot by given changes to joint angles""" 65 | self.joint_msg.position += q_delta.ravel() # add (numpy) vector q_delta to current joint position vector 66 | # clip (prismatic) joints 67 | self.joint_msg.position[self.prismatic] = numpy.clip(self.joint_msg.position[self.prismatic], 68 | self.mins[self.prismatic], self.maxs[self.prismatic]) 69 | self.joint_pub.publish(self.joint_msg) # publish new joint state 70 | joints = dict(zip(self.joint_msg.name, self.joint_msg.position)) # turn list of names and joint values into map 71 | self.T, self.J = self.robot.fk(self.target_link, joints) # compute new forward kinematics and Jacobian 72 | 73 | # publish eef marker 74 | msg = MarkerArray(markers=frame(self.T, scale=0.05, ns='eef frame')) 75 | trace = self.trace_marker.points 76 | trace.append(Point(*self.T[0:3, 3])) 77 | if (len(trace) > 1000): 78 | del trace[0] 79 | msg.markers.append(self.trace_marker) 80 | self.marker_pub.publish(msg) 81 | 82 | def solve(self, tasks): 83 | """Hierarchically solve tasks of the form J dq = e""" 84 | def invert_clip(s): 85 | return 1./s if s > self.threshold else 0. 86 | 87 | def invert_damp(s): 88 | return s/(s**2 + self.damping**2) 89 | 90 | def invert_smooth_clip(s): 91 | return s/(self.threshold**2) if s < self.threshold else 1./s 92 | 93 | N = numpy.identity(self.N) # nullspace projector of previous tasks 94 | JA = numpy.zeros((0, self.N)) # accumulated Jacobians 95 | qdot = numpy.zeros(self.N) 96 | 97 | if isinstance(tasks, tuple): 98 | tasks = [tasks] 99 | 100 | for J, e in tasks: 101 | U, S, Vt = numpy.linalg.svd(J.dot(N) * self.joint_weights[None, :]) 102 | # compute V'.T = V.T * Mq.T 103 | Vt *= self.joint_weights[None, :] 104 | 105 | rank = min(U.shape[0], Vt.shape[1]) 106 | for i in range(rank): 107 | S[i] = invert_smooth_clip(S[i]) 108 | 109 | qdot += numpy.dot(Vt.T[:, 0:rank], S * U.T.dot(numpy.array(e) - J.dot(qdot))).reshape(qdot.shape) 110 | 111 | # compute new nullspace projector 112 | JA = numpy.vstack([JA, J]) 113 | U, S, Vt = numpy.linalg.svd(JA) 114 | accepted_singular_values = (S > 1e-3).sum() 115 | VN = Vt[accepted_singular_values:].T 116 | N = VN.dot(VN.T) 117 | self.nullspace = VN # remember nullspace basis 118 | return qdot 119 | 120 | def solve_qp(self, tasks): 121 | """Solve tasks (J, ub, lb) of the form lb ≤ J dq ≤ ub 122 | using quadratic optimization: https://pypi.org/project/qpsolvers""" 123 | maxM = numpy.amax([task[0].shape[0] for task in tasks]) # max task dimension 124 | sumM = numpy.sum([task[0].shape[0] for task in tasks]) # sum of all task dimensions 125 | usedM = 0 126 | # allocate arrays once 127 | G, h = numpy.zeros((2*sumM, self.N + maxM)), numpy.zeros(2*sumM) 128 | P = numpy.identity(self.N+maxM) 129 | P[self.N:, self.N:] *= 1.0 # use different scaling for slack variables? 130 | q = numpy.zeros(self.N + maxM) 131 | 132 | # joint velocity bounds + slack bounds 133 | upper = numpy.hstack([numpy.minimum(0.1, self.maxs - self.joint_msg.position), numpy.zeros(maxM)]) 134 | lower = numpy.hstack([numpy.maximum(-0.1, self.mins - self.joint_msg.position), numpy.full(maxM, -numpy.infty)]) 135 | 136 | # fallback solution 137 | dq = numpy.zeros(self.N) 138 | 139 | def add_constraint(A, bound): 140 | G[usedM:usedM+M, :N] = A 141 | G[usedM:usedM+M, N:N+M] = numpy.identity(M) # allow (negative) slack variables 142 | h[usedM:usedM+M] = bound 143 | return usedM + M 144 | 145 | for idx, task in enumerate(tasks): 146 | try: # inequality tasks are pairs of (J, ub, lb=None) 147 | J, ub, lb = task 148 | except ValueError: # equality tasks are pairs of (J, err) 149 | J, ub = task 150 | lb = ub # turn into inequality task: err ≤ J dq ≤ err 151 | J = numpy.atleast_2d(J) 152 | M, N = J.shape 153 | 154 | # augment G, h with current task's constraints 155 | oldM = usedM 156 | usedM = add_constraint(J, ub) 157 | if lb is not None: 158 | usedM = add_constraint(-J, -lb) 159 | 160 | result = qpsolvers.solve_qp(P=P[:N+M, :N+M], q=q[:N+M], 161 | G=G[:usedM, :N+M], h=h[:usedM], A=None, b=None, 162 | lb=lower[:N+M], ub=upper[:N+M], 163 | solver='osqp') 164 | if result is None: 165 | print("{}: failed ".format(idx), end='') 166 | usedM = oldM # ignore subtask and continue with subsequent tasks 167 | else: # adapt added constraints for next iteration 168 | dq, slacks = result[:N], result[N:] 169 | print("{}:".format(idx), slacks, " ", end='') 170 | G[oldM:usedM,N:N+M] = 0 171 | h[oldM:oldM+M] += slacks 172 | if oldM+M < usedM: 173 | h[oldM+M:usedM] -= slacks 174 | print() 175 | self.nullspace = numpy.zeros((self.N, 0)) 176 | return dq 177 | 178 | @staticmethod 179 | def vstack(items): 180 | return numpy.vstack(items) if items else None 181 | 182 | @staticmethod 183 | def hstack(items): 184 | return numpy.hstack(items) if items else None 185 | 186 | @staticmethod 187 | def stack(tasks): 188 | """Combine all tasks by stacking them into a single Jacobian""" 189 | Js, errs = zip(*tasks) 190 | return numpy.vstack(Js), numpy.hstack(errs) 191 | 192 | def position_task(self, T_tgt, T_cur, scale=1.0): 193 | """Move eef towards a specific target point in base frame""" 194 | return self.J[:3], scale*(T_tgt[0:3, 3]-T_cur[0:3, 3]) 195 | 196 | def orientation_task(self, T_tgt, T_cur, scale=1.0): 197 | """Move eef into a specific target orientation in base frame""" 198 | delta = numpy.identity(4) 199 | delta[0:3, 0:3] = T_cur[0:3, 0:3].T.dot(T_tgt[0:3, 0:3]) 200 | angle, axis, _ = tf.rotation_from_matrix(delta) 201 | # transform rotational velocity from end-effector into base frame orientation (only R!) 202 | return self.J[3:], scale*(T_cur[0:3, 0:3].dot(angle * axis)) 203 | 204 | def pose_task(self, T_tgt, T_cur, scale=(1.0, 1.0)): 205 | """Perform position and orientation task with same priority""" 206 | return self.stack([self.position_task(T_tgt, T_cur, scale=scale[0]), 207 | self.orientation_task(T_tgt, T_cur, scale=scale[1])]) 208 | 209 | def joint_task(self, scale=0.1): 210 | """Move towards a set of preferred joints, consumes all DOFs""" 211 | qerr = self.joint_msg.position - self.preferred_joints 212 | J = numpy.identity(self.J.shape[1]) 213 | return J, -scale*qerr 214 | 215 | def distance_task(self, T_tgt, T_cur, dist=0, scale=1.0): 216 | """Keep distance to target position, not considering (approach) direction""" 217 | delta = T_cur[0:3, 3] - T_tgt[0:3, 3] 218 | return delta.T.dot(self.J[:3]), -scale * (numpy.linalg.norm(delta) - dist) 219 | 220 | def plane_task(self, normal, dist, scale=1.0): 221 | """Move eef within plane given by normal vector and distance to origin""" 222 | return normal.T.dot(self.J[:3]), -scale * (normal.dot(self.T[0:3, 3]) - dist) 223 | 224 | def parallel_axes_task(self, axis, reference, scale=1.0): 225 | """Align axis in eef frame to be parallel to reference axis in base frame""" 226 | axis = self.T[0:3, 0:3].dot(axis) # transform axis from eef frame to base frame 227 | return (skew(reference).dot(skew(axis))).dot(self.J[3:]), scale * numpy.cross(reference, axis) 228 | 229 | def cone_task(self, axis, reference, threshold): 230 | """Align axis in eef frame to lie in cone spanned by reference axis and opening angle acos(threshold)""" 231 | axis = self.T[0:3, 0:3].dot(axis) # transform axis from eef frame to base frame 232 | return reference.T.dot(skew(axis)).dot(self.J[3:]), (reference.T.dot(axis) - threshold), None 233 | 234 | def position_control(self, target): 235 | q_delta = self.solve(self.position_task(target, self.T)) 236 | self.actuate(q_delta) 237 | 238 | def pose_control(self, target): 239 | q_delta = self.solve(self.pose_task(target, self.T)) 240 | self.actuate(q_delta) 241 | 242 | def hierarchic_control(self, target): 243 | # position control > orientation control > joints 244 | q_delta = self.solve([self.position_task(target, self.T), 245 | self.orientation_task(target, self.T), 246 | self.joint_task()]) 247 | # pose > joints 248 | #q_delta = self.solve([self.pose_task(target, self.T), self.joint_task()]) 249 | self.actuate(q_delta) 250 | 251 | def lissajous(self, w=0.1*2*numpy.pi, n=2): 252 | # Compute offset for Lissajous figure 253 | t = rospy.get_time() 254 | offset = numpy.asarray([0.3 * numpy.sin(w * t), 0.3 * numpy.sin(n * w * t), 0.]) 255 | # add offset to current marker pose to draw Lissajous figure in x-y-plane of marker 256 | target = numpy.copy(self.targets['pose']) 257 | target[0:3, 3] += target[0:3, 0:3].dot(offset) 258 | self.pose_control(target) 259 | 260 | 261 | if __name__ == '__main__': 262 | rospy.init_node('ik') # create a ROS node 263 | c = Controller() 264 | 265 | ims = InteractiveMarkerServer('controller') 266 | addMarker(ims, iPoseMarker(c.T), processFeedback(c.setTarget)) 267 | ims.applyChanges() 268 | 269 | rate = rospy.Rate(50) # Run control loop at 50 Hz 270 | while not rospy.is_shutdown(): 271 | c.hierarchic_control(c.targets['pose']) 272 | rate.sleep() 273 | --------------------------------------------------------------------------------