Initial commit of this meta-package

.gitignore

+*~

.gitmodules

+[submodule "talos_roscontrol_sot/cmake"]
+	path = talos_roscontrol_sot/cmake
+	url = git@github.com:jrl-umi3218/jrl-cmakemodules.git

metapkg_talos_roscontrol_sot/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(metapkg_talos_roscontrol_sot)
+find_package(catkin REQUIRED)
+catkin_metapackage()

metapkg_talos_roscontrol_sot/package.xml

+<package>
+  <name>metapkg_talos_roscontrol_sot</name>
+  <version>0.0.0</version>
+  <description>A set of packages that include sot-wrapper and talos sot params .</description>
+
+  <maintainer email="olivier.stasse@laas.fr">Olivier Stasse</maintainer>
+
+  <license>BSD</license>
+
+  <url type="repository">https://github.com/stack-of-tasks/roscontrol_sot</url>
+
+  <author>Olivier Stasse</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>talos_roscontrol_sot</run_depend>
+  <run_depend>talos_roscontrol_sot_talos</run_depend>
+  <run_depend>realtime_tools</run_depend>
+  <run_depend>control_toolbox</run_depend>
+
+  <export>
+    <metapackage/>
+  </export>
+</package>

talos_roscontrol_sot/CMakeLists.txt

+# Copyright (C) 2017 LAAS-CNRS
+#
+# Author: Olivier Stasse
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program.  If not, see <http://www.gnu.org/licenses/>.
+cmake_minimum_required(VERSION 2.8.3)
+
+include(cmake/base.cmake)
+include(cmake/ros.cmake)
+include(cmake/GNUInstallDirs.cmake)
+include(cmake/python.cmake)
+
+project(roscontrol_sot)
+
+find_package(PkgConfig REQUIRED)
+
+add_required_dependency(bullet)
+
+#set(bullet_FOUND 0)
+#pkg_check_modules(bullet REQUIRED bullet)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  dynamic_graph_bridge
+  control_msgs
+  sensor_msgs
+  realtime_tools
+  controller_interface
+)
+
+## LAAS cmake submodule part
+set(PROJECT_DESCRIPTION "Integration of the Stack of Tasks in roscontrol")
+set(PROJECT_NAME roscontrol_sot)
+set(PROJECT_URL "https://github.com/stack-of-tasks/roscontrol_sot")
+
+set(CXX_DISABLE_WERROR False)
+
+include_directories(include ${bullet_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
+
+link_directories(${bullet_LIBRARY_DIRS})
+
+SETUP_PROJECT()
+
+# This is necessary so that the pc file generated by catking is similar to the on
+# done directly by jrl-cmake-modules
+catkin_package(CATKIN_DEPENDS
+roscpp realtime_tools message_runtime dynamic_graph_bridge
+LIBRARIES rcsot_controller)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories( 
+  ${catkin_INCLUDE_DIRS}
+  )
+
+## Declare a C++ library
+add_library(rcsot_controller 
+src/roscontrol-sot-controller.cpp
+src/log.cpp
+)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(roscontrol_sot_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(rcsot_controller
+  ${catkin_LIBRARIES}
+  ${bullet_LIBRARIES}
+  )
+
+
+## Mark executables and/or libraries for installation
+install(TARGETS rcsot_controller DESTINATION lib )
+
+
+foreach(dir config launch)
+  install(DIRECTORY ${dir}
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  )
+endforeach()
+
+SETUP_PROJECT_FINALIZE()
+
+

talos_roscontrol_sot/LICENSE

+BSD 2-Clause License
+
+Copyright (c) 2017, Stack Of Tasks development team
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

talos_roscontrol_sot/README.md

+# Introduction
+This package encapsulates a SoT graph to control a robot in the ros-control framework.
+The intent is to make it generic and adapted to any robot through rosparam.
+As the Stack-Of-Taks is a whole-body controller it tries to connect to all the available
+resources of the robot. 
+
+# rosparam
+
+## Namespace
+All the parameters regarding the SoT inside ros-control are in the namespace
+```
+/sot_controller
+```
+
+## Setting the SoT dynamic library which contains the robot device.
+
+Let us assume that you want to control a TALOS robot.
+You should then write a YAML file than can be named:
+```
+sot_talos_param.yaml
+```
+
+Its SoT device entity is located inside the following dynamic library:
+```
+/opt/openrobots/lib/libsot-pyrene-controller.so
+```
+Then inside the file sot_talos_param.yaml
+```
+  libname: libsot-pyrene-controller.so
+```
+
+## Specifying the actuated state vector
+To map the joints from the URDF model to the SoT actuated state vector, it is simply done by giving the ordered list of the joints name in the URDF model.
+For instance:
+```
+     joint_names: [ leg_left_1_joint, leg_left_2_joint, leg_left_3_joint, leg_left_4_joint, leg_left_5_joint, leg_left_6_joint,
+    leg_right_1_joint, leg_right_2_joint, leg_right_3_joint, leg_right_4_joint, leg_right_5_joint, leg_right_6_joint,
+    torso_1_joint,torso_2_joint,head_1_joint, head_2_joint,
+    arm_left_1_joint, arm_left_2_joint, arm_left_3_joint, arm_left_4_joint, arm_left_5_joint, arm_left_6_joint, arm_left_7_joint, gripper_left_joint,
+    arm_right_1_joint, arm_right_2_joint, arm_right_3_joint, arm_right_4_joint, arm_right_5_joint, arm_right_6_joint, arm_right_7_joint, gripper_right_joint
+  ] 
+```
+
+## Specifying the map between the ros control data and the sot device entity.
+
+This ros-control plugin connect to the robot (or simulated robot) by requesting the available ressources such as:
+1. motor-angles: Reading of the motor position
+2. joint-angles: Reading of the joint position
+3. velocities: Reading/estimation of the joint velocities
+4. torques: Reading/estimation of the joint torques
+5. cmd-position: Command for the joint positions
+6. cmd-effort: Command for the joint torques
+
+For instance the map for a robot which maps __cmd-position__ to __joints__ and __cmd-effort__ to __effort__ in its device will hae
+have the following lines in its param file:
+```
+map_rc_to_sot_device: { motor-angles: motor-angles ,
+  joint-angles: joint-angles, velocities: velocities,
+  torques: torques, cmd-joints: joints, cmd-effort: effort }
+```
+## Specifying the control mode
+Robots with __ros-control__ can be controlled either in position (POSITION) or in torque (EFFORT)
+using the __control_mode__ variable:
+
+```
+     control_mode: EFFORT
+```	
+
+
+

cmake @ 38d4efa7

+Subproject commit 38d4efa72f8e3fe087eb5abc72efd51747b2271d

talos_roscontrol_sot/config/roscontrol_sot_controller_plugins.xml

+<library path="lib/libtalos_rcsot_controller">
+
+  <class name="talos_sot_controller/RCSotController" type="talos_sot_controller::RCSotController" base_class_type="controller_interface::ControllerBase">
+    <description>
+       The roscontrol SotController generates whole body motions for your robot (following the talos interface).
+    </description>
+  </class>
+</library>
+

talos_roscontrol_sot/config/rosdoc.yaml

+- builder: doxygen
+  name: C++ API
+  output_dir: c++
+  file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'
+  #exclude_patterns: '*/ARDroneLib/*'

talos_roscontrol_sot/doc/Doxyfile.extra.in

+INPUT                  = @PROJECT_SOURCE_DIR@/include \
+			 @PROJECT_SOURCE_DIR@/doc
+

talos_roscontrol_sot/doc/manifest.yaml

+actions: []
+authors: Olivier Stasse <ostasse@laas.fr>
+brief: ''
+bugtracker: ''
+depends:
+- controller_interface
+- roscpp
+- std_msgs
+- sensor_msgs
+- realtime_tools
+- catkin
+- cmake_modules
+- rospy
+- control_msgs
+description: Wrapping the Stack-of-tasks framework in ros-control
+license: BSD
+maintainers: Olivier Stasse <ostasse@laas.fr>
+msgs: []
+package_type: package
+repo_url: ''
+srvs: []
+url: ''

talos_roscontrol_sot/launch/sot_controller_gazebo.launch

+<launch>
+
+  <!-- Sot Controller configuration -->
+  <!-- <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_your_robot_params_gazebo.yaml"/> -->
+  <!-- <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_your_robot_controller.yaml" /> -->
+  
+  <!-- Spawn walking controller -->
+  <node name="sot_controller_spawner"
+        pkg="talos_controller_manager" type="spawner" output="screen"
+        args="sot_controller" />
+
+</launch>
+

talos_roscontrol_sot/package.xml

+<?xml version="1.0"?>
+<package>
+  <name>talos_roscontrol_sot</name>
+  <version>0.0.0</version>
+  <description>Wrapping the Stack-of-tasks framework in ros-control for talos</description>
+
+  <!-- Maintainer email --> 
+  <maintainer email="ostasse@laas.fr">Olivier Stasse</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/roscontrol_sot</url> -->
+
+
+  <!-- Author tag -->
+ <author email="ostasse@laas.fr">Olivier Stasse</author>
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>control_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>realtime_tools</build_depend>
+  <build_depend>talos_controller_interface</build_depend>
+  <build_depend>cmake_modules</build_depend>
+  <build_depend>dynamic_graph_bridge</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>control_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>realtime_tools</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>talos_controller_interface</run_depend>
+  <run_depend>cmake_modules</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>dynamic_graph_bridge</run_depend>
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <controller_interface plugin="${prefix}/config/roscontrol_sot_controller_plugins.xml"/>
+  </export>
+</package>

talos_roscontrol_sot/src/log.cpp

+/* 
+   Olivier Stasse CNRS,
+   31/07/2012
+   Object to log the low-level informations of a robot.
+*/
+#include "log.hh"
+#include <sys/time.h>
+#include <sstream>
+#include <fstream>
+#include <iostream>
+
+using namespace std;
+using namespace rc_sot_system;
+
+DataToLog::DataToLog()
+{
+}
+
+void DataToLog::init(unsigned int nbDofs,long int length)
+{
+  motor_angle.resize(nbDofs*length);
+  joint_angle.resize(nbDofs*length);
+  velocities.resize(nbDofs*length);
+  torques.resize(nbDofs*length);
+  motor_currents.resize(nbDofs*length);
+  orientation.resize(4*length);
+  accelerometer.resize(3*length);
+  gyrometer.resize(3*length);
+  force_sensors.resize(4*6*length);
+  temperatures.resize(nbDofs*length);
+  timestamp.resize(length);
+
+  for(unsigned int i=0;i<nbDofs*length;i++)
+    { motor_angle[i] = joint_angle[i] = 
+	velocities[i] = 0.0;  }
+
+}
+
+
+Log::Log():  
+  lref_(0),
+  lrefts_(0)
+{
+}
+
+void Log::init(unsigned int nbDofs, unsigned int length)
+{
+  lref_ =0;
+  lrefts_=0;
+  nbDofs_=nbDofs;
+  length_=length;
+  StoredData_.init(nbDofs,length);
+  struct timeval current;
+  gettimeofday(&current,0);
+
+  timeorigin_ = (double)current.tv_sec + 0.000001 * ((double)current.tv_usec);
+
+}
+
+void Log::record(DataToLog &aDataToLog)
+{
+  if ( (aDataToLog.motor_angle.size()!=nbDofs_) ||
+       (aDataToLog.velocities.size()!=nbDofs_))
+    return;
+
+  for(unsigned int JointID=0;JointID<nbDofs_;JointID++)
+    {
+      if (aDataToLog.motor_angle.size()>JointID)
+	StoredData_.motor_angle[JointID+lref_]= aDataToLog.motor_angle[JointID];
+      if (aDataToLog.joint_angle.size()>JointID)
+	StoredData_.joint_angle[JointID+lref_]= aDataToLog.joint_angle[JointID];
+      if (aDataToLog.velocities.size()>JointID)
+	StoredData_.velocities[JointID+lref_]= aDataToLog.velocities[JointID];
+      if (aDataToLog.torques.size()>JointID)
+	StoredData_.torques[JointID+lref_]= aDataToLog.torques[JointID];
+      if (aDataToLog.motor_currents.size()>JointID)
+	StoredData_.motor_currents[JointID+lref_]= aDataToLog.motor_currents[JointID];
+      if (aDataToLog.temperatures.size()>JointID)
+	StoredData_.temperatures[JointID+lref_]= aDataToLog.temperatures[JointID];
+    }
+  for(unsigned int axis=0;axis<3;axis++)
+    {
+      StoredData_.accelerometer[lrefts_*3+axis] = aDataToLog.accelerometer[axis];
+      StoredData_.gyrometer[lrefts_*3+axis] = aDataToLog.gyrometer[axis];
+    }
+  for(unsigned int fsID=0;fsID<4;fsID++)
+    {
+      for(unsigned int axis=0;axis<6;axis++)
+	{
+	  StoredData_.force_sensors[lrefts_*24+fsID*6+axis] = 
+	    aDataToLog.force_sensors[fsID*6+axis];
+	}
+    }
+  struct timeval current;
+  gettimeofday(&current,0);
+
+  StoredData_.timestamp[lrefts_] = 
+    ((double)current.tv_sec + 0.000001 * (double)current.tv_usec) - timeorigin_;
+
+  lref_ += nbDofs_;
+  lrefts_ ++;
+  if (lref_>=nbDofs_*length_)
+    {
+      lref_=0;
+      lrefts_=0;
+    }
+}
+
+void Log::save(std::string &fileName)
+{
+  std::string suffix("-mastate.log");
+  saveVector(fileName,suffix,StoredData_.motor_angle, nbDofs_);
+  suffix="-jastate.log";
+  saveVector(fileName,suffix,StoredData_.joint_angle, nbDofs_);
+  suffix = "-vstate.log";
+  saveVector(fileName,suffix,StoredData_.velocities, nbDofs_);
+  suffix = "-torques.log";
+  saveVector(fileName,suffix,StoredData_.torques, nbDofs_);
+  suffix = "-motor-currents.log";
+  saveVector(fileName,suffix,StoredData_.motor_currents, nbDofs_);
+  suffix = "-accelero.log";
+  saveVector(fileName,suffix,StoredData_.accelerometer, 3);
+  suffix = "-gyro.log";
+  saveVector(fileName,suffix,StoredData_.gyrometer, 3);
+
+  ostringstream oss;
+  oss << "-forceSensors.log";
+  suffix = oss.str();
+  saveVector(fileName,suffix,StoredData_.force_sensors, 6);
+
+  suffix = "-temperatures.log";
+  saveVector(fileName,suffix,StoredData_.temperatures, nbDofs_);
+  
+}
+
+void Log::saveVector(std::string &fileName,std::string &suffix,
+		     std::vector<double> &avector,
+		     unsigned int size)
+{
+  ostringstream oss;
+  oss << fileName;
+  oss << suffix.c_str();
+  std::string actualFileName= oss.str();
+  ofstream aof(actualFileName.c_str());
+  if (aof.is_open())
+    {
+      for(unsigned long int i=0;i<length_;i++)
+	{
+	  // Save timestamp
+	  aof << StoredData_.timestamp[i] << " " ;
+
+	  // Compute and save dt
+	  if (i==0)
+	    aof << StoredData_.timestamp[i] - StoredData_.timestamp[length_-1] << " ";
+	  else
+	    aof << StoredData_.timestamp[i] - StoredData_.timestamp[i-1] << " ";
+
+	  // Save all data
+	  for(unsigned long int datumID=0;datumID<size;datumID++)
+	    aof << avector[i*size+datumID] << " " ;
+
+	  aof << std::endl;
+	}
+      aof.close();
+    }
+}

talos_roscontrol_sot/src/log.hh

+/* 
+   Olivier Stasse CNRS,
+   19/12/2016
+   Object to control the low-level part of TALOS.
+*/
+
+#ifndef _RC_SOT_SYSTEM_LOG_H_
+#define _RC_SOT_SYSTEM_LOG_H_
+
+#include <vector>
+#include <string>
+
+namespace rc_sot_system {
+
+  struct DataToLog
+  {
+    // Measured angle values at the motor side.
+    std::vector<double> motor_angle;
+    // Measured angle at the joint side.
+    std::vector<double> joint_angle;
+    // Measured or computed velocities.
+    std::vector<double> velocities;
+    // Measured torques.
+    std::vector<double> torques;
+    // Reconstructed orientation (from internal IMU).
+    std::vector<double> orientation;
+    // Measured linear acceleration
+    std::vector<double> accelerometer;
+    // Measured angular velocities
+    std::vector<double> gyrometer;
+    // Measured force sensors
+    std::vector<double> force_sensors;
+    // Measured motor currents
+    std::vector<double> motor_currents;
+    // Measured temperatures
+    std::vector<double> temperatures;
+
+    // Timestamp
+    std::vector<double> timestamp;
+
+    DataToLog();
+    void init(unsigned int nbDofs, long int length);
+
+
+  };
+
+  class Log
+  {
+  private:
+    // Actuated informations logged.
+    unsigned int nbDofs_;
+    // Number of iterations to be logged.
+    unsigned int length_;
+
+    // Current position in the circular buffer for angles.
+    long unsigned int lref_;
+    // Current position int the circular buffer for timestamp
+    // lref_ = lrefts_ * nbDofs_
+    long unsigned int lrefts_;
+
+    // Circular buffer for all the data.
+    DataToLog StoredData_;
+
+    double timeorigin_;
+
+
+    // Save one vector of information.
+    void saveVector(std::string &filename, 
+		    std::string &suffix,
+		    std::vector<double> &avector,
+		    unsigned int);
+
+  public:
+  
+    Log();
+
+    void init(unsigned int nbDofs, unsigned int length);
+    void record(DataToLog &aDataToLog);
+