Merge branch 'set_constraints_' into 'move_platform'

Set constraints

See merge request nen/modules/tiago_modules!7

CMakeLists.txt

@@ -2,12 +2,12 @@ cmake_minimum_required(VERSION 2.8.3)
 project(tiago_modules)
 
 ## Add support for C++11, supported in ROS Kinetic and newer
-# add_definitions(-std=c++11)
+add_definitions(-std=c++11)
 
 ## 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 dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs control_msgs geometry_msgs std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs pal_interaction_msgs pal_waypoint_msgs)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs control_msgs geometry_msgs std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs pal_interaction_msgs pal_waypoint_msgs moveit_ros_planning_interface)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -108,7 +108,7 @@ generate_dynamic_reconfigure_options(
 catkin_package(
  INCLUDE_DIRS include
  LIBRARIES gripper_module play_motion_module head_module torso_module nav_module tts_module arm_module move_platform_module
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs control_msgs geometry_msgs std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs pal_waypoint_msgs pal_interaction_msgs
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs control_msgs geometry_msgs std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs pal_waypoint_msgs pal_interaction_msgs moveit_ros_planning_interface
 
 #  DEPENDS system_lib
 )

cfg/ArmModule.cfg

@@ -41,6 +41,7 @@ arm_action = gen.add_group("Move group action")
 joints_subs = gen.add_group("Joint states subscriber")
 plan_params = gen.add_group("Planner parameters")
 workspace = gen.add_group("Planner workspace")
+constraints = gen.add_group("Joints Orientation Constraints")
 
 #       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
 #gen.add("velocity_scale_factor",  double_t,  0,                               "Maximum velocity scale factor",  0.5,      0.0,  1.0)
@@ -70,4 +71,12 @@ workspace.add("ws_x_max",          double_t,  0,                               "
 workspace.add("ws_y_max",          double_t,  0,                               "Workspace max Y",                1.0,     -2.0, 2.0)
 workspace.add("ws_z_max",          double_t,  0,                               "Workspace max Z",                1.0,     -2.0, 2.0)
 
+constraints.add("arm_joint_position",   double_t, 0,    "Initial position for the arm joint constraint", 0.0, -0.5, 0.8)
+constraints.add("arm_tolerance_above",   double_t, 0,    "Tolerance above arm joint",                     0.1,  0.05, 0.2)
+constraints.add("arm_tolerance_below",   double_t, 0,    "Tolerance below arm joint",                     0.1,  0.05, 0.2)
+constraints.add("torso_joint_position",   double_t, 0,    "Initial position for the torso joint constraint", 0.25, 0.15, 0.30)
+constraints.add("torso_tolerance_above",   double_t, 0,    "Tolerance above torso joint",                     0.05,  0.05, 0.1)
+constraints.add("torso_tolerance_below",   double_t, 0,    "Tolerance below torso joint",                     0.05,  0.05, 0.1)
+constraints.add("set_constraints", bool_t, 0, "Set the given constraints", False)
+
 exit(gen.generate(PACKAGE, "ArmModuleAlgorithm", "ArmModule"))

include/tiago_modules/arm_module.h

@@ -48,6 +48,11 @@ class CArmModule : public CModule<arm_module::ArmModuleConfig>
     CModuleAction<moveit_msgs::MoveGroupAction> arm_action;
     moveit_msgs::MoveGroupGoal goal;
 
+    //constraints variables
+    const std::string PLANNING_GROUP = "arm_torso";
+    const std::string arm_joint      = "arm_2_joint";
+    const std::string torso_joint    = "torso_lift_joint";
+
     // joint states
     CROSWatchdog joint_states_watchdog;
     ros::Subscriber joint_states_subscriber;
@@ -62,12 +67,14 @@ class CArmModule : public CModule<arm_module::ArmModuleConfig>
     // planning objects
     std::vector<TVisualObject> objects;
     ros::Publisher scene_publisher;
-	CModuleService<moveit_msgs::GetPlanningScene> get_scene;
-	moveit_msgs::PlanningScene current_pscene;
+    CModuleService<moveit_msgs::GetPlanningScene> get_scene;
+    moveit_msgs::PlanningScene current_pscene;
 
     // path constraints
     moveit_msgs::Constraints path_constraints;
 
+    void reconfigure_constraints();
+
   protected:
     void state_machine(void);
     void reconfigure_callback(arm_module::ArmModuleConfig &config, uint32_t level);
@@ -101,6 +108,7 @@ class CArmModule : public CModule<arm_module::ArmModuleConfig>
     void add_orientation_path_constraint(geometry_msgs::QuaternionStamped &quat,double orientation_tol=0.1);
     void add_plane_path_constraint(path_const_plane_t plane,std::string &frame_id,double offset=0.0,double position_tol=0.1);
     void add_plane_path_constraint(std::vector<double> &normal,std::string &frame_id,double offset=0.0,double position_tol=0.1);
+    void add_joint_goal_constraint(std::vector<std::string> &joint_names,std::vector<double> &position, std::vector<double> &tol);
     // joint motion
     bool set_workspace(std::string &frame_id,double x_min,double y_min, double z_min, double x_max, double y_max, double z_max);
     bool move_to(sensor_msgs::JointState &target_joints,double joint_tol=0.1);

package.xml

@@ -54,6 +54,7 @@
   <build_depend>pal_waypoint_msgs</build_depend>
   <build_depend>pal_interaction_msgs</build_depend>
   <build_depend>tf</build_depend>
+  <build_depend>moveit_ros_planning_interface</build_depend>
   <!-- new dependencies -->
   <!--<build_depend>new build dependency</build_depend>-->
   <run_depend>iri_ros_tools</run_depend>
@@ -70,6 +71,7 @@
   <run_depend>pal_waypoint_msgs</run_depend>
   <run_depend>pal_interaction_msgs</run_depend>
   <run_depend>tf</run_depend>
+  <run_depend>moveit_ros_planning_interface</run_depend>
   <!-- new dependencies -->
   <!--<run_depend>new run dependency</run_depend>-->
 

src/arm_module.cpp

@@ -464,6 +464,7 @@ bool CArmModule::get_current_planning_scene(int component) {
 // constraint functions
 void CArmModule::clear_path_constraints(void)
 {
+  this->goal.request.goal_constraints[0].joint_constraints.clear();
   this->path_constraints.joint_constraints.clear();
   this->path_constraints.position_constraints.clear();
   this->path_constraints.orientation_constraints.clear();
@@ -551,6 +552,21 @@ void CArmModule::add_plane_path_constraint(std::vector<double> &normal,std::stri
 
 }
 
+void CArmModule::add_joint_goal_constraint(std::vector<std::string> &joint_names,std::vector<double> &position, std::vector<double> &tol)
+{
+  moveit_msgs::JointConstraint joint_const;
+
+  for(unsigned int i=0;i<joint_names.size();i++)
+  {
+    joint_const.joint_name=joint_names[i];
+    joint_const.position=position[i];
+    joint_const.tolerance_above=tol[i]/2.0;;
+    joint_const.tolerance_below=tol[i]/2.0;;
+    joint_const.weight=10.0;
+    this->goal.request.goal_constraints[0].joint_constraints.push_back(joint_const);
+  }
+}
+
 // joint motion
 bool CArmModule::set_workspace(std::string &frame_id,double x_min,double y_min, double z_min, double x_max, double y_max, double z_max)
 {
@@ -627,7 +643,6 @@ void CArmModule::move_to(geometry_msgs::PoseStamped &pose,double position_tol,do
   this->lock();
   if(!this->is_finished())
     this->stop();
-  this->goal.request.goal_constraints[0].joint_constraints.clear();
   this->goal.request.goal_constraints[0].position_constraints.resize(1);
   this->goal.request.goal_constraints[0].position_constraints[0].header=pose.header;
   this->goal.request.goal_constraints[0].position_constraints[0].link_name=this->config.end_effector_tool;