Component/aikido ros control

aikido/src/util/CMakeLists.txt

@@ -3,6 +3,7 @@ aikido_add_library("${PROJECT_NAME}_util"
   PseudoInverse.cpp
   VanDerCorput.cpp
   StepSequence.cpp
+  KinBodyParser.cpp
   RNG.cpp
 )
 target_link_libraries("${PROJECT_NAME}_util"

aikido_ros_control/CMakeLists.txt

@@ -0,0 +1,77 @@
+cmake_minimum_required(VERSION 2.8.8)
+project(aikido_ros_control)
+
+find_package(catkin REQUIRED COMPONENTS 
+    roscpp
+    geometry_msgs
+    control_msgs 
+    std_msgs
+    trajectory_msgs
+    actionlib_msgs 
+    actionlib
+)
+
+find_package(DART REQUIRED COMPONENTS core)
+find_package(Boost REQUIRED)
+find_package(aikido REQUIRED COMPONENTS util)
+
+include(FindPkgConfig)
+
+include_directories(SYSTEM
+  ${DART_INCLUDE_DIRS}
+  ${Eigen_INCLUDE_DIRS}
+  ${aikido_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
+
+link_directories(
+  ${catkin_LIBRARY_DIRS}
+)
+
+add_definitions(-DPROJECT_NAME=${PROJECT_NAME})
+
+catkin_package(
+  INCLUDE_DIRS
+    include
+  LIBRARIES
+    ${PROJECT_NAME}
+  CATKIN_DEPENDS
+    roscpp
+    geometry_msgs
+    control_msgs
+    trajectory_msgs
+    std_msgs
+    actionlib_msgs
+    actionlib
+  DEPENDS
+    Eigen
+)
+
+include_directories(SYSTEM
+  include
+  ${Boost_INCLUDE_DIRS}
+  ${Eigen_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+
+add_library(${PROJECT_NAME} SHARED
+    src/RosTrajectoryExecutor.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${DART_LIBRARIES}
+  ${aikido_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+)
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})

aikido_ros_control/include/aikido/ros_control/RosTrajectoryExecutor.hpp

@@ -0,0 +1,75 @@
+#ifndef ROS_TRAJECTORYEXECUTOR_HPP
+#define ROS_TRAJECTORYEXECUTOR_HPP
+#include <aikido/control/TrajectoryExecutor.hpp>
+#include <aikido/trajectory/Trajectory.hpp>
+#include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
+
+#include <future>
+#include <mutex>
+#include <condition_variable>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <ros/ros.h>
+#include <actionlib/client/simple_action_client.h>
+
+namespace aikido {
+
+namespace ros_control {
+
+class RosTrajectoryExecutor : public aikido::control::TrajectoryExecutor
+{
+public:
+
+    RosTrajectoryExecutor(
+        ::dart::dynamics::SkeletonPtr skeleton, 
+        std::chrono::milliseconds period,
+        std::string serverName,
+        ros::NodeHandle node,
+        double trajTimeStep);
+
+    virtual ~RosTrajectoryExecutor();
+
+    std::future<void> execute(
+        trajectory::TrajectoryPtr _traj) override;
+
+private:
+
+    typedef actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> TrajectoryClient;
+
+    std::shared_ptr<TrajectoryClient> mTrajClientPtr;
+
+    ros::NodeHandle mNode;
+
+    ::dart::dynamics::SkeletonPtr mSkeleton;
+    std::unique_ptr<std::promise<void>> mPromise;
+    trajectory::TrajectoryPtr mTraj;
+
+    std::string mServerName;
+
+    /// The time resolution at which to publish trajectory
+    double mTrajTimeStep;
+
+    /// spin()'s trajectory execution cycle.
+    std::chrono::milliseconds mPeriod;
+
+    /// Blocks spin() until execute(...) is called; paired with mSpinLock.
+    std::condition_variable mCv;
+
+    /// Lock for keeping spin thread alive and executing a trajectory. 
+    /// Manages access on mTraj, mPromise, mRunning
+    std::mutex mSpinMutex;
+
+    /// Thread for spin().
+    std::thread mThread;
+
+    /// Flag for killing spin thread. 
+    bool mRunning;
+
+    /// Simulates mTraj. To be executed on a separate thread.
+    void spin();
+
+};
+
+}
+}
+
+#endif

aikido_ros_control/notes.txt

@@ -0,0 +1,9 @@
+1. Can be either RN or SO2 -> Check if can be downcasted to either
+
+2. Let the timestep be a parameter
+
+3. Use getDerivative of trajectory to get velocity and acceleration
+
+4. Check that it is metaskeleton state space and dof names respect that ordering
+
+5. SO2 is a single effective value

aikido_ros_control/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>aikido_ros_control</name>
+  <version>1.0.0</version>
+  <description>
+    Roscontrol Trajectory Execution for KIDO.
+  </description>
+  <maintainer email="[email protected]">Shushman Choudhury</maintainer>
+  <author email="[email protected]">Shushman Choudhury</author>
+  <license>BSD</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>eigen</depend>
+  <depend>dart</depend>
+  <depend>boost</depend>
+  <depend>aikido</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>trajectory_msgs</depend>
+  <depend>roscpp</depend>
+  <depend>actionlib</depend>
+  <depend>actionlib_msgs</depend>
+  <depend>control_msgs</depend>
+</package>