Merge pull request bcoudrin#2 from francois-keith/master

Use the pr2 with the sot.

.gitignore

@@ -9,5 +9,5 @@ src/lisp/
 src/sot_pr2/
 srv/lisp/
 srv_gen/
-
+*~
 *.cfgc

.travis.yml

@@ -0,0 +1,14 @@
+language: cpp
+compiler:
+- gcc
+matrix:
+notifications:
+  email:
+  - [email protected]
+branches:
+  only:
+  - master
+  - groovy
+script: ./.travis/build
+before_install: ./.travis/before_install
+after_success: ./.travis/after_success

.travis/after_success

@@ -0,0 +1,6 @@
+#!/bin/sh
+set -ev
+
+git config --global user.name "Travis CI"
+git config --global user.email "[email protected]"
+

.travis/before_install

@@ -0,0 +1,58 @@
+#!/bin/bash
+set -ev
+
+export UBUNTU_CODENAME=`lsb_release -s -c`
+export ROS_VERSION=hydro
+
+echo "Ubuntu codename: $UBUNTU_CODENAME"
+echo "ROS version: $ROS_VERSION"
+
+if `test x$UBUNTU_CODENAME = x`; then
+    echo "failed to determine Ubuntu codename"
+    return 1
+fi
+if `test x$UBUNTU_CODENAME = x`; then
+    echo "failed to determine ROS version"
+    return 1
+fi
+
+git submodule update --init --recursive
+
+sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $UBUNTU_CODENAME main\" > /etc/apt/sources.list.d/ros-latest.list"
+wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+
+sudo apt-get update -qq
+
+sudo apt-get install -qq	\
+    doxygen doxygen-latex	\
+    libboost-all-dev		\
+    libeigen3-dev		\
+    liblapack-dev		\
+    libblas-dev			\
+    gfortran			\
+    python-dev			\
+    python-sphinx		\
+    python-numpy    \
+    libbullet-dev
+
+
+sudo apt-get install -qq		\
+    ros-$ROS_VERSION-ros-base		\
+    ros-$ROS_VERSION-ros-control	\
+    ros-$ROS_VERSION-pr2-mechanism	\
+    ros-$ROS_VERSION-pr2-controllers	\
+    ros-$ROS_VERSION-control-msgs	\
+    ros-$ROS_VERSION-tf			\
+    ros-$ROS_VERSION-resource-retriever	\
+    ros-$ROS_VERSION-urdf		\
+    ros-$ROS_VERSION-urdfdom-py	  \
+    ros-$ROS_VERSION-common-msgs	\
+    ros-$ROS_VERSION-rosdoc-lite	\
+    ros-$ROS_VERSION-robot-state-publisher	\
+    ros-$ROS_VERSION-cmake-modules	\
+    ros-hydro-ros-control  \
+    ros-hydro-pr2-controllers-msgs
+
+
+sudo rosdep init
+rosdep update

.travis/build

@@ -0,0 +1,88 @@
+#!/bin/sh
+set -ev
+
+# Directories.
+root_dir=`pwd`
+build_dir="$root_dir/_travis/build"
+install_dir="$root_dir/_travis/install"
+
+# Shortcuts.
+git_clone="git clone --quiet --recursive"
+
+# Source ROS setup file
+. /opt/ros/*/setup.sh
+
+# Create layout.
+rm -rf "$build_dir" "$install_dir"
+mkdir -p "$build_dir"
+mkdir -p "$install_dir"
+
+# Setup environment variables.
+export LD_LIBRARY_PATH="$install_dir/lib:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`:$LD_LIBRARY_PATH"
+export PKG_CONFIG_PATH="$install_dir/lib/pkgconfig:$PKG_CONFIG_PATH"
+export PKG_CONFIG_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`/pkgconfig:$PKG_CONFIG_PATH"
+
+export ROS_PACKAGE_PATH="$root_dir:$ROS_PACKAGE_PATH"
+
+install_dependency()
+{
+    echo "--> Compiling $1"
+    mkdir -p "$build_dir/$1"
+    cd "$build_dir"
+    $git_clone "git://github.com/$1" "$1"
+    cd "$build_dir/$1"
+    cmake . -DCMAKE_INSTALL_PREFIX:STRING="$install_dir"
+    make install
+}
+
+install_ros_stack()
+{
+    echo "--> Compiling $1"
+    mkdir -p "$build_dir/$1"
+    cd "$build_dir"
+    if [ "$#" -eq "2" ]; then
+      $git_clone "git://github.com/$1" "$1" -b $2
+    else
+      $git_clone "git://github.com/$1" "$1"
+    fi
+    export ROS_PACKAGE_PATH="$build_dir/$1:$ROS_PACKAGE_PATH"
+}
+
+# Retrieve jrl-mathtools
+install_dependency jrl-umi3218/jrl-mathtools
+install_dependency jrl-umi3218/jrl-mal
+install_dependency laas/abstract-robot-dynamics
+install_dependency jrl-umi3218/jrl-dynamics
+install_dependency stack-of-tasks/dynamic-graph
+install_dependency stack-of-tasks/dynamic-graph-python
+install_dependency stack-of-tasks/sot-core
+install_dependency stack-of-tasks/sot-tools
+install_dependency stack-of-tasks/sot-dynamic
+install_dependency laas/jrl_dynamics_urdf
+install_ros_stack stack-of-tasks/redundant_manipulator_control
+
+cd "$root_dir"
+# build dependencies
+rosdep install dynamic_graph_bridge_msgs
+rosmake dynamic_graph_bridge_msgs
+
+rosdep install dynamic_graph_bridge
+rosmake dynamic_graph_bridge
+cd "$build_dir"
+cd stack-of-tasks/redundant_manipulator_control/dynamic_graph_bridge/build
+cmake .. -DCMAKE_INSTALL_PREFIX:STRING="$install_dir"
+rosmake dynamic_graph_bridge
+make install
+
+
+# rosmake and install sot_pr2
+cd "$root_dir"
+export ROS_PACKAGE_PATH="$root_dir:$ROS_PACKAGE_PATH"
+rosdep install sot_pr2
+rosmake sot_pr2
+cd build
+cmake .. -DCMAKE_INSTALL_PREFIX:STRING="$install_dir"
+rosmake sot_pr2
+make install
+

CMakeLists.txt

@@ -3,6 +3,10 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
 
 include(cmake/base.cmake)
 include(cmake/python.cmake)
+if(NOT DEFINED CMAKE_INSTALL_LIBDIR)
+  set(CMAKE_INSTALL_LIBDIR lib)
+endif()
+include(cmake/GNUInstallDirs.cmake)
 
 set(ROS_BUILD_TYPE RelWithDebInfo)
 
@@ -17,31 +21,53 @@ ADD_REQUIRED_DEPENDENCY("jrl-mal")
 ADD_REQUIRED_DEPENDENCY("dynamic-graph")
 ADD_REQUIRED_DEPENDENCY("dynamic-graph-python")
 ADD_REQUIRED_DEPENDENCY("sot-core")
+ADD_REQUIRED_DEPENDENCY("jrl-dynamics-urdf >= 2.0.1-14")
 
 # This is required by the Python prologue.
 ADD_REQUIRED_DEPENDENCY("sot-dynamic")
 
+
+# Handle rpath necessary to handle ROS multiplace packages
+# libraries inclusion
+SET(CMAKE_SKIP_BUILD_RPATH  FALSE)
+SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
+SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}")
+SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+
 # Python
 INCLUDE(cmake/python.cmake)
 FINDPYTHON()
 INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
 LINK_DIRECTORIES(${PYTHON_LIBRARY_DIRS})
 
 set(SOURCES
+    src/pr2_controller_plugin.cpp
+    src/pr2_threaded_sot_controller.cpp
     src/pr2_sot_controller.cpp
     src/pr2_device.cpp)
 
 set(HEADERS
+    include/sot_pr2/pr2_controller_plugin.h
+    include/sot_pr2/pr2_threaded_sot_controller.h
     include/sot_pr2/pr2_sot_controller.h
     include/sot_pr2/pr2_device.h)
 
 rosbuild_add_library(sot_pr2 ${HEADERS} ${SOURCES})
 
-target_link_libraries(sot_pr2 dynamic-graph)
-target_link_libraries(sot_pr2 sot-core)
-target_link_libraries(sot_pr2 dynamic-graph-python)
 PKG_CONFIG_USE_DEPENDENCY(sot_pr2 "dynamic-graph")
 PKG_CONFIG_USE_DEPENDENCY(sot_pr2 "sot-core")
 PKG_CONFIG_USE_DEPENDENCY(sot_pr2 "dynamic-graph-python")
+PKG_CONFIG_USE_DEPENDENCY(sot_pr2 "jrl-dynamics-urdf")
+install(TARGETS sot_pr2 DESTINATION ${CMAKE_INSTALL_LIBDIR})
+
+rosbuild_add_executable(sot_pr2_fake_controller ${HEADERS} src/fake_controller.cpp)
+PKG_CONFIG_USE_DEPENDENCY(sot_pr2_fake_controller "dynamic-graph")
+PKG_CONFIG_USE_DEPENDENCY(sot_pr2_fake_controller "sot-core")
+PKG_CONFIG_USE_DEPENDENCY(sot_pr2_fake_controller "dynamic-graph-python")
+PKG_CONFIG_USE_DEPENDENCY(sot_pr2_fake_controller "jrl-dynamics-urdf")
+target_link_libraries(sot_pr2_fake_controller sot_pr2)
+install(TARGETS sot_pr2_fake_controller DESTINATION ${CMAKE_INSTALL_LIBDIR})
 
 ADD_SUBDIRECTORY(src)
+ADD_SUBDIRECTORY(python)
+

controller_plugins.xml

@@ -1,5 +1,5 @@
 <library path="lib/libsot_pr2">
   <class name="sot_pr2/Pr2SotControllerPlugin"
-         type="sot_pr2::Pr2SotController"
+         type="sot_pr2::Pr2ControllerPlugin"
          base_class_type="pr2_controller_interface::Controller" />
 </library>

include/sot_pr2/pr2_controller_plugin.h

@@ -0,0 +1,78 @@
+#ifndef PR2_CONTROLLER_PLUGIN_H
+#define PR2_CONTROLLER_PLUGIN_H
+
+#include <pr2_controller_interface/controller.h>
+#include <pr2_mechanism_model/joint.h>
+#include <sot_pr2/pr2_threaded_sot_controller.h>
+#include <control_toolbox/pid.h>
+#include <boost/scoped_ptr.hpp>
+#include <realtime_tools/realtime_publisher.h>
+#include <control_msgs/JointTrajectoryControllerState.h>
+#include <tf/transform_listener.h>
+#include <actionlib/client/simple_action_client.h>
+#include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
+
+namespace sot_pr2 {
+
+typedef boost::shared_ptr<pr2_mechanism_model::JointState> Pr2JointPtr;
+
+class Pr2ControllerPlugin : public pr2_controller_interface::Controller {
+public:
+    explicit Pr2ControllerPlugin();
+    virtual ~Pr2ControllerPlugin();
+
+    virtual bool init(pr2_mechanism_model::RobotState *robot,
+                      ros::NodeHandle &n);
+    virtual void starting();
+    virtual void update();
+    virtual void stopping();
+
+private:
+    void fillSensors();
+    void readControl();
+
+private:
+    // SoT Controller
+    Pr2ThreadedSotController sot_controller_;
+    SensorMap sensorsIn_;
+    ControlMap controlValues_;
+
+    std::vector<double> joint_encoder_;
+    std::vector<double> joint_velocity_;
+    std::vector<double> joint_control_;
+    std::vector<double> error_raw;
+    std::vector<double> error;
+
+
+    // Pr2 Controller
+    int loop_count_;
+    ros::Time last_time_;
+    std::vector<Pr2JointPtr> joints_;
+    std::vector<control_toolbox::Pid> pids_;
+    pr2_mechanism_model::RobotState *robot_;
+
+    // Gripper
+    actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> *r_gripper_client_;
+    actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> *l_gripper_client_;
+    double r_gripper_position;
+    double l_gripper_position;
+
+    // ROS interface
+    //ros::NodeHandle node_;
+    boost::scoped_ptr<
+        realtime_tools::RealtimePublisher<
+            control_msgs::JointTrajectoryControllerState> > controller_state_publisher_;
+
+    ros::Publisher cmd_vel_pub_;
+
+    tf::TransformListener listener_;
+
+    double timeFromStart_;
+
+    int _iter;
+    double _mean;
+};
+
+}
+
+#endif

include/sot_pr2/pr2_device.h

@@ -2,34 +2,46 @@
 #define PR2_DEVICE_H
 
 #include <sot/core/device.hh>
-#include <urdf_model/joint.h>
-//#include <pr2_mechanism_model/joint.h>
-#include <pr2_mechanism_model/robot.h>
-#include <ros/node_handle.h>
-
+#include <dynamic-graph/linear-algebra.h>
+#include <sot/core/abstract-sot-external-interface.hh>
 
 namespace sot_pr2 {
 
-    typedef boost::shared_ptr<urdf::Joint> UrdfJointPtr;
-    typedef boost::shared_ptr<pr2_mechanism_model::JointState> Pr2JointPtr;
-    typedef std::pair<UrdfJointPtr,Pr2JointPtr> jointAssociation_t;
-    typedef std::map<std::string, jointAssociation_t> jointMap_t;
+    typedef std::map<std::string, dynamicgraph::sot::SensorValues> SensorMap;
+    typedef std::map<std::string, dynamicgraph::sot::ControlValues> ControlMap;
 
     class Pr2Device : public dynamicgraph::sot::Device {
         DYNAMIC_GRAPH_ENTITY_DECL();
+    public:
+        static const double TIMESTEP_DEFAULT;
+
     public:
         Pr2Device(const std::string &name);
         virtual ~Pr2Device();
 
-        virtual bool init();
+        void setSensors(SensorMap &sensorsIn);
 
-        virtual void setup(jointMap_t &jm);
+        void setupSetSensors(SensorMap &sensorsIn);
+        void nominalSetSensors(SensorMap &sensorsIn);
+        void cleanupSetSensors(SensorMap &sensorsIn);
 
-        virtual void control(jointMap_t &jm);
+        void getControl(ControlMap &controlOut);
+        void getFakeControl(ControlMap &controlOut);
 
-    private:
-        static const double TIMESTEP_DEFAULT;
+    protected:
+        void updateRobotState(const std::vector<double> &anglesIn);
+
+    protected:
         double timestep_;
+        dynamicgraph::Vector previous_state_;
+        dynamicgraph::Signal<dynamicgraph::Vector, int> robotState_;
+
+    private:
+        dynamicgraph::Vector mlRobotState;
+        dynamicgraph::sot::MatrixRotation pose;
+        std::vector<double> baseff_;
+
+        int loop_count_;
     };
 }