[WIP]: Controller Switching

CMakeLists.txt

@@ -145,7 +145,7 @@ target_link_libraries(libada
 
 #=============================================================================
 #Pybind
-option(BUILD_ADAPY "Build adapy (the python binding)" ON)
+option(BUILD_ADAPY "Build adapy (the python binding)" OFF)
 if(pybind11_FOUND AND BUILD_ADAPY)
 
   set(pybind_sources

config/gen3_6dof_config.yaml

@@ -9,3 +9,25 @@ hand_base: end_effector_link
 arm_controller: trajectory_controller
 hand_controller: hand_controller
 named_configs: package://libada/resources/g2_6d_named_configs.yaml  
+arm_executors:
+  enable_controller_switching: true
+  mode_controller: joint_mode_controller
+  # Note: type can be TRAJECTOTY / JOINT_COMMAND / TASK_COMMAND / VISUAL_SERVOING
+  # Note: executor mode can be different from controller mode, for example joint command position executor and impedance controller
+  # Note: type TRAJECTORY does not require mode specification
+  # Note: last-controller is default when calling setActiveExecutor(ExecutorType)
+  executors:
+    - {id: effort_trajectory_executor, type: TRAJECTORY, mode: EFFORT, controller: effort_joint_trajectory_controller}
+    - {id: velocity_trajectory_executor, type: TRAJECTORY, mode: VELOCITY, controller: move_until_touch_topic_joint_trajectory_controller}
+    - {id: bite_transfer_executor, type: JOINT_COMMAND, mode: EFFORT, controller: task_space_compliant_controller}
+  # executors:
+  #   - {id: velocity_trajectory_executor, type: TRAJECTORY, mode: VELOCITY, controller: rewd_trajectory_controller}
+  #   - {id: bite_transfer_executor, type: JOINT_COMMAND, mode: EFFORT, controller: task_space_compliant_controller}
+hand_executors:
+  enable_controller_switching: true
+  mode_controller: hand_mode_controller
+  # Note: type can be TRAJECTOTY / JOINT_COMMAND / TASK_COMMAND / VISUAL_SERVOING
+  # Note: executor mode can be different from controller mode, for example joint command position executor and impedance controller
+  # Note: last-controller is default when calling setActiveExecutor(ExecutorType)
+  executors:
+    - {id: forque_pickup_executor, type: JOINT_COMMAND, mode: POSITION, controller: hand_position_controller}

config/gen3_6dof_controllers.yaml

@@ -1,7 +1,7 @@
 # Publish all joint states -----------------------------------
 joint_state_controller:
   type: joint_state_controller/JointStateController
-  publish_rate: 50
+  publish_rate: 500
 
 # whole-arm joint mode controller
 joint_mode_controller:
@@ -65,10 +65,46 @@ trajectory_controller:
     joint_5: {p: 3,  d: 0, i: 0, i_clamp: 1}
     joint_6: {p: 3,  d: 0, i: 0, i_clamp: 1}
 
+task_space_compliant_controller:
+  mode: EFFORT
+  mode_handle: joint_mode
+  type: rewd_controllers/TaskSpaceCompliantController
+  joints: [joint_1, joint_2, joint_3,
+    joint_4, joint_5, joint_6]
+  constraints:
+    stopped_velocity_tolerance: 1.0
+    joint_1:
+      goal: 0.02
+      trajectory: 10.0
+    joint_2:
+      goal: 0.02
+      trajectory: 10.0
+    joint_3:
+      goal: 0.02
+      trajectory: 10.0
+    joint_4:
+      goal: 0.02
+      trajectory: 10.0
+    joint_5:
+      goal: 0.02
+      trajectory: 10.0
+    joint_6:
+      goal: 0.02
+      trajectory: 10.0
+  gains: # Required because we're controlling a velocity interface
+    # high stiff
+    joint_1: {p: 80, d: 8}
+    joint_2: {p: 80, d: 8}
+    joint_3: {p: 80, d: 8}
+    joint_4: {p: 60, d: 6}
+    joint_5: {p: 60, d: 6}
+    joint_6: {p: 60, d: 6}
+
 # whole-arm base trajectory controller
-move_until_touch_topic_controller:
+move_until_touch_topic_joint_trajectory_controller:
   mode: TRAJECTORY
-  type: rewd_controllers/MoveUntilTouchTopicController
+  controller_mode: VELOCITY
+  type: rewd_controllers/MoveUntilTouchTopicJointTrajectoryController
   # Forque Sensor
   forcetorque_wrench_name: /forque/forqueSensor
   forcetorque_tare_name: /forque/bias_controller/trigger
@@ -77,6 +113,7 @@ move_until_touch_topic_controller:
   # forcetorque_tare_name: /fingervision/fingerVisionTaring
   sensor_force_limit: 50
   sensor_torque_limit: 5
+  max_ft_delay: 100
   joints: [joint_1, joint_2, joint_3,
     joint_4, joint_5, joint_6]
   control_type: velocity
@@ -108,41 +145,88 @@ move_until_touch_topic_controller:
     joint_5: {p: 1.2,  d: 0, i: 0, i_clamp: 1.0}
     joint_6: {p: 1.2,  d: 0, i: 0, i_clamp: 1.0}
 
+# whole-arm base trajectory controller
+effort_joint_trajectory_controller:
+  mode: TRAJECTORY
+  controller_mode: EFFORT
+  type: rewd_controllers/MoveUntilTouchTopicJointTrajectoryController
+  # Forque Sensor
+  forcetorque_wrench_name: /forque/forqueSensor
+  forcetorque_tare_name: /forque/bias_controller/trigger
+  # FingerVision
+  # forcetorque_wrench_name: /fingervision/fv_3_l/wrench
+  # forcetorque_tare_name: /fingervision/fingerVisionTaring
+  sensor_force_limit: 50
+  sensor_torque_limit: 5
+  max_ft_delay: 100
+  joints: [joint_1, joint_2, joint_3,
+    joint_4, joint_5, joint_6]
+  control_type: effort
+  constraints:
+    stopped_velocity_tolerance: 1.0
+    joint_1:
+      goal: 0.1
+      trajectory: 5.0
+    joint_2:
+      goal: 0.1
+      trajectory: 5.0
+    joint_3:
+      goal: 0.1
+      trajectory: 5.0
+    joint_4:
+      goal: 0.1
+      trajectory: 5.0
+    joint_5:
+      goal: 0.1
+      trajectory: 5.0
+    joint_6:
+      goal: 0.1
+      trajectory: 5.0
+  gains: # Required because we're controlling a velocity interface
+    joint_1: {p: 100,  d: 25, i: 0, i_clamp: 1.0}
+    joint_2: {p: 100,  d: 25, i: 0, i_clamp: 1.0}
+    joint_3: {p: 100,  d: 25, i: 0, i_clamp: 1.0}
+    joint_4: {p: 100,  d: 25, i: 0, i_clamp: 1.0}
+    joint_5: {p: 100,  d: 25, i: 0, i_clamp: 1.0}
+    joint_6: {p: 100,  d: 25, i: 0, i_clamp: 1.0}
+  mode_handle: joint_mode
+
 # Optional controller that might be useful for testing.
 # Should do the same thing as trajectory_controller, but this is 
 # the lab's rewd_controllers implementation
 rewd_trajectory_controller:
   mode: TRAJECTORY
+  controller_mode: VELOCITY
   type: rewd_controllers/JointTrajectoryController
   joints: [joint_1, joint_2, joint_3,
     joint_4, joint_5, joint_6]
   constraints:
     stopped_velocity_tolerance: 1.0
     joint_1:
       goal: 0.02
-      trajectory: 10.0
+      trajectory: 1
     joint_2:
       goal: 0.02
-      trajectory: 10.0
+      trajectory: 1
     joint_3:
       goal: 0.02
-      trajectory: 10.0
+      trajectory: 1
     joint_4:
       goal: 0.02
-      trajectory: 10.0
+      trajectory: 1
     joint_5:
       goal: 0.02
-      trajectory: 10.0
+      trajectory: 1
     joint_6:
       goal: 0.02
-      trajectory: 10.0
+      trajectory: 1
   gains: # Required because we're controlling a velocity interface
-    joint_1: {p: 0.2,  d: 0, i: 0, i_clamp: 1}
-    joint_2: {p: 0.2,  d: 0, i: 0, i_clamp: 1}
-    joint_3: {p: 0.2,  d: 0, i: 0, i_clamp: 1}
-    joint_4: {p: 0.2,  d: 0, i: 0, i_clamp: 1}
-    joint_5: {p: 0.2,  d: 0, i: 0, i_clamp: 1}
-    joint_6: {p: 0.2,  d: 0, i: 0, i_clamp: 1}
+    joint_1: {p: 1.2,  d: 0, i: 0, i_clamp: 1.0}
+    joint_2: {p: 1.2,  d: 0, i: 0, i_clamp: 1.0}
+    joint_3: {p: 1.2,  d: 0, i: 0, i_clamp: 1.0}
+    joint_4: {p: 1.2,  d: 0, i: 0, i_clamp: 1.0}
+    joint_5: {p: 1.2,  d: 0, i: 0, i_clamp: 1.0}
+    joint_6: {p: 1.2,  d: 0, i: 0, i_clamp: 1.0}
   control_type: velocity
 
 ##############################################

include/libada/Ada.hpp

@@ -13,6 +13,11 @@
 #include <aikido/constraint/dart/CollisionFree.hpp>
 #include <aikido/constraint/dart/TSR.hpp>
 #include <aikido/control/TrajectoryExecutor.hpp>
+#include <aikido/control/JacobianExecutor.hpp>
+#include <aikido/control/KinematicSimulationJointCommandExecutor.hpp>
+#include <aikido/control/KinematicSimulationTrajectoryExecutor.hpp>
+#include <aikido/control/VisualServoingVelocityExecutor.hpp>
+#include <aikido/control/ros/RosJointModeCommandClient.hpp>
 #include <aikido/control/ros/RosJointStateClient.hpp>
 #include <aikido/io/CatkinResourceRetriever.hpp>
 #include <aikido/planner/World.hpp>
@@ -110,16 +115,16 @@ class Ada final : public aikido::robot::ros::RosRobot
   void step(const std::chrono::system_clock::time_point& timepoint) override;
 
   /// Get the arm.
-  aikido::robot::RobotPtr getArm();
+  aikido::robot::ros::RosRobotPtr getArm();
 
   /// Get the const arm.
-  aikido::robot::ConstRobotPtr getArm() const;
+  aikido::robot::ros::ConstRosRobotPtr getArm() const;
 
   /// Get the hand as an Aikido::Robot
-  aikido::robot::RobotPtr getHandRobot();
+  aikido::robot::ros::RosRobotPtr getHandRobot();
 
   /// Get the const hand as an Aikido::Robot
-  aikido::robot::ConstRobotPtr getHandRobot() const;
+  aikido::robot::ros::ConstRosRobotPtr getHandRobot() const;
 
   /// Get the hand as an Aikido::Hand
   std::shared_ptr<AdaHand> getHand();
@@ -162,16 +167,6 @@ class Ada final : public aikido::robot::ros::RosRobot
       const Eigen::VectorXd& accelerationLimits = Eigen::VectorXd(),
       const typename PostProcessor::Params& params = KunzParams());
 
-  /// Starts the provided trajectory controllers if not started already.
-  /// Makes sure that no other controllers are running and conflicting
-  /// with the ones we are about to start.
-  /// \return true if all controllers have been successfully switched
-  bool startTrajectoryControllers();
-
-  /// Turns off provided trajectory controllers
-  /// \return true if all controllers have been successfully switched
-  bool stopTrajectoryControllers();
-
   /// Opens Ada's hand
   std::future<void> openHand();
 
@@ -188,27 +183,12 @@ class Ada final : public aikido::robot::ros::RosRobot
   dart::dynamics::BodyNodePtr getEndEffectorBodyNode();
 
 private:
-  /// Switches between controllers.
-  /// \param[in] startControllers Controllers to start.
-  /// \param[in] stopControllers Controllers to stop.
-  /// Returns true if controllers successfully switched.
-  bool switchControllers(
-      const std::vector<std::string>& startControllers,
-      const std::vector<std::string>& stopControllers);
-
   // Call to spin first to pass current time to step
   void spin();
 
-  // Utility function to (re)-create and set trajectory executors
-  void createTrajectoryExecutor(bool isHand);
-
   // True if running in simulation.
   const bool mSimulation;
 
-  // Names of the ros trajectory controllers
-  std::string mArmTrajControllerName;
-  std::string mHandTrajControllerName;
-
   // Soft velocity and acceleration limits
   Eigen::VectorXd mSoftVelocityLimits;
   Eigen::VectorXd mSoftAccelerationLimits;
@@ -218,9 +198,6 @@ class Ada final : public aikido::robot::ros::RosRobot
   // Ros node associated with this robot.
   ::ros::NodeHandle mNode;
 
-  // Ros controller service client.
-  std::unique_ptr<::ros::ServiceClient> mControllerServiceClient;
-
   // Ros joint state client.
   std::unique_ptr<aikido::control::ros::RosJointStateClient> mJointStateClient;
 
@@ -229,10 +206,10 @@ class Ada final : public aikido::robot::ros::RosRobot
   std::string mEndEffectorName;
 
   // The robot arm
-  aikido::robot::RobotPtr mArm;
+  aikido::robot::ros::RosRobotPtr mArm;
 
   // The robot hand as an Aikido Robot
-  aikido::robot::RobotPtr mHandRobot;
+  aikido::robot::ros::RosRobotPtr mHandRobot;
 
   // Self-driving thread
   std::unique_ptr<aikido::common::ExecutorThread> mThread;

include/libada/util.hpp

@@ -7,6 +7,8 @@
 #include <aikido/trajectory/Trajectory.hpp>
 #include <ros/ros.h>
 
+#include <hardware_interface/joint_mode_interface.h>
+
 #include "libada/Ada.hpp"
 
 namespace ada {
@@ -60,6 +62,19 @@ Eigen::MatrixXd createBwMatrixForTSR(
     double pitchTolerance,
     double yawTolerance);
 
+/// Get jointcommandmode enum from string
+hardware_interface::JointCommandModes modeFromString(std::string str);
+
+
+struct ExecutorDetails {
+  std::string mId;
+  std::string mType;
+  std::string mMode;
+  std::string mController;
+};
+
+std::vector<ExecutorDetails> loadExecutorsDetailsFromParameter(const ros::NodeHandle &nodeHandle, const std::string &parameterName);
+
 } // namespace util
 } // namespace ada
 

launch/DEPRECATED/default.launch

@@ -56,16 +56,6 @@
         hand_controller
         " />
 
-  <!-- Gen3 Controllers -->
-  <node name="base_controller_spawner_stopped" pkg="controller_manager" type="spawner" respawn="false"
-      output="screen" if="$(eval arg('version') == 3)"
-      args="
-        --stopped
-        velocity_controller
-        trajectory_controller
-        hand_controller
-        " />
-
   <!-- TODO: Initialize controllers directly via Aikido -->
 
 
@@ -111,12 +101,6 @@
     </group>
     <param name="robot_description"
           command="$(find xacro)/xacro --inorder $(find kortex_description)/robots/gen3_robotiq_2f_85.xacro sim:=false" if="$(eval arg('version') == 3)"/>
-    <node name="forque_controller_spawner_stopped" pkg="controller_manager" type="spawner" respawn="false"
-          output="screen"
-          args="
-             --stopped
-             move_until_touch_topic_controller
-             " />
   </group>
 
   <group if="$(arg perception)">