Merge the new features of irim_demo to master

.gitignore

@@ -0,0 +1 @@
+CATKIN_IGNORE

panda_softhand/package.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package>
+<package format="2">
   <name>panda_softhand</name>
   <version>1.0.0</version>
   <description>The panda_softhand package</description>
@@ -11,6 +11,11 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <exec_depend>panda_softhand_description</exec_depend>
+  <exec_depend>panda_softhand_control</exec_depend>
+  <exec_depend>panda_softhand_safety</exec_depend>
+  <exec_depend>panda_softhand_moveit_config</exec_depend>
+
   <export>
     <metapackage/>
   </export>

panda_softhand_control/CMakeLists.txt

@@ -35,6 +35,7 @@ add_service_files(FILES
   pose_control.srv
   joint_control.srv
   set_object.srv
+  complex_grasp.srv
 )
 
 generate_messages(DEPENDENCIES

panda_softhand_control/configs/task_sequences_params.yaml

@@ -10,23 +10,28 @@ task_sequencer:
   pos_controller: position_joint_trajectory_controller
   # Impedance controller
   imp_controller: cartesian_impedance_controller_softbots_stiff_matrix
+  # Name of the topic where the object pose is published
+  object_topic_name: /irim_demo/chosen_object
 
   # The joint configuration for home
   home_joints: [-0.035, -0.109, -0.048, -1.888, 0.075, 1.797, -0.110] 
   # Transform from object frame to grasp pose frame (xyzrpy) (DEFAULT)
-  grasp_transform: [0.0, 0.0, 0.030, -3.1415, 0.0, 0.0]
+  # grasp_transform: [-0.04, -0.04, 0.145, -3.1415, 0.0, 0.0] (default)
+  grasp_transform: [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166]
   # Transform from grasp pose frame to pre-grasp pose frame (xyzrpy)
   pre_grasp_transform: [0.0, 0.0, -0.1, 0.0, 0.0, 0.0]
+  # The joint configuration for place
+  place_joints: [-0.068, 0.152, 0.193, -2.352, 0.0405, 2.519, 0.117]
   # The joint configuration for handover
   handover_joints: [-0.101, 0.161, 0.159, -1.651, 2.023, 2.419, -0.006]
   # The threshold on tau_ext for handover hand opening
   handover_thresh: 1.0
 
   # The personalized grasp pose maps for different objects
   poses_map:
-    "kettle" : [0.066, 0.012, 0.203, -2.973, 0.620, -0.032]
-    "teddy" : [0.046, -0.238, 0.106, 3.131, -0.052, -0.400]
-    "ball" : [-0.055, 0.113, 0.07, -3.104, 0.122, -0.241]
-    "two_cubes" : [-0.055, 0.113, 0.061, -3.104, 0.122, -0.241]
-    "bottle" : [0.068, 0.077, 0.019, -2.455, -0.249, 0.409]
-    "helmet" : [0.120, -0.039, 0.179, 3.007, 0.200, 0.247]
+    "object1" : [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166]
+    "object2" : [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166]
+    "object3" : [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166]
+    "object4" : [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166]
+    "object5" : [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166]
+    "object6" : [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166]

panda_softhand_control/include/panda_softhand_control/TaskSequencer.h

@@ -12,10 +12,11 @@ Email: gpollayil@gmail.com, mathewjosepollayil@gmail.com  */
 #include "std_msgs/Bool.h"
 #include "std_srvs/SetBool.h"
 #include "panda_softhand_control/set_object.h"
+#include "panda_softhand_control/complex_grasp.h"
 #include "geometry_msgs/Pose.h"
 #include <controller_manager_msgs/SwitchController.h>
 #include <franka_msgs/FrankaState.h>
-#include <franka_control/ErrorRecoveryActionGoal.h>
+#include <franka_msgs/ErrorRecoveryActionGoal.h>
 
 // Parsing includes
 #include <XmlRpcValue.h>
@@ -56,6 +57,18 @@ class TaskSequencer {
         // Callback for simple grasp task service
         bool call_simple_grasp_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res);
 
+        // Callback for complex grasp task service (goes to specified pose)
+        bool call_complex_grasp_task(panda_softhand_control::complex_grasp::Request &req, panda_softhand_control::complex_grasp::Response &res);
+
+        // Callback for simple place task service
+        bool call_simple_place_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res);
+
+        // Callback for simple home task service
+        bool call_simple_home_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res);
+
+        // Callback for simple handover task service
+        bool call_simple_handover_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res);
+
         // Callback for set object service
         bool call_set_object(panda_softhand_control::set_object::Request &req, panda_softhand_control::set_object::Response &res);
 
@@ -84,14 +97,21 @@ class TaskSequencer {
         // The switch controller service name
         std::string switch_service_name = "/controller_manager/switch_controller";
 
-        // Topic and service names
-        std::string object_topic_name;
+        // Service names
         std::string franka_state_topic_name = "/franka_state_controller/franka_states";
         std::string grasp_task_service_name;
+        std::string complex_grasp_task_service_name;
+        std::string place_task_service_name;
+        std::string home_task_service_name;
+        std::string handover_task_service_name;
         std::string set_object_service_name;
 
         // Service Servers
         ros::ServiceServer grasp_task_server;
+        ros::ServiceServer complex_grasp_task_server;
+        ros::ServiceServer place_task_server;
+        ros::ServiceServer home_task_server;
+        ros::ServiceServer handover_task_server;
         ros::ServiceServer set_object_server;
 
         // The XmlRpc value for parsing complex params
@@ -102,14 +122,16 @@ class TaskSequencer {
         std::string robot_joints_name;              // Name of the robot joints (without the number of the joints)
         std::string pos_controller;                 // Name of position controller
         std::string imp_controller;                 // Name of impedance controller
+        std::string object_topic_name;              // Name of the topic where the object pose is published
         std::vector<double> home_joints;
         std::vector<double> grasp_transform;
         geometry_msgs::Pose grasp_T;
         std::vector<double> pre_grasp_transform;
         geometry_msgs::Pose pre_grasp_T;
+        std::vector<double> place_joints;
         std::vector<double> handover_joints;
         double handover_thresh;
 
         std::map<std::string, std::vector<double>> poses_map;     // The map containing the notable poses
 
-};
+};

panda_softhand_control/launch/launchSimulateObjectPose.launch

@@ -2,7 +2,7 @@
 <launch>
 
 	<!-- Temporarily publishing the object pose and tf. TODO: comment out when using vision -->
-	<node name="rostopic_object_pose_node" pkg="rostopic" type="rostopic" args="pub -r 10 object_pose geometry_msgs/Pose 
+	<node name="rostopic_object_pose_node" pkg="rostopic" type="rostopic" args="pub -r 10 /irim_demo/aruco_chosen_object/pose/pose geometry_msgs/Pose 
 	'{position: {x: 0.627, y: -0.040, z: 0.049}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}}'"/>
 
 	<node name="world_to_object" pkg="tf" type="static_transform_publisher" args="0.627 -0.040 0.049 0.0 0.0 0.0 world object 100"/>