diff --git a/CMakeLists.txt b/CMakeLists.txt index 8d1b538..9392892 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/config/gen3_6dof_config.yaml b/config/gen3_6dof_config.yaml index 4a715cb..e5d7917 100644 --- a/config/gen3_6dof_config.yaml +++ b/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} \ No newline at end of file diff --git a/config/gen3_6dof_controllers.yaml b/config/gen3_6dof_controllers.yaml index eaa8301..83a8081 100644 --- a/config/gen3_6dof_controllers.yaml +++ b/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,11 +145,58 @@ 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] @@ -120,29 +204,29 @@ rewd_trajectory_controller: 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 ############################################## diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 214ec64..4725477 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -13,6 +13,11 @@ #include #include #include +#include +#include +#include +#include +#include #include #include #include @@ -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 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 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& startControllers, - const std::vector& 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 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 mThread; diff --git a/include/libada/util.hpp b/include/libada/util.hpp index 87d4cf1..e8b7632 100644 --- a/include/libada/util.hpp +++ b/include/libada/util.hpp @@ -7,6 +7,8 @@ #include #include +#include + #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 loadExecutorsDetailsFromParameter(const ros::NodeHandle &nodeHandle, const std::string ¶meterName); + } // namespace util } // namespace ada diff --git a/launch/DEPRECATED/default.launch b/launch/DEPRECATED/default.launch index 6d6d06b..84d5a7f 100644 --- a/launch/DEPRECATED/default.launch +++ b/launch/DEPRECATED/default.launch @@ -56,16 +56,6 @@ hand_controller " /> - - - @@ -111,12 +101,6 @@ - diff --git a/launch/gen3_hardware.launch b/launch/gen3_hardware.launch index 5091d4c..8fcd262 100644 --- a/launch/gen3_hardware.launch +++ b/launch/gen3_hardware.launch @@ -1,10 +1,13 @@ - + + + + + + + @@ -27,13 +33,4 @@ - - \ No newline at end of file diff --git a/launch/gen3_urdf.launch b/launch/gen3_urdf.launch index 402d534..86ddbeb 100644 --- a/launch/gen3_urdf.launch +++ b/launch/gen3_urdf.launch @@ -5,18 +5,12 @@ + command="$(find xacro)/xacro --inorder $(find kortex_description)/robots/gen3_robotiq_2f_85.xacro sim:=false forque:=false" /> - \ No newline at end of file diff --git a/launch/libada.launch b/launch/libada.launch index a6f291a..ef8cbda 100644 --- a/launch/libada.launch +++ b/launch/libada.launch @@ -19,7 +19,10 @@ - + + + + diff --git a/python/adapy/ada_bindings.cpp b/python/adapy/ada_bindings.cpp index cd042bd..56c29fd 100644 --- a/python/adapy/ada_bindings.cpp +++ b/python/adapy/ada_bindings.cpp @@ -26,12 +26,14 @@ aikido::constraint::TestablePtr get_self_collision_constraint(ada::Ada* self) bool start_trajectory_controllers(ada::Ada* self) { - return self->startTrajectoryControllers(); + return true; + // return self->startTrajectoryControllers(); } bool stop_trajectory_controllers(ada::Ada* self) { - return self->stopTrajectoryControllers(); + return true; + // return self->stopTrajectoryControllers(); } std::string get_name(ada::Ada* self) diff --git a/src/Ada.cpp b/src/Ada.cpp index f4624c6..016602f 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include @@ -126,7 +127,7 @@ Ada::Ada( auto armBase = internal::getBodyNodeOrThrow(mMetaSkeleton, armNodes[0]); auto armEnd = internal::getBodyNodeOrThrow(mMetaSkeleton, armNodes[1]); auto arm = dart::dynamics::Chain::create(armBase, armEnd, "adaArm"); - mArm = registerSubRobot(arm, "adaArm"); + mArm = std::dynamic_pointer_cast(registerSubRobot(arm, "adaArm")); mArm->clearExecutors(); if (!mArm) { @@ -151,34 +152,13 @@ Ada::Ada( auto handBase = internal::getBodyNodeOrThrow(mMetaSkeleton, handBaseName); auto hand = dart::dynamics::Branch::create( dart::dynamics::Branch::Criteria(handBase), "adaHand"); - mHandRobot = registerSubRobot(hand, "adaHand"); + mHandRobot = std::dynamic_pointer_cast(registerSubRobot(hand, "adaHand")); mHandRobot->clearExecutors(); if (!mHandRobot) { throw std::runtime_error("Could not create hand"); } - // Create Trajectory Executors - // Should not execute trajectories on whole arm by default - // This ensures that trajectories are executed on subrobots only. - - // Load Arm Trajectory controller name - mNode.param( - "/" + confNamespace + "/arm_controller", - mArmTrajControllerName, - DEFAULT_ARM_TRAJ_CTRL); - // Create executor for controller - createTrajectoryExecutor(false); - - // Load Hand Trajectory controller name - mNode.param( - "/" + confNamespace + "/hand_controller", - mHandTrajControllerName, - DEFAULT_HAND_TRAJ_CTRL); - - // Create executor for controller - createTrajectoryExecutor(true); - // Load the named configurations if available std::string nameConfigs; if (mNode.getParam("/" + confNamespace + "/named_configs", nameConfigs)) @@ -204,10 +184,6 @@ Ada::Ada( // Create joint state updates if (!mSimulation) { - // Real Robot, create state client - mControllerServiceClient = std::make_unique<::ros::ServiceClient>( - mNode.serviceClient( - "controller_manager/switch_controller")); mJointStateClient = std::make_unique( mMetaSkeleton->getBodyNode(0)->getSkeleton(), @@ -224,9 +200,204 @@ Ada::Ada( // Create Inner AdaHand mHand = std::make_shared(this, handBase, handEnd); + // Register all executors + if (mSimulation) + { + // Kinematic Executors + for (auto subrobot : + std::set{mArm, mHandRobot}) + { + subrobot->registerExecutor( + std::make_shared< + aikido::control::KinematicSimulationTrajectoryExecutor>( + subrobot->getMetaSkeleton())); + subrobot->registerExecutor( + std::make_shared< + aikido::control::KinematicSimulationPositionExecutor>( + subrobot->getMetaSkeleton())); + auto jvExec = std::make_shared( + handEnd); + auto vsExec + = std::make_shared( + handEnd, jvExec); + subrobot->registerExecutor(vsExec); + subrobot->registerExecutor(jvExec); + } + } + else + { + auto rosLoadControllerServiceClient = std::make_shared<::ros::ServiceClient>( + mNode.serviceClient( + "controller_manager/load_controller")); + auto rosListControllersServiceClient = std::make_shared<::ros::ServiceClient>( + mNode.serviceClient( + "controller_manager/list_controllers")); + auto rosSwitchControllerServiceClient = std::make_shared<::ros::ServiceClient>( + mNode.serviceClient( + "controller_manager/switch_controller")); + // Real ROS Executors + for (auto subrobot : + std::set{mArm}) + // std::set{mArm, mHandRobot}) + { + subrobot->setRosLoadControllerServiceClient(rosLoadControllerServiceClient); + subrobot->setRosListControllersServiceClient(rosListControllersServiceClient); + bool isHand = (subrobot == mHandRobot); + std::string ns = isHand ? "/" + confNamespace + "/hand_executors/" + : "/" + confNamespace + "/arm_executors/"; + // Controller Switching Service Client + bool enableControllerSwitching = mNode.param( + ns + "enable_controller_switching", + false); + if(enableControllerSwitching) + subrobot->setRosSwitchControllerServiceClient(rosSwitchControllerServiceClient); + // Mode controller + auto modeControllerName = mNode.param( + ns + "mode_controller", + std::string("")); + std::cout<<"Mode controller name: "<( + mNode, + modeControllerName + "/joint_mode_command", + std::vector{"joint_mode"}); + subrobot->setRosJointModeCommandClient(rosJointModeCommandClient); + } + std::vector executorsDetails = util::loadExecutorsDetailsFromParameter( + mNode, + ns + "executors"); + if(executorsDetails.size() == 0) + { + std::stringstream message; + message << "Unable to load executors details for " << ns; + throw std::runtime_error(message.str()); + } + for(auto executorDetails: executorsDetails) + { + if(executorDetails.mType == "TRAJECTORY") + { + std::cout<<"Executor type is TRAJECTORY!!"<( + mNode, + executorDetails.mController + "/follow_joint_trajectory", + DEFAULT_ROS_TRAJ_INTERP_TIME, + DEFAULT_ROS_TRAJ_GOAL_TIME_TOL, + subrobot->getMetaSkeleton()); + auto controllerMode = util::modeFromString( + mNode.param("/" + executorDetails.mController + "/controller_mode", "")); + subrobot->registerExecutor(trajExec, executorDetails.mId, executorDetails.mController, controllerMode); + } + else if(executorDetails.mType == "JOINT_COMMAND") + { + if(executorDetails.mMode == "POSITION") + { + auto posExec + = std::make_shared( + mNode, executorDetails.mController, subrobot->getMetaSkeleton()->getDofs()); + auto controllerMode = util::modeFromString( + mNode.param("/" + executorDetails.mController + "/controller_mode", "POSITION")); + subrobot->registerExecutor(posExec, executorDetails.mId, executorDetails.mController, controllerMode); + } + else if(executorDetails.mMode == "VELOCITY") + { + auto velExec + = std::make_shared( + mNode, executorDetails.mController, subrobot->getMetaSkeleton()->getDofs()); + auto controllerMode = util::modeFromString( + mNode.param("/" + executorDetails.mController + "/controller_mode", "VELOCITY")); + subrobot->registerExecutor(velExec, executorDetails.mId, executorDetails.mController, controllerMode); + } + else if(executorDetails.mMode == "EFFORT") + { + auto effExec + = std::make_shared( + mNode, executorDetails.mController, subrobot->getMetaSkeleton()->getDofs()); + auto controllerMode = util::modeFromString( + mNode.param("/" + executorDetails.mController + "/controller_mode", "EFFORT")); + subrobot->registerExecutor(effExec, executorDetails.mId, executorDetails.mController, controllerMode); + } + else + { + std::stringstream message; + message << "Executor mode for [/" << ns << "/executors] - with id: "<( + mNode, executorDetails.mController, subrobot->getMetaSkeleton()->getDofs()); + auto jvExec + = std::make_shared( + handEnd, velExec); + auto controllerMode = util::modeFromString( + mNode.param("/" + executorDetails.mController + "/controller_mode", "VELOCITY")); + subrobot->registerExecutor(jvExec, executorDetails.mId, executorDetails.mController, controllerMode); + } + else if(executorDetails.mMode == "EFFORT") + { + auto effExec + = std::make_shared( + mNode, executorDetails.mController, subrobot->getMetaSkeleton()->getDofs()); + auto effJacExec + = std::make_shared( + handEnd, effExec); + auto controllerMode = util::modeFromString( + mNode.param("/" + executorDetails.mController + "/controller_mode", "EFFORT")); + subrobot->registerExecutor(effJacExec, executorDetails.mId, executorDetails.mController, controllerMode); + } + else + { + std::stringstream message; + message << "Executor mode for [/" << ns << "/executors] - with id: "<( + mNode, executorDetails.mController, subrobot->getMetaSkeleton()->getDofs()); + auto jvExec + = std::make_shared( + handEnd, velExec); + auto vsExec + = std::make_shared( + handEnd, jvExec); + auto controllerMode = util::modeFromString( + mNode.param("/" + executorDetails.mController + "/controller_mode", "VELOCITY")); + subrobot->registerExecutor(vsExec, executorDetails.mId, executorDetails.mController, controllerMode); + } + else + { + std::stringstream message; + message << "Executor mode for [/" << ns << "/executors] - with id: "<( std::bind(&Ada::spin, this), threadCycle); + + std::cout<<"Thread created!"<activateExecutor(aikido::control::ExecutorType::TRAJECTORY); + std::cout<<"Activated trajectory executor!"<{mArmTrajControllerName, mHandTrajControllerName}, - std::vector()); -} - -//============================================================================== -bool Ada::stopTrajectoryControllers() -{ - cancelAllCommands(); - return switchControllers( - std::vector(), - std::vector{mArmTrajControllerName, - mHandTrajControllerName}); -} - //============================================================================== Eigen::VectorXd Ada::getVelocityLimits(bool armOnly) const { @@ -414,66 +564,6 @@ Eigen::VectorXd Ada::getAccelerationLimits(bool armOnly) const : mSoftAccelerationLimits; } -//============================================================================== -void Ada::createTrajectoryExecutor(bool isHand) -{ - std::string controller - = isHand ? mHandTrajControllerName : mArmTrajControllerName; - aikido::robot::RobotPtr subrobot = isHand ? mHandRobot : mArm; - using aikido::control::KinematicSimulationTrajectoryExecutor; - using aikido::control::ros::RosTrajectoryExecutor; - - if (mSimulation) - { - auto id = subrobot->registerExecutor( - std::make_shared( - subrobot->getMetaSkeleton())); - if (!subrobot->activateExecutor(id)) - throw std::runtime_error("Could not activate arm executor"); - } - else - { - std::string serverName = controller + "/follow_joint_trajectory"; - auto exec = std::make_shared( - mNode, - serverName, - DEFAULT_ROS_TRAJ_INTERP_TIME, - DEFAULT_ROS_TRAJ_GOAL_TIME_TOL, - subrobot->getMetaSkeleton()); - auto id = subrobot->registerExecutor(exec); - if (!subrobot->activateExecutor(id)) - throw std::runtime_error("Could not activate arm executor"); - } -} - -//============================================================================== -bool Ada::switchControllers( - const std::vector& startControllers, - const std::vector& stopControllers) -{ - if (!mNode.ok()) - throw std::runtime_error("Ros is not active."); - - if (!mControllerServiceClient) - throw std::runtime_error("ServiceClient not instantiated."); - - controller_manager_msgs::SwitchController srv; - // First try stopping the started controllers - // Avoids us falsely detecting a failure if already started - srv.request.stop_controllers = startControllers; - srv.request.strictness - = controller_manager_msgs::SwitchControllerRequest::BEST_EFFORT; - mControllerServiceClient->call(srv); // Don't care about response code - - // Actual command - srv.request.start_controllers = startControllers; - srv.request.stop_controllers = stopControllers; - srv.request.strictness - = controller_manager_msgs::SwitchControllerRequest::STRICT; - - return mControllerServiceClient->call(srv) && srv.response.ok; -} - //============================================================================== std::future Ada::openHand() { diff --git a/src/util.cpp b/src/util.cpp index b04b3dc..38ed61e 100644 --- a/src/util.cpp +++ b/src/util.cpp @@ -22,7 +22,7 @@ void waitForUser(const std::string& msg, const std::shared_ptr& ada) if (input == 'n') { ROS_INFO_STREAM("Aborting with user input " << input); - ada->stopTrajectoryControllers(); + ada->deactivateExecutor(); exit(1); } } @@ -79,5 +79,104 @@ Eigen::MatrixXd createBwMatrixForTSR( return bw; } +//============================================================================== +hardware_interface::JointCommandModes modeFromString(std::string str) +{ + if (str == "BEGIN") + return hardware_interface::JointCommandModes::BEGIN; + if (str == "POSITION" || str == "MODE_POSITION") + return hardware_interface::JointCommandModes::MODE_POSITION; + if (str == "VELOCITY" || str == "MODE_VELOCITY") + return hardware_interface::JointCommandModes::MODE_VELOCITY; + if (str == "EFFORT" || str == "MODE_EFFORT") + return hardware_interface::JointCommandModes::MODE_EFFORT; + if (str == "NOMODE") + return hardware_interface::JointCommandModes::NOMODE; + if (str == "ESTOP" || str == "EMERGENCY_STOP") + return hardware_interface::JointCommandModes::EMERGENCY_STOP; + if (str == "SWITCHING") + return hardware_interface::JointCommandModes::SWITCHING; + + return hardware_interface::JointCommandModes::ERROR; +} + +//============================================================================= +std::vector loadExecutorsDetailsFromParameter(const ros::NodeHandle &nodeHandle, const std::string ¶meterName) +{ + using XmlRpc::XmlRpcValue; + + static const std::vector emptyResult; + + XmlRpcValue executorsDetailsXml; + std::cout<<"parameterName: "< output; + for (int i = 0; i < executorsDetailsXml.size(); ++i) { + ExecutorDetails executorDetails; + auto &executorDetailsXml = executorsDetailsXml[i]; + + if (executorDetailsXml.getType() == XmlRpcValue::TypeStruct) { + + auto &idXml = executorDetailsXml["id"]; + if (idXml.getType() != XmlRpcValue::TypeString) { + ROS_ERROR_STREAM("Parameter '" << nodeHandle.getNamespace() + << "/executors[" << i + << "]/id' is not a string."); + return emptyResult; + } + executorDetails.mId = static_cast(idXml); + + auto &typeXml = executorDetailsXml["type"]; + if (typeXml.getType() != XmlRpcValue::TypeString) { + ROS_ERROR_STREAM("Parameter '" << nodeHandle.getNamespace() + << "/executors[" << i + << "]/type' is not a string."); + return emptyResult; + } + executorDetails.mType = static_cast(typeXml); + + if(executorDetails.mType != "TRAJECTORY") + { + auto &modeXml = executorDetailsXml["mode"]; + if (modeXml.getType() != XmlRpcValue::TypeString) { + ROS_ERROR_STREAM("Parameter '" << nodeHandle.getNamespace() + << "/executors[" << i + << "]/mode' is not a string."); + return emptyResult; + } + executorDetails.mMode = static_cast(modeXml); + } + + auto &controllerXml = executorDetailsXml["controller"]; + if (controllerXml.getType() != XmlRpcValue::TypeString) { + ROS_ERROR_STREAM("Parameter '" << nodeHandle.getNamespace() + << "/executors[" << i + << "]/controller' is not a string."); + return emptyResult; + } + executorDetails.mController = static_cast(controllerXml); + + } else { + ROS_ERROR_STREAM("Parameter '" << nodeHandle.getNamespace() << "/executors[" + << i << "]' is not a struct."); + return emptyResult; + } + + output.emplace_back(executorDetails); + } + + return output; +} + } // namespace util } // namespace ada