From 75a865058a16cc82772c5cc56d0dcb8517d876e9 Mon Sep 17 00:00:00 2001 From: RKJenamani Date: Wed, 11 May 2022 18:37:14 -0400 Subject: [PATCH 01/13] Removed controller and executor functions --- CMakeLists.txt | 2 +- include/libada/Ada.hpp | 35 ++---------- src/Ada.cpp | 119 +++-------------------------------------- src/util.cpp | 2 +- 4 files changed, 12 insertions(+), 146 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ff29c22..59120cf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -148,7 +148,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/include/libada/Ada.hpp b/include/libada/Ada.hpp index 0186201..b287cae 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -100,7 +100,9 @@ class Ada final : public aikido::robot::ros::RosRobot const ::ros::NodeHandle* node = nullptr, aikido::common::RNG::result_type rngSeed = std::random_device{}(), const dart::common::ResourceRetrieverPtr& retriever - = std::make_shared()); + = std::make_shared(), + const std::string rosControllerManagerServerName = "", + const std::string rosJointModeServerName = ""); virtual ~Ada(); @@ -162,16 +164,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,39 +180,18 @@ 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; Eigen::VectorXd mDefaultVelocityLimits; Eigen::VectorXd mDefaultAccelerationLimits; - // Ros node associated with this robot. - std::unique_ptr<::ros::NodeHandle> mNode; - - // Ros controller service client. - std::unique_ptr<::ros::ServiceClient> mControllerServiceClient; - // Ros joint state client. std::unique_ptr mJointStateClient; diff --git a/src/Ada.cpp b/src/Ada.cpp index 6aaad9e..96e6525 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -77,12 +77,17 @@ Ada::Ada( const std::chrono::milliseconds threadCycle, const ::ros::NodeHandle* node, aikido::common::RNG::result_type rngSeed, - const dart::common::ResourceRetrieverPtr& retriever) + const dart::common::ResourceRetrieverPtr& retriever, + const std::string rosControllerManagerServerName, + const std::string rosJointModeServerName) : aikido::robot::ros::RosRobot( internal::getDartURI(confNamespace, "default_urdf", DEFAULT_URDF), internal::getDartURI(confNamespace, "default_srdf", DEFAULT_SRDF), "ada", - retriever) + retriever, + node, + rosControllerManagerServerName, + rosJointModeServerName) , mSimulation(simulation) { @@ -91,16 +96,6 @@ Ada::Ada( setWorld(env); setRNG(std::make_unique>(rngSeed)); - // Set up ROS Node - if (!node) - { - mNode = std::make_unique<::ros::NodeHandle>(); - } - else - { - mNode = std::make_unique<::ros::NodeHandle>(*node); - } - // Read default soft accleration/velocity limits double limit; mNode->param("/" + confNamespace + "/default_accel_lim", limit, 0); @@ -156,28 +151,6 @@ Ada::Ada( 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. - setTrajectoryExecutor(nullptr); - - // 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)) @@ -203,10 +176,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(), @@ -368,24 +337,6 @@ aikido::trajectory::TrajectoryPtr Ada::computeArmJointSpacePath( return traj; } -//============================================================================== -bool Ada::startTrajectoryControllers() -{ - return switchControllers( - std::vector{mArmTrajControllerName, mHandTrajControllerName}, - std::vector()); -} - -//============================================================================== -bool Ada::stopTrajectoryControllers() -{ - cancelAllTrajectories(); - return switchControllers( - std::vector(), - std::vector{mArmTrajControllerName, - mHandTrajControllerName}); -} - //============================================================================== Eigen::VectorXd Ada::getVelocityLimits(bool armOnly) const { @@ -402,62 +353,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) - { - subrobot->setTrajectoryExecutor( - std::make_shared( - subrobot->getMetaSkeleton())); - } - 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()); - subrobot->setTrajectoryExecutor(exec); - } -} - -//============================================================================== -bool Ada::switchControllers( - const std::vector& startControllers, - const std::vector& stopControllers) -{ - if (!mNode) - throw std::runtime_error("Ros node has not been instantiated."); - - 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..da7353e 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); } } From 3c5af3f2b96c9692b240292a65b08ea4b6dcf573 Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Thu, 12 May 2022 14:08:34 -0700 Subject: [PATCH 02/13] Added bindings to new executor/controller switching framework --- config/gen2_6dof.yaml | 23 ++++-- include/libada/Ada.hpp | 45 +++++++++-- include/libada/util.hpp | 5 ++ src/Ada.cpp | 169 ++++++++++++++++++++++++++++++++++++++-- src/util.cpp | 21 +++++ 5 files changed, 241 insertions(+), 22 deletions(-) diff --git a/config/gen2_6dof.yaml b/config/gen2_6dof.yaml index 3640824..06f8cc4 100644 --- a/config/gen2_6dof.yaml +++ b/config/gen2_6dof.yaml @@ -7,9 +7,19 @@ adaConf: end_effector: j2n6s200_end_effector arm: [j2n6s200_link_base, j2n6s200_link_6] hand_base: j2n6s200_hand_base - arm_controller: trajectory_controller - hand_controller: hand_controller named_configs: package://libada/resources/g2_6d_named_configs.yaml + arm_controllers: + # Note: last-controller is default when calling setActiveExecutor(ExecutorType) + traj_controllers: ["trajectory_controller"] + pos_controllers: ["position_controller"] + vel_controllers: ["velocity_controller"] + eff_controllers: ["effort_controller"] + hand_controllers: + # Note: last-controller is default when calling setActiveExecutor(ExecutorType) + traj_controllers: ["hand_controller"] + pos_controllers: ["hand_position_controller"] + vel_controllers: ["hand_velocity_controller"] + eff_controllers: ["hand_effort_controller"] # hardware parameters soft_limits: @@ -22,7 +32,6 @@ joint_state_controller: # whole-arm joint mode controller joint_mode_controller: - mode: MODE type: pr_ros_controllers/JointModeController default: VELOCITY joints: [joint_mode] @@ -50,7 +59,7 @@ effort_controller: # whole-arm base trajectory controller trajectory_controller: - mode: TRAJECTORY + mode: VELOCITY type: velocity_controllers/JointTrajectoryController joints: [j2n6s200_joint_1, j2n6s200_joint_2, j2n6s200_joint_3, j2n6s200_joint_4, j2n6s200_joint_5, j2n6s200_joint_6] @@ -84,7 +93,7 @@ trajectory_controller: # whole-arm base trajectory controller move_until_touch_topic_controller: - mode: TRAJECTORY + mode: VELOCITY type: rewd_controllers/MoveUntilTouchTopicController # Forque Sensor forcetorque_wrench_name: /forque/forqueSensor @@ -129,7 +138,7 @@ move_until_touch_topic_controller: # Should do the same thing as trajectory_controller, but this is # the lab's rewd_controllers implementation rewd_trajectory_controller: - mode: TRAJECTORY + mode: VELOCITY type: rewd_controllers/JointTrajectoryController joints: [j2n6s200_joint_1, j2n6s200_joint_2, j2n6s200_joint_3, j2n6s200_joint_4, j2n6s200_joint_5, j2n6s200_joint_6] @@ -183,7 +192,7 @@ hand_effort_controller: joints: [j2n6s200_joint_finger_1, j2n6s200_joint_finger_2] hand_controller: - mode: TRAJECTORY + mode: VELOCITY type: velocity_controllers/JointTrajectoryController joints: [j2n6s200_joint_finger_1, j2n6s200_joint_finger_2] constraints: diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index b287cae..8aea633 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -12,7 +12,11 @@ #include #include #include -#include +#include +#include +#include +#include +#include #include #include #include @@ -97,7 +101,7 @@ class Ada final : public aikido::robot::ros::RosRobot aikido::planner::WorldPtr env = aikido::planner::World::create(), const std::string confNamespace = DEFAULT_CONF_OBJ_NS, const std::chrono::milliseconds threadCycle = DEFAULT_THREAD_CYCLE, - const ::ros::NodeHandle* node = nullptr, + std::shared_ptr<::ros::NodeHandle> node = nullptr, aikido::common::RNG::result_type rngSeed = std::random_device{}(), const dart::common::ResourceRetrieverPtr& retriever = std::make_shared(), @@ -112,16 +116,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(); @@ -179,6 +183,31 @@ class Ada final : public aikido::robot::ros::RosRobot /// Get Body Node of End Effector dart::dynamics::BodyNodePtr getEndEffectorBodyNode(); + /************ Convenience Execution Methods **************/ + + /// + /// Convenience: executes given SE3 command on end-effector. + /// Will auto-activate a JacobianVelocityExecutor. + /// future will have exception if one doesn't exist + /// + /// \param[in] command SE3, applied to end-effector + /// \param[in] timeout Timeout for the command + std::future executeJacobianCommand( + const Eigen::Vector6d command, + const std::chrono::duration& timeout); + + /// + /// Convenience: runs velocity-command Visual Servoing + /// Will auto-activate a VisualServoingVelocityExecutor. + /// future will have exception if one doesn't exist. + /// + /// \param[in] command SE3, applied to end-effector + /// \param[in] timeout Timeout for the command + std::future executeVisualServoing( + std::function(void)> perception, + aikido::control::VisualServoingVelocityExecutor::Properties properties + = aikido::control::VisualServoingVelocityExecutor::Properties()); + private: // Call to spin first to pass current time to step void spin(); @@ -200,10 +229,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..86745ff 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,9 @@ Eigen::MatrixXd createBwMatrixForTSR( double pitchTolerance, double yawTolerance); +/// Get jointcommandmode enum from string +hardware_interface::JointCommandModes modeFromString(std::string str); + } // namespace util } // namespace ada diff --git a/src/Ada.cpp b/src/Ada.cpp index 96e6525..fa0ac1e 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -75,7 +76,7 @@ Ada::Ada( aikido::planner::WorldPtr env, const std::string confNamespace, const std::chrono::milliseconds threadCycle, - const ::ros::NodeHandle* node, + std::shared_ptr<::ros::NodeHandle> node, aikido::common::RNG::result_type rngSeed, const dart::common::ResourceRetrieverPtr& retriever, const std::string rosControllerManagerServerName, @@ -121,7 +122,8 @@ 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")); if (!mArm) { throw std::runtime_error("Could not create arm"); @@ -145,7 +147,8 @@ 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")); if (!mHandRobot) { throw std::runtime_error("Could not create hand"); @@ -192,6 +195,113 @@ 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 + { + // Real ROS Executors + for (auto subrobot : + std::set{mArm, mHandRobot}) + { + bool isHand = (subrobot == mHandRobot); + std::string ns = isHand ? "/" + confNamespace + "/hand_controllers/" + : "/" + confNamespace + "/arm_controllers/"; + // Trajectory executors + auto controllerNames = mNode->param>( + ns + "traj_controllers", + std::vector{isHand ? DEFAULT_HAND_TRAJ_CTRL + : DEFAULT_ARM_TRAJ_CTRL}); + for (auto name : controllerNames) + { + auto trajExec + = std::make_shared( + *mNode, + name + "/follow_joint_trajectory", + DEFAULT_ROS_TRAJ_INTERP_TIME, + DEFAULT_ROS_TRAJ_GOAL_TIME_TOL, + subrobot->getMetaSkeleton()); + auto mode = util::modeFromString( + mNode->param("/" + name + "/mode", "VELOCITY")); + subrobot->registerExecutor(trajExec, name, mode); + } + + // Position Executors + controllerNames = mNode->param>( + ns + "pos_controllers", std::vector()); + for (auto name : controllerNames) + { + auto posExec + = std::make_shared( + *mNode, name, subrobot->getMetaSkeleton()->getDofs()); + auto mode = util::modeFromString( + mNode->param("/" + name + "/mode", "POSITION")); + subrobot->registerExecutor(posExec, name, mode); + } + + // Effort Executors + controllerNames = mNode->param>( + ns + "eff_controllers", std::vector()); + for (auto name : controllerNames) + { + auto effExec + = std::make_shared( + *mNode, name, subrobot->getMetaSkeleton()->getDofs()); + auto effJacExec + = std::make_shared( + handEnd, effExec); + auto mode = util::modeFromString( + mNode->param("/" + name + "/mode", "EFFORT")); + subrobot->registerExecutor(effJacExec, name, mode); + } + + // Velocity Executors + controllerNames = mNode->param>( + ns + "vel_controllers", std::vector()); + for (auto name : controllerNames) + { + auto velExec + = std::make_shared( + *mNode, name, subrobot->getMetaSkeleton()->getDofs()); + auto jvExec + = std::make_shared( + handEnd, velExec); + auto vsExec + = std::make_shared( + handEnd, jvExec); + auto mode = util::modeFromString( + mNode->param("/" + name + "/mode", "VELOCITY")); + subrobot->registerExecutor(vsExec, name, mode); + subrobot->registerExecutor(jvExec, name, mode); + } + } + } + + // Activate Trajectory Executor + activateExecutor(aikido::control::ExecutorType::TRAJECTORY); + // Start driving self-thread mThread = std::make_unique( std::bind(&Ada::spin, this), threadCycle); @@ -250,25 +360,25 @@ void Ada::step(const std::chrono::system_clock::time_point& timepoint) } //============================================================================== -aikido::robot::RobotPtr Ada::getArm() +aikido::robot::ros::RosRobotPtr Ada::getArm() { return mArm; } //============================================================================== -aikido::robot::ConstRobotPtr Ada::getArm() const +aikido::robot::ros::ConstRosRobotPtr Ada::getArm() const { return mArm; } //============================================================================== -aikido::robot::RobotPtr Ada::getHandRobot() +aikido::robot::ros::RosRobotPtr Ada::getHandRobot() { return mHandRobot; } //============================================================================== -aikido::robot::ConstRobotPtr Ada::getHandRobot() const +aikido::robot::ros::ConstRosRobotPtr Ada::getHandRobot() const { return mHandRobot; } @@ -371,4 +481,49 @@ dart::dynamics::BodyNodePtr Ada::getEndEffectorBodyNode() return internal::getBodyNodeOrThrow(mMetaSkeleton, mEndEffectorName); } +//============================================================================== +std::future Ada::executeJacobianCommand( + const Eigen::Vector6d command, const std::chrono::duration& timeout) +{ + std::shared_ptr executor; + // Search for last added executor of given type + for (int i = mExecutors.size() - 1; i >= 0; i--) + { + executor + = std::dynamic_pointer_cast( + mExecutors[i]); + if (executor && activateExecutor(i)) + { + return executor->execute(command, timeout); + } + } + + deactivateExecutor(); + return aikido::control::make_exceptional_future( + "No JacobianVelocityExecutor registered."); +} + +//============================================================================== +std::future Ada::executeVisualServoing( + std::function(void)> perception, + aikido::control::VisualServoingVelocityExecutor::Properties properties) +{ + std::shared_ptr executor; + // Search for last added executor of given type + for (int i = mExecutors.size() - 1; i >= 0; i--) + { + executor = std::dynamic_pointer_cast< + aikido::control::VisualServoingVelocityExecutor>(mExecutors[i]); + if (executor && activateExecutor(i)) + { + executor->resetProperties(properties); + return executor->execute(perception); + } + } + + deactivateExecutor(); + return aikido::control::make_exceptional_future( + "No JacobianVelocityExecutor registered."); +} + } // namespace ada diff --git a/src/util.cpp b/src/util.cpp index da7353e..e303b88 100644 --- a/src/util.cpp +++ b/src/util.cpp @@ -79,5 +79,26 @@ 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; +} + } // namespace util } // namespace ada From 0a0032dee481535752c941908fd3489ec5e11d6c Mon Sep 17 00:00:00 2001 From: RKJenamani Date: Fri, 20 May 2022 02:37:22 -0400 Subject: [PATCH 03/13] Changed logic for mode controller and ros service controller clients --- config/gen2_6dof.yaml | 3 +++ config/gen3_6dof.yaml | 25 +++++++++++++++++++++++-- include/libada/Ada.hpp | 4 +--- src/Ada.cpp | 37 +++++++++++++++++++++++++++++++------ 4 files changed, 58 insertions(+), 11 deletions(-) diff --git a/config/gen2_6dof.yaml b/config/gen2_6dof.yaml index 06f8cc4..74a3ba4 100644 --- a/config/gen2_6dof.yaml +++ b/config/gen2_6dof.yaml @@ -9,6 +9,8 @@ adaConf: hand_base: j2n6s200_hand_base named_configs: package://libada/resources/g2_6d_named_configs.yaml arm_controllers: + enable_controller_switching: true + mode_controller: joint_mode_controller # Note: last-controller is default when calling setActiveExecutor(ExecutorType) traj_controllers: ["trajectory_controller"] pos_controllers: ["position_controller"] @@ -21,6 +23,7 @@ adaConf: vel_controllers: ["hand_velocity_controller"] eff_controllers: ["hand_effort_controller"] + # hardware parameters soft_limits: eff: [16,16,16,10,10,10,1.3,1.3] diff --git a/config/gen3_6dof.yaml b/config/gen3_6dof.yaml index 3388397..9525fd8 100644 --- a/config/gen3_6dof.yaml +++ b/config/gen3_6dof.yaml @@ -7,9 +7,23 @@ adaConf: end_effector: robotiq_arg2f_base_link arm: [base_link, bracelet_link] hand_base: end_effector_link - arm_controller: trajectory_controller - hand_controller: hand_controller named_configs: package://libada/resources/g2_6d_named_configs.yaml + arm_controllers: + enable_controller_switching: true + mode_controller: joint_mode_controller + # Note: last-controller is default when calling setActiveExecutor(ExecutorType) + traj_controllers: ["trajectory_controller"] + pos_controllers: ["position_controller"] + vel_controllers: ["velocity_controller"] + eff_controllers: ["effort_controller"] + hand_controllers: + enable_controller_switching: true + mode_controller: hand_mode_controller + # Note: last-controller is default when calling setActiveExecutor(ExecutorType) + traj_controllers: ["hand_controller"] + pos_controllers: ["hand_position_controller"] + vel_controllers: ["hand_velocity_controller"] + eff_controllers: ["hand_effort_controller"] # Publish all joint states ----------------------------------- joint_state_controller: @@ -160,6 +174,13 @@ rewd_trajectory_controller: ############################################## +# gripper base mode controller +hand_mode_controller: + mode: MODE + type: pr_ros_controllers/JointModeController + default: VELOCITY + joints: [finger_mode] + # gripper base velocity controller hand_velocity_controller: mode: VELOCITY diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 8aea633..7625c43 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -104,9 +104,7 @@ class Ada final : public aikido::robot::ros::RosRobot std::shared_ptr<::ros::NodeHandle> node = nullptr, aikido::common::RNG::result_type rngSeed = std::random_device{}(), const dart::common::ResourceRetrieverPtr& retriever - = std::make_shared(), - const std::string rosControllerManagerServerName = "", - const std::string rosJointModeServerName = ""); + = std::make_shared()); virtual ~Ada(); diff --git a/src/Ada.cpp b/src/Ada.cpp index fa0ac1e..76e2129 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -78,17 +78,13 @@ Ada::Ada( const std::chrono::milliseconds threadCycle, std::shared_ptr<::ros::NodeHandle> node, aikido::common::RNG::result_type rngSeed, - const dart::common::ResourceRetrieverPtr& retriever, - const std::string rosControllerManagerServerName, - const std::string rosJointModeServerName) + const dart::common::ResourceRetrieverPtr& retriever) : aikido::robot::ros::RosRobot( internal::getDartURI(confNamespace, "default_urdf", DEFAULT_URDF), internal::getDartURI(confNamespace, "default_srdf", DEFAULT_SRDF), "ada", retriever, - node, - rosControllerManagerServerName, - rosJointModeServerName) + node) , mSimulation(simulation) { @@ -222,13 +218,42 @@ Ada::Ada( } else { + auto ros_controller_service_client = std::make_shared<::ros::ServiceClient>( + mNode->serviceClient( + "controller_manager/switch_controller")); + // Real ROS Executors for (auto subrobot : std::set{mArm, mHandRobot}) { + bool isHand = (subrobot == mHandRobot); std::string ns = isHand ? "/" + confNamespace + "/hand_controllers/" : "/" + confNamespace + "/arm_controllers/"; + + // Controller Switching Service Client + bool enableControllerSwitching = mNode->param( + ns + "enable_controller_switching", + false); + + if(enableControllerSwitching) + subrobot->setRosControllerServiceClient(ros_controller_service_client); + + // Mode controller + auto modeControllerName = mNode->param( + ns + "mode_controller", + std::string("")); + + if(!modeControllerName.empty()) + { + auto rosJointModeCommandClient + = std::make_shared( + *mNode, + modeControllerName + "/joint_mode_command", + std::vector{"joint_mode"}); + subrobot->setRosJointModeCommandClient(rosJointModeCommandClient); + } + // Trajectory executors auto controllerNames = mNode->param>( ns + "traj_controllers", From 46781df7a19a0d54b91c3bcce0bd56e46138b4bb Mon Sep 17 00:00:00 2001 From: RKJenamani Date: Sun, 22 May 2022 23:07:58 -0400 Subject: [PATCH 04/13] Updated bindings for new executor framework with string ids and executor map --- config/gen3_6dof.yaml | 22 +++-- include/libada/util.hpp | 10 +++ src/Ada.cpp | 187 ++++++++++++++++++++++++++-------------- src/util.cpp | 77 +++++++++++++++++ 4 files changed, 220 insertions(+), 76 deletions(-) diff --git a/config/gen3_6dof.yaml b/config/gen3_6dof.yaml index 9525fd8..d291433 100644 --- a/config/gen3_6dof.yaml +++ b/config/gen3_6dof.yaml @@ -8,22 +8,26 @@ adaConf: arm: [base_link, bracelet_link] hand_base: end_effector_link named_configs: package://libada/resources/g2_6d_named_configs.yaml - arm_controllers: + 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) - traj_controllers: ["trajectory_controller"] - pos_controllers: ["position_controller"] - vel_controllers: ["velocity_controller"] - eff_controllers: ["effort_controller"] + executors: + - {id: bite_acquisition_trajectory_executor, type: TRAJECTORY, mode: VELOCITY, controller: move_until_touch_topic_controller} + - {id: bite_transfer_trajectory_executor, type: TRAJECTORY, mode: EFFORT, controller: trajectory_controller} + - {id: bite_transfer_visual_servoing_executor, type: VISUAL_SERVOING, mode: EFFORT, mode: controller: effort_controller} + - {id: bite_transfer_inside_mouth_executor, type: TASK_COMMAND, mode: EFFORT, controller: effort_controller} hand_controllers: 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) - traj_controllers: ["hand_controller"] - pos_controllers: ["hand_position_controller"] - vel_controllers: ["hand_velocity_controller"] - eff_controllers: ["hand_effort_controller"] + executors: + - {id: forque_pickup_executor, type: JOINT_COMMAND, mode: POSITION, controller: hand_position_controller} # Publish all joint states ----------------------------------- joint_state_controller: diff --git a/include/libada/util.hpp b/include/libada/util.hpp index 86745ff..e8b7632 100644 --- a/include/libada/util.hpp +++ b/include/libada/util.hpp @@ -65,6 +65,16 @@ Eigen::MatrixXd createBwMatrixForTSR( /// 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/src/Ada.cpp b/src/Ada.cpp index 76e2129..5352144 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -254,72 +254,125 @@ Ada::Ada( subrobot->setRosJointModeCommandClient(rosJointModeCommandClient); } - // Trajectory executors - auto controllerNames = mNode->param>( - ns + "traj_controllers", - std::vector{isHand ? DEFAULT_HAND_TRAJ_CTRL - : DEFAULT_ARM_TRAJ_CTRL}); - for (auto name : controllerNames) + 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) { - auto trajExec + if(executorDetails.mType == "TRAJECTORY") + { + auto trajExec = std::make_shared( *mNode, - name + "/follow_joint_trajectory", + executorDetails.mController + "/follow_joint_trajectory", DEFAULT_ROS_TRAJ_INTERP_TIME, DEFAULT_ROS_TRAJ_GOAL_TIME_TOL, subrobot->getMetaSkeleton()); - auto mode = util::modeFromString( - mNode->param("/" + name + "/mode", "VELOCITY")); - subrobot->registerExecutor(trajExec, name, mode); - } - - // Position Executors - controllerNames = mNode->param>( - ns + "pos_controllers", std::vector()); - for (auto name : controllerNames) - { - auto posExec - = std::make_shared( - *mNode, name, subrobot->getMetaSkeleton()->getDofs()); - auto mode = util::modeFromString( - mNode->param("/" + name + "/mode", "POSITION")); - subrobot->registerExecutor(posExec, name, mode); - } - - // Effort Executors - controllerNames = mNode->param>( - ns + "eff_controllers", std::vector()); - for (auto name : controllerNames) - { - auto effExec - = std::make_shared( - *mNode, name, subrobot->getMetaSkeleton()->getDofs()); - auto effJacExec - = std::make_shared( - handEnd, effExec); - auto mode = util::modeFromString( - mNode->param("/" + name + "/mode", "EFFORT")); - subrobot->registerExecutor(effJacExec, name, mode); - } - - // Velocity Executors - controllerNames = mNode->param>( - ns + "vel_controllers", std::vector()); - for (auto name : controllerNames) - { - auto velExec - = std::make_shared( - *mNode, name, subrobot->getMetaSkeleton()->getDofs()); - auto jvExec - = std::make_shared( - handEnd, velExec); - auto vsExec - = std::make_shared( - handEnd, jvExec); - auto mode = util::modeFromString( - mNode->param("/" + name + "/mode", "VELOCITY")); - subrobot->registerExecutor(vsExec, name, mode); - subrobot->registerExecutor(jvExec, name, mode); + auto controllerMode = util::modeFromString( + mNode->param("/" + executorDetails.mController + "/mode", "")); + subrobot->registerExecutor(trajExec, executorDetails.mController, controllerMode, executorDetails.mId); + } + 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 + "/mode", "POSITION")); + subrobot->registerExecutor(posExec, executorDetails.mController, controllerMode, executorDetails.mId); + } + else if(executorDetails.mMode == "VELOCITY") + { + auto velExec + = std::make_shared( + *mNode, executorDetails.mController, subrobot->getMetaSkeleton()->getDofs()); + auto controllerMode = util::modeFromString( + mNode->param("/" + executorDetails.mController + "/mode", "VELOCITY")); + subrobot->registerExecutor(velExec, executorDetails.mController, controllerMode, executorDetails.mId); + } + else if(executorDetails.mMode == "EFFORT") + { + auto effExec + = std::make_shared( + *mNode, executorDetails.mController, subrobot->getMetaSkeleton()->getDofs()); + auto controllerMode = util::modeFromString( + mNode->param("/" + executorDetails.mController + "/mode", "EFFORT")); + subrobot->registerExecutor(effExec, executorDetails.mController, controllerMode, executorDetails.mId); + } + 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 + "/mode", "VELOCITY")); + subrobot->registerExecutor(jvExec, executorDetails.mController, controllerMode, executorDetails.mId); + } + 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 + "/mode", "EFFORT")); + subrobot->registerExecutor(effJacExec, executorDetails.mController, controllerMode, executorDetails.mId); + } + 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 + "/mode", "VELOCITY")); + subrobot->registerExecutor(vsExec, executorDetails.mController, controllerMode, executorDetails.mId); + } + else + { + std::stringstream message; + message << "Executor mode for [/" << ns << "/executors] - with id: "< Ada::executeJacobianCommand( const Eigen::Vector6d command, const std::chrono::duration& timeout) { std::shared_ptr executor; - // Search for last added executor of given type - for (int i = mExecutors.size() - 1; i >= 0; i--) + // Search for last added executor of given type + for (auto id = mExecutorsInsertionOrder.rbegin(); id != mExecutorsInsertionOrder.rend(); id++) { executor = std::dynamic_pointer_cast( - mExecutors[i]); - if (executor && activateExecutor(i)) + mExecutors[*id]); + if (executor && activateExecutor(*id)) { return executor->execute(command, timeout); } @@ -535,11 +588,11 @@ std::future Ada::executeVisualServoing( { std::shared_ptr executor; // Search for last added executor of given type - for (int i = mExecutors.size() - 1; i >= 0; i--) + for (auto id = mExecutorsInsertionOrder.rbegin(); id != mExecutorsInsertionOrder.rend(); id++) { executor = std::dynamic_pointer_cast< - aikido::control::VisualServoingVelocityExecutor>(mExecutors[i]); - if (executor && activateExecutor(i)) + aikido::control::VisualServoingVelocityExecutor>(mExecutors[*id]); + if (executor && activateExecutor(*id)) { executor->resetProperties(properties); return executor->execute(perception); diff --git a/src/util.cpp b/src/util.cpp index e303b88..813ad78 100644 --- a/src/util.cpp +++ b/src/util.cpp @@ -100,5 +100,82 @@ hardware_interface::JointCommandModes modeFromString(std::string str) 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; + if (!nodeHandle.getParam(parameterName, executorsDetailsXml)) { + ROS_ERROR_STREAM("Parameter '" << nodeHandle.getNamespace() + << "/executors' is required."); + } + + if (executorsDetailsXml.getType() != XmlRpcValue::TypeArray) { + ROS_ERROR_STREAM("Parameter '" << nodeHandle.getNamespace() + << "/executors' is not an array."); + return emptyResult; + } + + std::vector 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 From 835ec6269ddf0a8d4a1aa52ceda8aae0aa218b7c Mon Sep 17 00:00:00 2001 From: RKJenamani Date: Mon, 23 May 2022 10:45:39 -0400 Subject: [PATCH 05/13] Added functionality to set trajectory limits from param server + controllers for bite transfer --- config/gen3_6dof.yaml | 106 +++++++++++++++++++++++++++++++++++++++-- include/libada/Ada.hpp | 2 + src/Ada.cpp | 20 ++++++++ 3 files changed, 123 insertions(+), 5 deletions(-) diff --git a/config/gen3_6dof.yaml b/config/gen3_6dof.yaml index d291433..a19c6c8 100644 --- a/config/gen3_6dof.yaml +++ b/config/gen3_6dof.yaml @@ -2,8 +2,12 @@ adaConf: default_urdf: package://kortex_description/robots/gen3_6dof_vision.urdf default_srdf: package://kortex_description/robots/gen3_6dof.srdf - default_accel_lim: 2.0 - default_vel_lim: 0.7 + default_accel_lim: + aquisition: 2.0 + transfer: 1.0 + default_vel_lim: + acquisition: 0.7 + transfer: 0.35 end_effector: robotiq_arg2f_base_link arm: [base_link, bracelet_link] hand_base: end_effector_link @@ -16,10 +20,10 @@ adaConf: # Note: type TRAJECTORY does not require mode specification # Note: last-controller is default when calling setActiveExecutor(ExecutorType) executors: - - {id: bite_acquisition_trajectory_executor, type: TRAJECTORY, mode: VELOCITY, controller: move_until_touch_topic_controller} - - {id: bite_transfer_trajectory_executor, type: TRAJECTORY, mode: EFFORT, controller: trajectory_controller} + - {id: bite_transfer_trajectory_executor, type: TRAJECTORY, mode: EFFORT, controller: towards_mouth_bite_transfer_controller} - {id: bite_transfer_visual_servoing_executor, type: VISUAL_SERVOING, mode: EFFORT, mode: controller: effort_controller} - - {id: bite_transfer_inside_mouth_executor, type: TASK_COMMAND, mode: EFFORT, controller: effort_controller} + - {id: bite_transfer_inside_mouth_executor, type: TRAJECTORY, mode: EFFORT, controller: inside_mouth_bite_transfer_controller} + - {id: bite_acquisition_trajectory_executor, type: TRAJECTORY, mode: VELOCITY, controller: move_until_touch_topic_controller} hand_controllers: enable_controller_switching: true mode_controller: hand_mode_controller @@ -139,6 +143,98 @@ 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 +towards_mouth_bite_transfer_controller: + mode_handle: joint_mode + is_lazy: NOPE + mode: TRAJECTORY + type: rewd_controllers/MoveUntilTouchTopicController + # 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: impedance + constraints: + stopped_velocity_tolerance: 1.0 + joint_1: + goal: 0.04 + trajectory: 5.0 + joint_2: + goal: 0.04 + trajectory: 5.0 + joint_3: + goal: 0.04 + trajectory: 5.0 + joint_4: + goal: 0.04 + trajectory: 5.0 + joint_5: + goal: 0.04 + trajectory: 5.0 + joint_6: + goal: 0.04 + trajectory: 5.0 + gains: # Required because we're controlling an impedance interface + joint_1: {k: 34, d: 9} + joint_2: {k: 33, d: 9} + joint_3: {k: 25, d: 6} + joint_4: {k: 25, d: 6} + joint_5: {k: 34, d: 15} + joint_6: {k: 25, d: 6} + +# # whole-arm base trajectory controller +inside_mouth_bite_transfer_controller: + mode_handle: joint_mode + is_lazy: OKAY + mode: TRAJECTORY + type: rewd_controllers/MoveUntilTouchTopicController + # 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: impedance + constraints: + stopped_velocity_tolerance: 1.0 + joint_1: + goal: 0.04 + trajectory: 5.0 + joint_2: + goal: 0.04 + trajectory: 5.0 + joint_3: + goal: 0.04 + trajectory: 5.0 + joint_4: + goal: 0.04 + trajectory: 5.0 + joint_5: + goal: 0.04 + trajectory: 5.0 + joint_6: + goal: 0.04 + trajectory: 5.0 + gains: # Required because we're controlling an impedance interface + joint_1: {k: 34, d: 9} + joint_2: {k: 33, d: 9} + joint_3: {k: 25, d: 6} + joint_4: {k: 25, d: 6} + joint_5: {k: 34, d: 15} + joint_6: {k: 25, d: 6} + # 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 diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 7625c43..d9dbe6c 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -206,6 +206,8 @@ class Ada final : public aikido::robot::ros::RosRobot aikido::control::VisualServoingVelocityExecutor::Properties properties = aikido::control::VisualServoingVelocityExecutor::Properties()); + void setTrajectoryLimitsFromParam(std::string type); + private: // Call to spin first to pass current time to step void spin(); diff --git a/src/Ada.cpp b/src/Ada.cpp index 5352144..7ce1dfd 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -388,6 +388,7 @@ Ada::Ada( //============================================================================== Ada::~Ada() { + // deactivateExecutor(); mThread->stop(); } @@ -604,4 +605,23 @@ std::future Ada::executeVisualServoing( "No JacobianVelocityExecutor registered."); } +void Ada::setTrajectoryLimitsFromParam(std::string type) +{ + double limit; + mNode->param("/adaConf/default_accel_lim/"+type, limit, 0); + mSoftAccelerationLimits = mDefaultAccelerationLimits + = (limit != 0) + ? Eigen::VectorXd::Constant(mMetaSkeleton->getNumDofs(), limit) + : mMetaSkeleton->getAccelerationUpperLimits(); + mNode->param("/adaConf/default_vel_lim/"+type, limit, 0); + mSoftVelocityLimits = mDefaultVelocityLimits + = (limit != 0) + ? Eigen::VectorXd::Constant(mMetaSkeleton->getNumDofs(), limit) + : mMetaSkeleton->getVelocityUpperLimits(); + + // Use limits to set default postprocessor + setDefaultPostProcessor( + mSoftVelocityLimits, mSoftAccelerationLimits, KunzParams()); +} + } // namespace ada From b68686cbbfb309e63070930ac653c050e7a31e61 Mon Sep 17 00:00:00 2001 From: RKJenamani Date: Mon, 23 May 2022 16:18:58 -0400 Subject: [PATCH 06/13] Updated bindings for new executor framework --- config/gen3_6dof.yaml | 14 ++++-- include/libada/Ada.hpp | 3 +- launch/default.launch | 6 --- python/adapy/ada_bindings.cpp | 6 ++- src/Ada.cpp | 83 +++++++++++++++++++++-------------- src/util.cpp | 1 + 6 files changed, 67 insertions(+), 46 deletions(-) diff --git a/config/gen3_6dof.yaml b/config/gen3_6dof.yaml index a19c6c8..8fe3d2f 100644 --- a/config/gen3_6dof.yaml +++ b/config/gen3_6dof.yaml @@ -1,9 +1,9 @@ # Arm configuration parameters adaConf: - default_urdf: package://kortex_description/robots/gen3_6dof_vision.urdf + default_urdf: package://kortex_description/robots/gen3_6dof_vision_forque.urdf default_srdf: package://kortex_description/robots/gen3_6dof.srdf default_accel_lim: - aquisition: 2.0 + acquisition: 2.0 transfer: 1.0 default_vel_lim: acquisition: 0.7 @@ -21,10 +21,10 @@ adaConf: # Note: last-controller is default when calling setActiveExecutor(ExecutorType) executors: - {id: bite_transfer_trajectory_executor, type: TRAJECTORY, mode: EFFORT, controller: towards_mouth_bite_transfer_controller} - - {id: bite_transfer_visual_servoing_executor, type: VISUAL_SERVOING, mode: EFFORT, mode: controller: effort_controller} + # - {id: bite_transfer_visual_servoing_executor, type: VISUAL_SERVOING, mode: EFFORT, controller: effort_controller} - {id: bite_transfer_inside_mouth_executor, type: TRAJECTORY, mode: EFFORT, controller: inside_mouth_bite_transfer_controller} - {id: bite_acquisition_trajectory_executor, type: TRAJECTORY, mode: VELOCITY, controller: move_until_touch_topic_controller} - hand_controllers: + hand_executors: enable_controller_switching: true mode_controller: hand_mode_controller # Note: type can be TRAJECTOTY / JOINT_COMMAND / TASK_COMMAND / VISUAL_SERVOING @@ -103,6 +103,8 @@ trajectory_controller: # whole-arm base trajectory controller move_until_touch_topic_controller: mode: TRAJECTORY + controller_mode: VELOCITY + is_lazy: NOPE type: rewd_controllers/MoveUntilTouchTopicController # Forque Sensor forcetorque_wrench_name: /forque/forqueSensor @@ -112,6 +114,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 @@ -148,6 +151,7 @@ towards_mouth_bite_transfer_controller: mode_handle: joint_mode is_lazy: NOPE mode: TRAJECTORY + controller_mode: EFFORT type: rewd_controllers/MoveUntilTouchTopicController # Forque Sensor forcetorque_wrench_name: /forque/forqueSensor @@ -194,6 +198,7 @@ inside_mouth_bite_transfer_controller: mode_handle: joint_mode is_lazy: OKAY mode: TRAJECTORY + controller_mode: EFFORT type: rewd_controllers/MoveUntilTouchTopicController # Forque Sensor forcetorque_wrench_name: /forque/forqueSensor @@ -240,6 +245,7 @@ inside_mouth_bite_transfer_controller: # the lab's rewd_controllers implementation rewd_trajectory_controller: mode: TRAJECTORY + is_lazy: NOPE type: rewd_controllers/JointTrajectoryController joints: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index d9dbe6c..0f89bad 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -99,9 +99,10 @@ class Ada final : public aikido::robot::ros::RosRobot /// \param[in] retriever Resource retriever for retrieving Ada Ada(bool simulation, aikido::planner::WorldPtr env = aikido::planner::World::create(), + std::string name = std::string("ada"), const std::string confNamespace = DEFAULT_CONF_OBJ_NS, const std::chrono::milliseconds threadCycle = DEFAULT_THREAD_CYCLE, - std::shared_ptr<::ros::NodeHandle> node = nullptr, + const ::ros::NodeHandle* node = nullptr, aikido::common::RNG::result_type rngSeed = std::random_device{}(), const dart::common::ResourceRetrieverPtr& retriever = std::make_shared()); diff --git a/launch/default.launch b/launch/default.launch index 6d6d06b..9c24f03 100644 --- a/launch/default.launch +++ b/launch/default.launch @@ -111,12 +111,6 @@ - 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 7ce1dfd..6bb0b83 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -74,17 +74,17 @@ inline const dart::common::Uri getDartURI( Ada::Ada( bool simulation, aikido::planner::WorldPtr env, + std::string name, const std::string confNamespace, const std::chrono::milliseconds threadCycle, - std::shared_ptr<::ros::NodeHandle> node, + const ::ros::NodeHandle* node, aikido::common::RNG::result_type rngSeed, const dart::common::ResourceRetrieverPtr& retriever) : aikido::robot::ros::RosRobot( internal::getDartURI(confNamespace, "default_urdf", DEFAULT_URDF), internal::getDartURI(confNamespace, "default_srdf", DEFAULT_SRDF), - "ada", - retriever, - node) + name, + retriever) , mSimulation(simulation) { @@ -94,17 +94,17 @@ Ada::Ada( setRNG(std::make_unique>(rngSeed)); // Read default soft accleration/velocity limits - double limit; - mNode->param("/" + confNamespace + "/default_accel_lim", limit, 0); - mSoftAccelerationLimits = mDefaultAccelerationLimits - = (limit != 0) - ? Eigen::VectorXd::Constant(mMetaSkeleton->getNumDofs(), limit) - : mMetaSkeleton->getAccelerationUpperLimits(); - mNode->param("/" + confNamespace + "/default_vel_lim", limit, 0); - mSoftVelocityLimits = mDefaultVelocityLimits - = (limit != 0) - ? Eigen::VectorXd::Constant(mMetaSkeleton->getNumDofs(), limit) - : mMetaSkeleton->getVelocityUpperLimits(); + // double limit; + // mNode->param("/" + confNamespace + "/default_accel_lim", limit, 0); + // mSoftAccelerationLimits = mDefaultAccelerationLimits + // = (limit != 0) + // ? Eigen::VectorXd::Constant(mMetaSkeleton->getNumDofs(), limit) + // : mMetaSkeleton->getAccelerationUpperLimits(); + // mNode->param("/" + confNamespace + "/default_vel_lim", limit, 0); + // mSoftVelocityLimits = mDefaultVelocityLimits + // = (limit != 0) + // ? Eigen::VectorXd::Constant(mMetaSkeleton->getNumDofs(), limit) + // : mMetaSkeleton->getVelocityUpperLimits(); // Create sub-robot (arm) std::vector armNodes; @@ -168,9 +168,10 @@ Ada::Ada( } } + setTrajectoryLimitsFromParam("acquisition"); // Use limits to set default postprocessor - setDefaultPostProcessor( - mSoftVelocityLimits, mSoftAccelerationLimits, KunzParams()); + // setDefaultPostProcessor( + // mSoftVelocityLimits, mSoftAccelerationLimits, KunzParams()); // Create joint state updates if (!mSimulation) @@ -185,7 +186,7 @@ Ada::Ada( else { // Simulation, create state publisher - mPub = mNode->advertise("joint_states", 5); + mPub = mNode->advertise("joint_states_1", 5); } // Create Inner AdaHand @@ -218,7 +219,11 @@ Ada::Ada( } else { - auto ros_controller_service_client = std::make_shared<::ros::ServiceClient>( + auto rosLoadControllerServiceClient = std::make_shared<::ros::ServiceClient>( + mNode->serviceClient( + "controller_manager/load_controller")); + + auto rosSwitchControllerServiceClient = std::make_shared<::ros::ServiceClient>( mNode->serviceClient( "controller_manager/switch_controller")); @@ -227,9 +232,11 @@ Ada::Ada( std::set{mArm, mHandRobot}) { + subrobot->setRosLoadControllerServiceClient(rosLoadControllerServiceClient); + bool isHand = (subrobot == mHandRobot); - std::string ns = isHand ? "/" + confNamespace + "/hand_controllers/" - : "/" + confNamespace + "/arm_controllers/"; + std::string ns = isHand ? "/" + confNamespace + "/hand_executors/" + : "/" + confNamespace + "/arm_executors/"; // Controller Switching Service Client bool enableControllerSwitching = mNode->param( @@ -237,13 +244,14 @@ Ada::Ada( false); if(enableControllerSwitching) - subrobot->setRosControllerServiceClient(ros_controller_service_client); + subrobot->setRosSwitchControllerServiceClient(rosSwitchControllerServiceClient); // Mode controller auto modeControllerName = mNode->param( ns + "mode_controller", std::string("")); + std::cout<<"Mode controller name: "< executorsDetails = util::loadExecutorsDetailsFromParameter( *mNode, - ns + "/executors"); + ns + "executors"); if(executorsDetails.size() == 0) { @@ -269,6 +277,7 @@ Ada::Ada( { if(executorDetails.mType == "TRAJECTORY") { + std::cout<<"Executor type is TRAJECTORY!!"<( *mNode, @@ -277,7 +286,7 @@ Ada::Ada( DEFAULT_ROS_TRAJ_GOAL_TIME_TOL, subrobot->getMetaSkeleton()); auto controllerMode = util::modeFromString( - mNode->param("/" + executorDetails.mController + "/mode", "")); + mNode->param("/" + executorDetails.mController + "/controller_mode", "")); subrobot->registerExecutor(trajExec, executorDetails.mController, controllerMode, executorDetails.mId); } else if(executorDetails.mType == "JOINT_COMMAND") @@ -288,7 +297,7 @@ Ada::Ada( = std::make_shared( *mNode, executorDetails.mController, subrobot->getMetaSkeleton()->getDofs()); auto controllerMode = util::modeFromString( - mNode->param("/" + executorDetails.mController + "/mode", "POSITION")); + mNode->param("/" + executorDetails.mController + "/controller_mode", "POSITION")); subrobot->registerExecutor(posExec, executorDetails.mController, controllerMode, executorDetails.mId); } else if(executorDetails.mMode == "VELOCITY") @@ -297,7 +306,7 @@ Ada::Ada( = std::make_shared( *mNode, executorDetails.mController, subrobot->getMetaSkeleton()->getDofs()); auto controllerMode = util::modeFromString( - mNode->param("/" + executorDetails.mController + "/mode", "VELOCITY")); + mNode->param("/" + executorDetails.mController + "/controller_mode", "VELOCITY")); subrobot->registerExecutor(velExec, executorDetails.mController, controllerMode, executorDetails.mId); } else if(executorDetails.mMode == "EFFORT") @@ -306,7 +315,7 @@ Ada::Ada( = std::make_shared( *mNode, executorDetails.mController, subrobot->getMetaSkeleton()->getDofs()); auto controllerMode = util::modeFromString( - mNode->param("/" + executorDetails.mController + "/mode", "EFFORT")); + mNode->param("/" + executorDetails.mController + "/controller_mode", "EFFORT")); subrobot->registerExecutor(effExec, executorDetails.mController, controllerMode, executorDetails.mId); } else @@ -327,7 +336,7 @@ Ada::Ada( = std::make_shared( handEnd, velExec); auto controllerMode = util::modeFromString( - mNode->param("/" + executorDetails.mController + "/mode", "VELOCITY")); + mNode->param("/" + executorDetails.mController + "/controller_mode", "VELOCITY")); subrobot->registerExecutor(jvExec, executorDetails.mController, controllerMode, executorDetails.mId); } else if(executorDetails.mMode == "EFFORT") @@ -339,7 +348,7 @@ Ada::Ada( = std::make_shared( handEnd, effExec); auto controllerMode = util::modeFromString( - mNode->param("/" + executorDetails.mController + "/mode", "EFFORT")); + mNode->param("/" + executorDetails.mController + "/controller_mode", "EFFORT")); subrobot->registerExecutor(effJacExec, executorDetails.mController, controllerMode, executorDetails.mId); } else @@ -363,7 +372,7 @@ Ada::Ada( = std::make_shared( handEnd, jvExec); auto controllerMode = util::modeFromString( - mNode->param("/" + executorDetails.mController + "/mode", "VELOCITY")); + mNode->param("/" + executorDetails.mController + "/controller_mode", "VELOCITY")); subrobot->registerExecutor(vsExec, executorDetails.mController, controllerMode, executorDetails.mId); } else @@ -377,12 +386,20 @@ Ada::Ada( } } - // Activate Trajectory Executor - activateExecutor(aikido::control::ExecutorType::TRAJECTORY); + std::cout<<"Creating thread for executors!"<( std::bind(&Ada::spin, this), threadCycle); + + std::cout<<"Thread created!"<activateExecutor(aikido::control::ExecutorType::TRAJECTORY); + + std::cout<<"Activated trajectory executor!"<isRunning()) return; - Robot::step(timepoint); + RosRobot::step(timepoint); if (!mSimulation && mJointStateClient) { diff --git a/src/util.cpp b/src/util.cpp index 813ad78..38ed61e 100644 --- a/src/util.cpp +++ b/src/util.cpp @@ -108,6 +108,7 @@ std::vector loadExecutorsDetailsFromParameter(const ros::NodeHa static const std::vector emptyResult; XmlRpcValue executorsDetailsXml; + std::cout<<"parameterName: "< Date: Mon, 23 May 2022 16:42:41 -0400 Subject: [PATCH 07/13] Removed unrequired params --- config/gen3_6dof.yaml | 6 +++--- launch/default.launch | 10 ---------- 2 files changed, 3 insertions(+), 13 deletions(-) diff --git a/config/gen3_6dof.yaml b/config/gen3_6dof.yaml index 8fe3d2f..1799f70 100644 --- a/config/gen3_6dof.yaml +++ b/config/gen3_6dof.yaml @@ -20,9 +20,9 @@ adaConf: # Note: type TRAJECTORY does not require mode specification # Note: last-controller is default when calling setActiveExecutor(ExecutorType) executors: - - {id: bite_transfer_trajectory_executor, type: TRAJECTORY, mode: EFFORT, controller: towards_mouth_bite_transfer_controller} - # - {id: bite_transfer_visual_servoing_executor, type: VISUAL_SERVOING, mode: EFFORT, controller: effort_controller} - - {id: bite_transfer_inside_mouth_executor, type: TRAJECTORY, mode: EFFORT, controller: inside_mouth_bite_transfer_controller} + - {id: bite_transfer_trajectory_executor, type: TRAJECTORY, controller: towards_mouth_bite_transfer_controller} + - {id: bite_transfer_visual_servoing_executor, type: VISUAL_SERVOING, mode: VELOCITY, controller: velocity_controller} + - {id: bite_transfer_inside_mouth_executor, type: TRAJECTORY, controller: inside_mouth_bite_transfer_controller} - {id: bite_acquisition_trajectory_executor, type: TRAJECTORY, mode: VELOCITY, controller: move_until_touch_topic_controller} hand_executors: enable_controller_switching: true diff --git a/launch/default.launch b/launch/default.launch index 9c24f03..84d5a7f 100644 --- a/launch/default.launch +++ b/launch/default.launch @@ -56,16 +56,6 @@ hand_controller " /> - - - From 7b7f20201dc20821870aae296fbbdbc02882b449 Mon Sep 17 00:00:00 2001 From: RKJenamani Date: Sun, 19 Jun 2022 17:24:47 -0400 Subject: [PATCH 08/13] Changed to required executors in config --- config/gen3_6dof.yaml | 106 ++++++++++++++++-------------------------- 1 file changed, 40 insertions(+), 66 deletions(-) diff --git a/config/gen3_6dof.yaml b/config/gen3_6dof.yaml index 1799f70..dca8a63 100644 --- a/config/gen3_6dof.yaml +++ b/config/gen3_6dof.yaml @@ -20,10 +20,9 @@ adaConf: # Note: type TRAJECTORY does not require mode specification # Note: last-controller is default when calling setActiveExecutor(ExecutorType) executors: - - {id: bite_transfer_trajectory_executor, type: TRAJECTORY, controller: towards_mouth_bite_transfer_controller} - - {id: bite_transfer_visual_servoing_executor, type: VISUAL_SERVOING, mode: VELOCITY, controller: velocity_controller} - - {id: bite_transfer_inside_mouth_executor, type: TRAJECTORY, controller: inside_mouth_bite_transfer_controller} - - {id: bite_acquisition_trajectory_executor, type: TRAJECTORY, mode: VELOCITY, controller: move_until_touch_topic_controller} + - {id: effort_command_executor, type: JOINT_COMMAND, mode: EFFORT, controller: move_until_touch_topic_joint_group_command_controller} + - {id: effort_trajectory_executor, type: TRAJECTORY, mode: EFFORT, controller: effort_move_until_touch_topic_joint_trajectory_controller} + - {id: velocity_trajectory_executor, type: TRAJECTORY, mode: VELOCITY, controller: velocity_move_until_touch_topic_joint_trajectory_controller} hand_executors: enable_controller_switching: true mode_controller: hand_mode_controller @@ -101,11 +100,10 @@ trajectory_controller: joint_6: {p: 3, d: 0, i: 0, i_clamp: 1} # whole-arm base trajectory controller -move_until_touch_topic_controller: +velocity_move_until_touch_topic_joint_trajectory_controller: mode: TRAJECTORY controller_mode: VELOCITY - is_lazy: NOPE - type: rewd_controllers/MoveUntilTouchTopicController + type: rewd_controllers/MoveUntilTouchTopicJointTrajectoryController # Forque Sensor forcetorque_wrench_name: /forque/forqueSensor forcetorque_tare_name: /forque/bias_controller/trigger @@ -146,13 +144,11 @@ 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 -towards_mouth_bite_transfer_controller: - mode_handle: joint_mode - is_lazy: NOPE +# whole-arm base trajectory controller +effort_move_until_touch_topic_joint_trajectory_controller: mode: TRAJECTORY controller_mode: EFFORT - type: rewd_controllers/MoveUntilTouchTopicController + type: rewd_controllers/MoveUntilTouchTopicJointTrajectoryController # Forque Sensor forcetorque_wrench_name: /forque/forqueSensor forcetorque_tare_name: /forque/bias_controller/trigger @@ -164,42 +160,40 @@ towards_mouth_bite_transfer_controller: max_ft_delay: 100 joints: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] - control_type: impedance + control_type: velocity constraints: stopped_velocity_tolerance: 1.0 joint_1: - goal: 0.04 - trajectory: 5.0 + goal: 0.02 + trajectory: 1 joint_2: - goal: 0.04 - trajectory: 5.0 + goal: 0.02 + trajectory: 1 joint_3: - goal: 0.04 - trajectory: 5.0 + goal: 0.02 + trajectory: 1 joint_4: - goal: 0.04 - trajectory: 5.0 + goal: 0.02 + trajectory: 1 joint_5: - goal: 0.04 - trajectory: 5.0 + goal: 0.02 + trajectory: 1 joint_6: - goal: 0.04 - trajectory: 5.0 - gains: # Required because we're controlling an impedance interface - joint_1: {k: 34, d: 9} - joint_2: {k: 33, d: 9} - joint_3: {k: 25, d: 6} - joint_4: {k: 25, d: 6} - joint_5: {k: 34, d: 15} - joint_6: {k: 25, d: 6} + goal: 0.02 + trajectory: 1 + gains: # Required because we're controlling a velocity interface + 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} -# # whole-arm base trajectory controller -inside_mouth_bite_transfer_controller: - mode_handle: joint_mode - is_lazy: OKAY - mode: TRAJECTORY +# whole-arm base trajectory controller +move_until_touch_topic_joint_group_command_controller: + # mode: TRAJECTORY controller_mode: EFFORT - type: rewd_controllers/MoveUntilTouchTopicController + type: rewd_controllers/MoveUntilTouchTopicJointGroupCommandController # Forque Sensor forcetorque_wrench_name: /forque/forqueSensor forcetorque_tare_name: /forque/bias_controller/trigger @@ -211,34 +205,14 @@ inside_mouth_bite_transfer_controller: max_ft_delay: 100 joints: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] - control_type: impedance - constraints: - stopped_velocity_tolerance: 1.0 - joint_1: - goal: 0.04 - trajectory: 5.0 - joint_2: - goal: 0.04 - trajectory: 5.0 - joint_3: - goal: 0.04 - trajectory: 5.0 - joint_4: - goal: 0.04 - trajectory: 5.0 - joint_5: - goal: 0.04 - trajectory: 5.0 - joint_6: - goal: 0.04 - trajectory: 5.0 - gains: # Required because we're controlling an impedance interface - joint_1: {k: 34, d: 9} - joint_2: {k: 33, d: 9} - joint_3: {k: 25, d: 6} - joint_4: {k: 25, d: 6} - joint_5: {k: 34, d: 15} - joint_6: {k: 25, d: 6} + control_type: effort_forward + gains: # Required because we're controlling a velocity interface + 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} # Optional controller that might be useful for testing. # Should do the same thing as trajectory_controller, but this is From bac0e67b29d9edde23fd87dbfbeb5507f6a33a86 Mon Sep 17 00:00:00 2001 From: RKJenamani Date: Tue, 26 Jul 2022 19:09:17 -0400 Subject: [PATCH 09/13] Small changes to setup demo --- config/gen3_6dof.yaml | 32 +++++++++++++++++++++++++++++--- include/libada/Ada.hpp | 12 ------------ src/Ada.cpp | 23 ----------------------- 3 files changed, 29 insertions(+), 38 deletions(-) diff --git a/config/gen3_6dof.yaml b/config/gen3_6dof.yaml index dca8a63..98f3943 100644 --- a/config/gen3_6dof.yaml +++ b/config/gen3_6dof.yaml @@ -23,6 +23,7 @@ adaConf: - {id: effort_command_executor, type: JOINT_COMMAND, mode: EFFORT, controller: move_until_touch_topic_joint_group_command_controller} - {id: effort_trajectory_executor, type: TRAJECTORY, mode: EFFORT, controller: effort_move_until_touch_topic_joint_trajectory_controller} - {id: velocity_trajectory_executor, type: TRAJECTORY, mode: VELOCITY, controller: velocity_move_until_touch_topic_joint_trajectory_controller} + - {id: velocity_visual_servoing_executor, type: VISUAL_SERVOING, mode: VELOCITY, controller: velocity_visual_servoing_controller} hand_executors: enable_controller_switching: true mode_controller: hand_mode_controller @@ -160,7 +161,7 @@ effort_move_until_touch_topic_joint_trajectory_controller: max_ft_delay: 100 joints: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] - control_type: velocity + control_type: velocity_effort constraints: stopped_velocity_tolerance: 1.0 joint_1: @@ -181,6 +182,31 @@ effort_move_until_touch_topic_joint_trajectory_controller: joint_6: goal: 0.02 trajectory: 1 + gains: # Required because we're controlling a velocity interface + joint_1: {p: 34, d: 9, i: 0, i_clamp: 1.0} + joint_2: {p: 33, d: 9, i: 0, i_clamp: 1.0} + joint_3: {p: 25, d: 6, i: 0, i_clamp: 1.0} + joint_4: {p: 25, d: 6, i: 0, i_clamp: 1.0} + joint_5: {p: 34, d: 15, i: 0, i_clamp: 1.0} + joint_6: {p: 25, d: 6, i: 0, i_clamp: 1.0} + +# whole-arm base trajectory controller +move_until_touch_topic_joint_group_command_controller: + # mode: TRAJECTORY + controller_mode: EFFORT + type: rewd_controllers/MoveUntilTouchTopicJointGroupCommandController + # 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: forward_effort gains: # Required because we're controlling a velocity interface 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} @@ -190,7 +216,7 @@ effort_move_until_touch_topic_joint_trajectory_controller: joint_6: {p: 1.2, d: 0, i: 0, i_clamp: 1.0} # whole-arm base trajectory controller -move_until_touch_topic_joint_group_command_controller: +velocity_visual_servoing_controller: # mode: TRAJECTORY controller_mode: EFFORT type: rewd_controllers/MoveUntilTouchTopicJointGroupCommandController @@ -205,7 +231,7 @@ move_until_touch_topic_joint_group_command_controller: max_ft_delay: 100 joints: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] - control_type: effort_forward + control_type: velocity_effort gains: # Required because we're controlling a velocity interface 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} diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 0f89bad..d7e3eb5 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -195,18 +195,6 @@ class Ada final : public aikido::robot::ros::RosRobot const Eigen::Vector6d command, const std::chrono::duration& timeout); - /// - /// Convenience: runs velocity-command Visual Servoing - /// Will auto-activate a VisualServoingVelocityExecutor. - /// future will have exception if one doesn't exist. - /// - /// \param[in] command SE3, applied to end-effector - /// \param[in] timeout Timeout for the command - std::future executeVisualServoing( - std::function(void)> perception, - aikido::control::VisualServoingVelocityExecutor::Properties properties - = aikido::control::VisualServoingVelocityExecutor::Properties()); - void setTrajectoryLimitsFromParam(std::string type); private: diff --git a/src/Ada.cpp b/src/Ada.cpp index 6bb0b83..30ef77d 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -599,29 +599,6 @@ std::future Ada::executeJacobianCommand( "No JacobianVelocityExecutor registered."); } -//============================================================================== -std::future Ada::executeVisualServoing( - std::function(void)> perception, - aikido::control::VisualServoingVelocityExecutor::Properties properties) -{ - std::shared_ptr executor; - // Search for last added executor of given type - for (auto id = mExecutorsInsertionOrder.rbegin(); id != mExecutorsInsertionOrder.rend(); id++) - { - executor = std::dynamic_pointer_cast< - aikido::control::VisualServoingVelocityExecutor>(mExecutors[*id]); - if (executor && activateExecutor(*id)) - { - executor->resetProperties(properties); - return executor->execute(perception); - } - } - - deactivateExecutor(); - return aikido::control::make_exceptional_future( - "No JacobianVelocityExecutor registered."); -} - void Ada::setTrajectoryLimitsFromParam(std::string type) { double limit; From ed5624e0a9ed3d40c0bae1a5088c2ee29bfa13d8 Mon Sep 17 00:00:00 2001 From: RKJenamani Date: Mon, 16 Jan 2023 16:30:18 -0500 Subject: [PATCH 10/13] Fudging for Gen 3 --- launch/gen3_urdf.launch | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) 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 From 9775aca7efaa410f885c72cc7a62465ff583e315 Mon Sep 17 00:00:00 2001 From: RKJenamani Date: Mon, 16 Jan 2023 16:59:52 -0500 Subject: [PATCH 11/13] Added newline EOF --- config/gen2_6dof_controllers.yaml | 2 +- config/gen3_6dof_controllers.yaml | 2 +- include/libada/Ada.hpp | 2 +- src/Ada.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/config/gen2_6dof_controllers.yaml b/config/gen2_6dof_controllers.yaml index b4e4a65..f6f2b3d 100644 --- a/config/gen2_6dof_controllers.yaml +++ b/config/gen2_6dof_controllers.yaml @@ -132,4 +132,4 @@ hand_controller: stopped_velocity_tolerance: 1.0 gains: # Required because we're controlling a velocity interface j2n6s200_joint_finger_1: {p: 1, d: 0, i: 0, i_clamp: 1} - j2n6s200_joint_finger_2: {p: 1, d: 0, i: 0, i_clamp: 1} \ No newline at end of file + j2n6s200_joint_finger_2: {p: 1, d: 0, i: 0, i_clamp: 1} diff --git a/config/gen3_6dof_controllers.yaml b/config/gen3_6dof_controllers.yaml index 3849751..eaa8301 100644 --- a/config/gen3_6dof_controllers.yaml +++ b/config/gen3_6dof_controllers.yaml @@ -172,4 +172,4 @@ hand_controller: constraints: stopped_velocity_tolerance: 1.0 gains: # Required because we're controlling a velocity interface - finger_joint: {p: 1, d: 0, i: 0, i_clamp: 1} \ No newline at end of file + finger_joint: {p: 1, d: 0, i: 0, i_clamp: 1} diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 801002b..214ec64 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -250,4 +250,4 @@ class Ada final : public aikido::robot::ros::RosRobot #include "libada/AdaHand.hpp" -#endif // LIBADA_ADA_HPP_ \ No newline at end of file +#endif // LIBADA_ADA_HPP_ diff --git a/src/Ada.cpp b/src/Ada.cpp index 921cdd2..f4624c6 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -492,4 +492,4 @@ dart::dynamics::BodyNodePtr Ada::getEndEffectorBodyNode() return internal::getBodyNodeOrThrow(mMetaSkeleton, mEndEffectorName); } -} // namespace ada \ No newline at end of file +} // namespace ada From 6bdf6988e728aac3782c1352bd101102a1007b99 Mon Sep 17 00:00:00 2001 From: RKJenamani Date: Mon, 16 Jan 2023 18:47:59 -0500 Subject: [PATCH 12/13] Move until touch running --- config/gen3_6dof_config.yaml | 17 ++ include/libada/Ada.hpp | 45 ++--- launch/gen3_hardware.launch | 9 - src/Ada.cpp | 309 ++++++++++++++++++++++------------- 4 files changed, 225 insertions(+), 155 deletions(-) diff --git a/config/gen3_6dof_config.yaml b/config/gen3_6dof_config.yaml index 4a715cb..e0d3725 100644 --- a/config/gen3_6dof_config.yaml +++ b/config/gen3_6dof_config.yaml @@ -9,3 +9,20 @@ 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: velocity_trajectory_executor, type: TRAJECTORY, mode: VELOCITY, controller: move_until_touch_topic_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/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/launch/gen3_hardware.launch b/launch/gen3_hardware.launch index 5091d4c..29878bc 100644 --- a/launch/gen3_hardware.launch +++ b/launch/gen3_hardware.launch @@ -27,13 +27,4 @@ - - \ No newline at end of file diff --git a/src/Ada.cpp b/src/Ada.cpp index f4624c6..e3401c2 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,199 @@ 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 rosSwitchControllerServiceClient = std::make_shared<::ros::ServiceClient>( + mNode.serviceClient( + "controller_manager/switch_controller")); + // Real ROS Executors + for (auto subrobot : + std::set{mArm, mHandRobot}) + { + subrobot->setRosLoadControllerServiceClient(rosLoadControllerServiceClient); + 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 +559,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() { From ce639ed9a4beb5d5680b35d44d1a3b4953d19d47 Mon Sep 17 00:00:00 2001 From: RKJenamani Date: Fri, 10 Mar 2023 23:45:33 -0500 Subject: [PATCH 13/13] Update --- config/gen3_6dof_config.yaml | 7 +- config/gen3_6dof_controllers.yaml | 114 ++++++++++++++++++++++++++---- launch/gen3_hardware.launch | 8 ++- launch/libada.launch | 5 +- src/Ada.cpp | 7 +- 5 files changed, 122 insertions(+), 19 deletions(-) diff --git a/config/gen3_6dof_config.yaml b/config/gen3_6dof_config.yaml index e0d3725..e5d7917 100644 --- a/config/gen3_6dof_config.yaml +++ b/config/gen3_6dof_config.yaml @@ -17,7 +17,12 @@ arm_executors: # Note: type TRAJECTORY does not require mode specification # Note: last-controller is default when calling setActiveExecutor(ExecutorType) executors: - - {id: velocity_trajectory_executor, type: TRAJECTORY, mode: VELOCITY, controller: move_until_touch_topic_controller} + - {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 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/launch/gen3_hardware.launch b/launch/gen3_hardware.launch index 29878bc..8fcd262 100644 --- a/launch/gen3_hardware.launch +++ b/launch/gen3_hardware.launch @@ -1,10 +1,13 @@ - + + + + + + + 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/src/Ada.cpp b/src/Ada.cpp index e3401c2..016602f 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -229,14 +229,19 @@ Ada::Ada( 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, mHandRobot}) + 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/";