Motion module of FollowJointTrajectoryAction server

op3_joint_trajectory_control_module/CMakeLists.txt

@@ -0,0 +1,78 @@
+################################################################################
+# Set minimum required version of cmake, project name and compile options
+################################################################################
+cmake_minimum_required(VERSION 2.8.3)
+project(op3_joint_trajectory_control_module)
+
+################################################################################
+# Find catkin packages and libraries for catkin and system dependencies
+################################################################################
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  actionlib
+  std_msgs
+  sensor_msgs
+  control_msgs
+  robotis_controller_msgs
+  cmake_modules
+  robotis_framework_common
+  robotis_device
+  robotis_math
+  op3_kinematics_dynamics
+)
+
+find_package(Boost REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+################################################################################
+# Setup for python modules and scripts
+################################################################################
+
+################################################################################
+# Declare ROS messages, services and actions
+################################################################################
+
+################################################################################
+# Declare ROS dynamic reconfigure parameters
+################################################################################
+
+################################################################################
+# Declare catkin specific configuration to be passed to dependent projects
+################################################################################
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS roscpp actionlib std_msgs sensor_msgs robotis_controller_msgs cmake_modules robotis_framework_common robotis_device robotis_math op3_kinematics_dynamics 
+  DEPENDS Boost EIGEN3
+)
+
+################################################################################
+# Build
+################################################################################
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+add_library(${PROJECT_NAME} src/joint_trajectory_control_module.cpp)
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${Eigen3_LIBRARIES})
+
+################################################################################
+# Install
+################################################################################
+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/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+################################################################################
+# Test
+################################################################################

op3_joint_trajectory_control_module/include/op3_joint_trajectory_control_module/joint_trajectory_control_module.h

@@ -0,0 +1,133 @@
+/*******************************************************************************
+* Copyright 2017 ROBOTIS CO., LTD.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*     http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*******************************************************************************/
+
+
+#ifndef JOINT_TRAJECTORY_CONTROL_MODULE_H_
+#define JOINT_TRAJECTORY_CONTROL_MODULE_H_
+
+#include <boost/thread.hpp>
+#include <eigen3/Eigen/Eigen>
+
+#include <ros/ros.h>
+#include <std_msgs/Empty.h>
+#include <std_msgs/String.h>
+#include <actionlib/server/simple_action_server.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+
+#include "robotis_controller_msgs/StatusMsg.h"
+#include "robotis_framework_common/motion_module.h"
+#include "robotis_math/robotis_math.h"
+
+#include "op3_kinematics_dynamics/op3_kinematics_dynamics.h"
+
+namespace robotis_op
+{
+
+class JointTrajectoryControlModule : public robotis_framework::MotionModule, public robotis_framework::Singleton<JointTrajectoryControlModule>
+{
+ public:
+  JointTrajectoryControlModule();
+  virtual ~JointTrajectoryControlModule();
+
+  void initialize(const int control_cycle_msec, robotis_framework::Robot *robot);
+  void process(std::map<std::string, robotis_framework::Dynamixel *> dxls, std::map<std::string, double> sensors);
+
+  void stop();
+  bool isRunning();
+
+  void onModuleEnable();
+  void onModuleDisable();
+
+ private:
+   enum TraIndex
+   {
+     Position = 0,
+     Velocity = 1,
+     Acceleration = 2,
+     Count
+   };
+
+   const int BASE_INDEX;
+   const int HEAD_INDEX;
+   const int RIGHT_END_EFFECTOR_INDEX;
+   const int RIGHT_ELBOW_INDEX;
+   const int LEFT_END_EFFECTOR_INDEX;
+   const int LEFT_ELBOW_INDEX;
+
+  ros::NodeHandle ros_node_;
+
+  /* ROS Action Server */
+  actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> follow_joint_trajectory_server_;
+  bool follow_action_initialized_;
+  void onFollowJointTrajectoryActionGoal();
+  void onFollowJointTrajectoryActionPreempt();
+
+  void jointTraGeneThread();
+
+  void startMoving();
+  void finishMoving();
+  void stopMoving();
+
+  void publishStatusMsg(unsigned int type, std::string msg);
+
+  Eigen::MatrixXd calcMinimumJerkTraPVA(double pos_start, double vel_start, double accel_start, double pos_end,
+                                        double vel_end, double accel_end, double smp_time, double mov_time);
+
+  std::map<std::string, bool> collision_;
+
+  bool checkSelfCollision();
+  bool getDiff(OP3KinematicsDynamics *kinematics, int end_index, int base_index, double &diff);
+
+  double default_moving_angle_;
+  bool check_collision_;
+
+  int control_cycle_msec_;
+  boost::thread *tra_gene_thread_;
+  boost::mutex tra_lock_;
+  ros::Publisher status_msg_pub_;
+  const bool DEBUG;
+  bool stop_process_;
+  bool is_moving_;
+  bool is_updated_;
+  bool is_blocked_;
+  bool will_be_collision_;
+  int tra_count_, tra_size_;
+  double moving_time_;
+  double r_min_diff_, l_min_diff_;
+
+  Eigen::MatrixXd target_position_;
+  Eigen::MatrixXd present_position_;
+  Eigen::MatrixXd goal_position_;
+  Eigen::MatrixXd goal_velocity_;
+  Eigen::MatrixXd goal_acceleration_;
+  Eigen::MatrixXd calc_joint_tra_;
+  Eigen::MatrixXd calc_joint_vel_tra_;
+  Eigen::MatrixXd calc_joint_accel_tra_;
+
+  std::map<std::string, int> using_joint_name_;
+  std::map<int, double> max_angle_;
+  std::map<int, double> min_angle_;
+
+  ros::Time last_msg_time_;
+  std::string last_msg_;
+
+  OP3KinematicsDynamics *op3_kinematics_;
+};
+
+}
+
+#endif /* HEAD_CONTROL_MODULE_H_ */

op3_joint_trajectory_control_module/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>op3_joint_trajectory_control_module</name>
+  <version>0.2.1</version>
+  <description>
+    The op3_joint_trajectory_control_module package
+  </description>
+  <license>Apache 2.0</license>
+  <author email="[email protected]">Masaki Murooka</author>
+  <maintainer email="[email protected]">Masaki Murooka</maintainer>
+  <url type="website">http://wiki.ros.org/op3_joint_trajectory_control_module</url>
+  <url type="emanual">http://emanual.robotis.com/docs/en/platform/op3/introduction/</url>
+  <url type="repository">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3</url>
+  <url type="bugtracker">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3/issues</url>
+  <buildtool_depend>catkin</buildtool_depend>  
+  <depend>roscpp</depend>
+  <depend>actionlib</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>control_msgs</depend>
+  <depend>robotis_controller_msgs</depend>
+  <depend>cmake_modules</depend>
+  <depend>robotis_framework_common</depend>
+  <depend>robotis_device</depend>
+  <depend>robotis_math</depend>
+  <depend>op3_kinematics_dynamics</depend>
+  <depend>boost</depend>
+  <depend>eigen</depend>
+</package>